123 lines
4.5 KiB
C++
123 lines
4.5 KiB
C++
#pragma once
|
|
|
|
#include "Placement.h"
|
|
#include "Polar.h"
|
|
#include "Sensor.h"
|
|
|
|
namespace Passer {
|
|
namespace RoboidControl {
|
|
|
|
class PerceivedObject {
|
|
public:
|
|
PerceivedObject();
|
|
PerceivedObject(Polar position, float radius = 0.1F);
|
|
|
|
static constexpr float equalityDistance = 0.3F;
|
|
static constexpr float equalityAngle = 5.0F;
|
|
bool IsTheSameAs(PerceivedObject *otherObj);
|
|
|
|
char id;
|
|
|
|
Polar position;
|
|
float radius;
|
|
|
|
static constexpr char maxConfidence = 255;
|
|
static constexpr char confidenceDropSpeed = 2;
|
|
char confidence;
|
|
bool DegradeConfidence(float deltaTime);
|
|
void Refresh(Polar position, float radius);
|
|
};
|
|
|
|
/// @brief Module to which keeps track of objects around the roboid
|
|
class Perception {
|
|
public:
|
|
/// @brief Default Constructor
|
|
Perception();
|
|
|
|
/// @brief Template to make it possible to leave out ths sensorCount
|
|
/// @tparam sensorCount
|
|
/// @param sensors An array of sensor placements
|
|
template <unsigned int sensorCount>
|
|
inline Perception(Placement (&sensors)[sensorCount]) {
|
|
Perception(sensors, sensorCount);
|
|
}
|
|
/// @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);
|
|
|
|
/// @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);
|
|
|
|
/// @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 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);
|
|
|
|
// Object Perception
|
|
|
|
void AddPerceivedObject(Polar position);
|
|
void AddPerceivedObject(PerceivedObject *obj);
|
|
unsigned char PerceivedObjectCount();
|
|
PerceivedObject **GetPerceivedObjects();
|
|
|
|
// mainly used for confidence update
|
|
void Update(float currentTimeMs);
|
|
|
|
protected:
|
|
/// @brief The Placement of the Sensors used for Perception
|
|
Placement *sensorPlacements = nullptr;
|
|
/// @brief The number of Sensors used for Perception
|
|
unsigned int sensorCount = 0;
|
|
|
|
float lastUpdateTimeMs = 0;
|
|
|
|
static const unsigned char maxObjectCount = 7;
|
|
PerceivedObject
|
|
*perceivedObjects[maxObjectCount]; // 7 is typically the maximum number of
|
|
// object which can be tracked by a
|
|
// human
|
|
};
|
|
|
|
} // namespace RoboidControl
|
|
} // namespace Passer
|
|
using namespace Passer::RoboidControl;
|