From 99c86b26c9b34f9be14577d358ea9710daf70bfb Mon Sep 17 00:00:00 2001 From: Caio Freitas Date: Tue, 18 Nov 2025 19:34:14 +0100 Subject: [PATCH 1/4] Check robot description validity on AdmittanceController and test for it --- .../src/admittance_controller.cpp | 28 +++++++++++++++++++ .../test/test_admittance_controller.cpp | 8 ++---- .../test/test_admittance_controller.hpp | 16 +++++++---- 3 files changed, 42 insertions(+), 10 deletions(-) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index f0db9f96f5..1f1a52ccda 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 = get_node()->get_parameter("robot_description").as_string(); + + 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..692ab333bd 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -76,11 +76,17 @@ class TestableAdmittanceController : public admittance_controller::AdmittanceCon public: 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_}); + if (!get_node()->has_parameter("robot_description")) + { + get_node()->declare_parameter("robot_description", rclcpp::ParameterType::PARAMETER_STRING); + get_node()->set_parameter({"robot_description", robot_description_}); + } + if (!get_node()->has_parameter("robot_description_semantic")) + { + get_node()->declare_parameter( + "robot_description_semantic", rclcpp::ParameterType::PARAMETER_STRING); + get_node()->set_parameter({"robot_description_semantic", robot_description_semantic_}); + } return admittance_controller::AdmittanceController::on_init(); } From 5143698886db0765148b5980ad1dd832fbd61fbf Mon Sep 17 00:00:00 2001 From: Caio Freitas Date: Thu, 20 Nov 2025 20:10:03 +0100 Subject: [PATCH 2/4] Remove commented lines and unnecessary check for robot_description_semantic --- .../test/test_admittance_controller.hpp | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 692ab333bd..21618c8e32 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -81,12 +81,10 @@ class TestableAdmittanceController : public admittance_controller::AdmittanceCon get_node()->declare_parameter("robot_description", rclcpp::ParameterType::PARAMETER_STRING); get_node()->set_parameter({"robot_description", robot_description_}); } - if (!get_node()->has_parameter("robot_description_semantic")) - { - get_node()->declare_parameter( - "robot_description_semantic", rclcpp::ParameterType::PARAMETER_STRING); - get_node()->set_parameter({"robot_description_semantic", robot_description_semantic_}); - } + + get_node()->declare_parameter( + "robot_description_semantic", rclcpp::ParameterType::PARAMETER_STRING); + get_node()->set_parameter({"robot_description_semantic", robot_description_semantic_}); return admittance_controller::AdmittanceController::on_init(); } @@ -390,8 +388,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"; From a07b6669d2d35fb6a6e62df0fbaaa3affb0cefa9 Mon Sep 17 00:00:00 2001 From: Caio Freitas Date: Sun, 23 Nov 2025 20:23:46 +0100 Subject: [PATCH 3/4] Add tynxml dependency to package.xml and CMakeLists.txt --- admittance_controller/CMakeLists.txt | 1 + admittance_controller/package.xml | 1 + 2 files changed, 2 insertions(+) diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index 6cd1ba6385..222f3b3dda 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 ) diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 9fcd2e56a7..1d8a7e5819 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 From 8cceebccae71f9d4edf433bc217ef2463393c3f1 Mon Sep 17 00:00:00 2001 From: Caio Freitas Date: Sun, 23 Nov 2025 20:24:14 +0100 Subject: [PATCH 4/4] Use get_robot_description on AdmittanceController::on_init --- admittance_controller/src/admittance_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 1f1a52ccda..061f64b8db 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -93,7 +93,7 @@ controller_interface::CallbackReturn AdmittanceController::on_init() reference_admittance_ = last_reference_; joint_state_ = last_reference_; - std::string robot_description = get_node()->get_parameter("robot_description").as_string(); + std::string robot_description = this->get_robot_description(); if (robot_description.empty()) {