Solved some bugs in the server code.

src/dynamixelserver.cpp

@@ -86,7 +86,7 @@ CDynamixelServer *CDynamixelServer::instance(void)
 
 void CDynamixelServer::send_instruction_packet_v1(dyn_inst_t inst,unsigned char *data,unsigned char len,unsigned char id)
 {
-  unsigned char *packet;
+  unsigned char *packet=NULL;
   int i,length;
 
   switch(inst)
@@ -99,7 +99,7 @@ void CDynamixelServer::send_instruction_packet_v1(dyn_inst_t inst,unsigned char
                    break;
     case dyn_sync_write: id=0xFE;
                          length=6+len;
-                         data=new unsigned char[length];
+                         packet=new unsigned char[length];
                          break;
     default: throw CDynamixelServerException(_HERE_,"Instruction not supported");
              break;
@@ -120,13 +120,19 @@ void CDynamixelServer::send_instruction_packet_v1(dyn_inst_t inst,unsigned char
     if(this->comm_dev->write(packet,length)!=length)
     {
       /* handle exceptions */
+      if(packet!=NULL)
+        delete[] packet;
       this->dynamixel_access.exit();
       throw CDynamixelServerException(_HERE_,"Unexpected error while writing to the communication device");
     }
+    if(packet!=NULL)
+      delete[] packet;
     this->dynamixel_access.exit();
   }
   else
   {
+    if(packet!=NULL)
+      delete[] packet;
     /* handle exceptions */
     throw CDynamixelServerException(_HERE_,"The communication device is not properly configured");
   }
@@ -134,7 +140,7 @@ void CDynamixelServer::send_instruction_packet_v1(dyn_inst_t inst,unsigned char
 
 void CDynamixelServer::send_instruction_packet_v2(dyn_inst_t inst,unsigned char *data,unsigned short int len,unsigned char id)
 {
-  unsigned char *packet;
+  unsigned char *packet=NULL;
   int i,length,crc;
 
   switch(inst)
@@ -145,9 +151,12 @@ void CDynamixelServer::send_instruction_packet_v2(dyn_inst_t inst,unsigned char
                    if(len!=0)
                      throw CDynamixelServerException(_HERE_,"Invalid data length");
                    break;
+    case dyn_bulk_read:
+    case dyn_bulk_write:
+    case dyn_sync_read:
     case dyn_sync_write: id=0xFE;
                          length=10+len;
-                         data=new unsigned char[length];
+                         packet=new unsigned char[length];
                          break;
     default: throw CDynamixelServerException(_HERE_,"Instruction not supported");
              break;
@@ -173,13 +182,19 @@ void CDynamixelServer::send_instruction_packet_v2(dyn_inst_t inst,unsigned char
     if(this->comm_dev->write(packet,length)!=length)
     {
       /* handle exceptions */
+      if(packet!=NULL)
+        delete[] packet;
       this->dynamixel_access.exit();
       throw CDynamixelServerException(_HERE_,"Unexpected error while writing to the communication device");
     }
+    if(packet!=NULL)
+      delete[] packet;
     this->dynamixel_access.exit();
   }
   else
   {
+    if(packet!=NULL)
+      delete[] packet;
     /* handle exceptions */
     throw CDynamixelServerException(_HERE_,"The communication device is not properly configured");
   }
@@ -1066,7 +1081,7 @@ void CDynamixelServer::close(void)
 
 void CDynamixelServer::write_sync(std::vector<unsigned char>& servo_ids,unsigned short int start_addr, std::vector< std::vector<unsigned char> >& data,dyn_version_t version)
 {
-  unsigned char *data_int;
+  unsigned char *data_int=NULL;
   unsigned int i,j=0;
   int length=0;
 
@@ -1085,41 +1100,52 @@ void CDynamixelServer::write_sync(std::vector<unsigned char>& servo_ids,unsigned
     /* handle exception */
     throw CDynamixelServerException(_HERE_,"Too much data blocks or some missing servos");
   }
-  if(version==dyn_version1)
-  {
-    length=2+(data[0].size()+1)*servo_ids.size();
-    data_int=new unsigned char[length];
-    data_int[0]=(unsigned char)start_addr;
-    data_int[1]=data[0].size();
-    for(i=0;i<servo_ids.size();i++)
+  try{
+    if(version==dyn_version1)
     {
-      data_int[2+(data[i].size()+1)*i]= servo_ids[i];
-      for(j=0;j<data[i].size();j++)
-        data_int[2+(data[i].size()+1)*i+1+j]=data[i][j];
+      length=2+(data[0].size()+1)*servo_ids.size();
+      data_int=new unsigned char[length];
+      data_int[0]=(unsigned char)start_addr;
+      data_int[1]=data[0].size();
+      for(i=0;i<servo_ids.size();i++)
+      {
+        data_int[2+(data[i].size()+1)*i]= servo_ids[i];
+        for(j=0;j<data[i].size();j++)
+          data_int[2+(data[i].size()+1)*i+1+j]=data[i][j];
+      }
+      this->send_instruction_packet_v1(dyn_sync_write,data_int,length);
+      delete[] data_int;
     }
-    this->send_instruction_packet_v1(dyn_sync_write,data_int,length);
-  }
-  else
-  {
-    length=4+(data[0].size()+1)*servo_ids.size();
-    data_int=new unsigned char[length];
-    data_int[0]=start_addr%256;
-    data_int[1]=start_addr/256;
-    data_int[2]=data[0].size()%256;
-    data_int[3]=data[0].size()/256;
-    for(i=0;i<servo_ids.size();i++)
+    else
+    {
+      length=4+(data[0].size()+1)*servo_ids.size();
+      data_int=new unsigned char[length];
+      data_int[0]=start_addr%256;
+      data_int[1]=start_addr/256;
+      data_int[2]=data[0].size()%256;
+      data_int[3]=data[0].size()/256;
+      for(i=0;i<servo_ids.size();i++)
+      {
+        data_int[4+(data[i].size()+1)*i]= servo_ids[i];
+        for(j=0;j<data[i].size();j++)
+          data_int[4+(data[i].size()+1)*i+1+j]=data[i][j];
+      }
+      this->send_instruction_packet_v2(dyn_sync_write,data_int,length);
+      delete[] data_int;
+    }
+  }catch(CException &e){
+    if(data_int!=NULL)
     {
-      data_int[4+(data[i].size()+1)*i]= servo_ids[i];
-      for(j=0;j<data[i].size();j++)
-        data_int[4+(data[i].size()+1)*i+1+j]=data[i][j];
+      delete[] data_int;
+      data_int=NULL;
     }
-    this->send_instruction_packet_v2(dyn_sync_write,data_int,length);
+    throw e;
   }
 }
 
 void CDynamixelServer::read_sync(std::vector<unsigned char>& servo_ids,unsigned short int start_addr,unsigned short int length, std::vector< std::vector<unsigned char> >& data,dyn_version_t version)
 {
-  unsigned char *data_int,*data_out,error;
+  unsigned char *data_int=NULL,*data_out=NULL,error;
   unsigned short length_v2;
   unsigned int i,j=0;
   int length_int=0;
@@ -1136,41 +1162,60 @@ void CDynamixelServer::read_sync(std::vector<unsigned char>& servo_ids,unsigned
   }
   else
   {
-    length_int=4+servo_ids.size();
-    data_int=new unsigned char[length_int];
-    data_int[0]=start_addr%256;
-    data_int[1]=start_addr/256;
-    data_int[2]=length%256;
-    data_int[3]=length/256;
-    for(i=0;i<servo_ids.size();i++)
-      data_int[4+i]=servo_ids[i];
-    this->send_instruction_packet_v2(dyn_sync_read,data_int,length_int);
-    /* read all the data from all the servos */
-    data.resize(servo_ids.size());
-    for(i=0;i<servo_ids.size();i++)
-    {
-      error=this->receive_status_packet_v2(&data_out,&length_v2);
-      try{
-        this->handle_error(error);
-        if(data_out!=NULL)
-        {
-          data[i].resize(length_v2);
-          for(j=0;j<length_v2;j++)
-            data[i][j]=data_out[j];
-          delete[] data_out;
-        }
-      }catch(CException &e){
-        if(data_out!=NULL)
-          delete[] data_out;
+    try{
+      length_int=4+servo_ids.size();
+      data_int=new unsigned char[length_int];
+      data_int[0]=start_addr%256;
+      data_int[1]=start_addr/256;
+      data_int[2]=length%256;
+      data_int[3]=length/256;
+      for(i=0;i<servo_ids.size();i++)
+        data_int[4+i]=servo_ids[i];
+      this->send_instruction_packet_v2(dyn_sync_read,data_int,length_int);
+      /* read all the data from all the servos */
+      if(data_int!=NULL)
+      {
+        delete[] data_int;
+        data_int=NULL;
+      }
+      data.resize(servo_ids.size());
+      for(i=0;i<servo_ids.size();i++)
+      {
+        try{
+          error=this->receive_status_packet_v2(&data_out,&length_v2);
+          this->handle_error(error);
+          if(data_out!=NULL)
+          {
+            data[i].resize(length_v2);
+            for(j=0;j<length_v2;j++)
+              data[i][j]=data_out[j];
+            delete[] data_out;
+            data_out=NULL;
+          }
+        }catch(CException &e){
+          if(data_out!=NULL)
+          {
+            delete[] data_out;
+            data_out=NULL;
+          }
+          throw e;
+        } 
+      }
+    }catch(CException &e){
+      if(data_int!=NULL)
+      {
+        delete[] data_int;
+        data_out=NULL;
       }
+      throw e;
     }
   }
 }
 
 void CDynamixelServer::write_bulk(std::vector<unsigned char>& servo_ids,std::vector<unsigned short int> &start_addr, std::vector< std::vector<unsigned char> >& data,dyn_version_t version)
 {
-  unsigned char *data_int;
-  unsigned int i;
+  unsigned char *data_int=NULL;
+  unsigned int i,j;
   int length=0,offset=0;
 
   if(servo_ids.size()==0)
@@ -1195,26 +1240,38 @@ void CDynamixelServer::write_bulk(std::vector<unsigned char>& servo_ids,std::vec
   }
   else
   {
-    length=0;
-    for(i=0;i<servo_ids.size();i++)
-      length+=5+data[i].size();
-    data_int=new unsigned char[length];
-    for(i=0;i<servo_ids.size();i++)
-    {
-      data_int[offset]=servo_ids[i];
-      data_int[offset+1]=start_addr[i]%256;
-      data_int[offset+2]=start_addr[i]/256;
-      data_int[offset+3]=data[i].size()%256;
-      data_int[offset+4]=data[i].size()/256;
-      offset+=5+data[i].size();
+    try{
+      length=0;
+      for(i=0;i<servo_ids.size();i++)
+        length+=5+data[i].size();
+      data_int=new unsigned char[length];
+      for(i=0;i<servo_ids.size();i++)
+      {
+        data_int[offset]=servo_ids[i];
+        data_int[offset+1]=start_addr[i]%256;
+        data_int[offset+2]=start_addr[i]/256;
+        data_int[offset+3]=data[i].size()%256;
+        data_int[offset+4]=data[i].size()/256;
+        for(j=0;j<data[i].size();j++)
+          data_int[offset+5+j]=data[i][j];
+        offset+=5+data[i].size();
+      }
+      this->send_instruction_packet_v2(dyn_bulk_write,data_int,length);
+      delete[] data_int;
+    }catch(CException &e){
+      if(data_int!=NULL)
+      {
+        delete[] data_int;
+        data_int=NULL;
+      }
+      throw e;
     }
-    this->send_instruction_packet_v2(dyn_sync_read,data_int,length);
   }
 }
 
 void CDynamixelServer::read_bulk(std::vector<unsigned char>& servo_ids,std::vector<unsigned short int> &start_addr,std::vector<unsigned short int> &length, std::vector< std::vector<unsigned char> >& data,dyn_version_t version)
 {
-  unsigned char *data_int,*data_out,error;
+  unsigned char *data_int=NULL,*data_out=NULL,error;
   unsigned short int length_v2;
   unsigned int i,j=0;
   int length_int=0;
@@ -1241,35 +1298,54 @@ void CDynamixelServer::read_bulk(std::vector<unsigned char>& servo_ids,std::vect
   }
   else
   {
-    length_int=5*servo_ids.size();
-    data_int=new unsigned char[length_int];
-    for(i=0;i<servo_ids.size();i++)
-    {
-      data_int[i*5]=servo_ids[i];
-      data_int[i*5+1]=start_addr[i]%256;
-      data_int[i*5+2]=start_addr[i]/256;
-      data_int[i*5+3]=length[i]%256;
-      data_int[i*5+4]=length[i]/256;
-    }
-    this->send_instruction_packet_v2(dyn_sync_read,data_int,length_int);
-    /* read all the data from all the servos */
-    data.resize(servo_ids.size());
-    for(i=0;i<servo_ids.size();i++)
-    {
-      error=this->receive_status_packet_v2(&data_out,&length_v2);
-      try{
-        this->handle_error(error);
-        if(data_out!=NULL)
-        {
-          data[i].resize(length_v2);
-          for(j=0;j<length_v2;j++)
-            data[i][j]=data_out[j];
-          delete[] data_out;
+    try{
+      length_int=5*servo_ids.size();
+      data_int=new unsigned char[length_int];
+      for(i=0;i<servo_ids.size();i++)
+      {
+        data_int[i*5]=servo_ids[i];
+        data_int[i*5+1]=start_addr[i]%256;
+        data_int[i*5+2]=start_addr[i]/256;
+        data_int[i*5+3]=length[i]%256;
+        data_int[i*5+4]=length[i]/256;
+      }
+      this->send_instruction_packet_v2(dyn_bulk_read,data_int,length_int);
+      /* read all the data from all the servos */
+      if(data_int!=NULL)
+      {
+        delete[] data_int;
+        data_int=NULL;
+      }
+      data.resize(servo_ids.size());
+      for(i=0;i<servo_ids.size();i++)
+      {
+        error=this->receive_status_packet_v2(&data_out,&length_v2);
+        try{
+          this->handle_error(error);
+          if(data_out!=NULL)
+          {
+            data[i].resize(length_v2);
+            for(j=0;j<length_v2;j++)
+              data[i][j]=data_out[j];
+            delete[] data_out;
+            data_out=NULL;
+          }
+        }catch(CException &e){
+          if(data_out!=NULL)
+          {
+            delete[] data_out;
+            data_out=NULL;
+          }
+          throw e;
         }
-      }catch(CException &e){
-        if(data_out!=NULL)
-          delete[] data_out;
       }
+    }catch(CException &e){
+      if(data_int!=NULL)
+      {
+        delete[] data_int;
+        data_int=NULL;
+      }
+      throw e;
     }
   }
 }

src/examples/test_dynamixel_server.cpp

@@ -6,21 +6,25 @@ int main(int argc, char *argv[])
 {
   CDynamixelServer *dyn_server=CDynamixelServer::instance();
   CEventServer *event_server=CEventServer::instance();
-  int num_buses=0,num_dev=0,baudrate=0,i=0,event_id;
+  int num_buses=0,num_dev=0,baudrate=0,i=0,event_id,j;
   std::list<std::string> events;
   std::vector<float> position;
   std::vector<int> devices;
   unsigned short int model;
   unsigned char margin;
   CDynamixel *dyn_motor;
+  std::vector<unsigned char> servo_ids;
+  std::vector< std::vector<unsigned char> > data;
+  std::vector<unsigned short int> address,length;
 
   num_buses=dyn_server->get_num_buses();
   std::cout << "Num. buses: " << num_buses << std::endl;
   if(num_buses>0)
   {
+    try{
     events.push_back(dyn_server->get_scan_done_event_id());
     events.push_back(dyn_server->get_scan_error_event_id());
-    dyn_server->config_bus(0,1000000);
+/*    dyn_server->config_bus(0,1000000);
     dyn_server->start_scan();
     event_id=event_server->wait_first(events);
     if(event_id==0)
@@ -43,17 +47,38 @@ int main(int argc, char *argv[])
         std::cout << "Device " << std::dec << i << ": id -> " << devices[i] << " model: " << std::hex << model << " margin: " << (int)margin << std::endl;
         dyn_server->free_device(devices[i]);
       }
+      // sync write operation
+      std::cout << "sync write" << std::endl;
+      data.resize(num_dev);
+      servo_ids.resize(num_dev);
+      for(i=0;i<num_dev;i++)
+      {
+        servo_ids[i]=devices[i];
+        data[i].clear();
+        data[i].push_back(0xFF);
+        data[i].push_back(0x03);
+      }
+      dyn_server->write_sync(servo_ids,0x1E,data,dyn_version1);
+      sleep(2);
+      for(i=0;i<num_dev;i++)
+      {
+        servo_ids[i]=devices[i];
+        data[i].clear();
+        data[i].push_back(0x00);
+        data[i].push_back(0x00);
+      }
+      dyn_server->write_sync(servo_ids,0x1E,data,dyn_version1);
     }
     else
-      std::cout << "Error while scanning the bus: " << dyn_server->get_scan_error() << std::endl;
-    try{
+      std::cout << "Error while scanning the bus: " << dyn_server->get_scan_error() << std::endl;*/
+/*    try{
       dyn_server->config_bus(0,1000000);
       dyn_server->start_scan();
       event_server->wait_first(events,2000);
     }catch(CException &e){
       dyn_server->stop_scan();
       std::cout << "Scanning canceled !!!" << std::endl;
-    }
+    }*/
     /* scan for version 2 devices */
     dyn_server->config_bus(0,1000000);
     dyn_server->start_scan(dyn_version2);
@@ -71,15 +96,52 @@ int main(int argc, char *argv[])
         dyn_motor->write_byte_register(0x1A,0x10);
         dyn_motor->read_word_register(0x00,&model);
         dyn_motor->read_byte_register(0x1A,&margin);
-        dyn_motor->write_word_register(0x1E,0x03FF);
-        sleep(2);
-        dyn_motor->write_word_register(0x1E,0x000);
-        sleep(2);
+        //dyn_motor->write_word_register(0x1E,0x03FF);
+        //sleep(2);
+        //dyn_motor->write_word_register(0x1E,0x000);
+        //sleep(2);
         std::cout << "Device " << std::dec << i << ": id -> " << devices[i] << " model: " << std::hex << model << " margin: " << (int)margin << std::endl;
         dyn_server->free_device(devices[i],dyn_version2);
       }
+      /* sync read operation */
+      servo_ids.resize(num_dev);
+      for(i=0;i<num_dev;i++)
+        servo_ids[i]=devices[i];
+      std::cout << "sync read" << std::endl;
+      dyn_server->read_sync(servo_ids,0x1B,3,data,dyn_version2);
+      for(i=0;i<data.size();i++)
+      {
+        std::cout << "Servo " << (int)servo_ids[i] << " PID" << std::endl;
+        for(j=0;j<data[i].size();j++)
+          std::cout << std::hex << "\t" << (int)data[i][j] << std::endl;
+      }
+      /* bulk write */
+      address.resize(num_dev);
+      length.resize(num_dev);
+      for(i=0;i<num_dev;i++)
+      {
+        address[i]=0x1B;
+        data[i][0]=0xFF;
+        data[i][1]=0xFF;
+        data[i][2]=0xFD;
+        length[i]=3;
+      }
+      std::cout << "bulk write" << std::endl;
+      dyn_server->write_bulk(servo_ids,address,data,dyn_version2);
+      std::cout << "bulk read" << std::endl;
+      data.clear();
+      dyn_server->read_bulk(servo_ids,address,length,data,dyn_version2);
+      for(i=0;i<data.size();i++)
+      {
+        std::cout << "Servo " << (int)servo_ids[i] << " PID" << std::endl;
+        for(j=0;j<data[i].size();j++)
+          std::cout << std::hex << "\t" << (int)data[i][j] << std::endl;
+      }
     }
     else
       std::cout << "Error while scanning the bus: " << dyn_server->get_scan_error() << std::endl;
+    }catch(CException &e){
+      std::cout << e.what() << std::endl;
+    }
   }
 }