[primary_robot/main_ia] Adds mode selection + improves traj send method
[primary_robot/A*] Fixes minor bug: not setting arc_len in some case