diff --git a/fuse_models/src/odometry_2d_publisher.cpp b/fuse_models/src/odometry_2d_publisher.cpp index 5ca1fb6e2..a93b8a147 100644 --- a/fuse_models/src/odometry_2d_publisher.cpp +++ b/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( diff --git a/fuse_optimizers/src/batch_optimizer.cpp b/fuse_optimizers/src/batch_optimizer.cpp index cfd325106..e6b45121b 100644 --- a/fuse_optimizers/src/batch_optimizer.cpp +++ b/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) ); diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index 442ccb50a..901bcde9e 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/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 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(); } diff --git a/fuse_optimizers/src/optimizer.cpp b/fuse_optimizers/src/optimizer.cpp index bbc68c83f..88c92c009 100644 --- a/fuse_optimizers/src/optimizer.cpp +++ b/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_) ); diff --git a/fuse_publishers/src/pose_2d_publisher.cpp b/fuse_publishers/src/pose_2d_publisher.cpp index 785033266..1a05319f5 100644 --- a/fuse_publishers/src/pose_2d_publisher.cpp +++ b/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(); } } diff --git a/fuse_tutorials/src/range_sensor_simulator.cpp b/fuse_tutorials/src/range_sensor_simulator.cpp index cd44e3b2b..39d41c197 100644 --- a/fuse_tutorials/src/range_sensor_simulator.cpp +++ b/fuse_tutorials/src/range_sensor_simulator.cpp @@ -119,12 +119,12 @@ std::vector createNoisyBeacons(const std::vector& beacons) */ sensor_msgs::PointCloud2::ConstPtr beaconsToPointcloud( const std::vector& beacons, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface=nullptr) + rclcpp::Clock::SharedPtr clock=nullptr) { auto msg = boost::make_shared(); - 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();