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

9
Perception.h
View File

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