Use rclcpp::guard_condition

Signed-off-by: Mauro Passerino <mpasserino@irobot.com>

rclcpp/CMakeLists.txt

@@ -41,6 +41,7 @@ set(${PROJECT_NAME}_SRCS
  src/rclcpp/clock.cpp
  src/rclcpp/context.cpp
  src/rclcpp/contexts/default_context.cpp
+ src/rclcpp/detail/add_guard_condition_to_rcl_wait_set.cpp
  src/rclcpp/detail/mutex_two_priorities.cpp
  src/rclcpp/detail/resolve_parameter_overrides.cpp
  src/rclcpp/detail/rmw_implementation_specific_payload.cpp

rclcpp/include/rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp

@@ -0,0 +1,39 @@
+// Copyright 2021 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.
+
+#ifndef RCLCPP__DETAIL__ADD_GUARD_CONDITION_TO_RCL_WAIT_SET_HPP_
+#define RCLCPP__DETAIL__ADD_GUARD_CONDITION_TO_RCL_WAIT_SET_HPP_
+
+#include "rclcpp/guard_condition.hpp"
+
+namespace rclcpp
+{
+namespace detail
+{
+
+/// Adds the guard condition to a waitset
+/**
+ * This function is thread-safe.
+ * \param[in] wait_set pointer to a wait set where to add the guard condition
+ */
+RCLCPP_PUBLIC
+void
+add_guard_condition_to_rcl_wait_set(
+ rcl_wait_set_t & wait_set,
+ const rclcpp::GuardCondition & guard_condition);
+
+} // namespace detail
+} // namespace rclcpp
+
+#endif // RCLCPP__DETAIL__ADD_GUARD_CONDITION_TO_RCL_WAIT_SET_HPP_

rclcpp/include/rclcpp/executor.hpp

@@ -525,7 +525,7 @@ class Executor
  std::atomic_bool spinning;
 
  /// Guard condition for signaling the rmw layer to wake up for special events.
- rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition();
+ rclcpp::GuardCondition interrupt_guard_condition_;
 
  std::shared_ptr<rclcpp::GuardCondition> shutdown_guard_condition_;
 
@@ -549,7 +549,7 @@ class Executor
  spin_once_impl(std::chrono::nanoseconds timeout);
 
  typedef std::map<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
- const rcl_guard_condition_t *,
+ const rclcpp::GuardCondition *,
  std::owner_less<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>>
  WeakNodesToGuardConditionsMap;
 

rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp

@@ -65,7 +65,7 @@ class StaticExecutorEntitiesCollector final
  init(
  rcl_wait_set_t * p_wait_set,
  rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy,
- rcl_guard_condition_t * executor_guard_condition);
+ rclcpp::GuardCondition & executor_guard_condition);
 
  /// Finalize StaticExecutorEntitiesCollector to clear resources
  RCLCPP_PUBLIC
@@ -330,7 +330,7 @@ class StaticExecutorEntitiesCollector final
  WeakCallbackGroupsToNodesMap weak_groups_to_nodes_associated_with_executor_;
 
  typedef std::map<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
- const rcl_guard_condition_t *,
+ const rclcpp::GuardCondition *,
  std::owner_less<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>>
  WeakNodesToGuardConditionsMap;
  WeakNodesToGuardConditionsMap weak_nodes_to_guard_conditions_;

rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp

@@ -67,7 +67,7 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase
  const std::string & topic_name,
  rmw_qos_profile_t qos_profile,
  rclcpp::IntraProcessBufferType buffer_type)
- : SubscriptionIntraProcessBase(topic_name, qos_profile),
+ : SubscriptionIntraProcessBase(context, topic_name, qos_profile),
  any_callback_(callback)
  {
  if (!std::is_same<MessageT, CallbackMessageT>::value) {
@@ -80,18 +80,6 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase
  qos_profile,
  allocator);
 
- // Create the guard condition.
- rcl_guard_condition_options_t guard_condition_options =
- rcl_guard_condition_get_default_options();
-
- gc_ = rcl_get_zero_initialized_guard_condition();
- rcl_ret_t ret = rcl_guard_condition_init(
- &gc_, context->get_rcl_context().get(), guard_condition_options);
-
- if (RCL_RET_OK != ret) {
- throw std::runtime_error("SubscriptionIntraProcess init error initializing guard condition");
- }
-
  TRACEPOINT(
  rclcpp_subscription_callback_added,
  static_cast<const void *>(this),
@@ -104,15 +92,7 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase
 #endif
  }
 
- ~SubscriptionIntraProcess()
- {
- if (rcl_guard_condition_fini(&gc_) != RCL_RET_OK) {
- RCUTILS_LOG_ERROR_NAMED(
- "rclcpp",
- "Failed to destroy guard condition: %s",
- rcutils_get_error_string().str);
- }
- }
+ ~SubscriptionIntraProcess() = default;
 
  bool
  is_ready(rcl_wait_set_t * wait_set)
@@ -182,8 +162,7 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase
  void
  trigger_guard_condition()
  {
- rcl_ret_t ret = rcl_trigger_guard_condition(&gc_);
- (void)ret;
+ gc_.trigger();
  }
 
  template<typename T>

rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp

@@ -26,6 +26,7 @@
 #include "rmw/impl/cpp/demangle.hpp"
 
 #include "rclcpp/logging.hpp"
+#include "rclcpp/guard_condition.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/waitable.hpp"
 
@@ -45,8 +46,11 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable
  };
 
  RCLCPP_PUBLIC
- SubscriptionIntraProcessBase(const std::string & topic_name, rmw_qos_profile_t qos_profile)
- : topic_name_(topic_name), qos_profile_(qos_profile)
+ SubscriptionIntraProcessBase(
+ rclcpp::Context::SharedPtr context,
+ const std::string & topic_name,
+ rmw_qos_profile_t qos_profile)
+ : gc_(context), topic_name_(topic_name), qos_profile_(qos_profile)
  {}
 
  virtual ~SubscriptionIntraProcessBase() = default;
@@ -163,7 +167,7 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable
 
 protected:
  std::recursive_mutex reentrant_mutex_;
- rcl_guard_condition_t gc_;
+ rclcpp::GuardCondition gc_;
 
  std::function<void(size_t)> on_new_message_callback_ {nullptr};
  size_t unread_count_{0};

rclcpp/include/rclcpp/graph_listener.hpp

@@ -24,6 +24,7 @@
 #include "rcl/guard_condition.h"
 #include "rcl/wait.h"
 #include "rclcpp/context.hpp"
+#include "rclcpp/guard_condition.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_graph_interface.hpp"
 #include "rclcpp/visibility_control.hpp"
@@ -187,7 +188,7 @@ class GraphListener : public std::enable_shared_from_this<GraphListener>
  mutable std::mutex node_graph_interfaces_mutex_;
  std::vector<rclcpp::node_interfaces::NodeGraphInterface *> node_graph_interfaces_;
 
- rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition();
+ rclcpp::GuardCondition interrupt_guard_condition_;
  rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set();
 };
 

rclcpp/include/rclcpp/guard_condition.hpp

@@ -47,7 +47,9 @@ class GuardCondition
  RCLCPP_PUBLIC
  explicit GuardCondition(
  rclcpp::Context::SharedPtr context =
- rclcpp::contexts::get_global_default_context());
+ rclcpp::contexts::get_global_default_context(),
+ rcl_guard_condition_options_t guard_condition_options =
+ rcl_guard_condition_get_default_options());
 
  RCLCPP_PUBLIC
  virtual
@@ -59,6 +61,10 @@ class GuardCondition
  get_context() const;
 
  /// Return the underlying rcl guard condition structure.
+ RCLCPP_PUBLIC
+ rcl_guard_condition_t &
+ get_rcl_guard_condition();
+
  RCLCPP_PUBLIC
  const rcl_guard_condition_t &
  get_rcl_guard_condition() const;

rclcpp/include/rclcpp/memory_strategy.hpp

@@ -64,9 +64,17 @@ class RCLCPP_PUBLIC MemoryStrategy
  virtual void clear_handles() = 0;
  virtual void remove_null_handles(rcl_wait_set_t * wait_set) = 0;
 
- virtual void add_guard_condition(const rcl_guard_condition_t * guard_condition) = 0;
+ virtual void
+ add_guard_condition(const rclcpp::GuardCondition & guard_condition) = 0;
+
+ virtual void
+ add_guard_condition(const rclcpp::GuardCondition::SharedPtr guard_condition) = 0;
 
- virtual void remove_guard_condition(const rcl_guard_condition_t * guard_condition) = 0;
+ virtual void
+ remove_guard_condition(const rclcpp::GuardCondition & guard_condition) = 0;
+
+ virtual void
+ remove_guard_condition(const rclcpp::GuardCondition::SharedPtr guard_condition) = 0;
 
  virtual void
  get_next_subscription(

rclcpp/include/rclcpp/node_interfaces/node_base.hpp

@@ -104,13 +104,9 @@ class NodeBase : public NodeBaseInterface
  get_associated_with_executor_atomic() override;
 
  RCLCPP_PUBLIC
- rcl_guard_condition_t *
+ rclcpp::GuardCondition &
  get_notify_guard_condition() override;
 
- RCLCPP_PUBLIC
- std::unique_lock<std::recursive_mutex>
- acquire_notify_guard_condition_lock() const override;
-
  RCLCPP_PUBLIC
  bool
  get_use_intra_process_default() const override;
@@ -122,6 +118,10 @@ class NodeBase : public NodeBaseInterface
  resolve_topic_or_service_name(
  const std::string & name, bool is_service, bool only_expand = false) const override;
 
+ RCLCPP_PUBLIC
+ void
+ trigger_notify_guard_condition() override;
+
 private:
  RCLCPP_DISABLE_COPY(NodeBase)
 
@@ -138,7 +138,7 @@ class NodeBase : public NodeBaseInterface
 
  /// Guard condition for notifying the Executor of changes to this node.
  mutable std::recursive_mutex notify_guard_condition_mutex_;
- rcl_guard_condition_t notify_guard_condition_ = rcl_get_zero_initialized_guard_condition();
+ rclcpp::GuardCondition notify_guard_condition_;
  bool notify_guard_condition_is_valid_;
 };
 

rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp

@@ -24,6 +24,7 @@
 
 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/context.hpp"
+#include "rclcpp/guard_condition.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -134,24 +135,17 @@ class NodeBaseInterface
  std::atomic_bool &
  get_associated_with_executor_atomic() = 0;
 
- /// Return guard condition that should be notified when the internal node state changes.
+ /// Return a guard condition that should be notified when the internal node state changes.
  /**
  * For example, this should be notified when a publisher is added or removed.
  *
- * \return the rcl_guard_condition_t if it is valid, else nullptr
+ * \return the GuardCondition if it is valid, else thow runtime error
  */
  RCLCPP_PUBLIC
  virtual
- rcl_guard_condition_t *
+ rclcpp::GuardCondition &
  get_notify_guard_condition() = 0;
 
- /// Acquire and return a scoped lock that protects the notify guard condition.
- /** This should be used when triggering the notify guard condition. */
- RCLCPP_PUBLIC
- virtual
- std::unique_lock<std::recursive_mutex>
- acquire_notify_guard_condition_lock() const = 0;
-
  /// Return the default preference for using intra process communication.
  RCLCPP_PUBLIC
  virtual
@@ -170,6 +164,12 @@ class NodeBaseInterface
  std::string
  resolve_topic_or_service_name(
  const std::string & name, bool is_service, bool only_expand = false) const = 0;
+
+ /// Trigger the node's notify guard condition.
+ RCLCPP_PUBLIC
+ virtual
+ void
+ trigger_notify_guard_condition() = 0;
 };
 
 } // namespace node_interfaces