Fix logic for planning scene publishing from MoveItCpp

Author: sea-bass

moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp

@@ -99,7 +99,8 @@ bool MoveItCpp::loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& opti
     RCLCPP_INFO(logger_, "Listening to '%s' for joint states", options.joint_state_topic.c_str());
     // Subscribe to JointState sensor messages
     planning_scene_monitor_->startStateMonitor(options.joint_state_topic, options.attached_collision_object_topic);
-    // Publish planning scene updates to remote monitors like RViz
+    // Publish planning scene updates to remote monitors like RViz, ensuring first that any active publisher is stopped.
+    planning_scene_monitor_->stopPublishingPlanningScene();
     planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,
                                                           options.monitored_planning_scene_topic);
     // Monitor and apply planning scene updates from remote publishers like the PlanningSceneInterface

moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp

@@ -423,6 +423,15 @@ void PlanningSceneMonitor::startPublishingPlanningScene(SceneUpdateType update_t
     monitorDiffs(true);
     publish_planning_scene_ = std::make_unique<std::thread>([this] { scenePublishingThread(); });
   }
+  else if (publish_planning_scene_)
+  {
+    RCLCPP_WARN(logger_, "Did not start publishing planning scene as it is already publishing. Ensure to call "
+                         "stopPublishingPlanningScene() first.");
+  }
+  else
+  {
+    RCLCPP_WARN(logger_, "Did not find a planning scene, so cannot publish it.");
+  }
 }
 
 void PlanningSceneMonitor::scenePublishingThread()