Added rudimentary test

Signed-off-by: Oren Bell <oren.bell@wustl.edu>

rclcpp/include/rclcpp/experimental/executors/events_executor/priority_events_queue.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__SIMPLE_EVENTS_QUEUE_HPP_
-#define RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__SIMPLE_EVENTS_QUEUE_HPP_
+#ifndef RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__PRIORITY_EVENTS_QUEUE_HPP_
+#define RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__PRIORITY_EVENTS_QUEUE_HPP_
 
 #include <condition_variable>
 #include <functional>
@@ -30,11 +30,20 @@ namespace experimental
 namespace executors
 {
 
-struct PriorityEvent {
+struct PriorityEvent
+{
   int priority;
   rclcpp::experimental::executors::ExecutorEvent event;
 };
 
+struct ComparePriorities : public std::binary_function<PriorityEvent, PriorityEvent, bool>
+{
+  _GLIBCXX14_CONSTEXPR
+  bool
+  operator()(const PriorityEvent & __x, const PriorityEvent & __y) const
+  {return __x.priority < __y.priority;}
+};
+
 /**
  * @brief This class implements an EventsQueue as a simple wrapper around a std::priority_queue.
  * It does not perform any checks about the size of queue, which can grow
@@ -44,11 +53,13 @@ class PriorityEventsQueue : public EventsQueue
 {
 public:
   RCLCPP_PUBLIC
-  PriorityEventsQueue() {
+  PriorityEventsQueue()
+  {
     // Default callback to extract priority from event
     extract_priority_ = [](const rclcpp::experimental::executors::ExecutorEvent & event) {
-      return 0;
-    };
+        (void)(event);
+        return 0;
+      };
   }
 
   RCLCPP_PUBLIC
@@ -141,7 +152,9 @@ class PriorityEventsQueue : public EventsQueue
   // Callback to extract priority from event
   std::function<int(const rclcpp::experimental::executors::ExecutorEvent &)> extract_priority_;
   // The underlying queue implementation
-  std::priority_queue<rclcpp::experimental::executors::PriorityEvent> event_queue_;
+  std::priority_queue<rclcpp::experimental::executors::PriorityEvent,
+    std::vector<rclcpp::experimental::executors::PriorityEvent>,
+    ComparePriorities> event_queue_;
   // Mutex to protect read/write access to the queue
   mutable std::mutex mutex_;
   // Variable used to notify when an event is added to the queue
@@ -152,4 +165,4 @@ class PriorityEventsQueue : public EventsQueue
 }  // namespace experimental
 }  // namespace rclcpp
 
-#endif  // RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__SIMPLE_EVENTS_QUEUE_HPP_
+#endif  // RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__PRIORITY_EVENTS_QUEUE_HPP_

rclcpp/test/rclcpp/CMakeLists.txt

@@ -487,6 +487,13 @@ if(TARGET test_events_queue)
   target_link_libraries(test_events_queue ${PROJECT_NAME})
 endif()
 
+ament_add_gtest(test_priority_events_executor executors/test_priority_events_executor.cpp TIMEOUT 5)
+if(TARGET test_priority_events_executor)
+  ament_target_dependencies(test_priority_events_executor
+    "test_msgs")
+  target_link_libraries(test_priority_events_executor ${PROJECT_NAME})
+endif()
+
 ament_add_gtest(test_guard_condition test_guard_condition.cpp
   APPEND_LIBRARY_DIRS "${append_library_dirs}")
 if(TARGET test_guard_condition)

rclcpp/test/rclcpp/executors/test_priority_events_executor.cpp

@@ -0,0 +1,132 @@
+// Copyright 2023 Washington University in St. Louis.
+//
+// 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 <chrono>
+#include <memory>
+#include <string>
+
+#include "rclcpp/experimental/executors/events_executor/events_executor.hpp"
+#include "rclcpp/experimental/executors/events_executor/priority_events_queue.hpp"
+
+#include "test_msgs/srv/empty.hpp"
+#include "test_msgs/msg/empty.hpp"
+
+using namespace std::chrono_literals;
+
+using rclcpp::experimental::executors::EventsExecutor;
+using rclcpp::experimental::executors::PriorityEventsQueue;
+
+class TestPriorityEventsExecutor : public ::testing::Test
+{
+public:
+  void SetUp()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  void TearDown()
+  {
+    rclcpp::shutdown();
+  }
+};
+
+TEST_F(TestPriorityEventsExecutor, priority_subs)
+{
+  // rmw_connextdds doesn't support events-executor
+  if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) {
+    GTEST_SKIP();
+  }
+
+  // Create timer publishing to 3 subscriptions
+  auto node = std::make_shared<rclcpp::Node>("node");
+
+  int subscriptions_executed = 0;
+
+  bool msg_received_low = false;
+  auto sub_low = node->create_subscription<test_msgs::msg::Empty>(
+    "topic", rclcpp::SensorDataQoS(),
+    [&msg_received_low, &subscriptions_executed](test_msgs::msg::Empty::ConstSharedPtr msg)
+    {
+      (void)msg;
+      msg_received_low = true;
+      EXPECT_EQ(subscriptions_executed, 2);
+      subscriptions_executed++;
+    });
+
+  bool msg_received_medium = false;
+  auto sub_medium = node->create_subscription<test_msgs::msg::Empty>(
+    "topic", rclcpp::SensorDataQoS(),
+    [&msg_received_medium, &subscriptions_executed](test_msgs::msg::Empty::ConstSharedPtr msg)
+    {
+      (void)msg;
+      msg_received_medium = true;
+      EXPECT_EQ(subscriptions_executed, 1);
+      subscriptions_executed++;
+    });
+
+  bool msg_received_high = false;
+  auto sub_high = node->create_subscription<test_msgs::msg::Empty>(
+    "topic", rclcpp::SensorDataQoS(),
+    [&msg_received_high, &subscriptions_executed](test_msgs::msg::Empty::ConstSharedPtr msg)
+    {
+      (void)msg;
+      msg_received_high = true;
+      EXPECT_EQ(subscriptions_executed, 0);
+      subscriptions_executed++;
+    });
+
+  auto publisher = node->create_publisher<test_msgs::msg::Empty>("topic", rclcpp::SensorDataQoS());
+
+  // Create executor
+  auto extract_priority = [](const rclcpp::experimental::executors::ExecutorEvent & event) {
+      return event.type == rclcpp::experimental::executors::ExecutorEventType::TIMER_EVENT ?
+             0 : 1;
+    };
+  EventsExecutor executor(std::make_unique<PriorityEventsQueue>(extract_priority));
+  executor.add_node(node);
+
+
+  bool spin_exited = false;
+  std::thread spinner([&spin_exited, &executor, this]() {
+      executor.spin();
+      spin_exited = true;
+    });
+
+  auto msg = std::make_unique<test_msgs::msg::Empty>();
+  publisher->publish(std::move(msg));
+
+  // Wait some time for the subscription to receive the message
+  auto start = std::chrono::high_resolution_clock::now();
+  while (
+    !msg_received_low &&
+    !spin_exited &&
+    (std::chrono::high_resolution_clock::now() - start < 1s))
+  {
+    auto time = std::chrono::high_resolution_clock::now() - start;
+    auto time_msec = std::chrono::duration_cast<std::chrono::milliseconds>(time);
+    std::this_thread::sleep_for(25ms);
+  }
+
+  executor.cancel();
+  spinner.join();
+  executor.remove_node(node);
+
+  EXPECT_TRUE(msg_received_low);
+  EXPECT_TRUE(msg_received_medium);
+  EXPECT_TRUE(msg_received_high);
+  EXPECT_EQ(subscriptions_executed, 3);
+  EXPECT_TRUE(spin_exited);
+}