Simplified perception

Perception.cpp

@@ -44,7 +44,7 @@ Sensor* Perception::GetSensor(unsigned int sensorId) {
 
   return nullptr;
 }
-
+/*
 float Perception::DistanceForward(float angle) {
   float minDistance = INFINITY;
   for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
@@ -131,6 +131,61 @@ float Perception::DistanceDown(float angle) {
   }
   return minDistance;
 }
+*/
+
+float Perception::GetDistance(float fromAngle, float toAngle) {
+  float minDistance = INFINITY;
+  if (toAngle < fromAngle)
+    // Hmm. Can't look backward properly for now
+    return minDistance;
+
+  for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
+    Placement placement = sensorPlacements[sensorIx];
+    float angle = placement.horizontalDirection;
+    if (angle > fromAngle && angle < toAngle) {
+      Thing* thing = placement.thing;
+      if (thing == nullptr)
+        continue;
+
+      if (thing->type == Thing::DistanceSensorType) {
+        DistanceSensor* distanceSensor = (DistanceSensor*)thing;
+        if (distanceSensor != nullptr && distanceSensor->IsOn())
+          minDistance = fmin(minDistance, distanceSensor->GetDistance());
+      }
+    }
+  }
+  return minDistance;
+}
+
+float Perception::GetDistance(float fromHorizontalAngle,
+                              float toHorizontalAngle,
+                              float fromVerticalAngle,
+                              float toVerticalAngle) {
+  float minDistance = INFINITY;
+  if (toHorizontalAngle < fromHorizontalAngle ||
+      toVerticalAngle < fromVerticalAngle)
+    // Hmm. Can't look backward properly for now
+    return minDistance;
+
+  for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
+    Placement placement = sensorPlacements[sensorIx];
+    if (placement.horizontalDirection > fromHorizontalAngle &&
+        placement.horizontalDirection < toHorizontalAngle &&
+        placement.verticalDirection > fromVerticalAngle &&
+        placement.verticalDirection < toVerticalAngle) {
+      Thing* thing = placement.thing;
+      if (thing == nullptr)
+        continue;
+
+      if (thing->type == Thing::DistanceSensorType) {
+        DistanceSensor* distanceSensor = (DistanceSensor*)thing;
+        if (distanceSensor != nullptr && distanceSensor->IsOn())
+          minDistance = fmin(minDistance, distanceSensor->GetDistance());
+      }
+    }
+  }
+  return minDistance;
+}
 
 bool Perception::SwitchOn(float fromAngle, float toAngle) {
   if (toAngle < fromAngle)
@@ -165,24 +220,27 @@ unsigned int Perception::ToDepthMapIndex(float angle) {
   return depthMapIx;
 }
 
-float Perception::GetDistance(float angle) {
+// float Perception::GetDistance(float angle) {
-  if (depthMap != nullptr) {
+//   if (depthMap != nullptr) {
-    if (angle < rangeMinimum || angle > rangeMaximum)
+//     if (angle < rangeMinimum || angle > rangeMaximum)
-      return INFINITY;
+//       return INFINITY;
-    unsigned int depthMapIx = ToDepthMapIndex(angle);
+//     unsigned int depthMapIx = ToDepthMapIndex(angle);
-    return depthMap[depthMapIx];
+//     return depthMap[depthMapIx];
-  } else {
+//   } else {
-    for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
+//     for (unsigned int sensorIx = 0; sensorIx < this->sensorCount;
-      Placement placement = sensorPlacements[sensorIx];
+//     sensorIx++)
-      float placementAngle = placement.horizontalDirection;
+//     {
-      if (placementAngle == angle) {
+//       Placement placement = sensorPlacements[sensorIx];
-        DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
+//       float placementAngle = placement.horizontalDirection;
-        return distanceSensor->GetDistance();
+//       if (placementAngle == angle) {
-      }
+//         DistanceSensor* distanceSensor =
-    }
+//         (DistanceSensor*)placement.thing; return
-  }
+//         distanceSensor->GetDistance();
-  return INFINITY;
+//       }
-}
+//     }
+//   }
+//   return INFINITY;
+// }
 
 void Perception::SetResolution(unsigned int resolution) {
   this->resolution = resolution;

Perception.h

@@ -24,7 +24,7 @@ class Perception {
 
   unsigned int GetSensorCount();
   Sensor* GetSensor(unsigned int sensorId);
-
+  /*
     float DistanceForward(float angle = 90);
 
     /// @brief Distance to the closest object on the left
@@ -36,7 +36,8 @@ class Perception {
     /// @brief Distance to the closest object on the left
     /// @param angle the maximum angle on the left used for detection.
     /// @return distance in meters, INFINITY when no object is detected.
-  /// @note An object is on the left when the `angle` is between -`angle` and 0
+    /// @note An object is on the left when the `angle` is between -`angle` and
+    0
     /// degrees.
     /// @note An object dead straight (0 degrees) is not reported.
     /// @note When an object is beyond `angle` meters, it is not reported.
@@ -61,9 +62,13 @@ class Perception {
     float DistanceUp(float angle);
     float DistanceDown() { return DistanceDown(180); }
     float DistanceDown(float angle);
-
+  */
-  float Distance(float leftAngle, float rightAngle);
+  float GetDistance(float fromAngle, float toAngle);
-  float GetDistance(float angle);
+  float GetDistance(float fromHorizontalAngle,
+                    float toHorizontalAngle,
+                    float fromVerticalAngle,
+                    float toVerticalAngle);
+  //   float GetDistance(float angle);
 
   bool SwitchOn(float fromAngle, float toAngle);