-
Notifications
You must be signed in to change notification settings - Fork 527
Expand file tree
/
Copy pathservice.cpp
More file actions
138 lines (117 loc) · 3.45 KB
/
service.cpp
File metadata and controls
138 lines (117 loc) · 3.45 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
// Copyright 2015 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/service.hpp"
#include <functional>
#include <iostream>
#include <memory>
#include <sstream>
#include <string>
#include "rclcpp/any_service_callback.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/qos.hpp"
#include "rmw/error_handling.h"
#include "rmw/rmw.h"
using rclcpp::ServiceBase;
ServiceBase::ServiceBase(const std::shared_ptr<rcl_node_t> & node_handle)
: node_handle_(node_handle),
node_logger_(rclcpp::get_node_logger(node_handle_.get()))
{}
bool
ServiceBase::take_type_erased_request(void * request_out, rmw_request_id_t & request_id_out)
{
rcl_ret_t ret = rcl_take_request(
this->get_service_handle().get(),
&request_id_out,
request_out);
if (RCL_RET_SERVICE_TAKE_FAILED == ret) {
return false;
} else if (RCL_RET_OK != ret) {
rclcpp::exceptions::throw_from_rcl_error(ret);
}
return true;
}
const char *
ServiceBase::get_service_name()
{
return rcl_service_get_service_name(this->get_service_handle().get());
}
std::shared_ptr<rcl_service_t>
ServiceBase::get_service_handle()
{
return service_handle_;
}
std::shared_ptr<const rcl_service_t>
ServiceBase::get_service_handle() const
{
return service_handle_;
}
rcl_node_t *
ServiceBase::get_rcl_node_handle()
{
return node_handle_.get();
}
const rcl_node_t *
ServiceBase::get_rcl_node_handle() const
{
return node_handle_.get();
}
bool
ServiceBase::exchange_in_use_by_wait_set_state(bool in_use_state)
{
return in_use_by_wait_set_.exchange(in_use_state);
}
rclcpp::QoS
ServiceBase::get_response_publisher_actual_qos() const
{
const rmw_qos_profile_t * qos =
rcl_service_response_publisher_get_actual_qos(service_handle_.get());
if (!qos) {
auto msg =
std::string("failed to get service's response publisher qos settings: ") +
rcl_get_error_string().str;
rcl_reset_error();
throw std::runtime_error(msg);
}
rclcpp::QoS response_publisher_qos =
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos);
return response_publisher_qos;
}
rclcpp::QoS
ServiceBase::get_request_subscription_actual_qos() const
{
const rmw_qos_profile_t * qos =
rcl_service_request_subscription_get_actual_qos(service_handle_.get());
if (!qos) {
auto msg =
std::string("failed to get service's request subscription qos settings: ") +
rcl_get_error_string().str;
rcl_reset_error();
throw std::runtime_error(msg);
}
rclcpp::QoS request_subscription_qos =
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos);
return request_subscription_qos;
}
void
ServiceBase::set_on_new_request_callback(rcl_event_callback_t callback, const void * user_data)
{
rcl_ret_t ret = rcl_service_set_on_new_request_callback(
service_handle_.get(),
callback,
user_data);
if (RCL_RET_OK != ret) {
rclcpp::exceptions::throw_from_rcl_error(
ret, "failed to set the on new request callback for service");
}
}