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

43
Perception.cpp
View File

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

55
Perception.h
View File

@ -15,37 +15,38 @@ class Roboid;
/// @brief Module to which keeps track of objects around the roboid
class Perception {
public:
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);
Perception(Sensor** sensors, unsigned int sensorCount);
/// @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
/// @return The number of sensors, zero when no sensors are present
unsigned int GetSensorCount();
Sensor *GetSensor(unsigned int sensorId);
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);
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 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
@ -56,7 +57,8 @@ public:
/// @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 GetDistance(float horizontalDirection,
float verticalDirection,
float range = 10.0F);
/// @brief Checks if an object is nearby
@ -87,24 +89,29 @@ public:
/// @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, Polar position,
void AddTrackedObject(Sensor* sensor,
Polar position,
unsigned char objectType = 0x00,
unsigned char networkId = 0x00);
InterestingThing *
AddTrackedObject(Sensor *sensor, Spherical position,
InterestingThing* AddTrackedObject(
Sensor* sensor,
Spherical16 position,
Quaternion orientation = Quaternion::identity,
unsigned char objectType = 0x00,
unsigned char objectId = 0x00, unsigned networkId = 0x00);
unsigned char objectId = 0x00,
unsigned networkId = 0x00);
InterestingThing *
AddTrackedObject(Sensor *sensor, unsigned char networkId,
unsigned char objectId, Spherical position,
InterestingThing* AddTrackedObject(
Sensor* sensor,
unsigned char networkId,
unsigned char objectId,
Spherical16 position,
Quaternion orientation = Quaternion::identity);
bool IsInteresting(float distance);
InterestingThing *FindTrackedObject(char objectId);
InterestingThing *FindTrackedObject(unsigned char networkId,
InterestingThing* FindTrackedObject(char objectId);
InterestingThing* FindTrackedObject(unsigned char networkId,
unsigned char objectId);
/// @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
/// each array entry may be a nullptr when less than maxObjectCount objects is
/// currently being tracked.
InterestingThing **GetTrackedObjects();
InterestingThing** GetTrackedObjects();
unsigned char ThingsOfType(unsigned char objectType,
InterestingThing *buffer[],
InterestingThing* buffer[],
unsigned char bufferSize);
InterestingThing *ThingOfType(unsigned char objectType);
InterestingThing* ThingOfType(unsigned char objectType);
InterestingThing *GetMostInterestingThing();
InterestingThing* GetMostInterestingThing();
// mainly used for confidence update
@ -154,9 +161,9 @@ public:
/// objects
float nearbyDistance = 0.02F;
public:
public:
/// @brief The Sensors used for Perception
Sensor **sensors = nullptr;
Sensor** sensors = nullptr;
/// @brief The number of Sensors used for Perception
unsigned int sensorCount = 0;
@ -166,7 +173,7 @@ public:
static unsigned char maxObjectCount; // = 7; // 7 is typically the maximum
// number of object which can
// be tracked by a human
InterestingThing **trackedObjects;
InterestingThing** trackedObjects;
};
} // namespace RoboidControl

18
TrackedObject.cpp
View File

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

7
TrackedObject.h
View File

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