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

20
NetworkPerception.cpp
View File

@ -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);
}
}
}