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

replace sensor scan by object scan

Browse Source
This commit is contained in:
Pascal Serrarens 2023-12-30 17:26:40 +01:00
parent e120cbe020
commit 5e6bd0e748
2 changed files with 61 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

113
Perception.cpp
View File

@ -1,6 +1,7 @@
#include "Perception.h"
#include "Angle.h"
#include "DistanceSensor.h"
#include "RoboidWiFi.h"
#include "Switch.h"
#include <Arduino.h>
@ -45,20 +46,31 @@ float Perception::GetDistance(float direction, float range) {
if (range < 0)
range = -range;
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
Placement placement = sensorPlacements[sensorIx];
// This still needs support for angles wrapping around 180 degrees !!!!
if (placement.horizontalDirection > direction - range &&
placement.horizontalDirection < direction + range) {
Thing *thing = placement.thing;
if (thing == nullptr)
continue;
// for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
// Placement placement = sensorPlacements[sensorIx];
// // This still needs support for angles wrapping around 180 degrees !!!!
// if (placement.horizontalDirection > direction - range &&
// placement.horizontalDirection < direction + range) {
// Thing *thing = placement.thing;
// if (thing == nullptr)
// continue;
if (thing->type == Thing::DistanceSensorType) {
DistanceSensor *distanceSensor = (DistanceSensor *)thing;
if (distanceSensor != nullptr && distanceSensor->ObjectNearby())
minDistance = fmin(minDistance, distanceSensor->GetDistance());
}
// if (thing->type == Thing::DistanceSensorType) {
// DistanceSensor *distanceSensor = (DistanceSensor *)thing;
// if (distanceSensor != nullptr && distanceSensor->ObjectNearby())
// minDistance = fmin(minDistance, distanceSensor->GetDistance());
// }
// }
// }
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
PerceivedObject *obj = perceivedObjects[objIx];
if (obj == nullptr)
continue;
if (obj->position.angle > direction - range &&
obj->position.angle < direction + range) {
minDistance = fmin(minDistance, obj->position.distance);
}
}
return minDistance;
@ -70,34 +82,13 @@ float Perception::GetDistance(float horizontalDirection,
if (range < 0)
range = -range;
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
Placement placement = sensorPlacements[sensorIx];
// This still needs support for angles wrapping around 180 degrees !!!!
if (placement.horizontalDirection > horizontalDirection - range &&
placement.horizontalDirection < horizontalDirection + range &&
placement.verticalDirection > verticalDirection - range &&
placement.verticalDirection < verticalDirection + range) {
Thing *thing = placement.thing;
if (thing == nullptr)
continue;
if (thing->type == Thing::DistanceSensorType) {
DistanceSensor *distanceSensor = (DistanceSensor *)thing;
if (distanceSensor != nullptr && distanceSensor->ObjectNearby())
minDistance = fmin(minDistance, distanceSensor->GetDistance());
}
}
}
return minDistance;
}
bool Perception::ObjectNearby(float direction, float range) {
if (range < 0)
range = -range;
// for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
// Placement placement = sensorPlacements[sensorIx];
// if (placement.horizontalDirection > direction - range &&
// placement.horizontalDirection < direction + range) {
// // This still needs support for angles wrapping around 180 degrees !!!!
// if (placement.horizontalDirection > horizontalDirection - range &&
// placement.horizontalDirection < horizontalDirection + range &&
// placement.verticalDirection > verticalDirection - range &&
// placement.verticalDirection < verticalDirection + range) {
// Thing *thing = placement.thing;
// if (thing == nullptr)
// continue;
@ -105,14 +96,26 @@ bool Perception::ObjectNearby(float direction, float range) {
// if (thing->type == Thing::DistanceSensorType) {
// DistanceSensor *distanceSensor = (DistanceSensor *)thing;
// if (distanceSensor != nullptr && distanceSensor->ObjectNearby())
// return true;
// } else if (thing->type == Thing::SwitchType) {
// Switch *switchSensor = (Switch *)thing;
// if (switchSensor != nullptr && switchSensor->IsOn())
// return true;
// minDistance = fmin(minDistance, distanceSensor->GetDistance());
// }
// }
// }
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
PerceivedObject *obj = perceivedObjects[objIx];
if (obj == nullptr)
continue;
if (obj->position.angle > horizontalDirection - range &&
obj->position.angle < horizontalDirection + range) {
minDistance = fmin(minDistance, obj->position.distance);
}
}
return minDistance;
}
bool Perception::ObjectNearby(float direction, float range) {
if (range < 0)
range = -range;
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
PerceivedObject *obj = perceivedObjects[objIx];
@ -205,7 +208,7 @@ void Perception::AddPerceivedObject(Sensor *sensor, Polar position) {
// this->perceivedObjects[objIx]->position.angle,
// obj->position.distance, obj->position.angle);
if (obj->IsTheSameAs(this->perceivedObjects[objIx])) {
printf("[%d] Updating...\n", objIx);
// printf("[%d] Updating...\n", objIx);
this->perceivedObjects[objIx]->Refresh(obj->position, obj->radius);
return;
}
@ -264,14 +267,10 @@ void Perception::Update(float currentTimeMs) {
if (thing->type == Thing::DistanceSensorType) {
DistanceSensor *distanceSensor = (DistanceSensor *)thing;
printf("S%d: %d %f\n", sensorIx, (int)distanceSensor,
distanceSensor->GetDistance());
if (distanceSensor != nullptr && distanceSensor->ObjectNearby()) {
Polar position = Polar(thingPlacement.horizontalDirection,
distanceSensor->GetDistance());
AddPerceivedObject(distanceSensor, position);
}
float distance = distanceSensor->GetDistance();
float angle = thingPlacement.horizontalDirection;
Polar position = Polar(angle, distance);
AddPerceivedObject(distanceSensor, position);
} else if (thing->type == Thing::SwitchType) {
Switch *switchSensor = (Switch *)thing;
if (switchSensor != nullptr && switchSensor->IsOn()) {
@ -290,10 +289,12 @@ void Perception::Update(float currentTimeMs) {
if (obj->DegradeConfidence(deltaTime) == false) {
// delete obj
printf("[%d] delete object\n", objIx);
if (roboid != nullptr && roboid->networkSync != nullptr)
roboid->networkSync->DestroyObject(obj);
this->perceivedObjects[objIx] = nullptr;
} else {
Serial.printf("[%d] confidence: %d\n", objIx,
this->perceivedObjects[objIx]->confidence);
// } else {
// Serial.printf("[%d] confidence: %d\n", objIx,
// this->perceivedObjects[objIx]->confidence);
}
}
if (this->perceivedObjects[0] != nullptr) {

4
Perception.h
View File

@ -8,6 +8,8 @@
namespace Passer {
namespace RoboidControl {
class Roboid;
class PerceivedObject {
public:
PerceivedObject();
@ -48,6 +50,8 @@ public:
/// @param sensorCount The number of sensors in the placement array
Perception(Placement *sensors, unsigned int sensorCount);
Roboid *roboid = nullptr;
/// @brief Get the number of Sensors
/// @return The number of sensors, zero when no sensors are present
unsigned int GetSensorCount();