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

Add sensor reference for perceived objects

Browse Source
This commit is contained in:
Pascal Serrarens 2023-12-30 09:28:26 +01:00
parent fe16f25859
commit 9a70b3d3d2
2 changed files with 28 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
Perception.cpp
View File

@ -30,6 +30,16 @@ Sensor *Perception::GetSensor(unsigned int sensorId) {
return nullptr;
}
Sensor *Perception::FindSensorOfType(unsigned int sensorType) {
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
Sensor *sensor = (Sensor *)this->sensorPlacements[sensorIx].thing;
if (sensor->type == sensorType)
return sensor;
}
return nullptr;
}
float Perception::GetDistance(float direction, float range) {
float minDistance = INFINITY;
if (range < 0)
@ -129,8 +139,9 @@ PerceivedObject::PerceivedObject() {
this->radius = INFINITY;
}
PerceivedObject::PerceivedObject(Polar position, float radius)
PerceivedObject::PerceivedObject(Sensor *sensor, Polar position, float radius)
: PerceivedObject() {
this->sensor = sensor;
this->position = position;
this->radius = radius;
}
@ -168,11 +179,11 @@ void PerceivedObject::Refresh(Polar position, float radius) {
this->confidence = maxConfidence;
}
void Perception::AddPerceivedObject(Polar position) {
void Perception::AddPerceivedObject(Sensor *sensor, Polar position) {
// int objCount = PerceivedObjectCount();
// printf("perc obj count %d\n");
PerceivedObject *obj = new PerceivedObject(position);
PerceivedObject *obj = new PerceivedObject(sensor, position);
// objCount = PerceivedObjectCount();
// printf("perc obj count %d\n");
// AddPerceivedObject(obj);
@ -189,10 +200,10 @@ void Perception::AddPerceivedObject(Polar position) {
}
// Do we see the same object?
else {
printf("(%d) my %f %f =^= received %f %f\n", objIx,
this->perceivedObjects[objIx]->position.distance,
this->perceivedObjects[objIx]->position.angle,
obj->position.distance, obj->position.angle);
// printf("(%d) my %f %f =^= received %f %f\n", objIx,
// this->perceivedObjects[objIx]->position.distance,
// this->perceivedObjects[objIx]->position.angle,
// obj->position.distance, obj->position.angle);
if (obj->IsTheSameAs(this->perceivedObjects[objIx])) {
printf("[%d] Updating...\n", objIx);
this->perceivedObjects[objIx]->Refresh(obj->position, obj->radius);
@ -252,19 +263,21 @@ 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(position);
AddPerceivedObject(distanceSensor, position);
}
} else if (thing->type == Thing::SwitchType) {
Switch *switchSensor = (Switch *)thing;
if (switchSensor != nullptr && switchSensor->IsOn()) {
Polar position =
Polar(thingPlacement.horizontalDirection, nearbyDistance);
AddPerceivedObject(position);
AddPerceivedObject(switchSensor, position);
}
}
}

9
Perception.h
View File

@ -11,7 +11,7 @@ namespace RoboidControl {
class PerceivedObject {
public:
PerceivedObject();
PerceivedObject(Polar position, float radius = 0.1F);
PerceivedObject(Sensor *sensor, Polar position, float radius = 0.1F);
static constexpr float equalityDistance = 0.3F;
static constexpr float equalityAngle = 5.0F;
@ -19,8 +19,9 @@ public:
char id;
Polar position;
Polar position = Polar::zero;
float radius;
Sensor *sensor = nullptr;
static constexpr char maxConfidence = 255;
static constexpr char confidenceDropSpeed = 2;
@ -52,6 +53,8 @@ public:
unsigned int GetSensorCount();
Sensor *GetSensor(unsigned int sensorId);
Sensor *FindSensorOfType(unsigned int sensorType);
/// @brief Gets the distance to the closest object
/// @param direction The direction to look for objects
/// @param range The range in which objects should be looked for
@ -95,7 +98,7 @@ public:
// Object Perception
void AddPerceivedObject(Polar position);
void AddPerceivedObject(Sensor *sensor, Polar position);
// void AddPerceivedObject(PerceivedObject *obj);
unsigned char PerceivedObjectCount();
PerceivedObject **GetPerceivedObjects();