From 2a7a6ef93489ebf9a803203ffb36591d4f93a863 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= Date: Wed, 6 Dec 2017 16:36:36 +0100 Subject: [PATCH] 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. --- .../mapping/collated_trajectory_builder.cc | 2 +- cartographer/mapping/map_builder_interface.h | 1 + cartographer/mapping/trajectory_builder.h | 1 - cartographer/sensor/data.cc | 45 +++++++++++++++++++ cartographer/sensor/data.h | 25 ++++++++--- 5 files changed, 67 insertions(+), 7 deletions(-) create mode 100644 cartographer/sensor/data.cc diff --git a/cartographer/mapping/collated_trajectory_builder.cc b/cartographer/mapping/collated_trajectory_builder.cc index 7065aba..5686376 100644 --- a/cartographer/mapping/collated_trajectory_builder.cc +++ b/cartographer/mapping/collated_trajectory_builder.cc @@ -72,7 +72,7 @@ void CollatedTrajectoryBuilder::HandleCollatedSensorData( last_logging_time_ = std::chrono::steady_clock::now(); } - data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get()); + data->AddToGlobalTrajectoryBuilder(wrapped_trajectory_builder_.get()); } } // namespace mapping diff --git a/cartographer/mapping/map_builder_interface.h b/cartographer/mapping/map_builder_interface.h index 7e174ea..4dfbf4c 100644 --- a/cartographer/mapping/map_builder_interface.h +++ b/cartographer/mapping/map_builder_interface.h @@ -25,6 +25,7 @@ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/port.h" #include "cartographer/io/proto_stream.h" +#include "cartographer/mapping/global_trajectory_builder_interface.h" #include "cartographer/mapping/id.h" #include "cartographer/mapping/pose_graph.h" #include "cartographer/mapping/proto/submap_visualization.pb.h" diff --git a/cartographer/mapping/trajectory_builder.h b/cartographer/mapping/trajectory_builder.h index 3c28d6e..5b04072 100644 --- a/cartographer/mapping/trajectory_builder.h +++ b/cartographer/mapping/trajectory_builder.h @@ -28,7 +28,6 @@ #include "cartographer/mapping/pose_estimate.h" #include "cartographer/mapping/proto/trajectory_builder_options.pb.h" #include "cartographer/mapping/submaps.h" -#include "cartographer/sensor/data.h" #include "cartographer/sensor/point_cloud.h" namespace cartographer { diff --git a/cartographer/sensor/data.cc b/cartographer/sensor/data.cc new file mode 100644 index 0000000..c6e3626 --- /dev/null +++ b/cartographer/sensor/data.cc @@ -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::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::AddToTrajectoryBuilder( + mapping::TrajectoryBuilder* const trajectory_builder, + const std::string& sensor_id) { + trajectory_builder->AddOdometerData(sensor_id, data_.time, data_.pose); +} + +template <> +void Dispatchable::AddToTrajectoryBuilder( + mapping::TrajectoryBuilder* const trajectory_builder, + const std::string& sensor_id) { + trajectory_builder->AddFixedFramePoseData(sensor_id, data_.time, data_.pose); +} + +} // namespace sensor +} // namespace cartographer diff --git a/cartographer/sensor/data.h b/cartographer/sensor/data.h index 8cc016a..098ce94 100644 --- a/cartographer/sensor/data.h +++ b/cartographer/sensor/data.h @@ -20,8 +20,10 @@ #include "cartographer/common/make_unique.h" #include "cartographer/common/time.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/imu_data.h" +#include "cartographer/sensor/odometry_data.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/range_data.h" #include "cartographer/transform/rigid_transform.h" @@ -34,8 +36,11 @@ class Data { virtual ~Data() {} virtual common::Time GetTime() const = 0; - virtual void AddToTrajectoryBuilder( + virtual void AddToGlobalTrajectoryBuilder( mapping::GlobalTrajectoryBuilderInterface* trajectory_builder) = 0; + virtual void AddToTrajectoryBuilder( + mapping::TrajectoryBuilder* const trajectory_builder, + const std::string& sensor_id) = 0; }; class DispatchableRangefinderData : public Data { @@ -46,10 +51,16 @@ class DispatchableRangefinderData : public Data { : time_(time), origin_(origin), ranges_(ranges) {} common::Time GetTime() const override { return time_; } - void AddToTrajectoryBuilder(mapping::GlobalTrajectoryBuilderInterface* const - trajectory_builder) override { + void AddToGlobalTrajectoryBuilder( + mapping::GlobalTrajectoryBuilderInterface* const trajectory_builder) + override { 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: const common::Time time_; @@ -63,10 +74,14 @@ class Dispatchable : public Data { Dispatchable(const DataType& data) : data_(data) {} common::Time GetTime() const override { return data_.time; } - void AddToTrajectoryBuilder(mapping::GlobalTrajectoryBuilderInterface* const - trajectory_builder) override { + void AddToGlobalTrajectoryBuilder( + mapping::GlobalTrajectoryBuilderInterface* const trajectory_builder) + override { trajectory_builder->AddSensorData(data_); } + void AddToTrajectoryBuilder( + mapping::TrajectoryBuilder* const trajectory_builder, + const std::string& sensor_id) override; private: const DataType data_;