Support rising tag

Signed-off-by: Tatsuro Sakaguchi <[email protected]>

Author: Tacha-S

src/dynamixel_hardware_interface.cpp

@@ -16,6 +16,8 @@
 
 #include "dynamixel_hardware_interface/dynamixel_hardware_interface.hpp"
 
+#include <tinyxml2.h>
+
 #include <chrono>
 #include <cmath>
 #include <limits>
@@ -215,6 +217,43 @@ hardware_interface::CallbackReturn DynamixelHardware::on_init(
     }
   }
 
+  auto urdf = info_.original_xml;
+  tinyxml2::XMLDocument doc;
+  if (doc.Parse(urdf.c_str()) != tinyxml2::XML_SUCCESS) {
+    RCLCPP_ERROR(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_.gpios.end()) {
+          auto params = info_.gpios[std::distance(info_.joints.begin(), itr)].parameters;
+          if (std::find_if(params.begin(), params.end(),
+              [](const std::pair<std::string, std::string> & p) {
+                return p.first == "Homing Offset";
+              }) == params.end())
+          {
+            info_.gpios[std::distance(info_.joints.begin(), itr)].parameters.emplace(
+              "Homing Offset",
+              std::to_string(std::round(rising * (180.0 / M_PI) / (360.0 / 4095))));
+          }
+        }
+      }
+    }
+    joint_element = joint_element->NextSiblingElement("joint");
+  }
+
   torque_enabled_comm_id_id_.clear();
   for (const hardware_interface::ComponentInfo & gpio : info_.gpios) {
     uint8_t id = static_cast<uint8_t>(stoi(gpio.parameters.at("ID")));