PSM: Correctly handle full planning scene message (#3610) (#2876)

Fixes #3538/#3609 If the message is not a diff and parent_scene is not set either, the message should be handled as a full planning scene message as before #3538. Co-authored-by: Robert Haschke <[email protected]>
moveit · Jul 11, 2024 · 865b006 · 865b006
1 parent 9b3d96a
commit 865b006
Show file tree

Hide file tree

Showing 2 changed files with 2 additions and 1 deletion.
diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp
@@ -1262,6 +1262,7 @@ bool PlanningScene::setPlanningSceneDiffMsg(const moveit_msgs::msg::PlanningScen
 
 bool PlanningScene::setPlanningSceneMsg(const moveit_msgs::msg::PlanningScene& scene_msg)
 {
+  assert(scene_msg.is_diff == false);
   RCLCPP_DEBUG(getLogger(), "Setting new planning scene: '%s'", scene_msg.name.c_str());
   name_ = scene_msg.name;
 

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
@@ -740,7 +740,7 @@ bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::msg::Plann
     }
     else
     {
-      result = scene_->setPlanningSceneDiffMsg(scene);
+      result = scene_->usePlanningSceneMsg(scene);
     }
 
     if (octomap_monitor_)