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

Synchronizing perception

Browse Source
This commit is contained in:
Pascal Serrarens 2024-04-22 16:07:24 +02:00
parent bd335a123a
commit 0ba9d55a4f
7 changed files with 268 additions and 101 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

212
NetworkPerception.cpp
View File

@ -7,10 +7,146 @@ void NetworkPerception::ProcessPacket(Roboid *roboid, unsigned char *buffer,
// printf("packet received, type = 0x%02x %d %d %d %d %d\n", buffer[0],
// buffer[2], buffer[2], buffer[3], buffer[4], buffer[5]);
if (buffer[0] == NetworkSync::PoseMsg) {
ReceiveObject(buffer, roboid);
} else if (buffer[0] == NetworkSync::PoseTypeMsg) {
switch (buffer[0]) {
case NetworkSync::CreateMsg:
ReceiveCreateMsg(buffer, roboid);
break;
case NetworkSync::PoseMsg:
ReceivePoseMsg(buffer, roboid);
break;
case NetworkSync::PoseTypeMsg:
ReceiveTypedObject(buffer, roboid);
break;
}
}
#include <Arduino.h>
void NetworkPerception::ReceiveCreateMsg(unsigned char *data, Roboid *roboid) {
unsigned char networkId = data[1];
unsigned char objectId = data[2];
unsigned char objectType = data[3];
if (networkId == roboid->networkSync->networkId)
return;
// printf("Received create message [%d/%d]\n", networkId, objectId);
InterestingThing *thing =
roboid->perception->FindTrackedObject(networkId, objectId);
if (thing != nullptr) {
thing->type = objectType;
}
}
void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
unsigned char networkId = data[1];
unsigned char objectId = data[2];
unsigned char poseType = data[3];
printf("Received pose message [%d/%d]\n", networkId, objectId);
if ((poseType & NetworkSync::Pose_Position) != 0) {
Vector3 worldPosition = ReceiveVector3(data, 4);
if (objectId == 0) {
roboid->SetPosition(worldPosition);
} else {
Vector3 myPosition = roboid->GetPosition(); // Vector3::zero;
// Vector2 myPosition2 = Vector3::ProjectHorizontalPlane(myPosition);
Quaternion myOrientation = roboid->GetOrientation();
// Vector3 myDirection = myOrientation * Vector3::forward;
// Vector2 myDirection2 = Vector3::ProjectHorizontalPlane(myDirection);
Vector3 localPosition =
Quaternion::Inverse(myOrientation) * (worldPosition - myPosition);
// float distance = Vector3::Distance(myPosition, worldPosition);
// float angle = Vector3::SignedAngle(myDirection, localPosition,
// Vector3::up);
float distance = Vector3::Magnitude(localPosition);
float angle =
Vector3::SignedAngle(Vector3::forward, localPosition, Vector3::up);
Polar position = Polar(angle, distance);
// int objCount = roboid->perception->TrackedObjectCount();
// printf("object received at (%f, %f)/(%f, %f) -> (%f %f)\n",
// worldPosition.x, worldPosition.z, localPosition.x,
// localPosition.z, distance, angle);
// roboid->perception->AddTrackedObject(this, position);
InterestingThing *thing = roboid->perception->AddTrackedObject(
this, networkId, objectId, position);
if (thing->type == 0xFF) {
// Unknown thing
roboid->networkSync->SendInvestigateThing(thing);
}
}
}
/*
Vector3 worldAngles = ReceiveVector3(data, 16);
Quaternion worldOrientation = Quaternion::Euler(worldAngles);
Vector3 worldDirection = worldOrientation * Vector3::forward;
if (objectId == 0) {
roboid->SetPosition(worldPosition);
roboid->SetOrientation(worldOrientation);
// printf("Received pose (%f, %f) (%f, %f)\n", worldPosition.x,
// worldPosition.z, worldDirection.x, worldDirection.z);
} else {
Vector3 myPosition = roboid->GetPosition(); // Vector3::zero;
Vector2 myPosition2 = Vector3::ProjectHorizontalPlane(myPosition);
Quaternion myOrientation = roboid->GetOrientation();
Vector3 myDirection = myOrientation * Vector3::forward;
Vector2 myDirection2 = Vector3::ProjectHorizontalPlane(myDirection);
Vector3 localPosition =
Quaternion::Inverse(myOrientation) * (worldPosition - myPosition);
// float distance = Vector3::Distance(myPosition, worldPosition);
// float angle = Vector3::SignedAngle(myDirection, localPosition,
// Vector3::up);
float distance = Vector3::Magnitude(localPosition);
float angle =
Vector3::SignedAngle(Vector3::forward, localPosition, Vector3::up);
Polar position = Polar(angle, distance);
// int objCount = roboid->perception->TrackedObjectCount();
// printf("object received at (%f, %f)/(%f, %f) -> (%f %f)\n",
// worldPosition.x,
// worldPosition.z, localPosition.x, localPosition.z, distance,
// angle);
roboid->perception->AddTrackedObject(this, position);
}
*/
}
// HACK!!!
// #include "../../../src/LighthouseTracker.h"
void NetworkPerception::ReceiveTypedObject(unsigned char *data,
Roboid *roboid) {
unsigned char objectType = data[1];
unsigned char objectId = data[2];
if (objectType == 0x02) { // lighthouse
// We require position and orientation for lighthouses
if (data[3] !=
(NetworkSync::Pose_Position | NetworkSync::Pose_Orientation)) {
return;
}
Vector3 worldPosition = ReceiveVector3(data, 4);
Vector3 worldAngles = ReceiveVector3(data, 16);
Quaternion worldOrientation = Quaternion::Euler(worldAngles);
// roboid->perception->AddTrackedObject(this, objectType, worldPosition,
// worldOrientation);
// Sensor *sensor =
// roboid->perception->FindSensorOfType(0x82); // Lighthouse type
// sensor
// LighthouseTracker *lhSensor = (LighthouseTracker *)sensor;
// lhSensor->UpdateGeometry(objectId, worldPosition, worldOrientation);
}
}
@ -35,73 +171,3 @@ Vector3 NetworkPerception::ReceiveVector3(unsigned char *data, int startIndex) {
Vector3 v = Vector3(x, y, z);
return v;
}
void NetworkPerception::ReceiveObject(unsigned char *data, Roboid *roboid) {
unsigned char objectId = data[1];
// char poseType = data[2];
Vector3 worldPosition = ReceiveVector3(data, 3);
Vector3 worldAngles = ReceiveVector3(data, 15);
Quaternion worldOrientation = Quaternion::Euler(worldAngles);
Vector3 worldDirection = worldOrientation * Vector3::forward;
if (objectId == 0) {
roboid->SetPosition(worldPosition);
roboid->SetOrientation(worldOrientation);
// printf("Received pose (%f, %f) (%f, %f)\n", worldPosition.x,
// worldPosition.z, worldDirection.x, worldDirection.z);
} else {
Vector3 myPosition = roboid->GetPosition(); // Vector3::zero;
Vector2 myPosition2 = Vector3::ProjectHorizontalPlane(myPosition);
Quaternion myOrientation = roboid->GetOrientation();
Vector3 myDirection = myOrientation * Vector3::forward;
Vector2 myDirection2 = Vector3::ProjectHorizontalPlane(myDirection);
Vector3 localPosition =
Quaternion::Inverse(myOrientation) * (worldPosition - myPosition);
// float distance = Vector3::Distance(myPosition, worldPosition);
// float angle = Vector3::SignedAngle(myDirection, localPosition,
// Vector3::up);
float distance = Vector3::Magnitude(localPosition);
float angle =
Vector3::SignedAngle(Vector3::forward, localPosition, Vector3::up);
Polar position = Polar(angle, distance);
// int objCount = roboid->perception->TrackedObjectCount();
// printf("object received at (%f, %f)/(%f, %f) -> (%f %f)\n",
// worldPosition.x,
// worldPosition.z, localPosition.x, localPosition.z, distance,
// angle);
roboid->perception->AddTrackedObject(this, position);
}
}
// HACK!!!
// #include "../../../src/LighthouseTracker.h"
void NetworkPerception::ReceiveTypedObject(unsigned char *data,
Roboid *roboid) {
unsigned char objectType = data[1];
unsigned char objectId = data[2];
if (objectType == 0x02) { // lighthouse
// We require position and orientation for lighthouses
if (data[3] !=
(NetworkSync::Pose_Position | NetworkSync::Pose_Orientation)) {
return;
}
Vector3 worldPosition = ReceiveVector3(data, 4);
Vector3 worldAngles = ReceiveVector3(data, 16);
Quaternion worldOrientation = Quaternion::Euler(worldAngles);
// roboid->perception->AddTrackedObject(this, objectType, worldPosition,
// worldOrientation);
// Sensor *sensor =
// roboid->perception->FindSensorOfType(0x82); // Lighthouse type sensor
// LighthouseTracker *lhSensor = (LighthouseTracker *)sensor;
// lhSensor->UpdateGeometry(objectId, worldPosition, worldOrientation);
}
}

3
NetworkPerception.h
View File

@ -12,7 +12,8 @@ public:
void ProcessPacket(Roboid *roboid, unsigned char *buffer, int packetsize);
protected:
void ReceiveObject(unsigned char *data, Roboid *roboid);
void ReceiveCreateMsg(unsigned char *data, Roboid *roboid);
void ReceivePoseMsg(unsigned char *data, Roboid *roboid);
void ReceiveTypedObject(unsigned char *data, Roboid *roboid);
Int32 ReceiveInt32(unsigned char *data, int startIndex);

49
NetworkSync.cpp
View File

@ -9,6 +9,22 @@
#include "LinearAlgebra/Angle8.h"
#include "LinearAlgebra/Spherical.h"
void NetworkSync::ReceiveMessage(Roboid *roboid, unsigned char bytecount) {
networkPerception->ProcessPacket(roboid, buffer, bytecount);
switch (buffer[0]) {
case NetworkIdMsg:
// this->networkId = NetworkIdMsg;
ReceiveNetworkId();
break;
}
}
void NetworkSync::ReceiveNetworkId() {
this->networkId = buffer[1];
printf("Received network Id %d\n", this->networkId);
}
void NetworkSync::SendVector3(unsigned char *data, unsigned char *startIndex,
const Vector3 v) {
SendSingle100(data, *startIndex, v.x);
@ -118,12 +134,25 @@ void NetworkSync::SendInt32(unsigned char *data, unsigned int startIndex,
}
}
void NetworkSync::PublishTrackedObjects(SendBuffer sendBuffer,
// static unsigned char buffer[100];
void NetworkSync::SendBuffer(unsigned char bufferSize) {}
void NetworkSync::PublishClient() {
unsigned char ix = 0;
buffer[ix++] = ClientMsg;
buffer[ix++] = 0; // No network ID
SendBuffer(ix);
}
void NetworkSync::PublishTrackedObjects(Buffer sendBuffer,
InterestingThing **objects) {
for (unsigned char objIx = 0; objIx < Perception::maxObjectCount; objIx++) {
InterestingThing *obj = objects[objIx];
if (obj == nullptr)
continue;
if (obj->networkId != 0x00)
continue; // object is external
// if (obj->sensor->type == Thing::ExternalType)
// continue;
@ -133,7 +162,7 @@ void NetworkSync::PublishTrackedObjects(SendBuffer sendBuffer,
}
}
void NetworkSync::PublishTrackedObject(SendBuffer sendBuffer,
void NetworkSync::PublishTrackedObject(Buffer sendBuffer,
InterestingThing *object) {
Vector2 worldPosition2 =
Vector2::Rotate(Vector2::forward * object->position.distance,
@ -162,7 +191,7 @@ void NetworkSync::PublishTrackedObject(SendBuffer sendBuffer,
#endif
}
void NetworkSync::PublishRelativeObject(SendBuffer sendBuffer, UInt8 parentId,
void NetworkSync::PublishRelativeObject(Buffer sendBuffer, UInt8 parentId,
InterestingThing *object) {
const UInt16 bufferSize = 4 + 12;
UInt8 buffer[bufferSize] = {
@ -177,7 +206,7 @@ void NetworkSync::PublishRelativeObject(SendBuffer sendBuffer, UInt8 parentId,
sendBuffer(buffer, bufferSize);
}
void NetworkSync::SendPoseMsg(SendBuffer sendBuffer, Roboid *roboid) {
void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid *roboid) {
Polar velocity = roboid->propulsion->GetVelocity();
Vector2 worldVelocity2 =
Vector2::Rotate(Vector2::forward * velocity.distance, velocity.angle);
@ -214,8 +243,7 @@ void NetworkSync::SendPoseMsg(SendBuffer sendBuffer, Roboid *roboid) {
#endif
}
void NetworkSync::SendDestroyObject(SendBuffer sendBuffer,
InterestingThing *obj) {
void NetworkSync::SendDestroyObject(Buffer sendBuffer, InterestingThing *obj) {
#ifdef RC_DEBUG
Serial.print("Send Destroy ");
Serial.println((int)obj->id);
@ -223,4 +251,13 @@ void NetworkSync::SendDestroyObject(SendBuffer sendBuffer,
unsigned char buffer[2] = {DestroyMsg, (unsigned char)obj->id};
sendBuffer(buffer, 2);
#endif
}
void NetworkSync::SendInvestigateThing(InterestingThing *thing) {
// printf("Investigate [%d/%d]\n", thing->networkId, thing->id);
unsigned char ix = 0;
buffer[ix++] = InvestigateMsg;
buffer[ix++] = thing->networkId;
buffer[ix++] = thing->id;
SendBuffer(ix);
}

47
NetworkSync.h
View File

@ -11,6 +11,8 @@ namespace RoboidControl {
/// @brief Interface for synchronizaing state between clients across a network
class NetworkSync {
public:
unsigned char networkId;
/// @brief Retreive and send the roboid state
/// @param roboid The roboid for which the state is updated
virtual void NetworkUpdate(Roboid *roboid) = 0;
@ -19,39 +21,53 @@ public:
virtual void DestroyObject(InterestingThing *obj) = 0;
virtual void NewObject(InterestingThing *obj){};
/// @brief The id of a Pose message
static const char PoseMsg = 0x10;
static const char PoseTypeMsg = 0x11;
static const char RelativePoseMsg = 0x12;
static const unsigned char PoseMsg = 0x10;
static const unsigned char PoseTypeMsg = 0x11;
static const unsigned 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;
static const unsigned char Pose_Position = 0x01;
/// @brief A bit pattern for the pose, stating that this message contains an
/// orientation in world coordinates
static const char Pose_Orientation = 0x02;
static const unsigned char Pose_Orientation = 0x02;
/// @brief A bit pattern for the pose, stating that this messsage contains a
/// linear velocity in world coordinates
static const char Pose_LinearVelocity = 0x04;
static const unsigned char Pose_LinearVelocity = 0x04;
/// @brief A bit pattern for the pose, stating that this message contains an
/// angular velocity in world coordinates
static const char Pose_AngularVelocity = 0x08;
static const char DestroyMsg = 0x20;
static const unsigned char DestroyMsg = 0x20;
typedef void (*SendBuffer)(UInt8 *buffer, UInt16 bufferSize);
static const unsigned char AngVelocity2DMsg = 0x46;
static const unsigned char Position2DMsg = 0x61;
static const unsigned char Velocity2DMsg = 0x62;
static const unsigned char CreateMsg = 0x80;
static const unsigned char InvestigateMsg = 0x81;
static const unsigned char ClientMsg = 0xA0;
static const unsigned char NetworkIdMsg = 0xA1;
void SendPoseMsg(SendBuffer sendBuffer, Roboid *roboid);
void SendDestroyObject(SendBuffer sendBuffer, InterestingThing *obj);
typedef void (*Buffer)(UInt8 *buffer, UInt16 bufferSize);
void ReceiveMessage(Roboid *roboid, unsigned char bytecount);
void ReceiveNetworkId();
void SendInvestigateThing(InterestingThing *thing);
void SendPoseMsg(Buffer sendBuffer, Roboid *roboid);
void SendDestroyObject(Buffer sendBuffer, InterestingThing *obj);
void PublishNewObject();
void PublishTrackedObjects(SendBuffer sendBuffer, InterestingThing **objects);
void PublishTrackedObjects(Buffer sendBuffer, InterestingThing **objects);
virtual void SendPosition(Vector3 worldPosition){};
virtual void SendPose(Vector3 worldPosition, Quaternion worldOrientation){};
protected:
NetworkPerception *networkPerception;
void PublishTrackedObject(SendBuffer sendBuffer, InterestingThing *object);
void PublishRelativeObject(SendBuffer sendBuffer, UInt8 parentId,
void PublishTrackedObject(Buffer sendBuffer, InterestingThing *object);
void PublishRelativeObject(Buffer sendBuffer, UInt8 parentId,
InterestingThing *object);
void SendSingle100(unsigned char *data, unsigned int startIndex, float value);
@ -73,6 +89,11 @@ protected:
// void SendSpherical32(unsigned char *data, int startIndex, Spherical s);
void SendQuat32(unsigned char *data, unsigned char *startIndex,
const Quaternion q);
unsigned char buffer[32];
virtual void SendBuffer(unsigned char bufferSize);
void PublishClient();
};
} // namespace RoboidControl

39
Perception.cpp
View File

@ -163,6 +163,7 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position,
if (availableSlotIx < maxObjectCount) {
// a slot is available
this->trackedObjects[availableSlotIx] = obj;
obj->networkId = 0x00;
obj->id = availableSlotIx + 1;
#ifdef RC_DEBUG2
Serial.print((int)obj->id);
@ -176,6 +177,7 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position,
this->trackedObjects[farthestObjIx]->position.distance) {
delete this->trackedObjects[farthestObjIx];
this->trackedObjects[farthestObjIx] = obj;
obj->networkId = 0x00;
obj->id = availableSlotIx + 1;
#ifdef RC_DEBUG2
Serial.print((int)obj->id);
@ -228,6 +230,7 @@ InterestingThing *Perception::AddTrackedObject(Sensor *sensor,
if (availableSlotIx < maxObjectCount) {
// a slot is available
this->trackedObjects[availableSlotIx] = obj;
obj->networkId = 0x00;
obj->id = availableSlotIx + 1;
#ifdef RC_DEBUG2
Serial.print((int)obj->id);
@ -240,6 +243,7 @@ InterestingThing *Perception::AddTrackedObject(Sensor *sensor,
this->trackedObjects[farthestObjIx]->position.distance) {
delete this->trackedObjects[farthestObjIx];
this->trackedObjects[farthestObjIx] = obj;
obj->networkId = 0x00;
obj->id = availableSlotIx + 1;
#ifdef RC_DEBUG2
Serial.print((int)obj->id);
@ -257,6 +261,21 @@ InterestingThing *Perception::AddTrackedObject(Sensor *sensor,
}
}
InterestingThing *Perception::AddTrackedObject(Sensor *sensor,
unsigned char networkId,
unsigned char objectId,
Spherical position,
Quaternion orientation) {
InterestingThing *thing = FindTrackedObject(networkId, objectId);
if (thing == nullptr) {
thing = AddTrackedObject(sensor, position, orientation);
thing->networkId = networkId;
thing->id = objectId;
thing->type = 0xFF; // unknown
}
return thing;
}
InterestingThing *Perception::FindTrackedObject(char objectId) {
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
InterestingThing *thing = this->trackedObjects[objIx];
@ -269,6 +288,20 @@ InterestingThing *Perception::FindTrackedObject(char objectId) {
return nullptr;
}
InterestingThing *Perception::FindTrackedObject(unsigned char networkId,
unsigned char objectId) {
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
InterestingThing *thing = this->trackedObjects[objIx];
if (thing == nullptr)
continue;
if (thing->networkId == networkId && thing->id == objectId) {
return thing;
}
}
return nullptr;
}
unsigned char Perception::TrackedObjectCount() {
unsigned char objectCount = 0;
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
@ -282,9 +315,9 @@ InterestingThing **Perception::GetTrackedObjects() {
return this->trackedObjects;
}
unsigned char Perception::GetThingsOfType(unsigned char objectType,
InterestingThing *buffer[],
unsigned char bufferSize) {
unsigned char Perception::ThingsOfType(unsigned char objectType,
InterestingThing *buffer[],
unsigned char bufferSize) {
unsigned char thingCount = 0;
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
InterestingThing *thing = this->trackedObjects[objIx];

16
Perception.h
View File

@ -91,11 +91,19 @@ public:
AddTrackedObject(Sensor *sensor, Spherical position,
Quaternion orientation = Quaternion::identity);
InterestingThing *
AddTrackedObject(Sensor *sensor, unsigned char networkId,
unsigned char objectId, Spherical position,
Quaternion orientation = Quaternion::identity);
InterestingThing *FindTrackedObject(char objectId);
InterestingThing *FindTrackedObject(unsigned char networkId,
unsigned char objectId);
/// @brief Retrieve the number of objects currently being tracked by the
/// roboid
/// @return The object of objects, which is always lower than maxObjectCount
/// @return The object of objects, which is always lower than
/// maxObjectCount
unsigned char TrackedObjectCount();
/// @brief Retreive the objects currently tracked by the roboid
/// @return An array of current objects
@ -104,9 +112,9 @@ public:
/// currently being tracked.
InterestingThing **GetTrackedObjects();
unsigned char GetThingsOfType(unsigned char objectType,
InterestingThing *buffer[],
unsigned char bufferSize);
unsigned char ThingsOfType(unsigned char objectType,
InterestingThing *buffer[],
unsigned char bufferSize);
InterestingThing *GetMostInterestingThing();

3
TrackedObject.h
View File

@ -58,8 +58,9 @@ public:
/// coordinated position->angle = 30 and position->angle = 27 can be the same.
static constexpr float equalityAngle = 5.0F;
unsigned char networkId;
/// @brief The id of the tracked object
char id;
unsigned char id;
char parentId = 0;
/// @brief The current position of the object