diff --git a/NetworkPerception.cpp b/NetworkPerception.cpp index a258f06..846a53f 100644 --- a/NetworkPerception.cpp +++ b/NetworkPerception.cpp @@ -56,12 +56,18 @@ void NetworkPerception::ReceiveInvestigateMsg(unsigned char *data, // We only response to investigation requests for our own objects return; - InterestingThing *thing = - roboid->perception->FindTrackedObject(0x00, objectId); - if (thing == nullptr) - return; + if (objectId == 0x00) { + // They are investigating the roboid itself! + if (roboid->modelUrl != nullptr) + roboid->networkSync->PublishModel(roboid); + } else { + InterestingThing *thing = + roboid->perception->FindTrackedObject(0x00, objectId); + if (thing == nullptr) + return; - roboid->networkSync->NewObject(thing); + roboid->networkSync->NewObject(thing); + } } void NetworkPerception::ReceivePlane(unsigned char *data, Roboid *roboid) { diff --git a/NetworkSync.cpp b/NetworkSync.cpp index 45eb409..1c35879 100644 --- a/NetworkSync.cpp +++ b/NetworkSync.cpp @@ -10,6 +10,8 @@ #include "LinearAlgebra/Spherical.h" #include "float16/float16.h" +#include + NetworkSync::NetworkSync(Roboid *roboid) { this->roboid = roboid; } void NetworkSync::ReceiveMessage(Roboid *roboid, unsigned char bytecount) { @@ -47,6 +49,29 @@ void NetworkSync::NewObject(InterestingThing *thing) { // PublishTrackedObject(roboid, obj); } +void NetworkSync::PublishModel(Roboid *roboid) { + int len = strlen(roboid->modelUrl); + if (len > 255) + return; + + unsigned char ix = 0; + buffer[ix++] = 0x90; // modelMsg + buffer[ix++] = 0x00; // objectId + // SendVector3(buffer, &ix, position); + // Polar polar = Polar(Vector2(position)); + // SendPolar(buffer, &ix, polar); + Spherical s = Spherical(roboid->modelPosition); + SendSpherical(buffer, &ix, s); + SendFloat16(buffer, &ix, roboid->modelScale); + + buffer[ix++] = len; + // printf("send url %d, scale = %f \n", ix, sscale); + for (int urlIx = 0; urlIx < len; urlIx++) { + buffer[ix++] = roboid->modelUrl[urlIx]; + } + SendBuffer(ix); +} + void NetworkSync::DestroyObject(InterestingThing *thing) { unsigned char ix = 0; buffer[ix++] = DestroyMsg; @@ -58,6 +83,30 @@ void NetworkSync::DestroyObject(InterestingThing *thing) { #endif } +void NetworkSync::SendPose(Roboid *roboid) { + if (roboid->propulsion == nullptr) + return; + + Polar velocity = roboid->propulsion->GetVelocity(); + Vector2 worldVelocity2 = + Vector2::Rotate(Vector2::forward * velocity.distance, velocity.angle); + Vector3 worldVelocity3 = Vector3(worldVelocity2.x, 0, worldVelocity2.y); + + float angularVelocity = roboid->propulsion->GetAngularVelocity(); + Vector3 worldAngularVelocity = Vector3(0, angularVelocity, 0); + + unsigned char buffer[3 + 12 + 12] = { + PoseMsg, + 0, // objectId; + Pose_LinearVelocity | Pose_AngularVelocity, + }; + unsigned char ix = 3; + SendVector3(buffer, &ix, worldVelocity3); + SendVector3(buffer, &ix, worldAngularVelocity); + + SendBuffer(ix); +} + void NetworkSync::SendVector3(unsigned char *data, unsigned char *startIndex, const Vector3 v) { SendSingle100(data, *startIndex, v.Right()); @@ -191,66 +240,119 @@ void NetworkSync::PublishClient() { #endif } -void NetworkSync::PublishTrackedObjects(Buffer sendBuffer, +void NetworkSync::PublishTrackedObjects(Roboid *roboid, InterestingThing **objects) { + int n = 0; 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; + if (obj->sensor->type == Thing::ExternalType) + continue; - // tmp - obj->id = objIx; - PublishTrackedObject(sendBuffer, obj); + if (obj->confidence > 0) { + PublishTrackedObject(roboid, obj); + } + n++; } } -void NetworkSync::PublishTrackedObject(Buffer sendBuffer, +void NetworkSync::PublishTrackedObject(Roboid *roboid, InterestingThing *object) { - Vector2 worldPosition2 = - Vector2::Rotate(Vector2::forward * object->position.distance, - -object->position.horizontalAngle); - Vector3 worldPosition3 = Vector3(worldPosition2.x, 0, worldPosition2.y); + if (object == nullptr || object->updated == false || + object->networkId != 0x00) { + return; + } -#ifdef RC_DEBUG - Serial.print("Send Pose "); - Serial.print((int)object->id); - Serial.print(" Position "); - Serial.print(worldPosition3.Right()); - Serial.print(", "); - Serial.print(worldPosition3.Up()); - Serial.print(", "); - Serial.println(worldPosition3.Forward()); -#else - const UInt16 bufferSize = 3 + 12; - UInt8 buffer[bufferSize] = { - PoseMsg, - (UInt8)object->id, - Pose_Position, - }; - unsigned char ix = 3; - SendVector3(buffer, &ix, worldPosition3); - sendBuffer(buffer, bufferSize); + // if (object->parentId != 0) + // return PublishRelativeObject(object); + + Vector3 roboidPosition = roboid->GetPosition(); + Quaternion roboidOrientation = roboid->GetOrientation(); + + // Vector3 localPosition = object->position.ToVector3(); + Vector3 localPosition = Vector3(object->position); + Vector3 worldPosition = roboidPosition + roboidOrientation * localPosition; + Quaternion worldOrientation = roboidOrientation * object->orientation; + + unsigned char ix = 0; + buffer[ix++] = PoseMsg; // Position2DMsg; + buffer[ix++] = object->id; // objectId; + buffer[ix++] = Pose_Position | Pose_Orientation; + // SendPolar(buffer, &ix, polar); // 3 bytes + SendVector3(buffer, &ix, worldPosition); + // SendQuat32(buffer, &ix, worldOrientation); + SendBuffer(ix); + + object->updated = false; + +#if DEBUG + printf("PublishTrackedObj [%d/%d] (%f %f)\n", object->networkId, buffer[1], + worldPosition.Right(), worldPosition.Forward()); #endif } -void NetworkSync::PublishRelativeObject(Buffer sendBuffer, UInt8 parentId, - InterestingThing *object) { - const UInt16 bufferSize = 4 + 12; - UInt8 buffer[bufferSize] = { - RelativePoseMsg, - (UInt8)parentId, - (UInt8)object->id, - Pose_Position, - }; - unsigned char ix = 4; - SendSpherical(buffer, &ix, object->position); - // SendVector3(buffer, ix, worldPosition3); - sendBuffer(buffer, bufferSize); -} +// 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; + +// // tmp +// obj->id = objIx; +// PublishTrackedObject(sendBuffer, obj); +// } +// } + +// void NetworkSync::PublishTrackedObject(Buffer sendBuffer, +// InterestingThing *object) { +// Vector2 worldPosition2 = +// Vector2::Rotate(Vector2::forward * object->position.distance, +// -object->position.horizontalAngle); +// Vector3 worldPosition3 = Vector3(worldPosition2.x, 0, worldPosition2.y); + +// #ifdef RC_DEBUG +// Serial.print("Send Pose "); +// Serial.print((int)object->id); +// Serial.print(" Position "); +// Serial.print(worldPosition3.Right()); +// Serial.print(", "); +// Serial.print(worldPosition3.Up()); +// Serial.print(", "); +// Serial.println(worldPosition3.Forward()); +// #else +// const UInt16 bufferSize = 3 + 12; +// UInt8 buffer[bufferSize] = { +// PoseMsg, +// (UInt8)object->id, +// Pose_Position, +// }; +// unsigned char ix = 3; +// SendVector3(buffer, &ix, worldPosition3); +// sendBuffer(buffer, bufferSize); +// #endif +// } + +// void NetworkSync::PublishRelativeObject(Buffer sendBuffer, UInt8 parentId, +// InterestingThing *object) { +// const UInt16 bufferSize = 4 + 12; +// UInt8 buffer[bufferSize] = { +// RelativePoseMsg, +// (UInt8)parentId, +// (UInt8)object->id, +// Pose_Position, +// }; +// unsigned char ix = 4; +// SendSpherical(buffer, &ix, object->position); +// // SendVector3(buffer, ix, worldPosition3); +// sendBuffer(buffer, bufferSize); +// } void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid *roboid) { Polar velocity = roboid->propulsion->GetVelocity(); @@ -308,4 +410,22 @@ void NetworkSync::SendInvestigateThing(InterestingThing *thing) { buffer[ix++] = thing->networkId; buffer[ix++] = thing->id; SendBuffer(ix); +} + +void NetworkSync::SendText(const char *s) { + unsigned char length; + for (length = 0; length < 253; length++) { + if (s[length] == '\0') + break; + } + if (length >= 253) + return; + + unsigned char ix; + buffer[ix++] = 0xB0; + buffer[ix++] = length; + for (int urlIx = 0; urlIx < length; urlIx++) + buffer[ix++] = s[urlIx]; + + SendBuffer(ix); } \ No newline at end of file diff --git a/NetworkSync.h b/NetworkSync.h index ae91458..a0253ca 100644 --- a/NetworkSync.h +++ b/NetworkSync.h @@ -22,6 +22,7 @@ public: /// @param obj virtual void DestroyObject(InterestingThing *obj); virtual void NewObject(InterestingThing *obj); + virtual void PublishModel(Roboid *obj); /// @brief The id of a Pose message static const unsigned char PoseMsg = 0x10; @@ -62,16 +63,19 @@ public: void SendDestroyObject(Buffer sendBuffer, InterestingThing *obj); void PublishNewObject(); - void PublishTrackedObjects(Buffer sendBuffer, InterestingThing **objects); + void PublishTrackedObjects(Roboid *roboid, InterestingThing **objects); virtual void SendPosition(Vector3 worldPosition) {}; virtual void SendPose(Vector3 worldPosition, Quaternion worldOrientation) {}; + void SendPose(Roboid *roboid); + + void SendText(const char *s); protected: Roboid *roboid; NetworkPerception *networkPerception; - void PublishTrackedObject(Buffer sendBuffer, InterestingThing *object); + void PublishTrackedObject(Roboid *roboid, InterestingThing *object); void PublishRelativeObject(Buffer sendBuffer, UInt8 parentId, InterestingThing *object); diff --git a/Roboid.cpp b/Roboid.cpp index 8d787d3..23a8016 100644 --- a/Roboid.cpp +++ b/Roboid.cpp @@ -3,6 +3,8 @@ #include "LinearAlgebra/FloatSingle.h" #include "NetworkSync.h" +#include + // #define RC_DEBUG #ifdef RC_DEBUG @@ -125,4 +127,12 @@ void Roboid::SetOrientation(Quaternion worldOrientation) { void Roboid::SetOrientation2D(float angle) { this->worldAngleAxis = AngleAxis(angle, Axis::up); -} \ No newline at end of file +} + +void Roboid::SetModel(const char *url, Vector3 position, Quaternion orientation, + float scale) { + this->modelUrl = url; + this->modelPosition = position; + this->modelOrientation = orientation; + this->modelScale = scale; +} diff --git a/Roboid.h b/Roboid.h index e75cd07..28c7189 100644 --- a/Roboid.h +++ b/Roboid.h @@ -69,6 +69,14 @@ public: virtual void SetOrientation(Quaternion worldOrientation); void SetOrientation2D(float angle); + void SetModel(const char *url, Vector3 position = Vector3(0, 0, 0), + Quaternion orientation = Quaternion::identity, float scale = 1); + + const char *modelUrl = nullptr; + Vector3 modelPosition = Vector3::zero; + Quaternion modelOrientation = Quaternion::identity; + float modelScale = 1; + private: /// @brief The position of the roboid in carthesian coordinates in world space /// @details This position may be set when NetworkSync is used to receive