BRSRI/bellande_humanoid_robot_intelligence_control_system_framework
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

added serivce for setting module

Browse Source
This commit is contained in:
Kayman
2018-03-14 15:16:16 +09:00
parent 0076da8a98
commit 5e9278ee51
2 changed files with 57 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
robotis_controller/include/robotis_controller/robotis_controller.h
View File

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

55
robotis_controller/src/robotis_controller/robotis_controller.cpp
View File

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