ready to test
delete comment-out codes and test on pr2-pc
Merge branch 'go_through_door_demo' of github.com:chiwunau/au-ros-pkg