417 lines
12 KiB
C++
417 lines
12 KiB
C++
#if GTEST
|
|
|
|
// #include <gmock/gmock.h>
|
|
// not supported using Visual Studio 2022 compiler...
|
|
#include <gtest/gtest.h>
|
|
|
|
#include "../DifferentialDrive.h"
|
|
#include "../DistanceSensor.h"
|
|
#include "../Roboid.h"
|
|
#include "../VectorAlgebra/Angle.h"
|
|
|
|
/// @brief A Distance sensor with testing support
|
|
/// With this sensor it is possible to simulate a measurement
|
|
class MockDistanceSensor : public DistanceSensor {
|
|
public:
|
|
MockDistanceSensor() : DistanceSensor(){};
|
|
MockDistanceSensor(float triggerDistance) : DistanceSensor(triggerDistance){};
|
|
|
|
void SimulateMeasurement(float distance) { this->distance = distance; }
|
|
};
|
|
|
|
TEST(BB2B, NoObstacle) {
|
|
#pragma region Setup
|
|
|
|
Motor *motorLeft = new Motor();
|
|
Motor *motorRight = new Motor();
|
|
|
|
MockDistanceSensor *sensorLeft = new MockDistanceSensor(10.0F);
|
|
sensorLeft->position.angle = -30;
|
|
MockDistanceSensor *sensorRight = new MockDistanceSensor(10.0F);
|
|
sensorRight->position.angle = 30;
|
|
|
|
Sensor *sensors[] = {sensorLeft, sensorRight};
|
|
|
|
Perception *perception = new Perception(sensors, 2);
|
|
|
|
DifferentialDrive *propulsion = new DifferentialDrive(motorLeft, motorRight);
|
|
propulsion->SetDimensions(
|
|
1 / Angle::pi, // we use this, such that motor speed 1 -> velocity 1 m/s
|
|
1);
|
|
|
|
Roboid *roboid = new Roboid(perception, propulsion);
|
|
|
|
#pragma endregion
|
|
|
|
#pragma region Test initial state
|
|
|
|
// Sensors should return no valid value
|
|
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
|
|
|
|
#pragma endregion
|
|
|
|
#pragma region Measurement 1
|
|
|
|
/// closest objects are too far away
|
|
sensorLeft->SimulateMeasurement(100.0F);
|
|
sensorRight->SimulateMeasurement(100.0F);
|
|
|
|
roboid->Update(0.0F);
|
|
|
|
#pragma endregion
|
|
|
|
#pragma region Test state 1
|
|
distanceLeft = sensorLeft->GetDistance();
|
|
distanceRight = sensorRight->GetDistance();
|
|
|
|
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);
|
|
|
|
bool obstacleLeft = roboid->perception->ObjectNearby(-30);
|
|
bool obstacleRight = roboid->perception->ObjectNearby(30);
|
|
|
|
EXPECT_FALSE(obstacleLeft);
|
|
EXPECT_FALSE(obstacleRight);
|
|
|
|
float leftMotorSpeed = obstacleRight ? -1.0F : 1.0F;
|
|
float rightMotorSpeed = obstacleLeft ? -1.0F : 1.0F;
|
|
|
|
EXPECT_FLOAT_EQ(leftMotorSpeed, 1.0F);
|
|
EXPECT_FLOAT_EQ(rightMotorSpeed, 1.0F);
|
|
|
|
DifferentialDrive *diffDrive = (DifferentialDrive *)roboid->propulsion;
|
|
diffDrive->SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed);
|
|
|
|
float leftActualSpeed = motorLeft->GetActualSpeed();
|
|
float rightActualSpeed = motorRight->GetActualSpeed();
|
|
|
|
EXPECT_FLOAT_EQ(leftActualSpeed, 1.0F);
|
|
EXPECT_FLOAT_EQ(rightActualSpeed, 1.0F);
|
|
|
|
Polar velocity =
|
|
diffDrive->GetActualVelocity(); // this depends on the wheel diameter.
|
|
EXPECT_FLOAT_EQ(velocity.distance, 1.0F);
|
|
EXPECT_FLOAT_EQ(velocity.angle, 0.0F);
|
|
|
|
trackedObjectCount = roboid->perception->TrackedObjectCount();
|
|
EXPECT_EQ(trackedObjectCount, 0);
|
|
|
|
TrackedObject **trackedObjects = roboid->perception->GetTrackedObjects();
|
|
TrackedObject *trackedObject = nullptr;
|
|
for (int i = 0; i < roboid->perception->maxObjectCount; i++) {
|
|
if (trackedObjects[i] != nullptr)
|
|
trackedObject = trackedObjects[0];
|
|
}
|
|
|
|
EXPECT_TRUE(trackedObject == NULL);
|
|
#pragma endregion
|
|
}
|
|
|
|
TEST(BB2B, ObstacleLeft) {
|
|
#pragma region Setup
|
|
|
|
Motor *motorLeft = new Motor();
|
|
Motor *motorRight = new Motor();
|
|
|
|
MockDistanceSensor *sensorLeft = new MockDistanceSensor();
|
|
sensorLeft->position.angle = -30;
|
|
MockDistanceSensor *sensorRight = new MockDistanceSensor();
|
|
sensorRight->position.angle = 30;
|
|
|
|
Sensor *sensors[] = {sensorLeft, sensorRight};
|
|
|
|
Perception *perception = new Perception(sensors, 2);
|
|
|
|
DifferentialDrive *propulsion = new DifferentialDrive(motorLeft, motorRight);
|
|
propulsion->SetDimensions(
|
|
1 / Angle::pi, // we use this, such that motor speed 1 -> velocity 1 m/s
|
|
8 / Angle::pi); // we use this, such that angular velocity will be 90
|
|
// degrees per second
|
|
|
|
Roboid *roboid = new Roboid(perception, propulsion);
|
|
|
|
#pragma endregion
|
|
|
|
#pragma region Sensor Measurement - only left
|
|
|
|
// place obstacle on the left
|
|
sensorLeft->SimulateMeasurement(0.1F);
|
|
|
|
roboid->Update(0.1F);
|
|
|
|
#pragma endregion
|
|
|
|
#pragma region Test Sensor output
|
|
// Distance
|
|
float distanceLeft = sensorLeft->GetDistance();
|
|
float distanceRight = sensorRight->GetDistance();
|
|
|
|
EXPECT_FLOAT_EQ(distanceLeft, 0.1F);
|
|
EXPECT_LT(distanceRight, 0.0F); // invalid
|
|
|
|
// Obstacle
|
|
bool obstacleLeft = roboid->perception->ObjectNearby(-30);
|
|
bool obstacleRight = roboid->perception->ObjectNearby(30);
|
|
|
|
EXPECT_TRUE(obstacleLeft);
|
|
EXPECT_FALSE(obstacleRight);
|
|
|
|
// Tracked objects
|
|
int trackedObjectCount = roboid->perception->TrackedObjectCount();
|
|
EXPECT_EQ(trackedObjectCount, 1);
|
|
|
|
// Find the single tracked object
|
|
TrackedObject **trackedObjects = roboid->perception->GetTrackedObjects();
|
|
TrackedObject *trackedObject = nullptr;
|
|
for (int i = 0; i < roboid->perception->maxObjectCount; i++) {
|
|
if (trackedObjects[i] != nullptr)
|
|
trackedObject = trackedObjects[i];
|
|
}
|
|
|
|
ASSERT_FALSE(trackedObject == nullptr);
|
|
EXPECT_FLOAT_EQ(trackedObject->position.distance, 0.1F);
|
|
EXPECT_FLOAT_EQ(trackedObject->position.angle, -30);
|
|
|
|
#pragma endregion
|
|
|
|
#pragma region Motor Control
|
|
|
|
// Motor speeds
|
|
float leftMotorSpeed = obstacleRight ? -1.0F : 1.0F;
|
|
float rightMotorSpeed = obstacleLeft ? -1.0F : 1.0F;
|
|
|
|
EXPECT_FLOAT_EQ(leftMotorSpeed, 1.0F);
|
|
EXPECT_FLOAT_EQ(rightMotorSpeed, -1.0F);
|
|
|
|
DifferentialDrive *diffDrive = (DifferentialDrive *)roboid->propulsion;
|
|
diffDrive->SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed);
|
|
|
|
#pragma endregion
|
|
|
|
#pragma region Test motor output results
|
|
|
|
float leftActualSpeed = motorLeft->GetActualSpeed();
|
|
float rightActualSpeed = motorRight->GetActualSpeed();
|
|
|
|
EXPECT_FLOAT_EQ(leftActualSpeed, 1.0F);
|
|
EXPECT_FLOAT_EQ(rightActualSpeed, -1.0F);
|
|
|
|
// Roboid velocity
|
|
Polar velocity =
|
|
diffDrive->GetActualVelocity(); // this depends on the wheel diameter.
|
|
EXPECT_FLOAT_EQ(velocity.distance, 0.0F);
|
|
|
|
float angularVelocity =
|
|
diffDrive
|
|
->GetActualYawVelocity(); // this also depends on wheel separation
|
|
EXPECT_FLOAT_EQ(angularVelocity, 90.0F);
|
|
|
|
#pragma endregion
|
|
}
|
|
|
|
TEST(BB2B, ObstacleRight) {
|
|
#pragma region Setup
|
|
|
|
Motor *motorLeft = new Motor();
|
|
Motor *motorRight = new Motor();
|
|
|
|
MockDistanceSensor *sensorLeft = new MockDistanceSensor();
|
|
sensorLeft->position.angle = -30;
|
|
MockDistanceSensor *sensorRight = new MockDistanceSensor();
|
|
sensorRight->position.angle = 30;
|
|
|
|
Sensor *sensors[] = {sensorLeft, sensorRight};
|
|
|
|
Perception *perception = new Perception(sensors, 2);
|
|
|
|
DifferentialDrive *propulsion = new DifferentialDrive(motorLeft, motorRight);
|
|
propulsion->SetDimensions(
|
|
1 / Angle::pi, // we use this, such that motor speed 1 -> velocity 1 m/s
|
|
8 / Angle::pi); // we use this, such that angular velocity will be 90
|
|
// degrees per second
|
|
|
|
Roboid *roboid = new Roboid(perception, propulsion);
|
|
|
|
#pragma endregion
|
|
|
|
#pragma region Sensor Measurement - only right
|
|
|
|
// place obstacle on the left
|
|
sensorRight->SimulateMeasurement(0.1F);
|
|
|
|
roboid->Update(0.1F);
|
|
|
|
#pragma endregion
|
|
|
|
#pragma region Test Sensor output
|
|
// Distance
|
|
float distanceLeft = sensorLeft->GetDistance();
|
|
float distanceRight = sensorRight->GetDistance();
|
|
|
|
EXPECT_LT(distanceLeft, 0.0F); // invalid
|
|
EXPECT_FLOAT_EQ(distanceRight, 0.1F);
|
|
|
|
// Obstacle
|
|
bool obstacleLeft = roboid->perception->ObjectNearby(-30);
|
|
bool obstacleRight = roboid->perception->ObjectNearby(30);
|
|
|
|
EXPECT_FALSE(obstacleLeft);
|
|
EXPECT_TRUE(obstacleRight);
|
|
|
|
// Tracked objects
|
|
int trackedObjectCount = roboid->perception->TrackedObjectCount();
|
|
EXPECT_EQ(trackedObjectCount, 1);
|
|
|
|
// Find the single tracked object
|
|
TrackedObject **trackedObjects = roboid->perception->GetTrackedObjects();
|
|
TrackedObject *trackedObject = nullptr;
|
|
for (int i = 0; i < roboid->perception->maxObjectCount; i++) {
|
|
if (trackedObjects[i] != nullptr)
|
|
trackedObject = trackedObjects[i];
|
|
}
|
|
|
|
ASSERT_FALSE(trackedObject == nullptr);
|
|
EXPECT_FLOAT_EQ(trackedObject->position.distance, 0.1F);
|
|
EXPECT_FLOAT_EQ(trackedObject->position.angle, 30);
|
|
|
|
#pragma endregion
|
|
|
|
#pragma region Motor Control
|
|
|
|
// Motor speeds
|
|
float leftMotorSpeed = obstacleRight ? -1.0F : 1.0F;
|
|
float rightMotorSpeed = obstacleLeft ? -1.0F : 1.0F;
|
|
|
|
EXPECT_FLOAT_EQ(leftMotorSpeed, -1.0F);
|
|
EXPECT_FLOAT_EQ(rightMotorSpeed, 1.0F);
|
|
|
|
DifferentialDrive *diffDrive = (DifferentialDrive *)roboid->propulsion;
|
|
diffDrive->SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed);
|
|
|
|
#pragma endregion
|
|
|
|
#pragma region Test motor output results
|
|
|
|
float leftActualSpeed = motorLeft->GetActualSpeed();
|
|
float rightActualSpeed = motorRight->GetActualSpeed();
|
|
|
|
EXPECT_FLOAT_EQ(leftActualSpeed, -1.0F);
|
|
EXPECT_FLOAT_EQ(rightActualSpeed, 1.0F);
|
|
|
|
// Roboid velocity
|
|
Polar velocity = diffDrive->GetActualVelocity();
|
|
EXPECT_FLOAT_EQ(velocity.distance, 0.0F);
|
|
|
|
float angularVelocity = diffDrive->GetActualYawVelocity();
|
|
EXPECT_FLOAT_EQ(angularVelocity, -90.0F);
|
|
|
|
#pragma endregion
|
|
}
|
|
|
|
TEST(BB2B, ObstacleBoth) {
|
|
#pragma region Setup
|
|
|
|
Motor *motorLeft = new Motor();
|
|
Motor *motorRight = new Motor();
|
|
|
|
MockDistanceSensor *sensorLeft = new MockDistanceSensor();
|
|
sensorLeft->position.angle = -30;
|
|
MockDistanceSensor *sensorRight = new MockDistanceSensor();
|
|
sensorRight->position.angle = 30;
|
|
|
|
Sensor *sensors[] = {sensorLeft, sensorRight};
|
|
|
|
Perception *perception = new Perception(sensors, 2);
|
|
|
|
DifferentialDrive *propulsion = new DifferentialDrive(motorLeft, motorRight);
|
|
propulsion->SetDimensions(
|
|
1 / Angle::pi, // we use this, such that motor speed 1 -> velocity 1 m/s
|
|
8 / Angle::pi); // we use this, such that angular velocity will be 90
|
|
// degrees per second
|
|
|
|
Roboid *roboid = new Roboid(perception, propulsion);
|
|
|
|
#pragma endregion
|
|
|
|
#pragma region Sensor Measurement - left and right
|
|
|
|
sensorLeft->SimulateMeasurement(0.1F);
|
|
sensorRight->SimulateMeasurement(0.1F);
|
|
|
|
roboid->Update(0.1F);
|
|
|
|
#pragma endregion
|
|
|
|
#pragma region Test Sensor output
|
|
// Distance
|
|
float distanceLeft = sensorLeft->GetDistance();
|
|
float distanceRight = sensorRight->GetDistance();
|
|
|
|
EXPECT_FLOAT_EQ(distanceLeft, 0.1F);
|
|
EXPECT_FLOAT_EQ(distanceRight, 0.1F);
|
|
|
|
// Obstacle
|
|
bool obstacleLeft = roboid->perception->ObjectNearby(-30);
|
|
bool obstacleRight = roboid->perception->ObjectNearby(30);
|
|
|
|
EXPECT_TRUE(obstacleLeft);
|
|
EXPECT_TRUE(obstacleRight);
|
|
|
|
// Tracked objects
|
|
int trackedObjectCount = roboid->perception->TrackedObjectCount();
|
|
EXPECT_EQ(trackedObjectCount, 2);
|
|
|
|
// Find the single tracked object
|
|
TrackedObject **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);
|
|
// EXPECT_THAT(trackedObjects[i] ->position.angle, AnyOf(FloatEq(-30),
|
|
// FloatEq(30));
|
|
}
|
|
}
|
|
|
|
#pragma endregion
|
|
|
|
#pragma region Motor Control
|
|
|
|
// Motor speeds
|
|
float leftMotorSpeed = obstacleRight ? -1.0F : 1.0F;
|
|
float rightMotorSpeed = obstacleLeft ? -1.0F : 1.0F;
|
|
|
|
EXPECT_FLOAT_EQ(leftMotorSpeed, -1.0F);
|
|
EXPECT_FLOAT_EQ(rightMotorSpeed, -1.0F);
|
|
|
|
DifferentialDrive *diffDrive = (DifferentialDrive *)roboid->propulsion;
|
|
diffDrive->SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed);
|
|
|
|
#pragma endregion
|
|
|
|
#pragma region Test motor output results
|
|
|
|
float leftActualSpeed = motorLeft->GetActualSpeed();
|
|
float rightActualSpeed = motorRight->GetActualSpeed();
|
|
|
|
EXPECT_FLOAT_EQ(leftActualSpeed, -1.0F);
|
|
EXPECT_FLOAT_EQ(rightActualSpeed, -1.0F);
|
|
|
|
// Roboid velocity
|
|
Polar velocity = diffDrive->GetActualVelocity();
|
|
EXPECT_FLOAT_EQ(velocity.distance, 1.0F);
|
|
EXPECT_FLOAT_EQ(velocity.angle, 180.0F);
|
|
|
|
float angularVelocity = diffDrive->GetActualYawVelocity();
|
|
EXPECT_FLOAT_EQ(angularVelocity, 0.0F);
|
|
|
|
#pragma endregion
|
|
}
|
|
|
|
#endif
|