Replaced Sensing by perception
This commit is contained in:
parent
d348070092
commit
07389498dd
198
Perception.cpp
198
Perception.cpp
@ -1,6 +1,202 @@
|
|||||||
#include "Perception.h"
|
#include "Perception.h"
|
||||||
#include <Arduino.h>
|
|
||||||
#include "Angle.h"
|
#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) {
|
DistanceSensor* Perception::GetSensor(float angle) {
|
||||||
angle = Angle::Normalize(angle);
|
angle = Angle::Normalize(angle);
|
||||||
|
|||||||
76
Perception.h
76
Perception.h
@ -1,13 +1,81 @@
|
|||||||
#include "Sensing.h"
|
#pragma once
|
||||||
|
|
||||||
|
#include "Placement.h"
|
||||||
|
#include "Sensor.h"
|
||||||
|
|
||||||
namespace Passer {
|
namespace Passer {
|
||||||
namespace RoboidControl {
|
namespace RoboidControl {
|
||||||
|
|
||||||
// using Perception = Sensing;
|
class Perception {
|
||||||
|
|
||||||
class Perception : public Sensing {
|
|
||||||
public:
|
public:
|
||||||
|
/// @brief Setup perception
|
||||||
|
Perception();
|
||||||
|
|
||||||
|
// 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);
|
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 RoboidControl
|
||||||
|
|||||||
213
Sensing.cpp
213
Sensing.cpp
@ -1,213 +0,0 @@
|
|||||||
#include "Sensing.h"
|
|
||||||
#include "DistanceSensor.h"
|
|
||||||
#include "Switch.h"
|
|
||||||
|
|
||||||
#include <math.h>
|
|
||||||
|
|
||||||
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;
|
|
||||||
}
|
|
||||||
99
Sensing.h
99
Sensing.h
@ -1,99 +0,0 @@
|
|||||||
#pragma once
|
|
||||||
|
|
||||||
#include "Placement.h"
|
|
||||||
#include "Sensor.h"
|
|
||||||
|
|
||||||
// #include <list>
|
|
||||||
|
|
||||||
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;
|
|
||||||
Loading…
x
Reference in New Issue
Block a user