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

10
NetworkSync.cpp
View File

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