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

Added Sphere perception

Browse Source
This commit is contained in:
Pascal Serrarens 2024-06-07 17:47:42 +02:00
parent 643129fdae
commit 4059e26027
3 changed files with 105 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
NetworkPerception.cpp
View File

@ -107,6 +107,30 @@ void NetworkPerception::ReceivePlane(unsigned char *data, Roboid *roboid) {
roboid->perception->AddTrackedObject(this, position, 0x80, networkId);
}
void NetworkPerception::ReceiveSphere(unsigned char *data, Roboid *roboid) {
unsigned char networkId = data[1];
float radius = ReceiveFloat100(data, 3);
Vector3 worldPosition = ReceiveVector3(data, 7);
Vector3 roboidPosition = roboid->GetPosition();
Vector3 deltaPosition = worldPosition - roboidPosition;
Quaternion roboidOrientation = roboid->GetOrientation();
Vector3 localPosition =
Quaternion::Inverse(roboidOrientation) * deltaPosition;
Spherical position = Spherical(localPosition);
roboid->perception->AddTrackedObject(this, position, Quaternion::identity,
0x81, 0x81, networkId);
roboid->networkSync->SendText("Received Sphere\n\0");
if ((float)position.horizontalAngle == 0)
roboid->networkSync->SendText("Zero hor angle\0");
if ((float)position.verticalAngle == 0)
roboid->networkSync->SendText("Zero vertical angle\0");
}
void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
unsigned char networkId = data[1];
unsigned char objectId = data[2];
@ -118,6 +142,8 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
if (objectId == 0x80)
return ReceivePlane(data, roboid);
else if (objectId = 0x81)
return ReceiveSphere(data, roboid);
if ((poseType & NetworkSync::Pose_Position) != 0) {
Vector3 worldPosition = ReceiveVector3(data, 4);

1
NetworkPerception.h
View File

@ -18,6 +18,7 @@ protected:
void ReceiveTypedObject(unsigned char *data, Roboid *roboid);
void ReceivePlane(unsigned char *data, Roboid *roboid);
void ReceiveSphere(unsigned char *data, Roboid *roboid);
Int32 ReceiveInt32(unsigned char *data, int startIndex);
float ReceiveFloat100(unsigned char *data, int startIndex);

165
Perception.cpp
View File

@ -189,83 +189,87 @@ bool Perception::ObjectNearby(float direction, float range) {
}
// #include <WifiSync.h>
// This function is deprecated
void Perception::AddTrackedObject(Sensor *sensor, Polar position,
unsigned char thingType,
unsigned char networkId) {
InterestingThing *obj = new InterestingThing(sensor, position);
obj->type = thingType;
Spherical sPos = Spherical(position);
Quaternion orientation = Quaternion::identity;
AddTrackedObject(sensor, sPos, orientation, thingType, thingType, networkId);
/*
InterestingThing *obj = new InterestingThing(sensor, position);
obj->type = thingType;
unsigned char farthestObjIx = 0;
int availableSlotIx = -1;
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
if (this->trackedObjects[objIx] == nullptr) {
availableSlotIx = objIx;
}
// 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");
printf("%d: update tracked object [/%d]\n", objIx, obj->id);
#endif
this->trackedObjects[objIx]->Refresh(obj->position);
this->trackedObjects[objIx]->type = thingType;
delete obj;
return;
unsigned char farthestObjIx = 0;
int availableSlotIx = -1;
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
if (this->trackedObjects[objIx] == nullptr) {
availableSlotIx = objIx;
}
// Is this the fartest object we see?
else if (this->trackedObjects[farthestObjIx] == nullptr ||
(this->trackedObjects[objIx]->position.distance >
this->trackedObjects[farthestObjIx]->position.distance)) {
farthestObjIx = objIx;
// 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");
printf("%d: update tracked object [/%d]\n", objIx, obj->id);
#endif
this->trackedObjects[objIx]->Refresh(obj->position);
this->trackedObjects[objIx]->type = thingType;
delete obj;
return;
}
// Is this the fartest object we see?
else if (this->trackedObjects[farthestObjIx] == nullptr ||
(this->trackedObjects[objIx]->position.distance >
this->trackedObjects[farthestObjIx]->position.distance)) {
farthestObjIx = objIx;
}
}
}
}
// Check if an perception slot is available (we currently see less than the
// max number of objects)
if (availableSlotIx >= 0) { //< maxObjectCount) {
// a slot is available
this->trackedObjects[availableSlotIx] = obj;
obj->networkId = networkId;
obj->id = lastObjectId++; // availableSlotIx + 1;
#ifdef RC_DEBUG2
printf("%d: new tracked object [%d/%d]\n", availableSlotIx, obj->networkId,
obj->id);
#endif
if (roboid->networkSync != nullptr) {
roboid->networkSync->NewObject(obj);
// ((WifiSync *)roboid->networkSync)->PublishTrackedObject(roboid, obj);
// Check if an perception slot is available (we currently see less than the
// max number of objects)
if (availableSlotIx >= 0) { //< maxObjectCount) {
// a slot is available
this->trackedObjects[availableSlotIx] = obj;
obj->networkId = networkId;
obj->id = lastObjectId++; // availableSlotIx + 1;
#ifdef RC_DEBUG2
printf("%d: new tracked object [%d/%d]\n", availableSlotIx,
obj->networkId, obj->id); #endif if (roboid->networkSync != nullptr) {
roboid->networkSync->NewObject(obj);
// ((WifiSync *)roboid->networkSync)->PublishTrackedObject(roboid, obj);
}
}
}
// 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->networkId = networkId;
obj->id = lastObjectId++; // availableSlotIx + 1;
#ifdef RC_DEBUG2
// Serial.print((int)obj->id);
// Serial.println(": replaced tracked object");
printf("%d: replaced tracked object [/%d]\n", farthestObjIx, obj->id);
#endif
if (roboid->networkSync != nullptr) {
roboid->networkSync->NewObject(obj);
// ((WifiSync *)roboid->networkSync)->PublishTrackedObject(roboid, obj);
}
} else {
#ifdef RC_DEBUG2
// Serial.print((int)obj->id);
// Serial.println(": delete tracked object");
printf("%d: delete tracked object [/%d]\n", -1, obj->id);
// 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->networkId = networkId;
obj->id = lastObjectId++; // availableSlotIx + 1;
#ifdef RC_DEBUG2
// Serial.print((int)obj->id);
// Serial.println(": replaced tracked object");
printf("%d: replaced tracked object [/%d]\n", farthestObjIx, obj->id);
#endif
if (roboid->networkSync != nullptr) {
roboid->networkSync->NewObject(obj);
// ((WifiSync *)roboid->networkSync)->PublishTrackedObject(roboid, obj);
}
} else {
#ifdef RC_DEBUG2
// Serial.print((int)obj->id);
// Serial.println(": delete tracked object");
printf("%d: delete tracked object [/%d]\n", -1, obj->id);
#endif
// No available slot, delete trackedobject
delete obj;
}
#endif
// No available slot, delete trackedobject
delete obj;
}
*/
}
InterestingThing *
@ -273,6 +277,8 @@ Perception::AddTrackedObject(Sensor *sensor, Spherical position,
Quaternion orientation, unsigned char thingType,
unsigned char thingId, unsigned networkId) {
InterestingThing *thing = new InterestingThing(sensor, position, orientation);
if (thingId != 0x00)
thing->id = thingId;
thing->type = thingType;
unsigned char farthestObjIx = 0;
@ -284,11 +290,11 @@ Perception::AddTrackedObject(Sensor *sensor, Spherical position,
// Do we see the same object?
else {
if (thing->IsTheSameAs(this->trackedObjects[thingIx])) {
#ifdef RC_DEBUG2
// Serial.print((int)this->trackedObjects[objIx]->id);
// Serial.println(": update tracked object");
#endif
if ((float)thing->position.horizontalAngle !=
(float)this->trackedObjects[thingIx]->position.horizontalAngle)
this->roboid->networkSync->SendText("Update\0");
else
this->roboid->networkSync->SendText("Refresh\0");
this->trackedObjects[thingIx]->Refresh(thing->position,
thing->orientation);
delete thing;
@ -311,12 +317,6 @@ Perception::AddTrackedObject(Sensor *sensor, Spherical position,
thing->networkId = networkId;
if (thingId == 0x00)
thing->id = lastObjectId++; // availableSlotIx + 1;
else
thing->id = thingId;
#ifdef RC_DEBUG2
printf("%d: new tracked object {%d/%d}\n", availableSlotIx, obj->networkId,
obj->id);
#endif
return thing;
}
// If this object is closer than the farthest object, then replace it
@ -327,17 +327,8 @@ Perception::AddTrackedObject(Sensor *sensor, Spherical position,
thing->networkId = networkId;
if (thingId == 0x00)
thing->id = lastObjectId++; // availableSlotIx + 1;
else
thing->id = thingId;
#ifdef RC_DEBUG2
printf("%d: replaced tracked object {%d/%d}\n", farthestObjIx,
obj->networkId, obj->id);
#endif
return thing;
} else {
#ifdef RC_DEBUG2
printf("%d: delete tracked object {%d/%d}\n", -1, obj->networkId, obj->id);
#endif
// No available slot, delete trackedobject
delete thing;
return nullptr;