Formula Student Autonomous Systems
The code for the main driverless system
Loading...
Searching...
No Matches
UnitTests.cpp
Go to the documentation of this file.
1#include <gtest/gtest.h>
2#include <yaml-cpp/yaml.h>
3#include "../../include/VehicleModel/VehicleModelBicycle.hpp"
4
5class VehicleModelTest : public ::testing::Test {
6protected:
7 VehicleModelBicycle vehicleModel; // Declare as a member
8 void SetUp() override {
9 vehicleModel = VehicleModelBicycle(); // Proper instantiation
10 auto node = YAML::LoadFile("../../../../src/pacsim/config/vehicleModel.yaml");
11 auto configVehicleModel = node["vehicle_model"];
12 ConfigElement configVehicleModelElem(configVehicleModel);
13 vehicleModel.readConfig(configVehicleModelElem);
14 }
15
16};
17/*
18-----------------------------------------------------------
19CalculateForces test - test for negative, zero and positive
20-----------------------------------------------------------
21*/
22
23
24TEST_F(VehicleModelTest, TestCalculateForcesZero) {
25 double velocityX = 0;
26 double downforce = 0;
27 double drag = 0;
28 vehicleModel.getAeroModel().calculateForces(velocityX, downforce, drag);
29 ASSERT_EQ(downforce, 0);
30 ASSERT_EQ(drag, 0);
31}
32
33TEST_F(VehicleModelTest, TestCalculateForcesPositive) {
34 double velocityX = 5;
35 double downforce = 0;
36 double drag = 0;
37 vehicleModel.getAeroModel().calculateForces(velocityX, downforce, drag);
38 ASSERT_NEAR(downforce , 6.1017 , 0.1);
39 ASSERT_NEAR(drag , -5.17935 , 0.1);
40}
41
42TEST_F(VehicleModelTest, TestCalculateForcesNegative) {
43 double velocityX = -5;
44 double downforce = 0;
45 double drag = 0;
46 vehicleModel.getAeroModel().calculateForces(velocityX, downforce, drag);
47 ASSERT_NEAR(downforce , 6.1017 , 0.1);
48 ASSERT_NEAR(drag , 5.17935 , 0.1);
49}
50
51/*
52-----------------------------------------------------------------------------------------------
53CalculateLongitudinalForces - test for Max torque , Medium Torque, Low torque , Negative torque
54-----------------------------------------------------------------------------------------------
55*/
56
57TEST_F(VehicleModelTest, CalculateLongitudinalHigh){
58 double Fx_FL = 0, Fx_FR = 0, Fx_RL = 0, Fx_RR = 0;
59 Wheels newTorques;
60 newTorques.FL = 0;
61 newTorques.FR = 0;
62 newTorques.RL = 60;
63 newTorques.RR = 60;
64 newTorques.timestamp = 0;
65 vehicleModel.setTorques(newTorques);
66 vehicleModel.getLongitudinalForces(Fx_FL, Fx_FR, Fx_RL, Fx_RR);
67 //ASSERT_EQ(vehicleModel.getPowertrainModel().calculateEfficiency(newTorques), 1); // 0.87396
68 ASSERT_EQ(Fx_FL, 0);
69 ASSERT_EQ(Fx_FR, 0);
70 ASSERT_NEAR(Fx_RL,1033.25 , 1);
71 ASSERT_NEAR(Fx_RR, 1033.25, 1);
72}
73
74TEST_F(VehicleModelTest, CalculateLongitudinalMid){
75 double Fx_FL = 0, Fx_FR = 0, Fx_RL = 0, Fx_RR = 0;
76 Wheels newTorques;
77 newTorques.FL = 0;
78 newTorques.FR = 0;
79 newTorques.RL = 30;
80 newTorques.RR = 30;
81 newTorques.timestamp = 0;
82 vehicleModel.setTorques(newTorques);
83 vehicleModel.getLongitudinalForces(Fx_FL, Fx_FR, Fx_RL, Fx_RR);
84 //ASSERT_EQ(vehicleModel.getPowertrainModel().calculateEfficiency(newTorques), 1); // 0.732
85 ASSERT_EQ(Fx_FL, 0);
86 ASSERT_EQ(Fx_FR, 0);
87 ASSERT_NEAR(Fx_RL,433.71 , 1);
88 ASSERT_NEAR(Fx_RR, 433.71, 1);
89}
90
91TEST_F(VehicleModelTest, CalculateLongitudinalLow){
92 double Fx_FL = 0, Fx_FR = 0, Fx_RL = 0, Fx_RR = 0;
93 Wheels newTorques;
94 newTorques.FL = 0;
95 newTorques.FR = 0;
96 newTorques.RL = 10;
97 newTorques.RR = 10;
98 newTorques.timestamp = 0;
99 vehicleModel.setTorques(newTorques);
100 vehicleModel.getLongitudinalForces(Fx_FL, Fx_FR, Fx_RL, Fx_RR);
101 //ASSERT_EQ(vehicleModel.getPowertrainModel().calculateEfficiency(newTorques), 1); // 0.64
102 ASSERT_EQ(Fx_FL, 0);
103 ASSERT_EQ(Fx_FR, 0);
104 ASSERT_NEAR(Fx_RL,127.10 , 1);
105 ASSERT_NEAR(Fx_RR, 127.10, 1);
106}
107
108TEST_F(VehicleModelTest, CalculateLongitudinalNeg){
109 double Fx_FL = 0, Fx_FR = 0, Fx_RL = 0, Fx_RR = 0;
110 Wheels newTorques;
111 newTorques.FL = 0;
112 newTorques.FR = 0;
113 newTorques.RL = -10;
114 newTorques.RR = -10;
115 newTorques.timestamp = 0;
116 vehicleModel.setTorques(newTorques);
117 vehicleModel.getLongitudinalForces(Fx_FL, Fx_FR, Fx_RL, Fx_RR);
118 //ASSERT_EQ(vehicleModel.getPowertrainModel().calculateEfficiency(newTorques), 1); // 0.64
119 ASSERT_EQ(Fx_FL, 0);
120 ASSERT_EQ(Fx_FR, 0);
121 ASSERT_NEAR(Fx_RL,-127.10 , 1);
122 ASSERT_NEAR(Fx_RR,-127.10, 1);
123}
124
125/*
126---------------------------------------------------------------
127calculateEfficency tests - test for Low , Mid , High , Negative
128---------------------------------------------------------------
129*/
130
131TEST_F(VehicleModelTest , testEfficencyLowT){
132 Wheels newTorques;
133 newTorques.FL = 0;
134 newTorques.FR = 0;
135 newTorques.RL = 5;
136 newTorques.RR = 5;
137 newTorques.timestamp = 0;
138 vehicleModel.setTorques(newTorques);
139 ASSERT_NEAR(vehicleModel.getPowertrainModel().calculateEfficiency(newTorques) , 0.617 , 0.01);
140}
141
142TEST_F(VehicleModelTest , testEfficencyMidT){
143 Wheels newTorques;
144 newTorques.FL = 0;
145 newTorques.FR = 0;
146 newTorques.RL = 30;
147 newTorques.RR = 30;
148 newTorques.timestamp = 0;
149 vehicleModel.setTorques(newTorques);
150 ASSERT_NEAR(vehicleModel.getPowertrainModel().calculateEfficiency(newTorques) , 0.732 , 0.01);
151}
152
153TEST_F(VehicleModelTest , testEfficencyHighT){
154 Wheels newTorques;
155 newTorques.FL = 0;
156 newTorques.FR = 0;
157 newTorques.RL = 60;
158 newTorques.RR = 60;
159 newTorques.timestamp = 0;
160 vehicleModel.setTorques(newTorques);
161 ASSERT_NEAR(vehicleModel.getPowertrainModel().calculateEfficiency(newTorques) , 0.879 , 0.01);
162}
163
164TEST_F(VehicleModelTest, testEfficiencyNegT){
165 Wheels newTorques;
166 newTorques.FL = 0;
167 newTorques.FR = 0;
168 newTorques.RL = -5;
169 newTorques.RR = -5;
170 newTorques.timestamp = 0;
171 vehicleModel.setTorques(newTorques);
172 ASSERT_NEAR(vehicleModel.getPowertrainModel().calculateEfficiency(newTorques) , 0.617 , 0.01);
173}
174/*
175-----------------------------------------------------
176calculateNormalForces - test for speed 0 and positive
177-----------------------------------------------------
178*/
179
180TEST_F(VehicleModelTest, TestCalculateNormalFZero) {
181 double Fz_Front = 0, Fz_Rear = 0;
182 vehicleModel.getNormalForces(Fz_Front, Fz_Rear);
183 ASSERT_NEAR(Fz_Front, 516.98 ,1);
184 ASSERT_NEAR(Fz_Rear, 466.36 , 1);
185}
186
187TEST_F(VehicleModelTest, TestCalculateNormalFPositive) {
188 double Fz_Front = 0, Fz_Rear = 0;
189 vehicleModel.setVelocity(Eigen::Vector3d(5, 0, 0)); // Set a positive velocity of 5 m/s in the x direction
190 vehicleModel.getNormalForces(Fz_Front, Fz_Rear);
191 ASSERT_NEAR(Fz_Front, 519.1 ,1);
192 ASSERT_NEAR(Fz_Rear, 469.3 , 1);
193}
194
195/*
196----------------------------------------------------------
197calculateSlipAngles - test for rest , straight and turning
198----------------------------------------------------------
199*/
200
201TEST_F(VehicleModelTest, TestCalculateSlipAnglesRest) {
202 double kappaFront = 0, kappaRear = 0;
203 vehicleModel.setVelocity(Eigen::Vector3d(0, 0, 0)); // Set velocity to zero
204 vehicleModel.getSlipAngles(kappaFront, kappaRear);
205 ASSERT_EQ(kappaFront, 0);
206 ASSERT_EQ(kappaRear, 0);
207}
208
209TEST_F(VehicleModelTest, TestCalculateSlipAnglesStraight) {
210 double kappaFront = 0, kappaRear = 0;
211 vehicleModel.setVelocity(Eigen::Vector3d(5, 0, 0)); // Set a positive velocity of 5 m/s in the x direction
212 vehicleModel.getSlipAngles(kappaFront, kappaRear);
213 ASSERT_EQ(kappaFront, 0);
214 ASSERT_EQ(kappaRear, 0);
215}
216
217TEST_F(VehicleModelTest, TestCalculateSlipAnglesTurning) {
218 double kappaFront = 0, kappaRear = 0;
219 vehicleModel.setVelocity(Eigen::Vector3d(5, 0, 0)); // Set a positive velocity of 5 m/s in the x direction
220 vehicleModel.setSteeringSetpointFront(0.1); // Set a small steering angle
221 vehicleModel.getSlipAngles(kappaFront, kappaRear);
222 ASSERT_NEAR(kappaFront, -0.1, 0.01); // Expect a small slip angle for the front wheel
223 ASSERT_EQ(kappaRear, 0); // Rear wheels should not have slip angle in this model
224}
225
226/*
227---------------------------------------------------------------------
228calculateWheelTorques - test for positive, negative and zero throtle
229---------------------------------------------------------------------
230*/
231
232TEST_F(VehicleModelTest, TestCalculateWheelTorquesPositiveThrottle) {
233 Wheels wheelTorques;
234 Wheels wheelspeeds = {0.0, 0.0, 0.0, 0.0, 0.0};
235 Wheels throttleInputs = {0.0, 0.0, 1.0, 1.0, 0.0};
236 vehicleModel.getPowertrainModel().calculateWheelTorques(throttleInputs, wheelspeeds, wheelTorques);
237 ASSERT_EQ(wheelTorques.FL, 0);
238 ASSERT_EQ(wheelTorques.FR, 0);
239 ASSERT_NEAR(wheelTorques.RL, 60.0, 1.0);
240 ASSERT_NEAR(wheelTorques.RR, 60.0, 1.0);
241}
242
243TEST_F(VehicleModelTest, TestCalculateWheelTorquesNegativeThrottle) {
244 Wheels wheelTorques;
245 Wheels wheelspeeds = {0.0, 0.0, 0.0, 0.0, 0.0};
246 Wheels throttleInputs = {0.0, 0.0, -1.0, -1.0, 0.0};
247 vehicleModel.getPowertrainModel().calculateWheelTorques(throttleInputs, wheelspeeds, wheelTorques);
248 ASSERT_EQ(wheelTorques.FL, 0);
249 ASSERT_EQ(wheelTorques.FR, 0);
250 ASSERT_NEAR(wheelTorques.RL, -60.0, 1.0);
251 ASSERT_NEAR(wheelTorques.RR, -60.0, 1.0);
252}
253
254TEST_F(VehicleModelTest, TestCalculateWheelTorquesZeroThrottle) {
255 Wheels wheelTorques;
256 Wheels wheelspeeds = {0.0, 0.0, 0.0, 0.0, 0.0};
257 Wheels throttleInputs = {0.0, 0.0, 0.0, 0.0, 0.0};
258 vehicleModel.getPowertrainModel().calculateWheelTorques(throttleInputs, wheelspeeds, wheelTorques);
259 ASSERT_EQ(wheelTorques.FL, 0);
260 ASSERT_EQ(wheelTorques.FR, 0);
261 ASSERT_EQ(wheelTorques.RL, 0);
262 ASSERT_EQ(wheelTorques.RR, 0);
263}
264
265
266/*
267--------------------------------------------------------------------------------------------------------------
268calculateCurrent - Tested for positive, negative and zero torque and positive and zero wheelspeed at 600 volts
269--------------------------------------------------------------------------------------------------------------
270*/
271
272TEST_F(VehicleModelTest, TestCalculateCurrentPositiveTorque) {
273 Wheels wheelTorques;
274 Wheels wheelSpeeds;
275 wheelTorques.FL = 0;
276 wheelTorques.FR = 0;
277 wheelTorques.RL = 60.0;
278 wheelTorques.RR = 60.0;
279
280 wheelSpeeds.FL = 10.0;
281 wheelSpeeds.FR = 10.0;
282 wheelSpeeds.RL = 10.0;
283 wheelSpeeds.RR = 10.0;
284
285 double voltage = 600.0;
286 double current = vehicleModel.getPowertrainModel().calculateCurrent(wheelTorques, wheelSpeeds, voltage);
287
288 ASSERT_NEAR(current, 0.2396, 0.01);
289}
290
291TEST_F(VehicleModelTest, TestCalculateCurrentNegativeTorque) {
292 Wheels wheelTorques;
293 Wheels wheelSpeeds;
294 wheelTorques.FL = 0;
295 wheelTorques.FR = 0;
296 wheelTorques.RL = -60.0;
297 wheelTorques.RR = -60.0;
298
299 wheelSpeeds.FL = 10.0;
300 wheelSpeeds.FR = 10.0;
301 wheelSpeeds.RL = 10.0;
302 wheelSpeeds.RR = 10.0;
303
304 double voltage = 600.0;
305 double current = vehicleModel.getPowertrainModel().calculateCurrent(wheelTorques, wheelSpeeds, voltage);
306
307 ASSERT_NEAR(current, -0.2396, 0.01);
308}
309
310TEST_F(VehicleModelTest, TestCalculateCurrentZeroTorque) {
311 Wheels wheelTorques;
312 Wheels wheelSpeeds;
313 wheelTorques.FL = 0;
314 wheelTorques.FR = 0;
315 wheelTorques.RL = 0.0;
316 wheelTorques.RR = 0.0;
317
318 wheelSpeeds.FL = 10.0;
319 wheelSpeeds.FR = 10.0;
320 wheelSpeeds.RL = 10.0;
321 wheelSpeeds.RR = 10.0;
322
323 double voltage = 600.0;
324 double current = vehicleModel.getPowertrainModel().calculateCurrent(wheelTorques, wheelSpeeds, voltage);
325
326 ASSERT_EQ(current, 0);
327}
328
329TEST_F(VehicleModelTest, TestCalculateCurrentZeroWheelspeed) {
330 Wheels wheelTorques;
331 Wheels wheelSpeeds;
332 wheelTorques.FL = 0;
333 wheelTorques.FR = 0;
334 wheelTorques.RL = 60.0;
335 wheelTorques.RR = 60.0;
336
337 wheelSpeeds.FL = 0.0;
338 wheelSpeeds.FR = 0.0;
339 wheelSpeeds.RL = 0.0;
340 wheelSpeeds.RR = 0.0;
341
342 double voltage = 600.0;
343 double current = vehicleModel.getPowertrainModel().calculateCurrent(wheelTorques, wheelSpeeds, voltage);
344
345 ASSERT_EQ(current, 0);
346}
347
348/*
349----------------------------------------------------------------------------
350calulateSteeringAngles - test for positive, negative and zero steering input
351----------------------------------------------------------------------------
352*/
353
354TEST_F(VehicleModelTest, TestCalculateSteeringAnglesPositive) {
355 Wheels steeringAngles;
356 vehicleModel.getSteeringModel().calculateSteeringAngles(1.0, steeringAngles);
357 ASSERT_NEAR(steeringAngles.FL, 1.08, 0.01);
358 ASSERT_NEAR(steeringAngles.FR, 0.92, 0.01);
359 ASSERT_EQ(steeringAngles.RL, 0.0);
360 ASSERT_EQ(steeringAngles.RR, 0.0);
361}
362
363TEST_F(VehicleModelTest, TestCalculateSteeringAnglesNegative) {
364 Wheels steeringAngles;
365 vehicleModel.getSteeringModel().calculateSteeringAngles(-1.0, steeringAngles);
366 ASSERT_NEAR(steeringAngles.FL, -0.92, 0.01);
367 ASSERT_NEAR(steeringAngles.FR, -1.08, 0.01);
368 ASSERT_EQ(steeringAngles.RL, 0.0);
369 ASSERT_EQ(steeringAngles.RR, 0.0);
370}
371
372TEST_F(VehicleModelTest, TestCalculateSteeringAnglesZero) {
373 Wheels steeringAngles;
374 vehicleModel.getSteeringModel().calculateSteeringAngles(0.0, steeringAngles);
375 ASSERT_EQ(steeringAngles.FL, 0.0);
376 ASSERT_EQ(steeringAngles.FR, 0.0);
377 ASSERT_EQ(steeringAngles.RL, 0.0);
378 ASSERT_EQ(steeringAngles.RR, 0.0);
379}
380
381/*
382----------------------------------------------------------------------------------
383calculateSteeringWheelAngle - test for positive, negative and zero steering angles
384----------------------------------------------------------------------------------
385*/
386
387TEST_F(VehicleModelTest, TestCalculateSteeringWheelAnglePositive) {
388 Wheels steeringAngles;
389 steeringAngles.FL = 1.0;
390 steeringAngles.FR = 0.5;
391 steeringAngles.RL = 0.0;
392 steeringAngles.RR = 0.0;
393
394 double steeringWheelAngle = vehicleModel.getSteeringModel().calculateSteeringWheelAngle(steeringAngles);
395 ASSERT_NEAR(steeringWheelAngle, 5.35, 0.1);
396}
397
398TEST_F(VehicleModelTest, TestCalculateSteeringWheelAngleNegative) {
399 Wheels steeringAngles;
400 steeringAngles.FL = -1.0;
401 steeringAngles.FR = -0.5;
402 steeringAngles.RL = 0.0;
403 steeringAngles.RR = 0.0;
404
405 double steeringWheelAngle = vehicleModel.getSteeringModel().calculateSteeringWheelAngle(steeringAngles);
406 ASSERT_NEAR(steeringWheelAngle, -6.32, 0.1);
407}
408
409TEST_F(VehicleModelTest, TestCalculateSteeringWheelAngleZero) {
410 Wheels steeringAngles;
411 steeringAngles.FL = 0.0;
412 steeringAngles.FR = 0.0;
413 steeringAngles.RL = 0.0;
414 steeringAngles.RR = 0.0;
415
416 double steeringWheelAngle = vehicleModel.getSteeringModel().calculateSteeringWheelAngle(steeringAngles);
417 ASSERT_EQ(steeringWheelAngle, 0.0);
418}
419
420/*
421-------------------------------------------------------------------------------------
422calculateWheelGeometry - for positive negative and zero steering angles and velocities
423-------------------------------------------------------------------------------------
424*/
425
426// positive steering and zero velocity
427TEST_F(VehicleModelTest, TestCalculateWheelGeometryPositiveSteering) {
428 double steeringFront;
429 Eigen::Vector3d rFL, rFR, rRL, rRR, rFront, rRear, vFL, vFR, vRL, vRR;
430
431 vehicleModel.setSteeringSetpointFront(0.1); // Set a small positive steering angle
432 vehicleModel.getWheelGeometry(steeringFront, rFL, rFR, rRL, rRR, rFront, rRear, vFL, vFR, vRL, vRR);
433
434 ASSERT_NEAR(steeringFront, 0.1, 0.01);
435 ASSERT_NEAR(rFL.x(), 0.726, 0.01);
436 ASSERT_NEAR(rFR.x(), 0.726, 0.01);
437 ASSERT_NEAR(rRL.x(), -0.804, 0.01);
438 ASSERT_NEAR(rRR.x(), -0.804, 0.01);
439 ASSERT_NEAR(rFL.y(), 0.6, 0.01);
440 ASSERT_NEAR(rFR.y(), -0.6, 0.01);
441 ASSERT_NEAR(rRL.y(), 0.6, 0.01);
442 ASSERT_NEAR(rRR.y(), -0.6, 0.01);
443 ASSERT_NEAR(rFL.z(), 0.0, 0.01);
444 ASSERT_NEAR(rFR.z(), 0.0, 0.01);
445 ASSERT_NEAR(rRL.z(), 0.0, 0.01);
446 ASSERT_NEAR(rRR.z(), 0.0, 0.01);
447 ASSERT_NEAR(vFL.x(), 0.0, 0.01);
448 ASSERT_NEAR(vFR.x(), 0.0, 0.01);
449 ASSERT_NEAR(vRL.x(), 0.0, 0.01);
450 ASSERT_NEAR(vRR.x(), 0.0, 0.01);
451 ASSERT_NEAR(vFL.y(), 0.0, 0.01);
452 ASSERT_NEAR(vFR.y(), 0.0, 0.01);
453 ASSERT_NEAR(vRL.y(), 0.0, 0.01);
454 ASSERT_NEAR(vRR.y(), 0.0, 0.01);
455 ASSERT_NEAR(vFL.z(), 0.0, 0.01);
456 ASSERT_NEAR(vFR.z(), 0.0, 0.01);
457 ASSERT_NEAR(vRL.z(), 0.0, 0.01);
458 ASSERT_NEAR(vRR.z(), 0.0, 0.01);
459}
460
461// negative steering and zero velocity
462
463TEST_F(VehicleModelTest, TestCalculateWheelGeometryNegativeSteering) {
464 double steeringFront;
465 Eigen::Vector3d rFL, rFR, rRL, rRR, rFront, rRear, vFL, vFR, vRL, vRR;
466
467 vehicleModel.setSteeringSetpointFront(-0.1); // Set a small negative steering angle
468 vehicleModel.getWheelGeometry(steeringFront, rFL, rFR, rRL, rRR, rFront, rRear, vFL, vFR, vRL, vRR);
469
470 ASSERT_NEAR(steeringFront, -0.1, 0.01);
471 ASSERT_NEAR(rFL.x(), 0.726, 0.01);
472 ASSERT_NEAR(rFR.x(), 0.726, 0.01);
473 ASSERT_NEAR(rRL.x(), -0.804, 0.01);
474 ASSERT_NEAR(rRR.x(), -0.804, 0.01);
475 ASSERT_NEAR(rFL.y(), 0.6, 0.01);
476 ASSERT_NEAR(rFR.y(), -0.6, 0.01);
477 ASSERT_NEAR(rRL.y(), 0.6, 0.01);
478 ASSERT_NEAR(rRR.y(), -0.6, 0.01);
479 ASSERT_NEAR(rFL.z(), 0.0, 0.01);
480 ASSERT_NEAR(rFR.z(), 0.0, 0.01);
481 ASSERT_NEAR(rRL.z(), 0.0, 0.01);
482 ASSERT_NEAR(rRR.z(), 0.0, 0.01);
483 ASSERT_NEAR(vFL.x(), 0.0, 0.01);
484 ASSERT_NEAR(vFR.x(), 0.0, 0.01);
485 ASSERT_NEAR(vRL.x(), 0.0, 0.01);
486 ASSERT_NEAR(vRR.x(), 0.0, 0.01);
487 ASSERT_NEAR(vFL.y(), 0.0, 0.01);
488 ASSERT_NEAR(vFR.y(), 0.0, 0.01);
489 ASSERT_NEAR(vRL.y(), 0.0, 0.01);
490 ASSERT_NEAR(vRR.y(), 0.0, 0.01);
491 ASSERT_NEAR(vFL.z(), 0.0, 0.01);
492 ASSERT_NEAR(vFR.z(), 0.0, 0.01);
493 ASSERT_NEAR(vRL.z(), 0.0, 0.01);
494 ASSERT_NEAR(vRR.z(), 0.0, 0.01);
495}
496
497TEST_F(VehicleModelTest, TestCalculateWheelGeometryZeroSteering) {
498 double steeringFront;
499 Eigen::Vector3d rFL, rFR, rRL, rRR, rFront, rRear, vFL, vFR, vRL, vRR;
500
501 vehicleModel.setSteeringSetpointFront(0.0); // Set zero steering angle
502 vehicleModel.getWheelGeometry(steeringFront, rFL, rFR, rRL, rRR, rFront, rRear, vFL, vFR, vRL, vRR);
503
504 ASSERT_NEAR(steeringFront, 0.0, 0.01);
505 ASSERT_NEAR(rFL.x(), 0.726, 0.01);
506 ASSERT_NEAR(rFR.x(), 0.726, 0.01);
507 ASSERT_NEAR(rRL.x(), -0.804, 0.01);
508 ASSERT_NEAR(rRR.x(), -0.804, 0.01);
509 ASSERT_NEAR(rFL.y(), 0.6, 0.01);
510 ASSERT_NEAR(rFR.y(), -0.6, 0.01);
511 ASSERT_NEAR(rRL.y(), 0.6, 0.01);
512 ASSERT_NEAR(rRR.y(), -0.6, 0.01);
513 ASSERT_NEAR(rFL.z(), 0.0, 0.01);
514 ASSERT_NEAR(rFR.z(), 0.0, 0.01);
515 ASSERT_NEAR(rRL.z(), 0.0, 0.01);
516 ASSERT_NEAR(rRR.z(), 0.0, 0.01);
517
518 // Velocities should be zero
519 ASSERT_NEAR(vFL.x(), 0.0, 0.01);
520 ASSERT_NEAR(vFR.x(), 0.0, 0.01);
521 ASSERT_NEAR(vRL.x(), 0.0, 0.01);
522 ASSERT_NEAR(vRR.x(), 0.0, 0.01);
523 ASSERT_NEAR(vFL.y(), 0.0, 0.01);
524 ASSERT_NEAR(vFR.y(), 0.0, 0.01);
525 ASSERT_NEAR(vRL.y(), 0.0, 0.01);
526 ASSERT_NEAR(vRR.y(), 0.0, 0.01);
527 ASSERT_NEAR(vFL.z(), 0.0, 0.01);
528 ASSERT_NEAR(vFR.z(), 0.0, 0.01);
529 ASSERT_NEAR(vRL.z(), 0.0, 0.01);
530 ASSERT_NEAR(vRR.z(), 0.0, 0.01);
531}
532
533
534TEST_F(VehicleModelTest, TestCalculateWheelGeometryPositiveVelocity) {
535 double steeringFront;
536 Eigen::Vector3d rFL, rFR, rRL, rRR, rFront, rRear, vFL, vFR, vRL, vRR;
537 vehicleModel.setVelocity(Eigen::Vector3d(5, 0, 0)); // Set a positive velocity of 5 m/s in the x direction
538 vehicleModel.getWheelGeometry(steeringFront, rFL, rFR, rRL, rRR, rFront, rRear, vFL, vFR, vRL, vRR);
539 ASSERT_NEAR(steeringFront, 0.0, 0.01);
540 ASSERT_NEAR(rFL.x(), 0.726, 0.01);
541 ASSERT_NEAR(rFR.x(), 0.726, 0.01);
542 ASSERT_NEAR(rRL.x(), -0.804, 0.01);
543 ASSERT_NEAR(rRR.x(), -0.804, 0.01);
544 ASSERT_NEAR(rFL.y(), 0.6, 0.01);
545 ASSERT_NEAR(rFR.y(), -0.6, 0.01);
546 ASSERT_NEAR(rRL.y(), 0.6, 0.01);
547 ASSERT_NEAR(rRR.y(), -0.6, 0.01);
548 ASSERT_NEAR(rFL.z(), 0.0, 0.01);
549 ASSERT_NEAR(rFR.z(), 0.0, 0.01);
550 ASSERT_NEAR(rRL.z(), 0.0, 0.01);
551 ASSERT_NEAR(rRR.z(), 0.0, 0.01);
552 ASSERT_NEAR(vFL.x(), 5.0, 0.01);
553 ASSERT_NEAR(vFR.x(), 5.0, 0.01);
554 ASSERT_NEAR(vRL.x(), 5.0, 0.01);
555 ASSERT_NEAR(vRR.x(), 5.0, 0.01);
556 ASSERT_NEAR(vFL.y(), 0.0, 0.01);
557 ASSERT_NEAR(vFR.y(), 0.0, 0.01);
558 ASSERT_NEAR(vRL.y(), 0.0, 0.01);
559 ASSERT_NEAR(vRR.y(), 0.0, 0.01);
560 ASSERT_NEAR(vFL.z(), 0.0, 0.01);
561 ASSERT_NEAR(vFR.z(), 0.0, 0.01);
562 ASSERT_NEAR(vRL.z(), 0.0, 0.01);
563 ASSERT_NEAR(vRR.z(), 0.0, 0.01);
564}
565
566/*
567-------------------------------------------------------------
568updateWheelSpeeds - tested for wheel speeds at 0 and positive
569-------------------------------------------------------------
570*/
571
572
573TEST_F(VehicleModelTest, TestUpdateWheelSpeeds) {
574 Eigen::Vector3d vFL(3.0, 0, 0), vFR(3.0, 0, 0), vRL(3.0, 0, 0), vRR(3.0, 0, 0);
575 vehicleModel.setWheelSpeedsTester(vFL, vFR, vRL, vRR);
576
577 // Calculate expected RPM
578 double rpm2ms = 0.203 * 2 * M_PI / 60.0;
579 double expectedRPM = 3.0 / rpm2ms;
580
581 ASSERT_NEAR(vehicleModel.getWheelspeeds().FL, expectedRPM, 0.01);
582 ASSERT_NEAR(vehicleModel.getWheelspeeds().FR, expectedRPM, 0.01);
583 ASSERT_NEAR(vehicleModel.getWheelspeeds().RL, expectedRPM, 0.01);
584 ASSERT_NEAR(vehicleModel.getWheelspeeds().RR, expectedRPM, 0.01);
585}
586
587TEST_F(VehicleModelTest, TestUpdateWheelSpeedsZero) {
588 Eigen::Vector3d vFL(0.0, 0, 0), vFR(0.0, 0, 0), vRL(0.0, 0, 0), vRR(0.0, 0, 0);
589 vehicleModel.setWheelSpeedsTester(vFL, vFR, vRL, vRR);
590
591 ASSERT_EQ(vehicleModel.getWheelspeeds().FL, 0);
592 ASSERT_EQ(vehicleModel.getWheelspeeds().FR, 0);
593 ASSERT_EQ(vehicleModel.getWheelspeeds().RL, 0);
594 ASSERT_EQ(vehicleModel.getWheelspeeds().RR, 0);
595}
596
597/*
598-------------------------------------------------------------------------------------------------
599getWheelPositions - tested for wheel positions at zero orientation and at a certain angle(90 deg)
600-------------------------------------------------------------------------------------------------
601*/
602
603
604TEST_F(VehicleModelTest, TestGetWheelPositionsZeroOrientation) {
605 vehicleModel.setPosition(Eigen::Vector3d(0, 0, 0));
606 vehicleModel.setOrientation(Eigen::Vector3d(0, 0, 0));
607
608 auto wheels = vehicleModel.getWheelPositions();
609
610
611 ASSERT_NEAR(wheels[0].x(), 0.726, 0.01); // FL
612 ASSERT_NEAR(wheels[0].y(), 0.6, 0.01);
613 ASSERT_NEAR(wheels[1].x(), 0.726, 0.01); // FR
614 ASSERT_NEAR(wheels[1].y(), -0.6, 0.01);
615 ASSERT_NEAR(wheels[2].x(), -0.804, 0.01); // RL
616 ASSERT_NEAR(wheels[2].y(), 0.6, 0.01);
617 ASSERT_NEAR(wheels[3].x(), -0.804, 0.01); // RR
618 ASSERT_NEAR(wheels[3].y(), -0.6, 0.01);
619}
620
621
622TEST_F(VehicleModelTest, TestGetWheelPositionsAngleOrientation) {
623 vehicleModel.setPosition(Eigen::Vector3d(0, 0, 0));
624 vehicleModel.setOrientation(Eigen::Vector3d(0, 0, - M_PI / 2)); // Rotate 90 degrees
625
626 auto wheels = vehicleModel.getWheelPositions();
627
628 ASSERT_NEAR(wheels[0].x(), -0.6, 0.01); // FL
629 ASSERT_NEAR(wheels[0].y(), 0.726, 0.01);
630 ASSERT_NEAR(wheels[1].x(), 0.6, 0.01); // FR
631 ASSERT_NEAR(wheels[1].y(), 0.726, 0.01);
632 ASSERT_NEAR(wheels[2].x(), -0.6, 0.01); // RL
633 ASSERT_NEAR(wheels[2].y(), -0.804, 0.01);
634 ASSERT_NEAR(wheels[3].x(), 0.6, 0.01); // RR
635 ASSERT_NEAR(wheels[3].y(), -0.804, 0.01);
636}
637
638TEST_F(VehicleModelTest, TestGetWheelPositionsNegativeAngleOrientation) {
639 vehicleModel.setPosition(Eigen::Vector3d(1, 1, 1));
640 vehicleModel.setOrientation(Eigen::Vector3d(0, 0, 0)); // Rotate -90 degrees
641
642 auto wheels = vehicleModel.getWheelPositions();
643
644 ASSERT_NEAR(wheels[0].x(), 1.726, 0.01); // FL
645 ASSERT_NEAR(wheels[0].y(), 1.6, 0.01);
646 ASSERT_NEAR(wheels[1].x(), 1.726, 0.01); // FR
647 ASSERT_NEAR(wheels[1].y(), -0.6 + 1, 0.01);
648 ASSERT_NEAR(wheels[2].x(), -0.804 + 1 , 0.01); // RL
649 ASSERT_NEAR(wheels[2].y(), 1.6, 0.01);
650 ASSERT_NEAR(wheels[3].x(), -0.804 + 1, 0.01); // RR
651 ASSERT_NEAR(wheels[3].y(), -0.6 + 1, 0.01);
652}
653
654/*
655-------------------------------------------------------------------------------------------------------------
656calculateAccelerations - zero , straigth acceleration , straight braking , turning , drag and friction , yaw
657-------------------------------------------------------------------------------------------------------------
658*/
659
660TEST_F(VehicleModelTest , TestcalcAccelZero){
661 Eigen::Vector3d friction = Eigen::Vector3d(0, 0, 0);
662 Eigen::Vector3d result = vehicleModel.getCalculateAccelerations(0,0,0,0,0,0,0,0,friction);
663 ASSERT_EQ(result.x() , 0);
664 ASSERT_EQ(result.y() , 0);
665 ASSERT_EQ(result.z() , 0);
666}
667
668/*
669! Friction is being added like simple acceleration this is not very realistic from a standstill point could be an error/bug in the sim
670*/
671TEST_F(VehicleModelTest , TestcalcAccelZeroWFriction){
672 Eigen::Vector3d friction = Eigen::Vector3d(10, 0, 0);
673 Eigen::Vector3d result = vehicleModel.getCalculateAccelerations(0,0,0,0,0,0,0,0,friction);
674 ASSERT_NEAR(result.x() , 0.0498 , 0.01);
675 ASSERT_EQ(result.y() , 0);
676 ASSERT_EQ(result.z() , 0);
677}
678
679TEST_F(VehicleModelTest , TestcalcAccelStraight){
680 Eigen::Vector3d friction = Eigen::Vector3d(0, 0, 0);
681 Eigen::Vector3d result = vehicleModel.getCalculateAccelerations(0,0,0,5,5,0,0,0,friction);
682 ASSERT_NEAR(result.x() , 0.0498 , 0.01);
683 ASSERT_EQ(result.y() , 0);
684 ASSERT_EQ(result.z() , 0);
685}
686
687TEST_F(VehicleModelTest , TestcalcAccelStraightBraking){
688 Eigen::Vector3d friction = Eigen::Vector3d(2, 0, 0);
689 Eigen::Vector3d result = vehicleModel.getCalculateAccelerations(0,0,0,-5,-5,0,0,0,friction);
690 ASSERT_NEAR(result.x() , -8/200.707 , 0.01);
691 ASSERT_EQ(result.y() , 0);
692 ASSERT_EQ(result.z() , 0);
693}
694
695TEST_F(VehicleModelTest , TestcalcAccelTurning){
696 Eigen::Vector3d friction = Eigen::Vector3d(0, 0, 0);
697 Eigen::Vector3d result = vehicleModel.getCalculateAccelerations(0,0,0,5,5,0.1,0,0,friction);
698 ASSERT_NEAR(result.x() , 0.0498 , 0.01);
699 ASSERT_NEAR(result.y() , -0.005 , 0.01); // Small lateral acceleration due to turning
700 ASSERT_NEAR(result.z() , 0 , 0.01); // Very small rotation (not noticable in this case)
701}
702
703/*
704------------------------------------------------------
705getDynamicStates - test for zero and positive velocity
706------------------------------------------------------
707*/
708
709// !Currently the fuction is dependent on the processSlipAngleLat function (which is wrong) so every y value will be 0.067313 instead of zero
710// !Same thing applies for the yaw which will always be the expect value + 0.00331369
711
712TEST_F(VehicleModelTest, TestGetDynamicStatesPositive) {
713 vehicleModel.setVelocity(Eigen::Vector3d(0, 0, 0)); // Set velocity to zero
714 vehicleModel.setSteeringSetpointFront(0.0); // Set steering angle to zero
715
716 Eigen::Vector3d result = vehicleModel.getGetDynamicStates(1);
717
718 ASSERT_EQ(result.x(), 0);
719 ASSERT_NEAR(result.y(), 0.067313 , 0.001 );
720 ASSERT_NEAR(result.z(), 0 + 0.00331369 , 0.001);
721
722}
723
724
725TEST_F(VehicleModelTest, TestGetDynamicStatesAccelerating) {
726 vehicleModel.setVelocity(Eigen::Vector3d(0, 0, 0));
727 vehicleModel.setSteeringSetpointFront(0.0);
728 vehicleModel.setTorques(Wheels{0, 0, 60, 60}); // Set back wheel torques to maximum (full acceleration)
729
730 Eigen::Vector3d result = vehicleModel.getGetDynamicStates(1);
731
732 ASSERT_NEAR(result.x(), 10.3 , 0.1);
733 ASSERT_NEAR(result.y(), 0.067313 , 0.001 );
734 ASSERT_NEAR(result.z(), 0+ 0.00331369, 0.001);
735}
736
737TEST_F(VehicleModelTest, TestGetDynamicStatesBraking) {
738 vehicleModel.setVelocity(Eigen::Vector3d(0, 0, 0));
739 vehicleModel.setSteeringSetpointFront(0);
740 vehicleModel.setTorques(Wheels{0, 0, -60, -60}); // Set back wheel torques to maximum (full braking)
741 Eigen::Vector3d result = vehicleModel.getGetDynamicStates(1);
742
743 ASSERT_NEAR(result.x(), -10.3 , 0.1);
744 ASSERT_NEAR(result.y(), 0.067313 , 0.001 );
745 ASSERT_NEAR(result.z(), 0 + 0.00331369, 0.001);
746}
747
TEST_F
TEST_F(VehicleModelTest, TestCalculateForcesZero)
Definition UnitTests.cpp:24
VehicleModelBicycle::readConfig
bool readConfig(ConfigElement &config) override
Definition VehicleModelBicycle.cpp:17
VehicleModelTest::vehicleModel
VehicleModelBicycle vehicleModel
Definition UnitTests.cpp:7
VehicleModelTest::SetUp
void SetUp() override
Definition UnitTests.cpp:8
Wheels::RL
double RL
Definition types.hpp:77
Wheels::FL
double FL
Definition types.hpp:75
Wheels::FR
double FR
Definition types.hpp:76
Wheels::RR
double RR
Definition types.hpp:78
Wheels::timestamp
double timestamp
Definition types.hpp:80