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

63
Perception.h
View File

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