From 7bbe3712f120126594fa1da1b4ec2b913c203f56 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 17 Mar 2026 19:56:30 +0100 Subject: [PATCH 1/2] [JSB] Fix joint_state message corruption issue (#2217) (cherry picked from commit 2942d730feecd296ddea9fb7e8f37c3072a2b99f) --- .../src/joint_state_broadcaster.cpp | 7 +- .../test/test_joint_state_broadcaster.cpp | 89 +++++++++++++++++++ .../test/test_joint_state_broadcaster.hpp | 1 + 3 files changed, 96 insertions(+), 1 deletion(-) diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index a56accbc07..7b138a9603 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -420,8 +420,13 @@ controller_interface::return_type JointStateBroadcaster::update( const auto & opt = state_interfaces_[i].get_optional(0); if (opt.has_value()) { - *mapped_values_[map_index++] = opt.value(); + *mapped_values_[map_index] = opt.value(); } + // Always advance map_index for every DOUBLE interface, regardless of whether the read + // succeeded. If we only advance on success, a temporary read failure (e.g. lock contention + // on a chained interface) causes all subsequent interfaces to be written into the wrong + // mapped_values_ slots, corrupting the published joint states. + ++map_index; } } diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index df18ad0d1c..865d729e12 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -14,9 +14,12 @@ #include +#include #include #include +#include #include +#include #include #include @@ -1369,3 +1372,89 @@ TEST_F(JointStateBroadcasterTest, NoThrowWithBooleanAndDoubleInterfaceTest) ASSERT_THAT(state_broadcaster_->joint_state_msg_.velocity, SizeIs(1)); ASSERT_THAT(state_broadcaster_->joint_state_msg_.effort, SizeIs(1)); } + +// Regression test: when a double interface temporarily fails to be read +// (get_optional returns nullopt as it is unable to lock), map_index must still advance so that +// subsequent interfaces are written to the correct mapped_values_ indexes. +// +// Without the fix, if state_interfaces_[i] returns nullopt, the next interface's value is +// written into index i instead of index i+1, corrupting all subsequent joint state values. +TEST_F(JointStateBroadcasterTest, CorrectMappingWhenInterfaceReadFailsTest) +{ + const std::string JOINT_NAMES[] = {"joint1", "joint2", "joint3"}; + const double INIT_POS[] = {1.1, 2.2, 3.3}; + + auto j1 = std::make_shared( + JOINT_NAMES[0], HW_IF_POSITION, "double", std::to_string(INIT_POS[0])); + auto j2 = std::make_shared( + JOINT_NAMES[1], HW_IF_POSITION, "double", std::to_string(INIT_POS[1])); + auto j3 = std::make_shared( + JOINT_NAMES[2], HW_IF_POSITION, "double", std::to_string(INIT_POS[2])); + + init_broadcaster_and_set_parameters( + "", {JOINT_NAMES[0], JOINT_NAMES[1], JOINT_NAMES[2]}, {HW_IF_POSITION}); + + std::vector state_ifs; + state_ifs.emplace_back(j1); + state_ifs.emplace_back(j2); + state_ifs.emplace_back(j3); + state_broadcaster_->assign_interfaces({}, std::move(state_ifs)); + + ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT( + state_broadcaster_->joint_state_msg_.name, + ElementsAreArray({JOINT_NAMES[0], JOINT_NAMES[1], JOINT_NAMES[2]})); + + /// Simulate a temporarily-unavailable first interface + // Hold an exclusive lock on state_interfaces_[0]'s mutex from a helper thread. + // While the lock is held, get_optional(0) on state_interfaces_[0] cannot acquire the + // shared lock and returns nullopt. + std::atomic lock_held{false}; + std::atomic release_lock{false}; + + std::thread locker( + [&]() + { + // Acquire exclusive lock on the first interface's handle mutex + std::unique_lock lk(j1->get_mutex()); + lock_held.store(true, std::memory_order_release); + // Hold it until the main thread finishes its update() call + while (!release_lock.load(std::memory_order_acquire)) + { + std::this_thread::yield(); + } + }); + + // Wait until the locker thread actually owns the mutex + while (!lock_held.load(std::memory_order_acquire)) + { + std::this_thread::yield(); + } + + // Call update(): joint1/position read will return nullopt (lock held by locker thread). + ASSERT_NO_THROW( + state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + + release_lock.store(true, std::memory_order_release); + locker.join(); + + const auto & names = state_broadcaster_->joint_state_msg_.name; + const auto & pos = state_broadcaster_->joint_state_msg_.position; + ASSERT_EQ(pos.size(), 3u); + + // joint1 was not readable, its index must NOT contain joint2's value (2.2). + // (It will hold the initial NaN or any prior value, not 2.2.) + EXPECT_EQ(names[0], JOINT_NAMES[0]); + EXPECT_NE(pos[0], INIT_POS[1]) + << "joint1's position slot was overwritten with joint2's value — map_index bug is present"; + + EXPECT_EQ(names[1], JOINT_NAMES[1]); + EXPECT_DOUBLE_EQ(pos[1], INIT_POS[1]) + << "joint2's position slot has wrong value — map_index was shifted by the nullopt"; + + EXPECT_EQ(names[2], JOINT_NAMES[2]); + EXPECT_DOUBLE_EQ(pos[2], INIT_POS[2]) + << "joint3's position slot has wrong value — map_index was shifted by the nullopt"; +} diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp index f12bb59635..56f9c28496 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp @@ -52,6 +52,7 @@ class FriendJointStateBroadcaster : public joint_state_broadcaster::JointStateBr FRIEND_TEST(JointStateBroadcasterTest, ExtraJointStatePublishTest); FRIEND_TEST(JointStateBroadcasterTest, NoThrowWithBooleanInterfaceTest); FRIEND_TEST(JointStateBroadcasterTest, NoThrowWithBooleanAndDoubleInterfaceTest); + FRIEND_TEST(JointStateBroadcasterTest, CorrectMappingWhenInterfaceReadFailsTest); }; class JointStateBroadcasterTest : public ::testing::Test From a0792847293eb2f6bf760e34ee22c35f6162802a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 3 Apr 2026 17:03:09 +0200 Subject: [PATCH 2/2] Fix tests --- .../test/test_joint_state_broadcaster.cpp | 34 +++++++------------ 1 file changed, 12 insertions(+), 22 deletions(-) diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 865d729e12..2c0b2b7376 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -1381,23 +1381,13 @@ TEST_F(JointStateBroadcasterTest, NoThrowWithBooleanAndDoubleInterfaceTest) // written into index i instead of index i+1, corrupting all subsequent joint state values. TEST_F(JointStateBroadcasterTest, CorrectMappingWhenInterfaceReadFailsTest) { - const std::string JOINT_NAMES[] = {"joint1", "joint2", "joint3"}; - const double INIT_POS[] = {1.1, 2.2, 3.3}; - - auto j1 = std::make_shared( - JOINT_NAMES[0], HW_IF_POSITION, "double", std::to_string(INIT_POS[0])); - auto j2 = std::make_shared( - JOINT_NAMES[1], HW_IF_POSITION, "double", std::to_string(INIT_POS[1])); - auto j3 = std::make_shared( - JOINT_NAMES[2], HW_IF_POSITION, "double", std::to_string(INIT_POS[2])); - init_broadcaster_and_set_parameters( - "", {JOINT_NAMES[0], JOINT_NAMES[1], JOINT_NAMES[2]}, {HW_IF_POSITION}); + "", {joint_names_[0], joint_names_[1], joint_names_[2]}, {HW_IF_POSITION}); std::vector state_ifs; - state_ifs.emplace_back(j1); - state_ifs.emplace_back(j2); - state_ifs.emplace_back(j3); + state_ifs.emplace_back(joint_1_pos_state_); + state_ifs.emplace_back(joint_2_pos_state_); + state_ifs.emplace_back(joint_3_pos_state_); state_broadcaster_->assign_interfaces({}, std::move(state_ifs)); ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -1405,7 +1395,7 @@ TEST_F(JointStateBroadcasterTest, CorrectMappingWhenInterfaceReadFailsTest) ASSERT_THAT( state_broadcaster_->joint_state_msg_.name, - ElementsAreArray({JOINT_NAMES[0], JOINT_NAMES[1], JOINT_NAMES[2]})); + ElementsAreArray({joint_names_[0], joint_names_[1], joint_names_[2]})); /// Simulate a temporarily-unavailable first interface // Hold an exclusive lock on state_interfaces_[0]'s mutex from a helper thread. @@ -1418,7 +1408,7 @@ TEST_F(JointStateBroadcasterTest, CorrectMappingWhenInterfaceReadFailsTest) [&]() { // Acquire exclusive lock on the first interface's handle mutex - std::unique_lock lk(j1->get_mutex()); + std::unique_lock lk(joint_1_pos_state_.get_mutex()); lock_held.store(true, std::memory_order_release); // Hold it until the main thread finishes its update() call while (!release_lock.load(std::memory_order_acquire)) @@ -1446,15 +1436,15 @@ TEST_F(JointStateBroadcasterTest, CorrectMappingWhenInterfaceReadFailsTest) // joint1 was not readable, its index must NOT contain joint2's value (2.2). // (It will hold the initial NaN or any prior value, not 2.2.) - EXPECT_EQ(names[0], JOINT_NAMES[0]); - EXPECT_NE(pos[0], INIT_POS[1]) + EXPECT_EQ(names[0], joint_names_[0]); + EXPECT_NE(pos[0], joint_values_[1]) << "joint1's position slot was overwritten with joint2's value — map_index bug is present"; - EXPECT_EQ(names[1], JOINT_NAMES[1]); - EXPECT_DOUBLE_EQ(pos[1], INIT_POS[1]) + EXPECT_EQ(names[1], joint_names_[1]); + EXPECT_DOUBLE_EQ(pos[1], joint_values_[1]) << "joint2's position slot has wrong value — map_index was shifted by the nullopt"; - EXPECT_EQ(names[2], JOINT_NAMES[2]); - EXPECT_DOUBLE_EQ(pos[2], INIT_POS[2]) + EXPECT_EQ(names[2], joint_names_[2]); + EXPECT_DOUBLE_EQ(pos[2], joint_values_[2]) << "joint3's position slot has wrong value — map_index was shifted by the nullopt"; }