From db644af1ca73b94f4d8c8e515b36766e6a0a72c3 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 24 Oct 2024 11:33:33 +0200 Subject: [PATCH] Replace roboid worldpos/orient with worldOriginPos/orient --- NetworkPerception.cpp | 149 ++++++++++++++++++++++++++++++------------ NetworkSync.cpp | 46 ++++++++----- Perception.cpp | 132 +++++++++++++++++++------------------ Roboid.cpp | 30 +++++---- Roboid.h | 25 +++---- Thing.h | 14 ++-- 6 files changed, 239 insertions(+), 157 deletions(-) diff --git a/NetworkPerception.cpp b/NetworkPerception.cpp index 14b0b7e..6772abe 100644 --- a/NetworkPerception.cpp +++ b/NetworkPerception.cpp @@ -37,12 +37,13 @@ void NetworkPerception::ReceiveCreateMsg(unsigned char *data, Roboid *roboid) { if (networkId == roboid->networkSync->networkId) return; - // printf("Received create message [%d/%d]\n", networkId, objectId); + 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, @@ -83,6 +84,7 @@ void NetworkPerception::ReceivePlane(unsigned char *data, Roboid *roboid) { return; Spherical16 worldPosition = Spherical16::FromVector3(ReceiveVector3(data, 4)); + /* Spherical16 roboidPosition = roboid->GetPosition(); Spherical16 deltaPosition = worldPosition - roboidPosition; @@ -101,7 +103,17 @@ void NetworkPerception::ReceivePlane(unsigned char *data, Roboid *roboid) { // 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, @@ -118,6 +130,7 @@ void NetworkPerception::ReceiveSphere(unsigned char *data, Roboid *roboid) { float radius = ReceiveFloat100(data, 3); Spherical16 worldPosition = Spherical16::FromVector3(ReceiveVector3(data, 7)); + /* Spherical16 roboidPosition = roboid->GetPosition(); Spherical16 deltaPosition = worldPosition - roboidPosition; @@ -127,6 +140,17 @@ void NetworkPerception::ReceiveSphere(unsigned char *data, Roboid *roboid) { 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); @@ -154,17 +178,84 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) { 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 + roboid->networkSync->SendInvestigate(thing); + } + } - SwingTwist16 roboidOrientation = roboid->GetOrientation(); + // SwingTwist16 roboidOrientation = roboid->GetOrientation(); Spherical16 position = Spherical16::zero; SwingTwist16 orientation = SwingTwist16::identity; - if ((poseType & NetworkSync::Pose_Position) != 0) { - Spherical16 worldPosition = - Spherical16::FromVector3(ReceiveVector3(data, 4)); + 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->SetPosition(worldPosition); + // 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); @@ -175,41 +266,17 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) { Spherical16 localPosition = SwingTwist16::Inverse(roboidOrientation) * (worldPosition - roboidPosition); - position = localPosition; // Spherical16::FromVector3(localPosition); - // printf(" worldPosition (%f %f %f) localPosition (%f %f %f)\n", - // worldPosition.Right(), worldPosition.Up(), - // worldPosition.Forward(), localPosition.Right(), - // localPosition.Up(), localPosition.Forward()); - // printf(" position: %f (%f %f)\n", position.distance, - // position.horizontal.InDegrees(), position.vertical.InDegrees()); - if (thing != nullptr) - thing->position = position; - } - } + 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()); - if ((poseType & NetworkSync::Pose_Orientation) != 0) { - Vector3 worldAngles = ReceiveVector3(data, 16); - SwingTwist16 worldOrientation = SwingTwist16( - Angle16::Degrees(worldAngles.Up()), - Angle16::Degrees(worldAngles.Right()), - Angle16::Degrees( - worldAngles.Forward())); // Quaternion::Euler(worldAngles); - if (objectId == 0) { - roboid->SetOrientation(worldOrientation); - } else { - orientation = SwingTwist16::Inverse(roboidOrientation) * worldOrientation; - if (thing != nullptr) { - thing->orientation = orientation; - } - } - } - - if (thing == nullptr) { - thing = roboid->perception->AddTrackedObject(this, position, orientation, - 0xFF, objectId, networkId); - if (thing->networkId != 0x00) { - // Unknown thing - roboid->networkSync->SendInvestigate(thing); + thing->position = localPosition; + */ } } } diff --git a/NetworkSync.cpp b/NetworkSync.cpp index 4222f83..bed2c2d 100644 --- a/NetworkSync.cpp +++ b/NetworkSync.cpp @@ -1,6 +1,6 @@ #include "NetworkSync.h" -// #define RC_DEBUG 1 +#define RC_DEBUG 1 #ifdef RC_DEBUG #include @@ -204,8 +204,8 @@ void NetworkSync::SendPose(Thing *thing, bool recurse) { #if RC_DEBUG if (thing->id == 0) - SERIALPORT.printf("Sent PoseMsg Thing [%d/%d]\n", this->networkId, - buffer[1]); + SERIALPORT.printf("Sent PoseMsg Thing [%d/%d] %f\n", this->networkId, + buffer[1], thing->position.distance); #endif } @@ -270,12 +270,33 @@ void NetworkSync::PublishTrackedObject(Roboid *roboid, return; } - Spherical16 roboidPosition = roboid->GetPosition(); - SwingTwist16 roboidOrientation = roboid->GetOrientation(); + /* + Spherical16 roboidPosition = roboid->GetPosition(); + SwingTwist16 roboidOrientation = roboid->GetOrientation(); + Spherical16 worldPosition = + roboidPosition + roboidOrientation * object->position; + SwingTwist16 worldOrientation = roboidOrientation * object->orientation; + */ + + SwingTwist16 inv_originOrientation; + Spherical16 originPosition; + if (roboid->worldOrigin == nullptr) { + inv_originOrientation = SwingTwist16::identity; + originPosition = Spherical16::zero; + } else { + inv_originOrientation = + SwingTwist16::Inverse(roboid->worldOrigin->orientation); + originPosition = roboid->worldOrigin->position; + } + + // SwingTwist16 inv_originOrientation = + // SwingTwist16::Inverse(roboid->worldOrigin->orientation); + // Spherical16 originPosition = roboid->worldOrigin->position; + + SwingTwist16 worldOrientation = inv_originOrientation * object->orientation; Spherical16 worldPosition = - roboidPosition + roboidOrientation * object->position; - SwingTwist16 worldOrientation = roboidOrientation * object->orientation; + inv_originOrientation * (object->position - originPosition); unsigned char ix = 0; buffer[ix++] = PoseMsg; // Position2DMsg; @@ -330,17 +351,6 @@ void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid *roboid) { #endif } -// void NetworkSync::SendDestroyThing(Buffer sendBuffer, InterestingThing* -// thing) { -// unsigned char ix = 0; -// buffer[ix++] = DestroyMsg; -// buffer[ix++] = thing->id; -// SendBuffer(ix); -// #ifdef RC_DEBUG -// printf("Sent DestroyThing [%d/%d]", thing->networkId, thing->id); -// #endif -// } - void NetworkSync::SendInvestigate(InterestingThing *thing) { unsigned char ix = 0; buffer[ix++] = InvestigateMsg; diff --git a/Perception.cpp b/Perception.cpp index 98d878e..23d6160 100644 --- a/Perception.cpp +++ b/Perception.cpp @@ -11,62 +11,60 @@ #include #endif -unsigned char Perception::maxObjectCount = 7; // 7 is typically the maximum - // number of object which can - // be tracked by a human +unsigned char Perception::maxObjectCount = 7; // 7 is typically the maximum + // number of object which can + // be tracked by a human Perception::Perception() { this->sensorCount = 0; - this->sensors = new Sensor*[0]; + this->sensors = new Sensor *[0]; - this->trackedObjects = new InterestingThing*[maxObjectCount]; + this->trackedObjects = new InterestingThing *[maxObjectCount]; for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) this->trackedObjects[objIx] = nullptr; } -Perception::Perception(Sensor** sensors, unsigned int sensorCount) +Perception::Perception(Sensor **sensors, unsigned int sensorCount) : Perception() { this->sensorCount = sensorCount; - this->sensors = new Sensor*[this->sensorCount]; + this->sensors = new Sensor *[this->sensorCount]; for (unsigned char sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) this->sensors[sensorIx] = sensors[sensorIx]; - this->trackedObjects = new InterestingThing*[maxObjectCount]; + this->trackedObjects = new InterestingThing *[maxObjectCount]; for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) this->trackedObjects[objIx] = nullptr; } -unsigned int Perception::GetSensorCount() { - return this->sensorCount; -} +unsigned int Perception::GetSensorCount() { return this->sensorCount; } -Sensor* Perception::GetSensor(unsigned int sensorId) { +Sensor *Perception::GetSensor(unsigned int sensorId) { if (sensorId >= this->sensorCount) return nullptr; - Sensor* sensor = this->sensors[sensorId]; + Sensor *sensor = this->sensors[sensorId]; return sensor; } -unsigned int Perception::AddSensor(Sensor* newSensor) { +unsigned int Perception::AddSensor(Sensor *newSensor) { unsigned int newSensorCount = this->sensorCount + 1; - Sensor** newSensors = new Sensor*[newSensorCount]; + Sensor **newSensors = new Sensor *[newSensorCount]; for (unsigned char sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) newSensors[sensorIx] = sensors[sensorIx]; unsigned int sensorId = this->sensorCount; newSensors[sensorId] = newSensor; - Sensor** oldSensors = this->sensors; + Sensor **oldSensors = this->sensors; this->sensors = newSensors; this->sensorCount = newSensorCount; delete[] oldSensors; return sensorId; } -Sensor* Perception::FindSensorOfType(unsigned int sensorType) { +Sensor *Perception::FindSensorOfType(unsigned int sensorType) { for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { - Sensor* sensor = this->sensors[sensorIx]; + Sensor *sensor = this->sensors[sensorIx]; if (sensor->type == sensorType) return sensor; } @@ -74,8 +72,7 @@ Sensor* Perception::FindSensorOfType(unsigned int sensorType) { return nullptr; } -float GetPlaneDistance(InterestingThing* plane, - float horizontalAngle, +float GetPlaneDistance(InterestingThing *plane, float horizontalAngle, float range) { float distance = plane->position.distance; float deltaAngle = @@ -97,7 +94,7 @@ float GetPlaneDistance(InterestingThing* plane, if (angle > -90) distance = distance / cosf(angle * Passer::LinearAlgebra::Deg2Rad); else - distance = 9999; // infinity? + distance = 9999; // infinity? } else if (deltaAngle > range) { float angle = deltaAngle - range; @@ -107,7 +104,7 @@ float GetPlaneDistance(InterestingThing* plane, if (angle < 90) distance = distance / cosf(angle * Passer::LinearAlgebra::Deg2Rad); else - distance = 9999; // infinity? + distance = 9999; // infinity? } // printf(" distance = %f\n", distance); return distance; @@ -119,11 +116,11 @@ float Perception::GetDistance(float horizontalDirection, float range) { range = -range; for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { - InterestingThing* obj = trackedObjects[objIx]; + InterestingThing *obj = trackedObjects[objIx]; if (obj == nullptr) continue; - if (obj->type == 0x080) { // plane + if (obj->type == 0x080) { // plane float planeDistance = GetPlaneDistance(obj, horizontalDirection, range); minDistance = fminf(minDistance, planeDistance); } else if (obj->position.direction.horizontal.InDegrees() > @@ -137,20 +134,19 @@ float Perception::GetDistance(float horizontalDirection, float range) { } float Perception::GetDistanceOfType(unsigned char thingType, - float horizontalAngle, - float range) { + float horizontalAngle, float range) { float minDistance = INFINITY; if (range < 0) range = -range; for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) { - InterestingThing* thing = trackedObjects[thingIx]; + InterestingThing *thing = trackedObjects[thingIx]; if (thing == nullptr) continue; if (thing->type != thingType) continue; - if (thing->type == 0x080) { // plane + if (thing->type == 0x080) { // plane float planeDistance = GetPlaneDistance(thing, horizontalAngle, range); minDistance = fminf(minDistance, planeDistance); } else if (thing->position.direction.horizontal.InDegrees() > @@ -164,14 +160,13 @@ float Perception::GetDistanceOfType(unsigned char thingType, } float Perception::GetDistance(float horizontalDirection, - float verticalDirection, - float range) { + float verticalDirection, float range) { float minDistance = INFINITY; if (range < 0) range = -range; for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { - InterestingThing* obj = trackedObjects[objIx]; + InterestingThing *obj = trackedObjects[objIx]; if (obj == nullptr) continue; if (obj->position.direction.horizontal.InDegrees() > @@ -189,7 +184,7 @@ bool Perception::ObjectNearby(float direction, float range) { range = -range; for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { - InterestingThing* obj = trackedObjects[objIx]; + InterestingThing *obj = trackedObjects[objIx]; if (obj == nullptr) continue; @@ -202,13 +197,11 @@ bool Perception::ObjectNearby(float direction, float range) { return false; } -InterestingThing* Perception::AddTrackedObject(Sensor* sensor, - Spherical16 position, - SwingTwist16 orientation, - unsigned char thingType, - unsigned char thingId, - unsigned char networkId) { - InterestingThing* thing = new InterestingThing(sensor, position, orientation); +InterestingThing * +Perception::AddTrackedObject(Sensor *sensor, Spherical16 position, + SwingTwist16 orientation, unsigned char thingType, + unsigned char thingId, unsigned char networkId) { + InterestingThing *thing = new InterestingThing(sensor, position, orientation); if (thingId != 0x00) thing->id = thingId; thing->type = thingType; @@ -223,7 +216,7 @@ InterestingThing* Perception::AddTrackedObject(Sensor* sensor, else { if (thing->IsTheSameAs(this->trackedObjects[thingIx])) { this->trackedObjects[thingIx]->Refresh( - thing->position, thing->orientation); //.ToQuaternion()); + thing->position, thing->orientation); //.ToQuaternion()); delete thing; return this->trackedObjects[thingIx]; @@ -244,7 +237,7 @@ InterestingThing* Perception::AddTrackedObject(Sensor* sensor, this->trackedObjects[availableSlotIx] = thing; thing->networkId = networkId; if (thingId == 0x00) - thing->id = lastObjectId++; // availableSlotIx + 1; + thing->id = lastObjectId++; // availableSlotIx + 1; return thing; } // If this object is closer than the farthest object, then replace it @@ -254,7 +247,7 @@ InterestingThing* Perception::AddTrackedObject(Sensor* sensor, this->trackedObjects[farthestObjIx] = thing; thing->networkId = networkId; if (thingId == 0x00) - thing->id = lastObjectId++; // availableSlotIx + 1; + thing->id = lastObjectId++; // availableSlotIx + 1; return thing; } else { // No available slot, delete trackedobject @@ -263,26 +256,26 @@ InterestingThing* Perception::AddTrackedObject(Sensor* sensor, } } -InterestingThing* Perception::AddTrackedObject(Sensor* sensor, +InterestingThing *Perception::AddTrackedObject(Sensor *sensor, unsigned char networkId, unsigned char thingId, Spherical16 position, SwingTwist16 orientation) { - InterestingThing* thing = FindTrackedObject(networkId, thingId); + InterestingThing *thing = FindTrackedObject(networkId, thingId); if (thing == nullptr) { thing = AddTrackedObject(sensor, position, orientation, 0xFF, thingId, networkId); // Don't we set this above already? thing->networkId = networkId; thing->id = thingId; - thing->type = 0xFF; // unknown + thing->type = 0xFF; // unknown } return thing; } bool Perception::IsInteresting(float distance) { for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { - InterestingThing* thing = this->trackedObjects[objIx]; + InterestingThing *thing = this->trackedObjects[objIx]; if (thing == nullptr) return true; if (thing->position.distance > distance) @@ -291,9 +284,9 @@ bool Perception::IsInteresting(float distance) { return false; } -InterestingThing* Perception::FindTrackedObject(char objectId) { +InterestingThing *Perception::FindTrackedObject(char objectId) { for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { - InterestingThing* thing = this->trackedObjects[objIx]; + InterestingThing *thing = this->trackedObjects[objIx]; if (thing == nullptr) continue; if (thing->id == objectId) { @@ -303,10 +296,10 @@ InterestingThing* Perception::FindTrackedObject(char objectId) { return nullptr; } -InterestingThing* Perception::FindTrackedObject(unsigned char networkId, +InterestingThing *Perception::FindTrackedObject(unsigned char networkId, unsigned char objectId) { for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { - InterestingThing* thing = this->trackedObjects[objIx]; + InterestingThing *thing = this->trackedObjects[objIx]; if (thing == nullptr) continue; @@ -326,16 +319,16 @@ unsigned char Perception::TrackedObjectCount() { return objectCount; } -InterestingThing** Perception::GetTrackedObjects() { +InterestingThing **Perception::GetTrackedObjects() { return this->trackedObjects; } unsigned char Perception::ThingsOfType(unsigned char thingType, - InterestingThing* buffer[], + InterestingThing *buffer[], unsigned char bufferSize) { unsigned char thingCount = 0; for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { - InterestingThing* thing = this->trackedObjects[objIx]; + InterestingThing *thing = this->trackedObjects[objIx]; if (thing == nullptr) continue; @@ -353,9 +346,9 @@ unsigned char Perception::ThingsOfType(unsigned char thingType, return thingCount; } -InterestingThing* Perception::ThingOfType(unsigned char thingType) { +InterestingThing *Perception::ThingOfType(unsigned char thingType) { for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { - InterestingThing* thing = this->trackedObjects[objIx]; + InterestingThing *thing = this->trackedObjects[objIx]; if (thing == nullptr) continue; @@ -365,14 +358,14 @@ InterestingThing* Perception::ThingOfType(unsigned char thingType) { return nullptr; } -InterestingThing* Perception::GetMostInterestingThing() { +InterestingThing *Perception::GetMostInterestingThing() { if (this->trackedObjects == nullptr) return nullptr; - InterestingThing* closestObject = nullptr; + InterestingThing *closestObject = nullptr; float closestDistance = INFINITY; for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { - InterestingThing* obj = this->trackedObjects[objIx]; + InterestingThing *obj = this->trackedObjects[objIx]; if (obj != nullptr) { if (obj->position.distance < closestDistance) { closestObject = obj; @@ -392,12 +385,12 @@ void Perception::Update(unsigned long currentTimeMs) { // Update sensing for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { - Sensor* sensor = sensors[sensorIx]; + Sensor *sensor = sensors[sensorIx]; if (sensor == nullptr) continue; if (sensor->type == Thing::DistanceSensorType) { - DistanceSensor* distanceSensor = (DistanceSensor*)sensor; + DistanceSensor *distanceSensor = (DistanceSensor *)sensor; float distance = distanceSensor->GetDistance(); if (distance >= 0) { @@ -411,7 +404,7 @@ void Perception::Update(unsigned long currentTimeMs) { } } else if (sensor->type == Thing::SwitchType) { - Switch* switchSensor = (Switch*)sensor; + Switch *switchSensor = (Switch *)sensor; if (switchSensor->IsOn()) { // Polar position = Polar(sensor->position.angle, nearbyDistance); Angle horizontal = Angle::Degrees(horizontal.InDegrees()); @@ -424,7 +417,7 @@ void Perception::Update(unsigned long currentTimeMs) { } for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { - InterestingThing* thing = trackedObjects[objIx]; + InterestingThing *thing = trackedObjects[objIx]; if (thing == nullptr) continue; @@ -439,15 +432,16 @@ void Perception::Update(unsigned long currentTimeMs) { } } +#include void Perception::UpdatePose(Polar16 translation) { for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) { - InterestingThing* thing = trackedObjects[thingIx]; + InterestingThing *thing = trackedObjects[thingIx]; if (thing == nullptr) continue; // We only support translations in the horizontal plane at this moment... // This needs Spherical operator- to be implemented to work in 3d space - if (thing->type == 0x80) { // plane + if (thing->type == 0x80) { // plane // printf("[1/%d] %f (%f %f) ", thing->id, thing->position.distance, // (float)thing->position.horizontalAngle, // (float)thing->position.verticalAngle); @@ -469,10 +463,18 @@ void Perception::UpdatePose(Polar16 translation) { // // obj->position.ProjectOnHorizontalPlane(); // Spherical16 newPosition = Spherical16(horizontalPosition - // translation); + Spherical16 translationS = Spherical16( translation.distance, Angle16::Degrees(translation.angle.InDegrees()), Angle16::Degrees(0)); Spherical16 newPosition = thing->position + translationS; + + Vector3 oldPos = thing->position.ToVector3(); + Vector3 newPos = newPosition.ToVector3(); + printf(" update percepted position (%f 0 %f) -> (%f 0 %f)\n", + oldPos.Right(), oldPos.Forward(), newPos.Right(), + newPos.Forward()); + thing->position = newPosition; } } @@ -489,7 +491,7 @@ void Perception::UpdatePose(SwingTwist16 rotation) { Angle16 rotationAngle = rotation.swing.horizontal; for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) { - InterestingThing* thing = trackedObjects[thingIx]; + InterestingThing *thing = trackedObjects[thingIx]; if (thing == nullptr) continue; diff --git a/Roboid.cpp b/Roboid.cpp index 032f01a..9156bfb 100644 --- a/Roboid.cpp +++ b/Roboid.cpp @@ -21,8 +21,8 @@ Roboid::Roboid() : Thing(0) { this->propulsion = nullptr; this->networkSync = nullptr; - this->worldPosition = Spherical16::zero; - this->worldOrientation = SwingTwist16::identity; + this->roboidPosition = Spherical16::zero; + this->roboidOrientation = SwingTwist16::identity; } Roboid::Roboid(Propulsion *propulsion) : Roboid() { @@ -33,7 +33,6 @@ Roboid::Roboid(Propulsion *propulsion) : Roboid() { void Roboid::SetName(const char *name) { this->name = name; } -#include void Roboid::Update(unsigned long currentTimeMs) { if (perception != nullptr) perception->Update(currentTimeMs); @@ -52,9 +51,12 @@ void Roboid::Update(unsigned long currentTimeMs) { this->angularVelocity = Spherical16(oldAngular, Angle16(), Angle16::Degrees(90)); - SetPosition(this->position + this->orientation * Spherical16::forward * - this->linearVelocity.distance * deltaTime); - this->worldPosition = this->position; // assuming the roboid is the root + // SetPosition(this->position + this->orientation * Spherical16::forward * + // this->linearVelocity.distance * + // deltaTime); + // this->roboidPosition = this->position; // assuming the roboid is the root + this->position += this->orientation * Spherical16::forward * + this->linearVelocity.distance * deltaTime; SwingTwist16 rotation = SwingTwist16::AngleAxis( this->angularVelocity.distance * deltaTime, Direction16::up); @@ -63,7 +65,7 @@ void Roboid::Update(unsigned long currentTimeMs) { this->orientation = this->orientation * rotation; - this->worldOrientation = + this->roboidOrientation = this->orientation; // assuming the roboid is the root } @@ -78,20 +80,24 @@ void Roboid::Update(unsigned long currentTimeMs) { lastUpdateTimeMs = currentTimeMs; } -Spherical16 Roboid::GetPosition() { return this->worldPosition; } +// Spherical16 Roboid::GetPosition() { return this->roboidPosition; } -SwingTwist16 Roboid::GetOrientation() { return this->worldOrientation; } +// SwingTwist16 Roboid::GetOrientation() { return this->roboidOrientation; } +/* void Roboid::SetPosition(Spherical16 newWorldPosition) { SwingTwist16 roboidOrientation = this->GetOrientation(); - Spherical16 translation = newWorldPosition - this->worldPosition; + Spherical16 translation = newWorldPosition - this->roboidPosition; float distance = translation.distance; Angle16 angle = Spherical16::SignedAngleBetween( roboidOrientation * Spherical16::forward, translation, Spherical16::up); Polar16 polarTranslation = Polar16( distance, angle); // Polar(angle.InDegrees(), Angle::Degrees(distance)); - if (perception != nullptr) + if (perception != nullptr) { + printf("roboid translation %f, %f\n", polarTranslation.distance, + polarTranslation.angle.InDegrees()); perception->UpdatePose(polarTranslation); + } this->position = newWorldPosition; // roboid is the root? // World position should be set in the update recursion @@ -111,7 +117,7 @@ void Roboid::SetOrientation(SwingTwist16 newOrientation) { this->orientation = newOrientation; } - +*/ void Roboid::AddChild(Thing *child) { Thing::AddChild(child); if (child->IsSensor()) { diff --git a/Roboid.h b/Roboid.h index 0d40845..e7cd8a6 100644 --- a/Roboid.h +++ b/Roboid.h @@ -42,14 +42,14 @@ public: /// @details The origin and units of the position depends on the position /// tracking system used. This value will be Vector3::zero unless a position /// is received through network synchronisation - virtual Spherical16 GetPosition(); + // virtual Spherical16 GetPosition(); // Vector2 GetPosition2D(); /// @brief Retrieve the current orientation of the roboid /// @return The orientation quaternion in world space /// @details The origin orientation depends on the position tracking system /// used. This value will be Quaternion::identity unless an orientation is /// received though network synchronization - virtual SwingTwist16 GetOrientation(); + // virtual SwingTwist16 GetOrientation(); /// @brief Update the current position of the roboid /// @param worldPosition The position of the roboid in carthesian coordinates @@ -58,31 +58,22 @@ public: /// orientations of the perceived objects by the roboid /// (roboid->perception->perceivedObjects), as these are local to the /// roboid's position. - virtual void SetPosition(Spherical16 worldPosition); + // virtual void SetPosition(Spherical16 worldPosition); /// @brief Update the current orientation of the roboid /// @param worldOrientation The orientation of the roboid in world space /// @details The use of this function will also update the orientations of the /// perceived objects by the roboid (roboid->perception->perceivedObjets), /// as these are local to the roboid' orientation. - virtual void SetOrientation(SwingTwist16 worldOrientation); + // virtual void SetOrientation(SwingTwist16 worldOrientation); virtual void AddChild(Thing *child) override; void Release(Thing *child); + Thing *worldOrigin = + nullptr; // thing to track the world origin to be able to transform + // world coordinates into roboid or local coordinates + // Perhaps this will move to perception at some point private: - /// @brief The position of the roboid in carthesian coordinates in world space - /// @details This position may be set when NetworkSync is used to receive - /// positions from an external tracking system. These values should not be set - /// directly, but SetPosition should be used instead. - // Spherical16 worldPosition = Spherical16::zero; - /// @brief The orientation of the roboid in world space - /// @details The position may be set when NetworkSync is used to receive - /// orientations from an external tracking system. This value should not be - /// set directly, but SetOrientation should be used instead. - // Quaternion worldOrientation = Quaternion::identity; - - // AngleAxis worldAngleAxis = AngleAxis(); - unsigned long lastUpdateTimeMs = 0; }; diff --git a/Thing.h b/Thing.h index face2bf..9211945 100644 --- a/Thing.h +++ b/Thing.h @@ -43,16 +43,22 @@ public: /// @return True when the Thing is a Roboid and False otherwise bool IsRoboid(); - /// @brief The position of this Thing + /// @brief The position in local space /// @remark When this Thing has a parent, the position is relative to the /// parent's position and orientation Spherical16 position; - Spherical16 worldPosition; - /// @brief The orientation of this Thing + /// @brief The position in roboid space + /// @remark This is the position relative to the root of the roboid, + /// or the Roboid itself. + Spherical16 roboidPosition; + /// @brief The orientation in local space /// @remark When this Thing has a parent, the orientation is relative to the /// parent's orientation SwingTwist16 orientation; - SwingTwist16 worldOrientation; + /// @brief The orientation in roboid space + /// @remark This is the orientation relative to the root of the roboid, + /// or the Roboid itself. + SwingTwist16 roboidOrientation; virtual Spherical16 GetLinearVelocity(); virtual Spherical16 GetAngularVelocity();