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