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

Improved rounding of spherical positions

Browse Source
This commit is contained in:
Pascal Serrarens 2024-12-14 12:39:01 +01:00
parent 7bcd80087a
commit 0cf4413814
8 changed files with 153 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
ControlCore/LowLevelMessages.cpp
View File

@ -2,6 +2,12 @@
#include "../float16/float16.h"
void LowLevelMessages::SendAngle8(unsigned char *buffer, unsigned char *ix,
const float angle) {
Angle8 packedAngle2 = Angle8::Degrees(angle);
buffer[(*ix)++] = packedAngle2.GetBinary();
}
void LowLevelMessages::SendFloat16(unsigned char *buffer, unsigned char *ix,
float value) {
float16 value16 = float16(value);
@ -10,3 +16,32 @@ void LowLevelMessages::SendFloat16(unsigned char *buffer, unsigned char *ix,
buffer[(*ix)++] = (binary >> 8) & 0xFF;
buffer[(*ix)++] = binary & 0xFF;
}
void Passer::Control::LowLevelMessages::SendSpherical16(unsigned char *buffer,
unsigned char *ix,
Spherical16 s) {
SendFloat16(buffer, ix, s.distance);
SendAngle8(buffer, ix, s.direction.horizontal.InDegrees());
SendAngle8(buffer, ix, s.direction.vertical.InDegrees());
}
void Passer::Control::LowLevelMessages::SendQuat32(unsigned char *buffer,
unsigned char *ix,
SwingTwist16 rotation) {
Quaternion q = rotation.ToQuaternion();
unsigned char qx = (char)(q.x * 127 + 128);
unsigned char qy = (char)(q.y * 127 + 128);
unsigned char qz = (char)(q.z * 127 + 128);
unsigned char qw = (char)(q.w * 255);
if (q.w < 0) {
qx = -qx;
qy = -qy;
qz = -qz;
qw = -qw;
}
// Serial.printf(" (%d) %d:%d:%d:%d ", startIndex, qx, qy, qz, qw);
buffer[(*ix)++] = qx;
buffer[(*ix)++] = qy;
buffer[(*ix)++] = qz;
buffer[(*ix)++] = qw;
}

7
ControlCore/LowLevelMessages.h
View File

@ -1,11 +1,18 @@
#include "../LinearAlgebra/Spherical.h"
#include "../LinearAlgebra/SwingTwist.h"
namespace Passer::Control {
class LowLevelMessages {
public:
static void SendAngle8(unsigned char *buffer, unsigned char *ix,
const float angle);
static void SendFloat16(unsigned char *buffer, unsigned char *ix,
float value);
static void SendSpherical16(unsigned char *buffer, unsigned char *ix,
Spherical16 s);
static void SendQuat32(unsigned char *buffer, unsigned char *ix,
SwingTwist16 q);
};
} // namespace Passer::Control

29
ControlCore/Messages.cpp
View File

@ -131,3 +131,32 @@ unsigned char ModelUrlMsg::Serialize(unsigned char *buffer) {
// Model Url
#pragma endregion
#pragma region PoseMsg
#include <Arduino.h>
PoseMsg::PoseMsg(unsigned char networkId, unsigned char thingId,
unsigned char poseType, Spherical16 position,
SwingTwist16 orientation) {
this->networkId = networkId;
this->thingId = thingId;
this->position = position;
this->orientation = orientation;
this->poseType = poseType;
}
unsigned char PoseMsg::Serialize(unsigned char *buffer) {
unsigned char ix = 0;
buffer[ix++] = PoseMsg::id;
buffer[ix++] = this->networkId;
buffer[ix++] = this->thingId;
buffer[ix++] = this->poseType;
LowLevelMessages::SendSpherical16(buffer, &ix, this->position);
printf("send spherical %f %f\n", this->position.distance,
this->position.direction.horizontal.InDegrees());
LowLevelMessages::SendQuat32(buffer, &ix, this->orientation);
return ix;
}
// Pose
#pragma endregion

26
ControlCore/Messages.h
View File

@ -1,4 +1,6 @@
#pragma once
#include "../LinearAlgebra/Spherical.h"
#include "../LinearAlgebra/SwingTwist.h"
#include "../float16/float16.h"
#include "CoreThing.h"
#include "Participant.h"
@ -67,8 +69,10 @@ public:
class ModelUrlMsg : public IMessage {
public:
static const unsigned char id = 0x90;
unsigned char networkId;
unsigned char thingId;
float scale;
unsigned char urlLength;
const char *url;
@ -79,5 +83,27 @@ public:
virtual unsigned char Serialize(unsigned char *buffer) override;
};
class PoseMsg : public IMessage {
public:
static const unsigned char id = 0x10;
unsigned char length = 4 + 4 + 4;
unsigned char networkId;
unsigned char thingId;
unsigned char poseType;
const unsigned char Pose_Position = 0x01;
const unsigned char Pose_Orientation = 0x02;
Spherical16 position;
SwingTwist16 orientation;
PoseMsg(unsigned char networkId, unsigned char thingId,
unsigned char poseType, Spherical16 position,
SwingTwist16 orientation);
virtual unsigned char Serialize(unsigned char *buffer) override;
};
} // namespace Passer::Control
using namespace Passer::Control;

15
Messages.cpp
View File

@ -61,13 +61,14 @@ void Messages::SendPolar(unsigned char *buffer, unsigned char *startIndex,
SendSingle100(buffer, (*startIndex) + 1, p.distance);
}
void Messages::SendSpherical16(unsigned char *buffer, unsigned char *startIndex,
Spherical16 s) {
// Receive first does distance...
SendAngle8(buffer, (*startIndex)++, s.direction.horizontal.InDegrees());
SendAngle8(buffer, (*startIndex)++, s.direction.vertical.InDegrees());
SendFloat16(buffer, startIndex, s.distance);
}
// void Messages::SendSpherical16(unsigned char *buffer, unsigned char
// *startIndex,
// Spherical16 s) {
// // Receive first does distance...
// SendAngle8(buffer, (*startIndex)++, s.direction.horizontal.InDegrees());
// SendAngle8(buffer, (*startIndex)++, s.direction.vertical.InDegrees());
// SendFloat16(buffer, startIndex, s.distance);
// }
void Messages::SendSwingTwist(unsigned char *buffer, unsigned char *ix,
const SwingTwist16 r) {

5
Messages.h
View File

@ -32,8 +32,9 @@ public:
static void SendPolar(unsigned char *buffer, unsigned char *startIndex,
Polar p);
static void SendSpherical16(unsigned char *buffer, unsigned char *startIndex,
Spherical16 s);
// static void SendSpherical16(unsigned char *buffer, unsigned char
// *startIndex,
// Spherical16 s);
static void SendSwingTwist(unsigned char *buffer, unsigned char *startIndex,
const SwingTwist16 r);
static void SendQuat32(unsigned char *buffer, unsigned char *startIndex,

47
NetworkSync.cpp
View File

@ -236,19 +236,23 @@ void NetworkSync::SendPose(Thing *thing, bool force, bool recurse) {
thing->positionUpdated |= thing->GetLinearVelocity().distance > 0;
thing->orientationUpdated |= thing->GetAngularVelocity().distance > 0;
if (force || thing->positionUpdated || thing->orientationUpdated) {
unsigned char ix = 0;
buffer[ix++] = PoseMsg;
buffer[ix++] = this->networkId;
buffer[ix++] = thing->id;
unsigned char pattern = 0;
// unsigned char ix = 0;
// buffer[ix++] = PoseMsg;
// buffer[ix++] = this->networkId;
// buffer[ix++] = thing->id;
unsigned char poseType = 0;
if (force || thing->positionUpdated)
pattern |= Pose_Position;
poseType |= Pose_Position;
if (force || thing->orientationUpdated)
pattern |= Pose_Orientation;
buffer[ix++] = pattern;
Messages::SendSpherical16(buffer, &ix, thing->position);
Messages::SendQuat32(buffer, &ix, thing->orientation.ToQuaternion());
SendBuffer(ix);
poseType |= Pose_Orientation;
// buffer[ix++] = poseType;
// Messages::SendSpherical16(buffer, &ix, thing->position);
// Messages::SendQuat32(buffer, &ix, thing->orientation.ToQuaternion());
// SendBuffer(ix);
Passer::Control::PoseMsg msg =
Passer::Control::PoseMsg(this->networkId, thing->id, poseType,
thing->position, thing->orientation);
SendBuffer(msg.Serialize(this->buffer));
thing->positionUpdated = false;
thing->orientationUpdated = false;
@ -353,14 +357,19 @@ void NetworkSync::PublishTrackedObject(Roboid *roboid,
Spherical16 worldPosition =
inv_originOrientation * (thing->position - originPosition);
unsigned char ix = 0;
buffer[ix++] = PoseMsg; // Position2DMsg;
buffer[ix++] = this->networkId;
buffer[ix++] = thing->id; // objectId;
buffer[ix++] = Pose_Position | Pose_Orientation;
Messages::SendSpherical16(buffer, &ix, worldPosition);
Messages::SendSwingTwist(buffer, &ix, worldOrientation);
SendBuffer(ix);
// unsigned char ix = 0;
// buffer[ix++] = PoseMsg; // Position2DMsg;
// buffer[ix++] = this->networkId;
// buffer[ix++] = thing->id; // objectId;
// buffer[ix++] = Pose_Position | Pose_Orientation;
// Messages::SendSpherical16(buffer, &ix, worldPosition);
// Messages::SendSwingTwist(buffer, &ix, worldOrientation);
// SendBuffer(ix);
Passer::Control::PoseMsg msg = Passer::Control::PoseMsg(
this->networkId, thing->id, Pose_Position | Pose_Orientation,
worldPosition, worldOrientation);
SendBuffer(msg.Serialize(this->buffer));
#if RC_DEBUG
printf("Sent Thing PoseMsg [%d/%d] %d\n", networkId, buffer[1], thing->type);

25
Roboid.cpp
View File

@ -31,6 +31,7 @@ Roboid::Roboid(Propulsion *propulsion) : Roboid() {
propulsion->roboid = this;
}
#include <Arduino.h>
void Roboid::Update(unsigned long currentTimeMs) {
if (this->lastUpdateTimeMs == 0)
this->lastUpdateTimeMs = currentTimeMs;
@ -41,22 +42,28 @@ void Roboid::Update(unsigned long currentTimeMs) {
if (propulsion != nullptr) {
propulsion->Update(currentTimeMs);
printf("orientation %f velocity %f\n",
orientation.swing.horizontal.InDegrees(),
linearVelocity.direction.horizontal);
float deltaTime = (float)(currentTimeMs - lastUpdateTimeMs) / 1000;
this->linearVelocity = this->propulsion->GetVelocity();
Spherical16 translation =
this->orientation * this->linearVelocity * deltaTime;
SetPosition(this->position + translation);
this->angularVelocity = this->propulsion->GetAngularVelocity();
SwingTwist16 rotation =
SwingTwist16::AngleAxis(this->angularVelocity.distance * deltaTime,
this->angularVelocity.direction);
// if (perception != nullptr)
// perception->UpdatePose(rotation);
SetOrientation(this->orientation * rotation);
this->linearVelocity = this->propulsion->GetVelocity();
printf(" orientation %f velocity %f\n",
orientation.swing.horizontal.InDegrees(),
linearVelocity.direction.horizontal);
Spherical16 translation =
this->orientation * this->linearVelocity * deltaTime;
SetPosition(this->position + translation);
printf(" orientation %f velocity %f\n",
orientation.swing.horizontal.InDegrees(),
linearVelocity.direction.horizontal);
}
if (children != nullptr) {