add tests for waitables, fix const-ness in some of the APIs

wjwwood · wjwwood · commit bf03d2f5e306 · 2024-03-27T13:18:45.000-07:00
Signed-off-by: William Woodall &lt;william@osrfoundation.org&gt;
diff --git a/rclcpp/include/rclcpp/event_handler.hpp b/rclcpp/include/rclcpp/event_handler.hpp
@@ -122,7 +122,7 @@ class EventHandlerBase : public Waitable
   /// Check if the Waitable is ready.
   RCLCPP_PUBLIC
   bool
-  is_ready(rcl_wait_set_t * wait_set) override;
+  is_ready(const rcl_wait_set_t * wait_set) override;
 
   /// Set a callback to be called when each new event instance occurs.
   /**
@@ -294,7 +294,7 @@ class EventHandler : public EventHandlerBase
 
   /// Execute any entities of the Waitable that are ready.
   void
-  execute(std::shared_ptr<void> & data) override
+  execute(const std::shared_ptr<void> & data) override
   {
     if (!data) {
       throw std::runtime_error("'data' is empty");
diff --git a/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp b/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp
@@ -68,7 +68,7 @@ class ExecutorNotifyWaitable : public rclcpp::Waitable
    */
   RCLCPP_PUBLIC
   bool
-  is_ready(rcl_wait_set_t * wait_set) override;
+  is_ready(const rcl_wait_set_t * wait_set) override;
 
   /// Perform work associated with the waitable.
   /**
@@ -77,7 +77,7 @@ class ExecutorNotifyWaitable : public rclcpp::Waitable
    */
   RCLCPP_PUBLIC
   void
-  execute(std::shared_ptr<void> & data) override;
+  execute(const std::shared_ptr<void> & data) override;
 
   /// Retrieve data to be used in the next execute call.
   /**
diff --git a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp b/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp
@@ -77,7 +77,7 @@ class StaticExecutorEntitiesCollector final
   /// Execute the waitable.
   RCLCPP_PUBLIC
   void
-  execute(std::shared_ptr<void> & data) override;
+  execute(const std::shared_ptr<void> & data) override;
 
   /// Take the data so that it can be consumed with `execute`.
   /**
@@ -196,7 +196,7 @@ class StaticExecutorEntitiesCollector final
    */
   RCLCPP_PUBLIC
   bool
-  is_ready(rcl_wait_set_t * wait_set) override;
+  is_ready(const rcl_wait_set_t * wait_set) override;
 
   /// Return number of timers
   /**
diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp
@@ -150,25 +150,25 @@ class SubscriptionIntraProcess
     );
   }
 
-  void execute(std::shared_ptr<void> & data) override
+  void execute(const std::shared_ptr<void> & data) override
   {
     execute_impl<SubscribedType>(data);
   }
 
 protected:
   template<typename T>
   typename std::enable_if<std::is_same<T, rcl_serialized_message_t>::value, void>::type
-  execute_impl(std::shared_ptr<void> & data)
+  execute_impl(const std::shared_ptr<void> & data)
   {
     (void)data;
     throw std::runtime_error("Subscription intra-process can't handle serialized messages");
   }
 
   template<class T>
   typename std::enable_if<!std::is_same<T, rcl_serialized_message_t>::value, void>::type
-  execute_impl(std::shared_ptr<void> & data)
+  execute_impl(const std::shared_ptr<void> & data)
   {
-    if (!data) {
+    if (nullptr == data) {
       return;
     }
 
diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp
@@ -72,7 +72,7 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable
   is_durability_transient_local() const;
 
   bool
-  is_ready(rcl_wait_set_t * wait_set) override = 0;
+  is_ready(const rcl_wait_set_t * wait_set) override = 0;
 
   std::shared_ptr<void>
   take_data() override = 0;
@@ -85,7 +85,7 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable
   }
 
   void
-  execute(std::shared_ptr<void> & data) override = 0;
+  execute(const std::shared_ptr<void> & data) override = 0;
 
   virtual
   bool
diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp
@@ -100,7 +100,7 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff
   }
 
   bool
-  is_ready(rcl_wait_set_t * wait_set) override
+  is_ready(const rcl_wait_set_t * wait_set) override
   {
     (void) wait_set;
     return buffer_->has_data();
diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp
@@ -124,7 +124,7 @@ class Waitable
   RCLCPP_PUBLIC
   virtual
   bool
-  is_ready(rcl_wait_set_t * wait_set) = 0;
+  is_ready(const rcl_wait_set_t * wait_set) = 0;
 
   /// Take the data so that it can be consumed with `execute`.
   /**
@@ -203,7 +203,7 @@ class Waitable
   RCLCPP_PUBLIC
   virtual
   void
-  execute(std::shared_ptr<void> & data) = 0;
+  execute(const std::shared_ptr<void> & data) = 0;
 
   /// Exchange the "in use by wait set" state for this timer.
   /**
diff --git a/rclcpp/src/rclcpp/event_handler.cpp b/rclcpp/src/rclcpp/event_handler.cpp
@@ -67,7 +67,7 @@ EventHandlerBase::add_to_wait_set(rcl_wait_set_t * wait_set)
 
 /// Check if the Waitable is ready.
 bool
-EventHandlerBase::is_ready(rcl_wait_set_t * wait_set)
+EventHandlerBase::is_ready(const rcl_wait_set_t * wait_set)
 {
   return wait_set->events[wait_set_event_index_] == &event_handle_;
 }
diff --git a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp
@@ -70,7 +70,7 @@ ExecutorNotifyWaitable::add_to_wait_set(rcl_wait_set_t * wait_set)
 }
 
 bool
-ExecutorNotifyWaitable::is_ready(rcl_wait_set_t * wait_set)
+ExecutorNotifyWaitable::is_ready(const rcl_wait_set_t * wait_set)
 {
   std::lock_guard<std::mutex> lock(guard_condition_mutex_);
 
@@ -92,7 +92,7 @@ ExecutorNotifyWaitable::is_ready(rcl_wait_set_t * wait_set)
 }
 
 void
-ExecutorNotifyWaitable::execute(std::shared_ptr<void> & data)
+ExecutorNotifyWaitable::execute(const std::shared_ptr<void> & data)
 {
   (void) data;
   this->execute_callback_();
diff --git a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp
@@ -96,7 +96,7 @@ StaticExecutorEntitiesCollector::take_data()
 }
 
 void
-StaticExecutorEntitiesCollector::execute(std::shared_ptr<void> & data)
+StaticExecutorEntitiesCollector::execute(const std::shared_ptr<void> & data)
 {
   (void) data;
   // Fill memory strategy with entities coming from weak_nodes_
@@ -434,7 +434,7 @@ StaticExecutorEntitiesCollector::remove_node(
 }
 
 bool
-StaticExecutorEntitiesCollector::is_ready(rcl_wait_set_t * p_wait_set)
+StaticExecutorEntitiesCollector::is_ready(const rcl_wait_set_t * p_wait_set)
 {
   // Check wait_set guard_conditions for added/removed entities to/from a node
   for (size_t i = 0; i < p_wait_set->size_of_guard_conditions; ++i) {
diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt
@@ -572,6 +572,11 @@ if(TARGET test_thread_safe_synchronization)
   target_link_libraries(test_thread_safe_synchronization ${PROJECT_NAME} ${test_msgs_TARGETS})
 endif()
 
+ament_add_gtest(test_intra_process_waitable waitables/test_intra_process_waitable.cpp)
+if(TARGET test_intra_process_waitable)
+  target_link_libraries(test_intra_process_waitable ${PROJECT_NAME} ${test_msgs_TARGETS})
+endif()
+
 ament_add_gtest(test_rosout_qos test_rosout_qos.cpp)
 if(TARGET test_rosout_qos)
   target_link_libraries(test_rosout_qos ${PROJECT_NAME} rcl::rcl rmw::rmw)
diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp
@@ -350,7 +350,7 @@ class TestWaitable : public rclcpp::Waitable
   }
 
   bool
-  is_ready(rcl_wait_set_t * wait_set) override
+  is_ready(const rcl_wait_set_t * wait_set) override
   {
     for (size_t i = 0; i < wait_set->size_of_guard_conditions; ++i) {
       auto rcl_guard_condition = wait_set->guard_conditions[i];
@@ -375,7 +375,7 @@ class TestWaitable : public rclcpp::Waitable
   }
 
   void
-  execute(std::shared_ptr<void> & data) override
+  execute(const std::shared_ptr<void> & data) override
   {
     (void) data;
     count_++;
diff --git a/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp b/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp
@@ -232,15 +232,15 @@ class TestWaitable : public rclcpp::Waitable
 public:
   void add_to_wait_set(rcl_wait_set_t *) override {}
 
-  bool is_ready(rcl_wait_set_t *) override {return true;}
+  bool is_ready(const rcl_wait_set_t *) override {return true;}
 
   std::shared_ptr<void>
   take_data() override
   {
     return nullptr;
   }
   void
-  execute(std::shared_ptr<void> & data) override
+  execute(const std::shared_ptr<void> & data) override
   {
     (void) data;
   }
diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp
@@ -29,15 +29,15 @@ class TestWaitable : public rclcpp::Waitable
 {
 public:
   void add_to_wait_set(rcl_wait_set_t *) override {}
-  bool is_ready(rcl_wait_set_t *) override {return false;}
+  bool is_ready(const rcl_wait_set_t *) override {return false;}
 
   std::shared_ptr<void>
   take_data() override
   {
     return nullptr;
   }
 
-  void execute(std::shared_ptr<void> & data) override
+  void execute(const std::shared_ptr<void> & data) override
   {
     (void) data;
   }
diff --git a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp
@@ -46,7 +46,7 @@ class TestWaitable : public rclcpp::Waitable
     }
   }
 
-  bool is_ready(rcl_wait_set_t *) override
+  bool is_ready(const rcl_wait_set_t *) override
   {
     return test_waitable_result;
   }
@@ -57,7 +57,7 @@ class TestWaitable : public rclcpp::Waitable
     return nullptr;
   }
 
-  void execute(std::shared_ptr<void> & data) override
+  void execute(const std::shared_ptr<void> & data) override
   {
     (void) data;
   }
diff --git a/rclcpp/test/rclcpp/test_memory_strategy.cpp b/rclcpp/test/rclcpp/test_memory_strategy.cpp
@@ -36,9 +36,9 @@ class TestWaitable : public rclcpp::Waitable
 {
 public:
   void add_to_wait_set(rcl_wait_set_t *) override {}
-  bool is_ready(rcl_wait_set_t *) override {return true;}
+  bool is_ready(const rcl_wait_set_t *) override {return true;}
   std::shared_ptr<void> take_data() override {return nullptr;}
-  void execute(std::shared_ptr<void> & data) override {(void)data;}
+  void execute(const std::shared_ptr<void> & data) override {(void)data;}
 };
 
 class TestMemoryStrategy : public ::testing::Test
diff --git a/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp b/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp
@@ -53,12 +53,12 @@ class TestWaitable : public rclcpp::Waitable
 
   void add_to_wait_set(rcl_wait_set_t *) override {}
 
-  bool is_ready(rcl_wait_set_t *) override {return is_ready_;}
+  bool is_ready(const rcl_wait_set_t *) override {return is_ready_;}
 
   std::shared_ptr<void> take_data() override {return nullptr;}
 
   void
-  execute(std::shared_ptr<void> & data) override {(void)data;}
+  execute(const std::shared_ptr<void> & data) override {(void)data;}
 
   void set_is_ready(bool value) {is_ready_ = value;}
 
diff --git a/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp b/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp
@@ -53,12 +53,12 @@ class TestWaitable : public rclcpp::Waitable
 
   void add_to_wait_set(rcl_wait_set_t *) override {}
 
-  bool is_ready(rcl_wait_set_t *) override {return is_ready_;}
+  bool is_ready(const rcl_wait_set_t *) override {return is_ready_;}
 
   std::shared_ptr<void> take_data() override {return nullptr;}
 
   void
-  execute(std::shared_ptr<void> & data) override {(void)data;}
+  execute(const std::shared_ptr<void> & data) override {(void)data;}
 
   void set_is_ready(bool value) {is_ready_ = value;}
 
diff --git a/rclcpp/test/rclcpp/wait_set_policies/test_storage_policy_common.cpp b/rclcpp/test/rclcpp/wait_set_policies/test_storage_policy_common.cpp
@@ -58,12 +58,12 @@ class TestWaitable : public rclcpp::Waitable
     }
   }
 
-  bool is_ready(rcl_wait_set_t *) override {return is_ready_;}
+  bool is_ready(const rcl_wait_set_t *) override {return is_ready_;}
 
   std::shared_ptr<void> take_data() override {return nullptr;}
 
   void
-  execute(std::shared_ptr<void> & data) override {(void)data;}
+  execute(const std::shared_ptr<void> & data) override {(void)data;}
 
   void set_is_ready(bool value) {is_ready_ = value;}
 
diff --git a/rclcpp/test/rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp b/rclcpp/test/rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp
@@ -53,12 +53,12 @@ class TestWaitable : public rclcpp::Waitable
 
   void add_to_wait_set(rcl_wait_set_t *) override {}
 
-  bool is_ready(rcl_wait_set_t *) override {return is_ready_;}
+  bool is_ready(const rcl_wait_set_t *) override {return is_ready_;}
 
   std::shared_ptr<void> take_data() override {return nullptr;}
 
   void
-  execute(std::shared_ptr<void> & data) override {(void)data;}
+  execute(const std::shared_ptr<void> & data) override {(void)data;}
 
   void set_is_ready(bool value) {is_ready_ = value;}
 
diff --git a/rclcpp/test/rclcpp/waitables/test_intra_process_waitable.cpp b/rclcpp/test/rclcpp/waitables/test_intra_process_waitable.cpp
@@ -0,0 +1,46 @@
+// Copyright 2024 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.
+
+#include <gtest/gtest.h>
+
+#include "rclcpp/rclcpp.hpp"
+#include "test_msgs/msg/empty.hpp"
+
+#include "./waitable_test_helpers.hpp"
+
+class TestIntraProcessWaitable : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase() { rclcpp::init(0, nullptr); }
+  static void TearDownTestCase() { rclcpp::shutdown(); }
+};
+
+TEST_F(TestIntraProcessWaitable, test_that_waitable_stays_ready_after_second_wait) {
+  auto node = std::make_shared<rclcpp::Node>(
+    "test_node",
+    rclcpp::NodeOptions().use_intra_process_comms(true));
+
+  using test_msgs::msg::Empty;
+  auto sub = node->create_subscription<Empty>("test_topic", 10, [](const Empty &) {});
+  auto pub = node->create_publisher<Empty>("test_topic", 10);
+
+  auto make_sub_intra_process_waitable_ready = [pub]() {
+    pub->publish(Empty());
+  };
+
+  rclcpp::test::waitables::do_test_that_waitable_stays_ready_after_second_wait(
+    sub->get_intra_process_waitable(),
+    make_sub_intra_process_waitable_ready,
+    true /* expected_to_stay_ready */);
+}
diff --git a/rclcpp/test/rclcpp/waitables/waitable_test_helpers.hpp b/rclcpp/test/rclcpp/waitables/waitable_test_helpers.hpp

Original file line number	Diff line number	Diff line change
`@@ -150,25 +150,25 @@ class SubscriptionIntraProcess`
`150`	`150`	`);`
`151`	`151`	`}`
`152`	`152`
`153`		`- void execute(std::shared_ptr<void> & data) override`
	`153`	`+ void execute(const std::shared_ptr<void> & data) override`
`154`	`154`	`{`
`155`	`155`	`execute_impl<SubscribedType>(data);`
`156`	`156`	`}`
`157`	`157`
`158`	`158`	`protected:`
`159`	`159`	`template<typename T>`
`160`	`160`	`typename std::enable_if<std::is_same<T, rcl_serialized_message_t>::value, void>::type`
`161`		`- execute_impl(std::shared_ptr<void> & data)`
	`161`	`+ execute_impl(const std::shared_ptr<void> & data)`
`162`	`162`	`{`
`163`	`163`	`(void)data;`
`164`	`164`	`throw std::runtime_error("Subscription intra-process can't handle serialized messages");`
`165`	`165`	`}`
`166`	`166`
`167`	`167`	`template<class T>`
`168`	`168`	`typename std::enable_if<!std::is_same<T, rcl_serialized_message_t>::value, void>::type`
`169`		`- execute_impl(std::shared_ptr<void> & data)`
	`169`	`+ execute_impl(const std::shared_ptr<void> & data)`
`170`	`170`	`{`
`171`		`- if (!data) {`
	`171`	`+ if (nullptr == data) {`
`172`	`172`	`return;`
`173`	`173`	`}`
`174`	`174`
Original file line number	Diff line number	Diff line change
`@@ -100,7 +100,7 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff`
`100`	`100`	`}`
`101`	`101`
`102`	`102`	`bool`
`103`		`- is_ready(rcl_wait_set_t * wait_set) override`
	`103`	`+ is_ready(const rcl_wait_set_t * wait_set) override`
`104`	`104`	`{`
`105`	`105`	`(void) wait_set;`
`106`	`106`	`return buffer_->has_data();`
Original file line number	Diff line number	Diff line change
`@@ -67,7 +67,7 @@ EventHandlerBase::add_to_wait_set(rcl_wait_set_t * wait_set)`
`67`	`67`
`68`	`68`	`/// Check if the Waitable is ready.`
`69`	`69`	`bool`
`70`		`-EventHandlerBase::is_ready(rcl_wait_set_t * wait_set)`
	`70`	`+EventHandlerBase::is_ready(const rcl_wait_set_t * wait_set)`
`71`	`71`	`{`
`72`	`72`	`return wait_set->events[wait_set_event_index_] == &event_handle_;`
`73`	`73`	`}`
Original file line number	Diff line number	Diff line change
`@@ -70,7 +70,7 @@ ExecutorNotifyWaitable::add_to_wait_set(rcl_wait_set_t * wait_set)`
`70`	`70`	`}`
`71`	`71`
`72`	`72`	`bool`
`73`		`-ExecutorNotifyWaitable::is_ready(rcl_wait_set_t * wait_set)`
	`73`	`+ExecutorNotifyWaitable::is_ready(const rcl_wait_set_t * wait_set)`
`74`	`74`	`{`
`75`	`75`	`std::lock_guard<std::mutex> lock(guard_condition_mutex_);`
`76`	`76`
`@@ -92,7 +92,7 @@ ExecutorNotifyWaitable::is_ready(rcl_wait_set_t * wait_set)`
`92`	`92`	`}`
`93`	`93`
`94`	`94`	`void`
`95`		`-ExecutorNotifyWaitable::execute(std::shared_ptr<void> & data)`
	`95`	`+ExecutorNotifyWaitable::execute(const std::shared_ptr<void> & data)`
`96`	`96`	`{`
`97`	`97`	`(void) data;`
`98`	`98`	`this->execute_callback_();`
Original file line number	Diff line number	Diff line change
`@@ -96,7 +96,7 @@ StaticExecutorEntitiesCollector::take_data()`
`96`	`96`	`}`
`97`	`97`
`98`	`98`	`void`
`99`		`-StaticExecutorEntitiesCollector::execute(std::shared_ptr<void> & data)`
	`99`	`+StaticExecutorEntitiesCollector::execute(const std::shared_ptr<void> & data)`
`100`	`100`	`{`
`101`	`101`	`(void) data;`
`102`	`102`	`// Fill memory strategy with entities coming from weak_nodes_`
`@@ -434,7 +434,7 @@ StaticExecutorEntitiesCollector::remove_node(`
`434`	`434`	`}`
`435`	`435`
`436`	`436`	`bool`
`437`		`-StaticExecutorEntitiesCollector::is_ready(rcl_wait_set_t * p_wait_set)`
	`437`	`+StaticExecutorEntitiesCollector::is_ready(const rcl_wait_set_t * p_wait_set)`
`438`	`438`	`{`
`439`	`439`	`// Check wait_set guard_conditions for added/removed entities to/from a node`
`440`	`440`	`for (size_t i = 0; i < p_wait_set->size_of_guard_conditions; ++i) {`