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

InterestingThings are now Things

Browse Source
This commit is contained in:
Pascal Serrarens 2024-10-22 11:25:18 +02:00
parent 0e2f628e3e
commit f77b00f639
5 changed files with 90 additions and 81 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

70
NetworkPerception.cpp
View File

@ -7,8 +7,7 @@
#include <Arduino.h>
#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);

57
Thing.cpp
View File

@ -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;
}
Spherical16 Thing::GetLinearVelocity() { return this->linearVelocity; }
Spherical16 Thing::GetAngularVelocity() { return this->angularVelocity; }

1
Thing.h
View File

@ -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

12
TrackedObject.cpp
View File

@ -2,7 +2,7 @@
#include <math.h>
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<float>(angle, axis);
this->orientation = orientation; // AngleAxisOf<float>(angle, axis);
this->updated = true;
}
// #include <Arduino.h>
bool InterestingThing::IsTheSameAs(InterestingThing* otherObj) {
bool InterestingThing::IsTheSameAs(InterestingThing *otherObj) {
if (id != 0 && id == otherObj->id)
return true;
if (type != otherObj->type)

31
TrackedObject.h
View File

@ -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
} // namespace RoboidControl
} // namespace Passer