Added a function to resynchronize the serial communication when some data is missing.

src/dynamixel.cpp

@@ -71,6 +71,36 @@ void CDynamixel::set_id(unsigned char id)
   }
 }
 
+void CDynamixel::resync(void)
+{
+  std::list<std::string> events;
+  unsigned char foo=0x00,*data;
+  bool synced=false;
+  int num=0,it=0;
+
+  std::cout << "resynchronizing ... ";
+  events.push_back(this->usb_rx_event_id); 
+  // write to the dynamixel bus until it returns a packet
+  do{
+    try{
+      this->usb_dev->write(&foo,1);
+      this->event_server->wait_all(events,500);
+      num=this->usb_dev->get_num_data();
+      data=new unsigned char[num];
+      this->usb_dev->read(data,num);
+      delete[] data;
+      synced=true;
+    }catch(CEventTimeoutException &e){
+      it++;
+      synced=false;
+    }
+  }while(!synced && it<10);
+  if(it<10)
+    std::cout << "synchronized after "<< it << " bytes !!" << std::endl;
+  else
+    std::cout << "impossible to synchronize!!!" << std::endl;
+}
+
 void CDynamixel::read_byte_register(unsigned char address,unsigned char *value)
 {
   unsigned char packet[8]={0xFF,0xFF,0x00,0x04,0x02,0x00,0x00,0x00};
@@ -108,6 +138,7 @@ void CDynamixel::read_byte_register(unsigned char address,unsigned char *value)
           read+=num;
         }
       }catch(CEventTimeoutException &e){
+        this->resync();
         this->usb_access->exit();
         throw;      
       }
@@ -188,6 +219,7 @@ void CDynamixel::read_word_register(unsigned char address,unsigned short int *va
           read+=num;
         }
       }catch(CEventTimeoutException &e){
+        this->resync();
         this->usb_access->exit();
         throw;      
       }
@@ -268,6 +300,7 @@ void CDynamixel::write_byte_register(unsigned char address, unsigned char data)
           read+=num;
         }
       }catch(CEventTimeoutException &e){
+        this->resync();
         this->usb_access->exit();
         throw;      
       }
@@ -348,6 +381,7 @@ void CDynamixel::write_word_register(unsigned char address, unsigned short int d
           read+=num;
         }
       }catch(CEventTimeoutException &e){
+        this->resync();
         this->usb_access->exit();
         throw;      
       }
@@ -427,6 +461,7 @@ void CDynamixel::registered_byte_write(unsigned char address, unsigned char data
           read+=num;
         }
       }catch(CEventTimeoutException &e){
+        this->resync();
         this->usb_access->exit();
         throw;      
       }
@@ -507,6 +542,7 @@ void CDynamixel::registered_word_write(unsigned char address, unsigned short int
           read+=num;
         }
       }catch(CEventTimeoutException &e){
+        this->resync();
         this->usb_access->exit();
         throw;      
       }

src/dynamixel.h

@@ -68,6 +68,11 @@ class CDynamixel
      *
      */
     void set_id(unsigned char id);
+    /**
+     * \brief
+     *
+     */
+    void resync(void);
   public:
     /**
      * \brief