Fix the issue with the corruption

Author: saikishor

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