From fb5314019ce5ebbc870a3e088d3f6a308cea31d4 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 29 Nov 2024 17:44:59 +0100 Subject: [PATCH] Improved interestingThings --- NetworkPerception.cpp | 5 ++-- NetworkSync.cpp | 28 ++++++++--------- Perception.cpp | 57 +++++++++++++++++++++++++++++++++++ Perception.h | 70 ++++++++++++++++++++----------------------- Roboid.cpp | 10 +++++-- Thing.cpp | 3 +- 6 files changed, 115 insertions(+), 58 deletions(-) diff --git a/NetworkPerception.cpp b/NetworkPerception.cpp index a453aeb..bbe3fe1 100644 --- a/NetworkPerception.cpp +++ b/NetworkPerception.cpp @@ -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); } } diff --git a/NetworkSync.cpp b/NetworkSync.cpp index c01d7af..be73a27 100644 --- a/NetworkSync.cpp +++ b/NetworkSync.cpp @@ -174,7 +174,6 @@ void NetworkSync::SendDestroyThing(InterestingThing *thing) { #endif } -#include 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) { diff --git a/Perception.cpp b/Perception.cpp index bf1b943..5f10bc6 100644 --- a/Perception.cpp +++ b/Perception.cpp @@ -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]; diff --git a/Perception.h b/Perception.h index 44dbd1d..ed5a86f 100644 --- a/Perception.h +++ b/Perception.h @@ -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; diff --git a/Roboid.cpp b/Roboid.cpp index a98cad1..c9b0e4b 100644 --- a/Roboid.cpp +++ b/Roboid.cpp @@ -108,10 +108,14 @@ void Roboid::AddChild(Thing *child) { } } +#include 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) { diff --git a/Thing.cpp b/Thing.cpp index a673f41..37a2766 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -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) {