diff --git a/cartographer/mapping/collated_trajectory_builder.cc b/cartographer/mapping/collated_trajectory_builder.cc index f610c40..b50b3d4 100644 --- a/cartographer/mapping/collated_trajectory_builder.cc +++ b/cartographer/mapping/collated_trajectory_builder.cc @@ -29,7 +29,7 @@ constexpr double kSensorDataRatesLoggingPeriodSeconds = 15.; } // namespace CollatedTrajectoryBuilder::CollatedTrajectoryBuilder( - sensor::Collator* const sensor_collator, const int trajectory_id, + sensor::CollatorInterface* const sensor_collator, const int trajectory_id, const std::unordered_set& expected_sensor_ids, std::unique_ptr wrapped_trajectory_builder) : sensor_collator_(sensor_collator), diff --git a/cartographer/mapping/collated_trajectory_builder.h b/cartographer/mapping/collated_trajectory_builder.h index 72696c1..389546a 100644 --- a/cartographer/mapping/collated_trajectory_builder.h +++ b/cartographer/mapping/collated_trajectory_builder.h @@ -27,18 +27,18 @@ #include "cartographer/common/rate_timer.h" #include "cartographer/mapping/submaps.h" #include "cartographer/mapping/trajectory_builder_interface.h" -#include "cartographer/sensor/collator.h" +#include "cartographer/sensor/collator_interface.h" #include "cartographer/sensor/dispatchable.h" namespace cartographer { namespace mapping { -// Handles collating sensor data using a sensor::Collator, then passing it on to +// Collates sensor data using a sensor::CollatorInterface, then passes it on to // a mapping::TrajectoryBuilderInterface which is common for 2D and 3D. class CollatedTrajectoryBuilder : public TrajectoryBuilderInterface { public: CollatedTrajectoryBuilder( - sensor::Collator* sensor_collator, int trajectory_id, + sensor::CollatorInterface* sensor_collator, int trajectory_id, const std::unordered_set& expected_sensor_ids, std::unique_ptr wrapped_trajectory_builder); ~CollatedTrajectoryBuilder() override; @@ -79,7 +79,7 @@ class CollatedTrajectoryBuilder : public TrajectoryBuilderInterface { void HandleCollatedSensorData(const std::string& sensor_id, std::unique_ptr data); - sensor::Collator* const sensor_collator_; + sensor::CollatorInterface* const sensor_collator_; const int trajectory_id_; std::unique_ptr wrapped_trajectory_builder_; diff --git a/cartographer/sensor/collator.h b/cartographer/sensor/collator.h index 600ed32..abf0e8c 100644 --- a/cartographer/sensor/collator.h +++ b/cartographer/sensor/collator.h @@ -23,44 +23,31 @@ #include #include +#include "cartographer/sensor/collator_interface.h" #include "cartographer/sensor/data.h" #include "cartographer/sensor/ordered_multi_queue.h" namespace cartographer { namespace sensor { -class Collator { +class Collator : public CollatorInterface { public: - using Callback = - std::function)>; - Collator() {} Collator(const Collator&) = delete; Collator& operator=(const Collator&) = delete; - // Adds a trajectory to produce sorted sensor output for. Calls 'callback' - // for each collated sensor data. void AddTrajectory(int trajectory_id, const std::unordered_set& expected_sensor_ids, - const Callback& callback); + const Callback& callback) override; - // Marks 'trajectory_id' as finished. - void FinishTrajectory(int trajectory_id); + void FinishTrajectory(int trajectory_id) override; - // Adds 'data' for 'trajectory_id' to be collated. 'data' must contain valid - // sensor data. Sensor packets with matching 'data.sensor_id_' must be added - // in time order. - void AddSensorData(int trajectory_id, std::unique_ptr data); + void AddSensorData(int trajectory_id, std::unique_ptr data) override; - // Dispatches all queued sensor packets. May only be called once. - // AddSensorData may not be called after Flush. - void Flush(); + void Flush() override; - // Must only be called if at least one unfinished trajectory exists. Returns - // the ID of the trajectory that needs more data before the Collator is - // unblocked. - int GetBlockingTrajectoryId() const; + int GetBlockingTrajectoryId() const override; private: // Queue keys are a pair of trajectory ID and sensor identifier. diff --git a/cartographer/sensor/collator_interface.h b/cartographer/sensor/collator_interface.h new file mode 100644 index 0000000..2183a08 --- /dev/null +++ b/cartographer/sensor/collator_interface.h @@ -0,0 +1,68 @@ +/* + * Copyright 2018 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. + */ + +#ifndef CARTOGRAPHER_SENSOR_COLLATOR_INTERFACE_H_ +#define CARTOGRAPHER_SENSOR_COLLATOR_INTERFACE_H_ + +#include +#include +#include +#include + +#include "cartographer/sensor/data.h" + +namespace cartographer { +namespace sensor { + +class CollatorInterface { + public: + using Callback = + std::function)>; + + CollatorInterface() {} + virtual ~CollatorInterface() {} + CollatorInterface(const CollatorInterface&) = delete; + CollatorInterface& operator=(const CollatorInterface&) = delete; + + // Adds a trajectory to produce sorted sensor output for. Calls 'callback' + // for each collated sensor data. + virtual void AddTrajectory( + int trajectory_id, + const std::unordered_set& expected_sensor_ids, + const Callback& callback) = 0; + + // Marks 'trajectory_id' as finished. + virtual void FinishTrajectory(int trajectory_id) = 0; + + // Adds 'data' for 'trajectory_id' to be collated. 'data' must contain valid + // sensor data. Sensor packets with matching 'data.sensor_id_' must be added + // in time order. + virtual void AddSensorData(int trajectory_id, std::unique_ptr data) = 0; + + // Dispatches all queued sensor packets. May only be called once. + // AddSensorData may not be called after Flush. + virtual void Flush() = 0; + + // Must only be called if at least one unfinished trajectory exists. Returns + // the ID of the trajectory that needs more data before the Collator is + // unblocked. + virtual int GetBlockingTrajectoryId() const = 0; +}; + +} // namespace sensor +} // namespace cartographer + +#endif // CARTOGRAPHER_SENSOR_COLLATOR_INTERFACE_H_