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

Add tracked object type

Browse Source
This commit is contained in:
Pascal Serrarens 2024-04-18 17:24:59 +02:00
parent 16e553907a
commit c9d2990da7
3 changed files with 15 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
Perception.cpp
View File

@ -6,12 +6,12 @@
#include <math.h> #include <math.h>
// #define RC_DEBUG2 #define RC_DEBUG2
#ifdef RC_DEBUG2 #ifdef RC_DEBUG2
#include <Arduino.h> #include <Arduino.h>
#endif #endif
unsigned char Perception::maxObjectCount = 7; // 7 is typically the maximum unsigned char Perception::maxObjectCount = 70; // 7 is typically the maximum
// number of object which can // number of object which can
// be tracked by a human // be tracked by a human
@ -124,8 +124,10 @@ bool Perception::ObjectNearby(float direction, float range) {
return false; return false;
} }
void Perception::AddTrackedObject(Sensor *sensor, Polar position) { void Perception::AddTrackedObject(Sensor *sensor, Polar position,
unsigned char objectType) {
InterestingThing *obj = new InterestingThing(sensor, position); InterestingThing *obj = new InterestingThing(sensor, position);
obj->type = objectType;
unsigned char farthestObjIx = 0; unsigned char farthestObjIx = 0;
unsigned char availableSlotIx = 0; unsigned char availableSlotIx = 0;
@ -142,6 +144,7 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position) {
#endif #endif
this->trackedObjects[objIx]->Refresh(obj->position); this->trackedObjects[objIx]->Refresh(obj->position);
this->trackedObjects[objIx]->type = objectType;
delete obj; delete obj;
return; return;
} }
@ -164,7 +167,7 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position) {
Serial.print((int)obj->id); Serial.print((int)obj->id);
Serial.println(": new tracked object"); Serial.println(": new tracked object");
#endif #endif
roboid->networkSync->NewObject(obj);
} }
// If this object is closer than the farthest object, then replace it // If this object is closer than the farthest object, then replace it
else if (obj->position.distance < else if (obj->position.distance <
@ -331,14 +334,14 @@ void Perception::Update(float currentTimeMs) {
if (distance >= 0) { if (distance >= 0) {
float angle = sensor->position.angle; float angle = sensor->position.angle;
Polar position = Polar(angle, distance); Polar position = Polar(angle, distance);
AddTrackedObject(distanceSensor, position); // AddTrackedObject(distanceSensor, position);
} }
} else if (sensor->type == Thing::SwitchType) { } else if (sensor->type == Thing::SwitchType) {
Switch *switchSensor = (Switch *)sensor; Switch *switchSensor = (Switch *)sensor;
if (switchSensor->IsOn()) { if (switchSensor->IsOn()) {
Polar position = Polar(sensor->position.angle, nearbyDistance); Polar position = Polar(sensor->position.angle, nearbyDistance);
AddTrackedObject(switchSensor, position); // AddTrackedObject(switchSensor, position);
} }
} else { } else {
sensor->Update(); sensor->Update();

5
Perception.h
View File

@ -85,7 +85,8 @@ public:
/// @param sensor The sensor which has detected the object /// @param sensor The sensor which has detected the object
/// @param position The position of the sensor in polar coordinates local to /// @param position The position of the sensor in polar coordinates local to
/// the roboid /// the roboid
void AddTrackedObject(Sensor *sensor, Polar position); void AddTrackedObject(Sensor *sensor, Polar position,
unsigned char objectType = 0x00);
InterestingThing * InterestingThing *
AddTrackedObject(Sensor *sensor, Spherical position, AddTrackedObject(Sensor *sensor, Spherical position,
Quaternion orientation = Quaternion::identity); Quaternion orientation = Quaternion::identity);
@ -135,7 +136,7 @@ public:
/// nearby. /// nearby.
/// @details This value is used by the ObjectNearby function to select the /// @details This value is used by the ObjectNearby function to select the
/// objects /// objects
float nearbyDistance = 0.3F; float nearbyDistance = 0.02F;
public: public:
/// @brief The Sensors used for Perception /// @brief The Sensors used for Perception

2
TrackedObject.h
View File

@ -74,7 +74,7 @@ public:
protected: protected:
static constexpr unsigned char maxConfidence = 255; static constexpr unsigned char maxConfidence = 255;
static constexpr unsigned char confidenceDropSpeed = 150; // 2; static constexpr unsigned char confidenceDropSpeed = 10; // 150; // 2;
}; };
} // namespace RoboidControl } // namespace RoboidControl