address review

MarqRazz · Jan 3, 2025 · 9586c7d · 9586c7d
1 parent 06b171f
commit 9586c7d
Showing 1 changed file with 4 additions and 2 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
@@ -732,9 +732,11 @@ 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 value from the current_state_monitor_
-      // so that we are not updating the scene with stale values for the robots state.
-      if (scene.robot_state.joint_state.name.empty())
+      // so that we are not updating the scene with stale values for the robot's state.
+      if (scene.robot_state.joint_state.name.empty() && current_state_monitor_->haveCompleteState())
+      {
         parent_scene_->setCurrentState(*current_state_monitor_->getCurrentState());
+      }
       // clear maintained (diff) scene_ and set the full new scene in parent_scene_ instead
       scene_->clearDiffs();
       result = parent_scene_->setPlanningSceneMsg(scene);