Add service introspection

nav2_amcl/include/nav2_amcl/amcl_node.hpp

@@ -47,6 +47,7 @@
 #include "pluginlib/class_loader.hpp"
 #include "rclcpp/node_options.hpp"
 #include "sensor_msgs/msg/laser_scan.hpp"
+#include "nav2_util/service_server.hpp"
 #include "std_srvs/srv/empty.hpp"
 #include "tf2_ros/message_filter.h"
 #include "tf2_ros/transform_broadcaster.h"
@@ -206,7 +207,8 @@ class AmclNode : public nav2_util::LifecycleNode
    * @brief Initialize state services
    */
   void initServices();
-  rclcpp::Service<std_srvs::srv::Empty>::SharedPtr global_loc_srv_;
+  nav2_util::ServiceServer<std_srvs::srv::Empty,
+    std::shared_ptr<nav2_util::LifecycleNode>>::SharedPtr global_loc_srv_;
   /*
    * @brief Service callback for a global relocalization request
    */
@@ -216,7 +218,8 @@ class AmclNode : public nav2_util::LifecycleNode
     std::shared_ptr<std_srvs::srv::Empty::Response> response);
 
   // service server for providing an initial pose guess
-  rclcpp::Service<nav2_msgs::srv::SetInitialPose>::SharedPtr initial_guess_srv_;
+  nav2_util::ServiceServer<nav2_msgs::srv::SetInitialPose,
+    std::shared_ptr<nav2_util::LifecycleNode>>::SharedPtr initial_guess_srv_;
   /*
    * @brief Service callback for an initial pose guess request
    */
@@ -226,7 +229,8 @@ class AmclNode : public nav2_util::LifecycleNode
     std::shared_ptr<nav2_msgs::srv::SetInitialPose::Response> response);
 
   // Let amcl update samples without requiring motion
-  rclcpp::Service<std_srvs::srv::Empty>::SharedPtr nomotion_update_srv_;
+  nav2_util::ServiceServer<std_srvs::srv::Empty,
+    std::shared_ptr<nav2_util::LifecycleNode>>::SharedPtr nomotion_update_srv_;
   /*
    * @brief Request an AMCL update even though the robot hasn't moved
    */

nav2_amcl/src/amcl_node.cpp

@@ -1564,18 +1564,21 @@ AmclNode::initPubSub()
 void
 AmclNode::initServices()
 {
-  global_loc_srv_ = create_service<std_srvs::srv::Empty>(
-    "reinitialize_global_localization",
+  global_loc_srv_ = std::make_shared<nav2_util::ServiceServer<std_srvs::srv::Empty,
+      std::shared_ptr<nav2_util::LifecycleNode>>>(
+    "reinitialize_global_localization", shared_from_this(),
     std::bind(&AmclNode::globalLocalizationCallback, this, std::placeholders::_1,
       std::placeholders::_2, std::placeholders::_3));
 
-  initial_guess_srv_ = create_service<nav2_msgs::srv::SetInitialPose>(
-    "set_initial_pose",
+  initial_guess_srv_ = std::make_shared<nav2_util::ServiceServer<nav2_msgs::srv::SetInitialPose,
+      std::shared_ptr<nav2_util::LifecycleNode>>>(
+    "set_initial_pose", shared_from_this(),
     std::bind(&AmclNode::initialPoseReceivedSrv, this, std::placeholders::_1, std::placeholders::_2,
       std::placeholders::_3));
 
-  nomotion_update_srv_ = create_service<std_srvs::srv::Empty>(
-    "request_nomotion_update",
+  nomotion_update_srv_ = std::make_shared<nav2_util::ServiceServer<std_srvs::srv::Empty,
+      std::shared_ptr<nav2_util::LifecycleNode>>>(
+    "request_nomotion_update", shared_from_this(),
     std::bind(&AmclNode::nomotionUpdateCallback, this, std::placeholders::_1, std::placeholders::_2,
       std::placeholders::_3));
 }

nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp

@@ -23,6 +23,7 @@
 #include "nav2_util/node_utils.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_behavior_tree/bt_utils.hpp"
+#include "nav2_util/service_client.hpp"
 
 namespace nav2_behavior_tree
 {
@@ -109,16 +110,8 @@
     if (service_new != service_name_ || !service_client_) {
       service_name_ = service_new;
       node_ = config().blackboard->template get<rclcpp::Node::SharedPtr>("node");
-      callback_group_ = node_->create_callback_group(
-        rclcpp::CallbackGroupType::MutuallyExclusive,
-        false);
-      callback_group_executor_.add_callback_group(callback_group_,
-          node_->get_node_base_interface());
-
-      service_client_ = node_->create_client<ServiceT>(
-        service_name_,
-        rclcpp::SystemDefaultsQoS(),
-        callback_group_);
+      service_client_ = std::make_shared<nav2_util::ServiceClient<ServiceT>>(
+      service_name_, node_, true);
     }
   }
 
@@ -173,7 +166,7 @@
         return BT::NodeStatus::FAILURE;
       }
 
-      future_result_ = service_client_->async_send_request(request_).share();
+      future_result_ = service_client_->async_call(request_);
       sent_time_ = node_->now();
       request_sent_ = true;
     }
@@ -220,15 +213,15 @@
     if (remaining > std::chrono::milliseconds(0)) {
       auto timeout = remaining > max_timeout_ ? max_timeout_ : remaining;
 
-      rclcpp::FutureReturnCode rc;
-      rc = callback_group_executor_.spin_until_future_complete(future_result_, timeout);
-      if (rc == rclcpp::FutureReturnCode::SUCCESS) {
+      std::future_status future;
+      future = future_result_.wait_for(timeout);
+      if(future == std::future_status::ready) {
         request_sent_ = false;
         BT::NodeStatus status = on_completion(future_result_.get());
         return status;
       }
 
-      if (rc == rclcpp::FutureReturnCode::TIMEOUT) {
+      if (future == std::future_status::timeout) {
         on_wait_for_result();
         elapsed = (node_->now() - sent_time_).template to_chrono<std::chrono::milliseconds>();
         if (elapsed < server_timeout_) {
@@ -265,13 +258,11 @@
   }
 
   std::string service_name_, service_node_name_;
-  typename std::shared_ptr<rclcpp::Client<ServiceT>> service_client_;
+  typename nav2_util::ServiceClient<ServiceT>::SharedPtr service_client_;
   std::shared_ptr<typename ServiceT::Request> request_;
 
   // The node that will be used for any ROS operations
   rclcpp::Node::SharedPtr node_;
-  rclcpp::CallbackGroup::SharedPtr callback_group_;
-  rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
 
   // The timeout value while to use in the tick loop while waiting for
   // a result from the server

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp

@@ -22,6 +22,7 @@
 #include "behaviortree_cpp/condition_node.h"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "nav2_msgs/srv/is_path_valid.hpp"
+#include "nav2_util/service_client.hpp"
 
 namespace nav2_behavior_tree
 {
@@ -73,7 +74,7 @@ class IsPathValidCondition : public BT::ConditionNode
 
 private:
   rclcpp::Node::SharedPtr node_;
-  rclcpp::Client<nav2_msgs::srv::IsPathValid>::SharedPtr client_;
+  nav2_util::ServiceClient<nav2_msgs::srv::IsPathValid>::SharedPtr client_;
   // The timeout value while waiting for a response from the
   // is path valid service
   std::chrono::milliseconds server_timeout_;

nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp

@@ -27,7 +27,8 @@ IsPathValidCondition::IsPathValidCondition(
   max_cost_(253), consider_unknown_as_obstacle_(false)
 {
   node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
-  client_ = node_->create_client<nav2_msgs::srv::IsPathValid>("is_path_valid");
+  client_ = std::make_shared<nav2_util::ServiceClient<nav2_msgs::srv::IsPathValid>>("is_path_valid",
+      node_, false);
 
   server_timeout_ = config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
 }
@@ -53,14 +54,9 @@ BT::NodeStatus IsPathValidCondition::tick()
   request->path = path;
   request->max_cost = max_cost_;
   request->consider_unknown_as_obstacle = consider_unknown_as_obstacle_;
-  auto result = client_->async_send_request(request);
-
-  if (rclcpp::spin_until_future_complete(node_, result, server_timeout_) ==
-    rclcpp::FutureReturnCode::SUCCESS)
-  {
-    if (result.get()->is_valid) {
-      return BT::NodeStatus::SUCCESS;
-    }
+  auto response = client_->invoke(request, server_timeout_);
+  if (response->is_valid) {
+    return BT::NodeStatus::SUCCESS;
   }
   return BT::NodeStatus::FAILURE;
 }

nav2_bringup/params/nav2_params.yaml

@@ -30,6 +30,7 @@ amcl:
     sigma_hit: 0.2
     tf_broadcast: true
     transform_tolerance: 1.0
+    service_introspection_mode: "disabled"
     update_min_a: 0.2
     update_min_d: 0.25
     z_hit: 0.5
@@ -48,6 +49,7 @@ bt_navigator:
     default_server_timeout: 20
     wait_for_service_timeout: 1000
     action_server_result_timeout: 900.0
+    service_introspection_mode: "disabled"
     navigators: ["navigate_to_pose", "navigate_through_poses"]
     navigate_to_pose:
       plugin: "nav2_bt_navigator::NavigateToPoseNavigator"
@@ -241,6 +243,7 @@ local_costmap:
         plugin: "nav2_costmap_2d::StaticLayer"
         map_subscribe_transient_local: True
       always_send_full_costmap: True
+      service_introspection_mode: "disabled"
 
 global_costmap:
   global_costmap:
@@ -281,26 +284,30 @@ global_costmap:
         cost_scaling_factor: 3.0
         inflation_radius: 0.7
       always_send_full_costmap: True
+      service_introspection_mode: "disabled"
 
 # The yaml_filename does not need to be specified since it going to be set by defaults in launch.
 # If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py
 # file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used.
-# map_server:
-#   ros__parameters:
-#     yaml_filename: ""
+map_server:
+  ros__parameters:
+    # yaml_filename: ""
+    service_introspection_mode: "disabled"
 
 map_saver:
   ros__parameters:
     save_map_timeout: 5.0
     free_thresh_default: 0.25
     occupied_thresh_default: 0.65
     map_subscribe_transient_local: True
+    service_introspection_mode: "disabled"
 
 planner_server:
   ros__parameters:
     expected_planner_frequency: 20.0
     planner_plugins: ["GridBased"]
     costmap_update_timeout: 1.0
+    service_introspection_mode: "disabled"
     GridBased:
       plugin: "nav2_navfn_planner::NavfnPlanner"
       tolerance: 0.5
@@ -353,6 +360,7 @@ waypoint_follower:
   ros__parameters:
     loop_rate: 20
     stop_on_failure: false
+    service_introspection_mode: "disabled"
     action_server_result_timeout: 900.0
     waypoint_task_executor_plugin: "wait_at_waypoint"
     wait_at_waypoint:
@@ -418,6 +426,7 @@ docking_server:
     fixed_frame: "odom"
     dock_backwards: false
     dock_prestaging_tolerance: 0.5
+    service_introspection_mode: "disabled"
 
     # Types of docks
     dock_plugins: ['simple_charging_dock']

nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp

@@ -25,6 +25,7 @@
 #include "nav2_msgs/srv/clear_entire_costmap.hpp"
 #include "nav2_costmap_2d/costmap_layer.hpp"
 #include "nav2_util/lifecycle_node.hpp"
+#include "nav2_util/service_server.hpp"
 
 namespace nav2_costmap_2d
 {
@@ -41,7 +42,8 @@ class ClearCostmapService
   /**
    * @brief A constructor
    */
-  ClearCostmapService(const nav2_util::LifecycleNode::WeakPtr & parent, Costmap2DROS & costmap);
+  ClearCostmapService(
+    const nav2_util::LifecycleNode::WeakPtr & parent, Costmap2DROS & costmap);
 
   /**
    * @brief A constructor
@@ -74,7 +76,8 @@ class ClearCostmapService
   unsigned char reset_value_;
 
   // Server for clearing the costmap
-  rclcpp::Service<nav2_msgs::srv::ClearCostmapExceptRegion>::SharedPtr clear_except_service_;
+  nav2_util::ServiceServer<nav2_msgs::srv::ClearCostmapExceptRegion,
+    std::shared_ptr<rclcpp_lifecycle::LifecycleNode>>::SharedPtr clear_except_service_;
   /**
    * @brief Callback to clear costmap except in a given region
    */
@@ -83,7 +86,8 @@ class ClearCostmapService
     const std::shared_ptr<nav2_msgs::srv::ClearCostmapExceptRegion::Request> request,
     const std::shared_ptr<nav2_msgs::srv::ClearCostmapExceptRegion::Response> response);
 
-  rclcpp::Service<nav2_msgs::srv::ClearCostmapAroundRobot>::SharedPtr clear_around_service_;
+  nav2_util::ServiceServer<nav2_msgs::srv::ClearCostmapAroundRobot,
+    std::shared_ptr<rclcpp_lifecycle::LifecycleNode>>::SharedPtr clear_around_service_;
   /**
    * @brief Callback to clear costmap in a given region
    */
@@ -92,7 +96,8 @@ class ClearCostmapService
     const std::shared_ptr<nav2_msgs::srv::ClearCostmapAroundRobot::Request> request,
     const std::shared_ptr<nav2_msgs::srv::ClearCostmapAroundRobot::Response> response);
 
-  rclcpp::Service<nav2_msgs::srv::ClearEntireCostmap>::SharedPtr clear_entire_service_;
+  nav2_util::ServiceServer<nav2_msgs::srv::ClearEntireCostmap,
+    std::shared_ptr<rclcpp_lifecycle::LifecycleNode>>::SharedPtr clear_entire_service_;
   /**
    * @brief Callback to clear costmap
    */

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp

@@ -54,6 +54,7 @@
 #include "nav2_util/lifecycle_node.hpp"
 #include "tf2/LinearMath/Quaternion.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "nav2_util/service_server.hpp"
 
 namespace nav2_costmap_2d
 {
@@ -180,7 +181,9 @@ class Costmap2DPublisher
     costmap_raw_update_pub_;
 
   // Service for getting the costmaps
-  rclcpp::Service<nav2_msgs::srv::GetCostmap>::SharedPtr costmap_service_;
+  nav2_util::ServiceServer<nav2_msgs::srv::GetCostmap,
+    std::shared_ptr<rclcpp_lifecycle::LifecycleNode>>::SharedPtr
+    costmap_service_;
 
   float grid_resolution_;
   unsigned int grid_width_, grid_height_;

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp

@@ -60,6 +60,7 @@
 #include "tf2_ros/transform_listener.h"
 #include "tf2/time.hpp"
 #include "tf2/transform_datatypes.hpp"
+#include "nav2_util/service_server.hpp"
 
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wpedantic"
@@ -414,7 +415,8 @@ class Costmap2DROS : public nav2_util::LifecycleNode
   std::vector<geometry_msgs::msg::Point> padded_footprint_;
 
   // Services
-  rclcpp::Service<nav2_msgs::srv::GetCosts>::SharedPtr get_cost_service_;
+  nav2_util::ServiceServer<nav2_msgs::srv::GetCosts,
+    std::shared_ptr<nav2_util::LifecycleNode>>::SharedPtr get_cost_service_;
   std::unique_ptr<ClearCostmapService> clear_costmap_service_;
 
   // Dynamic parameters handler

nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp

@@ -49,6 +49,7 @@
 #include "std_srvs/srv/set_bool.hpp"
 #include "nav2_costmap_2d/layer.hpp"
 #include "nav_msgs/msg/occupancy_grid.hpp"
+#include "nav2_util/service_server.hpp"
 
 namespace nav2_costmap_2d
 {
@@ -241,7 +242,8 @@ class CostmapFilter : public Layer
   /**
    * @brief: A service to enable/disable costmap filter
    */
-  rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr enable_service_;
+  nav2_util::ServiceServer<std_srvs::srv::SetBool,
+    std::shared_ptr<rclcpp_lifecycle::LifecycleNode>>::SharedPtr enable_service_;
 
 private:
   /**

nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp

@@ -83,11 +83,12 @@ void CostmapFilter::onInitialize()
     transform_tolerance_ = tf2::durationFromSec(transform_tolerance);
 
     // Costmap Filter enabling service
-    enable_service_ = node->create_service<std_srvs::srv::SetBool>(
+    enable_service_ = std::make_shared<nav2_util::ServiceServer<std_srvs::srv::SetBool,
+        std::shared_ptr<rclcpp_lifecycle::LifecycleNode>>>(
       name_ + "/toggle_filter",
-      std::bind(
-        &CostmapFilter::enableCallback, this,
-        std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+      node,
+      std::bind(&CostmapFilter::enableCallback, this, std::placeholders::_1,
+        std::placeholders::_2, std::placeholders::_3));
   } catch (const std::exception & ex) {
     RCLCPP_ERROR(logger_, "Parameter problem: %s", ex.what());
     throw ex;