Use node timer and fix timer logic

Signed-off-by: methylDragon <methylDragon@gmail.com>

fuse_models/src/odometry_2d_publisher.cpp

@@ -238,7 +238,7 @@ void Odometry2DPublisher::onStart()
   acceleration_output_ = geometry_msgs::AccelWithCovarianceStamped();
 
   // TODO(CH3): Add this to a separate callback group for async behavior
-  publish_timer_ = this->node_.create_wall_timer(
+  publish_timer_ = this->node_.create_timer(
     rclcpp::Duration::from_seconds(1.0 / params_.publish_frequency),
     std::bind(&Odometry2DPublisher::publishTimerCallback, this)
   );
@@ -248,7 +248,7 @@ void Odometry2DPublisher::onStart()
 
 void Odometry2DPublisher::onStop()
 {
-  publish_timer_.reset();
+  publish_timer_.cancel();
 }
 
 bool Odometry2DPublisher::getState(

fuse_optimizers/src/batch_optimizer.cpp

@@ -60,7 +60,7 @@ BatchOptimizer::BatchOptimizer(
   params_.loadFromROS(private_node_handle);
 
   // Configure a timer to trigger optimizations
-  optimize_timer_ = node_.create_wall_timer(
+  optimize_timer_ = node_.create_timer(
     params_.optimization_period,
     std::bind(&BatchOptimizer::optimizerTimerCallback, this)
   );

fuse_optimizers/src/fixed_lag_smoother.cpp

@@ -97,7 +97,7 @@ FixedLagSmoother::FixedLagSmoother(
   optimization_thread_ = std::thread(&FixedLagSmoother::optimizationLoop, this);
 
   // Configure a timer to trigger optimizations
-  optimize_timer_ = node_.create_wall_timer(
+  optimize_timer_ = node_.create_timer(
     params_.optimization_period,
     std::bind(&FixedLagSmoother::optimizerTimerCallback, this)
   );
@@ -196,7 +196,7 @@ void FixedLagSmoother::optimizationLoop()
       //         We do this to ensure state of the graph does not change between unlocking the pending_transactions
       //         queue and obtaining the lock for the graph. But we have now obtained two different locks. If we are
       //         not extremely careful, we could get a deadlock.
-      //  XXX make sure lag_expiration_ has been initialised
+      //  TODO(CH3): We might have to make sure lag_expiration_ has been initialised
       processQueue(*new_transaction, lag_expiration_);
       // Skip this optimization cycle if the transaction is empty because something failed while processing the pending
       // transactions queue.
@@ -286,11 +286,7 @@ void FixedLagSmoother::optimizerTimerCallback()
     {
       std::lock_guard<std::mutex> lock(optimization_requested_mutex_);
       optimization_request_ = true;
-      // TODO(CH3): ??????? No idea what this does...
-      // XXX event.current_expected refers to when ROS planned to execute the callback, not the current time.
-      //     wall-time and measurement-time should be decoupled further
-      // optimization_deadline_ = event.current_expected + params_.optimization_period;
-      optimization_deadline_ = get_clock()->now() + params_.optimization_period;
+      optimization_deadline_ = get_clock()->now() + optimize_timer_->time_until_trigger();
     }
     optimization_requested_.notify_one();
   }

fuse_optimizers/src/optimizer.cpp

@@ -96,7 +96,7 @@ Optimizer::Optimizer(
   private_node_handle_.param("diagnostic_updater_timer_period", diagnostic_updater_timer_period_,
                              diagnostic_updater_timer_period_);
 
-  diagnostic_updater_timer_ = node_.create_wall_timer(
+  diagnostic_updater_timer_ = node_.create_timer(
     rclcpp::Duration::from_seconds(diagnostic_updater_timer_period_),
     std::bind(&diagnostic_updater::Updater::update, &diagnostic_updater_)
   );

fuse_publishers/src/pose_2d_publisher.cpp

@@ -188,7 +188,7 @@ void Pose2DPublisher::onStart()
   // Start the tf timer
   if (publish_to_tf_)
   {
-    tf_publish_timer_ = node_.create_wall_timer(
+    tf_publish_timer_ = node_.create_timer(
       rclcpp::Duration::from_seconds(1.0 / tf_publish_frequency),
       std::bind(&Pose2DPublisher::tfPublishTimerCallback, this)
     );
@@ -200,7 +200,7 @@ void Pose2DPublisher::onStop()
   // Stop the tf timer
   if (publish_to_tf_)
   {
-    tf_publish_timer_.reset();
+    tf_publish_timer_.cancel();
   }
 }
 

fuse_tutorials/src/range_sensor_simulator.cpp

@@ -119,12 +119,12 @@ std::vector<Beacon> createNoisyBeacons(const std::vector<Beacon>& beacons)
  */
 sensor_msgs::PointCloud2::ConstPtr beaconsToPointcloud(
   const std::vector<Beacon>& beacons,
-  rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface=nullptr)
+  rclcpp::Clock::SharedPtr clock=nullptr)
 {
   auto msg = boost::make_shared<sensor_msgs::PointCloud2>();
 
-  if (clock_interface != nullptr) {
-    msg->header.stamp = clock_interface->get_clock()->now();
+  if (clock != nullptr) {
+    msg->header.stamp = clock->now();
   } else {
     msg->header.stamp = rclcpp::Clock(RCL_SYSTEM_TIME).now();
   }
@@ -199,7 +199,7 @@ nav_msgs::Odometry::ConstPtr robotToOdometry(const Robot& state)
  * The state estimator will not run until it has been sent a starting pose.
  */
 void initializeStateEstimation(
-  const Robot& state, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface=nullptr)
+  const Robot& state, rclcpp::Clock::SharedPtr clock=nullptr)
 {
   // Send the initial localization signal to the state estimator
   auto srv = fuse_models::SetPose();
@@ -223,8 +223,8 @@ void initializeStateEstimation(
   {
     rclcpp::Duration::from_seconds(0.1).sleep();
 
-    if (clock_interface != nullptr) {
-      srv.request.pose.header.stamp = clock_interface->get_clock()->now();
+    if (clock != nullptr) {
+      srv.request.pose.header.stamp = clock->now();
     } else {
       srv.request.pose.header.stamp = rclcpp::Clock(RCL_SYSTEM_TIME).now();
     }
@@ -365,11 +365,11 @@ int main(int argc, char **argv)
 
   // Create the true set of range beacons
   auto beacons = createBeacons();
-  true_beacons_publisher.publish(beaconsToPointcloud(beacons), node->get_node_clock_interface);
+  true_beacons_publisher.publish(beaconsToPointcloud(beacons), node->get_clock());
 
   // Publish a set of noisy beacon locations to act as the known priors
   auto noisy_beacons = createNoisyBeacons(beacons);
-  prior_beacons_publisher.publish(beaconsToPointcloud(noisy_beacons, node->get_node_clock_interface);
+  prior_beacons_publisher.publish(beaconsToPointcloud(noisy_beacons, node->get_clock());
 
   // Initialize the robot state
   auto state = Robot();