Move global and storage maps along with robot #143
Description
Reinitialize the global and storage maps to be centered around the robot after it moves a certain distance, so that the robot can handle arbitrarily large environments.
Currently, the global map and storage map have fixed origins and dimensions. The local map is cropped out of the storage map according to the robot center.
One easy way to achieve this is to clear all voxels of the global and storage map, reinitilize them to be centered around the robot whenever the robot is close to the boundaries of the global / storage map (while doing so, publish a state trigger for stopping policy to guarantee safety).
FYI, Storage map only exists to speed up the generation of the local map. A storage map is a map that is large and has the same resolution as the local map. Given a local map center and dimension, we just crop a submap from the storage map. Global map has similar dimensions as storage map but is much more coarsely discretized. The idea is that the global planner plans in global map and the local planner plans in the small local map, thus increasing computation efficiency, while not comprising much on the optimality (local planner can still deviate from the global path, and fly through small gaps if they exist).