Merge pull request #59 from ROBOTIS-GIT/feature-ffw-sensor

Support ffw sensor board

Author: Woojin-Crive

CHANGELOG.rst

@@ -2,6 +2,11 @@
 Changelog for package dynamixel_hardware_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.4.9 (2025-06-24)
+------------------
+* Support ffw sensor model
+* Contributors: Woojin Wie
+
 1.4.8 (2025-06-23)
 ------------------
 * Added new model for OMY to use virtual_dxl

package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>dynamixel_hardware_interface</name>
-  <version>1.4.8</version>
+  <version>1.4.9</version>
   <description>
     ROS 2 package providing a hardware interface for controlling Dynamixel motors via the ROS 2 control framework.
   </description>

param/dxl_model/dynamixel.model

@@ -1,5 +1,19 @@
 Number	Name
+220	omy_hat.model
+230	omy_end.model
+231	omy_end_rh_p12_rn.model
 350	xl320.model
+536	sensorxel_joy.model
+537	ffw_g40_imu.model
+600	sensorxel_joy.model
+601	ffw_g10_led.model
+602	ffw_g10_rcu.model
+620	ffw_sg2_steer_1.model
+621	ffw_sg2_steer_2.model
+622	ffw_sg2_steer_3.model
+623	ffw_sg2_drive_1.model
+624	ffw_sg2_drive_2.model
+625	ffw_sg2_drive_3.model
 1000	xh430_w350.model
 1001	xd430_t350.model
 1010	xh430_w210.model
@@ -44,15 +58,3 @@ Number	Name
 4150	ym080_230_r099.model
 4170	ym080_230_a099.model
 35074	rh_p12_rn.model
-220	omy_hat.model
-230	omy_end.model
-231	omy_end_rh_p12_rn.model
-536 sensorxel_joy.model
-600 sensorxel_joy.model
-602 ffw_g10_rcu.model
-620 ffw_sg2_steer_1.model
-621 ffw_sg2_steer_2.model
-622 ffw_sg2_steer_3.model
-623 ffw_sg2_drive_1.model
-624 ffw_sg2_drive_2.model
-625 ffw_sg2_drive_3.model

param/dxl_model/ffw_g10_led.model

@@ -0,0 +1,30 @@
+[control table]
+Address	Size	Data Name
+0	2	Model Number
+2	1	ROBOT_Generation
+3	1	Board_Number
+5	1	Firmware_Version_Major
+6	1	Firmware_Version_Minor
+7	1	ID
+8	1	Baud Rate (Bus)
+9	1	HW_REV
+10	1	Boot_Version_Major
+11	1	Boot_Version_Minor
+12	4	Error_Code
+20	4	Micros
+24	4	Millis
+100	1	Neopixel_Head_Left_Red
+101	1	Neopixel_Head_Left_Green
+102	1	Neopixel_Head_Left_Blue
+103	1	Neopixel_Head_Left_Brightness
+104	1	Neopixel_Head_Left_Mode
+105	2	Neopixel_Head_Left_Control_Time
+107	1	Neopixel_Head_Left_Led_Count
+108	2	Dummy_1
+110	1	Neopixel_Head_Right_Red
+111	1	Neopixel_Head_Right_Green
+112	1	Neopixel_Head_Right_Blue
+113	1	Neopixel_Head_Right_Brightness
+114	1	Neopixel_Head_Right_Mode
+115	2	Neopixel_Head_Right_Control_Time
+117	1	Neopixel_Head_Right_Led_Count

param/dxl_model/ffw_g40_imu.model

@@ -0,0 +1,24 @@
+[control table]
+Address	Size	Data Name
+0	2	Model Number
+2	1	ROBOT_Generation
+3	1	Board_Number
+5	1	Firmware_Version_Major
+6	1	Firmware_Version_Minor
+7	1	ID
+8	1	Baud Rate (Bus)
+9	1	HW_REV
+10	1	Boot_Version_Major
+11	1	Boot_Version_Minor
+12	4	Error_Code
+20	4	Micros
+24	4	Millis
+32	2	IMU ACC SCALE
+34	2	IMU GYRO SCALE
+36	4	IMU TIMESTAMP
+40	2	IMU ACC X
+42	2	IMU ACC Y
+44	2	IMU ACC Z
+46	2	IMU GYRO X
+48	2	IMU GYRO Y
+50	2	IMU GYRO Z

src/dynamixel/dynamixel.cpp

@@ -304,11 +304,7 @@ DxlError Dynamixel::SetDxlReadItems(
 
 DxlError Dynamixel::SetMultiDxlRead()
 {
-  if (read_data_list_.size() < 2) {
-    read_type_ = SYNC;
-  } else {
-    read_type_ = checkReadType();
-  }
+  read_type_ = checkReadType();
 
   fprintf(stderr, "Dynamixel Read Type : %s\n", read_type_ ? "bulk read" : "sync read");
   if (read_type_ == SYNC) {
@@ -411,13 +407,10 @@ DxlError Dynamixel::SetDxlWriteItems(
 
   return DxlError::OK;
 }
+
 DxlError Dynamixel::SetMultiDxlWrite()
 {
-  if (write_data_list_.size() < 2) {
-    write_type_ = SYNC;
-  } else {
-    write_type_ = checkWriteType();
-  }
+  write_type_ = checkWriteType();
 
   fprintf(stderr, "Dynamixel Write Type : %s\n", write_type_ ? "bulk write" : "sync write");
   if (write_type_ == SYNC) {
@@ -447,12 +440,10 @@ DxlError Dynamixel::SetMultiDxlWrite()
   }
 
   if (write_type_ == SYNC) {
-    SetSyncWriteItemAndHandler();
+    return SetSyncWriteItemAndHandler();
   } else {
-    SetBulkWriteItemAndHandler();
+    return SetBulkWriteItemAndHandler();
   }
-
-  return DxlError::OK;
 }
 
 DxlError Dynamixel::DynamixelEnable(std::vector<uint8_t> id_arr)
@@ -873,6 +864,12 @@ DxlError Dynamixel::WriteMultiDxlData()
 
 bool Dynamixel::checkReadType()
 {
+  if (read_data_list_.size() == 1) {
+    if (CheckIndirectReadAvailable(read_data_list_.at(0).comm_id) != DxlError::OK) {
+      return BULK;
+    }
+  }
+
   for (size_t dxl_index = 1; dxl_index < read_data_list_.size(); dxl_index++) {
     // Check if Indirect Data Read address and size are different
     uint16_t indirect_addr[2];  // [i-1], [i]
@@ -919,6 +916,12 @@ bool Dynamixel::checkReadType()
 
 bool Dynamixel::checkWriteType()
 {
+  if (write_data_list_.size() == 1) {
+    if (CheckIndirectWriteAvailable(write_data_list_.at(0).comm_id) != DxlError::OK) {
+      return BULK;
+    }
+  }
+
   for (size_t dxl_index = 1; dxl_index < write_data_list_.size(); dxl_index++) {
     // Check if Indirect Data Write address and size are different
     uint16_t indirect_addr[2];  // [i-1], [i]