merged changes with upstream
added image processing for incoming sensor_msgs/Image message, to compute error and publish out on /control_error