[TASK] Pool Grid Exploration

[philliab13]

Description of task

This task deals with the exploration phase of the docking task, that runs until the docking platform is detected.

This task depends on the completion of the following issues:

• Line Filtering Library
• Pool Wall Detection - Sonar Image Line Detection

Assumptions:

• Drift is negligible up until the docking platform is detected.
• Initial Drone orientation relative to the mission area(pool walls) are known up to an acceptable error.
• Sonar will be able to detect the "far side" pool wall and the wall to our right. The left wall will most likely also be detected but is it not a requirement for this solution.
• Upon startup the point generated by projecting the drone onto the right wall, will have a negative y-value (NED convention), that can be said to exist within an interval set by a prior min_dist and max_dist.
• Also the angle between the heading direction of our drone and the "right wall" should be less than a prior angle_threshold
• When a line satisfies this check for the "right wall" there should also exist a "far wall", where the point generated from the projection of of the drone onto this line will have a positive x-value and exist within an interval set by a prior min_dist and max_dist
• Also the angle between the heading direction of our drone and the "far wall" should be greater than a prior angle_threshold
• As an additional requirement the angle between these walls should be within an interval set by a prior min_angle and max_angle (can also be solved with one angle threshold based on how you define the angle)

EDIT: Acoustic reflections from the "right wall" might be weak and the wall might therefore be hard to detect. A possible solution could be to send an initial waypoint with mode ONLY_ORIENTATION to turn the drone slightly. This way the "left wall" should be detected and a similar scheme as to the assumptions above can be dervied.

Using the assumptions above we can create two lines that are perpendicular to their respective wall. These perpendicular lines will have an intersection that is located inside the pool. By controlling the distance from these perpendicular lines to the intersection between the two walls we can create a good prior estimate for where the docking platform will be located. These distances should be configurable by a far wall offset and right wall offset parameter.

Suggested Workflow

• Create a 2d Occupancy grid grid with a fixed size_x, size_y and resolution configurable by a resolution parameter.
• Create a "map" tf2 frame and set the frame of the occupancy grid to this map frame. Create a transformation from map->odom (map is parent) so that (0,0) in odom coordinates correspond to the center of the map. Inspiration for this transform can be found in this package. All operations when inserting into the map has to be transformed to this map frame, and waypoints extracted from the map has to be transformed to the odometry frame.
• Insert the "right" and "far" wall into the grid, and create a safe region close to the walls configurable by a dilation parameter.
• Create a communication channel between the behavior tree and this exploration node. Can either be an action without (or with minimal) feedback for clean abortion, or just a toggle service. This communication channels toggles the sending of waypoints from this node. When this node is actively sending waypoints the behavior tree should have an ongoing persitent WaypointManagerAction
• Send waypoints using the SendWaypoints service to the WaypointManager.
• Define a Constant value to use as occupied cell value.
• Continuously mark cells around the drone in a radius as explored (or occupied if we don't differentiate between explored and occupied). The radius we use for updates should be a configurable parameter.
• Using the estimated docking platform position generated from the assumptions above weigh cells around this position as desirable.
• Create a cost function to select cells to explore. The cost function should select cells based on both distance to cell and the cell value created by the weighted estimate position.
• Once the cell containing the waypoint is marked as "explored", use the cost function to query the grid to generate another waypoint.
• This runs until the docking platform is detected and we receive a signal from the behavior tree to cancel.

Specifications

• Have configurable ros parameters for relevant stuff that might need tuning.
• Have simulator test to test robustness and detect failure edge cases

Contacts

@jorgenfj

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)

[jorgenfj]

Inspiration for how to setup the occupancy grid and map->odom transform example-node