Merge pull request #12 from Cardinal-Space-Mining/v0.5.0

V0.5.0

Author: S1ink

.clang-tidy

@@ -0,0 +1,43 @@
+---
+Checks: "*,
+        -abseil-*,
+        -altera-*,
+        -android-*,
+        -fuchsia-*,
+        -google-*,
+        -llvm*,
+        -modernize-use-trailing-return-type,
+        -zircon-*,
+        -readability-else-after-return,
+        -readability-static-accessed-through-instance,
+        -readability-avoid-const-params-in-decls,
+        -cppcoreguidelines-non-private-member-variables-in-classes,
+        -misc-non-private-member-variables-in-classes,
+        -misc-include-cleaner,
+        -readability-isolate-declaration,
+        -cppcoreguidelines-avoid-magic-numbers,
+        -readability-magic-numbers,
+        -hicpp-braces-around-statements,
+        -readability-braces-around-statements,
+        -misc-const-correctness,
+        -bugprone-narrowing-conversions,
+        -cppcoreguidelines-narrowing-conversions,
+        -readability-function-cognitive-complexity,
+        -cppcoreguidelines-pro-type-union-access,
+        -readability-identifier-length,
+        -cppcoreguidelines-avoid-do-while,
+        -cppcoreguidelines-pro-bounds-constant-array-index,
+        -hicpp-use-auto,
+        -modernize-use-auto,
+        -hicpp-uppercase-literal-suffix,
+        -readability-uppercase-literal-suffix,
+
+"
+WarningsAsErrors: ''
+HeaderFilterRegex: ''
+FormatStyle:     none
+
+
+
+
+

CMakeLists.txt

@@ -74,17 +74,13 @@ add_library(nano_gicp STATIC "src/nano_gicp/lsq_registration.cpp" "src/nano_gicp
 target_link_libraries(nano_gicp ${PCL_LIBRARIES} ${OpenMP_LIBS} nanoflann)
 ament_target_dependencies(nano_gicp)
 
-# add_library(ikd_tree STATIC "src/ikd_tree/ikd_tree.cpp")
-# target_link_libraries(ikd_tree ${PCL_LIBRARIES})
-# ament_target_dependencies(ikd_tree)
-
 add_library(stats STATIC "src/stats/stats.cpp")
 ament_target_dependencies(stats)
 
 
 add_executable(perception_node
   "src/perception_node.cpp"
-  "src/core/localization.cpp"
+  "src/core/perception.cpp"
   "src/core/odometry.cpp")
 target_link_libraries(perception_node
   ${PCL_LIBRARIES}

README.md

@@ -1,9 +1,12 @@
-# Cardinal Perception
+<!-- # Cardinal Perception -->
+![Cardinal Perception](doc/cardinal-perception.png)
 
-This package currently comprises localization (lidar odometry + apriltag rebias) and mapping components as used in CSM's autonomy solution (2024 and onward). The system has been designed around our hardware setup, but theoretically is compatible with varying configurations. See the architecture section for more info on sensor inputs and their role in the pipeline.
+This package currently comprises localization (lidar odometry + fiducial rebiasing), terrain mapping, traversability evaluation (in progress), and path generation (also in progress) components as used in CSM's autonomy solution (2024 and onward). The system has been designed around our hardware setup, but theoretically is compatible with varying configurations. See the architecture section for more info on sensor inputs and their role in the pipeline.
 
-## Architecture
-TODO
+## Overview
+![architecture overview](doc/cardinal-perception-v050-overview.svg)
+
+Cardinal Perception is currently split into two [ROS] nodes - a core node which accomplishes the vast majority of functionality, as well as a helper node used for detecting AprilTags and calculating pose information. The primary node is architected to act as a pipeline - comprising stages which accomlish localization; terrain mapping; traversibility generation; and trajectory generation tasks, respectively. This architecture supports a failry simple multithreading paradigm as well as minimizes latency for crucial stages (localization) while decoupling later stages such that they don't bottleneck the system as a whole. For more information on each stage as well as I/O and performance considerations, see the [architecture documentation](doc/architecture.md).
 
 ## Build
 1. Install [ROS2](https://docs.ros.org/en/jazzy/Installation.html) if necessary
@@ -31,8 +34,35 @@ TODO
     source install/setup.bash
     ```
 
-## VSCode
+*This package has been verified on and developed for ROS2 Humble (with Ubuntu 22.04) as well as ROS2 Jazzy (with Ubuntu 24.04). WSL has not been tested but theoretically should work.*
+
+## Usage
+### Prerequisites
+**To run Cardinal Perception, you will need (REQUIRED):**
+- A `sensor_msgs::msg::PointCloud2` topic providing a 3D LiDAR scan.
+- A correctly configured `config/perception.yaml` file (or equivalent) - see the [related documentation](doc/config.md) for information on parameters.
+
+**Optionally, you may also need:**
+- A `sensor_msgs::msg::Imu` topic providing IMU samples. This can help stabilize the odometry system, especially when an orientation estimate is directly usable as apart of each sample.
+- A set of `/tf` of `/tf_static` transforms provided by `robot_state_publisher`. This is necessary when the coordinate frame of the LiDAR scan is different from the coordinate frame of the IMU, or if you want to compute odometry for a frame different than that of the LiDAR scan.
+- A customized launchfile
+
+**Finally, to use the AprilTag detector for global estimates, you will need:**
+- A set of `sensor_msgs::msg::Image` and accompanying `sensor_msgs::msg::CameraInfo` topic for each camera to be used.
+- A correctly configured `config/tag_detection.yaml` file (or equivalent) - see the [related documentation](doc/config.md#tag-detection-node) for information on parameters.
 
+### Running
+If using the included launchfile:
+```bash
+ros2 launch cardinal_perception cardinal_perception.launch.py
+```
+To run the perception node by itself (not recommended):
+```bash
+ros2 run cardinal_perception perception_node --ros-args --params-file <PATH TO CARDINAL PERCEPTION INSTALL>/config/perception.yaml
+```
+**_For an advanced usage example, see [this repo](https://github.com/Cardinal-Space-Mining/lance-2025)._**
+
+## VSCode
 If intellisense is not working properly, ensure the CMake and C++ extensions are installed, and the C/C++ configuration is correct. This configuration can be edited by clicking the current configuration name in the bottom-right corner of the window, and editing the JSON file. Under the configuration you plan to use, make sure the following line is present:
 ```json
 "configurationProvider": "ms-vscode.cmake-tools"
@@ -57,5 +87,5 @@ This tells the C/C++ extension to use CMake as a configuration source for includ
     "version": 4
 }
 ```
-__*Last updated: 11/28/24*__
+__*Last updated: 2/19/25*__
 

config/perception.yaml

@@ -8,14 +8,16 @@
     scan_topic: "/lance/lidar_scan"
     imu_topic: "/lance/imu"
 
-    use_tag_detections: -1                # 1: enable, 0: disable, -1: filtered only
-    require_rebias_before_tf_pub: true    # wait until successful trajectory filter rebias before publishing map --> odom tf
-    require_rebias_before_scan_pub: true  # wait until successful trajectory filter rebias before republishing scans
+    use_tag_detections:  1                # 1: enable, 0: disable, -1: filtered only
+    require_rebias_before_tf_pub: false    # wait until successful trajectory filter rebias before publishing map --> odom tf
+    require_rebias_before_scan_pub: false  # wait until successful trajectory filter rebias before republishing scans
 
     metrics_pub_freq: 10.  # frequency for printing stats
+    publish_odometry_debug: true
 
-    max_localization_threads: 3
-    max_mapping_threads: 2
+    cropbox_filter:   # points within this bounding box are exluded from processing (in base_frame coords)
+      min: [ -0.405, -0.6, 0.0 ]   # min corner ^
+      max: [ 0.735, 0.6, 0.959 ]   # max corner ^
 
     trajectory_filter:
       sampling_window_s: 0.6
@@ -27,12 +29,11 @@
         max_angular_deviation: 0.04
 
     dlo:
-      debug:
-        publish_scans: true
+      use_timestamps_as_init: true
+      gravity_align: false   # enable/disable gravity alignment on startup
       adaptive_params:
         use: true           # whether or not keyframe params scale with "spaciousness"
         lpf_coeff: 0.8
-      gravity_align: true   # enable/disable gravity alignment on startup
       keyframe:
         thresh_D: 1.    # distance threshold in meters for creating a new keyframe
         thresh_R: 30.   # rotation threshold in degrees for creating a new keyframe
@@ -44,10 +45,6 @@
         use: true                         # start with the provided pose?
         position: [ 0., 0., 0. ]          # x, y, z
         orientation: [ 1., 0., 0., 0. ]   # w, x, y, z
-      crop_filter:
-        use: true                     # exclude points within the provided bounding box (in the base frame) from processing
-        min: [ -0.405, -0.6, 0.0 ]   # min corner ^
-        max: [ 0.735, 0.6, 0.959 ]   # max corner ^
       voxel_filter:
         scan:
           use: true   # voxelize each input scan?
@@ -62,36 +59,53 @@
           floor: 0.04
           ceil: 0.5
           precision: 0.01
+      immediate_filter:
+        use: false
+        range: 1.0
+        thresh_proportion: 0.3
       imu:
         use: true           # integrate imu data to hint GICP?
         use_orientation: true
         calib_time: 3       # calibration time
-        buffer_size: 2000   # measurement buffer length
       gicp:
-        min_num_points: 1000  # minimum points required
+        num_threads: 4
+        min_num_points: 500  # minimum points required
         s2s:
-          k_correspondences: 10
-          max_correspondence_distance: 0.1
+          k_correspondences: 15
+          max_correspondence_distance: 0.25
           max_iterations: 32
           transformation_epsilon: 0.01
           euclidean_fitness_epsilon: 0.01
           ransac:
             iterations: 5
-            outlier_rejection_thresh: 0.5
+            outlier_rejection_thresh: 0.1
         s2m:
-          k_correspondences: 20
-          max_correspondence_distance: 0.05
+          k_correspondences: 30
+          max_correspondence_distance: 0.1
           max_iterations: 32
-          transformation_epsilon: 0.01
-          euclidean_fitness_epsilon: 0.01
+          transformation_epsilon: 0.0009
+          euclidean_fitness_epsilon: 0.005
           ransac:
-            iterations: 5
-            outlier_rejection_thresh: 1.
+            iterations: 6
+            outlier_rejection_thresh: 0.05
 
     mapping:
       frustum_search_radius: 0.009
       radial_distance_thresh: 0.01
       delete_delta_coeff: 0.03
-      delete_max_range: 3.
-      add_max_range: 5.
-      voxel_size: 0.04
+      delete_max_range: 4.
+      add_max_range: 4.
+      voxel_size: 0.08
+
+    fiducial_detection:
+      max_range: 2.
+      plane_distance_threshold: 0.005
+      plane_eps_thresh: 0.1
+      vox_resolution: 0.03
+      remaining_points_thresh: 0.05
+      minimum_input_points: 100
+      minimum_segmented_points: 15
+
+    traversibility:
+      chunk_horizontal_range: 3.5
+      chunk_vertical_range: 1.

doc/architecture.md

@@ -0,0 +1,33 @@
+## Cardinal Perception System Architecture
+### Localization
+The localization solution consists of two components: LiDAR odometry and fiducial detection for global alignment. The LiDAR odometry is based off of [direct_lidar_odometry](https://github.com/vectr-ucla/direct_lidar_odometry) (DLO) and utilizes a scan-to-scan and scan-to-map based odometry solution, with optional IMU input to seed registration. Fiducial detection is not required but can come in one of two forms - AprilTag detections or LiDAR-reflector detection (this is custom). Fusion between local and global measurements is currently only done on a simple transform-offset basis, utilizing the `map` and `odom` transform-tree frames (meaning that without fiducial detection, odometry is implicitly treated as the full localization solution).
+
+![localization overview](localization-v050.svg)
+
+**Localization subsribes to the following ROS2 topics:**
+1. A `sensor_msgs/msg/PointCloud2` topic as configured by the parameter `scan_topic` (default is `scan`). This can be any pointcloud and is not restricted to being organized or needing have timestamp data, etc..
+2. A `sensor_msgs/msg/Imu` topic as configured by the parameter `imu_topic` (default is `imu`). *This is optional and IMU usage can be disabled by setting the `dlo.imu.use` parameter to `false`.* IMU data must be in the same time-base as LiDAR scans, since these are timestamp matched internally.
+3. A `cardinal_perception/msg/TagsTransform` topic which defaults to `"/tags_detections"` in both nodes. This is only relevant when using the AprilTag detector node to provide global measurements, which can be parameter-disabled by setting `use_tag_detections` to `false` or compile-time disabled by using setting the `USE_TAG_DETECTION_PIPELINE` macro to `0`.
+
+### Terrain Mapping
+Terrain mapping attempts to model the real world utilizing a set of cartesian points. The map itself is stored as a pointcloud, and indexed using a specialized octree which automatically voxelizes all points in the same leaf and allows for direct deletion of points/leaves. Point-deletion is calculating using the custom-developed "KDTree Frustum Collision" (KFC) mapping model, which provides (more or less) realtime mapping using just LiDAR scans. This stage does not have any additonal ROS subscriptions but works directly off of buffers which are reused from the localization stage, as well as the most recent localization results.
+
+![mapping overview](mapping-v050.svg)
+
+#### KFC Map Iteration Steps
+1. Assemble submap from global map using distance from current LiDAR scanner position.
+2. Calculate the direction to each submap point from the LiDAR position, and normalize to be of unit length.
+3. Build a KDTree from the global submap direction vectors.
+4. Calculate the direction to each scan point from the LiDAR position, and normalize to be of unit length. For each point, complete a range search in the submap KDTree to find points within the "frustum cone" for the current LiDAR ray.
+5. Apply the collision model to determine which points should be deleted (points that are closer than the scan point get deleted).
+6. Perform delete operation on global map.
+7. Add scan points to global map.
+8. Optimize the underlying octree structure.
+
+### Traversability Generation
+(Not implemented yet!)
+
+### Trajectory Generation
+(Not implemented yet!)
+
+__*Last updated: 2/19/25*__