Prepare for googlecartographer/cartographer#605. (#551)
parent
adbaeb4fe6
commit
5d784e8adb
|
@ -171,6 +171,9 @@ MapBuilderBridge::GetTrajectoryStates() {
|
||||||
|
|
||||||
const cartographer::mapping::TrajectoryBuilder* const trajectory_builder =
|
const cartographer::mapping::TrajectoryBuilder* const trajectory_builder =
|
||||||
map_builder_.GetTrajectoryBuilder(trajectory_id);
|
map_builder_.GetTrajectoryBuilder(trajectory_id);
|
||||||
|
if (trajectory_builder == nullptr) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
const cartographer::mapping::TrajectoryBuilder::PoseEstimate pose_estimate =
|
const cartographer::mapping::TrajectoryBuilder::PoseEstimate pose_estimate =
|
||||||
trajectory_builder->pose_estimate();
|
trajectory_builder->pose_estimate();
|
||||||
if (cartographer::common::ToUniversal(pose_estimate.time) < 0) {
|
if (cartographer::common::ToUniversal(pose_estimate.time) < 0) {
|
||||||
|
|
Loading…
Reference in New Issue