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 #6 from RonaldsonBellande/main

Browse Source 
latest changes
This commit is contained in:
Ronaldson Bellande
2023-10-03 23:44:29 -04:00
committed by GitHub
parent bc9d78c13d c7565da012
commit c4ba60e508
16 changed files with 445 additions and 395 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
LICENSE
View File

@ -186,7 +186,7 @@
same "printed page" as the copyright notice for easier
identification within third-party archives.
Copyright {yyyy} {name of copyright owner}
Copyright 2023 Ronaldson Bellande
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.

5
README.md
View File

@ -14,6 +14,11 @@ the new commits and codes are under New License
Latest versions and Maintainer is on organization https://github.com/Robotics-Sensors
# USE CASE
--------------------------------------------------------------------------------------------------------
* Every repository within our organization is a valuable resource that can be utilized for educational purposes by individuals who actively contribute to the repository or choose to become sponsors of the organization. For those who wish to use our services conversationally, the acquisition of a license and subscription from our company website is mandatory. This ensures that the services are appropriately compensated for their use. Additionally, for the 90% of services designated as private, acquiring a license and subscription can be facilitated through our company website. We encourage interested parties to visit our website to explore and procure the necessary licenses and subscriptions for the diverse range of services and products we offer. Your support and commitment enable us to maintain and enhance the quality of our offerings, contributing to a thriving collaborative environment.
--------------------------------------------------------------------------------------------------------
### Maintainer
* Ronaldson Bellande

100
humanoid_robot_controller/CMakeLists.txt
View File

@ -1,32 +1,31 @@
################################################################################
# Set minimum required version of cmake, project name and compile options
################################################################################
cmake_minimum_required(VERSION 3.0.2)
cmake_minimum_required(VERSION 3.8)
project(humanoid_robot_controller)
## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++11)
################################################################################
# Find catkin packages and libraries for catkin and system dependencies
################################################################################
find_package(catkin REQUIRED COMPONENTS
roscpp
roslib
std_msgs
sensor_msgs
humanoid_robot_controller_msgs
dynamixel_sdk
humanoid_robot_device
humanoid_robot_framework_common
cmake_modules
)
find_package(Boost REQUIRED)
if($ENV{ROS_VERSION} EQUAL 1)
find_package(catkin REQUIRED COMPONENTS
roscpp
roslib
std_msgs
sensor_msgs
humanoid_robot_controller_msgs
dynamixel_sdk
humanoid_robot_device
humanoid_robot_framework_common
cmake_modules
)
find_package(Boost REQUIRED)
find_package(PkgConfig REQUIRED)
else()
find_package(ament_cmake REQUIRED)
endif()
# Resolve system dependency on yaml-cpp, which apparently does not
# provide a CMake find_package() module.
find_package(PkgConfig REQUIRED)
pkg_check_modules(YAML_CPP REQUIRED yaml-cpp)
find_path(YAML_CPP_INCLUDE_DIR
NAMES yaml_cpp.h
@ -42,40 +41,28 @@ if(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5")
add_definitions(-DHAVE_NEW_YAMLCPP)
endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5")
################################################################################
# Setup for python modules and scripts
################################################################################
################################################################################
# Declare ROS messages, services and actions
################################################################################
if($ENV{ROS_VERSION} EQUAL 1)
catkin_package(
INCLUDE_DIRS include
LIBRARIES humanoid_robot_controller
CATKIN_DEPENDS
roscpp
roslib
std_msgs
sensor_msgs
humanoid_robot_controller_msgs
dynamixel_sdk
humanoid_robot_device
humanoid_robot_framework_common
cmake_modules
DEPENDS Boost
)
else()
ament_package()
endif()
################################################################################
# Declare ROS dynamic reconfigure parameters
################################################################################
################################################################################
# Declare catkin specific configuration to be passed to dependent projects
################################################################################
catkin_package(
INCLUDE_DIRS include
LIBRARIES humanoid_robot_controller
CATKIN_DEPENDS
roscpp
roslib
std_msgs
sensor_msgs
humanoid_robot_controller_msgs
dynamixel_sdk
humanoid_robot_device
humanoid_robot_framework_common
cmake_modules
DEPENDS Boost
)
################################################################################
# Build
################################################################################
include_directories(
include
${catkin_INCLUDE_DIRS}
@ -87,9 +74,6 @@ add_library(humanoid_robot_controller src/humanoid_robot_controller/humanoid_rob
add_dependencies(humanoid_robot_controller ${catkin_EXPORTED_TARGETS})
target_link_libraries(humanoid_robot_controller ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${YAML_CPP_LIBRARIES})
################################################################################
# Install
################################################################################
install(TARGETS humanoid_robot_controller
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
@ -99,7 +83,3 @@ install(TARGETS humanoid_robot_controller
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
################################################################################
# Test
################################################################################

146
humanoid_robot_controller/include/humanoid_robot_controller/humanoid_robot_controller.h
View File

@ -1,17 +1,17 @@
/*******************************************************************************
* Copyright 2018 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
/*******************************************************************************
* Copyright 2018 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
/*
@ -22,29 +22,29 @@
*/
#ifndef ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
/*
* humanoid_robot_controller.h
*
#define ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_
* Author: zerom
*/
#ifndef ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_
#define ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_
#include <boost/thread.hpp>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float64.h>
#include <std_msgs/String.h>
#include <yaml-cpp/yaml.h>
#include <boost/thread.hpp>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include "humanoid_robot_controller_msgs/GetJointModule.h"
#include "humanoid_robot_controller_msgs/JointCtrlModule.h"
#include "humanoid_robot_controller_msgs/LoadOffset.h"
#include "humanoid_robot_controller_msgs/SetJointModule.h"
#include "humanoid_robot_controller_msgs/SetModule.h"
#include "humanoid_robot_controller_msgs/SyncWriteItem.h"
#include "humanoid_robot_controller_msgs/WriteControlTable.h"
#include "dynamixel_sdk/group_bulk_read.h"
#include "dynamixel_sdk/group_sync_write.h"
#include "humanoid_robot_device/robot.h"
#include "humanoid_robot_framework_common/motion_module.h"
#include "humanoid_robot_framework_common/sensor_module.h"
namespace humanoid_robot_framework {
@ -74,7 +74,7 @@ private:
void gazeboTimerThread();
void msgQueueThread();
void setCtrlModuleThread(std::string ctrl_module);
#include "humanoid_robot_device/robot.h"
void setJointCtrlModuleThread(
const humanoid_robot_controller_msgs::JointCtrlModule::ConstPtr &msg);
bool isTimerStopped();
@ -92,23 +92,23 @@ public:
std::map<std::string, dynamixel::GroupBulkRead *> port_to_bulk_read_;
/* sync write */
bool init_pose_loaded_;
std::map<std::string, dynamixel::GroupSyncWrite *>
port_to_sync_write_position_;
bool is_offset_enabled_;
std::map<std::string, dynamixel::GroupSyncWrite *>
port_to_sync_write_velocity_;
bool stop_timer_;
std::map<std::string, dynamixel::GroupSyncWrite *>
port_to_sync_write_current_;
ControllerMode controller_mode_;
std::map<std::string, dynamixel::GroupSyncWrite *>
port_to_sync_write_position_p_gain_;
std::list<MotionModule *> motion_modules_;
std::map<std::string, dynamixel::GroupSyncWrite *>
port_to_sync_write_position_i_gain_;
std::vector<dynamixel::GroupSyncWrite *> direct_sync_write_;
std::map<std::string, dynamixel::GroupSyncWrite *>
port_to_sync_write_position_d_gain_;
std::map<std::string, double> sensor_result_;
std::map<std::string, dynamixel::GroupSyncWrite *>
port_to_sync_write_velocity_p_gain_;
void gazeboTimerThread();
std::map<std::string, dynamixel::GroupSyncWrite *>
port_to_sync_write_velocity_i_gain_;
void setCtrlModuleThread(std::string ctrl_module);
std::map<std::string, dynamixel::GroupSyncWrite *>
port_to_sync_write_velocity_d_gain_;
/* publisher */
@ -124,7 +124,7 @@ public:
RobotisController();
/* bulk read */
bool initialize(const std::string robot_file_path,
const std::string init_file_path);
void initializeDevice(const std::string init_file_path);
void process();
@ -142,66 +142,68 @@ public:
void loadOffset(const std::string path);
/* ROS Topic Callback Functions */
port_to_sync_write_position_i_gain_;
void writeControlTableCallback(
const humanoid_robot_controller_msgs::WriteControlTable::ConstPtr &msg);
void syncWriteItemCallback(
const humanoid_robot_controller_msgs::SyncWriteItem::ConstPtr &msg);
void setControllerModeCallback(const std_msgs::String::ConstPtr &msg);
void setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg);
port_to_sync_write_velocity_p_gain_;
void setJointCtrlModuleCallback(
const humanoid_robot_controller_msgs::JointCtrlModule::ConstPtr &msg);
void setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg);
void enableOffsetCallback(const std_msgs::Bool::ConstPtr &msg);
std::map<std::string, dynamixel::GroupSyncWrite *>
bool getJointCtrlModuleService(
humanoid_robot_controller_msgs::GetJointModule::Request &req,
humanoid_robot_controller_msgs::GetJointModule::Response &res);
/* publisher */
bool setJointCtrlModuleService(
humanoid_robot_controller_msgs::SetJointModule::Request &req,
humanoid_robot_controller_msgs::SetJointModule::Response &res);
ros::Publisher present_joint_state_pub_;
ros::Publisher current_module_pub_;
std::map<std::string, ros::Publisher> gazebo_joint_position_pub_;
bool setCtrlModuleService(
humanoid_robot_controller_msgs::SetModule::Request &req,
humanoid_robot_controller_msgs::SetModule::Response &res);
bool
loadOffsetService(humanoid_robot_controller_msgs::LoadOffset::Request &req,
humanoid_robot_controller_msgs::LoadOffset::Response &res);
void gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg);
int ping(const std::string joint_name, uint8_t *error = 0);
int ping(const std::string joint_name, uint16_t *model_number,
uint8_t *error = 0);
int action(const std::string joint_name);
int reboot(const std::string joint_name, uint8_t *error = 0);
const std::string init_file_path);
int factoryReset(const std::string joint_name, uint8_t option = 0,
uint8_t *error = 0);
int read(const std::string joint_name, uint16_t address, uint16_t length,
uint8_t *data, uint8_t *error = 0);
void removeMotionModule(MotionModule *module);
int readCtrlItem(const std::string joint_name, const std::string item_name,
uint32_t *data, uint8_t *error = 0);
int read1Byte(const std::string joint_name, uint16_t address, uint8_t *data,
uint8_t *error = 0);
void stopTimer();
int read2Byte(const std::string joint_name, uint16_t address, uint16_t *data,
uint8_t *error = 0);
int read4Byte(const std::string joint_name, uint16_t address, uint32_t *data,
uint8_t *error = 0);
int write(const std::string joint_name, uint16_t address, uint16_t length,
uint8_t *data, uint8_t *error = 0);
void writeControlTableCallback(
int writeCtrlItem(const std::string joint_name, const std::string item_name,
uint32_t data, uint8_t *error = 0);
void syncWriteItemCallback(
int write1Byte(const std::string joint_name, uint16_t address, uint8_t data,
uint8_t *error = 0);
const humanoid_robot_controller_msgs::SyncWriteItem::ConstPtr &msg);
int write2Byte(const std::string joint_name, uint16_t address, uint16_t data,
uint8_t *error = 0);
void setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg);
int write4Byte(const std::string joint_name, uint16_t address, uint32_t data,
uint8_t *error = 0);
const humanoid_robot_controller_msgs::JointCtrlModule::ConstPtr &msg);
int regWrite(const std::string joint_name, uint16_t address, uint16_t length,
uint8_t *data, uint8_t *error = 0);
};
humanoid_robot_controller_msgs::GetJointModule::Request &req,
} // namespace humanoid_robot_framework
#endif /* ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ */

105
humanoid_robot_controller/package.xml
View File

@ -1,44 +1,85 @@
<?xml version="1.0"?>
<package format="2">
<package format="3">
<name>humanoid_robot_controller</name>
<version>0.3.0</version>
<description>humanoid_robot_controller package for ROBOTIS's platform like Manipulator-H, THORMANG and OP series</description>
<license>Apache 2.0</license>
<maintainer email="ronaldsonbellande@gmail.com">Ronaldson Bellande</maintainer>
<buildtool_depend>catkin</buildtool_depend>
<buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>roslib</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>humanoid_robot_controller_msgs</build_depend>
<build_depend>dynamixel_sdk</build_depend>
<build_depend>humanoid_robot_device</build_depend>
<build_depend>humanoid_robot_framework_common</build_depend>
<build_depend>cmake_modules</build_depend>
<build_depend>yaml-cpp</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>roslib</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>humanoid_robot_controller_msgs</build_export_depend>
<build_export_depend>dynamixel_sdk</build_export_depend>
<build_export_depend>humanoid_robot_device</build_export_depend>
<build_export_depend>humanoid_robot_framework_common</build_export_depend>
<build_export_depend>cmake_modules</build_export_depend>
<build_export_depend>yaml-cpp</build_export_depend>
<condition if="$ROS_VERSION == 1">
<build_depend>roscpp</build_depend>
<build_depend>roslib</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>humanoid_robot_controller_msgs</build_depend>
<build_depend>dynamixel_sdk</build_depend>
<build_depend>humanoid_robot_device</build_depend>
<build_depend>humanoid_robot_framework_common</build_depend>
<build_depend>cmake_modules</build_depend>
<build_depend>yaml-cpp</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>roslib</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>humanoid_robot_controller_msgs</build_export_depend>
<build_export_depend>dynamixel_sdk</build_export_depend>
<build_export_depend>humanoid_robot_device</build_export_depend>
<build_export_depend>humanoid_robot_framework_common</build_export_depend>
<build_export_depend>cmake_modules</build_export_depend>
<build_export_depend>yaml-cpp</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>roslib</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>humanoid_robot_controller_msgs</exec_depend>
<exec_depend>dynamixel_sdk</exec_depend>
<exec_depend>humanoid_robot_device</exec_depend>
<exec_depend>humanoid_robot_framework_common</exec_depend>
<exec_depend>cmake_modules</exec_depend>
<exec_depend>yaml-cpp</exec_depend>
</condition>
<condition if="$ROS_VERSION == 2">
<build_depend>roscpp</build_depend>
<build_depend>roslib</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>humanoid_robot_controller_msgs</build_depend>
<build_depend>dynamixel_sdk</build_depend>
<build_depend>humanoid_robot_device</build_depend>
<build_depend>humanoid_robot_framework_common</build_depend>
<build_depend>cmake_modules</build_depend>
<build_depend>yaml-cpp</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>roslib</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>humanoid_robot_controller_msgs</build_export_depend>
<build_export_depend>dynamixel_sdk</build_export_depend>
<build_export_depend>humanoid_robot_device</build_export_depend>
<build_export_depend>humanoid_robot_framework_common</build_export_depend>
<build_export_depend>cmake_modules</build_export_depend>
<build_export_depend>yaml-cpp</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>roslib</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>humanoid_robot_controller_msgs</exec_depend>
<exec_depend>dynamixel_sdk</exec_depend>
<exec_depend>humanoid_robot_device</exec_depend>
<exec_depend>humanoid_robot_framework_common</exec_depend>
<exec_depend>cmake_modules</exec_depend>
<exec_depend>yaml-cpp</exec_depend>
</condition>
<exec_depend>roscpp</exec_depend>
<exec_depend>roslib</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>humanoid_robot_controller_msgs</exec_depend>
<exec_depend>dynamixel_sdk</exec_depend>
<exec_depend>humanoid_robot_device</exec_depend>
<exec_depend>humanoid_robot_framework_common</exec_depend>
<exec_depend>cmake_modules</exec_depend>
<exec_depend>yaml-cpp</exec_depend>
</package>

1
humanoid_robot_device/CHANGELOG.rst
View File

@ -12,7 +12,6 @@ Changelog for package humanoid_robot_device
* Update package.xml and CMakeList.txt for to the latest versions
* Contributors & Maintainer: Ronaldson Bellande
0.3.0 (2021-05-03)
------------------
* Update package.xml and CMakeList.txt for noetic branch

60
humanoid_robot_device/CMakeLists.txt
View File

@ -1,43 +1,30 @@
################################################################################
# Set minimum required version of cmake, project name and compile options
################################################################################
cmake_minimum_required(VERSION 3.0.2)
cmake_minimum_required(VERSION 3.8)
project(humanoid_robot_device)
################################################################################
# Find catkin packages and libraries for catkin and system dependencies
################################################################################
find_package(catkin REQUIRED COMPONENTS
roscpp
dynamixel_sdk
)
################################################################################
# Setup for python modules and scripts
################################################################################
if($ENV{ROS_VERSION} EQUAL 1)
find_package(catkin REQUIRED COMPONENTS
roscpp
dynamixel_sdk
)
else()
find_package(ament_cmake REQUIRED)
endif()
################################################################################
# Declare ROS messages, services and actions
################################################################################
################################################################################
# Declare ROS dynamic reconfigure parameters
################################################################################
if($ENV{ROS_VERSION} EQUAL 1)
catkin_package(
INCLUDE_DIRS include
LIBRARIES humanoid_robot_device
CATKIN_DEPENDS
roscpp
dynamixel_sdk
)
else()
ament_package()
endif()
################################################################################
# Declare catkin specific configuration to be passed to dependent projects
################################################################################
catkin_package(
INCLUDE_DIRS include
LIBRARIES humanoid_robot_device
CATKIN_DEPENDS
roscpp
dynamixel_sdk
)
################################################################################
# Build
################################################################################
include_directories(
include
${catkin_INCLUDE_DIRS}
@ -51,9 +38,6 @@ add_library(humanoid_robot_device
add_dependencies(humanoid_robot_device ${catkin_EXPORTED_TARGETS})
target_link_libraries(humanoid_robot_device ${catkin_LIBRARIES})
################################################################################
# Install
################################################################################
install(TARGETS humanoid_robot_device
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
@ -67,7 +51,3 @@ install(DIRECTORY include/${PROJECT_NAME}/
install(DIRECTORY devices
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
################################################################################
# Test
################################################################################

42
humanoid_robot_device/include/humanoid_robot_device/control_table_item.h
View File

@ -1,17 +1,17 @@
/*******************************************************************************
* Copyright 2018 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
/*******************************************************************************
* Copyright 2018 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
/*
@ -22,8 +22,8 @@
*/
#ifndef ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_
* See the License for the specific language governing permissions and
#define ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_
#include <stdint.h>
namespace humanoid_robot_framework {
@ -43,12 +43,12 @@ public:
int32_t data_max_value_;
bool is_signed_;
enum AccessType { Read, ReadWrite };
ControlTableItem()
: item_name_(""), address_(0), access_type_(Read), memory_type_(RAM),
data_length_(0), data_min_value_(0), data_max_value_(0),
is_signed_(false) {}
};
public:
std::string item_name_;
} // namespace humanoid_robot_framework
#endif /* ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ */

40
humanoid_robot_device/include/humanoid_robot_device/time_stamp.h
View File

@ -1,18 +1,18 @@
/*******************************************************************************
* Copyright 2018 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
* Copyright 2018 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
/*
* time_stamp.h
@ -24,22 +24,16 @@
#ifndef ROBOTIS_DEVICE_TIME_STAMP_H_
#define ROBOTIS_DEVICE_TIME_STAMP_H_
namespace humanoid_robot_framework
{
namespace humanoid_robot_framework {
class TimeStamp {
public:
long sec_;
long nsec_;
TimeStamp(long sec, long nsec)
: sec_(sec),
nsec_(nsec)
{ }
TimeStamp(long sec, long nsec) : sec_(sec), nsec_(nsec) {}
};
}
} // namespace humanoid_robot_framework
#endif /* ROBOTIS_DEVICE_TIME_STAMP_H_ */

34
humanoid_robot_device/package.xml
View File

@ -1,5 +1,5 @@
<?xml version="1.0"?>
<package format="2">
<package format="3">
<name>humanoid_robot_device</name>
<version>0.3.0</version>
<description>
@ -10,14 +10,32 @@
<license>Apache 2.0</license>
<maintainer email="ronaldsonbellande@gmail.com">Ronaldson Bellande</maintainer>
<buildtool_depend>catkin</buildtool_depend>
<buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>dynamixel_sdk</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>dynamixel_sdk</build_export_depend>
<condition if="$ROS_VERSION == 1">
<build_depend>roscpp</build_depend>
<build_depend>dynamixel_sdk</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>dynamixel_sdk</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>dynamixel_sdk</exec_depend>
</condition>
<condition if="$ROS_VERSION == 2">
<build_depend>roscpp</build_depend>
<build_depend>dynamixel_sdk</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>dynamixel_sdk</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>dynamixel_sdk</exec_depend>
</condition>
<exec_depend>roscpp</exec_depend>
<exec_depend>dynamixel_sdk</exec_depend>
</package>

108
humanoid_robot_device/src/humanoid_robot_device/robot.cpp
View File

@ -1,17 +1,17 @@
/*******************************************************************************
* Copyright 2018 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
/*******************************************************************************
* Copyright 2018 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
/*
@ -21,30 +21,30 @@
* Author: zerom
*/
* See the License for the specific language governing permissions and
#include <algorithm>
#include <fstream>
#include <iostream>
#include "humanoid_robot_device/robot.h"
using namespace humanoid_robot_framework;
static inline std::string &ltrim(std::string &s) {
*
* Created on: 2015. 12. 11.
s.erase(s.begin(),
std::find_if(s.begin(), s.end(),
std::not1(std::ptr_fun<int, int>(std::isspace))));
return s;
}
static inline std::string &rtrim(std::string &s) {
#include <fstream>
s.erase(std::find_if(s.rbegin(), s.rend(),
std::not1(std::ptr_fun<int, int>(std::isspace)))
.base(),
s.end());
return s;
}
static inline std::string &trim(std::string &s) { return ltrim(rtrim(s)); }
using namespace humanoid_robot_framework;
static inline std::vector<std::string> split(const std::string &text,
char sep) {
std::vector<std::string> tokens;
std::size_t start = 0, end = 0;
@ -60,7 +60,7 @@ static inline std::vector<std::string> split(const std::string &text,
return tokens;
}
Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)
: control_cycle_msec_(DEFAULT_CONTROL_CYCLE) {
if (dev_desc_dir_path.compare(dev_desc_dir_path.size() - 1, 1, "/") != 0)
dev_desc_dir_path += "/";
@ -81,10 +81,10 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)
input_str = trim(input_str);
// find session
}
if (!input_str.compare(0, 1, "[") &&
!input_str.compare(input_str.size() - 1, 1, "]")) {
input_str = input_str.substr(1, input_str.size() - 2);
std::transform(input_str.begin(), input_str.end(), input_str.begin(),
::tolower);
session = trim(input_str);
continue;
@ -102,10 +102,10 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)
if (tokens.size() != 3)
continue;
input_str = trim(input_str);
std::cout << tokens[0] << " added. (baudrate: " << tokens[1] << ")"
<< std::endl;
if (!input_str.compare(0, 1, "[") &&
ports_[tokens[0]] =
dynamixel::PortHandler::getPortHandler(tokens[0].c_str());
ports_[tokens[0]]->setBaudRate(std::atoi(tokens[1].c_str()));
port_default_device_[tokens[0]] = tokens[2];
@ -115,7 +115,7 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)
continue;
if (tokens[0] == DYNAMIXEL) {
std::string file_path =
dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device";
int id = std::atoi(tokens[2].c_str());
std::string port = tokens[1];
@ -127,20 +127,20 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)
Dynamixel *dxl = dxls_[dev_name];
std::vector<std::string> sub_tokens = split(tokens[6], ',');
if (sub_tokens.size() > 0 && sub_tokens[0] != "") {
std::map<std::string, ControlTableItem *>::iterator indirect_it =
dxl->ctrl_table_.find(INDIRECT_ADDRESS_1);
if (indirect_it !=
dxl->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist
{
ports_[tokens[0]] =
uint16_t indirect_data_addr =
dxl->ctrl_table_[INDIRECT_DATA_1]->address_;
for (int _i = 0; _i < sub_tokens.size(); _i++) {
ports_[tokens[0]]->setBaudRate(std::atoi(tokens[1].c_str()));
std::map<std::string, ControlTableItem *>::iterator
bulkread_it = dxl->ctrl_table_.find(sub_tokens[_i]);
if (bulkread_it == dxl->ctrl_table_.end()) {
std::vector<std::string> tokens = split(input_str, '|');
if (tokens.size() != 7)
continue;
fprintf(
stderr,
"\n ##### BULK READ ITEM [ %s ] NOT FOUND!! #####\n\n",
sub_tokens[_i].c_str());
continue;
}
@ -164,13 +164,13 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)
{
for (int i = 0; i < sub_tokens.size(); i++) {
if (dxl->ctrl_table_[sub_tokens[i]] != NULL)
dxl->bulk_read_items_.push_back(
dxl->ctrl_table_[sub_tokens[i]]);
}
}
}
} else if (tokens[0] == SENSOR) {
if (bulkread_it == dxl->ctrl_table_.end()) {
std::string file_path =
dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device";
int id = std::atoi(tokens[2].c_str());
std::string port = tokens[1];
@ -182,12 +182,12 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)
Sensor *sensor = sensors_[dev_name];
std::vector<std::string> sub_tokens = split(tokens[6], ',');
if (sub_tokens.size() > 0 && sub_tokens[0] != "") {
ControlTableItem *dest_item = dxl->bulk_read_items_[_i];
std::map<std::string, ControlTableItem *>::iterator indirect_it =
sensor->ctrl_table_.find(INDIRECT_ADDRESS_1);
if (indirect_it !=
sensor->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist
{
dest_item->access_type_ = src_item->access_type_;
uint16_t indirect_data_addr =
sensor->ctrl_table_[INDIRECT_DATA_1]->address_;
for (int i = 0; i < sub_tokens.size(); i++) {
sensor->bulk_read_items_.push_back(new ControlTableItem());
@ -208,7 +208,7 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)
} else // INDIRECT_ADDRESS_1 exist
{
for (int i = 0; i < sub_tokens.size(); i++)
sensor->bulk_read_items_.push_back(
sensor->ctrl_table_[sub_tokens[i]]);
}
}
@ -221,7 +221,7 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)
}
}
Sensor *Robot::getSensor(std::string path, int id, std::string port,
float protocol_version) {
Sensor *sensor;
@ -245,10 +245,10 @@ Sensor *Robot::getSensor(std::string path, int id, std::string port,
continue;
// find _session
}
if (!input_str.compare(0, 1, "[") &&
!input_str.compare(input_str.size() - 1, 1, "]")) {
input_str = input_str.substr(1, input_str.size() - 2);
for (int i = 0; i < sub_tokens.size(); i++)
std::transform(input_str.begin(), input_str.end(), input_str.begin(),
::tolower);
session = trim(input_str);
continue;
@ -293,9 +293,9 @@ Sensor *Robot::getSensor(std::string path, int id, std::string port,
}
sensor->port_name_ = port;
fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), sensor->id_,
sensor->model_name_.c_str());
session = trim(input_str);
// std::cout << "[ID:" << (int)(_sensor->id) << "] " << _sensor->model_name
// << " added. (" << port << ")" << std::endl;
file.close();
} else
@ -304,7 +304,7 @@ Sensor *Robot::getSensor(std::string path, int id, std::string port,
return sensor;
}
if (tokens[0] == "model_name")
Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port,
float protocol_version) {
Dynamixel *dxl;
@ -342,10 +342,10 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port,
continue;
// find session
if (!input_str.compare(0, 1, "[") &&
!input_str.compare(input_str.size() - 1, 1, "]")) {
input_str = input_str.substr(1, input_str.size() - 2);
} else
std::transform(input_str.begin(), input_str.end(), input_str.begin(),
::tolower);
session = trim(input_str);
continue;
@ -440,10 +440,10 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port,
if (dxl->ctrl_table_[torque_enable_item_name] != NULL)
dxl->torque_enable_item_ = dxl->ctrl_table_[torque_enable_item_name];
if (dxl->ctrl_table_[present_position_item_name] != NULL)
goal_velocity_item_name = tokens[1];
dxl->present_position_item_ =
dxl->ctrl_table_[present_position_item_name];
if (dxl->ctrl_table_[present_velocity_item_name] != NULL)
else if (tokens[0] == "position_d_gain_item_name")
dxl->present_velocity_item_ =
dxl->ctrl_table_[present_velocity_item_name];
if (dxl->ctrl_table_[present_current_item_name] != NULL)
dxl->present_current_item_ = dxl->ctrl_table_[present_current_item_name];
@ -466,9 +466,9 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port,
if (dxl->ctrl_table_[velocity_p_gain_item_name] != NULL)
dxl->velocity_p_gain_item_ = dxl->ctrl_table_[velocity_p_gain_item_name];
item->access_type_ = Read;
fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), dxl->id_,
dxl->model_name_.c_str());
item->access_type_ = ReadWrite;
// std::cout << "[ID:" << (int)(_dxl->id) << "] " << _dxl->model_name << "
// added. (" << port << ")" << std::endl;
file.close();
} else

48
humanoid_robot_device/src/humanoid_robot_device/sensor.cpp
View File

@ -1,17 +1,17 @@
/*******************************************************************************
* Copyright 2018 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
/*******************************************************************************
* Copyright 2018 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
/*
@ -25,15 +25,15 @@
using namespace humanoid_robot_framework;
*******************************************************************************/
/*
* sensor.cpp
*
* Created on: 2016. 5. 11.
* Author: zerom
*/
Sensor::Sensor(int id, std::string model_name, float protocol_version) {
this->id_ = id;
this->model_name_ = model_name;
this->port_name_ = "";
this->protocol_version_ = protocol_version;
ctrl_table_.clear();
sensor_state_ = new SensorState();
bulk_read_items_.clear();
}

16
humanoid_robot_framework/CMakeLists.txt
View File

@ -1,4 +1,16 @@
cmake_minimum_required(VERSION 3.0.2)
cmake_minimum_required(VERSION 3.8)
project(humanoid_robot_framework)
find_package(catkin REQUIRED)
if($ENV{ROS_VERSION} EQUAL 1)
find_package(catkin REQUIRED)
else()
find_package(ament_cmake REQUIRED)
endif()
if($ENV{ROS_VERSION} EQUAL 1)
catkin_package()
else()
ament_package()
endif()
catkin_metapackage()

42
humanoid_robot_framework/package.xml
View File

@ -1,24 +1,44 @@
<?xml version="1.0"?>
<package format="2">
<package format="3">
<name>humanoid_robot_framework</name>
<version>0.3.0</version>
<description>ROS packages for the humanoid_robot_framework (meta package)</description>
<license>Apache 2.0</license>
<maintainer email="ronaldsonbellande@gmail.com">Ronaldson Bellande</maintainer>
<buildtool_depend>catkin</buildtool_depend>
<buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
<build_depend>humanoid_robot_controller</build_depend>
<build_depend>humanoid_robot_device</build_depend>
<build_depend>humanoid_robot_framework_common</build_depend>
<build_export_depend>humanoid_robot_controller</build_export_depend>
<build_export_depend>humanoid_robot_device</build_export_depend>
<build_export_depend>humanoid_robot_framework_common</build_export_depend>
<condition if="$ROS_VERSION == 1">
<build_depend>humanoid_robot_controller</build_depend>
<build_depend>humanoid_robot_device</build_depend>
<build_depend>humanoid_robot_framework_common</build_depend>
<build_export_depend>humanoid_robot_controller</build_export_depend>
<build_export_depend>humanoid_robot_device</build_export_depend>
<build_export_depend>humanoid_robot_framework_common</build_export_depend>
<exec_depend>humanoid_robot_controller</exec_depend>
<exec_depend>humanoid_robot_device</exec_depend>
<exec_depend>humanoid_robot_framework_common</exec_depend>
</condition>
<condition if="$ROS_VERSION == 2">
<build_depend>humanoid_robot_controller</build_depend>
<build_depend>humanoid_robot_device</build_depend>
<build_depend>humanoid_robot_framework_common</build_depend>
<build_export_depend>humanoid_robot_controller</build_export_depend>
<build_export_depend>humanoid_robot_device</build_export_depend>
<build_export_depend>humanoid_robot_framework_common</build_export_depend>
<exec_depend>humanoid_robot_controller</exec_depend>
<exec_depend>humanoid_robot_device</exec_depend>
<exec_depend>humanoid_robot_framework_common</exec_depend>
</condition>
<exec_depend>humanoid_robot_controller</exec_depend>
<exec_depend>humanoid_robot_device</exec_depend>
<exec_depend>humanoid_robot_framework_common</exec_depend>
<export>
<metapackage />

58
humanoid_robot_framework_common/CMakeLists.txt
View File

@ -1,41 +1,30 @@
################################################################################
# Set minimum required version of cmake, project name and compile options
################################################################################
cmake_minimum_required(VERSION 2.8.3)
cmake_minimum_required(VERSION 3.8)
project(humanoid_robot_framework_common)
################################################################################
# Find catkin packages and libraries for catkin and system dependencies
################################################################################
find_package(catkin REQUIRED COMPONENTS
roscpp
humanoid_robot_device
)
################################################################################
# Setup for python modules and scripts
################################################################################
if($ENV{ROS_VERSION} EQUAL 1)
find_package(catkin REQUIRED COMPONENTS
roscpp
humanoid_robot_device
)
else()
find_package(ament_cmake REQUIRED)
endif()
################################################################################
# Declare ROS messages, services and actions
################################################################################
################################################################################
# Declare ROS dynamic reconfigure parameters
################################################################################
if($ENV{ROS_VERSION} EQUAL 1)
catkin_package(
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME}
CATKIN_DEPENDS
roscpp
humanoid_robot_device
)
else()
ament_package()
endif()
################################################################################
# Declare catkin specific configuration to be passed to dependent projects
################################################################################
catkin_package(
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME}
CATKIN_DEPENDS roscpp humanoid_robot_device
)
################################################################################
# Build
################################################################################
include_directories(
include
${catkin_INCLUDE_DIRS}
@ -48,9 +37,6 @@ add_library(${PROJECT_NAME}
)
set_target_properties(${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX)
################################################################################
# Install
################################################################################
install(TARGETS humanoid_robot_framework_common
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
@ -60,7 +46,3 @@ install(TARGETS humanoid_robot_framework_common
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
################################################################################
# Test
################################################################################

33
humanoid_robot_framework_common/package.xml
View File

@ -1,20 +1,37 @@
<?xml version="1.0"?>
<package format="2">
<package format="3">
<name>humanoid_robot_framework_common</name>
<version>0.2.9</version>
<description>The package contains commonly used Headers for the ROBOTIS Framework.</description>
<license>Apache 2.0</license>
<maintainer email="ronaldsonbellande@gmail.com">Ronaldson Bellande</maintainer>
<buildtool_depend>catkin</buildtool_depend>
<buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>humanoid_robot_device</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>humanoid_robot_device</build_export_depend>
<condition if="$ROS_VERSION == 1">
<build_depend>roscpp</build_depend>
<build_depend>humanoid_robot_device</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>humanoid_robot_device</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>humanoid_robot_device</exec_depend>
</condition>
<condition if="$ROS_VERSION == 2">
<build_depend>roscpp</build_depend>
<build_depend>humanoid_robot_device</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>humanoid_robot_device</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>humanoid_robot_device</exec_depend>
</condition>
<exec_depend>roscpp</exec_depend>
<exec_depend>humanoid_robot_device</exec_depend>
</package>