From ac3dc6d9eb7be820dbb9816c70c6c72ea15cfc19 Mon Sep 17 00:00:00 2001 From: zerom Date: Tue, 11 Jul 2017 13:26:10 +0900 Subject: [PATCH 1/3] - OpenCR control table item name changed. (torque_enable -> dynamixel_power) - fixed to not update update_time_stamp_ if bulk read fails. --- .../robotis_controller/robotis_controller.cpp | 20 ++++++++++++------- robotis_device/devices/sensor/OPEN-CR.device | 2 +- 2 files changed, 14 insertions(+), 8 deletions(-) mode change 100644 => 100755 robotis_device/devices/sensor/OPEN-CR.device diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 966dbdc..c494cd0 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_) @@ -911,12 +911,14 @@ void RobotisController::process() if (dxl->bulk_read_items_.size() > 0) { - uint32_t data = 0; + bool updated = false; + 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) { + updated = true; data = port_to_bulk_read_[port_name]->getData(dxl->id_, item->address_, item->data_length_); // change dxl_state @@ -938,7 +940,8 @@ void RobotisController::process() } // -> 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); + if (updated == true) + dxl->dxl_state_->update_time_stamp_ = TimeStamp(present_state.header.stamp.sec, present_state.header.stamp.nsec); } } } @@ -954,12 +957,14 @@ void RobotisController::process() if (sensor->bulk_read_items_.size() > 0) { - uint32_t data = 0; + bool updated = false; + uint32_t data = 0; for (int i = 0; i < sensor->bulk_read_items_.size(); i++) { ControlTableItem *item = sensor->bulk_read_items_[i]; if (port_to_bulk_read_[port_name]->isAvailable(sensor->id_, item->address_, item->data_length_) == true) { + updated = true; data = port_to_bulk_read_[port_name]->getData(sensor->id_, item->address_, item->data_length_); // change sensor_state @@ -968,7 +973,8 @@ void RobotisController::process() } // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp - sensor->sensor_state_->update_time_stamp_ = TimeStamp(present_state.header.stamp.sec, present_state.header.stamp.nsec); + if (updated == true) + sensor->sensor_state_->update_time_stamp_ = TimeStamp(present_state.header.stamp.sec, present_state.header.stamp.nsec); } } } diff --git a/robotis_device/devices/sensor/OPEN-CR.device b/robotis_device/devices/sensor/OPEN-CR.device old mode 100644 new mode 100755 index 94b446e..71cee8e --- a/robotis_device/devices/sensor/OPEN-CR.device +++ b/robotis_device/devices/sensor/OPEN-CR.device @@ -10,7 +10,7 @@ device_type = sensor 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 + 24 | dynamixel_power | 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 From ef1c16664af20654154a7711e64932cf85af6311 Mon Sep 17 00:00:00 2001 From: zerom Date: Tue, 11 Jul 2017 14:42:15 +0900 Subject: [PATCH 2/3] - unnecessary debug print removed. --- .../robotis_controller/robotis_controller.cpp | 19 ++++++++----------- 1 file changed, 8 insertions(+), 11 deletions(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index c494cd0..94f4ccb 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -1469,9 +1469,11 @@ void RobotisController::removeSensorModule(SensorModule *module) void RobotisController::writeControlTableCallback(const robotis_controller_msgs::WriteControlTable::ConstPtr &msg) { - fprintf(stderr, "[WriteControlTable] led control msg received\n"); Device *device = NULL; + if (DEBUG_PRINT) + fprintf(stderr, "[WriteControlTable] led control msg received\n"); + auto dev_it1 = robot_->dxls_.find(msg->joint_name); if(dev_it1 != robot_->dxls_.end()) { @@ -1490,7 +1492,7 @@ void RobotisController::writeControlTableCallback(const robotis_controller_msgs: return; } } - fprintf(stderr, " #1 "); + ControlTableItem *item = NULL; auto item_it = device->ctrl_table_.find(msg->start_item_name); if(item_it != device->ctrl_table_.end()) @@ -1503,7 +1505,6 @@ void RobotisController::writeControlTableCallback(const robotis_controller_msgs: return; } - fprintf(stderr, " #2 "); dynamixel::PortHandler *port = robot_->ports_[device->port_name_]; dynamixel::PacketHandler *packet_handler = dynamixel::PacketHandler::getPacketHandler(device->protocol_version_); @@ -1512,17 +1513,13 @@ void RobotisController::writeControlTableCallback(const robotis_controller_msgs: queue_mutex_.lock(); - direct_sync_write_.push_back(new dynamixel::GroupSyncWrite(port, packet_handler, item->address_, msg->data_length)); - - fprintf(stderr, " #3 \n"); - direct_sync_write_[direct_sync_write_.size() - 1]->addParam(device->id_, (uint8_t *)(msg->data.data())); - fprintf(stderr, "[WriteControlTable] %s -> %s : ", msg->joint_name.c_str(), msg->start_item_name.c_str()); - for (auto &dt : msg->data) - fprintf(stderr, "%02X ", dt); - fprintf(stderr, "\n"); +// fprintf(stderr, "[WriteControlTable] %s -> %s : ", msg->joint_name.c_str(), msg->start_item_name.c_str()); +// for (auto &dt : msg->data) +// fprintf(stderr, "%02X ", dt); +// fprintf(stderr, "\n"); queue_mutex_.unlock(); From 05f4a5e51c7fac1659fb0eaecf50de29bda8051e Mon Sep 17 00:00:00 2001 From: zerom Date: Wed, 9 Aug 2017 14:27:40 +0900 Subject: [PATCH 3/3] multi thread bug fixed. --- .../src/robotis_controller/robotis_controller.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 94f4ccb..be70a4e 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -1501,7 +1501,7 @@ void RobotisController::writeControlTableCallback(const robotis_controller_msgs: } else { - ROS_WARN("WriteControlTable] Unknown item : %s", msg->start_item_name.c_str()); + ROS_WARN("[WriteControlTable] Unknown item : %s", msg->start_item_name.c_str()); return; } @@ -1569,6 +1569,8 @@ void RobotisController::syncWriteItemCallback(const robotis_controller_msgs::Syn if (item->access_type_ == Read) continue; + queue_mutex_.lock(); + int idx = 0; if (direct_sync_write_.size() == 0) { @@ -1604,6 +1606,8 @@ void RobotisController::syncWriteItemCallback(const robotis_controller_msgs::Syn } direct_sync_write_[idx]->addParam(device->id_, data); delete[] data; + + queue_mutex_.unlock(); } }