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

Fix sequential subdivisions (#806) 

FIXES=https://github.com/googlecartographer/cartographer/issues/1026
master
gaschler 2018-04-05 12:18:16 +02:00 committed by Wally B. Feed
parent 12816aacd3
commit b3e3dfd7d4
2 changed files with 12 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
cartographer_ros/cartographer_ros/sensor_bridge.cc
View File

@ -201,6 +201,16 @@ void SensorBridge::HandleLaserScan(
// send all other sensor data first.
const carto::common::Time subdivision_time =
time + carto::common::FromSeconds(time_to_subdivision_end);
auto it = sensor_to_previous_subdivision_time_.find(sensor_id);
if (it != sensor_to_previous_subdivision_time_.end() &&
it->second >= subdivision_time) {
LOG(WARNING) << "Ignored subdivision of a LaserScan message from sensor "
<< sensor_id << " because previous subdivision time "
<< it->second << " is not before current subdivision time "
<< subdivision_time;
continue;
}
sensor_to_previous_subdivision_time_[sensor_id] = subdivision_time;
for (Eigen::Vector4f& point : subdivision) {
point[3] -= time_to_subdivision_end;
}

2
cartographer_ros/cartographer_ros/sensor_bridge.h
View File

@ -85,6 +85,8 @@ class SensorBridge {
const ::cartographer::sensor::TimedPointCloud& ranges);
const int num_subdivisions_per_laser_scan_;
std::map<std::string, cartographer::common::Time>
sensor_to_previous_subdivision_time_;
const TfBridge tf_bridge_;
::cartographer::mapping::TrajectoryBuilderInterface* const
trajectory_builder_;