From 037b96de6a0a9680b91dbe199333671b0753a5c9 Mon Sep 17 00:00:00 2001 From: Zerom Date: Tue, 29 May 2018 17:57:11 +0900 Subject: [PATCH] - added enable offset service - added load offset service --- .../robotis_controller/robotis_controller.h | 3 + .../robotis_controller/robotis_controller.cpp | 95 ++++++++++++++++--- 2 files changed, 87 insertions(+), 11 deletions(-) diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h index aab8da0..a381a62 100755 --- a/robotis_controller/include/robotis_controller/robotis_controller.h +++ b/robotis_controller/include/robotis_controller/robotis_controller.h @@ -64,6 +64,7 @@ private: bool init_pose_loaded_; bool is_timer_running_; + bool is_offset_enabled_; bool stop_timer_; pthread_t timer_thread_; ControllerMode controller_mode_; @@ -142,6 +143,8 @@ public: bool getJointCtrlModuleService(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res); bool setJointCtrlModuleService(robotis_controller_msgs::SetJointModule::Request &req, robotis_controller_msgs::SetJointModule::Response &res); bool setCtrlModuleService(robotis_controller_msgs::SetModule::Request &req, robotis_controller_msgs::SetModule::Response &res); + bool enableOffsetService(robotis_controller_msgs::EnableOffset::Request &req, robotis_controller_msgs::EnableOffset::Response &res); + bool loadOffsetService(robotis_controller_msgs::LoadOffset::Request &req, robotis_controller_msgs::LoadOffset::Response &res); void gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 000bd59..eb8d74e 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -30,6 +30,7 @@ using namespace robotis_framework; RobotisController::RobotisController() : is_timer_running_(false), + is_offset_enabled_(true), stop_timer_(false), init_pose_loaded_(false), timer_thread_(0), @@ -142,7 +143,10 @@ void RobotisController::initializeSyncWrite() if ((dxl->present_position_item_ != 0) && (dxl->bulk_read_items_[i]->item_name_ == dxl->present_position_item_->item_name_)) { - dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(read_data) - dxl->dxl_state_->position_offset_; // remove offset + if(is_offset_enabled_) + dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(read_data) - dxl->dxl_state_->position_offset_; // remove offset + else + dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(read_data); dxl->dxl_state_->goal_position_ = dxl->dxl_state_->present_position_; port_to_sync_write_position_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); @@ -664,6 +668,10 @@ void RobotisController::msgQueueThread() &RobotisController::setJointCtrlModuleService, this); ros::ServiceServer set_module_server = ros_node.advertiseService("/robotis/set_present_ctrl_modules", &RobotisController::setCtrlModuleService, this); + ros::ServiceServer enable_offset_server = ros_node.advertiseService("/robotis/enable_offset", + &RobotisController::enableOffsetService, this); + ros::ServiceServer load_offset_server = ros_node.advertiseService("/robotis/load_offset", + &RobotisController::loadOffsetService, this); ros::WallDuration duration(robot_->getControlCycle() / 1000.0); while(ros_node.ok()) @@ -926,13 +934,23 @@ void RobotisController::process() // 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 + { + if(is_offset_enabled_) + dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset + else + dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data); + } 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 + { + if(is_offset_enabled_) + dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset + else + dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data); + } 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_) @@ -1146,13 +1164,23 @@ void RobotisController::process() // 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 + { + if(is_offset_enabled_) + dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset + else + dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data); + } 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 + { + if(is_offset_enabled_) + dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset + else + dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data); + } 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_) @@ -1255,7 +1283,12 @@ void RobotisController::process() if (gazebo_mode_ == false) { // add offset - uint32_t pos_data = dxl->convertRadian2Value(dxl_state->goal_position_ + dxl_state->position_offset_); + uint32_t pos_data; + if(is_offset_enabled_) + pos_data= dxl->convertRadian2Value(dxl_state->goal_position_ + dxl_state->position_offset_); + else + pos_data= dxl->convertRadian2Value(dxl_state->goal_position_); + uint8_t sync_write_data[4] = { 0 }; sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); @@ -1653,7 +1686,12 @@ void RobotisController::setJointStatesCallback(const sensor_msgs::JointState::Co if (gazebo_mode_ == false) { // add offset - uint32_t pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_); + uint32_t pos_data; + if(is_offset_enabled_) + pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_); + else + pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_); + uint8_t sync_write_data[4] = { 0 }; sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); @@ -1748,6 +1786,21 @@ bool RobotisController::setCtrlModuleService(robotis_controller_msgs::SetModule: set_module_thread_.join(); + res.result = true; + return true; +} + +bool RobotisController::enableOffsetService(robotis_controller_msgs::EnableOffset::Request &req, robotis_controller_msgs::EnableOffset::Response &res) +{ + is_offset_enabled_ = (bool)req.enable; + res.result = true; + return true; +} + +bool RobotisController::loadOffsetService(robotis_controller_msgs::LoadOffset::Request &req, robotis_controller_msgs::LoadOffset::Response &res) +{ + loadOffset((std::string)req.file_path); + res.result = true; return true; } @@ -1820,7 +1873,12 @@ void RobotisController::setJointCtrlModuleThread(const robotis_controller_msgs:: if(gazebo_mode_ == true) continue; - uint32_t _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + _dxl->dxl_state_->position_offset_); + uint32_t _pos_data; + if(is_offset_enabled_) + _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + _dxl->dxl_state_->position_offset_); + else + _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_); + 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)); @@ -1858,7 +1916,12 @@ void RobotisController::setJointCtrlModuleThread(const robotis_controller_msgs:: if(_mode == PositionControl) { - uint32_t _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + _dxl->dxl_state_->position_offset_); + uint32_t _pos_data; + if(is_offset_enabled_) + _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + _dxl->dxl_state_->position_offset_); + else + _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_); + 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)); @@ -2024,7 +2087,12 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) if (gazebo_mode_ == true) continue; - uint32_t pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_); + uint32_t pos_data; + if(is_offset_enabled_) + pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_); + else + pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_); + uint8_t sync_write_data[4] = { 0 }; sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); @@ -2062,7 +2130,12 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) if (mode == PositionControl) { - uint32_t pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_); + uint32_t pos_data; + if(is_offset_enabled_) + pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_); + else + pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_); + uint8_t sync_write_data[4] = { 0 }; sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data));