108 lines
4.0 KiB
C++
108 lines
4.0 KiB
C++
#include "NetworkPerception.h"
|
|
|
|
#include "RoboidControl/NetworkSync.h"
|
|
|
|
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]);
|
|
|
|
if (buffer[0] == NetworkSync::PoseMsg) {
|
|
ReceiveObject(buffer, roboid);
|
|
} else if (buffer[0] == NetworkSync::PoseTypeMsg) {
|
|
ReceiveTypedObject(buffer, roboid);
|
|
}
|
|
}
|
|
|
|
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 |
|
|
(UInt32)(data[startIndex + 0]));
|
|
return a;
|
|
}
|
|
|
|
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) {
|
|
float x = ReceiveFloat100(data, startIndex);
|
|
float y = ReceiveFloat100(data, startIndex + 4);
|
|
float z = ReceiveFloat100(data, startIndex + 8);
|
|
Vector3 v = Vector3(x, y, z);
|
|
return v;
|
|
}
|
|
|
|
void NetworkPerception::ReceiveObject(unsigned char *data, Roboid *roboid) {
|
|
unsigned char objectId = data[1];
|
|
// char poseType = data[2];
|
|
Vector3 worldPosition = ReceiveVector3(data, 3);
|
|
Vector3 worldAngles = ReceiveVector3(data, 15);
|
|
Quaternion worldOrientation = Quaternion::Euler(worldAngles);
|
|
Vector3 worldDirection = worldOrientation * Vector3::forward;
|
|
if (objectId == 0) {
|
|
roboid->SetPosition(worldPosition);
|
|
roboid->SetOrientation(worldOrientation);
|
|
// printf("Received pose (%f, %f) (%f, %f)\n", worldPosition.x,
|
|
// worldPosition.z, worldDirection.x, worldDirection.z);
|
|
|
|
} else {
|
|
Vector3 myPosition = roboid->GetPosition(); // Vector3::zero;
|
|
Vector2 myPosition2 = Vector3::ProjectHorizontalPlane(myPosition);
|
|
Quaternion myOrientation = roboid->GetOrientation();
|
|
Vector3 myDirection = myOrientation * Vector3::forward;
|
|
Vector2 myDirection2 = Vector3::ProjectHorizontalPlane(myDirection);
|
|
|
|
Vector3 localPosition =
|
|
Quaternion::Inverse(myOrientation) * (worldPosition - myPosition);
|
|
|
|
// float distance = Vector3::Distance(myPosition, worldPosition);
|
|
// float angle = Vector3::SignedAngle(myDirection, localPosition,
|
|
// Vector3::up);
|
|
float distance = Vector3::Magnitude(localPosition);
|
|
float angle =
|
|
Vector3::SignedAngle(Vector3::forward, localPosition, Vector3::up);
|
|
|
|
Polar position = Polar(angle, distance);
|
|
|
|
// int objCount = roboid->perception->TrackedObjectCount();
|
|
|
|
// printf("object received at (%f, %f)/(%f, %f) -> (%f %f)\n",
|
|
// worldPosition.x,
|
|
// worldPosition.z, localPosition.x, localPosition.z, distance,
|
|
// angle);
|
|
|
|
roboid->perception->AddTrackedObject(this, position);
|
|
}
|
|
}
|
|
|
|
// HACK!!!
|
|
#include "../../../src/LighthouseTracker.h"
|
|
|
|
void NetworkPerception::ReceiveTypedObject(unsigned char *data,
|
|
Roboid *roboid) {
|
|
unsigned char objectType = data[1];
|
|
unsigned char objectId = data[2];
|
|
if (objectType == 0x02) { // lighthouse
|
|
// We require position and orientation for lighthouses
|
|
if (data[3] !=
|
|
(NetworkSync::Pose_Position | NetworkSync::Pose_Orientation)) {
|
|
return;
|
|
}
|
|
|
|
Vector3 worldPosition = ReceiveVector3(data, 4);
|
|
Vector3 worldAngles = ReceiveVector3(data, 16);
|
|
Quaternion worldOrientation = Quaternion::Euler(worldAngles);
|
|
|
|
// roboid->perception->AddTrackedObject(this, objectType, worldPosition,
|
|
// worldOrientation);
|
|
Sensor *sensor =
|
|
roboid->perception->FindSensorOfType(0x82); // Lighthouse type sensor
|
|
LighthouseTracker *lhSensor = (LighthouseTracker *)sensor;
|
|
lhSensor->UpdateGeometry(objectId, worldPosition, worldOrientation);
|
|
}
|
|
}
|