12345qiupeng
/
cartographer_ros
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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.
master
Wolfgang Hess 2017-07-27 14:52:41 +02:00 committed by GitHub
parent 907586a7fc
commit 4bad3c4702
1 changed files with 7 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
cartographer_ros/cartographer_ros/node.cc
View File

@ -136,10 +136,13 @@ void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) {
geometry_msgs::TransformStamped stamped_transform; geometry_msgs::TransformStamped stamped_transform;
// If we do not publish a new point cloud, we still allow time of the // If we do not publish a new point cloud, we still allow time of the
// published poses to advance. // published poses to advance. If we already know a newer pose, we use its
stamped_transform.header.stamp = ros::Time::now(); // time instead. Since tf knows how to interpolate, providing newer
const Rigid3d tracking_to_local = // information is better.
extrapolator.ExtrapolatePose(FromRos(stamped_transform.header.stamp)); 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 = const Rigid3d tracking_to_map =
trajectory_state.local_to_map * tracking_to_local; trajectory_state.local_to_map * tracking_to_local;