diff --git a/NetworkPerception.cpp b/NetworkPerception.cpp index 2e15712..fa042f9 100644 --- a/NetworkPerception.cpp +++ b/NetworkPerception.cpp @@ -152,6 +152,9 @@ void NetworkPerception::ReceivePoseMsg(unsigned char* data, Roboid* roboid) { // printf("Received PoseMsg [%d/%d]\n", networkId, objectId); #endif + InterestingThing* thing = + roboid->perception->FindTrackedObject(networkId, objectId); + SwingTwist16 roboidOrientation = roboid->GetOrientation(); Spherical16 position = Spherical16::zero; SwingTwist16 orientation = SwingTwist16::identity; @@ -179,6 +182,8 @@ void NetworkPerception::ReceivePoseMsg(unsigned char* data, Roboid* roboid) { // localPosition.Up(), localPosition.Forward()); // printf(" position: %f (%f %f)\n", position.distance, // position.horizontal.InDegrees(), position.vertical.InDegrees()); + if (thing != nullptr) + thing->position = position; } } @@ -193,14 +198,19 @@ void NetworkPerception::ReceivePoseMsg(unsigned char* data, Roboid* roboid) { roboid->SetOrientation(worldOrientation); } else { orientation = SwingTwist16::Inverse(roboidOrientation) * worldOrientation; + if (thing != nullptr) { + thing->orientation = orientation; + } } } - InterestingThing* thing = roboid->perception->AddTrackedObject( - this, position, orientation, 0x81, 0x81, networkId); - if (thing->networkId != 0x00 && thing->type == 0xFF) { - // Unknown thing - roboid->networkSync->SendInvestigate(thing); + if (thing == nullptr) { + thing = roboid->perception->AddTrackedObject(this, position, orientation, + 0xFF, objectId, networkId); + if (thing->networkId != 0x00) { + // Unknown thing + roboid->networkSync->SendInvestigate(thing); + } } }