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

Improve receive pose orientation

Browse Source
This commit is contained in:
Pascal Serrarens 2024-06-18 13:06:58 +02:00
parent b5fe2fda47
commit edf2df6c7e
2 changed files with 23 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
LinearAlgebra

@ -1 +1 @@
Subproject commit 2bad384611e7d999b4c421781dab8f1c4543fe93
Subproject commit c70c079efc4fcfe9e9b2c29cd82f67faa63c827f

55
NetworkPerception.cpp
View File

@ -139,6 +139,10 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
else if (objectId == 0x81)
return ReceiveSphere(data, roboid);
Quaternion roboidOrientation = roboid->GetOrientation();
Spherical position = Spherical::zero;
Quaternion orientation = Quaternion::identity;
if ((poseType & NetworkSync::Pose_Position) != 0) {
Vector3 worldPosition = ReceiveVector3(data, 4);
if (objectId == 0) {
@ -152,43 +156,28 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
// 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);
// Polar position = Polar(distance, angle); // Polar(angle, distance);
/*
Vector2 worldPosition2D = Vector2(worldPosition);
Vector2 myPosition = roboid->GetPosition2D();
float myOrientation = roboid->GetOrientation2D();
Vector2 localPosition =
Vector2::Rotate(worldPosition2D - myPosition, -myOrientation);
float distance = Vector2::Magnitude(localPosition);
float angle = Vector2::SignedAngle(Vector3::forward, localPosition);
Polar position = Polar(angle, distance);
*/
// printf("object received at (%f, %f)/(%f, %f) -> (%f %f)\n",
// 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, position, Quaternion::identity, 0x81, 0x81, networkId);
// if (thing->networkId != 0x00 && thing->type == 0xFF) {
// // Unknown thing
// roboid->networkSync->SendInvestigateThing(thing);
// }
position = Spherical(localPosition);
}
}
if ((poseType & NetworkSync::Pose_Orientation) != 0) {
Vector3 worldAngles = ReceiveVector3(data, 16);
Quaternion worldOrientation = Quaternion::Euler(worldAngles);
if (objectId == 0) {
roboid->SetOrientation(worldOrientation);
} else {
orientation = Quaternion::Inverse(roboidOrientation) * worldOrientation;
}
}
InterestingThing *thing = roboid->perception->AddTrackedObject(
this, position, orientation, 0x81, 0x81, networkId);
if (thing->networkId != 0x00 && thing->type == 0xFF) {
// Unknown thing
roboid->networkSync->SendInvestigateThing(thing);
}
}
void NetworkPerception::ReceiveTypedObject(unsigned char *data,