test node done. TODO: convert into actionlib server