From 5f332fe3110c167119de6bc257ffc1067e8c7ea7 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Thu, 22 Jun 2023 13:01:03 -0700 Subject: [PATCH 01/13] First draft TypeDescriptions interface with readonly param configuration Signed-off-by: Emerson Knapp --- rclcpp/CMakeLists.txt | 1 + rclcpp/include/rclcpp/node.hpp | 7 + .../node_interfaces/node_interfaces.hpp | 2 + .../node_type_descriptions.hpp | 66 +++++++++ .../node_type_descriptions_interface.hpp | 44 ++++++ rclcpp/src/rclcpp/node.cpp | 14 ++ .../node_type_descriptions.cpp | 140 ++++++++++++++++++ .../rclcpp_lifecycle/lifecycle_node.hpp | 6 + rclcpp_lifecycle/src/lifecycle_node.cpp | 7 + 9 files changed, 287 insertions(+) create mode 100644 rclcpp/include/rclcpp/node_interfaces/node_type_descriptions.hpp create mode 100644 rclcpp/include/rclcpp/node_interfaces/node_type_descriptions_interface.hpp create mode 100644 rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index b2d1023064..bd705be06d 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -92,6 +92,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/node_interfaces/node_time_source.cpp src/rclcpp/node_interfaces/node_timers.cpp src/rclcpp/node_interfaces/node_topics.cpp + src/rclcpp/node_interfaces/node_type_descriptions.cpp src/rclcpp/node_interfaces/node_waitables.cpp src/rclcpp/node_options.cpp src/rclcpp/parameter.cpp diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 556a6d0318..50bd9da6a0 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -56,6 +56,7 @@ #include "rclcpp/node_interfaces/node_time_source_interface.hpp" #include "rclcpp/node_interfaces/node_timers_interface.hpp" #include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/node_interfaces/node_type_descriptions_interface.hpp" #include "rclcpp/node_interfaces/node_waitables_interface.hpp" #include "rclcpp/node_options.hpp" #include "rclcpp/parameter.hpp" @@ -1456,6 +1457,11 @@ class Node : public std::enable_shared_from_this rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr get_node_time_source_interface(); + /// Return the Node's internal NodeTypeDescriptionsInterface implementation. + RCLCPP_PUBLIC + rclcpp::node_interfaces::NodeTypeDescriptionsInterface::SharedPtr + get_node_type_descriptions_interface(); + /// Return the sub-namespace, if this is a sub-node, otherwise an empty string. /** * The returned sub-namespace is either the accumulated sub-namespaces which @@ -1588,6 +1594,7 @@ class Node : public std::enable_shared_from_this rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_; rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr node_time_source_; + rclcpp::node_interfaces::NodeTypeDescriptionsInterface::SharedPtr node_type_descriptions_; rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_; const rclcpp::NodeOptions node_options_; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp b/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp index bb4886d592..a7c7b7af96 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp @@ -30,6 +30,7 @@ rclcpp::node_interfaces::NodeTimeSourceInterface, \ rclcpp::node_interfaces::NodeTimersInterface, \ rclcpp::node_interfaces::NodeTopicsInterface, \ + rclcpp::node_interfaces::NodeTypeDescriptionsInterface, \ rclcpp::node_interfaces::NodeWaitablesInterface @@ -118,6 +119,7 @@ class NodeInterfaces * - rclcpp::node_interfaces::NodeTimeSourceInterface * - rclcpp::node_interfaces::NodeTimersInterface * - rclcpp::node_interfaces::NodeTopicsInterface + * - rclcpp::node_interfaces::NodeTypeDescriptionsInterface * - rclcpp::node_interfaces::NodeWaitablesInterface * * Or you use custom interfaces as long as you make a template specialization diff --git a/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions.hpp b/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions.hpp new file mode 100644 index 0000000000..a0e8596a3c --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions.hpp @@ -0,0 +1,66 @@ +// 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. + +#ifndef RCLCPP__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_HPP_ + +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/node_interfaces/node_type_descriptions_interface.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + + +/// Implementation of the NodeTypeDescriptions part of the Node API. +class NodeTypeDescriptions : public NodeTypeDescriptionsInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTypeDescriptions) + + RCLCPP_PUBLIC + explicit NodeTypeDescriptions( + NodeBaseInterface::SharedPtr node_base, + NodeLoggingInterface::SharedPtr node_logging, + NodeParametersInterface::SharedPtr node_parameters, + NodeServicesInterface::SharedPtr node_services, + NodeTopicsInterface::SharedPtr node_topics); + + RCLCPP_PUBLIC + virtual + ~NodeTypeDescriptions(); + +private: + RCLCPP_DISABLE_COPY(NodeTypeDescriptions) + + // rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; + // rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_; + // rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_; + // rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; + + class NodeTypeDescriptionsImpl; + std::unique_ptr impl_; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions_interface.hpp new file mode 100644 index 0000000000..e7e0b0af2e --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions_interface.hpp @@ -0,0 +1,44 @@ +// 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. + +#ifndef RCLCPP__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_INTERFACE_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_INTERFACE_HPP_ + +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Pure virtual interface class for the NodeTypeDescriptions part of the Node API. +class NodeTypeDescriptionsInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTypeDescriptionsInterface) + + RCLCPP_PUBLIC + virtual + ~NodeTypeDescriptionsInterface() = default; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT( + rclcpp::node_interfaces::NodeTypeDescriptionsInterface, type_descriptions) + +#endif // RCLCPP__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_INTERFACE_HPP_ diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 53e77dd796..5f3c53206f 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -34,6 +34,7 @@ #include "rclcpp/node_interfaces/node_parameters.hpp" #include "rclcpp/node_interfaces/node_services.hpp" #include "rclcpp/node_interfaces/node_time_source.hpp" +#include "rclcpp/node_interfaces/node_type_descriptions.hpp" #include "rclcpp/node_interfaces/node_timers.hpp" #include "rclcpp/node_interfaces/node_topics.hpp" #include "rclcpp/node_interfaces/node_waitables.hpp" @@ -206,6 +207,13 @@ Node::Node( options.clock_qos(), options.use_clock_thread() )), + node_type_descriptions_(new rclcpp::node_interfaces::NodeTypeDescriptions( + node_base_, + node_logging_, + node_parameters_, + node_services_, + node_topics_ + )), node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())), node_options_(options), sub_namespace_(""), @@ -591,6 +599,12 @@ Node::get_node_topics_interface() return node_topics_; } +rclcpp::node_interfaces::NodeTypeDescriptionsInterface::SharedPtr +Node::get_node_type_descriptions_interface() +{ + return node_type_descriptions_; +} + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr Node::get_node_services_interface() { diff --git a/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp b/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp new file mode 100644 index 0000000000..f1a8e466fd --- /dev/null +++ b/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp @@ -0,0 +1,140 @@ +// 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 "rclcpp/node_interfaces/node_type_descriptions.hpp" + +#include "rclcpp/parameter_client.hpp" + +#include "type_description_interfaces/srv/get_type_description.hpp" + +#include +#include + +using rclcpp::node_interfaces::NodeTypeDescriptions; + +namespace rclcpp +{ + +static constexpr const char * get_type_description_service_name = "get_type_description"; + +class NodeTypeDescriptions::NodeTypeDescriptionsImpl +{ +public: + bool enabled_ = false; + Logger logger_; + node_interfaces::OnSetParametersCallbackHandle param_callback_; + std::shared_ptr> param_sub_; + using ServiceT = Service; + + ServiceT::SharedPtr type_description_srv_; + + + NodeTypeDescriptionsImpl( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters, + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr /* node_topics */) + : logger_(node_logging->get_logger()) + { + const std::string enable_param_name = "enable_type_description_service"; + const std::string service_name = "get_type_description"; + + rclcpp::ParameterValue enable_param; + if (!node_parameters->has_parameter(enable_param_name)) { + enable_param = node_parameters->declare_parameter( + enable_param_name, rclcpp::ParameterValue(true)); + } else { + enable_param = node_parameters->get_parameter(enable_param_name).get_parameter_value(); + } + if (enable_param.get_type() == rclcpp::PARAMETER_BOOL) { + if (enable_param.get()) { + enabled_ = true; + } + } else { + RCLCPP_ERROR( + logger_, "Invalid type '%s' for parameter '%s', should be 'bool'", + rclcpp::to_string(enable_param.get_type()).c_str(), enable_param_name.c_str()); + throw std::invalid_argument( + "Invalid type for parameter '" + enable_param_name + "', should be bool"); + } + + if (enabled_) { + auto rcl_node = node_base->get_shared_rcl_node_handle(); + rcl_ret_t rcl_ret = rcl_node_type_description_service_init(rcl_node.get()); + if (rcl_ret != RCL_RET_OK) { + RCLCPP_ERROR( + logger_, "Failed to initialize ~/get_type_description_service: %s", + rcl_get_error_string().str); + throw std::runtime_error( + "Failed to initialize ~/get_type_description service."); + } + + rcl_service_t * rcl_srv = NULL; + rcl_ret = rcl_node_get_type_description_service(rcl_node.get(), &rcl_srv); + if (rcl_ret != RCL_RET_OK) { + throw std::runtime_error( + "Failed to get initialized ~/get_type_description service from rcl."); + } + + rclcpp::AnyServiceCallback callback; + callback.set( + [&rcl_node]( + std::shared_ptr /*header*/, + std::shared_ptr /*request*/ + ) { + rcl_node_type_description_service_on_new_request(rcl_node.get()); + }); + + type_description_srv_ = std::make_shared(rcl_node, rcl_srv, callback); + node_services->add_service( + std::dynamic_pointer_cast(type_description_srv_), nullptr); + } + + // param_callback_ = node_parameters->add_on_set_parameters_callback( + // std::bind(&NodeTypeDescriptionsImpl::on_set_parameters, this, std::placeholders::_1)); + // // TODO(tfoote) use parameters interface not subscribe to events via topic ticketed #609 + // param_sub_ = rclcpp::AsyncParametersClient::on_parameter_event( + // node_topics, + // std::bind(&NodeTypeDescriptionsImpl::on_parameter_event, this, std::placeholders::_1)); + } +}; + + +NodeTypeDescriptions::NodeTypeDescriptions( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters, + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics) +: + // node_base_(node_base), + // node_services_(node_services), + // node_logging_(node_logging), + // node_parameters_(node_parameters) + impl_(new NodeTypeDescriptionsImpl( + node_base, + node_logging, + node_parameters, + node_services, + node_topics + )) +{ + // TODO init rcl node type description service and attach callback +} + +NodeTypeDescriptions::~NodeTypeDescriptions() +{} + +} // namespace rclcpp diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 197ecf5c19..d3654ab2b2 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -72,6 +72,7 @@ #include "rclcpp/node_interfaces/node_time_source_interface.hpp" #include "rclcpp/node_interfaces/node_timers_interface.hpp" #include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/node_interfaces/node_type_descriptions_interface.hpp" #include "rclcpp/node_interfaces/node_waitables_interface.hpp" #include "rclcpp/parameter.hpp" #include "rclcpp/publisher.hpp" @@ -823,6 +824,10 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr get_node_time_source_interface(); + RCLCPP_LIFECYCLE_PUBLIC + rclcpp::node_interfaces::NodeTypeDescriptionsInterface::SharedPtr + get_node_type_descriptions_interface(); + /// Return the Node's internal NodeWaitablesInterface implementation. /** * \sa rclcpp::Node::get_node_waitables_interface @@ -1085,6 +1090,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_; rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr node_time_source_; + rclcpp::node_interfaces::NodeTypeDescriptionsInterface::SharedPtr node_type_descriptions_; rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_; const rclcpp::NodeOptions node_options_; diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 4c0b94cb42..256c3c5f12 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -41,6 +41,7 @@ #include "rclcpp/node_interfaces/node_parameters.hpp" #include "rclcpp/node_interfaces/node_services.hpp" #include "rclcpp/node_interfaces/node_time_source.hpp" +#include "rclcpp/node_interfaces/node_type_descriptions.hpp" #include "rclcpp/node_interfaces/node_timers.hpp" #include "rclcpp/node_interfaces/node_topics.hpp" #include "rclcpp/node_interfaces/node_waitables.hpp" @@ -113,6 +114,12 @@ LifecycleNode::LifecycleNode( options.clock_qos(), options.use_clock_thread() )), + node_type_descriptions_(new rclcpp::node_interfaces::NodeTypeDescriptions( + node_base_, + node_services_, + node_logging_, + node_parameters_ + )), node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())), node_options_(options), impl_(new LifecycleNodeInterfaceImpl(node_base_, node_services_)) From 0a4c474e03e6f2d5ccd7fdf2f4b34a5a089d17b9 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Thu, 22 Jun 2023 13:28:01 -0700 Subject: [PATCH 02/13] Add parameter descriptor, to make read only Signed-off-by: Emerson Knapp --- rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp b/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp index f1a8e466fd..57178edb58 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp @@ -53,6 +53,11 @@ class NodeTypeDescriptions::NodeTypeDescriptionsImpl rclcpp::ParameterValue enable_param; if (!node_parameters->has_parameter(enable_param_name)) { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.name = enable_param_name; + descriptor.type = rclcpp::PARAMETER_BOOL; + descriptor.description = "Enable the ~/get_type_description service for this node."; + descriptor.read_only = true; enable_param = node_parameters->declare_parameter( enable_param_name, rclcpp::ParameterValue(true)); } else { From 3fd34cdc1bed0cdcf2c84b899cab3b3acf6d4b7d Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Fri, 23 Jun 2023 15:09:53 -0700 Subject: [PATCH 03/13] Trying to fix C/C++ typesupport crash issues Signed-off-by: Emerson Knapp --- .../include/rclcpp/any_service_callback.hpp | 9 ++- .../node_type_descriptions.hpp | 5 -- .../src/rclcpp/node_interfaces/node_base.cpp | 10 ++++ .../node_type_descriptions.cpp | 58 ++++++++++++++----- rclcpp_lifecycle/src/lifecycle_node.cpp | 5 +- 5 files changed, 65 insertions(+), 22 deletions(-) diff --git a/rclcpp/include/rclcpp/any_service_callback.hpp b/rclcpp/include/rclcpp/any_service_callback.hpp index 918d8e5a29..713caff588 100644 --- a/rclcpp/include/rclcpp/any_service_callback.hpp +++ b/rclcpp/include/rclcpp/any_service_callback.hpp @@ -162,6 +162,11 @@ class AnyServiceCallback // to pass a callback at construnciton. throw std::runtime_error{"unexpected request without any callback set"}; } + if (std::holds_alternative(callback_)) { + const auto & cb = std::get(callback_); + cb(); + return nullptr; + } if (std::holds_alternative(callback_)) { const auto & cb = std::get(callback_); cb(request_header, std::move(request)); @@ -226,13 +231,15 @@ class AnyServiceCallback std::shared_ptr, std::shared_ptr )>; + using FullyDeferredCallback = std::function; std::variant< std::monostate, SharedPtrCallback, SharedPtrWithRequestHeaderCallback, SharedPtrDeferResponseCallback, - SharedPtrDeferResponseCallbackWithServiceHandle> callback_; + SharedPtrDeferResponseCallbackWithServiceHandle, + FullyDeferredCallback> callback_; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions.hpp b/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions.hpp index a0e8596a3c..14e5f9191e 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions.hpp @@ -51,11 +51,6 @@ class NodeTypeDescriptions : public NodeTypeDescriptionsInterface private: RCLCPP_DISABLE_COPY(NodeTypeDescriptions) - // rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; - // rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_; - // rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_; - // rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; - class NodeTypeDescriptionsImpl; std::unique_ptr impl_; }; diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index 6544d69214..1a147ead6e 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -20,6 +20,7 @@ #include "rclcpp/node_interfaces/node_base.hpp" #include "rcl/arguments.h" +#include "rcl/node_type_cache.h" #include "rclcpp/exceptions.hpp" #include "rcutils/logging_macros.h" #include "rmw/validate_namespace.h" @@ -114,6 +115,15 @@ NodeBase::NodeBase( throw_from_rcl_error(ret, "failed to initialize rcl node"); } + // If type description service will be initialized, it must capture all + // built-in services and topics that the node creates, even the ones by NodeParameters, + // which must be initialized first to let NodeTypeDescriptions be parameter-enabled + ret = rcl_node_type_cache_init(rcl_node.get()); + if (ret != RCL_RET_OK) { + throw std::runtime_error("It bad!"); + // TODO(ek) + } + node_handle_.reset( rcl_node.release(), [logging_mutex](rcl_node_t * node) -> void { diff --git a/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp b/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp index 57178edb58..196cfdb48c 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp @@ -16,18 +16,40 @@ #include "rclcpp/parameter_client.hpp" -#include "type_description_interfaces/srv/get_type_description.hpp" +#include "type_description_interfaces/srv/get_type_description.h" #include #include using rclcpp::node_interfaces::NodeTypeDescriptions; +// Even though we just want to use a C service, still need to define a C++-like struct +// for the Service to use to allocate responses/requests. +// There is an allocation and deserialization via the Executor callback mechanism, at least +// of the request, even though we just want to let the C layer take it... +struct GetTypeDescriptionC +{ + using Request = type_description_interfaces__srv__GetTypeDescription_Request; + using Response = type_description_interfaces__srv__GetTypeDescription_Response; + using Event = type_description_interfaces__srv__GetTypeDescription_Event; +}; + +namespace rosidl_typesupport_cpp +{ +template<> +rosidl_service_type_support_t const* +get_service_type_support_handle() +{ + return ROSIDL_GET_SRV_TYPE_SUPPORT(type_description_interfaces, srv, GetTypeDescription); +} +} // namespace rosidl_typesupport_cpp + namespace rclcpp { static constexpr const char * get_type_description_service_name = "get_type_description"; + class NodeTypeDescriptions::NodeTypeDescriptionsImpl { public: @@ -35,9 +57,10 @@ class NodeTypeDescriptions::NodeTypeDescriptionsImpl Logger logger_; node_interfaces::OnSetParametersCallbackHandle param_callback_; std::shared_ptr> param_sub_; - using ServiceT = Service; + // using ServiceT = type_description_interfaces::srv::GetTypeDescription; + using ServiceT = GetTypeDescriptionC; - ServiceT::SharedPtr type_description_srv_; + Service::SharedPtr type_description_srv_; NodeTypeDescriptionsImpl( @@ -93,16 +116,21 @@ class NodeTypeDescriptions::NodeTypeDescriptionsImpl "Failed to get initialized ~/get_type_description service from rcl."); } - rclcpp::AnyServiceCallback callback; - callback.set( - [&rcl_node]( - std::shared_ptr /*header*/, - std::shared_ptr /*request*/ - ) { - rcl_node_type_description_service_on_new_request(rcl_node.get()); - }); - - type_description_srv_ = std::make_shared(rcl_node, rcl_srv, callback); + rclcpp::AnyServiceCallback cb; + cb.set([this, &rcl_node]() { + RCLCPP_WARN(logger_, "SERVICE CALLBACK"); + rcl_node_type_description_service_on_new_request(rcl_node.get()); + }); + + type_description_srv_ = std::make_shared>( + rcl_node, + rcl_srv, + cb + // [this, &rcl_node]() { + // RCLCPP_ERROR(logger_, "SERBICE"); + // // rcl_node_type_description_service_on_new_request(rcl_node.get()); + // } + ); node_services->add_service( std::dynamic_pointer_cast(type_description_srv_), nullptr); } @@ -140,6 +168,8 @@ NodeTypeDescriptions::NodeTypeDescriptions( } NodeTypeDescriptions::~NodeTypeDescriptions() -{} +{ + RCUTILS_LOG_WARN("Destroy TDimpl"); +} } // namespace rclcpp diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 256c3c5f12..9eba8c5e10 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -116,9 +116,10 @@ LifecycleNode::LifecycleNode( )), node_type_descriptions_(new rclcpp::node_interfaces::NodeTypeDescriptions( node_base_, - node_services_, node_logging_, - node_parameters_ + node_parameters_, + node_services_, + node_topics_ )), node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())), node_options_(options), From e2caa55d5a1cf6c622fc13e5f1ececea1db2a693 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Fri, 23 Jun 2023 15:34:48 -0700 Subject: [PATCH 04/13] Need to hand on to node ptr otherwise the pointer is invalidated before the callback Signed-off-by: Emerson Knapp --- .../node_interfaces/node_type_descriptions.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp b/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp index 196cfdb48c..468f827332 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp @@ -62,6 +62,8 @@ class NodeTypeDescriptions::NodeTypeDescriptionsImpl Service::SharedPtr type_description_srv_; + std::shared_ptr rcl_node_; + NodeTypeDescriptionsImpl( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, @@ -99,8 +101,8 @@ class NodeTypeDescriptions::NodeTypeDescriptionsImpl } if (enabled_) { - auto rcl_node = node_base->get_shared_rcl_node_handle(); - rcl_ret_t rcl_ret = rcl_node_type_description_service_init(rcl_node.get()); + rcl_node_ = node_base->get_shared_rcl_node_handle(); + rcl_ret_t rcl_ret = rcl_node_type_description_service_init(rcl_node_.get()); if (rcl_ret != RCL_RET_OK) { RCLCPP_ERROR( logger_, "Failed to initialize ~/get_type_description_service: %s", @@ -110,20 +112,20 @@ class NodeTypeDescriptions::NodeTypeDescriptionsImpl } rcl_service_t * rcl_srv = NULL; - rcl_ret = rcl_node_get_type_description_service(rcl_node.get(), &rcl_srv); + rcl_ret = rcl_node_get_type_description_service(rcl_node_.get(), &rcl_srv); if (rcl_ret != RCL_RET_OK) { throw std::runtime_error( "Failed to get initialized ~/get_type_description service from rcl."); } rclcpp::AnyServiceCallback cb; - cb.set([this, &rcl_node]() { + cb.set([this]() { RCLCPP_WARN(logger_, "SERVICE CALLBACK"); - rcl_node_type_description_service_on_new_request(rcl_node.get()); + rcl_node_type_description_service_on_new_request(rcl_node_.get()); }); type_description_srv_ = std::make_shared>( - rcl_node, + rcl_node_, rcl_srv, cb // [this, &rcl_node]() { From fc3c8c9022f45f4abfda4e6835c0acebf86264b9 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Fri, 23 Jun 2023 17:40:17 -0700 Subject: [PATCH 05/13] Respones also Signed-off-by: Emerson Knapp --- .../node_type_descriptions.cpp | 60 +++++++------------ 1 file changed, 21 insertions(+), 39 deletions(-) diff --git a/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp b/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp index 468f827332..f41298389c 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp @@ -57,12 +57,9 @@ class NodeTypeDescriptions::NodeTypeDescriptionsImpl Logger logger_; node_interfaces::OnSetParametersCallbackHandle param_callback_; std::shared_ptr> param_sub_; - // using ServiceT = type_description_interfaces::srv::GetTypeDescription; using ServiceT = GetTypeDescriptionC; - Service::SharedPtr type_description_srv_; - - std::shared_ptr rcl_node_; + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; NodeTypeDescriptionsImpl( @@ -72,6 +69,7 @@ class NodeTypeDescriptions::NodeTypeDescriptionsImpl rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services, rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr /* node_topics */) : logger_(node_logging->get_logger()) + , node_base_(node_base) { const std::string enable_param_name = "enable_type_description_service"; const std::string service_name = "get_type_description"; @@ -101,8 +99,8 @@ class NodeTypeDescriptions::NodeTypeDescriptionsImpl } if (enabled_) { - rcl_node_ = node_base->get_shared_rcl_node_handle(); - rcl_ret_t rcl_ret = rcl_node_type_description_service_init(rcl_node_.get()); + auto rcl_node = node_base->get_rcl_node_handle(); + rcl_ret_t rcl_ret = rcl_node_type_description_service_init(rcl_node); if (rcl_ret != RCL_RET_OK) { RCLCPP_ERROR( logger_, "Failed to initialize ~/get_type_description_service: %s", @@ -112,37 +110,31 @@ class NodeTypeDescriptions::NodeTypeDescriptionsImpl } rcl_service_t * rcl_srv = NULL; - rcl_ret = rcl_node_get_type_description_service(rcl_node_.get(), &rcl_srv); + rcl_ret = rcl_node_get_type_description_service(rcl_node, &rcl_srv); if (rcl_ret != RCL_RET_OK) { throw std::runtime_error( "Failed to get initialized ~/get_type_description service from rcl."); } - rclcpp::AnyServiceCallback cb; - cb.set([this]() { + rclcpp::AnyServiceCallback cb; + cb.set([this]( + std::shared_ptr header, + std::shared_ptr request, + std::shared_ptr response + ) { RCLCPP_WARN(logger_, "SERVICE CALLBACK"); - rcl_node_type_description_service_on_new_request(rcl_node_.get()); + rcl_node_type_description_service_handle_request( + node_base_->get_rcl_node_handle(), + *header, + request.get(), + response.get()); }); type_description_srv_ = std::make_shared>( - rcl_node_, - rcl_srv, - cb - // [this, &rcl_node]() { - // RCLCPP_ERROR(logger_, "SERBICE"); - // // rcl_node_type_description_service_on_new_request(rcl_node.get()); - // } - ); + node_base_->get_shared_rcl_node_handle(), rcl_srv, cb); node_services->add_service( std::dynamic_pointer_cast(type_description_srv_), nullptr); } - - // param_callback_ = node_parameters->add_on_set_parameters_callback( - // std::bind(&NodeTypeDescriptionsImpl::on_set_parameters, this, std::placeholders::_1)); - // // TODO(tfoote) use parameters interface not subscribe to events via topic ticketed #609 - // param_sub_ = rclcpp::AsyncParametersClient::on_parameter_event( - // node_topics, - // std::bind(&NodeTypeDescriptionsImpl::on_parameter_event, this, std::placeholders::_1)); } }; @@ -153,25 +145,15 @@ NodeTypeDescriptions::NodeTypeDescriptions( rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters, rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services, rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics) -: - // node_base_(node_base), - // node_services_(node_services), - // node_logging_(node_logging), - // node_parameters_(node_parameters) - impl_(new NodeTypeDescriptionsImpl( +: impl_(new NodeTypeDescriptionsImpl( node_base, node_logging, node_parameters, node_services, - node_topics - )) -{ - // TODO init rcl node type description service and attach callback -} + node_topics)) +{} NodeTypeDescriptions::~NodeTypeDescriptions() -{ - RCUTILS_LOG_WARN("Destroy TDimpl"); -} +{} } // namespace rclcpp From f7c307bf2b5e8e6fc55c28dc516d9106cb63bc39 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 23 Jun 2023 18:53:04 -0700 Subject: [PATCH 06/13] example of spinning in thread for get_type_description service Signed-off-by: William Woodall --- .../node_type_descriptions.cpp | 73 +++++++++++-------- 1 file changed, 43 insertions(+), 30 deletions(-) diff --git a/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp b/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp index f41298389c..aa31895c38 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp @@ -12,15 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include +#include + #include "rclcpp/node_interfaces/node_type_descriptions.hpp" #include "rclcpp/parameter_client.hpp" #include "type_description_interfaces/srv/get_type_description.h" -#include -#include - using rclcpp::node_interfaces::NodeTypeDescriptions; // Even though we just want to use a C service, still need to define a C++-like struct @@ -61,6 +62,9 @@ class NodeTypeDescriptions::NodeTypeDescriptionsImpl Service::SharedPtr type_description_srv_; rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; + std::thread type_description_service_thread_; + rclcpp::CallbackGroup::SharedPtr type_description_service_callback_group_; + rclcpp::executors::SingleThreadedExecutor type_description_service_executor_; NodeTypeDescriptionsImpl( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, @@ -68,34 +72,28 @@ class NodeTypeDescriptions::NodeTypeDescriptionsImpl rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters, rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services, rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr /* node_topics */) - : logger_(node_logging->get_logger()) - , node_base_(node_base) + : logger_(node_logging->get_logger()), + node_base_(node_base), + type_description_service_callback_group_( + node_base_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false)) { const std::string enable_param_name = "enable_type_description_service"; const std::string service_name = "get_type_description"; - rclcpp::ParameterValue enable_param; - if (!node_parameters->has_parameter(enable_param_name)) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.name = enable_param_name; - descriptor.type = rclcpp::PARAMETER_BOOL; - descriptor.description = "Enable the ~/get_type_description service for this node."; - descriptor.read_only = true; - enable_param = node_parameters->declare_parameter( - enable_param_name, rclcpp::ParameterValue(true)); - } else { - enable_param = node_parameters->get_parameter(enable_param_name).get_parameter_value(); - } - if (enable_param.get_type() == rclcpp::PARAMETER_BOOL) { - if (enable_param.get()) { - enabled_ = true; - } - } else { - RCLCPP_ERROR( - logger_, "Invalid type '%s' for parameter '%s', should be 'bool'", - rclcpp::to_string(enable_param.get_type()).c_str(), enable_param_name.c_str()); - throw std::invalid_argument( - "Invalid type for parameter '" + enable_param_name + "', should be bool"); + bool enabled_; + try { + auto enable_param = node_parameters->declare_parameter( + enable_param_name, + rclcpp::ParameterValue(true), + rcl_interfaces::msg::ParameterDescriptor() + .set__name(enable_param_name) + .set__type(rclcpp::PARAMETER_BOOL) + .set__description("Enable the ~/get_type_description service for this node.") + .set__read_only(true)); + enabled_ = enable_param.get(); + } catch (const rclcpp::exceptions::InvalidParameterTypeException & exc) { + RCLCPP_ERROR(logger_, "%s", exc.what()); + throw; } if (enabled_) { @@ -109,7 +107,7 @@ class NodeTypeDescriptions::NodeTypeDescriptionsImpl "Failed to initialize ~/get_type_description service."); } - rcl_service_t * rcl_srv = NULL; + rcl_service_t * rcl_srv = nullptr; rcl_ret = rcl_node_get_type_description_service(rcl_node, &rcl_srv); if (rcl_ret != RCL_RET_OK) { throw std::runtime_error( @@ -131,11 +129,26 @@ class NodeTypeDescriptions::NodeTypeDescriptionsImpl }); type_description_srv_ = std::make_shared>( - node_base_->get_shared_rcl_node_handle(), rcl_srv, cb); + node_base_->get_shared_rcl_node_handle(), + rcl_srv, + cb); node_services->add_service( - std::dynamic_pointer_cast(type_description_srv_), nullptr); + std::dynamic_pointer_cast(type_description_srv_), + type_description_service_callback_group_); + + type_description_service_thread_ = std::thread([this]() { + type_description_service_executor_.add_callback_group( + type_description_service_callback_group_, + node_base_); + type_description_service_executor_.spin(); + }); } } + + virtual ~NodeTypeDescriptionsImpl() { + type_description_service_executor_.cancel(); + type_description_service_thread_.join(); + } }; From f34dc46af8363cc4dd8bce88e48c8c45c8811194 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Mon, 26 Jun 2023 17:21:08 -0700 Subject: [PATCH 07/13] Code cleanup, now that it is working Signed-off-by: Emerson Knapp --- .../include/rclcpp/any_service_callback.hpp | 9 +- .../node_type_descriptions.hpp | 5 +- rclcpp/src/rclcpp/node.cpp | 5 +- .../src/rclcpp/node_interfaces/node_base.cpp | 8 +- .../node_type_descriptions.cpp | 105 +++++++----------- rclcpp_lifecycle/src/lifecycle_node.cpp | 5 +- 6 files changed, 52 insertions(+), 85 deletions(-) diff --git a/rclcpp/include/rclcpp/any_service_callback.hpp b/rclcpp/include/rclcpp/any_service_callback.hpp index 713caff588..918d8e5a29 100644 --- a/rclcpp/include/rclcpp/any_service_callback.hpp +++ b/rclcpp/include/rclcpp/any_service_callback.hpp @@ -162,11 +162,6 @@ class AnyServiceCallback // to pass a callback at construnciton. throw std::runtime_error{"unexpected request without any callback set"}; } - if (std::holds_alternative(callback_)) { - const auto & cb = std::get(callback_); - cb(); - return nullptr; - } if (std::holds_alternative(callback_)) { const auto & cb = std::get(callback_); cb(request_header, std::move(request)); @@ -231,15 +226,13 @@ class AnyServiceCallback std::shared_ptr, std::shared_ptr )>; - using FullyDeferredCallback = std::function; std::variant< std::monostate, SharedPtrCallback, SharedPtrWithRequestHeaderCallback, SharedPtrDeferResponseCallback, - SharedPtrDeferResponseCallbackWithServiceHandle, - FullyDeferredCallback> callback_; + SharedPtrDeferResponseCallbackWithServiceHandle> callback_; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions.hpp b/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions.hpp index 14e5f9191e..f5f3ee2871 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions.hpp @@ -15,6 +15,8 @@ #ifndef RCLCPP__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_HPP_ #define RCLCPP__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_HPP_ +#include + #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/node_interfaces/node_logging_interface.hpp" @@ -41,8 +43,7 @@ class NodeTypeDescriptions : public NodeTypeDescriptionsInterface NodeBaseInterface::SharedPtr node_base, NodeLoggingInterface::SharedPtr node_logging, NodeParametersInterface::SharedPtr node_parameters, - NodeServicesInterface::SharedPtr node_services, - NodeTopicsInterface::SharedPtr node_topics); + NodeServicesInterface::SharedPtr node_services); RCLCPP_PUBLIC virtual diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 5f3c53206f..52012439e8 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -34,9 +34,9 @@ #include "rclcpp/node_interfaces/node_parameters.hpp" #include "rclcpp/node_interfaces/node_services.hpp" #include "rclcpp/node_interfaces/node_time_source.hpp" -#include "rclcpp/node_interfaces/node_type_descriptions.hpp" #include "rclcpp/node_interfaces/node_timers.hpp" #include "rclcpp/node_interfaces/node_topics.hpp" +#include "rclcpp/node_interfaces/node_type_descriptions.hpp" #include "rclcpp/node_interfaces/node_waitables.hpp" #include "rclcpp/qos_overriding_options.hpp" @@ -211,8 +211,7 @@ Node::Node( node_base_, node_logging_, node_parameters_, - node_services_, - node_topics_ + node_services_ )), node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())), node_options_(options), diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index 1a147ead6e..93af10f38d 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -115,13 +115,11 @@ NodeBase::NodeBase( throw_from_rcl_error(ret, "failed to initialize rcl node"); } - // If type description service will be initialized, it must capture all - // built-in services and topics that the node creates, even the ones by NodeParameters, - // which must be initialized first to let NodeTypeDescriptions be parameter-enabled + // To capture all types from builtin topics and services, the type cache needs to be initialized + // before any other components are initialized. ret = rcl_node_type_cache_init(rcl_node.get()); if (ret != RCL_RET_OK) { - throw std::runtime_error("It bad!"); - // TODO(ek) + throw_from_rcl_error(ret, "failed to initialize type cache"); } node_handle_.reset( diff --git a/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp b/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp index aa31895c38..63370ff707 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp @@ -17,28 +17,26 @@ #include #include "rclcpp/node_interfaces/node_type_descriptions.hpp" - #include "rclcpp/parameter_client.hpp" #include "type_description_interfaces/srv/get_type_description.h" -using rclcpp::node_interfaces::NodeTypeDescriptions; - -// Even though we just want to use a C service, still need to define a C++-like struct -// for the Service to use to allocate responses/requests. -// There is an allocation and deserialization via the Executor callback mechanism, at least -// of the request, even though we just want to let the C layer take it... +namespace +{ +// Helper wrapper for rclcpp::Service to access ::Request and ::Response types for allocation. struct GetTypeDescriptionC { using Request = type_description_interfaces__srv__GetTypeDescription_Request; using Response = type_description_interfaces__srv__GetTypeDescription_Response; using Event = type_description_interfaces__srv__GetTypeDescription_Event; }; +} // namespace +// Helper function for C typesupport. namespace rosidl_typesupport_cpp { template<> -rosidl_service_type_support_t const* +rosidl_service_type_support_t const * get_service_type_support_handle() { return ROSIDL_GET_SRV_TYPE_SUPPORT(type_description_interfaces, srv, GetTypeDescription); @@ -47,56 +45,45 @@ get_service_type_support_handle() namespace rclcpp { - -static constexpr const char * get_type_description_service_name = "get_type_description"; - +namespace node_interfaces +{ class NodeTypeDescriptions::NodeTypeDescriptionsImpl { public: - bool enabled_ = false; - Logger logger_; - node_interfaces::OnSetParametersCallbackHandle param_callback_; - std::shared_ptr> param_sub_; using ServiceT = GetTypeDescriptionC; + + Logger logger_; Service::SharedPtr type_description_srv_; rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; - std::thread type_description_service_thread_; - rclcpp::CallbackGroup::SharedPtr type_description_service_callback_group_; - rclcpp::executors::SingleThreadedExecutor type_description_service_executor_; - NodeTypeDescriptionsImpl( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters, - rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services, - rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr /* node_topics */) - : logger_(node_logging->get_logger()), - node_base_(node_base), - type_description_service_callback_group_( - node_base_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false)) + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services) + : logger_(node_logging->get_logger()), + node_base_(node_base) { - const std::string enable_param_name = "enable_type_description_service"; - const std::string service_name = "get_type_description"; + const std::string enable_param_name = "start_type_description_service"; - bool enabled_; + bool enabled = false; try { auto enable_param = node_parameters->declare_parameter( enable_param_name, rclcpp::ParameterValue(true), rcl_interfaces::msg::ParameterDescriptor() - .set__name(enable_param_name) - .set__type(rclcpp::PARAMETER_BOOL) - .set__description("Enable the ~/get_type_description service for this node.") - .set__read_only(true)); - enabled_ = enable_param.get(); + .set__name(enable_param_name) + .set__type(rclcpp::PARAMETER_BOOL) + .set__description("Start the ~/get_type_description service for this node.") + .set__read_only(true)); + enabled = enable_param.get(); } catch (const rclcpp::exceptions::InvalidParameterTypeException & exc) { RCLCPP_ERROR(logger_, "%s", exc.what()); throw; } - if (enabled_) { + if (enabled) { auto rcl_node = node_base->get_rcl_node_handle(); rcl_ret_t rcl_ret = rcl_node_type_description_service_init(rcl_node); if (rcl_ret != RCL_RET_OK) { @@ -104,29 +91,29 @@ class NodeTypeDescriptions::NodeTypeDescriptionsImpl logger_, "Failed to initialize ~/get_type_description_service: %s", rcl_get_error_string().str); throw std::runtime_error( - "Failed to initialize ~/get_type_description service."); + "Failed to initialize ~/get_type_description service."); } rcl_service_t * rcl_srv = nullptr; rcl_ret = rcl_node_get_type_description_service(rcl_node, &rcl_srv); if (rcl_ret != RCL_RET_OK) { throw std::runtime_error( - "Failed to get initialized ~/get_type_description service from rcl."); + "Failed to get initialized ~/get_type_description service from rcl."); } rclcpp::AnyServiceCallback cb; - cb.set([this]( + cb.set( + [this]( std::shared_ptr header, std::shared_ptr request, std::shared_ptr response - ) { - RCLCPP_WARN(logger_, "SERVICE CALLBACK"); - rcl_node_type_description_service_handle_request( - node_base_->get_rcl_node_handle(), - *header, - request.get(), - response.get()); - }); + ) { + rcl_node_type_description_service_handle_request( + node_base_->get_rcl_node_handle(), + header.get(), + request.get(), + response.get()); + }); type_description_srv_ = std::make_shared>( node_base_->get_shared_rcl_node_handle(), @@ -134,21 +121,12 @@ class NodeTypeDescriptions::NodeTypeDescriptionsImpl cb); node_services->add_service( std::dynamic_pointer_cast(type_description_srv_), - type_description_service_callback_group_); - - type_description_service_thread_ = std::thread([this]() { - type_description_service_executor_.add_callback_group( - type_description_service_callback_group_, - node_base_); - type_description_service_executor_.spin(); - }); + nullptr); } } - virtual ~NodeTypeDescriptionsImpl() { - type_description_service_executor_.cancel(); - type_description_service_thread_.join(); - } + virtual ~NodeTypeDescriptionsImpl() + {} }; @@ -156,17 +134,16 @@ NodeTypeDescriptions::NodeTypeDescriptions( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters, - rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services, - rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics) + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services) : impl_(new NodeTypeDescriptionsImpl( - node_base, - node_logging, - node_parameters, - node_services, - node_topics)) + node_base, + node_logging, + node_parameters, + node_services)) {} NodeTypeDescriptions::~NodeTypeDescriptions() {} +} // namespace node_interfaces } // namespace rclcpp diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 9eba8c5e10..94629911a3 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -41,9 +41,9 @@ #include "rclcpp/node_interfaces/node_parameters.hpp" #include "rclcpp/node_interfaces/node_services.hpp" #include "rclcpp/node_interfaces/node_time_source.hpp" -#include "rclcpp/node_interfaces/node_type_descriptions.hpp" #include "rclcpp/node_interfaces/node_timers.hpp" #include "rclcpp/node_interfaces/node_topics.hpp" +#include "rclcpp/node_interfaces/node_type_descriptions.hpp" #include "rclcpp/node_interfaces/node_waitables.hpp" #include "rclcpp/parameter_service.hpp" #include "rclcpp/qos.hpp" @@ -118,8 +118,7 @@ LifecycleNode::LifecycleNode( node_base_, node_logging_, node_parameters_, - node_services_, - node_topics_ + node_services_ )), node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())), node_options_(options), From 1b7b73dda184a7c66a26a3fa0643056572bb4ec0 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Mon, 26 Jun 2023 18:18:58 -0700 Subject: [PATCH 08/13] Add a basic test for the new interface Signed-off-by: Emerson Knapp --- .../src/rclcpp/node_interfaces/node_base.cpp | 7 --- rclcpp/test/rclcpp/CMakeLists.txt | 5 ++ .../test_node_type_descriptions.cpp | 63 +++++++++++++++++++ 3 files changed, 68 insertions(+), 7 deletions(-) create mode 100644 rclcpp/test/rclcpp/node_interfaces/test_node_type_descriptions.cpp diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index 93af10f38d..36e1afb932 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -115,13 +115,6 @@ NodeBase::NodeBase( throw_from_rcl_error(ret, "failed to initialize rcl node"); } - // To capture all types from builtin topics and services, the type cache needs to be initialized - // before any other components are initialized. - ret = rcl_node_type_cache_init(rcl_node.get()); - if (ret != RCL_RET_OK) { - throw_from_rcl_error(ret, "failed to initialize type cache"); - } - node_handle_.reset( rcl_node.release(), [logging_mutex](rcl_node_t * node) -> void { diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index dcb1d36d8e..8c31a95415 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -262,6 +262,11 @@ if(TARGET test_node_interfaces__node_topics) "test_msgs") target_link_libraries(test_node_interfaces__node_topics ${PROJECT_NAME} mimick) endif() +ament_add_gtest(test_node_interfaces__node_type_descriptions + node_interfaces/test_node_type_descriptions.cpp) +if(TARGET test_node_interfaces__node_type_descriptions) + target_link_libraries(test_node_interfaces__node_type_descriptions ${PROJECT_NAME} mimick) +endif() ament_add_gtest(test_node_interfaces__node_waitables node_interfaces/test_node_waitables.cpp) if(TARGET test_node_interfaces__node_waitables) diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_type_descriptions.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_type_descriptions.cpp new file mode 100644 index 0000000000..79ef6c3bcf --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_type_descriptions.cpp @@ -0,0 +1,63 @@ +// 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 "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_type_descriptions.hpp" + +class TestNodeTypeDescriptions : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestNodeTypeDescriptions, interface_created) +{ + rclcpp::Node node{"node", "ns"}; + ASSERT_NE(nullptr, node.get_node_type_descriptions_interface()); +} + +TEST_F(TestNodeTypeDescriptions, disabled_no_service) +{ + rclcpp::NodeOptions node_options; + node_options.append_parameter_override("start_type_description_service", false); + rclcpp::Node node{"node", "ns", node_options}; + + rcl_node_t * rcl_node = node.get_node_base_interface()->get_rcl_node_handle(); + rcl_service_t * srv = nullptr; + rcl_ret_t ret = rcl_node_get_type_description_service(rcl_node, &srv); + ASSERT_EQ(RCL_RET_NOT_INIT, ret); +} + +TEST_F(TestNodeTypeDescriptions, enabled_creates_service) +{ + rclcpp::NodeOptions node_options; + node_options.append_parameter_override("start_type_description_service", true); + rclcpp::Node node{"node", "ns", node_options}; + + rcl_node_t * rcl_node = node.get_node_base_interface()->get_rcl_node_handle(); + rcl_service_t * srv = nullptr; + rcl_ret_t ret = rcl_node_get_type_description_service(rcl_node, &srv); + ASSERT_EQ(RCL_RET_OK, ret); + ASSERT_NE(nullptr, srv); +} From c676802e2c13b576cbe0b427aea83fc842e08e86 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Mon, 3 Jul 2023 16:57:39 -0700 Subject: [PATCH 09/13] Take details out of impl Signed-off-by: Emerson Knapp --- .../node_type_descriptions.hpp | 18 +- .../node_type_descriptions.cpp | 174 ++++++++---------- 2 files changed, 92 insertions(+), 100 deletions(-) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions.hpp b/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions.hpp index f5f3ee2871..59c1db8cfc 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions.hpp @@ -26,12 +26,13 @@ #include "rclcpp/node_interfaces/node_type_descriptions_interface.hpp" #include "rclcpp/visibility_control.hpp" +#include "type_description_interfaces/srv/get_type_description.h" + namespace rclcpp { namespace node_interfaces { - /// Implementation of the NodeTypeDescriptions part of the Node API. class NodeTypeDescriptions : public NodeTypeDescriptionsInterface { @@ -49,10 +50,25 @@ class NodeTypeDescriptions : public NodeTypeDescriptionsInterface virtual ~NodeTypeDescriptions(); + // Helper wrapper for rclcpp::Service to access ::Request and ::Response types for allocation. + struct GetTypeDescriptionC + { + using Request = type_description_interfaces__srv__GetTypeDescription_Request; + using Response = type_description_interfaces__srv__GetTypeDescription_Response; + using Event = type_description_interfaces__srv__GetTypeDescription_Event; + }; + private: RCLCPP_DISABLE_COPY(NodeTypeDescriptions) + // Pimpl for future backport ABI stability assistance, not for general functionality class NodeTypeDescriptionsImpl; + + + rclcpp::Logger logger_; + rclcpp::Service::SharedPtr type_description_srv_; + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; + std::unique_ptr impl_; }; diff --git a/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp b/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp index 63370ff707..377b91235a 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp @@ -21,23 +21,13 @@ #include "type_description_interfaces/srv/get_type_description.h" -namespace -{ -// Helper wrapper for rclcpp::Service to access ::Request and ::Response types for allocation. -struct GetTypeDescriptionC -{ - using Request = type_description_interfaces__srv__GetTypeDescription_Request; - using Response = type_description_interfaces__srv__GetTypeDescription_Response; - using Event = type_description_interfaces__srv__GetTypeDescription_Event; -}; -} // namespace - // Helper function for C typesupport. namespace rosidl_typesupport_cpp { template<> rosidl_service_type_support_t const * -get_service_type_support_handle() +get_service_type_support_handle< + rclcpp::node_interfaces::NodeTypeDescriptions::GetTypeDescriptionC>() { return ROSIDL_GET_SRV_TYPE_SUPPORT(type_description_interfaces, srv, GetTypeDescription); } @@ -49,101 +39,87 @@ namespace node_interfaces { class NodeTypeDescriptions::NodeTypeDescriptionsImpl -{ -public: - using ServiceT = GetTypeDescriptionC; - - Logger logger_; - Service::SharedPtr type_description_srv_; - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; - - NodeTypeDescriptionsImpl( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters, - rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services) - : logger_(node_logging->get_logger()), - node_base_(node_base) - { - const std::string enable_param_name = "start_type_description_service"; - - bool enabled = false; - try { - auto enable_param = node_parameters->declare_parameter( - enable_param_name, - rclcpp::ParameterValue(true), - rcl_interfaces::msg::ParameterDescriptor() - .set__name(enable_param_name) - .set__type(rclcpp::PARAMETER_BOOL) - .set__description("Start the ~/get_type_description service for this node.") - .set__read_only(true)); - enabled = enable_param.get(); - } catch (const rclcpp::exceptions::InvalidParameterTypeException & exc) { - RCLCPP_ERROR(logger_, "%s", exc.what()); - throw; - } - - if (enabled) { - auto rcl_node = node_base->get_rcl_node_handle(); - rcl_ret_t rcl_ret = rcl_node_type_description_service_init(rcl_node); - if (rcl_ret != RCL_RET_OK) { - RCLCPP_ERROR( - logger_, "Failed to initialize ~/get_type_description_service: %s", - rcl_get_error_string().str); - throw std::runtime_error( - "Failed to initialize ~/get_type_description service."); - } - - rcl_service_t * rcl_srv = nullptr; - rcl_ret = rcl_node_get_type_description_service(rcl_node, &rcl_srv); - if (rcl_ret != RCL_RET_OK) { - throw std::runtime_error( - "Failed to get initialized ~/get_type_description service from rcl."); - } - - rclcpp::AnyServiceCallback cb; - cb.set( - [this]( - std::shared_ptr header, - std::shared_ptr request, - std::shared_ptr response - ) { - rcl_node_type_description_service_handle_request( - node_base_->get_rcl_node_handle(), - header.get(), - request.get(), - response.get()); - }); - - type_description_srv_ = std::make_shared>( - node_base_->get_shared_rcl_node_handle(), - rcl_srv, - cb); - node_services->add_service( - std::dynamic_pointer_cast(type_description_srv_), - nullptr); - } - } - - virtual ~NodeTypeDescriptionsImpl() - {} -}; - +{}; NodeTypeDescriptions::NodeTypeDescriptions( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters, rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services) -: impl_(new NodeTypeDescriptionsImpl( - node_base, - node_logging, - node_parameters, - node_services)) -{} +: logger_(node_logging->get_logger()), + node_base_(node_base) +{ + const std::string enable_param_name = "start_type_description_service"; + + bool enabled = false; + try { + auto enable_param = node_parameters->declare_parameter( + enable_param_name, + rclcpp::ParameterValue(true), + rcl_interfaces::msg::ParameterDescriptor() + .set__name(enable_param_name) + .set__type(rclcpp::PARAMETER_BOOL) + .set__description("Start the ~/get_type_description service for this node.") + .set__read_only(true)); + enabled = enable_param.get(); + } catch (const rclcpp::exceptions::InvalidParameterTypeException & exc) { + RCLCPP_ERROR(logger_, "%s", exc.what()); + throw; + } + + if (enabled) { + auto rcl_node = node_base->get_rcl_node_handle(); + rcl_ret_t rcl_ret = rcl_node_type_description_service_init(rcl_node); + if (rcl_ret != RCL_RET_OK) { + RCLCPP_ERROR( + logger_, "Failed to initialize ~/get_type_description_service: %s", + rcl_get_error_string().str); + throw std::runtime_error( + "Failed to initialize ~/get_type_description service."); + } + + rcl_service_t * rcl_srv = nullptr; + rcl_ret = rcl_node_get_type_description_service(rcl_node, &rcl_srv); + if (rcl_ret != RCL_RET_OK) { + throw std::runtime_error( + "Failed to get initialized ~/get_type_description service from rcl."); + } + + rclcpp::AnyServiceCallback cb; + cb.set( + [this]( + std::shared_ptr header, + std::shared_ptr request, + std::shared_ptr response + ) { + rcl_node_type_description_service_handle_request( + node_base_->get_rcl_node_handle(), + header.get(), + request.get(), + response.get()); + }); + + type_description_srv_ = std::make_shared>( + node_base_->get_shared_rcl_node_handle(), + rcl_srv, + cb); + node_services->add_service( + std::dynamic_pointer_cast(type_description_srv_), + nullptr); + } +} NodeTypeDescriptions::~NodeTypeDescriptions() -{} +{ + if ( + type_description_srv_ && + RCL_RET_OK != rcl_node_type_description_service_fini(node_base_->get_rcl_node_handle())) + { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Error in shutdown of get_type_description service: %s", rcl_get_error_string().str); + } +} } // namespace node_interfaces } // namespace rclcpp From 49ee3cce6a7cb0846809ad1a555d34d001d79d5f Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Mon, 3 Jul 2023 17:09:54 -0700 Subject: [PATCH 10/13] Move helper type out of class Signed-off-by: Emerson Knapp --- .../node_type_descriptions.hpp | 36 +++++++++++++------ .../node_type_descriptions.cpp | 12 +++---- 2 files changed, 31 insertions(+), 17 deletions(-) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions.hpp b/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions.hpp index 59c1db8cfc..4d53a8719d 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions.hpp @@ -28,6 +28,30 @@ #include "type_description_interfaces/srv/get_type_description.h" + +namespace type_description_interfaces +{ +namespace srv +{ +// Helper wrapper for rclcpp::Service to access ::Request and ::Response types for allocation. +struct GetTypeDescription__C +{ + using Request = type_description_interfaces__srv__GetTypeDescription_Request; + using Response = type_description_interfaces__srv__GetTypeDescription_Response; + using Event = type_description_interfaces__srv__GetTypeDescription_Event; +}; +} // namespace srv +} // namespace type_description_interfaces + +namespace rosidl_typesupport_cpp +{ +// Helper function for C typesupport. +template<> +RCLCPP_PUBLIC +rosidl_service_type_support_t const * +get_service_type_support_handle(); +} // namespace rosidl_typesupport_cpp + namespace rclcpp { namespace node_interfaces @@ -50,23 +74,15 @@ class NodeTypeDescriptions : public NodeTypeDescriptionsInterface virtual ~NodeTypeDescriptions(); - // Helper wrapper for rclcpp::Service to access ::Request and ::Response types for allocation. - struct GetTypeDescriptionC - { - using Request = type_description_interfaces__srv__GetTypeDescription_Request; - using Response = type_description_interfaces__srv__GetTypeDescription_Response; - using Event = type_description_interfaces__srv__GetTypeDescription_Event; - }; - private: RCLCPP_DISABLE_COPY(NodeTypeDescriptions) // Pimpl for future backport ABI stability assistance, not for general functionality class NodeTypeDescriptionsImpl; - + using ServiceT = type_description_interfaces::srv::GetTypeDescription__C; rclcpp::Logger logger_; - rclcpp::Service::SharedPtr type_description_srv_; + rclcpp::Service::SharedPtr type_description_srv_; rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; std::unique_ptr impl_; diff --git a/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp b/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp index 377b91235a..9c3efd8b19 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp @@ -21,13 +21,11 @@ #include "type_description_interfaces/srv/get_type_description.h" -// Helper function for C typesupport. namespace rosidl_typesupport_cpp { template<> rosidl_service_type_support_t const * -get_service_type_support_handle< - rclcpp::node_interfaces::NodeTypeDescriptions::GetTypeDescriptionC>() +get_service_type_support_handle() { return ROSIDL_GET_SRV_TYPE_SUPPORT(type_description_interfaces, srv, GetTypeDescription); } @@ -85,12 +83,12 @@ NodeTypeDescriptions::NodeTypeDescriptions( "Failed to get initialized ~/get_type_description service from rcl."); } - rclcpp::AnyServiceCallback cb; + rclcpp::AnyServiceCallback cb; cb.set( [this]( std::shared_ptr header, - std::shared_ptr request, - std::shared_ptr response + std::shared_ptr request, + std::shared_ptr response ) { rcl_node_type_description_service_handle_request( node_base_->get_rcl_node_handle(), @@ -99,7 +97,7 @@ NodeTypeDescriptions::NodeTypeDescriptions( response.get()); }); - type_description_srv_ = std::make_shared>( + type_description_srv_ = std::make_shared>( node_base_->get_shared_rcl_node_handle(), rcl_srv, cb); From 9e33af01eff3bcd323fcab1842e2be88c9cbd442 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Mon, 3 Jul 2023 21:48:41 -0700 Subject: [PATCH 11/13] Fix tests with new parameter Signed-off-by: Emerson Knapp --- .../node_interfaces/test_node_parameters.cpp | 4 +-- rclcpp/test/rclcpp/test_parameter_client.cpp | 13 ++++++--- rclcpp_lifecycle/test/test_lifecycle_node.cpp | 29 ++++++++++++------- 3 files changed, 29 insertions(+), 17 deletions(-) diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp index 58bf010767..8a03ad7097 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp @@ -77,9 +77,9 @@ TEST_F(TestNodeParameters, list_parameters) std::vector prefixes; const auto list_result = node_parameters->list_parameters(prefixes, 1u); - // Currently the only default parameter is 'use_sim_time', but that may change. + // currently 'use_sim_time' and 'start_type_description_service' size_t number_of_parameters = list_result.names.size(); - EXPECT_GE(1u, number_of_parameters); + EXPECT_GE(2u, number_of_parameters); const std::string parameter_name = "new_parameter"; const rclcpp::ParameterValue value(true); diff --git a/rclcpp/test/rclcpp/test_parameter_client.cpp b/rclcpp/test/rclcpp/test_parameter_client.cpp index 2ce414d327..f839d6f4a6 100644 --- a/rclcpp/test/rclcpp/test_parameter_client.cpp +++ b/rclcpp/test/rclcpp/test_parameter_client.cpp @@ -59,6 +59,7 @@ class TestParameterClient : public ::testing::Test node_with_option.reset(); } + const uint64_t builtin_param_count = 2; rclcpp::Node::SharedPtr node; rclcpp::Node::SharedPtr node_with_option; }; @@ -925,6 +926,7 @@ TEST_F(TestParameterClient, sync_parameter_delete_parameters) { Coverage for async load_parameters */ TEST_F(TestParameterClient, async_parameter_load_parameters) { + const uint64_t expected_param_count = 4 + builtin_param_count; auto load_node = std::make_shared( "load_node", "namespace", @@ -944,12 +946,13 @@ TEST_F(TestParameterClient, async_parameter_load_parameters) { auto list_parameters = asynchronous_client->list_parameters({}, 3); rclcpp::spin_until_future_complete( load_node, list_parameters, std::chrono::milliseconds(100)); - ASSERT_EQ(list_parameters.get().names.size(), static_cast(5)); + ASSERT_EQ(list_parameters.get().names.size(), expected_param_count); } /* Coverage for sync load_parameters */ TEST_F(TestParameterClient, sync_parameter_load_parameters) { + const uint64_t expected_param_count = 4 + builtin_param_count; auto load_node = std::make_shared( "load_node", "namespace", @@ -964,13 +967,14 @@ TEST_F(TestParameterClient, sync_parameter_load_parameters) { ASSERT_EQ(load_future[0].successful, true); // list parameters auto list_parameters = synchronous_client->list_parameters({}, 3); - ASSERT_EQ(list_parameters.names.size(), static_cast(5)); + ASSERT_EQ(list_parameters.names.size(), static_cast(expected_param_count)); } /* Coverage for async load_parameters with complicated regex expression */ TEST_F(TestParameterClient, async_parameter_load_parameters_complicated_regex) { + const uint64_t expected_param_count = 5 + builtin_param_count; auto load_node = std::make_shared( "load_node", "namespace", @@ -990,7 +994,7 @@ TEST_F(TestParameterClient, async_parameter_load_parameters_complicated_regex) { auto list_parameters = asynchronous_client->list_parameters({}, 3); rclcpp::spin_until_future_complete( load_node, list_parameters, std::chrono::milliseconds(100)); - ASSERT_EQ(list_parameters.get().names.size(), static_cast(6)); + ASSERT_EQ(list_parameters.get().names.size(), expected_param_count); // to check the parameter "a_value" std::string param_name = "a_value"; auto param = load_node->get_parameter(param_name); @@ -1020,6 +1024,7 @@ TEST_F(TestParameterClient, async_parameter_load_no_valid_parameter) { Coverage for async load_parameters from maps with complicated regex expression */ TEST_F(TestParameterClient, async_parameter_load_parameters_from_map) { + const uint64_t expected_param_count = 5 + builtin_param_count; auto load_node = std::make_shared( "load_node", "namespace", @@ -1068,7 +1073,7 @@ TEST_F(TestParameterClient, async_parameter_load_parameters_from_map) { auto list_parameters = asynchronous_client->list_parameters({}, 3); rclcpp::spin_until_future_complete( load_node, list_parameters, std::chrono::milliseconds(100)); - ASSERT_EQ(list_parameters.get().names.size(), static_cast(6)); + ASSERT_EQ(list_parameters.get().names.size(), expected_param_count); // to check the parameter "a_value" std::string param_name = "a_value"; auto param = load_node->get_parameter(param_name); diff --git a/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp index 31850c589b..9e43584be9 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -427,11 +427,14 @@ TEST_F(TestDefaultStateMachine, lifecycle_subscriber) { // Parameters are tested more thoroughly in rclcpp's test_node.cpp // These are provided for coverage of lifecycle node's API TEST_F(TestDefaultStateMachine, declare_parameters) { + const uint64_t builtin_param_count = 2; + const uint64_t expected_param_count = 6 + builtin_param_count; auto test_node = std::make_shared("testnode"); auto list_result = test_node->list_parameters({}, 0u); - EXPECT_EQ(list_result.names.size(), 1u); - EXPECT_STREQ(list_result.names[0].c_str(), "use_sim_time"); + EXPECT_EQ(list_result.names.size(), builtin_param_count); + EXPECT_STREQ(list_result.names[0].c_str(), "start_type_description_service"); + EXPECT_STREQ(list_result.names[1].c_str(), "use_sim_time"); const std::string bool_name = "test_boolean"; const std::string int_name = "test_int"; @@ -469,10 +472,11 @@ TEST_F(TestDefaultStateMachine, declare_parameters) { test_node->declare_parameters("test_double", double_parameters); list_result = test_node->list_parameters({}, 0u); - EXPECT_EQ(list_result.names.size(), 7u); + EXPECT_EQ(list_result.names.size(), expected_param_count); // The order of these names is not controlled by lifecycle_node, doing set equality std::set expected_names = { + "start_type_description_service", "test_boolean", "test_double.double_one", "test_double.double_two", @@ -487,11 +491,13 @@ TEST_F(TestDefaultStateMachine, declare_parameters) { } TEST_F(TestDefaultStateMachine, check_parameters) { + const uint64_t builtin_param_count = 2; auto test_node = std::make_shared("testnode"); auto list_result = test_node->list_parameters({}, 0u); - EXPECT_EQ(list_result.names.size(), 1u); - EXPECT_STREQ(list_result.names[0].c_str(), "use_sim_time"); + EXPECT_EQ(list_result.names.size(), builtin_param_count); + EXPECT_STREQ(list_result.names[0].c_str(), "start_type_description_service"); + EXPECT_STREQ(list_result.names[1].c_str(), "use_sim_time"); const std::string bool_name = "test_boolean"; const std::string int_name = "test_int"; @@ -549,8 +555,7 @@ TEST_F(TestDefaultStateMachine, check_parameters) { std::map parameter_map; EXPECT_TRUE(test_node->get_parameters({}, parameter_map)); - // int param, bool param, and use_sim_time - EXPECT_EQ(parameter_map.size(), 3u); + EXPECT_EQ(parameter_map.size(), parameter_names.size() + builtin_param_count); // Check parameter types auto parameter_types = test_node->get_parameter_types(parameter_names); @@ -585,10 +590,12 @@ TEST_F(TestDefaultStateMachine, check_parameters) { // List parameters list_result = test_node->list_parameters({}, 0u); - EXPECT_EQ(list_result.names.size(), 3u); - EXPECT_STREQ(list_result.names[0].c_str(), parameter_names[0].c_str()); - EXPECT_STREQ(list_result.names[1].c_str(), parameter_names[1].c_str()); - EXPECT_STREQ(list_result.names[2].c_str(), "use_sim_time"); + EXPECT_EQ(list_result.names.size(), parameter_names.size() + builtin_param_count); + size_t index = 0; + EXPECT_STREQ(list_result.names[index++].c_str(), "start_type_description_service"); + EXPECT_STREQ(list_result.names[index++].c_str(), parameter_names[0].c_str()); + EXPECT_STREQ(list_result.names[index++].c_str(), parameter_names[1].c_str()); + EXPECT_STREQ(list_result.names[index++].c_str(), "use_sim_time"); // Undeclare parameter test_node->undeclare_parameter(bool_name); From 72b4e2e989d27184afe98aaf91ab4390dd2e76ca Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Wed, 5 Jul 2023 12:22:46 -0700 Subject: [PATCH 12/13] Add comments about builtin parameters Signed-off-by: Emerson Knapp --- rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp | 2 +- rclcpp/test/rclcpp/test_parameter_client.cpp | 1 + rclcpp_lifecycle/test/test_lifecycle_node.cpp | 1 + 3 files changed, 3 insertions(+), 1 deletion(-) diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp index 8a03ad7097..9113c96ca5 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp @@ -77,7 +77,7 @@ TEST_F(TestNodeParameters, list_parameters) std::vector prefixes; const auto list_result = node_parameters->list_parameters(prefixes, 1u); - // currently 'use_sim_time' and 'start_type_description_service' + // Currently the default parameters are 'use_sim_time' and 'start_type_description_service' size_t number_of_parameters = list_result.names.size(); EXPECT_GE(2u, number_of_parameters); diff --git a/rclcpp/test/rclcpp/test_parameter_client.cpp b/rclcpp/test/rclcpp/test_parameter_client.cpp index f839d6f4a6..afc48a652b 100644 --- a/rclcpp/test/rclcpp/test_parameter_client.cpp +++ b/rclcpp/test/rclcpp/test_parameter_client.cpp @@ -59,6 +59,7 @@ class TestParameterClient : public ::testing::Test node_with_option.reset(); } + // "start_type_description_service" and "use_sim_time" const uint64_t builtin_param_count = 2; rclcpp::Node::SharedPtr node; rclcpp::Node::SharedPtr node_with_option; diff --git a/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp index 9e43584be9..db24bd0ee7 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -427,6 +427,7 @@ TEST_F(TestDefaultStateMachine, lifecycle_subscriber) { // Parameters are tested more thoroughly in rclcpp's test_node.cpp // These are provided for coverage of lifecycle node's API TEST_F(TestDefaultStateMachine, declare_parameters) { + // "start_type_description_service" and "use_sim_time" const uint64_t builtin_param_count = 2; const uint64_t expected_param_count = 6 + builtin_param_count; auto test_node = std::make_shared("testnode"); From d83685439ea87410aab6bf09e1077b5af16e4cd0 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Wed, 5 Jul 2023 12:43:56 -0700 Subject: [PATCH 13/13] Re-hide implementation and add a comment about the pimpl Signed-off-by: Emerson Knapp --- .../node_type_descriptions.hpp | 35 +--- .../node_type_descriptions.cpp | 180 +++++++++++------- 2 files changed, 109 insertions(+), 106 deletions(-) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions.hpp b/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions.hpp index 4d53a8719d..8aa563bba2 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions.hpp @@ -26,32 +26,6 @@ #include "rclcpp/node_interfaces/node_type_descriptions_interface.hpp" #include "rclcpp/visibility_control.hpp" -#include "type_description_interfaces/srv/get_type_description.h" - - -namespace type_description_interfaces -{ -namespace srv -{ -// Helper wrapper for rclcpp::Service to access ::Request and ::Response types for allocation. -struct GetTypeDescription__C -{ - using Request = type_description_interfaces__srv__GetTypeDescription_Request; - using Response = type_description_interfaces__srv__GetTypeDescription_Response; - using Event = type_description_interfaces__srv__GetTypeDescription_Event; -}; -} // namespace srv -} // namespace type_description_interfaces - -namespace rosidl_typesupport_cpp -{ -// Helper function for C typesupport. -template<> -RCLCPP_PUBLIC -rosidl_service_type_support_t const * -get_service_type_support_handle(); -} // namespace rosidl_typesupport_cpp - namespace rclcpp { namespace node_interfaces @@ -77,14 +51,9 @@ class NodeTypeDescriptions : public NodeTypeDescriptionsInterface private: RCLCPP_DISABLE_COPY(NodeTypeDescriptions) - // Pimpl for future backport ABI stability assistance, not for general functionality + // Pimpl hides helper types and functions used for wrapping a C service, which would be + // awkward to expose in this header. class NodeTypeDescriptionsImpl; - using ServiceT = type_description_interfaces::srv::GetTypeDescription__C; - - rclcpp::Logger logger_; - rclcpp::Service::SharedPtr type_description_srv_; - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; - std::unique_ptr impl_; }; diff --git a/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp b/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp index 9c3efd8b19..f4b5e20d30 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp @@ -21,11 +21,23 @@ #include "type_description_interfaces/srv/get_type_description.h" +namespace +{ +// Helper wrapper for rclcpp::Service to access ::Request and ::Response types for allocation. +struct GetTypeDescription__C +{ + using Request = type_description_interfaces__srv__GetTypeDescription_Request; + using Response = type_description_interfaces__srv__GetTypeDescription_Response; + using Event = type_description_interfaces__srv__GetTypeDescription_Event; +}; +} // namespace + +// Helper function for C typesupport. namespace rosidl_typesupport_cpp { template<> rosidl_service_type_support_t const * -get_service_type_support_handle() +get_service_type_support_handle() { return ROSIDL_GET_SRV_TYPE_SUPPORT(type_description_interfaces, srv, GetTypeDescription); } @@ -37,87 +49,109 @@ namespace node_interfaces { class NodeTypeDescriptions::NodeTypeDescriptionsImpl -{}; - -NodeTypeDescriptions::NodeTypeDescriptions( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters, - rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services) -: logger_(node_logging->get_logger()), - node_base_(node_base) { - const std::string enable_param_name = "start_type_description_service"; - - bool enabled = false; - try { - auto enable_param = node_parameters->declare_parameter( - enable_param_name, - rclcpp::ParameterValue(true), - rcl_interfaces::msg::ParameterDescriptor() - .set__name(enable_param_name) - .set__type(rclcpp::PARAMETER_BOOL) - .set__description("Start the ~/get_type_description service for this node.") - .set__read_only(true)); - enabled = enable_param.get(); - } catch (const rclcpp::exceptions::InvalidParameterTypeException & exc) { - RCLCPP_ERROR(logger_, "%s", exc.what()); - throw; - } +public: + using ServiceT = GetTypeDescription__C; - if (enabled) { - auto rcl_node = node_base->get_rcl_node_handle(); - rcl_ret_t rcl_ret = rcl_node_type_description_service_init(rcl_node); - if (rcl_ret != RCL_RET_OK) { - RCLCPP_ERROR( - logger_, "Failed to initialize ~/get_type_description_service: %s", - rcl_get_error_string().str); - throw std::runtime_error( - "Failed to initialize ~/get_type_description service."); - } + rclcpp::Logger logger_; + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; + rclcpp::Service::SharedPtr type_description_srv_; - rcl_service_t * rcl_srv = nullptr; - rcl_ret = rcl_node_get_type_description_service(rcl_node, &rcl_srv); - if (rcl_ret != RCL_RET_OK) { - throw std::runtime_error( - "Failed to get initialized ~/get_type_description service from rcl."); + NodeTypeDescriptionsImpl( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters, + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services) + : logger_(node_logging->get_logger()), + node_base_(node_base) + { + const std::string enable_param_name = "start_type_description_service"; + + bool enabled = false; + try { + auto enable_param = node_parameters->declare_parameter( + enable_param_name, + rclcpp::ParameterValue(true), + rcl_interfaces::msg::ParameterDescriptor() + .set__name(enable_param_name) + .set__type(rclcpp::PARAMETER_BOOL) + .set__description("Start the ~/get_type_description service for this node.") + .set__read_only(true)); + enabled = enable_param.get(); + } catch (const rclcpp::exceptions::InvalidParameterTypeException & exc) { + RCLCPP_ERROR(logger_, "%s", exc.what()); + throw; } - rclcpp::AnyServiceCallback cb; - cb.set( - [this]( - std::shared_ptr header, - std::shared_ptr request, - std::shared_ptr response - ) { - rcl_node_type_description_service_handle_request( - node_base_->get_rcl_node_handle(), - header.get(), - request.get(), - response.get()); - }); - - type_description_srv_ = std::make_shared>( - node_base_->get_shared_rcl_node_handle(), - rcl_srv, - cb); - node_services->add_service( - std::dynamic_pointer_cast(type_description_srv_), - nullptr); + if (enabled) { + auto rcl_node = node_base->get_rcl_node_handle(); + rcl_ret_t rcl_ret = rcl_node_type_description_service_init(rcl_node); + if (rcl_ret != RCL_RET_OK) { + RCLCPP_ERROR( + logger_, "Failed to initialize ~/get_type_description_service: %s", + rcl_get_error_string().str); + throw std::runtime_error( + "Failed to initialize ~/get_type_description service."); + } + + rcl_service_t * rcl_srv = nullptr; + rcl_ret = rcl_node_get_type_description_service(rcl_node, &rcl_srv); + if (rcl_ret != RCL_RET_OK) { + throw std::runtime_error( + "Failed to get initialized ~/get_type_description service from rcl."); + } + + rclcpp::AnyServiceCallback cb; + cb.set( + [this]( + std::shared_ptr header, + std::shared_ptr request, + std::shared_ptr response + ) { + rcl_node_type_description_service_handle_request( + node_base_->get_rcl_node_handle(), + header.get(), + request.get(), + response.get()); + }); + + type_description_srv_ = std::make_shared>( + node_base_->get_shared_rcl_node_handle(), + rcl_srv, + cb); + node_services->add_service( + std::dynamic_pointer_cast(type_description_srv_), + nullptr); + } } -} -NodeTypeDescriptions::~NodeTypeDescriptions() -{ - if ( - type_description_srv_ && - RCL_RET_OK != rcl_node_type_description_service_fini(node_base_->get_rcl_node_handle())) + ~NodeTypeDescriptionsImpl() { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Error in shutdown of get_type_description service: %s", rcl_get_error_string().str); + if ( + type_description_srv_ && + RCL_RET_OK != rcl_node_type_description_service_fini(node_base_->get_rcl_node_handle())) + { + RCLCPP_ERROR( + logger_, + "Error in shutdown of get_type_description service: %s", rcl_get_error_string().str); + } } -} +}; + +NodeTypeDescriptions::NodeTypeDescriptions( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters, + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services) +: impl_(new NodeTypeDescriptionsImpl( + node_base, + node_logging, + node_parameters, + node_services)) +{} + +NodeTypeDescriptions::~NodeTypeDescriptions() +{} } // namespace node_interfaces } // namespace rclcpp