diff --git a/NetworkPerception.cpp b/NetworkPerception.cpp index 6d28c13..7a34290 100644 --- a/NetworkPerception.cpp +++ b/NetworkPerception.cpp @@ -62,7 +62,7 @@ void NetworkPerception::ReceiveInvestigateMsg(unsigned char* data, if (objectId == 0x00) { // They are investigating the roboid itself! if (roboid->modelUrl != nullptr) - roboid->networkSync->PublishModel(roboid); + roboid->networkSync->SendModel(roboid); } else { InterestingThing* thing = roboid->perception->FindTrackedObject(0x00, objectId); @@ -76,7 +76,7 @@ void NetworkPerception::ReceiveInvestigateMsg(unsigned char* data, void NetworkPerception::ReceivePlane(unsigned char* data, Roboid* roboid) { unsigned char networkId = data[1]; unsigned char poseType = data[3]; - if ((poseType & NetworkSync::Pose_Position) == 0 | + if ((poseType & NetworkSync::Pose_Position) == 0 || (poseType & NetworkSync::Pose_Orientation) == 0) // both position and orientation are required diff --git a/NetworkSync.cpp b/NetworkSync.cpp index b6050ad..b25e12e 100644 --- a/NetworkSync.cpp +++ b/NetworkSync.cpp @@ -1,6 +1,6 @@ #include "NetworkSync.h" -// #define RC_DEBUG 1 +#define RC_DEBUG 1 #ifdef RC_DEBUG #include @@ -30,12 +30,14 @@ void NetworkSync::ReceiveMessage(Roboid* roboid, unsigned char bytecount) { #include +// Actually, this is a kind of investigate... void NetworkSync::ReceiveNetworkId() { this->networkId = buffer[1]; #ifdef RC_DEBUG - printf("_Received network Id %d\n", this->networkId); + Serial.printf("_Received network Id %d\n", this->networkId); #endif - PublishModel(roboid); + SendName(roboid); + SendModel(roboid); for (unsigned char childIx = 0; childIx < roboid->childCount; childIx++) { Thing* child = roboid->GetChild(childIx); if (child != nullptr) @@ -43,13 +45,12 @@ void NetworkSync::ReceiveNetworkId() { } } -void NetworkSync::BroadcastState(Roboid *roboid) -{ +void NetworkSync::PublishState(Roboid* roboid) { // if (roboid->updated == false) // return; - SendPose(roboid); - BroadcastPerception(roboid); + // SendPose(roboid); + // PublishPerception(roboid); } void NetworkSync::NewObject(InterestingThing* thing) { @@ -83,7 +84,7 @@ void NetworkSync::PublishRelativeThing(Thing* thing, bool recurse) { SendSpherical16(buffer, &ix, thing->position); SendBuffer(ix); - PublishModel(thing); + SendModel(thing); if (recurse) { for (unsigned char childIx = 0; childIx < thing->childCount; childIx++) { @@ -94,7 +95,30 @@ void NetworkSync::PublishRelativeThing(Thing* thing, bool recurse) { } } -void NetworkSync::PublishModel(Roboid* roboid) { +void NetworkSync::SendName(Roboid* roboid) { + if (roboid->name == nullptr) + return; + + unsigned char len = strlen(roboid->name); + if (len > 255) + return; + + unsigned char ix = 0; + buffer[ix++] = NameMsg; + buffer[ix++] = 0x00; // objectId + + buffer[ix++] = len; + for (unsigned char nameIx = 0; nameIx < len; nameIx++) + buffer[ix++] = roboid->name[nameIx]; + + SendBuffer(ix); + +#ifdef RC_DEBUG + printf("Sent Name [%d/%d] %s\n", networkId, buffer[1], roboid->name); +#endif +} + +void NetworkSync::SendModel(Roboid* roboid) { if (roboid->modelUrl == nullptr) return; @@ -103,7 +127,7 @@ void NetworkSync::PublishModel(Roboid* roboid) { return; unsigned char ix = 0; - buffer[ix++] = 0x90; // modelMsg + buffer[ix++] = ModelMsg; buffer[ix++] = 0x00; // objectId Spherical s = Spherical::zero; //(roboid->modelPosition); SendSpherical(buffer, &ix, s); @@ -116,7 +140,7 @@ void NetworkSync::PublishModel(Roboid* roboid) { SendBuffer(ix); } -void NetworkSync::PublishModel(Thing* thing) { +void NetworkSync::SendModel(Thing* thing) { if (thing->modelUrl == nullptr) return; @@ -125,7 +149,7 @@ void NetworkSync::PublishModel(Thing* thing) { return; unsigned char ix = 0; - buffer[ix++] = 0x90; // modelMsg + buffer[ix++] = ModelMsg; buffer[ix++] = thing->id; // objectId Spherical s = Spherical::zero; // Spherical(thing->modelPosition); SendSpherical(buffer, &ix, s); @@ -210,26 +234,27 @@ void NetworkSync::PublishClient() { #endif } -void NetworkSync::BroadcastState(Sensor* sensor) { - unsigned char ix = 0; - buffer[ix++] = StateMsg; - buffer[ix++] = sensor->type; - float* value = (float*)sensor->GetValue(); - SendFloat16(buffer, &ix, *value); - SendBuffer(ix); +void NetworkSync::PublishState(Sensor* sensor) { + unsigned char ix = 0; + buffer[ix++] = StateMsg; + buffer[ix++] = sensor->type; + float* value = (float*)sensor->GetValue(); + SendFloat16(buffer, &ix, *value); + SendBuffer(ix); } -void NetworkSync::BroadcastPerception(Roboid *roboid) { +void NetworkSync::PublishPerception(Roboid* roboid) { Perception* perception = roboid->perception; if (perception == nullptr) return; - - for (unsigned int sensorIx = 0; sensorIx < perception->sensorCount; sensorIx++) { + + for (unsigned int sensorIx = 0; sensorIx < perception->sensorCount; + sensorIx++) { Sensor* sensor = perception->sensors[sensorIx]; if (sensor == nullptr) continue; - // sensor->BroadcastState(); - BroadcastState(sensor); + // sensor->PublishState(); + PublishState(sensor); } PublishTrackedObjects(roboid, roboid->perception->GetTrackedObjects()); } diff --git a/NetworkSync.h b/NetworkSync.h index f511707..cdebd38 100644 --- a/NetworkSync.h +++ b/NetworkSync.h @@ -23,8 +23,9 @@ class NetworkSync { /// @param obj virtual void SendDestroyThing(InterestingThing* obj); virtual void NewObject(InterestingThing* obj); - virtual void PublishModel(Roboid* obj); - void PublishModel(Thing* thing); + void SendName(Roboid* roboid); + virtual void SendModel(Roboid* obj); + void SendModel(Thing* thing); /// @brief The id of a Pose message static const unsigned char PoseMsg = 0x10; @@ -51,6 +52,8 @@ class NetworkSync { static const unsigned char Velocity2DMsg = 0x62; static const unsigned char CreateMsg = 0x80; static const unsigned char InvestigateMsg = 0x81; + static const unsigned char ModelMsg = 0x90; + static const unsigned char NameMsg = 0x91; static const unsigned char ClientMsg = 0xA0; static const unsigned char NetworkIdMsg = 0xA1; @@ -60,7 +63,7 @@ class NetworkSync { void ReceiveNetworkId(); - void BroadcastState(Roboid* roboid); + void PublishState(Roboid* roboid); void SendInvestigate(InterestingThing* thing); @@ -69,7 +72,7 @@ class NetworkSync { // void PublishNewObject(); void PublishRelativeThing(Thing* thing, bool recurse = false); - void BroadcastPerception(Roboid *roboid); + void PublishPerception(Roboid* roboid); void PublishTrackedObjects(Roboid* roboid, InterestingThing** objects); virtual void SendPosition(Vector3 worldPosition) {}; @@ -84,7 +87,7 @@ class NetworkSync { Roboid* roboid; NetworkPerception* networkPerception; - void BroadcastState(Sensor* sensor); + void PublishState(Sensor* sensor); void PublishTrackedObject(Roboid* roboid, InterestingThing* object); void PublishRelativeObject(Buffer sendBuffer, diff --git a/Perception.cpp b/Perception.cpp index 180ad3f..c1f4829 100644 --- a/Perception.cpp +++ b/Perception.cpp @@ -194,93 +194,6 @@ bool Perception::ObjectNearby(float direction, float range) { return false; } -// #include -// This function is deprecated -/* -void Perception::AddTrackedObject(Sensor* sensor, - Spherical16 position, - unsigned char thingType, - unsigned char networkId) { - // Spherical16 sPos = - // Spherical16(position.distance, Angle16(position.angle.ToFloat()), 0); - Quaternion orientation = Quaternion::identity; - AddTrackedObject(sensor, sPos, orientation, 0x00, thingType, networkId); - /* - InterestingThing *obj = new InterestingThing(sensor, position); - obj->type = thingType; - - unsigned char farthestObjIx = 0; - int availableSlotIx = -1; - for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { - if (this->trackedObjects[objIx] == nullptr) { - availableSlotIx = objIx; - } - // Do we see the same object? - else { - if (obj->IsTheSameAs(this->trackedObjects[objIx])) { - #ifdef RC_DEBUG2 - // Serial.print((int)this->trackedObjects[objIx]->id); - // Serial.println(": update tracked object"); - printf("%d: update tracked object [/%d]\n", objIx, obj->id); - #endif - - this->trackedObjects[objIx]->Refresh(obj->position); - this->trackedObjects[objIx]->type = thingType; - delete obj; - return; - } - // Is this the fartest object we see? - else if (this->trackedObjects[farthestObjIx] == nullptr || - (this->trackedObjects[objIx]->position.distance > - this->trackedObjects[farthestObjIx]->position.distance)) { - farthestObjIx = objIx; - } - } - } - - // Check if an perception slot is available (we currently see less than the - // max number of objects) - if (availableSlotIx >= 0) { //< maxObjectCount) { - // a slot is available - this->trackedObjects[availableSlotIx] = obj; - obj->networkId = networkId; - obj->id = lastObjectId++; // availableSlotIx + 1; - #ifdef RC_DEBUG2 - printf("%d: new tracked object [%d/%d]\n", availableSlotIx, - obj->networkId, obj->id); #endif if (roboid->networkSync != nullptr) { - roboid->networkSync->NewObject(obj); - // ((WifiSync *)roboid->networkSync)->PublishTrackedObject(roboid, obj); - } - } - // If this object is closer than the farthest object, then replace it - else if (obj->position.distance < - this->trackedObjects[farthestObjIx]->position.distance) { - delete this->trackedObjects[farthestObjIx]; - this->trackedObjects[farthestObjIx] = obj; - obj->networkId = networkId; - obj->id = lastObjectId++; // availableSlotIx + 1; - #ifdef RC_DEBUG2 - // Serial.print((int)obj->id); - // Serial.println(": replaced tracked object"); - printf("%d: replaced tracked object [/%d]\n", farthestObjIx, obj->id); - #endif - if (roboid->networkSync != nullptr) { - roboid->networkSync->NewObject(obj); - // ((WifiSync *)roboid->networkSync)->PublishTrackedObject(roboid, obj); - } - } else { - #ifdef RC_DEBUG2 - // Serial.print((int)obj->id); - // Serial.println(": delete tracked object"); - printf("%d: delete tracked object [/%d]\n", -1, obj->id); - - #endif - // No available slot, delete trackedobject - delete obj; - } - } - */ - InterestingThing* Perception::AddTrackedObject(Sensor* sensor, Spherical16 position, Quaternion orientation, diff --git a/Roboid.cpp b/Roboid.cpp index 8c70293..85b2b4f 100644 --- a/Roboid.cpp +++ b/Roboid.cpp @@ -32,6 +32,10 @@ Roboid::Roboid(Propulsion* propulsion) : Roboid() { propulsion->roboid = this; } +void Passer::RoboidControl::Roboid::SetName(char* name) { + this->name = name; +} + void Roboid::Update(unsigned long currentTimeMs) { if (perception != nullptr) perception->Update(currentTimeMs); diff --git a/Roboid.h b/Roboid.h index f858e4b..d99eb25 100644 --- a/Roboid.h +++ b/Roboid.h @@ -20,6 +20,9 @@ class Roboid : public Thing { /// @param propulsion The Propulsion implementation to use for this Roboid Roboid(Propulsion* propulsion); + char* name = nullptr; + void SetName(char* name); + /// @brief Update the state of the Roboid /// @param currentTimeMs The time in milliseconds when calling this /// function diff --git a/Sensor.h b/Sensor.h index 11a79d8..a6615f4 100644 --- a/Sensor.h +++ b/Sensor.h @@ -15,7 +15,7 @@ public: /// @param parent The Thing which should become the parent virtual void SetParent(Thing *parent) override; - virtual void BroadcastState() {}; + virtual void PublishState() {}; virtual void* GetValue() { return nullptr; }; };