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 7 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 1 commit
Commits
Show all changes
7 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
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>
#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();

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
16 changes: 11 additions & 5 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 @@ -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();
}
Expand Down
Loading