From 5507b4bf844bb0a275de7cbea55b890fa39619de Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Wed, 15 May 2024 10:03:51 +0200
Subject: [PATCH] Solved a bug in the dynamixel v2.0 protocol for the bulk read
 operation.

---
 src/dynamixelserver.cpp | 144 ++++++++++++++++++++--------------------
 1 file changed, 72 insertions(+), 72 deletions(-)

diff --git a/src/dynamixelserver.cpp b/src/dynamixelserver.cpp
index a38ae2b..f8ea023 100644
--- a/src/dynamixelserver.cpp
+++ b/src/dynamixelserver.cpp
@@ -279,61 +279,61 @@ void CDynamixelServer::receive_sync_bulk_status_packets_v1(std::vector<unsigned
       // read up to the length field
       for(unsigned int i=0;i<servo_ids.size();)
       {
-	if(need_data)
+        if(need_data)
         {
           this->event_server->wait_all(events,100);
           new_data=this->comm_dev->get_num_data();
           this->comm_dev->read(&data_int[total],new_data);
-	  total+=new_data;
-	  num+=new_data;
-	  need_data=false;
+          total+=new_data;
+          num+=new_data;
+          need_data=false;
         }
-	if(length==-1)
-	{
-	  if(num>=4)
-	  {
-	    if(data_int[read+2]!=servo_ids[i])
+        if(length==-1)
+        {
+          if(num>=4)
+          {
+            if(data_int[read+2]!=servo_ids[i])
               throw CDynamixelServerException(_HERE_,"Invalid status packet order in Sync/Bulk read");
             length=data_int[read+3]+4;
-	  }
-	  else
-	    need_data=true;
-	}
-	if(length!=-1 && num>=length)
-	{
+          }
+          else
+            need_data=true;
+        }
+        if(length!=-1 && num>=length)
+        {
           if(this->compute_checksum_v1(&data_int[read],length)!=0x00)
           {
             data[i].clear();
-	    read+=length;
-	    num-=length;
-	    length=-1;
-	    i++;
-	    continue;
+            read+=length;
+            num-=length;
+            length=-1;
+            i++;
+            continue;
           }
-	  try{
+          try{
             this->handle_error(data_int[read+4]);
-	  }catch(CException &e){
+          }catch(CException &e){
             data[i].clear();
-	    read+=length;
-	    num-=length;
-	    length=-1;
-	    i++;
-	    continue;
-	  } 
+            read+=length;
+            num-=length;
+            length=-1;
+            i++;
+            continue;
+          } 
           if(length>6)
           {
             data[i].resize(length-6);
-	    for(unsigned j=0;j<(unsigned int)(length-6);j++)
-	      data[i][j]=data_int[read+5+j];
+            for(unsigned j=0;j<(unsigned int)(length-6);j++)
+              data[i][j]=data_int[read+5+j];
           }
           else
             data[i].clear();
-	  read+=length;
-	  num-=length;
-	  length=-1;
-	  i++;
-	}
-	else
+          read+=length;
+          num-=length;
+          length=-1;
+          i++;
+        }
+        else
           need_data=true;
       }
     }catch(CEventTimeoutException &e){
@@ -448,64 +448,64 @@ void CDynamixelServer::receive_sync_bulk_status_packets_v2(std::vector<unsigned
       // read up to the length field
       for(unsigned int i=0;i<servo_ids.size();)
       {
-	if(need_data)
+        if(need_data)
         {
           this->event_server->wait_all(events,100);
           new_data=this->comm_dev->get_num_data();
           this->comm_dev->read(&data_int[total],new_data);
-	  total+=new_data;
-	  num+=new_data;
-	  need_data=false;
+          total+=new_data;
+          num+=new_data;
+          need_data=false;
         }
-	if(length==-1)
-	{
-	  if(num>=7)
-	  {
-	    if(data_int[read+2]!=servo_ids[i])
+        if(length==-1)
+        {
+          if(num>=7)
+          {
+            if(data_int[read+4]!=servo_ids[i])
               throw CDynamixelServerException(_HERE_,"Invalid status packet order in Sync/Bulk read");
             length=data_int[read+5]+data_int[read+6]*256+7;
-	  }
-	  else
-	    need_data=true;
-	}
-	if(length!=-1 && num>=length)
-	{
+          }
+          else
+            need_data=true;
+         }
+         if(length!=-1 && num>=length)
+         {
           // check the checksum
           crc=this->compute_checksum_v2(&data_int[read],length-2);
           if((crc%256)!=data_int[read+length-2] || (crc/256)!=data_int[read+length-1])
           {
             /* handle exceptions */
             data[i].clear();
-	    read+=length;
-	    num-=length;
-	    length=-1;
-	    i++;
-	    continue;
+            read+=length;
+            num-=length;
+            length=-1;
+            i++;
+            continue;
           }
-	  try{
+          try{
             this->handle_error(data_int[read+8]);
-	  }catch(CException &e){
+          }catch(CException &e){
             data[i].clear();
-	    read+=length;
-	    num-=length;
-	    length=-1;
-	    i++;
-	    continue;
-	  }
+            read+=length;
+            num-=length;
+            length=-1;
+            i++;
+            continue;
+          }
           if(length>11)
           {
             data[i].resize(length-11);
-	    for(unsigned j=0;j<(unsigned int)(length-11);j++)
-	      data[i][j]=data_int[read+9+j];
+            for(unsigned j=0;j<(unsigned int)(length-11);j++)
+	            data[i][j]=data_int[read+9+j];
           }
           else
             data[i].clear();
-	  read+=length;
-	  num-=length;
-	  length=-1;
-	  i++;
-	}
-	else
+          read+=length;
+          num-=length;
+          length=-1;
+          i++;
+        }
+        else
           need_data=true;
       }
     }catch(CEventTimeoutException &e){
-- 
GitLab