Fixes for threshold testing

Publish to cmd_vel/monitor (otherwise it will fight with nav for control of the robot).