00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "cartographer/sensor/internal/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(Collator, Ordering) {
00038 const int kTrajectoryId = 0;
00039 const std::array<std::string, 4> kSensorId = {
00040 {"horizontal_rangefinder", "vertical_rangefinder", "imu", "odometry"}};
00041
00042 std::vector<CollatorInput> input_data;
00043
00044 input_data.push_back(
00045 CollatorInput::CreateTimedPointCloudData(kTrajectoryId, kSensorId[0], 0));
00046 input_data.push_back(
00047 CollatorInput::CreateTimedPointCloudData(kTrajectoryId, kSensorId[1], 0));
00048 input_data.push_back(
00049 CollatorInput::CreateImuData(kTrajectoryId, kSensorId[2], 0));
00050 input_data.push_back(
00051 CollatorInput::CreateOdometryData(kTrajectoryId, kSensorId[3], 0));
00052
00053 input_data.push_back(CollatorInput::CreateTimedPointCloudData(
00054 kTrajectoryId, kSensorId[0], 100));
00055 input_data.push_back(CollatorInput::CreateTimedPointCloudData(
00056 kTrajectoryId, kSensorId[1], 200));
00057 input_data.push_back(
00058 CollatorInput::CreateImuData(kTrajectoryId, kSensorId[2], 300));
00059 input_data.push_back(CollatorInput::CreateTimedPointCloudData(
00060 kTrajectoryId, kSensorId[0], 400));
00061 input_data.push_back(CollatorInput::CreateTimedPointCloudData(
00062 kTrajectoryId, kSensorId[1], 500));
00063 input_data.push_back(
00064 CollatorInput::CreateOdometryData(kTrajectoryId, kSensorId[3], 600));
00065
00066 std::vector<CollatorOutput> received;
00067 Collator collator;
00068 collator.AddTrajectory(
00069 kTrajectoryId,
00070 absl::flat_hash_set<std::string>(kSensorId.begin(), kSensorId.end()),
00071 [&received, kTrajectoryId](const std::string& sensor_id,
00072 std::unique_ptr<Data> data) {
00073 received.push_back(CollatorOutput(kTrajectoryId, data->GetSensorId(),
00074 data->GetTime()));
00075 });
00076
00077 input_data[0].MoveToCollator(&collator);
00078 input_data[1].MoveToCollator(&collator);
00079 input_data[2].MoveToCollator(&collator);
00080 input_data[3].MoveToCollator(&collator);
00081
00082 input_data[4].MoveToCollator(&collator);
00083 input_data[9].MoveToCollator(&collator);
00084 input_data[7].MoveToCollator(&collator);
00085 input_data[5].MoveToCollator(&collator);
00086 input_data[8].MoveToCollator(&collator);
00087 input_data[6].MoveToCollator(&collator);
00088 EXPECT_EQ(kTrajectoryId, collator.GetBlockingTrajectoryId().value());
00089
00090 ASSERT_EQ(7, received.size());
00091 EXPECT_EQ(input_data[4].expected_output, received[4]);
00092 EXPECT_EQ(input_data[5].expected_output, received[5]);
00093 EXPECT_EQ(input_data[6].expected_output, received[6]);
00094
00095 collator.FinishTrajectory(kTrajectoryId);
00096 collator.Flush();
00097 ASSERT_EQ(input_data.size(), received.size());
00098 for (size_t i = 4; i < input_data.size(); ++i) {
00099 EXPECT_EQ(input_data[i].expected_output, received[i]);
00100 }
00101 }
00102
00103 TEST(Collator, OrderingMultipleTrajectories) {
00104 const int kTrajectoryId[] = {8, 5};
00105 const std::array<std::string, 2> kSensorId = {{"my_points", "some_imu"}};
00106
00107 std::vector<CollatorInput> input_data;
00108
00109 input_data.push_back(CollatorInput::CreateTimedPointCloudData(
00110 kTrajectoryId[0], kSensorId[0], 0));
00111 input_data.push_back(
00112 CollatorInput::CreateImuData(kTrajectoryId[0], kSensorId[1], 0));
00113 input_data.push_back(CollatorInput::CreateTimedPointCloudData(
00114 kTrajectoryId[1], kSensorId[0], 0));
00115 input_data.push_back(
00116 CollatorInput::CreateImuData(kTrajectoryId[1], kSensorId[1], 0));
00117
00118 input_data.push_back(CollatorInput::CreateTimedPointCloudData(
00119 kTrajectoryId[0], kSensorId[0], 100));
00120 input_data.push_back(
00121 CollatorInput::CreateImuData(kTrajectoryId[1], kSensorId[1], 200));
00122 input_data.push_back(
00123 CollatorInput::CreateImuData(kTrajectoryId[0], kSensorId[1], 300));
00124 input_data.push_back(CollatorInput::CreateTimedPointCloudData(
00125 kTrajectoryId[1], kSensorId[0], 400));
00126 input_data.push_back(CollatorInput::CreateTimedPointCloudData(
00127 kTrajectoryId[1], kSensorId[0], 400));
00128 input_data.push_back(CollatorInput::CreateTimedPointCloudData(
00129 kTrajectoryId[1], kSensorId[0], 500));
00130 input_data.push_back(
00131 CollatorInput::CreateImuData(kTrajectoryId[1], kSensorId[1], 600));
00132
00133 std::vector<CollatorOutput> received;
00134 Collator collator;
00135 collator.AddTrajectory(
00136 kTrajectoryId[0],
00137 absl::flat_hash_set<std::string>(kSensorId.begin(), kSensorId.end()),
00138 [&received, kTrajectoryId](const std::string& sensor_id,
00139 std::unique_ptr<Data> data) {
00140 received.push_back(CollatorOutput(kTrajectoryId[0], data->GetSensorId(),
00141 data->GetTime()));
00142 });
00143 collator.AddTrajectory(
00144 kTrajectoryId[1],
00145 absl::flat_hash_set<std::string>(kSensorId.begin(), kSensorId.end()),
00146 [&received, kTrajectoryId](const std::string& sensor_id,
00147 std::unique_ptr<Data> data) {
00148 received.push_back(CollatorOutput(kTrajectoryId[1], data->GetSensorId(),
00149 data->GetTime()));
00150 });
00151
00152 input_data[0].MoveToCollator(&collator);
00153 input_data[1].MoveToCollator(&collator);
00154 input_data[2].MoveToCollator(&collator);
00155 input_data[3].MoveToCollator(&collator);
00156
00157 input_data[4].MoveToCollator(&collator);
00158 input_data[6].MoveToCollator(&collator);
00159 EXPECT_EQ(kTrajectoryId[1], collator.GetBlockingTrajectoryId().value());
00160 input_data[7].MoveToCollator(&collator);
00161 input_data[8].MoveToCollator(&collator);
00162 EXPECT_EQ(kTrajectoryId[1], collator.GetBlockingTrajectoryId().value());
00163 input_data[5].MoveToCollator(&collator);
00164 EXPECT_EQ(kTrajectoryId[0], collator.GetBlockingTrajectoryId().value());
00165 input_data[10].MoveToCollator(&collator);
00166 input_data[9].MoveToCollator(&collator);
00167 EXPECT_EQ(kTrajectoryId[0], collator.GetBlockingTrajectoryId().value());
00168
00169 ASSERT_EQ(5, received.size());
00170 EXPECT_EQ(input_data[4].expected_output, received[4]);
00171
00172 collator.FinishTrajectory(kTrajectoryId[0]);
00173 collator.FinishTrajectory(kTrajectoryId[1]);
00174 collator.Flush();
00175 ASSERT_EQ(input_data.size(), received.size());
00176 for (size_t i = 4; i < input_data.size(); ++i) {
00177 EXPECT_EQ(input_data[i].expected_output, received[i]);
00178 }
00179 }
00180
00181 }
00182 }
00183 }