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

Improved interestingThings

Browse Source
This commit is contained in:
Pascal Serrarens 2024-11-29 17:44:59 +01:00
parent 40c1e1d50c
commit fb5314019c
6 changed files with 115 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
NetworkPerception.cpp
View File

@ -71,8 +71,9 @@ void NetworkPerception::ReceiveInvestigateMsg(unsigned char *data,
return;
// roboid->networkSync->NewObject(thing);
roboid->networkSync->SendThing(thing);
roboid->networkSync->SendModel(thing);
// roboid->networkSync->SendThing(thing);
// roboid->networkSync->SendModel(thing);
roboid->networkSync->SendThingInfo(thing, false);
}
}

28
NetworkSync.cpp
View File

@ -174,7 +174,6 @@ void NetworkSync::SendDestroyThing(InterestingThing *thing) {
#endif
}
#include <Arduino.h>
void NetworkSync::SendPose(Thing *thing, bool recurse) {
if (this->networkId == 0) // We're not connected to a site yet
return;
@ -198,7 +197,7 @@ void NetworkSync::SendPose(Thing *thing, bool recurse) {
thing->positionUpdated = false;
thing->orientationUpdated = false;
// #if RC_DEBUG
#if RC_DEBUG
if (thing->id == 0) {
Vector3 v = thing->position.ToVector3();
printf("Sent PoseMsg Thing [%d/%d] %f(%f)-(%f %f %f) %f\n",
@ -207,7 +206,7 @@ void NetworkSync::SendPose(Thing *thing, bool recurse) {
v.Up(), v.Forward(),
thing->orientation.swing.horizontal.InDegrees());
}
// #endif
#endif
}
if (recurse) {
@ -265,12 +264,11 @@ void NetworkSync::PublishTrackedObjects(Roboid *roboid,
}
void NetworkSync::PublishTrackedObject(Roboid *roboid,
InterestingThing *object) {
if (object == nullptr || object->updated == false ||
object->networkId != 0x00) {
InterestingThing *thing) {
if (thing == nullptr || thing->updated == false || thing->networkId != 0x00) {
return;
}
printf("publish tracked\n");
/*
Spherical16 roboidPosition = roboid->GetPosition();
SwingTwist16 roboidOrientation = roboid->GetOrientation();
@ -295,23 +293,23 @@ void NetworkSync::PublishTrackedObject(Roboid *roboid,
// SwingTwist16::Inverse(roboid->worldOrigin->orientation);
// Spherical16 originPosition = roboid->worldOrigin->position;
SwingTwist16 worldOrientation = inv_originOrientation * object->orientation;
SwingTwist16 worldOrientation = inv_originOrientation * thing->orientation;
Spherical16 worldPosition =
inv_originOrientation * (object->position - originPosition);
inv_originOrientation * (thing->position - originPosition);
unsigned char ix = 0;
buffer[ix++] = PoseMsg; // Position2DMsg;
buffer[ix++] = object->id; // objectId;
buffer[ix++] = PoseMsg; // Position2DMsg;
buffer[ix++] = thing->id; // objectId;
buffer[ix++] = Pose_Position | Pose_Orientation;
SendSpherical16(buffer, &ix, worldPosition);
SendSwingTwist(buffer, &ix, worldOrientation);
SendBuffer(ix);
#if RC_DEBUG
printf("Sent Thing PoseMsg [%d/%d] %d\n", networkId, buffer[1], object->type);
#endif
// #if RC_DEBUG
printf("Sent Thing PoseMsg [%d/%d] %d\n", networkId, buffer[1], thing->type);
// #endif
object->updated = false;
thing->updated = false;
}
void NetworkSync::SendInvestigate(InterestingThing *thing) {

57
Perception.cpp
View File

@ -273,6 +273,63 @@ InterestingThing *Perception::AddTrackedObject(Sensor *sensor,
return thing;
}
InterestingThing *Perception::AddTrackedObject(Sensor *sensor,
Thing *orgThing) {
InterestingThing *thing =
new InterestingThing(sensor, orgThing->position, orgThing->orientation);
thing->id = orgThing->id;
thing->networkId = 0;
thing->type = orgThing->type;
thing->name = orgThing->name;
thing->modelUrl = orgThing->modelUrl;
thing->modelScale = orgThing->modelScale;
unsigned char farthestObjIx = 0;
unsigned char availableSlotIx = 0;
for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) {
if (this->trackedObjects[thingIx] == nullptr) {
availableSlotIx = thingIx;
}
// Do we see the same object?
else {
if (thing->IsTheSameAs(this->trackedObjects[thingIx])) {
this->trackedObjects[thingIx]->Refresh(
thing->position, thing->orientation); //.ToQuaternion());
delete thing;
return this->trackedObjects[thingIx];
}
// Is this the fartest object we see?
else if (this->trackedObjects[farthestObjIx] == nullptr ||
(this->trackedObjects[thingIx]->position.distance >
this->trackedObjects[farthestObjIx]->position.distance)) {
farthestObjIx = thingIx;
}
}
}
// Check if an perception slot is available (we currently see less than the
// max number of objects)
if (availableSlotIx < maxObjectCount) {
// a slot is available
this->trackedObjects[availableSlotIx] = thing;
if (thing->id == 0x00)
thing->id = lastObjectId++; // availableSlotIx + 1;
return thing;
}
// If this object is closer than the farthest object, then replace it
else if (thing->position.distance <
this->trackedObjects[farthestObjIx]->position.distance) {
delete this->trackedObjects[farthestObjIx];
this->trackedObjects[farthestObjIx] = thing;
if (thing->id == 0x00)
thing->id = lastObjectId++; // availableSlotIx + 1;
return thing;
} else {
return nullptr;
}
}
bool Perception::IsInteresting(float distance) {
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
InterestingThing *thing = this->trackedObjects[objIx];

70
Perception.h
View File

@ -15,38 +15,37 @@ class Roboid;
/// @brief Module to which keeps track of objects around the roboid
class Perception {
public:
public:
/// @brief Default Constructor
Perception();
/// @brief Create a perception setup with the given Sensors
/// @param sensors The Placement of Sensors on the Roboid
/// @param sensorCount The number of sensors in the placement array
Perception(Sensor** sensors, unsigned int sensorCount);
Perception(Sensor **sensors, unsigned int sensorCount);
/// @brief The roboid of this perception system
Roboid* roboid = nullptr;
Roboid *roboid = nullptr;
unsigned int AddSensor(Sensor* sensor);
unsigned int AddSensor(Sensor *sensor);
/// @brief Get the number of Sensors
/// @return The number of sensors, zero when no sensors are present
unsigned int GetSensorCount();
Sensor* GetSensor(unsigned int sensorId);
Sensor *GetSensor(unsigned int sensorId);
/// @brief Find the first sensor of the given type
/// @param sensorType The type of sensor as is defined in the Thing class (for
/// example Thing::SensorType)
/// @return The first sensor found or a nullptr which no sensor has been found
/// of the given type
Sensor* FindSensorOfType(unsigned int sensorType);
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
/// @return The distance to the closest object in meters
float GetDistance(float direction, float range = 10.0F);
float GetDistanceOfType(unsigned char thingType,
float horizontalAngle,
float GetDistanceOfType(unsigned char thingType, float horizontalAngle,
float range = 10.0F);
/// @brief Gets the distance to the closest object
/// @param horizontalDirection The direction in the horizontal plane to look
@ -57,8 +56,7 @@ class Perception {
/// @return The distance to the closest object in meters
/// @details The directions can be thought of as the polar angle (vertical)
/// and azimuthal angle (horizontal) in the spherical coordinate system.
float GetDistance(float horizontalDirection,
float verticalDirection,
float GetDistance(float horizontalDirection, float verticalDirection,
float range = 10.0F);
/// @brief Checks if an object is nearby
@ -93,25 +91,23 @@ class Perception {
// Spherical16 position,
// unsigned char objectType = 0x00,
// unsigned char networkId = 0x00);
InterestingThing* AddTrackedObject(
Sensor* sensor,
Spherical16 position,
SwingTwist16 orientation = SwingTwist16::identity,
unsigned char objectType = 0xFF,
unsigned char objectId = 0x00,
unsigned char networkId = 0x00);
InterestingThing *
AddTrackedObject(Sensor *sensor, Spherical16 position,
SwingTwist16 orientation = SwingTwist16::identity,
unsigned char objectType = 0xFF,
unsigned char objectId = 0x00,
unsigned char networkId = 0x00);
InterestingThing* AddTrackedObject(
Sensor* sensor,
unsigned char networkId,
unsigned char objectId,
Spherical16 position,
SwingTwist16 orientation = SwingTwist16::identity);
InterestingThing *
AddTrackedObject(Sensor *sensor, unsigned char networkId,
unsigned char objectId, Spherical16 position,
SwingTwist16 orientation = SwingTwist16::identity);
InterestingThing *AddTrackedObject(Sensor *sensor, Thing *thing);
bool IsInteresting(float distance);
InterestingThing* FindTrackedObject(char objectId);
InterestingThing* FindTrackedObject(unsigned char networkId,
InterestingThing *FindTrackedObject(char objectId);
InterestingThing *FindTrackedObject(unsigned char networkId,
unsigned char objectId);
/// @brief Retrieve the number of objects currently being tracked by the
@ -124,14 +120,14 @@ class Perception {
/// @details The returned array this should never be a nullptr, but
/// each array entry may be a nullptr when less than maxObjectCount objects is
/// currently being tracked.
InterestingThing** GetTrackedObjects();
InterestingThing **GetTrackedObjects();
unsigned char ThingsOfType(unsigned char objectType,
InterestingThing* buffer[],
InterestingThing *buffer[],
unsigned char bufferSize);
InterestingThing* ThingOfType(unsigned char objectType);
InterestingThing *ThingOfType(unsigned char objectType);
InterestingThing* GetMostInterestingThing();
InterestingThing *GetMostInterestingThing();
// mainly used for confidence update
@ -161,21 +157,21 @@ class Perception {
/// objects
float nearbyDistance = 0.02F;
public:
public:
/// @brief The Sensors used for Perception
Sensor** sensors = nullptr;
Sensor **sensors = nullptr;
/// @brief The number of Sensors used for Perception
unsigned int sensorCount = 0;
unsigned long lastUpdateTimeMs = 0;
unsigned char lastObjectId = 1;
static unsigned char maxObjectCount; // = 7; // 7 is typically the maximum
// number of object which can
// be tracked by a human
InterestingThing** trackedObjects;
static unsigned char maxObjectCount; // = 7; // 7 is typically the maximum
// number of object which can
// be tracked by a human
InterestingThing **trackedObjects;
};
} // namespace RoboidControl
} // namespace Passer
} // namespace RoboidControl
} // namespace Passer
using namespace Passer::RoboidControl;

10
Roboid.cpp
View File

@ -108,10 +108,14 @@ void Roboid::AddChild(Thing *child) {
}
}
#include <Arduino.h>
void Passer::RoboidControl::Roboid::Release(Thing *child) {
if (RemoveChild(child) != nullptr)
this->perception->AddTrackedObject(nullptr, Spherical16::zero,
SwingTwist16::identity, child->type);
if (RemoveChild(child) != nullptr) {
child->position = this->position;
child->orientation = this->orientation;
printf("obj distance %f\n", child->position.distance);
this->perception->AddTrackedObject(nullptr, child);
}
}
void Roboid::LoadModel(const char *url) {

3
Thing.cpp
View File

@ -66,8 +66,9 @@ void Thing::AddChild(Thing *child) {
}
Thing *Thing::RemoveChild(Thing *child) {
int newChildCount = this->childCount - 1;
unsigned char newChildCount = this->childCount - 1;
Thing **newChildren = new Thing *[newChildCount];
unsigned char newChildIx = 0;
for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
if (this->children[childIx] != child) {