RoboidControl/RoboidControl-cpp
RoboidControl/RoboidControl-cpp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fix test

Browse Source
This commit is contained in:
Pascal Serrarens 2024-11-15 15:09:13 +01:00
parent 4b21057fdd
commit c4d0ef95cf
1 changed files with 59 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

82
test/BB2B_Test.cc
View File

@ -12,7 +12,7 @@
/// @brief A Distance sensor with testing support /// @brief A Distance sensor with testing support
/// With this sensor it is possible to simulate a measurement /// With this sensor it is possible to simulate a measurement
class MockDistanceSensor : public DistanceSensor { class MockDistanceSensor : public DistanceSensor {
public: public:
MockDistanceSensor() : DistanceSensor() {}; MockDistanceSensor() : DistanceSensor() {};
MockDistanceSensor(float triggerDistance) MockDistanceSensor(float triggerDistance)
: DistanceSensor(triggerDistance) {}; : DistanceSensor(triggerDistance) {};
@ -23,21 +23,21 @@ class MockDistanceSensor : public DistanceSensor {
TEST(BB2B, NoObstacle) { TEST(BB2B, NoObstacle) {
#pragma region Setup #pragma region Setup
Motor* motorLeft = new Motor(); Motor *motorLeft = new Motor();
Motor* motorRight = new Motor(); Motor *motorRight = new Motor();
DifferentialDrive* propulsion = new DifferentialDrive(motorLeft, motorRight); DifferentialDrive *propulsion = new DifferentialDrive(motorLeft, motorRight);
propulsion->SetDimensions( propulsion->SetDimensions(
1 / Passer::LinearAlgebra::pi, // we use this, such that motor speed 1 -> 1 / Passer::LinearAlgebra::pi, // we use this, such that motor speed 1 ->
// velocity 1 m/s // velocity 1 m/s
1); 1);
Roboid* roboid = new Roboid(propulsion); Roboid *roboid = new Roboid(propulsion);
MockDistanceSensor* sensorLeft = new MockDistanceSensor(10.0F); MockDistanceSensor *sensorLeft = new MockDistanceSensor(10.0F);
sensorLeft->position.direction.horizontal = Angle16::Degrees(-30); sensorLeft->position.direction.horizontal = Angle16::Degrees(-30);
roboid->AddChild(sensorLeft); roboid->AddChild(sensorLeft);
MockDistanceSensor* sensorRight = new MockDistanceSensor(10.0F); MockDistanceSensor *sensorRight = new MockDistanceSensor(10.0F);
sensorRight->SetParent(roboid); sensorRight->SetParent(roboid);
sensorRight->position.direction.horizontal = Angle16::Degrees(30); sensorRight->position.direction.horizontal = Angle16::Degrees(30);
@ -88,7 +88,7 @@ TEST(BB2B, NoObstacle) {
EXPECT_FLOAT_EQ(leftMotorSpeed, 1.0F); EXPECT_FLOAT_EQ(leftMotorSpeed, 1.0F);
EXPECT_FLOAT_EQ(rightMotorSpeed, 1.0F); EXPECT_FLOAT_EQ(rightMotorSpeed, 1.0F);
DifferentialDrive* diffDrive = (DifferentialDrive*)roboid->propulsion; DifferentialDrive *diffDrive = (DifferentialDrive *)roboid->propulsion;
diffDrive->SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed); diffDrive->SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed);
float leftActualSpeed = motorLeft->GetActualSpeed(); float leftActualSpeed = motorLeft->GetActualSpeed();
@ -97,16 +97,16 @@ TEST(BB2B, NoObstacle) {
EXPECT_FLOAT_EQ(leftActualSpeed, 1.0F); EXPECT_FLOAT_EQ(leftActualSpeed, 1.0F);
EXPECT_FLOAT_EQ(rightActualSpeed, 1.0F); EXPECT_FLOAT_EQ(rightActualSpeed, 1.0F);
Polar velocity = Spherical16 velocity =
diffDrive->GetVelocity(); // this depends on the wheel diameter. diffDrive->GetVelocity(); // this depends on the wheel diameter.
EXPECT_FLOAT_EQ(velocity.distance, 1.0F); EXPECT_FLOAT_EQ(velocity.distance, 1.0F);
EXPECT_FLOAT_EQ(velocity.angle.InDegrees(), 0.0F); EXPECT_FLOAT_EQ(velocity.direction.horizontal.InDegrees(), 0.0F);
trackedObjectCount = roboid->perception->TrackedObjectCount(); trackedObjectCount = roboid->perception->TrackedObjectCount();
EXPECT_EQ(trackedObjectCount, 0); EXPECT_EQ(trackedObjectCount, 0);
InterestingThing** trackedObjects = roboid->perception->GetTrackedObjects(); InterestingThing **trackedObjects = roboid->perception->GetTrackedObjects();
InterestingThing* trackedObject = nullptr; InterestingThing *trackedObject = nullptr;
for (int i = 0; i < roboid->perception->maxObjectCount; i++) { for (int i = 0; i < roboid->perception->maxObjectCount; i++) {
if (trackedObjects[i] != nullptr) if (trackedObjects[i] != nullptr)
trackedObject = trackedObjects[0]; trackedObject = trackedObjects[0];
@ -119,22 +119,22 @@ TEST(BB2B, NoObstacle) {
TEST(BB2B, ObstacleLeft) { TEST(BB2B, ObstacleLeft) {
#pragma region Setup #pragma region Setup
Motor* motorLeft = new Motor(); Motor *motorLeft = new Motor();
Motor* motorRight = new Motor(); Motor *motorRight = new Motor();
DifferentialDrive* propulsion = new DifferentialDrive(motorLeft, motorRight); DifferentialDrive *propulsion = new DifferentialDrive(motorLeft, motorRight);
propulsion->SetDimensions( propulsion->SetDimensions(
1 / Passer::LinearAlgebra::pi, // we use this, such that motor speed 1 -> 1 / Passer::LinearAlgebra::pi, // we use this, such that motor speed 1 ->
// velocity 1 m/s // velocity 1 m/s
8 / Passer::LinearAlgebra::pi); // we use this, such that angular 8 / Passer::LinearAlgebra::pi); // we use this, such that angular
// velocity will be 90 degrees per second // velocity will be 90 degrees per second
Roboid* roboid = new Roboid(propulsion); Roboid *roboid = new Roboid(propulsion);
MockDistanceSensor* sensorLeft = new MockDistanceSensor(); MockDistanceSensor *sensorLeft = new MockDistanceSensor();
sensorLeft->position.direction.horizontal = Angle16::Degrees(-30); sensorLeft->position.direction.horizontal = Angle16::Degrees(-30);
roboid->AddChild(sensorLeft); roboid->AddChild(sensorLeft);
MockDistanceSensor* sensorRight = new MockDistanceSensor(); MockDistanceSensor *sensorRight = new MockDistanceSensor();
sensorRight->SetParent(roboid); sensorRight->SetParent(roboid);
sensorRight->position.direction.horizontal = Angle16::Degrees(30); sensorRight->position.direction.horizontal = Angle16::Degrees(30);
@ -171,8 +171,8 @@ TEST(BB2B, ObstacleLeft) {
EXPECT_EQ(trackedObjectCount, 1); EXPECT_EQ(trackedObjectCount, 1);
// Find the single tracked object // Find the single tracked object
InterestingThing** trackedObjects = roboid->perception->GetTrackedObjects(); InterestingThing **trackedObjects = roboid->perception->GetTrackedObjects();
InterestingThing* trackedObject = nullptr; InterestingThing *trackedObject = nullptr;
for (int i = 0; i < roboid->perception->maxObjectCount; i++) { for (int i = 0; i < roboid->perception->maxObjectCount; i++) {
if (trackedObjects[i] != nullptr) if (trackedObjects[i] != nullptr)
trackedObject = trackedObjects[i]; trackedObject = trackedObjects[i];
@ -194,7 +194,7 @@ TEST(BB2B, ObstacleLeft) {
EXPECT_FLOAT_EQ(leftMotorSpeed, 1.0F); EXPECT_FLOAT_EQ(leftMotorSpeed, 1.0F);
EXPECT_FLOAT_EQ(rightMotorSpeed, -1.0F); EXPECT_FLOAT_EQ(rightMotorSpeed, -1.0F);
DifferentialDrive* diffDrive = (DifferentialDrive*)roboid->propulsion; DifferentialDrive *diffDrive = (DifferentialDrive *)roboid->propulsion;
diffDrive->SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed); diffDrive->SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed);
#pragma endregion #pragma endregion
@ -222,10 +222,10 @@ TEST(BB2B, ObstacleLeft) {
TEST(BB2B, ObstacleRight) { TEST(BB2B, ObstacleRight) {
#pragma region Setup #pragma region Setup
Motor* motorLeft = new Motor(); Motor *motorLeft = new Motor();
Motor* motorRight = new Motor(); Motor *motorRight = new Motor();
DifferentialDrive* propulsion = new DifferentialDrive(motorLeft, motorRight); DifferentialDrive *propulsion = new DifferentialDrive(motorLeft, motorRight);
propulsion->SetDimensions( propulsion->SetDimensions(
1 / Passer::LinearAlgebra::pi, // we use this, such that motor speed 1 -> 1 / Passer::LinearAlgebra::pi, // we use this, such that motor speed 1 ->
// velocity 1 m/s // velocity 1 m/s
@ -233,12 +233,12 @@ TEST(BB2B, ObstacleRight) {
// velocity will be 90 degrees per // velocity will be 90 degrees per
// second // second
Roboid* roboid = new Roboid(propulsion); Roboid *roboid = new Roboid(propulsion);
MockDistanceSensor* sensorLeft = new MockDistanceSensor(); MockDistanceSensor *sensorLeft = new MockDistanceSensor();
sensorLeft->position.direction.horizontal = Angle16::Degrees(-30); sensorLeft->position.direction.horizontal = Angle16::Degrees(-30);
roboid->AddChild(sensorLeft); roboid->AddChild(sensorLeft);
MockDistanceSensor* sensorRight = new MockDistanceSensor(); MockDistanceSensor *sensorRight = new MockDistanceSensor();
sensorRight->SetParent(roboid); sensorRight->SetParent(roboid);
sensorRight->position.direction.horizontal = Angle16::Degrees(30); sensorRight->position.direction.horizontal = Angle16::Degrees(30);
@ -275,8 +275,8 @@ TEST(BB2B, ObstacleRight) {
EXPECT_EQ(trackedObjectCount, 1); EXPECT_EQ(trackedObjectCount, 1);
// Find the single tracked object // Find the single tracked object
InterestingThing** trackedObjects = roboid->perception->GetTrackedObjects(); InterestingThing **trackedObjects = roboid->perception->GetTrackedObjects();
InterestingThing* trackedObject = nullptr; InterestingThing *trackedObject = nullptr;
for (int i = 0; i < roboid->perception->maxObjectCount; i++) { for (int i = 0; i < roboid->perception->maxObjectCount; i++) {
if (trackedObjects[i] != nullptr) if (trackedObjects[i] != nullptr)
trackedObject = trackedObjects[i]; trackedObject = trackedObjects[i];
@ -298,7 +298,7 @@ TEST(BB2B, ObstacleRight) {
EXPECT_FLOAT_EQ(leftMotorSpeed, -1.0F); EXPECT_FLOAT_EQ(leftMotorSpeed, -1.0F);
EXPECT_FLOAT_EQ(rightMotorSpeed, 1.0F); EXPECT_FLOAT_EQ(rightMotorSpeed, 1.0F);
DifferentialDrive* diffDrive = (DifferentialDrive*)roboid->propulsion; DifferentialDrive *diffDrive = (DifferentialDrive *)roboid->propulsion;
diffDrive->SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed); diffDrive->SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed);
#pragma endregion #pragma endregion
@ -312,7 +312,7 @@ TEST(BB2B, ObstacleRight) {
EXPECT_FLOAT_EQ(rightActualSpeed, 1.0F); EXPECT_FLOAT_EQ(rightActualSpeed, 1.0F);
// Roboid velocity // Roboid velocity
Polar velocity = diffDrive->GetVelocity(); Spherical16 velocity = diffDrive->GetVelocity();
EXPECT_FLOAT_EQ(velocity.distance, 0.0F); EXPECT_FLOAT_EQ(velocity.distance, 0.0F);
float angularVelocity = diffDrive->GetAngularVelocity(); float angularVelocity = diffDrive->GetAngularVelocity();
@ -324,21 +324,21 @@ TEST(BB2B, ObstacleRight) {
TEST(BB2B, ObstacleBoth) { TEST(BB2B, ObstacleBoth) {
#pragma region Setup #pragma region Setup
Motor* motorLeft = new Motor(); Motor *motorLeft = new Motor();
Motor* motorRight = new Motor(); Motor *motorRight = new Motor();
DifferentialDrive* propulsion = new DifferentialDrive(motorLeft, motorRight); DifferentialDrive *propulsion = new DifferentialDrive(motorLeft, motorRight);
propulsion->SetDimensions( propulsion->SetDimensions(
1 / Passer::LinearAlgebra::pi, // we use this, such that motor speed 1 -> 1 / Passer::LinearAlgebra::pi, // we use this, such that motor speed 1 ->
// velocity 1 m/s // velocity 1 m/s
8 / Passer::LinearAlgebra::pi); // we use this, such that angular 8 / Passer::LinearAlgebra::pi); // we use this, such that angular
// velocity will be 90 degrees per second // velocity will be 90 degrees per second
Roboid* roboid = new Roboid(propulsion); Roboid *roboid = new Roboid(propulsion);
MockDistanceSensor* sensorLeft = new MockDistanceSensor(); MockDistanceSensor *sensorLeft = new MockDistanceSensor();
sensorLeft->position.direction.horizontal = Angle16::Degrees(-30); sensorLeft->position.direction.horizontal = Angle16::Degrees(-30);
roboid->AddChild(sensorLeft); roboid->AddChild(sensorLeft);
MockDistanceSensor* sensorRight = new MockDistanceSensor(); MockDistanceSensor *sensorRight = new MockDistanceSensor();
sensorRight->SetParent(roboid); sensorRight->SetParent(roboid);
sensorRight->position.direction.horizontal = Angle16::Degrees(30); sensorRight->position.direction.horizontal = Angle16::Degrees(30);
@ -375,7 +375,7 @@ TEST(BB2B, ObstacleBoth) {
EXPECT_EQ(trackedObjectCount, 2); EXPECT_EQ(trackedObjectCount, 2);
// Find the single tracked object // Find the single tracked object
InterestingThing** trackedObjects = roboid->perception->GetTrackedObjects(); InterestingThing **trackedObjects = roboid->perception->GetTrackedObjects();
for (int i = 0; i < roboid->perception->maxObjectCount; i++) { for (int i = 0; i < roboid->perception->maxObjectCount; i++) {
if (trackedObjects[i] != nullptr) { if (trackedObjects[i] != nullptr) {
EXPECT_FLOAT_EQ(trackedObjects[i]->position.distance, 0.1F); EXPECT_FLOAT_EQ(trackedObjects[i]->position.distance, 0.1F);
@ -395,7 +395,7 @@ TEST(BB2B, ObstacleBoth) {
EXPECT_FLOAT_EQ(leftMotorSpeed, -1.0F); EXPECT_FLOAT_EQ(leftMotorSpeed, -1.0F);
EXPECT_FLOAT_EQ(rightMotorSpeed, -1.0F); EXPECT_FLOAT_EQ(rightMotorSpeed, -1.0F);
DifferentialDrive* diffDrive = (DifferentialDrive*)roboid->propulsion; DifferentialDrive *diffDrive = (DifferentialDrive *)roboid->propulsion;
diffDrive->SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed); diffDrive->SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed);
#pragma endregion #pragma endregion
@ -409,9 +409,9 @@ TEST(BB2B, ObstacleBoth) {
EXPECT_FLOAT_EQ(rightActualSpeed, -1.0F); EXPECT_FLOAT_EQ(rightActualSpeed, -1.0F);
// Roboid velocity // Roboid velocity
Polar velocity = diffDrive->GetVelocity(); Spherical velocity = diffDrive->GetVelocity();
EXPECT_FLOAT_EQ(velocity.distance, 1.0F); EXPECT_FLOAT_EQ(velocity.distance, 1.0F);
EXPECT_FLOAT_EQ(velocity.angle.InDegrees(), -180.0F); EXPECT_FLOAT_EQ(velocity.direction.horizontal.InDegrees(), -180.0F);
float angularVelocity = diffDrive->GetAngularVelocity(); float angularVelocity = diffDrive->GetAngularVelocity();
EXPECT_FLOAT_EQ(angularVelocity, 0.0F); EXPECT_FLOAT_EQ(angularVelocity, 0.0F);