support non-actuator Dynamixel sensor devices (incl. HX Sensor Tip)

include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp

@@ -205,6 +205,11 @@ class Dynamixel
   bool read_type_;
   std::vector<RWItemList> 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<RWItemList> sensor_read_data_list_;
+
   // sync read
   dynamixel::GroupSyncRead * group_sync_read_ = nullptr;
   // bulk read
@@ -252,6 +257,21 @@ class Dynamixel
     std::vector<std::shared_ptr<double>> 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<std::string> item_names,
+    std::vector<std::shared_ptr<double>> 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<std::string> item_names,

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_;

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

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

src/dynamixel/dynamixel.cpp

@@ -16,6 +16,7 @@
 
 #include "dynamixel_hardware_interface/dynamixel/dynamixel.hpp"
 
+#include <algorithm>
 #include <queue>
 #include <vector>
 #include <string>
@@ -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<std::string> item_names,
+  std::vector<std::shared_ptr<double>> 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<uint8_t> item_ids;
+  std::vector<uint16_t> item_addrs;
+  std::vector<uint8_t> 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<uint16_t>(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<uint16_t>(list.item_addr.at(i) + list.item_size.at(i)));
+    }
+    const uint16_t length = end_addr - min_addr;
+    std::vector<uint8_t> 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<uint32_t>(buf.at(off + b)) << (8 * b);
+      }
+      *list.item_data_ptr_vec.at(i) = static_cast<double>(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<uint8_t> 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<uint8_t> 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);

src/dynamixel/dynamixel_info.cpp

@@ -20,6 +20,7 @@
 #include <utility>
 #include <vector>
 #include <regex>
+#include <stdexcept>
 
 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);