[JSB] Fix joint_state message corruption issue (backport #2217)

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;
     }
   }
 

joint_state_broadcaster/test/test_joint_state_broadcaster.cpp

@@ -14,9 +14,12 @@
 
 #include <cstddef>
 
+#include <atomic>
 #include <functional>
 #include <memory>
+#include <shared_mutex>
 #include <string>
+#include <thread>
 #include <utility>
 #include <vector>
 
@@ -1369,3 +1372,79 @@ 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)
+{
+  init_broadcaster_and_set_parameters(
+    "", {joint_names_[0], joint_names_[1], joint_names_[2]}, {HW_IF_POSITION});
+
+  std::vector<LoanedStateInterface> state_ifs;
+  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);
+  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<bool> lock_held{false};
+  std::atomic<bool> release_lock{false};
+
+  std::thread locker(
+    [&]()
+    {
+      // Acquire exclusive lock on the first interface's handle mutex
+      std::unique_lock<std::shared_mutex> 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))
+      {
+        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], 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], 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], joint_values_[2])
+    << "joint3's position slot has wrong value — map_index was shifted by the nullopt";
+}

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