Fixed some bugs
Merge branch 'hydro-devel' of github.com:PR2/pr2_props_stack into hydro-devel