RoboidControl/RoboidControl-cpp
RoboidControl/RoboidControl-cpp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Replace roboid worldpos/orient with worldOriginPos/orient

Browse Source
This commit is contained in:
Pascal Serrarens 2024-10-24 11:33:33 +02:00
parent 9b6bce5155
commit db644af1ca
6 changed files with 239 additions and 157 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

149
NetworkPerception.cpp
View File

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

46
NetworkSync.cpp
View File

@ -1,6 +1,6 @@
#include "NetworkSync.h"
// #define RC_DEBUG 1
#define RC_DEBUG 1
#ifdef RC_DEBUG
#include <Arduino.h>
@ -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;

132
Perception.cpp
View File

@ -11,62 +11,60 @@
#include <Arduino.h>
#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 <Arduino.h>
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;

30
Roboid.cpp
View File

@ -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 <Arduino.h>
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()) {

25
Roboid.h
View File

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

14
Thing.h
View File

@ -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();