Extended tests

DifferentialDrive.cpp

@@ -73,7 +73,6 @@ Polar DifferentialDrive::GetVelocity() {
   Motor *rightMotor = motors[1];
   float leftSpeed = leftMotor->GetActualSpeed();   // in revolutions per second
   float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions per second
-  // float rpsToMs = wheelDiameter * Angle::pi;
 
   leftSpeed = leftSpeed * rpsToMs;   // in meters per second
   rightSpeed = rightSpeed * rpsToMs; // in meters per second
@@ -91,7 +90,6 @@ float DifferentialDrive::GetAngularVelocity() {
   float leftSpeed = leftMotor->GetActualSpeed();   // in revolutions per second
   float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions per second
 
-  // float rpsToMs = wheelDiameter * Angle::pi;
   leftSpeed = leftSpeed * rpsToMs;   // in meters per second
   rightSpeed = rightSpeed * rpsToMs; // in meters per second
 

DistanceSensor.cpp

@@ -1,14 +1,28 @@
 #include "DistanceSensor.h"
 
-DistanceSensor::DistanceSensor() { this->type = Thing::DistanceSensorType; }
+#include <math.h>
 
-DistanceSensor::DistanceSensor(float triggerDistance) {
+DistanceSensor::DistanceSensor() {
+  this->type = Thing::DistanceSensorType;
+  this->distance = INFINITY;
+  this->triggerDistance = 1.0F;
+}
+
+DistanceSensor::DistanceSensor(float triggerDistance) : DistanceSensor() {
   this->triggerDistance = triggerDistance;
 }
 
-float DistanceSensor::GetDistance() { return distance; };
+float DistanceSensor::GetDistance() {
+  if (distance < minRange || distance > maxRange)
+    return -1; // invalid distance
+
+  return distance;
+};
 
 bool DistanceSensor::ObjectNearby() {
-  bool isOn = GetDistance() <= triggerDistance;
+  if (distance < minRange || distance > maxRange)
+    return false;
+
+  bool isOn = distance <= triggerDistance;
   return isOn;
 }

DistanceSensor.h

@@ -7,7 +7,7 @@ namespace RoboidControl {
 
 /// @brief A Sensor which can measure the distance to the nearest object
 class DistanceSensor : public Sensor {
- public:
+public:
   /// @brief Default constructor
   DistanceSensor();
   /// @brief Creates a DistanceSensor with the given trigger distance
@@ -19,6 +19,11 @@ class DistanceSensor : public Sensor {
   /// @return the measured distance in meters to the nearest object
   virtual float GetDistance();
 
+  /// @brief The minimum range at which the sensor gives reliable measurements
+  float minRange = 0.01F;
+  /// @brief The maximum range at which the sensor gives reliable measurements
+  float maxRange = 10.0F;
+
   /// @brief The distance at which ObjectNearby triggers
   float triggerDistance = 1;
 
@@ -26,11 +31,11 @@ class DistanceSensor : public Sensor {
   /// @return True when an object is nearby
   bool ObjectNearby();
 
- protected:
+protected:
   /// @brief Distance to the closest object
   float distance = 0;
 };
 
-}  // namespace RoboidControl
-}  // namespace Passer
+} // namespace RoboidControl
+} // namespace Passer
 using namespace Passer::RoboidControl;

Perception.cpp

@@ -26,6 +26,10 @@ Perception::Perception(Sensor **sensors, unsigned int sensorCount)
   this->sensors = new Sensor *[this->sensorCount];
   for (unsigned char sensorIx = 0; sensorIx < this->sensorCount; sensorIx++)
     this->sensors[sensorIx] = sensors[sensorIx];
+
+  this->trackedObjects = new TrackedObject *[maxObjectCount];
+  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++)
+    this->trackedObjects[objIx] = nullptr;
 }
 
 unsigned int Perception::GetSensorCount() { return this->sensorCount; }
@@ -209,9 +213,11 @@ void Perception::Update(float currentTimeMs) {
       DistanceSensor *distanceSensor = (DistanceSensor *)sensor;
 
       float distance = distanceSensor->GetDistance();
-      float angle = sensor->position.angle;
-      Polar position = Polar(angle, distance);
-      AddTrackedObject(distanceSensor, position);
+      if (distance >= 0) {
+        float angle = sensor->position.angle;
+        Polar position = Polar(angle, distance);
+        AddTrackedObject(distanceSensor, position);
+      }
 
     } else if (sensor->type == Thing::SwitchType) {
       Switch *switchSensor = (Switch *)sensor;

test/BB2B_Test.cc

@@ -1,4 +1,5 @@
 // #if GTEST
+
 #include <gtest/gtest.h>
 
 #include "../DifferentialDrive.h"
@@ -6,13 +7,25 @@
 #include "../Roboid.h"
 #include "../VectorAlgebra/Angle.h"
 
-TEST(BB2B, Basics) {
+/// @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();
 
-  DistanceSensor *sensorLeft = new DistanceSensor();
+  MockDistanceSensor *sensorLeft = new MockDistanceSensor(10.0F);
   sensorLeft->position.angle = -30;
-  DistanceSensor *sensorRight = new DistanceSensor();
+  MockDistanceSensor *sensorRight = new MockDistanceSensor(10.0F);
   sensorRight->position.angle = 30;
 
   Sensor *sensors[] = {sensorLeft, sensorRight};
@@ -26,6 +39,39 @@ TEST(BB2B, Basics) {
 
   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);
 
@@ -51,5 +97,223 @@ TEST(BB2B, Basics) {
       diffDrive->GetVelocity(); // 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
+      4 / Angle::pi); // we use this, such that angular velocity will be 180
+                      // 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->GetVelocity(); // this depends on the wheel diameter.
+  EXPECT_FLOAT_EQ(velocity.distance, 0.0F);
+  EXPECT_FLOAT_EQ(velocity.angle, 0.0F);
+
+  float angularVelocity =
+      diffDrive->GetAngularVelocity(); // this also depends on wheel separation
+  EXPECT_FLOAT_EQ(angularVelocity, 180.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
+      4 / Angle::pi); // we use this, such that angular velocity will be 180
+                      // 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->GetVelocity(); // this depends on the wheel diameter.
+  EXPECT_FLOAT_EQ(velocity.distance, 0.0F);
+  EXPECT_FLOAT_EQ(velocity.angle, 0.0F);
+
+  float angularVelocity =
+      diffDrive->GetAngularVelocity(); // this also depends on wheel separation
+  EXPECT_FLOAT_EQ(angularVelocity, -180.0F);
+
+#pragma endregion
+}
+
 // #endif

test/CMakeLists.txt

@@ -23,7 +23,7 @@ enable_testing()
 
 add_executable(
     RoboidControlTestNew
-    "BB2B_test.cc"
+    "BB2B_Test.cc"
 )
 
 target_link_libraries(