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

Improve receiving pose

Browse Source
This commit is contained in:
Pascal Serrarens 2024-06-18 11:21:26 +02:00
parent bca786534d
commit b5fe2fda47
3 changed files with 21 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
LinearAlgebra

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

43
NetworkPerception.cpp
View File

@ -124,26 +124,19 @@ void NetworkPerception::ReceiveSphere(unsigned char *data, Roboid *roboid) {
roboid->perception->AddTrackedObject(this, position, Quaternion::identity,
0x81, 0x81, networkId);
// roboid->networkSync->SendText("Received Sphere\n\0");
// roboid->networkSync->SendInt(position.distance * 100);
// if ((float)position.horizontalAngle == 0)
// roboid->networkSync->SendText("Zero hor angle\0");
// if ((float)position.verticalAngle == 0)
// roboid->networkSync->SendText("Zero vertical angle\0");
}
void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
unsigned char networkId = data[1];
unsigned char objectId = data[2];
unsigned char poseType = data[3];
// printf("My network id == %d\n", roboid->networkSync->networkId);
if (networkId == roboid->networkSync->networkId)
networkId = 0x00;
// printf("Received pose message [%d/%d]\n", networkId, objectId);
if (objectId == 0x80)
return ReceivePlane(data, roboid);
else if (objectId = 0x81)
else if (objectId == 0x81)
return ReceiveSphere(data, roboid);
if ((poseType & NetworkSync::Pose_Position) != 0) {
@ -153,20 +146,21 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
} else {
Vector3 roboidPosition = roboid->GetPosition();
float distance = Vector3::Distance(roboidPosition, worldPosition);
if (roboid->perception->IsInteresting(distance) == false) {
// printf(" not interesting\n");
return;
}
// float distance = Vector3::Distance(roboidPosition, worldPosition);
// if (roboid->perception->IsInteresting(distance) == false) {
// // printf(" not interesting\n");
// 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);
// float angle =
// Vector3::SignedAngle(Vector3::forward, localPosition, Vector3::up);
Polar position = Polar(distance, angle); // Polar(angle, distance);
// Polar position = Polar(distance, angle); // Polar(angle, distance);
/*
Vector2 worldPosition2D = Vector2(worldPosition);
@ -185,19 +179,18 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
// 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, networkId, objectId, position);
if (thing->networkId != 0x00 && thing->type == 0xFF) {
// Unknown thing
roboid->networkSync->SendInvestigateThing(thing);
}
this, position, Quaternion::identity, 0x81, 0x81, networkId);
// if (thing->networkId != 0x00 && thing->type == 0xFF) {
// // Unknown thing
// roboid->networkSync->SendInvestigateThing(thing);
// }
}
}
}
// HACK!!!
// #include "../../../src/LighthouseTracker.h"
void NetworkPerception::ReceiveTypedObject(unsigned char *data,
Roboid *roboid) {
unsigned char objectType = data[1];

10
NetworkSync.cpp
View File

@ -175,10 +175,6 @@ void NetworkSync::PublishClient() {
#ifdef RC_DEBUG
printf("Sent new Client\n");
#endif
// PublishModel(roboid);
// if (roboid->actuation != nullptr)
// PublishRelativeThing(roboid->actuationRoot, false);
}
void NetworkSync::PublishTrackedObjects(Roboid *roboid,
@ -188,12 +184,10 @@ void NetworkSync::PublishTrackedObjects(Roboid *roboid,
InterestingThing *obj = objects[objIx];
if (obj == nullptr)
continue;
// if (obj->sensor->type == Thing::ExternalType)
// continue;
if (obj->confidence > 0) {
if (obj->confidence > 0)
PublishTrackedObject(roboid, obj);
}
n++;
}
}