Add service introspection #4955

mini-1235 · 2025-03-02T14:07:31Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#4837
Primary OS tested on	Ubuntu
Robotic platform tested on	gazebo sim
Does this PR contain AI generated software?	No

Description of contribution in a few bullet points

Added service introspection as described in the issue (I created a service factory in nav2_util as not all service servers are derived from the LifecycleNode class)

Description of documentation updates required from your changes

Documents related to nav2_params would require a change, will raise a PR in the docs repo when this PR is ready

Description of how this change was tested

I add a simple unit test, and the changes were mostly tested in my sim env (using cli to make sure service introspection works)

Future work that may be required in bullet points

For Maintainers:

Check that any new parameters added are updated in docs.nav2.org
Check that any significant change is added to the migration guide
Check that any new features OR changes to existing behaviors are reflected in the tuning guide
Check that any new functions have Doxygen added
Check that any new features have test coverage
Check that any new plugins is added to the plugins page
If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

Signed-off-by: mini-1235 <[email protected]>

codecov · 2025-03-02T15:07:48Z

Codecov Report

Attention: Patch coverage is 88.46154% with 15 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
nav2_util/include/nav2_util/service_client.hpp	68.75%	5 Missing ⚠️
nav2_costmap_2d/src/clear_costmap_service.cpp	70.00%	3 Missing ⚠️
nav2_costmap_2d/src/costmap_2d_publisher.cpp	60.00%	2 Missing ⚠️
nav2_util/include/nav2_util/service_server.hpp	80.00%	2 Missing ⚠️
...tmap_2d/plugins/costmap_filters/costmap_filter.cpp	80.00%	1 Missing ⚠️
nav2_docking/opennav_docking/src/dock_database.cpp	80.00%	1 Missing ⚠️
nav2_lifecycle_manager/src/lifecycle_manager.cpp	94.73%	1 Missing ⚠️

Files with missing lines	Coverage Δ
nav2_amcl/src/amcl_node.cpp	`70.89% <100.00%> (+0.23%)`	⬆️
...ree/include/nav2_behavior_tree/bt_service_node.hpp	`83.82% <100.00%> (+0.74%)`	⬆️
...tree/plugins/condition/is_path_valid_condition.hpp	`100.00% <ø> (ø)`
...tree/plugins/condition/is_path_valid_condition.cpp	`100.00% <100.00%> (ø)`
...d/include/nav2_costmap_2d/costmap_2d_publisher.hpp	`100.00% <ø> (ø)`
...tmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp	`100.00% <ø> (ø)`
...nav2_costmap_2d/costmap_filters/costmap_filter.hpp	`66.66% <ø> (ø)`
nav2_costmap_2d/src/costmap_2d_ros.cpp	`85.75% <100.00%> (+0.38%)`	⬆️
...lifecycle_manager/src/lifecycle_manager_client.cpp	`88.63% <100.00%> (+0.83%)`	⬆️
nav2_map_server/src/map_saver/map_saver.cpp	`95.23% <100.00%> (+1.41%)`	⬆️
... and 11 more

... and 4 files with indirect coverage changes

🚀 New features to boost your workflow:

❄ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

Signed-off-by: mini-1235 <[email protected]>

SteveMacenski

Great first stab! For each server that has one of these service clients, we should add into the configuration guide docs the parameter so that our param guides remain up to date!

SteveMacenski · 2025-03-11T01:58:01Z

nav2_planner/src/planner_server.cpp

@@ -54,14 +54,15 @@ 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");


You should make this a part of the service client itself to declare / get the service introspection mode. Then it can be removed from all of these nodes. Within the ServiceClient / ServiceServer you can use the node to find these parameters.

That would remove a good chunk of the line changes in this PR by removing the declare/get/pass in the parameter in each server. That would help readability and separation of concerns

SteveMacenski · 2025-03-11T02:04:30Z

nav2_rviz_plugins/src/costmap_cost_tool.cpp

@@ -106,34 +116,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))) {


I think these should be reverted, the invoke shared would block the thread which would make all of rviz seize up until the service client got a response.

SteveMacenski · 2025-03-11T02:05:09Z

nav2_util/include/nav2_util/service_client.hpp

@@ -48,6 +49,18 @@ class ServiceClient
      service_name,
      rclcpp::SystemDefaultsQoS(),
      callback_group_);
+    rcl_service_introspection_state_t introspection_state = RCL_SERVICE_INTROSPECTION_OFF;
+


This is where you can declare and get the service introspection value internally to the class!

SteveMacenski · 2025-03-11T02:06:29Z

nav2_util/include/nav2_util/service_server.hpp

+ * @class nav2_util::ServiceServer
+ * @brief A simple wrapper on ROS2 services for invoke() and block-style calling
+ */
+template<class ServiceT, typename NodeT = rclcpp::Node::SharedPtr>


If you look over the annotations, there's some pretty big testing gaps in this and the service client file from these changes. I think a few more unit tests in the various introspection modes are necessary

SteveMacenski · 2025-03-11T02:07:42Z

nav2_util/include/nav2_util/service_client.hpp

+  * @param request The request object to call the service using
+  * @return std::shared_future<typename ResponseType::SharedPtr> The shared future of the service response
+  */
+  std::shared_future<typename ResponseType::SharedPtr> invoke_shared(


What's the value of this if we can return the response instead? Why return the shared future?

SteveMacenski · 2025-03-11T02:08:34Z

nav2_util/include/nav2_util/service_server.hpp

+
+  explicit ServiceServer(
+    const std::string & service_name,
+    std::string service_introspection_mode,


This could/should be removed and done in the constructor as well using the node input

SteveMacenski · 2025-03-11T02:10:56Z

nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp

@@ -148,7 +151,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_);


Is this the only necessary use of invoke_shared? If so, maybe we should have this remain a raw create_client and pass in the service introspection params into it

mergify · 2025-03-11T02:11:48Z

This pull request is in conflict. Could you fix it @mini-1235?

mini-1235 added 3 commits February 23, 2025 18:44

Add service introspection for client and server side, more tests to go

a9c75b9

Signed-off-by: mini-1235 <[email protected]>

Add test for service server, fix repeat declaration

68cfc0c

Signed-off-by: mini-1235 <[email protected]>

Fix cpplint

6b41eea

Signed-off-by: mini-1235 <[email protected]>

Add test for coverage

076b9a8

Signed-off-by: mini-1235 <[email protected]>

SteveMacenski reviewed Mar 11, 2025

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add service introspection #4955

Add service introspection #4955

mini-1235 commented Mar 2, 2025

codecov bot commented Mar 2, 2025 •

edited

Loading

SteveMacenski left a comment •

edited

Loading

SteveMacenski Mar 11, 2025 •

edited

Loading

SteveMacenski Mar 11, 2025

SteveMacenski Mar 11, 2025 •

edited

Loading

SteveMacenski Mar 11, 2025

SteveMacenski Mar 11, 2025

SteveMacenski Mar 11, 2025

SteveMacenski Mar 11, 2025

mergify bot commented Mar 11, 2025

Add service introspection #4955

Are you sure you want to change the base?

Add service introspection #4955

Conversation

mini-1235 commented Mar 2, 2025

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Description of how this change was tested

Future work that may be required in bullet points

For Maintainers:

codecov bot commented Mar 2, 2025 • edited Loading

Codecov Report

SteveMacenski left a comment • edited Loading

Choose a reason for hiding this comment

SteveMacenski Mar 11, 2025 • edited Loading

Choose a reason for hiding this comment

SteveMacenski Mar 11, 2025

Choose a reason for hiding this comment

SteveMacenski Mar 11, 2025 • edited Loading

Choose a reason for hiding this comment

SteveMacenski Mar 11, 2025

Choose a reason for hiding this comment

SteveMacenski Mar 11, 2025

Choose a reason for hiding this comment

SteveMacenski Mar 11, 2025

Choose a reason for hiding this comment

SteveMacenski Mar 11, 2025

Choose a reason for hiding this comment

mergify bot commented Mar 11, 2025

codecov bot commented Mar 2, 2025 •

edited

Loading

SteveMacenski left a comment •

edited

Loading

SteveMacenski Mar 11, 2025 •

edited

Loading

SteveMacenski Mar 11, 2025 •

edited

Loading