From ac03732099b83e58ff5244c04c61a69c36cded3b Mon Sep 17 00:00:00 2001
From: Tatsuro Sakaguchi <tatsuro.sakaguchi@g.softbank.co.jp>
Date: Fri, 6 Dec 2024 10:13:28 +0900
Subject: [PATCH 1/2] Support rising tag

---
 .../dynamixel_hardware/dynamixel_hardware.hpp |  1 +
 dynamixel_hardware/src/dynamixel_hardware.cpp | 38 ++++++++++++++++++-
 2 files changed, 37 insertions(+), 2 deletions(-)

diff --git a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp
index d3d5746..744341e 100644
--- a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp
+++ b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp
@@ -45,6 +45,7 @@ struct Joint
   JointValue state{};
   JointValue command{};
   JointValue prev_command{};
+  double rising_offset{0.0};
 };
 
 enum class ControlMode
diff --git a/dynamixel_hardware/src/dynamixel_hardware.cpp b/dynamixel_hardware/src/dynamixel_hardware.cpp
index 720db9e..9b20afb 100644
--- a/dynamixel_hardware/src/dynamixel_hardware.cpp
+++ b/dynamixel_hardware/src/dynamixel_hardware.cpp
@@ -14,6 +14,8 @@
 
 #include "dynamixel_hardware/dynamixel_hardware.hpp"
 
+#include <tinyxml2.h>
+
 #include <algorithm>
 #include <array>
 #include <limits>
@@ -72,6 +74,37 @@ CallbackReturn DynamixelHardware::on_init(const hardware_interface::HardwareInfo
     RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "joint_id %d: %d", i, joint_ids_[i]);
   }
 
+  // parse URDF for calibration parameters
+  auto urdf = info_.original_xml;
+  if (!urdf.empty()) {
+    tinyxml2::XMLDocument doc;
+    if (doc.Parse(urdf.c_str()) != tinyxml2::XML_SUCCESS) {
+      RCLCPP_ERROR_STREAM(get_logger(), "Failed to parse URDF XML");
+      return hardware_interface::CallbackReturn::ERROR;
+    }
+    const auto * joint_element = doc.RootElement()->FirstChildElement("joint");
+    while (joint_element != nullptr) {
+      const auto * name_attr = joint_element->FindAttribute("name");
+      const auto * calibration_element = joint_element->FirstChildElement("calibration");
+      if (calibration_element != nullptr) {
+        const auto * rising_attr = calibration_element->FindAttribute("rising");
+        if ((rising_attr != nullptr) && (name_attr != nullptr)) {
+          auto rising = std::atof(calibration_element->Attribute("rising"));
+          std::string name = joint_element->Attribute("name");
+          auto itr = std::find_if(
+            info_.joints.begin(), info_.joints.end(),
+            [&name](const hardware_interface::ComponentInfo & joint) {
+              return joint.name == name;
+            });
+          if (itr != info_.joints.end()) {
+            joints_[std::distance(info_.joints.begin(), itr)].rising_offset = rising;
+          }
+        }
+      }
+      joint_element = joint_element->NextSiblingElement("joint");
+    }
+  }
+
   if (
     info_.hardware_parameters.find("use_dummy") != info_.hardware_parameters.end() &&
     info_.hardware_parameters.at("use_dummy") == "true")
@@ -286,7 +319,8 @@ return_type DynamixelHardware::read(
   }
 
   for (uint i = 0; i < ids.size(); i++) {
-    joints_[i].state.position = dynamixel_workbench_.convertValue2Radian(ids[i], positions[i]);
+    joints_[i].state.position = dynamixel_workbench_.convertValue2Radian(ids[i], positions[i]) +
+      joints_[i].rising_offset;
     joints_[i].state.velocity = dynamixel_workbench_.convertValue2Velocity(ids[i], velocities[i]);
     joints_[i].state.effort = dynamixel_workbench_.convertValue2Current(currents[i]);
   }
@@ -473,7 +507,7 @@ CallbackReturn DynamixelHardware::set_joint_positions()
   for (uint i = 0; i < ids.size(); i++) {
     joints_[i].prev_command.position = joints_[i].command.position;
     commands[i] = dynamixel_workbench_.convertRadian2Value(
-      ids[i], static_cast<float>(joints_[i].command.position));
+      ids[i], static_cast<float>(joints_[i].command.position - joints_[i].rising_offset));
   }
   if (!dynamixel_workbench_.syncWrite(
       kGoalPositionIndex, ids.data(), ids.size(), commands.data(), 1, &log))

From 2a8c771fa31610399a43bf50c7f596bd9cad526e Mon Sep 17 00:00:00 2001
From: Tatsuro Sakaguchi <tatsuro.sakaguchi@g.softbank.co.jp>
Date: Tue, 10 Dec 2024 16:41:54 +0900
Subject: [PATCH 2/2] Add depend

---
 dynamixel_hardware/CMakeLists.txt | 1 +
 dynamixel_hardware/package.xml    | 1 +
 2 files changed, 2 insertions(+)

diff --git a/dynamixel_hardware/CMakeLists.txt b/dynamixel_hardware/CMakeLists.txt
index d97ca69..c9248ec 100644
--- a/dynamixel_hardware/CMakeLists.txt
+++ b/dynamixel_hardware/CMakeLists.txt
@@ -23,6 +23,7 @@ find_package(lifecycle_msgs REQUIRED)
 find_package(hardware_interface REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(dynamixel_workbench_toolbox REQUIRED)
+find_package(tinyxml2_vendor REQUIRED)
 
 add_library(
   ${PROJECT_NAME}
diff --git a/dynamixel_hardware/package.xml b/dynamixel_hardware/package.xml
index cab2424..3e2b712 100644
--- a/dynamixel_hardware/package.xml
+++ b/dynamixel_hardware/package.xml
@@ -14,6 +14,7 @@
   <depend>lifecycle_msgs</depend>
   <depend>hardware_interface</depend>
   <depend>pluginlib</depend>
+  <depend>tinyxml2_vendor</depend>
   <depend>dynamixel_workbench_toolbox</depend>
 
   <test_depend>ament_lint_auto</test_depend>