[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
andParseCSV
- 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,
Edited by Tong Fu