collator_test.cc
Go to the documentation of this file.
1 /*
2  * Copyright 2016 The Cartographer Authors
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 #include "cartographer/sensor/internal/collator.h"
18 
19 #include <array>
20 #include <memory>
21 
22 #include "cartographer/common/make_unique.h"
23 #include "cartographer/common/time.h"
24 #include "cartographer/sensor/imu_data.h"
25 #include "cartographer/sensor/internal/test_helpers.h"
26 #include "cartographer/sensor/odometry_data.h"
27 #include "cartographer/sensor/timed_point_cloud_data.h"
28 #include "gtest/gtest.h"
29 
30 namespace cartographer {
31 namespace sensor {
32 namespace {
33 
34 using test::CollatorInput;
35 using test::CollatorOutput;
36 
37 TEST(Collator, Ordering) {
38  const int kTrajectoryId = 0;
39  const std::array<std::string, 4> kSensorId = {
40  {"horizontal_rangefinder", "vertical_rangefinder", "imu", "odometry"}};
41 
42  std::vector<CollatorInput> input_data;
43  // Send each sensor_id once to establish a common start time.
44  input_data.push_back(
45  CollatorInput::CreateTimedPointCloudData(kTrajectoryId, kSensorId[0], 0));
46  input_data.push_back(
47  CollatorInput::CreateTimedPointCloudData(kTrajectoryId, kSensorId[1], 0));
48  input_data.push_back(
49  CollatorInput::CreateImuData(kTrajectoryId, kSensorId[2], 0));
50  input_data.push_back(
51  CollatorInput::CreateOdometryData(kTrajectoryId, kSensorId[3], 0));
52 
53  input_data.push_back(CollatorInput::CreateTimedPointCloudData(
54  kTrajectoryId, kSensorId[0], 100));
55  input_data.push_back(CollatorInput::CreateTimedPointCloudData(
56  kTrajectoryId, kSensorId[1], 200));
57  input_data.push_back(
58  CollatorInput::CreateImuData(kTrajectoryId, kSensorId[2], 300));
59  input_data.push_back(CollatorInput::CreateTimedPointCloudData(
60  kTrajectoryId, kSensorId[0], 400));
61  input_data.push_back(CollatorInput::CreateTimedPointCloudData(
62  kTrajectoryId, kSensorId[1], 500));
63  input_data.push_back(
64  CollatorInput::CreateOdometryData(kTrajectoryId, kSensorId[3], 600));
65 
66  std::vector<CollatorOutput> received;
67  Collator collator;
68  collator.AddTrajectory(
69  kTrajectoryId,
70  std::unordered_set<std::string>(kSensorId.begin(), kSensorId.end()),
71  [&received, kTrajectoryId](const std::string& sensor_id,
72  std::unique_ptr<Data> data) {
73  received.push_back(CollatorOutput(kTrajectoryId, data->GetSensorId(),
74  data->GetTime()));
75  });
76 
77  input_data[0].MoveToCollator(&collator);
78  input_data[1].MoveToCollator(&collator);
79  input_data[2].MoveToCollator(&collator);
80  input_data[3].MoveToCollator(&collator);
81 
82  input_data[4].MoveToCollator(&collator);
83  input_data[9].MoveToCollator(&collator);
84  input_data[7].MoveToCollator(&collator);
85  input_data[5].MoveToCollator(&collator);
86  input_data[8].MoveToCollator(&collator);
87  input_data[6].MoveToCollator(&collator);
88  EXPECT_EQ(kTrajectoryId, collator.GetBlockingTrajectoryId().value());
89 
90  ASSERT_EQ(7, received.size());
91  EXPECT_EQ(input_data[4].expected_output, received[4]);
92  EXPECT_EQ(input_data[5].expected_output, received[5]);
93  EXPECT_EQ(input_data[6].expected_output, received[6]);
94 
95  collator.FinishTrajectory(kTrajectoryId);
96  collator.Flush();
97  ASSERT_EQ(input_data.size(), received.size());
98  for (size_t i = 4; i < input_data.size(); ++i) {
99  EXPECT_EQ(input_data[i].expected_output, received[i]);
100  }
101 }
102 
103 TEST(Collator, OrderingMultipleTrajectories) {
104  const int kTrajectoryId[] = {8, 5};
105  const std::array<std::string, 2> kSensorId = {{"my_points", "some_imu"}};
106 
107  std::vector<CollatorInput> input_data;
108  // Send each sensor_id once to establish a common start time.
109  input_data.push_back(CollatorInput::CreateTimedPointCloudData(
110  kTrajectoryId[0], kSensorId[0], 0));
111  input_data.push_back(
112  CollatorInput::CreateImuData(kTrajectoryId[0], kSensorId[1], 0));
113  input_data.push_back(CollatorInput::CreateTimedPointCloudData(
114  kTrajectoryId[1], kSensorId[0], 0));
115  input_data.push_back(
116  CollatorInput::CreateImuData(kTrajectoryId[1], kSensorId[1], 0));
117 
118  input_data.push_back(CollatorInput::CreateTimedPointCloudData(
119  kTrajectoryId[0], kSensorId[0], 100));
120  input_data.push_back(
121  CollatorInput::CreateImuData(kTrajectoryId[1], kSensorId[1], 200));
122  input_data.push_back(
123  CollatorInput::CreateImuData(kTrajectoryId[0], kSensorId[1], 300));
124  input_data.push_back(CollatorInput::CreateTimedPointCloudData(
125  kTrajectoryId[1], kSensorId[0], 400));
126  input_data.push_back(CollatorInput::CreateTimedPointCloudData(
127  kTrajectoryId[1], kSensorId[0], 400));
128  input_data.push_back(CollatorInput::CreateTimedPointCloudData(
129  kTrajectoryId[1], kSensorId[0], 500));
130  input_data.push_back(
131  CollatorInput::CreateImuData(kTrajectoryId[1], kSensorId[1], 600));
132 
133  std::vector<CollatorOutput> received;
134  Collator collator;
135  collator.AddTrajectory(
136  kTrajectoryId[0],
137  std::unordered_set<std::string>(kSensorId.begin(), kSensorId.end()),
138  [&received, kTrajectoryId](const std::string& sensor_id,
139  std::unique_ptr<Data> data) {
140  received.push_back(CollatorOutput(kTrajectoryId[0], data->GetSensorId(),
141  data->GetTime()));
142  });
143  collator.AddTrajectory(
144  kTrajectoryId[1],
145  std::unordered_set<std::string>(kSensorId.begin(), kSensorId.end()),
146  [&received, kTrajectoryId](const std::string& sensor_id,
147  std::unique_ptr<Data> data) {
148  received.push_back(CollatorOutput(kTrajectoryId[1], data->GetSensorId(),
149  data->GetTime()));
150  });
151 
152  input_data[0].MoveToCollator(&collator);
153  input_data[1].MoveToCollator(&collator);
154  input_data[2].MoveToCollator(&collator);
155  input_data[3].MoveToCollator(&collator);
156 
157  input_data[4].MoveToCollator(&collator);
158  input_data[6].MoveToCollator(&collator);
159  EXPECT_EQ(kTrajectoryId[1], collator.GetBlockingTrajectoryId().value());
160  input_data[7].MoveToCollator(&collator);
161  input_data[8].MoveToCollator(&collator);
162  EXPECT_EQ(kTrajectoryId[1], collator.GetBlockingTrajectoryId().value());
163  input_data[5].MoveToCollator(&collator);
164  EXPECT_EQ(kTrajectoryId[0], collator.GetBlockingTrajectoryId().value());
165  input_data[10].MoveToCollator(&collator);
166  input_data[9].MoveToCollator(&collator);
167  EXPECT_EQ(kTrajectoryId[0], collator.GetBlockingTrajectoryId().value());
168 
169  ASSERT_EQ(5, received.size());
170  EXPECT_EQ(input_data[4].expected_output, received[4]);
171 
172  collator.FinishTrajectory(kTrajectoryId[0]);
173  collator.FinishTrajectory(kTrajectoryId[1]);
174  collator.Flush();
175  ASSERT_EQ(input_data.size(), received.size());
176  for (size_t i = 4; i < input_data.size(); ++i) {
177  EXPECT_EQ(input_data[i].expected_output, received[i]);
178  }
179 }
180 
181 } // namespace
182 } // namespace sensor
183 } // namespace cartographer
timed_point_cloud_data.h
odometry_data.h
imu_data.h
make_unique.h
test_helpers.h
cartographer::sensor::test::CollatorOutput
std::tuple< int, std::string, common::Time > CollatorOutput
Definition: sensor/internal/test_helpers.h:43
cartographer::mapping::TEST
TEST(TrajectoryConnectivityStateTest, UnknownTrajectory)
Definition: trajectory_connectivity_state_test.cc:24

cartographer
Author(s): The Cartographer Authors
autogenerated on Mon Feb 28 2022 22:00:58