178 lines
7.4 KiB
C++
178 lines
7.4 KiB
C++
#pragma once
|
|
|
|
#include "ControlCore/LinearAlgebra/Polar.h"
|
|
#include "ControlCore/LinearAlgebra/Quaternion.h"
|
|
#include "ControlCore/LinearAlgebra/Spherical.h"
|
|
#include "Sensor.h"
|
|
#include "TrackedObject.h"
|
|
|
|
// #include <vector.h>
|
|
|
|
namespace Passer {
|
|
namespace RoboidControl {
|
|
|
|
class Roboid;
|
|
|
|
/// @brief Module to which keeps track of objects around the roboid
|
|
class Perception {
|
|
public:
|
|
/// @brief Default Constructor
|
|
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(Sensor **sensors, unsigned int sensorCount);
|
|
|
|
/// @brief The roboid of this perception system
|
|
Roboid *roboid = nullptr;
|
|
|
|
unsigned int AddSensor(Sensor *sensor);
|
|
|
|
/// @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 Find the first sensor of the given type
|
|
/// @param sensorType The type of sensor as is defined in the Thing class (for
|
|
/// example Thing::SensorType)
|
|
/// @return The first sensor found or a nullptr which no sensor has been found
|
|
/// of the given type
|
|
Sensor *FindSensorOfType(unsigned int sensorType);
|
|
|
|
/// @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);
|
|
float GetDistanceOfType(unsigned char thingType, float horizontalAngle,
|
|
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
|
|
/// @details 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
|
|
/// @details 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
|
|
/// @details 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);
|
|
|
|
/// @brief Add or update an object detected by the given sensor
|
|
/// @param sensor The sensor which has detected the object
|
|
/// @param position The position of the sensor in polar coordinates local to
|
|
/// the roboid
|
|
// void AddTrackedObject(Sensor* sensor,
|
|
// Spherical16 position,
|
|
// unsigned char objectType = 0x00,
|
|
// unsigned char networkId = 0x00);
|
|
InterestingThing *
|
|
AddTrackedObject(Sensor *sensor, Spherical16 position,
|
|
SwingTwist16 orientation = SwingTwist16::identity,
|
|
unsigned char objectType = 0xFF,
|
|
unsigned char objectId = 0x00,
|
|
unsigned char networkId = 0x00);
|
|
|
|
InterestingThing *
|
|
AddTrackedObject(Sensor *sensor, unsigned char networkId,
|
|
unsigned char objectId, Spherical16 position,
|
|
SwingTwist16 orientation = SwingTwist16::identity);
|
|
InterestingThing *AddTrackedObject(Sensor *sensor, Thing *thing);
|
|
|
|
bool IsInteresting(float distance);
|
|
|
|
InterestingThing *FindTrackedObject(char objectId);
|
|
InterestingThing *FindTrackedObject(unsigned char networkId,
|
|
unsigned char objectId);
|
|
|
|
/// @brief Retrieve the number of objects currently being tracked by the
|
|
/// roboid
|
|
/// @return The object of objects, which is always lower than
|
|
/// maxObjectCount
|
|
unsigned char TrackedObjectCount();
|
|
/// @brief Retreive the objects currently tracked by the roboid
|
|
/// @return An array of current objects
|
|
/// @details The returned array this should never be a nullptr, but
|
|
/// each array entry may be a nullptr when less than maxObjectCount objects is
|
|
/// currently being tracked.
|
|
InterestingThing **GetTrackedObjects();
|
|
|
|
unsigned char ThingsOfType(unsigned char objectType,
|
|
InterestingThing *buffer[],
|
|
unsigned char bufferSize);
|
|
InterestingThing *ThingOfType(unsigned char objectType);
|
|
|
|
InterestingThing *GetMostInterestingThing();
|
|
|
|
// mainly used for confidence update
|
|
|
|
/// @brief Update the state of the perception.
|
|
/// @param currentTimeMs The current time in milliseconds
|
|
/// @details This will update the perceptoin of object. It will retrieve the
|
|
/// latest state for each sensor and update the confidence of the tracked
|
|
/// objects.
|
|
void Update(unsigned long currentTimeMs);
|
|
|
|
/// @brief Update the position/orientation of the preceived objects from the
|
|
/// given roboid translation
|
|
/// @param translation The translation of the roboid in world space in polar
|
|
/// coordinates
|
|
/// @details This function will be called through Roboid::SetPosition. It
|
|
/// is advised to use that function to update the roboid position instead of
|
|
/// this function.
|
|
void UpdatePose(Polar16 translation);
|
|
/// @brief Update the orientation of the perceived objecst from the given
|
|
/// roboid rotation
|
|
/// @param rotation The rotation of the roboid in world space
|
|
void UpdatePose(SwingTwist16 rotation);
|
|
|
|
/// @brief Objects with a distance closed that this value will be considered
|
|
/// nearby.
|
|
/// @details This value is used by the ObjectNearby function to select the
|
|
/// objects
|
|
float nearbyDistance = 0.02F;
|
|
|
|
public:
|
|
/// @brief The Sensors used for Perception
|
|
Sensor **sensors = nullptr;
|
|
/// @brief The number of Sensors used for Perception
|
|
unsigned int sensorCount = 0;
|
|
|
|
unsigned long lastUpdateTimeMs = 0;
|
|
unsigned char lastObjectId = 1;
|
|
|
|
static unsigned char maxObjectCount; // = 7; // 7 is typically the maximum
|
|
// number of object which can
|
|
// be tracked by a human
|
|
InterestingThing **trackedObjects;
|
|
};
|
|
|
|
} // namespace RoboidControl
|
|
} // namespace Passer
|
|
using namespace Passer::RoboidControl;
|