12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cartographer/mapping/pose_extrapolator.cc
View File

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