Merge pull request #54 from ROBOTIS-GIT/master

merge for sync kinetic-devel and master branch

Author: robotpilot

.travis.yml

@@ -0,0 +1,35 @@
+# This config file for Travis CI utilizes ros-industrial/industrial_ci package.
+# For more info for the package, see https://github.com/ros-industrial/industrial_ci/blob/master/README.rst
+
+dist: trusty
+sudo: required
+services:
+  - docker
+language: generic
+python:
+  - "2.7"
+compiler:
+  - gcc
+notifications:
+  email:
+    on_success: always
+    on_failure: always
+    recipients:
+      - [email protected]
+env:
+  matrix:
+    - ROS_DISTRO="kinetic"   ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu  USE_DEB=true
+    - ROS_DISTRO="kinetic"   PRERELEASE=true PRERELEASE_DOWNSTREAM_DEPTH=0
+matrix:
+  allow_failures:
+    - env: ROS_DISTRO="kinetic"   PRERELEASE=true PRERELEASE_DOWNSTREAM_DEPTH=0
+branches:
+  only:
+    - master
+    - develop
+    - kinetic-devel
+install:
+  - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config
+script:
+  - source .ci_config/travis.sh
+  

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;
 
@@ -497,8 +499,12 @@ void RobotisController::initializeDevice(const std::string init_file_path)
           bulkread_data_length += addr_leng;
           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);
+            //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);
+            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;
           }
         }
@@ -934,8 +946,8 @@ void RobotisController::process()
                   dxl->dxl_state_->goal_velocity_ = dxl->convertValue2Velocity(data);
                 else if (dxl->goal_current_item_ != 0 && item->item_name_ == dxl->goal_current_item_->item_name_)
                   dxl->dxl_state_->goal_torque_ = dxl->convertValue2Torque(data);
-                else
-                  dxl->dxl_state_->bulk_read_table_[item->item_name_] = data;
+
+                dxl->dxl_state_->bulk_read_table_[item->item_name_] = data;
               }
             }
 
@@ -1154,8 +1166,8 @@ void RobotisController::process()
                   dxl->dxl_state_->goal_velocity_ = dxl->convertValue2Velocity(data);
                 else if (dxl->goal_current_item_ != 0 && item->item_name_ == dxl->goal_current_item_->item_name_)
                   dxl->dxl_state_->goal_torque_ = dxl->convertValue2Torque(data);
-                else
-                  dxl->dxl_state_->bulk_read_table_[item->item_name_] = data;
+
+                dxl->dxl_state_->bulk_read_table_[item->item_name_] = data;
               }
             }
 
@@ -1239,7 +1251,7 @@ void RobotisController::process()
 
             if (result_state == NULL)
             {
-              fprintf(stderr, "[%s] %s", (*module_it)->getModuleName().c_str(), joint_name.c_str());
+              ROS_ERROR("[%s] %s ", (*module_it)->getModuleName().c_str(), joint_name.c_str());
               continue;
             }
 

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]];