From 74e22b9bc8e7865bfa49b4d3b5051fd8f1d3566f Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Tue, 23 Apr 2024 17:24:45 +0000
Subject: [PATCH] Throw if trying to configure a fixed joint

---
 hardware_interface/src/component_parser.cpp       |  7 +++++++
 hardware_interface/test/test_component_parser.cpp |  9 +++++++++
 .../ros2_control_test_assets/components_urdfs.hpp | 15 +++++++++++++++
 3 files changed, 31 insertions(+)

diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp
index 1098eb1217..f1245e8fc6 100644
--- 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
index 68752e92d5..407a8242c4 100644
--- 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
index 762f2dd98a..7b46eda9c0 100644
--- 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_