added serivce for setting module

Author: kaym9n

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 @@ 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

@@ -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,24 +1681,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++)
@@ -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