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.

[huckl3b3rry87]

TODO 1

• [x] Initialize with a particular case using yaml config file, get rid of CaseModule initialization in MAVs
• [x] Read new obstacle information data from a particular message and update optimization accordingly
• [x] 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

• [x] 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.

[amc-nu]

Hello @huckl3b3rry87 I’ve been following all the stuff you’ve working on. Everything look interesting. Could you please

1. tell us more about it. Write a short README for your package on how to use, test, etc.
2. merge develop branch into your PR
3. target your PR to develop branch instead of master. I’ll check it

Also can you tell us more about you. Are you a professor?, student? I can see you belong to UMich.

Thanks! Looking forward to hear about you and your projects.

[huckl3b3rry87]

@amc-nu this is great to hear and reassuring! Thanks!

1. Sure, I can do this. Currently it is only integrated into Autoware through a launch button in the gui, but it should work once we are able to integrate it with other parts of the system.
2. OK
3. OK

I am a PhD student at Michigan in Mechanical Engineering. I am planning on going into academia to be a professor though.

@amc-nu I had asked about how we should structure our workflow here, but did not get a response. Some feedback on this would be helpful, but let me tell you what we have done so far.

As you noticed we have many different features that we are trying to add and (up until the other night) we have kept them on separate branches that all started from an Autoware master. Now some of these branches (port_car_demo and trajectory_generator branches, that we made pull requests for) are ready to be tested with one another, so we merged them into our master and also made them even with our master (but I just reset them so that I can make the changes that you requested and there are not multiple features included in a single pull request).

If we can get them merged into Autoware quickly then it should not be an issue to wait to merge our master back into these branches, but if it takes a while to merge into Autoware then it may be too time consuming not to merge our master into these branches. Because some of the files need to be manually merged (due to overlap) and we would not want to have to redo this process each time we want to merge our feature branch into our master.

Any suggestions? Thanks!!