Make sensor::Data dispatchable to TrajectoryBuilder. (#738)
This change allows to dispatch sensor::Data to implementations of mapping::TrajectoryBuilder, i.e. CollatedTrajectoryBuilder. We need this for cartographer_grpc as the incoming sensor data is inserted into a queue by the gRPC threads and dequeued by a SLAM threads that inserts them into a CollatedTrajectoryBuilder.master
parent
4c999037b4
commit
2a7a6ef934
|
@ -72,7 +72,7 @@ void CollatedTrajectoryBuilder::HandleCollatedSensorData(
|
||||||
last_logging_time_ = std::chrono::steady_clock::now();
|
last_logging_time_ = std::chrono::steady_clock::now();
|
||||||
}
|
}
|
||||||
|
|
||||||
data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());
|
data->AddToGlobalTrajectoryBuilder(wrapped_trajectory_builder_.get());
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace mapping
|
} // namespace mapping
|
||||||
|
|
|
@ -25,6 +25,7 @@
|
||||||
#include "cartographer/common/lua_parameter_dictionary.h"
|
#include "cartographer/common/lua_parameter_dictionary.h"
|
||||||
#include "cartographer/common/port.h"
|
#include "cartographer/common/port.h"
|
||||||
#include "cartographer/io/proto_stream.h"
|
#include "cartographer/io/proto_stream.h"
|
||||||
|
#include "cartographer/mapping/global_trajectory_builder_interface.h"
|
||||||
#include "cartographer/mapping/id.h"
|
#include "cartographer/mapping/id.h"
|
||||||
#include "cartographer/mapping/pose_graph.h"
|
#include "cartographer/mapping/pose_graph.h"
|
||||||
#include "cartographer/mapping/proto/submap_visualization.pb.h"
|
#include "cartographer/mapping/proto/submap_visualization.pb.h"
|
||||||
|
|
|
@ -28,7 +28,6 @@
|
||||||
#include "cartographer/mapping/pose_estimate.h"
|
#include "cartographer/mapping/pose_estimate.h"
|
||||||
#include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
|
#include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
|
||||||
#include "cartographer/mapping/submaps.h"
|
#include "cartographer/mapping/submaps.h"
|
||||||
#include "cartographer/sensor/data.h"
|
|
||||||
#include "cartographer/sensor/point_cloud.h"
|
#include "cartographer/sensor/point_cloud.h"
|
||||||
|
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
|
|
|
@ -0,0 +1,45 @@
|
||||||
|
/*
|
||||||
|
* Copyright 2016 The Cartographer Authors
|
||||||
|
*
|
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||||
|
* you may not use this file except in compliance with the License.
|
||||||
|
* You may obtain a copy of the License at
|
||||||
|
*
|
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
*
|
||||||
|
* Unless required by applicable law or agreed to in writing, software
|
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||||
|
* See the License for the specific language governing permissions and
|
||||||
|
* limitations under the License.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "cartographer/sensor/data.h"
|
||||||
|
|
||||||
|
namespace cartographer {
|
||||||
|
namespace sensor {
|
||||||
|
|
||||||
|
template <>
|
||||||
|
void Dispatchable<sensor::ImuData>::AddToTrajectoryBuilder(
|
||||||
|
mapping::TrajectoryBuilder* const trajectory_builder,
|
||||||
|
const std::string& sensor_id) {
|
||||||
|
trajectory_builder->AddImuData(
|
||||||
|
sensor_id, data_.time, data_.linear_acceleration, data_.angular_velocity);
|
||||||
|
}
|
||||||
|
|
||||||
|
template <>
|
||||||
|
void Dispatchable<sensor::OdometryData>::AddToTrajectoryBuilder(
|
||||||
|
mapping::TrajectoryBuilder* const trajectory_builder,
|
||||||
|
const std::string& sensor_id) {
|
||||||
|
trajectory_builder->AddOdometerData(sensor_id, data_.time, data_.pose);
|
||||||
|
}
|
||||||
|
|
||||||
|
template <>
|
||||||
|
void Dispatchable<sensor::FixedFramePoseData>::AddToTrajectoryBuilder(
|
||||||
|
mapping::TrajectoryBuilder* const trajectory_builder,
|
||||||
|
const std::string& sensor_id) {
|
||||||
|
trajectory_builder->AddFixedFramePoseData(sensor_id, data_.time, data_.pose);
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace sensor
|
||||||
|
} // namespace cartographer
|
|
@ -20,8 +20,10 @@
|
||||||
#include "cartographer/common/make_unique.h"
|
#include "cartographer/common/make_unique.h"
|
||||||
#include "cartographer/common/time.h"
|
#include "cartographer/common/time.h"
|
||||||
#include "cartographer/mapping/global_trajectory_builder_interface.h"
|
#include "cartographer/mapping/global_trajectory_builder_interface.h"
|
||||||
|
#include "cartographer/mapping/trajectory_builder.h"
|
||||||
#include "cartographer/sensor/fixed_frame_pose_data.h"
|
#include "cartographer/sensor/fixed_frame_pose_data.h"
|
||||||
#include "cartographer/sensor/imu_data.h"
|
#include "cartographer/sensor/imu_data.h"
|
||||||
|
#include "cartographer/sensor/odometry_data.h"
|
||||||
#include "cartographer/sensor/point_cloud.h"
|
#include "cartographer/sensor/point_cloud.h"
|
||||||
#include "cartographer/sensor/range_data.h"
|
#include "cartographer/sensor/range_data.h"
|
||||||
#include "cartographer/transform/rigid_transform.h"
|
#include "cartographer/transform/rigid_transform.h"
|
||||||
|
@ -34,8 +36,11 @@ class Data {
|
||||||
virtual ~Data() {}
|
virtual ~Data() {}
|
||||||
|
|
||||||
virtual common::Time GetTime() const = 0;
|
virtual common::Time GetTime() const = 0;
|
||||||
virtual void AddToTrajectoryBuilder(
|
virtual void AddToGlobalTrajectoryBuilder(
|
||||||
mapping::GlobalTrajectoryBuilderInterface* trajectory_builder) = 0;
|
mapping::GlobalTrajectoryBuilderInterface* trajectory_builder) = 0;
|
||||||
|
virtual void AddToTrajectoryBuilder(
|
||||||
|
mapping::TrajectoryBuilder* const trajectory_builder,
|
||||||
|
const std::string& sensor_id) = 0;
|
||||||
};
|
};
|
||||||
|
|
||||||
class DispatchableRangefinderData : public Data {
|
class DispatchableRangefinderData : public Data {
|
||||||
|
@ -46,10 +51,16 @@ class DispatchableRangefinderData : public Data {
|
||||||
: time_(time), origin_(origin), ranges_(ranges) {}
|
: time_(time), origin_(origin), ranges_(ranges) {}
|
||||||
|
|
||||||
common::Time GetTime() const override { return time_; }
|
common::Time GetTime() const override { return time_; }
|
||||||
void AddToTrajectoryBuilder(mapping::GlobalTrajectoryBuilderInterface* const
|
void AddToGlobalTrajectoryBuilder(
|
||||||
trajectory_builder) override {
|
mapping::GlobalTrajectoryBuilderInterface* const trajectory_builder)
|
||||||
|
override {
|
||||||
trajectory_builder->AddRangefinderData(time_, origin_, ranges_);
|
trajectory_builder->AddRangefinderData(time_, origin_, ranges_);
|
||||||
}
|
}
|
||||||
|
void AddToTrajectoryBuilder(
|
||||||
|
mapping::TrajectoryBuilder* const trajectory_builder,
|
||||||
|
const std::string& sensor_id) override {
|
||||||
|
trajectory_builder->AddRangefinderData(sensor_id, time_, origin_, ranges_);
|
||||||
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
const common::Time time_;
|
const common::Time time_;
|
||||||
|
@ -63,10 +74,14 @@ class Dispatchable : public Data {
|
||||||
Dispatchable(const DataType& data) : data_(data) {}
|
Dispatchable(const DataType& data) : data_(data) {}
|
||||||
|
|
||||||
common::Time GetTime() const override { return data_.time; }
|
common::Time GetTime() const override { return data_.time; }
|
||||||
void AddToTrajectoryBuilder(mapping::GlobalTrajectoryBuilderInterface* const
|
void AddToGlobalTrajectoryBuilder(
|
||||||
trajectory_builder) override {
|
mapping::GlobalTrajectoryBuilderInterface* const trajectory_builder)
|
||||||
|
override {
|
||||||
trajectory_builder->AddSensorData(data_);
|
trajectory_builder->AddSensorData(data_);
|
||||||
}
|
}
|
||||||
|
void AddToTrajectoryBuilder(
|
||||||
|
mapping::TrajectoryBuilder* const trajectory_builder,
|
||||||
|
const std::string& sensor_id) override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
const DataType data_;
|
const DataType data_;
|
||||||
|
|
Loading…
Reference in New Issue