RoboidControl-cpp/NetworkPerception.cpp

#include "NetworkPerception.h"

#include "NetworkSync.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::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 RC_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;

  if (objectId == 0x00) {
    // They are investigating the roboid itself!
    if (roboid->modelUrl != nullptr)
      roboid->networkSync->PublishModel(roboid);
  } else {
    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);
  Spherical16 position = Spherical16::FromVector3(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, Quaternion::identity,
                                       0x80, 0x80, networkId);
}

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

  float radius = ReceiveFloat100(data, 3);

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

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

  Spherical16 position = Spherical16::FromVector3(localPosition);

  roboid->perception->AddTrackedObject(this, position, Quaternion::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

  Quaternion roboidOrientation = roboid->GetOrientation();
  Spherical16 position = Spherical16::zero;
  Quaternion orientation = Quaternion::identity;

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

      Vector3 localPosition = Quaternion::Inverse(roboidOrientation) *
                              (worldPosition - roboidPosition);
      position = Spherical16::FromVector3(localPosition);
    }
  }

  if ((poseType & NetworkSync::Pose_Orientation) != 0) {
    Vector3 worldAngles = ReceiveVector3(data, 16);
    Quaternion worldOrientation = Quaternion::Euler(worldAngles);
    if (objectId == 0) {
      roboid->SetOrientation(worldOrientation);
    } else {
      orientation = Quaternion::Inverse(roboidOrientation) * worldOrientation;
    }
  }

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

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