Merge branch 'master' into feature/swerve-drive-controller

Author: nitin2606

admittance_controller/CMakeLists.txt

@@ -23,6 +23,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   tf2_geometry_msgs
   tf2_kdl
   tf2_ros
+  TinyXML2
   trajectory_msgs
 )
 
@@ -65,7 +66,8 @@ target_link_libraries(admittance_controller PUBLIC
                       ${geometry_msgs_TARGETS}
                       ${trajectory_msgs_TARGETS}
                       ${control_msgs_TARGETS}
-                      ${tf2_geometry_msgs_TARGETS})
+                      ${tf2_geometry_msgs_TARGETS}
+                      ${TinyXML2_LIBRARIES})
 
 pluginlib_export_plugin_description_file(controller_interface admittance_controller.xml)
 

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,15 @@ 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 +389,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";

diff_drive_controller/src/diff_drive_controller_parameter.yaml

@@ -26,15 +26,15 @@ diff_drive_controller:
   wheel_radius: {
     type: double,
     default_value: 0.0,
-    description: "Radius of a wheel, i.e., wheels size, used for transformation of linear velocity into wheel rotations. If this parameter is wrong the robot will move faster or slower then expected.",
+    description: "Radius of a wheel, i.e., wheels size, used for transformation of linear velocity into wheel rotations. If this parameter is wrong the robot will move faster or slower than expected.",
     validation: {
       gt<>: [0.0]
     }
   }
   wheel_separation_multiplier: {
     type: double,
     default_value: 1.0,
-    description: "Correction factor for wheel separation (TODO(destogl): Please help me describe this correctly)",
+    description: "Correction factor when the actual wheel separation differs from the nominal value in the ``wheel_separation`` parameter.",
   }
   left_wheel_radius_multiplier: {
     type: double,

joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp

@@ -67,18 +67,6 @@ enum class InterpolationMethod
  */
 const InterpolationMethod DEFAULT_INTERPOLATION = InterpolationMethod::VARIABLE_DEGREE_SPLINE;
 
-/**
- * @brief Maps string representations to their corresponding InterpolationMethod enum values.
- *
- * @deprecated This map is deprecated. Use the direct lookup methods instead.
- * (Original use: Converting strings into the InterpolationMethod).
- */
-[[deprecated(
-  "InterpolationMethodMap will be removed in future releases. "
-  "Instead, use the direct lookup methods.")]]
-const std::unordered_map<std::string, InterpolationMethod> InterpolationMethodMap(
-  {{"none", InterpolationMethod::NONE}, {"splines", InterpolationMethod::VARIABLE_DEGREE_SPLINE}});
-
 /**
  * \brief Returns corresponding string value for the `InterpolationMethod`.
  * This function uses simple switch-case lookup to directly assign string value to