350 lines
12 KiB
C++
350 lines
12 KiB
C++
#include "NetworkPerception.h"
|
|
|
|
#include "ControlCore/Messages.h"
|
|
#include "NetworkSync.h"
|
|
#include "float16/float16.h"
|
|
|
|
#define RC_DEBUG true
|
|
#if RC_DEBUG
|
|
#include <Arduino.h>
|
|
#endif
|
|
|
|
void NetworkPerception::ProcessPacket(Roboid *roboid, unsigned char *buffer,
|
|
int packetsize) {
|
|
#if RC_DEBUG
|
|
// printf("packet received, type = 0x%02x %d %d %d %d %d\n", buffer[0],
|
|
// buffer[2], buffer[2], buffer[3], buffer[4], buffer[5]);
|
|
#endif
|
|
|
|
switch (buffer[0]) {
|
|
case NetworkSync::ThingMsg:
|
|
ReceiveThingMsg(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::ReceiveThingMsg(unsigned char *data, Roboid *roboid) {
|
|
unsigned char networkId = data[1];
|
|
unsigned char objectId = data[2];
|
|
unsigned char objectType = data[3];
|
|
if (networkId == roboid->networkSync->networkId)
|
|
return;
|
|
|
|
printf("Received create message [%d/%d]\n", networkId, objectId);
|
|
InterestingThing *thing =
|
|
roboid->perception->FindTrackedObject(networkId, objectId);
|
|
if (thing != nullptr) {
|
|
thing->type = objectType;
|
|
}
|
|
printf("complete\n");
|
|
}
|
|
|
|
void NetworkPerception::ReceiveInvestigateMsg(unsigned char *data,
|
|
Roboid *roboid) {
|
|
unsigned char networkId = data[1];
|
|
unsigned char thingId = data[2];
|
|
|
|
// #if RC_DEBUG
|
|
printf("Received InvestigateMsg [%d/%d]\n", networkId, thingId);
|
|
// #endif
|
|
|
|
if (networkId != roboid->networkSync->networkId)
|
|
// We only response to investigation requests for our own objects
|
|
return;
|
|
|
|
if (thingId == 0x00) {
|
|
// They are investigating the roboid itself!
|
|
if (roboid->modelUrl != nullptr)
|
|
// roboid->networkSync->SendModel(roboid);
|
|
roboid->networkSync->SendThingInfo(roboid);
|
|
} else {
|
|
Thing *thing = roboid->FindChild(thingId, true);
|
|
if (thing != nullptr)
|
|
roboid->networkSync->SendThingInfo(thing);
|
|
else {
|
|
InterestingThing *interestingThing =
|
|
roboid->perception->FindTrackedObject(0x00, thingId);
|
|
if (interestingThing == nullptr)
|
|
return;
|
|
|
|
roboid->networkSync->SendThingInfo(interestingThing, false);
|
|
}
|
|
}
|
|
}
|
|
|
|
void NetworkPerception::ReceivePlane(unsigned char *data, Roboid *roboid) {
|
|
unsigned char networkId = data[1];
|
|
unsigned char poseType = data[3];
|
|
if ((poseType & NetworkSync::Pose_Position) == 0 ||
|
|
(poseType & NetworkSync::Pose_Orientation) == 0)
|
|
|
|
// both position and orientation are required
|
|
return;
|
|
|
|
Spherical16 worldPosition = Spherical16::FromVector3(ReceiveVector3(data, 4));
|
|
/*
|
|
Spherical16 roboidPosition = roboid->GetPosition();
|
|
Spherical16 deltaPosition = worldPosition - roboidPosition;
|
|
|
|
float distance = deltaPosition.distance; // magnitude();
|
|
if (roboid->perception->IsInteresting(distance) == false) {
|
|
// printf(" plane not interesting\n");
|
|
return;
|
|
}
|
|
|
|
SwingTwist16 roboidOrientation = roboid->GetOrientation();
|
|
Spherical16 localPosition =
|
|
SwingTwist16::Inverse(roboidOrientation) * deltaPosition;
|
|
// float angle =
|
|
// Vector3::SignedAngle(Vector3::forward, localPosition, Vector3::up);
|
|
|
|
// Polar position = Polar(angle, distance);
|
|
Spherical16 position =
|
|
localPosition; // Spherical16::FromVector3(localPosition);
|
|
*/
|
|
SwingTwist16 originOrientation;
|
|
Spherical16 originPosition;
|
|
if (roboid->worldOrigin == nullptr) {
|
|
originOrientation = SwingTwist16::identity;
|
|
originPosition = Spherical16::zero;
|
|
} else {
|
|
originOrientation = roboid->worldOrigin->orientation;
|
|
originPosition = roboid->worldOrigin->position;
|
|
}
|
|
Spherical16 position = originPosition + originOrientation * worldPosition;
|
|
// printf("Received plane (%f %f %f) (%f %f %f) %f %f %f\n",
|
|
// worldPosition.x,
|
|
// worldPosition.y, worldPosition.z, roboidPosition.x,
|
|
// roboidPosition.y, roboidPosition.z, position.distance,
|
|
// (float)position.horizontalAngle, (float)position.verticalAngle);
|
|
// printf("Received plane %f (%f %f)\n", position.distance,
|
|
// (float)position.horizontalAngle, (float)position.verticalAngle);
|
|
roboid->perception->AddTrackedObject(this, position, SwingTwist16::identity,
|
|
0x80, 0x80, networkId);
|
|
}
|
|
|
|
void NetworkPerception::ReceiveSphere(unsigned char *data, Roboid *roboid) {
|
|
unsigned char networkId = data[1];
|
|
|
|
float radius = ReceiveFloat100(data, 3);
|
|
|
|
Spherical16 worldPosition = Spherical16::FromVector3(ReceiveVector3(data, 7));
|
|
/*
|
|
Spherical16 roboidPosition = roboid->GetPosition();
|
|
Spherical16 deltaPosition = worldPosition - roboidPosition;
|
|
|
|
SwingTwist16 roboidOrientation = roboid->GetOrientation();
|
|
Spherical16 localPosition =
|
|
SwingTwist16::Inverse(roboidOrientation) * deltaPosition;
|
|
|
|
Spherical16 position =
|
|
localPosition; // Spherical16::FromVector3(localPosition);
|
|
*/
|
|
SwingTwist16 originOrientation;
|
|
Spherical16 originPosition;
|
|
if (roboid->worldOrigin == nullptr) {
|
|
originOrientation = SwingTwist16::identity;
|
|
originPosition = Spherical16::zero;
|
|
} else {
|
|
originOrientation = roboid->worldOrigin->orientation;
|
|
originPosition = roboid->worldOrigin->position;
|
|
}
|
|
Spherical16 position = originPosition + originOrientation * worldPosition;
|
|
|
|
roboid->perception->AddTrackedObject(this, position, SwingTwist16::identity,
|
|
0x81, 0x81, networkId);
|
|
}
|
|
|
|
void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
|
|
if (roboid->networkSync->networkId == 0) // We're not connected to a site yet
|
|
return;
|
|
|
|
unsigned char networkId = data[1];
|
|
unsigned char objectId = data[2];
|
|
unsigned char poseType = data[3];
|
|
|
|
if (networkId == roboid->networkSync->networkId)
|
|
networkId = 0x00;
|
|
|
|
// if (objectId == 0x80)
|
|
// return ReceivePlane(data, roboid);
|
|
// else if (objectId == 0x81)
|
|
// return ReceiveSphere(data, roboid);
|
|
|
|
#if RC_DEBUG
|
|
// printf("Received PoseMsg [%d/%d]\n", networkId, objectId);
|
|
#endif
|
|
|
|
// InterestingThing *thing =
|
|
// roboid->perception->FindTrackedObject(networkId, objectId);
|
|
// if (thing == nullptr) {
|
|
// thing = roboid->perception->AddTrackedObject(this, position, orientation,
|
|
// 0xFF, objectId, networkId);
|
|
// if (thing->networkId != 0x00) {
|
|
// // Unknown thing
|
|
// InvestigateMsg::Send(thing);
|
|
// // roboid->networkSync->SendInvestigate(thing);
|
|
// }
|
|
// }
|
|
|
|
// SwingTwist16 roboidOrientation = roboid->GetOrientation();
|
|
// Spherical16 position = Spherical16::zero;
|
|
// SwingTwist16 orientation = SwingTwist16::identity;
|
|
|
|
// Vector3 worldAngles = ReceiveVector3(data, 16);
|
|
// SwingTwist16 worldOrientation = SwingTwist16(
|
|
// Angle16::Degrees(worldAngles.Up()),
|
|
// Angle16::Degrees(worldAngles.Right()), Angle16::Degrees(
|
|
// worldAngles.Forward())); // Quaternion::Euler(worldAngles);
|
|
|
|
// if ((poseType & NetworkSync::Pose_Orientation) != 0) {
|
|
// if (objectId == 0) {
|
|
// // roboid->SetOrientation(worldOrientation);
|
|
// if (roboid->worldOrigin == nullptr) {
|
|
// printf("creating new origin\n");
|
|
// roboid->worldOrigin = new Thing(0);
|
|
// printf("created\n");
|
|
// }
|
|
|
|
// roboid->worldOrigin->orientation =
|
|
// SwingTwist16::Inverse(worldOrientation);
|
|
// } else {
|
|
// // orientation = SwingTwist16::Inverse(roboidOrientation) *
|
|
// // worldOrientation; if (thing != nullptr) {
|
|
// // thing->orientation = orientation;
|
|
// // }
|
|
// SwingTwist16 originOrientation;
|
|
// Spherical16 originPosition;
|
|
// if (roboid->worldOrigin == nullptr) {
|
|
// originOrientation = SwingTwist16::identity;
|
|
// originPosition = Spherical16::zero;
|
|
// } else {
|
|
// originOrientation = roboid->worldOrigin->orientation;
|
|
// originPosition = roboid->worldOrigin->position;
|
|
// }
|
|
|
|
// thing->orientation = originOrientation * worldOrientation;
|
|
// }
|
|
// }
|
|
|
|
// if ((poseType & NetworkSync::Pose_Position) != 0) {
|
|
// Vector3 worldVector3 = ReceiveVector3(data, 4);
|
|
// Spherical16 worldPosition = Spherical16::FromVector3(worldVector3);
|
|
// if (objectId == 0) {
|
|
// // roboid->SetPosition(worldPosition);
|
|
// if (roboid->worldOrigin == nullptr) {
|
|
// printf("creating new origin again\n");
|
|
// roboid->worldOrigin = new Thing(0);
|
|
// }
|
|
|
|
// roboid->worldOrigin->position =
|
|
// roboid->worldOrigin->orientation * -worldPosition;
|
|
// } else {
|
|
// SwingTwist16 originOrientation;
|
|
// Spherical16 originPosition;
|
|
// if (roboid->worldOrigin == nullptr) {
|
|
// originOrientation = SwingTwist16::identity;
|
|
// originPosition = Spherical16::zero;
|
|
// } else {
|
|
// originOrientation = roboid->worldOrigin->orientation;
|
|
// originPosition = roboid->worldOrigin->position;
|
|
// }
|
|
|
|
// // SwingTwist16 roboidLocalOrientation =
|
|
// // originOrientation * worldOrientation;
|
|
|
|
// thing->position = originPosition + originOrientation * worldPosition;
|
|
|
|
// /*
|
|
// Spherical16 roboidPosition = roboid->GetPosition();
|
|
|
|
// // float distance = Vector3::Distance(roboidPosition, worldPosition);
|
|
// // if (roboid->perception->IsInteresting(distance) == false) {
|
|
// // // printf(" not interesting\n");
|
|
// // return;
|
|
// // }
|
|
|
|
// Spherical16 localPosition = SwingTwist16::Inverse(roboidOrientation) *
|
|
// (worldPosition - roboidPosition);
|
|
// Vector3 roboidVector3 = roboidPosition.ToVector3();
|
|
// printf(" roboid position (%f %f %f)\n", roboidVector3.Right(),
|
|
// roboidVector3.Up(), roboidVector3.Forward());
|
|
// printf(" [%d/%d] worldPosition (%f %f %f) localPosition %f (%f %f)\n
|
|
// ",
|
|
// networkId, objectId, worldVector3.Right(), worldVector3.Up(),
|
|
// worldVector3.Forward(), localPosition.distance,
|
|
// localPosition.direction.horizontal.InDegrees(),
|
|
// localPosition.direction.vertical.InDegrees());
|
|
|
|
// thing->position = localPosition;
|
|
// */
|
|
// }
|
|
// }
|
|
}
|
|
|
|
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);
|
|
}
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
float NetworkPerception::ReceiveFloat16(unsigned char *data,
|
|
unsigned char *startIndex) {
|
|
byte ix = *startIndex;
|
|
unsigned short value = data[ix++] << 8 | data[ix];
|
|
float16 f = float16();
|
|
f.setBinary(value);
|
|
|
|
*startIndex = ix;
|
|
return f.toDouble();
|
|
} |