Code refactoring, improved readability, added essential comments, fixed logging issue, implemented framework for multi-robot initial setup, removed outdated add_agent service
Separated the odometry update into two steps, one to update the agent position, and another to update the odometry as the robot moves across space. This should allow for the re-instantiation of odometry checks after loosing visual tracking
Tracking data is now stored in a msg_ buffer as soon as its available, and accessed/processed prior to Model/Planner steps. Decoupling provides up to date information without overflowing/taking up processing time given tracker frequency fluctuations.