Added test

test/BB2B_Test.cc

@@ -1,5 +1,7 @@
 // #if GTEST
 
+// #include <gmock/gmock.h>
+// not supported using Visual Studio 2022 compiler...
 #include <gtest/gtest.h>
 
 #include "../DifferentialDrive.h"
@@ -312,4 +314,102 @@ TEST(BB2B, ObstacleRight) {
 #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->GetVelocity();
+  EXPECT_FLOAT_EQ(velocity.distance, 1.0F);
+  EXPECT_FLOAT_EQ(velocity.angle, 180.0F);
+
+  float angularVelocity = diffDrive->GetAngularVelocity();
+  EXPECT_FLOAT_EQ(angularVelocity, 0.0F);
+
+#pragma endregion
+}
+
 // #endif