Fix duplication of team data information in robot filter #440
Conversation
There was a problem hiding this comment.
I see how this fixes the specific issue, but in general is this the correct way to handle this?
If I understand correctly, we add our teammates from the team communication team data, where in the current team variable we always overwrite with the latest data in the callback and then filter by timestamps of when this team data was received.
I see potential issues with this:
We publish team data for each message received from a teammate. In this way, we always use the latest received data even when the confidence is bad.
So this can then lead to "jumping" teammates, especially when a specific robot is not correctly localized or has bad calibration.
Wouldn't it be better to use a combination of recency and confidence of all teammate team data to decide for the best value all teammates should be using?
|
|
||
| self.robots.extend(list(map(build_robot_detection_from_team_data, self.team.values()))) | ||
| # Check if the team data is fresh enough |
There was a problem hiding this comment.
How about calling the method is_team_data_fresh and removing the comment?
| self.robots.extend(list(map(build_robot_detection_from_team_data, self.team.values()))) | ||
| # Check if the team data is fresh enough | ||
| def team_data_fresh(msg: TeamData) -> bool: | ||
| return abs((self.get_clock().now() - Time.from_msg(msg.header.stamp)).nanoseconds) < self.team_data_timeout |
There was a problem hiding this comment.
Any reason to call abs here. As far as I'm aware it should never happen, that the current timestamp < than the team data timestamp.
And should this be the case, then do we want to use the team data? In that case it seems more, that there is an issue somewhere else and we would not
|
We already talked about this in person, but I add it here for completion. The ball filter currently only includes the positions of the team mates via team com. It does not include the robots detected by the team mates. It also does not consider if we or the team mates are localized properly, as we don't have a good way of knowing if this is the case. |
Summary
Currently, the poses of the other robots from the team data are added to the robot filter, but they are not deduplicated. Instead, they stack up until a couple of hundred robots are present inside the filter. They are only removed after being in the filter for 10 seconds. This pr changes that, so the team data is only added before we publish the filter result. It does not influence the filter state and a lower timeout of 1 sec is applied, so we don't include team data of robots that have been removed.
Example
This image shows the 10-second trail from the team data of another robot. This should not happen. Instead, it should be a singular point.
This image also shows the team data of a robot that thought it was playing, but it was taken out and not localized. We should be careful with that are wrong team data might mess things up. So we should remember to turn them off if they are taken out.
I am not really sure how to properly test this. We might need a team data simulator.