Use real timestamps from pose and laser msgs

src/homer_navigation_node.cpp

@@ -412,23 +412,14 @@ bool HomerNavigationNode::calculatePath(const geometry_msgs::Point& target, bool
     m_waypoints.clear();
     m_waypoints = path.poses;
     ROS_INFO_STREAM("homer_navigation::calculatePath - Path Size: " << m_waypoints.size());
-    if (!m_waypoints.empty())
-    {
-        sendCurrentPathData();
-        m_state = FOLLOWING_PATH;
-    }
-    else
+    if (m_waypoints.empty())
     {
         sendTargetUnreachableMsg(homer_mapnav_msgs::TargetUnreachable::NO_PATH_FOUND);
         return false;
     }
 
-    // TODO (DM): This doesn't really make sense?
-    // It updates stamp which is used for checking for pose/laser timeouts, spin not called often
-    // enough to really perform update concurrently
-    m_last_laser_time = ros::Time::now();
-    m_last_pose_time = ros::Time::now();
-
+    sendCurrentPathData();
+    m_state = FOLLOWING_PATH;
     return true;
 }
 
@@ -823,7 +814,8 @@ bool HomerNavigationNode::updateSpeeds()
         if (std::fabs(angleToWaypoint) < 10)
             m_avoided_collision = false;
     }
-    // Angle is bigger than maximal driving angle, so stop driving forwards and adjust orientation first
+    // Angle is bigger than maximal driving angle, so stop driving forwards and adjust orientation
+    // first
     else if (std::fabs(angle) > m_max_drive_angle)
     {
         // NOTE: Could be better to not completely stop/reduce velocity depending on magnitude of
@@ -1271,8 +1263,7 @@ void HomerNavigationNode::poseCallback(const geometry_msgs::PoseStamped::ConstPt
     m_robot_pose = msg->pose;
     m_distance_to_target = map_tools::distance(m_robot_pose.position, m_target_point);
 
-    // TODO: Use message's stamp?
-    m_last_pose_time = ros::Time::now();
+    m_last_pose_time = msg->header.stamp; // ros::Time::now();
 }
 
 void HomerNavigationNode::calcMaxMoveDist()
@@ -1313,6 +1304,8 @@ void HomerNavigationNode::maxDepthMoveDistanceCallback(const std_msgs::Float32::
 void HomerNavigationNode::laserDataCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
 {
     sensor_msgs::LaserScan::Ptr tmp_scan = boost::make_shared<sensor_msgs::LaserScan>(*msg);
+    m_last_laser_time = msg->header.stamp; // ros::Time::now();
+
     int param_skip_ranges = 0;
     m_nh.getParamCached("scan/skip_ranges", param_skip_ranges);
     for (int i = 0; i < param_skip_ranges; i++)
@@ -1325,7 +1318,6 @@ void HomerNavigationNode::laserDataCallback(const sensor_msgs::LaserScan::ConstP
     }
     m_scan_map[msg->header.frame_id] = tmp_scan;
 
-    m_last_laser_time = ros::Time::now();
     if (m_state == IDLE)
         return;