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

Performance improvement

Browse Source
This commit is contained in:
Pascal Serrarens 2024-05-10 11:17:44 +02:00
parent 2b392ef045
commit ba18f5763f
5 changed files with 46 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
DifferentialDrive.cpp
View File

@ -47,7 +47,6 @@ void DifferentialDrive::SetMotorTargetSpeeds(float leftSpeed,
};
}
#include <Arduino.h>
void DifferentialDrive::SetTwistSpeed(float forward, float yaw) {
float leftSpeed =
Float::Clamp(forward + yaw, -1, 1); // revolutions per second

12
NetworkPerception.cpp
View File

@ -20,7 +20,6 @@ void NetworkPerception::ProcessPacket(Roboid *roboid, unsigned char *buffer,
}
}
#include <Arduino.h>
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;
}

36
Perception.cpp
View File

@ -6,7 +6,7 @@
#include <math.h>
#define RC_DEBUG2
// #define RC_DEBUG2
#ifdef RC_DEBUG2
#include <Arduino.h>
#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 <WifiSync.h>
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);

3
Perception.h
View File

@ -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,

25
TrackedObject.cpp
View File

@ -20,24 +20,33 @@ InterestingThing::InterestingThing(Sensor *sensor, Spherical position,
this->updated = true;
}
#include <Arduino.h>
// #include <Arduino.h>
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;
}