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) if (networkId == roboid->networkSync->networkId)
return; return;
// printf("Received create message [%d/%d]\n", networkId, objectId); printf("Received create message [%d/%d]\n", networkId, objectId);
InterestingThing *thing = InterestingThing *thing =
roboid->perception->FindTrackedObject(networkId, objectId); roboid->perception->FindTrackedObject(networkId, objectId);
if (thing != nullptr) { if (thing != nullptr) {
thing->type = objectType; thing->type = objectType;
} }
printf("complete\n");
} }
void NetworkPerception::ReceiveInvestigateMsg(unsigned char *data, void NetworkPerception::ReceiveInvestigateMsg(unsigned char *data,
@ -83,6 +84,7 @@ void NetworkPerception::ReceivePlane(unsigned char *data, Roboid *roboid) {
return; return;
Spherical16 worldPosition = Spherical16::FromVector3(ReceiveVector3(data, 4)); Spherical16 worldPosition = Spherical16::FromVector3(ReceiveVector3(data, 4));
/*
Spherical16 roboidPosition = roboid->GetPosition(); Spherical16 roboidPosition = roboid->GetPosition();
Spherical16 deltaPosition = worldPosition - roboidPosition; Spherical16 deltaPosition = worldPosition - roboidPosition;
@ -101,7 +103,17 @@ void NetworkPerception::ReceivePlane(unsigned char *data, Roboid *roboid) {
// Polar position = Polar(angle, distance); // Polar position = Polar(angle, distance);
Spherical16 position = Spherical16 position =
localPosition; // Spherical16::FromVector3(localPosition); 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, // printf("Received plane (%f %f %f) (%f %f %f) %f %f %f\n", worldPosition.x,
// worldPosition.y, worldPosition.z, roboidPosition.x, // worldPosition.y, worldPosition.z, roboidPosition.x,
// roboidPosition.y, roboidPosition.z, position.distance, // roboidPosition.y, roboidPosition.z, position.distance,
@ -118,6 +130,7 @@ void NetworkPerception::ReceiveSphere(unsigned char *data, Roboid *roboid) {
float radius = ReceiveFloat100(data, 3); float radius = ReceiveFloat100(data, 3);
Spherical16 worldPosition = Spherical16::FromVector3(ReceiveVector3(data, 7)); Spherical16 worldPosition = Spherical16::FromVector3(ReceiveVector3(data, 7));
/*
Spherical16 roboidPosition = roboid->GetPosition(); Spherical16 roboidPosition = roboid->GetPosition();
Spherical16 deltaPosition = worldPosition - roboidPosition; Spherical16 deltaPosition = worldPosition - roboidPosition;
@ -127,6 +140,17 @@ void NetworkPerception::ReceiveSphere(unsigned char *data, Roboid *roboid) {
Spherical16 position = Spherical16 position =
localPosition; // Spherical16::FromVector3(localPosition); 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, roboid->perception->AddTrackedObject(this, position, SwingTwist16::identity,
0x81, 0x81, networkId); 0x81, 0x81, networkId);
@ -154,17 +178,84 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
InterestingThing *thing = InterestingThing *thing =
roboid->perception->FindTrackedObject(networkId, objectId); 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; Spherical16 position = Spherical16::zero;
SwingTwist16 orientation = SwingTwist16::identity; SwingTwist16 orientation = SwingTwist16::identity;
if ((poseType & NetworkSync::Pose_Position) != 0) { Vector3 worldAngles = ReceiveVector3(data, 16);
Spherical16 worldPosition = SwingTwist16 worldOrientation = SwingTwist16(
Spherical16::FromVector3(ReceiveVector3(data, 4)); Angle16::Degrees(worldAngles.Up()), Angle16::Degrees(worldAngles.Right()),
Angle16::Degrees(
worldAngles.Forward())); // Quaternion::Euler(worldAngles);
if ((poseType & NetworkSync::Pose_Orientation) != 0) {
if (objectId == 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 { } 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(); Spherical16 roboidPosition = roboid->GetPosition();
// float distance = Vector3::Distance(roboidPosition, worldPosition); // float distance = Vector3::Distance(roboidPosition, worldPosition);
@ -175,41 +266,17 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
Spherical16 localPosition = SwingTwist16::Inverse(roboidOrientation) * Spherical16 localPosition = SwingTwist16::Inverse(roboidOrientation) *
(worldPosition - roboidPosition); (worldPosition - roboidPosition);
position = localPosition; // Spherical16::FromVector3(localPosition); Vector3 roboidVector3 = roboidPosition.ToVector3();
// printf(" worldPosition (%f %f %f) localPosition (%f %f %f)\n", printf(" roboid position (%f %f %f)\n", roboidVector3.Right(),
// worldPosition.Right(), worldPosition.Up(), roboidVector3.Up(), roboidVector3.Forward());
// worldPosition.Forward(), localPosition.Right(), printf(" [%d/%d] worldPosition (%f %f %f) localPosition %f (%f %f)\n ",
// localPosition.Up(), localPosition.Forward()); networkId, objectId, worldVector3.Right(), worldVector3.Up(),
// printf(" position: %f (%f %f)\n", position.distance, worldVector3.Forward(), localPosition.distance,
// position.horizontal.InDegrees(), position.vertical.InDegrees()); localPosition.direction.horizontal.InDegrees(),
if (thing != nullptr) localPosition.direction.vertical.InDegrees());
thing->position = position;
}
}
if ((poseType & NetworkSync::Pose_Orientation) != 0) { thing->position = localPosition;
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);
} }
} }
} }

38
NetworkSync.cpp
View File

@ -1,6 +1,6 @@
#include "NetworkSync.h" #include "NetworkSync.h"
// #define RC_DEBUG 1 #define RC_DEBUG 1
#ifdef RC_DEBUG #ifdef RC_DEBUG
#include <Arduino.h> #include <Arduino.h>
@ -204,8 +204,8 @@ void NetworkSync::SendPose(Thing *thing, bool recurse) {
#if RC_DEBUG #if RC_DEBUG
if (thing->id == 0) if (thing->id == 0)
SERIALPORT.printf("Sent PoseMsg Thing [%d/%d]\n", this->networkId, SERIALPORT.printf("Sent PoseMsg Thing [%d/%d] %f\n", this->networkId,
buffer[1]); buffer[1], thing->position.distance);
#endif #endif
} }
@ -270,12 +270,33 @@ void NetworkSync::PublishTrackedObject(Roboid *roboid,
return; return;
} }
/*
Spherical16 roboidPosition = roboid->GetPosition(); Spherical16 roboidPosition = roboid->GetPosition();
SwingTwist16 roboidOrientation = roboid->GetOrientation(); SwingTwist16 roboidOrientation = roboid->GetOrientation();
Spherical16 worldPosition = Spherical16 worldPosition =
roboidPosition + roboidOrientation * object->position; roboidPosition + roboidOrientation * object->position;
SwingTwist16 worldOrientation = roboidOrientation * object->orientation; 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 =
inv_originOrientation * (object->position - originPosition);
unsigned char ix = 0; unsigned char ix = 0;
buffer[ix++] = PoseMsg; // Position2DMsg; buffer[ix++] = PoseMsg; // Position2DMsg;
@ -330,17 +351,6 @@ void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid *roboid) {
#endif #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) { void NetworkSync::SendInvestigate(InterestingThing *thing) {
unsigned char ix = 0; unsigned char ix = 0;
buffer[ix++] = InvestigateMsg; buffer[ix++] = InvestigateMsg;

108
Perception.cpp
View File

@ -17,56 +17,54 @@ unsigned char Perception::maxObjectCount = 7; // 7 is typically the maximum
Perception::Perception() { Perception::Perception() {
this->sensorCount = 0; 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++) for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++)
this->trackedObjects[objIx] = nullptr; this->trackedObjects[objIx] = nullptr;
} }
Perception::Perception(Sensor** sensors, unsigned int sensorCount) Perception::Perception(Sensor **sensors, unsigned int sensorCount)
: Perception() { : Perception() {
this->sensorCount = sensorCount; this->sensorCount = sensorCount;
this->sensors = new Sensor*[this->sensorCount]; this->sensors = new Sensor *[this->sensorCount];
for (unsigned char sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) for (unsigned char sensorIx = 0; sensorIx < this->sensorCount; sensorIx++)
this->sensors[sensorIx] = sensors[sensorIx]; this->sensors[sensorIx] = sensors[sensorIx];
this->trackedObjects = new InterestingThing*[maxObjectCount]; this->trackedObjects = new InterestingThing *[maxObjectCount];
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++)
this->trackedObjects[objIx] = nullptr; this->trackedObjects[objIx] = nullptr;
} }
unsigned int Perception::GetSensorCount() { unsigned int Perception::GetSensorCount() { return this->sensorCount; }
return this->sensorCount;
}
Sensor* Perception::GetSensor(unsigned int sensorId) { Sensor *Perception::GetSensor(unsigned int sensorId) {
if (sensorId >= this->sensorCount) if (sensorId >= this->sensorCount)
return nullptr; return nullptr;
Sensor* sensor = this->sensors[sensorId]; Sensor *sensor = this->sensors[sensorId];
return sensor; return sensor;
} }
unsigned int Perception::AddSensor(Sensor* newSensor) { unsigned int Perception::AddSensor(Sensor *newSensor) {
unsigned int newSensorCount = this->sensorCount + 1; 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++) for (unsigned char sensorIx = 0; sensorIx < this->sensorCount; sensorIx++)
newSensors[sensorIx] = sensors[sensorIx]; newSensors[sensorIx] = sensors[sensorIx];
unsigned int sensorId = this->sensorCount; unsigned int sensorId = this->sensorCount;
newSensors[sensorId] = newSensor; newSensors[sensorId] = newSensor;
Sensor** oldSensors = this->sensors; Sensor **oldSensors = this->sensors;
this->sensors = newSensors; this->sensors = newSensors;
this->sensorCount = newSensorCount; this->sensorCount = newSensorCount;
delete[] oldSensors; delete[] oldSensors;
return sensorId; return sensorId;
} }
Sensor* Perception::FindSensorOfType(unsigned int sensorType) { Sensor *Perception::FindSensorOfType(unsigned int sensorType) {
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
Sensor* sensor = this->sensors[sensorIx]; Sensor *sensor = this->sensors[sensorIx];
if (sensor->type == sensorType) if (sensor->type == sensorType)
return sensor; return sensor;
} }
@ -74,8 +72,7 @@ Sensor* Perception::FindSensorOfType(unsigned int sensorType) {
return nullptr; return nullptr;
} }
float GetPlaneDistance(InterestingThing* plane, float GetPlaneDistance(InterestingThing *plane, float horizontalAngle,
float horizontalAngle,
float range) { float range) {
float distance = plane->position.distance; float distance = plane->position.distance;
float deltaAngle = float deltaAngle =
@ -119,7 +116,7 @@ float Perception::GetDistance(float horizontalDirection, float range) {
range = -range; range = -range;
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
InterestingThing* obj = trackedObjects[objIx]; InterestingThing *obj = trackedObjects[objIx];
if (obj == nullptr) if (obj == nullptr)
continue; continue;
@ -137,14 +134,13 @@ float Perception::GetDistance(float horizontalDirection, float range) {
} }
float Perception::GetDistanceOfType(unsigned char thingType, float Perception::GetDistanceOfType(unsigned char thingType,
float horizontalAngle, float horizontalAngle, float range) {
float range) {
float minDistance = INFINITY; float minDistance = INFINITY;
if (range < 0) if (range < 0)
range = -range; range = -range;
for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) { for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) {
InterestingThing* thing = trackedObjects[thingIx]; InterestingThing *thing = trackedObjects[thingIx];
if (thing == nullptr) if (thing == nullptr)
continue; continue;
if (thing->type != thingType) if (thing->type != thingType)
@ -164,14 +160,13 @@ float Perception::GetDistanceOfType(unsigned char thingType,
} }
float Perception::GetDistance(float horizontalDirection, float Perception::GetDistance(float horizontalDirection,
float verticalDirection, float verticalDirection, float range) {
float range) {
float minDistance = INFINITY; float minDistance = INFINITY;
if (range < 0) if (range < 0)
range = -range; range = -range;
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
InterestingThing* obj = trackedObjects[objIx]; InterestingThing *obj = trackedObjects[objIx];
if (obj == nullptr) if (obj == nullptr)
continue; continue;
if (obj->position.direction.horizontal.InDegrees() > if (obj->position.direction.horizontal.InDegrees() >
@ -189,7 +184,7 @@ bool Perception::ObjectNearby(float direction, float range) {
range = -range; range = -range;
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
InterestingThing* obj = trackedObjects[objIx]; InterestingThing *obj = trackedObjects[objIx];
if (obj == nullptr) if (obj == nullptr)
continue; continue;
@ -202,13 +197,11 @@ bool Perception::ObjectNearby(float direction, float range) {
return false; return false;
} }
InterestingThing* Perception::AddTrackedObject(Sensor* sensor, InterestingThing *
Spherical16 position, Perception::AddTrackedObject(Sensor *sensor, Spherical16 position,
SwingTwist16 orientation, SwingTwist16 orientation, unsigned char thingType,
unsigned char thingType, unsigned char thingId, unsigned char networkId) {
unsigned char thingId, InterestingThing *thing = new InterestingThing(sensor, position, orientation);
unsigned char networkId) {
InterestingThing* thing = new InterestingThing(sensor, position, orientation);
if (thingId != 0x00) if (thingId != 0x00)
thing->id = thingId; thing->id = thingId;
thing->type = thingType; thing->type = thingType;
@ -263,12 +256,12 @@ InterestingThing* Perception::AddTrackedObject(Sensor* sensor,
} }
} }
InterestingThing* Perception::AddTrackedObject(Sensor* sensor, InterestingThing *Perception::AddTrackedObject(Sensor *sensor,
unsigned char networkId, unsigned char networkId,
unsigned char thingId, unsigned char thingId,
Spherical16 position, Spherical16 position,
SwingTwist16 orientation) { SwingTwist16 orientation) {
InterestingThing* thing = FindTrackedObject(networkId, thingId); InterestingThing *thing = FindTrackedObject(networkId, thingId);
if (thing == nullptr) { if (thing == nullptr) {
thing = AddTrackedObject(sensor, position, orientation, 0xFF, thingId, thing = AddTrackedObject(sensor, position, orientation, 0xFF, thingId,
networkId); networkId);
@ -282,7 +275,7 @@ InterestingThing* Perception::AddTrackedObject(Sensor* sensor,
bool Perception::IsInteresting(float distance) { bool Perception::IsInteresting(float distance) {
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
InterestingThing* thing = this->trackedObjects[objIx]; InterestingThing *thing = this->trackedObjects[objIx];
if (thing == nullptr) if (thing == nullptr)
return true; return true;
if (thing->position.distance > distance) if (thing->position.distance > distance)
@ -291,9 +284,9 @@ bool Perception::IsInteresting(float distance) {
return false; return false;
} }
InterestingThing* Perception::FindTrackedObject(char objectId) { InterestingThing *Perception::FindTrackedObject(char objectId) {
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
InterestingThing* thing = this->trackedObjects[objIx]; InterestingThing *thing = this->trackedObjects[objIx];
if (thing == nullptr) if (thing == nullptr)
continue; continue;
if (thing->id == objectId) { if (thing->id == objectId) {
@ -303,10 +296,10 @@ InterestingThing* Perception::FindTrackedObject(char objectId) {
return nullptr; return nullptr;
} }
InterestingThing* Perception::FindTrackedObject(unsigned char networkId, InterestingThing *Perception::FindTrackedObject(unsigned char networkId,
unsigned char objectId) { unsigned char objectId) {
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
InterestingThing* thing = this->trackedObjects[objIx]; InterestingThing *thing = this->trackedObjects[objIx];
if (thing == nullptr) if (thing == nullptr)
continue; continue;
@ -326,16 +319,16 @@ unsigned char Perception::TrackedObjectCount() {
return objectCount; return objectCount;
} }
InterestingThing** Perception::GetTrackedObjects() { InterestingThing **Perception::GetTrackedObjects() {
return this->trackedObjects; return this->trackedObjects;
} }
unsigned char Perception::ThingsOfType(unsigned char thingType, unsigned char Perception::ThingsOfType(unsigned char thingType,
InterestingThing* buffer[], InterestingThing *buffer[],
unsigned char bufferSize) { unsigned char bufferSize) {
unsigned char thingCount = 0; unsigned char thingCount = 0;
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
InterestingThing* thing = this->trackedObjects[objIx]; InterestingThing *thing = this->trackedObjects[objIx];
if (thing == nullptr) if (thing == nullptr)
continue; continue;
@ -353,9 +346,9 @@ unsigned char Perception::ThingsOfType(unsigned char thingType,
return thingCount; return thingCount;
} }
InterestingThing* Perception::ThingOfType(unsigned char thingType) { InterestingThing *Perception::ThingOfType(unsigned char thingType) {
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
InterestingThing* thing = this->trackedObjects[objIx]; InterestingThing *thing = this->trackedObjects[objIx];
if (thing == nullptr) if (thing == nullptr)
continue; continue;
@ -365,14 +358,14 @@ InterestingThing* Perception::ThingOfType(unsigned char thingType) {
return nullptr; return nullptr;
} }
InterestingThing* Perception::GetMostInterestingThing() { InterestingThing *Perception::GetMostInterestingThing() {
if (this->trackedObjects == nullptr) if (this->trackedObjects == nullptr)
return nullptr; return nullptr;
InterestingThing* closestObject = nullptr; InterestingThing *closestObject = nullptr;
float closestDistance = INFINITY; float closestDistance = INFINITY;
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
InterestingThing* obj = this->trackedObjects[objIx]; InterestingThing *obj = this->trackedObjects[objIx];
if (obj != nullptr) { if (obj != nullptr) {
if (obj->position.distance < closestDistance) { if (obj->position.distance < closestDistance) {
closestObject = obj; closestObject = obj;
@ -392,12 +385,12 @@ void Perception::Update(unsigned long currentTimeMs) {
// Update sensing // Update sensing
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
Sensor* sensor = sensors[sensorIx]; Sensor *sensor = sensors[sensorIx];
if (sensor == nullptr) if (sensor == nullptr)
continue; continue;
if (sensor->type == Thing::DistanceSensorType) { if (sensor->type == Thing::DistanceSensorType) {
DistanceSensor* distanceSensor = (DistanceSensor*)sensor; DistanceSensor *distanceSensor = (DistanceSensor *)sensor;
float distance = distanceSensor->GetDistance(); float distance = distanceSensor->GetDistance();
if (distance >= 0) { if (distance >= 0) {
@ -411,7 +404,7 @@ void Perception::Update(unsigned long currentTimeMs) {
} }
} else if (sensor->type == Thing::SwitchType) { } else if (sensor->type == Thing::SwitchType) {
Switch* switchSensor = (Switch*)sensor; Switch *switchSensor = (Switch *)sensor;
if (switchSensor->IsOn()) { if (switchSensor->IsOn()) {
// Polar position = Polar(sensor->position.angle, nearbyDistance); // Polar position = Polar(sensor->position.angle, nearbyDistance);
Angle horizontal = Angle::Degrees(horizontal.InDegrees()); Angle horizontal = Angle::Degrees(horizontal.InDegrees());
@ -424,7 +417,7 @@ void Perception::Update(unsigned long currentTimeMs) {
} }
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
InterestingThing* thing = trackedObjects[objIx]; InterestingThing *thing = trackedObjects[objIx];
if (thing == nullptr) if (thing == nullptr)
continue; continue;
@ -439,9 +432,10 @@ void Perception::Update(unsigned long currentTimeMs) {
} }
} }
#include <Arduino.h>
void Perception::UpdatePose(Polar16 translation) { void Perception::UpdatePose(Polar16 translation) {
for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) { for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) {
InterestingThing* thing = trackedObjects[thingIx]; InterestingThing *thing = trackedObjects[thingIx];
if (thing == nullptr) if (thing == nullptr)
continue; continue;
@ -469,10 +463,18 @@ void Perception::UpdatePose(Polar16 translation) {
// // obj->position.ProjectOnHorizontalPlane(); // // obj->position.ProjectOnHorizontalPlane();
// Spherical16 newPosition = Spherical16(horizontalPosition - // Spherical16 newPosition = Spherical16(horizontalPosition -
// translation); // translation);
Spherical16 translationS = Spherical16( Spherical16 translationS = Spherical16(
translation.distance, Angle16::Degrees(translation.angle.InDegrees()), translation.distance, Angle16::Degrees(translation.angle.InDegrees()),
Angle16::Degrees(0)); Angle16::Degrees(0));
Spherical16 newPosition = thing->position + translationS; 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; thing->position = newPosition;
} }
} }
@ -489,7 +491,7 @@ void Perception::UpdatePose(SwingTwist16 rotation) {
Angle16 rotationAngle = rotation.swing.horizontal; Angle16 rotationAngle = rotation.swing.horizontal;
for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) { for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) {
InterestingThing* thing = trackedObjects[thingIx]; InterestingThing *thing = trackedObjects[thingIx];
if (thing == nullptr) if (thing == nullptr)
continue; continue;

30
Roboid.cpp
View File

@ -21,8 +21,8 @@ Roboid::Roboid() : Thing(0) {
this->propulsion = nullptr; this->propulsion = nullptr;
this->networkSync = nullptr; this->networkSync = nullptr;
this->worldPosition = Spherical16::zero; this->roboidPosition = Spherical16::zero;
this->worldOrientation = SwingTwist16::identity; this->roboidOrientation = SwingTwist16::identity;
} }
Roboid::Roboid(Propulsion *propulsion) : Roboid() { Roboid::Roboid(Propulsion *propulsion) : Roboid() {
@ -33,7 +33,6 @@ Roboid::Roboid(Propulsion *propulsion) : Roboid() {
void Roboid::SetName(const char *name) { this->name = name; } void Roboid::SetName(const char *name) { this->name = name; }
#include <Arduino.h>
void Roboid::Update(unsigned long currentTimeMs) { void Roboid::Update(unsigned long currentTimeMs) {
if (perception != nullptr) if (perception != nullptr)
perception->Update(currentTimeMs); perception->Update(currentTimeMs);
@ -52,9 +51,12 @@ void Roboid::Update(unsigned long currentTimeMs) {
this->angularVelocity = this->angularVelocity =
Spherical16(oldAngular, Angle16(), Angle16::Degrees(90)); Spherical16(oldAngular, Angle16(), Angle16::Degrees(90));
SetPosition(this->position + this->orientation * Spherical16::forward * // SetPosition(this->position + this->orientation * Spherical16::forward *
this->linearVelocity.distance * deltaTime); // this->linearVelocity.distance *
this->worldPosition = this->position; // assuming the roboid is the root // 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( SwingTwist16 rotation = SwingTwist16::AngleAxis(
this->angularVelocity.distance * deltaTime, Direction16::up); this->angularVelocity.distance * deltaTime, Direction16::up);
@ -63,7 +65,7 @@ void Roboid::Update(unsigned long currentTimeMs) {
this->orientation = this->orientation * rotation; this->orientation = this->orientation * rotation;
this->worldOrientation = this->roboidOrientation =
this->orientation; // assuming the roboid is the root this->orientation; // assuming the roboid is the root
} }
@ -78,20 +80,24 @@ void Roboid::Update(unsigned long currentTimeMs) {
lastUpdateTimeMs = 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) { void Roboid::SetPosition(Spherical16 newWorldPosition) {
SwingTwist16 roboidOrientation = this->GetOrientation(); SwingTwist16 roboidOrientation = this->GetOrientation();
Spherical16 translation = newWorldPosition - this->worldPosition; Spherical16 translation = newWorldPosition - this->roboidPosition;
float distance = translation.distance; float distance = translation.distance;
Angle16 angle = Spherical16::SignedAngleBetween( Angle16 angle = Spherical16::SignedAngleBetween(
roboidOrientation * Spherical16::forward, translation, Spherical16::up); roboidOrientation * Spherical16::forward, translation, Spherical16::up);
Polar16 polarTranslation = Polar16( Polar16 polarTranslation = Polar16(
distance, angle); // Polar(angle.InDegrees(), Angle::Degrees(distance)); 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); perception->UpdatePose(polarTranslation);
}
this->position = newWorldPosition; // roboid is the root? this->position = newWorldPosition; // roboid is the root?
// World position should be set in the update recursion // World position should be set in the update recursion
@ -111,7 +117,7 @@ void Roboid::SetOrientation(SwingTwist16 newOrientation) {
this->orientation = newOrientation; this->orientation = newOrientation;
} }
*/
void Roboid::AddChild(Thing *child) { void Roboid::AddChild(Thing *child) {
Thing::AddChild(child); Thing::AddChild(child);
if (child->IsSensor()) { 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 /// @details The origin and units of the position depends on the position
/// tracking system used. This value will be Vector3::zero unless a position /// tracking system used. This value will be Vector3::zero unless a position
/// is received through network synchronisation /// is received through network synchronisation
virtual Spherical16 GetPosition(); // virtual Spherical16 GetPosition();
// Vector2 GetPosition2D(); // Vector2 GetPosition2D();
/// @brief Retrieve the current orientation of the roboid /// @brief Retrieve the current orientation of the roboid
/// @return The orientation quaternion in world space /// @return The orientation quaternion in world space
/// @details The origin orientation depends on the position tracking system /// @details The origin orientation depends on the position tracking system
/// used. This value will be Quaternion::identity unless an orientation is /// used. This value will be Quaternion::identity unless an orientation is
/// received though network synchronization /// received though network synchronization
virtual SwingTwist16 GetOrientation(); // virtual SwingTwist16 GetOrientation();
/// @brief Update the current position of the roboid /// @brief Update the current position of the roboid
/// @param worldPosition The position of the roboid in carthesian coordinates /// @param worldPosition The position of the roboid in carthesian coordinates
@ -58,31 +58,22 @@ public:
/// orientations of the perceived objects by the roboid /// orientations of the perceived objects by the roboid
/// (roboid->perception->perceivedObjects), as these are local to the /// (roboid->perception->perceivedObjects), as these are local to the
/// roboid's position. /// roboid's position.
virtual void SetPosition(Spherical16 worldPosition); // virtual void SetPosition(Spherical16 worldPosition);
/// @brief Update the current orientation of the roboid /// @brief Update the current orientation of the roboid
/// @param worldOrientation The orientation of the roboid in world space /// @param worldOrientation The orientation of the roboid in world space
/// @details The use of this function will also update the orientations of the /// @details The use of this function will also update the orientations of the
/// perceived objects by the roboid (roboid->perception->perceivedObjets), /// perceived objects by the roboid (roboid->perception->perceivedObjets),
/// as these are local to the roboid' orientation. /// as these are local to the roboid' orientation.
virtual void SetOrientation(SwingTwist16 worldOrientation); // virtual void SetOrientation(SwingTwist16 worldOrientation);
virtual void AddChild(Thing *child) override; virtual void AddChild(Thing *child) override;
void Release(Thing *child); 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: 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; 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 /// @return True when the Thing is a Roboid and False otherwise
bool IsRoboid(); 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 /// @remark When this Thing has a parent, the position is relative to the
/// parent's position and orientation /// parent's position and orientation
Spherical16 position; Spherical16 position;
Spherical16 worldPosition; /// @brief The position in roboid space
/// @brief The orientation of this Thing /// @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 /// @remark When this Thing has a parent, the orientation is relative to the
/// parent's orientation /// parent's orientation
SwingTwist16 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 GetLinearVelocity();
virtual Spherical16 GetAngularVelocity(); virtual Spherical16 GetAngularVelocity();