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

made pose sending generic

Browse Source
This commit is contained in:
Pascal Serrarens 2024-01-22 15:57:02 +01:00
parent 6eefd5892c
commit b998903bd8
2 changed files with 38 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
NetworkSync.cpp
View File

@ -4,13 +4,25 @@
#include <Arduino.h>
#endif
void NetworkSync::SendVector3(unsigned char *data, int startIndex, Vector3 v) {
UInt8 NetworkSync::CreatePoseMsg(Roboid* roboid, UInt8* buffer) {
Vector3 position = roboid->GetPosition();
UInt8 bufferSize = 3 + 12;
buffer[0] = PoseMsg;
buffer[1] = 0; // ObjectId;
buffer[2] = Pose_Position;
SendVector3(buffer, 3, position);
return bufferSize;
}
void NetworkSync::SendVector3(unsigned char* data, int startIndex, Vector3 v) {
SendSingle100(data, startIndex, v.x);
SendSingle100(data, startIndex + 4, v.y);
SendSingle100(data, startIndex + 8, v.z);
}
void NetworkSync::SendSingle100(unsigned char *data, int startIndex,
void NetworkSync::SendSingle100(unsigned char* data,
int startIndex,
float value) {
// Sends a float with truncated 2 decimal precision
Int32 intValue = value * 100;
@ -20,16 +32,16 @@ void NetworkSync::SendSingle100(unsigned char *data, int startIndex,
// }
}
void NetworkSync::SendInt32(unsigned char *data, int startIndex, Int32 value) {
void NetworkSync::SendInt32(unsigned char* data, int startIndex, Int32 value) {
for (unsigned char ix = 0; ix < 4; ix++) {
data[startIndex + ix] = ((unsigned char *)&value)[ix];
data[startIndex + ix] = ((unsigned char*)&value)[ix];
}
}
void NetworkSync::PublishTrackedObjects(SendBuffer sendBuffer,
TrackedObject **objects) {
TrackedObject** objects) {
for (unsigned char objIx = 0; objIx < Perception::maxObjectCount; objIx++) {
TrackedObject *obj = objects[objIx];
TrackedObject* obj = objects[objIx];
if (obj == nullptr)
continue;
// if (obj->sensor->type == Thing::ExternalType)
@ -42,7 +54,7 @@ void NetworkSync::PublishTrackedObjects(SendBuffer sendBuffer,
}
void NetworkSync::PublishTrackedObject(SendBuffer sendBuffer,
TrackedObject *object) {
TrackedObject* object) {
Vector2 worldPosition2 = Vector2::Rotate(
Vector2::forward * object->position.distance, -object->position.angle);
Vector3 worldPosition3 = Vector3(worldPosition2.x, 0, worldPosition2.y);
@ -68,7 +80,7 @@ void NetworkSync::PublishTrackedObject(SendBuffer sendBuffer,
#endif
}
void NetworkSync::SendPoseMsg(SendBuffer sendBuffer, Roboid *roboid) {
void NetworkSync::SendPoseMsg(SendBuffer sendBuffer, Roboid* roboid) {
Polar velocity = roboid->propulsion->GetVelocity();
Vector2 worldVelocity2 =
Vector2::Rotate(Vector2::forward * velocity.distance, velocity.angle);
@ -94,7 +106,7 @@ void NetworkSync::SendPoseMsg(SendBuffer sendBuffer, Roboid *roboid) {
const unsigned int bufferSize = 3 + 12 + 12;
unsigned char buffer[bufferSize] = {
PoseMsg,
0, // objectId;
0, // objectId;
Pose_LinearVelocity | Pose_AngularVelocity,
};
SendVector3(buffer, 3, worldVelocity3);
@ -104,7 +116,7 @@ void NetworkSync::SendPoseMsg(SendBuffer sendBuffer, Roboid *roboid) {
#endif
}
void NetworkSync::SendDestroyObject(SendBuffer sendBuffer, TrackedObject *obj) {
void NetworkSync::SendDestroyObject(SendBuffer sendBuffer, TrackedObject* obj) {
#ifdef RC_DEBUG
Serial.print("Send Destroy ");
Serial.println((int)obj->id);

31
NetworkSync.h
View File

@ -10,13 +10,13 @@ namespace RoboidControl {
/// @brief Interface for synchronizaing state between clients across a network
class NetworkSync {
public:
public:
/// @brief Retreive and send the roboid state
/// @param roboid The roboid for which the state is updated
virtual void NetworkUpdate(Roboid *roboid) = 0;
virtual void NetworkUpdate(Roboid* roboid) = 0;
/// @brief Inform that the given object is no longer being tracked
/// @param obj
virtual void DestroyObject(TrackedObject *obj) = 0;
virtual void DestroyObject(TrackedObject* obj) = 0;
/// @brief The id of a Pose message
static const char PoseMsg = 0x10;
@ -35,21 +35,22 @@ public:
static const char DestroyMsg = 0x20;
typedef void (*SendBuffer)(UInt8 *buffer, UInt16 bufferSize);
typedef void (*SendBuffer)(UInt8* buffer, UInt16 bufferSize);
void SendPoseMsg(SendBuffer sendBuffer, Roboid *roboid);
void SendDestroyObject(SendBuffer sendBuffer, TrackedObject *obj);
void SendPoseMsg(SendBuffer sendBuffer, Roboid* roboid);
void SendDestroyObject(SendBuffer sendBuffer, TrackedObject* obj);
void PublishTrackedObjects(SendBuffer sendBuffer, TrackedObject **objects);
void PublishTrackedObjects(SendBuffer sendBuffer, TrackedObject** objects);
protected:
NetworkPerception *networkPerception;
void PublishTrackedObject(SendBuffer sendBuffer, TrackedObject *object);
protected:
NetworkPerception* networkPerception;
void PublishTrackedObject(SendBuffer sendBuffer, TrackedObject* object);
void SendVector3(unsigned char *data, int startIndex, Vector3 v);
void SendSingle100(unsigned char *data, int startIndex, float value);
void SendInt32(unsigned char *data, int startIndex, Int32 value);
UInt8 CreatePoseMsg(Roboid* roboid, UInt8* buffer);
void SendVector3(unsigned char* data, int startIndex, Vector3 v);
void SendSingle100(unsigned char* data, int startIndex, float value);
void SendInt32(unsigned char* data, int startIndex, Int32 value);
};
} // namespace RoboidControl
} // namespace Passer
} // namespace RoboidControl
} // namespace Passer