From 609b86763822e3c1276643de0b1f4f2aeb90ce90 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 21 Jul 2024 23:28:34 +0200 Subject: [PATCH] [JTC] Refactor URDF Model parsing (#1227) --- joint_trajectory_controller/CMakeLists.txt | 4 ++ .../joint_trajectory_controller.hpp | 3 - .../src/joint_trajectory_controller.cpp | 63 +++++++++---------- .../test/test_trajectory_controller.cpp | 7 +++ .../test/test_trajectory_controller_utils.hpp | 8 ++- 5 files changed, 47 insertions(+), 38 deletions(-) diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 46c245be9c..438661f7bf 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -59,10 +59,12 @@ if(BUILD_TESTING) ament_add_gmock(test_trajectory test/test_trajectory.cpp) target_link_libraries(test_trajectory joint_trajectory_controller) + ament_target_dependencies(test_trajectory ros2_control_test_assets) target_compile_definitions(test_trajectory PRIVATE _USE_MATH_DEFINES) ament_add_gmock(test_tolerances test/test_tolerances.cpp) target_link_libraries(test_tolerances joint_trajectory_controller) + ament_target_dependencies(test_tolerances ros2_control_test_assets) target_compile_definitions(test_tolerances PRIVATE _USE_MATH_DEFINES) ament_add_gmock(test_trajectory_controller @@ -71,6 +73,7 @@ if(BUILD_TESTING) target_link_libraries(test_trajectory_controller joint_trajectory_controller ) + ament_target_dependencies(test_trajectory_controller ros2_control_test_assets) target_compile_definitions(joint_trajectory_controller PRIVATE _USE_MATH_DEFINES) ament_add_gmock(test_load_joint_trajectory_controller @@ -93,6 +96,7 @@ if(BUILD_TESTING) target_link_libraries(test_trajectory_actions joint_trajectory_controller ) + ament_target_dependencies(test_trajectory_actions ros2_control_test_assets) endif() diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index e22dee8946..2f0a1f4df0 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -41,7 +41,6 @@ #include "realtime_tools/realtime_server_goal_handle.h" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" -#include "urdf/model.h" // auto-generated by generate_parameter_library #include "joint_trajectory_controller_parameters.hpp" @@ -300,8 +299,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa void resize_joint_trajectory_point_command( trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); - urdf::Model model_; - /** * @brief Assigns the values from a trajectory point interface to a joint interface. * diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index faa2efce9b..9563568ad5 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -43,15 +43,44 @@ JointTrajectoryController::JointTrajectoryController() controller_interface::CallbackReturn JointTrajectoryController::on_init() { + try + { + // Create the parameter listener and get the parameters + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } + const std::string & urdf = get_robot_description(); if (!urdf.empty()) { - if (!model_.initString(urdf)) + urdf::Model model; + if (!model.initString(urdf)) { - RCLCPP_ERROR(get_node()->get_logger(), "Failed to parse URDF file"); + RCLCPP_ERROR(get_node()->get_logger(), "Failed to parse robot description!"); + return CallbackReturn::ERROR; } else { + /// initialize the URDF model and update the joint angles wraparound vector + // Configure joint position error normalization (angle_wraparound) + joints_angle_wraparound_.resize(params_.joints.size(), false); + for (size_t i = 0; i < params_.joints.size(); ++i) + { + auto urdf_joint = model.getJoint(params_.joints[i]); + if (urdf_joint && urdf_joint->type == urdf::Joint::CONTINUOUS) + { + RCLCPP_DEBUG( + get_node()->get_logger(), "joint '%s' is of type continuous, use angle_wraparound.", + params_.joints[i].c_str()); + joints_angle_wraparound_[i] = true; + } + // do nothing if joint is not found in the URDF + } RCLCPP_DEBUG(get_node()->get_logger(), "Successfully parsed URDF file"); } } @@ -61,18 +90,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init() RCLCPP_DEBUG(get_node()->get_logger(), "No URDF file given"); } - try - { - // Create the parameter listener and get the parameters - param_listener_ = std::make_shared(get_node()); - params_ = param_listener_->get_params(); - } - catch (const std::exception & e) - { - fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return CallbackReturn::ERROR; - } - return CallbackReturn::SUCCESS; } @@ -698,24 +715,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( update_pids(); } - // Configure joint position error normalization (angle_wraparound) - joints_angle_wraparound_.resize(dof_); - for (size_t i = 0; i < dof_; ++i) - { - if (!get_robot_description().empty()) - { - auto urdf_joint = model_.getJoint(params_.joints[i]); - if (urdf_joint && urdf_joint->type == urdf::Joint::CONTINUOUS) - { - RCLCPP_DEBUG( - logger, "joint '%s' is of type continuous, use angle_wraparound.", - params_.joints[i].c_str()); - joints_angle_wraparound_[i] = true; - } - // do nothing if joint is not found in the URDF - } - } - if (params_.state_interfaces.empty()) { RCLCPP_ERROR(logger, "'state_interfaces' parameter is empty."); diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index e83e74ef41..f188c0f04b 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -39,6 +39,13 @@ using lifecycle_msgs::msg::State; using test_trajectory_controllers::TrajectoryControllerTest; using test_trajectory_controllers::TrajectoryControllerTestParameterized; +TEST_P(TrajectoryControllerTestParameterized, invalid_robot_description) +{ + ASSERT_EQ( + controller_interface::return_type::ERROR, + SetUpTrajectoryControllerLocal({}, "")); +} + TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) { rclcpp::executors::MultiThreadedExecutor executor; diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index a15b2e5bc5..796503c036 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -26,6 +26,7 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/joint_trajectory_controller.hpp" #include "joint_trajectory_controller/tolerances.hpp" +#include "ros2_control_test_assets/descriptions.hpp" namespace { @@ -249,7 +250,7 @@ class TrajectoryControllerTest : public ::testing::Test void SetUpTrajectoryController( rclcpp::Executor & executor, const std::vector & parameters = {}, - const std::string & urdf = "") + const std::string & urdf = ros2_control_test_assets::minimal_robot_urdf) { auto ret = SetUpTrajectoryControllerLocal(parameters, urdf); if (ret != controller_interface::return_type::OK) @@ -260,7 +261,8 @@ class TrajectoryControllerTest : public ::testing::Test } controller_interface::return_type SetUpTrajectoryControllerLocal( - const std::vector & parameters = {}, const std::string & urdf = "") + const std::vector & parameters = {}, + const std::string & urdf = ros2_control_test_assets::minimal_robot_urdf) { traj_controller_ = std::make_shared(); @@ -302,7 +304,7 @@ class TrajectoryControllerTest : public ::testing::Test const std::vector initial_vel_joints = INITIAL_VEL_JOINTS, const std::vector initial_acc_joints = INITIAL_ACC_JOINTS, const std::vector initial_eff_joints = INITIAL_EFF_JOINTS, - const std::string & urdf = "") + const std::string & urdf = ros2_control_test_assets::minimal_robot_urdf) { auto has_nonzero_vel_param = std::find_if(