This program is developed and tested under ros kinetic.
comiple with in a catkin workspace, with command
catkin_make demo
source the parent catkin workspace, then run following command in terminal.
roslaunch pathfinder demo.launch
modify the demo.yaml under config folder