Update executor usage and throttled callback

Signed-off-by: methylDragon <[email protected]>

Author: methylDragon

fuse_core/CMakeLists.txt

@@ -1,8 +1,6 @@
 cmake_minimum_required(VERSION 3.5)
 project(fuse_core)
 
-set(CMAKE_BUILD_TYPE RelWithDebInfo)
-
 # Default to C++17
 if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 17)

fuse_core/include/fuse_core/async_motion_model.h

@@ -42,7 +42,6 @@
 
 #include <rclcpp/rclcpp.hpp>
 
-#include <atomic>
 #include <string>
 
 
@@ -177,7 +176,6 @@ class AsyncMotionModel : public MotionModel
   rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_;  //!< A single/multi-threaded spinner assigned to the local callback queue
   size_t executor_thread_count_;
   std::thread spinner_;  //!< Internal thread for spinning the executor
-  std::atomic<bool> spinning_;  //!< Flag for spinning the spin thread
 
   /**
    * @brief Constructor

fuse_core/include/fuse_core/async_sensor_model.h

@@ -179,7 +179,6 @@ class AsyncSensorModel : public SensorModel
   TransactionCallback transaction_callback_;  //!< The function to be executed every time a Transaction is "published"
   size_t executor_thread_count_;
   std::thread spinner_;  //!< Internal thread for spinning the executor
-  std::atomic<bool> spinning_;  //!< Flag for spinning the spin thread
 
   /**
    * @brief Constructor

fuse_core/include/fuse_core/throttled_callback.h

@@ -37,7 +37,6 @@
 #include <functional>
 #include <utility>
 
-#include <fuse_core/node_interfaces/node_interfaces.hpp>
 #include <rclcpp/clock.hpp>
 #include <rclcpp/duration.hpp>
 #include <rclcpp/time.hpp>
@@ -62,27 +61,17 @@ class ThrottledCallback
    * @param[in] keep_callback   The callback to call when kept, i.e. not dropped. Defaults to nullptr
    * @param[in] drop_callback   The callback to call when dropped because of the throttling. Defaults to nullptr
    * @param[in] throttle_period The throttling period duration in seconds. Defaults to 0.0, i.e. no throttling
-   * @param[in] use_wall_time   Whether to use wall time or not. Defaults to false
-   * @throws if the use_wall_time is false and no NodeInterfaces is passed in
+   * @param[in] clock           The clock to throttle against. Defaults to using RCL_SYSTEM_TIME
    */
   ThrottledCallback(Callback&& keep_callback = nullptr,  // NOLINT(whitespace/operators)
                     Callback&& drop_callback = nullptr,  // NOLINT(whitespace/operators)
                     const rclcpp::Duration& throttle_period = rclcpp::Duration(0,0),
-                    const bool use_wall_time = false,
-                    node_interfaces::NodeInterfaces<
-                      node_interfaces::Clock
-                    >::SharedPtr interfaces = nullptr)
+                    rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>())
     : keep_callback_(keep_callback)
     , drop_callback_(drop_callback)
     , throttle_period_(throttle_period)
-    , use_wall_time_(use_wall_time)
-    , interfaces_(interfaces)
-  {
-    if (!use_wall_time && !interfaces) {
-      throw std::runtime_error(
-        "A NodeInterfaces object with a bound Clock interface must be passed in to use ROS time!");
-    }
-  }
+    , clock_(clock)
+  {}
 
   /**
    * @brief Throttle period getter
@@ -95,13 +84,16 @@ class ThrottledCallback
   }
 
   /**
-   * @brief Use wall time flag getter
+   * @brief Set the clock to throttle against
    *
-   * @return True if using wall time, false otherwise
+   * @param[in] clock The clock to set
    */
-  bool getUseWallTime() const
+  void setClock(rclcpp::Clock::SharedPtr clock)
   {
-    return use_wall_time_;
+    clock_ = clock;
+
+    // Reset since we changed the clock
+    last_called_time_ = rclcpp::Time();
   }
 
   /**
@@ -114,17 +106,6 @@ class ThrottledCallback
     throttle_period_ = throttle_period;
   }
 
-  // TODO(CH3): REFACTOR THIS OUT!!!! Take a clock type param instead
-  /**
-   * @brief Use wall time flag setter
-   *
-   * @param[in] use_wall_time Whether to use rclcpp::WallTime or not
-   */
-  void setUseWallTime(const bool use_wall_time)
-  {
-    use_wall_time_ = use_wall_time;
-  }
-
   /**
    * @brief Keep callback setter
    *
@@ -169,12 +150,7 @@ class ThrottledCallback
     // (a) This is the first call, i.e. the last called time is still invalid because it has not been set yet
     // (b) The throttle period is zero, so we should always keep the callbacks
     // (c) The elpased time between now and the last called time is greater than the throttle period
-    rclcpp::Time now;
-    if (use_wall_time_) {
-      now = rclcpp::Clock(RCL_SYSTEM_TIME).now();
-    } else {
-      now = interfaces_.get_node_clock_interface()->get_clock()->now();
-    }
+    rclcpp::Time now = clock_->now();
 
     if ((last_called_time_.nanoseconds() == 0)
         || (throttle_period_.nanoseconds() == 0)
@@ -215,9 +191,8 @@ class ThrottledCallback
   Callback keep_callback_;         //!< The callback to call when kept, i.e. not dropped
   Callback drop_callback_;         //!< The callback to call when dropped because of throttling
   rclcpp::Duration throttle_period_;  //!< The throttling period duration in seconds
-  bool use_wall_time_;             //<! The flag to indicate whether to use wall time or not
+  rclcpp::Clock::SharedPtr clock_; //!< The clock to throttle against
   rclcpp::Time last_called_time_;  //!< The last time the keep callback was called
-  node_interfaces::NodeInterfaces<node_interfaces::Clock> interfaces_;  //!< Node interfaces to use
 };
 
 /**

fuse_core/src/async_motion_model.cpp

@@ -56,7 +56,7 @@ AsyncMotionModel::AsyncMotionModel(size_t thread_count) :
 
 AsyncMotionModel::~AsyncMotionModel()
 {
-  spinning_ = false;
+  executor_->cancel();
   if (spinner_.joinable()) {
     spinner_.join();
   }
@@ -107,12 +107,8 @@ void AsyncMotionModel::initialize(const std::string& name)
   executor_->add_node(node_);
 
   // TODO(CH3): Remove this if the internal node is removed
-  spinning_ = true;
   spinner_ = std::thread([&](){
-    auto context = node_->get_node_base_interface()->get_context();
-    while(context->is_valid() && spinning_) {
-      executor_->spin_some();
-    }
+    executor_->spin();
   });
 }
 
@@ -153,7 +149,7 @@ void AsyncMotionModel::stop()
     executor_->remove_node(node_);
 
     // TODO(CH3): Remove this if the internal node is removed
-    spinning_ = false;
+    executor_->cancel();
     if (spinner_.joinable()) {
       spinner_.join();
     }

fuse_core/src/async_publisher.cpp

@@ -47,7 +47,7 @@ AsyncPublisher::AsyncPublisher(size_t thread_count) :
 
 AsyncPublisher::~AsyncPublisher()
 {
-  spinning_ = false;
+  executor_->cancel();
   if (spinner_.joinable()) {
     spinner_.join();
   }
@@ -83,10 +83,7 @@ void AsyncPublisher::initialize(const std::string& name)
   // TODO(CH3): Remove this if the internal node is removed
   spinning_ = true;
   spinner_ = std::thread([&](){
-    auto context = node_->get_node_base_interface()->get_context();
-    while(context->is_valid() && spinning_) {
-      executor_->spin_some();
-    }
+    executor_->spin();
   });
 }
 
@@ -122,7 +119,6 @@ void AsyncPublisher::stop()
     executor_->remove_node(node_);
 
     // TODO(CH3): Remove this if the internal node is removed
-    spinning_ = false;
     if (spinner_.joinable()) {
       spinner_.join();
     }

fuse_core/src/async_sensor_model.cpp

@@ -54,7 +54,7 @@ AsyncSensorModel::AsyncSensorModel(size_t thread_count) :
 
 AsyncSensorModel::~AsyncSensorModel()
 {
-  spinning_ = false;
+  executor_->cancel();
   if (spinner_.joinable()) {
     spinner_.join();
   }
@@ -93,12 +93,8 @@ void AsyncSensorModel::initialize(
   executor_->add_node(node_);
 
   // TODO(CH3): Remove this if the internal node is removed
-  spinning_ = true;
   spinner_ = std::thread([&](){
-    auto context = node_->get_node_base_interface()->get_context();
-    while(context->is_valid() && spinning_) {
-      executor_->spin_some();
-    }
+    executor_->spin();
   });
 }
 
@@ -142,7 +138,6 @@ void AsyncSensorModel::stop()
     executor_->remove_node(node_);
 
     // TODO(CH3): Remove this if the internal node is removed
-    spinning_ = false;
     if (spinner_.joinable()) {
       spinner_.join();
     }

fuse_core/test/launch_tests/test_parameters.cpp

@@ -44,30 +44,24 @@ class TestParameters : public ::testing::Test
 public:
   void SetUp() override
   {
-    exec_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
-
-    spinning_ = true;
+    executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
     spinner_ = std::thread([&](){
-      auto context = rclcpp::contexts::get_global_default_context();
-      while(context->is_valid() && spinning_) {
-        exec_->spin_some();
-      }
+      executor_->spin();
     });
   }
 
   void TearDown() override
   {
-    spinning_ = false;
+    executor_->cancel();
     if (spinner_.joinable()) {
       spinner_.join();
     }
-
-    exec_.reset();
+    executor_.reset();
   }
 
   std::thread spinner_;  //!< Internal thread for spinning the executor
   std::atomic<bool> spinning_;  //!< Flag for spinning the spin thread
-  rclcpp::executors::SingleThreadedExecutor::SharedPtr exec_;
+  rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
 };
 
 TEST_F(TestParameters, getPositiveParam)

fuse_core/test/test_throttled_callback.cpp

@@ -38,8 +38,6 @@
 
 #include <gtest/gtest.h>
 
-#include <atomic>
-
 /**
  * @brief A helper class to publish a given number geometry_msgs::msg::Point messages at a given frequency.
  *
@@ -93,9 +91,7 @@ class PointPublisher : public rclcpp::Node
     {
       geometry_msgs::msg::Point point_message;
       point_message.x = i;
-
       publisher_->publish(point_message);
-
       rate.sleep();
     }
   }
@@ -125,11 +121,7 @@ class PointSensorModel : public rclcpp::Node
     , throttled_callback_(
         std::bind(&PointSensorModel::keepCallback, this, std::placeholders::_1),
         std::bind(&PointSensorModel::dropCallback, this, std::placeholders::_1),
-        throttle_period,
-        false,
-        std::make_shared<
-          fuse_core::node_interfaces::NodeInterfaces<fuse_core::node_interfaces::Clock>
-        >(*this)
+        throttle_period
       )
   {
     subscription_ = this->create_subscription<geometry_msgs::msg::Point>(
@@ -219,31 +211,24 @@ class TestThrottledCallback : public ::testing::Test
   void SetUp() override
   {
     rclcpp::init(0, nullptr);
-    exec_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
-
-    spinning_ = true;
+    executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
     spinner_ = std::thread([&](){
-      auto context = rclcpp::contexts::get_global_default_context();
-      while(context->is_valid() && spinning_) {
-        exec_->spin_some();
-      }
+      executor_->spin();
     });
   }
 
   void TearDown() override
   {
-    spinning_ = false;
+    executor_->cancel();
     rclcpp::shutdown();
     if (spinner_.joinable()) {
       spinner_.join();
     }
-
-    exec_.reset();
+    executor_.reset();
   }
 
   std::thread spinner_;  //!< Internal thread for spinning the executor
-  std::atomic<bool> spinning_;  //!< Flag for spinning the spin thread
-  rclcpp::executors::SingleThreadedExecutor::SharedPtr exec_;
+  rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
 };
 
 TEST_F(TestThrottledCallback, NoDroppedMessagesIfThrottlePeriodIsZero)
@@ -253,7 +238,7 @@ TEST_F(TestThrottledCallback, NoDroppedMessagesIfThrottlePeriodIsZero)
   // Start sensor model to listen to messages:
   const rclcpp::Duration throttled_period(0, 0);
   auto sensor_model = std::make_shared<PointSensorModel>(throttled_period);
-  exec_->add_node(sensor_model);
+  executor_->add_node(sensor_model);
 
   // Time should be valid after the context is initialized. But it doesn't hurt to verify.
   ASSERT_TRUE(fuse_core::wait_for_valid(sensor_model->getNode()->get_clock(),
@@ -264,7 +249,7 @@ TEST_F(TestThrottledCallback, NoDroppedMessagesIfThrottlePeriodIsZero)
   const double frequency = 10.0;
 
   auto publisher = std::make_shared<PointPublisher>(frequency);
-  exec_->add_node(publisher);
+  executor_->add_node(publisher);
   publisher->publish(num_messages);
 
   // Check all messages are kept and none are dropped, because when the throttle period is zero, throttling is disabled:
@@ -277,7 +262,7 @@ TEST_F(TestThrottledCallback, DropMessagesIfThrottlePeriodIsGreaterThanPublishPe
   // Start sensor model to listen to messages:
   const rclcpp::Duration throttled_period(0, RCUTILS_S_TO_NS(0.2));
   auto sensor_model = std::make_shared<PointSensorModel>(throttled_period);
-  exec_->add_node(sensor_model);
+  executor_->add_node(sensor_model);
 
   // Time should be valid after the context is initialized. But it doesn't hurt to verify.
   ASSERT_TRUE(fuse_core::wait_for_valid(sensor_model->getNode()->get_clock(),
@@ -290,7 +275,7 @@ TEST_F(TestThrottledCallback, DropMessagesIfThrottlePeriodIsGreaterThanPublishPe
   const double frequency = 1.0 / period;
 
   auto publisher = std::make_shared<PointPublisher>(frequency);
-  exec_->add_node(publisher);
+  executor_->add_node(publisher);
   publisher->publish(num_messages);
 
   // Check the number of kept and dropped callbacks:
@@ -306,7 +291,7 @@ TEST_F(TestThrottledCallback, AlwaysKeepFirstMessageEvenIfThrottlePeriodIsTooLar
   // Start sensor model to listen to messages:
   const rclcpp::Duration throttled_period(10, 0);
   auto sensor_model = std::make_shared<PointSensorModel>(throttled_period);
-  exec_->add_node(sensor_model);
+  executor_->add_node(sensor_model);
 
   // Time should be valid after the context is initialized. But it doesn't hurt to verify.
   ASSERT_TRUE(fuse_core::wait_for_valid(sensor_model->getNode()->get_clock(),
@@ -321,7 +306,7 @@ TEST_F(TestThrottledCallback, AlwaysKeepFirstMessageEvenIfThrottlePeriodIsTooLar
 
   auto publisher = std::make_shared<PointPublisher>(frequency);
   publisher->publish(num_messages);
-  exec_->add_node(publisher);
+  executor_->add_node(publisher);
 
   // Check that regardless of the large throttled period, at least one message is ketpt:
   EXPECT_EQ(1u, sensor_model->getKeptMessages());

fuse_models/src/acceleration_2d.cpp

@@ -64,7 +64,10 @@ void Acceleration2D::onInit()
   params_.loadFromROS(private_node_handle_);
 
   throttled_callback_.setThrottlePeriod(params_.throttle_period);
-  throttled_callback_.setUseWallTime(params_.throttle_use_wall_time);
+
+  if (!params_.throttle_use_wall_time) {
+    throttled_callback_.setClock(node_->get_clock());
+  }
 
   if (params_.indices.empty())
   {