Moved the packet process loop to the reception thread.

src/dynamixel_slave.cpp

@@ -142,7 +142,11 @@ void *CDynamixelSlave::process_packets_thread(void *params)
               new_data=new unsigned char[num_bytes];
               memcpy(new_data,packet,num_bytes);
               slave->packets.push_back(new_data);
-              slave->event_server->set_event(slave->new_packet_available_event_id);
+              if(slave->dyn_ver==dyn_version1)
+                slave->dynamixel_loop_v1();
+              else
+                slave->dynamixel_loop_v2();
+              //slave->event_server->set_event(slave->new_packet_available_event_id);
               num_bytes=0;
             }
           }
@@ -185,7 +189,7 @@ void *CDynamixelSlave::dynamixel_loop_thread(void *params)
 
 void CDynamixelSlave::handle_error(unsigned char error)
 {
-
+  std::cout << "error!!!!!!!!!!" << std::endl;
 }
 
 void CDynamixelSlave::start(void)

src/examples/test_dynamixel_slave.cpp

@@ -13,27 +13,49 @@ unsigned char rx28_servo_data[SERVO_MEMORY_SIZE]={0x1c,0x00,0x00,0x01,0x22,0xFA,
                                                   0x00,0x00,0x78,0x19,0x00,0x00,0x00,0x00,
                                                   0x20,0x00};
 
-std::string serial="A400gavm";
+std::string serial="AI027ZFC";
 
-void on_ping(void)
+void on_ping_03(void)
 {
-  std::cout << "device pinged" << std::endl;
+  std::cout << "servo 3 pinged" << std::endl;
 }
 
-unsigned char on_read(unsigned short int address, unsigned short int length, unsigned char *data)
+unsigned char on_read_03(unsigned short int address, unsigned short int length, unsigned char *data)
 {
-  unsigned char num;
+  std::cout << "servo 3: read operation at address " << address << " with length " << length << std::endl;
+  for(unsigned int i=address,num=0;i<address+length || i<SERVO_MEMORY_SIZE;i++,num++)
+    data[num]=rx28_servo_data[i];
+
+  return 0x00;
+}
 
-  std::cout << "read operation at address " << address << " with length " << length << std::endl;
+unsigned char on_write_03(unsigned short int address, unsigned short int length, unsigned char *data)
+{
+  std::cout << "servo 3: wrirte operation at address " << address << " with length " << length << std::endl;
+
+  for(unsigned int i=address,num=0;i<address+length || i<SERVO_MEMORY_SIZE;i++,num++)
+    rx28_servo_data[i]=data[num];
+
+  return 0x00;
+}
+
+void on_ping_01(void)
+{
+  std::cout << "servo 1 pinged" << std::endl;
+}
+
+unsigned char on_read_01(unsigned short int address, unsigned short int length, unsigned char *data)
+{
+  std::cout << "servo 1: read operation at address " << address << " with length " << length << std::endl;
   for(unsigned int i=address,num=0;i<address+length || i<SERVO_MEMORY_SIZE;i++,num++)
     data[num]=rx28_servo_data[i];
 
   return 0x00;
 }
 
-unsigned char on_write(unsigned short int address, unsigned short int length, unsigned char *data)
+unsigned char on_write_01(unsigned short int address, unsigned short int length, unsigned char *data)
 {
-  std::cout << "wrirte operation at address " << address << " with length " << length << std::endl;
+  std::cout << "servo 1: wrirte operation at address " << address << " with length " << length << std::endl;
 
   for(unsigned int i=address,num=0;i<address+length || i<SERVO_MEMORY_SIZE;i++,num++)
     rx28_servo_data[i]=data[num];
@@ -51,8 +73,9 @@ int main(int argc, char *argv[])
     std::cout << "Num. buses: " << num_buses << std::endl;
     if(num_buses>0)
     {
-      slave.config_bus(serial,1000000);
-      slave.add_slave(0x10,boost::bind(&on_ping),boost::bind(&on_read,_1,_2,_3),boost::bind(&on_write,_1,_2,_3));
+      slave.config_bus(serial,500000);
+      slave.add_slave(0x01,boost::bind(&on_ping_01),boost::bind(&on_read_01,_1,_2,_3),boost::bind(&on_write_01,_1,_2,_3));
+      slave.add_slave(0x03,boost::bind(&on_ping_03),boost::bind(&on_read_03,_1,_2,_3),boost::bind(&on_write_03,_1,_2,_3));
       sleep(10000);
       std::cout << "exit" << std::endl;
     }