Skip to content

Commit

Permalink
Merge pull request #59 from ROBOTIS-GIT/master
Browse files Browse the repository at this point in the history 
merge for sync kinetic-devel and master branch
  • Loading branch information
@robotpilot
robotpilot authored Mar 22, 2018
2 parents f44c58a + f4473aa commit 45f57c8
Show file tree
Hide file tree
Showing 2 changed files with 59 additions and 13 deletions.
6 changes: 5 additions & 1 deletion robotis_controller/include/robotis_controller/robotis_controller.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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);

Expand Down
66 changes: 54 additions & 12 deletions robotis_controller/src/robotis_controller/robotis_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down Expand Up @@ -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())
Expand Down Expand Up @@ -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++)
Expand All @@ -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
Expand Down Expand Up @@ -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)
Expand All @@ -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
Expand Down

0 comments on commit 45f57c8

Please sign in to comment.