Merge pull request #4 from ROBOTIS-GIT/hotfix-indirect-check

Extend Bulk/Sync Selection Logic to Include Indirect Operations

Author: sunghowoo

src/dynamixel/dynamixel.cpp

@@ -658,6 +658,22 @@ DxlError Dynamixel::WriteMultiDxlData()
 bool Dynamixel::checkReadType()
 {
   for (size_t dxl_index = 1; dxl_index < read_data_list_.size(); dxl_index++) {
+    // Check if Indirect Data Read address and size are different
+    uint16_t indirect_addr[2];  // [i-1], [i]
+    uint8_t indirect_size[2];   // [i-1], [i]
+    
+    if (!dxl_info_.GetDxlControlItem(
+      read_data_list_.at(dxl_index).id, "Indirect Data Read", indirect_addr[1], indirect_size[1]) ||
+      !dxl_info_.GetDxlControlItem(
+      read_data_list_.at(dxl_index - 1).id, "Indirect Data Read", indirect_addr[0], indirect_size[0]))
+    {
+      return BULK;
+    }
+    
+    if (indirect_addr[1] != indirect_addr[0] || indirect_size[1] != indirect_size[0]) {
+      return BULK;
+    }
+
     if (read_data_list_.at(dxl_index).item_name.size() !=
       read_data_list_.at(dxl_index - 1).item_name.size())
     {
@@ -667,7 +683,11 @@ bool Dynamixel::checkReadType()
       item_index++)
     {
       if (read_data_list_.at(dxl_index).item_name.at(item_index) !=
-        read_data_list_.at(dxl_index - 1).item_name.at(item_index))
+        read_data_list_.at(dxl_index - 1).item_name.at(item_index) || 
+        read_data_list_.at(dxl_index).item_addr.at(item_index) !=
+        read_data_list_.at(dxl_index - 1).item_addr.at(item_index) ||
+        read_data_list_.at(dxl_index).item_size.at(item_index) !=
+        read_data_list_.at(dxl_index - 1).item_size.at(item_index))
       {
         return BULK;
       }
@@ -679,6 +699,22 @@ bool Dynamixel::checkReadType()
 bool Dynamixel::checkWriteType()
 {
   for (size_t dxl_index = 1; dxl_index < write_data_list_.size(); dxl_index++) {
+    // Check if Indirect Data Write address and size are different
+    uint16_t indirect_addr[2];  // [i-1], [i]
+    uint8_t indirect_size[2];   // [i-1], [i]
+    
+    if (!dxl_info_.GetDxlControlItem(
+      write_data_list_.at(dxl_index).id, "Indirect Data Write", indirect_addr[1], indirect_size[1]) ||
+      !dxl_info_.GetDxlControlItem(
+      write_data_list_.at(dxl_index - 1).id, "Indirect Data Write", indirect_addr[0], indirect_size[0]))
+    {
+      return BULK;
+    }
+    
+    if (indirect_addr[1] != indirect_addr[0] || indirect_size[1] != indirect_size[0]) {
+      return BULK;
+    }
+
     if (write_data_list_.at(dxl_index).item_name.size() !=
       write_data_list_.at(dxl_index - 1).item_name.size())
     {
@@ -688,7 +724,12 @@ bool Dynamixel::checkWriteType()
       item_index++)
     {
       if (write_data_list_.at(dxl_index).item_name.at(item_index) !=
-        write_data_list_.at(dxl_index - 1).item_name.at(item_index))
+        write_data_list_.at(dxl_index - 1).item_name.at(item_index) ||
+        write_data_list_.at(dxl_index).item_addr.at(item_index) !=
+        write_data_list_.at(dxl_index - 1).item_addr.at(item_index) ||
+        write_data_list_.at(dxl_index).item_size.at(item_index) !=
+        write_data_list_.at(dxl_index - 1).item_size.at(item_index)
+        )
       {
         return BULK;
       }
@@ -870,11 +911,12 @@ DxlError Dynamixel::SetBulkReadHandler(std::vector<uint8_t> id_arr)
     }
     // Set indirect addr.
     indirect_info_read_[it_id].indirect_data_addr = IN_ADDR;
+
+    fprintf(
+      stderr,
+      "set bulk read (indirect addr) : addr %d, size %d\n",
+      IN_ADDR, indirect_info_read_[id_arr.at(0)].size);
   }
-  fprintf(
-    stderr,
-    "set bulk read (indirect addr) : addr %d, size %d\n",
-    IN_ADDR, indirect_info_read_[id_arr.at(0)].size);
 
   group_bulk_read_ = new dynamixel::GroupBulkRead(port_handler_, packet_handler_);
 
@@ -1130,30 +1172,29 @@ DxlError Dynamixel::SetBulkWriteItemAndHandler()
 
 DxlError Dynamixel::SetBulkWriteHandler(std::vector<uint8_t> id_arr)
 {
-  uint16_t INDIRECT_ADDR = 0;
-  uint8_t INDIRECT_SIZE;
+  uint16_t IN_ADDR = 0;
+  uint8_t IN_SIZE = 0;
 
   for (auto it_id : id_arr) {
     // Get the indirect addr.
-    if (dxl_info_.GetDxlControlItem(
-        it_id, "Indirect Data Write", INDIRECT_ADDR,
-        INDIRECT_SIZE) == false)
-    {
+    if (dxl_info_.GetDxlControlItem(it_id, "Indirect Data Write", IN_ADDR, IN_SIZE) == false) {
       fprintf(
         stderr,
         "Fail to set indirect address bulk write. "
         "the dxl unincluding indirect address in control table are being used.\n");
       return DxlError::SET_BULK_WRITE_FAIL;
     }
     // Set indirect addr.
-    indirect_info_write_[it_id].indirect_data_addr = INDIRECT_ADDR;
+    indirect_info_write_[it_id].indirect_data_addr = IN_ADDR;
+
+    fprintf(
+      stderr,
+      "set bulk write (indirect addr) : addr %d, size %d\n",
+      IN_ADDR, indirect_info_write_[id_arr.at(0)].size);
   }
-  fprintf(
-    stderr,
-    "set bulk write (indirect addr) : addr %d, size %d\n",
-    INDIRECT_ADDR, indirect_info_write_[id_arr.at(0)].size);
 
   group_bulk_write_ = new dynamixel::GroupBulkWrite(port_handler_, packet_handler_);
+
   return DxlError::OK;
 }
 

src/dynamixel_hardware_interface.cpp

@@ -93,7 +93,7 @@ hardware_interface::CallbackReturn DynamixelHardware::on_init(
     } else if (gpio.parameters.at("type") == "sensor") {
       sensor_id_.push_back(static_cast<uint8_t>(stoi(gpio.parameters.at("ID"))));
     } else {
-      RCLCPP_ERROR_STREAM(logger_, "Invalid DXL / Sensoe type");
+      RCLCPP_ERROR_STREAM(logger_, "Invalid DXL / Sensor type");
       exit(-1);
     }
   }