[ROS2][feat] Get loop closure indices

Goal

Before, loop closure indices are provided by a csv file. It's difficult to know loop closure indices before a slam map is built. This MR aims to give two ways to obtain loop indices for users:

• In rviz, click a point on the trajectory which is considered as the loop frame of the current frame.
  • Play slam and stop the bag when a loop closure is observed.
  • Click the position where forms a loop with current position
  • the current pose and the closest pose to clicked point are added into LoopDetections
• Save trajectory with index information so that one can get loop closure indices with saved trajectory.
  • Play slam and stop the bag when a loop closure is observed.
  • Save trajectory
  • Open trajectory with a visual tool and get the loop closure indices
  • Create a csv file of loop closure indices. More than one loop can be added in the file

Changes

• Add ClickedPointCallback function
• Save trajectory with index term
• Add helper functions ParseSentence and ParseCSV
• Overload ReadCSV to check csv fields
• Refact ReadPose to check csv fields

Results

• demo 1: hesai_car.
• demo 2: acquisition_sido

Data

• data 1: car(hesai)
• data 2: Acquisition_SIDO/outdoor

Build

colcon build --base-paths slam/ros2_wrapping --cmake-args -DCMAKE_BUILD_TYPE=Release -DENABLE_g2o=ON

Launch file

• Use `slam_velodyne.py' but comment velodyne points conversion part.
• change "velodyne" to "hesai" in tf_base_to_velo_node

Parameters

• loop_closure.enable_offset: true
• n_threads: 4
• distance_threshold: 0.1
• leaf_size.edges: 0.1
• leaf_size.planes: 0.1
• decaying_threshold: 900
• min_frames_per_voxel: 0
• voxel_grid_resolution: 1.
• neighbors_radius: 0.05
• edge_depth_gap_threshold: 2.
• edge_nb_gap_points: 20

Run

For car

ros2 run lidar_conversions generic_conversion_node \
ros2 launch ros2 launch lidar_slam slam_velodyne.py \
ros2 bag play --clock path/to/bag --remap /hesai/pandar:=/generic_points

If the data is a pcap of hesai

ros2 run lidar_conversions generic_conversion_node \
ros2 launch ros2 launch lidar_slam slam_velodyne.py \
ros2 launch hesai_ros_driver start.py # some parameters need to be changed in config file, such as frame_id,