|
| 1 | +// Copyright 2023 Open Source Robotics Foundation, Inc. |
| 2 | +// |
| 3 | +// Licensed under the Apache License, Version 2.0 (the "License"); |
| 4 | +// you may not use this file except in compliance with the License. |
| 5 | +// You may obtain a copy of the License at |
| 6 | +// |
| 7 | +// http://www.apache.org/licenses/LICENSE-2.0 |
| 8 | +// |
| 9 | +// Unless required by applicable law or agreed to in writing, software |
| 10 | +// distributed under the License is distributed on an "AS IS" BASIS, |
| 11 | +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 12 | +// See the License for the specific language governing permissions and |
| 13 | +// limitations under the License. |
| 14 | + |
| 15 | +#include <memory> |
| 16 | +#include <string> |
| 17 | +#include <thread> |
| 18 | + |
| 19 | +#include "rclcpp/node_interfaces/node_type_descriptions.hpp" |
| 20 | +#include "rclcpp/parameter_client.hpp" |
| 21 | + |
| 22 | +#include "type_description_interfaces/srv/get_type_description.h" |
| 23 | + |
| 24 | +namespace |
| 25 | +{ |
| 26 | +// Helper wrapper for rclcpp::Service to access ::Request and ::Response types for allocation. |
| 27 | +struct GetTypeDescription__C |
| 28 | +{ |
| 29 | + using Request = type_description_interfaces__srv__GetTypeDescription_Request; |
| 30 | + using Response = type_description_interfaces__srv__GetTypeDescription_Response; |
| 31 | + using Event = type_description_interfaces__srv__GetTypeDescription_Event; |
| 32 | +}; |
| 33 | +} // namespace |
| 34 | + |
| 35 | +// Helper function for C typesupport. |
| 36 | +namespace rosidl_typesupport_cpp |
| 37 | +{ |
| 38 | +template<> |
| 39 | +rosidl_service_type_support_t const * |
| 40 | +get_service_type_support_handle<GetTypeDescription__C>() |
| 41 | +{ |
| 42 | + return ROSIDL_GET_SRV_TYPE_SUPPORT(type_description_interfaces, srv, GetTypeDescription); |
| 43 | +} |
| 44 | +} // namespace rosidl_typesupport_cpp |
| 45 | + |
| 46 | +namespace rclcpp |
| 47 | +{ |
| 48 | +namespace node_interfaces |
| 49 | +{ |
| 50 | + |
| 51 | +class NodeTypeDescriptions::NodeTypeDescriptionsImpl |
| 52 | +{ |
| 53 | +public: |
| 54 | + using ServiceT = GetTypeDescription__C; |
| 55 | + |
| 56 | + rclcpp::Logger logger_; |
| 57 | + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; |
| 58 | + rclcpp::Service<ServiceT>::SharedPtr type_description_srv_; |
| 59 | + |
| 60 | + NodeTypeDescriptionsImpl( |
| 61 | + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, |
| 62 | + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, |
| 63 | + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters, |
| 64 | + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services) |
| 65 | + : logger_(node_logging->get_logger()), |
| 66 | + node_base_(node_base) |
| 67 | + { |
| 68 | + const std::string enable_param_name = "start_type_description_service"; |
| 69 | + |
| 70 | + bool enabled = false; |
| 71 | + try { |
| 72 | + auto enable_param = node_parameters->declare_parameter( |
| 73 | + enable_param_name, |
| 74 | + rclcpp::ParameterValue(true), |
| 75 | + rcl_interfaces::msg::ParameterDescriptor() |
| 76 | + .set__name(enable_param_name) |
| 77 | + .set__type(rclcpp::PARAMETER_BOOL) |
| 78 | + .set__description("Start the ~/get_type_description service for this node.") |
| 79 | + .set__read_only(true)); |
| 80 | + enabled = enable_param.get<bool>(); |
| 81 | + } catch (const rclcpp::exceptions::InvalidParameterTypeException & exc) { |
| 82 | + RCLCPP_ERROR(logger_, "%s", exc.what()); |
| 83 | + throw; |
| 84 | + } |
| 85 | + |
| 86 | + if (enabled) { |
| 87 | + auto rcl_node = node_base->get_rcl_node_handle(); |
| 88 | + rcl_ret_t rcl_ret = rcl_node_type_description_service_init(rcl_node); |
| 89 | + if (rcl_ret != RCL_RET_OK) { |
| 90 | + RCLCPP_ERROR( |
| 91 | + logger_, "Failed to initialize ~/get_type_description_service: %s", |
| 92 | + rcl_get_error_string().str); |
| 93 | + throw std::runtime_error( |
| 94 | + "Failed to initialize ~/get_type_description service."); |
| 95 | + } |
| 96 | + |
| 97 | + rcl_service_t * rcl_srv = nullptr; |
| 98 | + rcl_ret = rcl_node_get_type_description_service(rcl_node, &rcl_srv); |
| 99 | + if (rcl_ret != RCL_RET_OK) { |
| 100 | + throw std::runtime_error( |
| 101 | + "Failed to get initialized ~/get_type_description service from rcl."); |
| 102 | + } |
| 103 | + |
| 104 | + rclcpp::AnyServiceCallback<ServiceT> cb; |
| 105 | + cb.set( |
| 106 | + [this]( |
| 107 | + std::shared_ptr<rmw_request_id_t> header, |
| 108 | + std::shared_ptr<ServiceT::Request> request, |
| 109 | + std::shared_ptr<ServiceT::Response> response |
| 110 | + ) { |
| 111 | + rcl_node_type_description_service_handle_request( |
| 112 | + node_base_->get_rcl_node_handle(), |
| 113 | + header.get(), |
| 114 | + request.get(), |
| 115 | + response.get()); |
| 116 | + }); |
| 117 | + |
| 118 | + type_description_srv_ = std::make_shared<Service<ServiceT>>( |
| 119 | + node_base_->get_shared_rcl_node_handle(), |
| 120 | + rcl_srv, |
| 121 | + cb); |
| 122 | + node_services->add_service( |
| 123 | + std::dynamic_pointer_cast<ServiceBase>(type_description_srv_), |
| 124 | + nullptr); |
| 125 | + } |
| 126 | + } |
| 127 | + |
| 128 | + ~NodeTypeDescriptionsImpl() |
| 129 | + { |
| 130 | + if ( |
| 131 | + type_description_srv_ && |
| 132 | + RCL_RET_OK != rcl_node_type_description_service_fini(node_base_->get_rcl_node_handle())) |
| 133 | + { |
| 134 | + RCLCPP_ERROR( |
| 135 | + logger_, |
| 136 | + "Error in shutdown of get_type_description service: %s", rcl_get_error_string().str); |
| 137 | + } |
| 138 | + } |
| 139 | +}; |
| 140 | + |
| 141 | +NodeTypeDescriptions::NodeTypeDescriptions( |
| 142 | + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, |
| 143 | + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, |
| 144 | + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters, |
| 145 | + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services) |
| 146 | +: impl_(new NodeTypeDescriptionsImpl( |
| 147 | + node_base, |
| 148 | + node_logging, |
| 149 | + node_parameters, |
| 150 | + node_services)) |
| 151 | +{} |
| 152 | + |
| 153 | +NodeTypeDescriptions::~NodeTypeDescriptions() |
| 154 | +{} |
| 155 | + |
| 156 | +} // namespace node_interfaces |
| 157 | +} // namespace rclcpp |
0 commit comments