Merge pull request #58 from ROBOTIS-GIT/develop

Develop

Author: kaym9n

robotis_controller/include/robotis_controller/robotis_controller.h

@@ -36,6 +36,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"
@@ -137,7 +139,9 @@ class RobotisController : public Singleton<RobotisController>
   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);
 

robotis_controller/src/robotis_controller/robotis_controller.cpp

@@ -485,7 +485,8 @@ 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);
+            // 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)
             {
@@ -657,8 +658,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())
@@ -1663,24 +1668,33 @@ 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));
 }
 
 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)
 {
   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++)
@@ -1699,6 +1713,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
@@ -1873,14 +1922,6 @@ void RobotisController::setJointCtrlModuleThread(const robotis_controller_msgs::
 
   queue_mutex_.unlock();
 
-  // log
-//  std::cout << "Enable Joint Ctrl Module : " << std::endl;
-//  for(std::list<MotionModule *>::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<std::string, Dynamixel *>::iterator _dxl_iter = robot_->dxls_.begin(); _dxl_iter  != robot_->dxls_.end(); ++_dxl_iter)
@@ -1892,6 +1933,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