LIVO Integrations

[JacopoPan]

Follows #85

TODOs

• Interface synthetic sensor data (IMU, camera, LiDAR)/TF
  • Check frames and timestamps in MAVROS/DDS IMU multiplexer
  • Make Gazebo IMU noise part of the YAML sensor intrinsics configuration
• Update instructions/dependencies/external repos/build times in README
• Update architectural diagram
• Custom gz GNSS plugin to simulate degraded/missing position (PX4 uses navsat, ArduPilot SITL internal)
  • Alternatively disable GPS from SITL (e.g. PX4_PARAM_EKF2_GPS_CTRL=0) through the YAML sensor config
• Use LIVO odometry in PX4/ArduPilot state estimation (e.g., PX4-Multiagent-Simulation, Ardupilot_Multiagent_Simulation)
• (optional) Add DeepStream to amd64 build
• (optional) ArUco OpenCV module on L4T of OpenVINS
• (optional) Add VSLAM-LAB

Tutorials

• https://docs.openvins.com/getting-started.html
• https://github.com/MIT-SPARK/spark-fast-lio#-how-to-run
• https://github.com/MIT-SPARK/KISS-Matcher/tree/main/ros#gear-how-to-build--run
• https://github.com/superxslam/SuperOdom#-5-launch-superodometry
• https://github.com/ntnu-arl/mimosa/tree/dev/ros2#usage

[JacopoPan]

TOPIC                                                        | TYPE                                     | PUBLISHERS (Nodes)                       | SUBSCRIBERS (Nodes)
-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------

# AAS SENSORS DATA

/camera_frames_0                                             | sensor_msgs/msg/Image                    | yolo_inference_node                      | None
/camera_frames_1 (if stereo)                                 | sensor_msgs/msg/Image                    | yolo_inference_node                      | None
/imu_data                                                    | sensor_msgs/msg/Imu                      | imu_publisher_node                       | None
/lidar_points                                                | sensor_msgs/msg/PointCloud2              | ros_gz_bridge                            | kiss_icp_node, mimosa_node

# OPENVINS INPUTS

/cam0/image_raw                                              | sensor_msgs/msg/Image                    | None                                     | run_subscribe_msckf
/cam1/image_raw                                              | sensor_msgs/msg/Image                    | None                                     | run_subscribe_msckf
/imu0                                                        | sensor_msgs/msg/Imu                      | None                                     | run_subscribe_msckf

# OPENVINS TOPICS

/ov_msckf/loop_depth                                         | sensor_msgs/msg/Image                    | run_subscribe_msckf                      | None
/ov_msckf/loop_depth_colored                                 | sensor_msgs/msg/Image                    | run_subscribe_msckf                      | None
/ov_msckf/loop_extrinsic                                     | nav_msgs/msg/Odometry                    | run_subscribe_msckf                      | None
/ov_msckf/loop_feats                                         | sensor_msgs/msg/PointCloud               | run_subscribe_msckf                      | None
/ov_msckf/loop_intrinsics                                    | sensor_msgs/msg/CameraInfo               | run_subscribe_msckf                      | None
/ov_msckf/loop_pose                                          | nav_msgs/msg/Odometry                    | run_subscribe_msckf                      | None
/ov_msckf/odomimu                                            | nav_msgs/msg/Odometry                    | run_subscribe_msckf                      | None
/ov_msckf/pathgt                                             | nav_msgs/msg/Path                        | run_subscribe_msckf                      | None
/ov_msckf/pathimu                                            | nav_msgs/msg/Path                        | run_subscribe_msckf                      | None
/ov_msckf/points_aruco                                       | sensor_msgs/msg/PointCloud2              | run_subscribe_msckf                      | None
/ov_msckf/points_msckf                                       | sensor_msgs/msg/PointCloud2              | run_subscribe_msckf                      | None
/ov_msckf/points_sim                                         | sensor_msgs/msg/PointCloud2              | run_subscribe_msckf                      | None
/ov_msckf/points_slam                                        | sensor_msgs/msg/PointCloud2              | run_subscribe_msckf                      | None
/ov_msckf/posegt                                             | geometry_msgs/msg/PoseStamped            | run_subscribe_msckf                      | None
/ov_msckf/poseimu                                            | geometry_msgs/msg/PoseWithCovarianceStamped | run_subscribe_msckf                   | None
/ov_msckf/trackhist                                          | sensor_msgs/msg/Image                    | run_subscribe_msckf                      | None

# FASTLIO INPUTS

/imu/data                                                    | sensor_msgs/msg/Imu                      | None                                     | lio_mapping
/ouster/points                                               | sensor_msgs/msg/PointCloud2              | None                                     | lio_mapping

# FASTLIO TOPICS

/cloud_registered                                            | sensor_msgs/msg/PointCloud2              | lio_mapping                              | rviz, rviz
/cloud_registered_base                                       | sensor_msgs/msg/PointCloud2              | lio_mapping                              | None
/cloud_registered_body                                       | sensor_msgs/msg/PointCloud2              | lio_mapping                              | None
/cloud_registered_lidar                                      | sensor_msgs/msg/PointCloud2              | lio_mapping                              | None
/odometry                                                    | nav_msgs/msg/Odometry                    | lio_mapping                              | None
/path                                                        | nav_msgs/msg/Path                        | lio_mapping                              | rviz

# KISS-MATCHER INPUTS

/cloud_registered                                            | sensor_msgs/msg/PointCloud2              | None                                     | kiss_matcher_sam
/odometry                                                    | nav_msgs/msg/Odometry                    | None                                     | kiss_matcher_sam
/km_sam/save_dir                                             | std_msgs/msg/String                      | None                                     | kiss_matcher_sam

# KISS-MATCHER TOPICS

/km_sam/curr_scan                                            | sensor_msgs/msg/PointCloud2              | kiss_matcher_sam                         | km_sam_rviz
/km_sam/global_map                                           | sensor_msgs/msg/PointCloud2              | kiss_matcher_sam                         | km_sam_rviz
/km_sam/lc/coarse_alignment                                  | sensor_msgs/msg/PointCloud2              | kiss_matcher_sam                         | None
/km_sam/lc/debug_cloud                                       | sensor_msgs/msg/PointCloud2              | kiss_matcher_sam                         | km_sam_rviz
/km_sam/lc/fine_alignment                                    | sensor_msgs/msg/PointCloud2              | kiss_matcher_sam                         | None
/km_sam/lc/src                                               | sensor_msgs/msg/PointCloud2              | kiss_matcher_sam                         | None
/km_sam/lc/tgt                                               | sensor_msgs/msg/PointCloud2              | kiss_matcher_sam                         | None
/km_sam/loop_detection                                       | visualization_msgs/msg/Marker            | kiss_matcher_sam                         | km_sam_rviz
/km_sam/loop_detection_array                                 | visualization_msgs/msg/MarkerArray       | None                                     | km_sam_rviz, km_sam_rviz
/km_sam/loop_detection_radius                                | visualization_msgs/msg/Marker            | kiss_matcher_sam                         | km_sam_rviz
/km_sam/loop_detection_radius_array                          | visualization_msgs/msg/MarkerArray       | None                                     | km_sam_rviz
/km_sam/path/corrected                                       | nav_msgs/msg/Path                        | kiss_matcher_sam                         | km_sam_rviz
/km_sam/path/original                                        | nav_msgs/msg/Path                        | kiss_matcher_sam                         | km_sam_rviz
/km_sam/pose_stamped                                         | geometry_msgs/msg/PoseStamped            | kiss_matcher_sam                         | None

# SUPERODOM INPUTS

/imu/data                                                    | sensor_msgs/msg/Imu                      | None                                     | feature_extraction_node, imu_preintegration_node
/integrated_to_init                                          | nav_msgs/msg/Odometry                    | None                                     | feature_extraction_node
/lidar/scan                                                  | livox_ros_driver2/msg/CustomMsg          | None                                     | feature_extraction_node

# SUPERODOM TOPICS

/aft_mapped_to_init_incremental                              | nav_msgs/msg/Odometry                    | laser_mapping_node                       | None
/bob_points                                                  | sensor_msgs/msg/PointCloud2              | feature_extraction_node                  | None
/edge_points                                                 | sensor_msgs/msg/PointCloud2              | feature_extraction_node                  | None
/feature_info                                                | super_odometry_msgs/msg/LaserFeature     | feature_extraction_node                  | laser_mapping_node
/imuodom_path                                                | nav_msgs/msg/Path                        | imu_preintegration_node                  | None
/laser_cloud_map                                             | sensor_msgs/msg/PointCloud2              | laser_mapping_node                       | None
/laser_cloud_surround                                        | sensor_msgs/msg/PointCloud2              | laser_mapping_node                       | None
/laser_odom_path                                             | nav_msgs/msg/Path                        | laser_mapping_node                       | None
/laser_odometry                                              | nav_msgs/msg/Odometry                    | laser_mapping_node                       | imu_preintegration_node
/lio_prediction                                              | nav_msgs/msg/Odometry                    | laser_mapping_node                       | None
/overall_map                                                 | sensor_msgs/msg/PointCloud2              | laser_mapping_node                       | None
/planner_points                                              | sensor_msgs/msg/PointCloud2              | feature_extraction_node                  | None
/prediction_source                                           | std_msgs/msg/String                      | laser_mapping_node                       | None
/registered_scan                                             | sensor_msgs/msg/PointCloud2              | laser_mapping_node                       | None
/state_estimation                                            | nav_msgs/msg/Odometry                    | imu_preintegration_node                  | None
/state_estimation_health                                     | std_msgs/msg/Bool                        | imu_preintegration_node                  | None
/super_odometry_stats                                        | super_odometry_msgs/msg/OptimizationStats | laser_mapping_node                       | None
/uncertainty_X                                               | std_msgs/msg/Float32                     | laser_mapping_node                       | None
/uncertainty_Y                                               | std_msgs/msg/Float32                     | laser_mapping_node                       | None
/uncertainty_Z                                               | std_msgs/msg/Float32                     | laser_mapping_node                       | None
/uncertainty_pitch                                           | std_msgs/msg/Float32                     | laser_mapping_node                       | None
/uncertainty_roll                                            | std_msgs/msg/Float32                     | laser_mapping_node                       | None
/uncertainty_yaw                                             | std_msgs/msg/Float32                     | laser_mapping_node                       | None
/velodyne_cloud_2                                            | sensor_msgs/msg/PointCloud2              | feature_extraction_node                  | None
/vio_prediction                                              | nav_msgs/msg/Odometry                    | laser_mapping_node                       | None

# MIMOSA INPUTS
/lidar_points                                                | sensor_msgs/msg/PointCloud2              | ros_gz_bridge                            | kiss_icp_node, mimosa_node
/radar/cloud                                                 | sensor_msgs/msg/PointCloud2              | None                                     | mimosa_node
/rovio/odometry                                              | nav_msgs/msg/Odometry                    | None                                     | mimosa_node
/vectornav_driver_node/imu/data                              | sensor_msgs/msg/Imu                      | None                                     | mimosa_node

# MIMOSA TOPICS

/mimosa_node/graph/debug                                     | mimosa_msgs/msg/GraphManagerDebug        | mimosa_node                              | None
/mimosa_node/graph/odometry                                  | nav_msgs/msg/Odometry                    | mimosa_node                              | None
/mimosa_node/graph/optimized_path                            | nav_msgs/msg/Path                        | mimosa_node                              | None
/mimosa_node/graph/path                                      | nav_msgs/msg/Path                        | mimosa_node                              | None
/mimosa_node/graph/transform                                 | geometry_msgs/msg/TransformStamped       | mimosa_node                              | None
/mimosa_node/imu/manager/debug                               | mimosa_msgs/msg/ImuManagerDebug          | mimosa_node                              | None
/mimosa_node/imu/manager/localizability_marker_array         | visualization_msgs/msg/MarkerArray       | mimosa_node                              | None
/mimosa_node/imu/manager/odometry                            | nav_msgs/msg/Odometry                    | mimosa_node                              | None
/mimosa_node/lidar/geometric/debug                           | mimosa_msgs/msg/LidarGeometricDebug      | mimosa_node                              | None
/mimosa_node/lidar/geometric/degen_marker_array              | visualization_msgs/msg/MarkerArray       | mimosa_node                              | None
/mimosa_node/lidar/geometric/keyframe_poses                  | geometry_msgs/msg/PoseArray              | mimosa_node                              | None
/mimosa_node/lidar/geometric/localizability_marker_array     | visualization_msgs/msg/MarkerArray       | mimosa_node                              | None
/mimosa_node/lidar/geometric/map                             | sensor_msgs/msg/PointCloud2              | mimosa_node                              | None
/mimosa_node/lidar/geometric/sm_cloud                        | sensor_msgs/msg/PointCloud2              | mimosa_node                              | None
/mimosa_node/lidar/geometric/sm_cloud_ds                     | sensor_msgs/msg/PointCloud2              | mimosa_node                              | None
/mimosa_node/lidar/geometric/sm_correspondances_ma           | visualization_msgs/msg/MarkerArray       | mimosa_node                              | None
/mimosa_node/lidar/manager/debug                             | mimosa_msgs/msg/LidarManagerDebug        | mimosa_node                              | None
/mimosa_node/lidar/manager/points_full_res                   | sensor_msgs/msg/PointCloud2              | mimosa_node                              | None
/mimosa_node/lidar/photometric/debug                         | mimosa_msgs/msg/LidarPhotometricDebug    | mimosa_node                              | None
/mimosa_node/lidar/photometric/feature_marker                | visualization_msgs/msg/Marker            | mimosa_node                              | None
/mimosa_node/lidar/photometric/feature_marker_array          | visualization_msgs/msg/MarkerArray       | mimosa_node                              | None
/mimosa_node/lidar/photometric/features                      | sensor_msgs/msg/PointCloud2              | mimosa_node                              | None
/mimosa_node/lidar/photometric/img_intensity                 | sensor_msgs/msg/Image                    | mimosa_node                              | None
/mimosa_node/lidar/photometric/img_mask                      | sensor_msgs/msg/Image                    | mimosa_node                              | None
/mimosa_node/lidar/photometric/img_new_features              | sensor_msgs/msg/Image                    | mimosa_node                              | None
/mimosa_node/lidar/photometric/img_tracked_keyframe_features | sensor_msgs/msg/Image                    | mimosa_node                              | None
/mimosa_node/lidar/photometric/localizability_marker_array   | visualization_msgs/msg/MarkerArray       | mimosa_node                              | None
/mimosa_node/radar/manager/debug                             | mimosa_msgs/msg/RadarManagerDebug        | mimosa_node                              | None
/mimosa_node/radar/manager/filtered_points                   | sensor_msgs/msg/PointCloud2              | mimosa_node                              | None

# ROVIO INPUTS

/abss/twist                                                  | geometry_msgs/msg/TwistStamped           | None                                     | rovio
/cam0/image_raw                                              | sensor_msgs/msg/Image                    | None                                     | rovio
/cam1/image_raw                                              | sensor_msgs/msg/Image                    | None                                     | rovio
/imu0                                                        | sensor_msgs/msg/Imu                      | None                                     | rovio
/odometry                                                    | nav_msgs/msg/Odometry                    | None                                     | rovio
/pose                                                        | geometry_msgs/msg/TransformStamped       | None                                     | rovio

# ROVIO TOPICS

/rovio/T_G_W                                                 | geometry_msgs/msg/TransformStamped       | rovio                                    | None
/rovio/extrinsics0                                           | geometry_msgs/msg/PoseWithCovarianceStamped | rovio                                    | None
/rovio/imu_biases                                            | sensor_msgs/msg/Imu                      | rovio                                    | None
/rovio/markers                                               | visualization_msgs/msg/Marker            | rovio                                    | None
/rovio/odometry                                              | nav_msgs/msg/Odometry                    | rovio                                    | None
/rovio/patch                                                 | sensor_msgs/msg/PointCloud2              | rovio                                    | None
/rovio/pcl                                                   | sensor_msgs/msg/PointCloud2              | rovio                                    | None
/rovio/pose_with_covariance_stamped                          | geometry_msgs/msg/PoseWithCovarianceStamped | rovio                                    | None
/rovio/transform                                             | geometry_msgs/msg/TransformStamped       | rovio                                    | None