- Added dependency on kf_yaw_kf for compass publisher
- Added TaskSetHeading to reach a desired heading angle (To Be Tested).
- Moved Environment Functions to the cpp file, could improve compilation time.