Service introspection (#602)

* Add in a rclcpp and rclpy demo of introspection.

Signed-off-by: Chris Lalancette <[email protected]>

demo_nodes_cpp/CMakeLists.txt

@@ -61,7 +61,9 @@ add_library(timers_library SHARED
 add_library(services_library SHARED
   src/services/add_two_ints_server.cpp
   src/services/add_two_ints_client.cpp
-  src/services/add_two_ints_client_async.cpp)
+  src/services/add_two_ints_client_async.cpp
+  src/services/introspection_service.cpp
+  src/services/introspection_client.cpp)
 add_library(parameters_library SHARED
   src/parameters/list_parameters.cpp
   src/parameters/parameter_blackboard.cpp
@@ -96,6 +98,12 @@ rclcpp_components_register_node(services_library
 rclcpp_components_register_node(services_library
   PLUGIN "demo_nodes_cpp::ClientNode"
   EXECUTABLE add_two_ints_client_async)
+rclcpp_components_register_node(services_library
+  PLUGIN "demo_nodes_cpp::IntrospectionServiceNode"
+  EXECUTABLE introspection_service)
+rclcpp_components_register_node(services_library
+  PLUGIN "demo_nodes_cpp::IntrospectionClientNode"
+  EXECUTABLE introspection_client)
 
 rclcpp_components_register_node(parameters_library
   PLUGIN "demo_nodes_cpp::ListParameters"

demo_nodes_cpp/launch/services/introspect_services.launch.py

@@ -0,0 +1,29 @@
+# Copyright 2023 Open Source Robotics Foundation, Inc.
+#
+# 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.
+
+"""Launch the introspection service and client."""
+
+import launch
+import launch_ros.actions
+
+
+def generate_launch_description():
+    server = launch_ros.actions.Node(
+        package='demo_nodes_cpp', executable='introspection_service', output='screen')
+    client = launch_ros.actions.Node(
+        package='demo_nodes_cpp', executable='introspection_client', output='screen')
+    return launch.LaunchDescription([
+        server,
+        client,
+    ])

demo_nodes_cpp/package.xml

@@ -21,6 +21,7 @@
   <build_depend>example_interfaces</build_depend>
   <build_depend>rclcpp</build_depend>
   <build_depend>rclcpp_components</build_depend>
+  <build_depend>rcl_interfaces</build_depend>
   <build_depend>rcutils</build_depend>
   <build_depend>rmw</build_depend>
   <build_depend>rmw_implementation_cmake</build_depend>
@@ -31,6 +32,7 @@
   <exec_depend>launch_xml</exec_depend>
   <exec_depend>rclcpp</exec_depend>
   <exec_depend>rclcpp_components</exec_depend>
+  <exec_depend>rcl_interfaces</exec_depend>
   <exec_depend>rcutils</exec_depend>
   <exec_depend>rmw</exec_depend>
   <exec_depend>std_msgs</exec_depend>

demo_nodes_cpp/src/services/introspection_client.cpp

@@ -0,0 +1,166 @@
+// Copyright 2023 Open Source Robotics Foundation, Inc.
+//
+// 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 <chrono>
+#include <memory>
+#include <vector>
+
+#include "rcl/service_introspection.h"
+
+#include "rclcpp/qos.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_components/register_node_macro.hpp"
+
+#include "example_interfaces/srv/add_two_ints.hpp"
+#include "rcl_interfaces/msg/set_parameters_result.hpp"
+
+#include "demo_nodes_cpp/visibility_control.h"
+
+// This demo program shows how to configure client introspection on the fly,
+// by hooking it up to a parameter.  This program consists of a client
+// node (IntrospectionClientNode) that has a timer callback that runs every
+// 500 milliseconds.  If the service is not yet ready, no further work is done.
+// If the client doesn't currently have a request in flight, then it creates a
+// new AddTwoInts service request, and asynchronously sends it to the service.
+// When that request completes, it sets the flag back to having no requests in
+// flight so another request is sent.
+//
+// The above is a fairly common ROS 2 client, but what this program is trying
+// to demonstrate is introspection capabilities.  The IntrospectionClientNode
+// has a string parameter called 'client_configure_introspection'.  If this is
+// set to 'off' (the default), then no introspection happens.  If this is set
+// to 'metadata' (see details on how to set the parameters below), then
+// essential metadata (timestamps, sequence numbers, etc) is sent to a hidden
+// topic called /add_two_ints/_service_event.
+//
+// To see this in action, run the following:
+//
+// ros2 launch demo_nodes_cpp introspect_services.launch.py
+//   Since the default for introspection is 'off', this is no different than
+//   a normal client and server.  No additional topics will be made, and
+//   no introspection data will be sent.  However, changing the introspection
+//   configuration dynamically is fully supported.  This can be seen by
+//   running 'ros2 param set /introspection_client client_configure_introspection metadata'
+//   which will configure the client to start sending service introspection
+//   metadata to /add_two_ints/_service_event.
+//
+// Once the parameter is set, introspection data can be seen by running:
+//   ros2 topic echo /add_two_ints/_service_event
+
+namespace demo_nodes_cpp
+{
+class IntrospectionClientNode : public rclcpp::Node
+{
+public:
+  DEMO_NODES_CPP_PUBLIC
+  explicit IntrospectionClientNode(const rclcpp::NodeOptions & options)
+  : Node("introspection_client", options)
+  {
+    client_ = create_client<example_interfaces::srv::AddTwoInts>("add_two_ints");
+
+    auto on_set_parameter_callback =
+      [this](std::vector<rclcpp::Parameter> parameters) {
+        rcl_interfaces::msg::SetParametersResult result;
+        result.successful = true;
+        for (const rclcpp::Parameter & param : parameters) {
+          if (param.get_name() != "client_configure_introspection") {
+            continue;
+          }
+
+          if (param.get_type() != rclcpp::ParameterType::PARAMETER_STRING) {
+            result.successful = false;
+            result.reason = "must be a string";
+            break;
+          }
+
+          if (param.as_string() != "off" && param.as_string() != "metadata" &&
+            param.as_string() != "contents")
+          {
+            result.successful = false;
+            result.reason = "must be one of 'off', 'metadata', or 'contents'";
+            break;
+          }
+        }
+
+        return result;
+      };
+
+    auto post_set_parameter_callback =
+      [this](const std::vector<rclcpp::Parameter> & parameters) {
+        for (const rclcpp::Parameter & param : parameters) {
+          if (param.get_name() != "client_configure_introspection") {
+            continue;
+          }
+
+          rcl_service_introspection_state_t introspection_state = RCL_SERVICE_INTROSPECTION_OFF;
+
+          if (param.as_string() == "off") {
+            introspection_state = RCL_SERVICE_INTROSPECTION_OFF;
+          } else if (param.as_string() == "metadata") {
+            introspection_state = RCL_SERVICE_INTROSPECTION_METADATA;
+          } else if (param.as_string() == "contents") {
+            introspection_state = RCL_SERVICE_INTROSPECTION_CONTENTS;
+          }
+
+          this->client_->configure_introspection(
+            this->get_clock(), rclcpp::SystemDefaultsQoS(), introspection_state);
+          break;
+        }
+      };
+
+    on_set_parameters_callback_handle_ = this->add_on_set_parameters_callback(
+      on_set_parameter_callback);
+    post_set_parameters_callback_handle_ = this->add_post_set_parameters_callback(
+      post_set_parameter_callback);
+
+    this->declare_parameter("client_configure_introspection", "off");
+
+    timer_ = this->create_wall_timer(
+      std::chrono::milliseconds(500),
+      [this]() {
+        if (!client_->service_is_ready()) {
+          return;
+        }
+
+        if (!request_in_progress_) {
+          auto request = std::make_shared<example_interfaces::srv::AddTwoInts::Request>();
+          request->a = 2;
+          request->b = 3;
+          request_in_progress_ = true;
+          client_->async_send_request(
+            request,
+            [this](rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedFuture cb_f)
+            {
+              request_in_progress_ = false;
+              RCLCPP_INFO(get_logger(), "Result of add_two_ints: %ld", cb_f.get()->sum);
+            }
+          );
+          return;
+        }
+      });
+  }
+
+private:
+  rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedPtr client_;
+  rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr
+    on_set_parameters_callback_handle_;
+  rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr
+    post_set_parameters_callback_handle_;
+  bool request_in_progress_{false};
+};
+
+}  // namespace demo_nodes_cpp
+
+RCLCPP_COMPONENTS_REGISTER_NODE(demo_nodes_cpp::IntrospectionClientNode)

demo_nodes_cpp/src/services/introspection_service.cpp

@@ -0,0 +1,150 @@
+// Copyright 2023 Open Source Robotics Foundation, Inc.
+//
+// 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 <cinttypes>
+#include <memory>
+#include <vector>
+
+#include "rcl/service_introspection.h"
+
+#include "rclcpp/qos.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_components/register_node_macro.hpp"
+
+#include "example_interfaces/srv/add_two_ints.hpp"
+#include "rcl_interfaces/msg/set_parameters_result.hpp"
+
+#include "demo_nodes_cpp/visibility_control.h"
+
+// This demo program shows how to configure service introspection on the fly,
+// by hooking it up to a parameter.  This program consists of a service
+// node (IntrospectionServiceNode) that listens on the '/add_two_ints' service
+// for clients.  When one connects and sends a request, it adds the two integers
+// and returns the result.
+//
+// The above is a fairly common ROS 2 service, but what this program is trying
+// to demonstrate is introspection capabilities.  The IntrospectionServiceNode
+// has a string parameter called 'service_configure_introspection'.  If this is
+// set to 'off' (the default), then no introspection happens.  If this is set
+// to 'metadata' (see details on how to set the parameters below), then
+// essential metadata (timestamps, sequence numbers, etc) is sent to a hidden
+// topic called /add_two_ints/_service_event.
+//
+// To see this in action, run the following:
+//
+// ros2 launch demo_nodes_cpp introspect_services.launch.py
+//   Since the default for introspection is 'off', this is no different than
+//   a normal client and server.  No additional topics will be made, and
+//   no introspection data will be sent.  However, changing the introspection
+//   configuration dynamically is fully supported.  This can be seen by
+//   running 'ros2 param set /introspection_service service_configure_introspection metadata'
+//   which will configure the service to start sending service introspection
+//   metadata to /add_two_ints/_service_event.
+//
+// Once the parameter is set, introspection data can be seen by running:
+//   ros2 topic echo /add_two_ints/_service_event
+
+namespace demo_nodes_cpp
+{
+
+class IntrospectionServiceNode : public rclcpp::Node
+{
+public:
+  DEMO_NODES_CPP_PUBLIC
+  explicit IntrospectionServiceNode(const rclcpp::NodeOptions & options)
+  : Node("introspection_service", options)
+  {
+    auto handle_add_two_ints =
+      [this](const std::shared_ptr<rmw_request_id_t> request_header,
+        const std::shared_ptr<example_interfaces::srv::AddTwoInts::Request> request,
+        std::shared_ptr<example_interfaces::srv::AddTwoInts::Response> response) -> void
+      {
+        (void)request_header;
+        RCLCPP_INFO(
+          this->get_logger(), "Incoming request\na: %" PRId64 " b: %" PRId64,
+          request->a, request->b);
+        response->sum = request->a + request->b;
+      };
+    // Create a service that will use the callback function to handle requests.
+    srv_ = create_service<example_interfaces::srv::AddTwoInts>("add_two_ints", handle_add_two_ints);
+
+    auto on_set_parameter_callback =
+      [this](std::vector<rclcpp::Parameter> parameters) {
+        rcl_interfaces::msg::SetParametersResult result;
+        result.successful = true;
+        for (const rclcpp::Parameter & param : parameters) {
+          if (param.get_name() != "service_configure_introspection") {
+            continue;
+          }
+
+          if (param.get_type() != rclcpp::ParameterType::PARAMETER_STRING) {
+            result.successful = false;
+            result.reason = "must be a string";
+            break;
+          }
+
+          if (param.as_string() != "off" && param.as_string() != "metadata" &&
+            param.as_string() != "contents")
+          {
+            result.successful = false;
+            result.reason = "must be one of 'off', 'metadata', or 'contents'";
+            break;
+          }
+        }
+
+        return result;
+      };
+
+    auto post_set_parameter_callback =
+      [this](const std::vector<rclcpp::Parameter> & parameters) {
+        for (const rclcpp::Parameter & param : parameters) {
+          if (param.get_name() != "service_configure_introspection") {
+            continue;
+          }
+
+          rcl_service_introspection_state_t introspection_state = RCL_SERVICE_INTROSPECTION_OFF;
+
+          if (param.as_string() == "off") {
+            introspection_state = RCL_SERVICE_INTROSPECTION_OFF;
+          } else if (param.as_string() == "metadata") {
+            introspection_state = RCL_SERVICE_INTROSPECTION_METADATA;
+          } else if (param.as_string() == "contents") {
+            introspection_state = RCL_SERVICE_INTROSPECTION_CONTENTS;
+          }
+
+          this->srv_->configure_introspection(
+            this->get_clock(), rclcpp::SystemDefaultsQoS(), introspection_state);
+          break;
+        }
+      };
+
+    on_set_parameters_callback_handle_ = this->add_on_set_parameters_callback(
+      on_set_parameter_callback);
+    post_set_parameters_callback_handle_ = this->add_post_set_parameters_callback(
+      post_set_parameter_callback);
+
+    this->declare_parameter("service_configure_introspection", "off");
+  }
+
+private:
+  rclcpp::Service<example_interfaces::srv::AddTwoInts>::SharedPtr srv_;
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr
+    on_set_parameters_callback_handle_;
+  rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr
+    post_set_parameters_callback_handle_;
+};
+
+}  // namespace demo_nodes_cpp
+
+RCLCPP_COMPONENTS_REGISTER_NODE(demo_nodes_cpp::IntrospectionServiceNode)