diff --git a/NetworkPerception.cpp b/NetworkPerception.cpp index fa042f9..14b0b7e 100644 --- a/NetworkPerception.cpp +++ b/NetworkPerception.cpp @@ -7,8 +7,7 @@ #include #endif -void NetworkPerception::ProcessPacket(Roboid* roboid, - unsigned char* buffer, +void NetworkPerception::ProcessPacket(Roboid *roboid, unsigned char *buffer, int packetsize) { #if RC_DEBUG // printf("packet received, type = 0x%02x %d %d %d %d %d\n", buffer[0], @@ -16,22 +15,22 @@ void NetworkPerception::ProcessPacket(Roboid* roboid, #endif switch (buffer[0]) { - case NetworkSync::CreateMsg: - ReceiveCreateMsg(buffer, roboid); - break; - case NetworkSync::InvestigateMsg: - ReceiveInvestigateMsg(buffer, roboid); - break; - case NetworkSync::PoseMsg: - ReceivePoseMsg(buffer, roboid); - break; - case NetworkSync::PoseTypeMsg: - ReceiveTypedObject(buffer, roboid); - break; + case NetworkSync::CreateMsg: + ReceiveCreateMsg(buffer, roboid); + break; + case NetworkSync::InvestigateMsg: + ReceiveInvestigateMsg(buffer, roboid); + break; + case NetworkSync::PoseMsg: + ReceivePoseMsg(buffer, roboid); + break; + case NetworkSync::PoseTypeMsg: + ReceiveTypedObject(buffer, roboid); + break; } } -void NetworkPerception::ReceiveCreateMsg(unsigned char* data, Roboid* roboid) { +void NetworkPerception::ReceiveCreateMsg(unsigned char *data, Roboid *roboid) { unsigned char networkId = data[1]; unsigned char objectId = data[2]; unsigned char objectType = data[3]; @@ -39,15 +38,15 @@ void NetworkPerception::ReceiveCreateMsg(unsigned char* data, Roboid* roboid) { return; // printf("Received create message [%d/%d]\n", networkId, objectId); - InterestingThing* thing = + InterestingThing *thing = roboid->perception->FindTrackedObject(networkId, objectId); if (thing != nullptr) { thing->type = objectType; } } -void NetworkPerception::ReceiveInvestigateMsg(unsigned char* data, - Roboid* roboid) { +void NetworkPerception::ReceiveInvestigateMsg(unsigned char *data, + Roboid *roboid) { unsigned char networkId = data[1]; unsigned char objectId = data[2]; @@ -64,16 +63,17 @@ void NetworkPerception::ReceiveInvestigateMsg(unsigned char* data, if (roboid->modelUrl != nullptr) roboid->networkSync->SendModel(roboid); } else { - InterestingThing* thing = + InterestingThing *thing = roboid->perception->FindTrackedObject(0x00, objectId); if (thing == nullptr) return; roboid->networkSync->NewObject(thing); + roboid->networkSync->SendModel(thing); } } -void NetworkPerception::ReceivePlane(unsigned char* data, Roboid* roboid) { +void NetworkPerception::ReceivePlane(unsigned char *data, Roboid *roboid) { unsigned char networkId = data[1]; unsigned char poseType = data[3]; if ((poseType & NetworkSync::Pose_Position) == 0 || @@ -86,7 +86,7 @@ void NetworkPerception::ReceivePlane(unsigned char* data, Roboid* roboid) { Spherical16 roboidPosition = roboid->GetPosition(); Spherical16 deltaPosition = worldPosition - roboidPosition; - float distance = deltaPosition.distance; // magnitude(); + float distance = deltaPosition.distance; // magnitude(); if (roboid->perception->IsInteresting(distance) == false) { // printf(" plane not interesting\n"); return; @@ -100,7 +100,7 @@ void NetworkPerception::ReceivePlane(unsigned char* data, Roboid* roboid) { // Polar position = Polar(angle, distance); Spherical16 position = - localPosition; // Spherical16::FromVector3(localPosition); + localPosition; // Spherical16::FromVector3(localPosition); // printf("Received plane (%f %f %f) (%f %f %f) %f %f %f\n", worldPosition.x, // worldPosition.y, worldPosition.z, roboidPosition.x, @@ -112,7 +112,7 @@ void NetworkPerception::ReceivePlane(unsigned char* data, Roboid* roboid) { 0x80, 0x80, networkId); } -void NetworkPerception::ReceiveSphere(unsigned char* data, Roboid* roboid) { +void NetworkPerception::ReceiveSphere(unsigned char *data, Roboid *roboid) { unsigned char networkId = data[1]; float radius = ReceiveFloat100(data, 3); @@ -126,14 +126,14 @@ void NetworkPerception::ReceiveSphere(unsigned char* data, Roboid* roboid) { SwingTwist16::Inverse(roboidOrientation) * deltaPosition; Spherical16 position = - localPosition; // Spherical16::FromVector3(localPosition); + localPosition; // Spherical16::FromVector3(localPosition); roboid->perception->AddTrackedObject(this, position, SwingTwist16::identity, 0x81, 0x81, networkId); } -void NetworkPerception::ReceivePoseMsg(unsigned char* data, Roboid* roboid) { - if (roboid->networkSync->networkId == 0) // We're not connected to a site yet +void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) { + if (roboid->networkSync->networkId == 0) // We're not connected to a site yet return; unsigned char networkId = data[1]; @@ -152,7 +152,7 @@ void NetworkPerception::ReceivePoseMsg(unsigned char* data, Roboid* roboid) { // printf("Received PoseMsg [%d/%d]\n", networkId, objectId); #endif - InterestingThing* thing = + InterestingThing *thing = roboid->perception->FindTrackedObject(networkId, objectId); SwingTwist16 roboidOrientation = roboid->GetOrientation(); @@ -175,7 +175,7 @@ void NetworkPerception::ReceivePoseMsg(unsigned char* data, Roboid* roboid) { Spherical16 localPosition = SwingTwist16::Inverse(roboidOrientation) * (worldPosition - roboidPosition); - position = localPosition; // Spherical16::FromVector3(localPosition); + position = localPosition; // Spherical16::FromVector3(localPosition); // printf(" worldPosition (%f %f %f) localPosition (%f %f %f)\n", // worldPosition.Right(), worldPosition.Up(), // worldPosition.Forward(), localPosition.Right(), @@ -193,7 +193,7 @@ void NetworkPerception::ReceivePoseMsg(unsigned char* data, Roboid* roboid) { Angle16::Degrees(worldAngles.Up()), Angle16::Degrees(worldAngles.Right()), Angle16::Degrees( - worldAngles.Forward())); // Quaternion::Euler(worldAngles); + worldAngles.Forward())); // Quaternion::Euler(worldAngles); if (objectId == 0) { roboid->SetOrientation(worldOrientation); } else { @@ -214,11 +214,11 @@ void NetworkPerception::ReceivePoseMsg(unsigned char* data, Roboid* roboid) { } } -void NetworkPerception::ReceiveTypedObject(unsigned char* data, - Roboid* roboid) { +void NetworkPerception::ReceiveTypedObject(unsigned char *data, + Roboid *roboid) { unsigned char objectType = data[1]; unsigned char objectId = data[2]; - if (objectType == 0x02) { // lighthouse + if (objectType == 0x02) { // lighthouse // We require position and orientation for lighthouses if (data[3] != (NetworkSync::Pose_Position | NetworkSync::Pose_Orientation)) { @@ -239,7 +239,7 @@ void NetworkPerception::ReceiveTypedObject(unsigned char* data, } } -Int32 NetworkPerception::ReceiveInt32(unsigned char* data, int startIndex) { +Int32 NetworkPerception::ReceiveInt32(unsigned char *data, int startIndex) { Int32 a = Int32((UInt32)(data[startIndex + 3]) << 24 | (UInt32)(data[startIndex + 2]) << 16 | (UInt32)(data[startIndex + 1]) << 8 | @@ -247,13 +247,13 @@ Int32 NetworkPerception::ReceiveInt32(unsigned char* data, int startIndex) { return a; } -float NetworkPerception::ReceiveFloat100(unsigned char* data, int startIndex) { +float NetworkPerception::ReceiveFloat100(unsigned char *data, int startIndex) { Int32 intValue = ReceiveInt32(data, startIndex); float f = (float)intValue / 100.0F; return f; } -Vector3 NetworkPerception::ReceiveVector3(unsigned char* data, int startIndex) { +Vector3 NetworkPerception::ReceiveVector3(unsigned char *data, int startIndex) { float x = ReceiveFloat100(data, startIndex); float y = ReceiveFloat100(data, startIndex + 4); float z = ReceiveFloat100(data, startIndex + 8); diff --git a/Thing.cpp b/Thing.cpp index 35b7eff..3138a0e 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -22,35 +22,25 @@ const unsigned int Thing::UncontrolledMotorType = MotorType | (unsigned int)Type::UncontrolledMotor; const unsigned int Thing::ServoType = (unsigned int)Type::Servo; -bool Thing::IsMotor() { - return (type & Thing::MotorType) != 0; -} +bool Thing::IsMotor() { return (type & Thing::MotorType) != 0; } -bool Thing::IsSensor() { - return (type & Thing::SensorType) != 0; -} +bool Thing::IsSensor() { return (type & Thing::SensorType) != 0; } -bool Thing::IsRoboid() { - return (type & Thing::RoboidType) != 0; -} +bool Thing::IsRoboid() { return (type & Thing::RoboidType) != 0; } -void Thing::SetModel(const char* url) { - this->modelUrl = url; -} +void Thing::SetModel(const char *url) { this->modelUrl = url; } -void Thing::SetParent(Thing* parent) { +void Thing::SetParent(Thing *parent) { if (parent == nullptr) return; parent->AddChild(this); } -Thing* Thing::GetParent() { - return this->parent; -} +Thing *Thing::GetParent() { return this->parent; } -void Thing::AddChild(Thing* child) { - Thing** newChildren = new Thing*[this->childCount + 1]; +void Thing::AddChild(Thing *child) { + Thing **newChildren = new Thing *[this->childCount + 1]; for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { newChildren[childIx] = this->children[childIx]; if (this->children[childIx] == child) { @@ -70,17 +60,36 @@ void Thing::AddChild(Thing* child) { this->childCount++; } -Thing* Thing::GetChild(unsigned char childIx) { +Thing *Thing::GetChild(unsigned char childIx) { if (childIx >= 0 && childIx < this->childCount) { return this->children[childIx]; } else return nullptr; } -Spherical16 Thing::GetLinearVelocity() { - return this->linearVelocity; +Thing *Passer::RoboidControl::Thing::RemoveChild(Thing *child) { + Thing **newChildren = new Thing *[this->childCount - 1]; + unsigned char newChildIx = 0; + for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { + if (this->children[childIx] != child) { + if (newChildIx == this->childCount - 1) { // We did not find the child + // stop copying and return nothing + delete[] newChildren; + return nullptr; + } else + newChildren[newChildIx++] = this->children[childIx]; + } + } + + child->parent = nullptr; + + delete[] this->children; + this->children = newChildren; + this->childCount--; + + return child; } -Spherical16 Thing::GetAngularVelocity() { - return this->angularVelocity; -} \ No newline at end of file +Spherical16 Thing::GetLinearVelocity() { return this->linearVelocity; } + +Spherical16 Thing::GetAngularVelocity() { return this->angularVelocity; } \ No newline at end of file diff --git a/Thing.h b/Thing.h index b5ce4d6..face2bf 100644 --- a/Thing.h +++ b/Thing.h @@ -74,6 +74,7 @@ public: /// @return The child at the given index or nullptr when the index is invalid /// or the child could not be found Thing *GetChild(unsigned char childIx); + Thing *RemoveChild(Thing *child); /// @brief Sets the location from where the 3D model of this Thing can be /// loaded from diff --git a/TrackedObject.cpp b/TrackedObject.cpp index 69cae01..3a7d3dc 100644 --- a/TrackedObject.cpp +++ b/TrackedObject.cpp @@ -2,7 +2,7 @@ #include -InterestingThing::InterestingThing(Sensor* sensor, Polar position) { +InterestingThing::InterestingThing(Sensor *sensor, Polar position) : Thing(0) { this->id = 0; this->confidence = maxConfidence; this->sensor = sensor; @@ -12,9 +12,9 @@ InterestingThing::InterestingThing(Sensor* sensor, Polar position) { this->updated = true; } -InterestingThing::InterestingThing(Sensor* sensor, - Spherical16 position, - SwingTwist16 orientation) { +InterestingThing::InterestingThing(Sensor *sensor, Spherical16 position, + SwingTwist16 orientation) + : Thing(0) { this->id = 0; this->confidence = maxConfidence; this->sensor = sensor; @@ -22,12 +22,12 @@ InterestingThing::InterestingThing(Sensor* sensor, // float angle; // Vector3 axis; // orientation.ToAngleAxis(&angle, &axis); - this->orientation = orientation; // AngleAxisOf(angle, axis); + this->orientation = orientation; // AngleAxisOf(angle, axis); this->updated = true; } // #include -bool InterestingThing::IsTheSameAs(InterestingThing* otherObj) { +bool InterestingThing::IsTheSameAs(InterestingThing *otherObj) { if (id != 0 && id == otherObj->id) return true; if (type != otherObj->type) diff --git a/TrackedObject.h b/TrackedObject.h index bf6dcaf..91c53f2 100644 --- a/TrackedObject.h +++ b/TrackedObject.h @@ -12,14 +12,13 @@ namespace Passer { namespace RoboidControl { /// @brief An object tracked by the roboid -class InterestingThing { - public: +class InterestingThing : public Thing { +public: /// @brief An object tracked by the roboid /// @param sensor The Sensor which detected this object /// @param position The position in polar coordinates local to the roboid - InterestingThing(Sensor* sensor, Polar position); - InterestingThing(Sensor* sensor, - Spherical16 position, + InterestingThing(Sensor *sensor, Polar position); + InterestingThing(Sensor *sensor, Spherical16 position, SwingTwist16 orientation = SwingTwist16::identity); /// @brief Update the position of the object @@ -43,7 +42,7 @@ class InterestingThing { /// @return Returns true when both objects are considered the same /// The result of this check depends on the equalityDistance and equalityAngle /// value. - bool IsTheSameAs(InterestingThing* otherObj); + bool IsTheSameAs(InterestingThing *otherObj); /// @brief The maximum difference in distance from the roboid in which two /// objects may be considered the same /// @details When the difference in distance is exactly this @@ -64,25 +63,25 @@ class InterestingThing { unsigned char networkId; /// @brief The id of the tracked object - unsigned char id; + // unsigned char id; char parentId = 0; /// @brief The current position of the object - Spherical16 position = Spherical16::zero; + // Spherical16 position = Spherical16::zero; /// @brief The current orientation of the object - SwingTwist16 orientation = SwingTwist16(); - // Quaternion orientation = Quaternion::identity; + // SwingTwist16 orientation = SwingTwist16(); + // Quaternion orientation = Quaternion::identity; /// @brief The sensor which provided that lastet pose this object - Sensor* sensor = nullptr; + Sensor *sensor = nullptr; - unsigned char type = 0x00; + // unsigned char type = 0x00; unsigned char confidence; bool updated = false; - protected: +protected: static constexpr unsigned char maxConfidence = 255; - static constexpr unsigned char confidenceDropSpeed = 10; // 150; // 2; + static constexpr unsigned char confidenceDropSpeed = 10; // 150; // 2; }; -} // namespace RoboidControl -} // namespace Passer \ No newline at end of file +} // namespace RoboidControl +} // namespace Passer \ No newline at end of file