From b7f84182acf7c6e1ce1605715887644ae16f3d97 Mon Sep 17 00:00:00 2001 From: Alexander Stumpf Date: Tue, 24 May 2016 16:33:40 +0200 Subject: [PATCH 1/9] added .gitignore --- .gitignore | 11 +++++++++++ 1 file changed, 11 insertions(+) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..36a5c0a --- /dev/null +++ b/.gitignore @@ -0,0 +1,11 @@ +build +devel +bin +lib +msg_gen +srv_gen +qtcreator-build +*~ +*.backup +*.user +*.autosave From 9947dbb6743781b4532da3503de5cb1a9ab0d227 Mon Sep 17 00:00:00 2001 From: pyo Date: Wed, 31 Aug 2016 08:08:18 +0900 Subject: [PATCH 2/9] modified the package information for new release --- robotis_controller/CHANGELOG.rst | 23 +++++++++++++++++++++++ robotis_controller/package.xml | 2 +- robotis_device/CHANGELOG.rst | 13 +++++++++++++ robotis_device/package.xml | 2 +- robotis_framework/CHANGELOG.rst | 5 ++++- robotis_framework/package.xml | 2 +- robotis_framework_common/CHANGELOG.rst | 7 +++++++ robotis_framework_common/package.xml | 2 +- 8 files changed, 51 insertions(+), 5 deletions(-) diff --git a/robotis_controller/CHANGELOG.rst b/robotis_controller/CHANGELOG.rst index 2a476ea..dd4e11c 100644 --- a/robotis_controller/CHANGELOG.rst +++ b/robotis_controller/CHANGELOG.rst @@ -2,6 +2,29 @@ Changelog for package robotis_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.0 (2016-08-31) +----------- +* bug fixed (position pid gain & velocity pid gain sync write). +* added velocity_to_value_ratio to DXL Pro-H series. +* changed some debug messages. +* added velocity p/i/d gain and position i/d gain sync_write code. +* SyncWriteItem bug fixed. +* add function / modified the code simple (using auto / range based for loop) +* added XM-430-W210 / XM-430-W350 device file. +* rename ControlMode(CurrentControl -> TorqueControl) +* rename (port_to_sync_write_torque\_ -> port_to_sync_write_current\_) +* rename (present_current\_ -> present_torque\_) +* modified torque control code +* fixed typos / changed ROS_INFO -> fprintf (for processing speed) +* startTimer() : after bulkread txpacket(), need some sleep() +* changed the order of processing in the Process() function. +* added missing mutex for gazebo +* fixed crash when running in gazebo simulation +* sync write bug fix. +* added position_p_gain sync write +* MotionModule/SensorModule member variable access changed (public -> protected). +* Contributors: Jay Song, Zerom, Pyo, SCH + 0.1.1 (2016-08-18) ----------- * updated the package information diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index b86d7bf..f1be9e2 100644 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -1,7 +1,7 @@ robotis_controller - 0.1.1 + 0.2.0 The main package that controls THORMANG3. diff --git a/robotis_device/CHANGELOG.rst b/robotis_device/CHANGELOG.rst index 9a5117c..ac3e9d9 100644 --- a/robotis_device/CHANGELOG.rst +++ b/robotis_device/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package robotis_device ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.0 (2016-08-31) +----------- +* bug fixed (position pid gain & velocity pid gain sync write). +* added velocity_to_value_ratio to DXL Pro-H series. +* added velocity p/i/d gain and position i/d gain sync_write code. +* fixed robotis_device build_depend. +* added XM-430-W210 / XM-430-W350 device file. +* rename (present_current\_ -> present_torque\_) +* modified torque control code +* added device file for MX-64 / MX-106 +* adjusted position min/max value. (MX-28, XM-430) +* Contributors: Zerom, Pyo + 0.1.1 (2016-08-18) ----------- * updated the package information diff --git a/robotis_device/package.xml b/robotis_device/package.xml index e5caa92..f889c3f 100644 --- a/robotis_device/package.xml +++ b/robotis_device/package.xml @@ -1,7 +1,7 @@ robotis_device - 0.1.1 + 0.2.0 The package that manages device information of ROBOTIS robots. This package is used when reading device information with the robot information file diff --git a/robotis_framework/CHANGELOG.rst b/robotis_framework/CHANGELOG.rst index 8f61ce3..513131b 100644 --- a/robotis_framework/CHANGELOG.rst +++ b/robotis_framework/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package robotis_framework ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.0 (2016-08-31) +----------- +* updated CHANGLOG.rst for minor release + 0.1.1 (2016-08-18) ----------- * updated the package information @@ -9,4 +13,3 @@ Changelog for package robotis_framework 0.1.0 (2016-08-12) ----------- * make a meta-package -* Contributors: Zerom, Pyo diff --git a/robotis_framework/package.xml b/robotis_framework/package.xml index 809e65e..09a449a 100644 --- a/robotis_framework/package.xml +++ b/robotis_framework/package.xml @@ -1,7 +1,7 @@ robotis_framework - 0.1.1 + 0.2.0 ROS packages for the robotis_framework (meta package) BSD Zerom diff --git a/robotis_framework_common/CHANGELOG.rst b/robotis_framework_common/CHANGELOG.rst index bc63720..e5a510b 100644 --- a/robotis_framework_common/CHANGELOG.rst +++ b/robotis_framework_common/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package robotis_framework_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.0 (2016-08-31) +----------- +* updated CHANGLOG.rst for minor release +* rename ControlMode(CurrentControl -> TorqueControl) +* rename (port_to_sync_write_torque\_ -> port_to_sync_write_current\_) +* Contributors: Zerom, Pyo + 0.1.1 (2016-08-18) ----------- * updated the package information diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index f01e4d5..7cce1b6 100644 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -1,7 +1,7 @@ robotis_framework_common - 0.1.1 + 0.2.0 The package contains commonly used Headers for the ROBOTIS Framework. From abbbec3fe39de67a99581c28212ed152d79e0c7a Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Thu, 10 Nov 2016 10:16:37 +0900 Subject: [PATCH 3/9] - Direct Control Mode bug fixed. --- .../robotis_controller/robotis_controller.cpp | 77 +++++++++++++++++-- 1 file changed, 72 insertions(+), 5 deletions(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 5a0c1ec..2be5d6a 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -1109,13 +1109,63 @@ void RobotisController::process() { if(gazebo_mode_ == false) { + // BulkRead Rx + for (auto& it : port_to_bulk_read_) + { + robot_->ports_[it.first]->setPacketTimeout(0.0); + it.second->rxPacket(); + } + + // -> save to robot->dxls_[]->dxl_state_ + if (robot_->dxls_.size() > 0) + { + for (auto& dxl_it : robot_->dxls_) + { + Dynamixel *dxl = dxl_it.second; + std::string port_name = dxl_it.second->port_name_; + std::string joint_name = dxl_it.first; + + if (dxl->bulk_read_items_.size() > 0) + { + uint32_t data = 0; + for (int i = 0; i < dxl->bulk_read_items_.size(); i++) + { + ControlTableItem *item = dxl->bulk_read_items_[i]; + if (port_to_bulk_read_[port_name]->isAvailable(dxl->id_, item->address_, item->data_length_) == true) + { + data = port_to_bulk_read_[port_name]->getData(dxl->id_, item->address_, item->data_length_); + + // change dxl_state + if (dxl->present_position_item_ != 0 && item->item_name_ == dxl->present_position_item_->item_name_) + dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset + else if (dxl->present_velocity_item_ != 0 && item->item_name_ == dxl->present_velocity_item_->item_name_) + dxl->dxl_state_->present_velocity_ = dxl->convertValue2Velocity(data); + else if (dxl->present_current_item_ != 0 && item->item_name_ == dxl->present_current_item_->item_name_) + dxl->dxl_state_->present_torque_ = dxl->convertValue2Torque(data); + else if (dxl->goal_position_item_ != 0 && item->item_name_ == dxl->goal_position_item_->item_name_) + dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset + else if (dxl->goal_velocity_item_ != 0 && item->item_name_ == dxl->goal_velocity_item_->item_name_) + dxl->dxl_state_->goal_velocity_ = dxl->convertValue2Velocity(data); + else if (dxl->goal_current_item_ != 0 && item->item_name_ == dxl->goal_current_item_->item_name_) + dxl->dxl_state_->goal_torque_ = dxl->convertValue2Torque(data); + else + dxl->dxl_state_->bulk_read_table_[item->item_name_] = data; + } + } + + // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp + dxl->dxl_state_->update_time_stamp_ = TimeStamp(present_state.header.stamp.sec, present_state.header.stamp.nsec); + } + } + } + queue_mutex_.lock(); - for (auto& it : port_to_sync_write_position_) - { - it.second->txPacket(); - it.second->clearParam(); - } +// for (auto& it : port_to_sync_write_position_) +// { +// it.second->txPacket(); +// it.second->clearParam(); +// } if (direct_sync_write_.size() > 0) { @@ -1128,6 +1178,10 @@ void RobotisController::process() } queue_mutex_.unlock(); + + // BulkRead Tx + for (auto& it : port_to_bulk_read_) + it.second->txPacket(); } } @@ -1553,9 +1607,22 @@ void RobotisController::syncWriteItemCallback(const robotis_controller_msgs::Syn void RobotisController::setControllerModeCallback(const std_msgs::String::ConstPtr &msg) { if (msg->data == "DirectControlMode") + { + for (auto& it : port_to_bulk_read_) + { + robot_->ports_[it.first]->setPacketTimeout(0.0); + it.second->rxPacket(); + } controller_mode_ = DirectControlMode; + } else if (msg->data == "MotionModuleMode") + { + for (auto& it : port_to_bulk_read_) + { + it.second->txPacket(); + } controller_mode_ = MotionModuleMode; + } } void RobotisController::setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) From d1d39d3c82710cafe6e68b5be545b8f6ffbe21b3 Mon Sep 17 00:00:00 2001 From: Pyo Date: Wed, 23 Nov 2016 09:13:50 +0900 Subject: [PATCH 4/9] update 0.2.0 > 0.2.1 --- robotis_controller/CHANGELOG.rst | 18 ++++++++++++++++++ robotis_controller/package.xml | 2 +- robotis_device/CHANGELOG.rst | 8 ++++++++ robotis_device/package.xml | 2 +- robotis_framework/CHANGELOG.rst | 4 ++++ robotis_framework/package.xml | 2 +- robotis_framework_common/CHANGELOG.rst | 7 +++++++ robotis_framework_common/package.xml | 2 +- 8 files changed, 41 insertions(+), 4 deletions(-) diff --git a/robotis_controller/CHANGELOG.rst b/robotis_controller/CHANGELOG.rst index dd4e11c..002d663 100755 --- a/robotis_controller/CHANGELOG.rst +++ b/robotis_controller/CHANGELOG.rst @@ -2,6 +2,24 @@ Changelog for package robotis_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.1 (2016-11-23) +----------- +* Merge the changes and update +* - Direct Control Mode bug fixed. +* update +* - added writeControlTableCallback +* - added WriteControlTable msg callback +* mode change debugging +* - optimized cpu usage by spin loop (by astumpf) +* - robotis_controller process() : processing order changed. + * 1st : packet communication + * 2nd : processing modules +* - dependencies fixed. (Pull requests `#26 `_) +* - make setJointCtrlModuleCallback() to the thread function & improved. +* - modified dependency problem. +* - reduce CPU consumption +* Contributors: Jay Song, Pyo, Zerom, SCH + 0.2.0 (2016-08-31) ----------- * bug fixed (position pid gain & velocity pid gain sync write). diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index f1be9e2..2b14099 100755 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -1,7 +1,7 @@ robotis_controller - 0.2.0 + 0.2.1 The main package that controls THORMANG3. diff --git a/robotis_device/CHANGELOG.rst b/robotis_device/CHANGELOG.rst index ac3e9d9..20b2638 100755 --- a/robotis_device/CHANGELOG.rst +++ b/robotis_device/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package robotis_device ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.1 (2016-11-23) +----------- +* Merge the changes and update +* mode change debugging +* - convertRadian2Value / convertValue2Radian : commented out the code that limits the maximum/minimum value. +* - modified dependency problem. +* Contributors: Jay Song, Pyo, Zerom, SCH + 0.2.0 (2016-08-31) ----------- * bug fixed (position pid gain & velocity pid gain sync write). diff --git a/robotis_device/package.xml b/robotis_device/package.xml index f889c3f..516770d 100755 --- a/robotis_device/package.xml +++ b/robotis_device/package.xml @@ -1,7 +1,7 @@ robotis_device - 0.2.0 + 0.2.1 The package that manages device information of ROBOTIS robots. This package is used when reading device information with the robot information file diff --git a/robotis_framework/CHANGELOG.rst b/robotis_framework/CHANGELOG.rst index 513131b..87f4509 100755 --- a/robotis_framework/CHANGELOG.rst +++ b/robotis_framework/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package robotis_framework ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.1 (2016-11-23) +----------- +* Merge the changes and update + 0.2.0 (2016-08-31) ----------- * updated CHANGLOG.rst for minor release diff --git a/robotis_framework/package.xml b/robotis_framework/package.xml index 09a449a..44a7089 100755 --- a/robotis_framework/package.xml +++ b/robotis_framework/package.xml @@ -1,7 +1,7 @@ robotis_framework - 0.2.0 + 0.2.1 ROS packages for the robotis_framework (meta package) BSD Zerom diff --git a/robotis_framework_common/CHANGELOG.rst b/robotis_framework_common/CHANGELOG.rst index e5a510b..fb68bac 100755 --- a/robotis_framework_common/CHANGELOG.rst +++ b/robotis_framework_common/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package robotis_framework_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.1 (2016-11-23) +----------- +* Merge the changes and update +* - dependencies fixed. (Pull requests `#26 `_) +* - modified dependency problem. +* Contributors: Jay Song, Pyo, Zerom + 0.2.0 (2016-08-31) ----------- * updated CHANGLOG.rst for minor release diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index 7cce1b6..0084962 100755 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -1,7 +1,7 @@ robotis_framework_common - 0.2.0 + 0.2.1 The package contains commonly used Headers for the ROBOTIS Framework. From a5bcabbac687c323d3167086dd66ddd3ae9a4243 Mon Sep 17 00:00:00 2001 From: Pyo Date: Wed, 23 Nov 2016 10:06:45 +0900 Subject: [PATCH 5/9] update 0.2.0 > 0.2.1 --- robotis_controller/package.xml | 1 + robotis_framework_common/package.xml | 2 ++ 2 files changed, 3 insertions(+) diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index 2b14099..e2368e3 100755 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -27,5 +27,6 @@ dynamixel_sdk robotis_device robotis_controller_msgs + robotis_framework_common diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index 0084962..8f0a6c7 100755 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -13,5 +13,7 @@ http://wiki.ros.org/robotis_framework_common catkin roscpp + robotis_device roscpp + robotis_device From 3f7c86f6118afbdba81760fd6aeb446c0faada22 Mon Sep 17 00:00:00 2001 From: Pyo Date: Wed, 23 Nov 2016 10:52:37 +0900 Subject: [PATCH 6/9] check the bloom error --- robotis_framework/CMakeLists.txt | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/robotis_framework/CMakeLists.txt b/robotis_framework/CMakeLists.txt index c5278ee..c0d90b3 100755 --- a/robotis_framework/CMakeLists.txt +++ b/robotis_framework/CMakeLists.txt @@ -2,3 +2,18 @@ cmake_minimum_required(VERSION 2.8.3) project(robotis_framework) find_package(catkin REQUIRED) catkin_metapackage() + +Invalid metapackage: + Metapackage 'robotis_framework': Invalid CMakeLists.txt +Expected: +<<>> +Got: +<<>> From f14a1fc54979e142da124f1d50c4309ee00626b7 Mon Sep 17 00:00:00 2001 From: Pyo Date: Wed, 23 Nov 2016 10:53:48 +0900 Subject: [PATCH 7/9] check the bloom error --- robotis_framework/CMakeLists.txt | 23 ++++------------------- 1 file changed, 4 insertions(+), 19 deletions(-) mode change 100755 => 100644 robotis_framework/CMakeLists.txt diff --git a/robotis_framework/CMakeLists.txt b/robotis_framework/CMakeLists.txt old mode 100755 new mode 100644 index c0d90b3..d336fe9 --- a/robotis_framework/CMakeLists.txt +++ b/robotis_framework/CMakeLists.txt @@ -1,19 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) -project(robotis_framework) -find_package(catkin REQUIRED) -catkin_metapackage() - -Invalid metapackage: - Metapackage 'robotis_framework': Invalid CMakeLists.txt -Expected: -<<>> -Got: -<<>> +cmake_minimum_required(VERSION 2.8.3) +project(robotis_framework) +find_package(catkin REQUIRED) +catkin_metapackage() From 0afd62afb38c82c3b1828c27d7b49dfeb89ada10 Mon Sep 17 00:00:00 2001 From: robotis Date: Fri, 9 Dec 2016 14:50:59 +0900 Subject: [PATCH 8/9] Now, control cycle can be changed. --- .../robotis_controller/robotis_controller.h | 3 ++- .../robotis_controller/robotis_controller.cpp | 19 ++++++++++--------- 2 files changed, 12 insertions(+), 10 deletions(-) diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h index 8c1ae27..f59dbd5 100755 --- a/robotis_controller/include/robotis_controller/robotis_controller.h +++ b/robotis_controller/include/robotis_controller/robotis_controller.h @@ -95,13 +95,14 @@ private: void initializeSyncWrite(); public: - static const int CONTROL_CYCLE_MSEC = 8; // 8 msec + static const int DEFAULT_CONTROL_CYCLE_MSEC = 8; // 8 msec bool DEBUG_PRINT; Robot *robot_; bool gazebo_mode_; std::string gazebo_robot_name_; + int control_cycle_msec_; /* bulk read */ std::map port_to_bulk_read_; diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 2be5d6a..d75684e 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -51,7 +51,8 @@ RobotisController::RobotisController() DEBUG_PRINT(false), robot_(0), gazebo_mode_(false), - gazebo_robot_name_("robotis") + gazebo_robot_name_("robotis"), + control_cycle_msec_(DEFAULT_CONTROL_CYCLE_MSEC) { direct_sync_write_.clear(); } @@ -603,7 +604,7 @@ void RobotisController::initializeDevice(const std::string init_file_path) void RobotisController::gazeboTimerThread() { - ros::Rate gazebo_rate(1000 / CONTROL_CYCLE_MSEC); + ros::Rate gazebo_rate(1000 / control_cycle_msec_); while (!stop_timer_) { @@ -662,7 +663,7 @@ void RobotisController::msgQueueThread() ros::ServiceServer joint_module_server = ros_node.advertiseService("/robotis/get_present_joint_ctrl_modules", &RobotisController::getCtrlModuleCallback, this); - ros::WallDuration duration(CONTROL_CYCLE_MSEC / 1000.0); + ros::WallDuration duration(control_cycle_msec_ / 1000.0); while(ros_node.ok()) callback_queue.callAvailable(duration); } @@ -679,8 +680,8 @@ void *RobotisController::timerThread(void *param) while (!controller->stop_timer_) { - next_time.tv_sec += (next_time.tv_nsec + CONTROL_CYCLE_MSEC * 1000000) / 1000000000; - next_time.tv_nsec = (next_time.tv_nsec + CONTROL_CYCLE_MSEC * 1000000) % 1000000000; + next_time.tv_sec += (next_time.tv_nsec + controller->control_cycle_msec_ * 1000000) / 1000000000; + next_time.tv_nsec = (next_time.tv_nsec + controller->control_cycle_msec_ * 1000000) % 1000000000; controller->process(); @@ -1429,7 +1430,7 @@ void RobotisController::addMotionModule(MotionModule *module) } } - module->initialize(CONTROL_CYCLE_MSEC, robot_); + module->initialize(control_cycle_msec_, robot_); motion_modules_.push_back(module); motion_modules_.unique(); } @@ -1451,7 +1452,7 @@ void RobotisController::addSensorModule(SensorModule *module) } } - module->initialize(CONTROL_CYCLE_MSEC, robot_); + module->initialize(control_cycle_msec_, robot_); sensor_modules_.push_back(module); sensor_modules_.unique(); } @@ -1731,7 +1732,7 @@ void RobotisController::setJointCtrlModuleThread(const robotis_controller_msgs:: for(std::list::iterator _stop_m_it = _stop_modules.begin(); _stop_m_it != _stop_modules.end(); _stop_m_it++) { while((*_stop_m_it)->isRunning()) - usleep(CONTROL_CYCLE_MSEC * 1000); + usleep(control_cycle_msec_ * 1000); } // disable module(s) @@ -1946,7 +1947,7 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) for (auto stop_m_it = stop_modules.begin(); stop_m_it != stop_modules.end(); stop_m_it++) { while ((*stop_m_it)->isRunning()) - usleep(CONTROL_CYCLE_MSEC * 1000); + usleep(control_cycle_msec_ * 1000); } // disable module(s) From 41a8e3073103eb4e813320273cd71be18daa48c5 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Wed, 14 Dec 2016 17:19:52 +0900 Subject: [PATCH 9/9] read control cycle from .robot file --- .../robotis_controller/robotis_controller.h | 3 --- .../robotis_controller/robotis_controller.cpp | 19 +++++++++---------- robotis_device/include/robotis_device/robot.h | 8 ++++++++ robotis_device/src/robotis_device/robot.cpp | 16 +++++++++++++++- 4 files changed, 32 insertions(+), 14 deletions(-) diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h index f59dbd5..f811b24 100755 --- a/robotis_controller/include/robotis_controller/robotis_controller.h +++ b/robotis_controller/include/robotis_controller/robotis_controller.h @@ -95,14 +95,11 @@ private: void initializeSyncWrite(); public: - static const int DEFAULT_CONTROL_CYCLE_MSEC = 8; // 8 msec - bool DEBUG_PRINT; Robot *robot_; bool gazebo_mode_; std::string gazebo_robot_name_; - int control_cycle_msec_; /* bulk read */ std::map port_to_bulk_read_; diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index d75684e..966dbdc 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -51,8 +51,7 @@ RobotisController::RobotisController() DEBUG_PRINT(false), robot_(0), gazebo_mode_(false), - gazebo_robot_name_("robotis"), - control_cycle_msec_(DEFAULT_CONTROL_CYCLE_MSEC) + gazebo_robot_name_("robotis") { direct_sync_write_.clear(); } @@ -604,7 +603,7 @@ void RobotisController::initializeDevice(const std::string init_file_path) void RobotisController::gazeboTimerThread() { - ros::Rate gazebo_rate(1000 / control_cycle_msec_); + ros::Rate gazebo_rate(1000 / robot_->getControlCycle()); while (!stop_timer_) { @@ -663,7 +662,7 @@ void RobotisController::msgQueueThread() ros::ServiceServer joint_module_server = ros_node.advertiseService("/robotis/get_present_joint_ctrl_modules", &RobotisController::getCtrlModuleCallback, this); - ros::WallDuration duration(control_cycle_msec_ / 1000.0); + ros::WallDuration duration(robot_->getControlCycle() / 1000.0); while(ros_node.ok()) callback_queue.callAvailable(duration); } @@ -680,8 +679,8 @@ void *RobotisController::timerThread(void *param) while (!controller->stop_timer_) { - next_time.tv_sec += (next_time.tv_nsec + controller->control_cycle_msec_ * 1000000) / 1000000000; - next_time.tv_nsec = (next_time.tv_nsec + controller->control_cycle_msec_ * 1000000) % 1000000000; + next_time.tv_sec += (next_time.tv_nsec + controller->robot_->getControlCycle() * 1000000) / 1000000000; + next_time.tv_nsec = (next_time.tv_nsec + controller->robot_->getControlCycle() * 1000000) % 1000000000; controller->process(); @@ -1430,7 +1429,7 @@ void RobotisController::addMotionModule(MotionModule *module) } } - module->initialize(control_cycle_msec_, robot_); + module->initialize(robot_->getControlCycle(), robot_); motion_modules_.push_back(module); motion_modules_.unique(); } @@ -1452,7 +1451,7 @@ void RobotisController::addSensorModule(SensorModule *module) } } - module->initialize(control_cycle_msec_, robot_); + module->initialize(robot_->getControlCycle(), robot_); sensor_modules_.push_back(module); sensor_modules_.unique(); } @@ -1732,7 +1731,7 @@ void RobotisController::setJointCtrlModuleThread(const robotis_controller_msgs:: for(std::list::iterator _stop_m_it = _stop_modules.begin(); _stop_m_it != _stop_modules.end(); _stop_m_it++) { while((*_stop_m_it)->isRunning()) - usleep(control_cycle_msec_ * 1000); + usleep(robot_->getControlCycle() * 1000); } // disable module(s) @@ -1947,7 +1946,7 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) for (auto stop_m_it = stop_modules.begin(); stop_m_it != stop_modules.end(); stop_m_it++) { while ((*stop_m_it)->isRunning()) - usleep(control_cycle_msec_ * 1000); + usleep(robot_->getControlCycle() * 1000); } // disable module(s) diff --git a/robotis_device/include/robotis_device/robot.h b/robotis_device/include/robotis_device/robot.h index 91b30f1..684fe6a 100755 --- a/robotis_device/include/robotis_device/robot.h +++ b/robotis_device/include/robotis_device/robot.h @@ -48,17 +48,23 @@ #define DYNAMIXEL "dynamixel" #define SENSOR "sensor" +#define SESSION_CONTROL_INFO "control info" #define SESSION_PORT_INFO "port info" #define SESSION_DEVICE_INFO "device info" #define SESSION_TYPE_INFO "type info" #define SESSION_CONTROL_TABLE "control table" +#define DEFAULT_CONTROL_CYCLE 8 // milliseconds + namespace robotis_framework { class Robot { +private: + int control_cycle_msec_; + public: std::map ports_; // string: port name std::map port_default_device_; // port name, default device name @@ -70,6 +76,8 @@ public: Sensor *getSensor(std::string path, int id, std::string port, float protocol_version); Dynamixel *getDynamixel(std::string path, int id, std::string port, float protocol_version); + + int getControlCycle(); }; } diff --git a/robotis_device/src/robotis_device/robot.cpp b/robotis_device/src/robotis_device/robot.cpp index fcc8fa0..94d4133 100755 --- a/robotis_device/src/robotis_device/robot.cpp +++ b/robotis_device/src/robotis_device/robot.cpp @@ -76,6 +76,7 @@ static inline std::vector split(const std::string &text, char sep) } 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 += "/"; @@ -106,7 +107,16 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) continue; } - if (session == SESSION_PORT_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) { std::vector tokens = split(input_str, '|'); if (tokens.size() != 3) @@ -485,3 +495,7 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float return dxl; } +int Robot::getControlCycle() +{ + return control_cycle_msec_; +}