diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index 6cd1ba6385..89a504d8e4 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 ) @@ -65,7 +66,8 @@ target_link_libraries(admittance_controller PUBLIC ${geometry_msgs_TARGETS} ${trajectory_msgs_TARGETS} ${control_msgs_TARGETS} - ${tf2_geometry_msgs_TARGETS}) + ${tf2_geometry_msgs_TARGETS} + ${TinyXML2_LIBRARIES}) pluginlib_export_plugin_description_file(controller_interface admittance_controller.xml) diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 211d1ffda8..ce2b073eb6 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 diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index f0db9f96f5..061f64b8db 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 = 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; } 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..19b471738a 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -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(); } @@ -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 @@ -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 = ""; @@ -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";