From 865b006036ebf21226859c3f64a3e829c087205d Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 11 Jul 2024 19:45:12 +0200 Subject: [PATCH] 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 --- moveit_core/planning_scene/src/planning_scene.cpp | 1 + .../planning_scene_monitor/src/planning_scene_monitor.cpp | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 908a053df8..eaffc31d42 100644 --- 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 index 648cd47162..103a7e1d09 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,7 +740,7 @@ bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::msg::Plann } else { - result = scene_->setPlanningSceneDiffMsg(scene); + result = scene_->usePlanningSceneMsg(scene); } if (octomap_monitor_)