diff --git a/cartographer/sensor/test_helpers.h b/cartographer/sensor/test_helpers.h index 7e62380..9806371 100644 --- a/cartographer/sensor/test_helpers.h +++ b/cartographer/sensor/test_helpers.h @@ -20,6 +20,13 @@ #include #include +#include "cartographer/common/make_unique.h" +#include "cartographer/common/time.h" +#include "cartographer/sensor/collator_interface.h" +#include "cartographer/sensor/dispatchable.h" +#include "cartographer/sensor/imu_data.h" +#include "cartographer/sensor/odometry_data.h" +#include "cartographer/sensor/timed_point_cloud_data.h" #include "gmock/gmock.h" namespace cartographer { @@ -29,6 +36,51 @@ MATCHER_P(Near, point, std::string(negation ? "Doesn't" : "Does") + " match.") { return arg.isApprox(point, 0.001f); } +namespace test { + +typedef std::tuple + CollatorOutput; + +struct CollatorInput { + static CollatorInput CreateImuData(int trajectory_id, + const std::string& sensor_id, int time) { + return CollatorInput{ + trajectory_id, + MakeDispatchable(sensor_id, ImuData{common::FromUniversal(time)}), + CollatorOutput{trajectory_id, sensor_id, common::FromUniversal(time)}}; + } + static CollatorInput CreateTimedPointCloudData(int trajectory_id, + const std::string& sensor_id, + int time) { + return CollatorInput{ + trajectory_id, + MakeDispatchable( + sensor_id, + TimedPointCloudData{ + common::FromUniversal(time), Eigen::Vector3f::Zero(), {}}), + CollatorOutput{trajectory_id, sensor_id, common::FromUniversal(time)}}; + } + static CollatorInput CreateOdometryData(int trajectory_id, + const std::string& sensor_id, + int time) { + return CollatorInput{ + trajectory_id, + MakeDispatchable(sensor_id, + OdometryData{common::FromUniversal(time), + transform::Rigid3d::Identity()}), + CollatorOutput{trajectory_id, sensor_id, common::FromUniversal(time)}}; + } + void MoveToCollator(CollatorInterface* collator) { + collator->AddSensorData(trajectory_id, std::move(data)); + } + + const int trajectory_id; + std::unique_ptr data; + const CollatorOutput expected_output; +}; + +} // namespace test } // namespace sensor } // namespace cartographer diff --git a/cartographer/sensor/trajectory_collator.cc b/cartographer/sensor/trajectory_collator.cc new file mode 100644 index 0000000..753151c --- /dev/null +++ b/cartographer/sensor/trajectory_collator.cc @@ -0,0 +1,61 @@ +/* + * 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. + */ + +#include "cartographer/sensor/trajectory_collator.h" + +namespace cartographer { +namespace sensor { + +void TrajectoryCollator::AddTrajectory( + const int trajectory_id, + const std::unordered_set& expected_sensor_ids, + const Callback& callback) { + CHECK(trajectory_to_queue_.count(trajectory_id) == 0); + for (const auto& sensor_id : expected_sensor_ids) { + const auto queue_key = QueueKey{trajectory_id, sensor_id}; + trajectory_to_queue_[trajectory_id].AddQueue( + queue_key, [callback, sensor_id](std::unique_ptr data) { + callback(sensor_id, std::move(data)); + }); + trajectory_to_queue_keys_[trajectory_id].push_back(queue_key); + } +} + +void TrajectoryCollator::FinishTrajectory(const int trajectory_id) { + for (const auto& queue_key : trajectory_to_queue_keys_[trajectory_id]) { + trajectory_to_queue_.at(trajectory_id).MarkQueueAsFinished(queue_key); + } +} + +void TrajectoryCollator::AddSensorData(const int trajectory_id, + std::unique_ptr data) { + QueueKey queue_key{trajectory_id, data->GetSensorId()}; + trajectory_to_queue_.at(trajectory_id) + .Add(std::move(queue_key), std::move(data)); +} + +void TrajectoryCollator::Flush() { + for (auto& it : trajectory_to_queue_) { + it.second.Flush(); + } +} + +common::optional TrajectoryCollator::GetBlockingTrajectoryId() const { + return common::optional(); +} + +} // namespace sensor +} // namespace cartographer diff --git a/cartographer/sensor/trajectory_collator.h b/cartographer/sensor/trajectory_collator.h new file mode 100644 index 0000000..f3ec4e7 --- /dev/null +++ b/cartographer/sensor/trajectory_collator.h @@ -0,0 +1,65 @@ +/* + * 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_TRAJECTORY_COLLATOR_H_ +#define CARTOGRAPHER_SENSOR_TRAJECTORY_COLLATOR_H_ + +#include +#include +#include + +#include "cartographer/sensor/collator_interface.h" +#include "cartographer/sensor/ordered_multi_queue.h" + +namespace cartographer { +namespace sensor { + +// Waits to see at least one data item for all sensor ids and dispatches data +// in merge-sorted order. Contrary to 'Collator', it does not wait for other +// trajectories. +// Also contrary to 'Collator', whose output is deterministic, the sequence in +// which data is dispatched is not sorted, so non-deterministic input sequences +// will result in non-deterministic output. +class TrajectoryCollator : public CollatorInterface { + public: + TrajectoryCollator() {} + + TrajectoryCollator(const TrajectoryCollator&) = delete; + TrajectoryCollator& operator=(const TrajectoryCollator&) = delete; + + void AddTrajectory(int trajectory_id, + const std::unordered_set& expected_sensor_ids, + const Callback& callback) override; + + void FinishTrajectory(int trajectory_id) override; + + void AddSensorData(int trajectory_id, std::unique_ptr data) override; + + void Flush() override; + + common::optional GetBlockingTrajectoryId() const override; + + private: + std::unordered_map trajectory_to_queue_; + + // Map of trajectory ID to all associated QueueKeys. + std::unordered_map> trajectory_to_queue_keys_; +}; + +} // namespace sensor +} // namespace cartographer + +#endif // CARTOGRAPHER_SENSOR_TRAJECTORY_COLLATOR_H_ diff --git a/cartographer/sensor/trajectory_collator_test.cc b/cartographer/sensor/trajectory_collator_test.cc new file mode 100644 index 0000000..44ebc3f --- /dev/null +++ b/cartographer/sensor/trajectory_collator_test.cc @@ -0,0 +1,121 @@ +/* + * 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. + */ + +#include "cartographer/sensor/trajectory_collator.h" + +#include +#include + +#include "cartographer/common/make_unique.h" +#include "cartographer/common/time.h" +#include "cartographer/sensor/imu_data.h" +#include "cartographer/sensor/odometry_data.h" +#include "cartographer/sensor/test_helpers.h" +#include "cartographer/sensor/timed_point_cloud_data.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace sensor { +namespace { + +using test::CollatorInput; +using test::CollatorOutput; + +TEST(TrajectoryCollator, OrderingMultipleTrajectories) { + const int kTrajectoryId[] = {4, 7}; + const std::array kSensorId = {{"my_points", "some_imu"}}; + + std::vector input_data; + + input_data.push_back(CollatorInput::CreateTimedPointCloudData( + kTrajectoryId[0], kSensorId[0], 0)); + input_data.push_back(CollatorInput::CreateTimedPointCloudData( + kTrajectoryId[1], kSensorId[0], 0)); + input_data.push_back( + CollatorInput::CreateImuData(kTrajectoryId[1], kSensorId[1], 0)); + input_data.push_back( + CollatorInput::CreateImuData(kTrajectoryId[0], kSensorId[1], 0)); + input_data.push_back(CollatorInput::CreateTimedPointCloudData( + kTrajectoryId[1], kSensorId[0], 100)); + input_data.push_back(CollatorInput::CreateTimedPointCloudData( + kTrajectoryId[0], kSensorId[0], 50)); + input_data.push_back( + CollatorInput::CreateImuData(kTrajectoryId[0], kSensorId[1], 60)); + input_data.push_back(CollatorInput::CreateTimedPointCloudData( + kTrajectoryId[1], kSensorId[0], 150)); + input_data.push_back( + CollatorInput::CreateImuData(kTrajectoryId[1], kSensorId[1], 120)); + + std::vector received; + TrajectoryCollator collator; + collator.AddTrajectory( + kTrajectoryId[0], + std::unordered_set(kSensorId.begin(), kSensorId.end()), + [&received, kTrajectoryId](const std::string& sensor_id, + std::unique_ptr data) { + received.push_back(CollatorOutput(kTrajectoryId[0], data->GetSensorId(), + data->GetTime())); + }); + collator.AddTrajectory( + kTrajectoryId[1], + std::unordered_set(kSensorId.begin(), kSensorId.end()), + [&received, kTrajectoryId](const std::string& sensor_id, + std::unique_ptr data) { + received.push_back(CollatorOutput(kTrajectoryId[1], data->GetSensorId(), + data->GetTime())); + }); + + // Send each sensor_id once to establish a common start time. + input_data[0].MoveToCollator(&collator); + input_data[1].MoveToCollator(&collator); + EXPECT_EQ(0, received.size()); + input_data[2].MoveToCollator(&collator); + input_data[3].MoveToCollator(&collator); + EXPECT_EQ(2, received.size()); + EXPECT_EQ(input_data[1].expected_output, received[0]); + EXPECT_EQ(input_data[0].expected_output, received[1]); + + // Does not wait for other trajectory. + input_data[4].MoveToCollator(&collator); + EXPECT_EQ(3, received.size()); + EXPECT_EQ(input_data[2].expected_output, received[2]); + + input_data[5].MoveToCollator(&collator); + EXPECT_EQ(4, received.size()); + EXPECT_EQ(input_data[3].expected_output, received[3]); + input_data[6].MoveToCollator(&collator); + EXPECT_EQ(5, received.size()); + EXPECT_EQ(input_data[5].expected_output, received[4]); + + // Sorts different sensors. + input_data[7].MoveToCollator(&collator); + EXPECT_EQ(5, received.size()); + input_data[8].MoveToCollator(&collator); + EXPECT_EQ(7, received.size()); + EXPECT_EQ(input_data[4].expected_output, received[5]); + EXPECT_EQ(input_data[8].expected_output, received[6]); + + EXPECT_FALSE(collator.GetBlockingTrajectoryId().has_value()); + + collator.FinishTrajectory(kTrajectoryId[0]); + collator.FinishTrajectory(kTrajectoryId[1]); + collator.Flush(); + ASSERT_EQ(input_data.size(), received.size()); +} + +} // namespace +} // namespace sensor +} // namespace cartographer