Throw if trying to configure a fixed joint

ros-controls · Apr 23, 2024 · 74e22b9 · 74e22b9
1 parent 9b21ba4
commit 74e22b9
Show file tree

Hide file tree

Showing 3 changed files with 31 additions and 0 deletions.
diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp
@@ -1003,6 +1003,13 @@ std::vector<HardwareInfo> parse_control_resources_from_urdf(const std::string &
                   << std::endl;
         continue;
       }
+      if (urdf_joint->type == urdf::Joint::FIXED)
+      {
+        throw std::runtime_error(
+          "Joint '" + joint.name +
+          "' is of type 'fixed'. "
+          "Fixed joints do not make sense in ros2_control.");
+      }
       joint_limits::JointLimits limits;
       if (getJointLimits(urdf_joint, limits))
       {

diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp
@@ -1053,6 +1053,15 @@ TEST_F(TestComponentParser, throw_on_parse_invalid_urdf_system_missing_limits)
   EXPECT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error);
 }
 
+TEST_F(TestComponentParser, throw_on_parse_urdf_system_with_command_fixed_joint)
+{
+  std::string urdf_to_test =
+    std::string(ros2_control_test_assets::urdf_head) +
+    ros2_control_test_assets::invalid_urdf_ros2_control_system_with_command_fixed_joint +
+    ros2_control_test_assets::urdf_tail;
+  EXPECT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error);
+}
+
 TEST_F(TestComponentParser, successfully_parse_urdf_system_continuous_missing_limits)
 {
   std::string urdf_to_test =

diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp
@@ -727,6 +727,21 @@ const auto invalid_urdf2_transmission_given_too_many_joints =
   </ros2_control>
 )";
 
+const auto invalid_urdf_ros2_control_system_with_command_fixed_joint =
+  R"(
+  <ros2_control name="2DOF_System_Robot_Position_Only" type="system">
+    <hardware>
+      <plugin>ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only</plugin>
+      <param name="example_param_write_for_sec">2</param>
+      <param name="example_param_read_for_sec">2</param>
+    </hardware>
+    <joint name="tool_joint">
+      <command_interface name="position">
+      </command_interface>
+    </joint>
+  </ros2_control>
+)";
+
 }  // namespace ros2_control_test_assets
 
 #endif  // ROS2_CONTROL_TEST_ASSETS__COMPONENTS_URDFS_HPP_