Optimize joint state broadcaster execution time

joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp

@@ -121,6 +121,14 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface
 
   std::vector<double *> mapped_values_;
 
+  // Indices into state_interfaces_ for joint state (position/velocity/effort) interfaces only
+  // Used for optimization when publish_dynamic_joint_states is false
+  std::vector<size_t> joint_state_interface_indices_;
+
+  // Pre-computed pointers to name_if_value_mapping_ values for joint state interfaces
+  // Avoids map lookups in the hot path when dynamic joint state publishing is disabled
+  std::vector<double *> joint_state_mapped_values_;
+
   struct JointStateData
   {
     JointStateData(const double & position, const double & velocity, const double & effort)

joint_state_broadcaster/src/joint_state_broadcaster.cpp

@@ -311,6 +311,12 @@ void JointStateBroadcaster::init_auxiliary_data()
 {
   // save the mapping of state interfaces to joint states
   mapped_values_.clear();
+  joint_state_interface_indices_.clear();
+  joint_state_mapped_values_.clear();
+
+  const std::vector<std::string> joint_state_interfaces = {
+    HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT};
+
   for (auto i = 0u; i < state_interfaces_.size(); ++i)
   {
     if (state_interfaces_[i].get_data_type() != hardware_interface::HandleDataType::DOUBLE)
@@ -322,8 +328,18 @@ void JointStateBroadcaster::init_auxiliary_data()
     {
       interface_name = map_interface_to_joint_state_[interface_name];
     }
-    mapped_values_.push_back(
-      &name_if_value_mapping_[state_interfaces_[i].get_prefix_name()][interface_name]);
+    double * value_ptr =
+      &name_if_value_mapping_[state_interfaces_[i].get_prefix_name()][interface_name];
+    mapped_values_.push_back(value_ptr);
+
+    // Track indices and pre-computed pointers for joint state interfaces only
+    if (
+      std::find(joint_state_interfaces.begin(), joint_state_interfaces.end(), interface_name) !=
+      joint_state_interfaces.end())
+    {
+      joint_state_interface_indices_.push_back(i);
+      joint_state_mapped_values_.push_back(value_ptr);
+    }
   }
 }
 
@@ -411,16 +427,32 @@ bool JointStateBroadcaster::use_all_available_interfaces() const
 controller_interface::return_type JointStateBroadcaster::update(
   const rclcpp::Time & time, const rclcpp::Duration & /*period*/)
 {
-  size_t map_index = 0u;
-  for (auto i = 0u; i < state_interfaces_.size(); ++i)
+  if (realtime_dynamic_joint_state_publisher_)
+  {
+    // Update all interfaces for dynamic joint state publishing
+    size_t map_index = 0u;
+    for (auto i = 0u; i < state_interfaces_.size(); ++i)
+    {
+      if (state_interfaces_[i].get_data_type() == hardware_interface::HandleDataType::DOUBLE)
+      {
+        // no retries, just try to get the latest value once
+        const auto & opt = state_interfaces_[i].get_optional(0);
+        if (opt.has_value())
+        {
+          *mapped_values_[map_index++] = opt.value();
+        }
+      }
+    }
+  }
+  else
   {
-    if (state_interfaces_[i].get_data_type() == hardware_interface::HandleDataType::DOUBLE)
+    // Optimized path: use pre-computed pointers to avoid map lookups
+    for (size_t i = 0; i < joint_state_interface_indices_.size(); ++i)
     {
-      // no retries, just try to get the latest value once
-      const auto & opt = state_interfaces_[i].get_optional(0);
+      const auto & opt = state_interfaces_[joint_state_interface_indices_[i]].get_optional(0);
       if (opt.has_value())
       {
-        *mapped_values_[map_index++] = opt.value();
+        *joint_state_mapped_values_[i] = opt.value();
       }
     }
   }