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"
#define RC_DEBUG 1
// #define RC_DEBUG 1
#ifdef RC_DEBUG
#include <Arduino.h>
@ -9,7 +9,7 @@
#include "LinearAlgebra/Angle8.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) {
networkPerception->ProcessPacket(roboid, buffer, bytecount);
@ -24,7 +24,37 @@ void NetworkSync::ReceiveMessage(Roboid *roboid, unsigned char bytecount) {
void NetworkSync::ReceiveNetworkId() {
this->networkId = buffer[1];
#ifdef RC_DEBUG
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,
@ -145,6 +175,10 @@ void NetworkSync::PublishClient() {
buffer[ix++] = ClientMsg;
buffer[ix++] = 0; // No network ID
SendBuffer(ix);
#ifdef RC_DEBUG
printf("Sent new Client\n");
#endif
}
void NetworkSync::PublishTrackedObjects(Buffer sendBuffer,
@ -187,8 +221,8 @@ void NetworkSync::PublishTrackedObject(Buffer sendBuffer,
(UInt8)object->id,
Pose_Position,
};
unsigned int ix = 3;
SendVector3(buffer, ix, worldPosition3);
unsigned char ix = 3;
SendVector3(buffer, &ix, worldPosition3);
sendBuffer(buffer, bufferSize);
#endif
}
@ -237,9 +271,9 @@ void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid *roboid) {
0, // objectId;
Pose_LinearVelocity | Pose_AngularVelocity,
};
unsigned int ix = 3;
SendVector3(buffer, ix, worldVelocity3);
SendVector3(buffer, ix, worldAngularVelocity);
unsigned char ix = 3;
SendVector3(buffer, &ix, worldVelocity3);
SendVector3(buffer, &ix, worldAngularVelocity);
sendBuffer(buffer, bufferSize);
#endif
@ -256,7 +290,9 @@ void NetworkSync::SendDestroyObject(Buffer sendBuffer, InterestingThing *obj) {
}
void NetworkSync::SendInvestigateThing(InterestingThing *thing) {
#ifdef RC_DEBUG
printf("Investigate [%d/%d]\n", thing->networkId, thing->id);
#endif
unsigned char ix = 0;
buffer[ix++] = InvestigateMsg;
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
class NetworkSync {
public:
NetworkSync();
NetworkSync(Roboid *roboid);
unsigned char networkId;
@ -20,8 +20,9 @@ public:
virtual void NetworkUpdate(Roboid *roboid) = 0;
/// @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 DestroyObject(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;
@ -67,7 +68,9 @@ public:
virtual void SendPose(Vector3 worldPosition, Quaternion worldOrientation) {};
protected:
Roboid *roboid;
NetworkPerception *networkPerception;
void PublishTrackedObject(Buffer sendBuffer, InterestingThing *object);
void PublishRelativeObject(Buffer sendBuffer, UInt8 parentId,
InterestingThing *object);
@ -92,7 +95,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();

63
Perception.cpp
View File

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

1
Perception.h
View File

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