diff --git a/NetworkPerception.cpp b/NetworkPerception.cpp index 6f96700..b43a5e7 100644 --- a/NetworkPerception.cpp +++ b/NetworkPerception.cpp @@ -36,6 +36,21 @@ void NetworkPerception::ReceiveCreateMsg(unsigned char *data, Roboid *roboid) { } } +void NetworkPerception::ReceivePlane(unsigned char *data, Roboid *roboid) { + unsigned char poseType = data[3]; + if ((poseType & NetworkSync::Pose_Position) == 0 | + (poseType & NetworkSync::Pose_Orientation) == 0) + + // both position and orientation are required + return; + + Vector3 worldPosition = ReceiveVector3(data, 4); + Vector3 worldAngles = ReceiveVector3(data, 16); + Quaternion worldOrientation = Quaternion::Euler(worldAngles); + + roboid->perception->AddTrackedObject(this, position, 0x80); +} + void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) { unsigned char networkId = data[1]; unsigned char objectId = data[2]; @@ -45,6 +60,9 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) { networkId = 0x00; printf("Received pose message [%d/%d]\n", networkId, objectId); + if (objectId == 0x80) + return ReceivePlane(data, roboid); + if ((poseType & NetworkSync::Pose_Position) != 0) { Vector3 worldPosition = ReceiveVector3(data, 4); if (objectId == 0) { @@ -92,46 +110,6 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) { } } } - - /* - 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!!! diff --git a/NetworkPerception.h b/NetworkPerception.h index f0e9064..0917ad4 100644 --- a/NetworkPerception.h +++ b/NetworkPerception.h @@ -16,6 +16,8 @@ protected: void ReceivePoseMsg(unsigned char *data, Roboid *roboid); void ReceiveTypedObject(unsigned char *data, Roboid *roboid); + void ReceivePlane(unsigned char *data, Roboid *roboid); + Int32 ReceiveInt32(unsigned char *data, int startIndex); float ReceiveFloat100(unsigned char *data, int startIndex); Vector3 ReceiveVector3(unsigned char *data, int startIndex); diff --git a/TrackedObject.cpp b/TrackedObject.cpp index c1eeb1b..b2ac3c7 100644 --- a/TrackedObject.cpp +++ b/TrackedObject.cpp @@ -7,6 +7,7 @@ InterestingThing::InterestingThing(Sensor *sensor, Polar position) { this->confidence = maxConfidence; this->sensor = sensor; this->position = Spherical(position); + this->updated = true; } InterestingThing::InterestingThing(Sensor *sensor, Spherical position, @@ -16,6 +17,7 @@ InterestingThing::InterestingThing(Sensor *sensor, Spherical position, this->sensor = sensor; this->position = position; this->orientation = orientation; + this->updated = true; } bool InterestingThing::IsTheSameAs(InterestingThing *otherObj) { @@ -54,10 +56,12 @@ bool InterestingThing::DegradeConfidence(float deltaTime) { void InterestingThing::Refresh(Polar position) { this->position = Spherical(position); this->confidence = maxConfidence; + this->updated = true; } void InterestingThing::Refresh(Spherical position, Quaternion orientation) { this->position = position; this->orientation = orientation; this->confidence = maxConfidence; + this->updated = true; } diff --git a/TrackedObject.h b/TrackedObject.h index 0ad35fb..707d003 100644 --- a/TrackedObject.h +++ b/TrackedObject.h @@ -72,6 +72,7 @@ public: unsigned char type = 0x00; unsigned char confidence; + bool updated = false; protected: static constexpr unsigned char maxConfidence = 255;