From 5d43eb2cb5450259a47555e906e4a9389f82f2ad Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Thu, 25 Nov 2021 09:20:03 -0800 Subject: [PATCH] Detect backwards time in an outer loop, remake the entire node instead of using reset --- .../src/fixed_lag_smoother_node.cpp | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/fuse_optimizers/src/fixed_lag_smoother_node.cpp b/fuse_optimizers/src/fixed_lag_smoother_node.cpp index 25c8230fb..dd4bb63aa 100644 --- a/fuse_optimizers/src/fixed_lag_smoother_node.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother_node.cpp @@ -43,8 +43,23 @@ int main(int argc, char **argv) ros::NodeHandle private_node_handle("~"); fuse_graphs::HashGraphParams hash_graph_params; hash_graph_params.loadFromROS(private_node_handle); - fuse_optimizers::FixedLagSmoother optimizer(fuse_graphs::HashGraph::make_unique(hash_graph_params)); - ros::spin(); + + while (ros::ok()) { + fuse_optimizers::FixedLagSmoother optimizer(fuse_graphs::HashGraph::make_unique(hash_graph_params)); + ros::Time last_update_time = ros::Time::now(); + while (ros::ok()) + { + const ros::Time update_time = ros::Time::now(); + if (update_time < last_update_time) { + ROS_WARN_STREAM("time went backwards, restarting fixed lag smoother"); + break; + } + // TODO(lucasw) what should be the sleep duration here? + ros::Duration(0.025).sleep(); + ros::spinOnce(); + last_update_time = update_time; + } + } return 0; }