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

…gure to improve real-time performance (ros-controls#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);
  }