[TASK] Exploration Node

[jorgenfj]

Description of task

We need a way to navigate and search for the docking platform in the pool during TACC. @Sam1-2 is currently working on integrating the explore_lite package with the auto stack. However, we need a fallback if this integration turn out to be impossible.

Luckily we are in a small controlled environment and have a reliable navigation sensor (DVL). Therefore drift can be ignore and we can focus solely on mapping of the static environment.

This task can be split into three parts:

1. Initializing of the Occupancy Grid. Here we can utilize that we know the initial desired direction to defined desired grid cells to explore. Could use a normal distribution centered over the initial guess of the docking location. Can also block of the map behind the starting position of the drone to prevent it from going there. Determine fitting resolution and size of the occupancy grid.
2. Updating of the map from the zed camera. The pool can be simplified to rectangular prism. One way to find the walls of the pool can be to use the depth image/pointcloud from the ZED camera to look for vertical planes. This can be done similar to how we filter lines for the pipeline task. First we filter out planes that are not vertical. Then extract the 2d-slice of the planes and once a plane is determined to be valid we integrate the 2d-slice into the Occupancy Grid.
3. We need to send waypoints to auto to move Orca around the pool. Perform lookups into the Occupancy Grid to find desired waypoints. Continuously marks cells in a small radius around Orca as explored. @Sam1-2 worked on path generation. If compatible it can be used here.

Suggested Workflow

work

Specifications

No response

Contacts

No response

Code Quality

• Every function in header files are documented (inputs/returns/exceptions)
• The project has automated tests that cover MOST of the functions and branches in functions (pytest/gtest)
• The code is documented on the wiki (provide link)