Fixed one crash bug and fixed goal-selection bug #339
Merge pull request #362 from ros-planning/fix-339

Fixed one crash bug and fixed goal-selection bug #339