Add steer to goals section in T2

Fix get position in slot for LEFT parking slot

Add goals to RRTBase

Rewrite perpendicular slot planner

Add list of goals

This list contains all the poses from slot entry point backwards to
first cusp point (of parking slot).

Add pose in perpendicular slot getter

This method returns a pose of perpendicular parking slot from where it
is possible to get out of the slot with full steer to farther corner
side (i.e. right corner for the RIGHT side).

Fix best final pose (FP) getter, add lper slot

Redefine order of slot border nodes

The node closest to init pose is the first.

Add slot debug output

Change guided sampling

Sampling info contains information of bounding box where to sample:
- `x0 - x/2 ... x0 + x/2`
- `y0 - y/2 ... y0 + y/2`
- `h0 - h/2 ... h0 + h/2`

Add middle node getter

First `fip()` then `getSamplingInfo()`

Just reorder methods

Enable plot just scenario

Fallback to scenario when info in traj missing

Comment out fill between

As multiple scenarios are currently considered.

Set slot heading with `satAll()`

Use find init pose with obstacles

Merge branch 'hotfix/0.6.1'

Update changelog (hotfix)

Switch between guided and random sampling

Fix missing declaration

Merge branch 'release/0.6.0'

Update changelog (release)

Update changelog

Merge branch 'feature/refactor'

Merge branch 'feature/trace-random-samples'

Merge branch 'feature/tune-sampling'

Merge branch 'feature/perpendicular-slotplanner'

Add plot trajectory script

Add rotate scenario script

Add perpendicular slot planner proposal

Add trajectory cusp private getter

Add random init position

Add car outer, inner radius

Include obstacles to slot planner

Log random sample with next steer

Add random sample getter/setter to RRTNode

Add 2nd simple sampling function

Rewrite sampling

Add `useSamplingInfo` variable to RRTBase

Tune sampling heading

Tune sampling coordinates

Fix sampling for [0, 0] init

Use different sampling info for USE_SLOTPLANNER

Tune sampling randomizing

Return heading in (-M_PI,+M_PI] interval

Add fipr method for RRTNode

Use slot sampling info when slot in scenario

Set `USE_SLOTPLANNER` macro when building

Change `fipr` method for perpendicular slot

Use inner diagonal for getting final pose in slot

Add bicycle car related macros

- Distance from wheelbase center to rear axle.
- Distance from wheelbase center to front axle.
- Diagonals in car (to rear, to front).
- Outer radius.

Add distance to rear corner getter

Fix simple perpendicular `fipr`

Fix final pose getter

Fix `std::abs`

Merge branch 'feature/matej-cost'

Update changelog

Add nn using Matej's heuristic cost

Add Matej's heuristic cost

Move MIN macro to aux header file, add MAX

Move EDIST macro to aux header file

Fix missing USE_GL macro usage

Fix sampling when slot planner not used

Use USE_GL macro for build, in code

Update readme with T2 as default planner

- Fix typo in changelog.
- Fix default planner in `bash.sh`.

Merge branch 'feature/perpendicular-slotplanner'

Update changelog

Use find init pose by reverse by default

Add perpendicular reverse parking proporsal

Generalize starting entry point

Generalize final pose used in fipr

Generalize sampling info

Add slot to rper scenario

Merge branch 'feature/generalize-samplingInfo'

Also refactor.

Update changelog

Fix indexing for negatives

Use EDIST in nn2

Add setter for slot side, type

Update RRT sampling with right side parking

Add and use entry point getters in fip

Merge branch 'feature/generalize-slotplanner'

Also refactor.

Update fip with right side parking

Update fipr with right side parking

Fix slot nodes order in rpar

Fix fipr iteration level

Use RRTEdge intersect in segment obstacle

Empty q before return

Fix car border

Update fipr

Add getter for final pose used in fipr

Update slot side getter

Stop when no parking maneuver found

Update find last not colliding

Update is inside slot check

Update slotplanner init pose

Merge branch 'feature/slotplanner-region'

Update changelog

Fix spacing