From 4bad3c4702b4dfc708e26627a19c9bb60ee75233 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Thu, 27 Jul 2017 14:52:41 +0200 Subject: [PATCH] Crash fix when "extrapolating". (#453) It can happen that ros::Time::now() is slightly behind the latest pose timestamp if sensor data had newer timestamps. We would crash before. Instead, we now publish the latest pose which is even newer that what we tried to publish before. --- cartographer_ros/cartographer_ros/node.cc | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 9b73139..74aa1b6 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -136,10 +136,13 @@ void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) { geometry_msgs::TransformStamped stamped_transform; // If we do not publish a new point cloud, we still allow time of the - // published poses to advance. - stamped_transform.header.stamp = ros::Time::now(); - const Rigid3d tracking_to_local = - extrapolator.ExtrapolatePose(FromRos(stamped_transform.header.stamp)); + // published poses to advance. If we already know a newer pose, we use its + // time instead. Since tf knows how to interpolate, providing newer + // information is better. + const ::cartographer::common::Time now = + std::max(FromRos(ros::Time::now()), extrapolator.GetLastPoseTime()); + stamped_transform.header.stamp = ToRos(now); + const Rigid3d tracking_to_local = extrapolator.ExtrapolatePose(now); const Rigid3d tracking_to_map = trajectory_state.local_to_map * tracking_to_local;