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

Refactoring

Browse Source
This commit is contained in:
Pascal Serrarens 2024-05-17 11:58:06 +02:00
parent 1aa4520646
commit 52a75d6b7b
4 changed files with 81 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
NetworkSync.cpp
View File

@ -1,6 +1,6 @@
#include "NetworkSync.h" #include "NetworkSync.h"
#define RC_DEBUG 1 // #define RC_DEBUG 1
#ifdef RC_DEBUG #ifdef RC_DEBUG
#include <Arduino.h> #include <Arduino.h>
@ -9,7 +9,7 @@
#include "LinearAlgebra/Angle8.h" #include "LinearAlgebra/Angle8.h"
#include "LinearAlgebra/Spherical.h" #include "LinearAlgebra/Spherical.h"
NetworkSync::NetworkSync() { buffer = new unsigned char[32]; } NetworkSync::NetworkSync(Roboid *roboid) { this->roboid = roboid; }
void NetworkSync::ReceiveMessage(Roboid *roboid, unsigned char bytecount) { void NetworkSync::ReceiveMessage(Roboid *roboid, unsigned char bytecount) {
networkPerception->ProcessPacket(roboid, buffer, bytecount); networkPerception->ProcessPacket(roboid, buffer, bytecount);
@ -24,7 +24,37 @@ void NetworkSync::ReceiveMessage(Roboid *roboid, unsigned char bytecount) {
void NetworkSync::ReceiveNetworkId() { void NetworkSync::ReceiveNetworkId() {
this->networkId = buffer[1]; this->networkId = buffer[1];
#ifdef RC_DEBUG
printf("Received network Id %d\n", this->networkId); printf("Received network Id %d\n", this->networkId);
#endif
}
void NetworkSync::NewObject(InterestingThing *thing) {
if (thing == nullptr || thing->networkId != 0x00)
return;
unsigned char ix = 0;
buffer[ix++] = CreateMsg;
buffer[ix++] = thing->id;
buffer[ix++] = thing->type;
SendBuffer(ix);
#ifdef RC_DEBUG
printf("Sent CreateMsg %d %d\n", buffer[1], buffer[2]);
#endif
// PublishTrackedObject(roboid, obj);
}
void NetworkSync::DestroyObject(InterestingThing *thing) {
unsigned char ix = 0;
buffer[ix++] = DestroyMsg;
buffer[ix++] = thing->id;
SendBuffer(ix);
#if RC_DEBUG
printf("Sent DestroyMsg %d %d\n", buffer[1], buffer[2]);
#endif
} }
void NetworkSync::SendVector3(unsigned char *data, unsigned char *startIndex, void NetworkSync::SendVector3(unsigned char *data, unsigned char *startIndex,
@ -145,6 +175,10 @@ void NetworkSync::PublishClient() {
buffer[ix++] = ClientMsg; buffer[ix++] = ClientMsg;
buffer[ix++] = 0; // No network ID buffer[ix++] = 0; // No network ID
SendBuffer(ix); SendBuffer(ix);
#ifdef RC_DEBUG
printf("Sent new Client\n");
#endif
} }
void NetworkSync::PublishTrackedObjects(Buffer sendBuffer, void NetworkSync::PublishTrackedObjects(Buffer sendBuffer,
@ -187,8 +221,8 @@ void NetworkSync::PublishTrackedObject(Buffer sendBuffer,
(UInt8)object->id, (UInt8)object->id,
Pose_Position, Pose_Position,
}; };
unsigned int ix = 3; unsigned char ix = 3;
SendVector3(buffer, ix, worldPosition3); SendVector3(buffer, &ix, worldPosition3);
sendBuffer(buffer, bufferSize); sendBuffer(buffer, bufferSize);
#endif #endif
} }
@ -237,9 +271,9 @@ void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid *roboid) {
0, // objectId; 0, // objectId;
Pose_LinearVelocity | Pose_AngularVelocity, Pose_LinearVelocity | Pose_AngularVelocity,
}; };
unsigned int ix = 3; unsigned char ix = 3;
SendVector3(buffer, ix, worldVelocity3); SendVector3(buffer, &ix, worldVelocity3);
SendVector3(buffer, ix, worldAngularVelocity); SendVector3(buffer, &ix, worldAngularVelocity);
sendBuffer(buffer, bufferSize); sendBuffer(buffer, bufferSize);
#endif #endif
@ -256,7 +290,9 @@ void NetworkSync::SendDestroyObject(Buffer sendBuffer, InterestingThing *obj) {
} }
void NetworkSync::SendInvestigateThing(InterestingThing *thing) { void NetworkSync::SendInvestigateThing(InterestingThing *thing) {
#ifdef RC_DEBUG
printf("Investigate [%d/%d]\n", thing->networkId, thing->id); printf("Investigate [%d/%d]\n", thing->networkId, thing->id);
#endif
unsigned char ix = 0; unsigned char ix = 0;
buffer[ix++] = InvestigateMsg; buffer[ix++] = InvestigateMsg;
buffer[ix++] = thing->networkId; buffer[ix++] = thing->networkId;

11
NetworkSync.h
View File

@ -11,7 +11,7 @@ namespace RoboidControl {
/// @brief Interface for synchronizaing state between clients across a network /// @brief Interface for synchronizaing state between clients across a network
class NetworkSync { class NetworkSync {
public: public:
NetworkSync(); NetworkSync(Roboid *roboid);
unsigned char networkId; unsigned char networkId;
@ -20,8 +20,9 @@ public:
virtual void NetworkUpdate(Roboid *roboid) = 0; virtual void NetworkUpdate(Roboid *roboid) = 0;
/// @brief Inform that the given object is no longer being tracked /// @brief Inform that the given object is no longer being tracked
/// @param obj /// @param obj
virtual void DestroyObject(InterestingThing *obj) = 0; virtual void DestroyObject(InterestingThing *obj);
virtual void NewObject(InterestingThing *obj) {}; virtual void NewObject(InterestingThing *obj);
/// @brief The id of a Pose message /// @brief The id of a Pose message
static const unsigned char PoseMsg = 0x10; static const unsigned char PoseMsg = 0x10;
static const unsigned char PoseTypeMsg = 0x11; static const unsigned char PoseTypeMsg = 0x11;
@ -67,7 +68,9 @@ public:
virtual void SendPose(Vector3 worldPosition, Quaternion worldOrientation) {}; virtual void SendPose(Vector3 worldPosition, Quaternion worldOrientation) {};
protected: protected:
Roboid *roboid;
NetworkPerception *networkPerception; NetworkPerception *networkPerception;
void PublishTrackedObject(Buffer sendBuffer, InterestingThing *object); void PublishTrackedObject(Buffer sendBuffer, InterestingThing *object);
void PublishRelativeObject(Buffer sendBuffer, UInt8 parentId, void PublishRelativeObject(Buffer sendBuffer, UInt8 parentId,
InterestingThing *object); InterestingThing *object);
@ -92,7 +95,7 @@ protected:
void SendQuat32(unsigned char *data, unsigned char *startIndex, void SendQuat32(unsigned char *data, unsigned char *startIndex,
const Quaternion q); const Quaternion q);
unsigned char *buffer; // =[32]; unsigned char buffer[32];
virtual void SendBuffer(unsigned char bufferSize); virtual void SendBuffer(unsigned char bufferSize);
void PublishClient(); void PublishClient();

63
Perception.cpp
View File

@ -268,36 +268,37 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position,
} }
} }
InterestingThing *Perception::AddTrackedObject(Sensor *sensor, InterestingThing *
Spherical position, Perception::AddTrackedObject(Sensor *sensor, Spherical position,
Quaternion orientation, Quaternion orientation, unsigned char thingType,
unsigned char objectId, unsigned char thingId, unsigned networkId) {
unsigned networkId) { InterestingThing *thing = new InterestingThing(sensor, position, orientation);
InterestingThing *obj = new InterestingThing(sensor, position, orientation); thing->type = thingType;
unsigned char farthestObjIx = 0; unsigned char farthestObjIx = 0;
unsigned char availableSlotIx = 0; unsigned char availableSlotIx = 0;
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) {
if (this->trackedObjects[objIx] == nullptr) { if (this->trackedObjects[thingIx] == nullptr) {
availableSlotIx = objIx; availableSlotIx = thingIx;
} }
// Do we see the same object? // Do we see the same object?
else { else {
if (obj->IsTheSameAs(this->trackedObjects[objIx])) { if (thing->IsTheSameAs(this->trackedObjects[thingIx])) {
#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, obj->orientation); this->trackedObjects[thingIx]->Refresh(thing->position,
delete obj; thing->orientation);
return this->trackedObjects[objIx]; delete thing;
return this->trackedObjects[thingIx];
} }
// Is this the fartest object we see? // Is this the fartest object we see?
else if (this->trackedObjects[farthestObjIx] == nullptr || else if (this->trackedObjects[farthestObjIx] == nullptr ||
(this->trackedObjects[objIx]->position.distance > (this->trackedObjects[thingIx]->position.distance >
this->trackedObjects[farthestObjIx]->position.distance)) { this->trackedObjects[farthestObjIx]->position.distance)) {
farthestObjIx = objIx; farthestObjIx = thingIx;
} }
} }
} }
@ -306,43 +307,39 @@ InterestingThing *Perception::AddTrackedObject(Sensor *sensor,
// max number of objects) // max number of objects)
if (availableSlotIx < maxObjectCount) { if (availableSlotIx < maxObjectCount) {
// a slot is available // a slot is available
this->trackedObjects[availableSlotIx] = obj; this->trackedObjects[availableSlotIx] = thing;
obj->networkId = networkId; thing->networkId = networkId;
if (objectId == 0x00) if (thingId == 0x00)
obj->id = lastObjectId++; // availableSlotIx + 1; thing->id = lastObjectId++; // availableSlotIx + 1;
else else
obj->id = objectId; thing->id = thingId;
#ifdef RC_DEBUG2 #ifdef RC_DEBUG2
printf("%d: new tracked object {%d/%d}\n", availableSlotIx, obj->networkId, printf("%d: new tracked object {%d/%d}\n", availableSlotIx, obj->networkId,
obj->id); obj->id);
#endif #endif
return obj; return thing;
} }
// If this object is closer than the farthest object, then replace it // If this object is closer than the farthest object, then replace it
else if (obj->position.distance < else if (thing->position.distance <
this->trackedObjects[farthestObjIx]->position.distance) { this->trackedObjects[farthestObjIx]->position.distance) {
delete this->trackedObjects[farthestObjIx]; delete this->trackedObjects[farthestObjIx];
this->trackedObjects[farthestObjIx] = obj; this->trackedObjects[farthestObjIx] = thing;
obj->networkId = networkId; thing->networkId = networkId;
if (objectId == 0x00) if (thingId == 0x00)
obj->id = lastObjectId++; // availableSlotIx + 1; thing->id = lastObjectId++; // availableSlotIx + 1;
else else
obj->id = objectId; thing->id = thingId;
#ifdef RC_DEBUG2 #ifdef RC_DEBUG2
// Serial.print((int)obj->id);
// Serial.println(": replaced tracked object");
printf("%d: replaced tracked object {%d/%d}\n", farthestObjIx, printf("%d: replaced tracked object {%d/%d}\n", farthestObjIx,
obj->networkId, obj->id); obj->networkId, obj->id);
#endif #endif
return obj; return thing;
} else { } else {
#ifdef RC_DEBUG2 #ifdef RC_DEBUG2
// Serial.print((int)obj->id);
// Serial.println(": delete tracked object");
printf("%d: delete tracked object {%d/%d}\n", -1, obj->networkId, obj->id); printf("%d: delete tracked object {%d/%d}\n", -1, obj->networkId, obj->id);
#endif #endif
// No available slot, delete trackedobject // No available slot, delete trackedobject
delete obj; delete thing;
return nullptr; return nullptr;
} }
} }

1
Perception.h
View File

@ -93,6 +93,7 @@ public:
InterestingThing * InterestingThing *
AddTrackedObject(Sensor *sensor, Spherical position, AddTrackedObject(Sensor *sensor, Spherical position,
Quaternion orientation = Quaternion::identity, Quaternion orientation = Quaternion::identity,
unsigned char objectType = 0x00,
unsigned char objectId = 0x00, unsigned networkId = 0x00); unsigned char objectId = 0x00, unsigned networkId = 0x00);
InterestingThing * InterestingThing *