Skip to content

Commit 3652646

Browse files
deng02wjwwood
deng02
authored and
wjwwood
committed
Store the subscriber, client, service and timer (#431)
* Convert all rcl_*_t types to shared pointers Converts all rcl_*_t types in the memory allocation strategy to shared pointers to prevent crash happening when a subscriber is reset. Issue: #349 * fixup! Convert all rcl_*_t types to shared pointers * fix { use on function definitions We always put the { on a new line for function definitions and class declarations.
1 parent 1a604b0 commit 3652646

13 files changed

Lines changed: 177 additions & 109 deletions

‎rclcpp/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -125,6 +125,7 @@ ament_export_include_directories(include)
125125
ament_export_libraries(${PROJECT_NAME})
126126

127127
if(BUILD_TESTING)
128+
find_package(ament_cmake_gtest REQUIRED)
128129
find_package(ament_lint_auto REQUIRED)
129130
ament_lint_auto_find_test_dependencies()
130131

‎rclcpp/include/rclcpp/client.hpp‎

Lines changed: 5 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -68,11 +68,11 @@ class ClientBase
6868
get_service_name() const;
6969

7070
RCLCPP_PUBLIC
71-
rcl_client_t *
71+
std::shared_ptr<rcl_client_t>
7272
get_client_handle();
7373

7474
RCLCPP_PUBLIC
75-
const rcl_client_t *
75+
std::shared_ptr<const rcl_client_t>
7676
get_client_handle() const;
7777

7878
RCLCPP_PUBLIC
@@ -112,7 +112,7 @@ class ClientBase
112112
rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_;
113113
std::shared_ptr<rcl_node_t> node_handle_;
114114

115-
rcl_client_t client_handle_ = rcl_get_zero_initialized_client();
115+
std::shared_ptr<rcl_client_t> client_handle_;
116116
std::string service_name_;
117117
};
118118

@@ -148,7 +148,7 @@ class Client : public ClientBase
148148
auto service_type_support_handle =
149149
get_service_type_support_handle<ServiceT>();
150150
rcl_ret_t ret = rcl_client_init(
151-
&client_handle_,
151+
client_handle_.get(),
152152
this->get_rcl_node_handle(),
153153
service_type_support_handle,
154154
service_name.c_str(),
@@ -170,11 +170,6 @@ class Client : public ClientBase
170170

171171
virtual ~Client()
172172
{
173-
if (rcl_client_fini(&client_handle_, this->get_rcl_node_handle()) != RCL_RET_OK) {
174-
fprintf(stderr,
175-
"Error in destruction of rcl client handle: %s\n", rcl_get_error_string_safe());
176-
rcl_reset_error();
177-
}
178173
}
179174

180175
std::shared_ptr<void>
@@ -238,7 +233,7 @@ class Client : public ClientBase
238233
{
239234
std::lock_guard<std::mutex> lock(pending_requests_mutex_);
240235
int64_t sequence_number;
241-
rcl_ret_t ret = rcl_send_request(get_client_handle(), request.get(), &sequence_number);
236+
rcl_ret_t ret = rcl_send_request(get_client_handle().get(), request.get(), &sequence_number);
242237
if (RCL_RET_OK != ret) {
243238
rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request");
244239
}

‎rclcpp/include/rclcpp/memory_strategy.hpp‎

Lines changed: 7 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -83,14 +83,18 @@ class RCLCPP_PUBLIC MemoryStrategy
8383

8484
static rclcpp::SubscriptionBase::SharedPtr
8585
get_subscription_by_handle(
86-
const rcl_subscription_t * subscriber_handle,
86+
std::shared_ptr<const rcl_subscription_t> subscriber_handle,
8787
const WeakNodeVector & weak_nodes);
8888

8989
static rclcpp::ServiceBase::SharedPtr
90-
get_service_by_handle(const rcl_service_t * service_handle, const WeakNodeVector & weak_nodes);
90+
get_service_by_handle(
91+
std::shared_ptr<const rcl_service_t> service_handle,
92+
const WeakNodeVector & weak_nodes);
9193

9294
static rclcpp::ClientBase::SharedPtr
93-
get_client_by_handle(const rcl_client_t * client_handle, const WeakNodeVector & weak_nodes);
95+
get_client_by_handle(
96+
std::shared_ptr<const rcl_client_t> client_handle,
97+
const WeakNodeVector & weak_nodes);
9498

9599
static rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
96100
get_node_by_group(

‎rclcpp/include/rclcpp/service.hpp‎

Lines changed: 49 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,8 @@
2121
#include <sstream>
2222
#include <string>
2323

24+
#include "rcutils/logging_macros.h"
25+
2426
#include "rcl/error_handling.h"
2527
#include "rcl/service.h"
2628

@@ -30,6 +32,7 @@
3032
#include "rclcpp/type_support_decl.hpp"
3133
#include "rclcpp/expand_topic_or_service_name.hpp"
3234
#include "rclcpp/visibility_control.hpp"
35+
#include "rclcpp/logging.hpp"
3336
#include "rmw/error_handling.h"
3437
#include "rmw/rmw.h"
3538

@@ -58,11 +61,11 @@ class ServiceBase
5861
get_service_name();
5962

6063
RCLCPP_PUBLIC
61-
rcl_service_t *
64+
std::shared_ptr<rcl_service_t>
6265
get_service_handle();
6366

6467
RCLCPP_PUBLIC
65-
const rcl_service_t *
68+
std::shared_ptr<const rcl_service_t>
6669
get_service_handle() const;
6770

6871
virtual std::shared_ptr<void> create_request() = 0;
@@ -84,7 +87,7 @@ class ServiceBase
8487

8588
std::shared_ptr<rcl_node_t> node_handle_;
8689

87-
rcl_service_t * service_handle_ = nullptr;
90+
std::shared_ptr<rcl_service_t> service_handle_;
8891
std::string service_name_;
8992
bool owns_rcl_handle_ = true;
9093
};
@@ -116,11 +119,22 @@ class Service : public ServiceBase
116119
auto service_type_support_handle = get_service_type_support_handle<ServiceT>();
117120

118121
// rcl does the static memory allocation here
119-
service_handle_ = new rcl_service_t;
120-
*service_handle_ = rcl_get_zero_initialized_service();
122+
service_handle_ = std::shared_ptr<rcl_service_t>(
123+
new rcl_service_t, [ = ](rcl_service_t * service)
124+
{
125+
if (rcl_service_fini(service, node_handle_.get()) != RCL_RET_OK) {
126+
RCLCPP_ERROR(
127+
rclcpp::get_logger(rcl_node_get_name(node_handle.get())).get_child("rclcpp"),
128+
"Error in destruction of rcl service handle: %s",
129+
rcl_get_error_string_safe());
130+
rcl_reset_error();
131+
}
132+
delete service;
133+
});
134+
*service_handle_.get() = rcl_get_zero_initialized_service();
121135

122136
rcl_ret_t ret = rcl_service_init(
123-
service_handle_,
137+
service_handle_.get(),
124138
node_handle.get(),
125139
service_type_support_handle,
126140
service_name.c_str(),
@@ -141,6 +155,29 @@ class Service : public ServiceBase
141155
}
142156
}
143157

158+
Service(
159+
std::shared_ptr<rcl_node_t> node_handle,
160+
std::shared_ptr<rcl_service_t> service_handle,
161+
AnyServiceCallback<ServiceT> any_callback)
162+
: ServiceBase(node_handle),
163+
any_callback_(any_callback)
164+
{
165+
// check if service handle was initialized
166+
if (!rcl_service_is_valid(service_handle.get(), nullptr)) {
167+
// *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
168+
throw std::runtime_error(
169+
std::string("rcl_service_t in constructor argument must be initialized beforehand."));
170+
// *INDENT-ON*
171+
}
172+
173+
const char * service_name = rcl_service_get_service_name(service_handle.get());
174+
if (!service_name) {
175+
throw std::runtime_error("failed to get service name");
176+
}
177+
service_handle_ = service_handle;
178+
service_name_ = std::string(service_name);
179+
}
180+
144181
Service(
145182
std::shared_ptr<rcl_node_t> node_handle,
146183
rcl_service_t * service_handle,
@@ -149,10 +186,7 @@ class Service : public ServiceBase
149186
any_callback_(any_callback)
150187
{
151188
// check if service handle was initialized
152-
// TODO(karsten1987): Take this verification
153-
// directly in rcl_*_t
154-
// see: https://github.com/ros2/rcl/issues/81
155-
if (!service_handle->impl) {
189+
if (!rcl_service_is_valid(service_handle, nullptr)) {
156190
// *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
157191
throw std::runtime_error(
158192
std::string("rcl_service_t in constructor argument must be initialized beforehand."));
@@ -163,26 +197,17 @@ class Service : public ServiceBase
163197
if (!service_name) {
164198
throw std::runtime_error("failed to get service name");
165199
}
166-
service_handle_ = service_handle;
167200
service_name_ = std::string(service_name);
168-
owns_rcl_handle_ = false;
201+
202+
// In this case, rcl owns the service handle memory
203+
service_handle_ = std::shared_ptr<rcl_service_t>(new rcl_service_t);
204+
service_handle_->impl = service_handle->impl;
169205
}
170206

171207
Service() = delete;
172208

173209
virtual ~Service()
174210
{
175-
// check if you have ownership of the handle
176-
if (owns_rcl_handle_) {
177-
if (rcl_service_fini(service_handle_, node_handle_.get()) != RCL_RET_OK) {
178-
std::stringstream ss;
179-
ss << "Error in destruction of rcl service_handle_ handle: " <<
180-
rcl_get_error_string_safe() << '\n';
181-
(std::cerr << ss.str()).flush();
182-
rcl_reset_error();
183-
}
184-
delete service_handle_;
185-
}
186211
}
187212

188213
std::shared_ptr<void> create_request()
@@ -211,7 +236,7 @@ class Service : public ServiceBase
211236
std::shared_ptr<rmw_request_id_t> req_id,
212237
std::shared_ptr<typename ServiceT::Response> response)
213238
{
214-
rcl_ret_t status = rcl_send_response(get_service_handle(), req_id.get(), response.get());
239+
rcl_ret_t status = rcl_send_response(get_service_handle().get(), req_id.get(), response.get());
215240

216241
if (status != RCL_RET_OK) {
217242
rclcpp::exceptions::throw_from_rcl_error(status, "failed to send response");

‎rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp‎

Lines changed: 12 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -98,22 +98,22 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
9898
{
9999
for (size_t i = 0; i < wait_set->size_of_subscriptions; ++i) {
100100
if (!wait_set->subscriptions[i]) {
101-
subscription_handles_[i] = nullptr;
101+
subscription_handles_[i].reset();
102102
}
103103
}
104104
for (size_t i = 0; i < wait_set->size_of_services; ++i) {
105105
if (!wait_set->services[i]) {
106-
service_handles_[i] = nullptr;
106+
service_handles_[i].reset();
107107
}
108108
}
109109
for (size_t i = 0; i < wait_set->size_of_clients; ++i) {
110110
if (!wait_set->clients[i]) {
111-
client_handles_[i] = nullptr;
111+
client_handles_[i].reset();
112112
}
113113
}
114114
for (size_t i = 0; i < wait_set->size_of_timers; ++i) {
115115
if (!wait_set->timers[i]) {
116-
timer_handles_[i] = nullptr;
116+
timer_handles_[i].reset();
117117
}
118118
}
119119

@@ -188,7 +188,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
188188
bool add_handles_to_wait_set(rcl_wait_set_t * wait_set)
189189
{
190190
for (auto subscription : subscription_handles_) {
191-
if (rcl_wait_set_add_subscription(wait_set, subscription) != RCL_RET_OK) {
191+
if (rcl_wait_set_add_subscription(wait_set, subscription.get()) != RCL_RET_OK) {
192192
RCUTILS_LOG_ERROR_NAMED(
193193
"rclcpp",
194194
"Couldn't add subscription to wait set: %s", rcl_get_error_string_safe());
@@ -197,7 +197,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
197197
}
198198

199199
for (auto client : client_handles_) {
200-
if (rcl_wait_set_add_client(wait_set, client) != RCL_RET_OK) {
200+
if (rcl_wait_set_add_client(wait_set, client.get()) != RCL_RET_OK) {
201201
RCUTILS_LOG_ERROR_NAMED(
202202
"rclcpp",
203203
"Couldn't add client to wait set: %s", rcl_get_error_string_safe());
@@ -206,7 +206,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
206206
}
207207

208208
for (auto service : service_handles_) {
209-
if (rcl_wait_set_add_service(wait_set, service) != RCL_RET_OK) {
209+
if (rcl_wait_set_add_service(wait_set, service.get()) != RCL_RET_OK) {
210210
RCUTILS_LOG_ERROR_NAMED(
211211
"rclcpp",
212212
"Couldn't add service to wait set: %s", rcl_get_error_string_safe());
@@ -215,7 +215,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
215215
}
216216

217217
for (auto timer : timer_handles_) {
218-
if (rcl_wait_set_add_timer(wait_set, timer) != RCL_RET_OK) {
218+
if (rcl_wait_set_add_timer(wait_set, timer.get()) != RCL_RET_OK) {
219219
RCUTILS_LOG_ERROR_NAMED(
220220
"rclcpp",
221221
"Couldn't add timer to wait set: %s", rcl_get_error_string_safe());
@@ -391,10 +391,10 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
391391

392392
VectorRebind<const rcl_guard_condition_t *> guard_conditions_;
393393

394-
VectorRebind<const rcl_subscription_t *> subscription_handles_;
395-
VectorRebind<const rcl_service_t *> service_handles_;
396-
VectorRebind<const rcl_client_t *> client_handles_;
397-
VectorRebind<const rcl_timer_t *> timer_handles_;
394+
VectorRebind<std::shared_ptr<const rcl_subscription_t>> subscription_handles_;
395+
VectorRebind<std::shared_ptr<const rcl_service_t>> service_handles_;
396+
VectorRebind<std::shared_ptr<const rcl_client_t>> client_handles_;
397+
VectorRebind<std::shared_ptr<const rcl_timer_t>> timer_handles_;
398398

399399
std::shared_ptr<ExecAlloc> executable_allocator_;
400400
std::shared_ptr<VoidAlloc> allocator_;

‎rclcpp/include/rclcpp/subscription.hpp‎

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -76,15 +76,15 @@ class SubscriptionBase
7676
get_topic_name() const;
7777

7878
RCLCPP_PUBLIC
79-
rcl_subscription_t *
79+
std::shared_ptr<rcl_subscription_t>
8080
get_subscription_handle();
8181

8282
RCLCPP_PUBLIC
83-
const rcl_subscription_t *
83+
const std::shared_ptr<rcl_subscription_t>
8484
get_subscription_handle() const;
8585

8686
RCLCPP_PUBLIC
87-
virtual const rcl_subscription_t *
87+
virtual const std::shared_ptr<rcl_subscription_t>
8888
get_intra_process_subscription_handle() const;
8989

9090
/// Borrow a new message.
@@ -110,8 +110,8 @@ class SubscriptionBase
110110
const rmw_message_info_t & message_info) = 0;
111111

112112
protected:
113-
rcl_subscription_t intra_process_subscription_handle_ = rcl_get_zero_initialized_subscription();
114-
rcl_subscription_t subscription_handle_ = rcl_get_zero_initialized_subscription();
113+
std::shared_ptr<rcl_subscription_t> intra_process_subscription_handle_;
114+
std::shared_ptr<rcl_subscription_t> subscription_handle_;
115115
std::shared_ptr<rcl_node_t> node_handle_;
116116

117117
private:
@@ -241,7 +241,7 @@ class Subscription : public SubscriptionBase
241241
{
242242
std::string intra_process_topic_name = std::string(get_topic_name()) + "/_intra";
243243
rcl_ret_t ret = rcl_subscription_init(
244-
&intra_process_subscription_handle_,
244+
intra_process_subscription_handle_.get(),
245245
node_handle_.get(),
246246
rclcpp::type_support::get_intra_process_message_msg_type_support(),
247247
intra_process_topic_name.c_str(),
@@ -266,13 +266,13 @@ class Subscription : public SubscriptionBase
266266
}
267267

268268
/// Implemenation detail.
269-
const rcl_subscription_t *
269+
const std::shared_ptr<rcl_subscription_t>
270270
get_intra_process_subscription_handle() const
271271
{
272272
if (!get_intra_process_message_callback_) {
273273
return nullptr;
274274
}
275-
return &intra_process_subscription_handle_;
275+
return intra_process_subscription_handle_;
276276
}
277277

278278
private:

‎rclcpp/include/rclcpp/timer.hpp‎

Lines changed: 3 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -62,7 +62,7 @@ class TimerBase
6262
execute_callback() = 0;
6363

6464
RCLCPP_PUBLIC
65-
const rcl_timer_t *
65+
std::shared_ptr<const rcl_timer_t>
6666
get_timer_handle();
6767

6868
/// Check how long the timer has until its next scheduled callback.
@@ -85,7 +85,7 @@ class TimerBase
8585
bool is_ready();
8686

8787
protected:
88-
rcl_timer_t timer_handle_ = rcl_get_zero_initialized_timer();
88+
std::shared_ptr<rcl_timer_t> timer_handle_;
8989
};
9090

9191

@@ -122,15 +122,12 @@ class GenericTimer : public TimerBase
122122
{
123123
// Stop the timer from running.
124124
cancel();
125-
if (rcl_timer_fini(&timer_handle_) != RCL_RET_OK) {
126-
fprintf(stderr, "Failed to clean up rcl timer handle: %s\n", rcl_get_error_string_safe());
127-
}
128125
}
129126

130127
void
131128
execute_callback()
132129
{
133-
rcl_ret_t ret = rcl_timer_call(&timer_handle_);
130+
rcl_ret_t ret = rcl_timer_call(timer_handle_.get());
134131
if (ret == RCL_RET_TIMER_CANCELED) {
135132
return;
136133
}

0 commit comments

Comments
 (0)