83 lines
2.6 KiB
C++
83 lines
2.6 KiB
C++
#include "PoseMsg.h"
|
|
#include "LowLevelMessages.h"
|
|
|
|
namespace RoboidControl {
|
|
|
|
// PoseMsg::PoseMsg(unsigned char networkId,
|
|
// unsigned char thingId,
|
|
// unsigned char poseType,
|
|
// Spherical16 position,
|
|
// SwingTwist16 orientation,
|
|
// Spherical16 linearVelocity,
|
|
// Spherical16 angularVelocity) {
|
|
// this->networkId = networkId;
|
|
// this->thingId = thingId;
|
|
|
|
// this->poseType = poseType;
|
|
// this->position = position;
|
|
// this->orientation = orientation;
|
|
// this->linearVelocity = linearVelocity;
|
|
// this->angularVelocity = angularVelocity;
|
|
// }
|
|
PoseMsg::PoseMsg(unsigned char networkId, Thing* thing, bool force) {
|
|
this->networkId = networkId;
|
|
this->thingId = thing->id;
|
|
|
|
this->poseType = 0;
|
|
if (thing->positionUpdated || force) {
|
|
this->position = thing->GetPosition();
|
|
this->poseType |= Pose_Position;
|
|
thing->positionUpdated = false;
|
|
}
|
|
if (thing->orientationUpdated || force ) {
|
|
this->orientation = thing->GetOrientation();
|
|
this->poseType |= Pose_Orientation;
|
|
thing->orientationUpdated = false;
|
|
}
|
|
if (thing->linearVelocityUpdated) {
|
|
this->linearVelocity = thing->GetLinearVelocity();
|
|
this->poseType |= Pose_LinearVelocity;
|
|
thing->linearVelocityUpdated = false;
|
|
}
|
|
if (thing->angularVelocityUpdated) {
|
|
this->angularVelocity = thing->GetAngularVelocity();
|
|
this->poseType |= Pose_AngularVelocity;
|
|
thing->angularVelocityUpdated = false;
|
|
}
|
|
}
|
|
|
|
PoseMsg::PoseMsg(const char* buffer) {
|
|
unsigned char ix = 1; // First byte is msg id
|
|
this->networkId = buffer[ix++];
|
|
this->thingId = buffer[ix++];
|
|
this->poseType = buffer[ix++];
|
|
this->position = LowLevelMessages::ReceiveSpherical16(buffer, &ix);
|
|
this->orientation = LowLevelMessages::ReceiveQuat32(buffer, &ix);
|
|
// linearVelocity
|
|
// angularVelocity
|
|
}
|
|
|
|
PoseMsg::~PoseMsg() {}
|
|
|
|
unsigned char PoseMsg::Serialize(char* buffer) {
|
|
if (this->poseType == 0)
|
|
return 0;
|
|
|
|
unsigned char ix = 0;
|
|
buffer[ix++] = PoseMsg::id;
|
|
buffer[ix++] = this->networkId;
|
|
buffer[ix++] = this->thingId;
|
|
buffer[ix++] = this->poseType;
|
|
if ((this->poseType & Pose_Position) != 0)
|
|
LowLevelMessages::SendSpherical16(buffer, &ix, this->position);
|
|
if ((this->poseType & Pose_Orientation) != 0)
|
|
LowLevelMessages::SendQuat32(buffer, &ix, this->orientation);
|
|
if ((this->poseType & Pose_LinearVelocity) != 0)
|
|
LowLevelMessages::SendSpherical16(buffer, &ix, this->linearVelocity);
|
|
if ((this->poseType & Pose_AngularVelocity) != 0)
|
|
LowLevelMessages::SendSpherical16(buffer, &ix, this->angularVelocity);
|
|
return ix;
|
|
}
|
|
|
|
} // namespace RoboidControl
|