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

Fix include

Browse Source
This commit is contained in:
Pascal Serrarens 2024-07-02 12:28:05 +02:00
parent 543ddf79b4
commit c31fd5f911
1 changed files with 30 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
NetworkPerception.cpp
View File

@ -1,34 +1,35 @@
#include "NetworkPerception.h" #include "NetworkPerception.h"
#include "RoboidControl/NetworkSync.h" #include "NetworkSync.h"
// #define DEBUG true // #define DEBUG true
#if DEBUG #if DEBUG
#include <Arduino.h> #include <Arduino.h>
#endif #endif
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 %d\n", buffer[0], // printf("packet received, type = 0x%02x %d %d %d %d %d\n", buffer[0],
// buffer[2], buffer[2], buffer[3], buffer[4], buffer[5]); // buffer[2], buffer[2], buffer[3], buffer[4], buffer[5]);
switch (buffer[0]) { switch (buffer[0]) {
case NetworkSync::CreateMsg: case NetworkSync::CreateMsg:
ReceiveCreateMsg(buffer, roboid); ReceiveCreateMsg(buffer, roboid);
break; break;
case NetworkSync::InvestigateMsg: case NetworkSync::InvestigateMsg:
ReceiveInvestigateMsg(buffer, roboid); ReceiveInvestigateMsg(buffer, roboid);
break; break;
case NetworkSync::PoseMsg: case NetworkSync::PoseMsg:
ReceivePoseMsg(buffer, roboid); ReceivePoseMsg(buffer, roboid);
break; break;
case NetworkSync::PoseTypeMsg: case NetworkSync::PoseTypeMsg:
ReceiveTypedObject(buffer, roboid); ReceiveTypedObject(buffer, roboid);
break; break;
} }
} }
void NetworkPerception::ReceiveCreateMsg(unsigned char *data, Roboid *roboid) { void NetworkPerception::ReceiveCreateMsg(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 objectType = data[3]; unsigned char objectType = data[3];
@ -36,15 +37,15 @@ void NetworkPerception::ReceiveCreateMsg(unsigned char *data, Roboid *roboid) {
return; return;
// printf("Received create message [%d/%d]\n", networkId, objectId); // printf("Received create message [%d/%d]\n", networkId, objectId);
InterestingThing *thing = InterestingThing* thing =
roboid->perception->FindTrackedObject(networkId, objectId); roboid->perception->FindTrackedObject(networkId, objectId);
if (thing != nullptr) { if (thing != nullptr) {
thing->type = objectType; thing->type = objectType;
} }
} }
void NetworkPerception::ReceiveInvestigateMsg(unsigned char *data, void NetworkPerception::ReceiveInvestigateMsg(unsigned char* data,
Roboid *roboid) { Roboid* roboid) {
unsigned char networkId = data[1]; unsigned char networkId = data[1];
unsigned char objectId = data[2]; unsigned char objectId = data[2];
@ -61,7 +62,7 @@ void NetworkPerception::ReceiveInvestigateMsg(unsigned char *data,
if (roboid->modelUrl != nullptr) if (roboid->modelUrl != nullptr)
roboid->networkSync->PublishModel(roboid); roboid->networkSync->PublishModel(roboid);
} else { } else {
InterestingThing *thing = InterestingThing* thing =
roboid->perception->FindTrackedObject(0x00, objectId); roboid->perception->FindTrackedObject(0x00, objectId);
if (thing == nullptr) if (thing == nullptr)
return; return;
@ -70,7 +71,7 @@ void NetworkPerception::ReceiveInvestigateMsg(unsigned char *data,
} }
} }
void NetworkPerception::ReceivePlane(unsigned char *data, Roboid *roboid) { void NetworkPerception::ReceivePlane(unsigned char* data, Roboid* roboid) {
unsigned char networkId = data[1]; unsigned char networkId = data[1];
unsigned char poseType = data[3]; unsigned char poseType = data[3];
if ((poseType & NetworkSync::Pose_Position) == 0 | if ((poseType & NetworkSync::Pose_Position) == 0 |
@ -107,7 +108,7 @@ void NetworkPerception::ReceivePlane(unsigned char *data, Roboid *roboid) {
roboid->perception->AddTrackedObject(this, position, 0x80, networkId); roboid->perception->AddTrackedObject(this, position, 0x80, networkId);
} }
void NetworkPerception::ReceiveSphere(unsigned char *data, Roboid *roboid) { void NetworkPerception::ReceiveSphere(unsigned char* data, Roboid* roboid) {
unsigned char networkId = data[1]; unsigned char networkId = data[1];
float radius = ReceiveFloat100(data, 3); float radius = ReceiveFloat100(data, 3);
@ -126,7 +127,7 @@ void NetworkPerception::ReceiveSphere(unsigned char *data, Roboid *roboid) {
0x81, 0x81, networkId); 0x81, 0x81, networkId);
} }
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];
@ -172,7 +173,7 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
} }
} }
InterestingThing *thing = roboid->perception->AddTrackedObject( InterestingThing* thing = roboid->perception->AddTrackedObject(
this, position, orientation, 0x81, 0x81, networkId); this, position, orientation, 0x81, 0x81, networkId);
if (thing->networkId != 0x00 && thing->type == 0xFF) { if (thing->networkId != 0x00 && thing->type == 0xFF) {
// Unknown thing // Unknown thing
@ -180,11 +181,11 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
} }
} }
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];
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] != if (data[3] !=
(NetworkSync::Pose_Position | NetworkSync::Pose_Orientation)) { (NetworkSync::Pose_Position | NetworkSync::Pose_Orientation)) {
@ -205,7 +206,7 @@ void NetworkPerception::ReceiveTypedObject(unsigned char *data,
} }
} }
Int32 NetworkPerception::ReceiveInt32(unsigned char *data, int startIndex) { Int32 NetworkPerception::ReceiveInt32(unsigned char* data, int startIndex) {
Int32 a = Int32((UInt32)(data[startIndex + 3]) << 24 | Int32 a = Int32((UInt32)(data[startIndex + 3]) << 24 |
(UInt32)(data[startIndex + 2]) << 16 | (UInt32)(data[startIndex + 2]) << 16 |
(UInt32)(data[startIndex + 1]) << 8 | (UInt32)(data[startIndex + 1]) << 8 |
@ -213,13 +214,13 @@ Int32 NetworkPerception::ReceiveInt32(unsigned char *data, int startIndex) {
return a; return a;
} }
float NetworkPerception::ReceiveFloat100(unsigned char *data, int startIndex) { float NetworkPerception::ReceiveFloat100(unsigned char* data, int startIndex) {
Int32 intValue = ReceiveInt32(data, startIndex); Int32 intValue = ReceiveInt32(data, startIndex);
float f = (float)intValue / 100.0F; float f = (float)intValue / 100.0F;
return f; return f;
} }
Vector3 NetworkPerception::ReceiveVector3(unsigned char *data, int startIndex) { Vector3 NetworkPerception::ReceiveVector3(unsigned char* data, int startIndex) {
float x = ReceiveFloat100(data, startIndex); float x = ReceiveFloat100(data, startIndex);
float y = ReceiveFloat100(data, startIndex + 4); float y = ReceiveFloat100(data, startIndex + 4);
float z = ReceiveFloat100(data, startIndex + 8); float z = ReceiveFloat100(data, startIndex + 8);