- Modified to prevent duplicate indirect address write

Author: s-changhyun

robotis_controller/src/robotis_controller/robotis_controller.cpp

@@ -485,6 +485,8 @@ void RobotisController::initializeDevice(const std::string init_file_path)
     {
       if (dxl->bulk_read_items_.size() != 0)
       {
+        uint16_t  data16 = 0;
+
         bulkread_start_addr = dxl->bulk_read_items_[0]->address_;
         bulkread_data_length = 0;
 
@@ -498,7 +500,11 @@ void RobotisController::initializeDevice(const std::string init_file_path)
           for (int l = 0; l < addr_leng; l++)
           {
 //            ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", joint_name.c_str(), indirect_addr, dxl->ctrl_table[dxl->bulk_read_items[i]->item_name]->address + _l);
-            write2Byte(joint_name, indirect_addr, dxl->ctrl_table_[dxl->bulk_read_items_[i]->item_name_]->address_ + l);
+            read2Byte(joint_name, indirect_addr, &data16);
+            if (data16 != dxl->ctrl_table_[dxl->bulk_read_items_[i]->item_name_]->address_ + l)
+            {
+              write2Byte(joint_name, indirect_addr, dxl->ctrl_table_[dxl->bulk_read_items_[i]->item_name_]->address_ + l);
+            }
             indirect_addr += 2;
           }
         }
@@ -552,6 +558,8 @@ void RobotisController::initializeDevice(const std::string init_file_path)
     {
       if (sensor->bulk_read_items_.size() != 0)
       {
+        uint16_t  data16 = 0;
+
         bulkread_start_addr = sensor->bulk_read_items_[0]->address_;
         bulkread_data_length = 0;
 
@@ -565,9 +573,13 @@ void RobotisController::initializeDevice(const std::string init_file_path)
           for (int l = 0; l < addr_leng; l++)
           {
 //            ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", sensor_name.c_str(), indirect_addr, sensor->ctrl_table[sensor->bulk_read_items[i]->item_name]->address + _l);
-            write2Byte(sensor_name,
-                       indirect_addr,
-                       sensor->ctrl_table_[sensor->bulk_read_items_[i]->item_name_]->address_ + l);
+            read2Byte(sensor_name, indirect_addr, &data16);
+            if (data16 != sensor->ctrl_table_[sensor->bulk_read_items_[i]->item_name_]->address_ + l)
+            {
+              write2Byte(sensor_name,
+                         indirect_addr,
+                         sensor->ctrl_table_[sensor->bulk_read_items_[i]->item_name_]->address_ + l);
+            }
             indirect_addr += 2;
           }
         }