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";