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

ControlCore compatibility

Browse Source
This commit is contained in:
Pascal Serrarens 2024-12-10 13:03:09 +01:00
parent 181deae258
commit f05ab5c742
11 changed files with 141 additions and 89 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
DirectionalSensor.cpp
View File

@ -2,7 +2,7 @@
#include "Messages.h"
DirectionalSensor::DirectionalSensor() {
DirectionalSensor::DirectionalSensor() : Sensor() {
this->type = DirectionalSensor::Type;
this->vector = Spherical16::zero;
}

8
DistanceSensor.cpp
View File

@ -15,10 +15,12 @@ DistanceSensor::DistanceSensor(float triggerDistance) : DistanceSensor() {
}
float DistanceSensor::GetDistance() {
if (distance < minRange || distance > maxRange)
return -1; // invalid distance
if (this->distance < minRange || this->distance > maxRange)
return INFINITY; // invalid distance
return distance;
float d = this->distance;
this->distance = INFINITY;
return d;
};
bool DistanceSensor::ObjectNearby() {

7
Messages.cpp
View File

@ -14,7 +14,7 @@ Angle8 Messages::ReceiveAngle8(unsigned char *buffer,
float Messages::ReceiveFloat16(unsigned char *buffer,
unsigned char *startIndex) {
unsigned char ix = *startIndex;
unsigned short value = buffer[ix++] << 8 | buffer[ix];
unsigned short value = buffer[ix++] << 8 | buffer[ix++];
float16 f = float16();
f.setBinary(value);
@ -27,10 +27,10 @@ Spherical16 Messages::ReceiveSpherical16(unsigned char *buffer,
float distance = ReceiveFloat16(buffer, startIndex);
Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
Angle16 horizontal = Angle16::Binary(horizontal8.GetBinary() * 255);
Angle16 horizontal = Angle16::Binary(horizontal8.GetBinary() * 256);
Angle8 vertical8 = ReceiveAngle8(buffer, startIndex);
Angle16 vertical = Angle16::Binary(vertical8.GetBinary() * 255);
Angle16 vertical = Angle16::Binary(vertical8.GetBinary() * 256);
Spherical16 s = Spherical16(distance, horizontal, vertical);
return s;
@ -63,6 +63,7 @@ void Messages::SendPolar(unsigned char *buffer, unsigned char *startIndex,
void Messages::SendSpherical16(unsigned char *buffer, unsigned char *startIndex,
Spherical16 s) {
// Receive first does distance...
SendAngle8(buffer, (*startIndex)++, s.direction.horizontal.InDegrees());
SendAngle8(buffer, (*startIndex)++, s.direction.vertical.InDegrees());
SendFloat16(buffer, startIndex, s.distance);

40
NetworkPerception.cpp
View File

@ -16,8 +16,8 @@ void NetworkPerception::ProcessPacket(Roboid *roboid, unsigned char *buffer,
#endif
switch (buffer[0]) {
case NetworkSync::CreateMsg:
ReceiveCreateMsg(buffer, roboid);
case NetworkSync::ThingMsg:
ReceiveThingMsg(buffer, roboid);
break;
case NetworkSync::InvestigateMsg:
ReceiveInvestigateMsg(buffer, roboid);
@ -31,7 +31,7 @@ void NetworkPerception::ProcessPacket(Roboid *roboid, unsigned char *buffer,
}
}
void NetworkPerception::ReceiveCreateMsg(unsigned char *data, Roboid *roboid) {
void NetworkPerception::ReceiveThingMsg(unsigned char *data, Roboid *roboid) {
unsigned char networkId = data[1];
unsigned char objectId = data[2];
unsigned char objectType = data[3];
@ -50,30 +50,33 @@ void NetworkPerception::ReceiveCreateMsg(unsigned char *data, Roboid *roboid) {
void NetworkPerception::ReceiveInvestigateMsg(unsigned char *data,
Roboid *roboid) {
unsigned char networkId = data[1];
unsigned char objectId = data[2];
unsigned char thingId = data[2];
#if RC_DEBUG
printf("Received InvestigateMsg [%d/%d]\n", networkId, objectId);
#endif
// #if RC_DEBUG
printf("Received InvestigateMsg [%d/%d]\n", networkId, thingId);
// #endif
if (networkId != roboid->networkSync->networkId)
// We only response to investigation requests for our own objects
return;
if (objectId == 0x00) {
if (thingId == 0x00) {
// They are investigating the roboid itself!
if (roboid->modelUrl != nullptr)
roboid->networkSync->SendModel(roboid);
// roboid->networkSync->SendModel(roboid);
roboid->networkSync->SendThingInfo(roboid);
} else {
InterestingThing *thing =
roboid->perception->FindTrackedObject(0x00, objectId);
if (thing == nullptr)
return;
Thing *thing = roboid->FindChild(thingId, true);
if (thing != nullptr)
roboid->networkSync->SendThingInfo(thing);
else {
InterestingThing *interestingThing =
roboid->perception->FindTrackedObject(0x00, thingId);
if (interestingThing == nullptr)
return;
// roboid->networkSync->NewObject(thing);
// roboid->networkSync->SendThing(thing);
// roboid->networkSync->SendModel(thing);
roboid->networkSync->SendThingInfo(thing, false);
roboid->networkSync->SendThingInfo(interestingThing, false);
}
}
}
@ -117,7 +120,8 @@ void NetworkPerception::ReceivePlane(unsigned char *data, Roboid *roboid) {
originPosition = roboid->worldOrigin->position;
}
Spherical16 position = originPosition + originOrientation * worldPosition;
// printf("Received plane (%f %f %f) (%f %f %f) %f %f %f\n", worldPosition.x,
// printf("Received plane (%f %f %f) (%f %f %f) %f %f %f\n",
// worldPosition.x,
// worldPosition.y, worldPosition.z, roboidPosition.x,
// roboidPosition.y, roboidPosition.z, position.distance,
// (float)position.horizontalAngle, (float)position.verticalAngle);

2
NetworkPerception.h
View File

@ -12,7 +12,7 @@ public:
void ProcessPacket(Roboid *roboid, unsigned char *buffer, int packetsize);
protected:
void ReceiveCreateMsg(unsigned char *data, Roboid *roboid);
void ReceiveThingMsg(unsigned char *data, Roboid *roboid);
void ReceiveInvestigateMsg(unsigned char *data, Roboid *roboid);
void ReceivePoseMsg(unsigned char *data, Roboid *roboid);
void ReceiveTypedObject(unsigned char *data, Roboid *roboid);

135
NetworkSync.cpp
View File

@ -24,50 +24,26 @@ NetworkSync::NetworkSync(Roboid *roboid) {
this->networkId = 0;
}
#include <Arduino.h>
void NetworkSync::ReceiveMessage(Roboid *roboid, unsigned char bytecount) {
// printf("Received msgId %d, length %d\n", buffer[0], bytecount);
networkPerception->ProcessPacket(roboid, buffer, bytecount);
switch (buffer[0]) {
case NetworkIdMsg:
ReceiveNetworkId();
break;
case BytesMsg:
ReceiveBytes();
case CustomMsg:
ReceiveCustom(bytecount);
break;
}
}
// Actually, this is a kind of investigate...
void NetworkSync::ReceiveNetworkId() {
this->networkId = buffer[1];
#ifdef RC_DEBUG
SERIALPORT.printf("Received network Id %d\n", this->networkId);
#endif
SendThingInfo(roboid, true);
}
void NetworkSync::ReceiveBytes() {
unsigned char ix = 1; // first byte is the msgId
unsigned char networkId = buffer[ix++];
unsigned char thingId = buffer[ix++];
unsigned char len = buffer[ix++];
unsigned char *bytes = new unsigned char[len];
// printf("Received %d bytes for [%d/%d]\n", len, networkId, thingId);
for (unsigned char bytesIx = 0; bytesIx < len; bytesIx++)
bytes[bytesIx] = buffer[ix++];
// networkId is not used right now, we assume networkId == 0
Thing *thing = roboid->FindChild(thingId);
// printf("Found thing %d\n", thing);
if (thing != nullptr)
thing->ProcessBytes(bytes);
delete bytes;
}
void NetworkSync::PublishClient() {
unsigned char ix = 0;
buffer[ix++] = DeviceMsg;
buffer[ix++] = ClientMsg;
buffer[ix++] = this->networkId;
PublishBuffer(ix);
@ -76,6 +52,49 @@ void NetworkSync::PublishClient() {
#endif
}
void NetworkSync::ReceiveNetworkId() {
this->networkId = buffer[1];
#ifdef RC_DEBUG
SERIALPORT.printf("Received network Id %d\n", this->networkId);
#endif
SendThingInfo(roboid, true);
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::ReceiveCustom(unsigned char packetSize) {
unsigned char ix = 1; // first byte is the msgId
unsigned char networkId = buffer[ix++];
unsigned char thingId = buffer[ix++];
// unsigned char len = buffer[ix++];
unsigned char len = packetSize - 3;
printf("Custom size = %d\n", len);
if (len > 0) {
unsigned char *bytes = new unsigned char[len];
// printf("Received %d bytes for [%d/%d]\n", len, networkId, thingId);
for (unsigned char bytesIx = 0; bytesIx < len; bytesIx++)
bytes[bytesIx] = buffer[ix++];
// networkId is not used right now, we assume networkId == 0
Thing *thing = roboid->FindChild(thingId);
printf("Found thing %s\n", thing->name);
if (thing != nullptr)
thing->ProcessBytes(bytes);
delete bytes;
}
}
void NetworkSync::PublishState(Roboid *roboid) {
SendPose(roboid);
PublishPerception(roboid);
@ -85,6 +104,8 @@ void NetworkSync::SendThingInfo(Thing *thing, bool recurse) {
SendThing(thing);
SendName(thing);
SendModel(thing);
SendCustom(thing);
SendPose(thing, true, false);
if (recurse) {
for (unsigned char childIx = 0; childIx < thing->childCount; childIx++) {
@ -100,9 +121,10 @@ void NetworkSync::SendThing(Thing *thing) {
return;
unsigned char ix = 0;
buffer[ix++] = CreateMsg;
buffer[ix++] = ThingMsg;
buffer[ix++] = this->networkId;
buffer[ix++] = thing->id;
buffer[ix++] = thing->type;
buffer[ix++] = (unsigned char)thing->type;
Thing *parentThing = thing->GetParent();
if (parentThing != nullptr)
buffer[ix++] = parentThing->id;
@ -111,7 +133,7 @@ void NetworkSync::SendThing(Thing *thing) {
SendBuffer(ix);
#ifdef RC_DEBUG
printf("Sent Thing [%d/%d] %d\n", networkId, thing->id, thing->type);
printf("Sent Thing [%d/%d] %d\n", networkId, thing->id, (byte)thing->type);
#endif
}
@ -125,6 +147,7 @@ void NetworkSync::SendName(Thing *thing) {
unsigned char ix = 0;
buffer[ix++] = NameMsg;
buffer[ix++] = this->networkId;
buffer[ix++] = thing->id;
buffer[ix++] = len;
@ -149,9 +172,10 @@ void NetworkSync::SendModel(Thing *thing) {
unsigned char ix = 0;
buffer[ix++] = ModelMsg;
buffer[ix++] = thing->id; // objectId
Spherical16 s = Spherical16::zero; // Spherical(thing->modelPosition);
Messages::SendSpherical16(buffer, &ix, s);
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;
@ -161,12 +185,24 @@ void NetworkSync::SendModel(Thing *thing) {
SendBuffer(ix);
}
void NetworkSync::SendCustom(Thing *thing) {
unsigned char ix = 0;
buffer[ix++] = NetworkSync::CustomMsg;
buffer[ix++] = this->networkId;
buffer[ix++] = thing->id;
thing->SendBytes(buffer, &ix);
if (ix > 3) // When ix <= 3 then there is no custom data
SendBuffer(ix);
}
void NetworkSync::SendDestroyThing(InterestingThing *thing) {
if (networkId == 0) // We're not connected to a site yet
return;
unsigned char ix = 0;
buffer[ix++] = DestroyMsg;
buffer[ix++] = this->networkId;
buffer[ix++] = thing->id;
SendBuffer(ix);
@ -175,20 +211,21 @@ void NetworkSync::SendDestroyThing(InterestingThing *thing) {
#endif
}
void NetworkSync::SendPose(Thing *thing, bool recurse) {
void NetworkSync::SendPose(Thing *thing, bool force, bool recurse) {
if (this->networkId == 0) // We're not connected to a site yet
return;
thing->positionUpdated |= thing->GetLinearVelocity().distance > 0;
thing->orientationUpdated |= thing->GetAngularVelocity().distance > 0;
if (thing->positionUpdated || thing->orientationUpdated) {
if (force || thing->positionUpdated || thing->orientationUpdated) {
unsigned char ix = 0;
buffer[ix++] = PoseMsg;
buffer[ix++] = this->networkId;
buffer[ix++] = thing->id;
unsigned char pattern = 0;
if (thing->positionUpdated)
if (force || thing->positionUpdated)
pattern |= Pose_Position;
if (thing->orientationUpdated)
if (force || thing->orientationUpdated)
pattern |= Pose_Orientation;
buffer[ix++] = pattern;
Messages::SendSpherical16(buffer, &ix, thing->position);
@ -214,7 +251,7 @@ void NetworkSync::SendPose(Thing *thing, bool recurse) {
for (unsigned char childIx = 0; childIx < thing->childCount; childIx++) {
Thing *child = thing->GetChild(childIx);
if (child != nullptr)
SendPose(thing->GetChild(childIx), true);
SendPose(thing->GetChild(childIx), force, recurse);
}
}
}
@ -299,7 +336,8 @@ void NetworkSync::PublishTrackedObject(Roboid *roboid,
inv_originOrientation * (thing->position - originPosition);
unsigned char ix = 0;
buffer[ix++] = PoseMsg; // Position2DMsg;
buffer[ix++] = PoseMsg; // Position2DMsg;
buffer[ix++] = this->networkId;
buffer[ix++] = thing->id; // objectId;
buffer[ix++] = Pose_Position | Pose_Orientation;
Messages::SendSpherical16(buffer, &ix, worldPosition);
@ -313,17 +351,6 @@ void NetworkSync::PublishTrackedObject(Roboid *roboid,
thing->updated = false;
}
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::SendText(const char *s) {
unsigned char length;
for (length = 0; length < 253; length++) {

11
NetworkSync.h
View File

@ -27,6 +27,7 @@ public:
void SendThing(Thing *thing);
void SendName(Thing *roboid);
void SendModel(Thing *thing);
void SendCustom(Thing *thing);
/// @brief The id of a Pose message
static const unsigned char PoseMsg = 0x10;
@ -51,13 +52,13 @@ public:
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 ThingMsg = 0x80;
static const unsigned char InvestigateMsg = 0x81;
static const unsigned char ModelMsg = 0x90;
static const unsigned char NameMsg = 0x91;
static const unsigned char DeviceMsg = 0xA0;
static const unsigned char ClientMsg = 0xA0;
static const unsigned char NetworkIdMsg = 0xA1;
static const unsigned char BytesMsg = 0xB1;
static const unsigned char CustomMsg = 0xB1;
typedef void (*Buffer)(UInt8 *buffer, UInt16 bufferSize);
@ -74,7 +75,7 @@ public:
virtual void SendPose(Spherical16 worldPosition,
SwingTwist16 worldOrientation) {};
// void SendPose(Roboid* roboid, bool recurse = true);
void SendPose(Thing *thing, bool recurse = true);
void SendPose(Thing *thing, bool force = 0, bool recurse = true);
virtual void SendText(const char *s);
void SendInt(const int x);
@ -86,7 +87,7 @@ protected:
NetworkPerception *networkPerception;
void ReceiveNetworkId();
void ReceiveBytes();
void ReceiveCustom(unsigned char packetSize);
void PublishState(Sensor *sensor);

8
Roboid.cpp
View File

@ -22,6 +22,7 @@ Roboid::Roboid() : Thing(0) {
this->networkSync = nullptr;
this->position = Spherical16::zero;
this->orientation = SwingTwist16::identity;
this->lastUpdateTimeMs = 0;
}
Roboid::Roboid(Propulsion *propulsion) : Roboid() {
@ -31,8 +32,11 @@ Roboid::Roboid(Propulsion *propulsion) : Roboid() {
}
void Roboid::Update(unsigned long currentTimeMs) {
if (perception != nullptr)
perception->Update(currentTimeMs);
if (this->lastUpdateTimeMs == 0)
this->lastUpdateTimeMs = currentTimeMs;
// if (perception != nullptr)
// perception->Update(currentTimeMs);
if (propulsion != nullptr) {
propulsion->Update(currentTimeMs);

2
Sensor.cpp
View File

@ -2,7 +2,7 @@
#include "Roboid.h"
Sensor::Sensor() : Thing(0) { // for now, id should be set properly later
Sensor::Sensor() : Thing() { // for now, id should be set properly later
this->type = Thing::SensorType;
}

10
Thing.cpp
View File

@ -4,6 +4,16 @@
using namespace Passer::RoboidControl;
int Thing::lastThingId = 1;
Thing::Thing() {
this->id = lastThingId++;
this->type = (unsigned int)Type::Undetermined;
this->childCount = 0;
this->parent = nullptr;
this->children = nullptr;
}
Thing::Thing(unsigned char id) : id(id) {
// this->position = SphericalOf<signed short>::zero;
this->type = (unsigned int)Type::Undetermined;

5
Thing.h
View File

@ -11,6 +11,7 @@ namespace RoboidControl {
/// @brief A thing is a functional component on a robot
class Thing {
public:
Thing();
/// @brief Default constructor for a Thing
Thing(unsigned char id);
@ -108,9 +109,11 @@ public:
const char *modelUrl = nullptr;
float modelScale = 1;
virtual void ProcessBytes(unsigned char *bytes) {}
virtual void SendBytes(unsigned char *buffer, unsigned char *ix) {};
virtual void ProcessBytes(unsigned char *bytes) {};
protected:
static int lastThingId;
/// @brief Bitmask for Motor type
static const unsigned int MotorType = 0x8000;
/// @brief Bitmap for Sensor type