diff --git a/robotis_controller/CHANGELOG.rst b/robotis_controller/CHANGELOG.rst new file mode 100644 index 0000000..2a476ea --- /dev/null +++ b/robotis_controller/CHANGELOG.rst @@ -0,0 +1,23 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package robotis_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.1 (2016-08-18) +----------- +* updated the package information + +0.1.0 (2016-08-12) +----------- +* first public release for Kinetic +* modified the package information for release +* develop branch -> master branch +* function name changed : DeviceInit() -> InitDevice() +* Fixed high CPU consumption due to busy waits +* add SensorState + add Singleton template +* XM-430 / CM-740 device file added. + Sensor device added. +* added code to support the gazebo simulator +* added first bulk read failure protection code +* renewal +* Contributors: Alexander Stumpf, Jay Song, Zerom, Pyo diff --git a/robotis_controller/CMakeLists.txt b/robotis_controller/CMakeLists.txt index 97c4a6d..8f36296 100644 --- a/robotis_controller/CMakeLists.txt +++ b/robotis_controller/CMakeLists.txt @@ -1,8 +1,14 @@ +################################################################################ +# CMake +################################################################################ cmake_minimum_required(VERSION 2.8.3) project(robotis_controller) set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") +################################################################################ +# Packages +################################################################################ find_package(catkin REQUIRED COMPONENTS roscpp roslib @@ -15,23 +21,48 @@ find_package(catkin REQUIRED COMPONENTS dynamixel_sdk ) +################################################################################ +# Declare ROS messages, services and actions +################################################################################ + +################################################################################ +# Declare ROS dynamic reconfigure parameters +################################################################################ + +################################################################################ +# Catkin specific configuration +################################################################################ catkin_package( INCLUDE_DIRS include LIBRARIES robotis_controller CATKIN_DEPENDS roscpp roslib sensor_msgs std_msgs -# DEPENDS system_lib ) +################################################################################ +# Build +################################################################################ include_directories( include ${catkin_INCLUDE_DIRS} ) -add_library(robotis_controller - src/robotis_controller/robotis_controller.cpp +add_library(robotis_controller src/robotis_controller/robotis_controller.cpp) +add_dependencies(robotis_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(robotis_controller yaml-cpp ${catkin_LIBRARIES}) + +################################################################################ +# Install +################################################################################ +install(TARGETS robotis_controller + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) -target_link_libraries(robotis_controller - yaml-cpp - ${catkin_LIBRARIES} +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) + +################################################################################ +# Test +################################################################################ diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index 6dcf6d2..b86d7bf 100644 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -2,18 +2,16 @@ robotis_controller 0.1.1 - The robotis_controller package - - ROBOTIS - + + The main package that controls THORMANG3. + BSD - - - - ROBOTIS - + Zerom + Pyo + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework + http://wiki.ros.org/robotis_controller catkin - roscpp roslib std_msgs @@ -22,7 +20,6 @@ robotis_device robotis_controller_msgs robotis_framework_common - roscpp roslib std_msgs @@ -30,5 +27,5 @@ dynamixel_sdk robotis_device robotis_controller_msgs - + diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 35c81b1..1bea26e 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -348,7 +348,119 @@ bool RobotisController::initialize(const std::string robot_file_path, const std: initializeDevice(init_file_path); + queue_thread_ = boost::thread(boost::bind(&RobotisController::msgQueueThread, this)); + return true; +} + +void RobotisController::initializeDevice(const std::string init_file_path) +{ + // device initialize + if (DEBUG_PRINT) + ROS_WARN("INIT FILE LOAD"); + + YAML::Node doc; + try + { + doc = YAML::LoadFile(init_file_path.c_str()); + + for (YAML::const_iterator it_doc = doc.begin(); it_doc != doc.end(); it_doc++) + { + std::string joint_name = it_doc->first.as(); + + YAML::Node joint_node = doc[joint_name]; + if (joint_node.size() == 0) + continue; + + Dynamixel *dxl = NULL; + auto dxl_it = robot_->dxls_.find(joint_name); + if (dxl_it != robot_->dxls_.end()) + dxl = dxl_it->second; + + if (dxl == NULL) + { + ROS_WARN("Joint [%s] was not found.", joint_name.c_str()); + continue; + } + if (DEBUG_PRINT) + ROS_INFO("JOINT_NAME: %s", joint_name.c_str()); + + for (YAML::const_iterator it_joint = joint_node.begin(); it_joint != joint_node.end(); it_joint++) + { + std::string item_name = it_joint->first.as(); + + if (DEBUG_PRINT) + ROS_INFO(" ITEM_NAME: %s", item_name.c_str()); + + uint32_t value = it_joint->second.as(); + + ControlTableItem *item = dxl->ctrl_table_[item_name]; + if (item == NULL) + { + ROS_WARN("Control Item [%s] was not found.", item_name.c_str()); + continue; + } + + if (item->memory_type_ == EEPROM) + { + uint8_t data8 = 0; + uint16_t data16 = 0; + uint32_t data32 = 0; + + switch (item->data_length_) + { + case 1: + read1Byte(joint_name, item->address_, &data8); + if (data8 == value) + continue; + break; + case 2: + read2Byte(joint_name, item->address_, &data16); + if (data16 == value) + continue; + break; + case 4: + read4Byte(joint_name, item->address_, &data32); + if (data32 == value) + continue; + break; + default: + break; + } + } + + switch (item->data_length_) + { + case 1: + write1Byte(joint_name, item->address_, (uint8_t) value); + break; + case 2: + write2Byte(joint_name, item->address_, (uint16_t) value); + break; + case 4: + write4Byte(joint_name, item->address_, value); + break; + default: + break; + } + + if (item->memory_type_ == EEPROM) + { + // Write to EEPROM -> delay is required (max delay: 55 msec per byte) + usleep(item->data_length_ * 55 * 1000); + } + } + } + } catch (const std::exception& e) + { + ROS_INFO("Dynamixel Init file not found."); + } + // [ BulkRead ] StartAddress : Present Position , Length : 10 ( Position/Velocity/Current ) + for (auto& it : robot_->ports_) + { + if (port_to_bulk_read_[it.first] != 0) + port_to_bulk_read_[it.first]->clearParam(); + } for (auto& it : robot_->dxls_) { std::string joint_name = it.first; @@ -487,113 +599,6 @@ bool RobotisController::initialize(const std::string robot_file_path, const std: if (bulkread_start_addr != 0) port_to_bulk_read_[sensor->port_name_]->addParam(sensor->id_, bulkread_start_addr, bulkread_data_length); } - - queue_thread_ = boost::thread(boost::bind(&RobotisController::msgQueueThread, this)); - return true; -} - -void RobotisController::initializeDevice(const std::string init_file_path) -{ - // device initialize - if (DEBUG_PRINT) - ROS_WARN("INIT FILE LOAD"); - - YAML::Node doc; - try - { - doc = YAML::LoadFile(init_file_path.c_str()); - - for (YAML::const_iterator it_doc = doc.begin(); it_doc != doc.end(); it_doc++) - { - std::string joint_name = it_doc->first.as(); - - YAML::Node joint_node = doc[joint_name]; - if (joint_node.size() == 0) - continue; - - Dynamixel *dxl = NULL; - auto dxl_it = robot_->dxls_.find(joint_name); - if (dxl_it != robot_->dxls_.end()) - dxl = dxl_it->second; - - if (dxl == NULL) - { - ROS_WARN("Joint [%s] was not found.", joint_name.c_str()); - continue; - } - if (DEBUG_PRINT) - ROS_INFO("JOINT_NAME: %s", joint_name.c_str()); - - for (YAML::const_iterator it_joint = joint_node.begin(); it_joint != joint_node.end(); it_joint++) - { - std::string item_name = it_joint->first.as(); - - if (DEBUG_PRINT) - ROS_INFO(" ITEM_NAME: %s", item_name.c_str()); - - uint32_t value = it_joint->second.as(); - - ControlTableItem *item = dxl->ctrl_table_[item_name]; - if (item == NULL) - { - ROS_WARN("Control Item [%s] was not found.", item_name.c_str()); - continue; - } - - if (item->memory_type_ == EEPROM) - { - uint8_t data8 = 0; - uint16_t data16 = 0; - uint32_t data32 = 0; - - switch (item->data_length_) - { - case 1: - read1Byte(joint_name, item->address_, &data8); - if (data8 == value) - continue; - break; - case 2: - read2Byte(joint_name, item->address_, &data16); - if (data16 == value) - continue; - break; - case 4: - read4Byte(joint_name, item->address_, &data32); - if (data32 == value) - continue; - break; - default: - break; - } - } - - switch (item->data_length_) - { - case 1: - write1Byte(joint_name, item->address_, (uint8_t) value); - break; - case 2: - write2Byte(joint_name, item->address_, (uint16_t) value); - break; - case 4: - write4Byte(joint_name, item->address_, value); - break; - default: - break; - } - - if (item->memory_type_ == EEPROM) - { - // Write to EEPROM -> delay is required (max delay: 55 msec per byte) - usleep(item->data_length_ * 55 * 1000); - } - } - } - } catch (const std::exception& e) - { - ROS_INFO("Dynamixel Init file not found."); - } } void RobotisController::gazeboTimerThread() @@ -1186,6 +1191,16 @@ void RobotisController::process() // SyncWrite if (gazebo_mode_ == false && do_sync_write) { + if (direct_sync_write_.size() > 0) + { + for (int i = 0; i < direct_sync_write_.size(); i++) + { + direct_sync_write_[i]->txPacket(); + direct_sync_write_[i]->clearParam(); + } + direct_sync_write_.clear(); + } + if (port_to_sync_write_position_p_gain_.size() > 0) { for (auto& it : port_to_sync_write_position_p_gain_) diff --git a/robotis_device/CHANGELOG.rst b/robotis_device/CHANGELOG.rst new file mode 100644 index 0000000..9a5117c --- /dev/null +++ b/robotis_device/CHANGELOG.rst @@ -0,0 +1,24 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package robotis_device +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.1 (2016-08-18) +----------- +* updated the package information + +0.1.0 (2016-08-12) +----------- +* first public release for Kinetic +* modified the package information for release +* develop branch -> master branch +* Setting the license to BSD. +* add SensorState + add Singleton template +* XM-430 / CM-740 device file added. + Sensor device added. +* modified. +* variable name changed. + ConvertRadian2Value / ConvertValue2Radian function bug fixed. +* added code to support the gazebo simulator +* renewal +* Contributors: ROBOTIS, ROBOTIS-zerom, pyo diff --git a/robotis_device/CMakeLists.txt b/robotis_device/CMakeLists.txt index e492450..abc1965 100644 --- a/robotis_device/CMakeLists.txt +++ b/robotis_device/CMakeLists.txt @@ -1,19 +1,38 @@ +################################################################################ +# CMake +################################################################################ cmake_minimum_required(VERSION 2.8.3) project(robotis_device) +################################################################################ +# Packages +################################################################################ find_package(catkin REQUIRED COMPONENTS roscpp rospy dynamixel_sdk ) +################################################################################ +# Declare ROS messages, services and actions +################################################################################ + +################################################################################ +# Declare ROS dynamic reconfigure parameters +################################################################################ + +################################################################################ +# Catkin specific configuration +################################################################################ catkin_package( INCLUDE_DIRS include LIBRARIES robotis_device CATKIN_DEPENDS roscpp rospy -# DEPENDS system_lib ) +################################################################################ +# Build +################################################################################ include_directories( include ${catkin_INCLUDE_DIRS} @@ -24,9 +43,26 @@ add_library(robotis_device src/robotis_device/sensor.cpp src/robotis_device/dynamixel.cpp ) - add_dependencies(robotis_device ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(robotis_device ${catkin_LIBRARIES}) -target_link_libraries(robotis_device - ${catkin_LIBRARIES} +################################################################################ +# Install +################################################################################ +install(TARGETS robotis_device + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +install(DIRECTORY devices/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +################################################################################ +# Test +################################################################################ diff --git a/robotis_device/package.xml b/robotis_device/package.xml index 6f7294a..e5caa92 100644 --- a/robotis_device/package.xml +++ b/robotis_device/package.xml @@ -1,24 +1,23 @@ robotis_device - 0.1.0 - The robotis_device package - - robotis - + 0.1.1 + + The package that manages device information of ROBOTIS robots. + This package is used when reading device information with the robot information file + from the robotis_controller package. + BSD - - - - robotis - + Zerom + Pyo + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework + http://wiki.ros.org/robotis_device catkin - roscpp rospy dynamixel_sdk - roscpp rospy dynamixel_sdk - \ No newline at end of file + diff --git a/robotis_framework/CHANGELOG.rst b/robotis_framework/CHANGELOG.rst new file mode 100644 index 0000000..8f61ce3 --- /dev/null +++ b/robotis_framework/CHANGELOG.rst @@ -0,0 +1,12 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package robotis_framework +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.1 (2016-08-18) +----------- +* updated the package information + +0.1.0 (2016-08-12) +----------- +* make a meta-package +* Contributors: Zerom, Pyo diff --git a/robotis_framework/CMakeLists.txt b/robotis_framework/CMakeLists.txt new file mode 100644 index 0000000..d336fe9 --- /dev/null +++ b/robotis_framework/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 2.8.3) +project(robotis_framework) +find_package(catkin REQUIRED) +catkin_metapackage() diff --git a/robotis_framework/package.xml b/robotis_framework/package.xml new file mode 100644 index 0000000..809e65e --- /dev/null +++ b/robotis_framework/package.xml @@ -0,0 +1,17 @@ + + + robotis_framework + 0.1.1 + ROS packages for the robotis_framework (meta package) + BSD + Zerom + Pyo + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework + http://wiki.ros.org/robotis_framework + catkin + robotis_framework_common + robotis_device + robotis_controller + + diff --git a/robotis_framework_common/CHANGELOG.rst b/robotis_framework_common/CHANGELOG.rst new file mode 100644 index 0000000..bc63720 --- /dev/null +++ b/robotis_framework_common/CHANGELOG.rst @@ -0,0 +1,15 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package robotis_framework_common +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.1 (2016-08-18) +----------- +* updated the package information + +0.1.0 (2016-08-12) +----------- +* modified the package information for release +* Setting the license to BSD. +* add SensorState + add Singleton template +* Contributors: Jay Song, Zerom, Pyo diff --git a/robotis_framework_common/CMakeLists.txt b/robotis_framework_common/CMakeLists.txt index 51f0f3f..20c330f 100644 --- a/robotis_framework_common/CMakeLists.txt +++ b/robotis_framework_common/CMakeLists.txt @@ -1,18 +1,45 @@ +################################################################################ +# CMake +################################################################################ cmake_minimum_required(VERSION 2.8.3) project(robotis_framework_common) +################################################################################ +# Packages +################################################################################ find_package(catkin REQUIRED COMPONENTS roscpp ) +################################################################################ +# Declare ROS messages, services and actions +################################################################################ + +################################################################################ +# Declare ROS dynamic reconfigure parameters +################################################################################ + +################################################################################ +# Catkin specific configuration +################################################################################ catkin_package( INCLUDE_DIRS include -# LIBRARIES robotis_framework_common -# CATKIN_DEPENDS roscpp -# DEPENDS system_lib ) +################################################################################ +# Build +################################################################################ include_directories( ${catkin_INCLUDE_DIRS} ) +################################################################################ +# Install +################################################################################ +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +################################################################################ +# Test +################################################################################ diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index 820f4a2..f01e4d5 100644 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -1,15 +1,17 @@ robotis_framework_common - 0.1.0 - The robotis_framework_common package - robotis - + 0.1.1 + + The package contains commonly used Headers for the ROBOTIS Framework. + BSD - - ROBOTIS - + Zerom + Pyo + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework + http://wiki.ros.org/robotis_framework_common catkin roscpp roscpp - \ No newline at end of file +