Merge pull request #1 from furushchev/fix-stamp-zero

Check if stamp of transforomed pose is non-zero
GITAI · Jun 6, 2022 · 81da4bd · 81da4bd
2 parents 6e9de3f + 03046d5
commit 81da4bd
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 2 deletions.
diff --git 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
@@ -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(),