diff --git a/Perception.cpp b/Perception.cpp index 15c7ac2..cf1b53d 100644 --- a/Perception.cpp +++ b/Perception.cpp @@ -1,6 +1,202 @@ #include "Perception.h" -#include #include "Angle.h" +#include "Switch.h" + +Perception::Perception() {} + +void Perception::AddSensors(Placement* things, unsigned int thingCount) { + sensorCount = 0; + for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) { + Thing* thing = things[thingIx].thing; + if ((thing->type & Thing::SensorType) != 0) + sensorCount++; + } + + sensorPlacements = new Placement[sensorCount]; + + unsigned int sensorIx = 0; + for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) { + Thing* thing = things[thingIx].thing; + if ((thing->type & Thing::SensorType) != 0) { + sensorPlacements[sensorIx++] = things[thingIx]; + } + } +} + +unsigned int Perception::GetSensorCount() { + return this->sensorCount; +} + +Sensor* Perception::GetSensor(unsigned int sensorId) { + if (sensorId >= this->sensorCount) + return nullptr; + + Thing* thing = this->sensorPlacements[sensorId].thing; + if (thing->type & Thing::SensorType != 0) + return (Sensor*)thing; + + return nullptr; +} + +float Perception::DistanceForward(float angle) { + float minDistance = INFINITY; + for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { + Placement placement = sensorPlacements[sensorIx]; + Sensor* sensor = (Sensor*)placement.thing; + if (sensor->type & Thing::SensorType != 0) + continue; + + DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; + float sensorAngle = placement.direction.z; + if (sensorAngle > 0 && sensorAngle < angle) { + minDistance = fmin(minDistance, distanceSensor->GetDistance()); + } + } + return minDistance; +} + +float Perception::DistanceLeft(float angle) { + float minDistance = INFINITY; + for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { + Placement placement = sensorPlacements[sensorIx]; + Sensor* sensor = (Sensor*)placement.thing; + if (sensor->type & Thing::SensorType != 0) + continue; + + DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; + float sensorAngle = placement.direction.y; + // Serial.printf(" distance sensor: %f %f 0\n", -angle, sensorAngle); + if (sensorAngle < 0 && sensorAngle > -angle) { + minDistance = fmin(minDistance, distanceSensor->GetDistance()); + } + } + return minDistance; +} + +float Perception::DistanceRight(float angle) { + float minDistance = INFINITY; + for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { + Placement placement = sensorPlacements[sensorIx]; + Sensor* sensor = (Sensor*)placement.thing; + if (sensor->type != Thing::DistanceSensorType) + continue; + + DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; + float sensorAngle = placement.direction.y; + if (sensorAngle > 0 && sensorAngle < angle) { + minDistance = fmin(minDistance, distanceSensor->GetDistance()); + } + } + return minDistance; +} + +float Perception::DistanceUp(float angle) { + float minDistance = INFINITY; + for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { + Placement placement = sensorPlacements[sensorIx]; + Sensor* sensor = (Sensor*)placement.thing; + if (sensor->type & Thing::SensorType != 0) + continue; + + DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; + float sensorAngle = placement.direction.y; // not correct! + if (sensorAngle > 0 && sensorAngle < angle) { + minDistance = fmin(minDistance, distanceSensor->GetDistance()); + } + } + return minDistance; +} + +float Perception::DistanceDown(float angle) { + float minDistance = INFINITY; + for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { + Placement placement = sensorPlacements[sensorIx]; + Sensor* sensor = (Sensor*)placement.thing; + if (sensor->type & Thing::SensorType != 0) + continue; + + DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; + float sensorAngle = placement.direction.y; // not correct! + if (sensorAngle < 0 && sensorAngle > -angle) { + minDistance = fmin(minDistance, distanceSensor->GetDistance()); + } + } + return minDistance; +} + +bool Perception::SwitchOn(float fromAngle, float toAngle) { + if (toAngle < fromAngle) + return false; + + for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { + Placement placement = sensorPlacements[sensorIx]; + float angle = placement.direction.y; + if (angle > fromAngle && angle < toAngle) { + Thing* thing = placement.thing; + if (thing == nullptr) + continue; + + if ((thing->type & (int)Thing::Type::DistanceSensor) != 0) { + DistanceSensor* distanceSensor = (DistanceSensor*)thing; + if (distanceSensor != nullptr && distanceSensor->IsOn()) + return true; + } else if ((thing->type & (int)Thing::Type::Switch) != 0) { + Switch* switchSensor = (Switch*)thing; + if (switchSensor != nullptr && switchSensor->IsOn()) + return true; + } + } + } + return false; +} + +unsigned int Perception::ToDepthMapIndex(float angle) { + unsigned int depthMapIx = + (unsigned int)(((angle - rangeMinimum) / (rangeMaximum - rangeMinimum)) * + (float)resolution); + return depthMapIx; +} + +float Perception::GetDistance(float angle) { + if (depthMap != nullptr) { + if (angle < rangeMinimum || angle > rangeMaximum) + return INFINITY; + unsigned int depthMapIx = ToDepthMapIndex(angle); + return depthMap[depthMapIx]; + } else { + for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { + Placement placement = sensorPlacements[sensorIx]; + float placementAngle = placement.direction.x; + if (placementAngle == angle) { + DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; + return distanceSensor->GetDistance(); + } + } + } + return INFINITY; +} + +void Perception::SetResolution(unsigned int resolution) { + this->resolution = resolution; + this->depthMap = new float[this->resolution]; +} + +void Perception::SetRange(float min, float max) { + this->rangeMinimum = min; + this->rangeMaximum = max; +} + +float* Perception::GetDepthMap() { + return this->depthMap; +} + +void Perception::SetDepthMap(float angle, float distance) { + if (angle < rangeMinimum || angle > rangeMaximum) + return; + + unsigned int depthMapIx = ToDepthMapIndex(angle); + depthMap[depthMapIx] = distance; +} DistanceSensor* Perception::GetSensor(float angle) { angle = Angle::Normalize(angle); diff --git a/Perception.h b/Perception.h index 48584d6..4d1aeb3 100644 --- a/Perception.h +++ b/Perception.h @@ -1,15 +1,83 @@ -#include "Sensing.h" +#pragma once + +#include "Placement.h" +#include "Sensor.h" namespace Passer { namespace RoboidControl { -// using Perception = Sensing; +class Perception { + public: + /// @brief Setup perception + Perception(); -class Perception : public Sensing { -public: - DistanceSensor *GetSensor(float angle); + // void AddSensors(SensorPlacement* sensors, unsigned int sensorCount); + void AddSensors(Placement* sensors, unsigned int sensorCount); + + unsigned int GetSensorCount(); + Sensor* GetSensor(unsigned int sensorId); + + float DistanceForward(float angle = 90); + + /// @brief Distance to the closest object on the left + /// @return distance in meters, INFINITY when no object is detected. + /// @note An object is on the left when the `angle` is between -180 and 0 + /// degrees. + /// @note An object dead straight (0 degrees) is not reported. + float DistanceLeft() { return DistanceLeft(180); } + /// @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 + /// degrees. + /// @note An object dead straight (0 degrees) is not reported. + /// @note When an object is beyond `angle` meters, it is not reported. + float DistanceLeft(float angle); + + /// @brief Distance to the closest object on the right + /// @return distance in meters, INFINITY when no object is detected + /// @note An object is on the right when the `angle` is between 0 and 180 + /// degrees + /// @note An object dead straight (0 degrees) is not reported + float DistanceRight() { return DistanceRight(180); } + /// @brief Distance to the closest object on the right + /// @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 0 and `angle` + /// degrees. + /// @note An object dead straight (0 degrees) is not reported. + /// @note When an object is beyond `angle` meters, it is not reported. + float DistanceRight(float angle); + + float DistanceUp() { return DistanceUp(180); } + float DistanceUp(float angle); + float DistanceDown() { return DistanceDown(180); } + float DistanceDown(float angle); + + float Distance(float leftAngle, float rightAngle); + float GetDistance(float angle); + + bool SwitchOn(float fromAngle, float toAngle); + + void SetResolution(unsigned int resolution); + void SetRange(float min, float max); + float* GetDepthMap(); + unsigned int ToDepthMapIndex(float angle); + void SetDepthMap(float angle, float distance); + + DistanceSensor* GetSensor(float angle); + + protected: + // SensorPlacement* sensors = nullptr; + Placement* sensorPlacements = nullptr; + unsigned int sensorCount = 0; + + unsigned int resolution; + float rangeMinimum; + float rangeMaximum; + float* depthMap = nullptr; }; -} // namespace RoboidControl -} // namespace Passer +} // namespace RoboidControl +} // namespace Passer using namespace Passer::RoboidControl; \ No newline at end of file diff --git a/Sensing.cpp b/Sensing.cpp deleted file mode 100644 index 85bb126..0000000 --- a/Sensing.cpp +++ /dev/null @@ -1,213 +0,0 @@ -#include "Sensing.h" -#include "DistanceSensor.h" -#include "Switch.h" - -#include - -SensorPlacement::SensorPlacement(DistanceSensor* distanceSensor, - Vector2 direction) { - this->distanceSensor = distanceSensor; - this->switchSensor = nullptr; - this->direction = direction; -} -SensorPlacement::SensorPlacement(Switch* switchSensor, Vector2 direction) { - this->distanceSensor = nullptr; - this->switchSensor = switchSensor; - this->direction = direction; -} - -Sensing::Sensing() {} - -void Sensing::AddSensors(Placement* things, unsigned int thingCount) { - sensorCount = 0; - for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) { - Thing* thing = things[thingIx].thing; - if ((thing->type & Thing::SensorType) != 0) - sensorCount++; - } - - sensorPlacements = new Placement[sensorCount]; - - unsigned int sensorIx = 0; - for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) { - Thing* thing = things[thingIx].thing; - if ((thing->type & Thing::SensorType) != 0) { - sensorPlacements[sensorIx++] = things[thingIx]; - } - } -} - -unsigned int Sensing::GetSensorCount() { - return this->sensorCount; -} - -Sensor* Sensing::GetSensor(unsigned int sensorId) { - if (sensorId >= this->sensorCount) - return nullptr; - - Thing* thing = this->sensorPlacements[sensorId].thing; - if (thing->type & Thing::SensorType != 0) - return (Sensor*)thing; - - return nullptr; -} - -float Sensing::DistanceForward(float angle) { - float minDistance = INFINITY; - for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { - Placement placement = sensorPlacements[sensorIx]; - Sensor* sensor = (Sensor*)placement.thing; - if (sensor->type & Thing::SensorType != 0) - continue; - - DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; - float sensorAngle = placement.direction.z; - if (sensorAngle > 0 && sensorAngle < angle) { - minDistance = fmin(minDistance, distanceSensor->GetDistance()); - } - } - return minDistance; -} - -float Sensing::DistanceLeft(float angle) { - float minDistance = INFINITY; - for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { - Placement placement = sensorPlacements[sensorIx]; - Sensor* sensor = (Sensor*)placement.thing; - if (sensor->type & Thing::SensorType != 0) - continue; - - DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; - float sensorAngle = placement.direction.y; - // Serial.printf(" distance sensor: %f %f 0\n", -angle, sensorAngle); - if (sensorAngle < 0 && sensorAngle > -angle) { - minDistance = fmin(minDistance, distanceSensor->GetDistance()); - } - } - return minDistance; -} - -float Sensing::DistanceRight(float angle) { - float minDistance = INFINITY; - for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { - Placement placement = sensorPlacements[sensorIx]; - Sensor* sensor = (Sensor*)placement.thing; - if (sensor->type != Thing::DistanceSensorType) - continue; - - DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; - float sensorAngle = placement.direction.y; - if (sensorAngle > 0 && sensorAngle < angle) { - minDistance = fmin(minDistance, distanceSensor->GetDistance()); - } - } - return minDistance; -} - -float Sensing::DistanceUp(float angle) { - float minDistance = INFINITY; - for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { - Placement placement = sensorPlacements[sensorIx]; - Sensor* sensor = (Sensor*)placement.thing; - if (sensor->type & Thing::SensorType != 0) - continue; - - DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; - float sensorAngle = placement.direction.y; // not correct! - if (sensorAngle > 0 && sensorAngle < angle) { - minDistance = fmin(minDistance, distanceSensor->GetDistance()); - } - } - return minDistance; -} - -float Sensing::DistanceDown(float angle) { - float minDistance = INFINITY; - for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { - Placement placement = sensorPlacements[sensorIx]; - Sensor* sensor = (Sensor*)placement.thing; - if (sensor->type & Thing::SensorType != 0) - continue; - - DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; - float sensorAngle = placement.direction.y; // not correct! - if (sensorAngle < 0 && sensorAngle > -angle) { - minDistance = fmin(minDistance, distanceSensor->GetDistance()); - } - } - return minDistance; -} - -bool Sensing::SwitchOn(float fromAngle, float toAngle) { - if (toAngle < fromAngle) - return false; - - for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { - Placement placement = sensorPlacements[sensorIx]; - float angle = placement.direction.y; - if (angle > fromAngle && angle < toAngle) { - Thing* thing = placement.thing; - if (thing == nullptr) - continue; - - if ((thing->type & (int)Thing::Type::DistanceSensor) != 0) { - DistanceSensor* distanceSensor = (DistanceSensor*)thing; - if (distanceSensor != nullptr && distanceSensor->IsOn()) - return true; - } else if ((thing->type & (int)Thing::Type::Switch) != 0) { - Switch* switchSensor = (Switch*)thing; - if (switchSensor != nullptr && switchSensor->IsOn()) - return true; - } - } - } - return false; -} - -unsigned int Sensing::ToDepthMapIndex(float angle) { - unsigned int depthMapIx = - (unsigned int)(((angle - rangeMinimum) / (rangeMaximum - rangeMinimum)) * - (float)resolution); - return depthMapIx; -} - -float Sensing::GetDistance(float angle) { - if (depthMap != nullptr) { - if (angle < rangeMinimum || angle > rangeMaximum) - return INFINITY; - unsigned int depthMapIx = ToDepthMapIndex(angle); - return depthMap[depthMapIx]; - } else { - for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { - Placement placement = sensorPlacements[sensorIx]; - float placementAngle = placement.direction.x; - if (placementAngle == angle) { - DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; - return distanceSensor->GetDistance(); - } - } - } - return INFINITY; -} - -void Sensing::SetResolution(unsigned int resolution) { - this->resolution = resolution; - this->depthMap = new float[this->resolution]; -} - -void Sensing::SetRange(float min, float max) { - this->rangeMinimum = min; - this->rangeMaximum = max; -} - -float* Sensing::GetDepthMap() { - return this->depthMap; -} - -void Sensing::SetDepthMap(float angle, float distance) { - if (angle < rangeMinimum || angle > rangeMaximum) - return; - - unsigned int depthMapIx = ToDepthMapIndex(angle); - depthMap[depthMapIx] = distance; -} \ No newline at end of file diff --git a/Sensing.h b/Sensing.h deleted file mode 100644 index 94d56c5..0000000 --- a/Sensing.h +++ /dev/null @@ -1,99 +0,0 @@ -#pragma once - -#include "Placement.h" -#include "Sensor.h" - -// #include - -namespace Passer::RoboidControl { - -class DistanceSensor; -class Switch; - -class NewSensorPlacement : public Placement {}; - -struct SensorPlacement { - DistanceSensor* distanceSensor; - Switch* switchSensor; - Vector2 direction; - - SensorPlacement(DistanceSensor* distanceSensor, Vector2 direction); - SensorPlacement(Switch* switchSensor, Vector2 direction); -}; - -// class Perception : public Sensing { -// } - -class Sensing { - public: - /// @brief Setup sensing - Sensing(); - - // void AddSensors(SensorPlacement* sensors, unsigned int sensorCount); - void AddSensors(Placement* sensors, unsigned int sensorCount); - - unsigned int GetSensorCount(); - Sensor* GetSensor(unsigned int sensorId); - - float DistanceForward(float angle = 90); - - /// @brief Distance to the closest object on the left - /// @return distance in meters, INFINITY when no object is detected. - /// @note An object is on the left when the `angle` is between -180 and 0 - /// degrees. - /// @note An object dead straight (0 degrees) is not reported. - float DistanceLeft() { return DistanceLeft(180); } - /// @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 - /// degrees. - /// @note An object dead straight (0 degrees) is not reported. - /// @note When an object is beyond `angle` meters, it is not reported. - float DistanceLeft(float angle); - - /// @brief Distance to the closest object on the right - /// @return distance in meters, INFINITY when no object is detected - /// @note An object is on the right when the `angle` is between 0 and 180 - /// degrees - /// @note An object dead straight (0 degrees) is not reported - float DistanceRight() { return DistanceRight(180); } - /// @brief Distance to the closest object on the right - /// @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 0 and `angle` - /// degrees. - /// @note An object dead straight (0 degrees) is not reported. - /// @note When an object is beyond `angle` meters, it is not reported. - float DistanceRight(float angle); - - float DistanceUp() { return DistanceUp(180); } - float DistanceUp(float angle); - float DistanceDown() { return DistanceDown(180); } - float DistanceDown(float angle); - - float Distance(float leftAngle, float rightAngle); - float GetDistance(float angle); - - bool SwitchOn(float fromAngle, float toAngle); - - void SetResolution(unsigned int resolution); - void SetRange(float min, float max); - float* GetDepthMap(); - unsigned int ToDepthMapIndex(float angle); - void SetDepthMap(float angle, float distance); - - protected: - // SensorPlacement* sensors = nullptr; - Placement* sensorPlacements = nullptr; - unsigned int sensorCount = 0; - - unsigned int resolution; - float rangeMinimum; - float rangeMaximum; - float* depthMap = nullptr; -}; - -} // namespace Passer::RoboidControl - -using namespace Passer::RoboidControl; \ No newline at end of file