Cardinal-Space-Mining · S1ink · Jan 19, 2026 · Oct 29, 2025 · Oct 29, 2025 · Oct 29, 2025
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -1,11 +1,12 @@
 cmake_minimum_required(VERSION 3.14)
-project(cardinal_perception VERSION 0.6.0)
+project(cardinal_perception VERSION 0.7.0)
 
 # --- Dependencies -------------------------------------------------------------
 find_package(rosidl_default_generators REQUIRED)
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
@@ -36,7 +37,7 @@ endif()
 
 # Default to C++17
 if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 17)
+  set(CMAKE_CXX_STANDARD 20)
 endif()
 
 if(NOT MSVC)
@@ -46,7 +47,7 @@ if(NOT MSVC)
 endif()
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
+  add_compile_options(-Wall -Wextra -Wpedantic -Wno-psabi)
 endif()
 
 # --- Misc. Options ------------------------------------------------------------
@@ -123,7 +124,12 @@ ament_target_dependencies(core_modules
 add_executable(perception_node
     "src/perception_node.cpp"
     "src/core/perception_core.cpp"
-    "src/core/perception_threads.cpp" )
+    "src/core/threads/imu_worker.cpp"
+    "src/core/threads/localization_worker.cpp"
+    "src/core/threads/mapping_worker.cpp"
+    "src/core/threads/mining_eval_worker.cpp"
+    "src/core/threads/path_planning_worker.cpp"
+    "src/core/threads/traversibility_worker.cpp" )
 target_include_directories(perception_node PUBLIC
     ${CMAKE_SOURCE_DIR}/include
     ${CMAKE_BINARY_DIR}/generated )
@@ -149,6 +155,7 @@ ament_target_dependencies(perception_node
     "rclcpp"
     "pcl_ros"
     "pcl_conversions"
+    "std_srvs"
     "sensor_msgs"
     "geometry_msgs"
     "nav_msgs"
@@ -202,6 +209,7 @@ ament_target_dependencies(pplan_client_node
 ament_export_dependencies(
     rclcpp
     std_msgs
+    std_srvs
     sensor_msgs
     geometry_msgs
     nav_msgs

diff --git a/LICENSE b/LICENSE
@@ -1,9 +1,21 @@
 MIT License
 
-Copyright (c) 2024-2025 Cardinal Space Mining Club
+Copyright (c) 2024-2026 Cardinal Space Mining Club
 
-Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
 
-The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
 
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
diff --git a/README.md b/README.md
@@ -1,73 +1,99 @@
 <!-- # Cardinal Perception -->
 ![Cardinal Perception](doc/cardinal-perception.png)
 
-Cardinal Perception is CSM's perception package used as a base for all advanced robot autonomy. It is structured as a multi-stage pipeline, consisting of **odometry**, **fiducial-based localization**, **mapping**, **traversiblity estimation**, and **path-planning** components.
+Cardinal Perception is CSM's ROS2 perception package which comprises all the necessary prerequisites for advanced autonomy; most notably localization, path-planning and terrain analysis. It has been designed around using the SICK MultiScan136 3D-LiDAR as its only input, although is highly configrable and likely to support other LiDAR/IMU setups.
 
-## Overview
-![architecture overview](doc/cardinal-perception-v050-overview.svg)
+# Setup
+> [!IMPORTANT]
+> **CSM team members:** Cardinal Perception is usually included as a submodule in larger robot code projects, where build/install procedures are integrated into a larger system/script. **YOU SHOULD NOT NEED TO CLONE/USE THIS REPO ON ITS OWN!**
 
-*This diagram is slightly out of date!*
+> [!NOTE]
+> Cardinal Perception has been verified to build and function on **ROS2 Humble (Ubuntu 22.04)**, **Jazzy (Ubuntu 24.04)** and **Kilted (Ubuntu 24.04)**, on both **x86-64** and **aarch64** architectures, as well as **WSL**.
 
-See the [architecture documentation](doc/architecture.md) for more information on individual pipeline stages.
-
-## Build
 1. Install [ROS2](https://docs.ros.org/en/jazzy/Installation.html) if necessary
 
-2. Use rosdep to install ROS package dependencies
+2. Setup your workspace and clone required repos
+    - Create directories:
+        ```bash
+        mkdir ros-ws && cd ros-ws
+        mkdir src && cd src
+        ```
+    - Clone repos:
+        ```bash
+        git clone https://github.com/Cardinal-Space-Mining/Cardinal-Perception -b main cardinal-perception
+        git clone https://github.com/Cardinal-Space-Mining/launch-utils -b main launch-utils
+        git clone https://github.com/Cardinal-Space-Mining/csm-metrics -b main csm-metrics
+        ```
+    - Navigate back to your workspace directory for the following steps:
+        ```bash
+        cd ..
+        ```
+
+3. Use rosdep to install ROS package dependencies
     - Initialize rosdep if necessary:
         ```bash
         sudo rosdep init
         ```
     - Update and install:
         ```bash
         rosdep update
-        rosdep install --ignore-src --from-paths . -r -y
+        rosdep install --ignore-src --from-paths ./src -r -y
         ```
 
-3. Install apt dependencies (should have already been resolved by rosdep)
+4. Install apt dependencies (should have already been resolved by rosdep)
     ```bash
     sudo apt update
     sudo apt-get install libpcl-dev libopencv-dev
     ```
 
-4. Build with colcon
+5. Build with colcon
     ```bash
-    colcon build --symlink-install <--executor parallel> <--event-handlers console_direct+> <--cmake-args=-DCMAKE_EXPORT_COMPILE_COMMANDS:BOOL=ON>
+    colcon build \
+        --symlink-install \
+        --event-handlers console_direct+ \
+        --cmake-args=-DCMAKE_EXPORT_COMPILE_COMMANDS:BOOL=ON
     source install/setup.bash
     ```
-    Additionally, there are various compile-time configurations which are exposed as CMake options. These are all listed in the [config generator template](cmake/config.hpp.in).
+> [!TIP]
+> There are various compile-time configurations which are exposed as CMake options. These are all listed in the [config generator template](cmake/config.hpp.in).
 
-*The project has been verified to build and function on ROS2 Humble (Ubuntu 22.04), Jazzy (Ubuntu 24.04) and Kilted (Ubuntu 24.04), on both x86-64 and aarch64 architectures, as well as WSL.*
+# Usage
+The best way to run Cardinal Perception is by using the included launchfile, which utilizes [launch-utils](https://github.com/Cardinal-Perception/launch-utils) to setup everything using the included [JSON config](config/perception.json):
+```bash
+ros2 launch cardinal_perception perception.launch.py
+```
+> [!IMPORTANT]
+> Review the following sections to ensure ROS2 and your config file are setup properly!
 
-## 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.
+## ROS Prerequisites
+Cardinal Perception requires the following in order to run:
+1. A `sensor_msgs::msg::PointCloud2` topic providing a 3D LiDAR scan. This can be live or from a bag.
+2. A proper transform definition, usually published on `/tf` and `/tf_static` by `robot_state_publisher`. The launch-utils package provides a way to bundle the config for this and automatically launch this node - see the [relevant docs](https://github.com/Cardinal-Space-Mining/launch-utils?tab=readme-ov-file#usage) for more information.
 
-**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 launch configuration to support your specific robot setup.
+The following are not required but are useful in some situations:
+- A `sensor_msgs::msg::Imu` topic providing IMU samples. This is used by the LIO system to pre-align scans and can drastically improve localization quality when in featureless environments or if scan message reliability is decreased. Some LiDARs contain integrated IMUs in which case this is a freebe.
+- A set of `sensor_msgs::msg::Image` and accompanying `sensor_msgs::msg::CameraInfo` topic for any number of cameras. These can be used to enable the optional AprilTag alignment pipeline.
 
-**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 launch configuration file describing the behavior of the fiducial-tag system. See the provided [config file](config/perception.json) for an example.
+## Configuration
+As already alluded, Cardinal Perception uses the [launch-utils](https://github.com/Cardinal-Perception/launch-utils) package to handle configuration and launching. This is due to the fact that many other ROS2 nodes must be run alongside Cardinal Perception in order to accomplish anything useful. By default, the included launch system uses [this file](config/perception.json) to configure and launch everything. This is merely a starting point which includes defaults for all the main parameters that can be used by Cardinal Perception, as well a barebones robot setup for the various boilerplate ROS2 nodes.
 
-### Running
-The following nodes are built, which can all be run individually or in a launch system:
-- `perception_node` - The core perception package
-- `tag_detection_node` - The apriltag detector
-- `pplan_client_node` - A service caller that can interface with foxglove studio cursor clicks
+The primary config sections used by Cardinal Perception are:
+- `perception`: Configures the main perception node.
+- `tag_detection`: Configures the AprilTag detector node.
+- `pplan_client`: Configures (enables/disables) the path planning client node.
 
-However, to fully configure the perception system, you will need to build and source the [launch_utils](https://github.com/Cardinal-Space-Mining/launch-utils) package. All three packages can then be configured an run using the [JSON config](config/perception.json) and following launch command:
-```bash
-ros2 launch cardinal_perception perception.launch.py <launch args...>
-```
-**_For an advanced usage example, see [this repo](https://github.com/Cardinal-Space-Mining/lance-2025)._**
+Due to the large number of paramters, up-to-date descriptions on what each one does have not been documented here. Old documentation can be referenced [here](doc/config.md) for some parameters that haven't changed in a while, and more specific docs about this may be included in the future. Fortunately, many configs can be left unchanged and the most crucial ones are quite straightforward in what they do.
+
+> [!TIP]
+> To better understand the config file layout, as well as boilerplate config sections, check out the [relevant docs](https://github.com/Cardinal-Space-Mining/launch-utils?tab=readme-ov-file#how-it-works).
+
+> [!TIP]
+> The example config is a cut down version of the primarly config used by CSM's LANCE robot(s). The full config can be found [here](https://github.com/Cardinal-Space-Mining/lance-2025/blob/main/lance/config/lance.json).
+
+# Development
 
 ## VSCode
-The build script exports compile commands which can help VSCode's C/C++ extension resolve correct syntax highlighting. To ensure this is working, paste the following code into the `c_cpp_properties.json` file (under .vscode directory in a workspace):
+The provided build command exports compile commands which can help VSCode's C/C++ extension resolve correct syntax highlighting. To ensure this is working, paste the following code into the `c_cpp_properties.json` file (under .vscode directory in a workspace):
 ```json
 {
     "configurations": [
@@ -89,5 +115,6 @@ The build script exports compile commands which can help VSCode's C/C++ extensio
     "version": 4
 }
 ```
-__*Last updated: 9/13/25*__
+
+__*Last updated on 1/19/26*__
 
diff --git a/cmake/Config.cmake b/cmake/Config.cmake
@@ -1,19 +1,14 @@
 # Config.cmake
 # Central place to declare all perception-related CMake options
 
-# --- MODULES CONFIGURATION -------------
-option(KFC_MAP_STORE_INSTANCE_BUFFERS "Enable map store instance buffers" ON)
-option(LFD_USE_ORTHO_PLANE_INTERSECTION "Use ortho plane intersection" ON)
-option(LFD_PRINT_DEBUG "Print LFD debug logs" OFF)
-option(PPLAN_PRINT_DEBUG "Print Path Planning debug logs" OFF)
-option(PATH_PLANNING_PEDANTIC "Enable pedantic path planning" OFF)
-
 # --- PROFILING CONFIGURATION -----------
 set(PROFILING_MODE "PROFILING_MODE_LIMITED" CACHE STRING "Profiling mode")          # full profiling use 'PROFILING_MODE_ALL'
 set(PROFILING_DEFAULT_BUFFER_SIZE 1 CACHE STRING "Profiling default buffer size")   # full profiling use 20
 
 # --- PRINTING ENABLE/DISABLE -----------
 option(PERCEPTION_PRINT_STARTUP_CONFIGS "Print startup configs" ON)
+option(LFD_PRINT_DEBUG "Print LFD debug logs" OFF)
+option(PPLAN_PRINT_DEBUG "Print Path Planning debug logs" OFF)
 option(TRANSFORM_SYNC_PRINT_DEBUG "Enable transform sync debug logs" OFF)
 option(TRAJECTORY_FILTER_PRINT_DEBUG "Enable trajectory filter debug logs" OFF)
 
@@ -30,9 +25,6 @@ option(PERCEPTION_USE_SCAN_DESKEW "Use scan deskew" OFF)
 option(PERCEPTION_USE_NULL_RAY_DELETION "Use null ray deletion" OFF)
 
 # --- PIPELINE STAGES ENABLE/DISABLE ----
-option(PERCEPTION_ENABLE_MAPPING "Enable mapping" ON)
-option(PERCEPTION_ENABLE_TRAVERSIBILITY "Enable traversibility" ON)
-option(PERCEPTION_ENABLE_PATH_PLANNING "Enable path planning" ON)
 option(PERCEPTION_USE_TAG_DETECTION_PIPELINE "Use tag detection pipeline" OFF)
 option(PERCEPTION_USE_LFD_PIPELINE "Use LFD pipeline" ON)
 
@@ -42,12 +34,9 @@ set(PERCEPTION_PUBSUB_QOS "rclcpp::SensorDataQoS()" CACHE STRING "PubSub QoS con
 
 # Boolean options
 foreach(opt
-    KFC_MAP_STORE_INSTANCE_BUFFERS
-    LFD_USE_ORTHO_PLANE_INTERSECTION
+    PERCEPTION_PRINT_STARTUP_CONFIGS
     LFD_PRINT_DEBUG
     PPLAN_PRINT_DEBUG
-    PATH_PLANNING_PEDANTIC
-    PERCEPTION_PRINT_STARTUP_CONFIGS
     TRANSFORM_SYNC_PRINT_DEBUG
     TRAJECTORY_FILTER_PRINT_DEBUG
     PERCEPTION_PUBLISH_GRAV_ESTIMATION
@@ -58,9 +47,6 @@ foreach(opt
     PERCEPTION_PUBLISH_FULL_MAP
     PERCEPTION_USE_SCAN_DESKEW
     PERCEPTION_USE_NULL_RAY_DELETION
-    PERCEPTION_ENABLE_MAPPING
-    PERCEPTION_ENABLE_TRAVERSIBILITY
-    PERCEPTION_ENABLE_PATH_PLANNING
     PERCEPTION_USE_TAG_DETECTION_PIPELINE
     PERCEPTION_USE_LFD_PIPELINE
 )

diff --git a/cmake/config.hpp.in b/cmake/config.hpp.in
@@ -3,19 +3,14 @@
 #include <csm_metrics/profiling_config.hpp>
 #include <point_def.hpp>
 
-// --- MODULES CONFIGURATION -------------
-#define KFC_MAP_STORE_INSTANCE_BUFFERS @KFC_MAP_STORE_INSTANCE_BUFFERS_VALUE@
-#define LFD_USE_ORTHO_PLANE_INTERSECTION @LFD_USE_ORTHO_PLANE_INTERSECTION_VALUE@
-#define LFD_PRINT_DEBUG @LFD_PRINT_DEBUG_VALUE@
-#define PPLAN_PRINT_DEBUG @PPLAN_PRINT_DEBUG_VALUE@
-#define PATH_PLANNING_PEDANTIC @PATH_PLANNING_PEDANTIC_VALUE@
-
 // --- PROFILING CONFIGURATION -----------
 #define PROFILING_MODE @PROFILING_MODE@
 #define PROFILING_DEFAULT_BUFFER_SIZE @PROFILING_DEFAULT_BUFFER_SIZE@
 
 // --- PRINTING ENABLE/DISABLE -----------
 #define PERCEPTION_PRINT_STARTUP_CONFIGS @PERCEPTION_PRINT_STARTUP_CONFIGS_VALUE@
+#define LFD_PRINT_DEBUG @LFD_PRINT_DEBUG_VALUE@
+#define PPLAN_PRINT_DEBUG @PPLAN_PRINT_DEBUG_VALUE@
 #define TRANSFORM_SYNC_PRINT_DEBUG @TRANSFORM_SYNC_PRINT_DEBUG_VALUE@
 #define TRAJECTORY_FILTER_PRINT_DEBUG @TRAJECTORY_FILTER_PRINT_DEBUG_VALUE@
 
@@ -32,9 +27,6 @@
 #define PERCEPTION_USE_NULL_RAY_DELETION @PERCEPTION_USE_NULL_RAY_DELETION_VALUE@
 
 // --- PIPELINE STAGES ENABLE/DISABLE ----
-#define PERCEPTION_ENABLE_MAPPING @PERCEPTION_ENABLE_MAPPING_VALUE@
-#define PERCEPTION_ENABLE_TRAVERSIBILITY @PERCEPTION_ENABLE_TRAVERSIBILITY_VALUE@
-#define PERCEPTION_ENABLE_PATH_PLANNING @PERCEPTION_ENABLE_PATH_PLANNING_VALUE@
 #define PERCEPTION_USE_TAG_DETECTION_PIPELINE @PERCEPTION_USE_TAG_DETECTION_PIPELINE_VALUE@
 #define PERCEPTION_USE_LFD_PIPELINE @PERCEPTION_USE_LFD_PIPELINE_VALUE@
 
@@ -48,15 +40,16 @@ namespace csm
 namespace perception
 {
 
-using OdomPointType = pcl::PointXYZ;
-using MappingPointType = pcl::PointXYZ;
+using MinimalPointType = pcl::PointXYZ;
+
+using OdomPointType = MinimalPointType;
+using MappingPointType = MinimalPointType;
 using FiducialPointType = csm::perception::PointXYZR;
-using CollisionPointType = pcl::PointXYZLNormal;
+using TraversibilityPointType = pcl::PointXYZI;
+
 using RayDirectionType = pcl::Axis;
 using SphericalDirectionPointType = csm::perception::PointSDir;
 using TimestampPointType = csm::perception::PointT_32HL;
-using TraversibilityPointType = pcl::PointXYZ;
-using TraversibilityMetaType = csm::perception::NormalTraversal;
 
 };  // namespace perception
 };  // namespace csm