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 2 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
2 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
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
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>
Copy link
Member

@christophfroehlich christophfroehlich Nov 18, 2025

Choose a reason for hiding this comment

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

we'll need to add this as dependency (do we need to link against in the Cmake file?)
https://github.com/ros/urdfdom/blob/973baf479512b0b34951b3d5ebb616c22990d781/package.xml#L20

#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 = get_node()->get_parameter("robot_description").as_string();
Copy link
Member

@christophfroehlich christophfroehlich Nov 18, 2025

Choose a reason for hiding this comment

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

I'm not sure about this. The admittance controller doesn't use the robot_description parameter, but uses this->get_robot_description() from controller_interface_base since #1247. We should use this here as well obviously.
For the tests, you have to set controller_->robot_description_ to override this.

Copy link
Member

@bmagyar bmagyar Nov 20, 2025

Choose a reason for hiding this comment

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

worst case we may have some work left to refactor on this controller to use the URDF we pass to the controller, we shouldn't fetch a param: https://github.com/ros-controls/ros2_control/pull/1088/files

Copy link
Member

@christophfroehlich christophfroehlich Nov 20, 2025

Choose a reason for hiding this comment

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

as i've seen we don't parse it as a parameter any more, that's only a remnant in the tests


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
10 changes: 6 additions & 4 deletions admittance_controller/test/test_admittance_controller.hpp
View file
Open in desktop
Copy link
Member

@christophfroehlich christophfroehlich Nov 18, 2025

Choose a reason for hiding this comment

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

L393-394 has commented code regarding robot_description, this should also be deleted.

Original file line number Diff line number Diff line change
Expand Up @@ -76,10 +76,14 @@ class TestableAdmittanceController : public admittance_controller::AdmittanceCon
public:
CallbackReturn on_init() override
{
get_node()->declare_parameter("robot_description", rclcpp::ParameterType::PARAMETER_STRING);
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_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 Down Expand Up @@ -384,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";
Expand Down