fix environment modeling and changed api to lock/unlock environment

update EnvironmentPlaneModeling
* add service to lock/unlock environment
* use grid map to check collision. it's super fast.