diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 5a0c1ec..2be5d6a 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -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)