Skip to content

Commit fe2fd49

Browse files
asorbiniasorbini
committed
Add support for listener callbacks.
Signed-off-by: Andrea Sorbini <[email protected]>
1 parent 35a37aa commit fe2fd49

File tree

4 files changed

+149
-41
lines changed

4 files changed

+149
-41
lines changed

‎rmw_connextdds_common/include/rmw_connextdds/rmw_api_impl.hpp‎

Lines changed: 9 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -90,8 +90,8 @@ RMW_CONNEXTDDS_PUBLIC
9090
rmw_ret_t
9191
rmw_api_connextdds_event_set_callback(
9292
rmw_event_t * event,
93-
rmw_event_callback_t callback,
94-
const void * user_data);
93+
const rmw_event_callback_t callback,
94+
const void * const user_data);
9595

9696
/*****************************************************************************
9797
* Info API
@@ -435,15 +435,15 @@ RMW_CONNEXTDDS_PUBLIC
435435
rmw_ret_t
436436
rmw_api_connextdds_service_set_on_new_request_callback(
437437
rmw_service_t * rmw_service,
438-
rmw_event_callback_t callback,
439-
const void * user_data);
438+
const rmw_event_callback_t callback,
439+
const void * const user_data);
440440

441441
RMW_CONNEXTDDS_PUBLIC
442442
rmw_ret_t
443443
rmw_api_connextdds_client_set_on_new_response_callback(
444444
rmw_client_t * rmw_client,
445-
rmw_event_callback_t callback,
446-
const void * user_data);
445+
const rmw_event_callback_t callback,
446+
const void * const user_data);
447447

448448
/*****************************************************************************
449449
* Subscription API
@@ -560,9 +560,9 @@ rmw_api_connextdds_return_loaned_message_from_subscription(
560560
RMW_CONNEXTDDS_PUBLIC
561561
rmw_ret_t
562562
rmw_api_connextdds_subscription_set_on_new_message_callback(
563-
rmw_subscription_t * rmw_subscription,
564-
rmw_event_callback_t callback,
565-
const void * user_data);
563+
rmw_subscription_t * const rmw_subscription,
564+
const rmw_event_callback_t callback,
565+
const void * const user_data);
566566

567567
/*****************************************************************************
568568
* WaitSet API

‎rmw_connextdds_common/include/rmw_connextdds/rmw_waitset_std.hpp‎

Lines changed: 72 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -333,8 +333,44 @@ class RMW_Connext_StatusCondition : public RMW_Connext_Condition
333333
virtual bool
334334
has_status(const rmw_event_type_t event_type) = 0;
335335

336+
void
337+
notify_new_event()
338+
{
339+
std::unique_lock<std::mutex> lock_mutex(new_event_mutex_);
340+
if (new_event_cb_) {
341+
new_event_cb_(user_data_, 1);
342+
} else {
343+
unread_events_count_++;
344+
}
345+
}
346+
347+
void
348+
set_new_event_callback(
349+
rmw_event_callback_t callback,
350+
const void * user_data)
351+
{
352+
std::unique_lock<std::mutex> lock_mutex(new_event_mutex_);
353+
354+
if (callback) {
355+
// Push events arrived before setting the executor's callback
356+
if (unread_events_count_) {
357+
callback(user_data, unread_events_count_);
358+
unread_events_count_ = 0;
359+
}
360+
user_data_ = user_data;
361+
new_event_cb_ = callback;
362+
} else {
363+
user_data_ = nullptr;
364+
new_event_cb_ = nullptr;
365+
}
366+
}
367+
336368
protected:
337369
DDS_StatusCondition * scond;
370+
std::mutex new_event_mutex_;
371+
rmw_event_callback_t new_event_cb_{nullptr};
372+
const void * user_data_{nullptr};
373+
uint64_t unread_events_count_ = 0;
338374
};
339375

340376
void
@@ -589,6 +625,9 @@ class RMW_Connext_SubscriberStatusCondition : public RMW_Connext_StatusCondition
589625
update_state(
590626
[this, available]() {
591627
this->triggered_data = available;
628+
if (available) {
629+
this->notify_new_data();
630+
}
592631
}, true /* notify */);
593632

594633
if (nullptr != this->loan_guard_condition) {
@@ -712,6 +751,34 @@ class RMW_Connext_SubscriberStatusCondition : public RMW_Connext_StatusCondition
712751
return RMW_RET_OK;
713752
}
714753

754+
void set_on_new_data_callback(
755+
const rmw_event_callback_t callback,
756+
const void * const user_data)
757+
{
758+
std::unique_lock<std::mutex> lock(new_data_event_mutex_);
759+
if (callback) {
760+
if (unread_data_events_count_ > 0) {
761+
callback(user_data, unread_data_events_count_);
762+
unread_data_events_count_ = 0;
763+
new_data_event_cb_ = callback;
764+
data_event_user_data_ = user_data;
765+
}
766+
} else {
767+
new_data_event_cb_ = nullptr;
768+
data_event_user_data_ = nullptr;
769+
}
770+
}
771+
772+
void notify_new_data()
773+
{
774+
std::unique_lock<std::mutex> lock_mutex(new_data_event_mutex_);
775+
if (new_data_event_cb_) {
776+
new_data_event_cb_(data_event_user_data_, 1);
777+
} else {
778+
unread_data_events_count_++;
779+
}
780+
}
781+
715782
protected:
716783
void update_status_deadline(
717784
const DDS_RequestedDeadlineMissedStatus * const status);
@@ -745,6 +812,11 @@ class RMW_Connext_SubscriberStatusCondition : public RMW_Connext_StatusCondition
745812

746813
RMW_Connext_Subscriber * sub;
747814

815+
std::mutex new_data_event_mutex_;
816+
rmw_event_callback_t new_data_event_cb_{nullptr};
817+
const void * data_event_user_data_{nullptr};
818+
uint64_t unread_data_events_count_ = 0;
819+
748820
friend class RMW_Connext_WaitSet;
749821
};
750822

‎rmw_connextdds_common/src/common/rmw_impl_waitset_std.cpp‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -706,6 +706,8 @@ RMW_Connext_SubscriberStatusCondition::update_status_deadline(
706706

707707
this->status_deadline.total_count_change = this->status_deadline.total_count;
708708
this->status_deadline.total_count_change -= this->status_deadline_last.total_count;
709+
710+
this->notify_new_event();
709711
}
710712

711713
void
@@ -720,6 +722,8 @@ RMW_Connext_SubscriberStatusCondition::update_status_liveliness(
720722
this->status_liveliness.alive_count_change -= this->status_liveliness_last.alive_count;
721723
this->status_liveliness.not_alive_count_change -=
722724
this->status_liveliness_last.not_alive_count;
725+
726+
this->notify_new_event();
723727
}
724728

725729
void
@@ -852,6 +856,8 @@ RMW_Connext_PublisherStatusCondition::update_status_deadline(
852856

853857
this->status_deadline.total_count_change = this->status_deadline.total_count;
854858
this->status_deadline.total_count_change -= this->status_deadline_last.total_count;
859+
860+
this->notify_new_event();
855861
}
856862

857863
void
@@ -863,6 +869,8 @@ RMW_Connext_PublisherStatusCondition::update_status_liveliness(
863869

864870
this->status_liveliness.total_count_change = this->status_liveliness.total_count;
865871
this->status_liveliness.total_count_change -= this->status_liveliness_last.total_count;
872+
873+
this->notify_new_event();
866874
}
867875

868876
void

‎rmw_connextdds_common/src/common/rmw_listener.cpp‎

Lines changed: 60 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -19,58 +19,86 @@
1919
******************************************************************************/
2020
rmw_ret_t
2121
rmw_api_connextdds_event_set_callback(
22-
rmw_event_t * event,
23-
rmw_event_callback_t callback,
24-
const void * user_data)
22+
rmw_event_t * const event,
23+
const rmw_event_callback_t callback,
24+
const void * const user_data)
2525
{
26-
UNUSED_ARG(event);
27-
UNUSED_ARG(callback);
28-
UNUSED_ARG(user_data);
29-
RMW_CONNEXT_LOG_ERROR_SET("rmw_event_set_callback not implemented")
30-
return RMW_RET_UNSUPPORTED;
26+
RMW_CHECK_ARGUMENT_FOR_NULL(event, RMW_RET_INVALID_ARGUMENT);
27+
RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
28+
event,
29+
event->implementation_identifier,
30+
RMW_CONNEXTDDS_ID,
31+
return RMW_RET_INVALID_ARGUMENT);
32+
33+
RMW_Connext_StatusCondition * condition = nullptr;
34+
if (RMW_Connext_Event::reader_event(event)) {
35+
condition = RMW_Connext_Event::subscriber(event)->condition();
36+
} else {
37+
condition = RMW_Connext_Event::publisher(event)->condition();
38+
}
39+
condition->set_new_event_callback(callback, user_data);
40+
return RMW_RET_OK;
3141
}
3242

3343
/******************************************************************************
3444
* Service Listener API
3545
******************************************************************************/
3646
rmw_ret_t
3747
rmw_api_connextdds_service_set_on_new_request_callback(
38-
rmw_service_t * rmw_service,
39-
rmw_event_callback_t callback,
40-
const void * user_data)
48+
rmw_service_t * const service,
49+
const rmw_event_callback_t callback,
50+
const void * const user_data)
4151
{
42-
UNUSED_ARG(rmw_service);
43-
UNUSED_ARG(callback);
44-
UNUSED_ARG(user_data);
45-
RMW_CONNEXT_LOG_ERROR_SET("rmw_service_set_on_new_request_callback not implemented")
46-
return RMW_RET_UNSUPPORTED;
52+
RMW_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT);
53+
RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
54+
service,
55+
service->implementation_identifier,
56+
RMW_CONNEXTDDS_ID,
57+
return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
58+
59+
RMW_Connext_Service * const svc_impl =
60+
reinterpret_cast<RMW_Connext_Service *>(service->data);
61+
svc_impl->subscriber()->condition()->set_on_new_data_callback(callback, user_data);
62+
return RMW_RET_OK;
4763
}
4864

4965
rmw_ret_t
5066
rmw_api_connextdds_client_set_on_new_response_callback(
51-
rmw_client_t * rmw_client,
52-
rmw_event_callback_t callback,
53-
const void * user_data)
67+
rmw_client_t * const client,
68+
const rmw_event_callback_t callback,
69+
const void * const user_data)
5470
{
55-
UNUSED_ARG(rmw_client);
56-
UNUSED_ARG(callback);
57-
UNUSED_ARG(user_data);
58-
RMW_CONNEXT_LOG_ERROR_SET("rmw_client_set_on_new_response_callback not implemented")
59-
return RMW_RET_UNSUPPORTED;
71+
RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT);
72+
RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
73+
client,
74+
client->implementation_identifier,
75+
RMW_CONNEXTDDS_ID,
76+
return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
77+
78+
RMW_Connext_Client * const client_impl =
79+
reinterpret_cast<RMW_Connext_Client *>(client->data);
80+
client_impl->subscriber()->condition()->set_on_new_data_callback(callback, user_data);
81+
return RMW_RET_OK;
6082
}
6183

6284
/******************************************************************************
6385
* Subscription Listener API
6486
******************************************************************************/
6587
rmw_ret_t
6688
rmw_api_connextdds_subscription_set_on_new_message_callback(
67-
rmw_subscription_t * rmw_subscription,
68-
rmw_event_callback_t callback,
69-
const void * user_data)
89+
rmw_subscription_t * const subscription,
90+
const rmw_event_callback_t callback,
91+
const void * const user_data)
7092
{
71-
UNUSED_ARG(rmw_subscription);
72-
UNUSED_ARG(callback);
73-
UNUSED_ARG(user_data);
74-
RMW_CONNEXT_LOG_ERROR_SET("rmw_subscription_set_on_new_message_callback not implemented")
75-
return RMW_RET_UNSUPPORTED;
93+
RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT);
94+
RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
95+
subscription,
96+
subscription->implementation_identifier,
97+
RMW_CONNEXTDDS_ID,
98+
return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
99+
100+
RMW_Connext_Subscriber * const sub_impl =
101+
reinterpret_cast<RMW_Connext_Subscriber *>(subscription->data);
102+
sub_impl->condition()->set_on_new_data_callback(callback, user_data);
103+
return RMW_RET_OK;
76104
}

0 commit comments

Comments
 (0)