Skip to content

Commit 09d5494

Browse files
ROBOTIS-zeromROBOTIS-zerom
committed
- make setJointCtrlModuleCallback() to the thread function & improved.
1 parent 7f15d56 commit 09d5494

File tree

2 files changed

+197
-61
lines changed

2 files changed

+197
-61
lines changed

‎robotis_controller/include/robotis_controller/robotis_controller.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -88,6 +88,7 @@ class RobotisController : public Singleton<RobotisController>
8888
void gazeboTimerThread();
8989
void msgQueueThread();
9090
void setCtrlModuleThread(std::string ctrl_module);
91+
void setJointCtrlModuleThread(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg);
9192

9293
bool isTimerStopped();
9394
void initializeSyncWrite();
@@ -141,6 +142,7 @@ class RobotisController : public Singleton<RobotisController>
141142
void stopTimer();
142143
bool isTimerRunning();
143144

145+
void setCtrlModule(std::string module_name);
144146
void loadOffset(const std::string path);
145147

146148
/* ROS Topic Callback Functions */

‎robotis_controller/src/robotis_controller/robotis_controller.cpp‎

Lines changed: 195 additions & 61 deletions
Original file line numberDiff line numberDiff line change
@@ -1529,94 +1529,230 @@ void RobotisController::setCtrlModuleCallback(const std_msgs::String::ConstPtr &
15291529
set_module_thread_ = boost::thread(boost::bind(&RobotisController::setCtrlModuleThread, this, _module_name_to_set));
15301530
}
15311531

1532+
void RobotisController::setCtrlModule(std::string module_name)
1533+
{
1534+
set_module_thread_ = boost::thread(boost::bind(&RobotisController::setCtrlModuleThread, this, module_name));
1535+
}
15321536
void RobotisController::setJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg)
15331537
{
15341538
if (msg->joint_name.size() != msg->module_name.size())
15351539
return;
15361540

1541+
set_module_thread_ = boost::thread(boost::bind(&RobotisController::setJointCtrlModuleThread, this, msg));
1542+
}
1543+
1544+
bool RobotisController::getCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req,
1545+
robotis_controller_msgs::GetJointModule::Response &res)
1546+
{
1547+
for (unsigned int idx = 0; idx < req.joint_name.size(); idx++)
1548+
{
1549+
auto d_it = robot_->dxls_.find((std::string) (req.joint_name[idx]));
1550+
if (d_it != robot_->dxls_.end())
1551+
{
1552+
res.joint_name.push_back(req.joint_name[idx]);
1553+
res.module_name.push_back(d_it->second->ctrl_module_name_);
1554+
}
1555+
}
1556+
1557+
if (res.joint_name.size() == 0)
1558+
return false;
1559+
1560+
return true;
1561+
}
1562+
1563+
void RobotisController::setJointCtrlModuleThread(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg)
1564+
{
1565+
// stop module list
1566+
std::list<MotionModule *> _stop_modules;
1567+
std::list<MotionModule *> _enable_modules;
1568+
1569+
for(unsigned int idx = 0; idx < msg->joint_name.size(); idx++)
1570+
{
1571+
Dynamixel *_dxl = NULL;
1572+
std::map<std::string, Dynamixel*>::iterator _dxl_it = robot_->dxls_.find((std::string)(msg->joint_name[idx]));
1573+
if(_dxl_it != robot_->dxls_.end())
1574+
_dxl = _dxl_it->second;
1575+
else
1576+
continue;
1577+
1578+
// enqueue
1579+
if(_dxl->ctrl_module_name_ != msg->module_name[idx])
1580+
{
1581+
for(std::list<MotionModule *>::iterator _stop_m_it = motion_modules_.begin(); _stop_m_it != motion_modules_.end(); _stop_m_it++)
1582+
{
1583+
if((*_stop_m_it)->getModuleName() == _dxl->ctrl_module_name_ && (*_stop_m_it)->getModuleEnable() == true)
1584+
_stop_modules.push_back(*_stop_m_it);
1585+
}
1586+
}
1587+
}
1588+
1589+
// stop the module
1590+
_stop_modules.unique();
1591+
for(std::list<MotionModule *>::iterator _stop_m_it = _stop_modules.begin(); _stop_m_it != _stop_modules.end(); _stop_m_it++)
1592+
{
1593+
(*_stop_m_it)->stop();
1594+
}
1595+
1596+
// wait to stop
1597+
for(std::list<MotionModule *>::iterator _stop_m_it = _stop_modules.begin(); _stop_m_it != _stop_modules.end(); _stop_m_it++)
1598+
{
1599+
while((*_stop_m_it)->isRunning())
1600+
usleep(CONTROL_CYCLE_MSEC * 1000);
1601+
}
1602+
1603+
// disable module(s)
1604+
for(std::list<MotionModule *>::iterator _stop_m_it = _stop_modules.begin(); _stop_m_it != _stop_modules.end(); _stop_m_it++)
1605+
{
1606+
(*_stop_m_it)->setModuleEnable(false);
1607+
}
1608+
1609+
// set ctrl module
15371610
queue_mutex_.lock();
15381611

1539-
for (unsigned int idx = 0; idx < msg->joint_name.size(); idx++)
1612+
for(unsigned int idx = 0; idx < msg->joint_name.size(); idx++)
15401613
{
1541-
Dynamixel *dxl = NULL;
1542-
auto dxl_it = robot_->dxls_.find((std::string) (msg->joint_name[idx]));
1543-
if (dxl_it != robot_->dxls_.end())
1544-
dxl = dxl_it->second;
1614+
std::string ctrl_module = msg->module_name[idx];
1615+
std::string joint_name = msg->joint_name[idx];
1616+
1617+
Dynamixel *_dxl = NULL;
1618+
std::map<std::string, Dynamixel*>::iterator _dxl_it = robot_->dxls_.find(joint_name);
1619+
if(_dxl_it != robot_->dxls_.end())
1620+
_dxl = _dxl_it->second;
15451621
else
15461622
continue;
15471623

15481624
// none
1549-
if (msg->module_name[idx] == "" || msg->module_name[idx] == "none")
1625+
if(ctrl_module == "" || ctrl_module == "none")
15501626
{
1551-
dxl->ctrl_module_name_ = msg->module_name[idx];
1552-
continue;
1553-
}
1627+
_dxl->ctrl_module_name_ = "none";
15541628

1555-
// check whether the module is using this joint
1556-
for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++)
1629+
if(gazebo_mode_ == true)
1630+
continue;
1631+
1632+
uint32_t _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + _dxl->dxl_state_->position_offset_);
1633+
uint8_t _sync_write_data[4];
1634+
_sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data));
1635+
_sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data));
1636+
_sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data));
1637+
_sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data));
1638+
1639+
if(port_to_sync_write_position_[_dxl->port_name_] != NULL)
1640+
port_to_sync_write_position_[_dxl->port_name_]->addParam(_dxl->id_, _sync_write_data);
1641+
1642+
if(port_to_sync_write_current_[_dxl->port_name_] != NULL)
1643+
port_to_sync_write_current_[_dxl->port_name_]->removeParam(_dxl->id_);
1644+
if(port_to_sync_write_velocity_[_dxl->port_name_] != NULL)
1645+
port_to_sync_write_velocity_[_dxl->port_name_]->removeParam(_dxl->id_);
1646+
}
1647+
else
15571648
{
1558-
if ((*m_it)->getModuleName() == msg->module_name[idx])
1649+
// check whether the module exist
1650+
for(std::list<MotionModule *>::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++)
15591651
{
1560-
if ((*m_it)->result_.find(msg->joint_name[idx]) != (*m_it)->result_.end())
1652+
// if it exist
1653+
if((*_m_it)->getModuleName() == ctrl_module)
15611654
{
1562-
dxl->ctrl_module_name_ = msg->module_name[idx];
1655+
std::map<std::string, DynamixelState*>::iterator _result_it = (*_m_it)->result_.find(joint_name);
1656+
if(_result_it == (*_m_it)->result_.end())
1657+
break;
1658+
1659+
_dxl->ctrl_module_name_ = ctrl_module;
1660+
1661+
// enqueue enable module list
1662+
_enable_modules.push_back(*_m_it);
1663+
ControlMode _mode = (*_m_it)->getControlMode();
1664+
1665+
if(gazebo_mode_ == true)
1666+
break;
1667+
1668+
if(_mode == PositionControl)
1669+
{
1670+
uint32_t _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + _dxl->dxl_state_->position_offset_);
1671+
uint8_t _sync_write_data[4];
1672+
_sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data));
1673+
_sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data));
1674+
_sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data));
1675+
_sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data));
1676+
1677+
if(port_to_sync_write_position_[_dxl->port_name_] != NULL)
1678+
port_to_sync_write_position_[_dxl->port_name_]->addParam(_dxl->id_, _sync_write_data);
1679+
1680+
if(port_to_sync_write_current_[_dxl->port_name_] != NULL)
1681+
port_to_sync_write_current_[_dxl->port_name_]->removeParam(_dxl->id_);
1682+
if(port_to_sync_write_velocity_[_dxl->port_name_] != NULL)
1683+
port_to_sync_write_velocity_[_dxl->port_name_]->removeParam(_dxl->id_);
1684+
}
1685+
else if(_mode == VelocityControl)
1686+
{
1687+
uint32_t _vel_data = _dxl->convertVelocity2Value(_dxl->dxl_state_->goal_velocity_);
1688+
uint8_t _sync_write_data[4];
1689+
_sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_vel_data));
1690+
_sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_vel_data));
1691+
_sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_vel_data));
1692+
_sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_vel_data));
1693+
1694+
if(port_to_sync_write_velocity_[_dxl->port_name_] != NULL)
1695+
port_to_sync_write_velocity_[_dxl->port_name_]->addParam(_dxl->id_, _sync_write_data);
1696+
1697+
if(port_to_sync_write_current_[_dxl->port_name_] != NULL)
1698+
port_to_sync_write_current_[_dxl->port_name_]->removeParam(_dxl->id_);
1699+
if(port_to_sync_write_position_[_dxl->port_name_] != NULL)
1700+
port_to_sync_write_position_[_dxl->port_name_]->removeParam(_dxl->id_);
1701+
}
1702+
else if(_mode == TorqueControl)
1703+
{
1704+
uint32_t _curr_data = _dxl->convertTorque2Value(_dxl->dxl_state_->goal_torque_);
1705+
uint8_t _sync_write_data[4];
1706+
_sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_curr_data));
1707+
_sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_curr_data));
1708+
_sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_curr_data));
1709+
_sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_curr_data));
1710+
1711+
if(port_to_sync_write_current_[_dxl->port_name_] != NULL)
1712+
port_to_sync_write_current_[_dxl->port_name_]->addParam(_dxl->id_, _sync_write_data);
1713+
1714+
if(port_to_sync_write_velocity_[_dxl->port_name_] != NULL)
1715+
port_to_sync_write_velocity_[_dxl->port_name_]->removeParam(_dxl->id_);
1716+
if(port_to_sync_write_position_[_dxl->port_name_] != NULL)
1717+
port_to_sync_write_position_[_dxl->port_name_]->removeParam(_dxl->id_);
1718+
}
15631719
break;
15641720
}
15651721
}
15661722
}
15671723
}
15681724

1569-
for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++)
1725+
// enable module(s)
1726+
_enable_modules.unique();
1727+
for(std::list<MotionModule *>::iterator _m_it = _enable_modules.begin(); _m_it != _enable_modules.end(); _m_it++)
15701728
{
1571-
// set all modules -> disable
1572-
(*m_it)->setModuleEnable(false);
1573-
1574-
// set all used modules -> enable
1575-
for (auto& d_it : robot_->dxls_)
1576-
{
1577-
if (d_it.second->ctrl_module_name_ == (*m_it)->getModuleName())
1578-
{
1579-
(*m_it)->setModuleEnable(true);
1580-
break;
1581-
}
1582-
}
1729+
(*_m_it)->setModuleEnable(true);
15831730
}
15841731

15851732
// TODO: set indirect address
15861733
// -> check module's control_mode
15871734

15881735
queue_mutex_.unlock();
15891736

1590-
robotis_controller_msgs::JointCtrlModule current_module_msg;
1591-
for (auto& dxl_iter : robot_->dxls_)
1592-
{
1593-
current_module_msg.joint_name.push_back(dxl_iter.first);
1594-
current_module_msg.module_name.push_back(dxl_iter.second->ctrl_module_name_);
1595-
}
1596-
1597-
if (current_module_msg.joint_name.size() == current_module_msg.module_name.size())
1598-
current_module_pub_.publish(current_module_msg);
1599-
}
1737+
// log
1738+
// std::cout << "Enable Joint Ctrl Module : " << std::endl;
1739+
// for(std::list<MotionModule *>::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++)
1740+
// {
1741+
// if((*_m_it)->GetModuleEnable() == true)
1742+
// std::cout << " - " << (*_m_it)->GetModuleName() << std::endl;
1743+
// }
16001744

1601-
bool RobotisController::getCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req,
1602-
robotis_controller_msgs::GetJointModule::Response &res)
1603-
{
1604-
for (unsigned int idx = 0; idx < req.joint_name.size(); idx++)
1745+
// publish current module
1746+
robotis_controller_msgs::JointCtrlModule _current_module_msg;
1747+
for(std::map<std::string, Dynamixel *>::iterator _dxl_iter = robot_->dxls_.begin(); _dxl_iter != robot_->dxls_.end(); ++_dxl_iter)
16051748
{
1606-
auto d_it = robot_->dxls_.find((std::string) (req.joint_name[idx]));
1607-
if (d_it != robot_->dxls_.end())
1608-
{
1609-
res.joint_name.push_back(req.joint_name[idx]);
1610-
res.module_name.push_back(d_it->second->ctrl_module_name_);
1611-
}
1749+
_current_module_msg.joint_name.push_back(_dxl_iter->first);
1750+
_current_module_msg.module_name.push_back(_dxl_iter->second->ctrl_module_name_);
16121751
}
16131752

1614-
if (res.joint_name.size() == 0)
1615-
return false;
1616-
1617-
return true;
1753+
if(_current_module_msg.joint_name.size() == _current_module_msg.module_name.size())
1754+
current_module_pub_.publish(_current_module_msg);
16181755
}
1619-
16201756
void RobotisController::setCtrlModuleThread(std::string ctrl_module)
16211757
{
16221758
// stop module
@@ -1679,6 +1815,13 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module)
16791815
usleep(CONTROL_CYCLE_MSEC * 1000);
16801816
}
16811817

1818+
// disable module(s)
1819+
for(std::list<MotionModule *>::iterator _stop_m_it = stop_modules.begin(); _stop_m_it != stop_modules.end(); _stop_m_it++)
1820+
{
1821+
(*_stop_m_it)->setModuleEnable(false);
1822+
}
1823+
1824+
16821825
// set ctrl module
16831826
queue_mutex_.lock();
16841827

@@ -1688,12 +1831,6 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module)
16881831
// none
16891832
if ((ctrl_module == "") || (ctrl_module == "none"))
16901833
{
1691-
// set all modules -> disable
1692-
for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++)
1693-
{
1694-
(*m_it)->setModuleEnable(false);
1695-
}
1696-
16971834
// set dxl's control module to "none"
16981835
for (auto& d_it : robot_->dxls_)
16991836
{
@@ -1800,9 +1937,6 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module)
18001937

18011938
for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++)
18021939
{
1803-
// set all modules -> disable
1804-
(*m_it)->setModuleEnable(false);
1805-
18061940
// set all used modules -> enable
18071941
for (auto& d_it : robot_->dxls_)
18081942
{

0 commit comments

Comments
 (0)