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

Plane update improvement

Browse Source
This commit is contained in:
Pascal Serrarens 2024-05-10 09:38:12 +02:00
parent 0be698689d
commit a41a2b2c04
7 changed files with 147 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
NetworkPerception.cpp
View File

@ -45,9 +45,30 @@ void NetworkPerception::ReceivePlane(unsigned char *data, Roboid *roboid) {
return;
Vector3 worldPosition = ReceiveVector3(data, 4);
Vector3 worldAngles = ReceiveVector3(data, 16);
Quaternion worldOrientation = Quaternion::Euler(worldAngles);
Vector3 roboidPosition = roboid->GetPosition();
Vector3 deltaPosition = worldPosition - roboidPosition;
float distance = deltaPosition.magnitude();
if (roboid->perception->IsInteresting(distance) == false) {
printf(" plane not interesting\n");
return;
}
Quaternion roboidOrientation = roboid->GetOrientation();
Vector3 localPosition =
Quaternion::Inverse(roboidOrientation) * deltaPosition;
// float angle =
// Vector3::SignedAngle(Vector3::forward, localPosition, Vector3::up);
// Polar position = Polar(angle, distance);
Spherical position = Spherical(localPosition);
// printf("Received plane (%f %f %f) (%f %f %f) %f %f %f\n", worldPosition.x,
// worldPosition.y, worldPosition.z, roboidPosition.x,
// roboidPosition.y, roboidPosition.z, position.distance,
// (float)position.horizontalAngle, (float)position.verticalAngle);
printf("Received plane %f (%f %f)\n", position.distance,
(float)position.horizontalAngle, (float)position.verticalAngle);
roboid->perception->AddTrackedObject(this, position, 0x80);
}
@ -58,7 +79,7 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
// printf("My network id == %d\n", roboid->networkSync->networkId);
if (networkId == roboid->networkSync->networkId)
networkId = 0x00;
printf("Received pose message [%d/%d]\n", networkId, objectId);
// printf("Received pose message [%d/%d]\n", networkId, objectId);
if (objectId == 0x80)
return ReceivePlane(data, roboid);

2
NetworkSync.cpp
View File

@ -9,6 +9,8 @@
#include "LinearAlgebra/Angle8.h"
#include "LinearAlgebra/Spherical.h"
NetworkSync::NetworkSync() { buffer = new unsigned char[32]; }
void NetworkSync::ReceiveMessage(Roboid *roboid, unsigned char bytecount) {
networkPerception->ProcessPacket(roboid, buffer, bytecount);

10
NetworkSync.h
View File

@ -11,6 +11,8 @@ namespace RoboidControl {
/// @brief Interface for synchronizaing state between clients across a network
class NetworkSync {
public:
NetworkSync();
unsigned char networkId;
/// @brief Retreive and send the roboid state
@ -19,7 +21,7 @@ public:
/// @brief Inform that the given object is no longer being tracked
/// @param obj
virtual void DestroyObject(InterestingThing *obj) = 0;
virtual void NewObject(InterestingThing *obj){};
virtual void NewObject(InterestingThing *obj) {};
/// @brief The id of a Pose message
static const unsigned char PoseMsg = 0x10;
static const unsigned char PoseTypeMsg = 0x11;
@ -61,8 +63,8 @@ public:
void PublishTrackedObjects(Buffer sendBuffer, InterestingThing **objects);
virtual void SendPosition(Vector3 worldPosition){};
virtual void SendPose(Vector3 worldPosition, Quaternion worldOrientation){};
virtual void SendPosition(Vector3 worldPosition) {};
virtual void SendPose(Vector3 worldPosition, Quaternion worldOrientation) {};
protected:
NetworkPerception *networkPerception;
@ -90,7 +92,7 @@ protected:
void SendQuat32(unsigned char *data, unsigned char *startIndex,
const Quaternion q);
unsigned char buffer[32];
unsigned char *buffer; // =[32];
virtual void SendBuffer(unsigned char bufferSize);
void PublishClient();

124
Perception.cpp
View File

@ -69,6 +69,37 @@ Sensor *Perception::FindSensorOfType(unsigned int sensorType) {
return nullptr;
}
float GetPlaneDistance(InterestingThing *plane, float horizontalAngle,
float range) {
float distance = plane->position.distance;
float deltaAngle = Angle::Normalize((float)plane->position.horizontalAngle -
horizontalAngle);
if (abs(deltaAngle) < abs(range)) {
// distance = distance
printf(" plane distance = %f (%f-%f)+%f=%f", distance,
(float)plane->position.horizontalAngle, horizontalAngle, range,
deltaAngle);
} else if (deltaAngle < -range) {
float angle = deltaAngle + range;
printf(" plane distance < %f (%f-%f)+%f=%f %f", distance,
(float)plane->position.horizontalAngle, horizontalAngle, range,
angle, cosf(angle * Angle::Deg2Rad));
if (angle > -90)
distance = distance / cosf(angle * Angle::Deg2Rad);
} else if (deltaAngle > range) {
float angle = deltaAngle - range;
printf(" plane distance > %f (%f-%f)-%f=%f %f", distance,
(float)plane->position.horizontalAngle, horizontalAngle, range,
angle, cosf(angle * Angle::Deg2Rad));
if (angle < 90)
distance = distance / cosf(angle * Angle::Deg2Rad);
}
printf(" distance = %f\n", distance);
return distance;
}
float Perception::GetDistance(float horizontalDirection, float range) {
float minDistance = INFINITY;
if (range < 0)
@ -78,8 +109,12 @@ float Perception::GetDistance(float horizontalDirection, float range) {
InterestingThing *obj = trackedObjects[objIx];
if (obj == nullptr)
continue;
if (obj->position.horizontalAngle > horizontalDirection - range &&
obj->position.horizontalAngle < horizontalDirection + range) {
if (obj->type == 0x080) { // plane
float planeDistance = GetPlaneDistance(obj, horizontalDirection, range);
minDistance = fminf(minDistance, planeDistance);
} else if (obj->position.horizontalAngle > horizontalDirection - range &&
obj->position.horizontalAngle < horizontalDirection + range) {
minDistance = fminf(minDistance, obj->position.distance);
}
@ -87,6 +122,31 @@ float Perception::GetDistance(float horizontalDirection, float range) {
return minDistance;
}
float Perception::GetDistanceOfType(unsigned char thingType,
float horizontalAngle, float range) {
float minDistance = INFINITY;
if (range < 0)
range = -range;
for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) {
InterestingThing *thing = trackedObjects[thingIx];
if (thing == nullptr)
continue;
if (thing->type != thingType)
continue;
if (thing->type == 0x080) { // plane
float planeDistance = GetPlaneDistance(thing, horizontalAngle, range);
minDistance = fminf(minDistance, planeDistance);
} else if (thing->position.horizontalAngle > horizontalAngle - range &&
thing->position.horizontalAngle < horizontalAngle + range) {
minDistance = fminf(minDistance, thing->position.distance);
}
}
return minDistance;
}
float Perception::GetDistance(float horizontalDirection,
float verticalDirection, float range) {
float minDistance = INFINITY;
@ -428,32 +488,50 @@ void Perception::Update(float currentTimeMs) {
}
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
InterestingThing *obj = trackedObjects[objIx];
if (obj == nullptr)
InterestingThing *thing = trackedObjects[objIx];
if (thing == nullptr)
continue;
if (obj->DegradeConfidence(deltaTime) == false) {
if (thing->DegradeConfidence(deltaTime) == false) {
// delete obj
if (roboid != nullptr && roboid->networkSync != nullptr) {
roboid->networkSync->DestroyObject(obj);
roboid->networkSync->DestroyObject(thing);
}
this->trackedObjects[objIx] = nullptr;
delete obj;
delete thing;
}
}
}
void Perception::UpdatePose(Polar translation) {
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
InterestingThing *obj = trackedObjects[objIx];
if (obj == nullptr)
for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) {
InterestingThing *thing = trackedObjects[thingIx];
if (thing == nullptr)
continue;
// We only support translations on polars at this moment...
// We only support translations in the horizontal plane at this moment...
// This needs Spherical operator- to be implemented to work in 3d space
Polar horizontalPosition = obj->position.ProjectOnHorizontalPlane();
Spherical newPosition = Spherical(horizontalPosition - translation);
obj->position = newPosition;
if (thing->type == 0x80) { // plane
printf("[1/%d] %f (%f %f) ", thing->id, thing->position.distance,
(float)thing->position.horizontalAngle,
(float)thing->position.verticalAngle);
// Update the closest point to the plane
float angle = (float)thing->position.horizontalAngle + translation.angle;
angle = abs(angle);
float deltaDistance = translation.distance * cosf(angle * Angle::Deg2Rad);
printf(" | translate %f %f %f | ", (float)translation.distance,
(float)angle, deltaDistance);
thing->position.distance -= deltaDistance;
printf("-> %f (%f %f)\n", thing->position.distance,
(float)thing->position.horizontalAngle,
(float)thing->position.verticalAngle);
} else {
Polar horizontalPosition =
Polar(thing->position); // obj->position.ProjectOnHorizontalPlane();
Spherical newPosition = Spherical(horizontalPosition - translation);
thing->position = newPosition;
}
}
}
@ -466,13 +544,21 @@ void Perception::UpdatePose(Quaternion rotation) {
if (rotationAxis.y < 0)
rotationAngle = -rotationAngle;
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
InterestingThing *obj = trackedObjects[objIx];
if (obj == nullptr)
for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) {
InterestingThing *thing = trackedObjects[thingIx];
if (thing == nullptr)
continue;
printf("[1/%d] %f (%f %f) ", thing->id, thing->position.distance,
(float)thing->position.horizontalAngle,
(float)thing->position.verticalAngle);
printf("| rotate %f | ", rotationAngle);
float updatedAngle =
Angle::Normalize(obj->position.horizontalAngle - rotationAngle);
obj->position.horizontalAngle = updatedAngle;
Angle::Normalize(thing->position.horizontalAngle - rotationAngle);
thing->position.horizontalAngle = updatedAngle;
printf("-> %f (%f %f) \n", thing->position.distance,
(float)thing->position.horizontalAngle,
(float)thing->position.verticalAngle);
}
}

2
Perception.h
View File

@ -45,6 +45,8 @@ public:
/// @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

3
Roboid.cpp
View File

@ -15,6 +15,7 @@ Roboid::Roboid() {
this->perception = new Perception();
this->perception->roboid = this;
this->propulsion = nullptr;
this->networkSync = nullptr;
this->actuationRoot = nullptr;
this->worldPosition = Vector3::zero;
// this->worldOrientation = Quaternion::identity;
@ -43,6 +44,7 @@ Roboid::Roboid(Perception *perception, ServoMotor *actuationRoot) : Roboid() {
Roboid::Roboid(ServoMotor *actuationRoot) : actuationRoot(actuationRoot) {}
#include <Arduino.h>
void Roboid::Update(float currentTimeMs) {
if (perception != nullptr)
perception->Update(currentTimeMs);
@ -65,6 +67,7 @@ void Roboid::Update(float currentTimeMs) {
if (actuationRoot != nullptr)
actuationRoot->Update(currentTimeMs);
lastUpdateTimeMs = currentTimeMs;
}

5
TrackedObject.cpp
View File

@ -20,11 +20,16 @@ InterestingThing::InterestingThing(Sensor *sensor, Spherical position,
this->updated = true;
}
#include <Arduino.h>
bool InterestingThing::IsTheSameAs(InterestingThing *otherObj) {
if (id != 0 && id == otherObj->id)
return true;
if (type != otherObj->type)
return false;
printf(" %f (%f %f) is same as %f (%f %f)\n", otherObj->position.distance,
(float)otherObj->position.horizontalAngle,
(float)otherObj->position.verticalAngle, position.distance,
(float)position.horizontalAngle, (float)position.verticalAngle);
if (fabsf(position.distance - otherObj->position.distance) > equalityDistance)
return false;
if (fabsf(position.horizontalAngle - otherObj->position.horizontalAngle) >