add test code for jsk_2013_04_pr2_610

add test code to check if demo works, 
@ furushchev, could you add test code for demo program with planner?