diff --git a/LinearAlgebra b/LinearAlgebra index a25a8be..2bad384 160000 --- a/LinearAlgebra +++ b/LinearAlgebra @@ -1 +1 @@ -Subproject commit a25a8be0670b8e3a53db38de9772cb04d73e8a5b +Subproject commit 2bad384611e7d999b4c421781dab8f1c4543fe93 diff --git a/NetworkPerception.cpp b/NetworkPerception.cpp index 20be813..5f53c17 100644 --- a/NetworkPerception.cpp +++ b/NetworkPerception.cpp @@ -124,26 +124,19 @@ void NetworkPerception::ReceiveSphere(unsigned char *data, Roboid *roboid) { roboid->perception->AddTrackedObject(this, position, Quaternion::identity, 0x81, 0x81, networkId); - // roboid->networkSync->SendText("Received Sphere\n\0"); - // roboid->networkSync->SendInt(position.distance * 100); - // if ((float)position.horizontalAngle == 0) - // roboid->networkSync->SendText("Zero hor angle\0"); - // if ((float)position.verticalAngle == 0) - // roboid->networkSync->SendText("Zero vertical angle\0"); } void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) { unsigned char networkId = data[1]; unsigned char objectId = data[2]; unsigned char poseType = data[3]; - // printf("My network id == %d\n", roboid->networkSync->networkId); + if (networkId == roboid->networkSync->networkId) networkId = 0x00; - // printf("Received pose message [%d/%d]\n", networkId, objectId); if (objectId == 0x80) return ReceivePlane(data, roboid); - else if (objectId = 0x81) + else if (objectId == 0x81) return ReceiveSphere(data, roboid); if ((poseType & NetworkSync::Pose_Position) != 0) { @@ -153,20 +146,21 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) { } else { Vector3 roboidPosition = roboid->GetPosition(); - float distance = Vector3::Distance(roboidPosition, worldPosition); - if (roboid->perception->IsInteresting(distance) == false) { - // printf(" not interesting\n"); - return; - } + // float distance = Vector3::Distance(roboidPosition, worldPosition); + // if (roboid->perception->IsInteresting(distance) == false) { + // // printf(" not interesting\n"); + // return; + // } Quaternion roboidOrientation = roboid->GetOrientation(); Vector3 localPosition = Quaternion::Inverse(roboidOrientation) * (worldPosition - roboidPosition); + Spherical position = Spherical(localPosition); - float angle = - Vector3::SignedAngle(Vector3::forward, localPosition, Vector3::up); + // float angle = + // Vector3::SignedAngle(Vector3::forward, localPosition, Vector3::up); - Polar position = Polar(distance, angle); // Polar(angle, distance); + // Polar position = Polar(distance, angle); // Polar(angle, distance); /* Vector2 worldPosition2D = Vector2(worldPosition); @@ -185,19 +179,18 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) { // worldPosition.x, worldPosition.z, localPosition.x, // localPosition.z, distance, angle); + // InterestingThing *thing = roboid->perception->AddTrackedObject( + // this, networkId, objectId, position); InterestingThing *thing = roboid->perception->AddTrackedObject( - this, networkId, objectId, position); - if (thing->networkId != 0x00 && thing->type == 0xFF) { - // Unknown thing - roboid->networkSync->SendInvestigateThing(thing); - } + this, position, Quaternion::identity, 0x81, 0x81, networkId); + // if (thing->networkId != 0x00 && thing->type == 0xFF) { + // // Unknown thing + // roboid->networkSync->SendInvestigateThing(thing); + // } } } } -// HACK!!! -// #include "../../../src/LighthouseTracker.h" - void NetworkPerception::ReceiveTypedObject(unsigned char *data, Roboid *roboid) { unsigned char objectType = data[1]; diff --git a/NetworkSync.cpp b/NetworkSync.cpp index a044b1c..4292514 100644 --- a/NetworkSync.cpp +++ b/NetworkSync.cpp @@ -175,10 +175,6 @@ void NetworkSync::PublishClient() { #ifdef RC_DEBUG printf("Sent new Client\n"); #endif - - // PublishModel(roboid); - // if (roboid->actuation != nullptr) - // PublishRelativeThing(roboid->actuationRoot, false); } void NetworkSync::PublishTrackedObjects(Roboid *roboid, @@ -188,12 +184,10 @@ void NetworkSync::PublishTrackedObjects(Roboid *roboid, InterestingThing *obj = objects[objIx]; if (obj == nullptr) continue; - // if (obj->sensor->type == Thing::ExternalType) - // continue; - if (obj->confidence > 0) { + if (obj->confidence > 0) PublishTrackedObject(roboid, obj); - } + n++; } }