From 920c90163da7a36361f406759e834b939517ee3f Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 5 Apr 2024 12:48:36 +0200 Subject: [PATCH] Publish relative object --- LinearAlgebra | 2 +- NetworkSync.cpp | 73 +++++++++++++++++++++++++++++++++++++++++++++---- NetworkSync.h | 18 +++++++++--- Perception.cpp | 12 +++++--- Perception.h | 4 ++- TrackedObject.h | 5 ++-- 6 files changed, 97 insertions(+), 17 deletions(-) diff --git a/LinearAlgebra b/LinearAlgebra index ae55a24..66ff721 160000 --- a/LinearAlgebra +++ b/LinearAlgebra @@ -1 +1 @@ -Subproject commit ae55a24dc09d1603179b388d90297be3a4eb457c +Subproject commit 66ff721dfe5da8209d3650a530364235630aeb40 diff --git a/NetworkSync.cpp b/NetworkSync.cpp index 3fa7dcb..fe649fa 100644 --- a/NetworkSync.cpp +++ b/NetworkSync.cpp @@ -1,10 +1,16 @@ #include "NetworkSync.h" +#define RC_DEBUG 1 + #ifdef RC_DEBUG #include #endif -#include "LinearAlgebra/DiscreteAngle.h" +#include "LinearAlgebra/Angle.h" +#include "LinearAlgebra/Angle16.h" +#include "LinearAlgebra/Angle32.h" +#include "LinearAlgebra/AngleUsing.h" +#include "LinearAlgebra/Spherical.h" void NetworkSync::SendVector3(unsigned char *data, unsigned int &startIndex, const Vector3 v) { @@ -22,6 +28,27 @@ void NetworkSync::SendQuaternion(unsigned char *data, const int startIndex, SendAngle(data, ix++, angles.z); } +void NetworkSync::SendSpherical(unsigned char *data, int startIndex, + Spherical s) { + SendAngle(data, startIndex++, s.horizontalAngle); + SendAngle(data, startIndex++, s.verticalAngle); + SendAngle(data, startIndex++, s.distance); +} + +void NetworkSync::SendSpherical16(unsigned char *data, int startIndex, + Spherical s) { + SendAngle16(data, startIndex, s.horizontalAngle); + SendAngle16(data, startIndex += 2, s.verticalAngle); + SendSingle100(data, startIndex += 2, s.distance); +} + +void NetworkSync::SendSpherical32(unsigned char *data, int startIndex, + Spherical s) { + SendAngle32(data, startIndex, s.horizontalAngle); + SendAngle32(data, startIndex += 4, s.verticalAngle); + SendSingle100(data, startIndex += 4, s.distance); +} + void NetworkSync::SendQuat32(unsigned char *data, int startIndex, const Quaternion q) { unsigned char qx = (char)(q.x * 127 + 128); @@ -40,13 +67,34 @@ void NetworkSync::SendQuat32(unsigned char *data, int startIndex, data[startIndex++] = qw; } -void NetworkSync::SendAngle(unsigned char *data, const int startIndex, - float angle) { +void NetworkSync::SendAngle(unsigned char *data, unsigned int startIndex, + const float angle) { AngleUsing packedAngle = AngleUsing(angle); data[startIndex] = packedAngle.GetValue(); } -void NetworkSync::SendSingle100(unsigned char *data, unsigned int &startIndex, +void NetworkSync::SendAngle16(unsigned char *data, unsigned int startIndex, + const float angle) { + AngleUsing packedAngle = AngleUsing(angle); + signed short value = packedAngle.GetValue(); + data[startIndex] = value >> 8; + data[startIndex + 1] = value & 0xFF; +} + +void NetworkSync::SendAngle32(unsigned char *data, unsigned int startIndex, + const float angle) { + AngleUsing packedAngle = AngleUsing(angle); + unsigned long value = packedAngle.GetValue(); + data[startIndex] = value >> 24 & 0xFF; + data[startIndex + 1] = value >> 16 & 0xFF; + data[startIndex + 2] = value >> 8 & 0xFF; + data[startIndex + 3] = value & 0xFF; + Serial.printf(" %lu=%d:%d:%d:%d ", value, data[startIndex], + data[startIndex + 1], data[startIndex + 2], + data[startIndex + 3]); +} + +void NetworkSync::SendSingle100(unsigned char *data, unsigned int startIndex, float value) { // Sends a float with truncated 2 decimal precision Int32 intValue = value * 100; @@ -56,7 +104,7 @@ void NetworkSync::SendSingle100(unsigned char *data, unsigned int &startIndex, // } } -void NetworkSync::SendInt32(unsigned char *data, unsigned int &startIndex, +void NetworkSync::SendInt32(unsigned char *data, unsigned int startIndex, Int32 value) { for (unsigned char ix = 0; ix < 4; ix++) { data[startIndex++] = ((unsigned char *)&value)[ix]; @@ -107,6 +155,21 @@ void NetworkSync::PublishTrackedObject(SendBuffer sendBuffer, #endif } +void NetworkSync::PublishRelativeObject(SendBuffer sendBuffer, UInt8 parentId, + InterestingThing *object) { + const UInt16 bufferSize = 4 + 12; + UInt8 buffer[bufferSize] = { + RelativePoseMsg, + (UInt8)parentId, + (UInt8)object->id, + Pose_Position, + }; + unsigned int ix = 4; + SendSpherical32(buffer, ix, object->position); + // SendVector3(buffer, ix, worldPosition3); + sendBuffer(buffer, bufferSize); +} + void NetworkSync::SendPoseMsg(SendBuffer sendBuffer, Roboid *roboid) { Polar velocity = roboid->propulsion->GetVelocity(); Vector2 worldVelocity2 = diff --git a/NetworkSync.h b/NetworkSync.h index dac812b..c9efed8 100644 --- a/NetworkSync.h +++ b/NetworkSync.h @@ -21,6 +21,7 @@ public: /// @brief The id of a Pose message static const char PoseMsg = 0x10; static const char PoseTypeMsg = 0x11; + static const char RelativePoseMsg = 0x12; /// @brief A bit pattern for the pose, stating that this message contains a /// position in world coordinates static const char Pose_Position = 0x01; @@ -49,15 +50,24 @@ public: protected: NetworkPerception *networkPerception; void PublishTrackedObject(SendBuffer sendBuffer, InterestingThing *object); + void PublishRelativeObject(SendBuffer sendBuffer, UInt8 parentId, + InterestingThing *object); - void SendSingle100(unsigned char *data, unsigned int &startIndex, - float value); - void SendInt32(unsigned char *data, unsigned int &startIndex, Int32 value); - void SendAngle(unsigned char *data, const int startIndex, float value); + void SendSingle100(unsigned char *data, unsigned int startIndex, float value); + void SendInt32(unsigned char *data, unsigned int startIndex, Int32 value); + void SendAngle(unsigned char *data, unsigned int startIndex, + const float value); + void SendAngle16(unsigned char *data, unsigned int startIndex, + const float value); + void SendAngle32(unsigned char *data, unsigned int startIndex, + const float value); void SendVector3(unsigned char *data, unsigned int &startIndex, const Vector3 v); void SendQuaternion(unsigned char *data, const int startIndex, const Quaternion q); + void SendSpherical(unsigned char *data, int startIndex, Spherical s); + void SendSpherical16(unsigned char *data, int startIndex, Spherical s); + void SendSpherical32(unsigned char *data, int startIndex, Spherical s); void SendQuat32(unsigned char *data, int startIndex, const Quaternion q); }; diff --git a/Perception.cpp b/Perception.cpp index 5383955..3c47616 100644 --- a/Perception.cpp +++ b/Perception.cpp @@ -6,6 +6,7 @@ #include +// #define RC_DEBUG2 #ifdef RC_DEBUG2 #include #endif @@ -185,8 +186,9 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position) { } } -void Perception::AddTrackedObject(Sensor *sensor, Spherical position, - Quaternion orientation) { +InterestingThing *Perception::AddTrackedObject(Sensor *sensor, + Spherical position, + Quaternion orientation) { InterestingThing *obj = new InterestingThing(sensor, position, orientation); unsigned char farthestObjIx = 0; @@ -205,7 +207,7 @@ void Perception::AddTrackedObject(Sensor *sensor, Spherical position, this->trackedObjects[objIx]->Refresh(obj->position); delete obj; - return; + return this->trackedObjects[objIx]; } // Is this the fartest object we see? else if (this->trackedObjects[farthestObjIx] == nullptr || @@ -226,7 +228,7 @@ void Perception::AddTrackedObject(Sensor *sensor, Spherical position, Serial.print((int)obj->id); Serial.println(": new tracked object"); #endif - + return obj; } // If this object is closer than the farthest object, then replace it else if (obj->position.distance < @@ -238,6 +240,7 @@ void Perception::AddTrackedObject(Sensor *sensor, Spherical position, Serial.print((int)obj->id); Serial.println(": replaced tracked object"); #endif + return obj; } else { #ifdef RC_DEBUG2 Serial.print((int)obj->id); @@ -245,6 +248,7 @@ void Perception::AddTrackedObject(Sensor *sensor, Spherical position, #endif // No available slot, delete trackedobject delete obj; + return nullptr; } } diff --git a/Perception.h b/Perception.h index abbf40b..c3c6556 100644 --- a/Perception.h +++ b/Perception.h @@ -86,7 +86,9 @@ public: /// @param position The position of the sensor in polar coordinates local to /// the roboid void AddTrackedObject(Sensor *sensor, Polar position); - void AddTrackedObject(Sensor *sensor, Spherical position, Quaternion orientation = Quaternion::identity); + InterestingThing * + AddTrackedObject(Sensor *sensor, Spherical position, + Quaternion orientation = Quaternion::identity); /// @brief Retrieve the number of objects currently being tracked by the /// roboid diff --git a/TrackedObject.h b/TrackedObject.h index 7d0fba9..b34ea8c 100644 --- a/TrackedObject.h +++ b/TrackedObject.h @@ -47,7 +47,7 @@ public: /// = 1.2F and position->distance = 1.6 will be considered different, but /// objects with coordinates position->distance = 1.2 and position->distance /// = 1.0 can be the same. - static constexpr float equalityDistance = 0.3F; + static constexpr float equalityDistance = 0.01F; // 0.3F; /// @brief The maximum difference in angle from the roboids orientation in /// which two objects may be considered the same /// @details When the difference in angle is exactly this value, the objects @@ -55,11 +55,12 @@ public: /// With a value of 5.0, object with coordinates position->angle = 30 /// and position->angle = 36 will be considered different, but object with /// coordinated position->angle = 30 and position->angle = 27 can be the same. - static constexpr float equalityAngle = 5.0F; + static constexpr float equalityAngle = 0.01F; // 5.0F; /// @brief The id of the tracked object char id; + char parentId = 0; /// @brief The current position of the object Spherical position = Spherical::zero; /// @brief The current orientation of the object