diff --git a/.gitignore b/.gitignore index 6702710..36a5c0a 100755 --- a/.gitignore +++ b/.gitignore @@ -1 +1,11 @@ -/robotis_device/devices/dynamixel/H54-100-B210-R-NR.device +build +devel +bin +lib +msg_gen +srv_gen +qtcreator-build +*~ +*.backup +*.user +*.autosave diff --git a/robotis_controller/CHANGELOG.rst b/robotis_controller/CHANGELOG.rst index c310553..002d663 100755 --- a/robotis_controller/CHANGELOG.rst +++ b/robotis_controller/CHANGELOG.rst @@ -1,23 +1,64 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package robotis_controller -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.1.1 (2016-08-18) ------------ -* updated the package information - -0.1.0 (2016-08-12) ------------ -* first public release for Kinetic -* modified the package information for release -* develop branch -> master branch -* function name changed : DeviceInit() -> InitDevice() -* Fixed high CPU consumption due to busy waits -* add SensorState - add Singleton template -* XM-430 / CM-740 device file added. - Sensor device added. -* added code to support the gazebo simulator -* added first bulk read failure protection code -* renewal -* Contributors: Alexander Stumpf, Jay Song, Zerom, Pyo +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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). +* 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 + +0.1.0 (2016-08-12) +----------- +* first public release for Kinetic +* modified the package information for release +* develop branch -> master branch +* function name changed : DeviceInit() -> InitDevice() +* Fixed high CPU consumption due to busy waits +* add SensorState + add Singleton template +* XM-430 / CM-740 device file added. + Sensor device added. +* added code to support the gazebo simulator +* added first bulk read failure protection code +* renewal +* Contributors: Alexander Stumpf, Jay Song, Zerom, Pyo diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h index 8c1ae27..f811b24 100755 --- a/robotis_controller/include/robotis_controller/robotis_controller.h +++ b/robotis_controller/include/robotis_controller/robotis_controller.h @@ -95,8 +95,6 @@ private: void initializeSyncWrite(); public: - static const int CONTROL_CYCLE_MSEC = 8; // 8 msec - bool DEBUG_PRINT; Robot *robot_; diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index fd96c43..e2368e3 100755 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -1,28 +1,32 @@ - - - robotis_controller - 0.1.1 - - The main package that controls THORMANG3. - - BSD - Zerom - Pyo - https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues - https://github.com/ROBOTIS-GIT/ROBOTIS-Framework - http://wiki.ros.org/robotis_controller - catkin - roscpp - roslib - std_msgs - sensor_msgs - robotis_controller_msgs - robotis_framework_common - roscpp - roslib - std_msgs - sensor_msgs - robotis_controller_msgs - robotis_framework_common - - + + + robotis_controller + 0.2.1 + + The main package that controls THORMANG3. + + BSD + Zerom + Pyo + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework + http://wiki.ros.org/robotis_controller + catkin + roscpp + roslib + std_msgs + sensor_msgs + dynamixel_sdk + robotis_device + robotis_controller_msgs + robotis_framework_common + roscpp + roslib + std_msgs + sensor_msgs + dynamixel_sdk + robotis_device + robotis_controller_msgs + robotis_framework_common + + diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 5a0c1ec..966dbdc 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -603,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_) { @@ -662,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); } @@ -679,8 +679,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->robot_->getControlCycle() * 1000000) / 1000000000; + next_time.tv_nsec = (next_time.tv_nsec + controller->robot_->getControlCycle() * 1000000) % 1000000000; controller->process(); @@ -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(); } } @@ -1375,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(); } @@ -1397,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(); } @@ -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) @@ -1664,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) @@ -1879,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/CHANGELOG.rst b/robotis_device/CHANGELOG.rst index 91b9800..20b2638 100755 --- a/robotis_device/CHANGELOG.rst +++ b/robotis_device/CHANGELOG.rst @@ -1,24 +1,45 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package robotis_device -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.1.1 (2016-08-18) ------------ -* updated the package information - -0.1.0 (2016-08-12) ------------ -* first public release for Kinetic -* modified the package information for release -* develop branch -> master branch -* Setting the license to BSD. -* add SensorState - add Singleton template -* XM-430 / CM-740 device file added. - Sensor device added. -* modified. -* variable name changed. - ConvertRadian2Value / ConvertValue2Radian function bug fixed. -* added code to support the gazebo simulator -* renewal -* Contributors: ROBOTIS, ROBOTIS-zerom, pyo +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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). +* 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 + +0.1.0 (2016-08-12) +----------- +* first public release for Kinetic +* modified the package information for release +* develop branch -> master branch +* Setting the license to BSD. +* add SensorState + add Singleton template +* XM-430 / CM-740 device file added. + Sensor device added. +* modified. +* variable name changed. + ConvertRadian2Value / ConvertValue2Radian function bug fixed. +* added code to support the gazebo simulator +* renewal +* Contributors: ROBOTIS, ROBOTIS-zerom, pyo 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/package.xml b/robotis_device/package.xml index 1b0d2f3..516770d 100755 --- a/robotis_device/package.xml +++ b/robotis_device/package.xml @@ -1,23 +1,23 @@ - - - robotis_device - 0.1.1 - - The package that manages device information of ROBOTIS robots. - This package is used when reading device information with the robot information file - from the robotis_controller package. - - BSD - Zerom - Pyo - https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues - https://github.com/ROBOTIS-GIT/ROBOTIS-Framework - http://wiki.ros.org/robotis_device - catkin - roscpp - rospy - dynamixel_sdk - roscpp - rospy - dynamixel_sdk - + + + robotis_device + 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 + from the robotis_controller package. + + BSD + Zerom + Pyo + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework + http://wiki.ros.org/robotis_device + catkin + roscpp + rospy + dynamixel_sdk + roscpp + rospy + dynamixel_sdk + 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_; +} diff --git a/robotis_framework/CHANGELOG.rst b/robotis_framework/CHANGELOG.rst index 2a746ef..87f4509 100755 --- a/robotis_framework/CHANGELOG.rst +++ b/robotis_framework/CHANGELOG.rst @@ -1,12 +1,19 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package robotis_framework -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.1.1 (2016-08-18) ------------ -* updated the package information - -0.1.0 (2016-08-12) ------------ -* make a meta-package -* Contributors: Zerom, Pyo +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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 + +0.1.1 (2016-08-18) +----------- +* updated the package information + +0.1.0 (2016-08-12) +----------- +* make a meta-package diff --git a/robotis_framework/CMakeLists.txt b/robotis_framework/CMakeLists.txt old mode 100755 new mode 100644 index c5278ee..d336fe9 --- a/robotis_framework/CMakeLists.txt +++ b/robotis_framework/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) -project(robotis_framework) -find_package(catkin REQUIRED) -catkin_metapackage() +cmake_minimum_required(VERSION 2.8.3) +project(robotis_framework) +find_package(catkin REQUIRED) +catkin_metapackage() diff --git a/robotis_framework/package.xml b/robotis_framework/package.xml index 87628dd..44a7089 100755 --- a/robotis_framework/package.xml +++ b/robotis_framework/package.xml @@ -1,17 +1,17 @@ - - - robotis_framework - 0.1.1 - ROS packages for the robotis_framework (meta package) - BSD - Zerom - Pyo - https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues - https://github.com/ROBOTIS-GIT/ROBOTIS-Framework - http://wiki.ros.org/robotis_framework - catkin - robotis_framework_common - robotis_device - robotis_controller - - + + + robotis_framework + 0.2.1 + ROS packages for the robotis_framework (meta package) + BSD + Zerom + Pyo + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework + http://wiki.ros.org/robotis_framework + catkin + robotis_framework_common + robotis_device + robotis_controller + + diff --git a/robotis_framework_common/CHANGELOG.rst b/robotis_framework_common/CHANGELOG.rst index 1adc0e1..fb68bac 100755 --- a/robotis_framework_common/CHANGELOG.rst +++ b/robotis_framework_common/CHANGELOG.rst @@ -1,15 +1,29 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package robotis_framework_common -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.1.1 (2016-08-18) ------------ -* updated the package information - -0.1.0 (2016-08-12) ------------ -* modified the package information for release -* Setting the license to BSD. -* add SensorState - add Singleton template -* Contributors: Jay Song, Zerom, Pyo +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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 +* 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 + +0.1.0 (2016-08-12) +----------- +* modified the package information for release +* Setting the license to BSD. +* add SensorState + add Singleton template +* Contributors: Jay Song, Zerom, Pyo diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index 0957d69..8f0a6c7 100755 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -1,19 +1,19 @@ - - - robotis_framework_common - 0.1.1 - - The package contains commonly used Headers for the ROBOTIS Framework. - - BSD - Zerom - Pyo - https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues - https://github.com/ROBOTIS-GIT/ROBOTIS-Framework - http://wiki.ros.org/robotis_framework_common - catkin - roscpp - robotis_device - roscpp - robotis_device - + + + robotis_framework_common + 0.2.1 + + The package contains commonly used Headers for the ROBOTIS Framework. + + BSD + Zerom + Pyo + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework + http://wiki.ros.org/robotis_framework_common + catkin + roscpp + robotis_device + roscpp + robotis_device +