RoboidControl/RoboidControl-cpp
RoboidControl/RoboidControl-cpp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Thing improvement

Browse Source
This commit is contained in:
Pascal Serrarens 2024-10-21 12:52:25 +02:00
parent 25bc715e13
commit 6e996c64eb
1 changed files with 15 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
NetworkPerception.cpp
View File

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