diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h index 05ae93c..75a4580 100755 --- a/robotis_controller/include/robotis_controller/robotis_controller.h +++ b/robotis_controller/include/robotis_controller/robotis_controller.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. + *******************************************************************************/ /* * robotis_controller.h @@ -22,96 +22,99 @@ */ #ifndef ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ -#define ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ +#define ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include "robotis_controller_msgs/GetJointModule.h" +#include "robotis_controller_msgs/JointCtrlModule.h" +#include "robotis_controller_msgs/LoadOffset.h" +#include "robotis_controller_msgs/SetJointModule.h" +#include "robotis_controller_msgs/SetModule.h" +#include "robotis_controller_msgs/SyncWriteItem.h" +#include "robotis_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 "robotis_device/robot.h" +#include "robotis_framework_common/motion_module.h" +#include "robotis_framework_common/sensor_module.h" -#include "robotis_controller_msgs/WriteControlTable.h" -#include "robotis_controller_msgs/SyncWriteItem.h" -#include "robotis_controller_msgs/JointCtrlModule.h" -#include "robotis_controller_msgs/GetJointModule.h" -#include "robotis_controller_msgs/SetJointModule.h" -#include "robotis_controller_msgs/SetModule.h" -#include "robotis_controller_msgs/LoadOffset.h" +namespace robotis_framework { -#include "robotis_device/robot.h" -#include "robotis_framework_common/motion_module.h" -#include "robotis_framework_common/sensor_module.h" -#include "dynamixel_sdk/group_bulk_read.h" -#include "dynamixel_sdk/group_sync_write.h" +enum ControllerMode { MotionModuleMode, DirectControlMode }; -namespace robotis_framework -{ - -enum ControllerMode -{ - MotionModuleMode, - DirectControlMode -}; - -class RobotisController : public Singleton -{ +class RobotisController : public Singleton { private: - boost::thread queue_thread_; - boost::thread gazebo_thread_; - boost::thread set_module_thread_; - boost::mutex queue_mutex_; + boost::thread queue_thread_; + boost::thread gazebo_thread_; + boost::thread set_module_thread_; + boost::mutex queue_mutex_; - bool init_pose_loaded_; - bool is_timer_running_; - bool is_offset_enabled_; - double offset_ratio_; - bool stop_timer_; - pthread_t timer_thread_; - ControllerMode controller_mode_; + bool init_pose_loaded_; + bool is_timer_running_; + bool is_offset_enabled_; + double offset_ratio_; + bool stop_timer_; + pthread_t timer_thread_; + ControllerMode controller_mode_; std::list motion_modules_; std::list sensor_modules_; - std::vector direct_sync_write_; + std::vector direct_sync_write_; std::map sensor_result_; void gazeboTimerThread(); void msgQueueThread(); void setCtrlModuleThread(std::string ctrl_module); - void setJointCtrlModuleThread(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg); + void setJointCtrlModuleThread( + const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg); bool isTimerStopped(); void initializeSyncWrite(); public: - const int NONE_GAIN = 65535; - bool DEBUG_PRINT; - Robot *robot_; + const int NONE_GAIN = 65535; + bool DEBUG_PRINT; + Robot *robot_; - bool gazebo_mode_; - std::string gazebo_robot_name_; + bool gazebo_mode_; + std::string gazebo_robot_name_; /* bulk read */ - std::map port_to_bulk_read_; + std::map port_to_bulk_read_; /* sync write */ - std::map port_to_sync_write_position_; - std::map port_to_sync_write_velocity_; - std::map port_to_sync_write_current_; - std::map port_to_sync_write_position_p_gain_; - std::map port_to_sync_write_position_i_gain_; - std::map port_to_sync_write_position_d_gain_; - std::map port_to_sync_write_velocity_p_gain_; - std::map port_to_sync_write_velocity_i_gain_; - std::map port_to_sync_write_velocity_d_gain_; + std::map + port_to_sync_write_position_; + std::map + port_to_sync_write_velocity_; + std::map + port_to_sync_write_current_; + std::map + port_to_sync_write_position_p_gain_; + std::map + port_to_sync_write_position_i_gain_; + std::map + port_to_sync_write_position_d_gain_; + std::map + port_to_sync_write_velocity_p_gain_; + std::map + port_to_sync_write_velocity_i_gain_; + std::map + port_to_sync_write_velocity_d_gain_; /* publisher */ - ros::Publisher goal_joint_state_pub_; - ros::Publisher present_joint_state_pub_; - ros::Publisher current_module_pub_; + ros::Publisher goal_joint_state_pub_; + ros::Publisher present_joint_state_pub_; + ros::Publisher current_module_pub_; std::map gazebo_joint_position_pub_; std::map gazebo_joint_velocity_pub_; @@ -121,62 +124,84 @@ public: RobotisController(); - bool initialize(const std::string robot_file_path, const std::string init_file_path); - void initializeDevice(const std::string init_file_path); - void process(); + bool initialize(const std::string robot_file_path, + const std::string init_file_path); + void initializeDevice(const std::string init_file_path); + void process(); - void addMotionModule(MotionModule *module); - void removeMotionModule(MotionModule *module); - void addSensorModule(SensorModule *module); - void removeSensorModule(SensorModule *module); + void addMotionModule(MotionModule *module); + void removeMotionModule(MotionModule *module); + void addSensorModule(SensorModule *module); + void removeSensorModule(SensorModule *module); - void startTimer(); - void stopTimer(); - bool isTimerRunning(); + void startTimer(); + void stopTimer(); + bool isTimerRunning(); - void setCtrlModule(std::string module_name); - void loadOffset(const std::string path); + void setCtrlModule(std::string module_name); + void loadOffset(const std::string path); /* ROS Topic Callback Functions */ - void writeControlTableCallback(const robotis_controller_msgs::WriteControlTable::ConstPtr &msg); - void syncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg); - void setControllerModeCallback(const std_msgs::String::ConstPtr &msg); - void setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); - void setJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg); - void setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg); - void enableOffsetCallback(const std_msgs::Bool::ConstPtr &msg); - bool getJointCtrlModuleService(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res); - bool setJointCtrlModuleService(robotis_controller_msgs::SetJointModule::Request &req, robotis_controller_msgs::SetJointModule::Response &res); - bool setCtrlModuleService(robotis_controller_msgs::SetModule::Request &req, robotis_controller_msgs::SetModule::Response &res); - bool loadOffsetService(robotis_controller_msgs::LoadOffset::Request &req, robotis_controller_msgs::LoadOffset::Response &res); + void writeControlTableCallback( + const robotis_controller_msgs::WriteControlTable::ConstPtr &msg); + void syncWriteItemCallback( + const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg); + void setControllerModeCallback(const std_msgs::String::ConstPtr &msg); + void setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); + void setJointCtrlModuleCallback( + const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg); + void setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg); + void enableOffsetCallback(const std_msgs::Bool::ConstPtr &msg); + bool getJointCtrlModuleService( + robotis_controller_msgs::GetJointModule::Request &req, + robotis_controller_msgs::GetJointModule::Response &res); + bool setJointCtrlModuleService( + robotis_controller_msgs::SetJointModule::Request &req, + robotis_controller_msgs::SetJointModule::Response &res); + bool setCtrlModuleService(robotis_controller_msgs::SetModule::Request &req, + robotis_controller_msgs::SetModule::Response &res); + bool loadOffsetService(robotis_controller_msgs::LoadOffset::Request &req, + robotis_controller_msgs::LoadOffset::Response &res); - void gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); + 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 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); - int factoryReset(const std::string joint_name, uint8_t option = 0, 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, + uint8_t *error = 0); - 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, uint32_t *data, uint8_t *error = 0); + 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, + uint32_t *data, uint8_t *error = 0); - 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, uint8_t *error = 0); - int read4Byte (const std::string joint_name, uint16_t address, uint32_t *data, uint8_t *error = 0); + 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, + 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); - int writeCtrlItem(const std::string joint_name, const std::string item_name, 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); + 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, uint8_t *error = 0); - 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, uint8_t *error = 0); + 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, + uint8_t *error = 0); + 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, uint8_t *data, uint8_t *error = 0); + int regWrite(const std::string joint_name, uint16_t address, uint16_t length, + uint8_t *data, uint8_t *error = 0); }; -} - - +} // namespace robotis_framework + #endif /* ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ */ diff --git a/robotis_device/include/robotis_device/control_table_item.h b/robotis_device/include/robotis_device/control_table_item.h index ed7b956..7249a90 100755 --- a/robotis_device/include/robotis_device/control_table_item.h +++ b/robotis_device/include/robotis_device/control_table_item.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. + *******************************************************************************/ /* * control_table_item.h @@ -22,49 +22,33 @@ */ #ifndef ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ -#define ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ - - +#define ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ + #include -namespace robotis_framework -{ +namespace robotis_framework { -enum AccessType { - Read, - ReadWrite -}; +enum AccessType { Read, ReadWrite }; -enum MemoryType { - EEPROM, - RAM -}; +enum MemoryType { EEPROM, RAM }; -class ControlTableItem -{ +class ControlTableItem { public: std::string item_name_; - uint16_t address_; - AccessType access_type_; - MemoryType memory_type_; - uint8_t data_length_; - int32_t data_min_value_; - int32_t data_max_value_; - bool is_signed_; + uint16_t address_; + AccessType access_type_; + MemoryType memory_type_; + uint8_t data_length_; + int32_t data_min_value_; + 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), - is_signed_(false) - { } + 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 robotis_framework + #endif /* ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ */ diff --git a/robotis_device/src/robotis_device/dynamixel.cpp b/robotis_device/src/robotis_device/dynamixel.cpp index 500393c..beda733 100755 --- a/robotis_device/src/robotis_device/dynamixel.cpp +++ b/robotis_device/src/robotis_device/dynamixel.cpp @@ -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. + *******************************************************************************/ /* * dynamixel.cpp @@ -26,28 +26,15 @@ using namespace robotis_framework; Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version) - : ctrl_module_name_("none"), - torque_to_current_value_ratio_(1.0), - velocity_to_value_ratio_(1.0), - value_of_0_radian_position_(0), - value_of_min_radian_position_(0), - value_of_max_radian_position_(0), - min_radian_(-3.14159265), - max_radian_(3.14159265), - torque_enable_item_(0), - present_position_item_(0), - present_velocity_item_(0), - present_current_item_(0), - goal_position_item_(0), - goal_velocity_item_(0), - goal_current_item_(0), - position_p_gain_item_(0), - position_i_gain_item_(0), - position_d_gain_item_(0), - velocity_p_gain_item_(0), - velocity_i_gain_item_(0), - velocity_d_gain_item_(0) -{ + : ctrl_module_name_("none"), torque_to_current_value_ratio_(1.0), + velocity_to_value_ratio_(1.0), value_of_0_radian_position_(0), + value_of_min_radian_position_(0), value_of_max_radian_position_(0), + min_radian_(-3.14159265), max_radian_(3.14159265), torque_enable_item_(0), + present_position_item_(0), present_velocity_item_(0), + present_current_item_(0), goal_position_item_(0), goal_velocity_item_(0), + goal_current_item_(0), position_p_gain_item_(0), position_i_gain_item_(0), + position_d_gain_item_(0), velocity_p_gain_item_(0), + velocity_i_gain_item_(0), velocity_d_gain_item_(0) { this->id_ = id; this->model_name_ = model_name; this->port_name_ = ""; @@ -59,80 +46,74 @@ Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version) bulk_read_items_.clear(); } -double Dynamixel::convertValue2Radian(int32_t value) -{ +double Dynamixel::convertValue2Radian(int32_t value) { double radian = 0.0; - if (value > value_of_0_radian_position_) - { + if (value > value_of_0_radian_position_) { if (max_radian_ <= 0) return max_radian_; - radian = (double) (value - value_of_0_radian_position_) * max_radian_ - / (double) (value_of_max_radian_position_ - value_of_0_radian_position_); - } - else if (value < value_of_0_radian_position_) - { + radian = + (double)(value - value_of_0_radian_position_) * max_radian_ / + (double)(value_of_max_radian_position_ - value_of_0_radian_position_); + } else if (value < value_of_0_radian_position_) { if (min_radian_ >= 0) return min_radian_; - radian = (double) (value - value_of_0_radian_position_) * min_radian_ - / (double) (value_of_min_radian_position_ - value_of_0_radian_position_); + radian = + (double)(value - value_of_0_radian_position_) * min_radian_ / + (double)(value_of_min_radian_position_ - value_of_0_radian_position_); } -// if (radian > max_radian_) -// return max_radian_; -// else if (radian < min_radian_) -// return min_radian_; + // if (radian > max_radian_) + // return max_radian_; + // else if (radian < min_radian_) + // return min_radian_; return radian; } -int32_t Dynamixel::convertRadian2Value(double radian) -{ +int32_t Dynamixel::convertRadian2Value(double radian) { int32_t value = 0; - if (radian > 0) - { + if (radian > 0) { if (value_of_max_radian_position_ <= value_of_0_radian_position_) return value_of_max_radian_position_; - value = (radian * (value_of_max_radian_position_ - value_of_0_radian_position_) / max_radian_) - + value_of_0_radian_position_; - } - else if (radian < 0) - { + value = (radian * + (value_of_max_radian_position_ - value_of_0_radian_position_) / + max_radian_) + + value_of_0_radian_position_; + } else if (radian < 0) { if (value_of_min_radian_position_ >= value_of_0_radian_position_) return value_of_min_radian_position_; - value = (radian * (value_of_min_radian_position_ - value_of_0_radian_position_) / min_radian_) - + value_of_0_radian_position_; - } - else + value = (radian * + (value_of_min_radian_position_ - value_of_0_radian_position_) / + min_radian_) + + value_of_0_radian_position_; + } else value = value_of_0_radian_position_; -// if (value > value_of_max_radian_position_) -// return value_of_max_radian_position_; -// else if (value < value_of_min_radian_position_) -// return value_of_min_radian_position_; + // if (value > value_of_max_radian_position_) + // return value_of_max_radian_position_; + // else if (value < value_of_min_radian_position_) + // return value_of_min_radian_position_; return value; } -double Dynamixel::convertValue2Velocity(int32_t value) -{ - return (double) value / velocity_to_value_ratio_; +double Dynamixel::convertValue2Velocity(int32_t value) { + return (double)value / velocity_to_value_ratio_; } -int32_t Dynamixel::convertVelocity2Value(double velocity) -{ - return (int32_t) (velocity * velocity_to_value_ratio_);; +int32_t Dynamixel::convertVelocity2Value(double velocity) { + return (int32_t)(velocity * velocity_to_value_ratio_); + ; } -double Dynamixel::convertValue2Torque(int16_t value) -{ - return (double) value / torque_to_current_value_ratio_; +double Dynamixel::convertValue2Torque(int16_t value) { + return (double)value / torque_to_current_value_ratio_; } -int16_t Dynamixel::convertTorque2Value(double torque) -{ - return (int16_t) (torque * torque_to_current_value_ratio_); +int16_t Dynamixel::convertTorque2Value(double torque) { + return (int16_t)(torque * torque_to_current_value_ratio_); } diff --git a/robotis_device/src/robotis_device/robot.cpp b/robotis_device/src/robotis_device/robot.cpp index 45a97c5..ea738f1 100755 --- a/robotis_device/src/robotis_device/robot.cpp +++ b/robotis_device/src/robotis_device/robot.cpp @@ -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. + *******************************************************************************/ /* * robot.cpp @@ -21,36 +21,35 @@ * Author: zerom */ -#include -#include -#include +#include +#include +#include #include "robotis_device/robot.h" using namespace robotis_framework; -static inline std::string <rim(std::string &s) -{ - s.erase(s.begin(), std::find_if(s.begin(), s.end(), std::not1(std::ptr_fun(std::isspace)))); +static inline std::string <rim(std::string &s) { + 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.end()); +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.end()); return s; } -static inline std::string &trim(std::string &s) -{ - return ltrim(rtrim(s)); -} +static inline std::string &trim(std::string &s) { return ltrim(rtrim(s)); } -static inline std::vector split(const std::string &text, char sep) -{ +static inline std::vector split(const std::string &text, + char sep) { std::vector tokens; std::size_t start = 0, end = 0; - while ((end = text.find(sep, start)) != (std::string::npos)) - { + while ((end = text.find(sep, start)) != (std::string::npos)) { tokens.push_back(text.substr(start, end - start)); trim(tokens.back()); start = end + 1; @@ -61,19 +60,16 @@ static inline std::vector split(const std::string &text, char sep) return tokens; } -Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) - : control_cycle_msec_(DEFAULT_CONTROL_CYCLE) -{ +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 += "/"; std::ifstream file(robot_file_path.c_str()); - if (file.is_open()) - { + if (file.is_open()) { std::string session = ""; std::string input_str; - while (!file.eof()) - { + while (!file.eof()) { std::getline(file, input_str); // remove comment ( # ) @@ -85,160 +81,157 @@ 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, "]")) - { + 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); + std::transform(input_str.begin(), input_str.end(), input_str.begin(), + ::tolower); session = trim(input_str); continue; } - if (session == SESSION_CONTROL_INFO) - { + if (session == SESSION_CONTROL_INFO) { std::vector tokens = split(input_str, '='); if (tokens.size() != 2) continue; if (tokens[0] == "control_cycle") control_cycle_msec_ = std::atoi(tokens[1].c_str()); - } - else if (session == SESSION_PORT_INFO) - { + } else if (session == SESSION_PORT_INFO) { std::vector tokens = split(input_str, '|'); if (tokens.size() != 3) continue; - std::cout << tokens[0] << " added. (baudrate: " << tokens[1] << ")" << std::endl; + std::cout << tokens[0] << " added. (baudrate: " << tokens[1] << ")" + << std::endl; - ports_[tokens[0]] = dynamixel::PortHandler::getPortHandler(tokens[0].c_str()); + 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]; - } - else if (session == SESSION_DEVICE_INFO) - { + } else if (session == SESSION_DEVICE_INFO) { std::vector tokens = split(input_str, '|'); if (tokens.size() != 7) 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]; - float protocol = std::atof(tokens[4].c_str()); - std::string dev_name = tokens[5]; + 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]; + float protocol = std::atof(tokens[4].c_str()); + std::string dev_name = tokens[5]; dxls_[dev_name] = getDynamixel(file_path, id, port, protocol); 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 = dxl->ctrl_table_.find(INDIRECT_ADDRESS_1); - if (indirect_it != dxl->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist + if (sub_tokens.size() > 0 && sub_tokens[0] != "") { + std::map::iterator indirect_it = + dxl->ctrl_table_.find(INDIRECT_ADDRESS_1); + if (indirect_it != + dxl->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist { - uint16_t indirect_data_addr = dxl->ctrl_table_[INDIRECT_DATA_1]->address_; - for (int _i = 0; _i < sub_tokens.size(); _i++) - { - 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", sub_tokens[_i].c_str()); + uint16_t indirect_data_addr = + dxl->ctrl_table_[INDIRECT_DATA_1]->address_; + for (int _i = 0; _i < sub_tokens.size(); _i++) { + 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", + sub_tokens[_i].c_str()); continue; } dxl->bulk_read_items_.push_back(new ControlTableItem()); ControlTableItem *dest_item = dxl->bulk_read_items_[_i]; - ControlTableItem *src_item = dxl->ctrl_table_[sub_tokens[_i]]; + ControlTableItem *src_item = dxl->ctrl_table_[sub_tokens[_i]]; - dest_item->item_name_ = src_item->item_name_; - dest_item->address_ = indirect_data_addr; - dest_item->access_type_ = src_item->access_type_; - dest_item->memory_type_ = src_item->memory_type_; - dest_item->data_length_ = src_item->data_length_; - dest_item->data_min_value_ = src_item->data_min_value_; - dest_item->data_max_value_ = src_item->data_max_value_; - dest_item->is_signed_ = src_item->is_signed_; + dest_item->item_name_ = src_item->item_name_; + dest_item->address_ = indirect_data_addr; + dest_item->access_type_ = src_item->access_type_; + dest_item->memory_type_ = src_item->memory_type_; + dest_item->data_length_ = src_item->data_length_; + dest_item->data_min_value_ = src_item->data_min_value_; + dest_item->data_max_value_ = src_item->data_max_value_; + dest_item->is_signed_ = src_item->is_signed_; indirect_data_addr += dest_item->data_length_; } - } - else // INDIRECT_ADDRESS_1 not exist + } else // INDIRECT_ADDRESS_1 not exist { - for (int i = 0; i < sub_tokens.size(); i++) - { + 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]]); + dxl->bulk_read_items_.push_back( + dxl->ctrl_table_[sub_tokens[i]]); } } } - } - else if (tokens[0] == SENSOR) - { - 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]; - float protocol = std::atof(tokens[4].c_str()); - std::string dev_name = tokens[5]; + } else if (tokens[0] == SENSOR) { + 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]; + float protocol = std::atof(tokens[4].c_str()); + std::string dev_name = tokens[5]; sensors_[dev_name] = getSensor(file_path, id, port, protocol); 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 = sensor->ctrl_table_.find(INDIRECT_ADDRESS_1); - if (indirect_it != sensor->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist + if (sub_tokens.size() > 0 && sub_tokens[0] != "") { + std::map::iterator indirect_it = + sensor->ctrl_table_.find(INDIRECT_ADDRESS_1); + if (indirect_it != + sensor->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist { - uint16_t indirect_data_addr = sensor->ctrl_table_[INDIRECT_DATA_1]->address_; - for (int i = 0; i < sub_tokens.size(); i++) - { + 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()); ControlTableItem *dest_item = sensor->bulk_read_items_[i]; - ControlTableItem *src_item = sensor->ctrl_table_[sub_tokens[i]]; + ControlTableItem *src_item = sensor->ctrl_table_[sub_tokens[i]]; - dest_item->item_name_ = src_item->item_name_; - dest_item->address_ = indirect_data_addr; - dest_item->access_type_ = src_item->access_type_; - dest_item->memory_type_ = src_item->memory_type_; - dest_item->data_length_ = src_item->data_length_; - dest_item->data_min_value_ = src_item->data_min_value_; - dest_item->data_max_value_ = src_item->data_max_value_; - dest_item->is_signed_ = src_item->is_signed_; + dest_item->item_name_ = src_item->item_name_; + dest_item->address_ = indirect_data_addr; + dest_item->access_type_ = src_item->access_type_; + dest_item->memory_type_ = src_item->memory_type_; + dest_item->data_length_ = src_item->data_length_; + dest_item->data_min_value_ = src_item->data_min_value_; + dest_item->data_max_value_ = src_item->data_max_value_; + dest_item->is_signed_ = src_item->is_signed_; indirect_data_addr += dest_item->data_length_; } - } - else // INDIRECT_ADDRESS_1 exist + } 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]]); + sensor->bulk_read_items_.push_back( + sensor->ctrl_table_[sub_tokens[i]]); } } } } } file.close(); - } - else - { + } else { std::cout << "Unable to open file : " + robot_file_path << std::endl; } } -Sensor *Robot::getSensor(std::string path, int id, std::string port, float protocol_version) -{ +Sensor *Robot::getSensor(std::string path, int id, std::string port, + float protocol_version) { Sensor *sensor; // load file model_name.device std::ifstream file(path.c_str()); - if (file.is_open()) - { + if (file.is_open()) { std::string session = ""; std::string input_str; - while (!file.eof()) - { + while (!file.eof()) { std::getline(file, input_str); // remove comment ( # ) @@ -252,33 +245,31 @@ Sensor *Robot::getSensor(std::string path, int id, std::string port, float proto continue; // find _session - if (!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size() - 1, 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(), ::tolower); + std::transform(input_str.begin(), input_str.end(), input_str.begin(), + ::tolower); session = trim(input_str); continue; } - if (session == SESSION_DEVICE_INFO) - { + if (session == SESSION_DEVICE_INFO) { std::vector tokens = split(input_str, '='); if (tokens.size() != 2) continue; if (tokens[0] == "model_name") sensor = new Sensor(id, tokens[1], protocol_version); - } - else if (session == SESSION_CONTROL_TABLE) - { + } else if (session == SESSION_CONTROL_TABLE) { std::vector tokens = split(input_str, '|'); if (tokens.size() != 8) continue; ControlTableItem *item = new ControlTableItem(); - item->item_name_ = tokens[1]; - item->address_ = std::atoi(tokens[0].c_str()); - item->data_length_ = std::atoi(tokens[2].c_str()); + item->item_name_ = tokens[1]; + item->address_ = std::atoi(tokens[0].c_str()); + item->data_length_ = std::atoi(tokens[2].c_str()); if (tokens[3] == "R") item->access_type_ = Read; @@ -302,24 +293,24 @@ Sensor *Robot::getSensor(std::string path, int id, std::string port, float proto } sensor->port_name_ = port; - 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 << " added. (" << port << ")" << std::endl; + 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 + // << " added. (" << port << ")" << std::endl; file.close(); - } - else + } else std::cout << "Unable to open file : " + path << std::endl; return sensor; } -Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float protocol_version) -{ +Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, + float protocol_version) { Dynamixel *dxl; // load file model_name.device std::ifstream file(path.c_str()); - if (file.is_open()) - { + if (file.is_open()) { std::string session = ""; std::string input_str; @@ -337,8 +328,7 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float std::string velocity_i_gain_item_name = ""; std::string velocity_p_gain_item_name = ""; - while (!file.eof()) - { + while (!file.eof()) { std::getline(file, input_str); // remove comment ( # ) @@ -352,25 +342,23 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float continue; // find session - if (!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size() - 1, 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(), ::tolower); + std::transform(input_str.begin(), input_str.end(), input_str.begin(), + ::tolower); session = trim(input_str); continue; } - if (session == SESSION_DEVICE_INFO) - { + if (session == SESSION_DEVICE_INFO) { std::vector tokens = split(input_str, '='); if (tokens.size() != 2) continue; if (tokens[0] == "model_name") dxl = new Dynamixel(id, tokens[1], protocol_version); - } - else if (session == SESSION_TYPE_INFO) - { + } else if (session == SESSION_TYPE_INFO) { std::vector tokens = split(input_str, '='); if (tokens.size() != 2) continue; @@ -417,17 +405,15 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float velocity_i_gain_item_name = tokens[1]; else if (tokens[0] == "velocity_p_gain_item_name") velocity_p_gain_item_name = tokens[1]; - } - else if (session == SESSION_CONTROL_TABLE) - { + } else if (session == SESSION_CONTROL_TABLE) { std::vector tokens = split(input_str, '|'); if (tokens.size() != 8) continue; ControlTableItem *item = new ControlTableItem(); - item->item_name_ = tokens[1]; - item->address_ = std::atoi(tokens[0].c_str()); - item->data_length_ = std::atoi(tokens[2].c_str()); + item->item_name_ = tokens[1]; + item->address_ = std::atoi(tokens[0].c_str()); + item->data_length_ = std::atoi(tokens[2].c_str()); if (tokens[3] == "R") item->access_type_ = Read; @@ -454,9 +440,11 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float 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->ctrl_table_[present_position_item_name]; + 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->ctrl_table_[present_velocity_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]; if (dxl->ctrl_table_[goal_position_item_name] != NULL) @@ -478,17 +466,15 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float 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_, dxl->model_name_.c_str()); - //std::cout << "[ID:" << (int)(_dxl->id) << "] " << _dxl->model_name << " added. (" << port << ")" << std::endl; + 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 << " + // added. (" << port << ")" << std::endl; file.close(); - } - else + } else std::cout << "Unable to open file : " + path << std::endl; return dxl; } -int Robot::getControlCycle() -{ - return control_cycle_msec_; -} +int Robot::getControlCycle() { return control_cycle_msec_; } diff --git a/robotis_device/src/robotis_device/sensor.cpp b/robotis_device/src/robotis_device/sensor.cpp index ba7bf29..096de2a 100755 --- a/robotis_device/src/robotis_device/sensor.cpp +++ b/robotis_device/src/robotis_device/sensor.cpp @@ -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. + *******************************************************************************/ /* * sensor.cpp @@ -25,15 +25,15 @@ using namespace robotis_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(); - - bulk_read_items_.clear(); +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(); }