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 c030a2d0a4..c0fd12e94c 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 @@ -416,13 +416,24 @@ void PlanningSceneMonitor::startPublishingPlanningScene(SceneUpdateType update_t const std::string& planning_scene_topic) { publish_update_types_ = update_type; - if (!publish_planning_scene_ && scene_) + + if (publish_planning_scene_) + { + RCLCPP_INFO(logger_, "Stopping existing planning scene publisher."); + stopPublishingPlanningScene(); + } + + if (scene_) { planning_scene_publisher_ = pnode_->create_publisher(planning_scene_topic, 100); RCLCPP_INFO(logger_, "Publishing maintained planning scene on '%s'", planning_scene_topic.c_str()); monitorDiffs(true); publish_planning_scene_ = std::make_unique([this] { scenePublishingThread(); }); } + else + { + RCLCPP_WARN(logger_, "Did not find a planning scene, so cannot publish it."); + } } void PlanningSceneMonitor::scenePublishingThread()