Skip to content

Optimize joint state broadcaster execution time#2159

Open
saikishor wants to merge 2 commits intoros-controls:masterfrom
pal-robotics-forks:optimize/joint_state_broadcaster
Open

Optimize joint state broadcaster execution time#2159
saikishor wants to merge 2 commits intoros-controls:masterfrom
pal-robotics-forks:optimize/joint_state_broadcaster

Conversation

@saikishor
Copy link
Copy Markdown
Member

Hello!

When testing the joint state broadcaster on high-frequency systems, it is observed that it is taking around 120-140 microseconds of execution, and it doesn't improve even when we choose to set the parameter publish_dynamic_joint_states: false . So, upon closer look, it seems like it always does the whole map lookup of the interfaces and it is consuming quite some time. When running 2 of these broadcaster it consumes half of our cycle time.

This PR proposes minor improvements that reduce the execution time by 70%

With the current state:
image

With the proposed fix:
image

Tested on TALOS robot by setting parameter publish_dynamic_joint_states: false

@saikishor saikishor added backport-jazzy Triggers PR backport to ROS 2 jazzy. backport-kilted Triggers PR backport to ROS 2 kilted. labels Feb 10, 2026
@codecov
Copy link
Copy Markdown

codecov bot commented Feb 10, 2026

Codecov Report

❌ Patch coverage is 75.00000% with 5 lines in your changes missing coverage. Please review.
✅ Project coverage is 84.77%. Comparing base (f72f813) to head (3b830fa).
⚠️ Report is 1 commits behind head on master.

Files with missing lines Patch % Lines
..._state_broadcaster/src/joint_state_broadcaster.cpp 75.00% 3 Missing and 2 partials ⚠️
Additional details and impacted files
@@            Coverage Diff             @@
##           master    #2159      +/-   ##
==========================================
- Coverage   84.79%   84.77%   -0.03%     
==========================================
  Files         151      151              
  Lines       14833    14846      +13     
  Branches     1286     1289       +3     
==========================================
+ Hits        12578    12586       +8     
- Misses       1784     1788       +4     
- Partials      471      472       +1     
Flag Coverage Δ
unittests 84.77% <75.00%> (-0.03%) ⬇️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files with missing lines Coverage Δ
...oint_state_broadcaster/joint_state_broadcaster.hpp 100.00% <ø> (ø)
..._state_broadcaster/src/joint_state_broadcaster.cpp 85.85% <75.00%> (-1.71%) ⬇️
🚀 New features to boost your workflow:
  • ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

// 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
Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sorry for nitpicking :)

Suggested change
// 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

Here I think caches suits better.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

backport-jazzy Triggers PR backport to ROS 2 jazzy. backport-kilted Triggers PR backport to ROS 2 kilted.

Projects

Status: Needs review

Development

Successfully merging this pull request may close these issues.

3 participants