Fix warnings

A couple of warnings found by -Wall. Distance Field change is trivial, robot_state change might actually fix potential issues -- @isucan, can you double check the robot_state change return value is correct?