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

Fixes moveit#3538/moveit#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 moveit#3538.
sjahr · Jun 21, 2024 · 327e6a1 · 327e6a1
1 parent 627d282
commit 327e6a1
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_)