From c07f81bce3bd8559e8c79ee436bedbc8a9b332f8 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 12 Jan 2024 17:16:08 +0100 Subject: [PATCH] Improved network sync --- DifferentialDrive.cpp | 13 +++-- NetworkPerception.cpp | 78 ++++++++++++++++++++++++++++ NetworkPerception.h | 23 +++++++++ NetworkSync.cpp | 117 ++++++++++++++++++++++++++++++++++++++---- NetworkSync.h | 16 +++++- Perception.cpp | 44 ++++++++++++++++ Perception.h | 8 +-- Roboid.cpp | 12 ++++- Types.h | 22 ++++++++ 9 files changed, 311 insertions(+), 22 deletions(-) create mode 100644 NetworkPerception.cpp create mode 100644 NetworkPerception.h create mode 100644 Types.h diff --git a/DifferentialDrive.cpp b/DifferentialDrive.cpp index dd617c5..ce36d1f 100644 --- a/DifferentialDrive.cpp +++ b/DifferentialDrive.cpp @@ -25,7 +25,7 @@ void DifferentialDrive::SetDimensions(float wheelDiameter, float wheelSeparation) { this->wheelDiameter = wheelDiameter; this->wheelSeparation = wheelSeparation; - this->rpsToMs = wheelDiameter * Angle::PI; + this->rpsToMs = wheelDiameter * Angle::pi; float distance = this->wheelSeparation / 2; this->motors[0]->position.distance = distance; @@ -73,7 +73,7 @@ Polar DifferentialDrive::GetVelocity() { Motor *rightMotor = motors[1]; float leftSpeed = leftMotor->GetActualSpeed(); // in revolutions per second float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions per second - // float rpsToMs = wheelDiameter * Angle::PI; + // float rpsToMs = wheelDiameter * Angle::pi; leftSpeed = leftSpeed * rpsToMs; // in meters per second rightSpeed = rightSpeed * rpsToMs; // in meters per second @@ -85,19 +85,22 @@ Polar DifferentialDrive::GetVelocity() { return velocity; } +#include + float DifferentialDrive::GetAngularVelocity() { Motor *leftMotor = motors[0]; Motor *rightMotor = motors[1]; float leftSpeed = leftMotor->GetActualSpeed(); // in revolutions per second float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions per second - // float rpsToMs = wheelDiameter * Angle::PI; + + // float rpsToMs = wheelDiameter * Angle::pi; leftSpeed = leftSpeed * rpsToMs; // in meters per second rightSpeed = rightSpeed * rpsToMs; // in meters per second float angularSpeed = (rightSpeed - leftSpeed) / 2; - float angularDistance = wheelSeparation / 2 * Angle::PI; + float angularDistance = wheelSeparation / 2 * Angle::pi; float rotationsPerSecond = angularSpeed / angularDistance; - float degreesPerSecond = 360 * rotationsPerSecond; + float degreesPerSecond = Angle::Normalize(360 * rotationsPerSecond); float angularVelocity = degreesPerSecond; return angularVelocity; diff --git a/NetworkPerception.cpp b/NetworkPerception.cpp new file mode 100644 index 0000000..1973401 --- /dev/null +++ b/NetworkPerception.cpp @@ -0,0 +1,78 @@ +#include "NetworkPerception.h" + +#include "RoboidControl/NetworkSync.h" + +void NetworkPerception::ProcessPacket(Roboid *roboid, unsigned char *buffer, + int packetsize) { + // printf("packet received, type = 0x%02x %d %d %d %d\n", buffer[0], + // buffer[2], buffer[3], buffer[4], buffer[5]); + + if (buffer[0] == NetworkSync::PoseMsg) { + ReceiveObject(buffer, roboid); + } +} + +Int32 NetworkPerception::ReceiveInt32(unsigned char *data, int startIndex) { + Int32 a = Int32((UInt32)(data[startIndex + 3]) << 24 | + (UInt32)(data[startIndex + 2]) << 16 | + (UInt32)(data[startIndex + 1]) << 8 | + (UInt32)(data[startIndex + 0])); + return a; +} + +float NetworkPerception::ReceiveFloat100(unsigned char *data, int startIndex) { + Int32 intValue = ReceiveInt32(data, startIndex); + float f = (float)intValue / 100.0F; + return f; +} + +Vector3 NetworkPerception::ReceiveVector3(unsigned char *data, int startIndex) { + float x = ReceiveFloat100(data, startIndex); + float y = ReceiveFloat100(data, startIndex + 4); + float z = ReceiveFloat100(data, startIndex + 8); + Vector3 v = Vector3(x, y, z); + return v; +} + +void NetworkPerception::ReceiveObject(unsigned char *data, Roboid *roboid) { + unsigned char objectId = data[1]; + char poseType = data[2]; + Vector3 worldPosition = ReceiveVector3(data, 3); + Vector3 worldAngles = ReceiveVector3(data, 15); + Quaternion worldOrientation = Quaternion::Euler(worldAngles); + Vector3 worldDirection = worldOrientation * Vector3::forward; + if (objectId == 0) { + roboid->SetPosition(worldPosition); + roboid->SetOrientation(worldOrientation); + // printf("Received pose (%f, %f) (%f, %f)\n", worldPosition.x, + // worldPosition.z, worldDirection.x, worldDirection.z); + + } else { + Vector3 myPosition = roboid->GetPosition(); // Vector3::zero; + Vector2 myPosition2 = Vector3::ProjectHorizontalPlane(myPosition); + Quaternion myOrientation = roboid->GetOrientation(); + Vector3 myDirection = myOrientation * Vector3::forward; + Vector2 myDirection2 = Vector3::ProjectHorizontalPlane(myDirection); + + Vector3 localPosition = + Quaternion::Inverse(myOrientation) * (worldPosition - myPosition); + + // float distance = Vector3::Distance(myPosition, worldPosition); + // float angle = Vector3::SignedAngle(myDirection, localPosition, + // Vector3::up); + float distance = Vector3::Magnitude(localPosition); + float angle = + Vector3::SignedAngle(Vector3::forward, localPosition, Vector3::up); + + Polar position = Polar(angle, distance); + + // int objCount = roboid->perception->TrackedObjectCount(); + + // printf("object received at (%f, %f)/(%f, %f) -> (%f %f)\n", + // worldPosition.x, + // worldPosition.z, localPosition.x, localPosition.z, distance, + // angle); + + roboid->perception->AddTrackedObject(this, position); + } +} diff --git a/NetworkPerception.h b/NetworkPerception.h new file mode 100644 index 0000000..7f7050e --- /dev/null +++ b/NetworkPerception.h @@ -0,0 +1,23 @@ +#pragma once + +#include "RoboidControl/Roboid.h" +#include "RoboidControl/Sensor.h" +#include "RoboidControl/Types.h" + +namespace Passer { +namespace RoboidControl { + +class NetworkPerception : public Sensor { +public: + void ProcessPacket(Roboid *roboid, unsigned char *buffer, int packetsize); + +protected: + void ReceiveObject(unsigned char *data, Roboid *roboid); + + Int32 ReceiveInt32(unsigned char *data, int startIndex); + float ReceiveFloat100(unsigned char *data, int startIndex); + Vector3 ReceiveVector3(unsigned char *data, int startIndex); +}; + +} // namespace RoboidControl +} // namespace Passer \ No newline at end of file diff --git a/NetworkSync.cpp b/NetworkSync.cpp index f1e962f..742feb5 100644 --- a/NetworkSync.cpp +++ b/NetworkSync.cpp @@ -1,20 +1,115 @@ #include "NetworkSync.h" +#ifdef RC_DEBUG +#include +#endif + void NetworkSync::SendVector3(unsigned char *data, int startIndex, Vector3 v) { - SendFloat100(data, startIndex, v.x); - SendFloat100(data, startIndex + 4, v.y); - SendFloat100(data, startIndex + 8, v.z); + SendSingle100(data, startIndex, v.x); + SendSingle100(data, startIndex + 4, v.y); + SendSingle100(data, startIndex + 8, v.z); } -void NetworkSync::SendFloat100(unsigned char *data, int startIndex, - float value) { +void NetworkSync::SendSingle100(unsigned char *data, int startIndex, + float value) { // Sends a float with truncated 2 decimal precision -#ifdef ARDUINO_AVR_UNO - long intValue = value * 100; -#else - int intValue = value * 100; -#endif + Int32 intValue = value * 100; + SendInt32(data, startIndex, intValue); + // for (unsigned char ix = 0; ix < 4; ix++) { + // data[startIndex + ix] = ((unsigned char *)&intValue)[ix]; + // } +} + +void NetworkSync::SendInt32(unsigned char *data, int startIndex, Int32 value) { for (unsigned char ix = 0; ix < 4; ix++) { - data[startIndex + ix] = ((unsigned char *)&intValue)[ix]; + data[startIndex + ix] = ((unsigned char *)&value)[ix]; } +} + +void NetworkSync::PublishTrackedObjects(SendBuffer sendBuffer, + TrackedObject **objects) { + for (unsigned char objIx = 0; objIx < Perception::maxObjectCount; objIx++) { + TrackedObject *obj = objects[objIx]; + if (obj == nullptr) + continue; + // if (obj->sensor->type == Thing::ExternalType) + // continue; + + // tmp + obj->id = objIx; + PublishTrackedObject(sendBuffer, obj); + } +} + +void NetworkSync::PublishTrackedObject(SendBuffer sendBuffer, + TrackedObject *object) { + Vector2 worldPosition2 = Vector2::Rotate( + Vector2::forward * object->position.distance, -object->position.angle); + Vector3 worldPosition3 = Vector3(worldPosition2.x, 0, worldPosition2.y); + +#ifdef RC_DEBUG + Serial.print("Send Pose "); + Serial.print((int)object->id); + Serial.print(" Position "); + Serial.print(worldPosition3.x); + Serial.print(", "); + Serial.print(worldPosition3.y); + Serial.print(", "); + Serial.println(worldPosition3.z); +#else + UInt16 bufferSize = 3 + 12; + UInt8 buffer[bufferSize] = { + PoseMsg, + object->id, + Pose_Position, + }; + SendVector3(buffer, 3, worldPosition3); + sendBuffer(buffer, bufferSize); +#endif +} + +void NetworkSync::SendPoseMsg(SendBuffer sendBuffer, Roboid *roboid) { + Polar velocity = roboid->propulsion->GetVelocity(); + Vector2 worldVelocity2 = + Vector2::Rotate(Vector2::forward * velocity.distance, velocity.angle); + Vector3 worldVelocity3 = Vector3(worldVelocity2.x, 0, worldVelocity2.y); + + float angularVelocity = roboid->propulsion->GetAngularVelocity(); + Vector3 worldAngularVelocity = Vector3(0, angularVelocity, 0); + +#ifdef RC_DEBUG + Serial.print("Send Pose 0 LinearVelocity "); + Serial.print(worldVelocity3.x); + Serial.print(", "); + Serial.print(worldVelocity3.y); + Serial.print(", "); + Serial.print(worldVelocity3.z); + Serial.print(" AngularVelocity "); + Serial.print(worldAngularVelocity.x); + Serial.print(", "); + Serial.print(worldAngularVelocity.y); + Serial.print(", "); + Serial.println(worldAngularVelocity.z); +#else + const unsigned int bufferSize = 3 + 12 + 12; + unsigned char buffer[bufferSize] = { + PoseMsg, + 0, // objectId; + Pose_LinearVelocity | Pose_AngularVelocity, + }; + SendVector3(buffer, 3, worldVelocity3); + SendVector3(buffer, 15, worldAngularVelocity); + + sendBuffer(buffer, bufferSize); +#endif +} + +void NetworkSync::SendDestroyObject(SendBuffer sendBuffer, TrackedObject *obj) { +#ifdef RC_DEBUG + Serial.print("Send Destroy "); + Serial.println((int)obj->id); +#else + unsigned char buffer[2] = {DestroyMsg, (unsigned char)obj->id}; + sendBuffer(buffer, 2); +#endif } \ No newline at end of file diff --git a/NetworkSync.h b/NetworkSync.h index dcffaad..9f207d2 100644 --- a/NetworkSync.h +++ b/NetworkSync.h @@ -1,7 +1,9 @@ #pragma once +#include "NetworkPerception.h" #include "Perception.h" #include "Roboid.h" +#include "Types.h" namespace Passer { namespace RoboidControl { @@ -33,8 +35,20 @@ public: static const char DestroyMsg = 0x20; + typedef void (*SendBuffer)(UInt8 *buffer, UInt16 bufferSize); + + void SendPoseMsg(SendBuffer sendBuffer, Roboid *roboid); + void SendDestroyObject(SendBuffer sendBuffer, TrackedObject *obj); + + void PublishTrackedObjects(SendBuffer sendBuffer, TrackedObject **objects); + +protected: + NetworkPerception *networkPerception; + void PublishTrackedObject(SendBuffer sendBuffer, TrackedObject *object); + void SendVector3(unsigned char *data, int startIndex, Vector3 v); - void SendFloat100(unsigned char *data, int startIndex, float value); + void SendSingle100(unsigned char *data, int startIndex, float value); + void SendInt32(unsigned char *data, int startIndex, Int32 value); }; } // namespace RoboidControl diff --git a/Perception.cpp b/Perception.cpp index 1ae47aa..7f42961 100644 --- a/Perception.cpp +++ b/Perception.cpp @@ -6,6 +6,14 @@ #include +#ifdef RC_DEBUG2 +#include +#endif + +unsigned char Perception::maxObjectCount = 7; // 7 is typically the maximum + // number of object which can + // be tracked by a human + Perception::Perception() { this->trackedObjects = new TrackedObject *[maxObjectCount]; for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) @@ -30,6 +38,22 @@ Sensor *Perception::GetSensor(unsigned int sensorId) { return sensor; } +unsigned int Perception::AddSensor(Sensor *newSensor) { + unsigned int newSensorCount = this->sensorCount + 1; + Sensor **newSensors = new Sensor *[newSensorCount]; + for (unsigned char sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) + newSensors[sensorIx] = sensors[sensorIx]; + + unsigned int sensorId = this->sensorCount; + newSensors[sensorId] = newSensor; + + Sensor **oldSensors = this->sensors; + this->sensors = newSensors; + this->sensorCount = newSensorCount; + delete[] oldSensors; + return sensorId; +} + Sensor *Perception::FindSensorOfType(unsigned int sensorType) { for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { Sensor *sensor = this->sensors[sensorIx]; @@ -107,6 +131,11 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position) { // Do we see the same object? else { if (obj->IsTheSameAs(this->trackedObjects[objIx])) { +#ifdef RC_DEBUG2 + Serial.print((int)this->trackedObjects[objIx]->id); + Serial.println(": update tracked object"); +#endif + this->trackedObjects[objIx]->Refresh(obj->position); delete obj; return; @@ -125,13 +154,28 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position) { if (availableSlotIx < maxObjectCount) { // a slot is available this->trackedObjects[availableSlotIx] = obj; + obj->id = availableSlotIx; +#ifdef RC_DEBUG2 + Serial.print((int)obj->id); + Serial.println(": new tracked object"); +#endif + } // If this object is closer than the farthest object, then replace it else if (obj->position.distance < this->trackedObjects[farthestObjIx]->position.distance) { delete this->trackedObjects[farthestObjIx]; this->trackedObjects[farthestObjIx] = obj; + obj->id = availableSlotIx; +#ifdef RC_DEBUG2 + Serial.print((int)obj->id); + Serial.println(": replaced tracked object"); +#endif } else { +#ifdef RC_DEBUG2 + Serial.print((int)obj->id); + Serial.println(": delete tracked object"); +#endif // No available slot, delete trackedobject delete obj; } diff --git a/Perception.h b/Perception.h index 1c60872..ac3f3f1 100644 --- a/Perception.h +++ b/Perception.h @@ -26,6 +26,8 @@ public: /// @brief The roboid of this perception system Roboid *roboid = nullptr; + unsigned int AddSensor(Sensor *sensor); + /// @brief Get the number of Sensors /// @return The number of sensors, zero when no sensors are present unsigned int GetSensorCount(); @@ -130,9 +132,9 @@ public: float lastUpdateTimeMs = 0; - unsigned char maxObjectCount = 7; // 7 is typically the maximum - // number of object which can - // be tracked by a human + static unsigned char maxObjectCount; // = 7; // 7 is typically the maximum + // number of object which can + // be tracked by a human TrackedObject **trackedObjects; }; diff --git a/Roboid.cpp b/Roboid.cpp index f236fef..2ea020d 100644 --- a/Roboid.cpp +++ b/Roboid.cpp @@ -2,9 +2,17 @@ #include "NetworkSync.h" -Roboid::Roboid() {} +#ifdef RC_DEBUG +#include +#endif -Roboid::Roboid(Perception *perception, Propulsion *propulsion) { +Roboid::Roboid() { +#ifdef RC_DEBUG + Serial.begin(115200); +#endif +} + +Roboid::Roboid(Perception *perception, Propulsion *propulsion) : Roboid() { this->perception = perception; perception->roboid = this; this->propulsion = propulsion; diff --git a/Types.h b/Types.h new file mode 100644 index 0000000..403b824 --- /dev/null +++ b/Types.h @@ -0,0 +1,22 @@ +#pragma once + +using UInt8 = unsigned char; +using SInt8 = signed char; +using Int8 = SInt8; + +using UInt16 = unsigned short; +using SInt16 = signed short; +using Int16 = SInt16; + +#ifdef ARDUINO_AVR_UNO +using UInt32 = unsigned long; +using SInt32 = signed long; +#else +using UInt32 = unsigned int; +using SInt32 = signed int; +#endif +using Int32 = SInt32; + +using UInt64 = unsigned long long; +using SInt64 = signed long long; +using Int64 = SInt64;