From 6d6f34f730a937192d1508721acee5b2e5682e6b Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Sun, 23 Feb 2025 18:44:43 +0000 Subject: [PATCH 01/19] Add service introspection for client and server side, more tests to go Signed-off-by: mini-1235 --- nav2_amcl/include/nav2_amcl/amcl_node.hpp | 11 ++- nav2_amcl/src/amcl_node.cpp | 21 +++-- .../nav2_behavior_tree/bt_service_node.hpp | 6 +- .../condition/is_path_valid_condition.hpp | 4 +- .../condition/is_path_valid_condition.cpp | 20 ++--- nav2_bringup/params/nav2_params.yaml | 15 +++- .../nav2_costmap_2d/clear_costmap_service.hpp | 14 +++- .../nav2_costmap_2d/costmap_2d_publisher.hpp | 8 +- .../nav2_costmap_2d/costmap_2d_ros.hpp | 5 +- .../costmap_filters/costmap_filter.hpp | 4 +- .../costmap_filters/costmap_filter.cpp | 13 ++- nav2_costmap_2d/src/clear_costmap_service.cpp | 23 ++++-- nav2_costmap_2d/src/costmap_2d_publisher.cpp | 16 ++-- nav2_costmap_2d/src/costmap_2d_ros.cpp | 21 +++-- .../include/opennav_docking/dock_database.hpp | 6 +- .../opennav_docking/src/dock_database.cpp | 14 +++- .../lifecycle_manager.hpp | 13 ++- .../src/lifecycle_manager.cpp | 38 ++++++--- .../src/lifecycle_manager_client.cpp | 7 +- .../include/nav2_map_server/map_saver.hpp | 4 +- .../include/nav2_map_server/map_server.hpp | 8 +- nav2_map_server/src/map_saver/map_saver.cpp | 8 +- nav2_map_server/src/map_server/map_server.cpp | 12 ++- .../include/nav2_planner/planner_server.hpp | 7 +- nav2_planner/src/planner_server.cpp | 15 ++-- .../nav2_rviz_plugins/costmap_cost_tool.hpp | 8 +- nav2_rviz_plugins/src/costmap_cost_tool.cpp | 44 ++++------ .../nav2_util/lifecycle_service_client.hpp | 7 +- .../include/nav2_util/service_client.hpp | 48 +++++++++++ .../include/nav2_util/service_server.hpp | 81 +++++++++++++++++++ nav2_util/src/lifecycle_service_client.cpp | 15 ++-- nav2_util/test/test_service_client.cpp | 13 ++- .../waypoint_follower.hpp | 1 + .../src/waypoint_follower.cpp | 3 + 34 files changed, 398 insertions(+), 135 deletions(-) create mode 100644 nav2_util/include/nav2_util/service_server.hpp diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 7928f5e9545..e4e802fc927 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/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::SharedPtr global_loc_srv_; + std::shared_ptr>> global_loc_srv_; /* * @brief Service callback for a global relocalization request */ @@ -216,7 +218,8 @@ class AmclNode : public nav2_util::LifecycleNode std::shared_ptr response); // service server for providing an initial pose guess - rclcpp::Service::SharedPtr initial_guess_srv_; + std::shared_ptr>> 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 response); // Let amcl update samples without requiring motion - rclcpp::Service::SharedPtr nomotion_update_srv_; + std::shared_ptr>> nomotion_update_srv_; /* * @brief Request an AMCL update even though the robot hasn't moved */ @@ -387,6 +391,7 @@ class AmclNode : public nav2_util::LifecycleNode double sigma_hit_; bool tf_broadcast_; tf2::Duration transform_tolerance_; + std::string service_introspection_mode_; double a_thresh_; double d_thresh_; double z_hit_; diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 7ae2bb61048..093089a7a2b 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -192,6 +192,11 @@ AmclNode::AmclNode(const rclcpp::NodeOptions & options) "Time with which to post-date the transform that is published, to indicate that this transform " "is valid into the future"); + add_parameter( + "service_introspection_mode", rclcpp::ParameterValue(std::string("disabled")), + "Set this to disabled for no introspection, metadata for Only metadata without any user data contents, " + "and contents for User data contents with metadata"); + add_parameter( "update_min_a", rclcpp::ParameterValue(0.2), "Rotational movement required before performing a filter update"); @@ -1097,6 +1102,7 @@ AmclNode::initParameters() get_parameter("sigma_hit", sigma_hit_); get_parameter("tf_broadcast", tf_broadcast_); get_parameter("transform_tolerance", tmp_tol); + get_parameter("service_introspection_mode", service_introspection_mode_); get_parameter("update_min_a", a_thresh_); get_parameter("update_min_d", d_thresh_); get_parameter("z_hit", z_hit_); @@ -1564,18 +1570,21 @@ AmclNode::initPubSub() void AmclNode::initServices() { - global_loc_srv_ = create_service( - "reinitialize_global_localization", + global_loc_srv_ = std::make_shared>>( + "reinitialize_global_localization", service_introspection_mode_, shared_from_this(), std::bind(&AmclNode::globalLocalizationCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - initial_guess_srv_ = create_service( - "set_initial_pose", + initial_guess_srv_ = std::make_shared>>( + "set_initial_pose", service_introspection_mode_, shared_from_this(), std::bind(&AmclNode::initialPoseReceivedSrv, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - nomotion_update_srv_ = create_service( - "request_nomotion_update", + nomotion_update_srv_ = std::make_shared>>( + "request_nomotion_update", service_introspection_mode_, shared_from_this(), std::bind(&AmclNode::nomotionUpdateCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index 18d6ea851bb..eedf5a57b84 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/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 { @@ -173,7 +174,7 @@ class BtServiceNode : public BT::ActionNodeBase return BT::NodeStatus::FAILURE; } - future_result_ = service_client_->async_send_request(request_).share(); + future_result_ = service_client_->invoke_shared(request_); sent_time_ = node_->now(); request_sent_ = true; } @@ -265,8 +266,9 @@ class BtServiceNode : public BT::ActionNodeBase } std::string service_name_, service_node_name_; - typename std::shared_ptr> service_client_; + std::shared_ptr> service_client_; std::shared_ptr request_; + std::string service_introspection_mode_; // The node that will be used for any ROS operations rclcpp::Node::SharedPtr node_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp index 5acc938a28f..04e04a298b9 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp +++ b/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,12 +74,13 @@ class IsPathValidCondition : public BT::ConditionNode private: rclcpp::Node::SharedPtr node_; - rclcpp::Client::SharedPtr client_; + std::shared_ptr> client_; // The timeout value while waiting for a response from the // is path valid service std::chrono::milliseconds server_timeout_; unsigned int max_cost_; bool consider_unknown_as_obstacle_; + std::string service_introspection_mode_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp index 8caed0c3971..575bded7622 100644 --- a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp @@ -16,6 +16,7 @@ #include #include #include +#include "nav2_util/node_utils.hpp" namespace nav2_behavior_tree { @@ -27,7 +28,13 @@ IsPathValidCondition::IsPathValidCondition( max_cost_(253), consider_unknown_as_obstacle_(false) { node_ = config().blackboard->get("node"); - client_ = node_->create_client("is_path_valid"); + nav2_util::declare_parameter_if_not_declared( + node_, "service_introspection_mode", + rclcpp::ParameterValue(std::string("disabled"))); + node_->get_parameter("service_introspection_mode", service_introspection_mode_); + client_ = std::make_shared>("is_path_valid", + service_introspection_mode_, + node_); server_timeout_ = config().blackboard->template get("server_timeout"); } @@ -53,14 +60,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; } diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 04021ac6d52..535cb89b0e1 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/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" # disabled, metadata, contents 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" # disabled, metadata, contents 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,13 +284,15 @@ global_costmap: cost_scaling_factor: 3.0 inflation_radius: 0.7 always_send_full_costmap: True + service_introspection_mode: "disabled" # disabled, metadata, contents # 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 # disabled, metadata, contents map_saver: ros__parameters: @@ -295,12 +300,14 @@ map_saver: free_thresh_default: 0.25 occupied_thresh_default: 0.65 map_subscribe_transient_local: True + service_introspection_mode: "disabled" # disabled, metadata, contents planner_server: ros__parameters: expected_planner_frequency: 20.0 planner_plugins: ["GridBased"] costmap_update_timeout: 1.0 + service_introspection_mode: "disabled" # disabled, metadata, contents 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" # disabled, metadata, contents 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" # disabled, metadata, contents # Types of docks dock_plugins: ['simple_charging_dock'] diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp index 844902dd100..61d8b082381 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp +++ b/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,9 @@ class ClearCostmapService /** * @brief A constructor */ - ClearCostmapService(const nav2_util::LifecycleNode::WeakPtr & parent, Costmap2DROS & costmap); + ClearCostmapService( + const nav2_util::LifecycleNode::WeakPtr & parent, Costmap2DROS & costmap, + std::string service_introspection_mode); /** * @brief A constructor @@ -74,7 +77,8 @@ class ClearCostmapService unsigned char reset_value_; // Server for clearing the costmap - rclcpp::Service::SharedPtr clear_except_service_; + std::shared_ptr>> clear_except_service_; /** * @brief Callback to clear costmap except in a given region */ @@ -83,7 +87,8 @@ class ClearCostmapService const std::shared_ptr request, const std::shared_ptr response); - rclcpp::Service::SharedPtr clear_around_service_; + std::shared_ptr>> clear_around_service_; /** * @brief Callback to clear costmap in a given region */ @@ -92,7 +97,8 @@ class ClearCostmapService const std::shared_ptr request, const std::shared_ptr response); - rclcpp::Service::SharedPtr clear_entire_service_; + std::shared_ptr>> clear_entire_service_; /** * @brief Callback to clear costmap */ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp index 41041c44b6e..d4eee45fb57 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp +++ b/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 { @@ -73,7 +74,8 @@ class Costmap2DPublisher std::string global_frame, std::string topic_name, bool always_send_full_costmap = false, - double map_vis_z = 0.0); + double map_vis_z = 0.0, + std::string service_introspection_mode = "disabled"); /** * @brief Destructor @@ -180,7 +182,9 @@ class Costmap2DPublisher costmap_raw_update_pub_; // Service for getting the costmaps - rclcpp::Service::SharedPtr costmap_service_; + std::shared_ptr>> + costmap_service_; float grid_resolution_; unsigned int grid_width_, grid_height_; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp index 5fa4c1cacc2..5a57df38d99 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp +++ b/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" @@ -410,11 +411,13 @@ class Costmap2DROS : public nav2_util::LifecycleNode // Derived parameters bool use_radius_{false}; + std::string service_introspection_mode_{"disabled"}; std::vector unpadded_footprint_; std::vector padded_footprint_; // Services - rclcpp::Service::SharedPtr get_cost_service_; + std::shared_ptr>> get_cost_service_; std::unique_ptr clear_costmap_service_; // Dynamic parameters handler diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp index d1128b5ccc6..19b4e4d23cd 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp +++ b/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::SharedPtr enable_service_; + std::shared_ptr>> enable_service_; private: /** diff --git a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp index f05726a6b32..6647aaf9f22 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp @@ -74,6 +74,7 @@ void CostmapFilter::onInitialize() declareParameter("enabled", rclcpp::ParameterValue(true)); declareParameter("filter_info_topic", rclcpp::PARAMETER_STRING); declareParameter("transform_tolerance", rclcpp::ParameterValue(0.1)); + declareParameter("service_introspection_mode", rclcpp::ParameterValue("disabled")); // Get parameters node->get_parameter(name_ + "." + "enabled", enabled_); @@ -81,13 +82,17 @@ void CostmapFilter::onInitialize() double transform_tolerance {}; node->get_parameter(name_ + "." + "transform_tolerance", transform_tolerance); transform_tolerance_ = tf2::durationFromSec(transform_tolerance); + std::string service_introspection_mode_ = + node->get_parameter(name_ + "." + "service_introspection_mode").as_string(); // Costmap Filter enabling service - enable_service_ = node->create_service( + enable_service_ = std::make_shared>>( name_ + "/toggle_filter", - std::bind( - &CostmapFilter::enableCallback, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + service_introspection_mode_, + node->shared_from_this(), + 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; diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp index e11aca2ce5a..e1882dd71e9 100644 --- a/nav2_costmap_2d/src/clear_costmap_service.cpp +++ b/nav2_costmap_2d/src/clear_costmap_service.cpp @@ -33,27 +33,36 @@ using ClearEntirely = nav2_msgs::srv::ClearEntireCostmap; ClearCostmapService::ClearCostmapService( const nav2_util::LifecycleNode::WeakPtr & parent, - Costmap2DROS & costmap) + Costmap2DROS & costmap, std::string service_introspection_mode) : costmap_(costmap) { auto node = parent.lock(); logger_ = node->get_logger(); reset_value_ = costmap_.getCostmap()->getDefaultValue(); - clear_except_service_ = node->create_service( - "clear_except_" + costmap_.getName(), + clear_except_service_ = std::make_shared>>( + std::string("clear_except_") + costmap_.getName(), + service_introspection_mode, + node->shared_from_this(), std::bind( &ClearCostmapService::clearExceptRegionCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - clear_around_service_ = node->create_service( - "clear_around_" + costmap.getName(), + clear_around_service_ = std::make_shared>>( + std::string("clear_around_") + costmap_.getName(), + service_introspection_mode, + node->shared_from_this(), std::bind( &ClearCostmapService::clearAroundRobotCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - clear_entire_service_ = node->create_service( - "clear_entirely_" + costmap_.getName(), + clear_entire_service_ = std::make_shared>>( + std::string("clear_entirely_") + costmap_.getName(), + service_introspection_mode, + node->shared_from_this(), std::bind( &ClearCostmapService::clearEntireCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp index 484ceb14225..72f5aa04215 100644 --- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp @@ -55,7 +55,8 @@ Costmap2DPublisher::Costmap2DPublisher( std::string global_frame, std::string topic_name, bool always_send_full_costmap, - double map_vis_z) + double map_vis_z, + std::string service_introspection_mode) : costmap_(costmap), global_frame_(global_frame), topic_name_(topic_name), @@ -82,11 +83,14 @@ Costmap2DPublisher::Costmap2DPublisher( topic_name + "_raw_updates", custom_qos); // Create a service that will use the callback function to handle requests. - costmap_service_ = node->create_service( - "get_" + topic_name, std::bind( - &Costmap2DPublisher::costmap_service_callback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); + costmap_service_ = std::make_shared>>( + std::string("get_") + topic_name, + service_introspection_mode, + node->shared_from_this(), + std::bind( + &Costmap2DPublisher::costmap_service_callback, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); if (cost_translation_table_ == NULL) { cost_translation_table_ = new char[256]; diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 98f5cbbef0c..1c83f7bc44f 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -125,6 +125,7 @@ void Costmap2DROS::init() declare_parameter("unknown_cost_value", rclcpp::ParameterValue(static_cast(0xff))); declare_parameter("update_frequency", rclcpp::ParameterValue(5.0)); declare_parameter("use_maximum", rclcpp::ParameterValue(false)); + declare_parameter("service_introspection_mode", rclcpp::ParameterValue("disabled")); } Costmap2DROS::~Costmap2DROS() @@ -222,7 +223,7 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) costmap_publisher_ = std::make_unique( shared_from_this(), layered_costmap_->getCostmap(), global_frame_, - "costmap", always_send_full_costmap_, map_vis_z_); + "costmap", always_send_full_costmap_, map_vis_z_, service_introspection_mode_); auto layers = layered_costmap_->getPlugins(); @@ -233,7 +234,7 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) std::make_unique( shared_from_this(), costmap_layer.get(), global_frame_, - layer->getName(), always_send_full_costmap_, map_vis_z_) + layer->getName(), always_send_full_costmap_, map_vis_z_, service_introspection_mode_) ); } } @@ -248,14 +249,17 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) } // Service to get the cost at a point - get_cost_service_ = create_service( - "get_cost_" + getName(), - std::bind( - &Costmap2DROS::getCostsCallback, this, std::placeholders::_1, std::placeholders::_2, + get_cost_service_ = std::make_shared>>( + std::string("get_cost_") + get_name(), + service_introspection_mode_, + shared_from_this(), + std::bind(&Costmap2DROS::getCostsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); // Add cleaning service - clear_costmap_service_ = std::make_unique(shared_from_this(), *this); + clear_costmap_service_ = std::make_unique(shared_from_this(), *this, + service_introspection_mode_); executor_ = std::make_shared(); executor_->add_callback_group(callback_group_, get_node_base_interface()); @@ -360,7 +364,7 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up"); executor_thread_.reset(); - + get_cost_service_.reset(); costmap_publisher_.reset(); clear_costmap_service_.reset(); @@ -410,6 +414,7 @@ Costmap2DROS::getParameters() get_parameter("width", map_width_meters_); get_parameter("plugins", plugin_names_); get_parameter("filters", filter_names_); + get_parameter("service_introspection_mode", service_introspection_mode_); auto node = shared_from_this(); diff --git a/nav2_docking/opennav_docking/include/opennav_docking/dock_database.hpp b/nav2_docking/opennav_docking/include/opennav_docking/dock_database.hpp index e6bcf96eac0..67a27106c43 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/dock_database.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/dock_database.hpp @@ -23,7 +23,7 @@ #include "rclcpp/rclcpp.hpp" #include "pluginlib/class_loader.hpp" #include "pluginlib/class_list_macros.hpp" - +#include "nav2_util/service_server.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_util/node_utils.hpp" #include "nav2_util/simple_action_server.hpp" @@ -124,6 +124,7 @@ class DockDatabase * @param response Service response */ void reloadDbCb( + const std::shared_ptr request_header, const std::shared_ptr request, std::shared_ptr response); @@ -132,7 +133,8 @@ class DockDatabase DockPluginMap dock_plugins_; DockMap dock_instances_; pluginlib::ClassLoader dock_loader_; - rclcpp::Service::SharedPtr reload_db_service_; + std::shared_ptr>> reload_db_service_; }; } // namespace opennav_docking diff --git a/nav2_docking/opennav_docking/src/dock_database.cpp b/nav2_docking/opennav_docking/src/dock_database.cpp index b6a1f888817..0d9b0e104c1 100644 --- a/nav2_docking/opennav_docking/src/dock_database.cpp +++ b/nav2_docking/opennav_docking/src/dock_database.cpp @@ -26,6 +26,7 @@ DockDatabase::~DockDatabase() { dock_instances_.clear(); dock_plugins_.clear(); + reload_db_service_.reset(); } bool DockDatabase::initialize( @@ -54,11 +55,19 @@ bool DockDatabase::initialize( "Docking Server has %u dock types and %u dock instances available.", this->plugin_size(), this->instance_size()); - reload_db_service_ = node->create_service( + // declare the service to reload the database + node->declare_parameter("service_introspection_mode", "disabled"); + std::string service_introspection_mode_ = + node->get_parameter("service_introspection_mode").as_string(); + + reload_db_service_ = std::make_shared>>( "~/reload_database", + service_introspection_mode_, + node->shared_from_this(), std::bind( &DockDatabase::reloadDbCb, this, - std::placeholders::_1, std::placeholders::_2)); + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); return true; } @@ -80,6 +89,7 @@ void DockDatabase::deactivate() } void DockDatabase::reloadDbCb( + const std::shared_ptr/*request_header*/, const std::shared_ptr request, std::shared_ptr response) { diff --git a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp index a42350d5c06..11b7b9278a1 100644 --- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp +++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp @@ -25,6 +25,7 @@ #include "nav2_util/lifecycle_service_client.hpp" #include "nav2_util/node_thread.hpp" +#include "nav2_util/service_server.hpp" #include "rclcpp/rclcpp.hpp" #include "std_srvs/srv/empty.hpp" #include "nav2_msgs/srv/manage_lifecycle_nodes.hpp" @@ -74,8 +75,8 @@ class LifecycleManager : public rclcpp::Node std::unique_ptr service_thread_; // The services provided by this node - rclcpp::Service::SharedPtr manager_srv_; - rclcpp::Service::SharedPtr is_active_srv_; + std::shared_ptr> manager_srv_; + std::shared_ptr> is_active_srv_; /** * @brief Lifecycle node manager callback function * @param request_header Header of the service request @@ -148,6 +149,12 @@ class LifecycleManager : public rclcpp::Node */ void createLifecycleServiceClients(); + // Support function for creating service servers + /** + * @brief Support function for creating service servers + */ + void createLifecycleServiceServers(); + // Support functions for shutdown /** * @brief Support function for shutdown @@ -256,6 +263,8 @@ class LifecycleManager : public rclcpp::Node rclcpp::Time bond_respawn_start_time_{0}; rclcpp::Duration bond_respawn_max_duration_{10s}; + + std::string service_introspection_mode_; }; } // namespace nav2_lifecycle_manager diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp index e37f53cf1c1..4b598e1b6d1 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager.cpp @@ -44,6 +44,7 @@ LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options) declare_parameter("bond_timeout", 4.0); declare_parameter("bond_respawn_max_duration", 10.0); declare_parameter("attempt_respawn_reconnection", true); + declare_parameter("service_introspection_mode", "disabled"); registerRclPreshutdownCallback(); @@ -59,19 +60,9 @@ LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options) bond_respawn_max_duration_ = rclcpp::Duration::from_seconds(respawn_timeout_s); get_parameter("attempt_respawn_reconnection", attempt_respawn_reconnection_); + get_parameter("service_introspection_mode", service_introspection_mode_); callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); - manager_srv_ = create_service( - get_name() + std::string("/manage_nodes"), - std::bind(&LifecycleManager::managerCallback, this, _1, _2, _3), - rclcpp::SystemDefaultsQoS(), - callback_group_); - - is_active_srv_ = create_service( - get_name() + std::string("/is_active"), - std::bind(&LifecycleManager::isActiveCallback, this, _1, _2, _3), - rclcpp::SystemDefaultsQoS(), - callback_group_); transition_state_map_[Transition::TRANSITION_CONFIGURE] = State::PRIMARY_STATE_INACTIVE; transition_state_map_[Transition::TRANSITION_CLEANUP] = State::PRIMARY_STATE_UNCONFIGURED; @@ -92,6 +83,7 @@ LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options) [this]() -> void { init_timer_->cancel(); createLifecycleServiceClients(); + createLifecycleServiceServers(); if (autostart_) { init_timer_ = this->create_wall_timer( 0s, @@ -197,10 +189,32 @@ LifecycleManager::createLifecycleServiceClients() message("Creating and initializing lifecycle service clients"); for (auto & node_name : node_names_) { node_map_[node_name] = - std::make_shared(node_name, shared_from_this()); + std::make_shared(node_name, shared_from_this(), + service_introspection_mode_); } } +void +LifecycleManager::createLifecycleServiceServers() +{ + message("Creating and initializing lifecycle service servers"); + manager_srv_ = std::make_shared>( + get_name() + std::string("/manage_nodes"), + service_introspection_mode_, + shared_from_this(), + std::bind(&LifecycleManager::managerCallback, this, _1, _2, _3), + rclcpp::SystemDefaultsQoS(), + callback_group_); + + is_active_srv_ = std::make_shared>( + get_name() + std::string("/is_active"), + service_introspection_mode_, + shared_from_this(), + std::bind(&LifecycleManager::isActiveCallback, this, _1, _2, _3), + rclcpp::SystemDefaultsQoS(), + callback_group_); +} + void LifecycleManager::destroyLifecycleServiceClients() { diff --git a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp index 4f2f14ed544..c87a4bcd485 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp @@ -34,12 +34,15 @@ LifecycleManagerClient::LifecycleManagerClient( // Use parent node for service call and logging node_ = parent_node; + node_->declare_parameter("service_introspection_mode", "disabled"); + std::string service_introspection_mode = + node_->get_parameter("service_introspection_mode").as_string(); // Create the service clients manager_client_ = std::make_shared>( - manage_service_name_, node_); + manage_service_name_, service_introspection_mode, node_); is_active_client_ = std::make_shared>( - active_service_name_, node_); + active_service_name_, service_introspection_mode, node_); } bool diff --git a/nav2_map_server/include/nav2_map_server/map_saver.hpp b/nav2_map_server/include/nav2_map_server/map_saver.hpp index 18c26be3a71..c42d1aa20ba 100644 --- a/nav2_map_server/include/nav2_map_server/map_saver.hpp +++ b/nav2_map_server/include/nav2_map_server/map_saver.hpp @@ -23,6 +23,7 @@ #include "rclcpp_lifecycle/state.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_msgs/srv/save_map.hpp" +#include "nav2_util/service_server.hpp" #include "map_io.hpp" @@ -111,7 +112,8 @@ class MapSaver : public nav2_util::LifecycleNode // The name of the service for saving a map from topic const std::string save_map_service_name_{"save_map"}; // A service to save the map to a file at run time (SaveMap) - rclcpp::Service::SharedPtr save_map_service_; + std::shared_ptr>> save_map_service_; }; } // namespace nav2_map_server diff --git a/nav2_map_server/include/nav2_map_server/map_server.hpp b/nav2_map_server/include/nav2_map_server/map_server.hpp index 23c9fae9d4c..a38a69f8c7f 100644 --- a/nav2_map_server/include/nav2_map_server/map_server.hpp +++ b/nav2_map_server/include/nav2_map_server/map_server.hpp @@ -19,12 +19,14 @@ #include #include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/service_server.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "nav_msgs/srv/get_map.hpp" #include "nav2_msgs/srv/load_map.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/state.hpp" + namespace nav2_map_server { @@ -125,10 +127,12 @@ class MapServer : public nav2_util::LifecycleNode const std::string load_map_service_name_{"load_map"}; // A service to provide the occupancy grid (GetMap) and the message to return - rclcpp::Service::SharedPtr occ_service_; + std::shared_ptr>> occ_service_; // A service to load the occupancy grid from file at run time (LoadMap) - rclcpp::Service::SharedPtr load_map_service_; + std::shared_ptr>> load_map_service_; // A topic on which the occupancy grid will be published rclcpp_lifecycle::LifecyclePublisher::SharedPtr occ_pub_; diff --git a/nav2_map_server/src/map_saver/map_saver.cpp b/nav2_map_server/src/map_saver/map_saver.cpp index 1517e787526..99e6e19fee8 100644 --- a/nav2_map_server/src/map_saver/map_saver.cpp +++ b/nav2_map_server/src/map_saver/map_saver.cpp @@ -51,6 +51,7 @@ MapSaver::MapSaver(const rclcpp::NodeOptions & options) declare_parameter("free_thresh_default", 0.25); declare_parameter("occupied_thresh_default", 0.65); declare_parameter("map_subscribe_transient_local", true); + declare_parameter("service_introspection_mode", "disabled"); } MapSaver::~MapSaver() @@ -70,10 +71,15 @@ MapSaver::on_configure(const rclcpp_lifecycle::State & /*state*/) free_thresh_default_ = get_parameter("free_thresh_default").as_double(); occupied_thresh_default_ = get_parameter("occupied_thresh_default").as_double(); map_subscribe_transient_local_ = get_parameter("map_subscribe_transient_local").as_bool(); + std::string service_introspection_mode_ = get_parameter("service_introspection_mode").as_string(); + // Create a service that saves the occupancy grid from map topic to a file - save_map_service_ = create_service( + save_map_service_ = std::make_shared>>( service_prefix + save_map_service_name_, + service_introspection_mode_, + shared_from_this(), std::bind(&MapSaver::saveMapCallback, this, _1, _2, _3)); return nav2_util::CallbackReturn::SUCCESS; diff --git a/nav2_map_server/src/map_server/map_server.cpp b/nav2_map_server/src/map_server/map_server.cpp index 6f2a6e04837..2a8429b9f50 100644 --- a/nav2_map_server/src/map_server/map_server.cpp +++ b/nav2_map_server/src/map_server/map_server.cpp @@ -71,6 +71,7 @@ MapServer::MapServer(const rclcpp::NodeOptions & options) declare_parameter("yaml_filename", rclcpp::PARAMETER_STRING); declare_parameter("topic_name", "map"); declare_parameter("frame_id", "map"); + declare_parameter("service_introspection_mode", "disabled"); } MapServer::~MapServer() @@ -84,6 +85,7 @@ MapServer::on_configure(const rclcpp_lifecycle::State & /*state*/) // Get the name of the YAML file to use (can be empty if no initial map should be used) std::string yaml_filename = get_parameter("yaml_filename").as_string(); + std::string service_introspection_mode_ = get_parameter("service_introspection_mode").as_string(); std::string topic_name = get_parameter("topic_name").as_string(); frame_id_ = get_parameter("frame_id").as_string(); @@ -108,8 +110,11 @@ MapServer::on_configure(const rclcpp_lifecycle::State & /*state*/) const std::string service_prefix = get_name() + std::string("/"); // Create a service that provides the occupancy grid - occ_service_ = create_service( + occ_service_ = std::make_shared>>( service_prefix + std::string(service_name_), + service_introspection_mode_, + shared_from_this(), std::bind(&MapServer::getMapCallback, this, _1, _2, _3)); // Create a publisher using the QoS settings to emulate a ROS1 latched topic @@ -118,8 +123,11 @@ MapServer::on_configure(const rclcpp_lifecycle::State & /*state*/) rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); // Create a service that loads the occupancy grid from a file - load_map_service_ = create_service( + load_map_service_ = std::make_shared>>( service_prefix + std::string(load_map_service_name_), + service_introspection_mode_, + shared_from_this(), std::bind(&MapServer::loadMapCallback, this, _1, _2, _3)); return nav2_util::CallbackReturn::SUCCESS; diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index 8d5ce2903e9..058f58c0db5 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -31,6 +31,7 @@ #include "nav2_msgs/msg/costmap.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_util/simple_action_server.hpp" +#include "nav2_util/service_server.hpp" #include "tf2_ros/transform_listener.h" #include "tf2_ros/create_timer_ros.h" #include "nav2_costmap_2d/costmap_2d_ros.hpp" @@ -204,6 +205,7 @@ class PlannerServer : public nav2_util::LifecycleNode * @param response from the service */ void isPathValid( + const std::shared_ptr request_header, const std::shared_ptr request, std::shared_ptr response); @@ -259,8 +261,11 @@ class PlannerServer : public nav2_util::LifecycleNode // Publishers for the path rclcpp_lifecycle::LifecyclePublisher::SharedPtr plan_publisher_; + // String to store the service introspection mode + std::string service_introspection_mode_; // Service to determine if the path is valid - rclcpp::Service::SharedPtr is_path_valid_service_; + std::shared_ptr>> is_path_valid_service_; }; } // namespace nav2_planner diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index be65ffef9c8..d6c622d91d0 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -54,6 +54,7 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) declare_parameter("expected_planner_frequency", 1.0); declare_parameter("action_server_result_timeout", 10.0); declare_parameter("costmap_update_timeout", 1.0); + declare_parameter("service_introspection_mode", "disabled"); get_parameter("planner_plugins", planner_ids_); if (planner_ids_ == default_ids_) { @@ -61,7 +62,7 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) declare_parameter(default_ids_[i] + ".plugin", default_types_[i]); } } - + service_introspection_mode_ = get_parameter("service_introspection_mode").as_string(); // Setup the global costmap costmap_ros_ = std::make_shared( "global_costmap", std::string{get_namespace()}, @@ -197,11 +198,13 @@ PlannerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) auto node = shared_from_this(); - is_path_valid_service_ = node->create_service( + is_path_valid_service_ = std::make_shared>>( "is_path_valid", - std::bind( - &PlannerServer::isPathValid, this, - std::placeholders::_1, std::placeholders::_2)); + service_introspection_mode_, + node, + std::bind(&PlannerServer::isPathValid, this, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3)); // Add callback for dynamic parameters dyn_params_handler_ = node->add_on_set_parameters_callback( @@ -249,6 +252,7 @@ PlannerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up"); + is_path_valid_service_.reset(); action_server_pose_.reset(); action_server_poses_.reset(); plan_publisher_.reset(); @@ -645,6 +649,7 @@ PlannerServer::publishPlan(const nav_msgs::msg::Path & path) } void PlannerServer::isPathValid( + const std::shared_ptr/*request_header*/, const std::shared_ptr request, std::shared_ptr response) { diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp index 6b57eee6170..d92f6132bc5 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp @@ -23,6 +23,7 @@ #include #include #include +#include "nav2_util/service_client.hpp" namespace nav2_rviz_plugins { @@ -42,14 +43,11 @@ class CostmapCostTool : public rviz_common::Tool void callCostService(float x, float y); - void handleLocalCostResponse(rclcpp::Client::SharedFuture); - void handleGlobalCostResponse(rclcpp::Client::SharedFuture); - private Q_SLOTS: private: - rclcpp::Client::SharedPtr local_cost_client_; - rclcpp::Client::SharedPtr global_cost_client_; + std::shared_ptr> local_cost_client_; + std::shared_ptr> global_cost_client_; // The Node pointer that we need to keep alive for the duration of this plugin. std::shared_ptr node_ptr_; diff --git a/nav2_rviz_plugins/src/costmap_cost_tool.cpp b/nav2_rviz_plugins/src/costmap_cost_tool.cpp index 03435a913e4..f14d9957dc3 100644 --- a/nav2_rviz_plugins/src/costmap_cost_tool.cpp +++ b/nav2_rviz_plugins/src/costmap_cost_tool.cpp @@ -53,10 +53,19 @@ void CostmapCostTool::onInitialize() } rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node(); + node->declare_parameter("service_introspection_mode", "disabled"); + std::string service_introspection_mode; + node->get_parameter("service_introspection_mode", service_introspection_mode); local_cost_client_ = - node->create_client("/local_costmap/get_cost_local_costmap"); + std::make_shared>( + "/local_costmap/get_cost_local_costmap", + service_introspection_mode, + node); global_cost_client_ = - node->create_client("/global_costmap/get_cost_global_costmap"); + std::make_shared>( + "/global_costmap/get_cost_global_costmap", + service_introspection_mode, + node); } void CostmapCostTool::activate() {} @@ -106,34 +115,11 @@ void CostmapCostTool::callCostService(float x, float y) request->use_footprint = false; // Call local costmap service - if (local_cost_client_->wait_for_service(std::chrono::seconds(1))) { - local_cost_client_->async_send_request( - request, - std::bind(&CostmapCostTool::handleLocalCostResponse, this, std::placeholders::_1)); - } - - // Call global costmap service - if (global_cost_client_->wait_for_service(std::chrono::seconds(1))) { - global_cost_client_->async_send_request( - request, - std::bind(&CostmapCostTool::handleGlobalCostResponse, this, std::placeholders::_1)); - } -} - -void CostmapCostTool::handleLocalCostResponse( - rclcpp::Client::SharedFuture future) -{ - rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node(); - auto response = future.get(); - RCLCPP_INFO(node->get_logger(), "Local costmap cost: %.1f", response->costs[0]); -} + auto local_cost_response_ = local_cost_client_->invoke_shared(request).get(); + RCLCPP_INFO(node->get_logger(), "Local costmap cost: %.1f", local_cost_response_->costs[0]); -void CostmapCostTool::handleGlobalCostResponse( - rclcpp::Client::SharedFuture future) -{ - rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node(); - auto response = future.get(); - RCLCPP_INFO(node->get_logger(), "Global costmap cost: %.1f", response->costs[0]); + auto global_cost_response_ = global_cost_client_->invoke_shared(request).get(); + RCLCPP_INFO(node->get_logger(), "Global costmap cost: %.1f", global_cost_response_->costs[0]); } } // namespace nav2_rviz_plugins diff --git a/nav2_util/include/nav2_util/lifecycle_service_client.hpp b/nav2_util/include/nav2_util/lifecycle_service_client.hpp index cceb5393684..61c4e34e514 100644 --- a/nav2_util/include/nav2_util/lifecycle_service_client.hpp +++ b/nav2_util/include/nav2_util/lifecycle_service_client.hpp @@ -31,10 +31,13 @@ namespace nav2_util class LifecycleServiceClient { public: - explicit LifecycleServiceClient(const std::string & lifecycle_node_name); + explicit LifecycleServiceClient( + const std::string & lifecycle_node_name, + std::string service_introspection_mode = "disabled"); LifecycleServiceClient( const std::string & lifecycle_node_name, - rclcpp::Node::SharedPtr parent_node); + rclcpp::Node::SharedPtr parent_node, + std::string service_introspection_mode = "disabled"); /// Trigger a state change /** diff --git a/nav2_util/include/nav2_util/service_client.hpp b/nav2_util/include/nav2_util/service_client.hpp index 6f25965d1e3..55eb3a35d86 100644 --- a/nav2_util/include/nav2_util/service_client.hpp +++ b/nav2_util/include/nav2_util/service_client.hpp @@ -37,6 +37,7 @@ class ServiceClient */ explicit ServiceClient( const std::string & service_name, + std::string service_introspection_mode, const NodeT & provided_node) : service_name_(service_name), node_(provided_node) { @@ -48,6 +49,18 @@ class ServiceClient service_name, rclcpp::SystemDefaultsQoS(), callback_group_); + rcl_service_introspection_state_t introspection_state = RCL_SERVICE_INTROSPECTION_OFF; + + if (service_introspection_mode == "disabled") { + introspection_state = RCL_SERVICE_INTROSPECTION_OFF; + } else if (service_introspection_mode == "metadata") { + introspection_state = RCL_SERVICE_INTROSPECTION_METADATA; + } else if (service_introspection_mode == "contents") { + introspection_state = RCL_SERVICE_INTROSPECTION_CONTENTS; + } + + this->client_->configure_introspection( + node_->get_clock(), rclcpp::SystemDefaultsQoS(), introspection_state); } using RequestType = typename ServiceT::Request; @@ -126,6 +139,41 @@ class ServiceClient return response.get(); } + /** + * @brief Invoke the service and block until completed + * @param request The request object to call the service using + * @return std::shared_future The shared future of the service response + */ + std::shared_future invoke_shared( + typename RequestType::SharedPtr & request, + const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)) + { + while (!client_->wait_for_service(std::chrono::seconds(1))) { + if (!rclcpp::ok()) { + throw std::runtime_error( + service_name_ + " service client: interrupted while waiting for service"); + } + RCLCPP_INFO( + node_->get_logger(), "%s service client: waiting for service to appear...", + service_name_.c_str()); + } + + RCLCPP_DEBUG( + node_->get_logger(), "%s service client: send async request", + service_name_.c_str()); + auto future_result = client_->async_send_request(request); + + if (callback_group_executor_.spin_until_future_complete(future_result, timeout) != + rclcpp::FutureReturnCode::SUCCESS) + { + // Pending request must be manually cleaned up if execution is interrupted or timed out + client_->remove_pending_request(future_result); + throw std::runtime_error(service_name_ + " service client: async_send_request failed"); + } + + return future_result.share(); + } + /** * @brief Block until a service is available or timeout * @param timeout Maximum timeout to wait for, default infinite diff --git a/nav2_util/include/nav2_util/service_server.hpp b/nav2_util/include/nav2_util/service_server.hpp new file mode 100644 index 00000000000..cc4ef75222f --- /dev/null +++ b/nav2_util/include/nav2_util/service_server.hpp @@ -0,0 +1,81 @@ +// Copyright (c) 2025 Maurice +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_UTIL__SERVICE_SERVER_HPP_ +#define NAV2_UTIL__SERVICE_SERVER_HPP_ + +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +namespace nav2_util +{ + +/** + * @class nav2_util::ServiceServer + * @brief A simple wrapper on ROS2 services for invoke() and block-style calling + */ +template +class ServiceServer +{ +public: + using RequestType = typename ServiceT::Request; + using ResponseType = typename ServiceT::Response; + using CallbackType = std::function, + const std::shared_ptr, std::shared_ptr)>; + + explicit ServiceServer( + const std::string & service_name, + std::string service_introspection_mode, + const NodeT & node, + CallbackType callback, + const rclcpp::QoS & qos = rclcpp::ServicesQoS(), + rclcpp::CallbackGroup::SharedPtr callback_group = nullptr) + : service_name_(service_name), callback_(callback) + { + server_ = node->template create_service( + service_name, + [this](const std::shared_ptr request_header, + const std::shared_ptr request, std::shared_ptr response) { + this->callback_(request_header, request, response); + }, + qos, + callback_group); + + rcl_service_introspection_state_t introspection_state = RCL_SERVICE_INTROSPECTION_OFF; + if (service_introspection_mode == "disabled") { + introspection_state = RCL_SERVICE_INTROSPECTION_OFF; + } else if (service_introspection_mode == "metadata") { + introspection_state = RCL_SERVICE_INTROSPECTION_METADATA; + } else if (service_introspection_mode == "contents") { + introspection_state = RCL_SERVICE_INTROSPECTION_CONTENTS; + } + + this->server_->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), introspection_state); + } + +protected: + std::string service_name_; + CallbackType callback_; + typename rclcpp::Service::SharedPtr server_; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr + on_set_parameters_callback_handle_; +}; + +} // namespace nav2_util + + +#endif // NAV2_UTIL__SERVICE_SERVER_HPP_ diff --git a/nav2_util/src/lifecycle_service_client.cpp b/nav2_util/src/lifecycle_service_client.cpp index 70656d9f297..7ad8505443c 100644 --- a/nav2_util/src/lifecycle_service_client.cpp +++ b/nav2_util/src/lifecycle_service_client.cpp @@ -30,10 +30,12 @@ using namespace std::chrono_literals; namespace nav2_util { -LifecycleServiceClient::LifecycleServiceClient(const string & lifecycle_node_name) +LifecycleServiceClient::LifecycleServiceClient( + const string & lifecycle_node_name, + std::string service_introspection_mode) : node_(generate_internal_node(lifecycle_node_name + "_lifecycle_client")), - change_state_(lifecycle_node_name + "/change_state", node_), - get_state_(lifecycle_node_name + "/get_state", node_) + change_state_(lifecycle_node_name + "/change_state", service_introspection_mode, node_), + get_state_(lifecycle_node_name + "/get_state", service_introspection_mode, node_) { // Block until server is up rclcpp::Rate r(20); @@ -46,10 +48,11 @@ LifecycleServiceClient::LifecycleServiceClient(const string & lifecycle_node_nam LifecycleServiceClient::LifecycleServiceClient( const string & lifecycle_node_name, - rclcpp::Node::SharedPtr parent_node) + rclcpp::Node::SharedPtr parent_node, + std::string service_introspection_mode) : node_(parent_node), - change_state_(lifecycle_node_name + "/change_state", node_), - get_state_(lifecycle_node_name + "/get_state", node_) + change_state_(lifecycle_node_name + "/change_state", service_introspection_mode, node_), + get_state_(lifecycle_node_name + "/get_state", service_introspection_mode, node_) { // Block until server is up rclcpp::Rate r(20); diff --git a/nav2_util/test/test_service_client.cpp b/nav2_util/test/test_service_client.cpp index 521d3f30568..c6608684ac8 100644 --- a/nav2_util/test/test_service_client.cpp +++ b/nav2_util/test/test_service_client.cpp @@ -36,8 +36,10 @@ class TestServiceClient : public ServiceClient public: TestServiceClient( const std::string & name, + std::string service_introspection_mode = "disabled", const rclcpp::Node::SharedPtr & provided_node = rclcpp::Node::SharedPtr()) - : ServiceClient(name, provided_node) {} + : ServiceClient(name, service_introspection_mode, provided_node) + {} string name() {return node_->get_name();} const rclcpp::Node::SharedPtr & getNode() {return node_;} @@ -45,8 +47,9 @@ class TestServiceClient : public ServiceClient TEST(ServiceClient, can_ServiceClient_use_passed_in_node) { + std::string service_introspection_mode_ = "disabled"; auto node = rclcpp::Node::make_shared("test_node"); - TestServiceClient t("bar", node); + TestServiceClient t("bar", service_introspection_mode_, node); ASSERT_EQ(t.getNode(), node); ASSERT_EQ(t.name(), "test_node"); } @@ -69,7 +72,8 @@ TEST(ServiceClient, can_ServiceClient_invoke_in_callback) auto pub_thread = std::thread([&]() {rclcpp::spin(pub_node);}); auto sub_node = rclcpp::Node::make_shared("sub_node"); - ServiceClient client("empty_srv", sub_node); + std::string service_introspection_mode_ = "disabled"; + ServiceClient client("empty_srv", service_introspection_mode_, sub_node); auto sub = sub_node->create_subscription( "empty_topic", rclcpp::QoS(1), @@ -91,7 +95,8 @@ TEST(ServiceClient, can_ServiceClient_timeout) { rclcpp::init(0, nullptr); auto node = rclcpp::Node::make_shared("test_node"); - TestServiceClient t("bar", node); + std::string service_introspection_mode_ = "disabled"; + TestServiceClient t("bar", service_introspection_mode_, node); rclcpp::spin_some(node); bool ready = t.wait_for_service(std::chrono::milliseconds(10)); rclcpp::shutdown(); diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index 578b33b1339..92bd1981818 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -221,6 +221,7 @@ class WaypointFollower : public nav2_util::LifecycleNode waypoint_task_executor_; std::string waypoint_task_executor_id_; std::string waypoint_task_executor_type_; + std::string service_introspection_mode_; }; } // namespace nav2_waypoint_follower diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 55db83df847..060d177e07d 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -35,6 +35,7 @@ WaypointFollower::WaypointFollower(const rclcpp::NodeOptions & options) RCLCPP_INFO(get_logger(), "Creating"); declare_parameter("stop_on_failure", true); + declare_parameter("service_introspection_mode", "disabled"); declare_parameter("loop_rate", 20); declare_parameter("action_server_result_timeout", 900.0); @@ -61,6 +62,7 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & state) auto node = shared_from_this(); stop_on_failure_ = get_parameter("stop_on_failure").as_bool(); + service_introspection_mode_ = get_parameter("service_introspection_mode").as_string(); loop_rate_ = get_parameter("loop_rate").as_int(); waypoint_task_executor_id_ = get_parameter("waypoint_task_executor_plugin").as_string(); global_frame_id_ = get_parameter("global_frame_id").as_string(); @@ -96,6 +98,7 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & state) nav2_util::ServiceClient>>( "/fromLL", + service_introspection_mode_, node); gps_action_server_ = std::make_unique( From c79b012fcb352ac5c4b5812fefd86d89d0a914ad Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Sun, 2 Mar 2025 13:56:18 +0000 Subject: [PATCH 02/19] Add test for service server, fix repeat declaration Signed-off-by: mini-1235 --- nav2_bringup/params/nav2_params.yaml | 2 +- .../opennav_docking/src/dock_database.cpp | 5 +- .../src/lifecycle_manager_client.cpp | 4 +- .../launch/map_saver_server.launch.py | 2 + nav2_map_server/src/map_saver/map_saver.cpp | 1 - nav2_rviz_plugins/src/costmap_cost_tool.cpp | 5 +- .../include/nav2_util/service_server.hpp | 2 +- nav2_util/test/CMakeLists.txt | 3 + nav2_util/test/test_service_server.cpp | 76 +++++++++++++++++++ 9 files changed, 92 insertions(+), 8 deletions(-) create mode 100644 nav2_util/test/test_service_server.cpp diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 535cb89b0e1..7c34a309457 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -292,7 +292,7 @@ global_costmap: map_server: ros__parameters: # yaml_filename: "" - service_introspection_mode: disabled # disabled, metadata, contents + service_introspection_mode: "disabled" # disabled, metadata, contents map_saver: ros__parameters: diff --git a/nav2_docking/opennav_docking/src/dock_database.cpp b/nav2_docking/opennav_docking/src/dock_database.cpp index 0d9b0e104c1..72bac7cf055 100644 --- a/nav2_docking/opennav_docking/src/dock_database.cpp +++ b/nav2_docking/opennav_docking/src/dock_database.cpp @@ -55,8 +55,9 @@ bool DockDatabase::initialize( "Docking Server has %u dock types and %u dock instances available.", this->plugin_size(), this->instance_size()); - // declare the service to reload the database - node->declare_parameter("service_introspection_mode", "disabled"); + if(!node->has_parameter("service_introspection_mode")) { + node->declare_parameter("service_introspection_mode", "disabled"); + } std::string service_introspection_mode_ = node->get_parameter("service_introspection_mode").as_string(); diff --git a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp index c87a4bcd485..e6559eb02a0 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp @@ -34,7 +34,9 @@ LifecycleManagerClient::LifecycleManagerClient( // Use parent node for service call and logging node_ = parent_node; - node_->declare_parameter("service_introspection_mode", "disabled"); + if(!node_->has_parameter("service_introspection_mode")) { + node_->declare_parameter("service_introspection_mode", "disabled"); + } std::string service_introspection_mode = node_->get_parameter("service_introspection_mode").as_string(); diff --git a/nav2_map_server/launch/map_saver_server.launch.py b/nav2_map_server/launch/map_saver_server.launch.py index aa3f2c4f7bc..436e357e08d 100644 --- a/nav2_map_server/launch/map_saver_server.launch.py +++ b/nav2_map_server/launch/map_saver_server.launch.py @@ -26,6 +26,7 @@ def generate_launch_description(): save_map_timeout = 2.0 free_thresh_default = 0.25 occupied_thresh_default = 0.65 + service_introspection_mode = 'disabled' # Nodes launching commands start_map_saver_server_cmd = launch_ros.actions.Node( @@ -37,6 +38,7 @@ def generate_launch_description(): {'save_map_timeout': save_map_timeout}, {'free_thresh_default': free_thresh_default}, {'occupied_thresh_default': occupied_thresh_default}, + {'service_introspection_mode': service_introspection_mode}, ], ) diff --git a/nav2_map_server/src/map_saver/map_saver.cpp b/nav2_map_server/src/map_saver/map_saver.cpp index 99e6e19fee8..27e66dc93db 100644 --- a/nav2_map_server/src/map_saver/map_saver.cpp +++ b/nav2_map_server/src/map_saver/map_saver.cpp @@ -73,7 +73,6 @@ MapSaver::on_configure(const rclcpp_lifecycle::State & /*state*/) map_subscribe_transient_local_ = get_parameter("map_subscribe_transient_local").as_bool(); std::string service_introspection_mode_ = get_parameter("service_introspection_mode").as_string(); - // Create a service that saves the occupancy grid from map topic to a file save_map_service_ = std::make_shared>>( diff --git a/nav2_rviz_plugins/src/costmap_cost_tool.cpp b/nav2_rviz_plugins/src/costmap_cost_tool.cpp index f14d9957dc3..40207a83805 100644 --- a/nav2_rviz_plugins/src/costmap_cost_tool.cpp +++ b/nav2_rviz_plugins/src/costmap_cost_tool.cpp @@ -52,8 +52,9 @@ void CostmapCostTool::onInitialize() return; } rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node(); - - node->declare_parameter("service_introspection_mode", "disabled"); + if(!node->has_parameter("service_introspection_mode")) { + node->declare_parameter("service_introspection_mode", "disabled"); + } std::string service_introspection_mode; node->get_parameter("service_introspection_mode", service_introspection_mode); local_cost_client_ = diff --git a/nav2_util/include/nav2_util/service_server.hpp b/nav2_util/include/nav2_util/service_server.hpp index cc4ef75222f..9eb1abda801 100644 --- a/nav2_util/include/nav2_util/service_server.hpp +++ b/nav2_util/include/nav2_util/service_server.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2025 Maurice +// Copyright (c) 2025 Maurice Alexander Purnawan // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index 14d774c2986..9ba40dfe512 100644 --- a/nav2_util/test/CMakeLists.txt +++ b/nav2_util/test/CMakeLists.txt @@ -10,6 +10,9 @@ find_package(test_msgs REQUIRED) ament_add_gtest(test_service_client test_service_client.cpp) target_link_libraries(test_service_client ${library_name} ${std_srvs_TARGETS}) +ament_add_gtest(test_service_server test_service_server.cpp) +target_link_libraries(test_service_server ${library_name} ${std_srvs_TARGETS}) + ament_add_gtest(test_string_utils test_string_utils.cpp) target_link_libraries(test_string_utils ${library_name}) diff --git a/nav2_util/test/test_service_server.cpp b/nav2_util/test/test_service_server.cpp new file mode 100644 index 00000000000..23a9e0ed2ed --- /dev/null +++ b/nav2_util/test/test_service_server.cpp @@ -0,0 +1,76 @@ +// Copyright (c) 2025 Maurice Alexander Purnawan +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include "nav2_util/service_server.hpp" +#include "rclcpp/rclcpp.hpp" +#include "std_srvs/srv/empty.hpp" +#include "std_msgs/msg/empty.hpp" +#include "gtest/gtest.h" + +using nav2_util::ServiceServer; +using std::string; + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +class TestServiceServer : public ServiceServer +{ +public: + TestServiceServer( + const std::string & name, + std::string service_introspection_mode, + const rclcpp::Node::SharedPtr & provided_node, + CallbackType callback) + : ServiceServer(name, service_introspection_mode, provided_node, callback) + {} +}; + +TEST(ServiceServer, can_ServiceServer_handle_request) +{ + int a = 0; + auto node = rclcpp::Node::make_shared("test_node"); + std::string service_introspection_mode_ = "disabled"; + + auto callback = [&a](const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr) { + a = 1; + }; + + TestServiceServer server("empty_srv", service_introspection_mode_, node, callback); + + auto client_node = rclcpp::Node::make_shared("client_node"); + auto client = client_node->create_client("empty_srv"); + + auto req = std::make_shared(); + auto result = client->async_send_request(req); + + rclcpp::spin_some(node); + rclcpp::spin_some(client_node); + + ASSERT_EQ(a, 1); +} + +int main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From 821ac4d2ea0575c02991da4e273f565d28a12b90 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Sun, 2 Mar 2025 14:18:24 +0000 Subject: [PATCH 03/19] Fix cpplint Signed-off-by: mini-1235 --- nav2_amcl/src/amcl_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 093089a7a2b..2886c262b5a 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -194,8 +194,8 @@ AmclNode::AmclNode(const rclcpp::NodeOptions & options) add_parameter( "service_introspection_mode", rclcpp::ParameterValue(std::string("disabled")), - "Set this to disabled for no introspection, metadata for Only metadata without any user data contents, " - "and contents for User data contents with metadata"); + "Set this to disabled for no introspection, metadata for Only metadata without" + "any user data contents, and contents for User data contents with metadata"); add_parameter( "update_min_a", rclcpp::ParameterValue(0.2), From a6bcd92de4bdb914f8898b6b101a3765e9cb4d6e Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Fri, 7 Mar 2025 16:05:21 +0000 Subject: [PATCH 04/19] Add test for coverage Signed-off-by: mini-1235 --- nav2_util/test/test_service_client.cpp | 35 +++++++++++++++++++++++++- 1 file changed, 34 insertions(+), 1 deletion(-) diff --git a/nav2_util/test/test_service_client.cpp b/nav2_util/test/test_service_client.cpp index c6608684ac8..646e0cc27fa 100644 --- a/nav2_util/test/test_service_client.cpp +++ b/nav2_util/test/test_service_client.cpp @@ -72,7 +72,7 @@ TEST(ServiceClient, can_ServiceClient_invoke_in_callback) auto pub_thread = std::thread([&]() {rclcpp::spin(pub_node);}); auto sub_node = rclcpp::Node::make_shared("sub_node"); - std::string service_introspection_mode_ = "disabled"; + std::string service_introspection_mode_ = "metadata"; ServiceClient client("empty_srv", service_introspection_mode_, sub_node); auto sub = sub_node->create_subscription( "empty_topic", @@ -91,6 +91,39 @@ TEST(ServiceClient, can_ServiceClient_invoke_in_callback) ASSERT_EQ(a, 1); } +TEST(ServiceClient, can_ServiceClient_invoke_shared) +{ + rclcpp::init(0, nullptr); + int a = 0; + auto service_node = rclcpp::Node::make_shared("service_node"); + auto service = service_node->create_service( + "empty_srv", + [&a](std_srvs::srv::Empty::Request::SharedPtr, std_srvs::srv::Empty::Response::SharedPtr) { + a = 1; + }); + auto srv_thread = std::thread([&]() {rclcpp::spin(service_node);}); + + auto pub_node = rclcpp::Node::make_shared("pub_node"); + auto pub = pub_node->create_publisher( + "empty_topic", + rclcpp::QoS(1).transient_local()); + auto pub_thread = std::thread([&]() {rclcpp::spin(pub_node);}); + + auto sub_node = rclcpp::Node::make_shared("sub_node"); + std::string service_introspection_mode_ = "contents"; + ServiceClient client("empty_srv", service_introspection_mode_, sub_node); + + auto req = std::make_shared(); + auto future_result = client.invoke_shared(req); + + ASSERT_EQ(future_result.wait_for(std::chrono::seconds(1)), std::future_status::ready); + ASSERT_EQ(a, 1); + + rclcpp::shutdown(); + srv_thread.join(); + pub_thread.join(); +} + TEST(ServiceClient, can_ServiceClient_timeout) { rclcpp::init(0, nullptr); From 57dbef883cbf2f53d6cb5836e1cdf83f34a726e9 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Fri, 21 Mar 2025 18:29:12 +0000 Subject: [PATCH 05/19] Declare and set the parameter in service client and server class Signed-off-by: mini-1235 --- nav2_amcl/include/nav2_amcl/amcl_node.hpp | 1 - nav2_amcl/src/amcl_node.cpp | 12 ++--- .../nav2_behavior_tree/bt_service_node.hpp | 3 +- .../condition/is_path_valid_condition.hpp | 1 - .../condition/is_path_valid_condition.cpp | 5 --- .../nav2_costmap_2d/clear_costmap_service.hpp | 3 +- .../nav2_costmap_2d/costmap_2d_publisher.hpp | 3 +- .../nav2_costmap_2d/costmap_2d_ros.hpp | 1 - .../costmap_filters/costmap_filter.cpp | 4 -- nav2_costmap_2d/src/clear_costmap_service.cpp | 5 +-- nav2_costmap_2d/src/costmap_2d_publisher.cpp | 4 +- nav2_costmap_2d/src/costmap_2d_ros.cpp | 10 ++--- .../opennav_docking/src/dock_database.cpp | 7 --- .../lifecycle_manager.hpp | 2 - .../src/lifecycle_manager.cpp | 7 +-- .../src/lifecycle_manager_client.cpp | 9 +--- .../launch/map_saver_server.launch.py | 2 - nav2_map_server/src/map_saver/map_saver.cpp | 3 -- nav2_map_server/src/map_server/map_server.cpp | 4 -- .../include/nav2_planner/planner_server.hpp | 2 - nav2_planner/src/planner_server.cpp | 3 -- nav2_rviz_plugins/src/costmap_cost_tool.cpp | 21 ++++----- nav2_smac_planner/test/test_smac_2d.cpp | 1 + .../nav2_util/lifecycle_service_client.hpp | 6 +-- .../include/nav2_util/service_client.hpp | 42 +++++------------ .../include/nav2_util/service_server.hpp | 12 +++-- nav2_util/src/lifecycle_service_client.cpp | 14 +++--- nav2_util/test/test_service_client.cpp | 45 ++----------------- nav2_util/test/test_service_server.cpp | 6 +-- .../waypoint_follower.hpp | 1 - .../src/waypoint_follower.cpp | 3 -- 31 files changed, 57 insertions(+), 185 deletions(-) diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index e4e802fc927..00f8d78bd01 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -391,7 +391,6 @@ class AmclNode : public nav2_util::LifecycleNode double sigma_hit_; bool tf_broadcast_; tf2::Duration transform_tolerance_; - std::string service_introspection_mode_; double a_thresh_; double d_thresh_; double z_hit_; diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 2886c262b5a..5dcc83eb93a 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -192,11 +192,6 @@ AmclNode::AmclNode(const rclcpp::NodeOptions & options) "Time with which to post-date the transform that is published, to indicate that this transform " "is valid into the future"); - add_parameter( - "service_introspection_mode", rclcpp::ParameterValue(std::string("disabled")), - "Set this to disabled for no introspection, metadata for Only metadata without" - "any user data contents, and contents for User data contents with metadata"); - add_parameter( "update_min_a", rclcpp::ParameterValue(0.2), "Rotational movement required before performing a filter update"); @@ -1102,7 +1097,6 @@ AmclNode::initParameters() get_parameter("sigma_hit", sigma_hit_); get_parameter("tf_broadcast", tf_broadcast_); get_parameter("transform_tolerance", tmp_tol); - get_parameter("service_introspection_mode", service_introspection_mode_); get_parameter("update_min_a", a_thresh_); get_parameter("update_min_d", d_thresh_); get_parameter("z_hit", z_hit_); @@ -1572,19 +1566,19 @@ AmclNode::initServices() { global_loc_srv_ = std::make_shared>>( - "reinitialize_global_localization", service_introspection_mode_, shared_from_this(), + "reinitialize_global_localization", shared_from_this(), std::bind(&AmclNode::globalLocalizationCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); initial_guess_srv_ = std::make_shared>>( - "set_initial_pose", service_introspection_mode_, shared_from_this(), + "set_initial_pose", shared_from_this(), std::bind(&AmclNode::initialPoseReceivedSrv, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); nomotion_update_srv_ = std::make_shared>>( - "request_nomotion_update", service_introspection_mode_, shared_from_this(), + "request_nomotion_update", shared_from_this(), std::bind(&AmclNode::nomotionUpdateCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index eedf5a57b84..41a00d4022a 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp @@ -174,7 +174,7 @@ class BtServiceNode : public BT::ActionNodeBase return BT::NodeStatus::FAILURE; } - future_result_ = service_client_->invoke_shared(request_); + future_result_ = service_client_->async_call(request_); sent_time_ = node_->now(); request_sent_ = true; } @@ -268,7 +268,6 @@ class BtServiceNode : public BT::ActionNodeBase std::string service_name_, service_node_name_; std::shared_ptr> service_client_; std::shared_ptr request_; - std::string service_introspection_mode_; // The node that will be used for any ROS operations rclcpp::Node::SharedPtr node_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp index 04e04a298b9..9ae93a3bf47 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp @@ -80,7 +80,6 @@ class IsPathValidCondition : public BT::ConditionNode std::chrono::milliseconds server_timeout_; unsigned int max_cost_; bool consider_unknown_as_obstacle_; - std::string service_introspection_mode_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp index 575bded7622..15272cfead4 100644 --- a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp @@ -28,12 +28,7 @@ IsPathValidCondition::IsPathValidCondition( max_cost_(253), consider_unknown_as_obstacle_(false) { node_ = config().blackboard->get("node"); - nav2_util::declare_parameter_if_not_declared( - node_, "service_introspection_mode", - rclcpp::ParameterValue(std::string("disabled"))); - node_->get_parameter("service_introspection_mode", service_introspection_mode_); client_ = std::make_shared>("is_path_valid", - service_introspection_mode_, node_); server_timeout_ = config().blackboard->template get("server_timeout"); diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp index 61d8b082381..ccaa7b8cacf 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp @@ -43,8 +43,7 @@ class ClearCostmapService * @brief A constructor */ ClearCostmapService( - const nav2_util::LifecycleNode::WeakPtr & parent, Costmap2DROS & costmap, - std::string service_introspection_mode); + const nav2_util::LifecycleNode::WeakPtr & parent, Costmap2DROS & costmap); /** * @brief A constructor diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp index d4eee45fb57..8d16be1bb7c 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp @@ -74,8 +74,7 @@ class Costmap2DPublisher std::string global_frame, std::string topic_name, bool always_send_full_costmap = false, - double map_vis_z = 0.0, - std::string service_introspection_mode = "disabled"); + double map_vis_z = 0.0); /** * @brief Destructor diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp index 5a57df38d99..45568f8fd30 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp @@ -411,7 +411,6 @@ class Costmap2DROS : public nav2_util::LifecycleNode // Derived parameters bool use_radius_{false}; - std::string service_introspection_mode_{"disabled"}; std::vector unpadded_footprint_; std::vector padded_footprint_; diff --git a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp index 6647aaf9f22..36ac8ab9585 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp @@ -74,7 +74,6 @@ void CostmapFilter::onInitialize() declareParameter("enabled", rclcpp::ParameterValue(true)); declareParameter("filter_info_topic", rclcpp::PARAMETER_STRING); declareParameter("transform_tolerance", rclcpp::ParameterValue(0.1)); - declareParameter("service_introspection_mode", rclcpp::ParameterValue("disabled")); // Get parameters node->get_parameter(name_ + "." + "enabled", enabled_); @@ -82,14 +81,11 @@ void CostmapFilter::onInitialize() double transform_tolerance {}; node->get_parameter(name_ + "." + "transform_tolerance", transform_tolerance); transform_tolerance_ = tf2::durationFromSec(transform_tolerance); - std::string service_introspection_mode_ = - node->get_parameter(name_ + "." + "service_introspection_mode").as_string(); // Costmap Filter enabling service enable_service_ = std::make_shared>>( name_ + "/toggle_filter", - service_introspection_mode_, node->shared_from_this(), std::bind(&CostmapFilter::enableCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp index e1882dd71e9..add69369a55 100644 --- a/nav2_costmap_2d/src/clear_costmap_service.cpp +++ b/nav2_costmap_2d/src/clear_costmap_service.cpp @@ -33,7 +33,7 @@ using ClearEntirely = nav2_msgs::srv::ClearEntireCostmap; ClearCostmapService::ClearCostmapService( const nav2_util::LifecycleNode::WeakPtr & parent, - Costmap2DROS & costmap, std::string service_introspection_mode) + Costmap2DROS & costmap) : costmap_(costmap) { auto node = parent.lock(); @@ -43,7 +43,6 @@ ClearCostmapService::ClearCostmapService( clear_except_service_ = std::make_shared>>( std::string("clear_except_") + costmap_.getName(), - service_introspection_mode, node->shared_from_this(), std::bind( &ClearCostmapService::clearExceptRegionCallback, this, @@ -52,7 +51,6 @@ ClearCostmapService::ClearCostmapService( clear_around_service_ = std::make_shared>>( std::string("clear_around_") + costmap_.getName(), - service_introspection_mode, node->shared_from_this(), std::bind( &ClearCostmapService::clearAroundRobotCallback, this, @@ -61,7 +59,6 @@ ClearCostmapService::ClearCostmapService( clear_entire_service_ = std::make_shared>>( std::string("clear_entirely_") + costmap_.getName(), - service_introspection_mode, node->shared_from_this(), std::bind( &ClearCostmapService::clearEntireCallback, this, diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp index 72f5aa04215..c06907c812a 100644 --- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp @@ -55,8 +55,7 @@ Costmap2DPublisher::Costmap2DPublisher( std::string global_frame, std::string topic_name, bool always_send_full_costmap, - double map_vis_z, - std::string service_introspection_mode) + double map_vis_z) : costmap_(costmap), global_frame_(global_frame), topic_name_(topic_name), @@ -86,7 +85,6 @@ Costmap2DPublisher::Costmap2DPublisher( costmap_service_ = std::make_shared>>( std::string("get_") + topic_name, - service_introspection_mode, node->shared_from_this(), std::bind( &Costmap2DPublisher::costmap_service_callback, this, diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 1c83f7bc44f..139d3b9f3fc 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -125,7 +125,6 @@ void Costmap2DROS::init() declare_parameter("unknown_cost_value", rclcpp::ParameterValue(static_cast(0xff))); declare_parameter("update_frequency", rclcpp::ParameterValue(5.0)); declare_parameter("use_maximum", rclcpp::ParameterValue(false)); - declare_parameter("service_introspection_mode", rclcpp::ParameterValue("disabled")); } Costmap2DROS::~Costmap2DROS() @@ -223,7 +222,7 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) costmap_publisher_ = std::make_unique( shared_from_this(), layered_costmap_->getCostmap(), global_frame_, - "costmap", always_send_full_costmap_, map_vis_z_, service_introspection_mode_); + "costmap", always_send_full_costmap_, map_vis_z_); auto layers = layered_costmap_->getPlugins(); @@ -234,7 +233,7 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) std::make_unique( shared_from_this(), costmap_layer.get(), global_frame_, - layer->getName(), always_send_full_costmap_, map_vis_z_, service_introspection_mode_) + layer->getName(), always_send_full_costmap_, map_vis_z_) ); } } @@ -252,14 +251,12 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) get_cost_service_ = std::make_shared>>( std::string("get_cost_") + get_name(), - service_introspection_mode_, shared_from_this(), std::bind(&Costmap2DROS::getCostsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); // Add cleaning service - clear_costmap_service_ = std::make_unique(shared_from_this(), *this, - service_introspection_mode_); + clear_costmap_service_ = std::make_unique(shared_from_this(), *this); executor_ = std::make_shared(); executor_->add_callback_group(callback_group_, get_node_base_interface()); @@ -414,7 +411,6 @@ Costmap2DROS::getParameters() get_parameter("width", map_width_meters_); get_parameter("plugins", plugin_names_); get_parameter("filters", filter_names_); - get_parameter("service_introspection_mode", service_introspection_mode_); auto node = shared_from_this(); diff --git a/nav2_docking/opennav_docking/src/dock_database.cpp b/nav2_docking/opennav_docking/src/dock_database.cpp index 72bac7cf055..9c59a1190ff 100644 --- a/nav2_docking/opennav_docking/src/dock_database.cpp +++ b/nav2_docking/opennav_docking/src/dock_database.cpp @@ -55,16 +55,9 @@ bool DockDatabase::initialize( "Docking Server has %u dock types and %u dock instances available.", this->plugin_size(), this->instance_size()); - if(!node->has_parameter("service_introspection_mode")) { - node->declare_parameter("service_introspection_mode", "disabled"); - } - std::string service_introspection_mode_ = - node->get_parameter("service_introspection_mode").as_string(); - reload_db_service_ = std::make_shared>>( "~/reload_database", - service_introspection_mode_, node->shared_from_this(), std::bind( &DockDatabase::reloadDbCb, this, diff --git a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp index 11b7b9278a1..56a807d9195 100644 --- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp +++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp @@ -263,8 +263,6 @@ class LifecycleManager : public rclcpp::Node rclcpp::Time bond_respawn_start_time_{0}; rclcpp::Duration bond_respawn_max_duration_{10s}; - - std::string service_introspection_mode_; }; } // namespace nav2_lifecycle_manager diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp index 4b598e1b6d1..de1d918ae8e 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager.cpp @@ -44,7 +44,6 @@ LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options) declare_parameter("bond_timeout", 4.0); declare_parameter("bond_respawn_max_duration", 10.0); declare_parameter("attempt_respawn_reconnection", true); - declare_parameter("service_introspection_mode", "disabled"); registerRclPreshutdownCallback(); @@ -60,7 +59,6 @@ LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options) bond_respawn_max_duration_ = rclcpp::Duration::from_seconds(respawn_timeout_s); get_parameter("attempt_respawn_reconnection", attempt_respawn_reconnection_); - get_parameter("service_introspection_mode", service_introspection_mode_); callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); @@ -189,8 +187,7 @@ LifecycleManager::createLifecycleServiceClients() message("Creating and initializing lifecycle service clients"); for (auto & node_name : node_names_) { node_map_[node_name] = - std::make_shared(node_name, shared_from_this(), - service_introspection_mode_); + std::make_shared(node_name, shared_from_this()); } } @@ -200,7 +197,6 @@ LifecycleManager::createLifecycleServiceServers() message("Creating and initializing lifecycle service servers"); manager_srv_ = std::make_shared>( get_name() + std::string("/manage_nodes"), - service_introspection_mode_, shared_from_this(), std::bind(&LifecycleManager::managerCallback, this, _1, _2, _3), rclcpp::SystemDefaultsQoS(), @@ -208,7 +204,6 @@ LifecycleManager::createLifecycleServiceServers() is_active_srv_ = std::make_shared>( get_name() + std::string("/is_active"), - service_introspection_mode_, shared_from_this(), std::bind(&LifecycleManager::isActiveCallback, this, _1, _2, _3), rclcpp::SystemDefaultsQoS(), diff --git a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp index e6559eb02a0..4f2f14ed544 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp @@ -34,17 +34,12 @@ LifecycleManagerClient::LifecycleManagerClient( // Use parent node for service call and logging node_ = parent_node; - if(!node_->has_parameter("service_introspection_mode")) { - node_->declare_parameter("service_introspection_mode", "disabled"); - } - std::string service_introspection_mode = - node_->get_parameter("service_introspection_mode").as_string(); // Create the service clients manager_client_ = std::make_shared>( - manage_service_name_, service_introspection_mode, node_); + manage_service_name_, node_); is_active_client_ = std::make_shared>( - active_service_name_, service_introspection_mode, node_); + active_service_name_, node_); } bool diff --git a/nav2_map_server/launch/map_saver_server.launch.py b/nav2_map_server/launch/map_saver_server.launch.py index 436e357e08d..aa3f2c4f7bc 100644 --- a/nav2_map_server/launch/map_saver_server.launch.py +++ b/nav2_map_server/launch/map_saver_server.launch.py @@ -26,7 +26,6 @@ def generate_launch_description(): save_map_timeout = 2.0 free_thresh_default = 0.25 occupied_thresh_default = 0.65 - service_introspection_mode = 'disabled' # Nodes launching commands start_map_saver_server_cmd = launch_ros.actions.Node( @@ -38,7 +37,6 @@ def generate_launch_description(): {'save_map_timeout': save_map_timeout}, {'free_thresh_default': free_thresh_default}, {'occupied_thresh_default': occupied_thresh_default}, - {'service_introspection_mode': service_introspection_mode}, ], ) diff --git a/nav2_map_server/src/map_saver/map_saver.cpp b/nav2_map_server/src/map_saver/map_saver.cpp index 27e66dc93db..43226a00349 100644 --- a/nav2_map_server/src/map_saver/map_saver.cpp +++ b/nav2_map_server/src/map_saver/map_saver.cpp @@ -51,7 +51,6 @@ MapSaver::MapSaver(const rclcpp::NodeOptions & options) declare_parameter("free_thresh_default", 0.25); declare_parameter("occupied_thresh_default", 0.65); declare_parameter("map_subscribe_transient_local", true); - declare_parameter("service_introspection_mode", "disabled"); } MapSaver::~MapSaver() @@ -71,13 +70,11 @@ MapSaver::on_configure(const rclcpp_lifecycle::State & /*state*/) free_thresh_default_ = get_parameter("free_thresh_default").as_double(); occupied_thresh_default_ = get_parameter("occupied_thresh_default").as_double(); map_subscribe_transient_local_ = get_parameter("map_subscribe_transient_local").as_bool(); - std::string service_introspection_mode_ = get_parameter("service_introspection_mode").as_string(); // Create a service that saves the occupancy grid from map topic to a file save_map_service_ = std::make_shared>>( service_prefix + save_map_service_name_, - service_introspection_mode_, shared_from_this(), std::bind(&MapSaver::saveMapCallback, this, _1, _2, _3)); diff --git a/nav2_map_server/src/map_server/map_server.cpp b/nav2_map_server/src/map_server/map_server.cpp index 2a8429b9f50..a9470ef9ce6 100644 --- a/nav2_map_server/src/map_server/map_server.cpp +++ b/nav2_map_server/src/map_server/map_server.cpp @@ -71,7 +71,6 @@ MapServer::MapServer(const rclcpp::NodeOptions & options) declare_parameter("yaml_filename", rclcpp::PARAMETER_STRING); declare_parameter("topic_name", "map"); declare_parameter("frame_id", "map"); - declare_parameter("service_introspection_mode", "disabled"); } MapServer::~MapServer() @@ -85,7 +84,6 @@ MapServer::on_configure(const rclcpp_lifecycle::State & /*state*/) // Get the name of the YAML file to use (can be empty if no initial map should be used) std::string yaml_filename = get_parameter("yaml_filename").as_string(); - std::string service_introspection_mode_ = get_parameter("service_introspection_mode").as_string(); std::string topic_name = get_parameter("topic_name").as_string(); frame_id_ = get_parameter("frame_id").as_string(); @@ -113,7 +111,6 @@ MapServer::on_configure(const rclcpp_lifecycle::State & /*state*/) occ_service_ = std::make_shared>>( service_prefix + std::string(service_name_), - service_introspection_mode_, shared_from_this(), std::bind(&MapServer::getMapCallback, this, _1, _2, _3)); @@ -126,7 +123,6 @@ MapServer::on_configure(const rclcpp_lifecycle::State & /*state*/) load_map_service_ = std::make_shared>>( service_prefix + std::string(load_map_service_name_), - service_introspection_mode_, shared_from_this(), std::bind(&MapServer::loadMapCallback, this, _1, _2, _3)); diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index 058f58c0db5..fcab0b677da 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -261,8 +261,6 @@ class PlannerServer : public nav2_util::LifecycleNode // Publishers for the path rclcpp_lifecycle::LifecyclePublisher::SharedPtr plan_publisher_; - // String to store the service introspection mode - std::string service_introspection_mode_; // Service to determine if the path is valid std::shared_ptr>> is_path_valid_service_; diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index d6c622d91d0..d08caa5909a 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -54,7 +54,6 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) declare_parameter("expected_planner_frequency", 1.0); declare_parameter("action_server_result_timeout", 10.0); declare_parameter("costmap_update_timeout", 1.0); - declare_parameter("service_introspection_mode", "disabled"); get_parameter("planner_plugins", planner_ids_); if (planner_ids_ == default_ids_) { @@ -62,7 +61,6 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) declare_parameter(default_ids_[i] + ".plugin", default_types_[i]); } } - service_introspection_mode_ = get_parameter("service_introspection_mode").as_string(); // Setup the global costmap costmap_ros_ = std::make_shared( "global_costmap", std::string{get_namespace()}, @@ -201,7 +199,6 @@ PlannerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) is_path_valid_service_ = std::make_shared>>( "is_path_valid", - service_introspection_mode_, node, std::bind(&PlannerServer::isPathValid, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); diff --git a/nav2_rviz_plugins/src/costmap_cost_tool.cpp b/nav2_rviz_plugins/src/costmap_cost_tool.cpp index 40207a83805..859a14760b7 100644 --- a/nav2_rviz_plugins/src/costmap_cost_tool.cpp +++ b/nav2_rviz_plugins/src/costmap_cost_tool.cpp @@ -52,20 +52,13 @@ void CostmapCostTool::onInitialize() return; } rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node(); - if(!node->has_parameter("service_introspection_mode")) { - node->declare_parameter("service_introspection_mode", "disabled"); - } - std::string service_introspection_mode; - node->get_parameter("service_introspection_mode", service_introspection_mode); local_cost_client_ = std::make_shared>( "/local_costmap/get_cost_local_costmap", - service_introspection_mode, node); global_cost_client_ = std::make_shared>( "/global_costmap/get_cost_global_costmap", - service_introspection_mode, node); } @@ -116,11 +109,15 @@ void CostmapCostTool::callCostService(float x, float y) request->use_footprint = false; // Call local costmap service - auto local_cost_response_ = local_cost_client_->invoke_shared(request).get(); - RCLCPP_INFO(node->get_logger(), "Local costmap cost: %.1f", local_cost_response_->costs[0]); - - auto global_cost_response_ = global_cost_client_->invoke_shared(request).get(); - RCLCPP_INFO(node->get_logger(), "Global costmap cost: %.1f", global_cost_response_->costs[0]); + if(local_cost_client_->wait_for_service(std::chrono::seconds(1))) { + auto local_cost_response_ = local_cost_client_->async_call(request).get(); + RCLCPP_INFO(node->get_logger(), "Local costmap cost: %.1f", local_cost_response_->costs[0]); + } + // Call global costmap service + if(global_cost_client_->wait_for_service(std::chrono::seconds(1))) { + auto global_cost_response_ = global_cost_client_->async_call(request).get(); + RCLCPP_INFO(node->get_logger(), "Global costmap cost: %.1f", global_cost_response_->costs[0]); + } } } // namespace nav2_rviz_plugins diff --git a/nav2_smac_planner/test/test_smac_2d.cpp b/nav2_smac_planner/test/test_smac_2d.cpp index 35ef1a90d12..37936770a35 100644 --- a/nav2_smac_planner/test/test_smac_2d.cpp +++ b/nav2_smac_planner/test/test_smac_2d.cpp @@ -83,6 +83,7 @@ TEST(SmacTest, test_smac_2d) { costmap_ros->on_cleanup(rclcpp_lifecycle::State()); node2D.reset(); costmap_ros.reset(); + std::cout << "SMAC 2D test complete" << std::endl; } TEST(SmacTest, test_smac_2d_reconfigure) { diff --git a/nav2_util/include/nav2_util/lifecycle_service_client.hpp b/nav2_util/include/nav2_util/lifecycle_service_client.hpp index 61c4e34e514..46d322f7709 100644 --- a/nav2_util/include/nav2_util/lifecycle_service_client.hpp +++ b/nav2_util/include/nav2_util/lifecycle_service_client.hpp @@ -32,12 +32,10 @@ class LifecycleServiceClient { public: explicit LifecycleServiceClient( - const std::string & lifecycle_node_name, - std::string service_introspection_mode = "disabled"); + const std::string & lifecycle_node_name); LifecycleServiceClient( const std::string & lifecycle_node_name, - rclcpp::Node::SharedPtr parent_node, - std::string service_introspection_mode = "disabled"); + rclcpp::Node::SharedPtr parent_node); /// Trigger a state change /** diff --git a/nav2_util/include/nav2_util/service_client.hpp b/nav2_util/include/nav2_util/service_client.hpp index 55eb3a35d86..a743fed43a1 100644 --- a/nav2_util/include/nav2_util/service_client.hpp +++ b/nav2_util/include/nav2_util/service_client.hpp @@ -37,7 +37,6 @@ class ServiceClient */ explicit ServiceClient( const std::string & service_name, - std::string service_introspection_mode, const NodeT & provided_node) : service_name_(service_name), node_(provided_node) { @@ -50,12 +49,16 @@ class ServiceClient rclcpp::SystemDefaultsQoS(), callback_group_); rcl_service_introspection_state_t introspection_state = RCL_SERVICE_INTROSPECTION_OFF; - - if (service_introspection_mode == "disabled") { + if(!node_->has_parameter("service_introspection_mode")) { + node_->declare_parameter("service_introspection_mode", "disabled"); + } + std::string service_introspection_mode_ = + node_->get_parameter("service_introspection_mode").as_string(); + if (service_introspection_mode_ == "disabled") { introspection_state = RCL_SERVICE_INTROSPECTION_OFF; - } else if (service_introspection_mode == "metadata") { + } else if (service_introspection_mode_ == "metadata") { introspection_state = RCL_SERVICE_INTROSPECTION_METADATA; - } else if (service_introspection_mode == "contents") { + } else if (service_introspection_mode_ == "contents") { introspection_state = RCL_SERVICE_INTROSPECTION_CONTENTS; } @@ -140,37 +143,14 @@ class ServiceClient } /** - * @brief Invoke the service and block until completed + * @brief Asynchronously call the service * @param request The request object to call the service using * @return std::shared_future The shared future of the service response */ - std::shared_future invoke_shared( - typename RequestType::SharedPtr & request, - const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)) + std::shared_future async_call( + typename RequestType::SharedPtr & request) { - while (!client_->wait_for_service(std::chrono::seconds(1))) { - if (!rclcpp::ok()) { - throw std::runtime_error( - service_name_ + " service client: interrupted while waiting for service"); - } - RCLCPP_INFO( - node_->get_logger(), "%s service client: waiting for service to appear...", - service_name_.c_str()); - } - - RCLCPP_DEBUG( - node_->get_logger(), "%s service client: send async request", - service_name_.c_str()); auto future_result = client_->async_send_request(request); - - if (callback_group_executor_.spin_until_future_complete(future_result, timeout) != - rclcpp::FutureReturnCode::SUCCESS) - { - // Pending request must be manually cleaned up if execution is interrupted or timed out - client_->remove_pending_request(future_result); - throw std::runtime_error(service_name_ + " service client: async_send_request failed"); - } - return future_result.share(); } diff --git a/nav2_util/include/nav2_util/service_server.hpp b/nav2_util/include/nav2_util/service_server.hpp index 9eb1abda801..dcaab95e882 100644 --- a/nav2_util/include/nav2_util/service_server.hpp +++ b/nav2_util/include/nav2_util/service_server.hpp @@ -38,7 +38,6 @@ class ServiceServer explicit ServiceServer( const std::string & service_name, - std::string service_introspection_mode, const NodeT & node, CallbackType callback, const rclcpp::QoS & qos = rclcpp::ServicesQoS(), @@ -55,11 +54,16 @@ class ServiceServer callback_group); rcl_service_introspection_state_t introspection_state = RCL_SERVICE_INTROSPECTION_OFF; - if (service_introspection_mode == "disabled") { + if(!node->has_parameter("service_introspection_mode")) { + node->declare_parameter("service_introspection_mode", "disabled"); + } + std::string service_introspection_mode_ = + node->get_parameter("service_introspection_mode").as_string(); + if (service_introspection_mode_ == "disabled") { introspection_state = RCL_SERVICE_INTROSPECTION_OFF; - } else if (service_introspection_mode == "metadata") { + } else if (service_introspection_mode_ == "metadata") { introspection_state = RCL_SERVICE_INTROSPECTION_METADATA; - } else if (service_introspection_mode == "contents") { + } else if (service_introspection_mode_ == "contents") { introspection_state = RCL_SERVICE_INTROSPECTION_CONTENTS; } diff --git a/nav2_util/src/lifecycle_service_client.cpp b/nav2_util/src/lifecycle_service_client.cpp index 7ad8505443c..91a550abe45 100644 --- a/nav2_util/src/lifecycle_service_client.cpp +++ b/nav2_util/src/lifecycle_service_client.cpp @@ -31,11 +31,10 @@ namespace nav2_util { LifecycleServiceClient::LifecycleServiceClient( - const string & lifecycle_node_name, - std::string service_introspection_mode) + const string & lifecycle_node_name) : node_(generate_internal_node(lifecycle_node_name + "_lifecycle_client")), - change_state_(lifecycle_node_name + "/change_state", service_introspection_mode, node_), - get_state_(lifecycle_node_name + "/get_state", service_introspection_mode, node_) + change_state_(lifecycle_node_name + "/change_state", node_), + get_state_(lifecycle_node_name + "/get_state", node_) { // Block until server is up rclcpp::Rate r(20); @@ -48,11 +47,10 @@ LifecycleServiceClient::LifecycleServiceClient( LifecycleServiceClient::LifecycleServiceClient( const string & lifecycle_node_name, - rclcpp::Node::SharedPtr parent_node, - std::string service_introspection_mode) + rclcpp::Node::SharedPtr parent_node) : node_(parent_node), - change_state_(lifecycle_node_name + "/change_state", service_introspection_mode, node_), - get_state_(lifecycle_node_name + "/get_state", service_introspection_mode, node_) + change_state_(lifecycle_node_name + "/change_state", node_), + get_state_(lifecycle_node_name + "/get_state", node_) { // Block until server is up rclcpp::Rate r(20); diff --git a/nav2_util/test/test_service_client.cpp b/nav2_util/test/test_service_client.cpp index 646e0cc27fa..ab6876c27ea 100644 --- a/nav2_util/test/test_service_client.cpp +++ b/nav2_util/test/test_service_client.cpp @@ -36,9 +36,8 @@ class TestServiceClient : public ServiceClient public: TestServiceClient( const std::string & name, - std::string service_introspection_mode = "disabled", const rclcpp::Node::SharedPtr & provided_node = rclcpp::Node::SharedPtr()) - : ServiceClient(name, service_introspection_mode, provided_node) + : ServiceClient(name, provided_node) {} string name() {return node_->get_name();} @@ -47,9 +46,8 @@ class TestServiceClient : public ServiceClient TEST(ServiceClient, can_ServiceClient_use_passed_in_node) { - std::string service_introspection_mode_ = "disabled"; auto node = rclcpp::Node::make_shared("test_node"); - TestServiceClient t("bar", service_introspection_mode_, node); + TestServiceClient t("bar", node); ASSERT_EQ(t.getNode(), node); ASSERT_EQ(t.name(), "test_node"); } @@ -72,8 +70,7 @@ TEST(ServiceClient, can_ServiceClient_invoke_in_callback) auto pub_thread = std::thread([&]() {rclcpp::spin(pub_node);}); auto sub_node = rclcpp::Node::make_shared("sub_node"); - std::string service_introspection_mode_ = "metadata"; - ServiceClient client("empty_srv", service_introspection_mode_, sub_node); + ServiceClient client("empty_srv", sub_node); auto sub = sub_node->create_subscription( "empty_topic", rclcpp::QoS(1), @@ -91,45 +88,11 @@ TEST(ServiceClient, can_ServiceClient_invoke_in_callback) ASSERT_EQ(a, 1); } -TEST(ServiceClient, can_ServiceClient_invoke_shared) -{ - rclcpp::init(0, nullptr); - int a = 0; - auto service_node = rclcpp::Node::make_shared("service_node"); - auto service = service_node->create_service( - "empty_srv", - [&a](std_srvs::srv::Empty::Request::SharedPtr, std_srvs::srv::Empty::Response::SharedPtr) { - a = 1; - }); - auto srv_thread = std::thread([&]() {rclcpp::spin(service_node);}); - - auto pub_node = rclcpp::Node::make_shared("pub_node"); - auto pub = pub_node->create_publisher( - "empty_topic", - rclcpp::QoS(1).transient_local()); - auto pub_thread = std::thread([&]() {rclcpp::spin(pub_node);}); - - auto sub_node = rclcpp::Node::make_shared("sub_node"); - std::string service_introspection_mode_ = "contents"; - ServiceClient client("empty_srv", service_introspection_mode_, sub_node); - - auto req = std::make_shared(); - auto future_result = client.invoke_shared(req); - - ASSERT_EQ(future_result.wait_for(std::chrono::seconds(1)), std::future_status::ready); - ASSERT_EQ(a, 1); - - rclcpp::shutdown(); - srv_thread.join(); - pub_thread.join(); -} - TEST(ServiceClient, can_ServiceClient_timeout) { rclcpp::init(0, nullptr); auto node = rclcpp::Node::make_shared("test_node"); - std::string service_introspection_mode_ = "disabled"; - TestServiceClient t("bar", service_introspection_mode_, node); + TestServiceClient t("bar", node); rclcpp::spin_some(node); bool ready = t.wait_for_service(std::chrono::milliseconds(10)); rclcpp::shutdown(); diff --git a/nav2_util/test/test_service_server.cpp b/nav2_util/test/test_service_server.cpp index 23a9e0ed2ed..40b01d05b5e 100644 --- a/nav2_util/test/test_service_server.cpp +++ b/nav2_util/test/test_service_server.cpp @@ -36,10 +36,9 @@ class TestServiceServer : public ServiceServer public: TestServiceServer( const std::string & name, - std::string service_introspection_mode, const rclcpp::Node::SharedPtr & provided_node, CallbackType callback) - : ServiceServer(name, service_introspection_mode, provided_node, callback) + : ServiceServer(name, provided_node, callback) {} }; @@ -47,7 +46,6 @@ TEST(ServiceServer, can_ServiceServer_handle_request) { int a = 0; auto node = rclcpp::Node::make_shared("test_node"); - std::string service_introspection_mode_ = "disabled"; auto callback = [&a](const std::shared_ptr, const std::shared_ptr, @@ -55,7 +53,7 @@ TEST(ServiceServer, can_ServiceServer_handle_request) a = 1; }; - TestServiceServer server("empty_srv", service_introspection_mode_, node, callback); + TestServiceServer server("empty_srv", node, callback); auto client_node = rclcpp::Node::make_shared("client_node"); auto client = client_node->create_client("empty_srv"); diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index 92bd1981818..578b33b1339 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -221,7 +221,6 @@ class WaypointFollower : public nav2_util::LifecycleNode waypoint_task_executor_; std::string waypoint_task_executor_id_; std::string waypoint_task_executor_type_; - std::string service_introspection_mode_; }; } // namespace nav2_waypoint_follower diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 060d177e07d..55db83df847 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -35,7 +35,6 @@ WaypointFollower::WaypointFollower(const rclcpp::NodeOptions & options) RCLCPP_INFO(get_logger(), "Creating"); declare_parameter("stop_on_failure", true); - declare_parameter("service_introspection_mode", "disabled"); declare_parameter("loop_rate", 20); declare_parameter("action_server_result_timeout", 900.0); @@ -62,7 +61,6 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & state) auto node = shared_from_this(); stop_on_failure_ = get_parameter("stop_on_failure").as_bool(); - service_introspection_mode_ = get_parameter("service_introspection_mode").as_string(); loop_rate_ = get_parameter("loop_rate").as_int(); waypoint_task_executor_id_ = get_parameter("waypoint_task_executor_plugin").as_string(); global_frame_id_ = get_parameter("global_frame_id").as_string(); @@ -98,7 +96,6 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & state) nav2_util::ServiceClient>>( "/fromLL", - service_introspection_mode_, node); gps_action_server_ = std::make_unique( From c22a1621bb7219dc5ab06db2aa81971d0f4d352b Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Sat, 22 Mar 2025 17:05:13 +0000 Subject: [PATCH 06/19] Add typedef; Fix costmap cost tool and bt service Signed-off-by: mini-1235 --- nav2_amcl/include/nav2_amcl/amcl_node.hpp | 12 +++---- .../nav2_behavior_tree/bt_service_node.hpp | 6 ++-- .../nav2_costmap_2d/clear_costmap_service.hpp | 12 +++---- .../nav2_costmap_2d/costmap_2d_publisher.hpp | 6 ++-- .../nav2_costmap_2d/costmap_2d_ros.hpp | 4 +-- .../costmap_filters/costmap_filter.hpp | 4 +-- .../include/opennav_docking/dock_database.hpp | 4 +-- .../lifecycle_manager.hpp | 4 +-- .../include/nav2_map_server/map_saver.hpp | 4 +-- .../include/nav2_map_server/map_server.hpp | 8 ++--- .../test/trajectory_visualizer_tests.cpp | 2 +- .../include/nav2_planner/planner_server.hpp | 4 +-- .../nav2_rviz_plugins/costmap_cost_tool.hpp | 3 ++ nav2_rviz_plugins/src/costmap_cost_tool.cpp | 31 +++++++++++++++---- nav2_smac_planner/src/smac_planner_2d.cpp | 4 +++ nav2_smac_planner/src/smac_planner_hybrid.cpp | 5 +++ .../include/nav2_util/service_client.hpp | 26 ++++++++++++---- .../include/nav2_util/service_server.hpp | 1 + 18 files changed, 92 insertions(+), 48 deletions(-) diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 00f8d78bd01..5910181b3a6 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -207,8 +207,8 @@ class AmclNode : public nav2_util::LifecycleNode * @brief Initialize state services */ void initServices(); - std::shared_ptr>> global_loc_srv_; + nav2_util::ServiceServer>::SharedPtr global_loc_srv_; /* * @brief Service callback for a global relocalization request */ @@ -218,8 +218,8 @@ class AmclNode : public nav2_util::LifecycleNode std::shared_ptr response); // service server for providing an initial pose guess - std::shared_ptr>> initial_guess_srv_; + nav2_util::ServiceServer>::SharedPtr initial_guess_srv_; /* * @brief Service callback for an initial pose guess request */ @@ -229,8 +229,8 @@ class AmclNode : public nav2_util::LifecycleNode std::shared_ptr response); // Let amcl update samples without requiring motion - std::shared_ptr>> nomotion_update_srv_; + nav2_util::ServiceServer>::SharedPtr nomotion_update_srv_; /* * @brief Request an AMCL update even though the robot hasn't moved */ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index 41a00d4022a..fae2f0dc0b5 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp @@ -116,10 +116,8 @@ class BtServiceNode : public BT::ActionNodeBase callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); - service_client_ = node_->create_client( - service_name_, - rclcpp::SystemDefaultsQoS(), - callback_group_); + service_client_ = std::make_shared>( + service_name_, node_, callback_group_); } } diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp index ccaa7b8cacf..eae9a90b524 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp @@ -76,8 +76,8 @@ class ClearCostmapService unsigned char reset_value_; // Server for clearing the costmap - std::shared_ptr>> clear_except_service_; + nav2_util::ServiceServer>::SharedPtr clear_except_service_; /** * @brief Callback to clear costmap except in a given region */ @@ -86,8 +86,8 @@ class ClearCostmapService const std::shared_ptr request, const std::shared_ptr response); - std::shared_ptr>> clear_around_service_; + nav2_util::ServiceServer>::SharedPtr clear_around_service_; /** * @brief Callback to clear costmap in a given region */ @@ -96,8 +96,8 @@ class ClearCostmapService const std::shared_ptr request, const std::shared_ptr response); - std::shared_ptr>> clear_entire_service_; + nav2_util::ServiceServer>::SharedPtr clear_entire_service_; /** * @brief Callback to clear costmap */ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp index 8d16be1bb7c..d6e1b8038b3 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp @@ -181,9 +181,9 @@ class Costmap2DPublisher costmap_raw_update_pub_; // Service for getting the costmaps - std::shared_ptr>> - costmap_service_; + nav2_util::ServiceServer>::SharedPtr + costmap_service_; float grid_resolution_; unsigned int grid_width_, grid_height_; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp index 45568f8fd30..b7f3bf80c83 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp @@ -415,8 +415,8 @@ class Costmap2DROS : public nav2_util::LifecycleNode std::vector padded_footprint_; // Services - std::shared_ptr>> get_cost_service_; + nav2_util::ServiceServer>::SharedPtr get_cost_service_; std::unique_ptr clear_costmap_service_; // Dynamic parameters handler diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp index 19b4e4d23cd..00a3cb833c1 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp @@ -242,8 +242,8 @@ class CostmapFilter : public Layer /** * @brief: A service to enable/disable costmap filter */ - std::shared_ptr>> enable_service_; + nav2_util::ServiceServer>::SharedPtr enable_service_; private: /** diff --git a/nav2_docking/opennav_docking/include/opennav_docking/dock_database.hpp b/nav2_docking/opennav_docking/include/opennav_docking/dock_database.hpp index 67a27106c43..d151992ea42 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/dock_database.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/dock_database.hpp @@ -133,8 +133,8 @@ class DockDatabase DockPluginMap dock_plugins_; DockMap dock_instances_; pluginlib::ClassLoader dock_loader_; - std::shared_ptr>> reload_db_service_; + nav2_util::ServiceServer>::SharedPtr reload_db_service_; }; } // namespace opennav_docking diff --git a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp index 56a807d9195..2f3aa38bf2a 100644 --- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp +++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp @@ -75,8 +75,8 @@ class LifecycleManager : public rclcpp::Node std::unique_ptr service_thread_; // The services provided by this node - std::shared_ptr> manager_srv_; - std::shared_ptr> is_active_srv_; + nav2_util::ServiceServer::SharedPtr manager_srv_; + nav2_util::ServiceServer::SharedPtr is_active_srv_; /** * @brief Lifecycle node manager callback function * @param request_header Header of the service request diff --git a/nav2_map_server/include/nav2_map_server/map_saver.hpp b/nav2_map_server/include/nav2_map_server/map_saver.hpp index c42d1aa20ba..123e6723b80 100644 --- a/nav2_map_server/include/nav2_map_server/map_saver.hpp +++ b/nav2_map_server/include/nav2_map_server/map_saver.hpp @@ -112,8 +112,8 @@ class MapSaver : public nav2_util::LifecycleNode // The name of the service for saving a map from topic const std::string save_map_service_name_{"save_map"}; // A service to save the map to a file at run time (SaveMap) - std::shared_ptr>> save_map_service_; + nav2_util::ServiceServer>::SharedPtr save_map_service_; }; } // namespace nav2_map_server diff --git a/nav2_map_server/include/nav2_map_server/map_server.hpp b/nav2_map_server/include/nav2_map_server/map_server.hpp index a38a69f8c7f..4d73ac1d313 100644 --- a/nav2_map_server/include/nav2_map_server/map_server.hpp +++ b/nav2_map_server/include/nav2_map_server/map_server.hpp @@ -127,12 +127,12 @@ class MapServer : public nav2_util::LifecycleNode const std::string load_map_service_name_{"load_map"}; // A service to provide the occupancy grid (GetMap) and the message to return - std::shared_ptr>> occ_service_; + nav2_util::ServiceServer>::SharedPtr occ_service_; // A service to load the occupancy grid from file at run time (LoadMap) - std::shared_ptr>> load_map_service_; + nav2_util::ServiceServer>::SharedPtr load_map_service_; // A topic on which the occupancy grid will be published rclcpp_lifecycle::LifecyclePublisher::SharedPtr occ_pub_; diff --git a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp index e9a30d9f237..fd172c7b445 100644 --- a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp +++ b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp @@ -82,7 +82,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory) EXPECT_EQ(received_msg.markers.size(), 0u); // Now populated with content, should publish - optimal_trajectory = Eigen::ArrayXXf::Ones(20, 2); + optimal_trajectory = Eigen::ArrayXXf::Ones(20, 3); vis.add(optimal_trajectory, "Optimal Trajectory", bogus_stamp); vis.visualize(bogus_path); diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index fcab0b677da..21bc032e5fa 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -262,8 +262,8 @@ class PlannerServer : public nav2_util::LifecycleNode rclcpp_lifecycle::LifecyclePublisher::SharedPtr plan_publisher_; // Service to determine if the path is valid - std::shared_ptr>> is_path_valid_service_; + nav2_util::ServiceServer>::SharedPtr is_path_valid_service_; }; } // namespace nav2_planner diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp index d92f6132bc5..2d9e3d10986 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp @@ -43,6 +43,9 @@ class CostmapCostTool : public rviz_common::Tool void callCostService(float x, float y); + void handleLocalCostResponse(rclcpp::Client::SharedFuture); + void handleGlobalCostResponse(rclcpp::Client::SharedFuture); + private Q_SLOTS: private: diff --git a/nav2_rviz_plugins/src/costmap_cost_tool.cpp b/nav2_rviz_plugins/src/costmap_cost_tool.cpp index 859a14760b7..6716bc9e434 100644 --- a/nav2_rviz_plugins/src/costmap_cost_tool.cpp +++ b/nav2_rviz_plugins/src/costmap_cost_tool.cpp @@ -109,16 +109,35 @@ void CostmapCostTool::callCostService(float x, float y) request->use_footprint = false; // Call local costmap service - if(local_cost_client_->wait_for_service(std::chrono::seconds(1))) { - auto local_cost_response_ = local_cost_client_->async_call(request).get(); - RCLCPP_INFO(node->get_logger(), "Local costmap cost: %.1f", local_cost_response_->costs[0]); + if (local_cost_client_->wait_for_service(std::chrono::seconds(1))) { + local_cost_client_->async_call( + request, + std::bind(&CostmapCostTool::handleLocalCostResponse, this, std::placeholders::_1)); } + // Call global costmap service - if(global_cost_client_->wait_for_service(std::chrono::seconds(1))) { - auto global_cost_response_ = global_cost_client_->async_call(request).get(); - RCLCPP_INFO(node->get_logger(), "Global costmap cost: %.1f", global_cost_response_->costs[0]); + if (global_cost_client_->wait_for_service(std::chrono::seconds(1))) { + global_cost_client_->async_call( + request, + std::bind(&CostmapCostTool::handleGlobalCostResponse, this, std::placeholders::_1)); } } + +void CostmapCostTool::handleLocalCostResponse( + rclcpp::Client::SharedFuture future) +{ + rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node(); + auto response = future.get(); + RCLCPP_INFO(node->get_logger(), "Local costmap cost: %.1f", response->costs[0]); +} + +void CostmapCostTool::handleGlobalCostResponse( + rclcpp::Client::SharedFuture future) +{ + rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node(); + auto response = future.get(); + RCLCPP_INFO(node->get_logger(), "Global costmap cost: %.1f", response->costs[0]); +} } // namespace nav2_rviz_plugins #include diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 99734849a0a..3936fc7157d 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -93,6 +93,10 @@ void SmacPlanner2D::configure( nav2_util::declare_parameter_if_not_declared( node, name + ".max_planning_time", rclcpp::ParameterValue(2.0)); node->get_parameter(name + ".max_planning_time", _max_planning_time); + // Note that we need to declare it here to prevent the parameter from being declared in the + // dynamic reconfigure callback + nav2_util::declare_parameter_if_not_declared( + node, "service_introspection_mode", rclcpp::ParameterValue("disabled")); _motion_model = MotionModel::TWOD; diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index fcce89cdfea..8522817ed9e 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -166,6 +166,11 @@ void SmacPlannerHybrid::configure( nav2_util::declare_parameter_if_not_declared( node, name + ".motion_model_for_search", rclcpp::ParameterValue(std::string("DUBIN"))); node->get_parameter(name + ".motion_model_for_search", _motion_model_for_search); + // Note that we need to declare it here to prevent the parameter from being declared in the + // dynamic reconfigure callback + nav2_util::declare_parameter_if_not_declared( + node, "service_introspection_mode", rclcpp::ParameterValue("disabled")); + _motion_model = fromString(_motion_model_for_search); if (_motion_model == MotionModel::UNKNOWN) { RCLCPP_WARN( diff --git a/nav2_util/include/nav2_util/service_client.hpp b/nav2_util/include/nav2_util/service_client.hpp index a743fed43a1..fcf31a97c90 100644 --- a/nav2_util/include/nav2_util/service_client.hpp +++ b/nav2_util/include/nav2_util/service_client.hpp @@ -16,7 +16,7 @@ #define NAV2_UTIL__SERVICE_CLIENT_HPP_ #include - +#include #include "rclcpp/rclcpp.hpp" namespace nav2_util @@ -37,13 +37,19 @@ class ServiceClient */ explicit ServiceClient( const std::string & service_name, - const NodeT & provided_node) + const NodeT & provided_node, rclcpp::CallbackGroup::SharedPtr callback_group = nullptr) : service_name_(service_name), node_(provided_node) { - callback_group_ = node_->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive, - false); - callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); + if(!callback_group) { + callback_group_ = node_->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + callback_group_executor_.add_callback_group(callback_group_, + node_->get_node_base_interface()); + } else { + callback_group_ = callback_group; + } + client_ = node_->template create_client( service_name, rclcpp::SystemDefaultsQoS(), @@ -154,6 +160,14 @@ class ServiceClient return future_result.share(); } + template + void async_call( + typename RequestType::SharedPtr & request, + CallbackT && callback) + { + client_->async_send_request(request, std::forward(callback)); + } + /** * @brief Block until a service is available or timeout * @param timeout Maximum timeout to wait for, default infinite diff --git a/nav2_util/include/nav2_util/service_server.hpp b/nav2_util/include/nav2_util/service_server.hpp index dcaab95e882..796bc80098f 100644 --- a/nav2_util/include/nav2_util/service_server.hpp +++ b/nav2_util/include/nav2_util/service_server.hpp @@ -35,6 +35,7 @@ class ServiceServer using ResponseType = typename ServiceT::Response; using CallbackType = std::function, const std::shared_ptr, std::shared_ptr)>; + using SharedPtr = std::shared_ptr>; explicit ServiceServer( const std::string & service_name, From 3a0c50dabfa1fd999ccd0a286362761dc1b6d7c6 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Sat, 22 Mar 2025 18:13:23 +0000 Subject: [PATCH 07/19] Rebase; Add tests for coverage Signed-off-by: mini-1235 --- .../include/nav2_util/service_client.hpp | 6 +++ nav2_util/test/test_service_client.cpp | 14 +++++-- nav2_util/test/test_service_server.cpp | 40 +++++++++++-------- 3 files changed, 40 insertions(+), 20 deletions(-) diff --git a/nav2_util/include/nav2_util/service_client.hpp b/nav2_util/include/nav2_util/service_client.hpp index fcf31a97c90..d7c20f849cf 100644 --- a/nav2_util/include/nav2_util/service_client.hpp +++ b/nav2_util/include/nav2_util/service_client.hpp @@ -160,6 +160,12 @@ class ServiceClient return future_result.share(); } + + /** + * @brief Asynchronously call the service with a callback + * @param request The request object to call the service using + * @param callback The callback to call when the service response is received + */ template void async_call( typename RequestType::SharedPtr & request, diff --git a/nav2_util/test/test_service_client.cpp b/nav2_util/test/test_service_client.cpp index ab6876c27ea..b36b242e791 100644 --- a/nav2_util/test/test_service_client.cpp +++ b/nav2_util/test/test_service_client.cpp @@ -46,10 +46,16 @@ class TestServiceClient : public ServiceClient TEST(ServiceClient, can_ServiceClient_use_passed_in_node) { - auto node = rclcpp::Node::make_shared("test_node"); - TestServiceClient t("bar", node); - ASSERT_EQ(t.getNode(), node); - ASSERT_EQ(t.name(), "test_node"); + std::vector introspection_modes = { + "disabled", "metadata", "contents" + }; + for(const auto & mode : introspection_modes) { + auto node = rclcpp::Node::make_shared("test_node" + mode); + node->declare_parameter("service_introspection_mode", mode); + TestServiceClient t("bar", node); + ASSERT_EQ(t.getNode(), node); + ASSERT_EQ(t.name(), "test_node" + mode); + } } TEST(ServiceClient, can_ServiceClient_invoke_in_callback) diff --git a/nav2_util/test/test_service_server.cpp b/nav2_util/test/test_service_server.cpp index 40b01d05b5e..582f047259c 100644 --- a/nav2_util/test/test_service_server.cpp +++ b/nav2_util/test/test_service_server.cpp @@ -42,29 +42,37 @@ class TestServiceServer : public ServiceServer {} }; -TEST(ServiceServer, can_ServiceServer_handle_request) +TEST(ServiceServer, can_handle_all_introspection_modes) { - int a = 0; - auto node = rclcpp::Node::make_shared("test_node"); + std::vector introspection_modes = { + "disabled", "metadata", "contents" + }; - auto callback = [&a](const std::shared_ptr, - const std::shared_ptr, - std::shared_ptr) { - a = 1; - }; + for (const auto & mode : introspection_modes) { + int a = 0; + auto node = rclcpp::Node::make_shared("test_node_" + mode); - TestServiceServer server("empty_srv", node, callback); + node->declare_parameter("service_introspection_mode", mode); - auto client_node = rclcpp::Node::make_shared("client_node"); - auto client = client_node->create_client("empty_srv"); + auto callback = [&a](const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr) { + a = 1; + }; - auto req = std::make_shared(); - auto result = client->async_send_request(req); + TestServiceServer server("empty_srv_" + mode, node, callback); - rclcpp::spin_some(node); - rclcpp::spin_some(client_node); + auto client_node = rclcpp::Node::make_shared("client_node_" + mode); + auto client = client_node->create_client("empty_srv_" + mode); - ASSERT_EQ(a, 1); + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); + + auto req = std::make_shared(); + auto result = client->async_send_request(req); + rclcpp::spin_some(node); + rclcpp::spin_some(client_node); + ASSERT_EQ(a, 1); + } } int main(int argc, char **argv) From c68f09e627e5f2d40a87fbb86729814433c161b4 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Tue, 25 Mar 2025 08:07:43 +0000 Subject: [PATCH 08/19] Fix costmap cost tool; Typedef Signed-off-by: mini-1235 --- .../nav2_behavior_tree/bt_service_node.hpp | 2 +- .../condition/is_path_valid_condition.hpp | 2 +- .../costmap_filters/costmap_filter.cpp | 2 +- nav2_costmap_2d/src/clear_costmap_service.cpp | 6 +++--- nav2_costmap_2d/src/costmap_2d_publisher.cpp | 2 +- .../opennav_docking/src/dock_database.cpp | 2 +- .../lifecycle_manager_client.hpp | 4 ++-- nav2_planner/src/planner_server.cpp | 1 + .../nav2_rviz_plugins/costmap_cost_tool.hpp | 4 ++-- nav2_rviz_plugins/src/costmap_cost_tool.cpp | 8 +++++--- .../include/nav2_util/service_client.hpp | 20 +++++++++---------- .../include/nav2_util/service_server.hpp | 12 ++++------- 12 files changed, 31 insertions(+), 34 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index fae2f0dc0b5..0352792c162 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp @@ -264,7 +264,7 @@ class BtServiceNode : public BT::ActionNodeBase } std::string service_name_, service_node_name_; - std::shared_ptr> service_client_; + typename nav2_util::ServiceClient::SharedPtr service_client_; std::shared_ptr request_; // The node that will be used for any ROS operations diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp index 9ae93a3bf47..4e20e931605 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp @@ -74,7 +74,7 @@ class IsPathValidCondition : public BT::ConditionNode private: rclcpp::Node::SharedPtr node_; - std::shared_ptr> client_; + nav2_util::ServiceClient::SharedPtr client_; // The timeout value while waiting for a response from the // is path valid service std::chrono::milliseconds server_timeout_; diff --git a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp index 36ac8ab9585..6a5d83dd583 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp @@ -86,7 +86,7 @@ void CostmapFilter::onInitialize() enable_service_ = std::make_shared>>( name_ + "/toggle_filter", - node->shared_from_this(), + node, std::bind(&CostmapFilter::enableCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); } catch (const std::exception & ex) { diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp index add69369a55..2fa4e4a3bb1 100644 --- a/nav2_costmap_2d/src/clear_costmap_service.cpp +++ b/nav2_costmap_2d/src/clear_costmap_service.cpp @@ -43,7 +43,7 @@ ClearCostmapService::ClearCostmapService( clear_except_service_ = std::make_shared>>( std::string("clear_except_") + costmap_.getName(), - node->shared_from_this(), + node, std::bind( &ClearCostmapService::clearExceptRegionCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); @@ -51,7 +51,7 @@ ClearCostmapService::ClearCostmapService( clear_around_service_ = std::make_shared>>( std::string("clear_around_") + costmap_.getName(), - node->shared_from_this(), + node, std::bind( &ClearCostmapService::clearAroundRobotCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); @@ -59,7 +59,7 @@ ClearCostmapService::ClearCostmapService( clear_entire_service_ = std::make_shared>>( std::string("clear_entirely_") + costmap_.getName(), - node->shared_from_this(), + node, std::bind( &ClearCostmapService::clearEntireCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp index c06907c812a..f7ee5a2c544 100644 --- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp @@ -85,7 +85,7 @@ Costmap2DPublisher::Costmap2DPublisher( costmap_service_ = std::make_shared>>( std::string("get_") + topic_name, - node->shared_from_this(), + node, std::bind( &Costmap2DPublisher::costmap_service_callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); diff --git a/nav2_docking/opennav_docking/src/dock_database.cpp b/nav2_docking/opennav_docking/src/dock_database.cpp index 9c59a1190ff..cd62cef607b 100644 --- a/nav2_docking/opennav_docking/src/dock_database.cpp +++ b/nav2_docking/opennav_docking/src/dock_database.cpp @@ -58,7 +58,7 @@ bool DockDatabase::initialize( reload_db_service_ = std::make_shared>>( "~/reload_database", - node->shared_from_this(), + node, std::bind( &DockDatabase::reloadDbCb, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); diff --git a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp index 80e9305e9e0..ddcfe26daca 100644 --- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp +++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp @@ -108,8 +108,8 @@ class LifecycleManagerClient // The node to use for the service call rclcpp::Node::SharedPtr node_; - std::shared_ptr> manager_client_; - std::shared_ptr> is_active_client_; + nav2_util::ServiceClient::SharedPtr manager_client_; + nav2_util::ServiceClient::SharedPtr is_active_client_; std::string manage_service_name_; std::string active_service_name_; }; diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index d08caa5909a..f949f980a95 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -61,6 +61,7 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) declare_parameter(default_ids_[i] + ".plugin", default_types_[i]); } } + // Setup the global costmap costmap_ros_ = std::make_shared( "global_costmap", std::string{get_namespace()}, diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp index 2d9e3d10986..837201ea231 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp @@ -49,8 +49,8 @@ class CostmapCostTool : public rviz_common::Tool private Q_SLOTS: private: - std::shared_ptr> local_cost_client_; - std::shared_ptr> global_cost_client_; + nav2_util::ServiceClient::SharedPtr local_cost_client_; + nav2_util::ServiceClient::SharedPtr global_cost_client_; // The Node pointer that we need to keep alive for the duration of this plugin. std::shared_ptr node_ptr_; diff --git a/nav2_rviz_plugins/src/costmap_cost_tool.cpp b/nav2_rviz_plugins/src/costmap_cost_tool.cpp index 6716bc9e434..1ffa22a5386 100644 --- a/nav2_rviz_plugins/src/costmap_cost_tool.cpp +++ b/nav2_rviz_plugins/src/costmap_cost_tool.cpp @@ -55,11 +55,13 @@ void CostmapCostTool::onInitialize() local_cost_client_ = std::make_shared>( "/local_costmap/get_cost_local_costmap", - node); + node, + node->get_node_base_interface()->get_default_callback_group()); global_cost_client_ = std::make_shared>( "/global_costmap/get_cost_global_costmap", - node); + node, + node->get_node_base_interface()->get_default_callback_group()); } void CostmapCostTool::activate() {} @@ -109,7 +111,7 @@ void CostmapCostTool::callCostService(float x, float y) request->use_footprint = false; // Call local costmap service - if (local_cost_client_->wait_for_service(std::chrono::seconds(1))) { + if(local_cost_client_->wait_for_service(std::chrono::seconds(1))) { local_cost_client_->async_call( request, std::bind(&CostmapCostTool::handleLocalCostResponse, this, std::placeholders::_1)); diff --git a/nav2_util/include/nav2_util/service_client.hpp b/nav2_util/include/nav2_util/service_client.hpp index d7c20f849cf..317dc4845fc 100644 --- a/nav2_util/include/nav2_util/service_client.hpp +++ b/nav2_util/include/nav2_util/service_client.hpp @@ -16,15 +16,16 @@ #define NAV2_UTIL__SERVICE_CLIENT_HPP_ #include -#include +#include #include "rclcpp/rclcpp.hpp" +#include namespace nav2_util { /** * @class nav2_util::ServiceClient - * @brief A simple wrapper on ROS2 services for invoke() and block-style calling + * @brief A simple wrapper on ROS2 services client */ template class ServiceClient @@ -58,13 +59,11 @@ class ServiceClient if(!node_->has_parameter("service_introspection_mode")) { node_->declare_parameter("service_introspection_mode", "disabled"); } - std::string service_introspection_mode_ = + std::string service_introspection_mode = node_->get_parameter("service_introspection_mode").as_string(); - if (service_introspection_mode_ == "disabled") { - introspection_state = RCL_SERVICE_INTROSPECTION_OFF; - } else if (service_introspection_mode_ == "metadata") { + if (service_introspection_mode == "metadata") { introspection_state = RCL_SERVICE_INTROSPECTION_METADATA; - } else if (service_introspection_mode_ == "contents") { + } else if (service_introspection_mode == "contents") { introspection_state = RCL_SERVICE_INTROSPECTION_CONTENTS; } @@ -74,6 +73,7 @@ class ServiceClient using RequestType = typename ServiceT::Request; using ResponseType = typename ServiceT::Response; + using SharedPtr = std::shared_ptr>; /** * @brief Invoke the service and block until completed or timed out @@ -167,11 +167,9 @@ class ServiceClient * @param callback The callback to call when the service response is received */ template - void async_call( - typename RequestType::SharedPtr & request, - CallbackT && callback) + void async_call(typename RequestType::SharedPtr request, CallbackT && callback) { - client_->async_send_request(request, std::forward(callback)); + client_->async_send_request(request, callback); } /** diff --git a/nav2_util/include/nav2_util/service_server.hpp b/nav2_util/include/nav2_util/service_server.hpp index 796bc80098f..acdc374a9ea 100644 --- a/nav2_util/include/nav2_util/service_server.hpp +++ b/nav2_util/include/nav2_util/service_server.hpp @@ -25,7 +25,7 @@ namespace nav2_util /** * @class nav2_util::ServiceServer - * @brief A simple wrapper on ROS2 services for invoke() and block-style calling + * @brief A simple wrapper on ROS2 services server */ template class ServiceServer @@ -58,13 +58,11 @@ class ServiceServer if(!node->has_parameter("service_introspection_mode")) { node->declare_parameter("service_introspection_mode", "disabled"); } - std::string service_introspection_mode_ = + std::string service_introspection_mode = node->get_parameter("service_introspection_mode").as_string(); - if (service_introspection_mode_ == "disabled") { - introspection_state = RCL_SERVICE_INTROSPECTION_OFF; - } else if (service_introspection_mode_ == "metadata") { + if (service_introspection_mode == "metadata") { introspection_state = RCL_SERVICE_INTROSPECTION_METADATA; - } else if (service_introspection_mode_ == "contents") { + } else if (service_introspection_mode == "contents") { introspection_state = RCL_SERVICE_INTROSPECTION_CONTENTS; } @@ -76,8 +74,6 @@ class ServiceServer std::string service_name_; CallbackType callback_; typename rclcpp::Service::SharedPtr server_; - rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr - on_set_parameters_callback_handle_; }; } // namespace nav2_util From f7cf25a03a40970835b18845bdfc56c4f11ddaeb Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Tue, 25 Mar 2025 15:11:35 +0000 Subject: [PATCH 09/19] Cleanup Signed-off-by: mini-1235 --- .../condition/is_path_valid_condition.cpp | 1 - nav2_bringup/params/nav2_params.yaml | 16 ++++++++-------- .../include/nav2_map_server/map_server.hpp | 1 - nav2_smac_planner/test/test_smac_2d.cpp | 1 - 4 files changed, 8 insertions(+), 11 deletions(-) diff --git a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp index 15272cfead4..80fc5c43331 100644 --- a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp @@ -16,7 +16,6 @@ #include #include #include -#include "nav2_util/node_utils.hpp" namespace nav2_behavior_tree { diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 7c34a309457..a244624f8ae 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -30,7 +30,7 @@ amcl: sigma_hit: 0.2 tf_broadcast: true transform_tolerance: 1.0 - service_introspection_mode: "disabled" # disabled, metadata, contents + service_introspection_mode: "disabled" update_min_a: 0.2 update_min_d: 0.25 z_hit: 0.5 @@ -49,7 +49,7 @@ bt_navigator: default_server_timeout: 20 wait_for_service_timeout: 1000 action_server_result_timeout: 900.0 - service_introspection_mode: "disabled" # disabled, metadata, contents + service_introspection_mode: "disabled" navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: plugin: "nav2_bt_navigator::NavigateToPoseNavigator" @@ -284,7 +284,7 @@ global_costmap: cost_scaling_factor: 3.0 inflation_radius: 0.7 always_send_full_costmap: True - service_introspection_mode: "disabled" # disabled, metadata, contents + 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 @@ -292,7 +292,7 @@ global_costmap: map_server: ros__parameters: # yaml_filename: "" - service_introspection_mode: "disabled" # disabled, metadata, contents + service_introspection_mode: "disabled" map_saver: ros__parameters: @@ -300,14 +300,14 @@ map_saver: free_thresh_default: 0.25 occupied_thresh_default: 0.65 map_subscribe_transient_local: True - service_introspection_mode: "disabled" # disabled, metadata, contents + 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" # disabled, metadata, contents + service_introspection_mode: "disabled" GridBased: plugin: "nav2_navfn_planner::NavfnPlanner" tolerance: 0.5 @@ -360,7 +360,7 @@ waypoint_follower: ros__parameters: loop_rate: 20 stop_on_failure: false - service_introspection_mode: "disabled" # disabled, metadata, contents + service_introspection_mode: "disabled" action_server_result_timeout: 900.0 waypoint_task_executor_plugin: "wait_at_waypoint" wait_at_waypoint: @@ -426,7 +426,7 @@ docking_server: fixed_frame: "odom" dock_backwards: false dock_prestaging_tolerance: 0.5 - service_introspection_mode: "disabled" # disabled, metadata, contents + service_introspection_mode: "disabled" # Types of docks dock_plugins: ['simple_charging_dock'] diff --git a/nav2_map_server/include/nav2_map_server/map_server.hpp b/nav2_map_server/include/nav2_map_server/map_server.hpp index 4d73ac1d313..4e6ca327b57 100644 --- a/nav2_map_server/include/nav2_map_server/map_server.hpp +++ b/nav2_map_server/include/nav2_map_server/map_server.hpp @@ -26,7 +26,6 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/state.hpp" - namespace nav2_map_server { diff --git a/nav2_smac_planner/test/test_smac_2d.cpp b/nav2_smac_planner/test/test_smac_2d.cpp index 37936770a35..35ef1a90d12 100644 --- a/nav2_smac_planner/test/test_smac_2d.cpp +++ b/nav2_smac_planner/test/test_smac_2d.cpp @@ -83,7 +83,6 @@ TEST(SmacTest, test_smac_2d) { costmap_ros->on_cleanup(rclcpp_lifecycle::State()); node2D.reset(); costmap_ros.reset(); - std::cout << "SMAC 2D test complete" << std::endl; } TEST(SmacTest, test_smac_2d_reconfigure) { From d60fe075b051533ea5bfe77583f7baf80c15dd92 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Wed, 26 Mar 2025 10:59:39 +0000 Subject: [PATCH 10/19] Add spin thread Signed-off-by: mini-1235 --- .../nav2_behavior_tree/bt_service_node.hpp | 18 ++--- .../condition/is_path_valid_condition.cpp | 2 +- .../src/lifecycle_manager_client.cpp | 4 +- nav2_rviz_plugins/src/costmap_cost_tool.cpp | 6 +- .../include/nav2_util/service_client.hpp | 65 +++++++++++-------- nav2_util/src/lifecycle_service_client.cpp | 8 +-- nav2_util/test/test_service_client.cpp | 43 ++++++++++-- .../src/waypoint_follower.cpp | 3 +- 8 files changed, 94 insertions(+), 55 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index 0352792c162..a74825a7464 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp @@ -110,14 +110,8 @@ class BtServiceNode : public BT::ActionNodeBase if (service_new != service_name_ || !service_client_) { service_name_ = service_new; node_ = config().blackboard->template get("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_ = std::make_shared>( - service_name_, node_, callback_group_); + service_name_, node_, true); } } @@ -219,15 +213,15 @@ class BtServiceNode : public BT::ActionNodeBase 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(); if (elapsed < server_timeout_) { @@ -269,8 +263,6 @@ class BtServiceNode : public BT::ActionNodeBase // 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 diff --git a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp index 80fc5c43331..9b55fa3c2e7 100644 --- a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp @@ -28,7 +28,7 @@ IsPathValidCondition::IsPathValidCondition( { node_ = config().blackboard->get("node"); client_ = std::make_shared>("is_path_valid", - node_); + node_, false); server_timeout_ = config().blackboard->template get("server_timeout"); } diff --git a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp index 4f2f14ed544..06d4a0c6c84 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp @@ -37,9 +37,9 @@ LifecycleManagerClient::LifecycleManagerClient( // Create the service clients manager_client_ = std::make_shared>( - manage_service_name_, node_); + manage_service_name_, node_, true); is_active_client_ = std::make_shared>( - active_service_name_, node_); + active_service_name_, node_, true); } bool diff --git a/nav2_rviz_plugins/src/costmap_cost_tool.cpp b/nav2_rviz_plugins/src/costmap_cost_tool.cpp index 1ffa22a5386..9b8aa894225 100644 --- a/nav2_rviz_plugins/src/costmap_cost_tool.cpp +++ b/nav2_rviz_plugins/src/costmap_cost_tool.cpp @@ -56,12 +56,12 @@ void CostmapCostTool::onInitialize() std::make_shared>( "/local_costmap/get_cost_local_costmap", node, - node->get_node_base_interface()->get_default_callback_group()); + false); global_cost_client_ = std::make_shared>( "/global_costmap/get_cost_global_costmap", node, - node->get_node_base_interface()->get_default_callback_group()); + false); } void CostmapCostTool::activate() {} @@ -111,7 +111,7 @@ void CostmapCostTool::callCostService(float x, float y) request->use_footprint = false; // Call local costmap service - if(local_cost_client_->wait_for_service(std::chrono::seconds(1))) { + if (local_cost_client_->wait_for_service(std::chrono::seconds(1))) { local_cost_client_->async_call( request, std::bind(&CostmapCostTool::handleLocalCostResponse, this, std::placeholders::_1)); diff --git a/nav2_util/include/nav2_util/service_client.hpp b/nav2_util/include/nav2_util/service_client.hpp index 317dc4845fc..ff8cd1c95ab 100644 --- a/nav2_util/include/nav2_util/service_client.hpp +++ b/nav2_util/include/nav2_util/service_client.hpp @@ -17,8 +17,13 @@ #include #include +#include #include "rclcpp/rclcpp.hpp" -#include +#include "nav2_util/node_thread.hpp" + +using std::chrono::nanoseconds; +using std::chrono::seconds; +using std::chrono::steady_clock; namespace nav2_util { @@ -35,20 +40,17 @@ class ServiceClient * @brief A constructor * @param service_name name of the service to call * @param provided_node Node to create the service client off of + * @param spin_thread Whether to spin with a dedicated thread internally */ explicit ServiceClient( const std::string & service_name, - const NodeT & provided_node, rclcpp::CallbackGroup::SharedPtr callback_group = nullptr) + const NodeT & provided_node, bool spin_thread = false) : service_name_(service_name), node_(provided_node) { - if(!callback_group) { + if(spin_thread) { callback_group_ = node_->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); - callback_group_executor_.add_callback_group(callback_group_, - node_->get_node_base_interface()); - } else { - callback_group_ = callback_group; } client_ = node_->template create_client( @@ -69,6 +71,12 @@ class ServiceClient this->client_->configure_introspection( node_->get_clock(), rclcpp::SystemDefaultsQoS(), introspection_state); + if(spin_thread) { + callback_group_executor_ = std::make_shared(); + callback_group_executor_->add_callback_group(callback_group_, + node_->get_node_base_interface()); + executor_thread_ = std::make_unique(callback_group_executor_); + } } using RequestType = typename ServiceT::Request; @@ -99,15 +107,27 @@ class ServiceClient node_->get_logger(), "%s service client: send async request", service_name_.c_str()); auto future_result = client_->async_send_request(request); - - if (callback_group_executor_.spin_until_future_complete(future_result, timeout) != - rclcpp::FutureReturnCode::SUCCESS) - { - // Pending request must be manually cleaned up if execution is interrupted or timed out - client_->remove_pending_request(future_result); - throw std::runtime_error(service_name_ + " service client: async_send_request failed"); + if (callback_group_executor_) { + // Internal thread is spinning executor already + if (timeout < std::chrono::nanoseconds(0)) { + future_result.wait(); + } else { + if (future_result.wait_for(timeout) != std::future_status::ready) { + // Pending request must be manually cleaned up if execution is interrupted or timed out + client_->remove_pending_request(future_result); + throw std::runtime_error(service_name_ + " service client: async_send_request failed"); + } + } + } else { + // No internal spin thread; spin node directly + if (rclcpp::spin_until_future_complete(node_, future_result, timeout) != + rclcpp::FutureReturnCode::SUCCESS) + { + // Pending request must be manually cleaned up if execution is interrupted or timed out + client_->remove_pending_request(future_result); + throw std::runtime_error(service_name_ + " service client: async_send_request failed"); + } } - return future_result.get(); } @@ -135,15 +155,7 @@ class ServiceClient node_->get_logger(), "%s service client: send async request", service_name_.c_str()); auto future_result = client_->async_send_request(request); - - if (callback_group_executor_.spin_until_future_complete(future_result) != - rclcpp::FutureReturnCode::SUCCESS) - { - // Pending request must be manually cleaned up if execution is interrupted or timed out - client_->remove_pending_request(future_result); - return false; - } - + future_result.wait(); response = future_result.get(); return response.get(); } @@ -194,8 +206,9 @@ class ServiceClient protected: std::string service_name_; NodeT node_; - rclcpp::CallbackGroup::SharedPtr callback_group_; - rclcpp::executors::SingleThreadedExecutor callback_group_executor_; + rclcpp::CallbackGroup::SharedPtr callback_group_{nullptr}; + rclcpp::executors::SingleThreadedExecutor::SharedPtr callback_group_executor_; + std::unique_ptr executor_thread_; typename rclcpp::Client::SharedPtr client_; }; diff --git a/nav2_util/src/lifecycle_service_client.cpp b/nav2_util/src/lifecycle_service_client.cpp index 91a550abe45..705157cb175 100644 --- a/nav2_util/src/lifecycle_service_client.cpp +++ b/nav2_util/src/lifecycle_service_client.cpp @@ -33,8 +33,8 @@ namespace nav2_util LifecycleServiceClient::LifecycleServiceClient( const string & lifecycle_node_name) : node_(generate_internal_node(lifecycle_node_name + "_lifecycle_client")), - change_state_(lifecycle_node_name + "/change_state", node_), - get_state_(lifecycle_node_name + "/get_state", node_) + change_state_(lifecycle_node_name + "/change_state", node_, true), + get_state_(lifecycle_node_name + "/get_state", node_, true) { // Block until server is up rclcpp::Rate r(20); @@ -49,8 +49,8 @@ LifecycleServiceClient::LifecycleServiceClient( const string & lifecycle_node_name, rclcpp::Node::SharedPtr parent_node) : node_(parent_node), - change_state_(lifecycle_node_name + "/change_state", node_), - get_state_(lifecycle_node_name + "/get_state", node_) + change_state_(lifecycle_node_name + "/change_state", node_, true), + get_state_(lifecycle_node_name + "/get_state", node_, true) { // Block until server is up rclcpp::Rate r(20); diff --git a/nav2_util/test/test_service_client.cpp b/nav2_util/test/test_service_client.cpp index b36b242e791..932fd5fa66f 100644 --- a/nav2_util/test/test_service_client.cpp +++ b/nav2_util/test/test_service_client.cpp @@ -36,8 +36,9 @@ class TestServiceClient : public ServiceClient public: TestServiceClient( const std::string & name, - const rclcpp::Node::SharedPtr & provided_node = rclcpp::Node::SharedPtr()) - : ServiceClient(name, provided_node) + const rclcpp::Node::SharedPtr & provided_node = rclcpp::Node::SharedPtr(), + bool spin_thread = false) + : ServiceClient(name, provided_node, spin_thread) {} string name() {return node_->get_name();} @@ -52,7 +53,7 @@ TEST(ServiceClient, can_ServiceClient_use_passed_in_node) for(const auto & mode : introspection_modes) { auto node = rclcpp::Node::make_shared("test_node" + mode); node->declare_parameter("service_introspection_mode", mode); - TestServiceClient t("bar", node); + TestServiceClient t("bar", node, true); ASSERT_EQ(t.getNode(), node); ASSERT_EQ(t.name(), "test_node" + mode); } @@ -76,7 +77,7 @@ TEST(ServiceClient, can_ServiceClient_invoke_in_callback) auto pub_thread = std::thread([&]() {rclcpp::spin(pub_node);}); auto sub_node = rclcpp::Node::make_shared("sub_node"); - ServiceClient client("empty_srv", sub_node); + ServiceClient client("empty_srv", sub_node, true); auto sub = sub_node->create_subscription( "empty_topic", rclcpp::QoS(1), @@ -98,9 +99,41 @@ TEST(ServiceClient, can_ServiceClient_timeout) { rclcpp::init(0, nullptr); auto node = rclcpp::Node::make_shared("test_node"); - TestServiceClient t("bar", node); + TestServiceClient t("bar", node, true); rclcpp::spin_some(node); bool ready = t.wait_for_service(std::chrono::milliseconds(10)); rclcpp::shutdown(); ASSERT_EQ(ready, false); } + +TEST(ServiceClient, can_ServiceClient_async_call) { + rclcpp::init(0, nullptr); + + int a = 0; + bool callback_called = false; + // Define service server + auto service_node = rclcpp::Node::make_shared("service_node"); + auto service = service_node->create_service( + "empty_srv", + [&a](std_srvs::srv::Empty::Request::SharedPtr, std_srvs::srv::Empty::Response::SharedPtr) { + a = 1; + }); + auto srv_thread = std::thread([&]() {rclcpp::spin(service_node);}); + // Define service client + auto node = rclcpp::Node::make_shared("test_node"); + ServiceClient client("empty_srv", node, true); + auto req = std::make_shared(); + auto callback = + [&callback_called](rclcpp::Client::SharedFuture /*future*/) { + callback_called = true; + }; + // Test async_call + client.async_call(req, callback); + rclcpp::spin_some(node); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + rclcpp::shutdown(); + srv_thread.join(); + ASSERT_EQ(a, 1); + ASSERT_TRUE(callback_called); +} diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 55db83df847..ebc2788e98e 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -96,7 +96,8 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & state) nav2_util::ServiceClient>>( "/fromLL", - node); + node, + true); gps_action_server_ = std::make_unique( get_node_base_interface(), From 0ec54200e4d0087fa81fe03eafe899abd43dc0df Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Wed, 26 Mar 2025 11:06:12 +0000 Subject: [PATCH 11/19] Add spin Signed-off-by: mini-1235 --- nav2_util/include/nav2_util/service_client.hpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/nav2_util/include/nav2_util/service_client.hpp b/nav2_util/include/nav2_util/service_client.hpp index ff8cd1c95ab..dd33567e5a1 100644 --- a/nav2_util/include/nav2_util/service_client.hpp +++ b/nav2_util/include/nav2_util/service_client.hpp @@ -155,7 +155,13 @@ class ServiceClient node_->get_logger(), "%s service client: send async request", service_name_.c_str()); auto future_result = client_->async_send_request(request); - future_result.wait(); + if(callback_group_executor_) { + // Internal thread is spinning executor already + future_result.wait(); + } else { + // No internal spin thread; spin node directly + rclcpp::spin_until_future_complete(node_, future_result); + } response = future_result.get(); return response.get(); } From c00c85dced9558a60275dadb400c7689cd2a1a93 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Thu, 27 Mar 2025 02:43:02 +0000 Subject: [PATCH 12/19] Change spin thread to internal executor Signed-off-by: mini-1235 --- .../nav2_behavior_tree/bt_service_node.hpp | 10 +- .../condition/is_path_valid_condition.cpp | 2 +- .../src/lifecycle_manager_client.cpp | 4 +- nav2_rviz_plugins/src/costmap_cost_tool.cpp | 4 +- .../include/nav2_util/service_client.hpp | 96 +++++++++++-------- nav2_util/src/lifecycle_service_client.cpp | 12 ++- nav2_util/test/test_service_client.cpp | 8 +- .../src/waypoint_follower.cpp | 2 +- 8 files changed, 77 insertions(+), 61 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index a74825a7464..23a6334a9e1 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp @@ -111,7 +111,7 @@ class BtServiceNode : public BT::ActionNodeBase service_name_ = service_new; node_ = config().blackboard->template get("node"); service_client_ = std::make_shared>( - service_name_, node_, true); + service_name_, node_, true /*creates and spins an internal executor*/); } } @@ -213,15 +213,15 @@ class BtServiceNode : public BT::ActionNodeBase if (remaining > std::chrono::milliseconds(0)) { auto timeout = remaining > max_timeout_ ? max_timeout_ : remaining; - std::future_status future; - future = future_result_.wait_for(timeout); - if(future == std::future_status::ready) { + rclcpp::FutureReturnCode rc; + rc = service_client_->spin_until_complete(future_result_, timeout); + if (rc == rclcpp::FutureReturnCode::SUCCESS) { request_sent_ = false; BT::NodeStatus status = on_completion(future_result_.get()); return status; } - if (future == std::future_status::timeout) { + if (rc == rclcpp::FutureReturnCode::TIMEOUT) { on_wait_for_result(); elapsed = (node_->now() - sent_time_).template to_chrono(); if (elapsed < server_timeout_) { diff --git a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp index 9b55fa3c2e7..81dc265a8b5 100644 --- a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp @@ -28,7 +28,7 @@ IsPathValidCondition::IsPathValidCondition( { node_ = config().blackboard->get("node"); client_ = std::make_shared>("is_path_valid", - node_, false); + node_, false /* Does not create and spin an internal executor*/); server_timeout_ = config().blackboard->template get("server_timeout"); } diff --git a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp index 06d4a0c6c84..7c287f2defb 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp @@ -37,9 +37,9 @@ LifecycleManagerClient::LifecycleManagerClient( // Create the service clients manager_client_ = std::make_shared>( - manage_service_name_, node_, true); + manage_service_name_, node_, true /*creates and spins an internal executor*/); is_active_client_ = std::make_shared>( - active_service_name_, node_, true); + active_service_name_, node_, true /*creates and spins an internal executor*/); } bool diff --git a/nav2_rviz_plugins/src/costmap_cost_tool.cpp b/nav2_rviz_plugins/src/costmap_cost_tool.cpp index 9b8aa894225..4e4ffdbf820 100644 --- a/nav2_rviz_plugins/src/costmap_cost_tool.cpp +++ b/nav2_rviz_plugins/src/costmap_cost_tool.cpp @@ -56,12 +56,12 @@ void CostmapCostTool::onInitialize() std::make_shared>( "/local_costmap/get_cost_local_costmap", node, - false); + false /* Does not create and spin an internal executor*/); global_cost_client_ = std::make_shared>( "/global_costmap/get_cost_global_costmap", node, - false); + false /* Does not create and spin an internal executor*/); } void CostmapCostTool::activate() {} diff --git a/nav2_util/include/nav2_util/service_client.hpp b/nav2_util/include/nav2_util/service_client.hpp index dd33567e5a1..4021af4e4fb 100644 --- a/nav2_util/include/nav2_util/service_client.hpp +++ b/nav2_util/include/nav2_util/service_client.hpp @@ -19,11 +19,6 @@ #include #include #include "rclcpp/rclcpp.hpp" -#include "nav2_util/node_thread.hpp" - -using std::chrono::nanoseconds; -using std::chrono::seconds; -using std::chrono::steady_clock; namespace nav2_util { @@ -40,17 +35,26 @@ class ServiceClient * @brief A constructor * @param service_name name of the service to call * @param provided_node Node to create the service client off of - * @param spin_thread Whether to spin with a dedicated thread internally + * @param use_internal_executor Whether to create an internal executor or not */ explicit ServiceClient( const std::string & service_name, - const NodeT & provided_node, bool spin_thread = false) - : service_name_(service_name), node_(provided_node) + const NodeT & provided_node, bool use_internal_executor = false) + : service_name_(service_name), node_(provided_node), use_internal_executor_(use_internal_executor) { - if(spin_thread) { + if(use_internal_executor) { callback_group_ = node_->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); + callback_group_executor_ = std::make_shared(); + callback_group_executor_->add_callback_group(callback_group_, + node_->get_node_base_interface()); + } else { + callback_group_ = node_->get_node_base_interface()->get_default_callback_group(); + rclcpp::ExecutorOptions options; + options.context = node_->get_node_base_interface()->get_context(); + callback_group_executor_ = + std::make_shared(options); } client_ = node_->template create_client( @@ -71,12 +75,6 @@ class ServiceClient this->client_->configure_introspection( node_->get_clock(), rclcpp::SystemDefaultsQoS(), introspection_state); - if(spin_thread) { - callback_group_executor_ = std::make_shared(); - callback_group_executor_->add_callback_group(callback_group_, - node_->get_node_base_interface()); - executor_thread_ = std::make_unique(callback_group_executor_); - } } using RequestType = typename ServiceT::Request; @@ -107,27 +105,20 @@ class ServiceClient node_->get_logger(), "%s service client: send async request", service_name_.c_str()); auto future_result = client_->async_send_request(request); - if (callback_group_executor_) { - // Internal thread is spinning executor already - if (timeout < std::chrono::nanoseconds(0)) { - future_result.wait(); - } else { - if (future_result.wait_for(timeout) != std::future_status::ready) { - // Pending request must be manually cleaned up if execution is interrupted or timed out - client_->remove_pending_request(future_result); - throw std::runtime_error(service_name_ + " service client: async_send_request failed"); - } - } - } else { - // No internal spin thread; spin node directly - if (rclcpp::spin_until_future_complete(node_, future_result, timeout) != - rclcpp::FutureReturnCode::SUCCESS) - { - // Pending request must be manually cleaned up if execution is interrupted or timed out - client_->remove_pending_request(future_result); - throw std::runtime_error(service_name_ + " service client: async_send_request failed"); - } + if(!use_internal_executor_) { + callback_group_executor_->add_node(node_->get_node_base_interface()); } + if (callback_group_executor_->spin_until_future_complete(future_result, timeout) != + rclcpp::FutureReturnCode::SUCCESS) + { + // Pending request must be manually cleaned up if execution is interrupted or timed out + client_->remove_pending_request(future_result); + throw std::runtime_error(service_name_ + " service client: async_send_request failed"); + } + if(!use_internal_executor_) { + callback_group_executor_->remove_node(node_->get_node_base_interface()); + } + return future_result.get(); } @@ -155,12 +146,18 @@ class ServiceClient node_->get_logger(), "%s service client: send async request", service_name_.c_str()); auto future_result = client_->async_send_request(request); - if(callback_group_executor_) { - // Internal thread is spinning executor already - future_result.wait(); - } else { - // No internal spin thread; spin node directly - rclcpp::spin_until_future_complete(node_, future_result); + if(!use_internal_executor_) { + callback_group_executor_->add_node(node_->get_node_base_interface()); + } + if (callback_group_executor_->spin_until_future_complete(future_result) != + rclcpp::FutureReturnCode::SUCCESS) + { + // Pending request must be manually cleaned up if execution is interrupted or timed out + client_->remove_pending_request(future_result); + return false; + } + if(!use_internal_executor_) { + callback_group_executor_->remove_node(node_->get_node_base_interface()); } response = future_result.get(); return response.get(); @@ -200,6 +197,21 @@ class ServiceClient return client_->wait_for_service(timeout); } + /** + * @brief Spins the executor until the provided future is complete or the timeout is reached. + * + * @param future The shared future to wait for completion. + * @param timeout The maximum time to wait for the future to complete. Default is -1 (no timeout). + * @return rclcpp::FutureReturnCode indicating the result of the spin operation. + */ + template + rclcpp::FutureReturnCode spin_until_complete( + std::shared_future future, + const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)) + { + return callback_group_executor_->spin_until_future_complete(future, timeout); + } + /** * @brief Gets the service name * @return string Service name @@ -214,8 +226,8 @@ class ServiceClient NodeT node_; rclcpp::CallbackGroup::SharedPtr callback_group_{nullptr}; rclcpp::executors::SingleThreadedExecutor::SharedPtr callback_group_executor_; - std::unique_ptr executor_thread_; typename rclcpp::Client::SharedPtr client_; + bool use_internal_executor_; }; } // namespace nav2_util diff --git a/nav2_util/src/lifecycle_service_client.cpp b/nav2_util/src/lifecycle_service_client.cpp index 705157cb175..9674e76e090 100644 --- a/nav2_util/src/lifecycle_service_client.cpp +++ b/nav2_util/src/lifecycle_service_client.cpp @@ -33,8 +33,10 @@ namespace nav2_util LifecycleServiceClient::LifecycleServiceClient( const string & lifecycle_node_name) : node_(generate_internal_node(lifecycle_node_name + "_lifecycle_client")), - change_state_(lifecycle_node_name + "/change_state", node_, true), - get_state_(lifecycle_node_name + "/get_state", node_, true) + change_state_(lifecycle_node_name + "/change_state", node_, + true /*creates and spins an internal executor*/), + get_state_(lifecycle_node_name + "/get_state", node_, + true /*creates and spins an internal executor*/) { // Block until server is up rclcpp::Rate r(20); @@ -49,8 +51,10 @@ LifecycleServiceClient::LifecycleServiceClient( const string & lifecycle_node_name, rclcpp::Node::SharedPtr parent_node) : node_(parent_node), - change_state_(lifecycle_node_name + "/change_state", node_, true), - get_state_(lifecycle_node_name + "/get_state", node_, true) + change_state_(lifecycle_node_name + "/change_state", node_, + true /*creates and spins an internal executor*/), + get_state_(lifecycle_node_name + "/get_state", node_, + true /*creates and spins an internal executor*/) { // Block until server is up rclcpp::Rate r(20); diff --git a/nav2_util/test/test_service_client.cpp b/nav2_util/test/test_service_client.cpp index 932fd5fa66f..7ee89331274 100644 --- a/nav2_util/test/test_service_client.cpp +++ b/nav2_util/test/test_service_client.cpp @@ -37,8 +37,8 @@ class TestServiceClient : public ServiceClient TestServiceClient( const std::string & name, const rclcpp::Node::SharedPtr & provided_node = rclcpp::Node::SharedPtr(), - bool spin_thread = false) - : ServiceClient(name, provided_node, spin_thread) + bool use_internal_executor = false) + : ServiceClient(name, provided_node, use_internal_executor) {} string name() {return node_->get_name();} @@ -121,7 +121,7 @@ TEST(ServiceClient, can_ServiceClient_async_call) { auto srv_thread = std::thread([&]() {rclcpp::spin(service_node);}); // Define service client auto node = rclcpp::Node::make_shared("test_node"); - ServiceClient client("empty_srv", node, true); + ServiceClient client("empty_srv", node, false); auto req = std::make_shared(); auto callback = [&callback_called](rclcpp::Client::SharedFuture /*future*/) { @@ -129,8 +129,8 @@ TEST(ServiceClient, can_ServiceClient_async_call) { }; // Test async_call client.async_call(req, callback); - rclcpp::spin_some(node); std::this_thread::sleep_for(std::chrono::seconds(1)); + rclcpp::spin_some(node); rclcpp::shutdown(); srv_thread.join(); diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index ebc2788e98e..fb967e7d964 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -97,7 +97,7 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & state) std::shared_ptr>>( "/fromLL", node, - true); + true /*creates and spins an internal executor*/); gps_action_server_ = std::make_unique( get_node_base_interface(), From 0455e66eb4b0c33f7f141363459d5bed155e1bdc Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Tue, 1 Apr 2025 04:45:35 +0000 Subject: [PATCH 13/19] Use rclcpp spin when no internal executor Signed-off-by: mini-1235 --- .../include/nav2_util/service_client.hpp | 71 ++++++++++--------- 1 file changed, 39 insertions(+), 32 deletions(-) diff --git a/nav2_util/include/nav2_util/service_client.hpp b/nav2_util/include/nav2_util/service_client.hpp index 4021af4e4fb..546354392eb 100644 --- a/nav2_util/include/nav2_util/service_client.hpp +++ b/nav2_util/include/nav2_util/service_client.hpp @@ -49,20 +49,14 @@ class ServiceClient callback_group_executor_ = std::make_shared(); callback_group_executor_->add_callback_group(callback_group_, node_->get_node_base_interface()); - } else { - callback_group_ = node_->get_node_base_interface()->get_default_callback_group(); - rclcpp::ExecutorOptions options; - options.context = node_->get_node_base_interface()->get_context(); - callback_group_executor_ = - std::make_shared(options); } - + // When a nullptr is passed, the client will use the default callback group client_ = node_->template create_client( service_name, rclcpp::SystemDefaultsQoS(), callback_group_); rcl_service_introspection_state_t introspection_state = RCL_SERVICE_INTROSPECTION_OFF; - if(!node_->has_parameter("service_introspection_mode")) { + if (!node_->has_parameter("service_introspection_mode")) { node_->declare_parameter("service_introspection_mode", "disabled"); } std::string service_introspection_mode = @@ -105,18 +99,22 @@ class ServiceClient node_->get_logger(), "%s service client: send async request", service_name_.c_str()); auto future_result = client_->async_send_request(request); - if(!use_internal_executor_) { - callback_group_executor_->add_node(node_->get_node_base_interface()); - } - if (callback_group_executor_->spin_until_future_complete(future_result, timeout) != - rclcpp::FutureReturnCode::SUCCESS) - { - // Pending request must be manually cleaned up if execution is interrupted or timed out - client_->remove_pending_request(future_result); - throw std::runtime_error(service_name_ + " service client: async_send_request failed"); - } - if(!use_internal_executor_) { - callback_group_executor_->remove_node(node_->get_node_base_interface()); + if(use_internal_executor_) { + if (callback_group_executor_->spin_until_future_complete(future_result, timeout) != + rclcpp::FutureReturnCode::SUCCESS) + { + // Pending request must be manually cleaned up if execution is interrupted or timed out + client_->remove_pending_request(future_result); + throw std::runtime_error(service_name_ + " service client: async_send_request failed"); + } + } else { + if (rclcpp::spin_until_future_complete(node_, future_result, timeout) != + rclcpp::FutureReturnCode::SUCCESS) + { + // Pending request must be manually cleaned up if execution is interrupted or timed out + client_->remove_pending_request(future_result); + throw std::runtime_error(service_name_ + " service client: async_send_request failed"); + } } return future_result.get(); @@ -146,18 +144,22 @@ class ServiceClient node_->get_logger(), "%s service client: send async request", service_name_.c_str()); auto future_result = client_->async_send_request(request); - if(!use_internal_executor_) { - callback_group_executor_->add_node(node_->get_node_base_interface()); - } - if (callback_group_executor_->spin_until_future_complete(future_result) != - rclcpp::FutureReturnCode::SUCCESS) - { + if(use_internal_executor_) { + if (callback_group_executor_->spin_until_future_complete(future_result) != + rclcpp::FutureReturnCode::SUCCESS) + { // Pending request must be manually cleaned up if execution is interrupted or timed out - client_->remove_pending_request(future_result); - return false; - } - if(!use_internal_executor_) { - callback_group_executor_->remove_node(node_->get_node_base_interface()); + client_->remove_pending_request(future_result); + return false; + } + } else { + if (rclcpp::spin_until_future_complete(node_, future_result) != + rclcpp::FutureReturnCode::SUCCESS) + { + // Pending request must be manually cleaned up if execution is interrupted or timed out + client_->remove_pending_request(future_result); + return false; + } } response = future_result.get(); return response.get(); @@ -209,7 +211,12 @@ class ServiceClient std::shared_future future, const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)) { - return callback_group_executor_->spin_until_future_complete(future, timeout); + // return callback_group_executor_->spin_until_future_complete(future, timeout); + if(use_internal_executor_) { + return callback_group_executor_->spin_until_future_complete(future, timeout); + } else { + return rclcpp::spin_until_future_complete(node_, future, timeout); + } } /** From 5abdb13071dec66a3092b117ece079379e45debc Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 1 Apr 2025 09:55:48 -0700 Subject: [PATCH 14/19] Update nav2_util/include/nav2_util/service_client.hpp Signed-off-by: Steve Macenski --- nav2_util/include/nav2_util/service_client.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_util/include/nav2_util/service_client.hpp b/nav2_util/include/nav2_util/service_client.hpp index 546354392eb..d0eaf3ec7c9 100644 --- a/nav2_util/include/nav2_util/service_client.hpp +++ b/nav2_util/include/nav2_util/service_client.hpp @@ -42,7 +42,7 @@ class ServiceClient const NodeT & provided_node, bool use_internal_executor = false) : service_name_(service_name), node_(provided_node), use_internal_executor_(use_internal_executor) { - if(use_internal_executor) { + if (use_internal_executor) { callback_group_ = node_->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); From 4d30fc595ac2b6b727f26921acbfcd35307c0c70 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 1 Apr 2025 09:55:55 -0700 Subject: [PATCH 15/19] Update nav2_util/include/nav2_util/service_client.hpp Signed-off-by: Steve Macenski --- nav2_util/include/nav2_util/service_client.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_util/include/nav2_util/service_client.hpp b/nav2_util/include/nav2_util/service_client.hpp index d0eaf3ec7c9..008db2825f2 100644 --- a/nav2_util/include/nav2_util/service_client.hpp +++ b/nav2_util/include/nav2_util/service_client.hpp @@ -99,7 +99,7 @@ class ServiceClient node_->get_logger(), "%s service client: send async request", service_name_.c_str()); auto future_result = client_->async_send_request(request); - if(use_internal_executor_) { + if (use_internal_executor_) { if (callback_group_executor_->spin_until_future_complete(future_result, timeout) != rclcpp::FutureReturnCode::SUCCESS) { From 4a79e64b5355e12a8e77af36a77d830931d8feb9 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 1 Apr 2025 09:56:03 -0700 Subject: [PATCH 16/19] Update nav2_util/include/nav2_util/service_client.hpp Signed-off-by: Steve Macenski --- nav2_util/include/nav2_util/service_client.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_util/include/nav2_util/service_client.hpp b/nav2_util/include/nav2_util/service_client.hpp index 008db2825f2..a138d3df76f 100644 --- a/nav2_util/include/nav2_util/service_client.hpp +++ b/nav2_util/include/nav2_util/service_client.hpp @@ -144,7 +144,7 @@ class ServiceClient node_->get_logger(), "%s service client: send async request", service_name_.c_str()); auto future_result = client_->async_send_request(request); - if(use_internal_executor_) { + if (use_internal_executor_) { if (callback_group_executor_->spin_until_future_complete(future_result) != rclcpp::FutureReturnCode::SUCCESS) { From 1009d0e296b2608afcf2fa9efbf3a5b0aeb01e46 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 1 Apr 2025 09:56:10 -0700 Subject: [PATCH 17/19] Update nav2_util/include/nav2_util/service_client.hpp Signed-off-by: Steve Macenski --- nav2_util/include/nav2_util/service_client.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_util/include/nav2_util/service_client.hpp b/nav2_util/include/nav2_util/service_client.hpp index a138d3df76f..fcadc8c8ede 100644 --- a/nav2_util/include/nav2_util/service_client.hpp +++ b/nav2_util/include/nav2_util/service_client.hpp @@ -161,6 +161,7 @@ class ServiceClient return false; } } + response = future_result.get(); return response.get(); } From 03e5369b94aeec1277f46e58aa1d0cbcc5ed6a7f Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Wed, 2 Apr 2025 07:44:10 +0000 Subject: [PATCH 18/19] Refactor Signed-off-by: mini-1235 --- .../include/nav2_util/service_client.hpp | 42 ++++--------------- 1 file changed, 9 insertions(+), 33 deletions(-) diff --git a/nav2_util/include/nav2_util/service_client.hpp b/nav2_util/include/nav2_util/service_client.hpp index fcadc8c8ede..8d68ee72166 100644 --- a/nav2_util/include/nav2_util/service_client.hpp +++ b/nav2_util/include/nav2_util/service_client.hpp @@ -99,22 +99,10 @@ class ServiceClient node_->get_logger(), "%s service client: send async request", service_name_.c_str()); auto future_result = client_->async_send_request(request); - if (use_internal_executor_) { - if (callback_group_executor_->spin_until_future_complete(future_result, timeout) != - rclcpp::FutureReturnCode::SUCCESS) - { - // Pending request must be manually cleaned up if execution is interrupted or timed out - client_->remove_pending_request(future_result); - throw std::runtime_error(service_name_ + " service client: async_send_request failed"); - } - } else { - if (rclcpp::spin_until_future_complete(node_, future_result, timeout) != - rclcpp::FutureReturnCode::SUCCESS) - { - // Pending request must be manually cleaned up if execution is interrupted or timed out - client_->remove_pending_request(future_result); - throw std::runtime_error(service_name_ + " service client: async_send_request failed"); - } + if (spin_until_complete(future_result, timeout) != rclcpp::FutureReturnCode::SUCCESS) { + // Pending request must be manually cleaned up if execution is interrupted or timed out + client_->remove_pending_request(future_result); + throw std::runtime_error(service_name_ + " service client: async_send_request failed"); } return future_result.get(); @@ -144,22 +132,10 @@ class ServiceClient node_->get_logger(), "%s service client: send async request", service_name_.c_str()); auto future_result = client_->async_send_request(request); - if (use_internal_executor_) { - if (callback_group_executor_->spin_until_future_complete(future_result) != - rclcpp::FutureReturnCode::SUCCESS) - { + if (spin_until_complete(future_result) != rclcpp::FutureReturnCode::SUCCESS) { // Pending request must be manually cleaned up if execution is interrupted or timed out - client_->remove_pending_request(future_result); - return false; - } - } else { - if (rclcpp::spin_until_future_complete(node_, future_result) != - rclcpp::FutureReturnCode::SUCCESS) - { - // Pending request must be manually cleaned up if execution is interrupted or timed out - client_->remove_pending_request(future_result); - return false; - } + client_->remove_pending_request(future_result); + return false; } response = future_result.get(); @@ -209,11 +185,11 @@ class ServiceClient */ template rclcpp::FutureReturnCode spin_until_complete( - std::shared_future future, + const FutureT & future, const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)) { // return callback_group_executor_->spin_until_future_complete(future, timeout); - if(use_internal_executor_) { + if (use_internal_executor_) { return callback_group_executor_->spin_until_future_complete(future, timeout); } else { return rclcpp::spin_until_future_complete(node_, future, timeout); From d2358260c2e73ae6dffde1e4c365c23bcb9985b8 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 2 Apr 2025 11:50:39 -0700 Subject: [PATCH 19/19] Update nav2_util/include/nav2_util/service_client.hpp Signed-off-by: Steve Macenski --- nav2_util/include/nav2_util/service_client.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_util/include/nav2_util/service_client.hpp b/nav2_util/include/nav2_util/service_client.hpp index 8d68ee72166..5d8ffcbe99b 100644 --- a/nav2_util/include/nav2_util/service_client.hpp +++ b/nav2_util/include/nav2_util/service_client.hpp @@ -188,7 +188,6 @@ class ServiceClient const FutureT & future, const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)) { - // return callback_group_executor_->spin_until_future_complete(future, timeout); if (use_internal_executor_) { return callback_group_executor_->spin_until_future_complete(future, timeout); } else {