355 navigation loss of gps signal or weak gps signal

[rosalievanark]

Description

Adjusted the Nav2 behaviour tree to include a custom GPS health check condition, and added a diagnostics package (set up in a modular manner such that it can easily be expanded on). Once the GPS is too unstable or even not received, the behaviour tree will stay in waiting mode until the GPS signal is restored, or until a stop command is send.

Fixes: #355

Testing

Simulation:

• OK GPS case easily tested since the GPS signal always works in simulation
• Redirected the GPS to a different topic to have more control, used topic_tools for this purpose which is now by default added to the alliander_core image. Specifically tested like so:

1. In diagnostics.launch.py, change the gps_topic to .../fix_relay
2. Run uv run start.py panther_gps_navigation
3. Attach to any running container, and run ros2 run topic_tools relay /gps/gps/fix to send the GPS data also to the .../fix_relay topic, which should result in a GPS OK status. Not running this command should result in an ERROR status, and the robot should stop driving after the no_data_received_limit period that is defined in gps_diagnostics.hpp.

Hardware:

• Covered the GPS in tin foil to block the signal (mixed success). The robot indeed started driving once the foil was removed, suggesting it was waiting for a signal.

Used uv run start.py gps for purely testing the diagnostics package.
Used uv run start.py panther_gps_navigation to test the waiting behaviour.

Documentation

TODO:

• Update the documentation

Additional Notes

Any relevant screenshots, logs, or context.

[geurto]

Perhaps the no_data_received_limit you defined in gps_diagnostics.hpp can be moved to this, as we can expect the same timeout behavior in other future diagnostics classes? Maybe in the form of three variables (warning_timeout / error_timeout / stale_timeout) as rclcpp::Durations? That way an absence of (GPS) data will escalate slowly, giving the operator or BT time to take appropriate action

[geurto]

Very nice!

[geurto]

This only takes into account latitude, not longitude. Maybe a max(msg->position_covariance[0], msg->position_covariance[4]) to take whichever is longer out of lat/lon?

[geurto]

If multiple things are wrong, only one fault will be returned. Is this intentional?

[geurto]

Could be more semantically meaningful if this was a rclcpp::Duration instance (as with the other double/float members representing time instances.

Also, no need for doubles here: a float already gives you a 0.00001 second precision. I find doubles are usually only needed when making very exact representations, e.g. lat/lon.

Also also, could be nice to put these into variables that are declared/gotten in the nodes. That way we keep the configuration in the launch files or even predefined_configurations.py.

[geurto]

Nitpick: I would vote in favor of naming our nodes ${PROJECT_NAME}_node if there is only one node for this package.

[geurto]

Any specific reason for putting this executable in lib/?

[geurto]

How do you envision this when we expand the diagnostics in the future? Would each diagnostic (GPS, IMU, LiDAR, costmap, network, etc.) have its own node? Or would it all be in this node, which will then be something like diagnostics_health_check.cpp?