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

Added thing test, but not correcrt results yet

Browse Source
This commit is contained in:
Pascal Serrarens 2025-01-03 23:06:28 +01:00
parent 4c9018d451
commit a8afeeb141
4 changed files with 71 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
Participant.cpp
View File

@ -341,12 +341,15 @@ void Participant::Process(Participant *sender, NetworkIdMsg *msg) {
<< " " << (int)msg->networkId << "\n"; << " " << (int)msg->networkId << "\n";
if (this->networkId != msg->networkId) { if (this->networkId != msg->networkId) {
this->networkId = msg->networkId; this->networkId = msg->networkId;
Thing **allThings = Thing::GetAllThings(); // Thing **allThings = Thing::GetAllThings();
for (uint16_t ix = 0; ix < THING_STORE_SIZE; ix++) { // for (uint16_t ix = 0; ix < THING_STORE_SIZE; ix++) {
Thing *thing = allThings[ix]; // Thing *thing = allThings[ix];
if (thing == nullptr) // if (thing == nullptr)
continue; // continue;
// sender->SendThingInfo(thing);
// }
for (Thing* thing : allThings) {
sender->SendThingInfo(thing); sender->SendThingInfo(thing);
} }
} }

62
Thing.cpp
View File

@ -5,8 +5,8 @@
#include <string.h> #include <string.h>
Thing::Thing(unsigned char networkId, unsigned char thingType) { Thing::Thing(unsigned char networkId, unsigned char thingType) {
this->position = Spherical16::zero; // this->position = Spherical16::zero;
this->orientation = SwingTwist16::identity; // this->orientation = SwingTwist16::identity;
this->type = thingType; this->type = thingType;
this->networkId = networkId; this->networkId = networkId;
@ -152,44 +152,60 @@ Spherical16 Thing::GetLinearVelocity() { return this->linearVelocity; }
Spherical16 Thing::GetAngularVelocity() { return this->angularVelocity; } Spherical16 Thing::GetAngularVelocity() { return this->angularVelocity; }
// All things // All things
Thing *Thing::allThings[THING_STORE_SIZE] = {nullptr}; // Thing *Thing::allThings[THING_STORE_SIZE] = {nullptr};
Thing **Passer::Control::Thing::GetAllThings() { return allThings; } // Thing **Passer::Control::Thing::GetAllThings() { return allThings; }
Thing *Thing::Get(unsigned char networkId, unsigned char thingId) { Thing *Thing::Get(unsigned char networkId, unsigned char thingId) {
for (uint16_t ix = 0; ix < THING_STORE_SIZE; ix++) { // for (uint16_t ix = 0; ix < THING_STORE_SIZE; ix++) {
Thing *thing = allThings[ix]; // Thing *thing = allThings[ix];
if (thing == nullptr) // if (thing == nullptr)
continue; // continue;
if (thing->networkId == networkId && thing->id == thingId) // if (thing->networkId == networkId && thing->id == thingId)
return thing; // return thing;
} // }
return nullptr; // return nullptr;
std::find_if(allThings.begin(), allThings.end(), [networkId, thingId](Thing* thing){
return thing->networkId == networkId && thing->id == thingId;
});
} }
int Thing::Add(Thing *newThing) { int Thing::Add(Thing *newThing) {
// Exclude 0 because that value is reserved for 'no thing' // Exclude 0 because that value is reserved for 'no thing'
for (uint16_t ix = 1; ix < THING_STORE_SIZE; ix++) { // for (uint16_t ix = 1; ix < THING_STORE_SIZE; ix++) {
Thing *thing = allThings[ix]; // Thing *thing = allThings[ix];
if (thing == nullptr) { // if (thing == nullptr) {
allThings[ix] = newThing; // allThings[ix] = newThing;
// std::cout << " Add new thing " << (int)ix << "\n"; // std::cout << " Add new thing " << (int)ix << "\n";
return ix; // return ix;
} // }
// }
for (Thing* thing : allThings) {
if (thing == newThing)
return thing->id;
} }
return -1; allThings.push_back(newThing);
} }
void Thing::Remove(Thing *thing) { void Thing::Remove(Thing *thing) {
// std::cout << " remove " << (int)thing->id << "\n"; // std::cout << " remove " << (int)thing->id << "\n";
allThings[thing->id] = nullptr; // allThings[thing->id] = nullptr;
allThings.remove_if([thing](Thing* obj) { return obj == thing; });
} }
void Thing::UpdateAll(unsigned long currentTimeMs) { void Thing::UpdateAll(unsigned long currentTimeMs) {
// Not very efficient, but it works for now. // Not very efficient, but it works for now.
for (uint16_t ix = 0; ix < THING_STORE_SIZE; ix++) { // for (uint16_t ix = 0; ix < THING_STORE_SIZE; ix++) {
Thing *thing = allThings[ix]; // Thing *thing = allThings[ix];
// if (thing != nullptr &&
// thing->parent == nullptr) { // update all root things
// // std::cout << " update " << (int)ix << " thingid " << (int)thing->id
// // << "\n";
// thing->Update(currentTimeMs);
// }
// }
for (Thing* thing : allThings) {
if (thing != nullptr && if (thing != nullptr &&
thing->parent == nullptr) { // update all root things thing->parent == nullptr) { // update all root things
// std::cout << " update " << (int)ix << " thingid " << (int)thing->id // std::cout << " update " << (int)ix << " thingid " << (int)thing->id

9
Thing.h
View File

@ -2,6 +2,7 @@
#include "LinearAlgebra/Spherical.h" #include "LinearAlgebra/Spherical.h"
#include "LinearAlgebra/SwingTwist.h" #include "LinearAlgebra/SwingTwist.h"
#include <iostream> #include <iostream>
#include <list>
namespace Passer { namespace Passer {
namespace Control { namespace Control {
@ -123,16 +124,18 @@ protected:
//------------ All things //------------ All things
public: public:
static Thing **GetAllThings(); // static Thing **GetAllThings();
static Thing *Get(unsigned char networkId, unsigned char thingId); static Thing *Get(unsigned char networkId, unsigned char thingId);
static int Add(Thing *thing); static int Add(Thing *thing);
static void Remove(Thing *thing); static void Remove(Thing *thing);
static void UpdateAll(unsigned long currentTimeMs); static void UpdateAll(unsigned long currentTimeMs);
private: //private:
static Thing *allThings[]; // static Thing *allThings[];
}; };
static std::list<Thing *> allThings;
} // namespace Control } // namespace Control
} // namespace Passer } // namespace Passer
using namespace Passer::Control; using namespace Passer::Control;

18
test/thing_test.cc
View File

@ -77,5 +77,23 @@ TEST_F(ControlCoreSuite, SiteParticipant) {
ASSERT_EQ(1, 1); ASSERT_EQ(1, 1);
} }
TEST_F(ControlCoreSuite, Thing) {
SiteServer site = SiteServer(7681);
Participant participant = Participant("127.0.0.1", 7681);
Thing thing = Thing();
unsigned long milliseconds = get_time_ms();
unsigned long startTime = milliseconds;
while (milliseconds < startTime + 1000) {
site.Update(milliseconds);
participant.Update(milliseconds);
std::this_thread::sleep_for(std::chrono::milliseconds(100));
milliseconds = get_time_ms();
}
ASSERT_EQ(1, 1);
}
} // namespace Passer } // namespace Passer
#endif #endif