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

118
test/BB2B_Test.cc
View File

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