[JSB] Move the initialize of urdf::Model from on_activate to on_confi…

…gure to improve real-time performance (#1269)

joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp

@@ -27,6 +27,7 @@
 #include "joint_state_broadcaster_parameters.hpp"
 #include "realtime_tools/realtime_publisher.h"
 #include "sensor_msgs/msg/joint_state.hpp"
+#include "urdf/model.h"
 
 namespace joint_state_broadcaster
 {
@@ -111,6 +112,9 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface
     dynamic_joint_state_publisher_;
   std::shared_ptr<realtime_tools::RealtimePublisher<control_msgs::msg::DynamicJointState>>
     realtime_dynamic_joint_state_publisher_;
+
+  urdf::Model model_;
+  bool is_model_loaded_ = false;
 };
 
 }  // namespace joint_state_broadcaster

joint_state_broadcaster/src/joint_state_broadcaster.cpp

@@ -164,6 +164,18 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_configure(
     fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
     return CallbackReturn::ERROR;
   }
+
+  const std::string & urdf = get_robot_description();
+
+  is_model_loaded_ = !urdf.empty() && model_.initString(urdf);
+  if (!is_model_loaded_)
+  {
+    RCLCPP_ERROR(
+      get_node()->get_logger(),
+      "Failed to parse robot description. Will publish all the interfaces with '%s', '%s' and '%s'",
+      HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT);
+  }
+
   return CallbackReturn::SUCCESS;
 }
 
@@ -243,17 +255,6 @@ bool JointStateBroadcaster::init_joint_data()
     name_if_value_mapping_[si->get_prefix_name()][interface_name] = kUninitializedValue;
   }
 
-  const std::string & urdf = get_robot_description();
-
-  urdf::Model model;
-  const bool is_model_loaded = !urdf.empty() && model.initString(urdf);
-  if (!is_model_loaded)
-  {
-    RCLCPP_ERROR(
-      get_node()->get_logger(),
-      "Failed to parse robot description. Will publish all the interfaces with '%s', '%s' and '%s'",
-      HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT);
-  }
   // filter state interfaces that have at least one of the joint_states fields,
   // the rest will be ignored for this message
   for (const auto & name_ifv : name_if_value_mapping_)
@@ -262,8 +263,8 @@ bool JointStateBroadcaster::init_joint_data()
     if (has_any_key(interfaces_and_values, {HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT}))
     {
       if (
-        !params_.use_urdf_to_filter || !params_.joints.empty() || !is_model_loaded ||
-        model.getJoint(name_ifv.first))
+        !params_.use_urdf_to_filter || !params_.joints.empty() || !is_model_loaded_ ||
+        model_.getJoint(name_ifv.first))
       {
         joint_names_.push_back(name_ifv.first);
       }