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

Merge pull request #69 from ROBOTIS-GIT/master

Browse Source 
sync (master -> develop)
This commit is contained in:
zerom
2018-11-05 10:38:55 +09:00
committed by GitHub
parent 02acdb521c cd58b92487
commit e7afc19525
14 changed files with 142 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
.robotis_framework.rosinstall Normal file
View File

@ -0,0 +1 @@
- git: {local-name: robotis_controller_msgs, uri: 'https://github.com/ROBOTIS-GIT/ROBOTIS-Framework-msgs.git', version: master}

5
.travis.yml
View File

@ -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

24
README.md
View File

@ -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/)
## 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/)

9
robotis_controller/CHANGELOG.rst
View File

@ -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)

6
robotis_controller/CMakeLists.txt
View File

@ -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

5
robotis_controller/include/robotis_controller/robotis_controller.h
View File

@ -28,6 +28,7 @@
#include <ros/ros.h>
#include <boost/thread.hpp>
#include <yaml-cpp/yaml.h>
#include <std_msgs/Bool.h>
#include <std_msgs/String.h>
#include <std_msgs/Float64.h>
#include <sensor_msgs/JointState.h>
@ -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);

2
robotis_controller/package.xml
View File

@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="2">
<name>robotis_controller</name>
<version>0.2.8</version>
<version>0.2.9</version>
<description>
robotis_controller package for ROBOTIS's platform like Manipulator-H, THORMANG and OP series
</description>

93
robotis_controller/src/robotis_controller/robotis_controller.cpp
View File

@ -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));

6
robotis_device/CHANGELOG.rst
View File

@ -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

2
robotis_device/package.xml
View File

@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="2">
<name>robotis_device</name>
<version>0.2.8</version>
<version>0.2.9</version>
<description>
The package that manages device information of ROBOTIS robots.
This package is used when reading device information with the robot information file

9
robotis_framework/CHANGELOG.rst
View File

@ -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

2
robotis_framework/package.xml
View File

@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="2">
<name>robotis_framework</name>
<version>0.2.8</version>
<version>0.2.9</version>
<description>
ROS packages for the robotis_framework (meta package)
</description>

4
robotis_framework_common/CHANGELOG.rst
View File

@ -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

2
robotis_framework_common/package.xml
View File

@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="2">
<name>robotis_framework_common</name>
<version>0.2.8</version>
<version>0.2.9</version>
<description>
The package contains commonly used Headers for the ROBOTIS Framework.
</description>