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

Merge remote-tracking branch 'remotes/origin/master' into develop

Browse Source
This commit is contained in:
ROBOTIS-zerom
2016-08-19 17:08:09 +09:00
parent 3ff7e4fc25 d017cd622a
commit a0c6553cff
13 changed files with 355 additions and 153 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
robotis_controller/CHANGELOG.rst Normal file
View File

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

43
robotis_controller/CMakeLists.txt
View File

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

21
robotis_controller/package.xml
View File

@ -2,18 +2,16 @@
<package>
<name>robotis_controller</name>
<version>0.1.1</version>
<description>The robotis_controller package</description>
<maintainer email="zerom@robotis.com">ROBOTIS</maintainer>
<description>
The main package that controls THORMANG3.
</description>
<license>BSD</license>
<!-- <url type="website">http://wiki.ros.org/robotis_controller</url> -->
<author email="zerom@robotis.com">ROBOTIS</author>
<author email="zerom@robotis.com">Zerom</author>
<maintainer email="pyo@robotis.com">Pyo</maintainer>
<url type="bugtracker">https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues</url>
<url type="repository">https://github.com/ROBOTIS-GIT/ROBOTIS-Framework</url>
<url type="website">http://wiki.ros.org/robotis_controller</url>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>roslib</build_depend>
<build_depend>std_msgs</build_depend>
@ -22,7 +20,6 @@
<build_depend>robotis_device</build_depend>
<build_depend>robotis_controller_msgs</build_depend>
<build_depend>robotis_framework_common</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>roslib</run_depend>
<run_depend>std_msgs</run_depend>
@ -30,5 +27,5 @@
<run_depend>dynamixel_sdk</run_depend>
<run_depend>robotis_device</run_depend>
<run_depend>robotis_controller_msgs</run_depend>
<export></export>
</package>

229
robotis_controller/src/robotis_controller/robotis_controller.cpp
View File

@ -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<std::string>();
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<std::string>();
if (DEBUG_PRINT)
ROS_INFO(" ITEM_NAME: %s", item_name.c_str());
uint32_t value = it_joint->second.as<uint32_t>();
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<std::string>();
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<std::string>();
if (DEBUG_PRINT)
ROS_INFO(" ITEM_NAME: %s", item_name.c_str());
uint32_t value = it_joint->second.as<uint32_t>();
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_)

24
robotis_device/CHANGELOG.rst Normal file
View File

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

44
robotis_device/CMakeLists.txt
View File

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

25
robotis_device/package.xml
View File

@ -1,24 +1,23 @@
<?xml version="1.0"?>
<package>
<name>robotis_device</name>
<version>0.1.0</version>
<description>The robotis_device package</description>
<maintainer email="zerom@robotis.com">robotis</maintainer>
<version>0.1.1</version>
<description>
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.
</description>
<license>BSD</license>
<!-- <url type="website">http://wiki.ros.org/robotis_device</url> -->
<author email="zerom@robotis.com">robotis</author>
<author email="zerom@robotis.com">Zerom</author>
<maintainer email="pyo@robotis.com">Pyo</maintainer>
<url type="bugtracker">https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues</url>
<url type="repository">https://github.com/ROBOTIS-GIT/ROBOTIS-Framework</url>
<url type="website">http://wiki.ros.org/robotis_device</url>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>dynamixel_sdk</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>rospy</run_depend>
<run_depend>dynamixel_sdk</run_depend>
</package>
</package>

12
robotis_framework/CHANGELOG.rst Normal file
View File

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

4
robotis_framework/CMakeLists.txt Normal file
View File

@ -0,0 +1,4 @@
cmake_minimum_required(VERSION 2.8.3)
project(robotis_framework)
find_package(catkin REQUIRED)
catkin_metapackage()

17
robotis_framework/package.xml Normal file
View File

@ -0,0 +1,17 @@
<?xml version="1.0"?>
<package>
<name>robotis_framework</name>
<version>0.1.1</version>
<description>ROS packages for the robotis_framework (meta package)</description>
<license>BSD</license>
<author email="zerom@robotis.com">Zerom</author>
<maintainer email="pyo@robotis.com">Pyo</maintainer>
<url type="bugtracker">https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues</url>
<url type="repository">https://github.com/ROBOTIS-GIT/ROBOTIS-Framework</url>
<url type="website">http://wiki.ros.org/robotis_framework</url>
<buildtool_depend>catkin</buildtool_depend>
<run_depend>robotis_framework_common</run_depend>
<run_depend>robotis_device</run_depend>
<run_depend>robotis_controller</run_depend>
<export><metapackage/></export>
</package>

15
robotis_framework_common/CHANGELOG.rst Normal file
View File

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

33
robotis_framework_common/CMakeLists.txt
View File

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

18
robotis_framework_common/package.xml
View File

@ -1,15 +1,17 @@
<?xml version="1.0"?>
<package>
<name>robotis_framework_common</name>
<version>0.1.0</version>
<description>The robotis_framework_common package</description>
<maintainer email="zerom@robotis.com">robotis</maintainer>
<version>0.1.1</version>
<description>
The package contains commonly used Headers for the ROBOTIS Framework.
</description>
<license>BSD</license>
<author email="zerom@robotis.com">ROBOTIS</author>
<author email="zerom@robotis.com">Zerom</author>
<maintainer email="pyo@robotis.com">Pyo</maintainer>
<url type="bugtracker">https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues</url>
<url type="repository">https://github.com/ROBOTIS-GIT/ROBOTIS-Framework</url>
<url type="website">http://wiki.ros.org/robotis_framework_common</url>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<run_depend>roscpp</run_depend>
</package>
</package>