New Obstacle Avoidance Algorithm

[huckl3b3rry87]

Feature

Integrate a the obstacle avoidance algorithm described in this paper which I can now solve in real time using an official julia package that I developed called NLOptControl.

Motivation

If a vehicle needs to drive quickly and avoid moving obstacles then NLOptControl is well suited to solve the trajectory planning problem. Where A* and OpenPlanner are path planning algorithms and are mostly concerned with satisfying kinematic/geometric constraints, they can identify a path to follow, but without a temporal component, they do not tell you how to follow the path. While, trajectory planning also considers how you can follow the path. So, for instance, what are the optimal steering and throttle trajectories (not simply what are the X and Y waypoints).

NLOptControl is designed as a high level tool, so resarchers can easily define their own optimization problems, see my documentation here.

TODO

• Initialize with a particular case using yaml config file, get rid of CaseModule initialization in MAVs
• Read new obstacle information data from a particular message and update optimization accordingly
• Read new vehicle state from a particular message and update optimization accordingly

[huckl3b3rry87]

The first task is nearly completed with https://github.com/huckl3b3rry87/MAVs.jl/tree/feature/yaml. A few things still need to be tested and developed and then it can be merged into the master

[huckl3b3rry87]

with this commit the three items on TODO 1 are complete

[huckl3b3rry87]

TODO 2

• create a launch file that accepts the arguments needed to run from a higher level
• make sure that Docker file works
• check gui functionality

[huckl3b3rry87]

The first item on TODO 2 is done with this commit. Now, it should work as a stand alone module that updates these parameters

febbo@febbo-HP-ZBook-17-G2:~$ rosparam list
/nloptcontrol_planner/case_name
/nloptcontrol_planner/flags/3DOF_plant
/nloptcontrol_planner/flags/init
/nloptcontrol_planner/flags/known_environment
/nloptcontrol_planner/obstacle_name
...

it will publish trajectories as

/nloptcontrol_planner/traj/psi
/nloptcontrol_planner/traj/sa
/nloptcontrol_planner/traj/t
/nloptcontrol_planner/traj/vx
/nloptcontrol_planner/traj/x
/nloptcontrol_planner/traj/y

Given parameter information about the obstacles

/obstacle/radius
/obstacle/vx
/obstacle/vy
/obstacle/x
/obstacle/y

and the initial state of the vehicle

/state/ax
/state/psi
/state/r
/state/sa
/state/ux
/state/x
/state/y

In the demo.launch file a simple example is provided that assumes all perfect knowledge of the obstacles and the state of the vehicle, but args can be set if this is not the case and more realistic simulations can be ran.