diff --git a/.robotis_framework.rosinstall b/.robotis_framework.rosinstall new file mode 100644 index 0000000..9fd2d5e --- /dev/null +++ b/.robotis_framework.rosinstall @@ -0,0 +1 @@ +- git: {local-name: robotis_controller_msgs, uri: 'https://github.com/ROBOTIS-GIT/ROBOTIS-Framework-msgs.git', version: master} \ No newline at end of file diff --git a/.travis.yml b/.travis.yml index 68240e5..695ee46 100644 --- a/.travis.yml +++ b/.travis.yml @@ -18,8 +18,9 @@ notifications: - pyo@robotis.com env: matrix: - - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian +# - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian # - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian OS_NAME=debian OS_CODE_NAME=jessie + - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=file OS_NAME=ubuntu OS_CODE_NAME=xenial $ROSINSTALL_FILENAME=".robotis_framework.rosinstall" branches: only: - master @@ -29,4 +30,4 @@ install: - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config script: - source .ci_config/travis.sh - \ No newline at end of file + diff --git a/README.md b/README.md index c40af91..fc06982 100755 --- a/README.md +++ b/README.md @@ -1,18 +1,22 @@ -# ROBOTIS Framework Metapackage -[![GitHub version](https://badge.fury.io/gh/ROBOTIS-GIT%2FROBOTIS-Framework.svg)](https://badge.fury.io/gh/ROBOTIS-GIT%2FROBOTIS-Framework) [![Build Status](https://travis-ci.org/ROBOTIS-GIT/ROBOTIS-Framework.svg?branch=master)](https://travis-ci.org/ROBOTIS-GIT/ROBOTIS-Framework) +## ROS Packages for ROBOTIS Framework +|Version|Kinetic + Ubuntu Xenial|Melodic + Ubuntu Bionic| +|:---:|:---:|:---:| +|[![GitHub version](https://badge.fury.io/gh/ROBOTIS-GIT%2FROBOTIS-Framework.svg)](https://badge.fury.io/gh/ROBOTIS-GIT%2FROBOTIS-Framework)|[![Build Status](https://travis-ci.org/ROBOTIS-GIT/ROBOTIS-Framework.svg?branch=master)](https://travis-ci.org/ROBOTIS-GIT/ROBOTIS-Framework)|-| -# Documents for robotis_framework packages -- [ROBOTIS e-Manual](http://emanual.robotis.com/docs/en/software/robotis_framework_packages/#robotis-framework) -- http://wiki.ros.org/robotis_framework +## ROBOTIS e-Manual for ROBOTIS Framework +- [ROBOTIS e-Manual for ROBOTIS Framework](http://emanual.robotis.com/docs/en/software/robotis_framework_packages) + +## Wiki for robotis_framework Packages +- http://wiki.ros.org/robotis_framework (metapackage) - http://wiki.ros.org/robotis_controller - http://wiki.ros.org/robotis_device - http://wiki.ros.org/robotis_framework_common -# ROS packages related to ROBOTIS Framework -- [dynamixel_sdk](https://github.com/ROBOTIS-GIT/DynamixelSDK) +## Open Source related to ROBOTIS Framework - [robotis_framework](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework) - [robotis_controller_msgs](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework-msgs) +- [dynamixel_sdk](https://github.com/ROBOTIS-GIT/DynamixelSDK) -# Documents and Videos for ROBOTIS Framework -- [ROBOTIS e-Manual for Dynamixel SDK](http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/overview/) -- [ROBOTIS e-Manual for ROBOTIS Framework](http://emanual.robotis.com/docs/en/software/robotis_framework_packages/) \ No newline at end of file +## Documents and Videos related to ROBOTIS Framework +- [ROBOTIS e-Manual for ROBOTIS Framework](http://emanual.robotis.com/docs/en/software/robotis_framework_packages/) +- [ROBOTIS e-Manual for Dynamixel SDK](http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/overview/) \ No newline at end of file diff --git a/robotis_controller/CHANGELOG.rst b/robotis_controller/CHANGELOG.rst index dad6244..990b3bc 100755 --- a/robotis_controller/CHANGELOG.rst +++ b/robotis_controller/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package robotis_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.9 (2018-03-22) +------------------ +* added serivce for setting module +* deleted comment for debug +* modified to prevent duplicate indirect address write +* added boost system dependencies +* fixed a bug that occure when handling bulk read item that does not exist. +* Contributors: Kayman, Zerom, Pyo + 0.2.8 (2018-03-20) ------------------ * modified CMakeLists.txt for system dependencies (yaml-cpp) diff --git a/robotis_controller/CMakeLists.txt b/robotis_controller/CMakeLists.txt index 5c32abd..8e5dd9c 100755 --- a/robotis_controller/CMakeLists.txt +++ b/robotis_controller/CMakeLists.txt @@ -22,6 +22,8 @@ find_package(catkin REQUIRED COMPONENTS cmake_modules ) +find_package(Boost REQUIRED) + # Resolve system dependency on yaml-cpp, which apparently does not # provide a CMake find_package() module. find_package(PkgConfig REQUIRED) @@ -59,6 +61,7 @@ catkin_package( INCLUDE_DIRS include LIBRARIES robotis_controller CATKIN_DEPENDS roscpp roslib std_msgs sensor_msgs robotis_controller_msgs dynamixel_sdk robotis_device robotis_framework_common cmake_modules + DEPENDS Boost ) ################################################################################ @@ -67,12 +70,13 @@ catkin_package( include_directories( include ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} ${YAML_CPP_INCLUDE_DIRS} ) add_library(robotis_controller src/robotis_controller/robotis_controller.cpp) add_dependencies(robotis_controller ${catkin_EXPORTED_TARGETS}) -target_link_libraries(robotis_controller ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) +target_link_libraries(robotis_controller ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${YAML_CPP_LIBRARIES}) ################################################################################ # Install diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h index aab8da0..6ee5c26 100755 --- a/robotis_controller/include/robotis_controller/robotis_controller.h +++ b/robotis_controller/include/robotis_controller/robotis_controller.h @@ -28,6 +28,7 @@ #include #include #include +#include #include #include #include @@ -38,6 +39,7 @@ #include "robotis_controller_msgs/GetJointModule.h" #include "robotis_controller_msgs/SetJointModule.h" #include "robotis_controller_msgs/SetModule.h" +#include "robotis_controller_msgs/LoadOffset.h" #include "robotis_device/robot.h" #include "robotis_framework_common/motion_module.h" @@ -64,6 +66,7 @@ private: bool init_pose_loaded_; bool is_timer_running_; + bool is_offset_enabled_; bool stop_timer_; pthread_t timer_thread_; ControllerMode controller_mode_; @@ -139,9 +142,11 @@ public: void setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); void setJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg); void setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg); + void enableOffsetCallback(const std_msgs::Bool::ConstPtr &msg); 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 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/package.xml b/robotis_controller/package.xml index 48371dc..aeb4e91 100755 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -1,7 +1,7 @@ robotis_controller - 0.2.8 + 0.2.9 robotis_controller package for ROBOTIS's platform like Manipulator-H, THORMANG and OP series diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 1f78056..8a7ee7d 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); @@ -632,6 +636,8 @@ void RobotisController::msgQueueThread() &RobotisController::setControllerModeCallback, this); ros::Subscriber joint_states_sub = ros_node.subscribe("/robotis/set_joint_states", 10, &RobotisController::setJointStatesCallback, this); + ros::Subscriber enable_offset_sub = ros_node.subscribe("/robotis/enable_offset", 10, + &RobotisController::enableOffsetCallback, this); ros::Subscriber gazebo_joint_states_sub; if (gazebo_mode_ == true) @@ -664,6 +670,8 @@ void RobotisController::msgQueueThread() &RobotisController::setJointCtrlModuleService, this); ros::ServiceServer set_module_server = ros_node.advertiseService("/robotis/set_present_ctrl_modules", &RobotisController::setCtrlModuleService, 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)); @@ -1697,6 +1735,11 @@ void RobotisController::setJointCtrlModuleCallback(const robotis_controller_msgs set_module_thread_ = boost::thread(boost::bind(&RobotisController::setJointCtrlModuleThread, this, msg)); } +void RobotisController::enableOffsetCallback(const std_msgs::Bool::ConstPtr &msg) +{ + is_offset_enabled_ = (bool)msg->data; +} + bool RobotisController::getJointCtrlModuleService(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res) { @@ -1748,6 +1791,14 @@ bool RobotisController::setCtrlModuleService(robotis_controller_msgs::SetModule: set_module_thread_.join(); + 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 +1871,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 +1914,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 +2085,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 +2128,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)); diff --git a/robotis_device/CHANGELOG.rst b/robotis_device/CHANGELOG.rst index 8d62fbc..af36406 100755 --- a/robotis_device/CHANGELOG.rst +++ b/robotis_device/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package robotis_device ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.9 (2018-03-22) +------------------ +* modified to prevent duplicate indirect address write +* fixed a bug that occure when handling bulk read item that does not exist +* Contributors: Zerom + 0.2.8 (2018-03-20) ------------------ * added RH-P12-RN.device file diff --git a/robotis_device/package.xml b/robotis_device/package.xml index 565f964..09a81af 100755 --- a/robotis_device/package.xml +++ b/robotis_device/package.xml @@ -1,7 +1,7 @@ robotis_device - 0.2.8 + 0.2.9 The package that manages device information of ROBOTIS robots. This package is used when reading device information with the robot information file diff --git a/robotis_framework/CHANGELOG.rst b/robotis_framework/CHANGELOG.rst index 0424994..794034b 100755 --- a/robotis_framework/CHANGELOG.rst +++ b/robotis_framework/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package robotis_framework ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.9 (2018-03-22) +------------------ +* added serivce for setting module +* deleted comment for debug +* modified to prevent duplicate indirect address write +* added boost system dependencies +* fixed a bug that occure when handling bulk read item that does not exist +* Contributors: Kayman, Zerom, Pyo + 0.2.8 (2018-03-20) ------------------ * added RH-P12-RN.device file diff --git a/robotis_framework/package.xml b/robotis_framework/package.xml index fd9efbe..775bdd5 100755 --- a/robotis_framework/package.xml +++ b/robotis_framework/package.xml @@ -1,7 +1,7 @@ robotis_framework - 0.2.8 + 0.2.9 ROS packages for the robotis_framework (meta package) diff --git a/robotis_framework_common/CHANGELOG.rst b/robotis_framework_common/CHANGELOG.rst index d4c4bd6..7b3a4de 100755 --- a/robotis_framework_common/CHANGELOG.rst +++ b/robotis_framework_common/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package robotis_framework_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.9 (2018-03-22) +------------------ +* none + 0.2.8 (2018-03-20) ------------------ * tested for system dependencies diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index 47f8a65..08a423b 100755 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -1,7 +1,7 @@ robotis_framework_common - 0.2.8 + 0.2.9 The package contains commonly used Headers for the ROBOTIS Framework.