diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 7928f5e9545..5910181b3a6 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_; + nav2_util::ServiceServer>::SharedPtr global_loc_srv_; /* * @brief Service callback for a global relocalization request */ @@ -216,7 +218,8 @@ class AmclNode : public nav2_util::LifecycleNode std::shared_ptr response); // service server for providing an initial pose guess - rclcpp::Service::SharedPtr initial_guess_srv_; + nav2_util::ServiceServer>::SharedPtr initial_guess_srv_; /* * @brief Service callback for an initial pose guess request */ @@ -226,7 +229,8 @@ class AmclNode : public nav2_util::LifecycleNode std::shared_ptr response); // Let amcl update samples without requiring motion - rclcpp::Service::SharedPtr 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_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 7ae2bb61048..5dcc83eb93a 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1564,18 +1564,21 @@ AmclNode::initPubSub() void AmclNode::initServices() { - global_loc_srv_ = create_service( - "reinitialize_global_localization", + global_loc_srv_ = std::make_shared>>( + "reinitialize_global_localization", shared_from_this(), std::bind(&AmclNode::globalLocalizationCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - initial_guess_srv_ = create_service( - "set_initial_pose", + initial_guess_srv_ = std::make_shared>>( + "set_initial_pose", shared_from_this(), std::bind(&AmclNode::initialPoseReceivedSrv, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - nomotion_update_srv_ = create_service( - "request_nomotion_update", + nomotion_update_srv_ = std::make_shared>>( + "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 18d6ea851bb..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 @@ -23,6 +23,7 @@ #include "nav2_util/node_utils.hpp" #include "rclcpp/rclcpp.hpp" #include "nav2_behavior_tree/bt_utils.hpp" +#include "nav2_util/service_client.hpp" namespace nav2_behavior_tree { @@ -109,16 +110,8 @@ 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_ = node_->create_client( - service_name_, - rclcpp::SystemDefaultsQoS(), - callback_group_); + service_client_ = std::make_shared>( + service_name_, node_, true /*creates and spins an internal executor*/); } } @@ -173,7 +166,7 @@ class BtServiceNode : public BT::ActionNodeBase return BT::NodeStatus::FAILURE; } - future_result_ = service_client_->async_send_request(request_).share(); + future_result_ = service_client_->async_call(request_); sent_time_ = node_->now(); request_sent_ = true; } @@ -221,7 +214,7 @@ class BtServiceNode : public BT::ActionNodeBase auto timeout = remaining > max_timeout_ ? max_timeout_ : remaining; rclcpp::FutureReturnCode rc; - rc = callback_group_executor_.spin_until_future_complete(future_result_, timeout); + 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()); @@ -265,13 +258,11 @@ class BtServiceNode : public BT::ActionNodeBase } std::string service_name_, service_node_name_; - typename 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 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/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..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 @@ -22,6 +22,7 @@ #include "behaviortree_cpp/condition_node.h" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_msgs/srv/is_path_valid.hpp" +#include "nav2_util/service_client.hpp" namespace nav2_behavior_tree { @@ -73,7 +74,7 @@ class IsPathValidCondition : public BT::ConditionNode private: rclcpp::Node::SharedPtr node_; - rclcpp::Client::SharedPtr 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_behavior_tree/plugins/condition/is_path_valid_condition.cpp b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp index 8caed0c3971..81dc265a8b5 100644 --- a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp @@ -27,7 +27,8 @@ IsPathValidCondition::IsPathValidCondition( max_cost_(253), consider_unknown_as_obstacle_(false) { node_ = config().blackboard->get("node"); - client_ = node_->create_client("is_path_valid"); + client_ = std::make_shared>("is_path_valid", + node_, false /* Does not create and spin an internal executor*/); server_timeout_ = config().blackboard->template get("server_timeout"); } @@ -53,14 +54,9 @@ BT::NodeStatus IsPathValidCondition::tick() request->path = path; request->max_cost = max_cost_; request->consider_unknown_as_obstacle = consider_unknown_as_obstacle_; - auto result = client_->async_send_request(request); - - if (rclcpp::spin_until_future_complete(node_, result, server_timeout_) == - rclcpp::FutureReturnCode::SUCCESS) - { - if (result.get()->is_valid) { - return BT::NodeStatus::SUCCESS; - } + auto response = client_->invoke(request, server_timeout_); + if (response->is_valid) { + return BT::NodeStatus::SUCCESS; } return BT::NodeStatus::FAILURE; } diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 04021ac6d52..a244624f8ae 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" update_min_a: 0.2 update_min_d: 0.25 z_hit: 0.5 @@ -48,6 +49,7 @@ bt_navigator: default_server_timeout: 20 wait_for_service_timeout: 1000 action_server_result_timeout: 900.0 + service_introspection_mode: "disabled" navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: plugin: "nav2_bt_navigator::NavigateToPoseNavigator" @@ -241,6 +243,7 @@ local_costmap: plugin: "nav2_costmap_2d::StaticLayer" map_subscribe_transient_local: True always_send_full_costmap: True + service_introspection_mode: "disabled" global_costmap: global_costmap: @@ -281,13 +284,15 @@ global_costmap: cost_scaling_factor: 3.0 inflation_radius: 0.7 always_send_full_costmap: True + service_introspection_mode: "disabled" # The yaml_filename does not need to be specified since it going to be set by defaults in launch. # If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py # file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used. -# map_server: -# ros__parameters: -# yaml_filename: "" +map_server: + ros__parameters: + # yaml_filename: "" + service_introspection_mode: "disabled" map_saver: ros__parameters: @@ -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" planner_server: ros__parameters: expected_planner_frequency: 20.0 planner_plugins: ["GridBased"] costmap_update_timeout: 1.0 + service_introspection_mode: "disabled" GridBased: plugin: "nav2_navfn_planner::NavfnPlanner" tolerance: 0.5 @@ -353,6 +360,7 @@ waypoint_follower: ros__parameters: loop_rate: 20 stop_on_failure: false + service_introspection_mode: "disabled" action_server_result_timeout: 900.0 waypoint_task_executor_plugin: "wait_at_waypoint" wait_at_waypoint: @@ -418,6 +426,7 @@ docking_server: fixed_frame: "odom" dock_backwards: false dock_prestaging_tolerance: 0.5 + service_introspection_mode: "disabled" # Types of docks dock_plugins: ['simple_charging_dock'] 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..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 @@ -25,6 +25,7 @@ #include "nav2_msgs/srv/clear_entire_costmap.hpp" #include "nav2_costmap_2d/costmap_layer.hpp" #include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/service_server.hpp" namespace nav2_costmap_2d { @@ -41,7 +42,8 @@ class ClearCostmapService /** * @brief A constructor */ - ClearCostmapService(const nav2_util::LifecycleNode::WeakPtr & parent, Costmap2DROS & costmap); + ClearCostmapService( + const nav2_util::LifecycleNode::WeakPtr & parent, Costmap2DROS & costmap); /** * @brief A constructor @@ -74,7 +76,8 @@ class ClearCostmapService unsigned char reset_value_; // Server for clearing the costmap - rclcpp::Service::SharedPtr clear_except_service_; + nav2_util::ServiceServer>::SharedPtr clear_except_service_; /** * @brief Callback to clear costmap except in a given region */ @@ -83,7 +86,8 @@ class ClearCostmapService const std::shared_ptr request, const std::shared_ptr response); - rclcpp::Service::SharedPtr clear_around_service_; + nav2_util::ServiceServer>::SharedPtr clear_around_service_; /** * @brief Callback to clear costmap in a given region */ @@ -92,7 +96,8 @@ class ClearCostmapService const std::shared_ptr request, const std::shared_ptr response); - rclcpp::Service::SharedPtr 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 41041c44b6e..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 @@ -54,6 +54,7 @@ #include "nav2_util/lifecycle_node.hpp" #include "tf2/LinearMath/Quaternion.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "nav2_util/service_server.hpp" namespace nav2_costmap_2d { @@ -180,7 +181,9 @@ class Costmap2DPublisher costmap_raw_update_pub_; // Service for getting the costmaps - rclcpp::Service::SharedPtr 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 5fa4c1cacc2..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 @@ -60,6 +60,7 @@ #include "tf2_ros/transform_listener.h" #include "tf2/time.hpp" #include "tf2/transform_datatypes.hpp" +#include "nav2_util/service_server.hpp" #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wpedantic" @@ -414,7 +415,8 @@ class Costmap2DROS : public nav2_util::LifecycleNode std::vector padded_footprint_; // Services - rclcpp::Service::SharedPtr 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 d1128b5ccc6..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 @@ -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_; + nav2_util::ServiceServer>::SharedPtr 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..6a5d83dd583 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp @@ -83,11 +83,12 @@ void CostmapFilter::onInitialize() transform_tolerance_ = tf2::durationFromSec(transform_tolerance); // Costmap Filter enabling service - enable_service_ = node->create_service( + enable_service_ = std::make_shared>>( name_ + "/toggle_filter", - std::bind( - &CostmapFilter::enableCallback, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + node, + std::bind(&CostmapFilter::enableCallback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); } catch (const std::exception & ex) { RCLCPP_ERROR(logger_, "Parameter problem: %s", ex.what()); throw ex; diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp index e11aca2ce5a..2fa4e4a3bb1 100644 --- a/nav2_costmap_2d/src/clear_costmap_service.cpp +++ b/nav2_costmap_2d/src/clear_costmap_service.cpp @@ -40,20 +40,26 @@ ClearCostmapService::ClearCostmapService( 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(), + node, 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(), + node, 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(), + 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 484ceb14225..f7ee5a2c544 100644 --- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp @@ -82,11 +82,13 @@ 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, + node, + 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..139d3b9f3fc 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -248,10 +248,11 @@ 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(), + shared_from_this(), + std::bind(&Costmap2DROS::getCostsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); // Add cleaning service @@ -360,7 +361,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(); 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..d151992ea42 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_; + nav2_util::ServiceServer>::SharedPtr 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..cd62cef607b 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,13 @@ 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( + reload_db_service_ = std::make_shared>>( "~/reload_database", + node, std::bind( &DockDatabase::reloadDbCb, this, - std::placeholders::_1, std::placeholders::_2)); + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); return true; } @@ -80,6 +83,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..2f3aa38bf2a 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_; + 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 @@ -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 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_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp index e37f53cf1c1..de1d918ae8e 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager.cpp @@ -61,17 +61,6 @@ LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options) get_parameter("attempt_respawn_reconnection", attempt_respawn_reconnection_); 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 +81,7 @@ LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options) [this]() -> void { init_timer_->cancel(); createLifecycleServiceClients(); + createLifecycleServiceServers(); if (autostart_) { init_timer_ = this->create_wall_timer( 0s, @@ -201,6 +191,25 @@ LifecycleManager::createLifecycleServiceClients() } } +void +LifecycleManager::createLifecycleServiceServers() +{ + message("Creating and initializing lifecycle service servers"); + manager_srv_ = std::make_shared>( + get_name() + std::string("/manage_nodes"), + 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"), + 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..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_); + manage_service_name_, node_, true /*creates and spins an internal executor*/); is_active_client_ = std::make_shared>( - active_service_name_, node_); + active_service_name_, node_, true /*creates and spins an internal executor*/); } 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..123e6723b80 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_; + 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 23c9fae9d4c..4e6ca327b57 100644 --- a/nav2_map_server/include/nav2_map_server/map_server.hpp +++ b/nav2_map_server/include/nav2_map_server/map_server.hpp @@ -19,6 +19,7 @@ #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" @@ -125,10 +126,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_; + nav2_util::ServiceServer>::SharedPtr occ_service_; // A service to load the occupancy grid from file at run time (LoadMap) - rclcpp::Service::SharedPtr 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_map_server/src/map_saver/map_saver.cpp b/nav2_map_server/src/map_saver/map_saver.cpp index 1517e787526..43226a00349 100644 --- a/nav2_map_server/src/map_saver/map_saver.cpp +++ b/nav2_map_server/src/map_saver/map_saver.cpp @@ -72,8 +72,10 @@ MapSaver::on_configure(const rclcpp_lifecycle::State & /*state*/) map_subscribe_transient_local_ = get_parameter("map_subscribe_transient_local").as_bool(); // 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_, + 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..a9470ef9ce6 100644 --- a/nav2_map_server/src/map_server/map_server.cpp +++ b/nav2_map_server/src/map_server/map_server.cpp @@ -108,8 +108,10 @@ 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_), + 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 +120,10 @@ 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_), + shared_from_this(), std::bind(&MapServer::loadMapCallback, this, _1, _2, _3)); return nav2_util::CallbackReturn::SUCCESS; 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 8d5ce2903e9..21bc032e5fa 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); @@ -260,7 +262,8 @@ class PlannerServer : public nav2_util::LifecycleNode rclcpp_lifecycle::LifecyclePublisher::SharedPtr plan_publisher_; // Service to determine if the path is valid - rclcpp::Service::SharedPtr is_path_valid_service_; + nav2_util::ServiceServer>::SharedPtr 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..f949f980a95 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -197,11 +197,12 @@ 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)); + 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 +250,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 +647,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..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 @@ -23,6 +23,7 @@ #include #include #include +#include "nav2_util/service_client.hpp" namespace nav2_rviz_plugins { @@ -48,8 +49,8 @@ class CostmapCostTool : public rviz_common::Tool private Q_SLOTS: private: - rclcpp::Client::SharedPtr local_cost_client_; - rclcpp::Client::SharedPtr 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 03435a913e4..4e4ffdbf820 100644 --- a/nav2_rviz_plugins/src/costmap_cost_tool.cpp +++ b/nav2_rviz_plugins/src/costmap_cost_tool.cpp @@ -52,11 +52,16 @@ void CostmapCostTool::onInitialize() return; } rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node(); - local_cost_client_ = - node->create_client("/local_costmap/get_cost_local_costmap"); + std::make_shared>( + "/local_costmap/get_cost_local_costmap", + node, + false /* Does not create and spin an internal executor*/); global_cost_client_ = - node->create_client("/global_costmap/get_cost_global_costmap"); + std::make_shared>( + "/global_costmap/get_cost_global_costmap", + node, + false /* Does not create and spin an internal executor*/); } void CostmapCostTool::activate() {} @@ -107,14 +112,14 @@ void CostmapCostTool::callCostService(float x, float y) // Call local costmap service if (local_cost_client_->wait_for_service(std::chrono::seconds(1))) { - local_cost_client_->async_send_request( + 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))) { - global_cost_client_->async_send_request( + global_cost_client_->async_call( request, std::bind(&CostmapCostTool::handleGlobalCostResponse, this, std::placeholders::_1)); } 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/lifecycle_service_client.hpp b/nav2_util/include/nav2_util/lifecycle_service_client.hpp index cceb5393684..46d322f7709 100644 --- a/nav2_util/include/nav2_util/lifecycle_service_client.hpp +++ b/nav2_util/include/nav2_util/lifecycle_service_client.hpp @@ -31,7 +31,8 @@ namespace nav2_util class LifecycleServiceClient { public: - explicit LifecycleServiceClient(const std::string & lifecycle_node_name); + explicit LifecycleServiceClient( + const std::string & lifecycle_node_name); LifecycleServiceClient( const std::string & lifecycle_node_name, rclcpp::Node::SharedPtr parent_node); diff --git a/nav2_util/include/nav2_util/service_client.hpp b/nav2_util/include/nav2_util/service_client.hpp index 6f25965d1e3..5d8ffcbe99b 100644 --- a/nav2_util/include/nav2_util/service_client.hpp +++ b/nav2_util/include/nav2_util/service_client.hpp @@ -16,7 +16,8 @@ #define NAV2_UTIL__SERVICE_CLIENT_HPP_ #include - +#include +#include #include "rclcpp/rclcpp.hpp" namespace nav2_util @@ -24,7 +25,7 @@ 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 @@ -34,24 +35,45 @@ 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 use_internal_executor Whether to create an internal executor or not */ explicit ServiceClient( const std::string & service_name, - const NodeT & provided_node) - : 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) { - callback_group_ = node_->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive, - false); - callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); + 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()); + } + // 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")) { + node_->declare_parameter("service_introspection_mode", "disabled"); + } + std::string service_introspection_mode = + node_->get_parameter("service_introspection_mode").as_string(); + 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; using ResponseType = typename ServiceT::Response; + using SharedPtr = std::shared_ptr>; /** * @brief Invoke the service and block until completed or timed out @@ -77,10 +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 (callback_group_executor_.spin_until_future_complete(future_result, timeout) != - rclcpp::FutureReturnCode::SUCCESS) - { + 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"); @@ -113,10 +132,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) - { + 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; @@ -126,6 +142,30 @@ class ServiceClient return response.get(); } + /** + * @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 async_call( + typename RequestType::SharedPtr & request) + { + auto future_result = client_->async_send_request(request); + 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, CallbackT && callback) + { + client_->async_send_request(request, callback); + } + /** * @brief Block until a service is available or timeout * @param timeout Maximum timeout to wait for, default infinite @@ -136,6 +176,25 @@ 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( + const FutureT & future, + const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)) + { + if (use_internal_executor_) { + return callback_group_executor_->spin_until_future_complete(future, timeout); + } else { + return rclcpp::spin_until_future_complete(node_, future, timeout); + } + } + /** * @brief Gets the service name * @return string Service name @@ -148,9 +207,10 @@ 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_; typename rclcpp::Client::SharedPtr client_; + bool use_internal_executor_; }; } // namespace nav2_util 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..acdc374a9ea --- /dev/null +++ b/nav2_util/include/nav2_util/service_server.hpp @@ -0,0 +1,82 @@ +// 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. + +#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 server + */ +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)>; + using SharedPtr = std::shared_ptr>; + + explicit ServiceServer( + const std::string & service_name, + 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(!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 == "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_; +}; + +} // 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..9674e76e090 100644 --- a/nav2_util/src/lifecycle_service_client.cpp +++ b/nav2_util/src/lifecycle_service_client.cpp @@ -30,10 +30,13 @@ using namespace std::chrono_literals; namespace nav2_util { -LifecycleServiceClient::LifecycleServiceClient(const string & lifecycle_node_name) +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 /*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); @@ -48,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_), - get_state_(lifecycle_node_name + "/get_state", node_) + 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/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_client.cpp b/nav2_util/test/test_service_client.cpp index 521d3f30568..7ee89331274 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, - const rclcpp::Node::SharedPtr & provided_node = rclcpp::Node::SharedPtr()) - : ServiceClient(name, provided_node) {} + const rclcpp::Node::SharedPtr & provided_node = rclcpp::Node::SharedPtr(), + bool use_internal_executor = false) + : ServiceClient(name, provided_node, use_internal_executor) + {} string name() {return node_->get_name();} const rclcpp::Node::SharedPtr & getNode() {return node_;} @@ -45,10 +47,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, true); + ASSERT_EQ(t.getNode(), node); + ASSERT_EQ(t.name(), "test_node" + mode); + } } TEST(ServiceClient, can_ServiceClient_invoke_in_callback) @@ -69,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), @@ -91,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, false); + auto req = std::make_shared(); + auto callback = + [&callback_called](rclcpp::Client::SharedFuture /*future*/) { + callback_called = true; + }; + // Test async_call + client.async_call(req, callback); + std::this_thread::sleep_for(std::chrono::seconds(1)); + rclcpp::spin_some(node); + + rclcpp::shutdown(); + srv_thread.join(); + ASSERT_EQ(a, 1); + ASSERT_TRUE(callback_called); +} diff --git a/nav2_util/test/test_service_server.cpp b/nav2_util/test/test_service_server.cpp new file mode 100644 index 00000000000..582f047259c --- /dev/null +++ b/nav2_util/test/test_service_server.cpp @@ -0,0 +1,82 @@ +// 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, + const rclcpp::Node::SharedPtr & provided_node, + CallbackType callback) + : ServiceServer(name, provided_node, callback) + {} +}; + +TEST(ServiceServer, can_handle_all_introspection_modes) +{ + std::vector introspection_modes = { + "disabled", "metadata", "contents" + }; + + for (const auto & mode : introspection_modes) { + int a = 0; + auto node = rclcpp::Node::make_shared("test_node_" + mode); + + node->declare_parameter("service_introspection_mode", mode); + + auto callback = [&a](const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr) { + a = 1; + }; + + TestServiceServer server("empty_srv_" + mode, node, callback); + + auto client_node = rclcpp::Node::make_shared("client_node_" + mode); + auto client = client_node->create_client("empty_srv_" + mode); + + 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) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 55db83df847..fb967e7d964 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 /*creates and spins an internal executor*/); gps_action_server_ = std::make_unique( get_node_base_interface(),