diff --git a/demo_nodes_cpp/CMakeLists.txt b/demo_nodes_cpp/CMakeLists.txt index 8e708ea75..8f405b2a5 100644 --- a/demo_nodes_cpp/CMakeLists.txt +++ b/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" diff --git a/demo_nodes_cpp/launch/services/introspect_services.launch.py b/demo_nodes_cpp/launch/services/introspect_services.launch.py new file mode 100644 index 000000000..25e55744b --- /dev/null +++ b/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, + ]) diff --git a/demo_nodes_cpp/package.xml b/demo_nodes_cpp/package.xml index 481df7ae1..d09560132 100644 --- a/demo_nodes_cpp/package.xml +++ b/demo_nodes_cpp/package.xml @@ -21,6 +21,7 @@ example_interfaces rclcpp rclcpp_components + rcl_interfaces rcutils rmw rmw_implementation_cmake @@ -31,6 +32,7 @@ launch_xml rclcpp rclcpp_components + rcl_interfaces rcutils rmw std_msgs diff --git a/demo_nodes_cpp/src/services/introspection_client.cpp b/demo_nodes_cpp/src/services/introspection_client.cpp new file mode 100644 index 000000000..ade6c9e1b --- /dev/null +++ b/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 +#include +#include + +#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("add_two_ints"); + + auto on_set_parameter_callback = + [this](std::vector 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 & 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(); + request->a = 2; + request->b = 3; + request_in_progress_ = true; + client_->async_send_request( + request, + [this](rclcpp::Client::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::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) diff --git a/demo_nodes_cpp/src/services/introspection_service.cpp b/demo_nodes_cpp/src/services/introspection_service.cpp new file mode 100644 index 000000000..62484b605 --- /dev/null +++ b/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 +#include +#include + +#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 request_header, + const std::shared_ptr request, + std::shared_ptr 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("add_two_ints", handle_add_two_ints); + + auto on_set_parameter_callback = + [this](std::vector 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 & 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::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) diff --git a/demo_nodes_py/demo_nodes_py/services/introspection.py b/demo_nodes_py/demo_nodes_py/services/introspection.py new file mode 100644 index 000000000..7718254c2 --- /dev/null +++ b/demo_nodes_py/demo_nodes_py/services/introspection.py @@ -0,0 +1,211 @@ +# 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. + +from example_interfaces.srv import AddTwoInts +from rcl_interfaces.msg import SetParametersResult + +import rclpy +from rclpy.executors import ExternalShutdownException +from rclpy.executors import SingleThreadedExecutor +from rclpy.node import Node +from rclpy.parameter import Parameter +from rclpy.qos import qos_profile_system_default +from rclpy.service_introspection import ServiceIntrospectionState + + +# This demo program shows how to configure client and service introspection +# on the fly, by hooking it up to a parameter. This program consists of both +# a client node (IntrospectionClientNode) and a service node (IntrospectionServiceNode). +# At startup time, one of each type of node is created, and added to an executor. +# The IntrospectionServiceNode 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 IntrospectionClientNode is slightly more complicated. It 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 future outstanding, then it +# creates a new AddTwoInts service request, and asynchronously sends it to the service. +# If the client does have a future outstanding, then we check if it is done. +# Once the future has been completed, we forget about the future so we can send +# another request. +# +# The above is a fairly common ROS 2 client and service, but what this program +# is trying to demonstrate is introspection capabilities. +# The IntrospectionClientNode has a string parameter called 'client_configure_introspection', +# and the IntrospectionServiceNode has a string parameter called 'service_configure_introspection'. +# If these are set to 'off' (the default), then no introspection happens. +# If these are set to 'metadata' (see details on how to set the parameters below), +# then essential metadata (timestamps, sequence numbers, etc) are sent to a +# hidden topic called /add_two_ints/_service_event. +# +# To see this in action, this program can be run in a couple of different ways: +# +# ros2 run demo_nodes_py introspection +# 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. +# +# ros2 run demo_nodes_py introspection \ +# --ros-args -p client_configure_introspection:=metadata \ +# -p service_configure_introspection:=metadata +# Here we've set both the client and service introspection to metadata, +# so the /add_two_ints/_service_event will be created. Additionally, +# every client request and response and every service acceptance and +# response will be sent to that topic. +# +# In either case, service introspection data can be seen by running: +# ros2 topic echo /add_two_ints/_service_event + +def check_parameter(parameter_list, parameter_name): + result = SetParametersResult() + result.successful = True + for param in parameter_list: + if param.name != parameter_name: + continue + + if param.type_ != Parameter.Type.STRING: + result.successful = False + result.reason = 'must be a string' + break + + if param.value not in ('off', 'metadata', 'contents'): + result.successful = False + result.reason = "must be one of 'off', 'metadata', or 'contents" + break + + return result + + +class IntrospectionClientNode(Node): + + def on_set_parameters_callback(self, parameter_list): + return check_parameter(parameter_list, 'client_configure_introspection') + + def on_post_set_parameters_callback(self, parameter_list): + for param in parameter_list: + if param.name != 'client_configure_introspection': + continue + + introspection_state = ServiceIntrospectionState.OFF + if param.value == 'off': + introspection_state = ServiceIntrospectionState.OFF + elif param.value == 'metadata': + introspection_state = ServiceIntrospectionState.METADATA + elif param.value == 'contents': + introspection_state = ServiceIntrospectionState.CONTENTS + + self.cli.configure_introspection(self.get_clock(), qos_profile_system_default, + introspection_state) + break + + def __init__(self): + super().__init__('introspection_client') + + self.cli = self.create_client(AddTwoInts, 'add_two_ints') + + self.add_on_set_parameters_callback(self.on_set_parameters_callback) + self.add_post_set_parameters_callback(self.on_post_set_parameters_callback) + self.declare_parameter('client_configure_introspection', 'off') + + self.timer = self.create_timer(0.5, self.timer_callback) + self.future = None + + def timer_callback(self): + if not self.cli.service_is_ready(): + return + + if self.future is None: + req = AddTwoInts.Request() + req.a = 2 + req.b = 3 + + self.future = self.cli.call_async(req) + + return + + if not self.future.done(): + return + + if self.future.result() is not None: + self.get_logger().info('Result of add_two_ints: %d' % self.future.result().sum) + else: + self.get_logger().error('Exception calling service: %r' % self.future.exception()) + + self.future = None + + +class IntrospectionServiceNode(Node): + + def on_set_parameters_callback(self, parameter_list): + return check_parameter(parameter_list, 'service_configure_introspection') + + def on_post_set_parameters_callback(self, parameter_list): + for param in parameter_list: + if param.name != 'service_configure_introspection': + continue + + introspection_state = ServiceIntrospectionState.OFF + if param.value == 'off': + introspection_state = ServiceIntrospectionState.OFF + elif param.value == 'metadata': + introspection_state = ServiceIntrospectionState.METADATA + elif param.value == 'contents': + introspection_state = ServiceIntrospectionState.CONTENTS + + self.srv.configure_introspection(self.get_clock(), qos_profile_system_default, + introspection_state) + break + + def __init__(self): + super().__init__('introspection_service') + + self.srv = self.create_service(AddTwoInts, 'add_two_ints', self.add_two_ints_callback) + + self.add_on_set_parameters_callback(self.on_set_parameters_callback) + self.add_post_set_parameters_callback(self.on_post_set_parameters_callback) + self.declare_parameter('service_configure_introspection', 'off') + + def add_two_ints_callback(self, request, response): + response.sum = request.a + request.b + self.get_logger().info('Incoming request\na: %d b: %d' % (request.a, request.b)) + + return response + + +def main(args=None): + rclpy.init(args=args) + + service_node = IntrospectionServiceNode() + + client_node = IntrospectionClientNode() + + executor = SingleThreadedExecutor() + executor.add_node(service_node) + executor.add_node(client_node) + + try: + executor.spin() + except (KeyboardInterrupt, ExternalShutdownException): + executor.remove_node(client_node) + executor.remove_node(service_node) + executor.shutdown() + service_node.destroy_node() + client_node.destroy_node() + rclpy.try_shutdown() + + +if __name__ == '__main__': + main() diff --git a/demo_nodes_py/package.xml b/demo_nodes_py/package.xml index 3c22ff438..482661451 100644 --- a/demo_nodes_py/package.xml +++ b/demo_nodes_py/package.xml @@ -20,6 +20,7 @@ example_interfaces rclpy + rcl_interfaces std_msgs ament_copyright diff --git a/demo_nodes_py/setup.py b/demo_nodes_py/setup.py index c505db55b..afbd9af0a 100644 --- a/demo_nodes_py/setup.py +++ b/demo_nodes_py/setup.py @@ -41,7 +41,8 @@ 'add_two_ints_client_async = demo_nodes_py.services.add_two_ints_client_async:main', 'add_two_ints_server = demo_nodes_py.services.add_two_ints_server:main', 'async_param_client = demo_nodes_py.parameters.async_param_client:main', - 'set_parameters_callback = demo_nodes_py.parameters.set_parameters_callback:main' + 'set_parameters_callback = demo_nodes_py.parameters.set_parameters_callback:main', + 'introspection = demo_nodes_py.services.introspection:main', ], }, )