Add walls points as occupied and floor points as free. Not using ray
tracing.
Fixed executor_node.cpp
Finished pose correction and executor