diff --git a/NetworkPerception.cpp b/NetworkPerception.cpp index cae6de6..d8c716b 100644 --- a/NetworkPerception.cpp +++ b/NetworkPerception.cpp @@ -2,16 +2,18 @@ #include "NetworkSync.h" -// #define DEBUG true -#if DEBUG +#define RC_DEBUG true +#if RC_DEBUG #include #endif void NetworkPerception::ProcessPacket(Roboid* roboid, unsigned char* buffer, int packetsize) { +#if RC_DEBUG // printf("packet received, type = 0x%02x %d %d %d %d %d\n", buffer[0], // buffer[2], buffer[2], buffer[3], buffer[4], buffer[5]); +#endif switch (buffer[0]) { case NetworkSync::CreateMsg: @@ -49,7 +51,7 @@ void NetworkPerception::ReceiveInvestigateMsg(unsigned char* data, unsigned char networkId = data[1]; unsigned char objectId = data[2]; -#if DEBUG +#if RC_DEBUG printf("Received InvestigateMsg [%d/%d]\n", networkId, objectId); #endif @@ -103,8 +105,8 @@ void NetworkPerception::ReceivePlane(unsigned char* data, Roboid* roboid) { // worldPosition.y, worldPosition.z, roboidPosition.x, // roboidPosition.y, roboidPosition.z, position.distance, // (float)position.horizontalAngle, (float)position.verticalAngle); - // printf("Received plane %f (%f %f)\n", position.distance, - // (float)position.horizontalAngle, (float)position.verticalAngle); + printf("Received plane %f (%f %f)\n", position.distance, + position.horizontalAngle.ToFloat(), position.verticalAngle.ToFloat()); roboid->perception->AddTrackedObject(this, position, 0x80, networkId); } @@ -135,6 +137,10 @@ void NetworkPerception::ReceivePoseMsg(unsigned char* data, Roboid* roboid) { if (networkId == roboid->networkSync->networkId) networkId = 0x00; +#if RC_DEBUG + printf("Received PoseMsg [%d/%d]\n", networkId, objectId); +#endif + if (objectId == 0x80) return ReceivePlane(data, roboid); else if (objectId == 0x81) diff --git a/NetworkSync.cpp b/NetworkSync.cpp index 5164b6e..99d978f 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 @@ -54,7 +54,7 @@ void NetworkSync::NewObject(InterestingThing* thing) { SendBuffer(ix); #ifdef RC_DEBUG - printf("Sent CreateMsg %d %d\n", buffer[1], buffer[2]); + printf("Sent CreateMsg [%d/%d] %d\n", networkId, buffer[1], buffer[2]); #endif // PublishTrackedObject(roboid, obj); @@ -148,6 +148,10 @@ void NetworkSync::SendPose(Roboid* roboid, bool recurse) { SendQuat32(buffer, &ix, roboid->GetOrientation()); SendBuffer(ix); +#if RC_DEBUG + printf("Sent PoseMsg [%d/%d]\n", networkId, buffer[1]); +#endif + if (recurse) { for (unsigned char childIx = 0; childIx < roboid->childCount; childIx++) { Thing* child = roboid->GetChild(childIx); @@ -166,6 +170,10 @@ void NetworkSync::SendPose(Thing* thing, bool recurse) { SendQuat32(buffer, &ix, thing->orientation); SendBuffer(ix); +#if RC_DEBUG + printf("Sent PoseMsg Thing [%d/%d]\n", networkId, buffer[1]); +#endif + if (recurse) { for (unsigned char childIx = 0; childIx < thing->childCount; childIx++) { Thing* child = thing->GetChild(childIx); @@ -224,16 +232,22 @@ void NetworkSync::PublishTrackedObject(Roboid* roboid, buffer[ix++] = PoseMsg; // Position2DMsg; buffer[ix++] = object->id; // objectId; buffer[ix++] = Pose_Position | Pose_Orientation; + SendSpherical(buffer, &ix, Spherical(worldPosition)); + SendQuat32(buffer, &ix, worldOrientation); // SendPolar(buffer, &ix, polar); // 3 bytes - SendVector3(buffer, &ix, worldPosition); + // SendVector3(buffer, &ix, worldPosition); // SendQuat32(buffer, &ix, worldOrientation); SendBuffer(ix); +#if RC_DEBUG + printf("Sent Thing PoseMsg [%d/%d] %d\n", networkId, buffer[1], object->type); +#endif + object->updated = false; -#if DEBUG - printf("PublishTrackedObj [%d/%d] (%f %f)\n", object->networkId, buffer[1], - worldPosition.Right(), worldPosition.Forward()); +#if RC_DEBUG + // printf("PublishTrackedObj [%d/%d] (%f %f)\n", object->networkId, buffer[1], + // worldPosition.Right(), worldPosition.Forward()); #endif } diff --git a/Perception.cpp b/Perception.cpp index c382d61..870aede 100644 --- a/Perception.cpp +++ b/Perception.cpp @@ -193,6 +193,7 @@ bool Perception::ObjectNearby(float direction, float range) { } return false; } +#include // #include // This function is deprecated @@ -203,7 +204,7 @@ void Perception::AddTrackedObject(Sensor* sensor, Spherical16 sPos = Spherical16(position.distance, Angle16(position.angle.ToFloat()), 0); Quaternion orientation = Quaternion::identity; - AddTrackedObject(sensor, sPos, orientation, thingType, thingType, networkId); + AddTrackedObject(sensor, sPos, orientation, 0x00, thingType, networkId); /* InterestingThing *obj = new InterestingThing(sensor, position); obj->type = thingType; @@ -300,14 +301,10 @@ InterestingThing* Perception::AddTrackedObject(Sensor* sensor, // Do we see the same object? else { if (thing->IsTheSameAs(this->trackedObjects[thingIx])) { - // if ((float)thing->position.horizontalAngle != - // (float)this->trackedObjects[thingIx]->position.horizontalAngle) - // this->roboid->networkSync->SendText("Update\0"); - // else - // this->roboid->networkSync->SendText("Refresh\0"); this->trackedObjects[thingIx]->Refresh( thing->position, thing->orientation.ToQuaternion()); delete thing; + return this->trackedObjects[thingIx]; } // Is this the fartest object we see? @@ -347,15 +344,16 @@ InterestingThing* Perception::AddTrackedObject(Sensor* sensor, InterestingThing* Perception::AddTrackedObject(Sensor* sensor, unsigned char networkId, - unsigned char objectId, - Spherical16 position, + unsigned char thingId, + Spherical position, Quaternion orientation) { - InterestingThing* thing = FindTrackedObject(networkId, objectId); + InterestingThing* thing = FindTrackedObject(networkId, thingId); if (thing == nullptr) { - thing = - AddTrackedObject(sensor, position, orientation, objectId, networkId); + thing = AddTrackedObject(sensor, position, orientation, 0xFF, thingId, + networkId); + // Don't we set this above already? thing->networkId = networkId; - thing->id = objectId; + thing->id = thingId; thing->type = 0xFF; // unknown } return thing; diff --git a/Perception.h b/Perception.h index 2750ca7..24d0a2a 100644 --- a/Perception.h +++ b/Perception.h @@ -91,13 +91,13 @@ class Perception { /// the roboid void AddTrackedObject(Sensor* sensor, Polar position, - unsigned char objectType = 0x00, + unsigned char objectType = 0xFF, unsigned char networkId = 0x00); InterestingThing* AddTrackedObject( Sensor* sensor, - Spherical16 position, + Spherical position, Quaternion orientation = Quaternion::identity, - unsigned char objectType = 0x00, + unsigned char objectType = 0xFF, unsigned char objectId = 0x00, unsigned networkId = 0x00); @@ -105,7 +105,7 @@ class Perception { Sensor* sensor, unsigned char networkId, unsigned char objectId, - Spherical16 position, + Spherical position, Quaternion orientation = Quaternion::identity); bool IsInteresting(float distance);