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

Bug fixes

Browse Source
This commit is contained in:
Pascal Serrarens 2024-05-10 12:44:41 +02:00
parent ba18f5763f
commit 6ee5bc44f8
2 changed files with 17 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
NetworkPerception.cpp
View File

@ -125,7 +125,7 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
InterestingThing *thing = roboid->perception->AddTrackedObject( InterestingThing *thing = roboid->perception->AddTrackedObject(
this, networkId, objectId, position); this, networkId, objectId, position);
if (thing->type == 0xFF) { if (thing->networkId != 0x00 && thing->type == 0xFF) {
// Unknown thing // Unknown thing
roboid->networkSync->SendInvestigateThing(thing); roboid->networkSync->SendInvestigateThing(thing);
} }

32
Perception.cpp
View File

@ -249,9 +249,9 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position,
// Serial.print((int)obj->id); // Serial.print((int)obj->id);
// Serial.println(": replaced tracked object"); // Serial.println(": replaced tracked object");
printf("%d: replaced tracked object [/%d]\n", farthestObjIx, obj->id); printf("%d: replaced tracked object [/%d]\n", farthestObjIx, obj->id);
#endif
roboid->networkSync->NewObject(obj); roboid->networkSync->NewObject(obj);
((WifiSync *)roboid->networkSync)->PublishTrackedObject(roboid, obj); ((WifiSync *)roboid->networkSync)->PublishTrackedObject(roboid, obj);
#endif
} else { } else {
#ifdef RC_DEBUG2 #ifdef RC_DEBUG2
// Serial.print((int)obj->id); // Serial.print((int)obj->id);
@ -518,20 +518,20 @@ void Perception::UpdatePose(Polar translation) {
// We only support translations in the horizontal plane at this moment... // We only support translations in the horizontal plane at this moment...
// This needs Spherical operator- to be implemented to work in 3d space // This needs Spherical operator- to be implemented to work in 3d space
if (thing->type == 0x80) { // plane if (thing->type == 0x80) { // plane
printf("[1/%d] %f (%f %f) ", thing->id, thing->position.distance, // printf("[1/%d] %f (%f %f) ", thing->id, thing->position.distance,
(float)thing->position.horizontalAngle, // (float)thing->position.horizontalAngle,
(float)thing->position.verticalAngle); // (float)thing->position.verticalAngle);
// Update the closest point to the plane // Update the closest point to the plane
float angle = (float)thing->position.horizontalAngle + translation.angle; float angle = (float)thing->position.horizontalAngle + translation.angle;
angle = abs(angle); angle = abs(angle);
float deltaDistance = translation.distance * cosf(angle * Angle::Deg2Rad); float deltaDistance = translation.distance * cosf(angle * Angle::Deg2Rad);
printf(" | translate %f %f %f | ", (float)translation.distance, // printf(" | translate %f %f %f | ", (float)translation.distance,
(float)angle, deltaDistance); // (float)angle, deltaDistance);
thing->position.distance -= deltaDistance; thing->position.distance -= deltaDistance;
printf("-> %f (%f %f)\n", thing->position.distance, // printf("-> %f (%f %f)\n", thing->position.distance,
(float)thing->position.horizontalAngle, // (float)thing->position.horizontalAngle,
(float)thing->position.verticalAngle); // (float)thing->position.verticalAngle);
} else { } else {
Polar horizontalPosition = Polar horizontalPosition =
Polar(thing->position); // obj->position.ProjectOnHorizontalPlane(); Polar(thing->position); // obj->position.ProjectOnHorizontalPlane();
@ -555,16 +555,16 @@ void Perception::UpdatePose(Quaternion rotation) {
if (thing == nullptr) if (thing == nullptr)
continue; continue;
printf("[1/%d] %f (%f %f) ", thing->id, thing->position.distance, // printf("[1/%d] %f (%f %f) ", thing->id, thing->position.distance,
(float)thing->position.horizontalAngle, // (float)thing->position.horizontalAngle,
(float)thing->position.verticalAngle); // (float)thing->position.verticalAngle);
printf("| rotate %f | ", rotationAngle); // printf("| rotate %f | ", rotationAngle);
float updatedAngle = float updatedAngle =
Angle::Normalize(thing->position.horizontalAngle - rotationAngle); Angle::Normalize(thing->position.horizontalAngle - rotationAngle);
thing->position.horizontalAngle = updatedAngle; thing->position.horizontalAngle = updatedAngle;
printf("-> %f (%f %f) \n", thing->position.distance, // printf("-> %f (%f %f) \n", thing->position.distance,
(float)thing->position.horizontalAngle, // (float)thing->position.horizontalAngle,
(float)thing->position.verticalAngle); // (float)thing->position.verticalAngle);
} }
} }