Simplify scene update that does not include new robot_state (#3206) (#…

…3207) Co-authored-by: Robert Haschke <[email protected]> (cherry picked from commit 75286c7) Co-authored-by: Marq Rasmussen <[email protected]>
moveit · Jan 7, 2025 · f1f6173 · f1f6173
1 parent cd6ac9b
commit f1f6173
Showing 1 changed file with 3 additions and 15 deletions.
diff --git a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp
@@ -678,21 +678,9 @@ bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::msg::Plann
 
     if (!scene.is_diff && parent_scene_)
     {
-      // if the scene does not contain a robot state then use the latest known values for the robot state
-      // so that we are not updating the scene with stale values.
-      if (scene.robot_state.joint_state.name.empty())
-      {
-        // if we have a valid current_state_monitor_ with complete state use the latest data otherwise try
-        // to use the maintained (diff) scene_ which may fall back to the parent_scene_ data and not update.
-        if (current_state_monitor_ && current_state_monitor_->haveCompleteState())
-        {
-          parent_scene_->setCurrentState(*current_state_monitor_->getCurrentState());
-        }
-        else
-        {
-          parent_scene_->setCurrentState(scene_->getCurrentState());
-        }
-      }
+      // If there is no new robot_state, transfer RobotState from current scene to parent scene
+      if (scene.robot_state.is_diff)
+        parent_scene_->setCurrentState(scene_->getCurrentState());
       // clear maintained (diff) scene_ and set the full new scene in parent_scene_ instead
       scene_->clearDiffs();
       result = parent_scene_->setPlanningSceneMsg(scene);