From 99c86b26c9b34f9be14577d358ea9710daf70bfb Mon Sep 17 00:00:00 2001 From: Caio Freitas Date: Tue, 18 Nov 2025 19:34:14 +0100 Subject: [PATCH 1/8] 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/8] 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/8] 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/8] 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()) { From 954d64b4f227e79ffc09a222ca643dac857d373a Mon Sep 17 00:00:00 2001 From: Caio Freitas Date: Wed, 26 Nov 2025 19:37:04 +0100 Subject: [PATCH 5/8] Remove occurences of robot_description_semantic --- admittance_controller/test/test_admittance_controller.hpp | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 21618c8e32..38cbceb14a 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -80,11 +80,6 @@ class TestableAdmittanceController : public admittance_controller::AdmittanceCon { get_node()->declare_parameter("robot_description", rclcpp::ParameterType::PARAMETER_STRING); get_node()->set_parameter({"robot_description", robot_description_}); - } - - 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(); } @@ -111,8 +106,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 From 9f684b1d55b020fb8cb45abc5b16a607a8e92abc Mon Sep 17 00:00:00 2001 From: Caio Freitas Date: Wed, 26 Nov 2025 19:38:00 +0100 Subject: [PATCH 6/8] Override robot_description_ with value from test case before initalization --- .../test/test_admittance_controller.hpp | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 38cbceb14a..1a680bf706 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -76,10 +76,8 @@ class TestableAdmittanceController : public admittance_controller::AdmittanceCon public: CallbackReturn on_init() override { - 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_}); + get_node()->declare_parameter("robot_description", rclcpp::ParameterType::PARAMETER_STRING); + get_node()->set_parameter({"robot_description", robot_description_}); return admittance_controller::AdmittanceController::on_init(); } @@ -164,6 +162,15 @@ class AdmittanceControllerTest : public ::testing::Test { controller_interface::ControllerInterfaceParams params; params.controller_name = controller_name; + // Extract robot_description from parameter overrides + for (const auto& param : options.parameter_overrides()) + { + if (param.get_name() == "robot_description") + { + controller_->robot_description_ = param.as_string(); + break; + } + } params.robot_description = controller_->robot_description_; params.update_rate = 0; params.node_namespace = ""; From d968b83b4ff71de022f31adda3120a24c7d9453e Mon Sep 17 00:00:00 2001 From: Caio Freitas Date: Wed, 26 Nov 2025 22:23:13 +0100 Subject: [PATCH 7/8] Replace for with find_if to look for robot_description override --- .../test/test_admittance_controller.hpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 1a680bf706..02b0568ac7 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -163,13 +163,15 @@ class AdmittanceControllerTest : public ::testing::Test controller_interface::ControllerInterfaceParams params; params.controller_name = controller_name; // Extract robot_description from parameter overrides - for (const auto& param : options.parameter_overrides()) - { - if (param.get_name() == "robot_description") + auto it = std::find_if( + options.parameter_overrides().begin(), options.parameter_overrides().end(), + [](const rclcpp::Parameter & p) { - controller_->robot_description_ = param.as_string(); - break; - } + 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; From 8c08f8c1953d7b09a7a1af41f6ae850f9f2c72ff Mon Sep 17 00:00:00 2001 From: Caio Freitas Date: Mon, 1 Dec 2025 21:39:44 +0100 Subject: [PATCH 8/8] Fix failing pre-commit tests --- admittance_controller/test/test_admittance_controller.hpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 02b0568ac7..19b471738a 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -165,12 +165,10 @@ class AdmittanceControllerTest : public ::testing::Test // 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"; - }); + [](const rclcpp::Parameter & p) { return p.get_name() == "robot_description"; }); - if (it != options.parameter_overrides().end()) { + if (it != options.parameter_overrides().end()) + { controller_->robot_description_ = it->as_string(); } params.robot_description = controller_->robot_description_;