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

Fixed poseType check

Browse Source
This commit is contained in:
Pascal Serrarens 2024-02-27 14:27:06 +01:00
parent c2daeb2657
commit 4272d912b9
1 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
NetworkPerception.cpp
View File

@ -4,8 +4,8 @@
void NetworkPerception::ProcessPacket(Roboid *roboid, unsigned char *buffer, void NetworkPerception::ProcessPacket(Roboid *roboid, unsigned char *buffer,
int packetsize) { int packetsize) {
// printf("packet received, type = 0x%02x %d %d %d %d\n", buffer[0], // printf("packet received, type = 0x%02x %d %d %d %d %d\n", buffer[0],
// buffer[2], buffer[3], buffer[4], buffer[5]); // buffer[2], buffer[2], buffer[3], buffer[4], buffer[5]);
if (buffer[0] == NetworkSync::PoseMsg) { if (buffer[0] == NetworkSync::PoseMsg) {
ReceiveObject(buffer, roboid); ReceiveObject(buffer, roboid);
@ -79,8 +79,8 @@ void NetworkPerception::ReceiveObject(unsigned char *data, Roboid *roboid) {
} }
} }
// HACK!!!
#include "../../../src/LighthouseTracker.h" #include "../../../src/LighthouseTracker.h"
#include <Arduino.h>
void NetworkPerception::ReceiveTypedObject(unsigned char *data, void NetworkPerception::ReceiveTypedObject(unsigned char *data,
Roboid *roboid) { Roboid *roboid) {
@ -88,8 +88,10 @@ void NetworkPerception::ReceiveTypedObject(unsigned char *data,
unsigned char objectId = data[2]; unsigned char objectId = data[2];
if (objectType == 0x02) { // lighthouse if (objectType == 0x02) { // lighthouse
// We require position and orientation for lighthouses // We require position and orientation for lighthouses
if (data[3] != NetworkSync::Pose_Position | NetworkSync::Pose_Orientation) if (data[3] !=
(NetworkSync::Pose_Position | NetworkSync::Pose_Orientation)) {
return; return;
}
Vector3 worldPosition = ReceiveVector3(data, 4); Vector3 worldPosition = ReceiveVector3(data, 4);
Vector3 worldAngles = ReceiveVector3(data, 16); Vector3 worldAngles = ReceiveVector3(data, 16);
@ -102,6 +104,4 @@ void NetworkPerception::ReceiveTypedObject(unsigned char *data,
LighthouseTracker *lhSensor = (LighthouseTracker *)sensor; LighthouseTracker *lhSensor = (LighthouseTracker *)sensor;
lhSensor->UpdateGeometry(objectId, worldPosition, worldOrientation); lhSensor->UpdateGeometry(objectId, worldPosition, worldOrientation);
} }
Serial.printf("Typed Object received\n");
} }