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

- make setJointCtrlModuleCallback() to the thread function & improved.

Browse Source
This commit is contained in:
ROBOTIS-zerom
2016-09-02 11:29:37 +09:00
parent 7f15d5639e
commit 09d54945f7
2 changed files with 207 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
robotis_controller/include/robotis_controller/robotis_controller.h
View File

@ -88,6 +88,7 @@ private:
void gazeboTimerThread();
void msgQueueThread();
void setCtrlModuleThread(std::string ctrl_module);
void setJointCtrlModuleThread(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg);
bool isTimerStopped();
void initializeSyncWrite();
@ -141,6 +142,7 @@ public:
void stopTimer();
bool isTimerRunning();
void setCtrlModule(std::string module_name);
void loadOffset(const std::string path);
/* ROS Topic Callback Functions */

276
robotis_controller/src/robotis_controller/robotis_controller.cpp
View File

@ -1529,75 +1529,18 @@ void RobotisController::setCtrlModuleCallback(const std_msgs::String::ConstPtr &
set_module_thread_ = boost::thread(boost::bind(&RobotisController::setCtrlModuleThread, this, _module_name_to_set));
}
void RobotisController::setCtrlModule(std::string module_name)
{
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;
queue_mutex_.lock();
for (unsigned int idx = 0; idx < msg->joint_name.size(); idx++)
{
Dynamixel *dxl = NULL;
auto dxl_it = robot_->dxls_.find((std::string) (msg->joint_name[idx]));
if (dxl_it != robot_->dxls_.end())
dxl = dxl_it->second;
else
continue;
// none
if (msg->module_name[idx] == "" || msg->module_name[idx] == "none")
{
dxl->ctrl_module_name_ = msg->module_name[idx];
continue;
}
// check whether the module is using this joint
for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++)
{
if ((*m_it)->getModuleName() == msg->module_name[idx])
{
if ((*m_it)->result_.find(msg->joint_name[idx]) != (*m_it)->result_.end())
{
dxl->ctrl_module_name_ = msg->module_name[idx];
break;
}
}
}
set_module_thread_ = boost::thread(boost::bind(&RobotisController::setJointCtrlModuleThread, this, msg));
}
for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++)
{
// set all modules -> disable
(*m_it)->setModuleEnable(false);
// set all used modules -> enable
for (auto& d_it : robot_->dxls_)
{
if (d_it.second->ctrl_module_name_ == (*m_it)->getModuleName())
{
(*m_it)->setModuleEnable(true);
break;
}
}
}
// TODO: set indirect address
// -> check module's control_mode
queue_mutex_.unlock();
robotis_controller_msgs::JointCtrlModule current_module_msg;
for (auto& dxl_iter : robot_->dxls_)
{
current_module_msg.joint_name.push_back(dxl_iter.first);
current_module_msg.module_name.push_back(dxl_iter.second->ctrl_module_name_);
}
if (current_module_msg.joint_name.size() == current_module_msg.module_name.size())
current_module_pub_.publish(current_module_msg);
}
bool RobotisController::getCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req,
robotis_controller_msgs::GetJointModule::Response &res)
{
@ -1617,6 +1560,199 @@ bool RobotisController::getCtrlModuleCallback(robotis_controller_msgs::GetJointM
return true;
}
void RobotisController::setJointCtrlModuleThread(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg)
{
// stop module list
std::list<MotionModule *> _stop_modules;
std::list<MotionModule *> _enable_modules;
for(unsigned int idx = 0; idx < msg->joint_name.size(); idx++)
{
Dynamixel *_dxl = NULL;
std::map<std::string, Dynamixel*>::iterator _dxl_it = robot_->dxls_.find((std::string)(msg->joint_name[idx]));
if(_dxl_it != robot_->dxls_.end())
_dxl = _dxl_it->second;
else
continue;
// enqueue
if(_dxl->ctrl_module_name_ != msg->module_name[idx])
{
for(std::list<MotionModule *>::iterator _stop_m_it = motion_modules_.begin(); _stop_m_it != motion_modules_.end(); _stop_m_it++)
{
if((*_stop_m_it)->getModuleName() == _dxl->ctrl_module_name_ && (*_stop_m_it)->getModuleEnable() == true)
_stop_modules.push_back(*_stop_m_it);
}
}
}
// stop the module
_stop_modules.unique();
for(std::list<MotionModule *>::iterator _stop_m_it = _stop_modules.begin(); _stop_m_it != _stop_modules.end(); _stop_m_it++)
{
(*_stop_m_it)->stop();
}
// wait to stop
for(std::list<MotionModule *>::iterator _stop_m_it = _stop_modules.begin(); _stop_m_it != _stop_modules.end(); _stop_m_it++)
{
while((*_stop_m_it)->isRunning())
usleep(CONTROL_CYCLE_MSEC * 1000);
}
// disable module(s)
for(std::list<MotionModule *>::iterator _stop_m_it = _stop_modules.begin(); _stop_m_it != _stop_modules.end(); _stop_m_it++)
{
(*_stop_m_it)->setModuleEnable(false);
}
// set ctrl module
queue_mutex_.lock();
for(unsigned int idx = 0; idx < msg->joint_name.size(); idx++)
{
std::string ctrl_module = msg->module_name[idx];
std::string joint_name = msg->joint_name[idx];
Dynamixel *_dxl = NULL;
std::map<std::string, Dynamixel*>::iterator _dxl_it = robot_->dxls_.find(joint_name);
if(_dxl_it != robot_->dxls_.end())
_dxl = _dxl_it->second;
else
continue;
// none
if(ctrl_module == "" || ctrl_module == "none")
{
_dxl->ctrl_module_name_ = "none";
if(gazebo_mode_ == true)
continue;
uint32_t _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + _dxl->dxl_state_->position_offset_);
uint8_t _sync_write_data[4];
_sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data));
_sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data));
_sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data));
_sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data));
if(port_to_sync_write_position_[_dxl->port_name_] != NULL)
port_to_sync_write_position_[_dxl->port_name_]->addParam(_dxl->id_, _sync_write_data);
if(port_to_sync_write_current_[_dxl->port_name_] != NULL)
port_to_sync_write_current_[_dxl->port_name_]->removeParam(_dxl->id_);
if(port_to_sync_write_velocity_[_dxl->port_name_] != NULL)
port_to_sync_write_velocity_[_dxl->port_name_]->removeParam(_dxl->id_);
}
else
{
// check whether the module exist
for(std::list<MotionModule *>::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++)
{
// if it exist
if((*_m_it)->getModuleName() == ctrl_module)
{
std::map<std::string, DynamixelState*>::iterator _result_it = (*_m_it)->result_.find(joint_name);
if(_result_it == (*_m_it)->result_.end())
break;
_dxl->ctrl_module_name_ = ctrl_module;
// enqueue enable module list
_enable_modules.push_back(*_m_it);
ControlMode _mode = (*_m_it)->getControlMode();
if(gazebo_mode_ == true)
break;
if(_mode == PositionControl)
{
uint32_t _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + _dxl->dxl_state_->position_offset_);
uint8_t _sync_write_data[4];
_sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data));
_sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data));
_sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data));
_sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data));
if(port_to_sync_write_position_[_dxl->port_name_] != NULL)
port_to_sync_write_position_[_dxl->port_name_]->addParam(_dxl->id_, _sync_write_data);
if(port_to_sync_write_current_[_dxl->port_name_] != NULL)
port_to_sync_write_current_[_dxl->port_name_]->removeParam(_dxl->id_);
if(port_to_sync_write_velocity_[_dxl->port_name_] != NULL)
port_to_sync_write_velocity_[_dxl->port_name_]->removeParam(_dxl->id_);
}
else if(_mode == VelocityControl)
{
uint32_t _vel_data = _dxl->convertVelocity2Value(_dxl->dxl_state_->goal_velocity_);
uint8_t _sync_write_data[4];
_sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_vel_data));
_sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_vel_data));
_sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_vel_data));
_sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_vel_data));
if(port_to_sync_write_velocity_[_dxl->port_name_] != NULL)
port_to_sync_write_velocity_[_dxl->port_name_]->addParam(_dxl->id_, _sync_write_data);
if(port_to_sync_write_current_[_dxl->port_name_] != NULL)
port_to_sync_write_current_[_dxl->port_name_]->removeParam(_dxl->id_);
if(port_to_sync_write_position_[_dxl->port_name_] != NULL)
port_to_sync_write_position_[_dxl->port_name_]->removeParam(_dxl->id_);
}
else if(_mode == TorqueControl)
{
uint32_t _curr_data = _dxl->convertTorque2Value(_dxl->dxl_state_->goal_torque_);
uint8_t _sync_write_data[4];
_sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_curr_data));
_sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_curr_data));
_sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_curr_data));
_sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_curr_data));
if(port_to_sync_write_current_[_dxl->port_name_] != NULL)
port_to_sync_write_current_[_dxl->port_name_]->addParam(_dxl->id_, _sync_write_data);
if(port_to_sync_write_velocity_[_dxl->port_name_] != NULL)
port_to_sync_write_velocity_[_dxl->port_name_]->removeParam(_dxl->id_);
if(port_to_sync_write_position_[_dxl->port_name_] != NULL)
port_to_sync_write_position_[_dxl->port_name_]->removeParam(_dxl->id_);
}
break;
}
}
}
}
// enable module(s)
_enable_modules.unique();
for(std::list<MotionModule *>::iterator _m_it = _enable_modules.begin(); _m_it != _enable_modules.end(); _m_it++)
{
(*_m_it)->setModuleEnable(true);
}
// TODO: set indirect address
// -> check module's control_mode
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)
{
_current_module_msg.joint_name.push_back(_dxl_iter->first);
_current_module_msg.module_name.push_back(_dxl_iter->second->ctrl_module_name_);
}
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
@ -1679,6 +1815,13 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module)
usleep(CONTROL_CYCLE_MSEC * 1000);
}
// disable module(s)
for(std::list<MotionModule *>::iterator _stop_m_it = stop_modules.begin(); _stop_m_it != stop_modules.end(); _stop_m_it++)
{
(*_stop_m_it)->setModuleEnable(false);
}
// set ctrl module
queue_mutex_.lock();
@ -1688,12 +1831,6 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module)
// none
if ((ctrl_module == "") || (ctrl_module == "none"))
{
// set all modules -> disable
for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++)
{
(*m_it)->setModuleEnable(false);
}
// set dxl's control module to "none"
for (auto& d_it : robot_->dxls_)
{
@ -1800,9 +1937,6 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module)
for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++)
{
// set all modules -> disable
(*m_it)->setModuleEnable(false);
// set all used modules -> enable
for (auto& d_it : robot_->dxls_)
{