RoboidControl/RoboidControl-cpp
RoboidControl/RoboidControl-cpp
1
0
Fork 0
Code Issues 1 Pull Requests Actions Packages Releases 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

100
Perception.cpp
View File

@ -12,9 +12,7 @@ Perception::Perception(Placement* sensors, unsigned int sensorCount) {
this->sensorPlacements = (Placement *)sensors;
}
unsigned int Perception::GetSensorCount() {
return this->sensorCount;
}
unsigned int Perception::GetSensorCount() { return this->sensorCount; }
Sensor *Perception::GetSensor(unsigned int sensorId) {
if (sensorId >= this->sensorCount)
@ -115,16 +113,40 @@ 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) {
// 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;
@ -138,23 +160,48 @@ 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) {
// 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;
@ -168,15 +215,14 @@ 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) {
if (placement.horizontalDirection > direction - range &&
placement.horizontalDirection < direction + range) {
Thing *thing = placement.thing;
if (thing == nullptr)
continue;

53
Perception.h
View File

@ -29,21 +29,46 @@ class Perception {
unsigned int GetSensorCount();
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:
/// @brief The Placement of the Sensors used for Perception