Resolves merge conflicts.

moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp

@@ -77,17 +77,12 @@ void MoveGroupExecuteTrajectoryAction::initialize()
         RCLCPP_INFO(LOGGER, "Received goal request");
         return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
       },
-<<<<<<< HEAD
       [](const std::shared_ptr<ExecTrajectoryGoal>& /* unused */) {
         RCLCPP_INFO(LOGGER, "Received request to cancel goal");
         return rclcpp_action::CancelResponse::ACCEPT;
       },
-      [this](const auto& goal) { executePathCallback(goal); });
-=======
-      [](const std::shared_ptr<ExecTrajectoryGoal>& /* unused */) { return rclcpp_action::CancelResponse::ACCEPT; },
       [this](const auto& goal) { executePathCallback(goal); }, rcl_action_server_get_default_options(),
       callback_group_);
->>>>>>> ba35aaa58 (Ports moveit #3676 and #3682 (#3283))
 }
 
 void MoveGroupExecuteTrajectoryAction::executePathCallback(const std::shared_ptr<ExecTrajectoryGoal>& goal)

moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h

@@ -326,8 +326,9 @@ class MOVEIT_PLANNING_SCENE_MONITOR_EXPORT PlanningSceneMonitor
 
   /** @brief Update the scene using the monitored state. This function is automatically called when an update to the
      current state is received (if startStateMonitor() has been called).
-      The updates are throttled to a maximum update frequency however, which is set by setStateUpdateFrequency(). */
-  void updateSceneWithCurrentState();
+      The updates are throttled to a maximum update frequency however, which is set by setStateUpdateFrequency().
+      @param skip_update_if_locked causes the update to be skipped if the planning scene is locked. */
+  void updateSceneWithCurrentState(bool skip_update_if_locked = false);
 
   /** @brief Update the scene using the monitored state at a specified frequency, in Hz. This function has an effect
      only when updates from the CurrentStateMonitor are received at a higher frequency.
@@ -581,15 +582,19 @@ class MOVEIT_PLANNING_SCENE_MONITOR_EXPORT PlanningSceneMonitor
   void updatePublishSettings(bool publish_geom_updates, bool publish_state_updates, bool publish_transform_updates,
                              bool publish_planning_scene, double publish_planning_scene_hz);
 
-  // Lock for state_update_pending_ and dt_state_update_
-  std::mutex state_pending_mutex_;
+  /// True if current_state_monitor_ has a newer RobotState than scene_
+  std::atomic<bool> state_update_pending_;
 
-  /// True when we need to update the RobotState from current_state_monitor_
-  // This field is protected by state_pending_mutex_
-  volatile bool state_update_pending_;
+  // Lock for writing last_robot_state_update_wall_time_ and dt_state_update_
+  std::mutex state_update_mutex_;
+
+  /// Last time the state was updated from current_state_monitor_
+  // Only access this from callback functions (and constructor)
+  // This field is protected by state_update_mutex_
+  std::chrono::system_clock::time_point last_robot_state_update_wall_time_;
 
   /// the amount of time to wait in between updates to the robot state
-  // This field is protected by state_pending_mutex_
+  // This field is protected by state_update_mutex_
   std::chrono::duration<double> dt_state_update_;  // 1hz
 
   /// the amount of time to wait when looking up transforms
@@ -598,15 +603,10 @@ class MOVEIT_PLANNING_SCENE_MONITOR_EXPORT PlanningSceneMonitor
   rclcpp::Duration shape_transform_cache_lookup_wait_time_;
 
   /// timer for state updates.
-  // Check if last_state_update_ is true and if so call updateSceneWithCurrentState()
+  // If state_update_pending_ is true, call updateSceneWithCurrentState()
   // Not safe to access from callback functions.
-
   rclcpp::TimerBase::SharedPtr state_update_timer_;
 
-  /// Last time the state was updated from current_state_monitor_
-  // Only access this from callback functions (and constructor)
-  std::chrono::system_clock::time_point last_robot_state_update_wall_time_;
-
   robot_model_loader::RobotModelLoaderPtr rm_loader_;
   moveit::core::RobotModelConstPtr robot_model_;