Load joint type from URDF

sbgisen · Oct 9, 2024 · ecd15c4 · ecd15c4
1 parent ac58184
commit ecd15c4
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 3 deletions.
diff --git a/vesc_hw_interface/launch/position_test.ros2_control.xacro b/vesc_hw_interface/launch/position_test.ros2_control.xacro
@@ -31,7 +31,6 @@
       <param name="servo/endstop_threshold">0.8</param>
       <param name="servo/endstop_margin">0.02</param>
       <param name="command_mode">position</param>
-      <param name="joint_type">prismatic</param>
     </hardware>
     <joint name="vesc_joint">
       <command_interface name="position">

diff --git a/vesc_hw_interface/src/vesc_hw_interface.cpp b/vesc_hw_interface/src/vesc_hw_interface.cpp
@@ -89,8 +89,29 @@ CallbackReturn VescHwInterface::on_init(const hardware_interface::HardwareInfo&
   command_mode_ = info_.hardware_parameters["command_mode"];
   RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "mode: %s", command_mode_.data());
 
-  // check joint type
-  joint_type_ = info_.hardware_parameters["joint_type"];
+  // parse URDF for joint type
+  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 tinyxml2::XMLElement * joint_it = doc.RootElement()->FirstChildElement("joint");
+    while (joint_it) {
+      const tinyxml2::XMLAttribute * name_attr = joint_it->FindAttribute("name");
+      const tinyxml2::XMLAttribute * type_attr = joint_it->FindAttribute("type");
+      if (name_attr && type_attr) {
+        std::string name = joint_it->Attribute("name");
+        std::string type = joint_it->Attribute("type");
+        if (name == joint_name_) {
+          joint_type_ = type;
+          break;
+        }
+      }
+      joint_it = joint_it->NextSiblingElement("joint");
+    }
+  }
   RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "joint type: %s", joint_type_.data());
   if ((joint_type_ != "revolute") && (joint_type_ != "continuous") && (joint_type_ != "prismatic"))
   {