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 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..21618c8e32 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -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(); @@ -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";