|
| 1 | +// Copyright 2026 Innate |
| 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 | + |
| 6 | +#ifndef RWS__GENERIC_SERVICE_HPP_ |
| 7 | +#define RWS__GENERIC_SERVICE_HPP_ |
| 8 | + |
| 9 | +#include <memory> |
| 10 | +#include <string> |
| 11 | + |
| 12 | +#include "rcl/service.h" |
| 13 | +#include "rclcpp/exceptions.hpp" |
| 14 | +#include "rclcpp/expand_topic_or_service_name.hpp" |
| 15 | +#include "rclcpp/serialized_message.hpp" |
| 16 | +#include "rclcpp/service.hpp" |
| 17 | +#include "rclcpp/typesupport_helpers.hpp" |
| 18 | +#include "rmw/rmw.h" |
| 19 | +#include "rosidl_typesupport_introspection_cpp/service_introspection.hpp" |
| 20 | +#include "rws/typesupport_helpers.hpp" |
| 21 | + |
| 22 | +namespace rws |
| 23 | +{ |
| 24 | + |
| 25 | +using rosidl_typesupport_introspection_cpp::ServiceMembers; |
| 26 | + |
| 27 | +class GenericService : public rclcpp::ServiceBase |
| 28 | +{ |
| 29 | +public: |
| 30 | + using SharedRequest = std::shared_ptr<rclcpp::SerializedMessage>; |
| 31 | + using SharedResponse = std::shared_ptr<rclcpp::SerializedMessage>; |
| 32 | + using CallbackType = std::function<void(std::shared_ptr<rmw_request_id_t>, SharedRequest)>; |
| 33 | + |
| 34 | + RCLCPP_SMART_PTR_DEFINITIONS(GenericService) |
| 35 | + |
| 36 | + GenericService( |
| 37 | + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, |
| 38 | + const std::string & service_name, |
| 39 | + const std::string & service_type, |
| 40 | + rcl_service_options_t & service_options, |
| 41 | + CallbackType callback) |
| 42 | + : rclcpp::ServiceBase(node_base->get_shared_rcl_node_handle()), |
| 43 | + service_type_(service_type), |
| 44 | + callback_(callback) |
| 45 | + { |
| 46 | + srv_ts_lib_ = rclcpp::get_typesupport_library(service_type, rws::ts_identifier_srv); |
| 47 | + srv_ts_hdl_ = |
| 48 | + rws::get_service_typesupport_handle(service_type, rws::ts_identifier_srv, *srv_ts_lib_); |
| 49 | + |
| 50 | + srv_intro_lib_ = rclcpp::get_typesupport_library(service_type, rws::ts_identifier); |
| 51 | + srv_intro_hdl_ = |
| 52 | + rws::get_service_typesupport_handle(service_type, rws::ts_identifier, *srv_intro_lib_); |
| 53 | + auto srv_members = static_cast<const ServiceMembers *>(srv_intro_hdl_->data); |
| 54 | + |
| 55 | + auto request_type = get_type_from_message_members(srv_members->request_members_); |
| 56 | + req_ts_srv_lib_ = rclcpp::get_typesupport_library(request_type, rws::ts_identifier_srv); |
| 57 | + req_ts_srv_hdl_ = |
| 58 | + rclcpp::get_typesupport_handle(request_type, rws::ts_identifier_srv, *req_ts_srv_lib_); |
| 59 | + |
| 60 | + auto response_type = get_type_from_message_members(srv_members->response_members_); |
| 61 | + res_ts_srv_lib_ = rclcpp::get_typesupport_library(response_type, rws::ts_identifier_srv); |
| 62 | + res_ts_srv_hdl_ = |
| 63 | + rclcpp::get_typesupport_handle(response_type, rws::ts_identifier_srv, *res_ts_srv_lib_); |
| 64 | + |
| 65 | + service_handle_ = std::shared_ptr<rcl_service_t>( |
| 66 | + new rcl_service_t, [handle = node_handle_](rcl_service_t * service) |
| 67 | + { |
| 68 | + if (rcl_service_fini(service, handle.get()) != RCL_RET_OK) { |
| 69 | + RCLCPP_ERROR( |
| 70 | + rclcpp::get_node_logger(handle.get()).get_child("rclcpp"), |
| 71 | + "Error in destruction of rcl service handle: %s", |
| 72 | + rcl_get_error_string().str); |
| 73 | + rcl_reset_error(); |
| 74 | + } |
| 75 | + delete service; |
| 76 | + }); |
| 77 | + *service_handle_.get() = rcl_get_zero_initialized_service(); |
| 78 | + |
| 79 | + rcl_ret_t ret = rcl_service_init( |
| 80 | + service_handle_.get(), |
| 81 | + node_handle_.get(), |
| 82 | + srv_ts_hdl_, |
| 83 | + service_name.c_str(), |
| 84 | + &service_options); |
| 85 | + if (ret != RCL_RET_OK) { |
| 86 | + if (ret == RCL_RET_SERVICE_NAME_INVALID) { |
| 87 | + rcl_reset_error(); |
| 88 | + rclcpp::expand_topic_or_service_name( |
| 89 | + service_name, |
| 90 | + rcl_node_get_name(node_handle_.get()), |
| 91 | + rcl_node_get_namespace(node_handle_.get()), |
| 92 | + true); |
| 93 | + } |
| 94 | + |
| 95 | + rclcpp::exceptions::throw_from_rcl_error(ret, "could not create generic service"); |
| 96 | + } |
| 97 | + } |
| 98 | + |
| 99 | + std::shared_ptr<void> create_request() override |
| 100 | + { |
| 101 | + auto srv_members = static_cast<const ServiceMembers *>(srv_intro_hdl_->data); |
| 102 | + return allocate_message(srv_members->request_members_); |
| 103 | + } |
| 104 | + |
| 105 | + std::shared_ptr<rmw_request_id_t> create_request_header() override |
| 106 | + { |
| 107 | + return std::make_shared<rmw_request_id_t>(); |
| 108 | + } |
| 109 | + |
| 110 | + void handle_request( |
| 111 | + std::shared_ptr<rmw_request_id_t> request_header, |
| 112 | + std::shared_ptr<void> request) override |
| 113 | + { |
| 114 | + auto serialized_request = std::make_shared<rclcpp::SerializedMessage>(); |
| 115 | + rmw_ret_t ret = |
| 116 | + rmw_serialize(request.get(), req_ts_srv_hdl_, &serialized_request->get_rcl_serialized_message()); |
| 117 | + if (ret != RMW_RET_OK) { |
| 118 | + RCUTILS_LOG_ERROR_NAMED( |
| 119 | + "rws", "Failed to serialize advertised service request: %s", |
| 120 | + rcutils_get_error_string().str); |
| 121 | + rcutils_reset_error(); |
| 122 | + return; |
| 123 | + } |
| 124 | + |
| 125 | + callback_(request_header, serialized_request); |
| 126 | + } |
| 127 | + |
| 128 | + void send_serialized_response( |
| 129 | + std::shared_ptr<rmw_request_id_t> request_header, |
| 130 | + SharedResponse serialized_response) |
| 131 | + { |
| 132 | + auto srv_members = static_cast<const ServiceMembers *>(srv_intro_hdl_->data); |
| 133 | + auto response = allocate_message(srv_members->response_members_); |
| 134 | + const rmw_serialized_message_t * sm = &serialized_response->get_rcl_serialized_message(); |
| 135 | + rmw_ret_t rmw_ret = rmw_deserialize(sm, res_ts_srv_hdl_, response.get()); |
| 136 | + if (rmw_ret != RMW_RET_OK) { |
| 137 | + rclcpp::exceptions::throw_from_rcl_error(rmw_ret, "failed to deserialize service response"); |
| 138 | + } |
| 139 | + |
| 140 | + rcl_ret_t ret = rcl_send_response(get_service_handle().get(), request_header.get(), response.get()); |
| 141 | + if (ret == RCL_RET_TIMEOUT) { |
| 142 | + RCLCPP_WARN( |
| 143 | + node_logger_.get_child("rclcpp"), |
| 144 | + "failed to send response to %s (timeout): %s", |
| 145 | + this->get_service_name(), rcl_get_error_string().str); |
| 146 | + rcl_reset_error(); |
| 147 | + return; |
| 148 | + } |
| 149 | + if (ret != RCL_RET_OK) { |
| 150 | + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send service response"); |
| 151 | + } |
| 152 | + } |
| 153 | + |
| 154 | + const std::string & service_type() const { return service_type_; } |
| 155 | + |
| 156 | +private: |
| 157 | + RCLCPP_DISABLE_COPY(GenericService) |
| 158 | + |
| 159 | + std::string service_type_; |
| 160 | + CallbackType callback_; |
| 161 | + std::shared_ptr<rcpputils::SharedLibrary> srv_ts_lib_; |
| 162 | + const rosidl_service_type_support_t * srv_ts_hdl_; |
| 163 | + std::shared_ptr<rcpputils::SharedLibrary> srv_intro_lib_; |
| 164 | + const rosidl_service_type_support_t * srv_intro_hdl_; |
| 165 | + std::shared_ptr<rcpputils::SharedLibrary> req_ts_srv_lib_; |
| 166 | + const rosidl_message_type_support_t * req_ts_srv_hdl_; |
| 167 | + std::shared_ptr<rcpputils::SharedLibrary> res_ts_srv_lib_; |
| 168 | + const rosidl_message_type_support_t * res_ts_srv_hdl_; |
| 169 | +}; |
| 170 | + |
| 171 | +} // namespace rws |
| 172 | + |
| 173 | +#endif // RWS__GENERIC_SERVICE_HPP_ |
0 commit comments