Planning scene monitor: reliable QoS (#3400) (#3410)

Co-authored-by: Aleksey Nogin <aleksey@redballoonsecurity.com>
(cherry picked from commit 49ff5d8)

Co-authored-by: Mark Johnson <104826595+rr-mark@users.noreply.github.com>

Author: mergify[bot]

moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp

@@ -41,6 +41,8 @@
 #include <moveit_msgs/srv/get_planning_scene.hpp>
 #include <moveit/utils/logger.hpp>
 
+#include <rclcpp/qos.hpp>
+
 // TODO: Remove conditional includes when released to all active distros.
 #if __has_include(<tf2/exceptions.hpp>)
 #include <tf2/exceptions.hpp>
@@ -1198,7 +1200,7 @@ void PlanningSceneMonitor::startSceneMonitor(const std::string& scene_topic)
   if (!scene_topic.empty())
   {
     planning_scene_subscriber_ = pnode_->create_subscription<moveit_msgs::msg::PlanningScene>(
-        scene_topic, rclcpp::SystemDefaultsQoS(), [this](const moveit_msgs::msg::PlanningScene::ConstSharedPtr& scene) {
+        scene_topic, rclcpp::ServicesQoS(), [this](const moveit_msgs::msg::PlanningScene::ConstSharedPtr& scene) {
           return newPlanningSceneCallback(scene);
         });
     RCLCPP_INFO(logger_, "Listening to '%s'", planning_scene_subscriber_->get_topic_name());
@@ -1295,15 +1297,15 @@ void PlanningSceneMonitor::startWorldGeometryMonitor(const std::string& collisio
   if (!collision_objects_topic.empty())
   {
     collision_object_subscriber_ = pnode_->create_subscription<moveit_msgs::msg::CollisionObject>(
-        collision_objects_topic, rclcpp::SystemDefaultsQoS(),
+        collision_objects_topic, rclcpp::ServicesQoS(),
         [this](const moveit_msgs::msg::CollisionObject::ConstSharedPtr& obj) { processCollisionObjectMsg(obj); });
     RCLCPP_INFO(logger_, "Listening to '%s'", collision_objects_topic.c_str());
   }
 
   if (!planning_scene_world_topic.empty())
   {
     planning_scene_world_subscriber_ = pnode_->create_subscription<moveit_msgs::msg::PlanningSceneWorld>(
-        planning_scene_world_topic, rclcpp::SystemDefaultsQoS(),
+        planning_scene_world_topic, rclcpp::ServicesQoS(),
         [this](const moveit_msgs::msg::PlanningSceneWorld::ConstSharedPtr& world) {
           return newPlanningSceneWorldCallback(world);
         });
@@ -1377,7 +1379,7 @@ void PlanningSceneMonitor::startStateMonitor(const std::string& joint_states_top
     {
       // using regular message filter as there's no header
       attached_collision_object_subscriber_ = pnode_->create_subscription<moveit_msgs::msg::AttachedCollisionObject>(
-          attached_objects_topic, rclcpp::SystemDefaultsQoS(),
+          attached_objects_topic, rclcpp::ServicesQoS(),
           [this](const moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr& obj) {
             processAttachedCollisionObjectMsg(obj);
           });

moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp

@@ -42,6 +42,8 @@
 #include <tf2_eigen/tf2_eigen.hpp>
 #include <moveit/utils/logger.hpp>
 
+#include <rclcpp/qos.hpp>
+
 namespace trajectory_execution_manager
 {
 
@@ -200,7 +202,7 @@ void TrajectoryExecutionManager::initialize()
   auto options = rclcpp::SubscriptionOptions();
   options.callback_group = callback_group;
   event_topic_subscriber_ = node_->create_subscription<std_msgs::msg::String>(
-      EXECUTION_EVENT_TOPIC, rclcpp::SystemDefaultsQoS(),
+      EXECUTION_EVENT_TOPIC, rclcpp::ServicesQoS(),
       [this](const std_msgs::msg::String::ConstSharedPtr& event) { return receiveEvent(event); }, options);
 
   controller_mgr_node_->get_parameter("trajectory_execution.execution_duration_monitoring",

moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp

@@ -71,6 +71,8 @@
 #include <moveit/utils/rclcpp_utils.hpp>
 #include <moveit/utils/logger.hpp>
 
+#include <rclcpp/qos.hpp>
+
 namespace moveit_rviz_plugin
 {
 
@@ -280,7 +282,7 @@ void MotionPlanningDisplay::toggleSelectPlanningGroupSubscription(bool enable)
   if (enable)
   {
     planning_group_sub_ = node_->create_subscription<std_msgs::msg::String>(
-        "/rviz/moveit/select_planning_group", rclcpp::SystemDefaultsQoS(),
+        "/rviz/moveit/select_planning_group", rclcpp::ServicesQoS(),
         [this](const std_msgs::msg::String::ConstSharedPtr& msg) { return selectPlanningGroupCallback(msg); });
   }
   else

moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp

@@ -38,6 +38,8 @@
 #include <moveit/robot_state/conversions.hpp>
 #include <moveit/utils/message_checks.hpp>
 
+#include <rclcpp/qos.hpp>
+
 // #include <rviz/visualization_manager.h>
 #include <rviz_default_plugins/robot/robot.hpp>
 #include <rviz_default_plugins/robot/robot_link.hpp>
@@ -304,7 +306,7 @@ void RobotStateDisplay::changedRobotStateTopic()
   setStatus(rviz_common::properties::StatusProperty::Warn, "RobotState", "No msg received");
 
   robot_state_subscriber_ = node_->create_subscription<moveit_msgs::msg::DisplayRobotState>(
-      robot_state_topic_property_->getStdString(), rclcpp::SystemDefaultsQoS(),
+      robot_state_topic_property_->getStdString(), rclcpp::ServicesQoS(),
       [this](const moveit_msgs::msg::DisplayRobotState::ConstSharedPtr& state) { return newRobotStateCallback(state); });
 }
 

moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp

@@ -56,6 +56,8 @@
 #include <rviz_default_plugins/robot/robot_link.hpp>
 #include <rviz_common/window_manager_interface.hpp>
 
+#include <rclcpp/qos.hpp>
+
 #include <string>
 
 using namespace std::placeholders;
@@ -184,7 +186,7 @@ void TrajectoryVisualization::onInitialize(const rclcpp::Node::SharedPtr& node,
   }
 
   trajectory_topic_sub_ = node_->create_subscription<moveit_msgs::msg::DisplayTrajectory>(
-      trajectory_topic_property_->getStdString(), rclcpp::SystemDefaultsQoS(),
+      trajectory_topic_property_->getStdString(), rclcpp::ServicesQoS(),
       [this](const moveit_msgs::msg::DisplayTrajectory::ConstSharedPtr& msg) { return incomingDisplayTrajectory(msg); });
 }
 
@@ -295,7 +297,7 @@ void TrajectoryVisualization::changedTrajectoryTopic()
   if (!trajectory_topic_property_->getStdString().empty() && robot_state_)
   {
     trajectory_topic_sub_ = node_->create_subscription<moveit_msgs::msg::DisplayTrajectory>(
-        trajectory_topic_property_->getStdString(), rclcpp::SystemDefaultsQoS(),
+        trajectory_topic_property_->getStdString(), rclcpp::ServicesQoS(),
         [this](const moveit_msgs::msg::DisplayTrajectory::ConstSharedPtr& msg) {
           return incomingDisplayTrajectory(msg);
         });