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

Support Destroy and UpdateAll

Browse Source
This commit is contained in:
Pascal Serrarens 2024-12-17 12:09:30 +01:00
parent b97769624f
commit 9fb690a71b
12 changed files with 143 additions and 102 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
ControlCore/CoreThing.cpp
View File

@ -1,5 +1,6 @@
#include "CoreThing.h"
#include "Participant.h"
#include <iostream>
CoreThing::CoreThing(unsigned char networkId, unsigned char thingType) {
@ -8,6 +9,7 @@ CoreThing::CoreThing(unsigned char networkId, unsigned char thingType) {
this->Init();
int thingId = CoreThing::Add(this);
if (thingId < 0) {
std::cout << "ERROR: Thing store is full\n";
this->id = 0; // what to do when we cannot store any more things?
@ -15,15 +17,17 @@ CoreThing::CoreThing(unsigned char networkId, unsigned char thingType) {
this->id = thingId;
}
void CoreThing::Terminate() { CoreThing::Remove(this); }
void CoreThing::Init() {}
void CoreThing::SetName(const char *name) { this->name = name; }
CoreThing *CoreThing::GetParent() { return this->parent; }
// All things
CoreThing *CoreThing::allThings[256] = {nullptr};
CoreThing *CoreThing::allThings[THING_STORE_SIZE] = {nullptr};
CoreThing *CoreThing::Get(unsigned char networkId, unsigned char thingId) {
for (unsigned char ix = 0; ix < 256; ix++) {
for (uint16_t ix = 0; ix < THING_STORE_SIZE; ix++) {
CoreThing *thing = allThings[ix];
if (thing == nullptr)
continue;
@ -34,13 +38,27 @@ CoreThing *CoreThing::Get(unsigned char networkId, unsigned char thingId) {
}
int CoreThing::Add(CoreThing *newThing) {
for (unsigned char ix = 0; ix < 256; ix++) {
for (uint16_t ix = 0; ix < THING_STORE_SIZE; ix++) {
CoreThing *thing = allThings[ix];
if (thing == nullptr) {
allThings[ix] = newThing;
// std::cout << " Add new thing " << (int)ix << "\n";
return ix;
}
}
return -1;
}
void CoreThing::Remove(CoreThing *thing) { allThings[thing->id] = nullptr; }
void CoreThing::UpdateAll(unsigned long currentTimeMs) {
// Not very efficient, but it works for now.
for (uint16_t ix = 0; ix < THING_STORE_SIZE; ix++) {
CoreThing *thing = allThings[ix];
if (thing != nullptr &&
thing->parent == nullptr) { // update all root things
thing->Update(currentTimeMs);
}
}
}

36
ControlCore/CoreThing.h
View File

@ -1,19 +1,25 @@
#pragma once
#include <iostream>
namespace Passer {
namespace Control {
#define THING_STORE_SIZE 256
// IMPORTANT: values higher than 256 will need to change the CoreThing::id type
// to 16-bit or higher, breaking the networking protocol!
class CoreThing {
public:
// Participant *client;
unsigned char networkId;
/// @char The id of the thing
unsigned char id;
// CoreThing *parent;
CoreThing *parent;
/// @brief The type of Thing
unsigned char type;
const char *name = nullptr;
const char *modelUrl = nullptr;
float modelScale = 1;
// protected Sensor sensor;
/// @brief Basic Thing types
@ -36,20 +42,32 @@ public:
public:
CoreThing(unsigned char networkId = 0,
unsigned char thingType = (unsigned char)Type::Undetermined);
/// @brief Terminated thins are no longer updated
void Terminate();
void SetName(const char *name);
/// @brief Gets the parent Thing
/// @return The parent Thing
CoreThing *GetParent();
/// @brief Updates the state of the thing
/// @param currentTimeMs The current clock time in milliseconds
virtual void Update(unsigned long currentTimeMs) {};
virtual void SendBytes(unsigned char *buffer, unsigned char *ix) {};
// All things
private:
static CoreThing *allThings[];
static CoreThing *Get(unsigned char networkId, unsigned char thingId);
static int Add(CoreThing *thing);
virtual void ProcessBytes(unsigned char *bytes) {};
protected:
virtual void Init();
//------------ All things
public:
static CoreThing *Get(unsigned char networkId, unsigned char thingId);
static int Add(CoreThing *thing);
static void Remove(CoreThing *thing);
static void UpdateAll(unsigned long currentTimeMs);
private:
static CoreThing *allThings[];
};
} // namespace Control

21
ControlCore/Messages.cpp
View File

@ -26,7 +26,7 @@ unsigned char *IMessage::ReceiveMsg(unsigned char packetSize) {
bool IMessage::Publish(Participant *participant) {
return participant->PublishBuffer(Serialize(participant->buffer));
}
bool IMessage::Send(Participant *participant) {
bool IMessage::SendTo(Participant *participant) {
return participant->SendBuffer(Serialize(participant->buffer));
}
@ -251,7 +251,6 @@ CustomMsg::CustomMsg(unsigned char networkId, CoreThing *thing) {
this->thing = thing;
}
#include <iostream>
unsigned char CustomMsg::Serialize(unsigned char *buffer) {
unsigned char ix = this->length;
this->thing->SendBytes(buffer, &ix);
@ -271,3 +270,21 @@ CustomMsg CustomMsg::Receive(unsigned char *buffer, unsigned char bufferSize) {
// CustomMsg
#pragma endregion
#pragma region DestroyMsg
DestroyMsg::DestroyMsg(unsigned char networkId, CoreThing *thing) {
this->networkId = networkId;
this->thingId = thing->id;
}
unsigned char DestroyMsg::Serialize(unsigned char *buffer) {
unsigned char ix = 0;
buffer[ix++] = this->id;
buffer[ix++] = this->networkId;
buffer[ix++] = this->thingId;
return ix;
}
// DestroyMsg
#pragma endregion

14
ControlCore/Messages.h
View File

@ -18,7 +18,7 @@ public:
static unsigned char *ReceiveMsg(unsigned char packetSize);
bool Publish(Participant *participant);
bool Send(Participant *participant);
bool SendTo(Participant *participant);
};
class ClientMsg : public IMessage {
@ -147,6 +147,18 @@ public:
static CustomMsg Receive(unsigned char *buffer, unsigned char bufferSize);
};
class DestroyMsg : public IMessage {
public:
static const unsigned char id = 0x20;
static const unsigned length = 3;
unsigned char networkId;
unsigned char thingId;
DestroyMsg(unsigned char networkId, CoreThing *thing);
virtual unsigned char Serialize(unsigned char *buffer) override;
};
} // namespace Control
} // namespace Passer

64
NetworkSync.cpp
View File

@ -58,32 +58,17 @@ void NetworkSync::ReceiveNetworkId() {
}
void NetworkSync::ProcessInvestigateMsg(InvestigateMsg msg) {
// #if RC_DEBUG
#if RC_DEBUG
printf("Received InvestigateMsg [%d/%d]\n", msg.networkId, msg.thingId);
// #endif
#endif
if (networkId != roboid->networkSync->networkId)
// We only response to investigation requests for our own objects
if (msg.networkId != this->networkId)
// We only respond to investigation requests for our own objects
return;
if (msg.thingId == 0x00) {
// They are investigating the roboid itself!
if (roboid->modelUrl != nullptr)
// roboid->networkSync->SendModel(roboid);
roboid->networkSync->SendThingInfo(roboid);
} else {
Thing *thing = roboid->FindChild(msg.thingId, true);
if (thing != nullptr)
roboid->networkSync->SendThingInfo(thing);
else {
InterestingThing *interestingThing =
roboid->perception->FindTrackedObject(0x00, msg.thingId);
if (interestingThing == nullptr)
return;
roboid->networkSync->SendThingInfo(interestingThing, false);
}
}
Thing *thing = (Thing *)Thing::Get(0, msg.thingId);
if (thing != nullptr)
this->SendThingInfo(thing);
}
void NetworkSync::ProcessThingMsg(ThingMsg msg) {
@ -133,7 +118,7 @@ void NetworkSync::SendThing(Thing *thing) {
Thing *parent = thing->GetParent();
ThingMsg msg = ThingMsg(this->networkId, thing->id, thing->type,
parent == nullptr ? 0 : parent->id);
msg.Send(this);
msg.SendTo(this);
#ifdef RC_DEBUG
printf("Sent Thing [%d/%d] %d\n", networkId, thing->id, (byte)thing->type);
@ -149,7 +134,7 @@ void NetworkSync::SendName(Thing *thing) {
return;
NameMsg msg = NameMsg(this->networkId, thing->id, thing->name, len);
msg.Send(this);
msg.SendTo(this);
#ifdef RC_DEBUG
SERIALPORT.printf("Sent Name [%d/%d] %s\n", networkId, buffer[1],
@ -167,12 +152,20 @@ void NetworkSync::SendModel(Thing *thing) {
ModelUrlMsg msg = ModelUrlMsg(this->networkId, thing->id, len,
thing->modelUrl, thing->modelScale);
msg.Send(this);
msg.SendTo(this);
}
void NetworkSync::SendCustom(Thing *thing) {
CustomMsg msg = CustomMsg(this->networkId, thing);
msg.Send(this);
msg.SendTo(this);
}
void NetworkSync::SendDestroy(Thing *thing) {
DestroyMsg msg = DestroyMsg(this->networkId, thing);
msg.SendTo(this);
#if RC_DEBUG
printf("Sent DestroyMsg [%d/%d]\n", thing->networkId, thing->id);
#endif
}
#include <iostream>
@ -183,21 +176,6 @@ void NetworkSync::ProcessCustomMsg(CustomMsg msg) {
thing->ProcessBytes(msg.data);
}
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);
#if RC_DEBUG
printf("Sent DestroyMsg [%d/%d]\n", thing->networkId, thing->id);
#endif
}
void NetworkSync::SendPose(Thing *thing, bool force, bool recurse) {
if (this->networkId == 0) // We're not connected to a site yet
return;
@ -212,13 +190,13 @@ void NetworkSync::SendPose(Thing *thing, bool force, bool recurse) {
poseType |= PoseMsg::Pose_Orientation;
PoseMsg msg = PoseMsg(this->networkId, thing->id, poseType, thing->position,
thing->orientation);
msg.Send(this);
msg.SendTo(this);
thing->positionUpdated = false;
thing->orientationUpdated = false;
#if RC_DEBUG
if (thing->id == 0) {
if (thing->id != 0) {
Vector3 v = thing->position.ToVector3();
printf("Sent PoseMsg Thing [%d/%d] %f(%f)-(%f %f %f) %f\n",
this->networkId, thing->id, thing->position.distance,

9
NetworkSync.h
View File

@ -19,17 +19,14 @@ public:
/// @brief Retreive and send the roboid state
/// @param roboid The roboid for which the state is updated
virtual void NetworkUpdate(Roboid *roboid) = 0;
/// @brief Inform that the given object is no longer being tracked
/// @param obj
virtual void SendDestroyThing(InterestingThing *obj);
void SendThingInfo(Thing *thing, bool recurse = false);
void SendThing(Thing *thing);
void SendName(Thing *roboid);
void SendModel(Thing *thing);
void SendCustom(Thing *thing);
static const unsigned char DestroyMsg = 0x20;
/// @brief Inform that the given object is no longer being tracked
/// @param obj
void SendDestroy(Thing *thing);
typedef void (*Buffer)(UInt8 *buffer, UInt16 bufferSize);

2
Perception.cpp
View File

@ -481,7 +481,7 @@ void Perception::Update(unsigned long currentTimeMs) {
if (thing->DegradeConfidence(deltaTime) == false) {
// delete obj
if (roboid != nullptr && roboid->networkSync != nullptr) {
roboid->networkSync->SendDestroyThing(thing);
roboid->networkSync->SendDestroy(thing);
}
this->trackedObjects[objIx] = nullptr;
delete thing;

3
Sensor.cpp
View File

@ -8,7 +8,8 @@ Sensor::Sensor() : Thing() { // for now, id should be set properly later
void Sensor::SetParent(Thing *parent) {
this->parent = parent;
if (this->parent->IsRoboid()) {
if (this->parent->parent == nullptr) { // Is the parent a root object?
// Then it is a roboid
Roboid *roboidParent = (Roboid *)this->parent;
roboidParent->perception->AddSensor(this);
}

23
Thing.cpp
View File

@ -4,7 +4,7 @@
using namespace Passer::RoboidControl;
int Thing::lastThingId = 1;
// int Thing::lastThingId = 1;
Thing::Thing() : CoreThing() {
// this->id = lastThingId++;
@ -42,7 +42,7 @@ void Thing::SetParent(Thing *parent) {
parent->AddChild(this);
}
Thing *Thing::GetParent() { return this->parent; }
Thing *Thing::GetParent() { return (Thing *)this->parent; }
void Thing::AddChild(Thing *child) {
Thing **newChildren = new Thing *[this->childCount + 1];
@ -146,4 +146,21 @@ void Passer::RoboidControl::Thing::SetOrientation(SwingTwist16 orientation) {
Spherical16 Thing::GetLinearVelocity() { return this->linearVelocity; }
Spherical16 Thing::GetAngularVelocity() { return this->angularVelocity; }
Spherical16 Thing::GetAngularVelocity() { return this->angularVelocity; }
// Thing *Thing::Get(unsigned char networkId, unsigned char thingId) {
// CoreThing *coreThing = CoreThing::Get(networkId, thingId);
// // All CoreThings are Things in this context, so we can cast directly
// Thing *thing = (Thing *)coreThing;
// return thing;
// }
// int Thing::Add(Thing *thing) { return CoreThing::Add(thing); }
// void Thing::UpdateAll(unsigned long currentTimeMs) {
// for (unsigned char ix = 0; ix < allThingsSize; ix++) {
// Thing *thing = Get(0, ix);
// if (thing != nullptr)
// thing->Update(currentTimeMs);
// }
// }

13
Thing.h
View File

@ -93,20 +93,9 @@ public:
/// the only official supported model format is .obj
void SetModel(const char *url);
/// @brief Updates the state of the thing
/// @param currentTimeMs The current clock time in milliseconds
virtual void Update(unsigned long currentTimeMs) {}
unsigned char childCount = 0;
float modelScale = 1;
virtual void ProcessBytes(unsigned char *bytes) {
std::cout << "no processing\n";
};
protected:
static int lastThingId;
/// @brief Bitmask for Motor type
static const unsigned int MotorType = 0x8000;
/// @brief Bitmap for Sensor type
@ -115,7 +104,7 @@ protected:
static const unsigned int RoboidType = 0x2000;
static const unsigned char CustomType = 0x80;
Thing *parent = nullptr;
// Thing *parent = nullptr;
Thing **children = nullptr;
public:

28
TrackedObject.cpp
View File

@ -2,20 +2,21 @@
#include <math.h>
InterestingThing::InterestingThing(Sensor *sensor, Polar position) : Thing() {
this->id = 0;
this->confidence = maxConfidence;
this->sensor = sensor;
this->position = Spherical16(position.distance,
Angle16::Degrees(position.angle.InDegrees()),
Angle16::Degrees(0));
this->updated = true;
}
// InterestingThing::InterestingThing(Sensor *sensor, Polar position) : Thing()
// {
// this->id = 0;
// this->confidence = maxConfidence;
// this->sensor = sensor;
// this->position = Spherical16(position.distance,
// Angle16::Degrees(position.angle.InDegrees()),
// Angle16::Degrees(0));
// this->updated = true;
// }
InterestingThing::InterestingThing(Sensor *sensor, Spherical16 position,
SwingTwist16 orientation)
: Thing() {
this->id = 0;
// this->id = 0;
this->confidence = maxConfidence;
this->sensor = sensor;
this->position = position;
@ -73,13 +74,6 @@ bool InterestingThing::DegradeConfidence(float deltaTime) {
}
}
// void InterestingThing::Refresh(Polar position) {
// this->position =
// Spherical16(position.distance, Angle16(position.angle.ToFloat()), 0);
// this->confidence = maxConfidence;
// this->updated = true;
// }
void InterestingThing::Refresh(Spherical16 position, SwingTwist16 orientation) {
this->position = position;
this->orientation = orientation;

6
TrackedObject.h
View File

@ -17,7 +17,7 @@ public:
/// @brief An object tracked by the roboid
/// @param sensor The Sensor which detected this object
/// @param position The position in polar coordinates local to the roboid
InterestingThing(Sensor *sensor, Polar position);
// InterestingThing(Sensor *sensor, Polar position);
InterestingThing(Sensor *sensor, Spherical16 position,
SwingTwist16 orientation = SwingTwist16::identity);
@ -61,11 +61,11 @@ public:
/// coordinated position->angle = 30 and position->angle = 27 can be the same.
static constexpr float equalityAngle = 5.0F;
unsigned char networkId;
// unsigned char networkId;
/// @brief The id of the tracked object
// unsigned char id;
char parentId = 0;
// char parentId = 0;
/// @brief The current position of the object
// Spherical16 position = Spherical16::zero;
/// @brief The current orientation of the object