Add SLAM to the software pipeline

[ugsfume]

Currently, the software pipeline only incorporates the mapping part of SLAM (Simultaneous Localisation and Mapping) handled by cone_sorter.py. This works perfectly fine in simulation as odometry data (/odom) is retrieved directly from Gazebo and therefore accurately reflects the robot's state. However, in real-world scenarios, /odom, which relies on built-in wheel encoders, is susceptible to accumulating noise over time.

This necessitates of a localisation function to correct and recalibrate /odom using the live 3D cone data (i.e. /transformed_yolo_3d_result). This localisation function would be implemented between the cone mapping node and the path planner node.

A potential solution could be the integration of an existing off-the-shelf SLAM system, which can be found here.

[ugsfume]

Landmark/RO-SLAM was implemented and tested. However, results were undesirable due to unreliable cone mapping and perception upstream and unreliable odometry.

VSLAM (perhaps the best solution for our application since the open field may lack features) was tested. It was unable to perform loop closure when I was testing it on realsense. (perhaps due to error in my camera matrix or that the vslam I was using was not good enough: https://github.com/stella-cv/stella_vslam_ros)

3D graph SLAM is the one currently fitted into the pipeline. This is chosen since 3D graph slam is a relatively mature slam technology, and that 3D Lidar is accurate and reliable. However, this will not work if the surrounding of the track is empty (i.e. featureless).

[ugsfume]

3D graph SLAM was used in F24 Silverstone. It provides both odometry and SLAM capability, and works surprisingly well with Velodyne VLP-16 PUCK at Silverstone with accurate pose estimation. There's always room for improvement though, such as combining with other odometry sources using EKF.