Fixed local planning error
Added a scoring parameter
Merge pull request #8 from open-rdc/fix-plan-failed

Reduced the width of the avoidance