Small fixes for PoseExtrapolator::UpdateVelocitiesFromPoses() (#1492)

- use `pose_queue_duration_` instead of hardcoded "0.001"
  (cartographer_ros sets this to 0.001, probably that's why this
  wasn't noticed)
- queue_delta is in seconds, log message said milliseconds
master
Michael Grupp 2019-01-03 10:03:48 +01:00 committed by Wally B. Feed
parent 2d698e70e8
commit 53a1f6c2f7
1 changed files with 2 additions and 2 deletions

View File

@ -164,9 +164,9 @@ void PoseExtrapolator::UpdateVelocitiesFromPoses() {
const TimedPose& oldest_timed_pose = timed_pose_queue_.front();
const auto oldest_time = oldest_timed_pose.time;
const double queue_delta = common::ToSeconds(newest_time - oldest_time);
if (queue_delta < 0.001) { // 1 ms
if (queue_delta < common::ToSeconds(pose_queue_duration_)) {
LOG(WARNING) << "Queue too short for velocity estimation. Queue duration: "
<< queue_delta << " ms";
<< queue_delta << " s";
return;
}
const transform::Rigid3d& newest_pose = newest_timed_pose.pose;