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

Publish relative object

Browse Source
This commit is contained in:
Pascal Serrarens 2024-04-05 12:48:36 +02:00
parent 141afa2772
commit 920c90163d
6 changed files with 97 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
LinearAlgebra

@ -1 +1 @@
Subproject commit ae55a24dc09d1603179b388d90297be3a4eb457c
Subproject commit 66ff721dfe5da8209d3650a530364235630aeb40

73
NetworkSync.cpp
View File

@ -1,10 +1,16 @@
#include "NetworkSync.h"
#define RC_DEBUG 1
#ifdef RC_DEBUG
#include <Arduino.h>
#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<unsigned char> packedAngle = AngleUsing<unsigned char>(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<signed short> packedAngle = AngleUsing<signed short>(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<signed long> packedAngle = AngleUsing<signed long>(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 =

18
NetworkSync.h
View File

@ -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);
};

12
Perception.cpp
View File

@ -6,6 +6,7 @@
#include <math.h>
// #define RC_DEBUG2
#ifdef RC_DEBUG2
#include <Arduino.h>
#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;
}
}

4
Perception.h
View File

@ -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

5
TrackedObject.h
View File

@ -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