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

Spherical16 support

Browse Source
This commit is contained in:
Pascal Serrarens 2024-08-28 09:42:51 +02:00
parent 28340b1620
commit 08eec48044
7 changed files with 81 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
LinearAlgebra

@ -1 +1 @@
Subproject commit e10e0100955f8f4dd139f52d71e37d6ace8c5fc0 Subproject commit e0ef43a69989c88e87b973ab5b45cc3bd474c1bf

6
NetworkPerception.cpp
View File

@ -121,7 +121,7 @@ void NetworkPerception::ReceiveSphere(unsigned char* data, Roboid* roboid) {
Vector3 localPosition = Vector3 localPosition =
Quaternion::Inverse(roboidOrientation) * deltaPosition; Quaternion::Inverse(roboidOrientation) * deltaPosition;
Spherical position = Spherical(localPosition); Spherical16 position = Spherical16::FromVector3(localPosition);
roboid->perception->AddTrackedObject(this, position, Quaternion::identity, roboid->perception->AddTrackedObject(this, position, Quaternion::identity,
0x81, 0x81, networkId); 0x81, 0x81, networkId);
@ -141,7 +141,7 @@ void NetworkPerception::ReceivePoseMsg(unsigned char* data, Roboid* roboid) {
return ReceiveSphere(data, roboid); return ReceiveSphere(data, roboid);
Quaternion roboidOrientation = roboid->GetOrientation(); Quaternion roboidOrientation = roboid->GetOrientation();
Spherical position = Spherical::zero; Spherical16 position = Spherical16::zero;
Quaternion orientation = Quaternion::identity; Quaternion orientation = Quaternion::identity;
if ((poseType & NetworkSync::Pose_Position) != 0) { if ((poseType & NetworkSync::Pose_Position) != 0) {
@ -159,7 +159,7 @@ void NetworkPerception::ReceivePoseMsg(unsigned char* data, Roboid* roboid) {
Vector3 localPosition = Quaternion::Inverse(roboidOrientation) * Vector3 localPosition = Quaternion::Inverse(roboidOrientation) *
(worldPosition - roboidPosition); (worldPosition - roboidPosition);
position = Spherical(localPosition); position = Spherical16::FromVector3(localPosition);
} }
} }

2
NetworkSync.cpp
View File

@ -215,7 +215,7 @@ void NetworkSync::PublishTrackedObject(Roboid* roboid,
Quaternion roboidOrientation = roboid->GetOrientation(); Quaternion roboidOrientation = roboid->GetOrientation();
// Vector3 localPosition = object->position.ToVector3(); // Vector3 localPosition = object->position.ToVector3();
Vector3 localPosition = Vector3(object->position); Vector3 localPosition = object->position.ToVector3();
Vector3 worldPosition = roboidPosition + roboidOrientation * localPosition; Vector3 worldPosition = roboidPosition + roboidOrientation * localPosition;
Quaternion worldOrientation = Quaternion worldOrientation =
roboidOrientation * object->orientation.ToQuaternion(); roboidOrientation * object->orientation.ToQuaternion();

43
Perception.cpp
View File

@ -76,7 +76,7 @@ float GetPlaneDistance(InterestingThing* plane,
float range) { float range) {
float distance = plane->position.distance; float distance = plane->position.distance;
float deltaAngle = float deltaAngle =
Angle::Normalize((float)plane->position.horizontalAngle.ToFloat() - Angle::Normalize((float)plane->position.horizontal.ToFloat() -
horizontalAngle) horizontalAngle)
.ToFloat(); .ToFloat();
if (fabsf(deltaAngle) < fabsf(range)) { if (fabsf(deltaAngle) < fabsf(range)) {
@ -122,9 +122,9 @@ float Perception::GetDistance(float horizontalDirection, float range) {
if (obj->type == 0x080) { // plane if (obj->type == 0x080) { // plane
float planeDistance = GetPlaneDistance(obj, horizontalDirection, range); float planeDistance = GetPlaneDistance(obj, horizontalDirection, range);
minDistance = fminf(minDistance, planeDistance); minDistance = fminf(minDistance, planeDistance);
} else if (obj->position.horizontalAngle.ToFloat() > } else if (obj->position.horizontal.ToFloat() >
horizontalDirection - range && horizontalDirection - range &&
obj->position.horizontalAngle.ToFloat() < obj->position.horizontal.ToFloat() <
horizontalDirection + range) { horizontalDirection + range) {
minDistance = fminf(minDistance, obj->position.distance); minDistance = fminf(minDistance, obj->position.distance);
} }
@ -149,10 +149,8 @@ float Perception::GetDistanceOfType(unsigned char thingType,
if (thing->type == 0x080) { // plane if (thing->type == 0x080) { // plane
float planeDistance = GetPlaneDistance(thing, horizontalAngle, range); float planeDistance = GetPlaneDistance(thing, horizontalAngle, range);
minDistance = fminf(minDistance, planeDistance); minDistance = fminf(minDistance, planeDistance);
} else if (thing->position.horizontalAngle.ToFloat() > } else if (thing->position.horizontal.ToFloat() > horizontalAngle - range &&
horizontalAngle - range && thing->position.horizontal.ToFloat() < horizontalAngle + range) {
thing->position.horizontalAngle.ToFloat() <
horizontalAngle + range) {
minDistance = fminf(minDistance, thing->position.distance); minDistance = fminf(minDistance, thing->position.distance);
} }
} }
@ -170,8 +168,8 @@ float Perception::GetDistance(float horizontalDirection,
InterestingThing* obj = trackedObjects[objIx]; InterestingThing* obj = trackedObjects[objIx];
if (obj == nullptr) if (obj == nullptr)
continue; continue;
if (obj->position.horizontalAngle.ToFloat() > horizontalDirection - range && if (obj->position.horizontal.ToFloat() > horizontalDirection - range &&
obj->position.horizontalAngle.ToFloat() < horizontalDirection + range) { obj->position.horizontal.ToFloat() < horizontalDirection + range) {
minDistance = fminf(minDistance, obj->position.distance); minDistance = fminf(minDistance, obj->position.distance);
} }
} }
@ -187,8 +185,8 @@ bool Perception::ObjectNearby(float direction, float range) {
if (obj == nullptr) if (obj == nullptr)
continue; continue;
if (obj->position.horizontalAngle.ToFloat() > direction - range && if (obj->position.horizontal.ToFloat() > direction - range &&
obj->position.horizontalAngle.ToFloat() < direction + range) { obj->position.horizontal.ToFloat() < direction + range) {
if (obj->position.distance <= nearbyDistance) if (obj->position.distance <= nearbyDistance)
return true; return true;
} }
@ -202,7 +200,8 @@ void Perception::AddTrackedObject(Sensor* sensor,
Polar position, Polar position,
unsigned char thingType, unsigned char thingType,
unsigned char networkId) { unsigned char networkId) {
Spherical sPos = Spherical(position); Spherical16 sPos =
Spherical16(position.distance, Angle16(position.angle.ToFloat()), 0);
Quaternion orientation = Quaternion::identity; Quaternion orientation = Quaternion::identity;
AddTrackedObject(sensor, sPos, orientation, thingType, thingType, networkId); AddTrackedObject(sensor, sPos, orientation, thingType, thingType, networkId);
/* /*
@ -282,7 +281,7 @@ void Perception::AddTrackedObject(Sensor* sensor,
} }
InterestingThing* Perception::AddTrackedObject(Sensor* sensor, InterestingThing* Perception::AddTrackedObject(Sensor* sensor,
Spherical position, Spherical16 position,
Quaternion orientation, Quaternion orientation,
unsigned char thingType, unsigned char thingType,
unsigned char thingId, unsigned char thingId,
@ -349,7 +348,7 @@ InterestingThing* Perception::AddTrackedObject(Sensor* sensor,
InterestingThing* Perception::AddTrackedObject(Sensor* sensor, InterestingThing* Perception::AddTrackedObject(Sensor* sensor,
unsigned char networkId, unsigned char networkId,
unsigned char objectId, unsigned char objectId,
Spherical position, Spherical16 position,
Quaternion orientation) { Quaternion orientation) {
InterestingThing* thing = FindTrackedObject(networkId, objectId); InterestingThing* thing = FindTrackedObject(networkId, objectId);
if (thing == nullptr) { if (thing == nullptr) {
@ -531,7 +530,7 @@ void Perception::UpdatePose(Polar translation) {
// (float)thing->position.horizontalAngle, // (float)thing->position.horizontalAngle,
// (float)thing->position.verticalAngle); // (float)thing->position.verticalAngle);
// Update the closest point to the plane // Update the closest point to the plane
float angle = (float)thing->position.horizontalAngle.ToFloat() + float angle = (float)thing->position.horizontal.ToFloat() +
translation.angle.ToFloat(); translation.angle.ToFloat();
angle = fabsf(angle); angle = fabsf(angle);
@ -544,9 +543,13 @@ void Perception::UpdatePose(Polar translation) {
// (float)thing->position.horizontalAngle, // (float)thing->position.horizontalAngle,
// (float)thing->position.verticalAngle); // (float)thing->position.verticalAngle);
} else { } else {
Polar horizontalPosition = // Polar horizontalPosition = Polar(thing->position);
Polar(thing->position); // obj->position.ProjectOnHorizontalPlane(); // // obj->position.ProjectOnHorizontalPlane();
Spherical newPosition = Spherical(horizontalPosition - translation); // Spherical16 newPosition = Spherical16(horizontalPosition -
// translation);
Spherical16 translationS = Spherical16(
translation.distance, Angle16(translation.angle.ToFloat()), 0);
Spherical16 newPosition = thing->position + translationS;
thing->position = newPosition; thing->position = newPosition;
} }
} }
@ -571,8 +574,8 @@ void Perception::UpdatePose(Quaternion rotation) {
// (float)thing->position.verticalAngle); // (float)thing->position.verticalAngle);
// printf("| rotate %f | ", rotationAngle); // printf("| rotate %f | ", rotationAngle);
thing->position.horizontalAngle = Angle::Normalize( thing->position.horizontal = Angle16(Angle16::Normalize(
thing->position.horizontalAngle.ToFloat() - rotationAngle); thing->position.horizontal.ToFloat() - rotationAngle));
// printf("-> %f (%f %f) \n", thing->position.distance, // printf("-> %f (%f %f) \n", thing->position.distance,
// (float)thing->position.horizontalAngle, // (float)thing->position.horizontalAngle,

71
Perception.h
View File

@ -15,37 +15,38 @@ class Roboid;
/// @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();
/// @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(Sensor **sensors, unsigned int sensorCount); Perception(Sensor** sensors, unsigned int sensorCount);
/// @brief The roboid of this perception system /// @brief The roboid of this perception system
Roboid *roboid = nullptr; Roboid* roboid = nullptr;
unsigned int AddSensor(Sensor *sensor); unsigned int AddSensor(Sensor* sensor);
/// @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);
/// @brief Find the first sensor of the given type /// @brief Find the first sensor of the given type
/// @param sensorType The type of sensor as is defined in the Thing class (for /// @param sensorType The type of sensor as is defined in the Thing class (for
/// example Thing::SensorType) /// example Thing::SensorType)
/// @return The first sensor found or a nullptr which no sensor has been found /// @return The first sensor found or a nullptr which no sensor has been found
/// of the given type /// of the given type
Sensor *FindSensorOfType(unsigned int sensorType); Sensor* FindSensorOfType(unsigned int sensorType);
/// @brief Gets the distance to the closest object /// @brief Gets the distance to the closest object
/// @param direction The direction to look for objects /// @param direction The direction to look for objects
/// @param range The range in which objects should be looked for /// @param range The range in which objects should be looked for
/// @return The distance to the closest object in meters /// @return The distance to the closest object in meters
float GetDistance(float direction, float range = 10.0F); float GetDistance(float direction, float range = 10.0F);
float GetDistanceOfType(unsigned char thingType, float horizontalAngle, float GetDistanceOfType(unsigned char thingType,
float horizontalAngle,
float range = 10.0F); float range = 10.0F);
/// @brief Gets the distance to the closest object /// @brief Gets the distance to the closest object
/// @param horizontalDirection The direction in the horizontal plane to look /// @param horizontalDirection The direction in the horizontal plane to look
@ -56,7 +57,8 @@ public:
/// @return The distance to the closest object in meters /// @return The distance to the closest object in meters
/// @details The directions can be thought of as the polar angle (vertical) /// @details The directions can be thought of as the polar angle (vertical)
/// and azimuthal angle (horizontal) in the spherical coordinate system. /// and azimuthal angle (horizontal) in the spherical coordinate system.
float GetDistance(float horizontalDirection, float verticalDirection, float GetDistance(float horizontalDirection,
float verticalDirection,
float range = 10.0F); float range = 10.0F);
/// @brief Checks if an object is nearby /// @brief Checks if an object is nearby
@ -87,24 +89,29 @@ public:
/// @param sensor The sensor which has detected the object /// @param sensor The sensor which has detected the object
/// @param position The position of the sensor in polar coordinates local to /// @param position The position of the sensor in polar coordinates local to
/// the roboid /// the roboid
void AddTrackedObject(Sensor *sensor, Polar position, void AddTrackedObject(Sensor* sensor,
Polar position,
unsigned char objectType = 0x00, unsigned char objectType = 0x00,
unsigned char networkId = 0x00); unsigned char networkId = 0x00);
InterestingThing * InterestingThing* AddTrackedObject(
AddTrackedObject(Sensor *sensor, Spherical position, Sensor* sensor,
Quaternion orientation = Quaternion::identity, Spherical16 position,
unsigned char objectType = 0x00, Quaternion orientation = Quaternion::identity,
unsigned char objectId = 0x00, unsigned networkId = 0x00); unsigned char objectType = 0x00,
unsigned char objectId = 0x00,
unsigned networkId = 0x00);
InterestingThing * InterestingThing* AddTrackedObject(
AddTrackedObject(Sensor *sensor, unsigned char networkId, Sensor* sensor,
unsigned char objectId, Spherical position, unsigned char networkId,
Quaternion orientation = Quaternion::identity); unsigned char objectId,
Spherical16 position,
Quaternion orientation = Quaternion::identity);
bool IsInteresting(float distance); bool IsInteresting(float distance);
InterestingThing *FindTrackedObject(char objectId); InterestingThing* FindTrackedObject(char objectId);
InterestingThing *FindTrackedObject(unsigned char networkId, InterestingThing* FindTrackedObject(unsigned char networkId,
unsigned char objectId); unsigned char objectId);
/// @brief Retrieve the number of objects currently being tracked by the /// @brief Retrieve the number of objects currently being tracked by the
@ -117,14 +124,14 @@ public:
/// @details The returned array this should never be a nullptr, but /// @details The returned array this should never be a nullptr, but
/// each array entry may be a nullptr when less than maxObjectCount objects is /// each array entry may be a nullptr when less than maxObjectCount objects is
/// currently being tracked. /// currently being tracked.
InterestingThing **GetTrackedObjects(); InterestingThing** GetTrackedObjects();
unsigned char ThingsOfType(unsigned char objectType, unsigned char ThingsOfType(unsigned char objectType,
InterestingThing *buffer[], InterestingThing* buffer[],
unsigned char bufferSize); unsigned char bufferSize);
InterestingThing *ThingOfType(unsigned char objectType); InterestingThing* ThingOfType(unsigned char objectType);
InterestingThing *GetMostInterestingThing(); InterestingThing* GetMostInterestingThing();
// mainly used for confidence update // mainly used for confidence update
@ -154,21 +161,21 @@ public:
/// objects /// objects
float nearbyDistance = 0.02F; float nearbyDistance = 0.02F;
public: public:
/// @brief The Sensors used for Perception /// @brief The Sensors used for Perception
Sensor **sensors = nullptr; Sensor** sensors = 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;
unsigned long lastUpdateTimeMs = 0; unsigned long lastUpdateTimeMs = 0;
unsigned char lastObjectId = 1; unsigned char lastObjectId = 1;
static unsigned char maxObjectCount; // = 7; // 7 is typically the maximum static unsigned char maxObjectCount; // = 7; // 7 is typically the maximum
// number of object which can // number of object which can
// be tracked by a human // be tracked by a human
InterestingThing **trackedObjects; InterestingThing** trackedObjects;
}; };
} // namespace RoboidControl } // namespace RoboidControl
} // namespace Passer } // namespace Passer
using namespace Passer::RoboidControl; using namespace Passer::RoboidControl;

18
TrackedObject.cpp
View File

@ -6,12 +6,13 @@ InterestingThing::InterestingThing(Sensor* sensor, Polar position) {
this->id = 0; this->id = 0;
this->confidence = maxConfidence; this->confidence = maxConfidence;
this->sensor = sensor; this->sensor = sensor;
this->position = Spherical(position); this->position =
Spherical16(position.distance, Angle16(position.angle.ToFloat()), 0);
this->updated = true; this->updated = true;
} }
InterestingThing::InterestingThing(Sensor* sensor, InterestingThing::InterestingThing(Sensor* sensor,
Spherical position, Spherical16 position,
Quaternion orientation) { Quaternion orientation) {
this->id = 0; this->id = 0;
this->confidence = maxConfidence; this->confidence = maxConfidence;
@ -44,11 +45,11 @@ bool InterestingThing::IsTheSameAs(InterestingThing* otherObj) {
// (float)position.horizontalAngle, (float)position.verticalAngle); // (float)position.horizontalAngle, (float)position.verticalAngle);
if (fabsf(position.distance - otherObj->position.distance) > equalDistance) if (fabsf(position.distance - otherObj->position.distance) > equalDistance)
return false; return false;
if (fabsf(position.horizontalAngle.ToFloat() - if (fabsf(position.horizontal.ToFloat() -
otherObj->position.horizontalAngle.ToFloat()) > equalAngle) otherObj->position.horizontal.ToFloat()) > equalAngle)
return false; return false;
if (fabsf(position.verticalAngle.ToFloat() - if (fabsf(position.vertical.ToFloat() -
otherObj->position.verticalAngle.ToFloat()) > equalAngle) otherObj->position.vertical.ToFloat()) > equalAngle)
return false; return false;
// printf(" -> yes "); // printf(" -> yes ");
return true; return true;
@ -72,12 +73,13 @@ bool InterestingThing::DegradeConfidence(float deltaTime) {
} }
void InterestingThing::Refresh(Polar position) { void InterestingThing::Refresh(Polar position) {
this->position = Spherical(position); this->position =
Spherical16(position.distance, Angle16(position.angle.ToFloat()), 0);
this->confidence = maxConfidence; this->confidence = maxConfidence;
this->updated = true; this->updated = true;
} }
void InterestingThing::Refresh(Spherical position, Quaternion orientation) { void InterestingThing::Refresh(Spherical16 position, Quaternion orientation) {
this->position = position; this->position = position;
float angle; float angle;

7
TrackedObject.h
View File

@ -1,5 +1,6 @@
#pragma once #pragma once
#include "LinearAlgebra/Angle16.h"
#include "LinearAlgebra/AngleAxis.h" #include "LinearAlgebra/AngleAxis.h"
#include "LinearAlgebra/Polar.h" #include "LinearAlgebra/Polar.h"
#include "LinearAlgebra/Quaternion.h" #include "LinearAlgebra/Quaternion.h"
@ -17,7 +18,7 @@ class InterestingThing {
/// @param position The position in polar coordinates local to the roboid /// @param position The position in polar coordinates local to the roboid
InterestingThing(Sensor* sensor, Polar position); InterestingThing(Sensor* sensor, Polar position);
InterestingThing(Sensor* sensor, InterestingThing(Sensor* sensor,
Spherical position, Spherical16 position,
Quaternion orientation = Quaternion::identity); Quaternion orientation = Quaternion::identity);
/// @brief Update the position of the object /// @brief Update the position of the object
@ -25,7 +26,7 @@ class InterestingThing {
/// @details This will also update the confidence of the object to the /// @details This will also update the confidence of the object to the
/// maxConfidence value /// maxConfidence value
void Refresh(Polar position); void Refresh(Polar position);
void Refresh(Spherical position, void Refresh(Spherical16 position,
Quaternion orientation = Quaternion::identity); Quaternion orientation = Quaternion::identity);
/// @brief Decrease the confidence based on the elapsed time /// @brief Decrease the confidence based on the elapsed time
@ -66,7 +67,7 @@ class InterestingThing {
char parentId = 0; char parentId = 0;
/// @brief The current position of the object /// @brief The current position of the object
Spherical position = Spherical::zero; Spherical16 position = Spherical16::zero;
/// @brief The current orientation of the object /// @brief The current orientation of the object
AngleAxis<float> orientation = AngleAxis<float>(); AngleAxis<float> orientation = AngleAxis<float>();
// Quaternion orientation = Quaternion::identity; // Quaternion orientation = Quaternion::identity;