use SwingTwist for orientation
This commit is contained in:
parent
b8eb9bb712
commit
9a2ce4dd5e
@ -5,6 +5,83 @@
|
|||||||
|
|
||||||
namespace RoboidControl {
|
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,
|
void LowLevelMessages::SendAngle8(char* buffer,
|
||||||
unsigned char* ix,
|
unsigned char* ix,
|
||||||
const float angle) {
|
const float angle) {
|
||||||
@ -42,58 +119,5 @@ float LowLevelMessages::ReceiveFloat16(const char* buffer,
|
|||||||
return (float)f.toFloat();
|
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
|
} // namespace RoboidControl
|
@ -11,6 +11,10 @@ class LowLevelMessages {
|
|||||||
static Spherical ReceiveSpherical(const char* buffer,
|
static Spherical ReceiveSpherical(const char* buffer,
|
||||||
unsigned char* startIndex);
|
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 void SendQuat32(char* buffer, unsigned char* ix, SwingTwist q);
|
||||||
static SwingTwist ReceiveQuat32(const char* buffer, unsigned char* ix);
|
static SwingTwist ReceiveQuat32(const char* buffer, unsigned char* ix);
|
||||||
|
|
||||||
|
@ -34,7 +34,7 @@ PoseMsg::PoseMsg(const char* buffer) {
|
|||||||
this->thingId = buffer[ix++];
|
this->thingId = buffer[ix++];
|
||||||
this->poseType = buffer[ix++];
|
this->poseType = buffer[ix++];
|
||||||
this->position = LowLevelMessages::ReceiveSpherical(buffer, &ix);
|
this->position = LowLevelMessages::ReceiveSpherical(buffer, &ix);
|
||||||
this->orientation = LowLevelMessages::ReceiveQuat32(buffer, &ix);
|
this->orientation = LowLevelMessages::ReceiveSwingTwist(buffer, &ix);
|
||||||
// linearVelocity
|
// linearVelocity
|
||||||
// angularVelocity
|
// angularVelocity
|
||||||
}
|
}
|
||||||
@ -57,7 +57,7 @@ unsigned char PoseMsg::Serialize(char* buffer) {
|
|||||||
if ((this->poseType & Pose_Position) != 0)
|
if ((this->poseType & Pose_Position) != 0)
|
||||||
LowLevelMessages::SendSpherical(buffer, &ix, this->position);
|
LowLevelMessages::SendSpherical(buffer, &ix, this->position);
|
||||||
if ((this->poseType & Pose_Orientation) != 0)
|
if ((this->poseType & Pose_Orientation) != 0)
|
||||||
LowLevelMessages::SendQuat32(buffer, &ix, this->orientation);
|
LowLevelMessages::SendSwingTwist(buffer, &ix, this->orientation);
|
||||||
if ((this->poseType & Pose_LinearVelocity) != 0)
|
if ((this->poseType & Pose_LinearVelocity) != 0)
|
||||||
LowLevelMessages::SendSpherical(buffer, &ix, this->linearVelocity);
|
LowLevelMessages::SendSpherical(buffer, &ix, this->linearVelocity);
|
||||||
if ((this->poseType & Pose_AngularVelocity) != 0)
|
if ((this->poseType & Pose_AngularVelocity) != 0)
|
||||||
|
Loading…
x
Reference in New Issue
Block a user