diff --git a/mbf_utility/src/navigation_utility.cpp b/mbf_utility/src/navigation_utility.cpp index 1c0ded3a..f7acd3f7 100644 --- a/mbf_utility/src/navigation_utility.cpp +++ b/mbf_utility/src/navigation_utility.cpp @@ -67,7 +67,7 @@ bool getRobotPose(const TF &tf, timeout, local_pose, robot_pose); - if (success && ros::Time::now() - robot_pose.header.stamp > timeout) + if (success && timeout.toSec() > 0 && ros::Time::now() - robot_pose.header.stamp > timeout) { ROS_WARN("Most recent robot pose is %gs old (tolerance %gs)", (ros::Time::now() - robot_pose.header.stamp).toSec(), timeout.toSec());