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..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" @@ -23,6 +24,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 +49,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 +183,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..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" @@ -73,11 +74,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) {