RoboidControl-cpp/NetworkPerception.cpp

#include "NetworkPerception.h"

#include "RoboidControl/NetworkSync.h"

// #define DEBUG true
#if DEBUG
#include <Arduino.h>
#endif

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;
  }
}

void NetworkPerception::ReceiveCreateMsg(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;
  }
}

void NetworkPerception::ReceiveInvestigateMsg(unsigned char *data,
                                              Roboid *roboid) {
  unsigned char networkId = data[1];
  unsigned char objectId = data[2];

#if DEBUG
  printf("Received InvestigateMsg [%d/%d]\n", networkId, objectId);
#endif

  if (networkId != roboid->networkSync->networkId)
    // We only response to investigation requests for our own objects
    return;

  InterestingThing *thing =
      roboid->perception->FindTrackedObject(0x00, objectId);
  if (thing == nullptr)
    return;

  roboid->networkSync->NewObject(thing);
}

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;

  Vector3 worldPosition = ReceiveVector3(data, 4);
  Vector3 roboidPosition = roboid->GetPosition();
  Vector3 deltaPosition = worldPosition - roboidPosition;

  float distance = deltaPosition.magnitude();
  if (roboid->perception->IsInteresting(distance) == false) {
    // printf("  plane not interesting\n");
    return;
  }

  Quaternion roboidOrientation = roboid->GetOrientation();
  Vector3 localPosition =
      Quaternion::Inverse(roboidOrientation) * deltaPosition;
  // float angle =
  //     Vector3::SignedAngle(Vector3::forward, localPosition, Vector3::up);

  // Polar position = Polar(angle, distance);
  Spherical position = Spherical(localPosition);

  // 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, 0x80, networkId);
}

void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
  unsigned char networkId = data[1];
  unsigned char objectId = data[2];
  unsigned char poseType = data[3];
  // printf("My network id == %d\n", roboid->networkSync->networkId);
  if (networkId == roboid->networkSync->networkId)
    networkId = 0x00;
  // printf("Received pose message [%d/%d]\n", networkId, objectId);

  if (objectId == 0x80)
    return ReceivePlane(data, roboid);

  if ((poseType & NetworkSync::Pose_Position) != 0) {
    Vector3 worldPosition = ReceiveVector3(data, 4);
    if (objectId == 0) {
      roboid->SetPosition(worldPosition);
    } else {
      Vector3 roboidPosition = roboid->GetPosition();

      float distance = Vector3::Distance(roboidPosition, worldPosition);
      if (roboid->perception->IsInteresting(distance) == false) {
        // printf("  not interesting\n");
        return;
      }

      Quaternion roboidOrientation = roboid->GetOrientation();
      Vector3 localPosition = Quaternion::Inverse(roboidOrientation) *
                              (worldPosition - roboidPosition);

      float angle =
          Vector3::SignedAngle(Vector3::forward, localPosition, Vector3::up);

      Polar position = Polar(distance, angle); // Polar(angle, distance);
      /*
      Vector2 worldPosition2D = Vector2(worldPosition);

      Vector2 myPosition = roboid->GetPosition2D();
      float myOrientation = roboid->GetOrientation2D();

      Vector2 localPosition =
          Vector2::Rotate(worldPosition2D - myPosition, -myOrientation);

      float distance = Vector2::Magnitude(localPosition);
      float angle = Vector2::SignedAngle(Vector3::forward, localPosition);
      Polar position = Polar(angle, distance);
      */

      // printf("object received at (%f, %f)/(%f, %f) -> (%f %f)\n",
      //        worldPosition.x, worldPosition.z, localPosition.x,
      //        localPosition.z, distance, angle);

      InterestingThing *thing = roboid->perception->AddTrackedObject(
          this, networkId, objectId, position);
      if (thing->networkId != 0x00 && thing->type == 0xFF) {
        // Unknown thing
        roboid->networkSync->SendInvestigateThing(thing);
      }
    }
  }
}

// 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);
  }
}

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;
}