From 6ca5d04d205e9fe523208278777c3099802a9469 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 10 Feb 2026 14:47:01 +0100 Subject: [PATCH 1/3] Optimize the execution time of the joint state broadcaster --- .../joint_state_broadcaster.hpp | 4 ++ .../src/joint_state_broadcaster.cpp | 47 ++++++++++++++++--- 2 files changed, 45 insertions(+), 6 deletions(-) diff --git a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp index 05d2334d0b..594188b605 100644 --- a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp @@ -121,6 +121,10 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface std::vector 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 joint_state_interface_indices_; + struct JointStateData { JointStateData(const double & position, const double & velocity, const double & effort) diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index a56accbc07..14d4b74a5b 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -311,6 +311,11 @@ void JointStateBroadcaster::init_auxiliary_data() { // save the mapping of state interfaces to joint states mapped_values_.clear(); + joint_state_interface_indices_.clear(); + + const std::vector 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) @@ -324,6 +329,14 @@ void JointStateBroadcaster::init_auxiliary_data() } mapped_values_.push_back( &name_if_value_mapping_[state_interfaces_[i].get_prefix_name()][interface_name]); + + // Track indices for joint state interfaces (position/velocity/effort) 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); + } } } @@ -411,16 +424,38 @@ 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_) { - if (state_interfaces_[i].get_data_type() == hardware_interface::HandleDataType::DOUBLE) + // Update all interfaces for dynamic joint state publishing + size_t map_index = 0u; + for (auto i = 0u; i < state_interfaces_.size(); ++i) { - // no retries, just try to get the latest value once - const auto & opt = state_interfaces_[i].get_optional(0); + 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 + { + // Optimized path: only update position/velocity/effort interfaces + for (const auto & idx : joint_state_interface_indices_) + { + const auto & opt = state_interfaces_[idx].get_optional(0); if (opt.has_value()) { - *mapped_values_[map_index++] = opt.value(); + const std::string & prefix_name = state_interfaces_[idx].get_prefix_name(); + std::string interface_name = state_interfaces_[idx].get_interface_name(); + if (map_interface_to_joint_state_.count(interface_name) > 0) + { + interface_name = map_interface_to_joint_state_[interface_name]; + } + name_if_value_mapping_[prefix_name][interface_name] = opt.value(); } } } From 3b830fa3e9758fd2b8d2a55746356a39ba155e4e Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 10 Feb 2026 15:47:02 +0100 Subject: [PATCH 2/3] more optimzation --- .../joint_state_broadcaster.hpp | 4 ++++ .../src/joint_state_broadcaster.cpp | 23 ++++++++----------- 2 files changed, 14 insertions(+), 13 deletions(-) diff --git a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp index 594188b605..51aad9eafa 100644 --- a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp @@ -125,6 +125,10 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface // Used for optimization when publish_dynamic_joint_states is false std::vector 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 joint_state_mapped_values_; + struct JointStateData { JointStateData(const double & position, const double & velocity, const double & effort) diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 14d4b74a5b..7337137f46 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -312,6 +312,7 @@ 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 joint_state_interfaces = { HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT}; @@ -327,15 +328,17 @@ 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 for joint state interfaces (position/velocity/effort) only + // 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); } } } @@ -443,19 +446,13 @@ controller_interface::return_type JointStateBroadcaster::update( } else { - // Optimized path: only update position/velocity/effort interfaces - for (const auto & idx : joint_state_interface_indices_) + // Optimized path: use pre-computed pointers to avoid map lookups + for (size_t i = 0; i < joint_state_interface_indices_.size(); ++i) { - const auto & opt = state_interfaces_[idx].get_optional(0); + const auto & opt = state_interfaces_[joint_state_interface_indices_[i]].get_optional(0); if (opt.has_value()) { - const std::string & prefix_name = state_interfaces_[idx].get_prefix_name(); - std::string interface_name = state_interfaces_[idx].get_interface_name(); - if (map_interface_to_joint_state_.count(interface_name) > 0) - { - interface_name = map_interface_to_joint_state_[interface_name]; - } - name_if_value_mapping_[prefix_name][interface_name] = opt.value(); + *joint_state_mapped_values_[i] = opt.value(); } } } From 92cf28077bab81104b42157aa623092085ce2aaa Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 5 May 2026 22:21:57 +0200 Subject: [PATCH 3/3] Update joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp Co-authored-by: Surya! --- .../include/joint_state_broadcaster/joint_state_broadcaster.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp index 51aad9eafa..90abf0af2e 100644 --- a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp @@ -125,7 +125,7 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface // Used for optimization when publish_dynamic_joint_states is false std::vector joint_state_interface_indices_; - // Pre-computed pointers to name_if_value_mapping_ values for joint state interfaces + // Caches 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 joint_state_mapped_values_;