From 4c03ddf2f0579ea2fb8e56acfa6cd993fb58b8a4 Mon Sep 17 00:00:00 2001 From: Masaki Murooka Date: Fri, 3 May 2019 00:31:27 +0900 Subject: [PATCH 1/2] dynamixel_workbench_controllers: [FixBug] remove incorrect loop about position_cnt. --- .../src/dynamixel_workbench_controllers.cpp | 17 +++++------------ 1 file changed, 5 insertions(+), 12 deletions(-) diff --git a/dynamixel_workbench_controllers/src/dynamixel_workbench_controllers.cpp b/dynamixel_workbench_controllers/src/dynamixel_workbench_controllers.cpp index c40c61f1..524f4856 100644 --- a/dynamixel_workbench_controllers/src/dynamixel_workbench_controllers.cpp +++ b/dynamixel_workbench_controllers/src/dynamixel_workbench_controllers.cpp @@ -581,7 +581,6 @@ void DynamixelController::writeCallback(const ros::TimerEvent&) int32_t dynamixel_position[dynamixel_.size()]; static uint32_t point_cnt = 0; - static uint32_t position_cnt = 0; for (auto const& joint:jnt_tra_msg_->joint_names) { @@ -600,19 +599,13 @@ void DynamixelController::writeCallback(const ros::TimerEvent&) ROS_ERROR("%s", log); } - position_cnt++; - if (position_cnt >= jnt_tra_msg_->points[point_cnt].positions.size()) + point_cnt++; + if (point_cnt >= jnt_tra_msg_->points.size()) { - point_cnt++; - position_cnt = 0; - if (point_cnt >= jnt_tra_msg_->points.size()) - { - is_moving_ = false; - point_cnt = 0; - position_cnt = 0; + is_moving_ = false; + point_cnt = 0; - ROS_INFO("Complete Execution"); - } + ROS_INFO("Complete Execution"); } } From 7ac5dccdda0ad475316c6558c1ed59a86e990829 Mon Sep 17 00:00:00 2001 From: Masaki Murooka Date: Fri, 3 May 2019 00:30:21 +0900 Subject: [PATCH 2/2] dynamixel_workbench_controllers: [FixBug] use correct id for getPresentPosition. --- .../src/dynamixel_workbench_controllers.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/dynamixel_workbench_controllers/src/dynamixel_workbench_controllers.cpp b/dynamixel_workbench_controllers/src/dynamixel_workbench_controllers.cpp index 524f4856..74290ecd 100644 --- a/dynamixel_workbench_controllers/src/dynamixel_workbench_controllers.cpp +++ b/dynamixel_workbench_controllers/src/dynamixel_workbench_controllers.cpp @@ -288,9 +288,9 @@ bool DynamixelController::getPresentPosition(std::vector dxl_name) { WayPoint wp; uint32_t read_position; - for (auto const& dxl:dynamixel_) + for (auto const id:id_array) { - result = dxl_wb_->readRegister((uint8_t)dxl.second, + result = dxl_wb_->readRegister(id, control_items_["Present_Position"]->address, control_items_["Present_Position"]->data_length, &read_position, @@ -300,7 +300,7 @@ bool DynamixelController::getPresentPosition(std::vector dxl_name) ROS_ERROR("%s", log); } - wp.position = dxl_wb_->convertValue2Radian((uint8_t)dxl.second, read_position); + wp.position = dxl_wb_->convertValue2Radian(id, read_position); pre_goal_.push_back(wp); } }