Minor changes. This program does NOT work in ROS.