new semaphore strategy in rmw_wait (#11)

* new semaphore strategy in rmw_wait

Signed-off-by: Poehnl Michael (CC-AD/ESW1) <[email protected]>

* apply latest uncrustify

Signed-off-by: Karsten Knese <[email protected]>

Co-authored-by: Karsten Knese <[email protected]>

iceoryx_ros2_bridge/src/generic_subscription.cpp

@@ -106,16 +106,17 @@ GenericSubscription::borrow_serialized_message(size_t capacity)
     rclcpp::exceptions::throw_from_rcl_error(init_return);
   }
 
-  auto serialized_msg = std::shared_ptr<rmw_serialized_message_t>(message,
-      [](rmw_serialized_message_t * msg) {
-        auto fini_return = rmw_serialized_message_fini(msg);
-        delete msg;
-        if (fini_return != RCL_RET_OK) {
-          RCLCPP_ERROR_STREAM(
-            rclcpp::get_logger("iceoryx_ros2_bridge"),
-            "Failed to destroy serialized message: " << rcl_get_error_string().str);
-        }
-      });
+  auto serialized_msg = std::shared_ptr<rmw_serialized_message_t>(
+    message,
+    [](rmw_serialized_message_t * msg) {
+      auto fini_return = rmw_serialized_message_fini(msg);
+      delete msg;
+      if (fini_return != RCL_RET_OK) {
+        RCLCPP_ERROR_STREAM(
+          rclcpp::get_logger("iceoryx_ros2_bridge"),
+          "Failed to destroy serialized message: " << rcl_get_error_string().str);
+      }
+    });
 
   return serialized_msg;
 }

iceoryx_ros2_bridge/src/iceoryx_ros2_bridge.cpp

@@ -231,7 +231,8 @@ int main(int argc, char ** argv)
     std::function<void(std::shared_ptr<rmw_serialized_message_t>)> cb =
       std::bind(&publish_to_iceoryx, std::placeholders::_1, ts, iceoryx_pubs.back());
 
-    ros2_subs.emplace_back(std::make_shared<iceoryx_ros2_bridge::GenericSubscription>(
+    ros2_subs.emplace_back(
+      std::make_shared<iceoryx_ros2_bridge::GenericSubscription>(
         node->get_node_base_interface().get(), *ts, topic, cb));
     node->get_node_topics_interface()->add_subscription(ros2_subs.back(), nullptr);
   }
@@ -261,7 +262,8 @@ int main(int argc, char ** argv)
 
     auto ts = iceoryx_ros2_bridge::get_typesupport(type, "rosidl_typesupport_cpp");
 
-    ros2_pubs.emplace_back(std::make_shared<iceoryx_ros2_bridge::GenericPublisher>(
+    ros2_pubs.emplace_back(
+      std::make_shared<iceoryx_ros2_bridge::GenericPublisher>(
         node->get_node_base_interface().get(), topic, *ts));
 
     auto service_desc =

rmw_iceoryx_cpp/src/internal/iceoryx_deserialize.cpp

@@ -203,8 +203,9 @@ const char * copy_payload_cpp(
     serialized_msg += size;
   } else {
     if (member->array_size_ > 0 && !member->is_upper_bound_) {
-      serialized_msg = copy_payload_fixed_array_cpp<T>(serialized_msg, ros_message_field,
-          member->array_size_);
+      serialized_msg = copy_payload_fixed_array_cpp<T>(
+        serialized_msg, ros_message_field,
+        member->array_size_);
     } else {
       serialized_msg = copy_payload_array_cpp<T>(serialized_msg, ros_message_field);
     }
@@ -271,8 +272,9 @@ const char * copy_payload_c(
     serialized_msg += size;
   } else {
     if (member->array_size_ > 0 && !member->is_upper_bound_) {
-      serialized_msg = copy_payload_fixed_array_cpp<T>(serialized_msg, ros_message_field,
-          member->array_size_);
+      serialized_msg = copy_payload_fixed_array_cpp<T>(
+        serialized_msg, ros_message_field,
+        member->array_size_);
     } else {
       serialized_msg = copy_payload_array_c<T>(serialized_msg, ros_message_field);
     }

rmw_iceoryx_cpp/src/internal/iceoryx_name_conversion.cpp

@@ -103,8 +103,9 @@ get_name_n_type_from_iceoryx_service_description(
     // ARA Naming
     std::string delimiter_msg = "_ara_msgs/msg/";
 
-    return std::make_tuple("/" + instance + "/" + service + "/" + event,
-             service + delimiter_msg + event);
+    return std::make_tuple(
+      "/" + instance + "/" + service + "/" + event,
+      service + delimiter_msg + event);
   }
 }
 
@@ -119,9 +120,10 @@ get_iceoryx_service_description(
   type_name = package_name + "/" + type_name;
 
   auto serviceDescriptionTuple = get_service_description_elements(topic_name, type_name);
-  return iox::capro::ServiceDescription(std::get<0>(serviceDescriptionTuple),
-           std::get<1>(serviceDescriptionTuple),
-           std::get<2>(serviceDescriptionTuple));
+  return iox::capro::ServiceDescription(
+    std::get<0>(serviceDescriptionTuple),
+    std::get<1>(serviceDescriptionTuple),
+    std::get<2>(serviceDescriptionTuple));
 }
 
 }  // namespace rmw_iceoryx_cpp

rmw_iceoryx_cpp/src/internal/iceoryx_serialize.cpp

@@ -242,9 +242,10 @@ void copy_data_c_ros_message(
   } else {
     void * subros_message = nullptr;
     size_t sub_members_size = sub_members->size_of_;
-    size_t array_elememts = get_array_elememts_and_assign_ros_message_field_c(member,
-        ros_message_field,
-        subros_message);
+    size_t array_elememts = get_array_elememts_and_assign_ros_message_field_c(
+      member,
+      ros_message_field,
+      subros_message);
 
     store_array_size(payloadVector, array_elememts);
 
@@ -268,9 +269,10 @@ void copy_data_cpp_ros_message(
     size_t array_elements;
     size_t sub_members_size = sub_members->size_of_;
 
-    array_elements = get_array_elememts_and_assign_ros_message_field_cpp(member, ros_message_field,
-        subros_message,
-        sub_members_size);
+    array_elements = get_array_elememts_and_assign_ros_message_field_cpp(
+      member, ros_message_field,
+      subros_message,
+      sub_members_size);
     store_array_size(payloadVector, array_elements);
     for (size_t index = 0; index < array_elements; ++index) {
       serialize(subros_message, sub_members, payloadVector);

rmw_iceoryx_cpp/src/internal/iceoryx_topic_names_and_types.cpp

@@ -81,7 +81,8 @@ void fill_topic_containers(
           std::string(receiver.m_caproEventMethodID.to_cstring()));
 
         names_n_types[std::get<0>(name_and_type)] = std::get<1>(name_and_type);
-        subscribers_topics[std::string(receiver.m_runnable.to_cstring())].push_back(std::get<0>(
+        subscribers_topics[std::string(receiver.m_runnable.to_cstring())].push_back(
+          std::get<0>(
             name_and_type));
       }
       for (auto & sender : port_sample->m_senderList) {
@@ -91,7 +92,8 @@ void fill_topic_containers(
           std::string(sender.m_caproEventMethodID.to_cstring()));
 
         names_n_types[std::get<0>(name_and_type)] = std::get<1>(name_and_type);
-        publishers_topics[std::string(sender.m_runnable.to_cstring())].push_back(std::get<0>(
+        publishers_topics[std::string(sender.m_runnable.to_cstring())].push_back(
+          std::get<0>(
             name_and_type));
       }
       port_receiver.releaseChunk(latest_chunk_header);
@@ -186,8 +188,9 @@ rmw_ret_t fill_rmw_names_and_types(
   rmw_ret_t rmw_ret = RMW_RET_ERROR;
 
   if (!iceoryx_topic_names_and_types.empty()) {
-    rmw_ret = rmw_names_and_types_init(rmw_topic_names_and_types,
-        iceoryx_topic_names_and_types.size(), allocator);
+    rmw_ret = rmw_names_and_types_init(
+      rmw_topic_names_and_types,
+      iceoryx_topic_names_and_types.size(), allocator);
     if (rmw_ret != RMW_RET_OK) {
       return rmw_ret;
     }

rmw_iceoryx_cpp/src/rmw_guard_condition.cpp

@@ -29,7 +29,8 @@ rmw_create_guard_condition(rmw_context_t * context)
 {
   RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, nullptr);
 
-  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(rmw_create_guard_condition
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    rmw_create_guard_condition
     : context,
     context->implementation_identifier,
     rmw_get_implementation_identifier(),
@@ -80,7 +81,8 @@ rmw_destroy_guard_condition(rmw_guard_condition_t * guard_condition)
 {
   RCUTILS_CHECK_ARGUMENT_FOR_NULL(guard_condition, RMW_RET_ERROR);
 
-  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(rmw_destroy_guard_condition
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    rmw_destroy_guard_condition
     : guard_condition,
     guard_condition->implementation_identifier,
     rmw_get_implementation_identifier(),

rmw_iceoryx_cpp/src/rmw_init.cpp

@@ -48,7 +48,8 @@ rmw_init_options_copy(const rmw_init_options_t * src, rmw_init_options_t * dst)
 {
   RMW_CHECK_ARGUMENT_FOR_NULL(src, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(dst, RMW_RET_INVALID_ARGUMENT);
-  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(rmw_init_options_copy
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    rmw_init_options_copy
     : source options,
     src->implementation_identifier,
     rmw_get_implementation_identifier(),
@@ -67,7 +68,8 @@ rmw_init_options_fini(rmw_init_options_t * init_options)
 {
   RCUTILS_CHECK_ARGUMENT_FOR_NULL(init_options, RMW_RET_INVALID_ARGUMENT);
   RCUTILS_CHECK_ALLOCATOR(&(init_options->allocator), return RMW_RET_INVALID_ARGUMENT);
-  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(rmw_init_options_fini
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    rmw_init_options_fini
     : options,
     init_options->implementation_identifier,
     rmw_get_implementation_identifier(),
@@ -81,7 +83,8 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context)
 {
   RCUTILS_CHECK_ARGUMENT_FOR_NULL(options, RMW_RET_INVALID_ARGUMENT);
   RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT);
-  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(rmw_init
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    rmw_init
     : options,
     options->implementation_identifier,
     rmw_get_implementation_identifier(),
@@ -110,7 +113,8 @@ rmw_ret_t
 rmw_shutdown(rmw_context_t * context)
 {
   RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT);
-  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(rmw_shutdown
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    rmw_shutdown
     : context,
     context->implementation_identifier,
     rmw_get_implementation_identifier(),
@@ -123,7 +127,8 @@ rmw_ret_t
 rmw_context_fini(rmw_context_t * context)
 {
   RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT);
-  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(rmw_context_fini
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    rmw_context_fini
     : context,
     context->implementation_identifier,
     rmw_get_implementation_identifier(),

rmw_iceoryx_cpp/src/rmw_node.cpp

@@ -72,7 +72,8 @@ rmw_create_node(
     RMW_SET_ERROR_MSG("failed to allocate memory for graph change notifier");
     goto fail;
   }
-  RMW_TRY_PLACEMENT_NEW(graph_change_notifier, graph_change_notifier, goto fail,
+  RMW_TRY_PLACEMENT_NEW(
+    graph_change_notifier, graph_change_notifier, goto fail,
     IceoryxGraphChangeNotifier, guard_condition)
 
   // allocate iceoryx_runnable
@@ -92,7 +93,8 @@ rmw_create_node(
     RMW_SET_ERROR_MSG("failed to allocate memory for node info");
     goto fail;
   }
-  RMW_TRY_PLACEMENT_NEW(node_info, node_info, goto fail, IceoryxNodeInfo, guard_condition,
+  RMW_TRY_PLACEMENT_NEW(
+    node_info, node_info, goto fail, IceoryxNodeInfo, guard_condition,
     graph_change_notifier, iceoryx_runnable)
 
   node_handle->data = node_info;
@@ -160,7 +162,8 @@ rmw_destroy_node(rmw_node_t * node)
   IceoryxNodeInfo * node_info = static_cast<IceoryxNodeInfo *>(node->data);
   if (node_info) {
     if (node_info->graph_change_notifier_) {
-      RMW_TRY_DESTRUCTOR(node_info->graph_change_notifier_->~IceoryxGraphChangeNotifier(),
+      RMW_TRY_DESTRUCTOR(
+        node_info->graph_change_notifier_->~IceoryxGraphChangeNotifier(),
         node_info->graph_change_notifier_,
         result = RMW_RET_ERROR)
       rmw_free(node_info->graph_change_notifier_);
@@ -170,12 +173,14 @@ rmw_destroy_node(rmw_node_t * node)
       result = RMW_RET_ERROR;
     }
     if (node_info->iceoryx_runnable_) {
-      RMW_TRY_DESTRUCTOR(node_info->iceoryx_runnable_->~Runnable(),
+      RMW_TRY_DESTRUCTOR(
+        node_info->iceoryx_runnable_->~Runnable(),
         node_info->iceoryx_runnable_,
         result = RMW_RET_ERROR)
       rmw_free(node_info->iceoryx_runnable_);
     }
-    RMW_TRY_DESTRUCTOR(node_info->~IceoryxNodeInfo(),
+    RMW_TRY_DESTRUCTOR(
+      node_info->~IceoryxNodeInfo(),
       node_info_,
       result = RMW_RET_ERROR)
     rmw_free(node_info);

rmw_iceoryx_cpp/src/rmw_publisher.cpp

@@ -183,12 +183,14 @@ rmw_destroy_publisher(rmw_node_t * node, rmw_publisher_t * publisher)
   IceoryxPublisher * iceoryx_publisher = static_cast<IceoryxPublisher *>(publisher->data);
   if (iceoryx_publisher) {
     if (iceoryx_publisher->iceoryx_sender_) {
-      RMW_TRY_DESTRUCTOR(iceoryx_publisher->iceoryx_sender_->~Publisher(),
+      RMW_TRY_DESTRUCTOR(
+        iceoryx_publisher->iceoryx_sender_->~Publisher(),
         iceoryx_publisher->iceoryx_sender_,
         result = RMW_RET_ERROR)
       rmw_free(iceoryx_publisher->iceoryx_sender_);
     }
-    RMW_TRY_DESTRUCTOR(iceoryx_publisher->~IceoryxPublisher(),
+    RMW_TRY_DESTRUCTOR(
+      iceoryx_publisher->~IceoryxPublisher(),
       iceoryx_publisher,
       result = RMW_RET_ERROR)
     rmw_free(iceoryx_publisher);

rmw_iceoryx_cpp/src/rmw_subscription.cpp

@@ -175,12 +175,14 @@ rmw_destroy_subscription(
     static_cast<IceoryxSubscription *>(subscription->data);
   if (iceoryx_subscription) {
     if (iceoryx_subscription->iceoryx_receiver_) {
-      RMW_TRY_DESTRUCTOR(iceoryx_subscription->iceoryx_receiver_->~Subscriber(),
+      RMW_TRY_DESTRUCTOR(
+        iceoryx_subscription->iceoryx_receiver_->~Subscriber(),
         iceoryx_subscription->iceoryx_receiver_,
         result = RMW_RET_ERROR)
       rmw_free(iceoryx_subscription->iceoryx_receiver_);
     }
-    RMW_TRY_DESTRUCTOR(iceoryx_subscription->~IceoryxSubscription(),
+    RMW_TRY_DESTRUCTOR(
+      iceoryx_subscription->~IceoryxSubscription(),
       iceoryx_subscription,
       result = RMW_RET_ERROR)
     rmw_free(iceoryx_subscription);

rmw_iceoryx_cpp/src/rmw_take.cpp

@@ -46,7 +46,8 @@ rmw_take(
   RCUTILS_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_ERROR);
   (void)allocation;
 
-  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(rmw_take
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    rmw_take
     : subscription,
     subscription->implementation_identifier,
     rmw_get_implementation_identifier(),
@@ -185,7 +186,8 @@ rmw_take_loaned_message(
 
   *taken = false;
 
-  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(rmw_take
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    rmw_take
     : subscription,
     subscription->implementation_identifier,
     rmw_get_implementation_identifier(),
@@ -243,7 +245,8 @@ rmw_return_loaned_message_from_subscription(
   RCUTILS_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_ERROR);
   RCUTILS_CHECK_ARGUMENT_FOR_NULL(loaned_message, RMW_RET_ERROR);
 
-  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(rmw_take
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    rmw_take
     : subscription,
     subscription->implementation_identifier,
     rmw_get_implementation_identifier(),

rmw_iceoryx_cpp/src/rmw_trigger_guard_condition.cpp

@@ -26,7 +26,8 @@ rmw_trigger_guard_condition(const rmw_guard_condition_t * guard_condition_handle
 {
   RCUTILS_CHECK_ARGUMENT_FOR_NULL(guard_condition_handle, RMW_RET_ERROR);
 
-  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(rmw_trigger_guard_condition
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    rmw_trigger_guard_condition
     : guard_condition_handle,
     guard_condition_handle->implementation_identifier,
     rmw_get_implementation_identifier(),