From f1f61736f62a7bc953e4be4b77431a7630902a7d Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 7 Jan 2025 18:57:54 -0500 Subject: [PATCH] Simplify scene update that does not include new robot_state (#3206) (#3207) Co-authored-by: Robert Haschke (cherry picked from commit 75286c7822e5f2d9babbb41b9a0ffaddb0d54808) Co-authored-by: Marq Rasmussen --- .../src/planning_scene_monitor.cpp | 18 +++--------------- 1 file changed, 3 insertions(+), 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 index 2fce03270a..d49c139b17 100644 --- 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);