Added SetFormation message and modified CMakeList for msg generation