From 0afd62afb38c82c3b1828c27d7b49dfeb89ada10 Mon Sep 17 00:00:00 2001 From: robotis Date: Fri, 9 Dec 2016 14:50:59 +0900 Subject: [PATCH 1/4] 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 @@ class RobotisController : public Singleton 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 2/4] 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 @@ class RobotisController : public Singleton 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 @@ class Robot 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_; +} From 60dfda6adeca4be6c5c6e98402f8dd3e60536d5f Mon Sep 17 00:00:00 2001 From: kayman Date: Tue, 28 Feb 2017 10:59:04 +0900 Subject: [PATCH 3/4] add deivce : OPEN-CR --- robotis_device/devices/sensor/OPEN-CR.device | 30 ++++++++++++++++++++ 1 file changed, 30 insertions(+) create mode 100644 robotis_device/devices/sensor/OPEN-CR.device diff --git a/robotis_device/devices/sensor/OPEN-CR.device b/robotis_device/devices/sensor/OPEN-CR.device new file mode 100644 index 0000000..94b446e --- /dev/null +++ b/robotis_device/devices/sensor/OPEN-CR.device @@ -0,0 +1,30 @@ +[device info] +model_name = OPEN-CR +device_type = sensor + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 2 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 3 | ID | 1 | RW | EEPROM | 0 | 252 | N + 4 | baudrate | 1 | RW | EEPROM | 0 | 252 | N + 5 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 16 | status_return_level | 1 | RW | EEPROM | 0 | 2 | N + 24 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 25 | LED | 1 | RW | RAM | 0 | 7 | N + 26 | LED_RGB | 2 | RW | RAM | 0 | 32767 | N + 28 | buzzer | 2 | RW | RAM | 0 | 65535 | N + 30 | button | 1 | R | RAM | 0 | 15 | N + 31 | present_voltage | 1 | R | RAM | 50 | 250 | N + 32 | gyro_x | 2 | R | RAM | -32800 | 32800 | Y + 34 | gyro_y | 2 | R | RAM | -32800 | 32800 | Y + 36 | gyro_z | 2 | R | RAM | -32800 | 32800 | Y + 38 | acc_x | 2 | R | RAM | -32768 | 32768 | Y + 40 | acc_y | 2 | R | RAM | -32768 | 32768 | Y + 42 | acc_z | 2 | R | RAM | -32768 | 32768 | Y + 44 | roll | 2 | R | RAM | 0 | 4096 | N + 46 | pitch | 2 | R | RAM | 0 | 4096 | N + 48 | yaw | 2 | R | RAM | 0 | 4096 | N + 50 | imu_control | 1 | RW | RAM | 0 | 255 | N + + \ No newline at end of file From c42dadf25f01bc7de48af9ab1d7c1ec9c5285b91 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Fri, 21 Apr 2017 15:56:31 +0900 Subject: [PATCH 4/4] Update robotis_controller.cpp some logs are commented out. --- .../src/robotis_controller/robotis_controller.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 966dbdc..ca6e374 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -61,7 +61,7 @@ void RobotisController::initializeSyncWrite() if (gazebo_mode_ == true) return; - ROS_INFO("FIRST BULKREAD"); + //ROS_INFO("FIRST BULKREAD"); for (auto& it : port_to_bulk_read_) it.second->txRxPacket(); for(auto& it : port_to_bulk_read_) @@ -72,7 +72,7 @@ void RobotisController::initializeSyncWrite() { if (++error_count > 10) { - ROS_ERROR("[RobotisController] bulk read fail!!"); + ROS_ERROR("[RobotisController] first bulk read fail!!"); exit(-1); } usleep(10 * 1000); @@ -80,7 +80,7 @@ void RobotisController::initializeSyncWrite() } while (result != COMM_SUCCESS); } init_pose_loaded_ = true; - ROS_INFO("FIRST BULKREAD END"); + //ROS_INFO("FIRST BULKREAD END"); // clear syncwrite param setting for (auto& it : port_to_sync_write_position_)