[JTC] Refactor URDF Model parsing (ros-controls#1227)

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()
 
 

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.
    *

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<ParamListener>(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<ParamListener>(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.");

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({}, "<invalid_robot_description/>"));
+}
+
 TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands)
 {
   rclcpp::executors::MultiThreadedExecutor executor;

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<rclcpp::Parameter> & 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<rclcpp::Parameter> & parameters = {}, const std::string & urdf = "")
+    const std::vector<rclcpp::Parameter> & parameters = {},
+    const std::string & urdf = ros2_control_test_assets::minimal_robot_urdf)
   {
     traj_controller_ = std::make_shared<TestableJointTrajectoryController>();
 
@@ -302,7 +304,7 @@ class TrajectoryControllerTest : public ::testing::Test
     const std::vector<double> initial_vel_joints = INITIAL_VEL_JOINTS,
     const std::vector<double> initial_acc_joints = INITIAL_ACC_JOINTS,
     const std::vector<double> 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(