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

Debug info improvement

Browse Source
This commit is contained in:
Pascal Serrarens 2024-04-26 12:43:01 +02:00
parent ebc1dfba9c
commit 0d3678350b
2 changed files with 6 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
NetworkPerception.cpp
View File

@ -40,10 +40,10 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
unsigned char networkId = data[1]; unsigned char networkId = data[1];
unsigned char objectId = data[2]; unsigned char objectId = data[2];
unsigned char poseType = data[3]; unsigned char poseType = data[3];
// printf("Received pose message [%d/%d]\n", networkId, objectId);
// printf("My network id == %d\n", roboid->networkSync->networkId); // printf("My network id == %d\n", roboid->networkSync->networkId);
if (networkId == roboid->networkSync->networkId) if (networkId == roboid->networkSync->networkId)
networkId = 0x00; networkId = 0x00;
printf("Received pose message [%d/%d]\n", networkId, objectId);
if ((poseType & NetworkSync::Pose_Position) != 0) { if ((poseType & NetworkSync::Pose_Position) != 0) {
Vector3 worldPosition = ReceiveVector3(data, 4); Vector3 worldPosition = ReceiveVector3(data, 4);
@ -53,8 +53,10 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
Vector3 roboidPosition = roboid->GetPosition(); Vector3 roboidPosition = roboid->GetPosition();
float distance = Vector3::Distance(roboidPosition, worldPosition); float distance = Vector3::Distance(roboidPosition, worldPosition);
if (roboid->perception->IsInteresting(distance) == false) if (roboid->perception->IsInteresting(distance) == false) {
printf(" not interesting\n");
return; return;
}
Quaternion roboidOrientation = roboid->GetOrientation(); Quaternion roboidOrientation = roboid->GetOrientation();
Vector3 localPosition = Quaternion::Inverse(roboidOrientation) * Vector3 localPosition = Quaternion::Inverse(roboidOrientation) *

4
Perception.cpp
View File

@ -269,7 +269,7 @@ InterestingThing *Perception::AddTrackedObject(Sensor *sensor,
#ifdef RC_DEBUG2 #ifdef RC_DEBUG2
// Serial.print((int)obj->id); // Serial.print((int)obj->id);
// Serial.println(": delete tracked object"); // Serial.println(": delete tracked object");
printf("%d: delete tracked object {/%d}\n", -1, obj->id); printf("%d: delete tracked object {%d/%d}\n", -1, obj->networkId, obj->id);
#endif #endif
// No available slot, delete trackedobject // No available slot, delete trackedobject
delete obj; delete obj;
@ -297,7 +297,7 @@ bool Perception::IsInteresting(float distance) {
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
InterestingThing *thing = this->trackedObjects[objIx]; InterestingThing *thing = this->trackedObjects[objIx];
if (thing == nullptr) if (thing == nullptr)
continue; return true;
if (thing->position.distance > distance) if (thing->position.distance > distance)
return true; return true;
} }