From 5506dd516a91bc145e462b493668ef8623d43521 Mon Sep 17 00:00:00 2001 From: Shobuj Paul <72087882+Shobuj-Paul@users.noreply.github.com> Date: Mon, 31 Jul 2023 19:34:17 +0530 Subject: [PATCH] Replaced numbers with SystemDefaultsQos() (#2271) --- .../src/local_planner_component.cpp | 3 ++- .../moveit_servo/test/pose_tracking_test.cpp | 2 +- .../semantic_world/src/semantic_world.cpp | 2 +- ...current_state_monitor_middleware_handle.cpp | 6 +----- .../src/planning_scene_monitor.cpp | 10 ++++++---- .../src/synchronized_string_parameter.cpp | 3 ++- .../src/trajectory_execution_manager.cpp | 2 +- .../src/robot_interaction.cpp | 4 ++-- .../src/motion_planning_display.cpp | 2 +- .../src/motion_planning_frame.cpp | 18 ++++++++++-------- .../src/robot_state_display.cpp | 2 +- .../src/trajectory_visualization.cpp | 4 ++-- moveit_ros/warehouse/src/save_to_warehouse.cpp | 8 +++++--- 13 files changed, 35 insertions(+), 31 deletions(-) diff --git a/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp b/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp index 0edc36e341..3810608b5c 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp +++ b/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp @@ -203,7 +203,8 @@ bool LocalPlannerComponent::initialize() // Initialize global trajectory listener global_solution_subscriber_ = node_->create_subscription( - config_.global_solution_topic, 1, [this](const moveit_msgs::msg::MotionPlanResponse::ConstSharedPtr& msg) { + config_.global_solution_topic, rclcpp::SystemDefaultsQoS(), + [this](const moveit_msgs::msg::MotionPlanResponse::ConstSharedPtr& msg) { // Add received trajectory to internal reference trajectory robot_trajectory::RobotTrajectory new_trajectory(planning_scene_monitor_->getRobotModel(), msg->group_name); moveit::core::RobotState start_state(planning_scene_monitor_->getRobotModel()); diff --git a/moveit_ros/moveit_servo/test/pose_tracking_test.cpp b/moveit_ros/moveit_servo/test/pose_tracking_test.cpp index 6651b5c93d..cf7f2d7eb1 100644 --- a/moveit_ros/moveit_servo/test/pose_tracking_test.cpp +++ b/moveit_ros/moveit_servo/test/pose_tracking_test.cpp @@ -178,7 +178,7 @@ TEST_F(PoseTrackingFixture, OutgoingMsgTest) return; }; auto traj_sub = node_->create_subscription( - "/panda_arm_controller/joint_trajectory", 1, traj_callback); + "/panda_arm_controller/joint_trajectory", rclcpp::SystemDefaultsQoS(), traj_callback); geometry_msgs::msg::PoseStamped target_pose; target_pose.header.frame_id = "panda_link4"; diff --git a/moveit_ros/perception/semantic_world/src/semantic_world.cpp b/moveit_ros/perception/semantic_world/src/semantic_world.cpp index 36d0e778a9..e63ac1c5f2 100644 --- a/moveit_ros/perception/semantic_world/src/semantic_world.cpp +++ b/moveit_ros/perception/semantic_world/src/semantic_world.cpp @@ -71,7 +71,7 @@ SemanticWorld::SemanticWorld(const rclcpp::Node::SharedPtr& node, { table_subscriber_ = node_handle_->create_subscription( - "table_array", 1, + "table_array", rclcpp::SystemDefaultsQoS(), [this](const object_recognition_msgs::msg::TableArray::ConstSharedPtr& msg) { return tableCallback(msg); }); visualization_publisher_ = node_handle_->create_publisher("visualize_place", 20); diff --git a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor_middleware_handle.cpp b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor_middleware_handle.cpp index feeda9baca..33f18180de 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor_middleware_handle.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor_middleware_handle.cpp @@ -56,10 +56,6 @@ namespace planning_scene_monitor { -namespace -{ -static const auto SUBSCRIPTION_QOS = rclcpp::QoS(25); -} CurrentStateMonitorMiddlewareHandle::CurrentStateMonitorMiddlewareHandle(const rclcpp::Node::SharedPtr& node) : node_(node) @@ -75,7 +71,7 @@ void CurrentStateMonitorMiddlewareHandle::createJointStateSubscription(const std JointStateUpdateCallback callback) { joint_state_subscription_ = - node_->create_subscription(topic, SUBSCRIPTION_QOS, callback); + node_->create_subscription(topic, rclcpp::SystemDefaultsQoS(), callback); } void CurrentStateMonitorMiddlewareHandle::resetJointStateSubscription() 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 63c5c161fc..7384fda583 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 @@ -1128,7 +1128,7 @@ void PlanningSceneMonitor::startSceneMonitor(const std::string& scene_topic) if (!scene_topic.empty()) { planning_scene_subscriber_ = pnode_->create_subscription( - scene_topic, 100, [this](const moveit_msgs::msg::PlanningScene::ConstSharedPtr& scene) { + scene_topic, rclcpp::SystemDefaultsQoS(), [this](const moveit_msgs::msg::PlanningScene::ConstSharedPtr& scene) { return newPlanningSceneCallback(scene); }); RCLCPP_INFO(LOGGER, "Listening to '%s'", planning_scene_subscriber_->get_topic_name()); @@ -1225,7 +1225,7 @@ void PlanningSceneMonitor::startWorldGeometryMonitor(const std::string& collisio if (!collision_objects_topic.empty()) { collision_object_subscriber_ = pnode_->create_subscription( - collision_objects_topic, 1024, + collision_objects_topic, rclcpp::SystemDefaultsQoS(), [this](const moveit_msgs::msg::CollisionObject::ConstSharedPtr& obj) { return collisionObjectCallback(obj); }); RCLCPP_INFO(LOGGER, "Listening to '%s'", collision_objects_topic.c_str()); } @@ -1233,7 +1233,8 @@ void PlanningSceneMonitor::startWorldGeometryMonitor(const std::string& collisio if (!planning_scene_world_topic.empty()) { planning_scene_world_subscriber_ = pnode_->create_subscription( - planning_scene_world_topic, 1, [this](const moveit_msgs::msg::PlanningSceneWorld::ConstSharedPtr& world) { + planning_scene_world_topic, rclcpp::SystemDefaultsQoS(), + [this](const moveit_msgs::msg::PlanningSceneWorld::ConstSharedPtr& world) { return newPlanningSceneWorldCallback(world); }); RCLCPP_INFO(LOGGER, "Listening to '%s' for planning scene world geometry", planning_scene_world_topic.c_str()); @@ -1306,7 +1307,8 @@ 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( - attached_objects_topic, 1024, [this](const moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr& obj) { + attached_objects_topic, rclcpp::SystemDefaultsQoS(), + [this](const moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr& obj) { return attachObjectCallback(obj); }); RCLCPP_INFO(LOGGER, "Listening to '%s' for attached collision objects", diff --git a/moveit_ros/planning/rdf_loader/src/synchronized_string_parameter.cpp b/moveit_ros/planning/rdf_loader/src/synchronized_string_parameter.cpp index 623214aadf..189e3b6c4e 100644 --- a/moveit_ros/planning/rdf_loader/src/synchronized_string_parameter.cpp +++ b/moveit_ros/planning/rdf_loader/src/synchronized_string_parameter.cpp @@ -123,7 +123,8 @@ bool SynchronizedStringParameter::waitForMessage(const rclcpp::Duration& timeout auto const nd_name = std::string(node_->get_name()).append("_ssp_").append(name_); auto const temp_node = std::make_shared(nd_name); string_subscriber_ = temp_node->create_subscription( - name_, rclcpp::QoS(1).transient_local().reliable(), + name_, + rclcpp::QoS(1).transient_local().reliable(), // "transient_local()" is required for supporting late subscriptions [this](const std_msgs::msg::String::ConstSharedPtr& msg) { return stringCallback(msg); }); rclcpp::WaitSet wait_set; diff --git a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp index 557659c483..b4b89168ff 100644 --- a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp +++ b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp @@ -186,7 +186,7 @@ void TrajectoryExecutionManager::initialize() auto options = rclcpp::SubscriptionOptions(); options.callback_group = callback_group; event_topic_subscriber_ = node_->create_subscription( - EXECUTION_EVENT_TOPIC, 100, + EXECUTION_EVENT_TOPIC, rclcpp::SystemDefaultsQoS(), [this](const std_msgs::msg::String::ConstSharedPtr& event) { return receiveEvent(event); }, options); controller_mgr_node_->get_parameter("trajectory_execution.execution_duration_monitoring", diff --git a/moveit_ros/robot_interaction/src/robot_interaction.cpp b/moveit_ros/robot_interaction/src/robot_interaction.cpp index 9bd80e3030..14b71ccdde 100644 --- a/moveit_ros/robot_interaction/src/robot_interaction.cpp +++ b/moveit_ros/robot_interaction/src/robot_interaction.cpp @@ -602,8 +602,8 @@ void RobotInteraction::toggleMoveInteractiveMarkerTopic(bool enable) [this, marker_name](const geometry_msgs::msg::PoseStamped::SharedPtr& msg) { moveInteractiveMarker(marker_name, *msg); }; - auto subscription = - node_->create_subscription(topic_name, 1, subscription_callback); + auto subscription = node_->create_subscription( + topic_name, rclcpp::SystemDefaultsQoS(), subscription_callback); int_marker_move_subscribers_.push_back(subscription); } } diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp index 300f7c627a..12588827e6 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp @@ -284,7 +284,7 @@ void MotionPlanningDisplay::toggleSelectPlanningGroupSubscription(bool enable) if (enable) { planning_group_sub_ = node_->create_subscription( - "/rviz/moveit/select_planning_group", 1, + "/rviz/moveit/select_planning_group", rclcpp::SystemDefaultsQoS(), [this](const std_msgs::msg::String::ConstSharedPtr& msg) { return selectPlanningGroupCallback(msg); }); } else diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp index 09ba2ad2e6..b6cbabab73 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp @@ -267,26 +267,28 @@ void MotionPlanningFrame::allowExternalProgramCommunication(bool enable) { using std::placeholders::_1; plan_subscriber_ = node_->create_subscription( - "/rviz/moveit/plan", 1, + "/rviz/moveit/plan", rclcpp::SystemDefaultsQoS(), [this](const std_msgs::msg::Empty::ConstSharedPtr& msg) { return remotePlanCallback(msg); }); execute_subscriber_ = node_->create_subscription( - "/rviz/moveit/execute", 1, + "/rviz/moveit/execute", rclcpp::SystemDefaultsQoS(), [this](const std_msgs::msg::Empty::ConstSharedPtr& msg) { return remoteExecuteCallback(msg); }); stop_subscriber_ = node_->create_subscription( - "/rviz/moveit/stop", 1, + "/rviz/moveit/stop", rclcpp::SystemDefaultsQoS(), [this](const std_msgs::msg::Empty::ConstSharedPtr& msg) { return remoteStopCallback(msg); }); update_start_state_subscriber_ = node_->create_subscription( - "/rviz/moveit/update_start_state", 1, + "/rviz/moveit/update_start_state", rclcpp::SystemDefaultsQoS(), [this](const std_msgs::msg::Empty::ConstSharedPtr& msg) { return remoteUpdateStartStateCallback(msg); }); update_goal_state_subscriber_ = node_->create_subscription( - "/rviz/moveit/update_goal_state", 1, + "/rviz/moveit/update_goal_state", rclcpp::SystemDefaultsQoS(), [this](const std_msgs::msg::Empty::ConstSharedPtr& msg) { return remoteUpdateGoalStateCallback(msg); }); update_custom_start_state_subscriber_ = node_->create_subscription( - "/rviz/moveit/update_custom_start_state", 1, [this](const moveit_msgs::msg::RobotState::ConstSharedPtr& msg) { + "/rviz/moveit/update_custom_start_state", rclcpp::SystemDefaultsQoS(), + [this](const moveit_msgs::msg::RobotState::ConstSharedPtr& msg) { return remoteUpdateCustomStartStateCallback(msg); }); update_custom_goal_state_subscriber_ = node_->create_subscription( - "/rviz/moveit/update_custom_goal_state", 1, [this](const moveit_msgs::msg::RobotState::ConstSharedPtr& msg) { + "/rviz/moveit/update_custom_goal_state", rclcpp::SystemDefaultsQoS(), + [this](const moveit_msgs::msg::RobotState::ConstSharedPtr& msg) { return remoteUpdateCustomGoalStateCallback(msg); }); } @@ -614,7 +616,7 @@ void MotionPlanningFrame::initFromMoveGroupNS() clear_octomap_service_client_ = node_->create_client(move_group::CLEAR_OCTOMAP_SERVICE_NAME); object_recognition_subscriber_ = node_->create_subscription( - "recognized_object_array", 1, + "recognized_object_array", rclcpp::SystemDefaultsQoS(), [this](const object_recognition_msgs::msg::RecognizedObjectArray::ConstSharedPtr& msg) { return listenDetectedObjects(msg); }); diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp index 2acdc9dceb..c85f2ed3f1 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp +++ b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp @@ -304,7 +304,7 @@ void RobotStateDisplay::changedRobotStateTopic() setStatus(rviz_common::properties::StatusProperty::Warn, "RobotState", "No msg received"); robot_state_subscriber_ = node_->create_subscription( - robot_state_topic_property_->getStdString(), 10, + robot_state_topic_property_->getStdString(), rclcpp::SystemDefaultsQoS(), [this](const moveit_msgs::msg::DisplayRobotState::ConstSharedPtr& state) { return newRobotStateCallback(state); }); } diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp index c0e42a62cd..0c2f7c6990 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp @@ -184,7 +184,7 @@ void TrajectoryVisualization::onInitialize(const rclcpp::Node::SharedPtr& node, } trajectory_topic_sub_ = node_->create_subscription( - trajectory_topic_property_->getStdString(), 2, + trajectory_topic_property_->getStdString(), rclcpp::SystemDefaultsQoS(), [this](const moveit_msgs::msg::DisplayTrajectory::ConstSharedPtr& msg) { return incomingDisplayTrajectory(msg); }); } @@ -294,7 +294,7 @@ void TrajectoryVisualization::changedTrajectoryTopic() if (!trajectory_topic_property_->getStdString().empty() && robot_state_) { trajectory_topic_sub_ = node_->create_subscription( - trajectory_topic_property_->getStdString(), 2, + trajectory_topic_property_->getStdString(), rclcpp::SystemDefaultsQoS(), [this](const moveit_msgs::msg::DisplayTrajectory::ConstSharedPtr& msg) { return incomingDisplayTrajectory(msg); }); diff --git a/moveit_ros/warehouse/src/save_to_warehouse.cpp b/moveit_ros/warehouse/src/save_to_warehouse.cpp index 1f0a479bfe..8a881866d6 100644 --- a/moveit_ros/warehouse/src/save_to_warehouse.cpp +++ b/moveit_ros/warehouse/src/save_to_warehouse.cpp @@ -210,12 +210,14 @@ int main(int argc, char** argv) psm.addUpdateCallback([&](auto&&) { return onSceneUpdate(psm, pss); }); auto mplan_req_sub = node->create_subscription( - "motion_plan_request", 100, + "motion_plan_request", rclcpp::SystemDefaultsQoS(), [&](const moveit_msgs::msg::MotionPlanRequest& msg) { onMotionPlanRequest(msg, psm, pss); }); auto constr_sub = node->create_subscription( - "constraints", 100, [&](const moveit_msgs::msg::Constraints& msg) { onConstraints(msg, cs); }); + "constraints", rclcpp::SystemDefaultsQoS(), + [&](const moveit_msgs::msg::Constraints& msg) { onConstraints(msg, cs); }); auto state_sub = node->create_subscription( - "robot_state", 100, [&](const moveit_msgs::msg::RobotState& msg) { onRobotState(msg, rs); }); + "robot_state", rclcpp::SystemDefaultsQoS(), + [&](const moveit_msgs::msg::RobotState& msg) { onRobotState(msg, rs); }); std::vector topics; psm.getMonitoredTopics(topics);