diff --git a/NetworkPerception.cpp b/NetworkPerception.cpp index e412418..9d11ff1 100644 --- a/NetworkPerception.cpp +++ b/NetworkPerception.cpp @@ -7,10 +7,146 @@ void NetworkPerception::ProcessPacket(Roboid *roboid, unsigned char *buffer, // printf("packet received, type = 0x%02x %d %d %d %d %d\n", buffer[0], // buffer[2], buffer[2], buffer[3], buffer[4], buffer[5]); - if (buffer[0] == NetworkSync::PoseMsg) { - ReceiveObject(buffer, roboid); - } else if (buffer[0] == NetworkSync::PoseTypeMsg) { + switch (buffer[0]) { + case NetworkSync::CreateMsg: + ReceiveCreateMsg(buffer, roboid); + break; + case NetworkSync::PoseMsg: + ReceivePoseMsg(buffer, roboid); + break; + case NetworkSync::PoseTypeMsg: ReceiveTypedObject(buffer, roboid); + break; + } +} + +#include +void NetworkPerception::ReceiveCreateMsg(unsigned char *data, Roboid *roboid) { + unsigned char networkId = data[1]; + unsigned char objectId = data[2]; + unsigned char objectType = data[3]; + if (networkId == roboid->networkSync->networkId) + return; + + // printf("Received create message [%d/%d]\n", networkId, objectId); + InterestingThing *thing = + roboid->perception->FindTrackedObject(networkId, objectId); + if (thing != nullptr) { + thing->type = objectType; + } +} + +void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) { + unsigned char networkId = data[1]; + unsigned char objectId = data[2]; + unsigned char poseType = data[3]; + printf("Received pose message [%d/%d]\n", networkId, objectId); + + if ((poseType & NetworkSync::Pose_Position) != 0) { + Vector3 worldPosition = ReceiveVector3(data, 4); + if (objectId == 0) { + roboid->SetPosition(worldPosition); + } else { + Vector3 myPosition = roboid->GetPosition(); // Vector3::zero; + // Vector2 myPosition2 = Vector3::ProjectHorizontalPlane(myPosition); + Quaternion myOrientation = roboid->GetOrientation(); + // Vector3 myDirection = myOrientation * Vector3::forward; + // Vector2 myDirection2 = Vector3::ProjectHorizontalPlane(myDirection); + + Vector3 localPosition = + Quaternion::Inverse(myOrientation) * (worldPosition - myPosition); + + // float distance = Vector3::Distance(myPosition, worldPosition); + // float angle = Vector3::SignedAngle(myDirection, localPosition, + // Vector3::up); + float distance = Vector3::Magnitude(localPosition); + float angle = + Vector3::SignedAngle(Vector3::forward, localPosition, Vector3::up); + + Polar position = Polar(angle, distance); + + // int objCount = roboid->perception->TrackedObjectCount(); + + // printf("object received at (%f, %f)/(%f, %f) -> (%f %f)\n", + // worldPosition.x, worldPosition.z, localPosition.x, + // localPosition.z, distance, angle); + + // roboid->perception->AddTrackedObject(this, position); + InterestingThing *thing = roboid->perception->AddTrackedObject( + this, networkId, objectId, position); + if (thing->type == 0xFF) { + // Unknown thing + roboid->networkSync->SendInvestigateThing(thing); + } + } + } + + /* + Vector3 worldAngles = ReceiveVector3(data, 16); + Quaternion worldOrientation = Quaternion::Euler(worldAngles); + Vector3 worldDirection = worldOrientation * Vector3::forward; + if (objectId == 0) { + roboid->SetPosition(worldPosition); + roboid->SetOrientation(worldOrientation); + // printf("Received pose (%f, %f) (%f, %f)\n", worldPosition.x, + // worldPosition.z, worldDirection.x, worldDirection.z); + + } else { + Vector3 myPosition = roboid->GetPosition(); // Vector3::zero; + Vector2 myPosition2 = Vector3::ProjectHorizontalPlane(myPosition); + Quaternion myOrientation = roboid->GetOrientation(); + Vector3 myDirection = myOrientation * Vector3::forward; + Vector2 myDirection2 = Vector3::ProjectHorizontalPlane(myDirection); + + Vector3 localPosition = + Quaternion::Inverse(myOrientation) * (worldPosition - myPosition); + + // float distance = Vector3::Distance(myPosition, worldPosition); + // float angle = Vector3::SignedAngle(myDirection, localPosition, + // Vector3::up); + float distance = Vector3::Magnitude(localPosition); + float angle = + Vector3::SignedAngle(Vector3::forward, localPosition, Vector3::up); + + Polar position = Polar(angle, distance); + + // int objCount = roboid->perception->TrackedObjectCount(); + + // printf("object received at (%f, %f)/(%f, %f) -> (%f %f)\n", + // worldPosition.x, + // worldPosition.z, localPosition.x, localPosition.z, distance, + // angle); + + roboid->perception->AddTrackedObject(this, position); + } + */ +} + +// HACK!!! +// #include "../../../src/LighthouseTracker.h" + +void NetworkPerception::ReceiveTypedObject(unsigned char *data, + Roboid *roboid) { + unsigned char objectType = data[1]; + unsigned char objectId = data[2]; + if (objectType == 0x02) { // lighthouse + // We require position and orientation for lighthouses + if (data[3] != + (NetworkSync::Pose_Position | NetworkSync::Pose_Orientation)) { + return; + } + + Vector3 worldPosition = ReceiveVector3(data, 4); + Vector3 worldAngles = ReceiveVector3(data, 16); + Quaternion worldOrientation = Quaternion::Euler(worldAngles); + + // roboid->perception->AddTrackedObject(this, objectType, worldPosition, + // worldOrientation); + // Sensor *sensor = + // roboid->perception->FindSensorOfType(0x82); // Lighthouse type + // sensor + // LighthouseTracker *lhSensor = (LighthouseTracker *)sensor; + // lhSensor->UpdateGeometry(objectId, worldPosition, worldOrientation); } } @@ -35,73 +171,3 @@ Vector3 NetworkPerception::ReceiveVector3(unsigned char *data, int startIndex) { Vector3 v = Vector3(x, y, z); return v; } - -void NetworkPerception::ReceiveObject(unsigned char *data, Roboid *roboid) { - unsigned char objectId = data[1]; - // char poseType = data[2]; - Vector3 worldPosition = ReceiveVector3(data, 3); - Vector3 worldAngles = ReceiveVector3(data, 15); - Quaternion worldOrientation = Quaternion::Euler(worldAngles); - Vector3 worldDirection = worldOrientation * Vector3::forward; - if (objectId == 0) { - roboid->SetPosition(worldPosition); - roboid->SetOrientation(worldOrientation); - // printf("Received pose (%f, %f) (%f, %f)\n", worldPosition.x, - // worldPosition.z, worldDirection.x, worldDirection.z); - - } else { - Vector3 myPosition = roboid->GetPosition(); // Vector3::zero; - Vector2 myPosition2 = Vector3::ProjectHorizontalPlane(myPosition); - Quaternion myOrientation = roboid->GetOrientation(); - Vector3 myDirection = myOrientation * Vector3::forward; - Vector2 myDirection2 = Vector3::ProjectHorizontalPlane(myDirection); - - Vector3 localPosition = - Quaternion::Inverse(myOrientation) * (worldPosition - myPosition); - - // float distance = Vector3::Distance(myPosition, worldPosition); - // float angle = Vector3::SignedAngle(myDirection, localPosition, - // Vector3::up); - float distance = Vector3::Magnitude(localPosition); - float angle = - Vector3::SignedAngle(Vector3::forward, localPosition, Vector3::up); - - Polar position = Polar(angle, distance); - - // int objCount = roboid->perception->TrackedObjectCount(); - - // printf("object received at (%f, %f)/(%f, %f) -> (%f %f)\n", - // worldPosition.x, - // worldPosition.z, localPosition.x, localPosition.z, distance, - // angle); - - roboid->perception->AddTrackedObject(this, position); - } -} - -// HACK!!! -// #include "../../../src/LighthouseTracker.h" - -void NetworkPerception::ReceiveTypedObject(unsigned char *data, - Roboid *roboid) { - unsigned char objectType = data[1]; - unsigned char objectId = data[2]; - if (objectType == 0x02) { // lighthouse - // We require position and orientation for lighthouses - if (data[3] != - (NetworkSync::Pose_Position | NetworkSync::Pose_Orientation)) { - return; - } - - Vector3 worldPosition = ReceiveVector3(data, 4); - Vector3 worldAngles = ReceiveVector3(data, 16); - Quaternion worldOrientation = Quaternion::Euler(worldAngles); - - // roboid->perception->AddTrackedObject(this, objectType, worldPosition, - // worldOrientation); - // Sensor *sensor = - // roboid->perception->FindSensorOfType(0x82); // Lighthouse type sensor - // LighthouseTracker *lhSensor = (LighthouseTracker *)sensor; - // lhSensor->UpdateGeometry(objectId, worldPosition, worldOrientation); - } -} diff --git a/NetworkPerception.h b/NetworkPerception.h index b6da74d..f0e9064 100644 --- a/NetworkPerception.h +++ b/NetworkPerception.h @@ -12,7 +12,8 @@ public: void ProcessPacket(Roboid *roboid, unsigned char *buffer, int packetsize); protected: - void ReceiveObject(unsigned char *data, Roboid *roboid); + void ReceiveCreateMsg(unsigned char *data, Roboid *roboid); + void ReceivePoseMsg(unsigned char *data, Roboid *roboid); void ReceiveTypedObject(unsigned char *data, Roboid *roboid); Int32 ReceiveInt32(unsigned char *data, int startIndex); diff --git a/NetworkSync.cpp b/NetworkSync.cpp index 30b4a06..7a36af5 100644 --- a/NetworkSync.cpp +++ b/NetworkSync.cpp @@ -9,6 +9,22 @@ #include "LinearAlgebra/Angle8.h" #include "LinearAlgebra/Spherical.h" +void NetworkSync::ReceiveMessage(Roboid *roboid, unsigned char bytecount) { + networkPerception->ProcessPacket(roboid, buffer, bytecount); + + switch (buffer[0]) { + case NetworkIdMsg: + // this->networkId = NetworkIdMsg; + ReceiveNetworkId(); + break; + } +} + +void NetworkSync::ReceiveNetworkId() { + this->networkId = buffer[1]; + printf("Received network Id %d\n", this->networkId); +} + void NetworkSync::SendVector3(unsigned char *data, unsigned char *startIndex, const Vector3 v) { SendSingle100(data, *startIndex, v.x); @@ -118,12 +134,25 @@ void NetworkSync::SendInt32(unsigned char *data, unsigned int startIndex, } } -void NetworkSync::PublishTrackedObjects(SendBuffer sendBuffer, +// static unsigned char buffer[100]; + +void NetworkSync::SendBuffer(unsigned char bufferSize) {} + +void NetworkSync::PublishClient() { + unsigned char ix = 0; + buffer[ix++] = ClientMsg; + buffer[ix++] = 0; // No network ID + SendBuffer(ix); +} + +void NetworkSync::PublishTrackedObjects(Buffer sendBuffer, InterestingThing **objects) { for (unsigned char objIx = 0; objIx < Perception::maxObjectCount; objIx++) { InterestingThing *obj = objects[objIx]; if (obj == nullptr) continue; + if (obj->networkId != 0x00) + continue; // object is external // if (obj->sensor->type == Thing::ExternalType) // continue; @@ -133,7 +162,7 @@ void NetworkSync::PublishTrackedObjects(SendBuffer sendBuffer, } } -void NetworkSync::PublishTrackedObject(SendBuffer sendBuffer, +void NetworkSync::PublishTrackedObject(Buffer sendBuffer, InterestingThing *object) { Vector2 worldPosition2 = Vector2::Rotate(Vector2::forward * object->position.distance, @@ -162,7 +191,7 @@ void NetworkSync::PublishTrackedObject(SendBuffer sendBuffer, #endif } -void NetworkSync::PublishRelativeObject(SendBuffer sendBuffer, UInt8 parentId, +void NetworkSync::PublishRelativeObject(Buffer sendBuffer, UInt8 parentId, InterestingThing *object) { const UInt16 bufferSize = 4 + 12; UInt8 buffer[bufferSize] = { @@ -177,7 +206,7 @@ void NetworkSync::PublishRelativeObject(SendBuffer sendBuffer, UInt8 parentId, sendBuffer(buffer, bufferSize); } -void NetworkSync::SendPoseMsg(SendBuffer sendBuffer, Roboid *roboid) { +void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid *roboid) { Polar velocity = roboid->propulsion->GetVelocity(); Vector2 worldVelocity2 = Vector2::Rotate(Vector2::forward * velocity.distance, velocity.angle); @@ -214,8 +243,7 @@ void NetworkSync::SendPoseMsg(SendBuffer sendBuffer, Roboid *roboid) { #endif } -void NetworkSync::SendDestroyObject(SendBuffer sendBuffer, - InterestingThing *obj) { +void NetworkSync::SendDestroyObject(Buffer sendBuffer, InterestingThing *obj) { #ifdef RC_DEBUG Serial.print("Send Destroy "); Serial.println((int)obj->id); @@ -223,4 +251,13 @@ void NetworkSync::SendDestroyObject(SendBuffer sendBuffer, unsigned char buffer[2] = {DestroyMsg, (unsigned char)obj->id}; sendBuffer(buffer, 2); #endif +} + +void NetworkSync::SendInvestigateThing(InterestingThing *thing) { + // printf("Investigate [%d/%d]\n", thing->networkId, thing->id); + unsigned char ix = 0; + buffer[ix++] = InvestigateMsg; + buffer[ix++] = thing->networkId; + buffer[ix++] = thing->id; + SendBuffer(ix); } \ No newline at end of file diff --git a/NetworkSync.h b/NetworkSync.h index 096c5b7..929e729 100644 --- a/NetworkSync.h +++ b/NetworkSync.h @@ -11,6 +11,8 @@ namespace RoboidControl { /// @brief Interface for synchronizaing state between clients across a network class NetworkSync { public: + unsigned char networkId; + /// @brief Retreive and send the roboid state /// @param roboid The roboid for which the state is updated virtual void NetworkUpdate(Roboid *roboid) = 0; @@ -19,39 +21,53 @@ public: virtual void DestroyObject(InterestingThing *obj) = 0; virtual void NewObject(InterestingThing *obj){}; /// @brief The id of a Pose message - static const char PoseMsg = 0x10; - static const char PoseTypeMsg = 0x11; - static const char RelativePoseMsg = 0x12; + static const unsigned char PoseMsg = 0x10; + static const unsigned char PoseTypeMsg = 0x11; + static const unsigned char RelativePoseMsg = 0x12; /// @brief A bit pattern for the pose, stating that this message contains a /// position in world coordinates - static const char Pose_Position = 0x01; + static const unsigned char Pose_Position = 0x01; /// @brief A bit pattern for the pose, stating that this message contains an /// orientation in world coordinates - static const char Pose_Orientation = 0x02; + static const unsigned char Pose_Orientation = 0x02; /// @brief A bit pattern for the pose, stating that this messsage contains a /// linear velocity in world coordinates - static const char Pose_LinearVelocity = 0x04; + static const unsigned char Pose_LinearVelocity = 0x04; /// @brief A bit pattern for the pose, stating that this message contains an /// angular velocity in world coordinates static const char Pose_AngularVelocity = 0x08; - static const char DestroyMsg = 0x20; + static const unsigned char DestroyMsg = 0x20; - typedef void (*SendBuffer)(UInt8 *buffer, UInt16 bufferSize); + 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 InvestigateMsg = 0x81; + static const unsigned char ClientMsg = 0xA0; + static const unsigned char NetworkIdMsg = 0xA1; - void SendPoseMsg(SendBuffer sendBuffer, Roboid *roboid); - void SendDestroyObject(SendBuffer sendBuffer, InterestingThing *obj); + typedef void (*Buffer)(UInt8 *buffer, UInt16 bufferSize); + + void ReceiveMessage(Roboid *roboid, unsigned char bytecount); + + void ReceiveNetworkId(); + + void SendInvestigateThing(InterestingThing *thing); + + void SendPoseMsg(Buffer sendBuffer, Roboid *roboid); + void SendDestroyObject(Buffer sendBuffer, InterestingThing *obj); void PublishNewObject(); - void PublishTrackedObjects(SendBuffer sendBuffer, InterestingThing **objects); + void PublishTrackedObjects(Buffer sendBuffer, InterestingThing **objects); virtual void SendPosition(Vector3 worldPosition){}; virtual void SendPose(Vector3 worldPosition, Quaternion worldOrientation){}; protected: NetworkPerception *networkPerception; - void PublishTrackedObject(SendBuffer sendBuffer, InterestingThing *object); - void PublishRelativeObject(SendBuffer sendBuffer, UInt8 parentId, + void PublishTrackedObject(Buffer sendBuffer, InterestingThing *object); + void PublishRelativeObject(Buffer sendBuffer, UInt8 parentId, InterestingThing *object); void SendSingle100(unsigned char *data, unsigned int startIndex, float value); @@ -73,6 +89,11 @@ protected: // void SendSpherical32(unsigned char *data, int startIndex, Spherical s); void SendQuat32(unsigned char *data, unsigned char *startIndex, const Quaternion q); + + unsigned char buffer[32]; + virtual void SendBuffer(unsigned char bufferSize); + + void PublishClient(); }; } // namespace RoboidControl diff --git a/Perception.cpp b/Perception.cpp index be1cedd..769c18c 100644 --- a/Perception.cpp +++ b/Perception.cpp @@ -163,6 +163,7 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position, if (availableSlotIx < maxObjectCount) { // a slot is available this->trackedObjects[availableSlotIx] = obj; + obj->networkId = 0x00; obj->id = availableSlotIx + 1; #ifdef RC_DEBUG2 Serial.print((int)obj->id); @@ -176,6 +177,7 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position, this->trackedObjects[farthestObjIx]->position.distance) { delete this->trackedObjects[farthestObjIx]; this->trackedObjects[farthestObjIx] = obj; + obj->networkId = 0x00; obj->id = availableSlotIx + 1; #ifdef RC_DEBUG2 Serial.print((int)obj->id); @@ -228,6 +230,7 @@ InterestingThing *Perception::AddTrackedObject(Sensor *sensor, if (availableSlotIx < maxObjectCount) { // a slot is available this->trackedObjects[availableSlotIx] = obj; + obj->networkId = 0x00; obj->id = availableSlotIx + 1; #ifdef RC_DEBUG2 Serial.print((int)obj->id); @@ -240,6 +243,7 @@ InterestingThing *Perception::AddTrackedObject(Sensor *sensor, this->trackedObjects[farthestObjIx]->position.distance) { delete this->trackedObjects[farthestObjIx]; this->trackedObjects[farthestObjIx] = obj; + obj->networkId = 0x00; obj->id = availableSlotIx + 1; #ifdef RC_DEBUG2 Serial.print((int)obj->id); @@ -257,6 +261,21 @@ InterestingThing *Perception::AddTrackedObject(Sensor *sensor, } } +InterestingThing *Perception::AddTrackedObject(Sensor *sensor, + unsigned char networkId, + unsigned char objectId, + Spherical position, + Quaternion orientation) { + InterestingThing *thing = FindTrackedObject(networkId, objectId); + if (thing == nullptr) { + thing = AddTrackedObject(sensor, position, orientation); + thing->networkId = networkId; + thing->id = objectId; + thing->type = 0xFF; // unknown + } + return thing; +} + InterestingThing *Perception::FindTrackedObject(char objectId) { for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { InterestingThing *thing = this->trackedObjects[objIx]; @@ -269,6 +288,20 @@ InterestingThing *Perception::FindTrackedObject(char objectId) { return nullptr; } +InterestingThing *Perception::FindTrackedObject(unsigned char networkId, + unsigned char objectId) { + for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { + InterestingThing *thing = this->trackedObjects[objIx]; + if (thing == nullptr) + continue; + + if (thing->networkId == networkId && thing->id == objectId) { + return thing; + } + } + return nullptr; +} + unsigned char Perception::TrackedObjectCount() { unsigned char objectCount = 0; for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { @@ -282,9 +315,9 @@ InterestingThing **Perception::GetTrackedObjects() { return this->trackedObjects; } -unsigned char Perception::GetThingsOfType(unsigned char objectType, - InterestingThing *buffer[], - unsigned char bufferSize) { +unsigned char Perception::ThingsOfType(unsigned char objectType, + InterestingThing *buffer[], + unsigned char bufferSize) { unsigned char thingCount = 0; for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { InterestingThing *thing = this->trackedObjects[objIx]; diff --git a/Perception.h b/Perception.h index 67bf2cc..c805315 100644 --- a/Perception.h +++ b/Perception.h @@ -91,11 +91,19 @@ public: AddTrackedObject(Sensor *sensor, Spherical position, Quaternion orientation = Quaternion::identity); + InterestingThing * + AddTrackedObject(Sensor *sensor, unsigned char networkId, + unsigned char objectId, Spherical position, + Quaternion orientation = Quaternion::identity); + InterestingThing *FindTrackedObject(char objectId); + InterestingThing *FindTrackedObject(unsigned char networkId, + unsigned char objectId); /// @brief Retrieve the number of objects currently being tracked by the /// roboid - /// @return The object of objects, which is always lower than maxObjectCount + /// @return The object of objects, which is always lower than + /// maxObjectCount unsigned char TrackedObjectCount(); /// @brief Retreive the objects currently tracked by the roboid /// @return An array of current objects @@ -104,9 +112,9 @@ public: /// currently being tracked. InterestingThing **GetTrackedObjects(); - unsigned char GetThingsOfType(unsigned char objectType, - InterestingThing *buffer[], - unsigned char bufferSize); + unsigned char ThingsOfType(unsigned char objectType, + InterestingThing *buffer[], + unsigned char bufferSize); InterestingThing *GetMostInterestingThing(); diff --git a/TrackedObject.h b/TrackedObject.h index 3c50755..0ad35fb 100644 --- a/TrackedObject.h +++ b/TrackedObject.h @@ -58,8 +58,9 @@ public: /// coordinated position->angle = 30 and position->angle = 27 can be the same. static constexpr float equalityAngle = 5.0F; + unsigned char networkId; /// @brief The id of the tracked object - char id; + unsigned char id; char parentId = 0; /// @brief The current position of the object