From ba18f5763f2ac1550d6d3373b55e32175e0975f0 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 10 May 2024 11:17:44 +0200 Subject: [PATCH] Performance improvement --- DifferentialDrive.cpp | 1 - NetworkPerception.cpp | 12 ++++++------ Perception.cpp | 36 +++++++++++++++++++++--------------- Perception.h | 3 ++- TrackedObject.cpp | 25 +++++++++++++++++-------- 5 files changed, 46 insertions(+), 31 deletions(-) diff --git a/DifferentialDrive.cpp b/DifferentialDrive.cpp index 3d432be..d3f2186 100644 --- a/DifferentialDrive.cpp +++ b/DifferentialDrive.cpp @@ -47,7 +47,6 @@ void DifferentialDrive::SetMotorTargetSpeeds(float leftSpeed, }; } -#include void DifferentialDrive::SetTwistSpeed(float forward, float yaw) { float leftSpeed = Float::Clamp(forward + yaw, -1, 1); // revolutions per second diff --git a/NetworkPerception.cpp b/NetworkPerception.cpp index a395b6b..a6c0a65 100644 --- a/NetworkPerception.cpp +++ b/NetworkPerception.cpp @@ -20,7 +20,6 @@ void NetworkPerception::ProcessPacket(Roboid *roboid, unsigned char *buffer, } } -#include void NetworkPerception::ReceiveCreateMsg(unsigned char *data, Roboid *roboid) { unsigned char networkId = data[1]; unsigned char objectId = data[2]; @@ -37,6 +36,7 @@ void NetworkPerception::ReceiveCreateMsg(unsigned char *data, Roboid *roboid) { } void NetworkPerception::ReceivePlane(unsigned char *data, Roboid *roboid) { + unsigned char networkId = data[1]; unsigned char poseType = data[3]; if ((poseType & NetworkSync::Pose_Position) == 0 | (poseType & NetworkSync::Pose_Orientation) == 0) @@ -50,7 +50,7 @@ void NetworkPerception::ReceivePlane(unsigned char *data, Roboid *roboid) { float distance = deltaPosition.magnitude(); if (roboid->perception->IsInteresting(distance) == false) { - printf(" plane not interesting\n"); + // printf(" plane not interesting\n"); return; } @@ -67,9 +67,9 @@ void NetworkPerception::ReceivePlane(unsigned char *data, Roboid *roboid) { // worldPosition.y, worldPosition.z, roboidPosition.x, // roboidPosition.y, roboidPosition.z, position.distance, // (float)position.horizontalAngle, (float)position.verticalAngle); - printf("Received plane %f (%f %f)\n", position.distance, - (float)position.horizontalAngle, (float)position.verticalAngle); - roboid->perception->AddTrackedObject(this, position, 0x80); + // printf("Received plane %f (%f %f)\n", position.distance, + // (float)position.horizontalAngle, (float)position.verticalAngle); + roboid->perception->AddTrackedObject(this, position, 0x80, networkId); } void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) { @@ -93,7 +93,7 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) { float distance = Vector3::Distance(roboidPosition, worldPosition); if (roboid->perception->IsInteresting(distance) == false) { - printf(" not interesting\n"); + // printf(" not interesting\n"); return; } diff --git a/Perception.cpp b/Perception.cpp index 3ffe56d..e3751d6 100644 --- a/Perception.cpp +++ b/Perception.cpp @@ -6,7 +6,7 @@ #include -#define RC_DEBUG2 +// #define RC_DEBUG2 #ifdef RC_DEBUG2 #include #endif @@ -76,27 +76,31 @@ float GetPlaneDistance(InterestingThing *plane, float horizontalAngle, horizontalAngle); if (abs(deltaAngle) < abs(range)) { // distance = distance - printf(" plane distance = %f (%f-%f)+%f=%f", distance, - (float)plane->position.horizontalAngle, horizontalAngle, range, - deltaAngle); + // printf(" plane distance = %f (%f-%f)+%f=%f", distance, + // (float)plane->position.horizontalAngle, horizontalAngle, range, + // deltaAngle); } else if (deltaAngle < -range) { float angle = deltaAngle + range; - printf(" plane distance < %f (%f-%f)+%f=%f %f", distance, - (float)plane->position.horizontalAngle, horizontalAngle, range, - angle, cosf(angle * Angle::Deg2Rad)); + // printf(" plane distance < %f (%f-%f)+%f=%f %f", distance, + // (float)plane->position.horizontalAngle, horizontalAngle, range, + // angle, cosf(angle * Angle::Deg2Rad)); if (angle > -90) distance = distance / cosf(angle * Angle::Deg2Rad); + else + distance = 9999; // infinity? } else if (deltaAngle > range) { float angle = deltaAngle - range; - printf(" plane distance > %f (%f-%f)-%f=%f %f", distance, - (float)plane->position.horizontalAngle, horizontalAngle, range, - angle, cosf(angle * Angle::Deg2Rad)); + // printf(" plane distance > %f (%f-%f)-%f=%f %f", distance, + // (float)plane->position.horizontalAngle, horizontalAngle, range, + // angle, cosf(angle * Angle::Deg2Rad)); if (angle < 90) distance = distance / cosf(angle * Angle::Deg2Rad); + else + distance = 9999; // infinity? } - printf(" distance = %f\n", distance); + // printf(" distance = %f\n", distance); return distance; } @@ -186,7 +190,8 @@ bool Perception::ObjectNearby(float direction, float range) { #include void Perception::AddTrackedObject(Sensor *sensor, Polar position, - unsigned char thingType) { + unsigned char thingType, + unsigned char networkId) { InterestingThing *obj = new InterestingThing(sensor, position); obj->type = thingType; @@ -224,10 +229,11 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position, if (availableSlotIx >= 0) { //< maxObjectCount) { // a slot is available this->trackedObjects[availableSlotIx] = obj; - obj->networkId = 0x00; + obj->networkId = networkId; obj->id = lastObjectId++; // availableSlotIx + 1; #ifdef RC_DEBUG2 - printf("%d: new tracked object [/%d]\n", availableSlotIx, obj->id); + printf("%d: new tracked object [%d/%d]\n", availableSlotIx, obj->networkId, + obj->id); #endif roboid->networkSync->NewObject(obj); ((WifiSync *)roboid->networkSync)->PublishTrackedObject(roboid, obj); @@ -237,7 +243,7 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position, this->trackedObjects[farthestObjIx]->position.distance) { delete this->trackedObjects[farthestObjIx]; this->trackedObjects[farthestObjIx] = obj; - obj->networkId = 0x00; + obj->networkId = networkId; obj->id = lastObjectId++; // availableSlotIx + 1; #ifdef RC_DEBUG2 // Serial.print((int)obj->id); diff --git a/Perception.h b/Perception.h index 746a01d..6abc9d1 100644 --- a/Perception.h +++ b/Perception.h @@ -88,7 +88,8 @@ public: /// @param position The position of the sensor in polar coordinates local to /// the roboid void AddTrackedObject(Sensor *sensor, Polar position, - unsigned char objectType = 0x00); + unsigned char objectType = 0x00, + unsigned char networkId = 0x00); InterestingThing * AddTrackedObject(Sensor *sensor, Spherical position, Quaternion orientation = Quaternion::identity, diff --git a/TrackedObject.cpp b/TrackedObject.cpp index e1ff025..5b46635 100644 --- a/TrackedObject.cpp +++ b/TrackedObject.cpp @@ -20,24 +20,33 @@ InterestingThing::InterestingThing(Sensor *sensor, Spherical position, this->updated = true; } -#include +// #include bool InterestingThing::IsTheSameAs(InterestingThing *otherObj) { if (id != 0 && id == otherObj->id) return true; if (type != otherObj->type) return false; - printf(" %f (%f %f) is same as %f (%f %f)\n", otherObj->position.distance, - (float)otherObj->position.horizontalAngle, - (float)otherObj->position.verticalAngle, position.distance, - (float)position.horizontalAngle, (float)position.verticalAngle); - if (fabsf(position.distance - otherObj->position.distance) > equalityDistance) + + float equalDistance = equalityDistance; + float equalAngle = equalityAngle; + if (otherObj->type == 0x80 || otherObj->id == 0x80) { + equalDistance = 0.1; + equalAngle = 10; + } + // printf(" %d %f (%f %f) is same as %f (%f %f)\n", otherObj->type, + // otherObj->position.distance, + // (float)otherObj->position.horizontalAngle, + // (float)otherObj->position.verticalAngle, position.distance, + // (float)position.horizontalAngle, (float)position.verticalAngle); + if (fabsf(position.distance - otherObj->position.distance) > equalDistance) return false; if (fabsf(position.horizontalAngle - otherObj->position.horizontalAngle) > - equalityAngle) + equalAngle) return false; if (fabsf(position.verticalAngle - otherObj->position.verticalAngle) > - equalityAngle) + equalAngle) return false; + // printf(" -> yes "); return true; }