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

Improved network sync

Browse Source
This commit is contained in:
Pascal Serrarens 2024-04-10 15:35:06 +02:00
parent 28bf3b4682
commit b0d0258ad6
4 changed files with 16 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
NetworkSync.cpp
View File

@ -12,11 +12,11 @@
#include "LinearAlgebra/AngleUsing.h" #include "LinearAlgebra/AngleUsing.h"
#include "LinearAlgebra/Spherical.h" #include "LinearAlgebra/Spherical.h"
void NetworkSync::SendVector3(unsigned char *data, unsigned int &startIndex, void NetworkSync::SendVector3(unsigned char *data, unsigned int startIndex,
const Vector3 v) { const Vector3 v) {
SendSingle100(data, startIndex, v.x); SendSingle100(data, startIndex, v.x);
SendSingle100(data, startIndex, v.y); SendSingle100(data, startIndex += 4, v.y);
SendSingle100(data, startIndex, v.z); SendSingle100(data, startIndex += 4, v.z);
} }
void NetworkSync::SendQuaternion(unsigned char *data, const int startIndex, void NetworkSync::SendQuaternion(unsigned char *data, const int startIndex,
@ -61,6 +61,7 @@ void NetworkSync::SendQuat32(unsigned char *data, int startIndex,
qz = -qz; qz = -qz;
qw = -qw; qw = -qw;
} }
// Serial.printf(" (%d) %d:%d:%d:%d ", startIndex, qx, qy, qz, qw);
data[startIndex++] = qx; data[startIndex++] = qx;
data[startIndex++] = qy; data[startIndex++] = qy;
data[startIndex++] = qz; data[startIndex++] = qz;

2
NetworkSync.h
View File

@ -61,7 +61,7 @@ protected:
const float value); const float value);
void SendAngle32(unsigned char *data, unsigned int startIndex, void SendAngle32(unsigned char *data, unsigned int startIndex,
const float value); const float value);
void SendVector3(unsigned char *data, unsigned int &startIndex, void SendVector3(unsigned char *data, unsigned int startIndex,
const Vector3 v); const Vector3 v);
void SendQuaternion(unsigned char *data, const int startIndex, void SendQuaternion(unsigned char *data, const int startIndex,
const Quaternion q); const Quaternion q);

18
Perception.cpp
View File

@ -201,11 +201,11 @@ InterestingThing *Perception::AddTrackedObject(Sensor *sensor,
else { else {
if (obj->IsTheSameAs(this->trackedObjects[objIx])) { if (obj->IsTheSameAs(this->trackedObjects[objIx])) {
#ifdef RC_DEBUG2 #ifdef RC_DEBUG2
Serial.print((int)this->trackedObjects[objIx]->id); // Serial.print((int)this->trackedObjects[objIx]->id);
Serial.println(": update tracked object"); // Serial.println(": update tracked object");
#endif #endif
this->trackedObjects[objIx]->Refresh(obj->position); this->trackedObjects[objIx]->Refresh(obj->position, obj->orientation);
delete obj; delete obj;
return this->trackedObjects[objIx]; return this->trackedObjects[objIx];
} }
@ -333,12 +333,12 @@ void Perception::Update(float currentTimeMs) {
if (obj->DegradeConfidence(deltaTime) == false) { if (obj->DegradeConfidence(deltaTime) == false) {
// delete obj // delete obj
// if (roboid != nullptr && roboid->networkSync != nullptr) { if (roboid != nullptr && roboid->networkSync != nullptr) {
// printf("Cleanup obj %d\n", obj->id); printf("Cleanup obj %d\n", obj->id);
// roboid->networkSync->DestroyObject(obj); roboid->networkSync->DestroyObject(obj);
// } }
// this->trackedObjects[objIx] = nullptr; this->trackedObjects[objIx] = nullptr;
// delete obj; delete obj;
} }
} }
} }

4
TrackedObject.h
View File

@ -48,7 +48,7 @@ public:
/// = 1.2F and position->distance = 1.6 will be considered different, but /// = 1.2F and position->distance = 1.6 will be considered different, but
/// objects with coordinates position->distance = 1.2 and position->distance /// objects with coordinates position->distance = 1.2 and position->distance
/// = 1.0 can be the same. /// = 1.0 can be the same.
static constexpr float equalityDistance = 0.01F; // 0.3F; static constexpr float equalityDistance = 0.3F;
/// @brief The maximum difference in angle from the roboids orientation in /// @brief The maximum difference in angle from the roboids orientation in
/// which two objects may be considered the same /// which two objects may be considered the same
/// @details When the difference in angle is exactly this value, the objects /// @details When the difference in angle is exactly this value, the objects
@ -56,7 +56,7 @@ public:
/// With a value of 5.0, object with coordinates position->angle = 30 /// With a value of 5.0, object with coordinates position->angle = 30
/// and position->angle = 36 will be considered different, but object with /// and position->angle = 36 will be considered different, but object with
/// coordinated position->angle = 30 and position->angle = 27 can be the same. /// coordinated position->angle = 30 and position->angle = 27 can be the same.
static constexpr float equalityAngle = 0.01F; // 5.0F; static constexpr float equalityAngle = 5.0F;
/// @brief The id of the tracked object /// @brief The id of the tracked object
char id; char id;