Formula Student Autonomous Systems
The code for the main driverless system
Loading...
Searching...
No Matches
slam_test.cpp
Go to the documentation of this file.
1#include <memory>
2#include <thread>
3
4#include "common_lib/structures/cone.hpp"
5#include "common_lib/structures/position.hpp"
6#include "common_lib/structures/vehicle_state.hpp"
7#include "gtest/gtest.h"
8#include "kalman_filter/ekf.hpp"
9
19TEST(EKF_SLAM, LINEAR_MOVEMENT_INTEGRITY_TEST) { // This test is not that
20 // great, to be improved
21 std::shared_ptr<common_lib::structures::VehicleState> vehicle_state =
22 std::make_shared<common_lib::structures::VehicleState>();
23 std::shared_ptr<MotionUpdate> motion_update = std::make_shared<MotionUpdate>();
24 std::shared_ptr<std::vector<common_lib::structures::Cone>> track_map =
25 std::make_shared<std::vector<common_lib::structures::Cone>>();
26 std::shared_ptr<std::vector<common_lib::structures::Cone>> initial_map =
27 std::make_shared<std::vector<common_lib::structures::Cone>>();
28 std::shared_ptr<std::vector<common_lib::structures::Cone>> predicted_map =
29 std::make_shared<std::vector<common_lib::structures::Cone>>();
30 motion_update->last_update = rclcpp::Clock().now();
31
32 Eigen::Matrix2f q_matrix = Eigen::Matrix2f::Zero();
33 q_matrix(0, 0) = static_cast<float>(0.1);
34 q_matrix(1, 1) = static_cast<float>(0.1);
35 Eigen::MatrixXf r_matrix = Eigen::MatrixXf::Zero(6, 6);
36 r_matrix(0, 0) = static_cast<float>(0.1);
37 r_matrix(1, 1) = static_cast<float>(0.1);
38 r_matrix(2, 2) = static_cast<float>(0.1);
39 r_matrix(3, 3) = static_cast<float>(0.1);
40 r_matrix(4, 4) = static_cast<float>(0.1);
41 r_matrix(5, 5) = static_cast<float>(0.1);
42 std::shared_ptr<MotionModel> motion_model = std::make_shared<ImuVelocityModel>(r_matrix);
43 std::shared_ptr<ObservationModel> observation_model =
44 std::make_shared<ObservationModel>(q_matrix);
45 std::shared_ptr<DataAssociationModel> data_association_model =
46 std::make_shared<MaxLikelihood>(20.0);
47
48 // Initial map
49 initial_map->push_back(
50 common_lib::structures::Cone(common_lib::structures::Position(0, -2),
51 common_lib::competition_logic::Color::LARGE_ORANGE, 1.0));
52 initial_map->push_back(
53 common_lib::structures::Cone(common_lib::structures::Position(0, 2),
54 common_lib::competition_logic::Color::LARGE_ORANGE, 1.0));
55 initial_map->push_back(common_lib::structures::Cone(
56 common_lib::structures::Position(2, -2), common_lib::competition_logic::Color::BLUE, 1.0));
57 initial_map->push_back(common_lib::structures::Cone(
58 common_lib::structures::Position(4, -2), common_lib::competition_logic::Color::BLUE, 1.0));
59 initial_map->push_back(common_lib::structures::Cone(
60 common_lib::structures::Position(6, -2), common_lib::competition_logic::Color::BLUE, 1.0));
61 initial_map->push_back(common_lib::structures::Cone(common_lib::structures::Position(7.5, -1.5),
62 common_lib::competition_logic::Color::BLUE,
63 1.0));
64 initial_map->push_back(common_lib::structures::Cone(common_lib::structures::Position(8.5, -0.5),
65 common_lib::competition_logic::Color::BLUE,
66 1.0));
67 initial_map->push_back(common_lib::structures::Cone(
68 common_lib::structures::Position(9, 0), common_lib::competition_logic::Color::BLUE, 1.0));
69 initial_map->push_back(common_lib::structures::Cone(
70 common_lib::structures::Position(9, 2), common_lib::competition_logic::Color::BLUE, 1.0));
71 initial_map->push_back(common_lib::structures::Cone(
72 common_lib::structures::Position(9, 4), common_lib::competition_logic::Color::BLUE, 1.0));
73 initial_map->push_back(common_lib::structures::Cone(
74 common_lib::structures::Position(2, 2), common_lib::competition_logic::Color::ORANGE, 1.0));
75 initial_map->push_back(common_lib::structures::Cone(
76 common_lib::structures::Position(4, 2), common_lib::competition_logic::Color::ORANGE, 1.0));
77 initial_map->push_back(common_lib::structures::Cone(
78 common_lib::structures::Position(6, 2), common_lib::competition_logic::Color::ORANGE, 1.0));
79 initial_map->push_back(common_lib::structures::Cone(
80 common_lib::structures::Position(6.5, 3), common_lib::competition_logic::Color::ORANGE, 1.0));
81 initial_map->push_back(common_lib::structures::Cone(
82 common_lib::structures::Position(7, 4), common_lib::competition_logic::Color::ORANGE, 1.0));
83 initial_map->push_back(common_lib::structures::Cone(
84 common_lib::structures::Position(7, 6), common_lib::competition_logic::Color::ORANGE, 1.0));
85 initial_map->push_back(common_lib::structures::Cone(
86 common_lib::structures::Position(7, 6), common_lib::competition_logic::Color::ORANGE, 1.0));
87
88 std::shared_ptr<ExtendedKalmanFilter> ekf =
89 std::make_shared<ExtendedKalmanFilter>(observation_model, data_association_model);
90 ekf->add_motion_model("imu", motion_model);
91
92 for (int i = -1; i < 10; i++) {
93 // motion_update->translational_velocity_x = 1;
94 motion_update->rotational_velocity = 0;
95 motion_update->last_update = rclcpp::Clock().now();
96 int delta_t = static_cast<int>(
97 (motion_update->last_update.seconds() - ekf->get_last_update().seconds()) * 1000);
98 double delta_x = static_cast<double>(i * delta_t) / 1000;
99 ekf->prediction_step(*motion_update, "imu");
100
101 if (i == -1) continue; // First iteration for setting last update correctly
102
103 predicted_map->clear();
104 predicted_map->push_back(
105 common_lib::structures::Cone(common_lib::structures::Position(0 - delta_x, -2),
106 common_lib::competition_logic::Color::LARGE_ORANGE, 1.0));
107 predicted_map->push_back(
108 common_lib::structures::Cone(common_lib::structures::Position(0 - delta_x, 2),
109 common_lib::competition_logic::Color::LARGE_ORANGE, 1.0));
110 predicted_map->push_back(
111 common_lib::structures::Cone(common_lib::structures::Position(2 - delta_x, -2),
112 common_lib::competition_logic::Color::BLUE, 1.0));
113 predicted_map->push_back(
114 common_lib::structures::Cone(common_lib::structures::Position(4 - delta_x, -2),
115 common_lib::competition_logic::Color::BLUE, 1.0));
116 predicted_map->push_back(
117 common_lib::structures::Cone(common_lib::structures::Position(6 - delta_x, -2),
118 common_lib::competition_logic::Color::BLUE, 1.0));
119 predicted_map->push_back(
120 common_lib::structures::Cone(common_lib::structures::Position(7.5 - delta_x, -1.5),
121 common_lib::competition_logic::Color::BLUE, 1.0));
122 predicted_map->push_back(
123 common_lib::structures::Cone(common_lib::structures::Position(8.5 - delta_x, -0.5),
124 common_lib::competition_logic::Color::BLUE, 1.0));
125 predicted_map->push_back(
126 common_lib::structures::Cone(common_lib::structures::Position(9 - delta_x, 0),
127 common_lib::competition_logic::Color::BLUE, 1.0));
128 predicted_map->push_back(
129 common_lib::structures::Cone(common_lib::structures::Position(9 - delta_x, 2),
130 common_lib::competition_logic::Color::BLUE, 1.0));
131 predicted_map->push_back(
132 common_lib::structures::Cone(common_lib::structures::Position(9 - delta_x, 4),
133 common_lib::competition_logic::Color::BLUE, 1.0));
134 predicted_map->push_back(
135 common_lib::structures::Cone(common_lib::structures::Position(2 - delta_x, 2),
136 common_lib::competition_logic::Color::ORANGE, 1.0));
137 predicted_map->push_back(
138 common_lib::structures::Cone(common_lib::structures::Position(4 - delta_x, 2),
139 common_lib::competition_logic::Color::ORANGE, 1.0));
140 predicted_map->push_back(
141 common_lib::structures::Cone(common_lib::structures::Position(6 - delta_x, 2),
142 common_lib::competition_logic::Color::ORANGE, 1.0));
143 predicted_map->push_back(
144 common_lib::structures::Cone(common_lib::structures::Position(6.5 - delta_x, 3),
145 common_lib::competition_logic::Color::ORANGE, 1.0));
146 predicted_map->push_back(
147 common_lib::structures::Cone(common_lib::structures::Position(7 - delta_x, 4),
148 common_lib::competition_logic::Color::ORANGE, 1.0));
149 predicted_map->push_back(
150 common_lib::structures::Cone(common_lib::structures::Position(7 - delta_x, 6),
151 common_lib::competition_logic::Color::ORANGE, 1.0));
152 predicted_map->push_back(
153 common_lib::structures::Cone(common_lib::structures::Position(7 - delta_x, 6),
154 common_lib::competition_logic::Color::ORANGE, 1.0));
155 ekf->correction_step(*predicted_map);
156 ekf->update(vehicle_state, track_map);
157
158 int orange_count = 0;
159 int blue_count = 0;
160 int big_orange_count = 0;
161 for (const common_lib::structures::Cone &cone : *track_map) {
162 if (cone.color == common_lib::competition_logic::Color::ORANGE) {
163 orange_count++;
164 } else if (cone.color == common_lib::competition_logic::Color::BLUE) {
165 blue_count++;
166 } else if (cone.color == common_lib::competition_logic::Color::LARGE_ORANGE) {
167 big_orange_count++;
168 }
169 EXPECT_GE(cone.position.x, -1);
170 EXPECT_NEAR(cone.position.y, 2, 9);
171 }
172 // TODO(marhcouto): add more assertions
173 // EXPECT_EQ(orange_count, 6);
174 // EXPECT_EQ(blue_count, 8);
175 // EXPECT_EQ(big_orange_count, 2);
176 EXPECT_GE(track_map->size(), static_cast<unsigned long int>(6));
177 EXPECT_LE(track_map->size(), static_cast<unsigned long int>(22));
178
179 EXPECT_GE(vehicle_state->pose.position.x, -0.5);
180 EXPECT_LE(vehicle_state->pose.position.x, 20);
181 std::this_thread::sleep_for(std::chrono::milliseconds(500));
182 }
183 EXPECT_GE(track_map->size(), static_cast<unsigned long int>(12));
184 EXPECT_LE(track_map->size(), static_cast<unsigned long int>(22));
185 EXPECT_GE(vehicle_state->pose.position.x, -0.5);
186 EXPECT_LE(vehicle_state->pose.position.x, 22);
187}
ekf.hpp
TEST
TEST(EKF_SLAM, LINEAR_MOVEMENT_INTEGRITY_TEST)
Test the EKF SLAM algorithm in a linear movement.
Definition slam_test.cpp:19
common_lib::structures::Cone
Definition cone.hpp:12
common_lib::structures::Position
Definition position.hpp:7
vehicle_state.hpp