diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt
index 6cd1ba6385..89a504d8e4 100644
--- a/admittance_controller/CMakeLists.txt
+++ b/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)
diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml
index 211d1ffda8..ce2b073eb6 100644
--- a/admittance_controller/package.xml
+++ b/admittance_controller/package.xml
@@ -42,6 +42,7 @@
tf2_kdl
tf2_ros
tf2
+ tinyxml2
trajectory_msgs
ament_cmake_gmock
diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp
index f0db9f96f5..061f64b8db 100644
--- a/admittance_controller/src/admittance_controller.cpp
+++ b/admittance_controller/src/admittance_controller.cpp
@@ -16,6 +16,7 @@
#include "admittance_controller/admittance_controller.hpp"
+#include
#include
#include
#include
@@ -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;
}
diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp
index 711f4edfd6..ba89d088e3 100644
--- a/admittance_controller/test/test_admittance_controller.cpp
+++ b/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() = {1, 2, 3}))
+ rclcpp::ParameterValue(std::vector() = {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)
diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp
index 7c955e0995..19b471738a 100644
--- a/admittance_controller/test/test_admittance_controller.hpp
+++ b/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";