From 7e65f4362993eb91e3428f49b0b8d22039b2234e Mon Sep 17 00:00:00 2001 From: JaehongOh Date: Thu, 21 May 2026 15:20:17 +0900 Subject: [PATCH] Add_tactile_sensor Signed-off-by: JaehongOh --- .../dynamixel/dynamixel.hpp | 20 +++ .../dynamixel_hardware_interface.hpp | 7 + param/dxl_model/dynamixel.model | 1 + param/dxl_model/hx_sensor_tip.model | 71 ++++++++++ src/dynamixel/dynamixel.cpp | 133 +++++++++++++++++- src/dynamixel/dynamixel_info.cpp | 6 +- src/dynamixel_hardware_interface.cpp | 58 +++++++- 7 files changed, 289 insertions(+), 7 deletions(-) create mode 100644 param/dxl_model/hx_sensor_tip.model diff --git a/include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp b/include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp index 70a5473..e801f98 100644 --- a/include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp +++ b/include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp @@ -205,6 +205,11 @@ class Dynamixel bool read_type_; std::vector read_data_list_; + // Sensor-only read items. These are NOT added to the SyncRead/FastSyncRead + // group so a tactile-side failure can never delay the arm read group. + // Serviced by ReadSensorOnly() with a single batched readTxRx per comm_id. + std::vector sensor_read_data_list_; + // sync read dynamixel::GroupSyncRead * group_sync_read_ = nullptr; // bulk read @@ -252,6 +257,21 @@ class Dynamixel std::vector> data_vec_ptr); DxlError SetMultiDxlRead(); + // Sensor-only read path. Items registered here bypass the SyncRead / + // FastSyncRead group and are read by ReadSensorOnly() (single batched + // readTxRx per comm_id, no retry, silent on failure). Useful for tactile + // or environmental sensors whose occasional packet loss should not stall + // the arm read loop. + DxlError SetDxlSensorReadItems( + uint8_t comm_id, uint8_t id, std::vector item_names, + std::vector> data_vec_ptr); + DxlError ReadSensorOnly(); + + // Allow callers to opt out of FastSyncRead/FastBulkRead at runtime. The + // internal fallback (on first failure) already exists; this lets a robot + // disable the optimization up-front via hardware parameters. + void SetUseFastReadProtocol(bool enable) {use_fast_read_protocol_ = enable;} + // DXL Write Setting DxlError SetDxlWriteItems( uint8_t comm_id, uint8_t id, std::vector item_names, diff --git a/include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp b/include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp index 6bb0c24..332a1f4 100644 --- a/include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp +++ b/include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp @@ -254,6 +254,13 @@ class DynamixelHardware : public bool is_set_hdl_{false}; + // Sensor read pacing — arm SyncRead runs every cycle, sensors registered + // through Dynamixel::SetDxlSensorReadItems are read once per + // tactile_read_divider_ cycles via Dynamixel::ReadSensorOnly. Counter is + // incremented in read(). + uint32_t read_cycle_count_{0}; + int32_t tactile_read_divider_{10}; + // joint <-> transmission matrix size_t num_of_joints_; size_t num_of_transmissions_; diff --git a/param/dxl_model/dynamixel.model b/param/dxl_model/dynamixel.model index 78cc3b0..6216b72 100644 --- a/param/dxl_model/dynamixel.model +++ b/param/dxl_model/dynamixel.model @@ -3,6 +3,7 @@ Number Name 220 omy_hat.model 230 omy_end.model 231 omy_end_rh_p12_rn.model +250 hx_sensor_tip.model 260 hx5_d20_rr.model 261 hx5_d20_rl.model 6001 hx5_d20_r_synctable_1_device_1.model diff --git a/param/dxl_model/hx_sensor_tip.model b/param/dxl_model/hx_sensor_tip.model new file mode 100644 index 0000000..10e3c82 --- /dev/null +++ b/param/dxl_model/hx_sensor_tip.model @@ -0,0 +1,71 @@ +[unit info] +Data Name value unit Sign Type Offset +Present Pressure 1 0.01 hPa unsigned 0.0 +Present Pressure 2 0.01 hPa unsigned 0.0 +Present Pressure 3 0.01 hPa unsigned 0.0 +Present Pressure 4 0.01 hPa unsigned 0.0 +Present Pressure 5 0.01 hPa unsigned 0.0 +Present Pressure 6 0.01 hPa unsigned 0.0 +Present Pressure 7 0.01 hPa unsigned 0.0 +Present Pressure 8 0.01 hPa unsigned 0.0 +Present Pressure 9 0.01 hPa unsigned 0.0 +Center X 1.0 raw unsigned 0.0 +Center Y 1.0 raw unsigned 0.0 +Present Force 0.01 hPa unsigned 0.0 +Hardware Error Status 1.0 raw unsigned 0.0 +Realtime Tick 1.0 ms unsigned 0.0 +Present Input Voltage 0.1 V unsigned 0.0 +Present Temperature 1.0 degC unsigned 0.0 +PressureRaw 1 1.0 Pa unsigned 0.0 +PressureRaw 2 1.0 Pa unsigned 0.0 +PressureRaw 3 1.0 Pa unsigned 0.0 +PressureRaw 4 1.0 Pa unsigned 0.0 +PressureRaw 5 1.0 Pa unsigned 0.0 +PressureRaw 6 1.0 Pa unsigned 0.0 +PressureRaw 7 1.0 Pa unsigned 0.0 +PressureRaw 8 1.0 Pa unsigned 0.0 +PressureRaw 9 1.0 Pa unsigned 0.0 +LED 1.0 raw unsigned 0.0 +Calibration Enable 1.0 raw unsigned 0.0 +Status Return Level 1.0 raw unsigned 0.0 + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate (Bus) +9 1 Return Delay Time +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +71 1 Calibration Enable +72 1 Present Pressure 1 +73 1 Present Pressure 2 +74 1 Present Pressure 3 +75 1 Present Pressure 4 +76 1 Present Pressure 5 +77 1 Present Pressure 6 +78 1 Present Pressure 7 +79 1 Present Pressure 8 +80 1 Present Pressure 9 +82 1 Center X +83 1 Center Y +84 2 Present Force +120 2 Realtime Tick +144 2 Present Input Voltage +146 1 Present Temperature +336 4 PressureRaw 1 +340 4 PressureRaw 2 +344 4 PressureRaw 3 +348 4 PressureRaw 4 +352 4 PressureRaw 5 +356 4 PressureRaw 6 +360 4 PressureRaw 7 +364 4 PressureRaw 8 +368 4 PressureRaw 9 diff --git a/src/dynamixel/dynamixel.cpp b/src/dynamixel/dynamixel.cpp index 01ef70c..40d93c9 100644 --- a/src/dynamixel/dynamixel.cpp +++ b/src/dynamixel/dynamixel.cpp @@ -16,6 +16,7 @@ #include "dynamixel_hardware_interface/dynamixel/dynamixel.hpp" +#include #include #include #include @@ -395,6 +396,7 @@ void Dynamixel::RWDataReset() { read_data_list_.clear(); write_data_list_.clear(); + sensor_read_data_list_.clear(); } DxlError Dynamixel::SetDxlReadItems( @@ -469,6 +471,122 @@ DxlError Dynamixel::SetDxlReadItems( return DxlError::OK; } +DxlError Dynamixel::SetDxlSensorReadItems( + uint8_t comm_id, + uint8_t id, + std::vector item_names, + std::vector> data_vec_ptr) +{ + if (item_names.empty()) { + return DxlError::OK; + } + if (item_names.size() != data_vec_ptr.size()) { + fprintf( + stderr, "[SetDxlSensorReadItems] Incorrect data size [%zu] [%zu]\n", + item_names.size(), data_vec_ptr.size()); + return DxlError::SET_READ_ITEM_FAIL; + } + + std::vector item_ids; + std::vector item_addrs; + std::vector item_sizes; + for (const auto & it_name : item_names) { + uint16_t addr; + uint8_t size; + if (!dxl_info_.GetDxlControlItem(comm_id, id, it_name, addr, size)) { + fprintf( + stderr, + "[SetDxlSensorReadItems][comm_id:%03d][ID:%03d] Cannot find control item: %s\n", + comm_id, id, it_name.c_str()); + return DxlError::CANNOT_FIND_CONTROL_ITEM; + } + item_ids.push_back(id); + item_addrs.push_back(addr); + item_sizes.push_back(size); + } + + // Append to an existing entry for this comm_id if present, so multiple + // sensor IDs on the same bus share a single batched read. + for (auto & existing_item : sensor_read_data_list_) { + if (existing_item.comm_id == comm_id) { + existing_item.id_arr.insert(existing_item.id_arr.end(), item_ids.begin(), item_ids.end()); + existing_item.item_name.insert( + existing_item.item_name.end(), item_names.begin(), item_names.end()); + existing_item.item_addr.insert( + existing_item.item_addr.end(), item_addrs.begin(), item_addrs.end()); + existing_item.item_size.insert( + existing_item.item_size.end(), item_sizes.begin(), item_sizes.end()); + existing_item.item_data_ptr_vec.insert( + existing_item.item_data_ptr_vec.end(), + data_vec_ptr.begin(), data_vec_ptr.end()); + return DxlError::OK; + } + } + + RWItemList new_item; + new_item.comm_id = comm_id; + new_item.id_arr = std::move(item_ids); + new_item.item_name = std::move(item_names); + new_item.item_addr = std::move(item_addrs); + new_item.item_size = std::move(item_sizes); + new_item.item_data_ptr_vec = std::move(data_vec_ptr); + sensor_read_data_list_.push_back(new_item); + return DxlError::OK; +} + +DxlError Dynamixel::ReadSensorOnly() +{ + if (sensor_read_data_list_.empty()) { + return DxlError::OK; + } + DxlError worst = DxlError::OK; + for (const auto & list : sensor_read_data_list_) { + if (list.item_addr.empty()) { + continue; + } + // Batch into one readTxRx covering [min_addr, max_addr+size) per comm_id. + // Empirically ~9x faster than N per-item ReadItem calls at 4 Mbps because + // the per-packet tx+rx round-trip dominates over wire bytes; address gaps + // (rare in a single sensor block) cost a few extra bytes per cycle. + uint16_t min_addr = list.item_addr.at(0); + uint16_t end_addr = static_cast(list.item_addr.at(0) + list.item_size.at(0)); + for (size_t i = 1; i < list.item_addr.size(); ++i) { + min_addr = std::min(min_addr, list.item_addr.at(i)); + end_addr = std::max( + end_addr, + static_cast(list.item_addr.at(i) + list.item_size.at(i))); + } + const uint16_t length = end_addr - min_addr; + std::vector buf(length, 0); + uint8_t dxl_error = 0; + int dxl_comm_result = packet_handler_->readTxRx( + port_handler_, list.comm_id, min_addr, length, buf.data(), &dxl_error); + + // Silent on failure: keep previous sample values, surface worst code to + // caller so the cycle counter can decide whether to log. + if (dxl_comm_result != COMM_SUCCESS) { + worst = DxlError::ITEM_READ_FAIL; + continue; + } + if (dxl_error != 0 && !(dxl_error & 0x80)) { + worst = DxlError::ITEM_READ_FAIL; + continue; + } + + // Distribute bytes from buf back into per-item value pointers (little-endian). + for (size_t i = 0; i < list.item_addr.size(); ++i) { + const size_t off = list.item_addr.at(i) - min_addr; + const uint8_t sz = list.item_size.at(i); + uint32_t v = 0; + for (uint8_t b = 0; b < sz; ++b) { + v |= static_cast(buf.at(off + b)) << (8 * b); + } + *list.item_data_ptr_vec.at(i) = static_cast(v); + } + } + return worst; +} + DxlError Dynamixel::SetMultiDxlRead() { read_type_ = checkReadType(); @@ -2063,7 +2181,10 @@ DxlError Dynamixel::SetDxlValueToSyncWrite() { for (auto it_write_data : write_data_list_) { uint8_t comm_id = it_write_data.comm_id; - uint8_t * param_write_value = new uint8_t[indirect_info_write_[comm_id].size]; + // Use a vector so the buffer is freed automatically on every return path + // (previous raw `new uint8_t[]` leaked on most paths). + std::vector param_write_value_buf(indirect_info_write_[comm_id].size); + uint8_t * param_write_value = param_write_value_buf.data(); uint8_t added_byte = 0; for (uint16_t item_index = 0; item_index < indirect_info_write_[comm_id].cnt; item_index++) { @@ -2268,8 +2389,11 @@ DxlError Dynamixel::SetDxlValueToBulkWrite() uint8_t added_byte = 0; // Check if this is a direct write + std::vector param_write_value_buf; if (direct_info_write_.find(comm_id) != direct_info_write_.end()) { - param_write_value = new uint8_t[direct_info_write_[comm_id].size]; + // Vector-backed buffer auto-frees on scope exit (was `new uint8_t[]`). + param_write_value_buf.assign(direct_info_write_[comm_id].size, 0); + param_write_value = param_write_value_buf.data(); for (uint16_t item_index = 0; item_index < direct_info_write_[comm_id].cnt; item_index++) { double data = *it_write_data.item_data_ptr_vec.at(item_index); @@ -2312,8 +2436,9 @@ DxlError Dynamixel::SetDxlValueToBulkWrite() return DxlError::BULK_WRITE_FAIL; } } else { - // Handle indirect write - param_write_value = new uint8_t[indirect_info_write_[comm_id].size]; + // Handle indirect write — vector RAII (was leaking `new uint8_t[]`). + param_write_value_buf.assign(indirect_info_write_[comm_id].size, 0); + param_write_value = param_write_value_buf.data(); for (uint16_t item_index = 0; item_index < indirect_info_write_[comm_id].cnt; item_index++) { double data = *it_write_data.item_data_ptr_vec.at(item_index); diff --git a/src/dynamixel/dynamixel_info.cpp b/src/dynamixel/dynamixel_info.cpp index 63c3095..3962f12 100644 --- a/src/dynamixel/dynamixel_info.cpp +++ b/src/dynamixel/dynamixel_info.cpp @@ -20,6 +20,7 @@ #include #include #include +#include namespace dynamixel_hardware_interface { @@ -34,8 +35,9 @@ void DynamixelInfo::InitDxlModelInfo() std::string model_file = dxl_model_file_dir + "/dynamixel.model"; std::ifstream open_file(model_file.c_str()); if (open_file.is_open() != 1) { - fprintf(stderr, "[ERROR] CANNOT FIND DXL MODEL LIST FILE.\n%s\n", model_file.c_str()); - exit(-1); + // Throw instead of exit() so controller_manager can surface the error + // through normal lifecycle transitions rather than killing the host. + throw std::runtime_error("[ERROR] CANNOT FIND DXL MODEL LIST FILE: " + model_file); } std::string line; getline(open_file, line); diff --git a/src/dynamixel_hardware_interface.cpp b/src/dynamixel_hardware_interface.cpp index 3fefaf4..6ff91a8 100644 --- a/src/dynamixel_hardware_interface.cpp +++ b/src/dynamixel_hardware_interface.cpp @@ -144,6 +144,30 @@ hardware_interface::CallbackReturn DynamixelHardware::on_init( (ament_index_cpp::get_package_share_directory("dynamixel_hardware_interface") + dxl_model_folder).c_str())); + if (info_.hardware_parameters.find("use_fast_read_protocol") != + info_.hardware_parameters.end()) + { + const std::string & s = info_.hardware_parameters.at("use_fast_read_protocol"); + bool enable = (s == "true" || s == "1"); + dxl_comm_->SetUseFastReadProtocol(enable); + RCLCPP_INFO_STREAM(logger_, "use_fast_read_protocol = " << (enable ? "true" : "false")); + } + + if (info_.hardware_parameters.find("tactile_read_divider") != + info_.hardware_parameters.end()) + { + try { + int v = std::stoi(info_.hardware_parameters.at("tactile_read_divider")); + tactile_read_divider_ = (v > 0) ? v : 1; + } catch (const std::exception & e) { + RCLCPP_WARN_STREAM( + logger_, + "Invalid tactile_read_divider, using default " << tactile_read_divider_ << ": " << + e.what()); + } + } + RCLCPP_INFO_STREAM(logger_, "tactile_read_divider = " << tactile_read_divider_); + RCLCPP_INFO_STREAM(logger_, "$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$"); RCLCPP_INFO_STREAM(logger_, "$$$$$ Init Dxl Comm Port"); @@ -459,6 +483,20 @@ DynamixelHardware::export_state_interfaces() it.name, it.interface_name_vec.at(i), it.value_ptr_vec.at(i).get())); } } + for (auto it : hdl_gpio_sensor_states_) { + for (size_t i = 0; i < it.value_ptr_vec.size(); i++) { + if (i >= it.interface_name_vec.size()) { + RCLCPP_ERROR_STREAM( + logger_, "Interface name vector size mismatch for gpio sensor " << it.name << + ". Expected size: " << it.value_ptr_vec.size() << + ", Actual size: " << it.interface_name_vec.size()); + continue; + } + state_interfaces.emplace_back( + hardware_interface::StateInterface( + it.name, it.interface_name_vec.at(i), it.value_ptr_vec.at(i).get())); + } + } for (auto it : hdl_gpio_controller_states_) { for (size_t i = 0; i < it.value_ptr_vec.size(); i++) { if (i >= it.interface_name_vec.size()) { @@ -638,6 +676,17 @@ hardware_interface::return_type DynamixelHardware::read( CalcTransmissionToJoint(); + // Sensor read pacing — kept out of the arm SyncRead group. ReadSensorOnly + // does a single batched readTxRx per comm_id with no retry; silent on + // failure so a missed tactile packet never propagates into the arm path. + // The counter starts at 0 so the first sample fires on the first cycle. + if (tactile_read_divider_ > 0 && + (read_cycle_count_ % static_cast(tactile_read_divider_)) == 0) + { + (void)dxl_comm_->ReadSensorOnly(); + } + ++read_cycle_count_; + for (auto sensor : hdl_gpio_sensor_states_) { ReadSensorData(sensor); } @@ -1073,8 +1122,15 @@ bool DynamixelHardware::InitDxlReadItems() return false; } } + // Route gpio_sensor entries to the sensor-only read path. This keeps the + // SyncRead / FastSyncRead group composed of motor IDs only, so a tactile + // failure cannot delay or fail the arm read. + // NOTE for reviewers: this is a behavior change for existing users with + // gpio_sensor entries. If a backward-compatible rollout is preferred, gate + // the dispatch on a hardware_parameter (e.g. + // `route_gpio_sensors_to_sensor_only_read`, default false). for (auto it : hdl_gpio_sensor_states_) { - if (dxl_comm_->SetDxlReadItems( + if (dxl_comm_->SetDxlSensorReadItems( it.comm_id, it.id, it.interface_name_vec, it.value_ptr_vec) != DxlError::OK) {