diff --git a/NetworkPerception.cpp b/NetworkPerception.cpp index a6c0a65..bc951fe 100644 --- a/NetworkPerception.cpp +++ b/NetworkPerception.cpp @@ -125,7 +125,7 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) { InterestingThing *thing = roboid->perception->AddTrackedObject( this, networkId, objectId, position); - if (thing->type == 0xFF) { + if (thing->networkId != 0x00 && thing->type == 0xFF) { // Unknown thing roboid->networkSync->SendInvestigateThing(thing); } diff --git a/Perception.cpp b/Perception.cpp index e3751d6..0f3bff6 100644 --- a/Perception.cpp +++ b/Perception.cpp @@ -249,9 +249,9 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position, // Serial.print((int)obj->id); // Serial.println(": replaced tracked object"); printf("%d: replaced tracked object [/%d]\n", farthestObjIx, obj->id); +#endif roboid->networkSync->NewObject(obj); ((WifiSync *)roboid->networkSync)->PublishTrackedObject(roboid, obj); -#endif } else { #ifdef RC_DEBUG2 // Serial.print((int)obj->id); @@ -518,20 +518,20 @@ void Perception::UpdatePose(Polar translation) { // We only support translations in the horizontal plane at this moment... // This needs Spherical operator- to be implemented to work in 3d space if (thing->type == 0x80) { // plane - printf("[1/%d] %f (%f %f) ", thing->id, thing->position.distance, - (float)thing->position.horizontalAngle, - (float)thing->position.verticalAngle); + // printf("[1/%d] %f (%f %f) ", thing->id, thing->position.distance, + // (float)thing->position.horizontalAngle, + // (float)thing->position.verticalAngle); // Update the closest point to the plane float angle = (float)thing->position.horizontalAngle + translation.angle; angle = abs(angle); float deltaDistance = translation.distance * cosf(angle * Angle::Deg2Rad); - printf(" | translate %f %f %f | ", (float)translation.distance, - (float)angle, deltaDistance); + // printf(" | translate %f %f %f | ", (float)translation.distance, + // (float)angle, deltaDistance); thing->position.distance -= deltaDistance; - printf("-> %f (%f %f)\n", thing->position.distance, - (float)thing->position.horizontalAngle, - (float)thing->position.verticalAngle); + // printf("-> %f (%f %f)\n", thing->position.distance, + // (float)thing->position.horizontalAngle, + // (float)thing->position.verticalAngle); } else { Polar horizontalPosition = Polar(thing->position); // obj->position.ProjectOnHorizontalPlane(); @@ -555,16 +555,16 @@ void Perception::UpdatePose(Quaternion rotation) { if (thing == nullptr) continue; - printf("[1/%d] %f (%f %f) ", thing->id, thing->position.distance, - (float)thing->position.horizontalAngle, - (float)thing->position.verticalAngle); - printf("| rotate %f | ", rotationAngle); + // printf("[1/%d] %f (%f %f) ", thing->id, thing->position.distance, + // (float)thing->position.horizontalAngle, + // (float)thing->position.verticalAngle); + // printf("| rotate %f | ", rotationAngle); float updatedAngle = Angle::Normalize(thing->position.horizontalAngle - rotationAngle); thing->position.horizontalAngle = updatedAngle; - printf("-> %f (%f %f) \n", thing->position.distance, - (float)thing->position.horizontalAngle, - (float)thing->position.verticalAngle); + // printf("-> %f (%f %f) \n", thing->position.distance, + // (float)thing->position.horizontalAngle, + // (float)thing->position.verticalAngle); } } \ No newline at end of file