feat: collision detection

[zthh626]

#27 is a pre-req for this ticket.

Thanks to @mohammad-rostampoor we now have this state diagram.

Using the LIDARs we need to have the state switching for these states (CD = collision detection):

• CD_TURN_LEFT
• CD_TURN_RIGHT
• CD_CLEAR_OBSTACLE

For testing purposes, we can assume that we will always be in "collision detection mode".

Algorithm:

1. Checks front LIDAR to see if there is an obstacle (if less than 20 m, do next steps)
2. Locate shore and determine if it is on the left or right of the usv
3. Change state to either CD_TURN_LEFT or CD_TURN_RIGHT where left or right will be opposite direction of the shore
4. Continue turning until the front LIDAR no longer sees an obstacle, at this point the shoreside LIDAR should see the object, then change state to CD_CLEAR_OBSTACLE
5. Continue going straight until the shoreside sees no obstacle
6. Switch back to Driving state