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

- Direct Control Mode bug fixed.

Browse Source
This commit is contained in:
ROBOTIS-zerom
2016-11-10 10:16:37 +09:00
parent c8abeff063
commit abbbec3fe3
1 changed files with 72 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

77
robotis_controller/src/robotis_controller/robotis_controller.cpp
View File

@ -1109,13 +1109,63 @@ void RobotisController::process()
{
if(gazebo_mode_ == false)
{
// BulkRead Rx
for (auto& it : port_to_bulk_read_)
{
robot_->ports_[it.first]->setPacketTimeout(0.0);
it.second->rxPacket();
}
// -> save to robot->dxls_[]->dxl_state_
if (robot_->dxls_.size() > 0)
{
for (auto& dxl_it : robot_->dxls_)
{
Dynamixel *dxl = dxl_it.second;
std::string port_name = dxl_it.second->port_name_;
std::string joint_name = dxl_it.first;
if (dxl->bulk_read_items_.size() > 0)
{
uint32_t data = 0;
for (int i = 0; i < dxl->bulk_read_items_.size(); i++)
{
ControlTableItem *item = dxl->bulk_read_items_[i];
if (port_to_bulk_read_[port_name]->isAvailable(dxl->id_, item->address_, item->data_length_) == true)
{
data = port_to_bulk_read_[port_name]->getData(dxl->id_, item->address_, item->data_length_);
// change dxl_state
if (dxl->present_position_item_ != 0 && item->item_name_ == dxl->present_position_item_->item_name_)
dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset
else if (dxl->present_velocity_item_ != 0 && item->item_name_ == dxl->present_velocity_item_->item_name_)
dxl->dxl_state_->present_velocity_ = dxl->convertValue2Velocity(data);
else if (dxl->present_current_item_ != 0 && item->item_name_ == dxl->present_current_item_->item_name_)
dxl->dxl_state_->present_torque_ = dxl->convertValue2Torque(data);
else if (dxl->goal_position_item_ != 0 && item->item_name_ == dxl->goal_position_item_->item_name_)
dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset
else if (dxl->goal_velocity_item_ != 0 && item->item_name_ == dxl->goal_velocity_item_->item_name_)
dxl->dxl_state_->goal_velocity_ = dxl->convertValue2Velocity(data);
else if (dxl->goal_current_item_ != 0 && item->item_name_ == dxl->goal_current_item_->item_name_)
dxl->dxl_state_->goal_torque_ = dxl->convertValue2Torque(data);
else
dxl->dxl_state_->bulk_read_table_[item->item_name_] = data;
}
}
// -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp
dxl->dxl_state_->update_time_stamp_ = TimeStamp(present_state.header.stamp.sec, present_state.header.stamp.nsec);
}
}
}
queue_mutex_.lock();
for (auto& it : port_to_sync_write_position_)
{
it.second->txPacket();
it.second->clearParam();
}
// for (auto& it : port_to_sync_write_position_)
// {
// it.second->txPacket();
// it.second->clearParam();
// }
if (direct_sync_write_.size() > 0)
{
@ -1128,6 +1178,10 @@ void RobotisController::process()
}
queue_mutex_.unlock();
// BulkRead Tx
for (auto& it : port_to_bulk_read_)
it.second->txPacket();
}
}
@ -1553,9 +1607,22 @@ void RobotisController::syncWriteItemCallback(const robotis_controller_msgs::Syn
void RobotisController::setControllerModeCallback(const std_msgs::String::ConstPtr &msg)
{
if (msg->data == "DirectControlMode")
{
for (auto& it : port_to_bulk_read_)
{
robot_->ports_[it.first]->setPacketTimeout(0.0);
it.second->rxPacket();
}
controller_mode_ = DirectControlMode;
}
else if (msg->data == "MotionModuleMode")
{
for (auto& it : port_to_bulk_read_)
{
it.second->txPacket();
}
controller_mode_ = MotionModuleMode;
}
}
void RobotisController::setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg)