RoboidControl/RoboidControl-cpp
RoboidControl/RoboidControl-cpp
1
0
Fork 0
Code Issues 1 Pull Requests Actions Packages Releases Activity

Simplified perception

Browse Source
This commit is contained in:
Pascal Serrarens 2023-12-06 12:46:08 +01:00
parent 733a09ce91
commit b527aeb97b
2 changed files with 119 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

96
Perception.cpp
View File

@ -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) {
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.horizontalDirection;
if (placementAngle == angle) {
DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
return distanceSensor->GetDistance();
}
}
}
return INFINITY;
}
// 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.horizontalDirection;
// if (placementAngle == angle) {
// DistanceSensor* distanceSensor =
// (DistanceSensor*)placement.thing; return
// distanceSensor->GetDistance();
// }
// }
// }
// return INFINITY;
// }
void Perception::SetResolution(unsigned int resolution) {
this->resolution = resolution;

15
Perception.h
View File

@ -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 angle);
*/
float GetDistance(float fromAngle, float toAngle);
float GetDistance(float fromHorizontalAngle,
float toHorizontalAngle,
float fromVerticalAngle,
float toVerticalAngle);
// float GetDistance(float angle);
bool SwitchOn(float fromAngle, float toAngle);