Merge pull request #29 from jefferyyjhsu/irobot/humble

Update irobot/humble to include ros2#633
irobot-ros · Sep 2, 2022 · b5d5c93 · b5d5c93
2 parents 7a191c3 + a3fdd26
commit b5d5c93
Show file tree

Hide file tree

Showing 41 changed files with 1,132 additions and 1,111 deletions.
diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/subscription.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/subscription.hpp
@@ -30,8 +30,7 @@ create_subscription(
   const char * topic_name,
   const rmw_qos_profile_t * qos_policies,
   const rmw_subscription_options_t * subscription_options,
-  bool keyed,
-  bool create_subscription_listener);
+  bool keyed);
 }  // namespace rmw_fastrtps_cpp
 
 #endif  // RMW_FASTRTPS_CPP__SUBSCRIPTION_HPP_
diff --git a/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp
@@ -42,7 +42,8 @@ using rmw_dds_common::msg::ParticipantEntitiesInfo;
 
 static
 rmw_ret_t
-init_context_impl(rmw_context_t * context)
+init_context_impl(
+  rmw_context_t * context)
 {
   rmw_publisher_options_t publisher_options = rmw_get_default_publisher_options();
   rmw_subscription_options_t subscription_options = rmw_get_default_subscription_options();
@@ -65,7 +66,8 @@ init_context_impl(rmw_context_t * context)
       (context->options.localhost_only == RMW_LOCALHOST_ONLY_ENABLED) ? 1 : 0,
       context->options.enclave,
       common_context.get()),
-    [&](CustomParticipantInfo * participant_info) {
+    [&](CustomParticipantInfo * participant_info)
+    {
       if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_participant(participant_info)) {
         RCUTILS_SAFE_FWRITE_TO_STDERR(
           "Failed to destroy participant after function: '"
@@ -92,9 +94,10 @@ init_context_impl(rmw_context_t * context)
       "ros_discovery_info",
       &qos,
       &publisher_options,
-      false,  // our fastrtps typesupport doesn't support keyed topics
+      false,       // our fastrtps typesupport doesn't support keyed topics
       true),
-    [&](rmw_publisher_t * pub) {
+    [&](rmw_publisher_t * pub)
+    {
       if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_publisher(
         eprosima_fastrtps_identifier,
         participant_info.get(),
@@ -119,9 +122,9 @@ init_context_impl(rmw_context_t * context)
       "ros_discovery_info",
       &qos,
       &subscription_options,
-      false,  // our fastrtps typesupport doesn't support keyed topics
-      true),
-    [&](rmw_subscription_t * sub) {
+      false),       // our fastrtps typesupport doesn't support keyed topics
+    [&](rmw_subscription_t * sub)
+    {
       if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_subscription(
         eprosima_fastrtps_identifier,
         participant_info.get(),
@@ -139,7 +142,8 @@ init_context_impl(rmw_context_t * context)
   std::unique_ptr<rmw_guard_condition_t, std::function<void(rmw_guard_condition_t *)>>
   graph_guard_condition(
     rmw_fastrtps_shared_cpp::__rmw_create_guard_condition(eprosima_fastrtps_identifier),
-    [&](rmw_guard_condition_t * p) {
+    [&](rmw_guard_condition_t * p)
+    {
       rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition(p);
       if (ret != RMW_RET_OK) {
         RMW_SAFE_FWRITE_TO_STDERR(
@@ -166,7 +170,8 @@ init_context_impl(rmw_context_t * context)
   }
 
   common_context->graph_cache.set_on_change_callback(
-    [guard_condition = graph_guard_condition.get()]() {
+    [guard_condition = graph_guard_condition.get()]()
+    {
       rmw_fastrtps_shared_cpp::__rmw_trigger_guard_condition(
         eprosima_fastrtps_identifier,
         guard_condition);
@@ -185,7 +190,8 @@ init_context_impl(rmw_context_t * context)
 }
 
 rmw_ret_t
-rmw_fastrtps_cpp::increment_context_impl_ref_count(rmw_context_t * context)
+rmw_fastrtps_cpp::increment_context_impl_ref_count(
+  rmw_context_t * context)
 {
   assert(context);
   assert(context->impl);

diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp
@@ -269,17 +269,22 @@ rmw_fastrtps_cpp::create_publisher(
     return nullptr;
   }
 
-  // Creates DataWriter
+  // Creates DataWriter with a mask enabling publication_matched calls for the listener
   info->data_writer_ = publisher->create_datawriter(
     topic.topic,
     writer_qos,
-    info->listener_);
+    info->listener_,
+    eprosima::fastdds::dds::StatusMask::publication_matched());
 
   if (!info->data_writer_) {
     RMW_SET_ERROR_MSG("create_publisher() could not create data writer");
     return nullptr;
   }
 
+  // Set the StatusCondition to none to prevent triggering via WaitSets
+  info->data_writer_->get_statuscondition().set_enabled_statuses(
+    eprosima::fastdds::dds::StatusMask::none());
+
   // lambda to delete datawriter
   auto cleanup_datawriter = rcpputils::make_scope_exit(
     [publisher, info]() {

diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp
@@ -324,13 +324,17 @@ rmw_create_client(
   info->response_reader_ = subscriber->create_datareader(
     response_topic_desc,
     reader_qos,
-    info->listener_);
+    info->listener_,
+    eprosima::fastdds::dds::StatusMask::subscription_matched());
 
   if (!info->response_reader_) {
     RMW_SET_ERROR_MSG("create_client() failed to create response DataReader");
     return nullptr;
   }
 
+  info->response_reader_->get_statuscondition().set_enabled_statuses(
+    eprosima::fastdds::dds::StatusMask::data_available());
+
   // lambda to delete datareader
   auto cleanup_datareader = rcpputils::make_scope_exit(
     [subscriber, info]() {
@@ -370,17 +374,22 @@ rmw_create_client(
     return nullptr;
   }
 
-  // Creates DataWriter
+  // Creates DataWriter with a mask enabling publication_matched calls for the listener
   info->request_writer_ = publisher->create_datawriter(
     request_topic.topic,
     writer_qos,
-    info->pub_listener_);
+    info->pub_listener_,
+    eprosima::fastdds::dds::StatusMask::publication_matched());
 
   if (!info->request_writer_) {
     RMW_SET_ERROR_MSG("create_client() failed to create request DataWriter");
     return nullptr;
   }
 
+  // Set the StatusCondition to none to prevent triggering via WaitSets
+  info->request_writer_->get_statuscondition().set_enabled_statuses(
+    eprosima::fastdds::dds::StatusMask::none());
+
   // lambda to delete datawriter
   auto cleanup_datawriter = rcpputils::make_scope_exit(
     [publisher, info]() {
@@ -532,6 +541,8 @@ rmw_client_set_on_new_response_callback(
   rmw_event_callback_t callback,
   const void * user_data)
 {
+  RMW_CHECK_ARGUMENT_FOR_NULL(rmw_client, RMW_RET_INVALID_ARGUMENT);
+
   return rmw_fastrtps_shared_cpp::__rmw_client_set_on_new_response_callback(
     rmw_client,
     callback,

diff --git a/rmw_fastrtps_cpp/src/rmw_event.cpp b/rmw_fastrtps_cpp/src/rmw_event.cpp
@@ -54,6 +54,8 @@ rmw_event_set_callback(
   rmw_event_callback_t callback,
   const void * user_data)
 {
+  RMW_CHECK_ARGUMENT_FOR_NULL(rmw_event, RMW_RET_INVALID_ARGUMENT);
+
   return rmw_fastrtps_shared_cpp::__rmw_event_set_callback(
     rmw_event,
     callback,

diff --git a/rmw_fastrtps_cpp/src/rmw_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp
@@ -323,13 +323,17 @@ rmw_create_service(
   info->request_reader_ = subscriber->create_datareader(
     request_topic_desc,
     reader_qos,
-    info->listener_);
+    info->listener_,
+    eprosima::fastdds::dds::StatusMask::subscription_matched());
 
   if (!info->request_reader_) {
     RMW_SET_ERROR_MSG("create_service() failed to create request DataReader");
     return nullptr;
   }
 
+  info->request_reader_->get_statuscondition().set_enabled_statuses(
+    eprosima::fastdds::dds::StatusMask::data_available());
+
   // lambda to delete datareader
   auto cleanup_datareader = rcpputils::make_scope_exit(
     [subscriber, info]() {
@@ -373,17 +377,22 @@ rmw_create_service(
     return nullptr;
   }
 
-  // Creates DataWriter
+  // Creates DataWriter with a mask enabling publication_matched calls for the listener
   info->response_writer_ = publisher->create_datawriter(
     response_topic.topic,
     writer_qos,
-    info->pub_listener_);
+    info->pub_listener_,
+    eprosima::fastdds::dds::StatusMask::publication_matched());
 
   if (!info->response_writer_) {
     RMW_SET_ERROR_MSG("create_service() failed to create response DataWriter");
     return nullptr;
   }
 
+  // Set the StatusCondition to none to prevent triggering via WaitSets
+  info->response_writer_->get_statuscondition().set_enabled_statuses(
+    eprosima::fastdds::dds::StatusMask::none());
+
   // lambda to delete datawriter
   auto cleanup_datawriter = rcpputils::make_scope_exit(
     [publisher, info]() {
@@ -531,6 +540,8 @@ rmw_service_set_on_new_request_callback(
   rmw_event_callback_t callback,
   const void * user_data)
 {
+  RMW_CHECK_ARGUMENT_FOR_NULL(rmw_service, RMW_RET_INVALID_ARGUMENT);
+
   return rmw_fastrtps_shared_cpp::__rmw_service_set_on_new_request_callback(
     rmw_service,
     callback,

diff --git a/rmw_fastrtps_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_cpp/src/rmw_subscription.cpp
@@ -77,8 +77,7 @@ rmw_create_subscription(
     topic_name,
     qos_policies,
     subscription_options,
-    false,  // use no keyed topic
-    true);  // create subscription listener
+    false);  // use no keyed topic
   if (!subscription) {
     return nullptr;
   }
@@ -215,6 +214,8 @@ rmw_subscription_set_on_new_message_callback(
   rmw_event_callback_t callback,
   const void * user_data)
 {
+  RMW_CHECK_ARGUMENT_FOR_NULL(rmw_subscription, RMW_RET_INVALID_ARGUMENT);
+
   return rmw_fastrtps_shared_cpp::__rmw_subscription_set_on_new_message_callback(
     rmw_subscription,
     callback,

diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp
@@ -65,8 +65,7 @@ create_subscription(
   const char * topic_name,
   const rmw_qos_profile_t * qos_policies,
   const rmw_subscription_options_t * subscription_options,
-  bool keyed,
-  bool create_subscription_listener)
+  bool keyed)
 {
   /////
   // Check input parameters
@@ -164,7 +163,8 @@ create_subscription(
   }
 
   auto cleanup_info = rcpputils::make_scope_exit(
-    [info, dds_participant]() {
+    [info, dds_participant]()
+    {
       delete info->listener_;
       if (info->type_support_) {
         dds_participant->unregister_type(info->type_support_.get_type_name());
@@ -208,12 +208,10 @@ create_subscription(
 
   /////
   // Create Listener
-  if (create_subscription_listener) {
-    info->listener_ = new (std::nothrow) SubListener(info, qos_policies->depth);
-    if (!info->listener_) {
-      RMW_SET_ERROR_MSG("create_subscription() could not create subscription listener");
-      return nullptr;
-    }
+  info->listener_ = new (std::nothrow) SubListener(info);
+  if (!info->listener_) {
+    RMW_SET_ERROR_MSG("create_subscription() could not create subscription listener");
+    return nullptr;
   }
 
   /////
@@ -297,9 +295,14 @@ create_subscription(
     return nullptr;
   }
 
+  // Initialize DataReader's StatusCondition to be notified when new data is available
+  info->data_reader_->get_statuscondition().set_enabled_statuses(
+    eprosima::fastdds::dds::StatusMask::data_available());
+
   // lambda to delete datareader
   auto cleanup_datareader = rcpputils::make_scope_exit(
-    [subscriber, info]() {
+    [subscriber, info]()
+    {
       subscriber->delete_datareader(info->data_reader_);
     });
 
@@ -316,7 +319,8 @@ create_subscription(
     return nullptr;
   }
   auto cleanup_rmw_subscription = rcpputils::make_scope_exit(
-    [rmw_subscription]() {
+    [rmw_subscription]()
+    {
       rmw_free(const_cast<char *>(rmw_subscription->topic_name));
       rmw_subscription_free(rmw_subscription);
     });
@@ -345,4 +349,5 @@ create_subscription(
     info->subscription_gid_.data);
   return rmw_subscription;
 }
+
 }  // namespace rmw_fastrtps_cpp
diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/subscription.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/subscription.hpp
@@ -30,8 +30,7 @@ create_subscription(
   const char * topic_name,
   const rmw_qos_profile_t * qos_policies,
   const rmw_subscription_options_t * subscription_options,
-  bool keyed,
-  bool create_subscription_listener);
+  bool keyed);
 
 }  // namespace rmw_fastrtps_dynamic_cpp
 

diff --git a/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp
@@ -42,7 +42,8 @@ using rmw_dds_common::msg::ParticipantEntitiesInfo;
 
 static
 rmw_ret_t
-init_context_impl(rmw_context_t * context)
+init_context_impl(
+  rmw_context_t * context)
 {
   rmw_publisher_options_t publisher_options = rmw_get_default_publisher_options();
   rmw_subscription_options_t subscription_options = rmw_get_default_subscription_options();
@@ -65,7 +66,8 @@ init_context_impl(rmw_context_t * context)
       (context->options.localhost_only == RMW_LOCALHOST_ONLY_ENABLED) ? 1 : 0,
       context->options.enclave,
       common_context.get()),
-    [&](CustomParticipantInfo * participant_info) {
+    [&](CustomParticipantInfo * participant_info)
+    {
       if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_participant(participant_info)) {
         RCUTILS_SAFE_FWRITE_TO_STDERR(
           "Failed to destroy participant after function: '"
@@ -92,9 +94,10 @@ init_context_impl(rmw_context_t * context)
       "ros_discovery_info",
       &qos,
       &publisher_options,
-      false,  // our fastrtps typesupport doesn't support keyed topics
+      false,       // our fastrtps typesupport doesn't support keyed topics
       true),
-    [&](rmw_publisher_t * pub) {
+    [&](rmw_publisher_t * pub)
+    {
       if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_publisher(
         eprosima_fastrtps_identifier,
         participant_info.get(),
@@ -119,9 +122,9 @@ init_context_impl(rmw_context_t * context)
       "ros_discovery_info",
       &qos,
       &subscription_options,
-      false,  // our fastrtps typesupport doesn't support keyed topics
-      true),
-    [&](rmw_subscription_t * sub) {
+      false),       // our fastrtps typesupport doesn't support keyed topics
+    [&](rmw_subscription_t * sub)
+    {
       if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_subscription(
         eprosima_fastrtps_identifier,
         participant_info.get(),
@@ -139,7 +142,8 @@ init_context_impl(rmw_context_t * context)
   std::unique_ptr<rmw_guard_condition_t, std::function<void(rmw_guard_condition_t *)>>
   graph_guard_condition(
     rmw_fastrtps_shared_cpp::__rmw_create_guard_condition(eprosima_fastrtps_identifier),
-    [&](rmw_guard_condition_t * p) {
+    [&](rmw_guard_condition_t * p)
+    {
       rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition(p);
       if (ret != RMW_RET_OK) {
         RMW_SAFE_FWRITE_TO_STDERR(
@@ -166,7 +170,8 @@ init_context_impl(rmw_context_t * context)
   }
 
   common_context->graph_cache.set_on_change_callback(
-    [guard_condition = graph_guard_condition.get()]() {
+    [guard_condition = graph_guard_condition.get()]()
+    {
       rmw_fastrtps_shared_cpp::__rmw_trigger_guard_condition(
         eprosima_fastrtps_identifier,
         guard_condition);
@@ -185,7 +190,8 @@ init_context_impl(rmw_context_t * context)
 }
 
 rmw_ret_t
-rmw_fastrtps_dynamic_cpp::increment_context_impl_ref_count(rmw_context_t * context)
+rmw_fastrtps_dynamic_cpp::increment_context_impl_ref_count(
+  rmw_context_t * context)
 {
   assert(context);
   assert(context->impl);