Formula Student Autonomous Systems
The code for the main driverless system
Loading...
Searching...
No Matches
integration_tests.cpp
Go to the documentation of this file.
1#include "adapter_planning/vehicle.hpp"
2#include "common_lib/communication/interfaces.hpp"
3#include "planning/planning.hpp"
4#include "test_utils/utils.hpp"
5
6class IntegrationTest : public ::testing::Test {
7protected:
8 // Required Nodes
9 std::shared_ptr<rclcpp::Node> locmap_sender;
10 std::shared_ptr<rclcpp::Node> control_receiver;
11 std::shared_ptr<Planning> planning_test_;
12
13 custom_interfaces::msg::ConeArray cone_array_msg; // message to receive
14 custom_interfaces::msg::PathPointArray received_path; // message to send
15
16 // Publisher and Subscriber
17 std::shared_ptr<rclcpp::Publisher<custom_interfaces::msg::ConeArray>> map_publisher;
18 std::shared_ptr<rclcpp::Publisher<custom_interfaces::msg::Pose>> vehicle_pose_publisher_;
19 std::shared_ptr<rclcpp::Subscription<custom_interfaces::msg::PathPointArray>> control_sub;
20
21 void SetUp() override {
22 rclcpp::init(0, nullptr);
23
24 // Init Nodes
25 control_receiver = rclcpp::Node::make_shared("control_receiver"); // gets path from planning
26 locmap_sender = rclcpp::Node::make_shared("locmap_sender"); // publishes map from
27
28 std::string adapter;
29 PlanningParameters params = Planning::load_config(adapter);
30
31 planning_test_ = std::make_shared<VehicleAdapter>(params);
32 planning_test_->set_mission(Mission::TRACKDRIVE);
33
34 cone_array_msg = custom_interfaces::msg::ConeArray(); // init received message
35
36 // Init Publisher
37 map_publisher = locmap_sender->create_publisher<custom_interfaces::msg::ConeArray>(
38 "/state_estimation/map", 10);
39
40 vehicle_pose_publisher_ =
41 locmap_sender->create_publisher<custom_interfaces::msg::Pose>(
42 "/state_estimation/vehicle_pose", 10);
43
44 // Init Subscriber
45 control_sub = control_receiver->create_subscription<custom_interfaces::msg::PathPointArray>(
46 "/path_planning/path", 10,
47 [this](const custom_interfaces::msg::PathPointArray::SharedPtr msg) {
48 RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Received path in mock control node");
49 received_path = *msg;
50 (void)rclcpp::shutdown();
51 RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Ended control callback in mock control");
52 });
53 }
54
55 void TearDown() override {
56 control_receiver.reset();
57 locmap_sender.reset();
58 map_publisher.reset();
59 control_sub.reset();
60 // planning_test_.reset();
61 vehicle_pose_publisher_.reset();
62 (void)rclcpp::shutdown();
63 }
64
65 std::chrono::duration<double, std::milli> run_nodes(
66 const custom_interfaces::msg::ConeArray &track_msg,
67 const custom_interfaces::msg::Pose &state_msg) {
68 this->map_publisher->publish(track_msg); // send the cones
69 this->vehicle_pose_publisher_->publish(state_msg);
70
71 // Add nodes to be executed
72 rclcpp::executors::MultiThreadedExecutor executor;
73 executor.add_node(this->locmap_sender);
74 executor.add_node(this->control_receiver);
75 executor.add_node(this->planning_test_);
76
77 this->map_publisher->publish(track_msg); // send the cones
78 this->vehicle_pose_publisher_->publish(state_msg);
79
80 auto start_time = std::chrono::high_resolution_clock::now();
81 executor.spin(); // Execute nodes
82 auto end_time = std::chrono::high_resolution_clock::now();
83 return std::chrono::duration<double, std::milli>(end_time - start_time);
84 }
85};
86
93TEST_F(IntegrationTest, PUBLISH_PATH1) {
94 std::vector<Cone> cone_array = {
95 Cone(19, 2), Cone(22, 2), Cone(25, 2), Cone(28, 2), Cone(31, 2), Cone(34, 2),
96 Cone(1, 2), Cone(4, 2), Cone(7, 2), Cone(10, 2), Cone(13, 2), Cone(16, 2),
97 Cone(19, -2), Cone(22, -2), Cone(25, -2), Cone(28, -2), Cone(31, -2), Cone(34, -2),
98 Cone(1, -2), Cone(4, -2), Cone(7, -2), Cone(10, -2), Cone(13, -2), Cone(16, -2)};
99 this->cone_array_msg = common_lib::communication::custom_interfaces_array_from_vector(cone_array);
100 custom_interfaces::msg::Pose vehicle_pose;
101 vehicle_pose.x = 0;
102 vehicle_pose.y = 0;
103 vehicle_pose.theta = 0;
104
105 RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Publishing cone array with size: %ld",
106 cone_array_msg.cone_array.size());
107
108 const auto duration = run_nodes(cone_array_msg, vehicle_pose);
109
110 RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Execution time: %f ms", duration.count());
111 for (const auto &p : received_path.pathpoint_array) {
112 EXPECT_NEAR(p.y, 0, 1e-10);
113 EXPECT_LE(p.x, 35.5);
114 EXPECT_GE(p.x, -2);
115 }
116 EXPECT_GE(static_cast<long unsigned>(received_path.pathpoint_array.size()),
117 (long unsigned int)100);
118}
119
124TEST_F(IntegrationTest, PUBLISH_PATH2) {
125 std::vector<Cone> cone_array = {
126 Cone((float)1.414, (float)-1.414), Cone((float)3.184, (float)0.356),
127 Cone((float)4.954, (float)2.126), Cone((float)6.724, (float)3.896),
128 Cone((float)8.494, (float)5.666), Cone((float)10.264, (float)7.436),
129 Cone((float)12.034, (float)9.206), Cone((float)13.804, (float)10.976),
130 Cone((float)15.574, (float)12.746), Cone((float)17.344, (float)14.516),
131 Cone((float)19.114, (float)16.286), Cone((float)20.884, (float)18.056),
132 Cone((float)-1.414, (float)1.414), Cone((float)0.356, (float)3.184),
133 Cone((float)2.126, (float)4.954), Cone((float)3.896, (float)6.724),
134 Cone((float)5.666, (float)8.494), Cone((float)7.436, (float)10.264),
135 Cone((float)9.206, (float)12.034), Cone((float)10.976, (float)13.804),
136 Cone((float)12.746, (float)15.574), Cone((float)14.516, (float)17.344),
137 Cone((float)16.286, (float)19.114), Cone((float)18.056, (float)20.884)};
138 this->cone_array_msg = common_lib::communication::custom_interfaces_array_from_vector(cone_array);
139
140 custom_interfaces::msg::Pose vehicle_pose;
141 vehicle_pose.x = 0;
142 vehicle_pose.y = 0;
143 vehicle_pose.theta = 0.785398; // 45 degrees
144
145 RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Publishing cone array with size: %ld",
146 cone_array_msg.cone_array.size());
147 const auto duration = run_nodes(cone_array_msg, vehicle_pose);
148
149 RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Execution time: %f ms", duration.count());
150 for (const auto &p : received_path.pathpoint_array) {
151 EXPECT_LE(p.y - p.x, 0.1);
152 }
153 EXPECT_GE(static_cast<long unsigned>(received_path.pathpoint_array.size()),
154 (long unsigned int)100);
155}
156
161TEST_F(IntegrationTest, PUBLISH_PATH3) {
162 custom_interfaces::msg::Cone cone_to_send;
163
164 std::vector<Cone> cone_array = {
165 Cone((float)-1.414, (float)-1.414), Cone((float)-3.184, (float)0.356),
166 Cone((float)-4.954, (float)2.126), Cone((float)-6.724, (float)3.896),
167 Cone((float)-8.494, (float)5.666), Cone((float)-10.264, (float)7.436),
168 Cone((float)-12.034, (float)9.206), Cone((float)-13.804, (float)10.976),
169 Cone((float)-15.574, (float)12.746), Cone((float)-17.344, (float)14.516),
170 Cone((float)-19.114, (float)16.286), Cone((float)-20.884, (float)18.056),
171 Cone((float)1.414, (float)1.414), Cone((float)-0.356, (float)3.184),
172 Cone((float)-2.126, (float)4.954), Cone((float)-3.896, (float)6.724),
173 Cone((float)-5.666, (float)8.494), Cone((float)-7.436, (float)10.264),
174 Cone((float)-9.206, (float)12.034), Cone((float)-10.976, (float)13.804),
175 Cone((float)-12.746, (float)15.574), Cone((float)-14.516, (float)17.344),
176 Cone((float)-16.286, (float)19.114), Cone((float)-18.056, (float)20.884)};
177 this->cone_array_msg = common_lib::communication::custom_interfaces_array_from_vector(cone_array);
178
179 custom_interfaces::msg::Pose vehicle_pose;
180 vehicle_pose.x = 0;
181 vehicle_pose.y = 0;
182 vehicle_pose.theta = 2.3561945; // 135 degrees
183
184 RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Publishing cone array with size: %ld",
185 cone_array_msg.cone_array.size());
186 const auto duration = run_nodes(cone_array_msg, vehicle_pose);
187 RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Execution time: %f ms", duration.count());
188 for (const auto &p : received_path.pathpoint_array) {
189 EXPECT_LE(p.y + p.x, 0.1);
190 }
191 EXPECT_GE(static_cast<long unsigned>(received_path.pathpoint_array.size()),
192 (long unsigned int)100);
193}
194
199TEST_F(IntegrationTest, PUBLISH_PATH4) {
200 std::vector<Cone> cone_array = {
201 Cone((float)-1.414, (float)1.414), Cone((float)-3.184, (float)-0.356),
202 Cone((float)-4.954, (float)-2.126), Cone((float)-6.724, (float)-3.896),
203 Cone((float)-8.494, (float)-5.666), Cone((float)-10.264, (float)-7.436),
204 Cone((float)-12.034, (float)-9.206), Cone((float)-13.804, (float)-10.976),
205 Cone((float)-15.574, (float)-12.746), Cone((float)-17.344, (float)-14.516),
206 Cone((float)-19.114, (float)-16.286), Cone((float)-20.884, (float)-18.056),
207 Cone((float)1.414, (float)-1.414), Cone((float)-0.356, (float)-3.184),
208 Cone((float)-2.126, (float)-4.954), Cone((float)-3.896, (float)-6.724),
209 Cone((float)-5.666, (float)-8.494), Cone((float)-7.436, (float)-10.264),
210 Cone((float)-9.206, (float)-12.034), Cone((float)-10.976, (float)-13.804),
211 Cone((float)-12.746, (float)-15.574), Cone((float)-14.516, (float)-17.344),
212 Cone((float)-16.286, (float)-19.114), Cone((float)-18.056, (float)-20.884)};
213 this->cone_array_msg = common_lib::communication::custom_interfaces_array_from_vector(cone_array);
214 custom_interfaces::msg::Pose vehicle_pose;
215 vehicle_pose.x = 0;
216 vehicle_pose.y = 0;
217 vehicle_pose.theta = 3.92699; // 225 degrees
218
219 RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Publishing cone array with size: %ld",
220 cone_array_msg.cone_array.size());
221 const auto duration = run_nodes(cone_array_msg, vehicle_pose);
222
223 RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Execution time: %f ms", duration.count());
224 for (const auto &p : received_path.pathpoint_array) {
225 EXPECT_LE(p.y - p.x, 0.1);
226 }
227 EXPECT_GE(static_cast<long unsigned>(received_path.pathpoint_array.size()),
228 (long unsigned int)100);
229}
230
235TEST_F(IntegrationTest, PUBLISH_PATH5) {
236 std::vector<Cone> cone_array = {
237 Cone((float)1.414, (float)1.414), Cone((float)3.184, (float)-0.356),
238 Cone((float)4.954, (float)-2.126), Cone((float)6.724, (float)-3.896),
239 Cone((float)8.494, (float)-5.666), Cone((float)10.264, (float)-7.436),
240 Cone((float)12.034, (float)-9.206), Cone((float)13.804, (float)-10.976),
241 Cone((float)15.574, (float)-12.746), Cone((float)17.344, (float)-14.516),
242 Cone((float)19.114, (float)-16.286), Cone((float)20.884, (float)-18.056),
243 Cone((float)-1.414, (float)-1.414), Cone((float)0.356, (float)-3.184),
244 Cone((float)2.126, (float)-4.954), Cone((float)3.896, (float)-6.724),
245 Cone((float)5.666, (float)-8.494), Cone((float)7.436, (float)-10.264),
246 Cone((float)9.206, (float)-12.034), Cone((float)10.976, (float)-13.804),
247 Cone((float)12.746, (float)-15.574), Cone((float)14.516, (float)-17.344),
248 Cone((float)16.286, (float)-19.114), Cone((float)18.056, (float)-20.884)};
249 this->cone_array_msg = common_lib::communication::custom_interfaces_array_from_vector(cone_array);
250 custom_interfaces::msg::Pose vehicle_pose;
251 vehicle_pose.x = 0;
252 vehicle_pose.y = 0;
253 vehicle_pose.theta = 5.49779; // 315 degrees
254
255 RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Publishing cone array with size: %ld",
256 cone_array_msg.cone_array.size());
257 const auto duration = run_nodes(cone_array_msg, vehicle_pose);
258
259 RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Execution time: %f ms", duration.count());
260 for (const auto &p : received_path.pathpoint_array) {
261 EXPECT_LE(p.y + p.x, 0.1);
262 }
263 EXPECT_GE(static_cast<long unsigned>(received_path.pathpoint_array.size()),
264 (long unsigned int)100);
265}
266
271TEST_F(IntegrationTest, PUBLISH_PATH6) {
272 std::vector<Cone> cone_array = {
273 Cone(2, 19), Cone(2, 22), Cone(2, 25), Cone(2, 28), Cone(2, 31), Cone(2, 34),
274 Cone(2, 1), Cone(2, 4), Cone(2, 7), Cone(2, 10), Cone(2, 13), Cone(2, 16),
275 Cone(-2, 1), Cone(-2, 4), Cone(-2, 7), Cone(-2, 25), Cone(-2, 28), Cone(-2, 31),
276 Cone(-2, 34), Cone(-2, 10), Cone(-2, 13), Cone(-2, 16), Cone(-2, 19), Cone(-2, 22)};
277 this->cone_array_msg = common_lib::communication::custom_interfaces_array_from_vector(cone_array);
278 custom_interfaces::msg::Pose vehicle_pose;
279 vehicle_pose.x = 0;
280 vehicle_pose.y = 0;
281 vehicle_pose.theta = 1.5708; // 90 degrees
282
283 RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Publishing cone array with size: %ld",
284 cone_array_msg.cone_array.size());
285 const auto duration = run_nodes(cone_array_msg, vehicle_pose);
286
287 RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Execution time: %f ms", duration.count());
288 for (const auto &p : received_path.pathpoint_array) {
289 EXPECT_LE(fabs(p.x), 0.1);
290 EXPECT_LE(p.y, 35.5);
291 EXPECT_GE(p.y, -1);
292 }
293 EXPECT_GE(static_cast<long unsigned>(received_path.pathpoint_array.size()),
294 (long unsigned int)100);
295}
296
301TEST_F(IntegrationTest, no_cones) {
302 custom_interfaces::msg::Pose vehicle_pose;
303 vehicle_pose.x = 0;
304 vehicle_pose.y = 0;
305 vehicle_pose.theta = 1.5708; // 90 degrees
306
307 RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Publishing cone array with size: %ld",
308 cone_array_msg.cone_array.size());
309
310 const auto duration = run_nodes(this->cone_array_msg, vehicle_pose);
311
312 RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Execution time: %f ms", duration.count());
313 EXPECT_EQ(static_cast<long unsigned>(received_path.pathpoint_array.size()), (long unsigned int)0);
314}
315
320TEST_F(IntegrationTest, one_cone) {
321 custom_interfaces::msg::Cone cone_to_send;
322
323 cone_to_send.position.x = 2;
324 cone_to_send.position.y = 19;
325 cone_to_send.color = "yellow_cone";
326 cone_array_msg.cone_array.push_back(cone_to_send);
327
328 custom_interfaces::msg::Pose vehicle_pose;
329 vehicle_pose.x = 0;
330 vehicle_pose.y = 0;
331 vehicle_pose.theta = 1.5708; // 90 degrees
332
333 RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Publishing cone array with size: %ld",
334 cone_array_msg.cone_array.size());
335 const auto duration = run_nodes(cone_array_msg, vehicle_pose);
336
337 RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Execution time: %f ms", duration.count());
338 EXPECT_EQ(static_cast<long unsigned>(received_path.pathpoint_array.size()), (long unsigned int)0);
339}
340
341
358void read_from_file(std::ifstream &file, double &finalxi, double &finalxf, double &finalyi,
359 double &finalyf, std::vector<Cone> &cone_array,
360 custom_interfaces::msg::Pose &vehicle_pose) {
361 std::string line;
362
363 while (std::getline(file, line)) {
364 std::istringstream iss(line);
365 std::string flag;
366 iss >> flag;
367 if (flag == "P") {
368 // Read initial vehicle state
369 iss >> vehicle_pose.x >> vehicle_pose.y >> vehicle_pose.theta;
370 } else if (flag == "C") {
371 Cone cone;
372 std::string color;
373 iss >> cone.position.x >> cone.position.y >> color;
374 cone.color = common_lib::competition_logic::get_color_enum(color);
375 cone_array.push_back(cone);
376 } else if (flag == "F") {
377 // Read final expected position range
378 iss >> finalxi >> finalxf >> finalyi >> finalyf;
379 } else {
380 // Ignore all other lines with no flags
381 }
382 }
383 file.close();
384}
385
398void save_debug_file(const std::string filename, const std::vector<Cone> cone_array,
399 const custom_interfaces::msg::PathPointArray received_path,
400 const custom_interfaces::msg::Pose vehicle_pose) {
401 std::ofstream debug_file("../../src/planning/test/integration_tests/results/" + filename);
402 if (debug_file.is_open()) {
403 for (const auto &point : received_path.pathpoint_array) {
404 debug_file << "P"
405 << " " << point.x << " " << point.y << "\n";
406 }
407 // also write the cone array
408 for (const auto &cone : cone_array) {
409 debug_file << "C " << cone.position.x << " " << cone.position.y << " "
410 << common_lib::competition_logic::get_color_string(cone.color) << "\n";
411 }
412 debug_file << "V " << vehicle_pose.x << " " << vehicle_pose.y << " "
413 << vehicle_pose.theta << "\n";
414 debug_file.close();
415 } else {
416 std::cerr << "Failed to open debug file for writing.\n";
417 }
418}
419
424TEST_F(IntegrationTest, simple_straight_path) {
425 // file with the testing scenario
426 const std::string filename = "straight_1.txt";
427
428 // box of the final point
429 double x1 = 0.0, x2 = 0.0, y1 = 0.0, y2 = 0.0;
430
431 // Open file straight_1.txt to read the initial state
432 std::ifstream file("../../src/planning/test/integration_tests/tests/" + filename);
433
434 if (!file.is_open()) {
435 FAIL() << "Failed to open file: straight_1.txt";
436 return;
437 }
438
439 std::vector<Cone> cone_array;
440 custom_interfaces::msg::Pose vehicle_pose;
441 ::read_from_file(file, x1, x2, y1, y2, cone_array, vehicle_pose);
442
443 // Convert the cone array to the message
444 this->cone_array_msg = common_lib::communication::custom_interfaces_array_from_vector(cone_array);
445 // Run the nodes
446 const auto duration = run_nodes(cone_array_msg, vehicle_pose);
447
448 RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Execution time: %f ms", duration.count());
449
450 ::save_debug_file(filename, cone_array, this->received_path, vehicle_pose);
451
452 // Verify final position
453 if ((this->received_path.pathpoint_array.back().x >= x1 &&
454 this->received_path.pathpoint_array.back().x <= x2) &&
455 (this->received_path.pathpoint_array.back().y >= y1 &&
456 this->received_path.pathpoint_array.back().y <= y2)) {
457 SUCCEED();
458 } else {
459 FAIL() << "The final point is not in the expected range. "
460 << "Expected range: (" << x1 << ", " << x2 << ", " << y1 << ", " << y2 << "), but got: ("
461 << this->received_path.pathpoint_array.back().x << ", "
462 << this->received_path.pathpoint_array.back().y << ")\n\n";
463 }
464}
465
469TEST_F(IntegrationTest, unbalanced_STRAIGHT_PATH) {
470 // file with the testing scenario
471 const std::string filename = "straight_2.txt";
472
473 // box of the final point
474 double x1 = 0.0, x2 = 0.0, y1 = 0.0, y2 = 0.0;
475
476 // Open file straight_1.txt to read the initial state
477 std::ifstream file("../../src/planning/test/integration_tests/tests/" + filename);
478
479 if (!file.is_open()) {
480 FAIL() << "Failed to open file: straight_1.txt";
481 return;
482 }
483
484 std::vector<Cone> cone_array;
485 custom_interfaces::msg::Pose vehicle_pose;
486 ::read_from_file(file, x1, x2, y1, y2, cone_array, vehicle_pose);
487
488 // Convert the cone array to the message
489 this->cone_array_msg = common_lib::communication::custom_interfaces_array_from_vector(cone_array);
490 // Run the nodes
491 const auto duration = run_nodes(cone_array_msg, vehicle_pose);
492 RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Execution time: %f ms", duration.count());
493
494 ::save_debug_file(filename, cone_array, this->received_path, vehicle_pose);
495
496 // Verify final position
497 if ((this->received_path.pathpoint_array.back().x >= x1 &&
498 this->received_path.pathpoint_array.back().x <= x2) &&
499 (this->received_path.pathpoint_array.back().y >= y1 &&
500 this->received_path.pathpoint_array.back().y <= y2)) {
501 SUCCEED();
502 } else {
503 FAIL() << "The final point is not in the expected range. "
504 << "Expected range: (" << x1 << ", " << x2 << ", " << y1 << ", " << y2 << "), but got: ("
505 << this->received_path.pathpoint_array.back().x << ", "
506 << this->received_path.pathpoint_array.back().y << ")\n\n";
507 }
508}
509
513TEST_F(IntegrationTest, FULL_CURVE_PATH) {
514 // file with the testing scenario
515 const std::string filename = "curve_1.txt";
516
517 // box of the final point
518 double x1 = 0.0, x2 = 0.0, y1 = 0.0, y2 = 0.0;
519
520 // Open file straight_1.txt to read the initial state
521 std::ifstream file("../../src/planning/test/integration_tests/tests/" + filename);
522
523 if (!file.is_open()) {
524 FAIL() << "Failed to open file: straight_1.txt";
525 return;
526 }
527
528 std::vector<Cone> cone_array;
529 custom_interfaces::msg::Pose vehicle_pose;
530 ::read_from_file(file, x1, x2, y1, y2, cone_array, vehicle_pose);
531
532 // Convert the cone array to the message
533 this->cone_array_msg = common_lib::communication::custom_interfaces_array_from_vector(cone_array);
534 // Run the nodes
535 const auto duration = run_nodes(cone_array_msg, vehicle_pose);
536 RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Execution time: %f ms", duration.count());
537
538 ::save_debug_file(filename, cone_array, this->received_path, vehicle_pose);
539
540 // Verify final position
541 if ((this->received_path.pathpoint_array.back().x >= x1 &&
542 this->received_path.pathpoint_array.back().x <= x2) &&
543 (this->received_path.pathpoint_array.back().y >= y1 &&
544 this->received_path.pathpoint_array.back().y <= y2)) {
545 SUCCEED();
546 } else {
547 FAIL() << "The final point is not in the expected range. "
548 << "Expected range: (" << x1 << ", " << x2 << ", " << y1 << ", " << y2 << "), but got: ("
549 << this->received_path.pathpoint_array.back().x << ", "
550 << this->received_path.pathpoint_array.back().y << ")\n\n";
551 }
552}
553
554
558TEST_F(IntegrationTest, CURVES_AND_CLOSE_TRACKSIDES) {
559 // file with the testing scenario
560 const std::string filename = "curve_2.txt";
561
562 // box of the final point
563 double x1 = 0.0, x2 = 0.0, y1 = 0.0, y2 = 0.0;
564
565 // Open file straight_1.txt to read the initial state
566 std::ifstream file("../../src/planning/test/integration_tests/tests/" + filename);
567
568 if (!file.is_open()) {
569 FAIL() << "Failed to open file: straight_1.txt";
570 return;
571 }
572
573 std::vector<Cone> cone_array;
574 custom_interfaces::msg::Pose vehicle_pose;
575 ::read_from_file(file, x1, x2, y1, y2, cone_array, vehicle_pose);
576
577 // Convert the cone array to the message
578 this->cone_array_msg = common_lib::communication::custom_interfaces_array_from_vector(cone_array);
579 // Run the nodes
580 const auto duration = run_nodes(cone_array_msg, vehicle_pose);
581 RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Execution time: %f ms", duration.count());
582
583 ::save_debug_file(filename, cone_array, this->received_path, vehicle_pose);
584
585 // Verify final position
586 if ((this->received_path.pathpoint_array.back().x >= x1 &&
587 this->received_path.pathpoint_array.back().x <= x2) &&
588 (this->received_path.pathpoint_array.back().y >= y1 &&
589 this->received_path.pathpoint_array.back().y <= y2)) {
590 SUCCEED();
591 } else {
592 FAIL() << "The final point is not in the expected range. "
593 << "Expected range: (" << x1 << ", " << x2 << ", " << y1 << ", " << y2 << "), but got: ("
594 << this->received_path.pathpoint_array.back().x << ", "
595 << this->received_path.pathpoint_array.back().y << ")\n\n";
596 }
597}
598
602TEST_F(IntegrationTest, SHARP_SINOSOIDAL_CURVE) {
603 // file with the testing scenario
604 const std::string filename = "curve_3.txt";
605
606 // box of the final point
607 double x1 = 0.0, x2 = 0.0, y1 = 0.0, y2 = 0.0;
608
609 // Open file straight_1.txt to read the initial state
610 std::ifstream file("../../src/planning/test/integration_tests/tests/" + filename);
611
612 if (!file.is_open()) {
613 FAIL() << "Failed to open file: straight_1.txt";
614 return;
615 }
616
617 std::vector<Cone> cone_array;
618 custom_interfaces::msg::Pose vehicle_pose;
619 ::read_from_file(file, x1, x2, y1, y2, cone_array, vehicle_pose);
620
621 // Convert the cone array to the message
622 this->cone_array_msg = common_lib::communication::custom_interfaces_array_from_vector(cone_array);
623 // Run the nodes
624 const auto duration = run_nodes(cone_array_msg, vehicle_pose);
625 RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Execution time: %f ms", duration.count());
626
627 ::save_debug_file(filename, cone_array, this->received_path, vehicle_pose);
628
629 // Verify final position
630 if ((this->received_path.pathpoint_array.back().x >= x1 &&
631 this->received_path.pathpoint_array.back().x <= x2) &&
632 (this->received_path.pathpoint_array.back().y >= y1 &&
633 this->received_path.pathpoint_array.back().y <= y2)) {
634 SUCCEED();
635 } else {
636 FAIL() << "The final point is not in the expected range. "
637 << "Expected range: (" << x1 << ", " << x2 << ", " << y1 << ", " << y2 << "), but got: ("
638 << this->received_path.pathpoint_array.back().x << ", "
639 << this->received_path.pathpoint_array.back().y << ")\n\n";
640 }
641}
642
646TEST_F(IntegrationTest, ROSBAG_PATH_1) {
647 // file with the testing scenario
648 const std::string filename = "rosbag_1.txt";
649
650 // box of the final point
651 double x1 = 0.0, x2 = 0.0, y1 = 0.0, y2 = 0.0;
652
653 // Open file straight_1.txt to read the initial state
654 std::ifstream file("../../src/planning/test/integration_tests/tests/" + filename);
655
656 if (!file.is_open()) {
657 FAIL() << "Failed to open file: straight_1.txt";
658 return;
659 }
660
661 std::vector<Cone> cone_array;
662 custom_interfaces::msg::Pose vehicle_pose;
663 ::read_from_file(file, x1, x2, y1, y2, cone_array, vehicle_pose);
664
665 // Convert the cone array to the message
666 this->cone_array_msg = common_lib::communication::custom_interfaces_array_from_vector(cone_array);
667 // Run the nodes
668 const auto duration = run_nodes(cone_array_msg, vehicle_pose);
669 RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Execution time: %f ms", duration.count());
670
671 ::save_debug_file(filename, cone_array, this->received_path, vehicle_pose);
672
673 // Verify final position
674 if ((this->received_path.pathpoint_array.back().x >= x1 &&
675 this->received_path.pathpoint_array.back().x <= x2) &&
676 (this->received_path.pathpoint_array.back().y >= y1 &&
677 this->received_path.pathpoint_array.back().y <= y2)) {
678 SUCCEED();
679 } else {
680 FAIL() << "The final point is not in the expected range. "
681 << "Expected range: (" << x1 << ", " << x2 << ", " << y1 << ", " << y2 << "), but got: ("
682 << this->received_path.pathpoint_array.back().x << ", "
683 << this->received_path.pathpoint_array.back().y << ")\n\n";
684 }
685}
686
690TEST_F(IntegrationTest, ROSBAG_PATH_2) {
691 // file with the testing scenario
692 const std::string filename = "rosbag_2.txt";
693
694 // box of the final point
695 double x1 = 0.0, x2 = 0.0, y1 = 0.0, y2 = 0.0;
696
697 // Open file straight_1.txt to read the initial state
698 std::ifstream file("../../src/planning/test/integration_tests/tests/" + filename);
699
700 if (!file.is_open()) {
701 FAIL() << "Failed to open file: straight_1.txt";
702 return;
703 }
704
705 std::vector<Cone> cone_array;
706 custom_interfaces::msg::Pose vehicle_pose;
707 ::read_from_file(file, x1, x2, y1, y2, cone_array, vehicle_pose);
708
709 // Convert the cone array to the message
710 this->cone_array_msg = common_lib::communication::custom_interfaces_array_from_vector(cone_array);
711 // Run the nodes
712 const auto duration = run_nodes(cone_array_msg, vehicle_pose);
713 RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Execution time: %f ms", duration.count());
714
715 ::save_debug_file(filename, cone_array, this->received_path, vehicle_pose);
716
717 // Verify final position
718 if ((this->received_path.pathpoint_array.back().x >= x1 &&
719 this->received_path.pathpoint_array.back().x <= x2) &&
720 (this->received_path.pathpoint_array.back().y >= y1 &&
721 this->received_path.pathpoint_array.back().y <= y2)) {
722 SUCCEED();
723 } else {
724 FAIL() << "The final point is not in the expected range. "
725 << "Expected range: (" << x1 << ", " << x2 << ", " << y1 << ", " << y2 << "), but got: ("
726 << this->received_path.pathpoint_array.back().x << ", "
727 << this->received_path.pathpoint_array.back().y << ")\n\n";
728 }
729}
730
731
735TEST_F(IntegrationTest, ROSBAG_PATH_3) {
736 // file with the testing scenario
737 const std::string filename = "rosbag_3.txt";
738
739 // box of the final point
740 double x1 = 0.0, x2 = 0.0, y1 = 0.0, y2 = 0.0;
741
742 // Open file straight_1.txt to read the initial state
743 std::ifstream file("../../src/planning/test/integration_tests/tests/" + filename);
744
745 if (!file.is_open()) {
746 FAIL() << "Failed to open file: straight_1.txt";
747 return;
748 }
749
750 std::vector<Cone> cone_array;
751 custom_interfaces::msg::Pose vehicle_pose;
752 ::read_from_file(file, x1, x2, y1, y2, cone_array, vehicle_pose);
753
754 // Convert the cone array to the message
755 this->cone_array_msg = common_lib::communication::custom_interfaces_array_from_vector(cone_array);
756 // Run the nodes
757 const auto duration = run_nodes(cone_array_msg, vehicle_pose);
758 RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Execution time: %f ms", duration.count());
759
760 ::save_debug_file(filename, cone_array, this->received_path, vehicle_pose);
761
762 // Verify final position
763 if ((this->received_path.pathpoint_array.back().x >= x1 &&
764 this->received_path.pathpoint_array.back().x <= x2) &&
765 (this->received_path.pathpoint_array.back().y >= y1 &&
766 this->received_path.pathpoint_array.back().y <= y2)) {
767 SUCCEED();
768 } else {
769 FAIL() << "The final point is not in the expected range. "
770 << "Expected range: (" << x1 << ", " << x2 << ", " << y1 << ", " << y2 << "), but got: ("
771 << this->received_path.pathpoint_array.back().x << ", "
772 << this->received_path.pathpoint_array.back().y << ")\n\n";
773 }
774}
775
779TEST_F(IntegrationTest, ROSBAG_PATH_4) {
780 // file with the testing scenario
781 const std::string filename = "rosbag_4.txt";
782
783 // box of the final point
784 double x1 = 0.0, x2 = 0.0, y1 = 0.0, y2 = 0.0;
785
786 // Open file straight_1.txt to read the initial state
787 std::ifstream file("../../src/planning/test/integration_tests/tests/" + filename);
788
789 if (!file.is_open()) {
790 FAIL() << "Failed to open file: straight_1.txt";
791 return;
792 }
793
794 std::vector<Cone> cone_array;
795 custom_interfaces::msg::Pose vehicle_pose;
796 ::read_from_file(file, x1, x2, y1, y2, cone_array, vehicle_pose);
797
798 // Convert the cone array to the message
799 this->cone_array_msg = common_lib::communication::custom_interfaces_array_from_vector(cone_array);
800 // Run the nodes
801 const auto duration = run_nodes(cone_array_msg, vehicle_pose);
802 RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Execution time: %f ms", duration.count());
803
804 ::save_debug_file(filename, cone_array, this->received_path, vehicle_pose);
805
806 // Verify final position
807 if ((this->received_path.pathpoint_array.back().x >= x1 &&
808 this->received_path.pathpoint_array.back().x <= x2) &&
809 (this->received_path.pathpoint_array.back().y >= y1 &&
810 this->received_path.pathpoint_array.back().y <= y2)) {
811 SUCCEED();
812 } else {
813 FAIL() << "The final point is not in the expected range. "
814 << "Expected range: (" << x1 << ", " << x2 << ", " << y1 << ", " << y2 << "), but got: ("
815 << this->received_path.pathpoint_array.back().x << ", "
816 << this->received_path.pathpoint_array.back().y << ")\n\n";
817 }
818}
819
823TEST_F(IntegrationTest, ROSBAG_PATH_5) {
824 // file with the testing scenario
825 const std::string filename = "rosbag_5.txt";
826
827 // box of the final point
828 double x1 = 0.0, x2 = 0.0, y1 = 0.0, y2 = 0.0;
829
830 // Open file straight_1.txt to read the initial state
831 std::ifstream file("../../src/planning/test/integration_tests/tests/" + filename);
832
833 if (!file.is_open()) {
834 FAIL() << "Failed to open file: straight_1.txt";
835 return;
836 }
837
838 std::vector<Cone> cone_array;
839 custom_interfaces::msg::Pose vehicle_pose;
840 ::read_from_file(file, x1, x2, y1, y2, cone_array, vehicle_pose);
841
842 // Convert the cone array to the message
843 this->cone_array_msg = common_lib::communication::custom_interfaces_array_from_vector(cone_array);
844 // Run the nodes
845 const auto duration = run_nodes(cone_array_msg, vehicle_pose);
846 RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Execution time: %f ms", duration.count());
847
848 ::save_debug_file(filename, cone_array, this->received_path, vehicle_pose);
849
850 // Verify final position
851 if ((this->received_path.pathpoint_array.back().x >= x1 &&
852 this->received_path.pathpoint_array.back().x <= x2) &&
853 (this->received_path.pathpoint_array.back().y >= y1 &&
854 this->received_path.pathpoint_array.back().y <= y2)) {
855 SUCCEED();
856 } else {
857 FAIL() << "The final point is not in the expected range. "
858 << "Expected range: (" << x1 << ", " << x2 << ", " << y1 << ", " << y2 << "), but got: ("
859 << this->received_path.pathpoint_array.back().x << ", "
860 << this->received_path.pathpoint_array.back().y << ")\n\n";
861 }
862}
863
867TEST_F(IntegrationTest, CIRCLE) {
868 // file with the testing scenario
869 const std::string filename = "circle.txt";
870
871 // box of the final point
872 double x1 = 0.0, x2 = 0.0, y1 = 0.0, y2 = 0.0;
873
874 // Open file straight_1.txt to read the initial state
875 std::ifstream file("../../src/planning/test/integration_tests/tests/" + filename);
876
877 if (!file.is_open()) {
878 FAIL() << "Failed to open file: straight_1.txt";
879 return;
880 }
881
882 std::vector<Cone> cone_array;
883 custom_interfaces::msg::Pose vehicle_pose;
884 ::read_from_file(file, x1, x2, y1, y2, cone_array, vehicle_pose);
885
886 // Convert the cone array to the message
887 this->cone_array_msg = common_lib::communication::custom_interfaces_array_from_vector(cone_array);
888 // Run the nodes
889 const auto duration = run_nodes(cone_array_msg, vehicle_pose);
890 RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Execution time: %f ms", duration.count());
891
892 ::save_debug_file(filename, cone_array, this->received_path, vehicle_pose);
893
894 // Verify final position
895 if ((this->received_path.pathpoint_array.back().x >= x1 &&
896 this->received_path.pathpoint_array.back().x <= x2) &&
897 (this->received_path.pathpoint_array.back().y >= y1 &&
898 this->received_path.pathpoint_array.back().y <= y2)) {
899 SUCCEED();
900 } else {
901 FAIL() << "The final point is not in the expected range. "
902 << "Expected range: (" << x1 << ", " << x2 << ", " << y1 << ", " << y2 << "), but got: ("
903 << this->received_path.pathpoint_array.back().x << ", "
904 << this->received_path.pathpoint_array.back().y << ")\n\n";
905 }
906}
907
911TEST_F(IntegrationTest, STRAIGHT_3) {
912 // file with the testing scenario
913 const std::string filename = "straight_3.txt";
914
915 // box of the final point
916 double x1 = 0.0, x2 = 0.0, y1 = 0.0, y2 = 0.0;
917
918 // Open file straight_1.txt to read the initial state
919 std::ifstream file("../../src/planning/test/integration_tests/tests/" + filename);
920
921 if (!file.is_open()) {
922 FAIL() << "Failed to open file: straight_1.txt";
923 return;
924 }
925
926 std::vector<Cone> cone_array;
927 custom_interfaces::msg::Pose vehicle_pose;
928 ::read_from_file(file, x1, x2, y1, y2, cone_array, vehicle_pose);
929
930 // Convert the cone array to the message
931 this->cone_array_msg = common_lib::communication::custom_interfaces_array_from_vector(cone_array);
932 // Run the nodes
933 const auto duration = run_nodes(cone_array_msg, vehicle_pose);
934 RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Execution time: %f ms", duration.count());
935
936 ::save_debug_file(filename, cone_array, this->received_path, vehicle_pose);
937
938 // Verify final position
939 if ((this->received_path.pathpoint_array.back().x >= x1 &&
940 this->received_path.pathpoint_array.back().x <= x2) &&
941 (this->received_path.pathpoint_array.back().y >= y1 &&
942 this->received_path.pathpoint_array.back().y <= y2)) {
943 SUCCEED();
944 } else {
945 FAIL() << "The final point is not in the expected range. "
946 << "Expected range: (" << x1 << ", " << x2 << ", " << y1 << ", " << y2 << "), but got: ("
947 << this->received_path.pathpoint_array.back().x << ", "
948 << this->received_path.pathpoint_array.back().y << ")\n\n";
949 }
950}
951
952
956TEST_F(IntegrationTest, TRACK) {
957 // file with the testing scenario
958 const std::string filename = "track.txt";
959
960 // box of the final point
961 double x1 = 0.0, x2 = 0.0, y1 = 0.0, y2 = 0.0;
962
963 // Open file straight_1.txt to read the initial state
964 std::ifstream file("../../src/planning/test/integration_tests/tests/" + filename);
965
966 if (!file.is_open()) {
967 FAIL() << "Failed to open file: straight_1.txt";
968 return;
969 }
970
971 std::vector<Cone> cone_array;
972 custom_interfaces::msg::Pose vehicle_pose;
973 ::read_from_file(file, x1, x2, y1, y2, cone_array, vehicle_pose);
974
975 // Convert the cone array to the message
976 this->cone_array_msg = common_lib::communication::custom_interfaces_array_from_vector(cone_array);
977 // Run the nodes
978 const auto duration = run_nodes(cone_array_msg, vehicle_pose);
979 RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Execution time: %f ms", duration.count());
980
981 ::save_debug_file(filename, cone_array, this->received_path, vehicle_pose);
982
983 // Verify final position
984 if ((this->received_path.pathpoint_array.back().x >= x1 &&
985 this->received_path.pathpoint_array.back().x <= x2) &&
986 (this->received_path.pathpoint_array.back().y >= y1 &&
987 this->received_path.pathpoint_array.back().y <= y2)) {
988 SUCCEED();
989 } else {
990 FAIL() << "The final point is not in the expected range. "
991 << "Expected range: (" << x1 << ", " << x2 << ", " << y1 << ", " << y2 << "), but got: ("
992 << this->received_path.pathpoint_array.back().x << ", "
993 << this->received_path.pathpoint_array.back().y << ")\n\n";
994 }
995}
996
1000TEST_F(IntegrationTest, TRACK_2) {
1001 // file with the testing scenario
1002 const std::string filename = "track_2.txt";
1003
1004 // box of the final point
1005 double x1 = 0.0, x2 = 0.0, y1 = 0.0, y2 = 0.0;
1006
1007 // Open file straight_1.txt to read the initial state
1008 std::ifstream file("../../src/planning/test/integration_tests/tests/" + filename);
1009
1010 if (!file.is_open()) {
1011 FAIL() << "Failed to open file: straight_1.txt";
1012 return;
1013 }
1014
1015 std::vector<Cone> cone_array;
1016 custom_interfaces::msg::Pose vehicle_pose;
1017 ::read_from_file(file, x1, x2, y1, y2, cone_array, vehicle_pose);
1018
1019 // Convert the cone array to the message
1020 this->cone_array_msg = common_lib::communication::custom_interfaces_array_from_vector(cone_array);
1021 // Run the nodes
1022 const auto duration = run_nodes(cone_array_msg, vehicle_pose);
1023 RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Execution time: %f ms", duration.count());
1024
1025 ::save_debug_file(filename, cone_array, this->received_path, vehicle_pose);
1026
1027 // Verify final position
1028 if ((this->received_path.pathpoint_array.back().x >= x1 &&
1029 this->received_path.pathpoint_array.back().x <= x2) &&
1030 (this->received_path.pathpoint_array.back().y >= y1 &&
1031 this->received_path.pathpoint_array.back().y <= y2)) {
1032 SUCCEED();
1033 } else {
1034 FAIL() << "The final point is not in the expected range. "
1035 << "Expected range: (" << x1 << ", " << x2 << ", " << y1 << ", " << y2 << "), but got: ("
1036 << this->received_path.pathpoint_array.back().x << ", "
1037 << this->received_path.pathpoint_array.back().y << ")\n\n";
1038 }
1039}
1040
1044TEST_F(IntegrationTest, OUTLIERS_1) {
1045 // file with the testing scenario
1046 const std::string filename = "outliers_1.txt";
1047
1048 // box of the final point
1049 double x1 = 0.0, x2 = 0.0, y1 = 0.0, y2 = 0.0;
1050
1051 // Open file straight_1.txt to read the initial state
1052 std::ifstream file("../../src/planning/test/integration_tests/tests/" + filename);
1053
1054 if (!file.is_open()) {
1055 FAIL() << "Failed to open file: straight_1.txt";
1056 return;
1057 }
1058
1059 std::vector<Cone> cone_array;
1060 custom_interfaces::msg::Pose vehicle_pose;
1061 ::read_from_file(file, x1, x2, y1, y2, cone_array, vehicle_pose);
1062
1063 // Convert the cone array to the message
1064 this->cone_array_msg = common_lib::communication::custom_interfaces_array_from_vector(cone_array);
1065 // Run the nodes
1066 const auto duration = run_nodes(cone_array_msg, vehicle_pose);
1067 RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Execution time: %f ms", duration.count());
1068
1069 ::save_debug_file(filename, cone_array, this->received_path, vehicle_pose);
1070
1071 // Verify final position
1072 if ((this->received_path.pathpoint_array.back().x >= x1 &&
1073 this->received_path.pathpoint_array.back().x <= x2) &&
1074 (this->received_path.pathpoint_array.back().y >= y1 &&
1075 this->received_path.pathpoint_array.back().y <= y2)) {
1076 SUCCEED();
1077 } else {
1078 FAIL() << "The final point is not in the expected range. "
1079 << "Expected range: (" << x1 << ", " << x2 << ", " << y1 << ", " << y2 << "), but got: ("
1080 << this->received_path.pathpoint_array.back().x << ", "
1081 << this->received_path.pathpoint_array.back().y << ")\n\n";
1082 }
1083}
1084
1088TEST_F(IntegrationTest, OUTLIERS_2) {
1089 // file with the testing scenario
1090 const std::string filename = "outliers_2.txt";
1091
1092 // box of the final point
1093 double x1 = 0.0, x2 = 0.0, y1 = 0.0, y2 = 0.0;
1094
1095 // Open file straight_1.txt to read the initial state
1096 std::ifstream file("../../src/planning/test/integration_tests/tests/" + filename);
1097
1098 if (!file.is_open()) {
1099 FAIL() << "Failed to open file: straight_1.txt";
1100 return;
1101 }
1102
1103 std::vector<Cone> cone_array;
1104 custom_interfaces::msg::Pose vehicle_pose;
1105 ::read_from_file(file, x1, x2, y1, y2, cone_array, vehicle_pose);
1106
1107 // Convert the cone array to the message
1108 this->cone_array_msg = common_lib::communication::custom_interfaces_array_from_vector(cone_array);
1109 // Run the nodes
1110 const auto duration = run_nodes(cone_array_msg, vehicle_pose);
1111 RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Execution time: %f ms", duration.count());
1112
1113 ::save_debug_file(filename, cone_array, this->received_path, vehicle_pose);
1114
1115 // Verify final position
1116 if ((this->received_path.pathpoint_array.back().x >= x1 &&
1117 this->received_path.pathpoint_array.back().x <= x2) &&
1118 (this->received_path.pathpoint_array.back().y >= y1 &&
1119 this->received_path.pathpoint_array.back().y <= y2)) {
1120 SUCCEED();
1121 } else {
1122 FAIL() << "The final point is not in the expected range. "
1123 << "Expected range: (" << x1 << ", " << x2 << ", " << y1 << ", " << y2 << "), but got: ("
1124 << this->received_path.pathpoint_array.back().x << ", "
1125 << this->received_path.pathpoint_array.back().y << ")\n\n";
1126 }
1127}
IntegrationTest::control_receiver
std::shared_ptr< rclcpp::Node > control_receiver
Definition integration_tests.cpp:10
IntegrationTest::received_path
custom_interfaces::msg::PathPointArray received_path
Definition integration_tests.cpp:14
IntegrationTest::SetUp
void SetUp() override
Definition integration_tests.cpp:21
IntegrationTest::locmap_sender
std::shared_ptr< rclcpp::Node > locmap_sender
Definition integration_tests.cpp:9
IntegrationTest::vehicle_pose_publisher_
std::shared_ptr< rclcpp::Publisher< custom_interfaces::msg::Pose > > vehicle_pose_publisher_
Definition integration_tests.cpp:18
IntegrationTest::planning_test_
std::shared_ptr< Planning > planning_test_
Definition integration_tests.cpp:11
IntegrationTest::control_sub
std::shared_ptr< rclcpp::Subscription< custom_interfaces::msg::PathPointArray > > control_sub
Definition integration_tests.cpp:19
IntegrationTest::TearDown
void TearDown() override
Definition integration_tests.cpp:55
IntegrationTest::map_publisher
std::shared_ptr< rclcpp::Publisher< custom_interfaces::msg::ConeArray > > map_publisher
Definition integration_tests.cpp:17
IntegrationTest::cone_array_msg
custom_interfaces::msg::ConeArray cone_array_msg
Definition integration_tests.cpp:13
IntegrationTest::run_nodes
std::chrono::duration< double, std::milli > run_nodes(const custom_interfaces::msg::ConeArray &track_msg, const custom_interfaces::msg::Pose &state_msg)
Definition integration_tests.cpp:65
Planning::load_config
static PlanningParameters load_config(std::string &adapter)
Loads planning configuration from YAML files.
Definition planning.cpp:11
Cone
common_lib::structures::Cone Cone
Definition colorpoint.hpp:16
save_debug_file
void save_debug_file(const std::string filename, const std::vector< Cone > cone_array, const custom_interfaces::msg::PathPointArray received_path, const custom_interfaces::msg::Pose vehicle_pose)
Saves debug information to a file.
Definition integration_tests.cpp:398
read_from_file
void read_from_file(std::ifstream &file, double &finalxi, double &finalxf, double &finalyi, double &finalyf, std::vector< Cone > &cone_array, custom_interfaces::msg::Pose &vehicle_pose)
Reads a file and processes its contents to initialize vehicle state, cones, and final position range.
Definition integration_tests.cpp:358
TEST_F
TEST_F(IntegrationTest, PUBLISH_PATH1)
Tests the full pipeline with a simple track of 24 cones placed in two straight lines parallel to the ...
Definition integration_tests.cpp:93
common_lib::communication::custom_interfaces_array_from_vector
custom_interfaces::msg::ConeArray custom_interfaces_array_from_vector(const std::vector< common_lib::structures::Cone > &input_cones)
Definition interfaces.cpp:18
common_lib::competition_logic::get_color_enum
Color get_color_enum(const std::string &color)
Definition color.cpp:22
common_lib::competition_logic::get_color_string
std::string get_color_string(int color)
Definition color.cpp:16
executor
std::shared_ptr< rclcpp::executors::SingleThreadedExecutor > executor
Definition pacsim_main.cpp:60
vehicle.hpp
utils.hpp
common_lib::structures::Cone
Definition cone.hpp:12
common_lib::structures::Cone::color
common_lib::competition_logic::Color color
Definition cone.hpp:14
common_lib::structures::Cone::position
Position position
Definition cone.hpp:13
common_lib::structures::Position::x
double x
Definition position.hpp:8
common_lib::structures::Position::y
double y
Definition position.hpp:9