Shut down PSM publishing before starting to publish on a potentially new topic (#2680)

Author: sea-bass

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<moveit_msgs::msg::PlanningScene>(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<std::thread>([this] { scenePublishingThread(); });
   }
+  else
+  {
+    RCLCPP_WARN(logger_, "Did not find a planning scene, so cannot publish it.");
+  }
 }
 
 void PlanningSceneMonitor::scenePublishingThread()