From 0076da8a98737272f8a8147cc8a493f6b145eb12 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Thu, 30 Nov 2017 17:14:28 +0900 Subject: [PATCH 1/3] - 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::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]]; From 5e9278ee51dc325e1be1659f2576ec5d07780b6d Mon Sep 17 00:00:00 2001 From: Kayman Date: Wed, 14 Mar 2018 15:16:16 +0900 Subject: [PATCH 2/3] added serivce for setting module --- .../robotis_controller/robotis_controller.h | 6 +- .../robotis_controller/robotis_controller.cpp | 55 ++++++++++++++++++- 2 files changed, 57 insertions(+), 4 deletions(-) diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h index f811b24..4653aac 100755 --- a/robotis_controller/include/robotis_controller/robotis_controller.h +++ b/robotis_controller/include/robotis_controller/robotis_controller.h @@ -50,6 +50,8 @@ #include "robotis_controller_msgs/SyncWriteItem.h" #include "robotis_controller_msgs/JointCtrlModule.h" #include "robotis_controller_msgs/GetJointModule.h" +#include "robotis_controller_msgs/SetJointModule.h" +#include "robotis_controller_msgs/SetModule.h" #include "robotis_device/robot.h" #include "robotis_framework_common/motion_module.h" @@ -151,7 +153,9 @@ public: void setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); void setJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg); void setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg); - bool getCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res); + bool getJointCtrlModuleService(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res); + bool setJointCtrlModuleService(robotis_controller_msgs::SetJointModule::Request &req, robotis_controller_msgs::SetJointModule::Response &res); + bool setCtrlModuleService(robotis_controller_msgs::SetModule::Request &req, robotis_controller_msgs::SetModule::Response &res); void gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 691df50..f53cb94 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -671,8 +671,12 @@ void RobotisController::msgQueueThread() } /* service */ - ros::ServiceServer joint_module_server = ros_node.advertiseService("/robotis/get_present_joint_ctrl_modules", - &RobotisController::getCtrlModuleCallback, this); + ros::ServiceServer get_joint_module_server = ros_node.advertiseService("/robotis/get_present_joint_ctrl_modules", + &RobotisController::getJointCtrlModuleService, this); + ros::ServiceServer set_joint_module_server = ros_node.advertiseService("/robotis/set_present_joint_ctrl_modules", + &RobotisController::setJointCtrlModuleService, this); + ros::ServiceServer set_module_server = ros_node.advertiseService("/robotis/set_present_ctrl_modules", + &RobotisController::setCtrlModuleService, this); ros::WallDuration duration(robot_->getControlCycle() / 1000.0); while(ros_node.ok()) @@ -1677,6 +1681,9 @@ void RobotisController::setJointStatesCallback(const sensor_msgs::JointState::Co void RobotisController::setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg) { + if(set_module_thread_.joinable()) + set_module_thread_.join(); + std::string _module_name_to_set = msg->data; set_module_thread_ = boost::thread(boost::bind(&RobotisController::setCtrlModuleThread, this, _module_name_to_set)); @@ -1684,6 +1691,9 @@ void RobotisController::setCtrlModuleCallback(const std_msgs::String::ConstPtr & void RobotisController::setCtrlModule(std::string module_name) { + if(set_module_thread_.joinable()) + set_module_thread_.join(); + set_module_thread_ = boost::thread(boost::bind(&RobotisController::setCtrlModuleThread, this, module_name)); } void RobotisController::setJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg) @@ -1691,10 +1701,13 @@ void RobotisController::setJointCtrlModuleCallback(const robotis_controller_msgs if (msg->joint_name.size() != msg->module_name.size()) return; + if(set_module_thread_.joinable()) + set_module_thread_.join(); + set_module_thread_ = boost::thread(boost::bind(&RobotisController::setJointCtrlModuleThread, this, msg)); } -bool RobotisController::getCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req, +bool RobotisController::getJointCtrlModuleService(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res) { for (unsigned int idx = 0; idx < req.joint_name.size(); idx++) @@ -1713,6 +1726,41 @@ bool RobotisController::getCtrlModuleCallback(robotis_controller_msgs::GetJointM return true; } +bool RobotisController::setJointCtrlModuleService(robotis_controller_msgs::SetJointModule::Request &req, robotis_controller_msgs::SetJointModule::Response &res) +{ + if(set_module_thread_.joinable()) + set_module_thread_.join(); + + robotis_controller_msgs::JointCtrlModule modules; + modules.joint_name = req.joint_name; + modules.module_name = req.module_name; + + robotis_controller_msgs::JointCtrlModule::ConstPtr msg_ptr(new robotis_controller_msgs::JointCtrlModule(modules)); + + if (modules.joint_name.size() != modules.module_name.size()) + return false; + + set_module_thread_ = boost::thread(boost::bind(&RobotisController::setJointCtrlModuleThread, this, msg_ptr)); + + set_module_thread_.join(); + + return true; +} + +bool RobotisController::setCtrlModuleService(robotis_controller_msgs::SetModule::Request &req, robotis_controller_msgs::SetModule::Response &res) +{ + if(set_module_thread_.joinable()) + set_module_thread_.join(); + + std::string _module_name_to_set = req.module_name; + + set_module_thread_ = boost::thread(boost::bind(&RobotisController::setCtrlModuleThread, this, _module_name_to_set)); + + set_module_thread_.join(); + + return true; +} + void RobotisController::setJointCtrlModuleThread(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg) { // stop module list @@ -1906,6 +1954,7 @@ void RobotisController::setJointCtrlModuleThread(const robotis_controller_msgs:: if(_current_module_msg.joint_name.size() == _current_module_msg.module_name.size()) current_module_pub_.publish(_current_module_msg); } + void RobotisController::setCtrlModuleThread(std::string ctrl_module) { // stop module From 04f04b06433280f40a1db26fa8616c95306df598 Mon Sep 17 00:00:00 2001 From: Kayman Date: Wed, 21 Mar 2018 19:03:11 +0900 Subject: [PATCH 3/3] deleted comment for debug --- .../src/robotis_controller/robotis_controller.cpp | 8 -------- 1 file changed, 8 deletions(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index f53cb94..921189c 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -1935,14 +1935,6 @@ void RobotisController::setJointCtrlModuleThread(const robotis_controller_msgs:: queue_mutex_.unlock(); - // log -// std::cout << "Enable Joint Ctrl Module : " << std::endl; -// for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) -// { -// if((*_m_it)->GetModuleEnable() == true) -// std::cout << " - " << (*_m_it)->GetModuleName() << std::endl; -// } - // publish current module robotis_controller_msgs::JointCtrlModule _current_module_msg; for(std::map::iterator _dxl_iter = robot_->dxls_.begin(); _dxl_iter != robot_->dxls_.end(); ++_dxl_iter)