00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "cartographer/sensor/internal/trajectory_collator.h"
00018
00019 #include <array>
00020 #include <memory>
00021
00022 #include "absl/memory/memory.h"
00023 #include "cartographer/common/time.h"
00024 #include "cartographer/sensor/imu_data.h"
00025 #include "cartographer/sensor/internal/test_helpers.h"
00026 #include "cartographer/sensor/odometry_data.h"
00027 #include "cartographer/sensor/timed_point_cloud_data.h"
00028 #include "gtest/gtest.h"
00029
00030 namespace cartographer {
00031 namespace sensor {
00032 namespace {
00033
00034 using testing::CollatorInput;
00035 using testing::CollatorOutput;
00036
00037 TEST(TrajectoryCollator, OrderingMultipleTrajectories) {
00038 const int kTrajectoryId[] = {4, 7};
00039 const std::array<std::string, 2> kSensorId = {{"my_points", "some_imu"}};
00040
00041 std::vector<CollatorInput> input_data;
00042
00043 input_data.push_back(CollatorInput::CreateTimedPointCloudData(
00044 kTrajectoryId[0], kSensorId[0], 0));
00045 input_data.push_back(CollatorInput::CreateTimedPointCloudData(
00046 kTrajectoryId[1], kSensorId[0], 0));
00047 input_data.push_back(
00048 CollatorInput::CreateImuData(kTrajectoryId[1], kSensorId[1], 0));
00049 input_data.push_back(
00050 CollatorInput::CreateImuData(kTrajectoryId[0], kSensorId[1], 0));
00051 input_data.push_back(CollatorInput::CreateTimedPointCloudData(
00052 kTrajectoryId[1], kSensorId[0], 100));
00053 input_data.push_back(CollatorInput::CreateTimedPointCloudData(
00054 kTrajectoryId[0], kSensorId[0], 50));
00055 input_data.push_back(
00056 CollatorInput::CreateImuData(kTrajectoryId[0], kSensorId[1], 60));
00057 input_data.push_back(CollatorInput::CreateTimedPointCloudData(
00058 kTrajectoryId[1], kSensorId[0], 150));
00059 input_data.push_back(
00060 CollatorInput::CreateImuData(kTrajectoryId[1], kSensorId[1], 120));
00061
00062 std::vector<CollatorOutput> received;
00063 TrajectoryCollator collator;
00064 collator.AddTrajectory(
00065 kTrajectoryId[0],
00066 absl::flat_hash_set<std::string>(kSensorId.begin(), kSensorId.end()),
00067 [&received, kTrajectoryId](const std::string& sensor_id,
00068 std::unique_ptr<Data> data) {
00069 received.push_back(CollatorOutput(kTrajectoryId[0], data->GetSensorId(),
00070 data->GetTime()));
00071 });
00072 collator.AddTrajectory(
00073 kTrajectoryId[1],
00074 absl::flat_hash_set<std::string>(kSensorId.begin(), kSensorId.end()),
00075 [&received, kTrajectoryId](const std::string& sensor_id,
00076 std::unique_ptr<Data> data) {
00077 received.push_back(CollatorOutput(kTrajectoryId[1], data->GetSensorId(),
00078 data->GetTime()));
00079 });
00080
00081
00082 input_data[0].MoveToCollator(&collator);
00083 input_data[1].MoveToCollator(&collator);
00084 EXPECT_EQ(0, received.size());
00085 input_data[2].MoveToCollator(&collator);
00086 input_data[3].MoveToCollator(&collator);
00087 EXPECT_EQ(2, received.size());
00088 EXPECT_EQ(input_data[1].expected_output, received[0]);
00089 EXPECT_EQ(input_data[0].expected_output, received[1]);
00090
00091
00092 input_data[4].MoveToCollator(&collator);
00093 EXPECT_EQ(3, received.size());
00094 EXPECT_EQ(input_data[2].expected_output, received[2]);
00095
00096 input_data[5].MoveToCollator(&collator);
00097 EXPECT_EQ(4, received.size());
00098 EXPECT_EQ(input_data[3].expected_output, received[3]);
00099 input_data[6].MoveToCollator(&collator);
00100 EXPECT_EQ(5, received.size());
00101 EXPECT_EQ(input_data[5].expected_output, received[4]);
00102
00103
00104 input_data[7].MoveToCollator(&collator);
00105 EXPECT_EQ(5, received.size());
00106 input_data[8].MoveToCollator(&collator);
00107 EXPECT_EQ(7, received.size());
00108 EXPECT_EQ(input_data[4].expected_output, received[5]);
00109 EXPECT_EQ(input_data[8].expected_output, received[6]);
00110
00111 EXPECT_FALSE(collator.GetBlockingTrajectoryId().has_value());
00112
00113 collator.FinishTrajectory(kTrajectoryId[0]);
00114 collator.FinishTrajectory(kTrajectoryId[1]);
00115 collator.Flush();
00116 ASSERT_EQ(input_data.size(), received.size());
00117 }
00118
00119 }
00120 }
00121 }