Check robot description validity on AdmittanceController and test for it

admittance_controller/CMakeLists.txt

@@ -23,6 +23,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   tf2_geometry_msgs
   tf2_kdl
   tf2_ros
+  tinyxml2
   trajectory_msgs
 )
 

admittance_controller/package.xml

@@ -42,6 +42,7 @@
   <depend>tf2_kdl</depend>
   <depend>tf2_ros</depend>
   <depend>tf2</depend>
+  <depend>tinyxml2</depend>
   <depend>trajectory_msgs</depend>
 
   <test_depend>ament_cmake_gmock</test_depend>

admittance_controller/src/admittance_controller.cpp

@@ -16,6 +16,7 @@
 
 #include "admittance_controller/admittance_controller.hpp"
 
+#include <tinyxml2.h>
 #include <cmath>
 #include <memory>
 #include <string>
@@ -92,6 +93,33 @@ controller_interface::CallbackReturn AdmittanceController::on_init()
   reference_admittance_ = last_reference_;
   joint_state_ = last_reference_;
 
+  std::string robot_description = this->get_robot_description();
+
+  if (robot_description.empty())
+  {
+    RCLCPP_ERROR(get_node()->get_logger(), "'robot_description' parameter is empty.");
+    return controller_interface::CallbackReturn::ERROR;
+  }
+
+  tinyxml2::XMLDocument doc;
+  if (!doc.Parse(robot_description.c_str()) && doc.Error())
+  {
+    RCLCPP_ERROR(
+      get_node()->get_logger(),
+      "Failed to parse robot description XML from parameter "
+      "'robot_description': %s",
+      doc.ErrorStr());
+    return controller_interface::CallbackReturn::ERROR;
+  }
+  if (doc.Error())
+  {
+    RCLCPP_ERROR(
+      get_node()->get_logger(),
+      "Error parsing robot description XML from parameter "
+      "'robot_description': %s",
+      doc.ErrorStr());
+    return controller_interface::CallbackReturn::ERROR;
+  }
   return controller_interface::CallbackReturn::SUCCESS;
 }
 

admittance_controller/test/test_admittance_controller.cpp

@@ -62,12 +62,10 @@ INSTANTIATE_TEST_SUITE_P(
     // wrong length selected axes
     std::make_tuple(
       std::string("admittance.selected_axes"),
-      rclcpp::ParameterValue(std::vector<double>() = {1, 2, 3}))
+      rclcpp::ParameterValue(std::vector<double>() = {1, 2, 3})),
     // invalid robot description.
-    // TODO(anyone): deactivated, because SetUpController returns SUCCESS here?
-    // ,std::make_tuple(
-    //   std::string("robot_description"), rclcpp::ParameterValue(std::string() = "bad_robot")))
-    ));
+    std::make_tuple(
+      std::string("robot_description"), rclcpp::ParameterValue(std::string() = "bad_robot"))));
 
 // Test on_init returns ERROR when a parameter is invalid
 TEST_P(AdmittanceControllerTestParameterizedInvalidParameters, invalid_parameters)

admittance_controller/test/test_admittance_controller.hpp

@@ -77,10 +77,7 @@ class TestableAdmittanceController : public admittance_controller::AdmittanceCon
   CallbackReturn on_init() override
   {
     get_node()->declare_parameter("robot_description", rclcpp::ParameterType::PARAMETER_STRING);
-    get_node()->declare_parameter(
-      "robot_description_semantic", rclcpp::ParameterType::PARAMETER_STRING);
     get_node()->set_parameter({"robot_description", robot_description_});
-    get_node()->set_parameter({"robot_description_semantic", robot_description_semantic_});
 
     return admittance_controller::AdmittanceController::on_init();
   }
@@ -107,8 +104,7 @@ class TestableAdmittanceController : public admittance_controller::AdmittanceCon
     }
   }
 
-  const std::string robot_description_ = ros2_control_test_assets::valid_6d_robot_urdf;
-  const std::string robot_description_semantic_ = ros2_control_test_assets::valid_6d_robot_srdf;
+  std::string robot_description_ = ros2_control_test_assets::valid_6d_robot_urdf;
 };
 
 class AdmittanceControllerTest : public ::testing::Test
@@ -166,6 +162,17 @@ class AdmittanceControllerTest : public ::testing::Test
   {
     controller_interface::ControllerInterfaceParams params;
     params.controller_name = controller_name;
+    // Extract robot_description from parameter overrides
+    auto it = std::find_if(
+      options.parameter_overrides().begin(), options.parameter_overrides().end(),
+      [](const rclcpp::Parameter & p)
+      {
+        return p.get_name() == "robot_description";
+      });
+
+    if (it != options.parameter_overrides().end()) {
+      controller_->robot_description_ = it->as_string();
+    }
     params.robot_description = controller_->robot_description_;
     params.update_rate = 0;
     params.node_namespace = "";
@@ -384,8 +391,6 @@ class AdmittanceControllerTest : public ::testing::Test
   const std::string ik_base_frame_ = "base_link";
   const std::string ik_tip_frame_ = "tool0";
   const std::string ik_group_name_ = "arm";
-  //  const std::string robot_description_ = ros2_control_test_assets::valid_6d_robot_urdf;
-  //  const std::string robot_description_semantic_ = ros2_control_test_assets::valid_6d_robot_srdf;
 
   const std::string control_frame_ = "tool0";
   const std::string endeffector_frame_ = "endeffector_frame";