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

Fix tests

Browse Source
This commit is contained in:
Pascal Serrarens 2024-08-05 11:47:07 +02:00
parent 9b0a5e066f
commit 10700da996
1 changed files with 67 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

110
test/BB2B_Test.cc
View File

@ -12,9 +12,10 @@
/// @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) : DistanceSensor(triggerDistance){}; MockDistanceSensor(float triggerDistance)
: DistanceSensor(triggerDistance) {};
void SimulateMeasurement(float distance) { this->distance = distance; } void SimulateMeasurement(float distance) { this->distance = distance; }
}; };
@ -22,20 +23,21 @@ public:
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 / Angle::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); 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.horizontalAngle = -30; sensorLeft->position.horizontalAngle = -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.horizontalAngle = 30; sensorRight->position.horizontalAngle = 30;
@ -86,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();
@ -98,13 +100,13 @@ TEST(BB2B, NoObstacle) {
Polar velocity = Polar 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, 0.0F); EXPECT_FLOAT_EQ(velocity.angle.ToFloat(), 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];
@ -117,21 +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 / Angle::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 ->
8 / Angle::pi); // we use this, such that angular velocity will be 90 // velocity 1 m/s
// 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.horizontalAngle = -30; sensorLeft->position.horizontalAngle = -30;
roboid->AddChild(sensorLeft); roboid->AddChild(sensorLeft);
MockDistanceSensor *sensorRight = new MockDistanceSensor(); MockDistanceSensor* sensorRight = new MockDistanceSensor();
sensorRight->SetParent(roboid); sensorRight->SetParent(roboid);
sensorRight->position.horizontalAngle = 30; sensorRight->position.horizontalAngle = 30;
@ -168,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];
@ -177,7 +180,7 @@ TEST(BB2B, ObstacleLeft) {
ASSERT_FALSE(trackedObject == nullptr); ASSERT_FALSE(trackedObject == nullptr);
EXPECT_FLOAT_EQ(trackedObject->position.distance, 0.1F); EXPECT_FLOAT_EQ(trackedObject->position.distance, 0.1F);
EXPECT_FLOAT_EQ(trackedObject->position.horizontalAngle, -30); EXPECT_FLOAT_EQ(trackedObject->position.horizontalAngle.ToFloat(), -30);
#pragma endregion #pragma endregion
@ -190,7 +193,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
@ -218,21 +221,23 @@ 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 / Angle::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 ->
8 / Angle::pi); // we use this, such that angular velocity will be 90 // velocity 1 m/s
// 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.horizontalAngle = -30; sensorLeft->position.horizontalAngle = -30;
roboid->AddChild(sensorLeft); roboid->AddChild(sensorLeft);
MockDistanceSensor *sensorRight = new MockDistanceSensor(); MockDistanceSensor* sensorRight = new MockDistanceSensor();
sensorRight->SetParent(roboid); sensorRight->SetParent(roboid);
sensorRight->position.horizontalAngle = 30; sensorRight->position.horizontalAngle = 30;
@ -269,8 +274,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];
@ -278,7 +283,7 @@ TEST(BB2B, ObstacleRight) {
ASSERT_FALSE(trackedObject == nullptr); ASSERT_FALSE(trackedObject == nullptr);
EXPECT_FLOAT_EQ(trackedObject->position.distance, 0.1F); EXPECT_FLOAT_EQ(trackedObject->position.distance, 0.1F);
EXPECT_FLOAT_EQ(trackedObject->position.horizontalAngle, 30); EXPECT_FLOAT_EQ(trackedObject->position.horizontalAngle.ToFloat(), 30);
#pragma endregion #pragma endregion
@ -291,7 +296,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
@ -317,20 +322,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 / Angle::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 ->
8 / Angle::pi); // we use this, such that angular velocity will be 90 // velocity 1 m/s
// degrees per second 8 / Passer::LinearAlgebra::pi); // we use this, such that angular
Roboid *roboid = new Roboid(propulsion); // velocity will be 90 degrees per second
Roboid* roboid = new Roboid(propulsion);
MockDistanceSensor *sensorLeft = new MockDistanceSensor(); MockDistanceSensor* sensorLeft = new MockDistanceSensor();
sensorLeft->position.horizontalAngle = -30; sensorLeft->position.horizontalAngle = -30;
roboid->AddChild(sensorLeft); roboid->AddChild(sensorLeft);
MockDistanceSensor *sensorRight = new MockDistanceSensor(); MockDistanceSensor* sensorRight = new MockDistanceSensor();
sensorRight->SetParent(roboid); sensorRight->SetParent(roboid);
sensorRight->position.horizontalAngle = 30; sensorRight->position.horizontalAngle = 30;
@ -367,7 +373,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);
@ -387,7 +393,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
@ -403,7 +409,7 @@ TEST(BB2B, ObstacleBoth) {
// Roboid velocity // Roboid velocity
Polar velocity = diffDrive->GetVelocity(); Polar velocity = diffDrive->GetVelocity();
EXPECT_FLOAT_EQ(velocity.distance, 1.0F); EXPECT_FLOAT_EQ(velocity.distance, 1.0F);
EXPECT_FLOAT_EQ(velocity.angle, 180.0F); EXPECT_FLOAT_EQ(velocity.angle.ToFloat(), 180.0F);
float angularVelocity = diffDrive->GetAngularVelocity(); float angularVelocity = diffDrive->GetAngularVelocity();
EXPECT_FLOAT_EQ(angularVelocity, 0.0F); EXPECT_FLOAT_EQ(angularVelocity, 0.0F);