PSM: finish thread on rclcpp::shutdown (#3484)

Calling rate.sleep() on shutdown will crash

(cherry picked from commit f4cf780)

Author: MarqRazz

moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp

@@ -546,6 +546,9 @@ void PlanningSceneMonitor::scenePublishingThread()
       planning_scene_publisher_->publish(msg);
       if (is_full)
         RCLCPP_DEBUG(logger_, "Published full planning scene: '%s'", msg.name.c_str());
+      // finish thread on rclcpp shutdown (otherwise rate.sleep() will crash)
+      if (!rclcpp::ok())
+        break;
       rate.sleep();
     }
   } while (publish_planning_scene_);