Skip to content

Check robot description validity on AdmittanceController and test for it #2009

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 8 commits into
base: master
Choose a base branch
from
Open

Check robot description validity on AdmittanceController and test for it #2009

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
99c86b2
Check robot description validity on AdmittanceController and test for it
Nov 18, 2025
5143698
Remove commented lines and unnecessary check for robot_description_se…
Nov 20, 2025
a07b666
Add tynxml dependency to package.xml and CMakeLists.txt
Nov 23, 2025
8cceebc
Use get_robot_description on AdmittanceController::on_init
Nov 23, 2025
954d64b
Remove occurences of robot_description_semantic
Nov 26, 2025
9f684b1
Override robot_description_ with value from test case before initaliz…
Nov 26, 2025
d968b83
Replace for with find_if to look for robot_description override
Nov 26, 2025
8c08f8c
Fix failing pre-commit tests
Dec 1, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions admittance_controller/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
tf2_geometry_msgs
tf2_kdl
tf2_ros
tinyxml2
Copy link
Member

@christophfroehlich christophfroehlich Dec 2, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more. 

   --- stderr: admittance_controller
  CMake Error at CMakeLists.txt:33 (find_package):
    By not providing "Findtinyxml2.cmake" in CMAKE_MODULE_PATH this project has
    asked CMake to find a package configuration file provided by "tinyxml2",
    but CMake did not find one.
  
    Could not find a package configuration file provided by "tinyxml2" with any
    of the following names:
  
      tinyxml2Config.cmake
      tinyxml2-config.cmake
  
    Add the installation prefix of "tinyxml2" to CMAKE_PREFIX_PATH or set
    "tinyxml2_DIR" to a directory containing one of the above files.  If
    "tinyxml2" provides a separate development package or SDK, be sure it has
    been installed.
  
  
  ---

But it works here
https://github.com/ros-controls/ros2_control/blob/master/hardware_interface/CMakeLists.txt#L16
with the capitalized version

Suggested change
tinyxml2
TinyXML2

Copy link
Member

@christophfroehlich christophfroehlich Dec 2, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

And please add to target_link_libraries below:

                  ${TinyXML2_LIBRARIES}

trajectory_msgs
)

Expand Down
1 change: 1 addition & 0 deletions admittance_controller/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down
28 changes: 28 additions & 0 deletions admittance_controller/src/admittance_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@

#include "admittance_controller/admittance_controller.hpp"

#include <tinyxml2.h>
#include <cmath>
#include <memory>
#include <string>
Expand Down Expand Up @@ -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;
}

Expand Down
8 changes: 3 additions & 5 deletions admittance_controller/test/test_admittance_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
17 changes: 10 additions & 7 deletions admittance_controller/test/test_admittance_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
}
Expand All @@ -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
Expand Down Expand Up @@ -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 = "";
Expand Down Expand Up @@ -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";
Expand Down