From 2c29ce176d225cf514ff9c6c123a01f4832e980a Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Mon, 17 Feb 2025 11:17:39 +0800 Subject: [PATCH 1/2] Update action cpp demos to support setting introspection Signed-off-by: Barry Xu --- .../src/fibonacci_action_client.cpp | 67 +++++++++++++++++++ .../src/fibonacci_action_server.cpp | 62 +++++++++++++++++ 2 files changed, 129 insertions(+) diff --git a/action_tutorials/action_tutorials_cpp/src/fibonacci_action_client.cpp b/action_tutorials/action_tutorials_cpp/src/fibonacci_action_client.cpp index 68d6cddb8..3932f500e 100644 --- a/action_tutorials/action_tutorials_cpp/src/fibonacci_action_client.cpp +++ b/action_tutorials/action_tutorials_cpp/src/fibonacci_action_client.cpp @@ -23,6 +23,12 @@ #include "action_tutorials_cpp/visibility_control.h" +// The program has a short runtime, so you can directly set the parameter +// "action_client_configure_introspection" at execution command +// e.g. +// ros2 run action_tutorials_cpp fibonacci_action_client --ros-args -p +// "action_client_configure_introspection:=contents" + namespace action_tutorials_cpp { class FibonacciActionClient : public rclcpp::Node @@ -42,6 +48,63 @@ class FibonacciActionClient : public rclcpp::Node this->get_node_waitables_interface(), "fibonacci"); + auto on_set_parameter_callback = + [](std::vector parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + for (const rclcpp::Parameter & param : parameters) { + if (param.get_name() != "action_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() != "disabled" && param.as_string() != "metadata" && + param.as_string() != "contents") + { + result.successful = false; + result.reason = "must be one of 'disabled', '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() != "action_client_configure_introspection") { + continue; + } + + rcl_service_introspection_state_t introspection_state = RCL_SERVICE_INTROSPECTION_OFF; + + if (param.as_string() == "disabled") { + 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_ptr_->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("action_client_configure_introspection", "disabled"); + this->timer_ = this->create_wall_timer( std::chrono::milliseconds(500), [this]() {return this->send_goal();}); @@ -119,6 +182,10 @@ class FibonacciActionClient : public rclcpp::Node private: rclcpp_action::Client::SharedPtr client_ptr_; rclcpp::TimerBase::SharedPtr timer_; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr + on_set_parameters_callback_handle_; + rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr + post_set_parameters_callback_handle_; }; // class FibonacciActionClient } // namespace action_tutorials_cpp diff --git a/action_tutorials/action_tutorials_cpp/src/fibonacci_action_server.cpp b/action_tutorials/action_tutorials_cpp/src/fibonacci_action_server.cpp index f82ae2942..7ca524e79 100644 --- a/action_tutorials/action_tutorials_cpp/src/fibonacci_action_server.cpp +++ b/action_tutorials/action_tutorials_cpp/src/fibonacci_action_server.cpp @@ -73,11 +73,73 @@ class FibonacciActionServer : public rclcpp::Node handle_goal, handle_cancel, handle_accepted); + + auto on_set_parameter_callback = + [](std::vector parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + for (const rclcpp::Parameter & param : parameters) { + if (param.get_name() != "action_server_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() != "disabled" && param.as_string() != "metadata" && + param.as_string() != "contents") + { + result.successful = false; + result.reason = "must be one of 'disabled', '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() != "action_server_configure_introspection") { + continue; + } + + rcl_service_introspection_state_t introspection_state = RCL_SERVICE_INTROSPECTION_OFF; + + if (param.as_string() == "disabled") { + 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->action_server_->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("action_server_configure_introspection", "disabled"); } private: rclcpp_action::Server::SharedPtr action_server_; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr + on_set_parameters_callback_handle_; + rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr + post_set_parameters_callback_handle_; + ACTION_TUTORIALS_CPP_LOCAL void execute(const std::shared_ptr goal_handle) { From dc9823d5ebd2323fa340bc012a55fedd16d3a41c Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Wed, 19 Feb 2025 14:27:52 +0800 Subject: [PATCH 2/2] Add the missing header file declaration Signed-off-by: Barry Xu --- .../action_tutorials_cpp/src/fibonacci_action_client.cpp | 1 + .../action_tutorials_cpp/src/fibonacci_action_server.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/action_tutorials/action_tutorials_cpp/src/fibonacci_action_client.cpp b/action_tutorials/action_tutorials_cpp/src/fibonacci_action_client.cpp index 3932f500e..6a9c605f1 100644 --- a/action_tutorials/action_tutorials_cpp/src/fibonacci_action_client.cpp +++ b/action_tutorials/action_tutorials_cpp/src/fibonacci_action_client.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include "example_interfaces/action/fibonacci.hpp" #include "rclcpp/rclcpp.hpp" diff --git a/action_tutorials/action_tutorials_cpp/src/fibonacci_action_server.cpp b/action_tutorials/action_tutorials_cpp/src/fibonacci_action_server.cpp index 7ca524e79..ea7d19674 100644 --- a/action_tutorials/action_tutorials_cpp/src/fibonacci_action_server.cpp +++ b/action_tutorials/action_tutorials_cpp/src/fibonacci_action_server.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include "example_interfaces/action/fibonacci.hpp" #include "rclcpp/rclcpp.hpp"