From 1bebcf4c9e9750bdf9f9d012c835c1ddad541c1d Mon Sep 17 00:00:00 2001 From: Marq Rasmussen Date: Tue, 7 Jan 2025 04:17:59 -0500 Subject: [PATCH] Fix planning_scene_monitor sync when passed empty robot state (#3187) * Fix planning_scene_monitor sync when passed empty robot state * address review * check for null current_state_monitor_ and fall back to diff scene_ if needed --------- Co-authored-by: Sebastian Jahr (cherry picked from commit 80c7f612bcaa45264ff8a5483f99325c7e5689f2) --- .../src/planning_scene_monitor.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) 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 0f3d1a067b..0ff7811e38 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 @@ -740,6 +740,21 @@ 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()); + } + } // clear maintained (diff) scene_ and set the full new scene in parent_scene_ instead scene_->clearDiffs(); result = parent_scene_->setPlanningSceneMsg(scene);