From 8cbed778b78293410ea4c87d53ef53a86ce5d651 Mon Sep 17 00:00:00 2001 From: S1ink Date: Mon, 19 Jan 2026 16:06:50 -0600 Subject: [PATCH 1/2] release docs and cleanup cmake config --- README.md | 103 +++++++++++++++--------- cmake/Config.cmake | 20 +---- cmake/config.hpp.in | 12 +-- doc/architecture.md | 9 ++- doc/config.md | 12 +-- include/modules/impl/kfc_map_impl.hpp | 109 +++++++++++++------------- include/modules/kfc_map.hpp | 13 --- include/modules/path_planner.hpp | 3 - launch/perception.launch.py | 7 +- src/core/perception.hpp | 1 + src/core/perception_core.cpp | 55 ++++++------- src/core/perception_presets.hpp | 42 ---------- 12 files changed, 166 insertions(+), 220 deletions(-) diff --git a/README.md b/README.md index c9f93f7..9bf5fb2 100644 --- a/README.md +++ b/README.md @@ -1,19 +1,35 @@ ![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 @@ -21,53 +37,63 @@ See the [architecture documentation](doc/architecture.md) for more information o - 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 -``` -**_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 index 47b22c8..bf353e7 100644 --- 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 index 96d74b4..6dcd7df 100644 --- a/cmake/config.hpp.in +++ b/cmake/config.hpp.in @@ -3,19 +3,14 @@ #include #include -// --- 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@ diff --git a/doc/architecture.md b/doc/architecture.md index c453989..a93da18 100644 --- a/doc/architecture.md +++ b/doc/architecture.md @@ -1,4 +1,11 @@ ## Cardinal Perception System Architecture + +> [!WARNING] +> This doc is out of date and currently under construction! + +### V0.5.0 System Overview +![architecture overview](cardinal-perception-v050-overview.svg) + ### 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). @@ -30,4 +37,4 @@ Terrain mapping attempts to model the real world utilizing a set of cartesian po ### Trajectory Generation (Not implemented yet!) -__*Last updated: 2/19/25*__ +__*Last updated: 1/19/26*__ diff --git a/doc/config.md b/doc/config.md index ec968cd..f284b66 100644 --- a/doc/config.md +++ b/doc/config.md @@ -1,11 +1,11 @@ ## Node Parameters -# THIS SPEC IS OUT OF DATE FOR V0.6.0+! -New documentation to come soon... hopefully :) +> [!CAUTION] +> This doc is out of date for V0.6.0+. Some parameters listed here have been removed, and others have been added. Use only as a guide for the parameters that are still relevant and listed in the example config file. --- ### Perception Node -_See file: [config/perception.yaml](../config/perception.yaml)_ +_Config block:_ `perception` * `map_frame_id` (String) : **The global frame id** - Default: `"map"` @@ -206,7 +206,7 @@ _See file: [config/perception.yaml](../config/perception.yaml)_ --- ### Tag Detection Node -_See file: [config/tag_detection.yaml](../config/tag_detection.yaml)_ +_Config block:_ `tag_detection` * `image_transport` (String) : **Transport type for image stream** - Default: `"raw"` @@ -310,4 +310,6 @@ _See file: [config/tag_detection.yaml](../config/tag_detection.yaml)_ * `tag5_corners` (Float Array) - Default: `[0.303, 0.360, 0.476, 0.364, 0.280, 0.477, 0.362, 0.280, 0.577, 0.301, 0.360, 0.576]` -__*Last updated: 2/19/25*__ +__*Last updated on 1/19/26*__ + +__*Parameters relevant as of 2/19/25*__ diff --git a/include/modules/impl/kfc_map_impl.hpp b/include/modules/impl/kfc_map_impl.hpp index 87a2e5d..1bc5df8 100644 --- a/include/modules/impl/kfc_map_impl.hpp +++ b/include/modules/impl/kfc_map_impl.hpp @@ -91,14 +91,20 @@ typename KFCMap::UpdateResult KFCMap::updateMap( const std::vector* inf_rays, const pcl::Indices* pt_indices) { + // 0. Init buffers --------------------------------------------------------- UpdateResult results{}; if (pt_indices && pt_indices->size() <= 0) { return results; } + pcl::Indices tmp_indices, points_to_add; + std::vector tmp_dists; + std::set submap_remove_indices; + std::unique_lock lock{this->mtx}; + // 1. Use raw if empty ----------------------------------------------------- auto map_cloud_ptr = this->map_octree.getInputCloud(); if (map_cloud_ptr->empty()) { @@ -107,43 +113,30 @@ typename KFCMap::UpdateResult KFCMap::updateMap( return results; } -#if KFC_MAP_STORE_INSTANCE_BUFFERS <= 0 - thread_local struct - { - pcl::Indices search_indices, points_to_add; - std::vector dists; - std::set submap_remove_indices; - } buff; -#endif - - // reset search buffers - buff.search_indices.clear(); - buff.dists.clear(); - - // collect indices for points within range from scanner origin + // 2. Collect indices for points within range from scanner origin ---------- PointT lp; lp.getVector3fMap() = origin; results.points_searched = this->map_octree.radiusSearch( lp, this->delete_max_range, - buff.search_indices, - buff.dists); + tmp_indices, + tmp_dists); - // prepare submap range buffer + // 3. Compute range, direction for each point in submap -------------------- if (!this->submap_ranges) { this->submap_ranges = std::make_shared(); } this->submap_ranges->clear(); - this->submap_ranges->reserve(buff.search_indices.size()); + this->submap_ranges->reserve(tmp_indices.size()); // IMPROVE: OMP parallelize - for (size_t i = 0; i < buff.search_indices.size(); i++) + for (size_t i = 0; i < tmp_indices.size(); i++) { // store distance and original index for each submap point auto& v = this->submap_ranges->points.emplace_back(); - v.curvature = std::sqrt(buff.dists[i]); - v.label = buff.search_indices[i]; + v.curvature = std::sqrt(tmp_dists[i]); + v.label = tmp_indices[i]; // store unit direction to each submap point from scanner const auto& p = map_cloud_ptr->points[v.label]; @@ -153,20 +146,18 @@ typename KFCMap::UpdateResult KFCMap::updateMap( this->submap_ranges->width = this->submap_ranges->points.size(); this->submap_ranges->height = 1; - // build kdtree using unit directions of submap points - this->collision_kdtree.setInputCloud( - this->submap_ranges); // TODO: handle no map points + // 4. Build kdtree using unit directions of submap points ------------------ + // TODO: handle no map points + this->collision_kdtree.setInputCloud(this->submap_ranges); auto& scan_vec = pts.points; - buff.submap_remove_indices.clear(); - buff.points_to_add.clear(); - buff.points_to_add.reserve( - pt_indices ? pt_indices->size() : scan_vec.size()); + points_to_add.reserve(pt_indices ? pt_indices->size() : scan_vec.size()); - // loop all input points + // 5. Analyze collisions for all input points ------------------------------ // IMPROVE: could possibly parallelize if the outputs are synchronized for (size_t idx = 0;; idx++) { + // 5-A. Handle selection size_t i = idx; if (pt_indices) { @@ -181,24 +172,26 @@ typename KFCMap::UpdateResult KFCMap::updateMap( break; } - // collision direction buffer + // 5-B. Compute direction and range for input point const auto& src_p = scan_vec[i]; CollisionPointT p; p.getNormalVector3fMap() = (src_p.getVector3fMap() - origin); p.curvature = p.getNormalVector3fMap().norm(); p.getVector3fMap() = p.getNormalVector3fMap().normalized(); - // search for points with directions in angular range + // 5-C. Search for points with directions in angular range using kdtree + tmp_indices.clear(); + tmp_dists.clear(); this->collision_kdtree.radiusSearch( p, this->frustum_search_radius, - buff.search_indices, - buff.dists); + tmp_indices, + tmp_dists); - // loop points in angular range - for (size_t j = 0; j < buff.search_indices.size(); j++) + // 5-D. Compare input point to all points in search set to analyze collisions + for (size_t j = 0; j < tmp_indices.size(); j++) { - pcl::index_t k = buff.search_indices[j]; + pcl::index_t k = tmp_indices[j]; // only consider keeping the point if it has a valid normal if (!(CollisionV & KF_COLLISION_MODEL_REMOVE_INVALID_NORMALS) || @@ -208,13 +201,14 @@ typename KFCMap::UpdateResult KFCMap::updateMap( { // keep the point if it's outside the radial zone if ((this->submap_ranges->points[k].curvature * - std::sqrt(buff.dists[j])) > this->radial_dist_thresh) + std::sqrt(tmp_dists[j])) > this->radial_dist_thresh) { continue; } } - // find local plane intersection, and keep the point if the new point is "in range" of this sample + // find local plane intersection, and keep the point if the new + // point is "in range" of this sample auto n = this->map_octree.pointNormals()[k].template head<3>(); const float inv_a = 1.f / p.getVector3fMap().dot(n); const float b = @@ -229,20 +223,19 @@ typename KFCMap::UpdateResult KFCMap::updateMap( } } - buff.submap_remove_indices.insert( - this->submap_ranges->points[k].label); + submap_remove_indices.insert(this->submap_ranges->points[k].label); } - // add point if it's in the currently configured bounds + // 5-E. Add point if it's in the currently configured bounds if ((src_p.getArray3fMap() > this->bounds_min).all() && (src_p.getArray3fMap() < this->bounds_max).all() && (p.curvature <= this->add_max_range)) { - buff.points_to_add.push_back(i); + points_to_add.push_back(i); } } - // if infinite range dirs provided, remove all points in respective collision zones + // 6. Repeat previous process with infinite ray directions, if provided ---- if (inf_rays) { for (const RayDirT& r : *inf_rays) @@ -250,45 +243,49 @@ typename KFCMap::UpdateResult KFCMap::updateMap( CollisionPointT p; p.getVector3fMap() = r.getNormalVector3fMap(); + tmp_indices.clear(); + tmp_dists.clear(); this->collision_kdtree.radiusSearch( p, this->frustum_search_radius, - buff.search_indices, - buff.dists); + tmp_indices, + tmp_dists); - for (size_t i = 0; i < buff.search_indices.size(); i++) + for (size_t i = 0; i < tmp_indices.size(); i++) { - pcl::index_t k = buff.search_indices[i]; + pcl::index_t k = tmp_indices[i]; if constexpr (CollisionV & KF_COLLISION_MODEL_USE_RADIAL) { if ((this->submap_ranges->points[k].curvature * - std::sqrt(buff.dists[i])) > this->radial_dist_thresh) + std::sqrt(tmp_dists[i])) > this->radial_dist_thresh) { continue; } } - buff.submap_remove_indices.insert( + submap_remove_indices.insert( this->submap_ranges->points[k].label); } } } - // delete all points which were "collided with" - for (auto itr = buff.submap_remove_indices.begin(); - itr != buff.submap_remove_indices.end(); + // 7. Delete all points which were "collided with" from the map ------------ + for (auto itr = submap_remove_indices.begin(); + itr != submap_remove_indices.end(); itr++) { this->map_octree.deletePoint(*itr, true); } - // add source points - this->map_octree.addPoints(pts, &buff.points_to_add); - // ensure point buffers are dense + + // 8. Add inputs points to map --------------------------------------------- + this->map_octree.addPoints(pts, &points_to_add); + + // 9. Ensure map point buffer is dense ------------------------------------- this->map_octree.optimizeStorage(); results.points_deleted = - static_cast(buff.submap_remove_indices.size()); + static_cast(submap_remove_indices.size()); return results; } diff --git a/include/modules/kfc_map.hpp b/include/modules/kfc_map.hpp index 191f60e..67c88d5 100644 --- a/include/modules/kfc_map.hpp +++ b/include/modules/kfc_map.hpp @@ -51,10 +51,6 @@ #include "map_octree.hpp" -#ifndef KFC_MAP_STORE_INSTANCE_BUFFERS - #define KFC_MAP_STORE_INSTANCE_BUFFERS 1 -#endif - namespace csm { @@ -222,15 +218,6 @@ class KFCMap std::mutex mtx; -#if KFC_MAP_STORE_INSTANCE_BUFFERS - struct - { - pcl::Indices search_indices, points_to_add; - std::vector dists; - std::set submap_remove_indices; - } buff; -#endif - double frustum_search_radius{0.01}; double radial_dist_thresh{0.01}; double half_surface_width{0.01}; diff --git a/include/modules/path_planner.hpp b/include/modules/path_planner.hpp index 9cd5ef0..8a46627 100644 --- a/include/modules/path_planner.hpp +++ b/include/modules/path_planner.hpp @@ -52,9 +52,6 @@ #include "path_plan_map.hpp" -#ifndef PATH_PLANNING_PEDANTIC - #define PATH_PLANNING_PEDANTIC 0 -#endif #ifndef PPLAN_PRINT_DEBUG #define PPLAN_PRINT_DEBUG 0 #endif diff --git a/launch/perception.launch.py b/launch/perception.launch.py index e154755..d403ba1 100644 --- a/launch/perception.launch.py +++ b/launch/perception.launch.py @@ -15,12 +15,13 @@ except Exception as e: HAVE_LAUNCH_UTILS = False -from perception_launch_utils import get_perception_actions - - PKG_PATH = get_package_share_directory('cardinal_perception') DEFAULT_JSON_PATH = os.path.join(PKG_PATH, 'config', 'perception.json') +sys.path.append(os.path.join(PKG_PATH, 'launch')) +from perception_launch_utils import get_perception_actions + + def launch(context, *args, **kwargs): actions = [] diff --git a/src/core/perception.hpp b/src/core/perception.hpp index 04df310..8bf31e5 100644 --- a/src/core/perception.hpp +++ b/src/core/perception.hpp @@ -58,6 +58,7 @@ #include #include +#include #include "threads/imu_worker.hpp" #include "threads/mapping_worker.hpp" diff --git a/src/core/perception_core.cpp b/src/core/perception_core.cpp index e74c167..1f86961 100644 --- a/src/core/perception_core.cpp +++ b/src/core/perception_core.cpp @@ -175,7 +175,7 @@ struct PerceptionConfig #if PERCEPTION_USE_TAG_DETECTION_PIPELINE return "AprilTag"; #elif PERCEPTION_USE_LFD_PIPELINE - return "Lidar fiducial"; + return "Lidar Fiducial"; #else return "Disabled"; #endif @@ -207,22 +207,10 @@ std::ostream& operator<<(std::ostream& os, const PerceptionConfig& config) os << std::setprecision(3); os << "\n" " +-- CONFIGURATION ---------------------------------+\n" - " |\n" - " +- PIPELINE STAGES\n" - << align("Fiducial mode") << PerceptionConfig::getFiducialModeStr() - << "\n" - << align("Mapping") - << PerceptionConfig::getEnableDisableStr(PERCEPTION_ENABLE_MAPPING) - << "\n" - << align("Traversibility") - << PerceptionConfig::getEnableDisableStr( - PERCEPTION_ENABLE_TRAVERSIBILITY) - << "\n" - << align("Path Planning") - << PerceptionConfig::getEnableDisableStr(PERCEPTION_ENABLE_PATH_PLANNING) - << "\n" " |\n" " +- FEATURES\n" + << align("Fiducial Mode") << PerceptionConfig::getFiducialModeStr() + << "\n" << align("Scan Deskew") << PerceptionConfig::getEnableDisableStr(PERCEPTION_USE_SCAN_DESKEW) << "\n" @@ -295,7 +283,7 @@ std::ostream& operator<<(std::ostream& os, const PerceptionConfig& config) << "\n"; #endif -#if MAPPING_ENABLED + // #if MAPPING_ENABLED os << " |\n" " +- MAPPING\n" << align("Horizontal Crop Range") << config.map_crop_horizontal_range @@ -311,9 +299,9 @@ std::ostream& operator<<(std::ostream& os, const PerceptionConfig& config) << " meters\n" << align("Add Max Range") << config.kfc_add_max_range << " meters\n" << align("Voxel Size") << config.kfc_voxel_size << " meters\n"; -#endif + // #endif -#if TRAVERSIBILITY_ENABLED + // #if TRAVERSIBILITY_ENABLED os << " |\n" " +- TRAVERSIBILITY\n" << align("Horizontal Export Range") << config.map_export_horizontal_range @@ -335,9 +323,9 @@ std::ostream& operator<<(std::ostream& os, const PerceptionConfig& config) << align("Vox Cell Points Thresh") << config.trav_min_vox_cell_points << "\n" << align("Point Samples") << config.trav_interp_point_samples << "\n"; -#endif + // #endif -#if PATH_PLANNING_ENABLED + // #if PATH_PLANNING_ENABLED os << " |\n" " +- PATH PLANNING\n" << align("Boundary Radius") << config.pplan_boundary_radius @@ -347,16 +335,19 @@ std::ostream& operator<<(std::ostream& os, const PerceptionConfig& config) << align("Distance Coeff") << config.pplan_dist_coeff << "\n" << align("Straightness Coeff") << config.pplan_dir_coeff << "\n" << align("Traversibility Coeff") << config.pplan_trav_coeff << "\n" - << align("Verification Range") << config.pplan_verification_range << " meters\n" - << align("Verification Degree") << config.pplan_verification_degree << " points\n" - << align("Max Num Neighbors") << config.pplan_max_neighbors << " points\n" + << align("Verification Range") << config.pplan_verification_range + << " meters\n" + << align("Verification Degree") << config.pplan_verification_degree + << " points\n" + << align("Max Num Neighbors") << config.pplan_max_neighbors + << " points\n" << align("Map Merge Window") << config.pplan_map_obstacle_merge_window << " meters\n" << align("Map Hrz. Crop Range") << config.pplan_map_passive_crop_horizontal_range << " meters\n" << align("Map Vrt. Crop Range") << config.pplan_map_passive_crop_vertical_range << " meters\n"; -#endif + // #endif os << " +\n"; return os; @@ -570,7 +561,7 @@ void PerceptionNode::getParams(PerceptionConfig& config) #endif // --- MAPPING ------------------------------------------------------------- -#if MAPPING_ENABLED + // #if MAPPING_ENABLED util::declare_param( this, "mapping.crop_horizontal_range", @@ -619,10 +610,10 @@ void PerceptionNode::getParams(PerceptionConfig& config) config.kfc_delete_max_range, config.kfc_add_max_range, config.kfc_voxel_size); -#endif + // #endif // --- TRAVERSIBILITY ------------------------------------------------------ -#if TRAVERSIBILITY_ENABLED + // #if TRAVERSIBILITY_ENABLED util::declare_param( this, "traversibility.chunk_horizontal_range", @@ -704,10 +695,10 @@ void PerceptionNode::getParams(PerceptionConfig& config) config.trav_score_grad_weight, config.trav_min_vox_cell_points, config.trav_interp_point_samples); -#endif + // #endif // --- PATH PLANNING ------------------------------------------------------- -#if PATH_PLANNING_ENABLED + // #if PATH_PLANNING_ENABLED util::declare_param( this, "pplan.boundary_radius", @@ -779,7 +770,7 @@ void PerceptionNode::getParams(PerceptionConfig& config) config.pplan_verification_range, config.pplan_verification_degree, config.pplan_max_neighbors); -#endif + // #endif this->imu_worker.configure(config.base_frame); this->localization_worker.configure( @@ -834,14 +825,14 @@ void PerceptionNode::initPubSubs(PerceptionConfig& config) resp->success = this->localization_worker.setGlobalAlignmentEnabled(req->data); }); -#if PATH_PLANNING_ENABLED + // #if PATH_PLANNING_ENABLED this->path_plan_service = this->create_service( PERCEPTION_TOPIC("update_path_planning"), [this]( UpdatePathPlanSrv::Request::SharedPtr req, UpdatePathPlanSrv::Response::SharedPtr resp) { this->path_planning_worker.accept(req, resp); }); -#endif + // #endif this->mining_eval_service = this->create_service( PERCEPTION_TOPIC("query_mining_eval"), [this]( diff --git a/src/core/perception_presets.hpp b/src/core/perception_presets.hpp index 3338dfd..116e676 100644 --- a/src/core/perception_presets.hpp +++ b/src/core/perception_presets.hpp @@ -79,18 +79,6 @@ #endif // --- PIPELINE STAGES ENABLE/DISABLE ----------------------------------------- -#ifndef PERCEPTION_ENABLE_MAPPING - #define PERCEPTION_ENABLE_MAPPING 1 -#endif - -#ifndef PERCEPTION_ENABLE_TRAVERSIBILITY - #define PERCEPTION_ENABLE_TRAVERSIBILITY (PERCEPTION_ENABLE_MAPPING) -#endif - -#ifndef PERCEPTION_ENABLE_PATH_PLANNING - #define PERCEPTION_ENABLE_PATH_PLANNING (PERCEPTION_ENABLE_TRAVERSIBILITY) -#endif - #ifndef PERCEPTION_USE_TAG_DETECTION_PIPELINE #define PERCEPTION_USE_TAG_DETECTION_PIPELINE 0 #endif @@ -125,39 +113,9 @@ static_assert( "Tag detection and lidar fiducial pipelines are mutually exclusive." "You may only enable one at a time."); #endif -#if ((PERCEPTION_ENABLE_TRAVERSIBILITY) && !(PERCEPTION_ENABLE_MAPPING)) - #undef PERCEPTION_ENABLE_TRAVERSIBILITY - #define PERCEPTION_ENABLE_TRAVERSIBILITY 0 -#endif -#if ((PERCEPTION_ENABLE_PATH_PLANNING) && !(PERCEPTION_ENABLE_TRAVERSIBILITY)) - #undef PERCEPTION_ENABLE_PATH_PLANNING - #define PERCEPTION_ENABLE_PATH_PLANNING 0 -#endif // --- HELPER MACROS ---------------------------------------------------------- -#if PERCEPTION_ENABLE_MAPPING > 0 - #define IF_MAPPING_ENABLED(...) __VA_ARGS__ - #define MAPPING_ENABLED 1 -#else - #define IF_MAPPING_ENABLED(...) - #define MAPPING_ENABLED 0 -#endif -#if PERCEPTION_ENABLE_TRAVERSIBILITY > 0 - #define IF_TRAVERSIBILITY_ENABLED(...) __VA_ARGS__ - #define TRAVERSIBILITY_ENABLED 1 -#else - #define IF_TRAVERSIBILITY_ENABLED(...) - #define TRAVERSIBILITY_ENABLED 0 -#endif -#if PERCEPTION_ENABLE_PATH_PLANNING > 0 - #define IF_PATH_PLANNING_ENABLED(...) __VA_ARGS__ - #define PATH_PLANNING_ENABLED 1 -#else - #define IF_PATH_PLANNING_ENABLED(...) - #define PATH_PLANNING_ENABLED 0 -#endif - #if PERCEPTION_USE_TAG_DETECTION_PIPELINE > 0 #define IF_TAG_DETECTION_ENABLED(...) __VA_ARGS__ #define TAG_DETECTION_ENABLED 1 From a401d5ff9be0ee93f4236e93965500f18b740bcd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sam=20Richter=20/=20=E4=BD=95=E6=89=8D=E5=8B=87?= <60528506+S1ink@users.noreply.github.com> Date: Mon, 19 Jan 2026 16:08:20 -0600 Subject: [PATCH 2/2] updated my tip --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 9bf5fb2..e5ba1e3 100644 --- a/README.md +++ b/README.md @@ -54,8 +54,8 @@ Cardinal Perception is CSM's ROS2 perception package which comprises all the nec --cmake-args=-DCMAKE_EXPORT_COMPILE_COMMANDS:BOOL=ON source install/setup.bash ``` - > [!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). +> [!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). # 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):