grid center is caculated, next should caculate the position according to center and 2 headers.
Then, obstacle pointer should be caculated and send to optimal_vel.