Cleaned tracker assignment code, moved Avg/Max speed code to separate function for clarity
Replaced assigning positions of agents 1 time-step previous with current position, used to help with model inferrence but this should improve planner, to be tested
Added manual assignment of TrackerID to each Youbot, now multiple robots can be tracked after initial setup, cinInteger() to read number from console, getTrackerIDs() to parse Trackers from msg_ and make code reusable, Youbots now move one meter forward and await tracker assignation, fixed odometry update so robots can switch to/away from using it seamlessly