Merge branch 'humble' into mergify/bp/humble/pr-1813

moveit · Aug 15, 2023 · e3982b7 · e3982b7
2 parents bdaac9b + 9d1a8d5
commit e3982b7
Show file tree

Hide file tree

Showing 16 changed files with 40 additions and 36 deletions.
diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml
@@ -23,9 +23,9 @@ jobs:
             ROS_DISTRO: humble
             IKFAST_TEST: true
             CLANG_TIDY: pedantic
-            # Silent gmock/gtest warnings: https://github.com/ament/googletest/pull/21
+            # Silent gmock/gtest warnings by picking more recent googletest version
             AFTER_BUILD_UPSTREAM_WORKSPACE: |
-              git clone --quiet --branch clalancette/update-to-gtest-1.11 https://github.com/ament/googletest "${BASEDIR}/upstream_ws/src/googletest"
+              git clone --depth 1 --quiet --branch 1.11.9000 https://github.com/ament/googletest "${BASEDIR}/upstream_ws/src/googletest"
               builder_run_build "/opt/ros/${ROS_DISTRO}" "${BASEDIR}/upstream_ws" --packages-select gtest_vendor gmock_vendor
     env:
       CXXFLAGS: >-

diff --git a/moveit_core/kinematic_constraints/src/utils.cpp b/moveit_core/kinematic_constraints/src/utils.cpp
@@ -479,7 +479,7 @@ static bool collectConstraints(const rclcpp::Node::SharedPtr& node, const std::v
     const auto constraint_param = "constraints." + constraint_id;
     if (!node->has_parameter(constraint_param + ".type"))
     {
-      RCLCPP_ERROR(LOGGER, "constraint parameter does not specify its type");
+      RCLCPP_ERROR(LOGGER, "constraint parameter \"%s\" does not specify its type", constraint_param.c_str());
       return false;
     }
     std::string constraint_type;
@@ -529,7 +529,7 @@ bool constructConstraints(const rclcpp::Node::SharedPtr& node, const std::string
     return false;
 
   for (auto& constraint_id : constraint_ids)
-    constraint_id.insert(0, constraints_param);
+    constraint_id.insert(0, constraints_param + std::string("."));
 
   return collectConstraints(node, constraint_ids, constraints);
 }

diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h
@@ -255,7 +255,7 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro
     return world_const_;
   }
 
-  // brief Get the representation of the world
+  /** \brief Get the representation of the world */
   const collision_detection::WorldPtr& getWorldNonConst()
   {
     // we always have a world representation

diff --git a/...ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp b/...ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp
@@ -201,7 +201,8 @@ bool LocalPlannerComponent::initialize()
 
   // Initialize global trajectory listener
   global_solution_subscriber_ = node_->create_subscription<moveit_msgs::msg::MotionPlanResponse>(
-      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
@@ -183,7 +183,7 @@ TEST_F(PoseTrackingFixture, OutgoingMsgTest)
         return;
       };
   auto traj_sub = node_->create_subscription<trajectory_msgs::msg::JointTrajectory>(
-      "/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
@@ -66,7 +66,7 @@ SemanticWorld::SemanticWorld(const rclcpp::Node::SharedPtr& node,
 
 {
   table_subscriber_ = node_handle_->create_subscription<object_recognition_msgs::msg::TableArray>(
-      "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<visualization_msgs::msg::MarkerArray>("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
@@ -51,10 +51,6 @@
 
 namespace planning_scene_monitor
 {
-namespace
-{
-static const auto SUBSCRIPTION_QOS = rclcpp::QoS(25);
-}
 
 CurrentStateMonitorMiddlewareHandle::CurrentStateMonitorMiddlewareHandle(const rclcpp::Node::SharedPtr& node)
   : node_(node)
@@ -70,7 +66,7 @@ void CurrentStateMonitorMiddlewareHandle::createJointStateSubscription(const std
                                                                        JointStateUpdateCallback callback)
 {
   joint_state_subscription_ =
-      node_->create_subscription<sensor_msgs::msg::JointState>(topic, SUBSCRIPTION_QOS, callback);
+      node_->create_subscription<sensor_msgs::msg::JointState>(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
@@ -1090,7 +1090,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, 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());
@@ -1178,15 +1178,16 @@ 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, 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());
   }
 
   if (!planning_scene_world_topic.empty())
   {
     planning_scene_world_subscriber_ = pnode_->create_subscription<moveit_msgs::msg::PlanningSceneWorld>(
-        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());
@@ -1257,7 +1258,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<moveit_msgs::msg::AttachedCollisionObject>(
-          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
@@ -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<rclcpp::Node>(nd_name);
   string_subscriber_ = temp_node->create_subscription<std_msgs::msg::String>(
-      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
@@ -186,7 +186,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, 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
@@ -574,8 +574,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<geometry_msgs::msg::PoseStamped>(topic_name, 1, subscription_callback);
+        auto subscription = node_->create_subscription<geometry_msgs::msg::PoseStamped>(
+            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
@@ -284,7 +284,7 @@ void MotionPlanningDisplay::toggleSelectPlanningGroupSubscription(bool enable)
   if (enable)
   {
     planning_group_sub_ = node_->create_subscription<std_msgs::msg::String>(
-        "/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
@@ -267,26 +267,28 @@ void MotionPlanningFrame::allowExternalProgramCommunication(bool enable)
   {
     using std::placeholders::_1;
     plan_subscriber_ = node_->create_subscription<std_msgs::msg::Empty>(
-        "/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<std_msgs::msg::Empty>(
-        "/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<std_msgs::msg::Empty>(
-        "/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<std_msgs::msg::Empty>(
-        "/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<std_msgs::msg::Empty>(
-        "/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<moveit_msgs::msg::RobotState>(
-        "/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<moveit_msgs::msg::RobotState>(
-        "/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);
         });
   }
@@ -610,7 +612,7 @@ void MotionPlanningFrame::initFromMoveGroupNS()
   clear_octomap_service_client_ = node_->create_client<std_srvs::srv::Empty>(move_group::CLEAR_OCTOMAP_SERVICE_NAME);
 
   object_recognition_subscriber_ = node_->create_subscription<object_recognition_msgs::msg::RecognizedObjectArray>(
-      "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
@@ -304,7 +304,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(), 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
@@ -184,7 +184,7 @@ void TrajectoryVisualization::onInitialize(const rclcpp::Node::SharedPtr& node,
   }
 
   trajectory_topic_sub_ = node_->create_subscription<moveit_msgs::msg::DisplayTrajectory>(
-      trajectory_topic_property_->getStdString(), 2,
+      trajectory_topic_property_->getStdString(), rclcpp::SystemDefaultsQoS(),
       [this](const moveit_msgs::msg::DisplayTrajectory::ConstSharedPtr& msg) { return incomingDisplayTrajectory(msg); });
 }
 
@@ -293,7 +293,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(), 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
@@ -197,12 +197,14 @@ int main(int argc, char** argv)
   psm.addUpdateCallback([&](auto&&) { return onSceneUpdate(psm, pss); });
 
   auto mplan_req_sub = node->create_subscription<moveit_msgs::msg::MotionPlanRequest>(
-      "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<moveit_msgs::msg::Constraints>(
-      "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<moveit_msgs::msg::RobotState>(
-      "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<std::string> topics;
   psm.getMonitoredTopics(topics);