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

- added WriteControlTable msg callback

Browse Source
This commit is contained in:
zerom@robotis.com
2016-10-10 10:16:03 +09:00
parent 260c03b3ae
commit 303e24c55e
1 changed files with 60 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

91
robotis_controller/src/robotis_controller/robotis_controller.cpp
View File

@@ -621,6 +621,8 @@ void RobotisController::msgQueueThread()
ros_node.setCallbackQueue(&callback_queue);
/* subscriber */
ros::Subscriber write_control_table_sub = ros_node.subscribe("/robotis/write_control_table", 5,
&RobotisController::writeControlTableCallback, this);
ros::Subscriber sync_write_item_sub = ros_node.subscribe("/robotis/sync_write_item", 10,
&RobotisController::syncWriteItemCallback, this);
ros::Subscriber joint_ctrl_modules_sub = ros_node.subscribe("/robotis/set_joint_ctrl_modules", 10,
@@ -1405,6 +1407,52 @@ void RobotisController::removeSensorModule(SensorModule *module)
sensor_modules_.remove(module);
}
void RobotisController::writeControlTableCallback(const robotis_controller_msgs::WriteControlTable::ConstPtr &msg)
{
Device *device = NULL;
auto dev_it1 = robot_->dxls_.find(msg->joint_name);
if(dev_it1 != robot_->dxls_.end())
{
device = dev_it1->second;
}
else
{
auto dev_it2 = robot_->sensors_.find(msg->joint_name);
if(dev_it2 != robot_->sensors_.end())
{
device = dev_it2->second;
}
else
{
ROS_WARN("[WriteControlTable] Unknown device : %s", msg->joint_name.c_str());
return;
}
}
ControlTableItem *item = NULL;
auto item_it = device->ctrl_table_.find(msg->start_item_name);
if(item_it != device->ctrl_table_.end())
{
item = item_it->second;
}
else
{
ROS_WARN("WriteControlTable] Unknown item : %s", msg->start_item_name.c_str());
return;
}
dynamixel::PortHandler *port = robot_->ports_[device->port_name_];
dynamixel::PacketHandler *packet_handler = dynamixel::PacketHandler::getPacketHandler(device->protocol_version_);
if (item->access_type_ == Read)
return;
direct_sync_write_.push_back(new dynamixel::GroupSyncWrite(port, packet_handler, item->address_, msg->data_length));
direct_sync_write_[direct_sync_write_.size() - 1]->addParam(device->id_, (uint8_t *)(msg->data.data()));
}
void RobotisController::syncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg)
{
for (int i = 0; i < msg->joint_name.size(); i++)
@@ -1430,7 +1478,18 @@ void RobotisController::syncWriteItemCallback(const robotis_controller_msgs::Syn
}
}
ControlTableItem *item = device->ctrl_table_[msg->item_name];
// ControlTableItem *item = device->ctrl_table_[msg->item_name];
ControlTableItem *item = NULL;
auto item_it = device->ctrl_table_.find(msg->item_name);
if(item_it != device->ctrl_table_.end())
{
item = item_it->second;
}
else
{
ROS_WARN("SyncWriteItem] Unknown item : %s", msg->item_name.c_str());
continue;
}
dynamixel::PortHandler *port = robot_->ports_[device->port_name_];
dynamixel::PacketHandler *packet_handler = dynamixel::PacketHandler::getPacketHandler(device->protocol_version_);
@@ -1519,11 +1578,6 @@ void RobotisController::setCtrlModuleCallback(const std_msgs::String::ConstPtr &
{
std::string _module_name_to_set = msg->data;
set_module_thread_ = boost::thread(boost::bind(&RobotisController::setCtrlModuleThread, this, _module_name_to_set));
}
@@ -1756,18 +1810,6 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module)
// stop module
std::list<MotionModule *> stop_modules;
ROS_INFO("----- Before -----");
for (auto& d_it : robot_->dxls_)
{
std::string joint_name = d_it.first;
Dynamixel *dxl = d_it.second;
double goal_position = dxl->dxl_state_->goal_position_;
fprintf(stderr, "%s : %f \n", joint_name.c_str(), goal_position);
}
ROS_INFO("----- ----- -----");
if (ctrl_module == "" || ctrl_module == "none")
{
// enqueue all modules in order to stop
@@ -1973,19 +2015,6 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module)
if (current_module_msg.joint_name.size() == current_module_msg.module_name.size())
current_module_pub_.publish(current_module_msg);
ROS_INFO("----- After -----");
for (auto& d_it : robot_->dxls_)
{
std::string joint_name = d_it.first;
Dynamixel *dxl = d_it.second;
double goal_position = dxl->dxl_state_->goal_position_;
fprintf(stderr, "%s : %f \n", joint_name.c_str(), goal_position);
}
ROS_INFO("----- ----- -----");
}
void RobotisController::gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg)