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

Make GetDistance using direction/range

Browse Source
This commit is contained in:
Pascal Serrarens 2023-12-07 10:02:08 +01:00
parent 2d5b3998d7
commit 47dbbc5b61
2 changed files with 131 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

124
Perception.cpp
View File

@ -7,22 +7,20 @@
Perception::Perception() {}
Perception::Perception(Placement* sensors, unsigned int sensorCount) {
Perception::Perception(Placement *sensors, unsigned int sensorCount) {
this->sensorCount = sensorCount;
this->sensorPlacements = (Placement*)sensors;
this->sensorPlacements = (Placement *)sensors;
}
unsigned int Perception::GetSensorCount() {
return this->sensorCount;
}
unsigned int Perception::GetSensorCount() { return this->sensorCount; }
Sensor* Perception::GetSensor(unsigned int sensorId) {
Sensor *Perception::GetSensor(unsigned int sensorId) {
if (sensorId >= this->sensorCount)
return nullptr;
Thing* thing = this->sensorPlacements[sensorId].thing;
Thing *thing = this->sensorPlacements[sensorId].thing;
if (thing->IsSensor())
return (Sensor*)thing;
return (Sensor *)thing;
return nullptr;
}
@ -115,22 +113,46 @@ float Perception::DistanceDown(float angle) {
}
*/
float Perception::GetDistance(float fromAngle, float toAngle) {
// 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->ObjectNearby())
// minDistance = fmin(minDistance, distanceSensor->GetDistance());
// }
// }
// }
// return minDistance;
// }
float Perception::GetDistance(float direction, float range) {
float minDistance = INFINITY;
if (toAngle < fromAngle)
// Hmm. Can't look backward properly for now
return minDistance;
if (range < 0)
range = -range;
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;
// This still needs support for angles wrapping around 180 degrees !!!!
if (placement.horizontalDirection > direction - range &&
placement.horizontalDirection < direction + range) {
Thing *thing = placement.thing;
if (thing == nullptr)
continue;
if (thing->type == Thing::DistanceSensorType) {
DistanceSensor* distanceSensor = (DistanceSensor*)thing;
DistanceSensor *distanceSensor = (DistanceSensor *)thing;
if (distanceSensor != nullptr && distanceSensor->ObjectNearby())
minDistance = fmin(minDistance, distanceSensor->GetDistance());
}
@ -138,29 +160,54 @@ float Perception::GetDistance(float fromAngle, float toAngle) {
}
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;
float Perception::GetDistance(float fromHorizontalAngle,
float toHorizontalAngle,
float fromVerticalAngle,
float toVerticalAngle) {
// 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->ObjectNearby())
// minDistance = fmin(minDistance, distanceSensor->GetDistance());
// }
// }
// }
// return minDistance;
// }
float Perception::GetDistance(float horizontalDirection,
float verticalDirection, float range) {
float minDistance = INFINITY;
if (toHorizontalAngle < fromHorizontalAngle ||
toVerticalAngle < fromVerticalAngle)
// Hmm. Can't look backward properly for now
return minDistance;
if (range < 0)
range = -range;
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;
// This still needs support for angles wrapping around 180 degrees !!!!
if (placement.horizontalDirection > horizontalDirection - range &&
placement.horizontalDirection < horizontalDirection + range &&
placement.verticalDirection > verticalDirection - range &&
placement.verticalDirection < verticalDirection + range) {
Thing *thing = placement.thing;
if (thing == nullptr)
continue;
if (thing->type == Thing::DistanceSensorType) {
DistanceSensor* distanceSensor = (DistanceSensor*)thing;
DistanceSensor *distanceSensor = (DistanceSensor *)thing;
if (distanceSensor != nullptr && distanceSensor->ObjectNearby())
minDistance = fmin(minDistance, distanceSensor->GetDistance());
}
@ -168,25 +215,24 @@ float Perception::GetDistance(float fromHorizontalAngle,
}
return minDistance;
}
bool Perception::ObjectNearby(float fromAngle, float toAngle) {
if (toAngle < fromAngle)
return false;
bool Perception::ObjectNearby(float direction, float range) {
if (range < 0)
range = -range;
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 (placement.horizontalDirection > direction - range &&
placement.horizontalDirection < direction + range) {
Thing *thing = placement.thing;
if (thing == nullptr)
continue;
if (thing->type == Thing::DistanceSensorType) {
DistanceSensor* distanceSensor = (DistanceSensor*)thing;
DistanceSensor *distanceSensor = (DistanceSensor *)thing;
if (distanceSensor != nullptr && distanceSensor->ObjectNearby())
return true;
} else if (thing->type == Thing::SwitchType) {
Switch* switchSensor = (Switch*)thing;
Switch *switchSensor = (Switch *)thing;
if (switchSensor != nullptr && switchSensor->IsOn())
return true;
}

67
Perception.h
View File

@ -8,7 +8,7 @@ namespace RoboidControl {
/// @brief Module to which keeps track of objects around the roboid
class Perception {
public:
public:
/// @brief Default Constructor
Perception();
@ -22,36 +22,61 @@ class Perception {
/// @brief Create a perception setup with the given Sensors
/// @param sensors The Placement of Sensors on the Roboid
/// @param sensorCount The number of sensors in the placement array
Perception(Placement* sensors, unsigned int sensorCount);
Perception(Placement *sensors, unsigned int sensorCount);
/// @brief Get the number of Sensors
/// @return The number of sensors, zero when no sensors are present
unsigned int GetSensorCount();
Sensor* GetSensor(unsigned int sensorId);
Sensor *GetSensor(unsigned int sensorId);
float GetDistance(float fromAngle, float toAngle);
float GetDistance(float fromHorizontalAngle,
float toHorizontalAngle,
float fromVerticalAngle,
float toVerticalAngle);
// float GetDistance(float angle);
/// @brief Gets the distance to the closest object
/// @param direction The direction to look for objects
/// @param range The range in which objects should be looked for
/// @return The distance to the closest object in meters
float GetDistance(float direction, float range = 10.0F);
/// @brief Gets the distance to the closest object
/// @param horizontalDirection The direction in the horizontal plane to look
/// for objects
/// @param verticalDirection The direction in the vertical plane to look for
/// objects
/// @param range The range in which objects should be looked for
/// @return The distance to the closest object in meters
/// The directions can be thought of as the polar angle (vertical) and
/// azimuthal angle (horizontal) in the spherical coordinate system.
float GetDistance(float horizontalDirection, float verticalDirection,
float range = 10.0F);
/// @brief Checks if an object is close within the give range in the
/// horizontal plane
/// @param fromAngle Start angle in the horizontal plane
/// @param toAngle End angle in the horizontal plane
/// @return True is an object is closeby
/// @note Whether an object is closeby depends on the Distance Sensor
/// @remark This function is likely to change in the near future
bool ObjectNearby(float fromAngle, float toAngle);
/// @brief Checks if an object is nearby
/// @param direction The direction to look for objects
/// @param range The range in which objects should be looked for
/// @return True when an object is close, False otherwise
/// Wether an object is closeby depends on the sensor. This can be a sensor
/// like a Switch or a DistanceSensor. The latter uses the
/// DistanceSensor::triggerDistance to check if an object is nearby.
bool ObjectNearby(float direction, float range = 10.0F);
/// @brief Checks if an object is nearby
/// @param horizontalDirection The direction in the horizontal plane to look
/// for objects
/// @param verticalDirection The direction in the vertical plane to look for
/// objects
/// @param range The range in which objects should be looked for
/// @return True when an object is close, False otherwise
/// Wether an object is closeby depends on the sensor. This can be a sensor
/// like a Switch or a DistanceSensor. The latter uses the
/// DistanceSensor::triggerDistance to check if an object is nearby.
///
/// The directions can be thought of as the polar angle (vertical) and
/// azimuthal angle (horizontal) in the spherical coordinate system.
bool ObjectNearby(float horizontalDirection, float verticalDirection,
float range = 10.0F);
protected:
protected:
/// @brief The Placement of the Sensors used for Perception
Placement* sensorPlacements = nullptr;
Placement *sensorPlacements = nullptr;
/// @brief The number of Sensors used for Perception
unsigned int sensorCount = 0;
};
} // namespace RoboidControl
} // namespace Passer
} // namespace RoboidControl
} // namespace Passer
using namespace Passer::RoboidControl;