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

- changed enable_offset from service to topic.

Browse Source
This commit is contained in:
zerom
2018-05-31 14:21:01 +09:00
parent bc20638cd1
commit b9ed3fecf2
2 changed files with 9 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
robotis_controller/include/robotis_controller/robotis_controller.h
View File

@ -28,6 +28,7 @@
#include <ros/ros.h>
#include <boost/thread.hpp>
#include <yaml-cpp/yaml.h>
#include <std_msgs/Bool.h>
#include <std_msgs/String.h>
#include <std_msgs/Float64.h>
#include <sensor_msgs/JointState.h>
@ -38,7 +39,6 @@
#include "robotis_controller_msgs/GetJointModule.h"
#include "robotis_controller_msgs/SetJointModule.h"
#include "robotis_controller_msgs/SetModule.h"
#include "robotis_controller_msgs/EnableOffset.h"
#include "robotis_controller_msgs/LoadOffset.h"
#include "robotis_device/robot.h"
@ -142,10 +142,10 @@ 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);
void enableOffsetCallback(const std_msgs::Bool::ConstPtr &msg);
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);
bool enableOffsetService(robotis_controller_msgs::EnableOffset::Request &req, robotis_controller_msgs::EnableOffset::Response &res);
bool loadOffsetService(robotis_controller_msgs::LoadOffset::Request &req, robotis_controller_msgs::LoadOffset::Response &res);
void gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg);

16
robotis_controller/src/robotis_controller/robotis_controller.cpp
View File

@ -636,6 +636,8 @@ void RobotisController::msgQueueThread()
&RobotisController::setControllerModeCallback, this);
ros::Subscriber joint_states_sub = ros_node.subscribe("/robotis/set_joint_states", 10,
&RobotisController::setJointStatesCallback, this);
ros::Subscriber enable_offset_sub = ros_node.subscribe("/robotis/enable_offset", 10,
&RobotisController::enableOffsetCallback, this);
ros::Subscriber gazebo_joint_states_sub;
if (gazebo_mode_ == true)
@ -668,8 +670,6 @@ void RobotisController::msgQueueThread()
&RobotisController::setJointCtrlModuleService, this);
ros::ServiceServer set_module_server = ros_node.advertiseService("/robotis/set_present_ctrl_modules",
&RobotisController::setCtrlModuleService, this);
ros::ServiceServer enable_offset_server = ros_node.advertiseService("/robotis/enable_offset",
&RobotisController::enableOffsetService, this);
ros::ServiceServer load_offset_server = ros_node.advertiseService("/robotis/load_offset",
&RobotisController::loadOffsetService, this);
@ -1735,6 +1735,11 @@ void RobotisController::setJointCtrlModuleCallback(const robotis_controller_msgs
set_module_thread_ = boost::thread(boost::bind(&RobotisController::setJointCtrlModuleThread, this, msg));
}
void RobotisController::enableOffsetCallback(const std_msgs::Bool::ConstPtr &msg)
{
is_offset_enabled_ = (bool)msg->data;
}
bool RobotisController::getJointCtrlModuleService(robotis_controller_msgs::GetJointModule::Request &req,
robotis_controller_msgs::GetJointModule::Response &res)
{
@ -1790,13 +1795,6 @@ bool RobotisController::setCtrlModuleService(robotis_controller_msgs::SetModule:
return true;
}
bool RobotisController::enableOffsetService(robotis_controller_msgs::EnableOffset::Request &req, robotis_controller_msgs::EnableOffset::Response &res)
{
is_offset_enabled_ = (bool)req.enable;
res.result = true;
return true;
}
bool RobotisController::loadOffsetService(robotis_controller_msgs::LoadOffset::Request &req, robotis_controller_msgs::LoadOffset::Response &res)
{
loadOffset((std::string)req.file_path);