From c7565da012d1060ccda99467d47f449c113811b5 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Tue, 3 Oct 2023 21:27:53 -0400 Subject: [PATCH] latest pushes --- humanoid_robot_controller/CMakeLists.txt | 98 +++++------- .../humanoid_robot_controller.h | 146 +++++++++--------- humanoid_robot_device/CMakeLists.txt | 58 +++---- .../control_table_item.h | 42 ++--- .../humanoid_robot_device/time_stamp.h | 40 ++--- .../src/humanoid_robot_device/robot.cpp | 108 ++++++------- .../src/humanoid_robot_device/sensor.cpp | 48 +++--- humanoid_robot_framework/CMakeLists.txt | 14 +- .../CMakeLists.txt | 56 +++---- 9 files changed, 280 insertions(+), 330 deletions(-) diff --git a/humanoid_robot_controller/CMakeLists.txt b/humanoid_robot_controller/CMakeLists.txt index 15cb7bf..3cdb850 100755 --- a/humanoid_robot_controller/CMakeLists.txt +++ b/humanoid_robot_controller/CMakeLists.txt @@ -1,32 +1,31 @@ -################################################################################ -# Set minimum required version of cmake, project name and compile options -################################################################################ 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 -################################################################################ diff --git a/humanoid_robot_controller/include/humanoid_robot_controller/humanoid_robot_controller.h b/humanoid_robot_controller/include/humanoid_robot_controller/humanoid_robot_controller.h index eca9641..c9d09e3 100755 --- a/humanoid_robot_controller/include/humanoid_robot_controller/humanoid_robot_controller.h +++ b/humanoid_robot_controller/include/humanoid_robot_controller/humanoid_robot_controller.h @@ -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 - * - * 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. *******************************************************************************/ /* @@ -22,29 +22,29 @@ */ #ifndef ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ -#define ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ - -#include -#include -#include -#include -#include -#include -#include +#define ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_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 +#include +#include +#include +#include +#include +#include -#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" +#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); - void setJointCtrlModuleThread( + void setJointCtrlModuleThread( const humanoid_robot_controller_msgs::JointCtrlModule::ConstPtr &msg); bool isTimerStopped(); @@ -92,23 +92,23 @@ public: std::map port_to_bulk_read_; /* sync write */ - std::map + std::map port_to_sync_write_position_; - std::map + std::map port_to_sync_write_velocity_; - std::map + std::map port_to_sync_write_current_; - std::map + std::map port_to_sync_write_position_p_gain_; - std::map + std::map port_to_sync_write_position_i_gain_; - std::map + std::map port_to_sync_write_position_d_gain_; - std::map + std::map port_to_sync_write_velocity_p_gain_; - std::map + std::map port_to_sync_write_velocity_i_gain_; - std::map + std::map port_to_sync_write_velocity_d_gain_; /* publisher */ @@ -124,7 +124,7 @@ public: RobotisController(); - bool initialize(const std::string robot_file_path, + 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 */ - void writeControlTableCallback( + void writeControlTableCallback( const humanoid_robot_controller_msgs::WriteControlTable::ConstPtr &msg); - void syncWriteItemCallback( + 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); - void setJointCtrlModuleCallback( + 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); - bool getJointCtrlModuleService( - humanoid_robot_controller_msgs::GetJointModule::Request &req, + bool getJointCtrlModuleService( + humanoid_robot_controller_msgs::GetJointModule::Request &req, humanoid_robot_controller_msgs::GetJointModule::Response &res); - bool setJointCtrlModuleService( - humanoid_robot_controller_msgs::SetJointModule::Request &req, + bool setJointCtrlModuleService( + humanoid_robot_controller_msgs::SetJointModule::Request &req, humanoid_robot_controller_msgs::SetJointModule::Response &res); - 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); + 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, + 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); - int factoryReset(const std::string joint_name, uint8_t option = 0, + 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, + int read(const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - int readCtrlItem(const std::string joint_name, const std::string item_name, + 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, + int read1Byte(const std::string joint_name, uint16_t address, uint8_t *data, uint8_t *error = 0); - int read2Byte(const std::string joint_name, uint16_t address, uint16_t *data, + 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, + 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, + int write(const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - int writeCtrlItem(const std::string joint_name, const std::string item_name, + int writeCtrlItem(const std::string joint_name, const std::string item_name, uint32_t data, uint8_t *error = 0); - int write1Byte(const std::string joint_name, uint16_t address, uint8_t data, + int write1Byte(const std::string joint_name, uint16_t address, uint8_t data, uint8_t *error = 0); - int write2Byte(const std::string joint_name, uint16_t address, uint16_t data, + int write2Byte(const std::string joint_name, uint16_t address, uint16_t data, uint8_t *error = 0); - int write4Byte(const std::string joint_name, uint16_t address, uint32_t data, + int write4Byte(const std::string joint_name, uint16_t address, uint32_t data, uint8_t *error = 0); - int regWrite(const std::string joint_name, uint16_t address, uint16_t length, + int regWrite(const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); }; -} // namespace humanoid_robot_framework - +} // namespace humanoid_robot_framework + #endif /* ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ */ diff --git a/humanoid_robot_device/CMakeLists.txt b/humanoid_robot_device/CMakeLists.txt index c838c1f..2e140bb 100755 --- a/humanoid_robot_device/CMakeLists.txt +++ b/humanoid_robot_device/CMakeLists.txt @@ -1,43 +1,30 @@ -################################################################################ -# Set minimum required version of cmake, project name and compile options -################################################################################ 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 -################################################################################ diff --git a/humanoid_robot_device/include/humanoid_robot_device/control_table_item.h b/humanoid_robot_device/include/humanoid_robot_device/control_table_item.h index 971c3d2..a5338df 100755 --- a/humanoid_robot_device/include/humanoid_robot_device/control_table_item.h +++ b/humanoid_robot_device/include/humanoid_robot_device/control_table_item.h @@ -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 - * - * 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. *******************************************************************************/ /* @@ -22,8 +22,8 @@ */ #ifndef ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ -#define ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ - +#define ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ + #include namespace humanoid_robot_framework { @@ -43,12 +43,12 @@ public: int32_t data_max_value_; bool is_signed_; - ControlTableItem() - : item_name_(""), address_(0), access_type_(Read), memory_type_(RAM), - data_length_(0), data_min_value_(0), data_max_value_(0), + 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) {} }; -} // namespace humanoid_robot_framework - +} // namespace humanoid_robot_framework + #endif /* ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ */ diff --git a/humanoid_robot_device/include/humanoid_robot_device/time_stamp.h b/humanoid_robot_device/include/humanoid_robot_device/time_stamp.h index 97ed81b..b2bc3f7 100755 --- a/humanoid_robot_device/include/humanoid_robot_device/time_stamp.h +++ b/humanoid_robot_device/include/humanoid_robot_device/time_stamp.h @@ -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_ */ diff --git a/humanoid_robot_device/src/humanoid_robot_device/robot.cpp b/humanoid_robot_device/src/humanoid_robot_device/robot.cpp index 6362c5b..4ae7219 100755 --- a/humanoid_robot_device/src/humanoid_robot_device/robot.cpp +++ b/humanoid_robot_device/src/humanoid_robot_device/robot.cpp @@ -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 - * - * 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. *******************************************************************************/ /* @@ -21,30 +21,30 @@ * Author: zerom */ -#include -#include -#include +#include +#include +#include #include "humanoid_robot_device/robot.h" using namespace humanoid_robot_framework; static inline std::string <rim(std::string &s) { - s.erase(s.begin(), - std::find_if(s.begin(), s.end(), + s.erase(s.begin(), + std::find_if(s.begin(), s.end(), std::not1(std::ptr_fun(std::isspace)))); return s; } static inline std::string &rtrim(std::string &s) { - s.erase(std::find_if(s.rbegin(), s.rend(), - std::not1(std::ptr_fun(std::isspace))) - .base(), + s.erase(std::find_if(s.rbegin(), s.rend(), + std::not1(std::ptr_fun(std::isspace))) + .base(), s.end()); return s; } static inline std::string &trim(std::string &s) { return ltrim(rtrim(s)); } -static inline std::vector split(const std::string &text, +static inline std::vector split(const std::string &text, char sep) { std::vector tokens; std::size_t start = 0, end = 0; @@ -60,7 +60,7 @@ static inline std::vector split(const std::string &text, return tokens; } -Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) +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, "[") && + 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(), + 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; - std::cout << tokens[0] << " added. (baudrate: " << tokens[1] << ")" + std::cout << tokens[0] << " added. (baudrate: " << tokens[1] << ")" << std::endl; - ports_[tokens[0]] = + 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 = + 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 sub_tokens = split(tokens[6], ','); if (sub_tokens.size() > 0 && sub_tokens[0] != "") { - std::map::iterator indirect_it = + std::map::iterator indirect_it = dxl->ctrl_table_.find(INDIRECT_ADDRESS_1); - if (indirect_it != + if (indirect_it != dxl->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist { - uint16_t indirect_data_addr = + uint16_t indirect_data_addr = dxl->ctrl_table_[INDIRECT_DATA_1]->address_; for (int _i = 0; _i < sub_tokens.size(); _i++) { - std::map::iterator + std::map::iterator bulkread_it = dxl->ctrl_table_.find(sub_tokens[_i]); if (bulkread_it == dxl->ctrl_table_.end()) { - fprintf( - stderr, - "\n ##### BULK READ ITEM [ %s ] NOT FOUND!! #####\n\n", + 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->bulk_read_items_.push_back( dxl->ctrl_table_[sub_tokens[i]]); } } } } else if (tokens[0] == SENSOR) { - std::string file_path = + 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 sub_tokens = split(tokens[6], ','); if (sub_tokens.size() > 0 && sub_tokens[0] != "") { - std::map::iterator indirect_it = + std::map::iterator indirect_it = sensor->ctrl_table_.find(INDIRECT_ADDRESS_1); - if (indirect_it != + if (indirect_it != sensor->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist { - uint16_t indirect_data_addr = + 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->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, +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, "[") && + 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(), + 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_, + fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), sensor->id_, sensor->model_name_.c_str()); - // std::cout << "[ID:" << (int)(_sensor->id) << "] " << _sensor->model_name + // 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; } -Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, +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, "[") && + 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(), + 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) - dxl->present_position_item_ = + dxl->present_position_item_ = dxl->ctrl_table_[present_position_item_name]; if (dxl->ctrl_table_[present_velocity_item_name] != NULL) - dxl->present_velocity_item_ = + 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]; - fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), dxl->id_, + fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), dxl->id_, dxl->model_name_.c_str()); - // std::cout << "[ID:" << (int)(_dxl->id) << "] " << _dxl->model_name << " + // std::cout << "[ID:" << (int)(_dxl->id) << "] " << _dxl->model_name << " // added. (" << port << ")" << std::endl; file.close(); } else diff --git a/humanoid_robot_device/src/humanoid_robot_device/sensor.cpp b/humanoid_robot_device/src/humanoid_robot_device/sensor.cpp index 166c6dc..0be04df 100755 --- a/humanoid_robot_device/src/humanoid_robot_device/sensor.cpp +++ b/humanoid_robot_device/src/humanoid_robot_device/sensor.cpp @@ -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 - * - * 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. *******************************************************************************/ /* @@ -25,15 +25,15 @@ using namespace humanoid_robot_framework; -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(); - +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(); } diff --git a/humanoid_robot_framework/CMakeLists.txt b/humanoid_robot_framework/CMakeLists.txt index 482cad1..15783dc 100644 --- a/humanoid_robot_framework/CMakeLists.txt +++ b/humanoid_robot_framework/CMakeLists.txt @@ -1,4 +1,16 @@ 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() diff --git a/humanoid_robot_framework_common/CMakeLists.txt b/humanoid_robot_framework_common/CMakeLists.txt index 185de24..f5d0175 100755 --- a/humanoid_robot_framework_common/CMakeLists.txt +++ b/humanoid_robot_framework_common/CMakeLists.txt @@ -1,41 +1,30 @@ -################################################################################ -# Set minimum required version of cmake, project name and compile options -################################################################################ 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 -################################################################################