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

use SwingTwist for orientation

Browse Source
This commit is contained in:
Pascal Serrarens 2025-06-06 09:36:42 +02:00
parent b8eb9bb712
commit 9a2ce4dd5e
3 changed files with 83 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

130
Messages/LowLevelMessages.cpp
View File

@ -5,6 +5,83 @@
namespace RoboidControl {
void LowLevelMessages::SendSpherical(char* buffer,
unsigned char* ix,
Spherical s) {
SendFloat16(buffer, ix, s.distance);
SendAngle8(buffer, ix, s.direction.horizontal.InDegrees());
SendAngle8(buffer, ix, s.direction.vertical.InDegrees());
}
Spherical LowLevelMessages::ReceiveSpherical(const char* buffer,
unsigned char* startIndex) {
float distance = ReceiveFloat16(buffer, startIndex);
Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
Angle horizontal = Angle::Radians(horizontal8.InRadians());
Angle8 vertical8 = ReceiveAngle8(buffer, startIndex);
Angle vertical = Angle::Radians(vertical8.InRadians());
Spherical s = Spherical(distance, horizontal, vertical);
return s;
}
void LowLevelMessages::SendSwingTwist(char* buffer,
unsigned char* ix,
SwingTwist s) {
SendAngle8(buffer, ix, s.swing.horizontal.InDegrees());
SendAngle8(buffer, ix, s.swing.vertical.InDegrees());
SendAngle8(buffer, ix, s.twist.InDegrees());
}
SwingTwist LowLevelMessages::ReceiveSwingTwist(const char* buffer,
unsigned char* startIndex) {
Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
Angle horizontal = Angle::Radians(horizontal8.InRadians());
Angle8 vertical8 = ReceiveAngle8(buffer, startIndex);
Angle vertical = Angle::Radians(vertical8.InRadians());
Angle8 twist8 = ReceiveAngle8(buffer, startIndex);
Angle twist = Angle::Radians(twist8.InRadians());
SwingTwist s = SwingTwist(horizontal, vertical, twist);
return s;
}
void LowLevelMessages::SendQuat32(char* buffer,
unsigned char* ix,
SwingTwist 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;
}
// std::cout << (int)qx << "," << (int)qy << "," << (int)qz << "," << (int)qw
// << "\n";
buffer[(*ix)++] = qx;
buffer[(*ix)++] = qy;
buffer[(*ix)++] = qz;
buffer[(*ix)++] = qw;
}
SwingTwist LowLevelMessages::ReceiveQuat32(const char* buffer,
unsigned char* ix) {
float qx = (buffer[(*ix)++] - 128.0F) / 127.0F;
float qy = (buffer[(*ix)++] - 128.0F) / 127.0F;
float qz = (buffer[(*ix)++] - 128.0F) / 127.0F;
float qw = buffer[(*ix)++] / 255.0F;
Quaternion q = Quaternion(qx, qy, qz, qw);
SwingTwist s = SwingTwist::FromQuaternion(q);
return s;
}
void LowLevelMessages::SendAngle8(char* buffer,
unsigned char* ix,
const float angle) {
@ -42,58 +119,5 @@ float LowLevelMessages::ReceiveFloat16(const char* buffer,
return (float)f.toFloat();
}
void LowLevelMessages::SendSpherical(char* buffer,
unsigned char* ix,
Spherical s) {
SendFloat16(buffer, ix, s.distance);
SendAngle8(buffer, ix, s.direction.horizontal.InDegrees());
SendAngle8(buffer, ix, s.direction.vertical.InDegrees());
}
Spherical LowLevelMessages::ReceiveSpherical(const char* buffer,
unsigned char* startIndex) {
float distance = ReceiveFloat16(buffer, startIndex);
Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
Angle horizontal = Angle::Radians(horizontal8.InRadians());
Angle8 vertical8 = ReceiveAngle8(buffer, startIndex);
Angle vertical = Angle::Radians(vertical8.InRadians());
Spherical s = Spherical(distance, horizontal, vertical);
return s;
}
void LowLevelMessages::SendQuat32(char* buffer,
unsigned char* ix,
SwingTwist 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;
}
// std::cout << (int)qx << "," << (int)qy << "," << (int)qz << "," << (int)qw
// << "\n";
buffer[(*ix)++] = qx;
buffer[(*ix)++] = qy;
buffer[(*ix)++] = qz;
buffer[(*ix)++] = qw;
}
SwingTwist LowLevelMessages::ReceiveQuat32(const char* buffer,
unsigned char* ix) {
float qx = (buffer[(*ix)++] - 128.0F) / 127.0F;
float qy = (buffer[(*ix)++] - 128.0F) / 127.0F;
float qz = (buffer[(*ix)++] - 128.0F) / 127.0F;
float qw = buffer[(*ix)++] / 255.0F;
Quaternion q = Quaternion(qx, qy, qz, qw);
SwingTwist s = SwingTwist::FromQuaternion(q);
return s;
}
} // namespace RoboidControl

4
Messages/LowLevelMessages.h
View File

@ -11,6 +11,10 @@ class LowLevelMessages {
static Spherical ReceiveSpherical(const char* buffer,
unsigned char* startIndex);
static void SendSwingTwist(char* buffer, unsigned char* ix, SwingTwist s);
static SwingTwist ReceiveSwingTwist(const char* buffer,
unsigned char* startIndex);
static void SendQuat32(char* buffer, unsigned char* ix, SwingTwist q);
static SwingTwist ReceiveQuat32(const char* buffer, unsigned char* ix);

4
Messages/PoseMsg.cpp
View File

@ -34,7 +34,7 @@ PoseMsg::PoseMsg(const char* buffer) {
this->thingId = buffer[ix++];
this->poseType = buffer[ix++];
this->position = LowLevelMessages::ReceiveSpherical(buffer, &ix);
this->orientation = LowLevelMessages::ReceiveQuat32(buffer, &ix);
this->orientation = LowLevelMessages::ReceiveSwingTwist(buffer, &ix);
// linearVelocity
// angularVelocity
}
@ -57,7 +57,7 @@ unsigned char PoseMsg::Serialize(char* buffer) {
if ((this->poseType & Pose_Position) != 0)
LowLevelMessages::SendSpherical(buffer, &ix, this->position);
if ((this->poseType & Pose_Orientation) != 0)
LowLevelMessages::SendQuat32(buffer, &ix, this->orientation);
LowLevelMessages::SendSwingTwist(buffer, &ix, this->orientation);
if ((this->poseType & Pose_LinearVelocity) != 0)
LowLevelMessages::SendSpherical(buffer, &ix, this->linearVelocity);
if ((this->poseType & Pose_AngularVelocity) != 0)