diff --git a/costmap_2d/src/costmap_2d_ros.cpp b/costmap_2d/src/costmap_2d_ros.cpp index 41b6c44b88..06196def32 100644 --- a/costmap_2d/src/costmap_2d_ros.cpp +++ b/costmap_2d/src/costmap_2d_ros.cpp @@ -610,7 +610,7 @@ bool Costmap2DROS::getRobotPose(geometry_msgs::PoseStamped& global_pose) const return false; } // check global_pose timeout - if (current_time.toSec() - global_pose.header.stamp.toSec() > transform_tolerance_) + if (!global_pose.header.stamp.isZero() && current_time.toSec() - global_pose.header.stamp.toSec() > transform_tolerance_) { ROS_WARN_THROTTLE(1.0, "Costmap2DROS transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f", diff --git a/move_base/src/move_base.cpp b/move_base/src/move_base.cpp index 007c54a31b..bd14df571d 100644 --- a/move_base/src/move_base.cpp +++ b/move_base/src/move_base.cpp @@ -1194,7 +1194,8 @@ namespace move_base { } // check if global_pose time stamp is within costmap transform tolerance - if (current_time.toSec() - global_pose.header.stamp.toSec() > costmap->getTransformTolerance()) + if (!global_pose.header.stamp.isZero() && + current_time.toSec() - global_pose.header.stamp.toSec() > costmap->getTransformTolerance()) { ROS_WARN_THROTTLE(1.0, "Transform timeout for %s. " \ "Current time: %.4f, pose stamp: %.4f, tolerance: %.4f", costmap->getName().c_str(),