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

Extended the ControlCore

Browse Source
This commit is contained in:
Pascal Serrarens 2024-12-11 18:01:06 +01:00
parent 14e3499742
commit 7e5876e2f8
9 changed files with 371 additions and 160 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ControlCore/Client.cpp Normal file
View File

@ -0,0 +1,6 @@
#include "Client.h"
bool Passer::Control::ControlClient::SendMsg(unsigned char *buffer,
unsigned char bufferSize) {
return false;
}

11
ControlCore/Client.h Normal file
View File

@ -0,0 +1,11 @@
#pragma once
namespace Passer::Control {
class ControlClient {
public:
unsigned char *buffer;
bool SendMsg(unsigned char *buffer, unsigned char bufferSize);
};
} // namespace Passer::Control

12
ControlCore/LowLevelMessages.cpp Normal file
View File

@ -0,0 +1,12 @@
#include "LowLevelMessages.h"
#include "../float16/float16.h"
void LowLevelMessages::SendFloat16(unsigned char *buffer, unsigned char *ix,
float value) {
float16 value16 = float16(value);
short binary = value16.getBinary();
buffer[(*ix)++] = (binary >> 8) & 0xFF;
buffer[(*ix)++] = binary & 0xFF;
}

12
ControlCore/LowLevelMessages.h Normal file
View File

@ -0,0 +1,12 @@
#include "../LinearAlgebra/Spherical.h"
namespace Passer::Control {
class LowLevelMessages {
public:
static void SendFloat16(unsigned char *buffer, unsigned char *ix,
float value);
};
} // namespace Passer::Control
using namespace Passer::Control;

125
ControlCore/Messages.cpp
View File

@ -1,39 +1,128 @@
#include "Messages.h"
#include "LowLevelMessages.h"
#pragma region IMessage
unsigned char *IMessage::Serialize() { return nullptr; }
unsigned char IMessage::Serialize(unsigned char *buffer) { return 0; }
bool IMessage::SendMsg(IMessage msg, unsigned char bufferSize) {
return SendMsg(msg.Serialize(), bufferSize);
bool IMessage::SendMsg(ControlClient client, IMessage msg,
unsigned char bufferSize) {
return SendMsg(client, client.buffer, msg.Serialize(client.buffer));
}
bool IMessage::SendMsg(unsigned char *buffer, unsigned char bufferSize) {
// SendBuffer(buffer, bufferSize);
return false;
bool IMessage::SendMsg(ControlClient client, unsigned char *buffer,
unsigned char bufferSize) {
return false; // client.SendMsg(buffer, bufferSize);
}
#pragma endregion
#pragma region Investigate
InvestigateMsg::InvestigateMsg(unsigned char thingId) {
this->networkId = 0;
InvestigateMsg::InvestigateMsg(unsigned char networkId, unsigned char thingId) {
this->networkId = networkId;
this->thingId = thingId;
}
unsigned char *InvestigateMsg::Serialize() {
unsigned char *buffer = IMessage::buffer;
buffer[0] = InvestigateMsg::id;
buffer[1] = this->networkId;
buffer[2] = this->thingId;
return buffer;
unsigned char InvestigateMsg::Serialize(unsigned char *buffer) {
unsigned char ix = 0;
buffer[ix++] = this->id;
buffer[ix++] = this->networkId;
buffer[ix++] = this->thingId;
return ix;
}
bool InvestigateMsg::Send(unsigned char thingId) {
InvestigateMsg msg = InvestigateMsg(thingId);
return SendMsg(msg, InvestigateMsg::length);
bool InvestigateMsg::Send(ControlClient client, unsigned char networkId,
unsigned char thingId) {
InvestigateMsg msg = InvestigateMsg(networkId, thingId);
return SendMsg(client, msg, InvestigateMsg::length);
}
// Investiaget
// Investigate
#pragma endregion
#pragma region Thing
Passer::Control::ThingMsg::ThingMsg(unsigned char networkId,
unsigned char thingId,
unsigned char thingType,
unsigned char parentId) {
this->networkId = networkId;
this->thingId = thingId;
this->thingType = thingType;
this->parentId = parentId;
}
unsigned char ThingMsg::Serialize(unsigned char *buffer) {
unsigned char ix = 0;
buffer[ix++] = this->id;
buffer[ix++] = this->networkId;
buffer[ix++] = this->thingId;
buffer[ix++] = this->thingType;
buffer[ix++] = this->parentId;
return ix;
}
bool Passer::Control::ThingMsg::Send(ControlClient client,
unsigned char networkId,
unsigned char thingId,
unsigned char thingType,
unsigned char parentId) {
ThingMsg msg = ThingMsg(networkId, thingId, thingType, parentId);
return SendMsg(client, msg, ThingMsg::length);
}
// Thing
#pragma endregion
#pragma region Name
NameMsg::NameMsg(unsigned char networkId, unsigned char thingId,
unsigned char nameLength, const char *name) {
this->networkId = networkId;
this->thingId = thingId;
this->nameLength = nameLength;
this->name = name;
}
unsigned char Passer::Control::NameMsg::Serialize(unsigned char *buffer) {
unsigned char ix = 0;
buffer[ix++] = this->id;
buffer[ix++] = this->networkId;
buffer[ix++] = this->thingId;
buffer[ix++] = this->nameLength;
for (int nameIx = 0; nameIx < this->nameLength; nameIx++)
buffer[ix++] = name[nameIx];
return ix;
}
// Name
#pragma endregion
#pragma region ModelUrl
ModelUrlMsg::ModelUrlMsg(unsigned char networkId, unsigned char thingId,
unsigned char urlLength, const char *url,
float scale) {
this->networkId = networkId;
this->thingId = thingId;
this->urlLength = urlLength;
this->url = url;
this->scale = scale;
}
unsigned char ModelUrlMsg::Serialize(unsigned char *buffer) {
unsigned char ix = 0;
buffer[ix++] = this->id;
buffer[ix++] = this->networkId;
buffer[ix++] = this->thingId;
LowLevelMessages::SendFloat16(buffer, &ix, this->scale);
buffer[ix++] = this->urlLength;
for (int urlIx = 0; urlIx < this->urlLength; urlIx++)
buffer[ix++] = url[urlIx];
return ix;
}
// Model Url
#pragma endregion

70
ControlCore/Messages.h
View File

@ -1,13 +1,19 @@
#pragma once
#include "../float16/float16.h"
#include "Client.h"
namespace Passer::Control {
class IMessage {
public:
static unsigned char buffer[256];
// static unsigned char buffer[256];
virtual unsigned char *Serialize();
virtual unsigned char Serialize(unsigned char *buffer);
static bool SendMsg(IMessage msg, unsigned char bufferSize);
static bool SendMsg(unsigned char *buffer, unsigned char bufferSize);
static bool SendMsg(ControlClient client, IMessage msg,
unsigned char bufferSize);
static bool SendMsg(ControlClient client, unsigned char *buffer,
unsigned char bufferSize);
};
class InvestigateMsg : public IMessage {
@ -17,11 +23,61 @@ public:
unsigned char networkId;
unsigned char thingId;
InvestigateMsg(unsigned char thingId);
InvestigateMsg(unsigned char networkId, unsigned char thingId);
virtual unsigned char *Serialize() override;
virtual unsigned char Serialize(unsigned char *buffer) override;
static bool Send(unsigned char thingId);
static bool Send(ControlClient client, unsigned char networkId,
unsigned char thingId);
};
class ThingMsg : public IMessage {
public:
static const unsigned char id = 0x80;
static const unsigned char length = 5;
unsigned char networkId;
unsigned char thingId;
unsigned char thingType;
unsigned char parentId;
ThingMsg(unsigned char networkId, unsigned char thingId,
unsigned char thingType, unsigned char parentId);
virtual unsigned char Serialize(unsigned char *buffer) override;
static bool Send(ControlClient client, unsigned char networkId,
unsigned char thingId, unsigned char thingType,
unsigned char parentId);
};
class NameMsg : public IMessage {
public:
static const unsigned char id = 0x91;
static const unsigned char length = 4;
unsigned char networkId;
unsigned char thingId;
unsigned char nameLength;
const char *name;
NameMsg(unsigned char networkId, unsigned char thingId,
unsigned char nameLength, const char *name);
virtual unsigned char Serialize(unsigned char *buffer) override;
};
class ModelUrlMsg : public IMessage {
public:
static const unsigned char id = 0x90;
unsigned char networkId;
unsigned char thingId;
float scale;
unsigned char urlLength;
const char *url;
ModelUrlMsg(unsigned char networkId, unsigned char thingId,
unsigned char urlLegth, const char *url, float scale = 1);
virtual unsigned char Serialize(unsigned char *buffer) override;
};
} // namespace Passer::Control

189
NetworkPerception.cpp
View File

@ -1,5 +1,6 @@
#include "NetworkPerception.h"
#include "ControlCore/Messages.h"
#include "NetworkSync.h"
#include "float16/float16.h"
@ -174,118 +175,120 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
if (networkId == roboid->networkSync->networkId)
networkId = 0x00;
if (objectId == 0x80)
return ReceivePlane(data, roboid);
else if (objectId == 0x81)
return ReceiveSphere(data, roboid);
// if (objectId == 0x80)
// return ReceivePlane(data, roboid);
// else if (objectId == 0x81)
// return ReceiveSphere(data, roboid);
#if RC_DEBUG
// printf("Received PoseMsg [%d/%d]\n", networkId, objectId);
#endif
InterestingThing *thing =
roboid->perception->FindTrackedObject(networkId, objectId);
if (thing == nullptr) {
thing = roboid->perception->AddTrackedObject(this, position, orientation,
0xFF, objectId, networkId);
if (thing->networkId != 0x00) {
// Unknown thing
roboid->networkSync->SendInvestigate(thing);
}
}
// InterestingThing *thing =
// roboid->perception->FindTrackedObject(networkId, objectId);
// if (thing == nullptr) {
// thing = roboid->perception->AddTrackedObject(this, position, orientation,
// 0xFF, objectId, networkId);
// if (thing->networkId != 0x00) {
// // Unknown thing
// InvestigateMsg::Send(thing);
// // roboid->networkSync->SendInvestigate(thing);
// }
// }
// SwingTwist16 roboidOrientation = roboid->GetOrientation();
Spherical16 position = Spherical16::zero;
SwingTwist16 orientation = SwingTwist16::identity;
// Spherical16 position = Spherical16::zero;
// SwingTwist16 orientation = SwingTwist16::identity;
Vector3 worldAngles = ReceiveVector3(data, 16);
SwingTwist16 worldOrientation = SwingTwist16(
Angle16::Degrees(worldAngles.Up()), Angle16::Degrees(worldAngles.Right()),
Angle16::Degrees(
worldAngles.Forward())); // Quaternion::Euler(worldAngles);
// Vector3 worldAngles = ReceiveVector3(data, 16);
// SwingTwist16 worldOrientation = SwingTwist16(
// Angle16::Degrees(worldAngles.Up()),
// Angle16::Degrees(worldAngles.Right()), Angle16::Degrees(
// worldAngles.Forward())); // Quaternion::Euler(worldAngles);
if ((poseType & NetworkSync::Pose_Orientation) != 0) {
if (objectId == 0) {
// roboid->SetOrientation(worldOrientation);
if (roboid->worldOrigin == nullptr) {
printf("creating new origin\n");
roboid->worldOrigin = new Thing(0);
printf("created\n");
}
// if ((poseType & NetworkSync::Pose_Orientation) != 0) {
// if (objectId == 0) {
// // roboid->SetOrientation(worldOrientation);
// if (roboid->worldOrigin == nullptr) {
// printf("creating new origin\n");
// roboid->worldOrigin = new Thing(0);
// printf("created\n");
// }
roboid->worldOrigin->orientation =
SwingTwist16::Inverse(worldOrientation);
} else {
// orientation = SwingTwist16::Inverse(roboidOrientation) *
// worldOrientation; if (thing != nullptr) {
// thing->orientation = orientation;
// }
SwingTwist16 originOrientation;
Spherical16 originPosition;
if (roboid->worldOrigin == nullptr) {
originOrientation = SwingTwist16::identity;
originPosition = Spherical16::zero;
} else {
originOrientation = roboid->worldOrigin->orientation;
originPosition = roboid->worldOrigin->position;
}
// roboid->worldOrigin->orientation =
// SwingTwist16::Inverse(worldOrientation);
// } else {
// // orientation = SwingTwist16::Inverse(roboidOrientation) *
// // worldOrientation; if (thing != nullptr) {
// // thing->orientation = orientation;
// // }
// SwingTwist16 originOrientation;
// Spherical16 originPosition;
// if (roboid->worldOrigin == nullptr) {
// originOrientation = SwingTwist16::identity;
// originPosition = Spherical16::zero;
// } else {
// originOrientation = roboid->worldOrigin->orientation;
// originPosition = roboid->worldOrigin->position;
// }
thing->orientation = originOrientation * worldOrientation;
}
}
// thing->orientation = originOrientation * worldOrientation;
// }
// }
if ((poseType & NetworkSync::Pose_Position) != 0) {
Vector3 worldVector3 = ReceiveVector3(data, 4);
Spherical16 worldPosition = Spherical16::FromVector3(worldVector3);
if (objectId == 0) {
// roboid->SetPosition(worldPosition);
if (roboid->worldOrigin == nullptr) {
printf("creating new origin again\n");
roboid->worldOrigin = new Thing(0);
}
// if ((poseType & NetworkSync::Pose_Position) != 0) {
// Vector3 worldVector3 = ReceiveVector3(data, 4);
// Spherical16 worldPosition = Spherical16::FromVector3(worldVector3);
// if (objectId == 0) {
// // roboid->SetPosition(worldPosition);
// if (roboid->worldOrigin == nullptr) {
// printf("creating new origin again\n");
// roboid->worldOrigin = new Thing(0);
// }
roboid->worldOrigin->position =
roboid->worldOrigin->orientation * -worldPosition;
} else {
SwingTwist16 originOrientation;
Spherical16 originPosition;
if (roboid->worldOrigin == nullptr) {
originOrientation = SwingTwist16::identity;
originPosition = Spherical16::zero;
} else {
originOrientation = roboid->worldOrigin->orientation;
originPosition = roboid->worldOrigin->position;
}
// roboid->worldOrigin->position =
// roboid->worldOrigin->orientation * -worldPosition;
// } else {
// SwingTwist16 originOrientation;
// Spherical16 originPosition;
// if (roboid->worldOrigin == nullptr) {
// originOrientation = SwingTwist16::identity;
// originPosition = Spherical16::zero;
// } else {
// originOrientation = roboid->worldOrigin->orientation;
// originPosition = roboid->worldOrigin->position;
// }
// SwingTwist16 roboidLocalOrientation =
// originOrientation * worldOrientation;
// // SwingTwist16 roboidLocalOrientation =
// // originOrientation * worldOrientation;
thing->position = originPosition + originOrientation * worldPosition;
// thing->position = originPosition + originOrientation * worldPosition;
/*
Spherical16 roboidPosition = roboid->GetPosition();
// /*
// Spherical16 roboidPosition = roboid->GetPosition();
// float distance = Vector3::Distance(roboidPosition, worldPosition);
// if (roboid->perception->IsInteresting(distance) == false) {
// // printf(" not interesting\n");
// return;
// }
// // float distance = Vector3::Distance(roboidPosition, worldPosition);
// // if (roboid->perception->IsInteresting(distance) == false) {
// // // printf(" not interesting\n");
// // return;
// // }
Spherical16 localPosition = SwingTwist16::Inverse(roboidOrientation) *
(worldPosition - roboidPosition);
Vector3 roboidVector3 = roboidPosition.ToVector3();
printf(" roboid position (%f %f %f)\n", roboidVector3.Right(),
roboidVector3.Up(), roboidVector3.Forward());
printf(" [%d/%d] worldPosition (%f %f %f) localPosition %f (%f %f)\n ",
networkId, objectId, worldVector3.Right(), worldVector3.Up(),
worldVector3.Forward(), localPosition.distance,
localPosition.direction.horizontal.InDegrees(),
localPosition.direction.vertical.InDegrees());
// Spherical16 localPosition = SwingTwist16::Inverse(roboidOrientation) *
// (worldPosition - roboidPosition);
// Vector3 roboidVector3 = roboidPosition.ToVector3();
// printf(" roboid position (%f %f %f)\n", roboidVector3.Right(),
// roboidVector3.Up(), roboidVector3.Forward());
// printf(" [%d/%d] worldPosition (%f %f %f) localPosition %f (%f %f)\n
// ",
// networkId, objectId, worldVector3.Right(), worldVector3.Up(),
// worldVector3.Forward(), localPosition.distance,
// localPosition.direction.horizontal.InDegrees(),
// localPosition.direction.vertical.InDegrees());
thing->position = localPosition;
*/
}
}
// thing->position = localPosition;
// */
// }
// }
}
void NetworkPerception::ReceiveTypedObject(unsigned char *data,

101
NetworkSync.cpp
View File

@ -12,6 +12,7 @@
#endif
#endif
#include "ControlCore/Messages.h"
#include "LinearAlgebra/Angle.h"
#include "LinearAlgebra/AngleUsing.h"
#include "LinearAlgebra/Spherical.h"
@ -61,16 +62,16 @@ void NetworkSync::ReceiveNetworkId() {
printf("completed\n");
}
void NetworkSync::SendInvestigate(InterestingThing *thing) {
unsigned char ix = 0;
buffer[ix++] = InvestigateMsg;
buffer[ix++] = thing->networkId;
buffer[ix++] = thing->id;
SendBuffer(ix);
#ifdef RC_DEBUG
printf("Sent Investigate [%d/%d]\n", thing->networkId, thing->id);
#endif
}
// void NetworkSync::SendInvestigate(InterestingThing *thing) {
// unsigned char ix = 0;
// buffer[ix++] = InvestigateMsg;
// buffer[ix++] = thing->networkId;
// buffer[ix++] = thing->id;
// SendBuffer(ix);
// #ifdef RC_DEBUG
// printf("Sent Investigate [%d/%d]\n", thing->networkId, thing->id);
// #endif
// }
void NetworkSync::ReceiveCustom(unsigned char packetSize) {
unsigned char ix = 1; // first byte is the msgId
@ -101,6 +102,9 @@ void NetworkSync::PublishState(Roboid *roboid) {
}
void NetworkSync::SendThingInfo(Thing *thing, bool recurse) {
if (thing == nullptr)
return;
SendThing(thing);
SendName(thing);
SendModel(thing);
@ -120,17 +124,23 @@ void NetworkSync::SendThing(Thing *thing) {
if (thing == nullptr)
return;
unsigned char ix = 0;
buffer[ix++] = ThingMsg;
buffer[ix++] = this->networkId;
buffer[ix++] = thing->id;
buffer[ix++] = (unsigned char)thing->type;
Thing *parentThing = thing->GetParent();
if (parentThing != nullptr)
buffer[ix++] = parentThing->id;
else
buffer[ix++] = 0x00;
SendBuffer(ix);
// unsigned char ix = 0;
// buffer[ix++] = ThingMsg;
// buffer[ix++] = this->networkId;
// buffer[ix++] = thing->id;
// buffer[ix++] = (unsigned char)thing->type;
// Thing *parentThing = thing->GetParent();
// if (parentThing != nullptr)
// buffer[ix++] = parentThing->id;
// else
// buffer[ix++] = 0x00;
// SendBuffer(ix);
Thing *parent = thing->GetParent();
Passer::Control::ThingMsg msg =
Passer::Control::ThingMsg(this->networkId, thing->id, thing->type,
parent == nullptr ? 0 : parent->id);
SendBuffer(msg.Serialize(this->buffer));
#ifdef RC_DEBUG
printf("Sent Thing [%d/%d] %d\n", networkId, thing->id, (byte)thing->type);
@ -145,16 +155,19 @@ void NetworkSync::SendName(Thing *thing) {
if (len > 255)
return;
unsigned char ix = 0;
buffer[ix++] = NameMsg;
buffer[ix++] = this->networkId;
buffer[ix++] = thing->id;
// unsigned char ix = 0;
// buffer[ix++] = NameMsg;
// buffer[ix++] = this->networkId;
// buffer[ix++] = thing->id;
buffer[ix++] = len;
for (unsigned char nameIx = 0; nameIx < len; nameIx++)
buffer[ix++] = thing->name[nameIx];
// buffer[ix++] = len;
// for (unsigned char nameIx = 0; nameIx < len; nameIx++)
// buffer[ix++] = thing->name[nameIx];
SendBuffer(ix);
// SendBuffer(ix);
Passer::Control::NameMsg msg =
Passer::Control::NameMsg(this->networkId, thing->id, len, thing->name);
SendBuffer(msg.Serialize(this->buffer));
#ifdef RC_DEBUG
SERIALPORT.printf("Sent Name [%d/%d] %s\n", networkId, buffer[1],
@ -170,19 +183,22 @@ void NetworkSync::SendModel(Thing *thing) {
if (len > 255)
return;
unsigned char ix = 0;
buffer[ix++] = ModelMsg;
buffer[ix++] = this->networkId;
buffer[ix++] = thing->id; // objectId
// Spherical16 s = Spherical16::zero; // Spherical(thing->modelPosition);
// Messages::SendSpherical16(buffer, &ix, s);
Messages::SendFloat16(buffer, &ix, thing->modelScale);
// unsigned char ix = 0;
// buffer[ix++] = ModelMsg;
// buffer[ix++] = this->networkId;
// buffer[ix++] = thing->id; // objectId
// // Spherical16 s = Spherical16::zero; // Spherical(thing->modelPosition);
// // Messages::SendSpherical16(buffer, &ix, s);
// Messages::SendFloat16(buffer, &ix, thing->modelScale);
buffer[ix++] = len;
for (int urlIx = 0; urlIx < len; urlIx++)
buffer[ix++] = thing->modelUrl[urlIx];
// buffer[ix++] = len;
// for (int urlIx = 0; urlIx < len; urlIx++)
// buffer[ix++] = thing->modelUrl[urlIx];
SendBuffer(ix);
// SendBuffer(ix);
Passer::Control::ModelUrlMsg msg = Passer::Control::ModelUrlMsg(
this->networkId, thing->id, len, thing->modelUrl, thing->modelScale);
SendBuffer(msg.Serialize(this->buffer));
}
void NetworkSync::SendCustom(Thing *thing) {
@ -392,4 +408,7 @@ void NetworkSync::SendInt(const int x) {
void NetworkSync::SendBuffer(unsigned char bufferSize) {}
void NetworkSync::PublishBuffer(unsigned char bufferSize) {}
void NetworkSync::PublishBuffer(unsigned char bufferSize) {}
void NetworkSync::SendCBuffer(unsigned char bufferSize, unsigned char *buffer) {
}

5
NetworkSync.h
View File

@ -1,5 +1,6 @@
#pragma once
#include "ControlCore/Messages.h"
#include "NetworkPerception.h"
#include "Perception.h"
#include "Roboid.h"
@ -66,7 +67,7 @@ public:
void PublishState(Roboid *roboid);
void SendInvestigate(InterestingThing *thing);
// void SendInvestigate(InterestingThing *thing);
void PublishPerception(Roboid *roboid);
void PublishTrackedObjects(Roboid *roboid, InterestingThing **objects);
@ -100,6 +101,8 @@ protected:
virtual void SendBuffer(unsigned char bufferSize);
virtual void PublishBuffer(unsigned char bufferSize);
virtual void SendCBuffer(unsigned char bufferSize, unsigned char *buffer);
void PublishClient();
};