From 0076da8a98737272f8a8147cc8a493f6b145eb12 Mon Sep 17 00:00:00 2001
From: ROBOTIS-zerom <zerom@robotis.com>
Date: Thu, 30 Nov 2017 17:14:28 +0900
Subject: [PATCH] - modified to prevent duplicate indirect address write -
 fixed a bug that occure when handling bulk read item that does not exist.

---
 .../robotis_controller/robotis_controller.cpp | 20 +++++++++++++++----
 robotis_device/src/robotis_device/robot.cpp   |  7 +++++++
 2 files changed, 23 insertions(+), 4 deletions(-)

diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp
index be70a4e..691df50 100755
--- a/robotis_controller/src/robotis_controller/robotis_controller.cpp
+++ b/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;
           }
         }
diff --git a/robotis_device/src/robotis_device/robot.cpp b/robotis_device/src/robotis_device/robot.cpp
index 94d4133..bcbda3a 100755
--- a/robotis_device/src/robotis_device/robot.cpp
+++ b/robotis_device/src/robotis_device/robot.cpp
@@ -154,6 +154,13 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)
               uint16_t indirect_data_addr = dxl->ctrl_table_[INDIRECT_DATA_1]->address_;
               for (int _i = 0; _i < sub_tokens.size(); _i++)
               {
+                std::map<std::string, ControlTableItem *>::iterator bulkread_it = dxl->ctrl_table_.find(sub_tokens[_i]);
+                if(bulkread_it == dxl->ctrl_table_.end())
+                {
+                  fprintf(stderr, "\n  ##### BULK READ ITEM [ %s ] NOT FOUND!! #####\n\n", sub_tokens[_i].c_str());
+                  continue;
+                }
+
                 dxl->bulk_read_items_.push_back(new ControlTableItem());
                 ControlTableItem *dest_item = dxl->bulk_read_items_[_i];
                 ControlTableItem *src_item  = dxl->ctrl_table_[sub_tokens[_i]];