From 4365b374299595845b45780e75b9940618cddde1 Mon Sep 17 00:00:00 2001 From: Bona Date: Mon, 15 Dec 2025 22:26:39 -0800 Subject: [PATCH 1/9] add fast_lio_ros2 A few things before you can run it: To enable auto-relocalization, edit localizer.yaml: auto_relocalize: true default_pcd_path: "/path/to/your/map.pcd" initial_x: 1.0 initial_y: 2.0 initial_z: 0.0 initial_yaw: 1.57 # radians Expected Log Output [INFO] [localizer_node]: [AUTO-RELOC] Enabled with map: /path/to/map.pcd [INFO] [localizer_node]: [AUTO-RELOC] Initial pose: x=1.000, y=2.000, z=0.000, yaw=1.570 ... [INFO] [localizer_node]: [AUTO-RELOC] Triggering relocalization... [INFO] [localizer_node]: [AUTO-RELOC] Map: /path/to/map.pcd [INFO] [localizer_node]: [AUTO-RELOC] Map loaded successfully [INFO] [localizer_node]: [AUTO-RELOC] Initial guess set, waiting for localization... [INFO] [localizer_node]: [RELOCALIZE] SUCCESS - Localization converged Usage: FASTLIO2 only (SLAM): ros2 launch fastlio2 lio_launch.py FASTLIO2 + Localizer (with relocalization): ros2 launch localizer localizer_launch.py Both launch files include rviz2 automatically. --- src/FASTLIO2_ROS2/README.md | 112 ++ src/FASTLIO2_ROS2/fastlio2/CMakeLists.txt | 80 + src/FASTLIO2_ROS2/fastlio2/LICENSE | 17 + src/FASTLIO2_ROS2/fastlio2/config/lio.yaml | 34 + .../fastlio2/launch/lio_launch.py | 37 + src/FASTLIO2_ROS2/fastlio2/package.xml | 29 + src/FASTLIO2_ROS2/fastlio2/rviz/fastlio2.rviz | 248 +++ src/FASTLIO2_ROS2/fastlio2/src/lio_node.cpp | 322 +++ .../fastlio2/src/map_builder/commons.cpp | 34 + .../fastlio2/src/map_builder/commons.h | 86 + .../fastlio2/src/map_builder/ieskf.cpp | 120 ++ .../fastlio2/src/map_builder/ieskf.h | 84 + .../fastlio2/src/map_builder/ikd_Tree.cpp | 1732 +++++++++++++++++ .../fastlio2/src/map_builder/ikd_Tree.h | 345 ++++ .../src/map_builder/imu_processor.cpp | 132 ++ .../fastlio2/src/map_builder/imu_processor.h | 24 + .../src/map_builder/lidar_processor.cpp | 268 +++ .../src/map_builder/lidar_processor.h | 48 + .../fastlio2/src/map_builder/map_builder.cpp | 29 + .../fastlio2/src/map_builder/map_builder.h | 27 + src/FASTLIO2_ROS2/fastlio2/src/utils.cpp | 39 + src/FASTLIO2_ROS2/fastlio2/src/utils.h | 29 + src/FASTLIO2_ROS2/hba/CMakeLists.txt | 68 + src/FASTLIO2_ROS2/hba/LICENSE | 17 + src/FASTLIO2_ROS2/hba/config/hba.yaml | 11 + src/FASTLIO2_ROS2/hba/launch/hba_launch.py | 32 + src/FASTLIO2_ROS2/hba/package.xml | 25 + src/FASTLIO2_ROS2/hba/rviz/hba.rviz | 170 ++ src/FASTLIO2_ROS2/hba/src/hba/blam.cpp | 458 +++++ src/FASTLIO2_ROS2/hba/src/hba/blam.h | 140 ++ src/FASTLIO2_ROS2/hba/src/hba/commons.cpp | 1 + src/FASTLIO2_ROS2/hba/src/hba/commons.h | 38 + src/FASTLIO2_ROS2/hba/src/hba/hba.cpp | 174 ++ src/FASTLIO2_ROS2/hba/src/hba/hba.h | 57 + src/FASTLIO2_ROS2/hba/src/hba_node copy.cpp | 126 ++ src/FASTLIO2_ROS2/hba/src/hba_node.cpp | 222 +++ src/FASTLIO2_ROS2/interface/CMakeLists.txt | 36 + src/FASTLIO2_ROS2/interface/LICENSE | 17 + src/FASTLIO2_ROS2/interface/package.xml | 24 + src/FASTLIO2_ROS2/interface/srv/IsValid.srv | 3 + src/FASTLIO2_ROS2/interface/srv/RefineMap.srv | 4 + .../interface/srv/Relocalize.srv | 10 + src/FASTLIO2_ROS2/interface/srv/SaveMaps.srv | 5 + src/FASTLIO2_ROS2/interface/srv/SavePoses.srv | 4 + src/FASTLIO2_ROS2/localizer/CMakeLists.txt | 65 + src/FASTLIO2_ROS2/localizer/LICENSE | 17 + .../localizer/config/localizer.yaml | 25 + .../localizer/launch/localizer_launch.py | 53 + src/FASTLIO2_ROS2/localizer/package.xml | 27 + .../localizer/rviz/localizer.rviz | 219 +++ .../localizer/src/localizer_node.cpp | 411 ++++ .../localizer/src/localizers/commons.cpp | 1 + .../localizer/src/localizers/commons.h | 15 + .../src/localizers/icp_localizer.cpp | 87 + .../localizer/src/localizers/icp_localizer.h | 46 + src/FASTLIO2_ROS2/pgo/CMakeLists.txt | 68 + src/FASTLIO2_ROS2/pgo/LICENSE | 17 + src/FASTLIO2_ROS2/pgo/config/pgo.yaml | 13 + src/FASTLIO2_ROS2/pgo/launch/pgo_launch.py | 46 + src/FASTLIO2_ROS2/pgo/package.xml | 30 + src/FASTLIO2_ROS2/pgo/rviz/pgo.rviz | 258 +++ src/FASTLIO2_ROS2/pgo/src/pgo_node.cpp | 307 +++ src/FASTLIO2_ROS2/pgo/src/pgos/commons.cpp | 8 + src/FASTLIO2_ROS2/pgo/src/pgos/commons.h | 30 + src/FASTLIO2_ROS2/pgo/src/pgos/simple_pgo.cpp | 211 ++ src/FASTLIO2_ROS2/pgo/src/pgos/simple_pgo.h | 78 + 66 files changed, 7550 insertions(+) create mode 100644 src/FASTLIO2_ROS2/README.md create mode 100644 src/FASTLIO2_ROS2/fastlio2/CMakeLists.txt create mode 100644 src/FASTLIO2_ROS2/fastlio2/LICENSE create mode 100644 src/FASTLIO2_ROS2/fastlio2/config/lio.yaml create mode 100644 src/FASTLIO2_ROS2/fastlio2/launch/lio_launch.py create mode 100644 src/FASTLIO2_ROS2/fastlio2/package.xml create mode 100644 src/FASTLIO2_ROS2/fastlio2/rviz/fastlio2.rviz create mode 100644 src/FASTLIO2_ROS2/fastlio2/src/lio_node.cpp create mode 100644 src/FASTLIO2_ROS2/fastlio2/src/map_builder/commons.cpp create mode 100644 src/FASTLIO2_ROS2/fastlio2/src/map_builder/commons.h create mode 100644 src/FASTLIO2_ROS2/fastlio2/src/map_builder/ieskf.cpp create mode 100644 src/FASTLIO2_ROS2/fastlio2/src/map_builder/ieskf.h create mode 100644 src/FASTLIO2_ROS2/fastlio2/src/map_builder/ikd_Tree.cpp create mode 100644 src/FASTLIO2_ROS2/fastlio2/src/map_builder/ikd_Tree.h create mode 100644 src/FASTLIO2_ROS2/fastlio2/src/map_builder/imu_processor.cpp create mode 100644 src/FASTLIO2_ROS2/fastlio2/src/map_builder/imu_processor.h create mode 100644 src/FASTLIO2_ROS2/fastlio2/src/map_builder/lidar_processor.cpp create mode 100644 src/FASTLIO2_ROS2/fastlio2/src/map_builder/lidar_processor.h create mode 100644 src/FASTLIO2_ROS2/fastlio2/src/map_builder/map_builder.cpp create mode 100644 src/FASTLIO2_ROS2/fastlio2/src/map_builder/map_builder.h create mode 100644 src/FASTLIO2_ROS2/fastlio2/src/utils.cpp create mode 100644 src/FASTLIO2_ROS2/fastlio2/src/utils.h create mode 100644 src/FASTLIO2_ROS2/hba/CMakeLists.txt create mode 100644 src/FASTLIO2_ROS2/hba/LICENSE create mode 100644 src/FASTLIO2_ROS2/hba/config/hba.yaml create mode 100644 src/FASTLIO2_ROS2/hba/launch/hba_launch.py create mode 100644 src/FASTLIO2_ROS2/hba/package.xml create mode 100644 src/FASTLIO2_ROS2/hba/rviz/hba.rviz create mode 100644 src/FASTLIO2_ROS2/hba/src/hba/blam.cpp create mode 100644 src/FASTLIO2_ROS2/hba/src/hba/blam.h create mode 100644 src/FASTLIO2_ROS2/hba/src/hba/commons.cpp create mode 100644 src/FASTLIO2_ROS2/hba/src/hba/commons.h create mode 100644 src/FASTLIO2_ROS2/hba/src/hba/hba.cpp create mode 100644 src/FASTLIO2_ROS2/hba/src/hba/hba.h create mode 100644 src/FASTLIO2_ROS2/hba/src/hba_node copy.cpp create mode 100644 src/FASTLIO2_ROS2/hba/src/hba_node.cpp create mode 100644 src/FASTLIO2_ROS2/interface/CMakeLists.txt create mode 100644 src/FASTLIO2_ROS2/interface/LICENSE create mode 100644 src/FASTLIO2_ROS2/interface/package.xml create mode 100644 src/FASTLIO2_ROS2/interface/srv/IsValid.srv create mode 100644 src/FASTLIO2_ROS2/interface/srv/RefineMap.srv create mode 100644 src/FASTLIO2_ROS2/interface/srv/Relocalize.srv create mode 100644 src/FASTLIO2_ROS2/interface/srv/SaveMaps.srv create mode 100644 src/FASTLIO2_ROS2/interface/srv/SavePoses.srv create mode 100644 src/FASTLIO2_ROS2/localizer/CMakeLists.txt create mode 100644 src/FASTLIO2_ROS2/localizer/LICENSE create mode 100644 src/FASTLIO2_ROS2/localizer/config/localizer.yaml create mode 100644 src/FASTLIO2_ROS2/localizer/launch/localizer_launch.py create mode 100644 src/FASTLIO2_ROS2/localizer/package.xml create mode 100644 src/FASTLIO2_ROS2/localizer/rviz/localizer.rviz create mode 100644 src/FASTLIO2_ROS2/localizer/src/localizer_node.cpp create mode 100644 src/FASTLIO2_ROS2/localizer/src/localizers/commons.cpp create mode 100644 src/FASTLIO2_ROS2/localizer/src/localizers/commons.h create mode 100644 src/FASTLIO2_ROS2/localizer/src/localizers/icp_localizer.cpp create mode 100644 src/FASTLIO2_ROS2/localizer/src/localizers/icp_localizer.h create mode 100644 src/FASTLIO2_ROS2/pgo/CMakeLists.txt create mode 100644 src/FASTLIO2_ROS2/pgo/LICENSE create mode 100644 src/FASTLIO2_ROS2/pgo/config/pgo.yaml create mode 100644 src/FASTLIO2_ROS2/pgo/launch/pgo_launch.py create mode 100644 src/FASTLIO2_ROS2/pgo/package.xml create mode 100644 src/FASTLIO2_ROS2/pgo/rviz/pgo.rviz create mode 100644 src/FASTLIO2_ROS2/pgo/src/pgo_node.cpp create mode 100644 src/FASTLIO2_ROS2/pgo/src/pgos/commons.cpp create mode 100644 src/FASTLIO2_ROS2/pgo/src/pgos/commons.h create mode 100644 src/FASTLIO2_ROS2/pgo/src/pgos/simple_pgo.cpp create mode 100644 src/FASTLIO2_ROS2/pgo/src/pgos/simple_pgo.h diff --git a/src/FASTLIO2_ROS2/README.md b/src/FASTLIO2_ROS2/README.md new file mode 100644 index 00000000..379ec385 --- /dev/null +++ b/src/FASTLIO2_ROS2/README.md @@ -0,0 +1,112 @@ +# FASTLIO2 ROS2 +## 主要工作 +1. 重构[FASTLIO2](https://github.com/hku-mars/FAST_LIO) 适配ROS2 +2. 添加回环节点，基于位置先验+ICP进行回环检测，基于GTSAM进行位姿图优化 +3. 添加重定位节点，基于由粗到细两阶段ICP进行重定位 +4. 增加一致性地图优化，基于[BLAM](https://github.com/hku-mars/BALM) (小场景地图) 和[HBA](https://github.com/hku-mars/HBA) (大场景地图) + +## 环境依赖 +1. Ubuntu 22.04 +2. ROS2 Humble + +## 编译依赖 +```text +pcl +Eigen +sophus +gtsam +livox_ros_driver2 +``` + +## 详细说明 +### 1.编译 LIVOX-SDK2 +```shell +git clone https://github.com/Livox-SDK/Livox-SDK2.git +cd ./Livox-SDK2/ +mkdir build +cd build +cmake .. && make -j +sudo make install +``` + +### 2.编译 livox_ros_driver2 +```shell +mkdir -r ws_livox/src +git clone https://github.com/Livox-SDK/livox_ros_driver2.git ws_livox/src/livox_ros_driver2 +cd ws_livox/src/livox_ros_driver2 +source /opt/ros/humble/setup.sh +./build.sh humble +``` + +### 3.编译 Sophus +```shell +git clone https://github.com/strasdat/Sophus.git +cd Sophus +git checkout 1.22.10 +mkdir build && cd build +cmake .. -DSOPHUS_USE_BASIC_LOGGING=ON +make +sudo make install +``` + +**新的Sophus依赖fmt，可以在CMakeLists.txt中添加add_compile_definitions(SOPHUS_USE_BASIC_LOGGING)去除，否则会报错** + + +## 实例数据集 +```text +链接: https://pan.baidu.com/s/1rTTUlVwxi1ZNo7ZmcpEZ7A?pwd=t6yb 提取码: t6yb +--来自百度网盘超级会员v7的分享 +``` + +## 部分脚本 + +### 1.激光惯性里程计 +```shell +ros2 launch fastlio2 lio_launch.py +ros2 bag play your_bag_file +``` + +### 2.里程计加回环 +#### 启动回环节点 +```shell +ros2 launch pgo pgo_launch.py +ros2 bag play your_bag_file +``` +#### 保存地图 +```shell +ros2 service call /pgo/save_maps interface/srv/SaveMaps "{file_path: 'your_save_dir', save_patches: true}" +``` + +### 3.里程计加重定位 +#### 启动重定位节点 +```shell +ros2 launch localizer localizer_launch.py +ros2 bag play your_bag_file // 可选 +``` +#### 设置重定位初始值 +```shell +ros2 service call /localizer/relocalize interface/srv/Relocalize "{"pcd_path": "your_map.pcd", "x": 0.0, "y": 0.0, "z": 0.0, "yaw": 0.0, "pitch": 0.0, "roll": 0.0}" +``` +#### 检查重定位结果 +```shell +ros2 service call /localizer/relocalize_check interface/srv/IsValid "{"code": 0}" +``` + +### 4.一致性地图优化 +#### 启动一致性地图优化节点 +```shell +ros2 launch hba hba_launch.py +``` +#### 调用优化服务 +```shell +ros2 service call /hba/refine_map interface/srv/RefineMap "{"maps_path": "your maps directory"}" +``` +**如果需要调用优化服务，保存地图时需要设置save_patches为true** + +## 特别感谢 +1. [FASTLIO2](https://github.com/hku-mars/FAST_LIO) +2. [BLAM](https://github.com/hku-mars/BALM) +3. [HBA](https://github.com/hku-mars/HBA) +## 性能相关的问题 +该代码主要使用timerCB作为频率触发主函数，由于ROS2中的timer、subscriber以及service的回调实际上运行在同一个线程上，在电脑性能不是好的时候，会出现调用阻塞的情况，建议使用线程并发的方式将耗时的回调独立出来(如timerCB)来提升性能 + diff --git a/src/FASTLIO2_ROS2/fastlio2/CMakeLists.txt b/src/FASTLIO2_ROS2/fastlio2/CMakeLists.txt new file mode 100644 index 00000000..7fd37a73 --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/CMakeLists.txt @@ -0,0 +1,80 @@ +cmake_minimum_required(VERSION 3.8) +project(fastlio2) + +set(CMAKE_BUILD_TYPE "Release") +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED True) + +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" ) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread -fexceptions") + + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-O3) + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +add_compile_definitions(SOPHUS_USE_BASIC_LOGGING) + +add_definitions(-DMP_EN) +add_definitions(-DMP_PROC_NUM=2) + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(livox_ros_driver2 REQUIRED) +find_package(pcl_conversions REQUIRED) +find_package(geometry_msgs REQUIRED) + +find_package(OpenMP QUIET) +find_package(PCL REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(Sophus REQUIRED) +find_package(yaml-cpp REQUIRED) + + +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") + +set(SRC_LIST src/map_builder/commons.cpp + src/map_builder/ieskf.cpp + src/map_builder/imu_processor.cpp + src/map_builder/ikd_Tree.cpp + src/map_builder/lidar_processor.cpp + src/map_builder/map_builder.cpp + src/utils.cpp) + +add_executable(lio_node src/lio_node.cpp ${SRC_LIST}) +ament_target_dependencies(lio_node rclcpp std_msgs tf2 tf2_ros nav_msgs sensor_msgs livox_ros_driver2 pcl_conversions geometry_msgs) +target_link_libraries(lio_node + yaml-cpp + ${PCL_LIBRARIES} +) + + +install(TARGETS lio_node DESTINATION lib/${PROJECT_NAME}) +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) + + + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/FASTLIO2_ROS2/fastlio2/LICENSE b/src/FASTLIO2_ROS2/fastlio2/LICENSE new file mode 100644 index 00000000..30e8e2ec --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/LICENSE @@ -0,0 +1,17 @@ +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 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/src/FASTLIO2_ROS2/fastlio2/config/lio.yaml b/src/FASTLIO2_ROS2/fastlio2/config/lio.yaml new file mode 100644 index 00000000..5d77ae3c --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/config/lio.yaml @@ -0,0 +1,34 @@ +imu_topic: /livox/imu +lidar_topic: /livox/lidar +body_frame: body +world_frame: lidar +print_time_cost: false +print_odom: true +odom_log_interval: 0.5 # seconds between odom logs (2Hz) + +lidar_filter_num: 6 +lidar_min_range: 0.5 +lidar_max_range: 30.0 +scan_resolution: 0.15 +map_resolution: 0.3 + +cube_len: 300 +det_range: 60 +move_thresh: 1.5 + +na: 0.01 +ng: 0.01 +nba: 0.0001 +nbg: 0.0001 + +imu_init_num: 20 +near_search_num: 5 +ieskf_max_iter: 5 + +gravity_align: true +esti_il: false + +r_il: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] +t_il: [-0.011, -0.02329, 0.04412] + +lidar_cov_inv: 1000.0 diff --git a/src/FASTLIO2_ROS2/fastlio2/launch/lio_launch.py b/src/FASTLIO2_ROS2/fastlio2/launch/lio_launch.py new file mode 100644 index 00000000..754b5818 --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/launch/lio_launch.py @@ -0,0 +1,37 @@ +import launch +import launch_ros.actions +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + + rviz_cfg = PathJoinSubstitution( + [FindPackageShare("fastlio2"), "rviz", "fastlio2.rviz"] + ) + + config_path = PathJoinSubstitution( + [FindPackageShare("fastlio2"), "config", "lio.yaml"] + ) + + + return launch.LaunchDescription( + [ + launch_ros.actions.Node( + package="fastlio2", + namespace="fastlio2", + executable="lio_node", + name="lio_node", + output="screen", + parameters=[{"config_path": config_path.perform(launch.LaunchContext())}] + ), + launch_ros.actions.Node( + package="rviz2", + namespace="fastlio2", + executable="rviz2", + name="rviz2", + output="screen", + arguments=["-d", rviz_cfg.perform(launch.LaunchContext())], + ), + ] + ) diff --git a/src/FASTLIO2_ROS2/fastlio2/package.xml b/src/FASTLIO2_ROS2/fastlio2/package.xml new file mode 100644 index 00000000..4bd70f57 --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/package.xml @@ -0,0 +1,29 @@ + + + + fastlio2 + 0.0.0 + TODO: Package description + zhouzhou + MIT + + ament_cmake + + rclcpp + std_msgs + tf2 + tf2_ros + nav_msgs + sensor_msgs + pcl_conversions + livox_ros_driver2 + geometry_msgs + + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/FASTLIO2_ROS2/fastlio2/rviz/fastlio2.rviz b/src/FASTLIO2_ROS2/fastlio2/rviz/fastlio2.rviz new file mode 100644 index 00000000..0b3e4a93 --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/rviz/fastlio2.rviz @@ -0,0 +1,248 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PointCloud21 + - /PointCloud22 + - /Axes1 + - /Axes2 + - /Path1 + Splitter Ratio: 0.5 + Tree Height: 1079 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 237 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 2 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /fastlio2/body_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 0.10000000149011612 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 200 + Enabled: true + Invert Rainbow: true + Max Color: 255; 255; 255 + Max Intensity: 255 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 1 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /fastlio2/world_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: lidar + Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 0.5 + Name: Axes + Radius: 0.05000000074505806 + Reference Frame: body + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 255; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /fastlio2/lio_path + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: lidar + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 7.265907287597656 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.8947967290878296 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.220390319824219 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1376 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000004c2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004c2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004c2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000004c2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009ba0000003efc0100000002fb0000000800540069006d00650100000000000009ba000002fb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000749000004c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2490 + X: 70 + Y: 27 diff --git a/src/FASTLIO2_ROS2/fastlio2/src/lio_node.cpp b/src/FASTLIO2_ROS2/fastlio2/src/lio_node.cpp new file mode 100644 index 00000000..ef006ffa --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/src/lio_node.cpp @@ -0,0 +1,322 @@ +#include +#include +#include +#include +#include +#include +// #include +#include +#include +#include + +#include "utils.h" +#include "map_builder/commons.h" +#include "map_builder/map_builder.h" + +#include +#include "tf2_ros/transform_broadcaster.h" +#include +#include +#include +#include +#include + +using namespace std::chrono_literals; +struct NodeConfig +{ + std::string imu_topic = "/livox/imu"; + std::string lidar_topic = "/livox/lidar"; + std::string body_frame = "body"; + std::string world_frame = "lidar"; + bool print_time_cost = false; + bool print_odom = false; + double odom_log_interval = 5.0; // seconds between odom logs +}; +struct StateData +{ + bool lidar_pushed = false; + std::mutex imu_mutex; + std::mutex lidar_mutex; + double last_lidar_time = -1.0; + double last_imu_time = -1.0; + double last_odom_log_time = -1.0; + std::deque imu_buffer; + std::deque::Ptr>> lidar_buffer; + nav_msgs::msg::Path path; +}; + +class LIONode : public rclcpp::Node +{ +public: + LIONode() : Node("lio_node") + { + RCLCPP_INFO(this->get_logger(), "LIO Node Started"); + loadParameters(); + + m_imu_sub = this->create_subscription(m_node_config.imu_topic, 10, std::bind(&LIONode::imuCB, this, std::placeholders::_1)); + m_lidar_sub = this->create_subscription(m_node_config.lidar_topic, 10, std::bind(&LIONode::lidarCB, this, std::placeholders::_1)); + + m_body_cloud_pub = this->create_publisher("body_cloud", 10000); + m_world_cloud_pub = this->create_publisher("world_cloud", 10000); + m_path_pub = this->create_publisher("lio_path", 10000); + m_odom_pub = this->create_publisher("lio_odom", 10000); + m_tf_broadcaster = std::make_shared(*this); + + m_state_data.path.poses.clear(); + m_state_data.path.header.frame_id = m_node_config.world_frame; + + m_kf = std::make_shared(); + m_builder = std::make_shared(m_builder_config, m_kf); + m_timer = this->create_wall_timer(20ms, std::bind(&LIONode::timerCB, this)); + } + + void loadParameters() + { + this->declare_parameter("config_path", ""); + std::string config_path; + this->get_parameter("config_path", config_path); + + YAML::Node config = YAML::LoadFile(config_path); + if (!config) + { + RCLCPP_WARN(this->get_logger(), "FAIL TO LOAD YAML FILE!"); + return; + } + + RCLCPP_INFO(this->get_logger(), "LOAD FROM YAML CONFIG PATH: %s", config_path.c_str()); + + m_node_config.imu_topic = config["imu_topic"].as(); + m_node_config.lidar_topic = config["lidar_topic"].as(); + m_node_config.body_frame = config["body_frame"].as(); + m_node_config.world_frame = config["world_frame"].as(); + m_node_config.print_time_cost = config["print_time_cost"].as(); + // Optional: load odom logging config with defaults + if (config["print_odom"]) + m_node_config.print_odom = config["print_odom"].as(); + if (config["odom_log_interval"]) + m_node_config.odom_log_interval = config["odom_log_interval"].as(); + + m_builder_config.lidar_filter_num = config["lidar_filter_num"].as(); + m_builder_config.lidar_min_range = config["lidar_min_range"].as(); + m_builder_config.lidar_max_range = config["lidar_max_range"].as(); + m_builder_config.scan_resolution = config["scan_resolution"].as(); + m_builder_config.map_resolution = config["map_resolution"].as(); + m_builder_config.cube_len = config["cube_len"].as(); + m_builder_config.det_range = config["det_range"].as(); + m_builder_config.move_thresh = config["move_thresh"].as(); + m_builder_config.na = config["na"].as(); + m_builder_config.ng = config["ng"].as(); + m_builder_config.nba = config["nba"].as(); + m_builder_config.nbg = config["nbg"].as(); + + m_builder_config.imu_init_num = config["imu_init_num"].as(); + m_builder_config.near_search_num = config["near_search_num"].as(); + m_builder_config.ieskf_max_iter = config["ieskf_max_iter"].as(); + m_builder_config.gravity_align = config["gravity_align"].as(); + m_builder_config.esti_il = config["esti_il"].as(); + std::vector t_il_vec = config["t_il"].as>(); + std::vector r_il_vec = config["r_il"].as>(); + m_builder_config.t_il << t_il_vec[0], t_il_vec[1], t_il_vec[2]; + m_builder_config.r_il << r_il_vec[0], r_il_vec[1], r_il_vec[2], r_il_vec[3], r_il_vec[4], r_il_vec[5], r_il_vec[6], r_il_vec[7], r_il_vec[8]; + m_builder_config.lidar_cov_inv = config["lidar_cov_inv"].as(); + } + + void imuCB(const sensor_msgs::msg::Imu::SharedPtr msg) + { + std::lock_guard lock(m_state_data.imu_mutex); + double timestamp = Utils::getSec(msg->header); + if (timestamp < m_state_data.last_imu_time) + { + RCLCPP_WARN(this->get_logger(), "IMU Message is out of order"); + std::deque().swap(m_state_data.imu_buffer); + } + m_state_data.imu_buffer.emplace_back(V3D(msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z) * 10.0, + V3D(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z), + timestamp); + m_state_data.last_imu_time = timestamp; + } + void lidarCB(const livox_ros_driver2::msg::CustomMsg::SharedPtr msg) + { + CloudType::Ptr cloud = Utils::livox2PCL(msg, m_builder_config.lidar_filter_num, m_builder_config.lidar_min_range, m_builder_config.lidar_max_range); + std::lock_guard lock(m_state_data.lidar_mutex); + double timestamp = Utils::getSec(msg->header); + if (timestamp < m_state_data.last_lidar_time) + { + RCLCPP_WARN(this->get_logger(), "Lidar Message is out of order"); + std::deque::Ptr>>().swap(m_state_data.lidar_buffer); + } + m_state_data.lidar_buffer.emplace_back(timestamp, cloud); + m_state_data.last_lidar_time = timestamp; + } + + bool syncPackage() + { + if (m_state_data.imu_buffer.empty() || m_state_data.lidar_buffer.empty()) + return false; + if (!m_state_data.lidar_pushed) + { + m_package.cloud = m_state_data.lidar_buffer.front().second; + std::sort(m_package.cloud->points.begin(), m_package.cloud->points.end(), [](PointType &p1, PointType &p2) + { return p1.curvature < p2.curvature; }); + m_package.cloud_start_time = m_state_data.lidar_buffer.front().first; + m_package.cloud_end_time = m_package.cloud_start_time + m_package.cloud->points.back().curvature / 1000.0; + m_state_data.lidar_pushed = true; + } + if (m_state_data.last_imu_time < m_package.cloud_end_time) + return false; + + Vec().swap(m_package.imus); + while (!m_state_data.imu_buffer.empty() && m_state_data.imu_buffer.front().time < m_package.cloud_end_time) + { + m_package.imus.emplace_back(m_state_data.imu_buffer.front()); + m_state_data.imu_buffer.pop_front(); + } + m_state_data.lidar_buffer.pop_front(); + m_state_data.lidar_pushed = false; + return true; + } + + void publishCloud(rclcpp::Publisher::SharedPtr pub, CloudType::Ptr cloud, std::string frame_id, const double &time) + { + if (pub->get_subscription_count() <= 0) + return; + sensor_msgs::msg::PointCloud2 cloud_msg; + pcl::toROSMsg(*cloud, cloud_msg); + cloud_msg.header.frame_id = frame_id; + cloud_msg.header.stamp = Utils::getTime(time); + pub->publish(cloud_msg); + } + + void publishOdometry(rclcpp::Publisher::SharedPtr odom_pub, std::string frame_id, std::string child_frame, const double &time) + { + nav_msgs::msg::Odometry odom; + odom.header.frame_id = frame_id; + odom.header.stamp = Utils::getTime(time); + odom.child_frame_id = child_frame; + odom.pose.pose.position.x = m_kf->x().t_wi.x(); + odom.pose.pose.position.y = m_kf->x().t_wi.y(); + odom.pose.pose.position.z = m_kf->x().t_wi.z(); + Eigen::Quaterniond q(m_kf->x().r_wi); + odom.pose.pose.orientation.x = q.x(); + odom.pose.pose.orientation.y = q.y(); + odom.pose.pose.orientation.z = q.z(); + odom.pose.pose.orientation.w = q.w(); + + V3D vel = m_kf->x().r_wi.transpose() * m_kf->x().v; + odom.twist.twist.linear.x = vel.x(); + odom.twist.twist.linear.y = vel.y(); + odom.twist.twist.linear.z = vel.z(); + + // Throttled odometry logging (logs at odom_log_interval seconds) + bool should_log = (m_state_data.last_odom_log_time < 0) || // First time + ((time - m_state_data.last_odom_log_time) >= m_node_config.odom_log_interval); + if (m_node_config.print_odom && should_log) + { + m_state_data.last_odom_log_time = time; + double yaw = std::atan2(2.0 * (q.w() * q.z() + q.x() * q.y()), + 1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z())); + double speed = std::sqrt(vel.x() * vel.x() + vel.y() * vel.y() + vel.z() * vel.z()); + RCLCPP_INFO(this->get_logger(), "[ODOM] pos: x=%.3f, y=%.3f, z=%.3f | yaw=%.3f rad (%.1f deg) | speed=%.3f m/s", + m_kf->x().t_wi.x(), m_kf->x().t_wi.y(), m_kf->x().t_wi.z(), + yaw, yaw * 180.0 / M_PI, speed); + } + + if (odom_pub->get_subscription_count() > 0) + odom_pub->publish(odom); + } + + void publishPath(rclcpp::Publisher::SharedPtr path_pub, std::string frame_id, const double &time) + { + if (path_pub->get_subscription_count() <= 0) + return; + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = frame_id; + pose.header.stamp = Utils::getTime(time); + pose.pose.position.x = m_kf->x().t_wi.x(); + pose.pose.position.y = m_kf->x().t_wi.y(); + pose.pose.position.z = m_kf->x().t_wi.z(); + Eigen::Quaterniond q(m_kf->x().r_wi); + pose.pose.orientation.x = q.x(); + pose.pose.orientation.y = q.y(); + pose.pose.orientation.z = q.z(); + pose.pose.orientation.w = q.w(); + m_state_data.path.poses.push_back(pose); + path_pub->publish(m_state_data.path); + } + + void broadCastTF(std::shared_ptr broad_caster, std::string frame_id, std::string child_frame, const double &time) + { + geometry_msgs::msg::TransformStamped transformStamped; + transformStamped.header.frame_id = frame_id; + transformStamped.child_frame_id = child_frame; + transformStamped.header.stamp = Utils::getTime(time); + Eigen::Quaterniond q(m_kf->x().r_wi); + V3D t = m_kf->x().t_wi; + transformStamped.transform.translation.x = t.x(); + transformStamped.transform.translation.y = t.y(); + transformStamped.transform.translation.z = t.z(); + transformStamped.transform.rotation.x = q.x(); + transformStamped.transform.rotation.y = q.y(); + transformStamped.transform.rotation.z = q.z(); + transformStamped.transform.rotation.w = q.w(); + broad_caster->sendTransform(transformStamped); + } + + void timerCB() + { + if (!syncPackage()) + return; + auto t1 = std::chrono::high_resolution_clock::now(); + m_builder->process(m_package); + auto t2 = std::chrono::high_resolution_clock::now(); + + if (m_node_config.print_time_cost) + { + auto time_used = std::chrono::duration_cast>(t2 - t1).count() * 1000; + RCLCPP_WARN(this->get_logger(), "Time cost: %.2f ms", time_used); + } + + if (m_builder->status() != BuilderStatus::MAPPING) + return; + + broadCastTF(m_tf_broadcaster, m_node_config.world_frame, m_node_config.body_frame, m_package.cloud_end_time); + + publishOdometry(m_odom_pub, m_node_config.world_frame, m_node_config.body_frame, m_package.cloud_end_time); + + CloudType::Ptr body_cloud = m_builder->lidar_processor()->transformCloud(m_package.cloud, m_kf->x().r_il, m_kf->x().t_il); + + publishCloud(m_body_cloud_pub, body_cloud, m_node_config.body_frame, m_package.cloud_end_time); + + CloudType::Ptr world_cloud = m_builder->lidar_processor()->transformCloud(m_package.cloud, m_builder->lidar_processor()->r_wl(), m_builder->lidar_processor()->t_wl()); + + publishCloud(m_world_cloud_pub, world_cloud, m_node_config.world_frame, m_package.cloud_end_time); + + publishPath(m_path_pub, m_node_config.world_frame, m_package.cloud_end_time); + } + +private: + rclcpp::Subscription::SharedPtr m_lidar_sub; + rclcpp::Subscription::SharedPtr m_imu_sub; + + rclcpp::Publisher::SharedPtr m_body_cloud_pub; + rclcpp::Publisher::SharedPtr m_world_cloud_pub; + rclcpp::Publisher::SharedPtr m_path_pub; + rclcpp::Publisher::SharedPtr m_odom_pub; + + rclcpp::TimerBase::SharedPtr m_timer; + StateData m_state_data; + SyncPackage m_package; + NodeConfig m_node_config; + Config m_builder_config; + std::shared_ptr m_kf; + std::shared_ptr m_builder; + std::shared_ptr m_tf_broadcaster; +}; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/fastlio2/src/map_builder/commons.cpp b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/commons.cpp new file mode 100644 index 00000000..6625b0c4 --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/commons.cpp @@ -0,0 +1,34 @@ +#include "commons.h" +bool esti_plane(PointVec &points, const double &thresh, V4D &out) +{ + Eigen::MatrixXd A(points.size(), 3); + Eigen::MatrixXd b(points.size(), 1); + A.setZero(); + b.setOnes(); + b *= -1.0; + for (size_t i = 0; i < points.size(); i++) + { + A(i, 0) = points[i].x; + A(i, 1) = points[i].y; + A(i, 2) = points[i].z; + } + V3D normvec = A.colPivHouseholderQr().solve(b); + double norm = normvec.norm(); + out[0] = normvec(0) / norm; + out[1] = normvec(1) / norm; + out[2] = normvec(2) / norm; + out[3] = 1.0 / norm; + for (size_t j = 0; j < points.size(); j++) + { + if (std::fabs(out(0) * points[j].x + out(1) * points[j].y + out(2) * points[j].z + out(3)) > thresh) + { + return false; + } + } + return true; +} + +float sq_dist(const PointType &p1, const PointType &p2) +{ + return (p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z); +} \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/fastlio2/src/map_builder/commons.h b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/commons.h new file mode 100644 index 00000000..419f2d59 --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/commons.h @@ -0,0 +1,86 @@ +#pragma once +#include +#include +#include + +using PointType = pcl::PointXYZINormal; +using CloudType = pcl::PointCloud; +using PointVec = std::vector>; + +using M3D = Eigen::Matrix3d; +using V3D = Eigen::Vector3d; +using M3F = Eigen::Matrix3f; +using V3F = Eigen::Vector3f; +using M2D = Eigen::Matrix2d; +using V2D = Eigen::Vector2d; +using M2F = Eigen::Matrix2f; +using V2F = Eigen::Vector2f; +using M4D = Eigen::Matrix4d; +using V4D = Eigen::Vector4d; + + +template +using Vec = std::vector; + + +bool esti_plane(PointVec &points, const double &thresh, V4D &out); + +float sq_dist(const PointType &p1, const PointType &p2); + +struct Config +{ + int lidar_filter_num = 3; + double lidar_min_range = 0.5; + double lidar_max_range = 20.0; + double scan_resolution = 0.15; + double map_resolution = 0.3; + + double cube_len = 300; + double det_range = 60; + double move_thresh = 1.5; + + double na = 0.01; + double ng = 0.01; + double nba = 0.0001; + double nbg = 0.0001; + int imu_init_num = 20; + int near_search_num = 5; + int ieskf_max_iter = 5; + bool gravity_align = true; + bool esti_il = false; + M3D r_il = M3D::Identity(); + V3D t_il = V3D::Zero(); + + double lidar_cov_inv = 1000.0; +}; + +struct IMUData +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + V3D acc; + V3D gyro; + double time; + IMUData() = default; + IMUData(const V3D &a, const V3D &g, double &t) : acc(a), gyro(g), time(t) {} +}; + +struct Pose +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + double offset; + V3D acc; + V3D gyro; + V3D vel; + V3D trans; + M3D rot; + Pose() = default; + Pose(double t, const V3D &a, const V3D &g, const V3D &v, const V3D &p, const M3D &r) : offset(t), acc(a), gyro(g), vel(v), trans(p), rot(r) {} +}; + +struct SyncPackage +{ + Vec imus; + CloudType::Ptr cloud; + double cloud_start_time = 0.0; + double cloud_end_time = 0.0; +}; \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/fastlio2/src/map_builder/ieskf.cpp b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/ieskf.cpp new file mode 100644 index 00000000..53b5ef95 --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/ieskf.cpp @@ -0,0 +1,120 @@ +#include "ieskf.h" + +double State::gravity = 9.81; + +M3D Jr(const V3D &inp) +{ + return Sophus::SO3d::leftJacobian(inp).transpose(); +} +M3D JrInv(const V3D &inp) +{ + return Sophus::SO3d::leftJacobianInverse(inp).transpose(); +} + +void State::operator+=(const V21D &delta) +{ + r_wi *= Sophus::SO3d::exp(delta.segment<3>(0)).matrix(); + t_wi += delta.segment<3>(3); + r_il *= Sophus::SO3d::exp(delta.segment<3>(6)).matrix(); + t_il += delta.segment<3>(9); + v += delta.segment<3>(12); + bg += delta.segment<3>(15); + ba += delta.segment<3>(18); +} + +V21D State::operator-(const State &other) const +{ + V21D delta = V21D::Zero(); + delta.segment<3>(0) = Sophus::SO3d(other.r_wi.transpose() * r_wi).log(); + delta.segment<3>(3) = t_wi - other.t_wi; + delta.segment<3>(6) = Sophus::SO3d(other.r_il.transpose() * r_il).log(); + delta.segment<3>(9) = t_il - other.t_il; + delta.segment<3>(12) = v - other.v; + delta.segment<3>(15) = bg - other.bg; + delta.segment<3>(18) = ba - other.ba; + return delta; +} + +std::ostream &operator<<(std::ostream &os, const State &state) +{ + os << "==============START===============" << std::endl; + os << "r_wi: " << state.r_wi.eulerAngles(2, 1, 0).transpose() << std::endl; + os << "t_il: " << state.t_il.transpose() << std::endl; + os << "r_il: " << state.r_il.eulerAngles(2, 1, 0).transpose() << std::endl; + os << "t_wi: " << state.t_wi.transpose() << std::endl; + os << "v: " << state.v.transpose() << std::endl; + os << "bg: " << state.bg.transpose() << std::endl; + os << "ba: " << state.ba.transpose() << std::endl; + os << "g: " << state.g.transpose() << std::endl; + os << "===============END================" << std::endl; + + return os; +} + +void IESKF::predict(const Input &inp, double dt, const M12D &Q) +{ + V21D delta = V21D::Zero(); + delta.segment<3>(0) = (inp.gyro - m_x.bg) * dt; + delta.segment<3>(3) = m_x.v * dt; + delta.segment<3>(12) = (m_x.r_wi * (inp.acc - m_x.ba) + m_x.g) * dt; + + m_F.setIdentity(); + m_F.block<3, 3>(0, 0) = Sophus::SO3d::exp(-(inp.gyro - m_x.bg) * dt).matrix(); + m_F.block<3, 3>(0, 15) = -Jr((inp.gyro - m_x.bg) * dt) * dt; + m_F.block<3, 3>(3, 12) = Eigen::Matrix3d::Identity() * dt; + m_F.block<3, 3>(12, 0) = -m_x.r_wi * Sophus::SO3d::hat(inp.acc - m_x.ba) * dt; + m_F.block<3, 3>(12, 18) = -m_x.r_wi * dt; + + m_G.setZero(); + m_G.block<3, 3>(0, 0) = -Jr((inp.gyro - m_x.bg) * dt) * dt; + m_G.block<3, 3>(12, 3) = -m_x.r_wi * dt; + m_G.block<3, 3>(15, 6) = Eigen::Matrix3d::Identity() * dt; + m_G.block<3, 3>(18, 9) = Eigen::Matrix3d::Identity() * dt; + + m_x += delta; + m_P = m_F * m_P * m_F.transpose() + m_G * Q * m_G.transpose(); +} + +void IESKF::update() +{ + State predict_x = m_x; + SharedState shared_data; + shared_data.iter_num = 0; + shared_data.res = 1e10; + V21D delta = V21D::Zero(); + M21D H = M21D::Identity(); + V21D b; + + for (size_t i = 0; i < m_max_iter; i++) + { + m_loss_func(m_x, shared_data); + if (!shared_data.valid) + break; + H.setZero(); + b.setZero(); + delta = m_x - predict_x; + M21D J = M21D::Identity(); + J.block<3, 3>(0, 0) = JrInv(delta.segment<3>(0)); + J.block<3, 3>(6, 6) = JrInv(delta.segment<3>(6)); + H += J.transpose() * m_P.inverse() * J; + b += J.transpose() * m_P.inverse() * delta; + + H.block<12, 12>(0, 0) += shared_data.H; + b.block<12, 1>(0, 0) += shared_data.b; + + delta = -H.inverse() * b; + + m_x += delta; + shared_data.iter_num += 1; + + if (m_stop_func(delta)) + break; + } + + M21D L = M21D::Identity(); + // L.block<3, 3>(0, 0) = JrInv(delta.segment<3>(0)); + // L.block<3, 3>(6, 6) = JrInv(delta.segment<3>(6)); + L.block<3, 3>(0, 0) = Jr(delta.segment<3>(0)); + L.block<3, 3>(6, 6) = Jr(delta.segment<3>(6)); + m_P = L * H.inverse() * L.transpose(); +} \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/fastlio2/src/map_builder/ieskf.h b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/ieskf.h new file mode 100644 index 00000000..b5232cf7 --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/ieskf.h @@ -0,0 +1,84 @@ +#pragma once +#include +#include +#include "commons.h" + +using M12D = Eigen::Matrix; +using M21D = Eigen::Matrix; + +using V12D = Eigen::Matrix; +using V21D = Eigen::Matrix; +using M21X12D = Eigen::Matrix; + +M3D Jr(const V3D &inp); +M3D JrInv(const V3D &inp); + +struct SharedState +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + M12D H; + V12D b; + double res = 1e10; + bool valid = false; + size_t iter_num = 0; +}; +struct Input +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + V3D acc; + V3D gyro; + Input() = default; + Input(V3D &a, V3D &g) : acc(a), gyro(g) {} + Input(double a1, double a2, double a3, double g1, double g2, double g3) : acc(a1, a2, a3), gyro(g1, g2, g3) {} +}; +struct State +{ + static double gravity; + M3D r_wi = M3D::Identity(); + V3D t_wi = V3D::Zero(); + M3D r_il = M3D::Identity(); + V3D t_il = V3D::Zero(); + V3D v = V3D::Zero(); + V3D bg = V3D::Zero(); + V3D ba = V3D::Zero(); + V3D g = V3D(0.0, 0.0, -9.81); + + void initGravityDir(const V3D &gravity_dir) { g = gravity_dir.normalized() * State::gravity; } + + void operator+=(const V21D &delta); + + V21D operator-(const State &other) const; + + friend std::ostream &operator<<(std::ostream &os, const State &state); +}; + +using loss_func = std::function; +using stop_func = std::function; + +class IESKF +{ +public: + IESKF() = default; + void setMaxIter(size_t iter) { m_max_iter = iter; } + void setLossFunction(loss_func func) { m_loss_func = func; } + void setStopFunction(stop_func func) { m_stop_func = func; } + + void predict(const Input &inp, double dt, const M12D &Q); + + void update(); + + State &x() { return m_x; } + + M21D &P() { return m_P; } + +private: + size_t m_max_iter = 10; + State m_x; + M21D m_P; + loss_func m_loss_func; + stop_func m_stop_func; + M21D m_F; + M21X12D m_G; +}; diff --git a/src/FASTLIO2_ROS2/fastlio2/src/map_builder/ikd_Tree.cpp b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/ikd_Tree.cpp new file mode 100644 index 00000000..022414ff --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/ikd_Tree.cpp @@ -0,0 +1,1732 @@ +#include "ikd_Tree.h" + +/* +Description: ikd-Tree: an incremental k-d tree for robotic applications +Author: Yixi Cai +email: yixicai@connect.hku.hk +*/ + +template +KD_TREE::KD_TREE(float delete_param, float balance_param, float box_length) +{ + delete_criterion_param = delete_param; + balance_criterion_param = balance_param; + downsample_size = box_length; + Rebuild_Logger.clear(); + termination_flag = false; + start_thread(); +} + +template +KD_TREE::~KD_TREE() +{ + stop_thread(); + Delete_Storage_Disabled = true; + delete_tree_nodes(&Root_Node); + PointVector().swap(PCL_Storage); + Rebuild_Logger.clear(); +} + +template +void KD_TREE::InitializeKDTree(float delete_param, float balance_param, float box_length) +{ + Set_delete_criterion_param(delete_param); + Set_balance_criterion_param(balance_param); + set_downsample_param(box_length); +} + +template +void KD_TREE::InitTreeNode(KD_TREE_NODE *root) +{ + root->point.x = 0.0f; + root->point.y = 0.0f; + root->point.z = 0.0f; + root->node_range_x[0] = 0.0f; + root->node_range_x[1] = 0.0f; + root->node_range_y[0] = 0.0f; + root->node_range_y[1] = 0.0f; + root->node_range_z[0] = 0.0f; + root->node_range_z[1] = 0.0f; + root->radius_sq = 0.0f; + root->division_axis = 0; + root->father_ptr = nullptr; + root->left_son_ptr = nullptr; + root->right_son_ptr = nullptr; + root->TreeSize = 0; + root->invalid_point_num = 0; + root->down_del_num = 0; + root->point_deleted = false; + root->tree_deleted = false; + root->need_push_down_to_left = false; + root->need_push_down_to_right = false; + root->point_downsample_deleted = false; + root->working_flag = false; + pthread_mutex_init(&(root->push_down_mutex_lock), NULL); +} + +template +int KD_TREE::size() +{ + int s = 0; + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + if (Root_Node != nullptr) + { + return Root_Node->TreeSize; + } + else + { + return 0; + } + } + else + { + if (!pthread_mutex_trylock(&working_flag_mutex)) + { + s = Root_Node->TreeSize; + pthread_mutex_unlock(&working_flag_mutex); + return s; + } + else + { + return Treesize_tmp; + } + } +} + +template +BoxPointType KD_TREE::tree_range() +{ + BoxPointType range; + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + if (Root_Node != nullptr) + { + range.vertex_min[0] = Root_Node->node_range_x[0]; + range.vertex_min[1] = Root_Node->node_range_y[0]; + range.vertex_min[2] = Root_Node->node_range_z[0]; + range.vertex_max[0] = Root_Node->node_range_x[1]; + range.vertex_max[1] = Root_Node->node_range_y[1]; + range.vertex_max[2] = Root_Node->node_range_z[1]; + } + else + { + memset(&range, 0, sizeof(range)); + } + } + else + { + if (!pthread_mutex_trylock(&working_flag_mutex)) + { + range.vertex_min[0] = Root_Node->node_range_x[0]; + range.vertex_min[1] = Root_Node->node_range_y[0]; + range.vertex_min[2] = Root_Node->node_range_z[0]; + range.vertex_max[0] = Root_Node->node_range_x[1]; + range.vertex_max[1] = Root_Node->node_range_y[1]; + range.vertex_max[2] = Root_Node->node_range_z[1]; + pthread_mutex_unlock(&working_flag_mutex); + } + else + { + memset(&range, 0, sizeof(range)); + } + } + return range; +} + +template +int KD_TREE::validnum() +{ + int s = 0; + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + if (Root_Node != nullptr) + return (Root_Node->TreeSize - Root_Node->invalid_point_num); + else + return 0; + } + else + { + if (!pthread_mutex_trylock(&working_flag_mutex)) + { + s = Root_Node->TreeSize - Root_Node->invalid_point_num; + pthread_mutex_unlock(&working_flag_mutex); + return s; + } + else + { + return -1; + } + } +} + +template +void KD_TREE::root_alpha(float &alpha_bal, float &alpha_del) +{ + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + alpha_bal = Root_Node->alpha_bal; + alpha_del = Root_Node->alpha_del; + return; + } + else + { + if (!pthread_mutex_trylock(&working_flag_mutex)) + { + alpha_bal = Root_Node->alpha_bal; + alpha_del = Root_Node->alpha_del; + pthread_mutex_unlock(&working_flag_mutex); + return; + } + else + { + alpha_bal = alpha_bal_tmp; + alpha_del = alpha_del_tmp; + return; + } + } +} + +template +void KD_TREE::start_thread() +{ + pthread_mutex_init(&termination_flag_mutex_lock, NULL); + pthread_mutex_init(&rebuild_ptr_mutex_lock, NULL); + pthread_mutex_init(&rebuild_logger_mutex_lock, NULL); + pthread_mutex_init(&points_deleted_rebuild_mutex_lock, NULL); + pthread_mutex_init(&working_flag_mutex, NULL); + pthread_mutex_init(&search_flag_mutex, NULL); + pthread_create(&rebuild_thread, NULL, multi_thread_ptr, (void *)this); + printf("Multi thread started \n"); +} + +template +void KD_TREE::stop_thread() +{ + pthread_mutex_lock(&termination_flag_mutex_lock); + termination_flag = true; + pthread_mutex_unlock(&termination_flag_mutex_lock); + if (rebuild_thread) + pthread_join(rebuild_thread, NULL); + pthread_mutex_destroy(&termination_flag_mutex_lock); + pthread_mutex_destroy(&rebuild_logger_mutex_lock); + pthread_mutex_destroy(&rebuild_ptr_mutex_lock); + pthread_mutex_destroy(&points_deleted_rebuild_mutex_lock); + pthread_mutex_destroy(&working_flag_mutex); + pthread_mutex_destroy(&search_flag_mutex); +} + +template +void *KD_TREE::multi_thread_ptr(void *arg) +{ + KD_TREE *handle = (KD_TREE *)arg; + handle->multi_thread_rebuild(); + return nullptr; +} + +template +void KD_TREE::multi_thread_rebuild() +{ + bool terminated = false; + KD_TREE_NODE *father_ptr, **new_node_ptr; + pthread_mutex_lock(&termination_flag_mutex_lock); + terminated = termination_flag; + pthread_mutex_unlock(&termination_flag_mutex_lock); + while (!terminated) + { + pthread_mutex_lock(&rebuild_ptr_mutex_lock); + pthread_mutex_lock(&working_flag_mutex); + if (Rebuild_Ptr != nullptr) + { + /* Traverse and copy */ + if (!Rebuild_Logger.empty()) + { + printf("\n\n\n\n\n\n\n\n\n\n\n ERROR!!! \n\n\n\n\n\n\n\n\n"); + } + rebuild_flag = true; + if (*Rebuild_Ptr == Root_Node) + { + Treesize_tmp = Root_Node->TreeSize; + Validnum_tmp = Root_Node->TreeSize - Root_Node->invalid_point_num; + alpha_bal_tmp = Root_Node->alpha_bal; + alpha_del_tmp = Root_Node->alpha_del; + } + KD_TREE_NODE *old_root_node = (*Rebuild_Ptr); + father_ptr = (*Rebuild_Ptr)->father_ptr; + PointVector().swap(Rebuild_PCL_Storage); + // Lock Search + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter != 0) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter = -1; + pthread_mutex_unlock(&search_flag_mutex); + // Lock deleted points cache + pthread_mutex_lock(&points_deleted_rebuild_mutex_lock); + flatten(*Rebuild_Ptr, Rebuild_PCL_Storage, MULTI_THREAD_REC); + // Unlock deleted points cache + pthread_mutex_unlock(&points_deleted_rebuild_mutex_lock); + // Unlock Search + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter = 0; + pthread_mutex_unlock(&search_flag_mutex); + pthread_mutex_unlock(&working_flag_mutex); + /* Rebuild and update missed operations*/ + Operation_Logger_Type Operation; + KD_TREE_NODE *new_root_node = nullptr; + if (int(Rebuild_PCL_Storage.size()) > 0) + { + BuildTree(&new_root_node, 0, Rebuild_PCL_Storage.size() - 1, Rebuild_PCL_Storage); + // Rebuild has been done. Updates the blocked operations into the new tree + pthread_mutex_lock(&working_flag_mutex); + pthread_mutex_lock(&rebuild_logger_mutex_lock); + int tmp_counter = 0; + while (!Rebuild_Logger.empty()) + { + Operation = Rebuild_Logger.front(); + max_queue_size = max(max_queue_size, Rebuild_Logger.size()); + Rebuild_Logger.pop(); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + pthread_mutex_unlock(&working_flag_mutex); + run_operation(&new_root_node, Operation); + tmp_counter++; + if (tmp_counter % 10 == 0) + usleep(1); + pthread_mutex_lock(&working_flag_mutex); + pthread_mutex_lock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + /* Replace to original tree*/ + // pthread_mutex_lock(&working_flag_mutex); + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter != 0) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter = -1; + pthread_mutex_unlock(&search_flag_mutex); + if (father_ptr->left_son_ptr == *Rebuild_Ptr) + { + father_ptr->left_son_ptr = new_root_node; + } + else if (father_ptr->right_son_ptr == *Rebuild_Ptr) + { + father_ptr->right_son_ptr = new_root_node; + } + else + { + throw "Error: Father ptr incompatible with current node\n"; + } + if (new_root_node != nullptr) + new_root_node->father_ptr = father_ptr; + (*Rebuild_Ptr) = new_root_node; + int valid_old = old_root_node->TreeSize - old_root_node->invalid_point_num; + int valid_new = new_root_node->TreeSize - new_root_node->invalid_point_num; + if (father_ptr == STATIC_ROOT_NODE) + Root_Node = STATIC_ROOT_NODE->left_son_ptr; + KD_TREE_NODE *update_root = *Rebuild_Ptr; + while (update_root != nullptr && update_root != Root_Node) + { + update_root = update_root->father_ptr; + if (update_root->working_flag) + break; + if (update_root == update_root->father_ptr->left_son_ptr && update_root->father_ptr->need_push_down_to_left) + break; + if (update_root == update_root->father_ptr->right_son_ptr && update_root->father_ptr->need_push_down_to_right) + break; + Update(update_root); + } + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter = 0; + pthread_mutex_unlock(&search_flag_mutex); + Rebuild_Ptr = nullptr; + pthread_mutex_unlock(&working_flag_mutex); + rebuild_flag = false; + /* Delete discarded tree nodes */ + delete_tree_nodes(&old_root_node); + } + else + { + pthread_mutex_unlock(&working_flag_mutex); + } + pthread_mutex_unlock(&rebuild_ptr_mutex_lock); + pthread_mutex_lock(&termination_flag_mutex_lock); + terminated = termination_flag; + pthread_mutex_unlock(&termination_flag_mutex_lock); + usleep(100); + } + printf("Rebuild thread terminated normally\n"); +} + +template +void KD_TREE::run_operation(KD_TREE_NODE **root, Operation_Logger_Type operation) +{ + switch (operation.op) + { + case ADD_POINT: + Add_by_point(root, operation.point, false, (*root)->division_axis); + break; + case ADD_BOX: + Add_by_range(root, operation.boxpoint, false); + break; + case DELETE_POINT: + Delete_by_point(root, operation.point, false); + break; + case DELETE_BOX: + Delete_by_range(root, operation.boxpoint, false, false); + break; + case DOWNSAMPLE_DELETE: + Delete_by_range(root, operation.boxpoint, false, true); + break; + case PUSH_DOWN: + (*root)->tree_downsample_deleted |= operation.tree_downsample_deleted; + (*root)->point_downsample_deleted |= operation.tree_downsample_deleted; + (*root)->tree_deleted = operation.tree_deleted || (*root)->tree_downsample_deleted; + (*root)->point_deleted = (*root)->tree_deleted || (*root)->point_downsample_deleted; + if (operation.tree_downsample_deleted) + (*root)->down_del_num = (*root)->TreeSize; + if (operation.tree_deleted) + (*root)->invalid_point_num = (*root)->TreeSize; + else + (*root)->invalid_point_num = (*root)->down_del_num; + (*root)->need_push_down_to_left = true; + (*root)->need_push_down_to_right = true; + break; + default: + break; + } +} + +template +void KD_TREE::Build(PointVector point_cloud) +{ + if (Root_Node != nullptr) + { + delete_tree_nodes(&Root_Node); + } + if (point_cloud.size() == 0) + return; + STATIC_ROOT_NODE = new KD_TREE_NODE; + InitTreeNode(STATIC_ROOT_NODE); + BuildTree(&STATIC_ROOT_NODE->left_son_ptr, 0, point_cloud.size() - 1, point_cloud); + Update(STATIC_ROOT_NODE); + STATIC_ROOT_NODE->TreeSize = 0; + Root_Node = STATIC_ROOT_NODE->left_son_ptr; +} + +template +void KD_TREE::Nearest_Search(PointType point, int k_nearest, PointVector &Nearest_Points, vector &Point_Distance, float max_dist) +{ + MANUAL_HEAP q(2 * k_nearest); + q.clear(); + vector().swap(Point_Distance); + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + Search(Root_Node, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(Root_Node, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + int k_found = min(k_nearest, int(q.size())); + PointVector().swap(Nearest_Points); + vector().swap(Point_Distance); + for (int i = 0; i < k_found; i++) + { + Nearest_Points.insert(Nearest_Points.begin(), q.top().point); + Point_Distance.insert(Point_Distance.begin(), q.top().dist); + q.pop(); + } + return; +} + +template +void KD_TREE::Box_Search(const BoxPointType &Box_of_Point, PointVector &Storage) +{ + Storage.clear(); + Search_by_range(Root_Node, Box_of_Point, Storage); +} + +template +void KD_TREE::Radius_Search(PointType point, const float radius, PointVector &Storage) +{ + Storage.clear(); + Search_by_radius(Root_Node, point, radius, Storage); +} + +template +int KD_TREE::Add_Points(PointVector &PointToAdd, bool downsample_on) +{ + int NewPointSize = PointToAdd.size(); + int tree_size = size(); + BoxPointType Box_of_Point; + PointType downsample_result, mid_point; + bool downsample_switch = downsample_on && DOWNSAMPLE_SWITCH; + float min_dist, tmp_dist; + int tmp_counter = 0; + for (int i = 0; i < PointToAdd.size(); i++) + { + if (downsample_switch) + { + Box_of_Point.vertex_min[0] = floor(PointToAdd[i].x / downsample_size) * downsample_size; + Box_of_Point.vertex_max[0] = Box_of_Point.vertex_min[0] + downsample_size; + Box_of_Point.vertex_min[1] = floor(PointToAdd[i].y / downsample_size) * downsample_size; + Box_of_Point.vertex_max[1] = Box_of_Point.vertex_min[1] + downsample_size; + Box_of_Point.vertex_min[2] = floor(PointToAdd[i].z / downsample_size) * downsample_size; + Box_of_Point.vertex_max[2] = Box_of_Point.vertex_min[2] + downsample_size; + mid_point.x = Box_of_Point.vertex_min[0] + (Box_of_Point.vertex_max[0] - Box_of_Point.vertex_min[0]) / 2.0; + mid_point.y = Box_of_Point.vertex_min[1] + (Box_of_Point.vertex_max[1] - Box_of_Point.vertex_min[1]) / 2.0; + mid_point.z = Box_of_Point.vertex_min[2] + (Box_of_Point.vertex_max[2] - Box_of_Point.vertex_min[2]) / 2.0; + PointVector().swap(Downsample_Storage); + Search_by_range(Root_Node, Box_of_Point, Downsample_Storage); + min_dist = calc_dist(PointToAdd[i], mid_point); + downsample_result = PointToAdd[i]; + for (int index = 0; index < Downsample_Storage.size(); index++) + { + tmp_dist = calc_dist(Downsample_Storage[index], mid_point); + if (tmp_dist < min_dist) + { + min_dist = tmp_dist; + downsample_result = Downsample_Storage[index]; + } + } + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + if (Downsample_Storage.size() > 1 || same_point(PointToAdd[i], downsample_result)) + { + if (Downsample_Storage.size() > 0) + Delete_by_range(&Root_Node, Box_of_Point, true, true); + Add_by_point(&Root_Node, downsample_result, true, Root_Node->division_axis); + tmp_counter++; + } + } + else + { + if (Downsample_Storage.size() > 1 || same_point(PointToAdd[i], downsample_result)) + { + Operation_Logger_Type operation_delete, operation; + operation_delete.boxpoint = Box_of_Point; + operation_delete.op = DOWNSAMPLE_DELETE; + operation.point = downsample_result; + operation.op = ADD_POINT; + pthread_mutex_lock(&working_flag_mutex); + if (Downsample_Storage.size() > 0) + Delete_by_range(&Root_Node, Box_of_Point, false, true); + Add_by_point(&Root_Node, downsample_result, false, Root_Node->division_axis); + tmp_counter++; + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + if (Downsample_Storage.size() > 0) + Rebuild_Logger.push(operation_delete); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + }; + } + } + else + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + Add_by_point(&Root_Node, PointToAdd[i], true, Root_Node->division_axis); + } + else + { + Operation_Logger_Type operation; + operation.point = PointToAdd[i]; + operation.op = ADD_POINT; + pthread_mutex_lock(&working_flag_mutex); + Add_by_point(&Root_Node, PointToAdd[i], false, Root_Node->division_axis); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + } + return tmp_counter; +} + +template +void KD_TREE::Add_Point_Boxes(vector &BoxPoints) +{ + for (int i = 0; i < BoxPoints.size(); i++) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + Add_by_range(&Root_Node, BoxPoints[i], true); + } + else + { + Operation_Logger_Type operation; + operation.boxpoint = BoxPoints[i]; + operation.op = ADD_BOX; + pthread_mutex_lock(&working_flag_mutex); + Add_by_range(&Root_Node, BoxPoints[i], false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + return; +} + +template +void KD_TREE::Delete_Points(PointVector &PointToDel) +{ + for (int i = 0; i < PointToDel.size(); i++) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + Delete_by_point(&Root_Node, PointToDel[i], true); + } + else + { + Operation_Logger_Type operation; + operation.point = PointToDel[i]; + operation.op = DELETE_POINT; + pthread_mutex_lock(&working_flag_mutex); + Delete_by_point(&Root_Node, PointToDel[i], false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + return; +} + +template +int KD_TREE::Delete_Point_Boxes(vector &BoxPoints) +{ + int tmp_counter = 0; + for (int i = 0; i < BoxPoints.size(); i++) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + tmp_counter += Delete_by_range(&Root_Node, BoxPoints[i], true, false); + } + else + { + Operation_Logger_Type operation; + operation.boxpoint = BoxPoints[i]; + operation.op = DELETE_BOX; + pthread_mutex_lock(&working_flag_mutex); + tmp_counter += Delete_by_range(&Root_Node, BoxPoints[i], false, false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + return tmp_counter; +} + +template +void KD_TREE::acquire_removed_points(PointVector &removed_points) +{ + pthread_mutex_lock(&points_deleted_rebuild_mutex_lock); + for (int i = 0; i < Points_deleted.size(); i++) + { + removed_points.push_back(Points_deleted[i]); + } + for (int i = 0; i < Multithread_Points_deleted.size(); i++) + { + removed_points.push_back(Multithread_Points_deleted[i]); + } + Points_deleted.clear(); + Multithread_Points_deleted.clear(); + pthread_mutex_unlock(&points_deleted_rebuild_mutex_lock); + return; +} + +template +void KD_TREE::BuildTree(KD_TREE_NODE **root, int l, int r, PointVector &Storage) +{ + if (l > r) + return; + *root = new KD_TREE_NODE; + InitTreeNode(*root); + int mid = (l + r) >> 1; + int div_axis = 0; + int i; + // Find the best division Axis + float min_value[3] = {INFINITY, INFINITY, INFINITY}; + float max_value[3] = {-INFINITY, -INFINITY, -INFINITY}; + float dim_range[3] = {0, 0, 0}; + for (i = l; i <= r; i++) + { + min_value[0] = min(min_value[0], Storage[i].x); + min_value[1] = min(min_value[1], Storage[i].y); + min_value[2] = min(min_value[2], Storage[i].z); + max_value[0] = max(max_value[0], Storage[i].x); + max_value[1] = max(max_value[1], Storage[i].y); + max_value[2] = max(max_value[2], Storage[i].z); + } + // Select the longest dimension as division axis + for (i = 0; i < 3; i++) + dim_range[i] = max_value[i] - min_value[i]; + for (i = 1; i < 3; i++) + if (dim_range[i] > dim_range[div_axis]) + div_axis = i; + // Divide by the division axis and recursively build. + + (*root)->division_axis = div_axis; + switch (div_axis) + { + case 0: + nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_x); + break; + case 1: + nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_y); + break; + case 2: + nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_z); + break; + default: + nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_x); + break; + } + (*root)->point = Storage[mid]; + KD_TREE_NODE *left_son = nullptr, *right_son = nullptr; + BuildTree(&left_son, l, mid - 1, Storage); + BuildTree(&right_son, mid + 1, r, Storage); + (*root)->left_son_ptr = left_son; + (*root)->right_son_ptr = right_son; + Update((*root)); + return; +} + +template +void KD_TREE::Rebuild(KD_TREE_NODE **root) +{ + KD_TREE_NODE *father_ptr; + if ((*root)->TreeSize >= Multi_Thread_Rebuild_Point_Num) + { + if (!pthread_mutex_trylock(&rebuild_ptr_mutex_lock)) + { + if (Rebuild_Ptr == nullptr || ((*root)->TreeSize > (*Rebuild_Ptr)->TreeSize)) + { + Rebuild_Ptr = root; + } + pthread_mutex_unlock(&rebuild_ptr_mutex_lock); + } + } + else + { + father_ptr = (*root)->father_ptr; + int size_rec = (*root)->TreeSize; + PCL_Storage.clear(); + flatten(*root, PCL_Storage, DELETE_POINTS_REC); + delete_tree_nodes(root); + BuildTree(root, 0, PCL_Storage.size() - 1, PCL_Storage); + if (*root != nullptr) + (*root)->father_ptr = father_ptr; + if (*root == Root_Node) + STATIC_ROOT_NODE->left_son_ptr = *root; + } + return; +} + +template +int KD_TREE::Delete_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild, bool is_downsample) +{ + if ((*root) == nullptr || (*root)->tree_deleted) + return 0; + (*root)->working_flag = true; + Push_Down(*root); + int tmp_counter = 0; + if (boxpoint.vertex_max[0] <= (*root)->node_range_x[0] || boxpoint.vertex_min[0] > (*root)->node_range_x[1]) + return 0; + if (boxpoint.vertex_max[1] <= (*root)->node_range_y[0] || boxpoint.vertex_min[1] > (*root)->node_range_y[1]) + return 0; + if (boxpoint.vertex_max[2] <= (*root)->node_range_z[0] || boxpoint.vertex_min[2] > (*root)->node_range_z[1]) + return 0; + if (boxpoint.vertex_min[0] <= (*root)->node_range_x[0] && boxpoint.vertex_max[0] > (*root)->node_range_x[1] && boxpoint.vertex_min[1] <= (*root)->node_range_y[0] && boxpoint.vertex_max[1] > (*root)->node_range_y[1] && boxpoint.vertex_min[2] <= (*root)->node_range_z[0] && boxpoint.vertex_max[2] > (*root)->node_range_z[1]) + { + (*root)->tree_deleted = true; + (*root)->point_deleted = true; + (*root)->need_push_down_to_left = true; + (*root)->need_push_down_to_right = true; + tmp_counter = (*root)->TreeSize - (*root)->invalid_point_num; + (*root)->invalid_point_num = (*root)->TreeSize; + if (is_downsample) + { + (*root)->tree_downsample_deleted = true; + (*root)->point_downsample_deleted = true; + (*root)->down_del_num = (*root)->TreeSize; + } + return tmp_counter; + } + if (!(*root)->point_deleted && boxpoint.vertex_min[0] <= (*root)->point.x && boxpoint.vertex_max[0] > (*root)->point.x && boxpoint.vertex_min[1] <= (*root)->point.y && boxpoint.vertex_max[1] > (*root)->point.y && boxpoint.vertex_min[2] <= (*root)->point.z && boxpoint.vertex_max[2] > (*root)->point.z) + { + (*root)->point_deleted = true; + tmp_counter += 1; + if (is_downsample) + (*root)->point_downsample_deleted = true; + } + Operation_Logger_Type delete_box_log; + struct timespec Timeout; + if (is_downsample) + delete_box_log.op = DOWNSAMPLE_DELETE; + else + delete_box_log.op = DELETE_BOX; + delete_box_log.boxpoint = boxpoint; + if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr) + { + tmp_counter += Delete_by_range(&((*root)->left_son_ptr), boxpoint, allow_rebuild, is_downsample); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + tmp_counter += Delete_by_range(&((*root)->left_son_ptr), boxpoint, false, is_downsample); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(delete_box_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr) + { + tmp_counter += Delete_by_range(&((*root)->right_son_ptr), boxpoint, allow_rebuild, is_downsample); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + tmp_counter += Delete_by_range(&((*root)->right_son_ptr), boxpoint, false, is_downsample); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(delete_box_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + Update(*root); + if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) + Rebuild_Ptr = nullptr; + bool need_rebuild = allow_rebuild & Criterion_Check((*root)); + if (need_rebuild) + Rebuild(root); + if ((*root) != nullptr) + (*root)->working_flag = false; + return tmp_counter; +} + +template +void KD_TREE::Delete_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild) +{ + if ((*root) == nullptr || (*root)->tree_deleted) + return; + (*root)->working_flag = true; + Push_Down(*root); + if (same_point((*root)->point, point) && !(*root)->point_deleted) + { + (*root)->point_deleted = true; + (*root)->invalid_point_num += 1; + if ((*root)->invalid_point_num == (*root)->TreeSize) + (*root)->tree_deleted = true; + return; + } + Operation_Logger_Type delete_log; + struct timespec Timeout; + delete_log.op = DELETE_POINT; + delete_log.point = point; + if (((*root)->division_axis == 0 && point.x < (*root)->point.x) || ((*root)->division_axis == 1 && point.y < (*root)->point.y) || ((*root)->division_axis == 2 && point.z < (*root)->point.z)) + { + if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr) + { + Delete_by_point(&(*root)->left_son_ptr, point, allow_rebuild); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + Delete_by_point(&(*root)->left_son_ptr, point, false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(delete_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + else + { + if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr) + { + Delete_by_point(&(*root)->right_son_ptr, point, allow_rebuild); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + Delete_by_point(&(*root)->right_son_ptr, point, false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(delete_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + Update(*root); + if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) + Rebuild_Ptr = nullptr; + bool need_rebuild = allow_rebuild & Criterion_Check((*root)); + if (need_rebuild) + Rebuild(root); + if ((*root) != nullptr) + (*root)->working_flag = false; + return; +} + +template +void KD_TREE::Add_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild) +{ + if ((*root) == nullptr) + return; + (*root)->working_flag = true; + Push_Down(*root); + if (boxpoint.vertex_max[0] <= (*root)->node_range_x[0] || boxpoint.vertex_min[0] > (*root)->node_range_x[1]) + return; + if (boxpoint.vertex_max[1] <= (*root)->node_range_y[0] || boxpoint.vertex_min[1] > (*root)->node_range_y[1]) + return; + if (boxpoint.vertex_max[2] <= (*root)->node_range_z[0] || boxpoint.vertex_min[2] > (*root)->node_range_z[1]) + return; + if (boxpoint.vertex_min[0] <= (*root)->node_range_x[0] && boxpoint.vertex_max[0] > (*root)->node_range_x[1] && boxpoint.vertex_min[1] <= (*root)->node_range_y[0] && boxpoint.vertex_max[1] > (*root)->node_range_y[1] && boxpoint.vertex_min[2] <= (*root)->node_range_z[0] && boxpoint.vertex_max[2] > (*root)->node_range_z[1]) + { + (*root)->tree_deleted = false || (*root)->tree_downsample_deleted; + (*root)->point_deleted = false || (*root)->point_downsample_deleted; + (*root)->need_push_down_to_left = true; + (*root)->need_push_down_to_right = true; + (*root)->invalid_point_num = (*root)->down_del_num; + return; + } + if (boxpoint.vertex_min[0] <= (*root)->point.x && boxpoint.vertex_max[0] > (*root)->point.x && boxpoint.vertex_min[1] <= (*root)->point.y && boxpoint.vertex_max[1] > (*root)->point.y && boxpoint.vertex_min[2] <= (*root)->point.z && boxpoint.vertex_max[2] > (*root)->point.z) + { + (*root)->point_deleted = (*root)->point_downsample_deleted; + } + Operation_Logger_Type add_box_log; + struct timespec Timeout; + add_box_log.op = ADD_BOX; + add_box_log.boxpoint = boxpoint; + if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr) + { + Add_by_range(&((*root)->left_son_ptr), boxpoint, allow_rebuild); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + Add_by_range(&((*root)->left_son_ptr), boxpoint, false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(add_box_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr) + { + Add_by_range(&((*root)->right_son_ptr), boxpoint, allow_rebuild); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + Add_by_range(&((*root)->right_son_ptr), boxpoint, false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(add_box_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + Update(*root); + if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) + Rebuild_Ptr = nullptr; + bool need_rebuild = allow_rebuild & Criterion_Check((*root)); + if (need_rebuild) + Rebuild(root); + if ((*root) != nullptr) + (*root)->working_flag = false; + return; +} + +template +void KD_TREE::Add_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild, int father_axis) +{ + if (*root == nullptr) + { + *root = new KD_TREE_NODE; + InitTreeNode(*root); + (*root)->point = point; + (*root)->division_axis = (father_axis + 1) % 3; + Update(*root); + return; + } + (*root)->working_flag = true; + Operation_Logger_Type add_log; + struct timespec Timeout; + add_log.op = ADD_POINT; + add_log.point = point; + Push_Down(*root); + if (((*root)->division_axis == 0 && point.x < (*root)->point.x) || ((*root)->division_axis == 1 && point.y < (*root)->point.y) || ((*root)->division_axis == 2 && point.z < (*root)->point.z)) + { + if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr) + { + Add_by_point(&(*root)->left_son_ptr, point, allow_rebuild, (*root)->division_axis); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + Add_by_point(&(*root)->left_son_ptr, point, false, (*root)->division_axis); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(add_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + else + { + if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr) + { + Add_by_point(&(*root)->right_son_ptr, point, allow_rebuild, (*root)->division_axis); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + Add_by_point(&(*root)->right_son_ptr, point, false, (*root)->division_axis); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(add_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + Update(*root); + if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) + Rebuild_Ptr = nullptr; + bool need_rebuild = allow_rebuild & Criterion_Check((*root)); + if (need_rebuild) + Rebuild(root); + if ((*root) != nullptr) + (*root)->working_flag = false; + return; +} + +template +void KD_TREE::Search(KD_TREE_NODE *root, int k_nearest, PointType point, MANUAL_HEAP &q, float max_dist) +{ + if (root == nullptr || root->tree_deleted) + return; + float cur_dist = calc_box_dist(root, point); + float max_dist_sqr = max_dist * max_dist; + if (cur_dist > max_dist_sqr) + return; + int retval; + if (root->need_push_down_to_left || root->need_push_down_to_right) + { + retval = pthread_mutex_trylock(&(root->push_down_mutex_lock)); + if (retval == 0) + { + Push_Down(root); + pthread_mutex_unlock(&(root->push_down_mutex_lock)); + } + else + { + pthread_mutex_lock(&(root->push_down_mutex_lock)); + pthread_mutex_unlock(&(root->push_down_mutex_lock)); + } + } + if (!root->point_deleted) + { + float dist = calc_dist(point, root->point); + if (dist <= max_dist_sqr && (q.size() < k_nearest || dist < q.top().dist)) + { + if (q.size() >= k_nearest) + q.pop(); + PointType_CMP current_point{root->point, dist}; + q.push(current_point); + } + } + int cur_search_counter; + float dist_left_node = calc_box_dist(root->left_son_ptr, point); + float dist_right_node = calc_box_dist(root->right_son_ptr, point); + if (q.size() < k_nearest || dist_left_node < q.top().dist && dist_right_node < q.top().dist) + { + if (dist_left_node <= dist_right_node) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr) + { + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + if (q.size() < k_nearest || dist_right_node < q.top().dist) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr) + { + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + } + } + else + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr) + { + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + if (q.size() < k_nearest || dist_left_node < q.top().dist) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr) + { + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + } + } + } + else + { + if (dist_left_node < q.top().dist) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr) + { + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + } + if (dist_right_node < q.top().dist) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr) + { + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + } + } + return; +} + +template +void KD_TREE::Search_by_range(KD_TREE_NODE *root, BoxPointType boxpoint, PointVector &Storage) +{ + if (root == nullptr) + return; + Push_Down(root); + if (boxpoint.vertex_max[0] <= root->node_range_x[0] || boxpoint.vertex_min[0] > root->node_range_x[1]) + return; + if (boxpoint.vertex_max[1] <= root->node_range_y[0] || boxpoint.vertex_min[1] > root->node_range_y[1]) + return; + if (boxpoint.vertex_max[2] <= root->node_range_z[0] || boxpoint.vertex_min[2] > root->node_range_z[1]) + return; + if (boxpoint.vertex_min[0] <= root->node_range_x[0] && boxpoint.vertex_max[0] > root->node_range_x[1] && boxpoint.vertex_min[1] <= root->node_range_y[0] && boxpoint.vertex_max[1] > root->node_range_y[1] && boxpoint.vertex_min[2] <= root->node_range_z[0] && boxpoint.vertex_max[2] > root->node_range_z[1]) + { + flatten(root, Storage, NOT_RECORD); + return; + } + if (boxpoint.vertex_min[0] <= root->point.x && boxpoint.vertex_max[0] > root->point.x && boxpoint.vertex_min[1] <= root->point.y && boxpoint.vertex_max[1] > root->point.y && boxpoint.vertex_min[2] <= root->point.z && boxpoint.vertex_max[2] > root->point.z) + { + if (!root->point_deleted) + Storage.push_back(root->point); + } + if ((Rebuild_Ptr == nullptr) || root->left_son_ptr != *Rebuild_Ptr) + { + Search_by_range(root->left_son_ptr, boxpoint, Storage); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + Search_by_range(root->left_son_ptr, boxpoint, Storage); + pthread_mutex_unlock(&search_flag_mutex); + } + if ((Rebuild_Ptr == nullptr) || root->right_son_ptr != *Rebuild_Ptr) + { + Search_by_range(root->right_son_ptr, boxpoint, Storage); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + Search_by_range(root->right_son_ptr, boxpoint, Storage); + pthread_mutex_unlock(&search_flag_mutex); + } + return; +} + +template +void KD_TREE::Search_by_radius(KD_TREE_NODE *root, PointType point, float radius, PointVector &Storage) +{ + if (root == nullptr) + return; + Push_Down(root); + PointType range_center; + range_center.x = (root->node_range_x[0] + root->node_range_x[1]) * 0.5; + range_center.y = (root->node_range_y[0] + root->node_range_y[1]) * 0.5; + range_center.z = (root->node_range_z[0] + root->node_range_z[1]) * 0.5; + float dist = sqrt(calc_dist(range_center, point)); + if (dist > radius + sqrt(root->radius_sq)) + return; + if (dist <= radius - sqrt(root->radius_sq)) + { + flatten(root, Storage, NOT_RECORD); + return; + } + if (!root->point_deleted && calc_dist(root->point, point) <= radius * radius) + { + Storage.push_back(root->point); + } + if ((Rebuild_Ptr == nullptr) || root->left_son_ptr != *Rebuild_Ptr) + { + Search_by_radius(root->left_son_ptr, point, radius, Storage); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + Search_by_radius(root->left_son_ptr, point, radius, Storage); + pthread_mutex_unlock(&search_flag_mutex); + } + if ((Rebuild_Ptr == nullptr) || root->right_son_ptr != *Rebuild_Ptr) + { + Search_by_radius(root->right_son_ptr, point, radius, Storage); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + Search_by_radius(root->right_son_ptr, point, radius, Storage); + pthread_mutex_unlock(&search_flag_mutex); + } + return; +} + +template +bool KD_TREE::Criterion_Check(KD_TREE_NODE *root) +{ + if (root->TreeSize <= Minimal_Unbalanced_Tree_Size) + { + return false; + } + float balance_evaluation = 0.0f; + float delete_evaluation = 0.0f; + KD_TREE_NODE *son_ptr = root->left_son_ptr; + if (son_ptr == nullptr) + son_ptr = root->right_son_ptr; + delete_evaluation = float(root->invalid_point_num) / root->TreeSize; + balance_evaluation = float(son_ptr->TreeSize) / (root->TreeSize - 1); + if (delete_evaluation > delete_criterion_param) + { + return true; + } + if (balance_evaluation > balance_criterion_param || balance_evaluation < 1 - balance_criterion_param) + { + return true; + } + return false; +} + +template +void KD_TREE::Push_Down(KD_TREE_NODE *root) +{ + if (root == nullptr) + return; + Operation_Logger_Type operation; + operation.op = PUSH_DOWN; + operation.tree_deleted = root->tree_deleted; + operation.tree_downsample_deleted = root->tree_downsample_deleted; + if (root->need_push_down_to_left && root->left_son_ptr != nullptr) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr) + { + root->left_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted; + root->left_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted; + root->left_son_ptr->tree_deleted = root->tree_deleted || root->left_son_ptr->tree_downsample_deleted; + root->left_son_ptr->point_deleted = root->left_son_ptr->tree_deleted || root->left_son_ptr->point_downsample_deleted; + if (root->tree_downsample_deleted) + root->left_son_ptr->down_del_num = root->left_son_ptr->TreeSize; + if (root->tree_deleted) + root->left_son_ptr->invalid_point_num = root->left_son_ptr->TreeSize; + else + root->left_son_ptr->invalid_point_num = root->left_son_ptr->down_del_num; + root->left_son_ptr->need_push_down_to_left = true; + root->left_son_ptr->need_push_down_to_right = true; + root->need_push_down_to_left = false; + } + else + { + pthread_mutex_lock(&working_flag_mutex); + root->left_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted; + root->left_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted; + root->left_son_ptr->tree_deleted = root->tree_deleted || root->left_son_ptr->tree_downsample_deleted; + root->left_son_ptr->point_deleted = root->left_son_ptr->tree_deleted || root->left_son_ptr->point_downsample_deleted; + if (root->tree_downsample_deleted) + root->left_son_ptr->down_del_num = root->left_son_ptr->TreeSize; + if (root->tree_deleted) + root->left_son_ptr->invalid_point_num = root->left_son_ptr->TreeSize; + else + root->left_son_ptr->invalid_point_num = root->left_son_ptr->down_del_num; + root->left_son_ptr->need_push_down_to_left = true; + root->left_son_ptr->need_push_down_to_right = true; + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + root->need_push_down_to_left = false; + pthread_mutex_unlock(&working_flag_mutex); + } + } + if (root->need_push_down_to_right && root->right_son_ptr != nullptr) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr) + { + root->right_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted; + root->right_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted; + root->right_son_ptr->tree_deleted = root->tree_deleted || root->right_son_ptr->tree_downsample_deleted; + root->right_son_ptr->point_deleted = root->right_son_ptr->tree_deleted || root->right_son_ptr->point_downsample_deleted; + if (root->tree_downsample_deleted) + root->right_son_ptr->down_del_num = root->right_son_ptr->TreeSize; + if (root->tree_deleted) + root->right_son_ptr->invalid_point_num = root->right_son_ptr->TreeSize; + else + root->right_son_ptr->invalid_point_num = root->right_son_ptr->down_del_num; + root->right_son_ptr->need_push_down_to_left = true; + root->right_son_ptr->need_push_down_to_right = true; + root->need_push_down_to_right = false; + } + else + { + pthread_mutex_lock(&working_flag_mutex); + root->right_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted; + root->right_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted; + root->right_son_ptr->tree_deleted = root->tree_deleted || root->right_son_ptr->tree_downsample_deleted; + root->right_son_ptr->point_deleted = root->right_son_ptr->tree_deleted || root->right_son_ptr->point_downsample_deleted; + if (root->tree_downsample_deleted) + root->right_son_ptr->down_del_num = root->right_son_ptr->TreeSize; + if (root->tree_deleted) + root->right_son_ptr->invalid_point_num = root->right_son_ptr->TreeSize; + else + root->right_son_ptr->invalid_point_num = root->right_son_ptr->down_del_num; + root->right_son_ptr->need_push_down_to_left = true; + root->right_son_ptr->need_push_down_to_right = true; + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + root->need_push_down_to_right = false; + pthread_mutex_unlock(&working_flag_mutex); + } + } + return; +} + +template +void KD_TREE::Update(KD_TREE_NODE *root) +{ + KD_TREE_NODE *left_son_ptr = root->left_son_ptr; + KD_TREE_NODE *right_son_ptr = root->right_son_ptr; + float tmp_range_x[2] = {INFINITY, -INFINITY}; + float tmp_range_y[2] = {INFINITY, -INFINITY}; + float tmp_range_z[2] = {INFINITY, -INFINITY}; + // Update Tree Size + if (left_son_ptr != nullptr && right_son_ptr != nullptr) + { + root->TreeSize = left_son_ptr->TreeSize + right_son_ptr->TreeSize + 1; + root->invalid_point_num = left_son_ptr->invalid_point_num + right_son_ptr->invalid_point_num + (root->point_deleted ? 1 : 0); + root->down_del_num = left_son_ptr->down_del_num + right_son_ptr->down_del_num + (root->point_downsample_deleted ? 1 : 0); + root->tree_downsample_deleted = left_son_ptr->tree_downsample_deleted & right_son_ptr->tree_downsample_deleted & root->point_downsample_deleted; + root->tree_deleted = left_son_ptr->tree_deleted && right_son_ptr->tree_deleted && root->point_deleted; + if (root->tree_deleted || (!left_son_ptr->tree_deleted && !right_son_ptr->tree_deleted && !root->point_deleted)) + { + tmp_range_x[0] = min(min(left_son_ptr->node_range_x[0], right_son_ptr->node_range_x[0]), root->point.x); + tmp_range_x[1] = max(max(left_son_ptr->node_range_x[1], right_son_ptr->node_range_x[1]), root->point.x); + tmp_range_y[0] = min(min(left_son_ptr->node_range_y[0], right_son_ptr->node_range_y[0]), root->point.y); + tmp_range_y[1] = max(max(left_son_ptr->node_range_y[1], right_son_ptr->node_range_y[1]), root->point.y); + tmp_range_z[0] = min(min(left_son_ptr->node_range_z[0], right_son_ptr->node_range_z[0]), root->point.z); + tmp_range_z[1] = max(max(left_son_ptr->node_range_z[1], right_son_ptr->node_range_z[1]), root->point.z); + } + else + { + if (!left_son_ptr->tree_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], left_son_ptr->node_range_x[0]); + tmp_range_x[1] = max(tmp_range_x[1], left_son_ptr->node_range_x[1]); + tmp_range_y[0] = min(tmp_range_y[0], left_son_ptr->node_range_y[0]); + tmp_range_y[1] = max(tmp_range_y[1], left_son_ptr->node_range_y[1]); + tmp_range_z[0] = min(tmp_range_z[0], left_son_ptr->node_range_z[0]); + tmp_range_z[1] = max(tmp_range_z[1], left_son_ptr->node_range_z[1]); + } + if (!right_son_ptr->tree_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], right_son_ptr->node_range_x[0]); + tmp_range_x[1] = max(tmp_range_x[1], right_son_ptr->node_range_x[1]); + tmp_range_y[0] = min(tmp_range_y[0], right_son_ptr->node_range_y[0]); + tmp_range_y[1] = max(tmp_range_y[1], right_son_ptr->node_range_y[1]); + tmp_range_z[0] = min(tmp_range_z[0], right_son_ptr->node_range_z[0]); + tmp_range_z[1] = max(tmp_range_z[1], right_son_ptr->node_range_z[1]); + } + if (!root->point_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], root->point.x); + tmp_range_x[1] = max(tmp_range_x[1], root->point.x); + tmp_range_y[0] = min(tmp_range_y[0], root->point.y); + tmp_range_y[1] = max(tmp_range_y[1], root->point.y); + tmp_range_z[0] = min(tmp_range_z[0], root->point.z); + tmp_range_z[1] = max(tmp_range_z[1], root->point.z); + } + } + } + else if (left_son_ptr != nullptr) + { + root->TreeSize = left_son_ptr->TreeSize + 1; + root->invalid_point_num = left_son_ptr->invalid_point_num + (root->point_deleted ? 1 : 0); + root->down_del_num = left_son_ptr->down_del_num + (root->point_downsample_deleted ? 1 : 0); + root->tree_downsample_deleted = left_son_ptr->tree_downsample_deleted & root->point_downsample_deleted; + root->tree_deleted = left_son_ptr->tree_deleted && root->point_deleted; + if (root->tree_deleted || (!left_son_ptr->tree_deleted && !root->point_deleted)) + { + tmp_range_x[0] = min(left_son_ptr->node_range_x[0], root->point.x); + tmp_range_x[1] = max(left_son_ptr->node_range_x[1], root->point.x); + tmp_range_y[0] = min(left_son_ptr->node_range_y[0], root->point.y); + tmp_range_y[1] = max(left_son_ptr->node_range_y[1], root->point.y); + tmp_range_z[0] = min(left_son_ptr->node_range_z[0], root->point.z); + tmp_range_z[1] = max(left_son_ptr->node_range_z[1], root->point.z); + } + else + { + if (!left_son_ptr->tree_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], left_son_ptr->node_range_x[0]); + tmp_range_x[1] = max(tmp_range_x[1], left_son_ptr->node_range_x[1]); + tmp_range_y[0] = min(tmp_range_y[0], left_son_ptr->node_range_y[0]); + tmp_range_y[1] = max(tmp_range_y[1], left_son_ptr->node_range_y[1]); + tmp_range_z[0] = min(tmp_range_z[0], left_son_ptr->node_range_z[0]); + tmp_range_z[1] = max(tmp_range_z[1], left_son_ptr->node_range_z[1]); + } + if (!root->point_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], root->point.x); + tmp_range_x[1] = max(tmp_range_x[1], root->point.x); + tmp_range_y[0] = min(tmp_range_y[0], root->point.y); + tmp_range_y[1] = max(tmp_range_y[1], root->point.y); + tmp_range_z[0] = min(tmp_range_z[0], root->point.z); + tmp_range_z[1] = max(tmp_range_z[1], root->point.z); + } + } + } + else if (right_son_ptr != nullptr) + { + root->TreeSize = right_son_ptr->TreeSize + 1; + root->invalid_point_num = right_son_ptr->invalid_point_num + (root->point_deleted ? 1 : 0); + root->down_del_num = right_son_ptr->down_del_num + (root->point_downsample_deleted ? 1 : 0); + root->tree_downsample_deleted = right_son_ptr->tree_downsample_deleted & root->point_downsample_deleted; + root->tree_deleted = right_son_ptr->tree_deleted && root->point_deleted; + if (root->tree_deleted || (!right_son_ptr->tree_deleted && !root->point_deleted)) + { + tmp_range_x[0] = min(right_son_ptr->node_range_x[0], root->point.x); + tmp_range_x[1] = max(right_son_ptr->node_range_x[1], root->point.x); + tmp_range_y[0] = min(right_son_ptr->node_range_y[0], root->point.y); + tmp_range_y[1] = max(right_son_ptr->node_range_y[1], root->point.y); + tmp_range_z[0] = min(right_son_ptr->node_range_z[0], root->point.z); + tmp_range_z[1] = max(right_son_ptr->node_range_z[1], root->point.z); + } + else + { + if (!right_son_ptr->tree_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], right_son_ptr->node_range_x[0]); + tmp_range_x[1] = max(tmp_range_x[1], right_son_ptr->node_range_x[1]); + tmp_range_y[0] = min(tmp_range_y[0], right_son_ptr->node_range_y[0]); + tmp_range_y[1] = max(tmp_range_y[1], right_son_ptr->node_range_y[1]); + tmp_range_z[0] = min(tmp_range_z[0], right_son_ptr->node_range_z[0]); + tmp_range_z[1] = max(tmp_range_z[1], right_son_ptr->node_range_z[1]); + } + if (!root->point_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], root->point.x); + tmp_range_x[1] = max(tmp_range_x[1], root->point.x); + tmp_range_y[0] = min(tmp_range_y[0], root->point.y); + tmp_range_y[1] = max(tmp_range_y[1], root->point.y); + tmp_range_z[0] = min(tmp_range_z[0], root->point.z); + tmp_range_z[1] = max(tmp_range_z[1], root->point.z); + } + } + } + else + { + root->TreeSize = 1; + root->invalid_point_num = (root->point_deleted ? 1 : 0); + root->down_del_num = (root->point_downsample_deleted ? 1 : 0); + root->tree_downsample_deleted = root->point_downsample_deleted; + root->tree_deleted = root->point_deleted; + tmp_range_x[0] = root->point.x; + tmp_range_x[1] = root->point.x; + tmp_range_y[0] = root->point.y; + tmp_range_y[1] = root->point.y; + tmp_range_z[0] = root->point.z; + tmp_range_z[1] = root->point.z; + } + memcpy(root->node_range_x, tmp_range_x, sizeof(tmp_range_x)); + memcpy(root->node_range_y, tmp_range_y, sizeof(tmp_range_y)); + memcpy(root->node_range_z, tmp_range_z, sizeof(tmp_range_z)); + float x_L = (root->node_range_x[1] - root->node_range_x[0]) * 0.5; + float y_L = (root->node_range_y[1] - root->node_range_y[0]) * 0.5; + float z_L = (root->node_range_z[1] - root->node_range_z[0]) * 0.5; + root->radius_sq = x_L * x_L + y_L * y_L + z_L * z_L; + if (left_son_ptr != nullptr) + left_son_ptr->father_ptr = root; + if (right_son_ptr != nullptr) + right_son_ptr->father_ptr = root; + if (root == Root_Node && root->TreeSize > 3) + { + KD_TREE_NODE *son_ptr = root->left_son_ptr; + if (son_ptr == nullptr) + son_ptr = root->right_son_ptr; + float tmp_bal = float(son_ptr->TreeSize) / (root->TreeSize - 1); + root->alpha_del = float(root->invalid_point_num) / root->TreeSize; + root->alpha_bal = (tmp_bal >= 0.5 - EPSS) ? tmp_bal : 1 - tmp_bal; + } + return; +} + +template +void KD_TREE::flatten(KD_TREE_NODE *root, PointVector &Storage, delete_point_storage_set storage_type) +{ + if (root == nullptr) + return; + Push_Down(root); + if (!root->point_deleted) + { + Storage.push_back(root->point); + } + flatten(root->left_son_ptr, Storage, storage_type); + flatten(root->right_son_ptr, Storage, storage_type); + switch (storage_type) + { + case NOT_RECORD: + break; + case DELETE_POINTS_REC: + if (root->point_deleted && !root->point_downsample_deleted) + { + Points_deleted.push_back(root->point); + } + break; + case MULTI_THREAD_REC: + if (root->point_deleted && !root->point_downsample_deleted) + { + Multithread_Points_deleted.push_back(root->point); + } + break; + default: + break; + } + return; +} + +template +void KD_TREE::delete_tree_nodes(KD_TREE_NODE **root) +{ + if (*root == nullptr) + return; + Push_Down(*root); + delete_tree_nodes(&(*root)->left_son_ptr); + delete_tree_nodes(&(*root)->right_son_ptr); + + pthread_mutex_destroy(&(*root)->push_down_mutex_lock); + delete *root; + *root = nullptr; + + return; +} + +template +bool KD_TREE::same_point(PointType a, PointType b) +{ + return (fabs(a.x - b.x) < EPSS && fabs(a.y - b.y) < EPSS && fabs(a.z - b.z) < EPSS); +} + +template +float KD_TREE::calc_dist(PointType a, PointType b) +{ + float dist = 0.0f; + dist = (a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y) + (a.z - b.z) * (a.z - b.z); + return dist; +} + +template +float KD_TREE::calc_box_dist(KD_TREE_NODE *node, PointType point) +{ + if (node == nullptr) + return INFINITY; + float min_dist = 0.0; + if (point.x < node->node_range_x[0]) + min_dist += (point.x - node->node_range_x[0]) * (point.x - node->node_range_x[0]); + if (point.x > node->node_range_x[1]) + min_dist += (point.x - node->node_range_x[1]) * (point.x - node->node_range_x[1]); + if (point.y < node->node_range_y[0]) + min_dist += (point.y - node->node_range_y[0]) * (point.y - node->node_range_y[0]); + if (point.y > node->node_range_y[1]) + min_dist += (point.y - node->node_range_y[1]) * (point.y - node->node_range_y[1]); + if (point.z < node->node_range_z[0]) + min_dist += (point.z - node->node_range_z[0]) * (point.z - node->node_range_z[0]); + if (point.z > node->node_range_z[1]) + min_dist += (point.z - node->node_range_z[1]) * (point.z - node->node_range_z[1]); + return min_dist; +} +template +bool KD_TREE::point_cmp_x(PointType a, PointType b) { return a.x < b.x; } +template +bool KD_TREE::point_cmp_y(PointType a, PointType b) { return a.y < b.y; } +template +bool KD_TREE::point_cmp_z(PointType a, PointType b) { return a.z < b.z; } + +template +void KD_TREE::ClearTree() +{ + Delete_Storage_Disabled = true; + delete_tree_nodes(&Root_Node); + PointVector().swap(PCL_Storage); + Rebuild_Logger.clear(); +} +// Manual heap + +// manual queue + +// Manual Instatiations +template class KD_TREE; +template class KD_TREE; +template class KD_TREE; diff --git a/src/FASTLIO2_ROS2/fastlio2/src/map_builder/ikd_Tree.h b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/ikd_Tree.h new file mode 100644 index 00000000..275dd301 --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/ikd_Tree.h @@ -0,0 +1,345 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define EPSS 1e-6 +#define Minimal_Unbalanced_Tree_Size 10 +#define Multi_Thread_Rebuild_Point_Num 1500 +#define DOWNSAMPLE_SWITCH true +#define ForceRebuildPercentage 0.2 +#define Q_LEN 1000000 + +using namespace std; + +// typedef pcl::PointXYZINormal PointType; +// typedef vector> PointVector; + +struct BoxPointType +{ + float vertex_min[3]; + float vertex_max[3]; +}; + +enum operation_set +{ + ADD_POINT, + DELETE_POINT, + DELETE_BOX, + ADD_BOX, + DOWNSAMPLE_DELETE, + PUSH_DOWN +}; + +enum delete_point_storage_set +{ + NOT_RECORD, + DELETE_POINTS_REC, + MULTI_THREAD_REC +}; + +template +class KD_TREE +{ + // using MANUAL_Q_ = MANUAL_Q; + // using PointVector = std::vector; + + // using MANUAL_Q_ = MANUAL_Q; +public: + using PointVector = std::vector>; + using Ptr = std::shared_ptr>; + + struct KD_TREE_NODE + { + PointType point; + int division_axis; + int TreeSize = 1; + int invalid_point_num = 0; + int down_del_num = 0; + bool point_deleted = false; + bool tree_deleted = false; + bool point_downsample_deleted = false; + bool tree_downsample_deleted = false; + bool need_push_down_to_left = false; + bool need_push_down_to_right = false; + bool working_flag = false; + pthread_mutex_t push_down_mutex_lock; + float node_range_x[2], node_range_y[2], node_range_z[2]; + float radius_sq; + KD_TREE_NODE *left_son_ptr = nullptr; + KD_TREE_NODE *right_son_ptr = nullptr; + KD_TREE_NODE *father_ptr = nullptr; + // For paper data record + float alpha_del; + float alpha_bal; + }; + + struct Operation_Logger_Type + { + PointType point; + BoxPointType boxpoint; + bool tree_deleted, tree_downsample_deleted; + operation_set op; + }; + // static const PointType zeroP; + + struct PointType_CMP + { + PointType point; + float dist = 0.0; + PointType_CMP(PointType p = PointType(), float d = INFINITY) + { + this->point = p; + this->dist = d; + }; + bool operator<(const PointType_CMP &a) const + { + if (fabs(dist - a.dist) < 1e-10) + return point.x < a.point.x; + else + return dist < a.dist; + } + }; + + class MANUAL_HEAP + { + + public: + MANUAL_HEAP(int max_capacity = 100) + + { + cap = max_capacity; + heap = new PointType_CMP[max_capacity]; + heap_size = 0; + } + + ~MANUAL_HEAP() + { + delete[] heap; + } + void pop() + { + if (heap_size == 0) + return; + heap[0] = heap[heap_size - 1]; + heap_size--; + MoveDown(0); + return; + } + PointType_CMP top() + { + return heap[0]; + } + void push(PointType_CMP point) + { + if (heap_size >= cap) + return; + heap[heap_size] = point; + FloatUp(heap_size); + heap_size++; + return; + } + int size() + { + return heap_size; + } + void clear() + { + heap_size = 0; + return; + } + + private: + PointType_CMP *heap; + void MoveDown(int heap_index) + { + int l = heap_index * 2 + 1; + PointType_CMP tmp = heap[heap_index]; + while (l < heap_size) + { + if (l + 1 < heap_size && heap[l] < heap[l + 1]) + l++; + if (tmp < heap[l]) + { + heap[heap_index] = heap[l]; + heap_index = l; + l = heap_index * 2 + 1; + } + else + break; + } + heap[heap_index] = tmp; + return; + } + void FloatUp(int heap_index) + { + int ancestor = (heap_index - 1) / 2; + PointType_CMP tmp = heap[heap_index]; + while (heap_index > 0) + { + if (heap[ancestor] < tmp) + { + heap[heap_index] = heap[ancestor]; + heap_index = ancestor; + ancestor = (heap_index - 1) / 2; + } + else + break; + } + heap[heap_index] = tmp; + return; + } + int heap_size = 0; + int cap = 0; + }; + + class MANUAL_Q + { + private: + int head = 0, tail = 0, counter = 0; + Operation_Logger_Type q[Q_LEN]; + bool is_empty; + + public: + void pop() + { + if (counter == 0) + return; + head++; + head %= Q_LEN; + counter--; + if (counter == 0) + is_empty = true; + return; + } + Operation_Logger_Type front() + { + return q[head]; + } + Operation_Logger_Type back() + { + return q[tail]; + } + void clear() + { + head = 0; + tail = 0; + counter = 0; + is_empty = true; + return; + } + void push(Operation_Logger_Type op) + { + q[tail] = op; + counter++; + if (is_empty) + is_empty = false; + tail++; + tail %= Q_LEN; + } + bool empty() + { + return is_empty; + } + int size() + { + return counter; + } + }; + +private: + // Multi-thread Tree Rebuild + bool termination_flag = false; + bool rebuild_flag = false; + pthread_t rebuild_thread; + pthread_mutex_t termination_flag_mutex_lock, rebuild_ptr_mutex_lock, working_flag_mutex, search_flag_mutex; + pthread_mutex_t rebuild_logger_mutex_lock, points_deleted_rebuild_mutex_lock; + // queue Rebuild_Logger; + MANUAL_Q Rebuild_Logger; + PointVector Rebuild_PCL_Storage; + KD_TREE_NODE **Rebuild_Ptr = nullptr; + int search_mutex_counter = 0; + static void *multi_thread_ptr(void *arg); + void multi_thread_rebuild(); + void start_thread(); + void stop_thread(); + void run_operation(KD_TREE_NODE **root, Operation_Logger_Type operation); + // KD Tree Functions and augmented variables + int Treesize_tmp = 0, Validnum_tmp = 0; + float alpha_bal_tmp = 0.5, alpha_del_tmp = 0.0; + float delete_criterion_param = 0.5f; + float balance_criterion_param = 0.7f; + float downsample_size = 0.2f; + bool Delete_Storage_Disabled = false; + KD_TREE_NODE *STATIC_ROOT_NODE = nullptr; + PointVector Points_deleted; + PointVector Downsample_Storage; + PointVector Multithread_Points_deleted; + void InitTreeNode(KD_TREE_NODE *root); + void Test_Lock_States(KD_TREE_NODE *root); + void BuildTree(KD_TREE_NODE **root, int l, int r, PointVector &Storage); + void Rebuild(KD_TREE_NODE **root); + int Delete_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild, bool is_downsample); + void Delete_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild); + void Add_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild, int father_axis); + void Add_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild); + void Search(KD_TREE_NODE *root, int k_nearest, PointType point, MANUAL_HEAP &q, float max_dist); //priority_queue + void Search_by_range(KD_TREE_NODE *root, BoxPointType boxpoint, PointVector &Storage); + void Search_by_radius(KD_TREE_NODE *root, PointType point, float radius, PointVector &Storage); + bool Criterion_Check(KD_TREE_NODE *root); + void Push_Down(KD_TREE_NODE *root); + void Update(KD_TREE_NODE *root); + void delete_tree_nodes(KD_TREE_NODE **root); + void downsample(KD_TREE_NODE **root); + bool same_point(PointType a, PointType b); + float calc_dist(PointType a, PointType b); + float calc_box_dist(KD_TREE_NODE *node, PointType point); + void ClearTree(); + static bool point_cmp_x(PointType a, PointType b); + static bool point_cmp_y(PointType a, PointType b); + static bool point_cmp_z(PointType a, PointType b); + +public: + KD_TREE(float delete_param = 0.5, float balance_param = 0.6, float box_length = 0.2); + ~KD_TREE(); + void Set_delete_criterion_param(float delete_param) + { + delete_criterion_param = delete_param; + } + void Set_balance_criterion_param(float balance_param) + { + balance_criterion_param = balance_param; + } + void set_downsample_param(float downsample_param) + { + downsample_size = downsample_param; + } + void InitializeKDTree(float delete_param = 0.5, float balance_param = 0.7, float box_length = 0.2); + int size(); + int validnum(); + void root_alpha(float &alpha_bal, float &alpha_del); + void Build(PointVector point_cloud); + void Nearest_Search(PointType point, int k_nearest, PointVector &Nearest_Points, vector &Point_Distance, float max_dist = INFINITY); + void Box_Search(const BoxPointType &Box_of_Point, PointVector &Storage); + void Radius_Search(PointType point, const float radius, PointVector &Storage); + int Add_Points(PointVector &PointToAdd, bool downsample_on); + void Add_Point_Boxes(vector &BoxPoints); + void Delete_Points(PointVector &PointToDel); + int Delete_Point_Boxes(vector &BoxPoints); + void flatten(KD_TREE_NODE *root, PointVector &Storage, delete_point_storage_set storage_type); + void acquire_removed_points(PointVector &removed_points); + BoxPointType tree_range(); + PointVector PCL_Storage; + KD_TREE_NODE *Root_Node = nullptr; + int max_queue_size = 0; +}; + +// template +// PointType KD_TREE::zeroP = PointType(0,0,0); diff --git a/src/FASTLIO2_ROS2/fastlio2/src/map_builder/imu_processor.cpp b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/imu_processor.cpp new file mode 100644 index 00000000..d5bc04a8 --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/imu_processor.cpp @@ -0,0 +1,132 @@ +#include "imu_processor.h" + +IMUProcessor::IMUProcessor(Config &config, std::shared_ptr kf) : m_config(config), m_kf(kf) +{ + m_Q.setIdentity(); + m_Q.block<3, 3>(0, 0) = M3D::Identity() * m_config.ng; + m_Q.block<3, 3>(3, 3) = M3D::Identity() * m_config.na; + m_Q.block<3, 3>(6, 6) = M3D::Identity() * m_config.nbg; + m_Q.block<3, 3>(9, 9) = M3D::Identity() * m_config.nba; + m_last_acc.setZero(); + m_last_gyro.setZero(); + m_imu_cache.clear(); + m_poses_cache.clear(); +} + +bool IMUProcessor::initialize(SyncPackage &package) +{ + m_imu_cache.insert(m_imu_cache.end(), package.imus.begin(), package.imus.end()); + if (m_imu_cache.size() < static_cast(m_config.imu_init_num)) + return false; + V3D acc_mean = V3D::Zero(); + V3D gyro_mean = V3D::Zero(); + for (const auto &imu : m_imu_cache) + { + acc_mean += imu.acc; + gyro_mean += imu.gyro; + } + acc_mean /= static_cast(m_imu_cache.size()); + gyro_mean /= static_cast(m_imu_cache.size()); + m_kf->x().r_il = m_config.r_il; + m_kf->x().t_il = m_config.t_il; + m_kf->x().bg = gyro_mean; + if (m_config.gravity_align) + { + m_kf->x().r_wi = (Eigen::Quaterniond::FromTwoVectors((-acc_mean).normalized(), V3D(0.0, 0.0, -1.0)).matrix()); + m_kf->x().initGravityDir(V3D(0, 0, -1.0)); + } + else + m_kf->x().initGravityDir(-acc_mean); + m_kf->P().setIdentity(); + m_kf->P().block<3, 3>(6, 6) = M3D::Identity() * 0.00001; + m_kf->P().block<3, 3>(9, 9) = M3D::Identity() * 0.00001; + m_kf->P().block<3, 3>(15, 15) = M3D::Identity() * 0.0001; + m_kf->P().block<3, 3>(18, 18) = M3D::Identity() * 0.0001; + + m_last_imu = m_imu_cache.back(); + m_last_propagate_end_time = package.cloud_end_time; + return true; +} + +void IMUProcessor::undistort(SyncPackage &package) +{ + + m_imu_cache.clear(); + m_imu_cache.push_back(m_last_imu); + m_imu_cache.insert(m_imu_cache.end(), package.imus.begin(), package.imus.end()); + + // const double imu_time_begin = m_imu_cache.front().time; + const double imu_time_end = m_imu_cache.back().time; + + const double cloud_time_begin = package.cloud_start_time; + const double propagate_time_end = package.cloud_end_time; + + m_poses_cache.clear(); + m_poses_cache.emplace_back(0.0, m_last_acc, m_last_gyro, m_kf->x().v, m_kf->x().t_wi, m_kf->x().r_wi); + + V3D acc_val, gyro_val; + double dt = 0.0; + Input inp; + inp.acc = m_imu_cache.back().acc; + inp.gyro = m_imu_cache.back().gyro; + for (auto it_imu = m_imu_cache.begin(); it_imu < (m_imu_cache.end() - 1); it_imu++) + { + IMUData &head = *it_imu; + IMUData &tail = *(it_imu + 1); + if (tail.time < m_last_propagate_end_time) + continue; + gyro_val = 0.5 * (head.gyro + tail.gyro); + acc_val = 0.5 * (head.acc + tail.acc); + + if (head.time < m_last_propagate_end_time) + dt = tail.time - m_last_propagate_end_time; + else + dt = tail.time - head.time; + + inp.acc = acc_val; + inp.gyro = gyro_val; + m_kf->predict(inp, dt, m_Q); + + m_last_gyro = gyro_val - m_kf->x().bg; + m_last_acc = m_kf->x().r_wi * (acc_val - m_kf->x().ba) + m_kf->x().g; + double offset = tail.time - cloud_time_begin; + m_poses_cache.emplace_back(offset, m_last_acc, m_last_gyro, m_kf->x().v, m_kf->x().t_wi, m_kf->x().r_wi); + } + + dt = propagate_time_end - imu_time_end; + m_kf->predict(inp, dt, m_Q); + m_last_imu = m_imu_cache.back(); + m_last_propagate_end_time = propagate_time_end; + + M3D cur_r_wi = m_kf->x().r_wi; + V3D cur_t_wi = m_kf->x().t_wi; + M3D cur_r_il = m_kf->x().r_il; + V3D cur_t_il = m_kf->x().t_il; + auto it_pcl = package.cloud->points.end() - 1; + + for (auto it_kp = m_poses_cache.end() - 1; it_kp != m_poses_cache.begin(); it_kp--) + { + auto head = it_kp - 1; + auto tail = it_kp; + + M3D imu_r_wi = head->rot; + V3D imu_t_wi = head->trans; + V3D imu_vel = head->vel; + V3D imu_acc = tail->acc; + V3D imu_gyro = tail->gyro; + + for (; it_pcl->curvature / double(1000) > head->offset; it_pcl--) + { + dt = it_pcl->curvature / double(1000) - head->offset; + V3D point(it_pcl->x, it_pcl->y, it_pcl->z); + M3D point_rot = imu_r_wi * Sophus::SO3d::exp(imu_gyro * dt).matrix(); + V3D point_pos = imu_t_wi + imu_vel * dt + 0.5 * imu_acc * dt * dt; + V3D p_compensate = cur_r_il.transpose() * (cur_r_wi.transpose() * (point_rot * (cur_r_il * point + cur_t_il) + point_pos - cur_t_wi) - cur_t_il); + it_pcl->x = p_compensate(0); + it_pcl->y = p_compensate(1); + it_pcl->z = p_compensate(2); + if (it_pcl == package.cloud->points.begin()) + break; + } + } +} \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/fastlio2/src/map_builder/imu_processor.h b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/imu_processor.h new file mode 100644 index 00000000..0751dfbb --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/imu_processor.h @@ -0,0 +1,24 @@ +#pragma once +#include "ieskf.h" +#include "commons.h" + +class IMUProcessor +{ +public: + IMUProcessor(Config &config, std::shared_ptr kf); + + bool initialize(SyncPackage &package); + + void undistort(SyncPackage &package); + +private: + Config m_config; + std::shared_ptr m_kf; + double m_last_propagate_end_time; + Vec m_imu_cache; + Vec m_poses_cache; + V3D m_last_acc; + V3D m_last_gyro; + M12D m_Q; + IMUData m_last_imu; +}; \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/fastlio2/src/map_builder/lidar_processor.cpp b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/lidar_processor.cpp new file mode 100644 index 00000000..674d35bc --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/lidar_processor.cpp @@ -0,0 +1,268 @@ +#include "lidar_processor.h" + +LidarProcessor::LidarProcessor(Config &config, std::shared_ptr kf) : m_config(config), m_kf(kf) +{ + m_ikdtree = std::make_shared>(); + m_ikdtree->set_downsample_param(m_config.map_resolution); + m_cloud_down_lidar.reset(new CloudType); + m_cloud_down_world.reset(new CloudType(10000, 1)); + m_norm_vec.reset(new CloudType(10000, 1)); + m_effect_cloud_lidar.reset(new CloudType(10000, 1)); + m_effect_norm_vec.reset(new CloudType(10000, 1)); + m_nearest_points.resize(10000); + m_point_selected_flag.resize(10000, false); + + if (m_config.scan_resolution > 0.0) + { + m_scan_filter.setLeafSize(m_config.scan_resolution, m_config.scan_resolution, m_config.scan_resolution); + } + + m_kf->setLossFunction([&](State &s, SharedState &d) + { updateLossFunc(s, d); }); + m_kf->setStopFunction([&](const V21D &delta) -> bool + { V3D rot_delta = delta.block<3, 1>(0, 0); + V3D t_delta = delta.block<3, 1>(3, 0); + return (rot_delta.norm() * 57.3 < 0.01) && (t_delta.norm() * 100 < 0.015); }); +} + +void LidarProcessor::trimCloudMap() +{ + m_local_map.cub_to_rm.clear(); + const State &state = m_kf->x(); + Eigen::Vector3d pos_lidar = state.t_wi + state.r_wi * state.t_il; + + if (!m_local_map.initialed) + { + for (int i = 0; i < 3; i++) + { + m_local_map.local_map_corner.vertex_min[i] = pos_lidar[i] - m_config.cube_len / 2.0; + m_local_map.local_map_corner.vertex_max[i] = pos_lidar[i] + m_config.cube_len / 2.0; + } + m_local_map.initialed = true; + return; + } + float dist_to_map_edge[3][2]; + bool need_move = false; + double det_thresh = m_config.move_thresh * m_config.det_range; + for (int i = 0; i < 3; i++) + { + dist_to_map_edge[i][0] = fabs(pos_lidar(i) - m_local_map.local_map_corner.vertex_min[i]); + dist_to_map_edge[i][1] = fabs(pos_lidar(i) - m_local_map.local_map_corner.vertex_max[i]); + + if (dist_to_map_edge[i][0] <= det_thresh || dist_to_map_edge[i][1] <= det_thresh) + need_move = true; + } + if (!need_move) + return; + BoxPointType new_corner, temp_corner; + new_corner = m_local_map.local_map_corner; + float mov_dist = std::max((m_config.cube_len - 2.0 * m_config.move_thresh * m_config.det_range) * 0.5 * 0.9, double(m_config.det_range * (m_config.move_thresh - 1))); + + for (int i = 0; i < 3; i++) + { + temp_corner = m_local_map.local_map_corner; + if (dist_to_map_edge[i][0] <= det_thresh) + { + new_corner.vertex_max[i] -= mov_dist; + new_corner.vertex_min[i] -= mov_dist; + temp_corner.vertex_min[i] = m_local_map.local_map_corner.vertex_max[i] - mov_dist; + m_local_map.cub_to_rm.push_back(temp_corner); + } + else if (dist_to_map_edge[i][1] <= det_thresh) + { + new_corner.vertex_max[i] += mov_dist; + new_corner.vertex_min[i] += mov_dist; + temp_corner.vertex_max[i] = m_local_map.local_map_corner.vertex_min[i] + mov_dist; + m_local_map.cub_to_rm.push_back(temp_corner); + } + } + m_local_map.local_map_corner = new_corner; + + PointVec points_history; + m_ikdtree->acquire_removed_points(points_history); + + // 删除局部地图之外的点云 + if (m_local_map.cub_to_rm.size() > 0) + m_ikdtree->Delete_Point_Boxes(m_local_map.cub_to_rm); + return; +} + +void LidarProcessor::incrCloudMap() +{ + if (m_cloud_down_lidar->empty()) + return; + const State &state = m_kf->x(); + int size = m_cloud_down_lidar->size(); + PointVec point_to_add; + PointVec point_no_need_downsample; + for (int i = 0; i < size; i++) + { + const PointType &p = m_cloud_down_lidar->points[i]; + Eigen::Vector3d point(p.x, p.y, p.z); + point = state.r_wi * (state.r_il * point + state.t_il) + state.t_wi; + m_cloud_down_world->points[i].x = point(0); + m_cloud_down_world->points[i].y = point(1); + m_cloud_down_world->points[i].z = point(2); + m_cloud_down_world->points[i].intensity = m_cloud_down_lidar->points[i].intensity; + // 如果该点附近没有近邻点则需要添加到地图中 + if (m_nearest_points[i].empty()) + { + point_to_add.push_back(m_cloud_down_world->points[i]); + continue; + } + + const PointVec &points_near = m_nearest_points[i]; + bool need_add = true; + PointType downsample_result, mid_point; + mid_point.x = std::floor(m_cloud_down_world->points[i].x / m_config.map_resolution) * m_config.map_resolution + 0.5 * m_config.map_resolution; + mid_point.y = std::floor(m_cloud_down_world->points[i].y / m_config.map_resolution) * m_config.map_resolution + 0.5 * m_config.map_resolution; + mid_point.z = std::floor(m_cloud_down_world->points[i].z / m_config.map_resolution) * m_config.map_resolution + 0.5 * m_config.map_resolution; + + // 如果该点所在的voxel没有点，则直接加入地图，且不需要降采样 + if (fabs(points_near[0].x - mid_point.x) > 0.5 * m_config.map_resolution && fabs(points_near[0].y - mid_point.y) > 0.5 * m_config.map_resolution && fabs(points_near[0].z - mid_point.z) > 0.5 * m_config.map_resolution) + { + point_no_need_downsample.push_back(m_cloud_down_world->points[i]); + continue; + } + float dist = sq_dist(m_cloud_down_world->points[i], mid_point); + + for (int readd_i = 0; readd_i < m_config.near_search_num; readd_i++) + { + // 如果该点的近邻点较少，则需要加入到地图中 + if (points_near.size() < static_cast(m_config.near_search_num)) + break; + // 如果该点的近邻点距离voxel中心点的距离比该点距离voxel中心点更近，则不需要加入该点 + if (sq_dist(points_near[readd_i], mid_point) < dist) + { + need_add = false; + break; + } + } + if (need_add) + point_to_add.push_back(m_cloud_down_world->points[i]); + } + m_ikdtree->Add_Points(point_to_add, true); + m_ikdtree->Add_Points(point_no_need_downsample, false); +} + +void LidarProcessor::initCloudMap(PointVec &point_vec) +{ + m_ikdtree->Build(point_vec); +} + +void LidarProcessor::process(SyncPackage &package) +{ + // m_kf->setLossFunction([&](State &s, SharedState &d) + // { updateLossFunc(s, d); }); + // m_kf->setStopFunction([&](const V21D &delta) -> bool + // { V3D rot_delta = delta.block<3, 1>(0, 0); + // V3D t_delta = delta.block<3, 1>(3, 0); + // return (rot_delta.norm() * 57.3 < 0.01) && (t_delta.norm() * 100 < 0.015); }); + if (m_config.scan_resolution > 0.0) + { + m_scan_filter.setInputCloud(package.cloud); + m_scan_filter.filter(*m_cloud_down_lidar); + } + else + { + pcl::copyPointCloud(*package.cloud, *m_cloud_down_lidar); + } + trimCloudMap(); + m_kf->update(); + incrCloudMap(); +} + +void LidarProcessor::updateLossFunc(State &state, SharedState &share_data) +{ + int size = m_cloud_down_lidar->size(); +#ifdef MP_EN + omp_set_num_threads(MP_PROC_NUM); +#pragma omp parallel for +#endif + for (int i = 0; i < size; i++) + { + PointType &point_body = m_cloud_down_lidar->points[i]; + PointType &point_world = m_cloud_down_world->points[i]; + Eigen::Vector3d point_body_vec(point_body.x, point_body.y, point_body.z); + Eigen::Vector3d point_world_vec = state.r_wi * (state.r_il * point_body_vec + state.t_il) + state.t_wi; + point_world.x = point_world_vec(0); + point_world.y = point_world_vec(1); + point_world.z = point_world_vec(2); + point_world.intensity = point_body.intensity; + std::vector point_sq_dist(m_config.near_search_num); + auto &points_near = m_nearest_points[i]; + m_ikdtree->Nearest_Search(point_world, m_config.near_search_num, points_near, point_sq_dist); + if (points_near.size() >= static_cast(m_config.near_search_num) && point_sq_dist[m_config.near_search_num - 1] <= 5) + m_point_selected_flag[i] = true; + else + m_point_selected_flag[i] = false; + if (!m_point_selected_flag[i]) + continue; + + Eigen::Vector4d pabcd; + m_point_selected_flag[i] = false; + if (esti_plane(points_near, 0.1, pabcd)) + { + double pd2 = pabcd(0) * point_world_vec(0) + pabcd(1) * point_world_vec(1) + pabcd(2) * point_world_vec(2) + pabcd(3); + double s = 1 - 0.9 * std::fabs(pd2) / std::sqrt(point_body_vec.norm()); + if (s > 0.9) + { + m_point_selected_flag[i] = true; + m_norm_vec->points[i].x = pabcd(0); + m_norm_vec->points[i].y = pabcd(1); + m_norm_vec->points[i].z = pabcd(2); + m_norm_vec->points[i].intensity = pd2; + } + } + } + + int effect_feat_num = 0; + for (int i = 0; i < size; i++) + { + if (!m_point_selected_flag[i]) + continue; + m_effect_cloud_lidar->points[effect_feat_num] = m_cloud_down_lidar->points[i]; + m_effect_norm_vec->points[effect_feat_num] = m_norm_vec->points[i]; + effect_feat_num++; + } + if (effect_feat_num < 1) + { + share_data.valid = false; + std::cerr << "NO Effective Points!" << std::endl; + return; + } + share_data.valid = true; + share_data.H.setZero(); + share_data.b.setZero(); + Eigen::Matrix J; + for (int i = 0; i < effect_feat_num; i++) + { + J.setZero(); + const PointType &laser_p = m_effect_cloud_lidar->points[i]; + const PointType &norm_p = m_effect_norm_vec->points[i]; + Eigen::Vector3d laser_p_vec(laser_p.x, laser_p.y, laser_p.z); + Eigen::Vector3d norm_vec(norm_p.x, norm_p.y, norm_p.z); + Eigen::Matrix B = -norm_vec.transpose() * state.r_wi * Sophus::SO3d::hat(state.r_il * laser_p_vec + state.t_wi); + J.block<1, 3>(0, 0) = B; + J.block<1, 3>(0, 3) = norm_vec.transpose(); + if (m_config.esti_il) + { + Eigen::Matrix C = -norm_vec.transpose() * state.r_wi * state.r_il * Sophus::SO3d::hat(laser_p_vec); + Eigen::Matrix D = norm_vec.transpose() * state.r_wi; + J.block<1, 3>(0, 6) = C; + J.block<1, 3>(0, 9) = D; + } + share_data.H += J.transpose() * m_config.lidar_cov_inv * J; + share_data.b += J.transpose() * m_config.lidar_cov_inv * norm_p.intensity; + } +} + +CloudType::Ptr LidarProcessor::transformCloud(CloudType::Ptr inp, const M3D &r, const V3D &t) +{ + Eigen::Matrix4f transform = Eigen::Matrix4f::Identity(); + transform.block<3, 3>(0, 0) = r.cast(); + transform.block<3, 1>(0, 3) = t.cast(); + CloudType::Ptr ret(new CloudType); + pcl::transformPointCloud(*inp, *ret, transform); + return ret; +} \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/fastlio2/src/map_builder/lidar_processor.h b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/lidar_processor.h new file mode 100644 index 00000000..34826940 --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/lidar_processor.h @@ -0,0 +1,48 @@ +#pragma once +#include "commons.h" +#include "ieskf.h" +#include "ikd_Tree.h" +#include +#include + +struct LocalMap +{ + bool initialed = false; + BoxPointType local_map_corner; + Vec cub_to_rm; +}; + +class LidarProcessor +{ +public: + LidarProcessor(Config &config, std::shared_ptr kf); + + void trimCloudMap(); + + void incrCloudMap(); + + void initCloudMap(PointVec &point_vec); + + void process(SyncPackage &package); + + void updateLossFunc(State &state, SharedState &share_data); + + static CloudType::Ptr transformCloud(CloudType::Ptr inp, const M3D &r, const V3D &t); + M3D r_wl() { return m_kf->x().r_wi * m_kf->x().r_il; } + V3D t_wl() { return m_kf->x().t_wi + m_kf->x().r_wi * m_kf->x().t_il; } + +private: + Config m_config; + LocalMap m_local_map; + std::shared_ptr m_kf; + std::shared_ptr> m_ikdtree; + CloudType::Ptr m_cloud_lidar; + CloudType::Ptr m_cloud_down_lidar; + CloudType::Ptr m_cloud_down_world; + std::vector m_point_selected_flag; + CloudType::Ptr m_norm_vec; + CloudType::Ptr m_effect_cloud_lidar; + CloudType::Ptr m_effect_norm_vec; + std::vector m_nearest_points; + pcl::VoxelGrid m_scan_filter; +}; diff --git a/src/FASTLIO2_ROS2/fastlio2/src/map_builder/map_builder.cpp b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/map_builder.cpp new file mode 100644 index 00000000..74a9329d --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/map_builder.cpp @@ -0,0 +1,29 @@ +#include "map_builder.h" +MapBuilder::MapBuilder(Config &config, std::shared_ptr kf) : m_config(config), m_kf(kf) +{ + m_imu_processor = std::make_shared(config, kf); + m_lidar_processor = std::make_shared(config, kf); + m_status = BuilderStatus::IMU_INIT; +} + +void MapBuilder::process(SyncPackage &package) +{ + if (m_status == BuilderStatus::IMU_INIT) + { + if (m_imu_processor->initialize(package)) + m_status = BuilderStatus::MAP_INIT; + return; + } + + m_imu_processor->undistort(package); + + if (m_status == BuilderStatus::MAP_INIT) + { + CloudType::Ptr cloud_world = LidarProcessor::transformCloud(package.cloud, m_lidar_processor->r_wl(), m_lidar_processor->t_wl()); + m_lidar_processor->initCloudMap(cloud_world->points); + m_status = BuilderStatus::MAPPING; + return; + } + + m_lidar_processor->process(package); +} \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/fastlio2/src/map_builder/map_builder.h b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/map_builder.h new file mode 100644 index 00000000..baa0fba2 --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/map_builder.h @@ -0,0 +1,27 @@ +#pragma once +#include "imu_processor.h" +#include "lidar_processor.h" + +enum BuilderStatus +{ + IMU_INIT, + MAP_INIT, + MAPPING +}; + +class MapBuilder +{ +public: + MapBuilder(Config &config, std::shared_ptr kf); + + void process(SyncPackage &package); + BuilderStatus status() { return m_status; } + std::shared_ptr lidar_processor(){return m_lidar_processor;} + +private: + Config m_config; + BuilderStatus m_status; + std::shared_ptr m_kf; + std::shared_ptr m_imu_processor; + std::shared_ptr m_lidar_processor; +}; diff --git a/src/FASTLIO2_ROS2/fastlio2/src/utils.cpp b/src/FASTLIO2_ROS2/fastlio2/src/utils.cpp new file mode 100644 index 00000000..1e358e62 --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/src/utils.cpp @@ -0,0 +1,39 @@ +#include "utils.h" +pcl::PointCloud::Ptr Utils::livox2PCL(const livox_ros_driver2::msg::CustomMsg::SharedPtr msg, int filter_num, double min_range, double max_range) +{ + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + int point_num = msg->point_num; + cloud->reserve(point_num / filter_num + 1); + for (int i = 0; i < point_num; i += filter_num) + { + if ((msg->points[i].line < 4) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00)) + { + + float x = msg->points[i].x; + float y = msg->points[i].y; + float z = msg->points[i].z; + if (x * x + y * y + z * z < min_range * min_range || x * x + y * y + z * z > max_range * max_range) + continue; + pcl::PointXYZINormal p; + p.x = x; + p.y = y; + p.z = z; + p.intensity = msg->points[i].reflectivity; + p.curvature = msg->points[i].offset_time / 1000000.0f; + cloud->push_back(p); + } + } + return cloud; +} + +double Utils::getSec(std_msgs::msg::Header &header) +{ + return static_cast(header.stamp.sec) + static_cast(header.stamp.nanosec) * 1e-9; +} +builtin_interfaces::msg::Time Utils::getTime(const double &sec) +{ + builtin_interfaces::msg::Time time_msg; + time_msg.sec = static_cast(sec); + time_msg.nanosec = static_cast((sec - time_msg.sec) * 1e9); + return time_msg; +} diff --git a/src/FASTLIO2_ROS2/fastlio2/src/utils.h b/src/FASTLIO2_ROS2/fastlio2/src/utils.h new file mode 100644 index 00000000..0fe11d69 --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/src/utils.h @@ -0,0 +1,29 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include + +#define RESET "\033[0m" +#define BLACK "\033[30m" /* Black */ +#define RED "\033[31m" /* Red */ +#define GREEN "\033[32m" /* Green */ +#define YELLOW "\033[33m" /* Yellow */ +#define BLUE "\033[34m" /* Blue */ +#define PURPLE "\033[35m" /* Purple */ +#define CYAN "\033[36m" /* Cyan */ +#define WHITE "\033[37m" /* White */ + +class Utils +{ +public: + // static pcl::PointCloud::Ptr convertToPCL(const sensor_msgs::msg::PointCloud2 &msg); + // static sensor_msgs::msg::PointCloud2 convertToROS(const pcl::PointCloud::Ptr &cloud); + static double getSec(std_msgs::msg::Header &header); + static pcl::PointCloud::Ptr livox2PCL(const livox_ros_driver2::msg::CustomMsg::SharedPtr msg, int filter_num, double min_range = 0.5, double max_range = 20.0); + static builtin_interfaces::msg::Time getTime(const double& sec); +}; \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/hba/CMakeLists.txt b/src/FASTLIO2_ROS2/hba/CMakeLists.txt new file mode 100644 index 00000000..98c76275 --- /dev/null +++ b/src/FASTLIO2_ROS2/hba/CMakeLists.txt @@ -0,0 +1,68 @@ +cmake_minimum_required(VERSION 3.8) +project(hba) + +set(CMAKE_BUILD_TYPE "Release") +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED True) +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" ) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread -fexceptions") + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-O3) + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +add_compile_definitions(SOPHUS_USE_BASIC_LOGGING) +add_definitions(-DMP_EN) +add_definitions(-DMP_PROC_NUM=4) + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(interface REQUIRED) +find_package(pcl_conversions REQUIRED) + + +find_package(GTSAM REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(PCL REQUIRED) +find_package(yaml-cpp REQUIRED) +find_package(OpenMP QUIET) +find_package(Sophus REQUIRED) + +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") + +include_directories( + ${EIGEN3_INCLUDE_DIR} + ${PCL_INCLUDE_DIRS} +) + +set(SRC_LIST src/hba/commons.cpp + src/hba/blam.cpp + src/hba/hba.cpp) + +add_executable(hba_node src/hba_node.cpp ${SRC_LIST}) +ament_target_dependencies(hba_node rclcpp std_msgs sensor_msgs interface pcl_conversions GTSAM) +target_link_libraries(hba_node ${PCL_LIBRARIES} gtsam yaml-cpp) + +install(TARGETS hba_node DESTINATION lib/${PROJECT_NAME}) +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/FASTLIO2_ROS2/hba/LICENSE b/src/FASTLIO2_ROS2/hba/LICENSE new file mode 100644 index 00000000..30e8e2ec --- /dev/null +++ b/src/FASTLIO2_ROS2/hba/LICENSE @@ -0,0 +1,17 @@ +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 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/src/FASTLIO2_ROS2/hba/config/hba.yaml b/src/FASTLIO2_ROS2/hba/config/hba.yaml new file mode 100644 index 00000000..714a470d --- /dev/null +++ b/src/FASTLIO2_ROS2/hba/config/hba.yaml @@ -0,0 +1,11 @@ +scan_resolution: 0.1 + +window_size: 20 +stride: 10 +voxel_size: 0.5 +min_point_num: 10 +max_layer: 3 +plane_thresh: 0.01 +ba_max_iter: 10 +hba_iter: 5 +down_sample: 0.1 \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/hba/launch/hba_launch.py b/src/FASTLIO2_ROS2/hba/launch/hba_launch.py new file mode 100644 index 00000000..b30137d6 --- /dev/null +++ b/src/FASTLIO2_ROS2/hba/launch/hba_launch.py @@ -0,0 +1,32 @@ +import launch +import launch_ros.actions +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + rviz_cfg = PathJoinSubstitution([FindPackageShare("hba"), "rviz", "hba.rviz"]) + config_path = PathJoinSubstitution([FindPackageShare("hba"), "config", "hba.yaml"]) + + return launch.LaunchDescription( + [ + launch_ros.actions.Node( + package="hba", + namespace="hba", + executable="hba_node", + name="hba", + output="screen", + parameters=[ + {"config_path": config_path.perform(launch.LaunchContext())} + ], + ), + launch_ros.actions.Node( + package="rviz2", + namespace="hba", + executable="rviz2", + name="rviz2", + output="screen", + arguments=["-d", rviz_cfg.perform(launch.LaunchContext())], + ), + ] + ) diff --git a/src/FASTLIO2_ROS2/hba/package.xml b/src/FASTLIO2_ROS2/hba/package.xml new file mode 100644 index 00000000..c594cdf5 --- /dev/null +++ b/src/FASTLIO2_ROS2/hba/package.xml @@ -0,0 +1,25 @@ + + + + hba + 0.0.0 + TODO: Package description + liangheming + MIT + + ament_cmake + + rclcpp + std_msgs + GTSAM + sensor_msgs + interface + pcl_conversions + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/FASTLIO2_ROS2/hba/rviz/hba.rviz b/src/FASTLIO2_ROS2/hba/rviz/hba.rviz new file mode 100644 index 00000000..1b26193e --- /dev/null +++ b/src/FASTLIO2_ROS2/hba/rviz/hba.rviz @@ -0,0 +1,170 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PointCloud21 + Splitter Ratio: 0.5 + Tree Height: 1079 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 0.800000011920929 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: true + Max Color: 255; 255; 255 + Max Intensity: 255 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 1 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /hba/map_points + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: map + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Angle: 0.2099999338388443 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 49.88230895996094 + Target Frame: + Value: TopDownOrtho (rviz_default_plugins) + X: 5.11167049407959 + Y: 2.8143327236175537 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1376 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000004c2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004c2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004c2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000004c2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009ba0000003efc0100000002fb0000000800540069006d00650100000000000009ba000002fb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000749000004c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2490 + X: 70 + Y: 27 diff --git a/src/FASTLIO2_ROS2/hba/src/hba/blam.cpp b/src/FASTLIO2_ROS2/hba/src/hba/blam.cpp new file mode 100644 index 00000000..ac24a4a0 --- /dev/null +++ b/src/FASTLIO2_ROS2/hba/src/hba/blam.cpp @@ -0,0 +1,458 @@ +#include "blam.h" +VoxelKey VoxelKey::index(double x, double y, double z, double resolution, double bias) +{ + V3D point(x, y, z); + V3D idx = (point / resolution + V3D(bias, bias, bias)).array().floor(); + return VoxelKey(static_cast(idx(0)), static_cast(idx(1)), static_cast(idx(2))); +} +OctoTree::OctoTree(int layer, int max_layer, int min_point_num, double plane_thresh, double quater_len, const V3D ¢er) + : m_layer(layer), m_max_layer(max_layer), m_min_point_num(min_point_num), m_plane_thresh(plane_thresh), m_quater_len(quater_len), m_is_valid(false), m_is_plane(false), m_center(center) +{ + m_leaves.resize(8, nullptr); +} +void OctoTree::insert(const PointType &point) +{ + m_points.push_back(point); +} +void OctoTree::buildPlanes() +{ + if (static_cast(m_points.size()) < m_min_point_num) + { + m_is_plane = false; + m_is_valid = false; + return; + } + + V3D mean = V3D::Zero(); + M3D cov = M3D::Zero(); + for (PointType &point : m_points) + { + V3D p_vec(point.x, point.y, point.z); + mean += p_vec; + cov += p_vec * p_vec.transpose(); + } + mean /= static_cast(m_points.size()); + cov = cov / static_cast(m_points.size()) - mean * mean.transpose(); + Eigen::SelfAdjointEigenSolver eigensolver(cov); + m_mean = mean; + m_eigen_val = eigensolver.eigenvalues(); + m_eigen_vec = eigensolver.eigenvectors(); + if (m_eigen_val[0] < m_plane_thresh) + { + m_mean = mean; + m_is_plane = true; + m_is_valid = true; + doMergePoints(); + return; + } + m_is_plane = false; + m_is_valid = splitPlanes(); +} +void OctoTree::doMergePoints() +{ + assert(m_is_valid && m_is_plane); + std::unordered_map id_point_map; + for (PointType &point : m_points) + { + uint32_t id = point.id; + if (id_point_map.find(id) == id_point_map.end()) + { + id_point_map[id] = point; + id_point_map[id].time = 1.0; + } + else + { + id_point_map[id].x += point.x; + id_point_map[id].y += point.y; + id_point_map[id].z += point.z; + id_point_map[id].intensity += point.intensity; + id_point_map[id].lx += point.lx; + id_point_map[id].ly += point.ly; + id_point_map[id].lz += point.lz; + id_point_map[id].time += 1.0; + } + } + + PointVec().swap(m_merged_points); + for (auto &iter : id_point_map) + { + PointType &point = iter.second; + point.x /= point.time; + point.y /= point.time; + point.z /= point.time; + point.intensity /= point.time; + point.lx /= point.time; + point.ly /= point.time; + point.lz /= point.time; + point.time = 1.0; + m_merged_points.push_back(point); + } +} +bool OctoTree::splitPlanes() +{ + if (m_layer >= m_max_layer - 1) + return false; + for (PointType &point : m_points) + { + int xyz[3] = {0, 0, 0}; + if (point.x > m_center.x()) + xyz[0] = 1; + if (point.y > m_center.y()) + xyz[1] = 1; + if (point.z > m_center.z()) + xyz[2] = 1; + + int leaf_num = 4 * xyz[0] + 2 * xyz[1] + xyz[2]; + if (m_leaves[leaf_num] == nullptr) + { + V3D shift((2 * xyz[0] - 1) * m_quater_len, (2 * xyz[1] - 1) * m_quater_len, (2 * xyz[2] - 1) * m_quater_len); + V3D center = m_center + shift; + m_leaves[leaf_num].reset(new OctoTree(m_layer + 1, m_max_layer, m_min_point_num, m_plane_thresh, m_quater_len / 2.0, center)); + } + m_leaves[leaf_num]->insert(point); + } + + PointVec().swap(m_points); + PointVec().swap(m_merged_points); + + bool has_sub_plane = false; + for (auto &leaf : m_leaves) + { + if (leaf != nullptr) + { + leaf->buildPlanes(); + if (leaf->isValid()) + has_sub_plane = true; + } + } + return has_sub_plane; +} +void OctoTree::getPlanes(Vec &planes) +{ + if (m_is_plane) + { + planes.push_back(this); + return; + } + if (!m_is_valid) + return; + + for (auto &leaf : m_leaves) + { + if (leaf != nullptr) + leaf->getPlanes(planes); + } +} +int OctoTree::planeCount() +{ + if (m_is_plane) + return 1; + + if (!m_is_valid) + return 0; + + int count = 0; + for (auto &leaf : m_leaves) + { + if (leaf != nullptr) + count += leaf->planeCount(); + } + return count; +} +double OctoTree::updateByPose(const Vec &poses) +{ + assert(m_is_valid && m_is_plane); + for (PointType &point : m_merged_points) + { + V3D p_vec(point.lx, point.ly, point.lz); + const Pose &pose = poses[point.id]; + p_vec = pose.r * p_vec + pose.t; + point.x = p_vec(0); + point.y = p_vec(1); + point.z = p_vec(2); + } + + V3D mean = V3D::Zero(); + M3D cov = M3D::Zero(); + for (PointType &point : m_points) + { + V3D p_vec(point.lx, point.ly, point.lz); + const Pose &pose = poses[point.id]; + p_vec = pose.r * p_vec + pose.t; + point.x = p_vec(0); + point.y = p_vec(1); + point.z = p_vec(2); + mean += p_vec; + cov += p_vec * p_vec.transpose(); + } + mean /= static_cast(m_points.size()); + cov = cov / static_cast(m_points.size()) - mean * mean.transpose(); + Eigen::SelfAdjointEigenSolver eigensolver(cov); + m_mean = mean; + m_eigen_val = eigensolver.eigenvalues(); + m_eigen_vec = eigensolver.eigenvectors(); + return m_eigen_val[0]; +} +double OctoTree::evalByPose(const Vec &poses) +{ + assert(m_is_valid && m_is_plane); + V3D mean = V3D::Zero(); + M3D cov = M3D::Zero(); + for (PointType &point : m_points) + { + V3D p_vec(point.lx, point.ly, point.lz); + const Pose &pose = poses[point.id]; + p_vec = pose.r * p_vec + pose.t; + point.x = p_vec(0); + point.y = p_vec(1); + point.z = p_vec(2); + mean += p_vec; + cov += p_vec * p_vec.transpose(); + } + mean /= static_cast(m_points.size()); + cov = cov / static_cast(m_points.size()) - mean * mean.transpose(); + Eigen::SelfAdjointEigenSolver eigensolver(cov); + return eigensolver.eigenvalues()[0]; +} +M3D OctoTree::fp(const V3D &p) +{ + M3D ret = M3D::Zero(); + ret.row(1) = (p - m_mean).transpose() * (m_eigen_vec.col(1) * m_eigen_vec.col(0).transpose() + m_eigen_vec.col(0) * m_eigen_vec.col(1).transpose()) / static_cast(m_points.size()) / (m_eigen_val(0) - m_eigen_val(1)); + ret.row(2) = (p - m_mean).transpose() * (m_eigen_vec.col(2) * m_eigen_vec.col(0).transpose() + m_eigen_vec.col(0) * m_eigen_vec.col(2).transpose()) / static_cast(m_points.size()) / (m_eigen_val(0) - m_eigen_val(2)); + return ret; +} +V3D OctoTree::dp(const V3D &p) +{ + return 2.0 / static_cast(m_points.size()) * ((p - m_mean).transpose() * m_eigen_vec.col(0) * m_eigen_vec.col(0).transpose()).transpose(); +} +M3D OctoTree::dp2(const V3D &p1, const V3D &p2, bool equal) +{ + double n = static_cast(m_points.size()); + V3D u0 = m_eigen_vec.col(0); + if (equal) + { + return 2.0 / n * ((n - 1) / n * u0 * u0.transpose() + u0 * (p1 - m_mean).transpose() * m_eigen_vec * fp(p2) + m_eigen_vec * fp(p2) * (u0.transpose() * (p1 - m_mean))); + } + else + { + return 2.0 / n * (-1.0 / n * u0 * u0.transpose() + u0 * (p1 - m_mean).transpose() * m_eigen_vec * fp(p2) + m_eigen_vec * fp(p2) * (u0.transpose() * (p1 - m_mean))); + } +} +BLAM::BLAM(const BLAMConfig &config) : m_config(config) +{ + Vec().swap(m_poses); + Vec::Ptr>().swap(m_clouds); +} +void BLAM::insert(pcl::PointCloud::Ptr &cloud, const Pose &pose) +{ + m_poses.push_back(pose); + m_clouds.push_back(cloud); +} + +void BLAM::addPoint(const PointType &point) +{ + VoxelKey loc = VoxelKey::index(point.x, point.y, point.z, m_config.voxel_size, 0.0); + if (m_voxel_map.find(loc) == m_voxel_map.end()) + { + V3D center = V3D((0.5 + loc.x) * m_config.voxel_size, (0.5 + loc.y) * m_config.voxel_size, (0.5 + loc.z) * m_config.voxel_size); + double quater_len = m_config.voxel_size / 4.0; + m_voxel_map[loc] = std::make_shared(0, m_config.max_layer, m_config.min_point_num, m_config.plane_thresh, quater_len, center); + } + m_voxel_map[loc]->insert(point); +} +void BLAM::buildVoxels() +{ + + Vec().swap(m_planes); + VoxelMap().swap(m_voxel_map); + for (size_t i = 0; i < m_clouds.size(); i++) + { + pcl::PointCloud::Ptr cloud = m_clouds[i]; + const Pose &pose = m_poses[i]; + for (pcl::PointXYZI &point : cloud->points) + { + PointType p; + V3D p_vec(point.x, point.y, point.z); + p_vec = pose.r * p_vec + pose.t; + p.x = static_cast(p_vec.x()); + p.y = static_cast(p_vec.y()); + p.z = static_cast(p_vec.z()); + p.lx = point.x; + p.ly = point.y; + p.lz = point.z; + p.intensity = point.intensity; + p.id = static_cast(i); + p.time = 1.0; + addPoint(p); + } + } + + for (auto &iter : m_voxel_map) + { + iter.second->buildPlanes(); + iter.second->getPlanes(m_planes); + } +} +void BLAM::updateJaccAndHess() +{ + m_H.setZero(); + m_J.setZero(); + for (size_t i = 0; i < m_planes.size(); i++) + { + OctoTree *plane_ptr = m_planes[i]; + const PointVec &points = plane_ptr->mergedPoints(); + Eigen::MatrixXd D_i = Eigen::MatrixXd::Zero(3 * points.size(), 6 * points.size()); + Eigen::MatrixXd H_i = Eigen::MatrixXd::Zero(3 * points.size(), 3 * points.size()); + Eigen::MatrixXd J_i = Eigen::MatrixXd::Zero(1, 3 * points.size()); + Vec idxs(points.size(), 0); + for (size_t m = 0; m < points.size(); m++) + { + const PointType &point1 = points[m]; + V3D p1(point1.x, point1.y, point1.z); + V3D pl1(point1.lx, point1.ly, point1.lz); + idxs[m] = point1.id; + Pose &pose = m_poses[point1.id]; + for (size_t n = 0; n < points.size(); n++) + { + const PointType &point2 = points[n]; + V3D p2(point2.x, point2.y, point2.z); + H_i.block<3, 3>(m * 3, n * 3) = plane_ptr->dp2(p1, p2, m == n); + } + J_i.block<1, 3>(0, m * 3) = plane_ptr->dp(p1).transpose(); + D_i.block<3, 3>(m * 3, m * 3) = -pose.r * Sophus::SO3d::hat(pl1); + D_i.block<3, 3>(m * 3, m * 3 + 3) = M3D::Identity(); + } + Eigen::MatrixXd H_bar = D_i.transpose() * H_i * D_i; // 6 n * 6 n + Eigen::VectorXd J_bar = (J_i * D_i).transpose(); + + for (size_t m = 0; m < idxs.size(); m++) + { + uint32_t p_id1 = idxs[m]; + for (size_t n = m; n < idxs.size(); n++) + { + uint32_t p_id2 = idxs[n]; + m_H.block<6, 6>(p_id1 * 6, p_id2 * 6) += H_bar.block<6, 6>(m * 6, n * 6); + if (m == n) + continue; + m_H.block<6, 6>(p_id2 * 6, p_id1 * 6) += H_bar.block<6, 6>(n * 6, m * 6); + } + m_J.block<6, 1>(p_id1 * 6, 0) += J_bar.block<6, 1>(m * 6, 0); + } + } +} +void BLAM::optimize() +{ + buildVoxels(); + Eigen::MatrixXd D; + D.resize(m_poses.size() * 6, m_poses.size() * 6); + m_H.resize(m_poses.size() * 6, m_poses.size() * 6); + m_J.resize(m_poses.size() * 6); + double residual = 0.0; + bool build_hess = true; + double u = 0.01, v = 2.0; + + for (size_t i = 0; i < m_config.max_iter; i++) + { + if (build_hess) + { + residual = updatePlanesByPoses(m_poses); + updateJaccAndHess(); + } + D = m_H.diagonal().asDiagonal(); + Eigen::MatrixXd Hess = m_H + u * D; + Eigen::VectorXd delta = Hess.colPivHouseholderQr().solve(-m_J); + Vec temp_pose(m_poses.begin(), m_poses.end()); + plusDelta(temp_pose, delta); + double residual_new = evalPlanesByPoses(temp_pose); + double pho = (residual - residual_new) / (0.5 * (delta.transpose() * (u * D * delta - m_J))[0]); + if (pho > 0) + { + build_hess = true; + // std::cout << "ITER : " << i << " LM ITER UPDATE" << std::endl; + m_poses = temp_pose; + v = 2.0; + double q = 1 - pow(2 * pho - 1, 3); + u *= (q < 0.33333333 ? 0.33333333 : q); + } + else + { + build_hess = false; + u *= v; + v *= 2; + } + if (std::abs(residual - residual_new) < 1e-9) + break; + } +} + +double BLAM::updatePlanesByPoses(const Vec &poses) +{ + double residual = 0.0; + for (size_t i = 0; i < m_planes.size(); i++) + { + OctoTree *plane = m_planes[i]; + residual += plane->updateByPose(poses); + } + return residual; +} + +double BLAM::evalPlanesByPoses(const Vec &poses) +{ + double residual = 0.0; + for (size_t i = 0; i < m_planes.size(); i++) + { + OctoTree *plane = m_planes[i]; + residual += plane->evalByPose(poses); + } + return residual; +} + +int BLAM::planeCount(bool with_sub_planes) +{ + int count = 0; + for (auto &iter : m_voxel_map) + { + if (with_sub_planes) + count += iter.second->planeCount(); + else if (iter.second->isPlane()) + count += 1; + } + return count; +} + +void BLAM::plusDelta(Vec &poses, const Eigen::VectorXd &x) +{ + assert(static_cast(x.rows()) == poses.size() * 6); + for (size_t i = 0; i < poses.size(); i++) + { + M3D &r = poses[i].r; + V3D &t = poses[i].t; + r = r * Sophus::SO3d::exp(x.segment<3>(i * 6)).matrix(); + t += x.segment<3>(i * 6 + 3); + } +} + +pcl::PointCloud::Ptr BLAM::getLocalCloud() +{ + pcl::PointCloud::Ptr ret(new pcl::PointCloud); + Pose &first_pose = m_poses[0]; + + for (OctoTree *plane : m_planes) + { + for (const PointType &point : plane->points()) + { + pcl::PointXYZI p; + Pose &cur_pose = m_poses[point.id]; + M3D r_fc = first_pose.r.transpose() * cur_pose.r; + V3D t_fc = first_pose.r.transpose() * (cur_pose.t - first_pose.t); + V3D p_vec(point.lx, point.ly, point.lz); + p_vec = r_fc * p_vec + t_fc; + p.x = p_vec[0]; + p.y = p_vec[1]; + p.z = p_vec[2]; + p.intensity = point.intensity; + ret->push_back(p); + } + } + return ret; +} \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/hba/src/hba/blam.h b/src/FASTLIO2_ROS2/hba/src/hba/blam.h new file mode 100644 index 00000000..7dca2ee8 --- /dev/null +++ b/src/FASTLIO2_ROS2/hba/src/hba/blam.h @@ -0,0 +1,140 @@ +#pragma once +#include +#include +#include +#include + +#include "commons.h" + +#define HASH_P 116101 +#define MAX_N 10000000000 + +struct Pose +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + M3D r; + V3D t; +}; + +class VoxelKey +{ +public: + int64_t x, y, z; + VoxelKey(int64_t _x = 0, int64_t _y = 0, int64_t _z = 0) : x(_x), y(_y), z(_z) {} + + static VoxelKey index(double x, double y, double z, double resolution, double bias = 0.0); + bool operator==(const VoxelKey &other) const + { + return (x == other.x && y == other.y && z == other.z); + } + struct Hasher + { + int64_t operator()(const VoxelKey &k) const + { + return ((((k.z) * HASH_P) % MAX_N + (k.y)) * HASH_P) % MAX_N + (k.x); + } + }; +}; + +class OctoTree +{ +public: + OctoTree(int layer, int max_layer, int min_point_num, double plane_thresh, double quater_len, const V3D ¢er); + bool isPlane() { return m_is_plane; } + bool isValid() { return m_is_valid; } + double &quaterLength() { return m_quater_len; } + V3D ¢er() { return m_center; } + V3D &mean() { return m_mean; } + V3D &eigenVal() { return m_eigen_val; } + M3D &eigenVec() { return m_eigen_vec; } + PointVec &points() { return m_points; } + PointVec &mergedPoints() { return m_merged_points; } + + void insert(const PointType &point); + + void buildPlanes(); + + bool splitPlanes(); + + void doMergePoints(); + + void getPlanes(Vec &planes); + + int planeCount(); + + double updateByPose(const Vec &poses); + + double evalByPose(const Vec &poses); + + M3D fp(const V3D &p); + V3D dp(const V3D &p); + M3D dp2(const V3D &p1, const V3D &p2, bool equal); + +private: + int m_layer; + int m_max_layer; + int m_min_point_num; + double m_plane_thresh; + double m_quater_len; + bool m_is_valid; + bool m_is_plane; + V3D m_center; + V3D m_mean; + V3D m_eigen_val; + M3D m_eigen_vec; + Vec> m_leaves; + PointVec m_points; + PointVec m_merged_points; +}; + +struct BLAMConfig +{ + double voxel_size = 1.0; + int min_point_num = 10; + int max_layer = 4; + double plane_thresh = 0.01; + size_t max_iter = 10; +}; + +using VoxelMap = std::unordered_map, VoxelKey::Hasher>; + +class BLAM +{ +public: + BLAM(const BLAMConfig &config); + BLAMConfig &config() { return m_config; } + VoxelMap &voxelMap() { return m_voxel_map; } + Vec &poses() { return m_poses; } + Vec &planes() { return m_planes; } + Eigen::MatrixXd &H() { return m_H; } + Eigen::VectorXd &J() { return m_J; } + + void insert(pcl::PointCloud::Ptr &cloud, const Pose &pose); + + void addPoint(const PointType &point); + + void buildVoxels(); + + void updateJaccAndHess(); + + void optimize(); + + double updatePlanesByPoses(const Vec &poses); + + double evalPlanesByPoses(const Vec &poses); + + int planeCount(bool with_sub_planes = false); + + static void plusDelta(Vec &poses, const Eigen::VectorXd &x); + + pcl::PointCloud::Ptr getLocalCloud(); + +private: + BLAMConfig m_config; + Vec m_poses; + VoxelMap m_voxel_map; + Vec m_planes; + Vec::Ptr> m_clouds; + Eigen::MatrixXd m_H; + Eigen::VectorXd m_J; +}; diff --git a/src/FASTLIO2_ROS2/hba/src/hba/commons.cpp b/src/FASTLIO2_ROS2/hba/src/hba/commons.cpp new file mode 100644 index 00000000..cb3a78a1 --- /dev/null +++ b/src/FASTLIO2_ROS2/hba/src/hba/commons.cpp @@ -0,0 +1 @@ +#include "commons.h" \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/hba/src/hba/commons.h b/src/FASTLIO2_ROS2/hba/src/hba/commons.h new file mode 100644 index 00000000..30dc7efb --- /dev/null +++ b/src/FASTLIO2_ROS2/hba/src/hba/commons.h @@ -0,0 +1,38 @@ +#pragma once +#include +#include +#include + +using M3D = Eigen::Matrix3d; +using V3D = Eigen::Vector3d; +using M3F = Eigen::Matrix3f; +using V3F = Eigen::Vector3f; +using M2D = Eigen::Matrix2d; +using V2D = Eigen::Vector2d; +using M2F = Eigen::Matrix2f; +using V2F = Eigen::Vector2f; +using M4D = Eigen::Matrix4d; +using V4D = Eigen::Vector4d; + +using M6D = Eigen::Matrix; + +template +using Vec = std::vector; + +struct PointXYZIDT +{ + PCL_ADD_POINT4D; + float lx; + float ly; + float lz; + float intensity; + uint32_t id; + double time; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +} EIGEN_ALIGN16; + +POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIDT, + (float, x, x)(float, y, y)(float, z, z)(float, lx, lx)(float, ly, ly)(float, lz, lz)(float, intensity, intensity)(uint32_t, id, id)(double, time, time)) +using PointType = PointXYZIDT; +using CloudType = pcl::PointCloud; +using PointVec = std::vector>; \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/hba/src/hba/hba.cpp b/src/FASTLIO2_ROS2/hba/src/hba/hba.cpp new file mode 100644 index 00000000..bccf2d92 --- /dev/null +++ b/src/FASTLIO2_ROS2/hba/src/hba/hba.cpp @@ -0,0 +1,174 @@ +#include "hba.h" +HBA::HBA(const HBAConfig config) : m_config(config) +{ + m_poses.clear(); + m_clouds.clear(); + m_lbas.clear(); +} +void HBA::insert(pcl::PointCloud::Ptr &cloud, const Pose &pose) +{ + m_clouds.push_back(cloud); + m_poses.push_back(pose); +} +void HBA::optimize() +{ + m_levels = calcLevels(); + gtsam::Values initial_estimates; + gtsam::LevenbergMarquardtParams lm_params; + gtsam::NonlinearFactorGraph graph; + + // for (size_t i = 0; i < m_config.hba_iter; i++) + // { + Vec>().swap(m_lbas); + m_lbas.resize(m_levels, Vec()); + Vec::Ptr> clouds = m_clouds; + Vec poses = m_poses; + initial_estimates.clear(); + graph.resize(0); + // 设置初始值 + + std::cout << "INIT POSE ESTIMATE" << std::endl; + for (size_t j = 0; j < m_poses.size(); j++) + { + initial_estimates.insert(j, gtsam::Pose3(gtsam::Rot3(poses[j].r), gtsam::Point3(poses[j].t))); + } + + for (int level = 0; level < m_levels; level++) + { + constructHierarchy(clouds, poses, level); + std::cout << "LBA OPTIMIZE LEVEL: " << level << std::endl; + + updateCloudsAndPose(clouds, poses, level); + } + Vec> between_factors_id; + Vec between_factors_pose; + Vec between_factors_info; + getAllFactors(between_factors_id, between_factors_pose, between_factors_info); + // 添加二元因子 + + std::cout << "CONSTRUCT BETWEEN FACTORS" << std::endl; + for (size_t j = 0; j < between_factors_id.size(); j++) + { + gtsam::BetweenFactor between_factor(between_factors_id[j].first, + between_factors_id[j].second, + gtsam::Pose3(gtsam::Rot3(between_factors_pose[j].r), gtsam::Point3(between_factors_pose[j].t)), + gtsam::noiseModel::Gaussian::Information(between_factors_info[j])); + graph.add(between_factor); + } + // 优化 + gtsam::LevenbergMarquardtOptimizer optimizer(graph, initial_estimates, lm_params); + + std::cout << "LM OPTIMIZE " << std::endl; + gtsam::Values result = optimizer.optimize(); + // 更新位姿 + std::cout << "UPDATE POSE ESTIMATE" << std::endl; + for (size_t j = 0; j < m_poses.size(); j++) + { + gtsam::Pose3 pose = result.at(j); + m_poses[j].t = pose.translation().matrix().cast(); + m_poses[j].r = pose.rotation().matrix().cast(); + } + // } +} +void HBA::updateCloudsAndPose(Vec::Ptr> &clouds, Vec &poses, int level) +{ + Vec::Ptr>().swap(clouds); + Vec().swap(poses); + for (BLAM &lba : m_lbas[level]) + { + poses.push_back(lba.poses()[0]); + pcl::PointCloud::Ptr cloud = lba.getLocalCloud(); + if (m_config.down_sample > 0.0) + { + pcl::VoxelGrid voxel_grid; + voxel_grid.setLeafSize(m_config.down_sample, m_config.down_sample, m_config.down_sample); + voxel_grid.setInputCloud(cloud); + voxel_grid.filter(*cloud); + } + clouds.push_back(cloud); + } +} +void HBA::constructHierarchy(Vec::Ptr> &clouds, Vec &poses, int level) +{ + BLAMConfig config; + config.voxel_size = m_config.voxel_size; + config.max_iter = m_config.ba_max_iter; + config.max_layer = m_config.max_layer; + config.min_point_num = m_config.min_point_num; + for (size_t i = 0; i < clouds.size(); i += m_config.stride) + { + m_lbas[level].emplace_back(config); + bool drop_last = false; + for (int j = 0; j < m_config.window_size; j++) + { + size_t idx = i + j; + if (i + j >= clouds.size()) + { + drop_last = true; + break; + } + m_lbas[level].back().insert(clouds[idx], poses[idx]); + } + // std::cout << "level: " << level << " window: " << i << std::endl; + m_lbas[level].back().optimize(); + if (drop_last) + break; + } +} +void HBA::getAllFactors(Vec> &ids, Vec &poses, Vec &infos) +{ + for (size_t level_idx = 0; level_idx < static_cast(m_levels); level_idx++) + { + for (size_t stride_idx = 0; stride_idx < m_lbas[level_idx].size(); stride_idx++) + { + BLAM &blam = m_lbas[level_idx][stride_idx]; + for (size_t pose_idx = 0; pose_idx < blam.poses().size() - 1; pose_idx++) + { + size_t from = pose_idx + stride_idx * m_config.stride; + size_t to = pose_idx + 1 + stride_idx * m_config.stride; + from = from * std::pow(m_config.stride, level_idx); + to = to * std::pow(m_config.stride, level_idx); + ids.emplace_back(std::make_pair(from, to)); + Pose &pose_from = blam.poses()[pose_idx]; + Pose &pose_to = blam.poses()[pose_idx + 1]; + Pose pose_fc; + pose_fc.r = pose_from.r.transpose() * pose_to.r; + pose_fc.t = pose_from.r.transpose() * (pose_to.t - pose_from.t); + // std::cout << "level: " << level_idx << " from: " << from << " to: " << to << std::endl; + poses.push_back(pose_fc); + infos.push_back(blam.H().block<6, 6>(6 * pose_idx, 6 * (pose_idx + 1))); + } + } + } +} +int HBA::calcLevels() +{ + + assert(m_poses.size() > 0 && m_config.window_size > 0 && m_config.stride > 0); + return static_cast(0.5 * std::log(3.0 * std::pow(static_cast(m_poses.size()), 2) * (std::pow(m_config.stride, 3) - m_config.stride) / std::pow(static_cast(m_config.window_size), 3)) / std::log(m_config.stride)); +} +pcl::PointCloud::Ptr HBA::getMapPoints() +{ + pcl::PointCloud::Ptr ret(new pcl::PointCloud); + for (size_t i = 0; i < m_poses.size(); i++) + { + Pose &pose = m_poses[i]; + pcl::PointCloud::Ptr cloud = m_clouds[i]; + pcl::PointCloud::Ptr transformed(new pcl::PointCloud); + pcl::transformPointCloud(*cloud, *transformed, pose.t, Eigen::Quaterniond(pose.r)); + *ret += *transformed; + } + return ret; +} +void HBA::writePoses(const std::string &path) +{ + std::ofstream txt_file(path); + for (size_t i = 0; i < m_poses.size(); i++) + { + Pose &pose = m_poses[i]; + V3D t = pose.t; + Eigen::Quaterniond q(pose.r); + txt_file << t.x() << " " << t.y() << " " << t.z() << " " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << std::endl; + } + txt_file.close(); +} \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/hba/src/hba/hba.h b/src/FASTLIO2_ROS2/hba/src/hba/hba.h new file mode 100644 index 00000000..cf03dfc0 --- /dev/null +++ b/src/FASTLIO2_ROS2/hba/src/hba/hba.h @@ -0,0 +1,57 @@ +#pragma once +#include "commons.h" +#include "blam.h" +#include +#include +#include +#include +#include +#include +#include +#include + + +struct HBAConfig +{ + int window_size = 20; + int stride = 10; + double voxel_size = 1.0; + int min_point_num = 10; + int max_layer = 4; + double plane_thresh = 0.01; + size_t ba_max_iter = 10; + size_t hba_iter = 2; + double down_sample = 0.1; +}; + +class HBA +{ +public: + HBA(const HBAConfig config); + void insert(pcl::PointCloud::Ptr &cloud, const Pose &pose); + + void optimize(); + + int calcLevels(); + + void constructHierarchy(Vec::Ptr> &clouds, Vec &poses, int level); + + void updateCloudsAndPose(Vec::Ptr> &clouds, Vec &poses, int level); + int levels() { return m_levels; } + HBAConfig &config() { return m_config; } + Vec &poses() { return m_poses; } + Vec::Ptr> &clouds() { return m_clouds; } + + void getAllFactors(Vec> &ids, Vec &poses, Vec &infos); + + pcl::PointCloud::Ptr getMapPoints(); + + void writePoses(const std::string &path); + +private: + int m_levels = 1; + HBAConfig m_config; + Vec m_poses; + Vec::Ptr> m_clouds; + Vec> m_lbas; +}; diff --git a/src/FASTLIO2_ROS2/hba/src/hba_node copy.cpp b/src/FASTLIO2_ROS2/hba/src/hba_node copy.cpp new file mode 100644 index 00000000..55e44396 --- /dev/null +++ b/src/FASTLIO2_ROS2/hba/src/hba_node copy.cpp @@ -0,0 +1,126 @@ +#include +#include +#include +#include +#include +#include "hba/blam.h" +#include +#include +#include "hba/hba.h" + +void fromStr(const std::string &str, std::string &file_name, Pose &pose) +{ + std::stringstream ss(str); + std::vector tokens; + std::string token; + while (std::getline(ss, token, ' ')) + { + tokens.push_back(token); + } + assert(tokens.size() == 8); + file_name = tokens[0]; + pose.t = V3D(std::stod(tokens[1]), std::stod(tokens[2]), std::stod(tokens[3])); + pose.r = Eigen::Quaterniond(std::stod(tokens[4]), std::stod(tokens[5]), std::stod(tokens[6]), std::stod(tokens[7])).normalized().toRotationMatrix(); +} + +int main1(int argc, char *argv[]) +{ + pcl::PCDReader reader; + BLAMConfig config; + + double scan_resolution = 0.1; + size_t skip_num = 1; + pcl::VoxelGrid voxel_grid; + voxel_grid.setLeafSize(scan_resolution, scan_resolution, scan_resolution); + + BLAM blam(config); + + std::filesystem::path p_dir(argv[1]); + std::filesystem::path txt_file = p_dir / "poses.txt"; + if (!std::filesystem::exists(txt_file)) + { + std::cout << "poses.txt not found" << std::endl; + return 0; + } + std::ifstream ifs(txt_file); + std::string line; + std::string file_name; + Pose pose; + + Vec::Ptr> raw_points; + + while (std::getline(ifs, line)) + { + + fromStr(line, file_name, pose); + std::filesystem::path pcd_file = p_dir / "patches" / file_name; + if (!std::filesystem::exists(pcd_file)) + { + std::cerr << "pcd file not found" << std::endl; + continue; + } + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + reader.read(pcd_file, *cloud); + voxel_grid.setInputCloud(cloud); + voxel_grid.filter(*cloud); + blam.insert(cloud, pose); + } + + std::cout << "start optimize" << std::endl; + std::cout << blam.poses()[0].t.transpose() << std::endl; + blam.optimize(); + std::cout << "after optimize" << std::endl; + std::cout << blam.poses()[0].t.transpose() << std::endl; + std::cout << blam.H().block<12, 12>(0, 0) << std::endl; + + std::cout << "plane count with sub: " << blam.planeCount(true) << " :: " << blam.planes().size() << std::endl; + std::cout << "plane count no sub: " << blam.planeCount(false) << std::endl; + return 0; +} + +int main(int argc, char *argv[]) +{ + pcl::PCDReader reader; + HBAConfig config; + + double scan_resolution = 0.1; + pcl::VoxelGrid voxel_grid; + voxel_grid.setLeafSize(scan_resolution, scan_resolution, scan_resolution); + + HBA hba(config); + + std::filesystem::path p_dir(argv[1]); + std::filesystem::path txt_file = p_dir / "poses.txt"; + if (!std::filesystem::exists(txt_file)) + { + std::cout << "poses.txt not found" << std::endl; + return 0; + } + std::ifstream ifs(txt_file); + std::string line; + std::string file_name; + Pose pose; + + Vec::Ptr> raw_points; + + while (std::getline(ifs, line)) + { + + fromStr(line, file_name, pose); + std::filesystem::path pcd_file = p_dir / "patches" / file_name; + if (!std::filesystem::exists(pcd_file)) + { + std::cerr << "pcd file not found" << std::endl; + continue; + } + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + reader.read(pcd_file, *cloud); + voxel_grid.setInputCloud(cloud); + voxel_grid.filter(*cloud); + hba.insert(cloud, pose); + } + pcl::io::savePCDFileBinary("/home/zhouzhou/temp/before_opt.pcd", *hba.getMapPoints()); + hba.optimize(); + pcl::io::savePCDFileBinary("/home/zhouzhou/temp/after_opt.pcd", *hba.getMapPoints()); + hba.writePoses("/home/zhouzhou/temp/refined_poses.txt"); +} \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/hba/src/hba_node.cpp b/src/FASTLIO2_ROS2/hba/src/hba_node.cpp new file mode 100644 index 00000000..83b08c9e --- /dev/null +++ b/src/FASTLIO2_ROS2/hba/src/hba_node.cpp @@ -0,0 +1,222 @@ +#include +#include +#include +#include "hba/hba.h" +#include +#include +#include +#include +#include +#include + +#include "interface/srv/refine_map.hpp" +#include "interface/srv/save_poses.hpp" + +using namespace std::chrono_literals; +void fromStr(const std::string &str, std::string &file_name, Pose &pose) +{ + std::stringstream ss(str); + std::vector tokens; + std::string token; + while (std::getline(ss, token, ' ')) + { + tokens.push_back(token); + } + assert(tokens.size() == 8); + file_name = tokens[0]; + pose.t = V3D(std::stod(tokens[1]), std::stod(tokens[2]), std::stod(tokens[3])); + pose.r = Eigen::Quaterniond(std::stod(tokens[4]), std::stod(tokens[5]), std::stod(tokens[6]), std::stod(tokens[7])).normalized().toRotationMatrix(); +} + +struct NodeConfig +{ + double scan_resolution = 0.1; +}; + +class HBANode : public rclcpp::Node +{ +public: + HBANode() : Node("hba_node") + { + RCLCPP_INFO(this->get_logger(), "HBA node started"); + loadParameters(); + m_hba = std::make_shared(m_hba_config); + m_voxel_grid.setLeafSize(m_node_config.scan_resolution, m_node_config.scan_resolution, m_node_config.scan_resolution); + m_refine_map_srv = this->create_service( + "refine_map", + std::bind(&HBANode::refineMapCB, this, std::placeholders::_1, std::placeholders::_2)); + m_save_poses_srv = this->create_service( + "save_poses", + std::bind(&HBANode::savePosesCB, this, std::placeholders::_1, std::placeholders::_2)); + m_timer = this->create_wall_timer(100ms, std::bind(&HBANode::mainCB, this)); + m_cloud_pub = this->create_publisher("map_points", 10); + } + + void loadParameters() + { + this->declare_parameter("config_path", ""); + std::string config_path; + this->get_parameter("config_path", config_path); + YAML::Node config = YAML::LoadFile(config_path); + if (!config) + { + RCLCPP_WARN(this->get_logger(), "FAIL TO LOAD YAML FILE!"); + return; + } + RCLCPP_INFO(this->get_logger(), "LOAD FROM YAML CONFIG PATH: %s", config_path.c_str()); + + m_node_config.scan_resolution = config["scan_resolution"].as(); + m_hba_config.window_size = config["window_size"].as(); + m_hba_config.stride = config["stride"].as(); + m_hba_config.voxel_size = config["voxel_size"].as(); + m_hba_config.min_point_num = config["min_point_num"].as(); + m_hba_config.max_layer = config["max_layer"].as(); + m_hba_config.plane_thresh = config["plane_thresh"].as(); + m_hba_config.ba_max_iter = config["ba_max_iter"].as(); + m_hba_config.hba_iter = config["hba_iter"].as(); + m_hba_config.down_sample = config["down_sample"].as(); + } + + void refineMapCB(const std::shared_ptr request, std::shared_ptr response) + { + std::lock_guard lock(m_service_mutex); + if (!std::filesystem::exists(request->maps_path)) + { + response->success = false; + response->message = "maps_path not exists"; + return; + } + + std::filesystem::path p_dir(request->maps_path); + std::filesystem::path pcd_dir = p_dir / "patches"; + if (!std::filesystem::exists(pcd_dir)) + { + response->success = false; + response->message = pcd_dir.string() + " not exists"; + return; + } + + std::filesystem::path txt_file = p_dir / "poses.txt"; + + if (!std::filesystem::exists(txt_file)) + { + response->success = false; + response->message = txt_file.string() + " not exists"; + return; + } + + std::ifstream ifs(txt_file); + std::string line; + std::string file_name; + Pose pose; + pcl::PCDReader reader; + while (std::getline(ifs, line)) + { + + fromStr(line, file_name, pose); + std::filesystem::path pcd_file = p_dir / "patches" / file_name; + if (!std::filesystem::exists(pcd_file)) + { + std::cerr << "pcd file not found" << std::endl; + continue; + } + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + reader.read(pcd_file, *cloud); + m_voxel_grid.setInputCloud(cloud); + m_voxel_grid.filter(*cloud); + m_hba->insert(cloud, pose); + } + RCLCPP_INFO(this->get_logger(), "LOAD POSE %lu;", m_hba->poses().size()); + response->success = true; + response->message = "load poses success!"; + m_do_optimize = true; + return; + } + + void savePosesCB(const std::shared_ptr request, std::shared_ptr response) + { + std::lock_guard lock(m_service_mutex); + std::filesystem::path file_path(request->file_path); + std::filesystem::path par_path = file_path.parent_path(); + if (!std::filesystem::exists(par_path)) + { + response->success = false; + response->message = "parent path not exists"; + return; + } + if (m_hba->poses().size() < 1) + { + response->success = false; + response->message = "poses is empty"; + return; + } + + if (std::filesystem::exists(file_path)) + { + std::filesystem::remove(file_path); + } + + m_hba->writePoses(file_path); + response->success = true; + response->message = "save poses success!"; + return; + } + void mainCB() + { + { + std::lock_guard lock(m_service_mutex); + if (!m_do_optimize) + return; + RCLCPP_WARN(this->get_logger(), "START OPTIMIZE"); + publishMap(); + for (size_t i = 0; i < m_hba_config.hba_iter; i++) + { + RCLCPP_INFO(this->get_logger(), "======HBA ITER %lu START======", i + 1); + m_hba->optimize(); + publishMap(); + RCLCPP_INFO(this->get_logger(), "======HBA ITER %lu END========", i + 1); + } + + m_do_optimize = false; + RCLCPP_WARN(this->get_logger(), "END OPTIMIZE"); + } + } + + void publishMap() + { + if (m_cloud_pub->get_subscription_count() < 1) + return; + if (m_hba->poses().size() < 1) + return; + pcl::PointCloud::Ptr cloud = m_hba->getMapPoints(); + m_voxel_grid.setInputCloud(cloud); + m_voxel_grid.filter(*cloud); + sensor_msgs::msg::PointCloud2 cloud_msg; + pcl::toROSMsg(*cloud, cloud_msg); + cloud_msg.header.frame_id = "map"; + cloud_msg.header.stamp = this->now(); + m_cloud_pub->publish(cloud_msg); + } + +private: + NodeConfig m_node_config; + HBAConfig m_hba_config; + std::shared_ptr m_hba; + rclcpp::Service::SharedPtr m_refine_map_srv; + rclcpp::Service::SharedPtr m_save_poses_srv; + + rclcpp::Publisher::SharedPtr m_cloud_pub; + + pcl::VoxelGrid m_voxel_grid; + std::mutex m_service_mutex; + bool m_do_optimize = false; + rclcpp::TimerBase::SharedPtr m_timer; +}; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/interface/CMakeLists.txt b/src/FASTLIO2_ROS2/interface/CMakeLists.txt new file mode 100644 index 00000000..8918f6de --- /dev/null +++ b/src/FASTLIO2_ROS2/interface/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 3.8) +project(interface) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(std_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/SaveMaps.srv" + "srv/Relocalize.srv" + "srv/IsValid.srv" + "srv/RefineMap.srv" + "srv/SavePoses.srv" + DEPENDENCIES std_msgs +) + + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/FASTLIO2_ROS2/interface/LICENSE b/src/FASTLIO2_ROS2/interface/LICENSE new file mode 100644 index 00000000..30e8e2ec --- /dev/null +++ b/src/FASTLIO2_ROS2/interface/LICENSE @@ -0,0 +1,17 @@ +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 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/src/FASTLIO2_ROS2/interface/package.xml b/src/FASTLIO2_ROS2/interface/package.xml new file mode 100644 index 00000000..8ead288e --- /dev/null +++ b/src/FASTLIO2_ROS2/interface/package.xml @@ -0,0 +1,24 @@ + + + + interface + 0.0.0 + TODO: Package description + zhouzhou + MIT + + ament_cmake + rosidl_default_generators + + std_msgs + + ament_lint_auto + ament_lint_common + + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + diff --git a/src/FASTLIO2_ROS2/interface/srv/IsValid.srv b/src/FASTLIO2_ROS2/interface/srv/IsValid.srv new file mode 100644 index 00000000..a38eb5c0 --- /dev/null +++ b/src/FASTLIO2_ROS2/interface/srv/IsValid.srv @@ -0,0 +1,3 @@ +int32 code +--- +bool valid \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/interface/srv/RefineMap.srv b/src/FASTLIO2_ROS2/interface/srv/RefineMap.srv new file mode 100644 index 00000000..6450115e --- /dev/null +++ b/src/FASTLIO2_ROS2/interface/srv/RefineMap.srv @@ -0,0 +1,4 @@ +string maps_path +--- +bool success +string message \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/interface/srv/Relocalize.srv b/src/FASTLIO2_ROS2/interface/srv/Relocalize.srv new file mode 100644 index 00000000..9cd10868 --- /dev/null +++ b/src/FASTLIO2_ROS2/interface/srv/Relocalize.srv @@ -0,0 +1,10 @@ +string pcd_path +float32 x +float32 y +float32 z +float32 yaw +float32 pitch +float32 roll +--- +bool success +string message \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/interface/srv/SaveMaps.srv b/src/FASTLIO2_ROS2/interface/srv/SaveMaps.srv new file mode 100644 index 00000000..13856556 --- /dev/null +++ b/src/FASTLIO2_ROS2/interface/srv/SaveMaps.srv @@ -0,0 +1,5 @@ +string file_path +bool save_patches +--- +bool success +string message \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/interface/srv/SavePoses.srv b/src/FASTLIO2_ROS2/interface/srv/SavePoses.srv new file mode 100644 index 00000000..00cb3840 --- /dev/null +++ b/src/FASTLIO2_ROS2/interface/srv/SavePoses.srv @@ -0,0 +1,4 @@ +string file_path +--- +bool success +string message \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/localizer/CMakeLists.txt b/src/FASTLIO2_ROS2/localizer/CMakeLists.txt new file mode 100644 index 00000000..52baf9cc --- /dev/null +++ b/src/FASTLIO2_ROS2/localizer/CMakeLists.txt @@ -0,0 +1,65 @@ +cmake_minimum_required(VERSION 3.8) +project(localizer) + +set(CMAKE_BUILD_TYPE "Release") +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED True) + +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" ) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread -fexceptions") + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-O3) + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(message_filters REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(interface REQUIRED) +find_package(pcl_conversions REQUIRED) + +find_package(Eigen3 REQUIRED) +find_package(PCL REQUIRED) +find_package(yaml-cpp REQUIRED) + + + +include_directories( + ${EIGEN3_INCLUDE_DIR} + ${PCL_INCLUDE_DIRS} +) + + +set(SRC_LIST src/localizers/commons.cpp + src/localizers/icp_localizer.cpp) + +add_executable(localizer_node src/localizer_node.cpp ${SRC_LIST}) +ament_target_dependencies(localizer_node rclcpp std_msgs sensor_msgs nav_msgs message_filters pcl_conversions tf2_ros geometry_msgs interface) +target_link_libraries(localizer_node ${PCL_LIBRARIES} yaml-cpp) + + +install(TARGETS localizer_node DESTINATION lib/${PROJECT_NAME}) +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/FASTLIO2_ROS2/localizer/LICENSE b/src/FASTLIO2_ROS2/localizer/LICENSE new file mode 100644 index 00000000..30e8e2ec --- /dev/null +++ b/src/FASTLIO2_ROS2/localizer/LICENSE @@ -0,0 +1,17 @@ +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 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/src/FASTLIO2_ROS2/localizer/config/localizer.yaml b/src/FASTLIO2_ROS2/localizer/config/localizer.yaml new file mode 100644 index 00000000..3e1fd012 --- /dev/null +++ b/src/FASTLIO2_ROS2/localizer/config/localizer.yaml @@ -0,0 +1,25 @@ +cloud_topic: /fastlio2/body_cloud +odom_topic: /fastlio2/lio_odom +map_frame: map +local_frame: lidar +update_hz: 1.0 + +# Auto-relocalization on startup +auto_relocalize: false +default_pcd_path: "" +initial_x: 0.0 +initial_y: 0.0 +initial_z: 0.0 +initial_yaw: 0.0 +initial_pitch: 0.0 +initial_roll: 0.0 + +rough_scan_resolution: 0.25 +rough_map_resolution: 0.25 +rough_max_iteration: 5 +rough_score_thresh: 0.2 + +refine_scan_resolution: 0.1 +refine_map_resolution: 0.1 +refine_max_iteration: 10 +refine_score_thresh: 0.1 \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/localizer/launch/localizer_launch.py b/src/FASTLIO2_ROS2/localizer/launch/localizer_launch.py new file mode 100644 index 00000000..02166788 --- /dev/null +++ b/src/FASTLIO2_ROS2/localizer/launch/localizer_launch.py @@ -0,0 +1,53 @@ +import launch +import launch_ros.actions +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + rviz_cfg = PathJoinSubstitution( + [FindPackageShare("localizer"), "rviz", "localizer.rviz"] + ) + localizer_config_path = PathJoinSubstitution( + [FindPackageShare("localizer"), "config", "localizer.yaml"] + ) + + lio_config_path = PathJoinSubstitution( + [FindPackageShare("fastlio2"), "config", "lio.yaml"] + ) + return launch.LaunchDescription( + [ + launch_ros.actions.Node( + package="fastlio2", + namespace="fastlio2", + executable="lio_node", + name="lio_node", + output="screen", + parameters=[ + {"config_path": lio_config_path.perform(launch.LaunchContext())} + ], + ), + launch_ros.actions.Node( + package="localizer", + namespace="localizer", + executable="localizer_node", + name="localizer_node", + output="screen", + parameters=[ + { + "config_path": localizer_config_path.perform( + launch.LaunchContext() + ) + } + ], + ), + launch_ros.actions.Node( + package="rviz2", + namespace="localizer", + executable="rviz2", + name="rviz2", + output="screen", + arguments=["-d", rviz_cfg.perform(launch.LaunchContext())], + ) + ] + ) diff --git a/src/FASTLIO2_ROS2/localizer/package.xml b/src/FASTLIO2_ROS2/localizer/package.xml new file mode 100644 index 00000000..c7b30624 --- /dev/null +++ b/src/FASTLIO2_ROS2/localizer/package.xml @@ -0,0 +1,27 @@ + + + + localizer + 0.0.0 + TODO: Package description + zhouzhou + MIT + + ament_cmake + + std_msgs + sensor_msgs + nav_msgs + tf2_ros + message_filters + pcl_conversions + geometry_msgs + interface + + ament_lint_auto + ament_lint_common + + + ament_cmake + + \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/localizer/rviz/localizer.rviz b/src/FASTLIO2_ROS2/localizer/rviz/localizer.rviz new file mode 100644 index 00000000..c1e6ffc5 --- /dev/null +++ b/src/FASTLIO2_ROS2/localizer/rviz/localizer.rviz @@ -0,0 +1,219 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PointCloud21 + - /PointCloud22 + - /Axes1 + - /Axes2 + Splitter Ratio: 0.5 + Tree Height: 1079 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 158 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 2 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /fastlio2/body_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: true + Max Color: 255; 255; 255 + Max Intensity: 255 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 1 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localizer/map_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: map + Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 0.5 + Name: Axes + Radius: 0.05000000074505806 + Reference Frame: body + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 38.8297004699707 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 1.4930500984191895 + Y: 3.4114441871643066 + Z: -0.03380456194281578 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.4947973489761353 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.220401763916016 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1376 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001ef000004c2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004c2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004c2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000004c2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009ba0000003efc0100000002fb0000000800540069006d00650100000000000009ba000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000006b0000004c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2490 + X: 70 + Y: 27 diff --git a/src/FASTLIO2_ROS2/localizer/src/localizer_node.cpp b/src/FASTLIO2_ROS2/localizer/src/localizer_node.cpp new file mode 100644 index 00000000..cd883e71 --- /dev/null +++ b/src/FASTLIO2_ROS2/localizer/src/localizer_node.cpp @@ -0,0 +1,411 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "localizers/commons.h" +#include "localizers/icp_localizer.h" +#include "interface/srv/relocalize.hpp" +#include "interface/srv/is_valid.hpp" +#include + +using namespace std::chrono_literals; + +struct NodeConfig +{ + std::string cloud_topic = "/fastlio2/body_cloud"; + std::string odom_topic = "/fastlio2/lio_odom"; + std::string map_frame = "map"; + std::string local_frame = "lidar"; + double update_hz = 1.0; + + // Auto-relocalization on startup + bool auto_relocalize = false; + std::string default_pcd_path = ""; + float initial_x = 0.0f; + float initial_y = 0.0f; + float initial_z = 0.0f; + float initial_yaw = 0.0f; + float initial_pitch = 0.0f; + float initial_roll = 0.0f; +}; + +struct NodeState +{ + std::mutex message_mutex; + std::mutex service_mutex; + + bool message_received = false; + bool service_received = false; + bool localize_success = false; + bool auto_reloc_triggered = false; // Track if auto-relocalization was triggered + rclcpp::Time last_send_tf_time = rclcpp::Clock().now(); + rclcpp::Time last_tf_log_time = rclcpp::Clock().now(); + builtin_interfaces::msg::Time last_message_time; + CloudType::Ptr last_cloud = std::make_shared(); + M3D last_r; // localmap_body_r + V3D last_t; // localmap_body_t + M3D last_offset_r = M3D::Identity(); // map_localmap_r + V3D last_offset_t = V3D::Zero(); // map_localmap_t + M4F initial_guess = M4F::Identity(); +}; + +class LocalizerNode : public rclcpp::Node +{ +public: + LocalizerNode() : Node("localizer_node") + { + RCLCPP_INFO(this->get_logger(), "Localizer Node Started"); + loadParameters(); + rclcpp::QoS qos = rclcpp::QoS(10); + m_cloud_sub.subscribe(this, m_config.cloud_topic, qos.get_rmw_qos_profile()); + m_odom_sub.subscribe(this, m_config.odom_topic, qos.get_rmw_qos_profile()); + + m_tf_broadcaster = std::make_shared(*this); + + m_sync = std::make_shared>>(message_filters::sync_policies::ApproximateTime(10), m_cloud_sub, m_odom_sub); + m_sync->setAgePenalty(0.1); + m_sync->registerCallback(std::bind(&LocalizerNode::syncCB, this, std::placeholders::_1, std::placeholders::_2)); + m_localizer = std::make_shared(m_localizer_config); + + m_reloc_srv = this->create_service("relocalize", std::bind(&LocalizerNode::relocCB, this, std::placeholders::_1, std::placeholders::_2)); + + m_reloc_check_srv = this->create_service("relocalize_check", std::bind(&LocalizerNode::relocCheckCB, this, std::placeholders::_1, std::placeholders::_2)); + + m_map_cloud_pub = this->create_publisher("map_cloud", 10); + + m_timer = this->create_wall_timer(10ms, std::bind(&LocalizerNode::timerCB, this)); + } + + void loadParameters() + { + this->declare_parameter("config_path", ""); + std::string config_path; + this->get_parameter("config_path", config_path); + YAML::Node config = YAML::LoadFile(config_path); + if (!config) + { + RCLCPP_WARN(this->get_logger(), "FAIL TO LOAD YAML FILE!"); + return; + } + RCLCPP_INFO(this->get_logger(), "LOAD FROM YAML CONFIG PATH: %s", config_path.c_str()); + + m_config.cloud_topic = config["cloud_topic"].as(); + m_config.odom_topic = config["odom_topic"].as(); + m_config.map_frame = config["map_frame"].as(); + m_config.local_frame = config["local_frame"].as(); + m_config.update_hz = config["update_hz"].as(); + + // Load auto-relocalization config + if (config["auto_relocalize"]) + m_config.auto_relocalize = config["auto_relocalize"].as(); + if (config["default_pcd_path"]) + m_config.default_pcd_path = config["default_pcd_path"].as(); + if (config["initial_x"]) + m_config.initial_x = config["initial_x"].as(); + if (config["initial_y"]) + m_config.initial_y = config["initial_y"].as(); + if (config["initial_z"]) + m_config.initial_z = config["initial_z"].as(); + if (config["initial_yaw"]) + m_config.initial_yaw = config["initial_yaw"].as(); + if (config["initial_pitch"]) + m_config.initial_pitch = config["initial_pitch"].as(); + if (config["initial_roll"]) + m_config.initial_roll = config["initial_roll"].as(); + + if (m_config.auto_relocalize) + { + RCLCPP_INFO(this->get_logger(), "[AUTO-RELOC] Enabled with map: %s", m_config.default_pcd_path.c_str()); + RCLCPP_INFO(this->get_logger(), "[AUTO-RELOC] Initial pose: x=%.3f, y=%.3f, z=%.3f, yaw=%.3f", + m_config.initial_x, m_config.initial_y, m_config.initial_z, m_config.initial_yaw); + } + + m_localizer_config.rough_scan_resolution = config["rough_scan_resolution"].as(); + m_localizer_config.rough_map_resolution = config["rough_map_resolution"].as(); + m_localizer_config.rough_max_iteration = config["rough_max_iteration"].as(); + m_localizer_config.rough_score_thresh = config["rough_score_thresh"].as(); + + m_localizer_config.refine_scan_resolution = config["refine_scan_resolution"].as(); + m_localizer_config.refine_map_resolution = config["refine_map_resolution"].as(); + m_localizer_config.refine_max_iteration = config["refine_max_iteration"].as(); + m_localizer_config.refine_score_thresh = config["refine_score_thresh"].as(); + } + bool triggerAutoRelocalize() + { + if (m_config.default_pcd_path.empty()) + { + RCLCPP_ERROR(this->get_logger(), "[AUTO-RELOC] FAILED - No PCD path specified"); + return false; + } + + if (!std::filesystem::exists(m_config.default_pcd_path)) + { + RCLCPP_ERROR(this->get_logger(), "[AUTO-RELOC] FAILED - PCD file not found: %s", m_config.default_pcd_path.c_str()); + return false; + } + + RCLCPP_INFO(this->get_logger(), "[AUTO-RELOC] Triggering relocalization..."); + RCLCPP_INFO(this->get_logger(), "[AUTO-RELOC] Map: %s", m_config.default_pcd_path.c_str()); + RCLCPP_INFO(this->get_logger(), "[AUTO-RELOC] Initial pose: x=%.3f, y=%.3f, z=%.3f, roll=%.3f, pitch=%.3f, yaw=%.3f", + m_config.initial_x, m_config.initial_y, m_config.initial_z, + m_config.initial_roll, m_config.initial_pitch, m_config.initial_yaw); + + bool load_flag = m_localizer->loadMap(m_config.default_pcd_path); + if (!load_flag) + { + RCLCPP_ERROR(this->get_logger(), "[AUTO-RELOC] FAILED - Could not load map"); + return false; + } + RCLCPP_INFO(this->get_logger(), "[AUTO-RELOC] Map loaded successfully"); + + Eigen::AngleAxisd yaw_angle = Eigen::AngleAxisd(m_config.initial_yaw, Eigen::Vector3d::UnitZ()); + Eigen::AngleAxisd roll_angle = Eigen::AngleAxisd(m_config.initial_roll, Eigen::Vector3d::UnitX()); + Eigen::AngleAxisd pitch_angle = Eigen::AngleAxisd(m_config.initial_pitch, Eigen::Vector3d::UnitY()); + + { + std::lock_guard(m_state.message_mutex); + m_state.initial_guess.setIdentity(); + m_state.initial_guess.block<3, 3>(0, 0) = (yaw_angle * roll_angle * pitch_angle).toRotationMatrix().cast(); + m_state.initial_guess.block<3, 1>(0, 3) = V3F(m_config.initial_x, m_config.initial_y, m_config.initial_z); + m_state.service_received = true; + m_state.localize_success = false; + } + + RCLCPP_INFO(this->get_logger(), "[AUTO-RELOC] Initial guess set, waiting for localization..."); + return true; + } + + void timerCB() + { + if (!m_state.message_received) + return; + + // Trigger auto-relocalization once when first message is received + if (m_config.auto_relocalize && !m_state.auto_reloc_triggered) + { + m_state.auto_reloc_triggered = true; + triggerAutoRelocalize(); + } + + rclcpp::Duration diff = rclcpp::Clock().now() - m_state.last_send_tf_time; + + bool update_tf = diff.seconds() > (1.0 / m_config.update_hz) && m_state.message_received; + + if (!update_tf) + { + sendBroadCastTF(m_state.last_message_time); + return; + } + + m_state.last_send_tf_time = rclcpp::Clock().now(); + + M4F initial_guess = M4F::Identity(); + if (m_state.service_received) + { + std::lock_guard(m_state.service_mutex); + initial_guess = m_state.initial_guess; + // m_state.service_received = false; + } + else + { + std::lock_guard(m_state.message_mutex); + initial_guess.block<3, 3>(0, 0) = (m_state.last_offset_r * m_state.last_r).cast(); + initial_guess.block<3, 1>(0, 3) = (m_state.last_offset_r * m_state.last_t + m_state.last_offset_t).cast(); + } + + M3D current_local_r; + V3D current_local_t; + builtin_interfaces::msg::Time current_time; + { + std::lock_guard(m_state.message_mutex); + current_local_r = m_state.last_r; + current_local_t = m_state.last_t; + current_time = m_state.last_message_time; + m_localizer->setInput(m_state.last_cloud); + } + + bool result = m_localizer->align(initial_guess); + if (result) + { + M3D map_body_r = initial_guess.block<3, 3>(0, 0).cast(); + V3D map_body_t = initial_guess.block<3, 1>(0, 3).cast(); + m_state.last_offset_r = map_body_r * current_local_r.transpose(); + m_state.last_offset_t = -map_body_r * current_local_r.transpose() * current_local_t + map_body_t; + if (!m_state.localize_success && m_state.service_received) + { + std::lock_guard(m_state.service_mutex); + m_state.localize_success = true; + m_state.service_received = false; + RCLCPP_INFO(this->get_logger(), "[RELOCALIZE] SUCCESS - Localization converged"); + Eigen::Quaterniond q_result(map_body_r); + // Convert quaternion to euler (yaw) + double yaw_result = std::atan2(2.0 * (q_result.w() * q_result.z() + q_result.x() * q_result.y()), + 1.0 - 2.0 * (q_result.y() * q_result.y() + q_result.z() * q_result.z())); + RCLCPP_INFO(this->get_logger(), "[RELOCALIZE] Final pose in map: x=%.3f, y=%.3f, z=%.3f, yaw=%.3f rad (%.1f deg)", + map_body_t.x(), map_body_t.y(), map_body_t.z(), yaw_result, yaw_result * 180.0 / M_PI); + } + } + else + { + RCLCPP_DEBUG(this->get_logger(), "[RELOCALIZE] ICP alignment iteration - not yet converged"); + } + sendBroadCastTF(current_time); + publishMapCloud(current_time); + } + void syncCB(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &cloud_msg, const nav_msgs::msg::Odometry::ConstSharedPtr &odom_msg) + { + + std::lock_guard(m_state.message_mutex); + + pcl::fromROSMsg(*cloud_msg, *m_state.last_cloud); + + m_state.last_r = Eigen::Quaterniond(odom_msg->pose.pose.orientation.w, + odom_msg->pose.pose.orientation.x, + odom_msg->pose.pose.orientation.y, + odom_msg->pose.pose.orientation.z) + .toRotationMatrix(); + m_state.last_t = V3D(odom_msg->pose.pose.position.x, + odom_msg->pose.pose.position.y, + odom_msg->pose.pose.position.z); + m_state.last_message_time = cloud_msg->header.stamp; + if (!m_state.message_received) + { + m_state.message_received = true; + m_config.local_frame = odom_msg->header.frame_id; + } + } + + void sendBroadCastTF(builtin_interfaces::msg::Time &time) + { + geometry_msgs::msg::TransformStamped transformStamped; + transformStamped.header.frame_id = m_config.map_frame; + transformStamped.child_frame_id = m_config.local_frame; + transformStamped.header.stamp = time; + Eigen::Quaterniond q(m_state.last_offset_r); + V3D t = m_state.last_offset_t; + transformStamped.transform.translation.x = t.x(); + transformStamped.transform.translation.y = t.y(); + transformStamped.transform.translation.z = t.z(); + transformStamped.transform.rotation.x = q.x(); + transformStamped.transform.rotation.y = q.y(); + transformStamped.transform.rotation.z = q.z(); + transformStamped.transform.rotation.w = q.w(); + m_tf_broadcaster->sendTransform(transformStamped); + + // Throttled TF logging (every 5 seconds) + rclcpp::Duration tf_log_diff = rclcpp::Clock().now() - m_state.last_tf_log_time; + if (tf_log_diff.seconds() > 5.0) + { + m_state.last_tf_log_time = rclcpp::Clock().now(); + double yaw = std::atan2(2.0 * (q.w() * q.z() + q.x() * q.y()), + 1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z())); + RCLCPP_INFO(this->get_logger(), "[TF] %s -> %s: x=%.3f, y=%.3f, z=%.3f, yaw=%.3f rad (%.1f deg)", + m_config.map_frame.c_str(), m_config.local_frame.c_str(), + t.x(), t.y(), t.z(), yaw, yaw * 180.0 / M_PI); + } + } + + void relocCB(const std::shared_ptr request, std::shared_ptr response) + { + std::string pcd_path = request->pcd_path; + float x = request->x; + float y = request->y; + float z = request->z; + float yaw = request->yaw; + float roll = request->roll; + float pitch = request->pitch; + + RCLCPP_INFO(this->get_logger(), "[RELOCALIZE] Service called - Initial pose: x=%.3f, y=%.3f, z=%.3f, roll=%.3f, pitch=%.3f, yaw=%.3f", + x, y, z, roll, pitch, yaw); + RCLCPP_INFO(this->get_logger(), "[RELOCALIZE] Map path: %s", pcd_path.c_str()); + + if (!std::filesystem::exists(pcd_path)) + { + RCLCPP_ERROR(this->get_logger(), "[RELOCALIZE] FAILED - PCD file not found: %s", pcd_path.c_str()); + response->success = false; + response->message = "pcd file not found"; + return; + } + + Eigen::AngleAxisd yaw_angle = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()); + Eigen::AngleAxisd roll_angle = Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()); + Eigen::AngleAxisd pitch_angle = Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()); + bool load_flag = m_localizer->loadMap(pcd_path); + if (!load_flag) + { + RCLCPP_ERROR(this->get_logger(), "[RELOCALIZE] FAILED - Could not load map from: %s", pcd_path.c_str()); + response->success = false; + response->message = "load map failed"; + return; + } + RCLCPP_INFO(this->get_logger(), "[RELOCALIZE] Map loaded successfully"); + { + std::lock_guard(m_state.message_mutex); + m_state.initial_guess.setIdentity(); + m_state.initial_guess.block<3, 3>(0, 0) = (yaw_angle * roll_angle * pitch_angle).toRotationMatrix().cast(); + m_state.initial_guess.block<3, 1>(0, 3) = V3F(x, y, z); + m_state.service_received = true; + m_state.localize_success = false; + } + + RCLCPP_INFO(this->get_logger(), "[RELOCALIZE] Initial guess set, waiting for localization..."); + response->success = true; + response->message = "relocalize success"; + return; + } + + void relocCheckCB(const std::shared_ptr request, std::shared_ptr response) + { + std::lock_guard(m_state.service_mutex); + if (request->code == 1) + response->valid = true; + else + response->valid = m_state.localize_success; + return; + } + void publishMapCloud(builtin_interfaces::msg::Time &time) + { + if (m_map_cloud_pub->get_subscription_count() < 1) + return; + CloudType::Ptr map_cloud = m_localizer->refineMap(); + if (map_cloud->size() < 1) + return; + sensor_msgs::msg::PointCloud2 map_cloud_msg; + pcl::toROSMsg(*map_cloud, map_cloud_msg); + map_cloud_msg.header.frame_id = m_config.map_frame; + map_cloud_msg.header.stamp = time; + m_map_cloud_pub->publish(map_cloud_msg); + } + +private: + NodeConfig m_config; + NodeState m_state; + + ICPConfig m_localizer_config; + std::shared_ptr m_localizer; + message_filters::Subscriber m_cloud_sub; + message_filters::Subscriber m_odom_sub; + rclcpp::TimerBase::SharedPtr m_timer; + std::shared_ptr>> m_sync; + std::shared_ptr m_tf_broadcaster; + rclcpp::Service::SharedPtr m_reloc_srv; + rclcpp::Service::SharedPtr m_reloc_check_srv; + rclcpp::Publisher::SharedPtr m_map_cloud_pub; +}; +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/FASTLIO2_ROS2/localizer/src/localizers/commons.cpp b/src/FASTLIO2_ROS2/localizer/src/localizers/commons.cpp new file mode 100644 index 00000000..cb3a78a1 --- /dev/null +++ b/src/FASTLIO2_ROS2/localizer/src/localizers/commons.cpp @@ -0,0 +1 @@ +#include "commons.h" \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/localizer/src/localizers/commons.h b/src/FASTLIO2_ROS2/localizer/src/localizers/commons.h new file mode 100644 index 00000000..e7e01325 --- /dev/null +++ b/src/FASTLIO2_ROS2/localizer/src/localizers/commons.h @@ -0,0 +1,15 @@ +#pragma once +#include +#include +#include + +using PointType = pcl::PointXYZI; +using CloudType = pcl::PointCloud; +using PointVec = std::vector>; + +using M3D = Eigen::Matrix3d; +using V3D = Eigen::Vector3d; +using M3F = Eigen::Matrix3f; +using V3F = Eigen::Vector3f; +using M4F = Eigen::Matrix4f; +using V4F = Eigen::Vector4f; \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/localizer/src/localizers/icp_localizer.cpp b/src/FASTLIO2_ROS2/localizer/src/localizers/icp_localizer.cpp new file mode 100644 index 00000000..c2f4f1f3 --- /dev/null +++ b/src/FASTLIO2_ROS2/localizer/src/localizers/icp_localizer.cpp @@ -0,0 +1,87 @@ +#include "icp_localizer.h" + +ICPLocalizer::ICPLocalizer(const ICPConfig &config) : m_config(config) +{ + m_refine_inp.reset(new CloudType); + m_refine_tgt.reset(new CloudType); + m_rough_inp.reset(new CloudType); + m_rough_tgt.reset(new CloudType); +} +bool ICPLocalizer::loadMap(const std::string &path) +{ + if (!std::filesystem::exists(path)) + { + std::cerr << "Map file not found: " << path << std::endl; + return false; + } + pcl::PCDReader reader; + CloudType::Ptr cloud(new CloudType); + reader.read(path, *cloud); + if (m_config.refine_map_resolution > 0) + { + m_voxel_filter.setLeafSize(m_config.refine_map_resolution, m_config.refine_map_resolution, m_config.refine_map_resolution); + m_voxel_filter.setInputCloud(cloud); + m_voxel_filter.filter(*m_refine_tgt); + } + else + { + pcl::copyPointCloud(*cloud, *m_refine_tgt); + } + + if (m_config.rough_map_resolution > 0) + { + m_voxel_filter.setLeafSize(m_config.rough_map_resolution, m_config.rough_map_resolution, m_config.rough_map_resolution); + m_voxel_filter.setInputCloud(cloud); + m_voxel_filter.filter(*m_rough_tgt); + } + else + { + pcl::copyPointCloud(*cloud, *m_rough_tgt); + } + return true; +} +void ICPLocalizer::setInput(const CloudType::Ptr &cloud) +{ + if (m_config.refine_scan_resolution > 0) + { + m_voxel_filter.setLeafSize(m_config.refine_scan_resolution, m_config.refine_scan_resolution, m_config.refine_scan_resolution); + m_voxel_filter.setInputCloud(cloud); + m_voxel_filter.filter(*m_refine_inp); + } + else + { + pcl::copyPointCloud(*cloud, *m_refine_inp); + } + + if (m_config.rough_scan_resolution > 0) + { + m_voxel_filter.setLeafSize(m_config.rough_scan_resolution, m_config.rough_scan_resolution, m_config.rough_scan_resolution); + m_voxel_filter.setInputCloud(cloud); + m_voxel_filter.filter(*m_rough_inp); + } + else + { + pcl::copyPointCloud(*cloud, *m_rough_inp); + } +} + +bool ICPLocalizer::align(M4F &guess) +{ + CloudType::Ptr aligned_cloud(new CloudType); + if (m_refine_tgt->size() == 0 || m_rough_tgt->size() == 0) + return false; + m_rough_icp.setMaximumIterations(m_config.rough_max_iteration); + m_rough_icp.setInputSource(m_rough_inp); + m_rough_icp.setInputTarget(m_rough_tgt); + m_rough_icp.align(*aligned_cloud, guess); + if (!m_rough_icp.hasConverged() || m_rough_icp.getFitnessScore() > m_config.rough_score_thresh) + return false; + m_refine_icp.setMaximumIterations(m_config.refine_max_iteration); + m_refine_icp.setInputSource(m_refine_inp); + m_refine_icp.setInputTarget(m_refine_tgt); + m_refine_icp.align(*aligned_cloud, m_rough_icp.getFinalTransformation()); + if (!m_refine_icp.hasConverged() || m_refine_icp.getFitnessScore() > m_config.refine_score_thresh) + return false; + guess = m_refine_icp.getFinalTransformation(); + return true; +} \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/localizer/src/localizers/icp_localizer.h b/src/FASTLIO2_ROS2/localizer/src/localizers/icp_localizer.h new file mode 100644 index 00000000..5fea433a --- /dev/null +++ b/src/FASTLIO2_ROS2/localizer/src/localizers/icp_localizer.h @@ -0,0 +1,46 @@ +#pragma once +#include "commons.h" +#include +#include +#include +#include + +struct ICPConfig +{ + double refine_scan_resolution = 0.1; + double refine_map_resolution = 0.1; + double refine_score_thresh = 0.1; + int refine_max_iteration = 10; + + double rough_scan_resolution = 0.25; + double rough_map_resolution = 0.25; + double rough_score_thresh = 0.2; + int rough_max_iteration = 5; +}; + +class ICPLocalizer +{ +public: + ICPLocalizer(const ICPConfig &config); + + bool loadMap(const std::string &path); + + void setInput(const CloudType::Ptr &cloud); + + bool align(M4F &guess); + ICPConfig &config() { return m_config; } + CloudType::Ptr roughMap() { return m_rough_tgt; } + CloudType::Ptr refineMap() { return m_refine_tgt; } + + +private: + ICPConfig m_config; + pcl::VoxelGrid m_voxel_filter; + pcl::IterativeClosestPoint m_refine_icp; + pcl::IterativeClosestPoint m_rough_icp; + CloudType::Ptr m_refine_inp; + CloudType::Ptr m_rough_inp; + CloudType::Ptr m_refine_tgt; + CloudType::Ptr m_rough_tgt; + std::string m_pcd_path; +}; \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/pgo/CMakeLists.txt b/src/FASTLIO2_ROS2/pgo/CMakeLists.txt new file mode 100644 index 00000000..d2e123c9 --- /dev/null +++ b/src/FASTLIO2_ROS2/pgo/CMakeLists.txt @@ -0,0 +1,68 @@ +cmake_minimum_required(VERSION 3.8) +project(pgo) + +set(CMAKE_BUILD_TYPE "Release") +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED True) +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" ) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread -fexceptions") + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-O3) + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(std_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(pcl_conversions REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(message_filters REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(visualization_msgs REQUIRED) + +find_package(GTSAM REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(PCL REQUIRED) +find_package(interface REQUIRED) +find_package(yaml-cpp REQUIRED) + + + +include_directories( + ${EIGEN3_INCLUDE_DIR} + ${PCL_INCLUDE_DIRS} +) + +set(SRC_LIST src/pgos/commons.cpp + src/pgos/simple_pgo.cpp) + +add_executable(pgo_node src/pgo_node.cpp ${SRC_LIST}) +ament_target_dependencies(pgo_node rclcpp std_msgs sensor_msgs nav_msgs message_filters pcl_conversions tf2_ros geometry_msgs visualization_msgs interface GTSAM) +target_link_libraries(pgo_node ${PCL_LIBRARIES} gtsam yaml-cpp) + + +install(TARGETS pgo_node DESTINATION lib/${PROJECT_NAME}) +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/FASTLIO2_ROS2/pgo/LICENSE b/src/FASTLIO2_ROS2/pgo/LICENSE new file mode 100644 index 00000000..30e8e2ec --- /dev/null +++ b/src/FASTLIO2_ROS2/pgo/LICENSE @@ -0,0 +1,17 @@ +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 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/src/FASTLIO2_ROS2/pgo/config/pgo.yaml b/src/FASTLIO2_ROS2/pgo/config/pgo.yaml new file mode 100644 index 00000000..4fc9087b --- /dev/null +++ b/src/FASTLIO2_ROS2/pgo/config/pgo.yaml @@ -0,0 +1,13 @@ +cloud_topic: /fastlio2/body_cloud +odom_topic: /fastlio2/lio_odom +map_frame: map +local_frame: lidar + +key_pose_delta_deg: 10 +key_pose_delta_trans: 0.5 +loop_search_radius: 1.0 +loop_time_tresh: 60.0 +loop_score_tresh: 0.15 +loop_submap_half_range: 5 +submap_resolution: 0.1 +min_loop_detect_duration: 5.0 \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/pgo/launch/pgo_launch.py b/src/FASTLIO2_ROS2/pgo/launch/pgo_launch.py new file mode 100644 index 00000000..2ffc30ad --- /dev/null +++ b/src/FASTLIO2_ROS2/pgo/launch/pgo_launch.py @@ -0,0 +1,46 @@ +import launch +import launch_ros.actions +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + +def generate_launch_description(): + rviz_cfg = PathJoinSubstitution( + [FindPackageShare("pgo"), "rviz", "pgo.rviz"] + ) + pgo_config_path = PathJoinSubstitution( + [FindPackageShare("pgo"), "config", "pgo.yaml"] + ) + + lio_config_path = PathJoinSubstitution( + [FindPackageShare("fastlio2"), "config", "lio.yaml"] + ) + + + return launch.LaunchDescription( + [ + launch_ros.actions.Node( + package="fastlio2", + namespace="fastlio2", + executable="lio_node", + name="lio_node", + output="screen", + parameters=[{"config_path": lio_config_path.perform(launch.LaunchContext())}] + ), + launch_ros.actions.Node( + package="pgo", + namespace="pgo", + executable="pgo_node", + name="pgo_node", + output="screen", + parameters=[{"config_path": pgo_config_path.perform(launch.LaunchContext())}] + ), + launch_ros.actions.Node( + package="rviz2", + namespace="pgo", + executable="rviz2", + name="rviz2", + output="screen", + arguments=["-d", rviz_cfg.perform(launch.LaunchContext())], + ) + ] + ) diff --git a/src/FASTLIO2_ROS2/pgo/package.xml b/src/FASTLIO2_ROS2/pgo/package.xml new file mode 100644 index 00000000..e077f042 --- /dev/null +++ b/src/FASTLIO2_ROS2/pgo/package.xml @@ -0,0 +1,30 @@ + + + + pgo + 0.0.0 + TODO: Package description + zhouzhou + MIT + + ament_cmake + + std_msgs + rclcpp + nav_msgs + tf2_ros + pcl_conversions + sensor_msgs + message_filters + geometry_msgs + visualization_msgs + interface + GTSAM + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/FASTLIO2_ROS2/pgo/rviz/pgo.rviz b/src/FASTLIO2_ROS2/pgo/rviz/pgo.rviz new file mode 100644 index 00000000..6b4101c2 --- /dev/null +++ b/src/FASTLIO2_ROS2/pgo/rviz/pgo.rviz @@ -0,0 +1,258 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PointCloud21 + - /PointCloud22 + Splitter Ratio: 0.5 + Tree Height: 1079 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 195 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 2 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /fastlio2/body_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 0.10000000149011612 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 200 + Enabled: true + Invert Rainbow: true + Max Color: 255; 255; 255 + Max Intensity: 195 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 1 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /fastlio2/world_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: map + Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 0.5 + Name: Axes + Radius: 0.05000000074505806 + Reference Frame: body + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 255; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /fastlio2/lio_path + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + pgo_edges: true + pgo_nodes: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /pgo/loop_markers + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/XYOrbit + Distance: 38.65398025512695 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 1.8247766494750977 + Y: -5.519836902618408 + Z: 5.7220458984375e-06 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.01979660987854 + Target Frame: + Value: XYOrbit (rviz_default_plugins) + Yaw: 4.747386932373047 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1376 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000004c2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004c2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004c2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000004c2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009ba0000003efc0100000002fb0000000800540069006d00650100000000000009ba000002fb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000749000004c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2490 + X: 70 + Y: 27 diff --git a/src/FASTLIO2_ROS2/pgo/src/pgo_node.cpp b/src/FASTLIO2_ROS2/pgo/src/pgo_node.cpp new file mode 100644 index 00000000..4f8a10f1 --- /dev/null +++ b/src/FASTLIO2_ROS2/pgo/src/pgo_node.cpp @@ -0,0 +1,307 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "pgos/commons.h" +#include "pgos/simple_pgo.h" +#include "interface/srv/save_maps.hpp" +#include +#include +#include + +using namespace std::chrono_literals; + +struct NodeConfig +{ + std::string cloud_topic = "/lio/body_cloud"; + std::string odom_topic = "/lio/odom"; + std::string map_frame = "map"; + std::string local_frame = "lidar"; +}; + +struct NodeState +{ + std::mutex message_mutex; + std::queue cloud_buffer; + double last_message_time; +}; + +class PGONode : public rclcpp::Node +{ +public: + PGONode() : Node("pgo_node") + { + RCLCPP_INFO(this->get_logger(), "PGO node started"); + loadParameters(); + m_pgo = std::make_shared(m_pgo_config); + rclcpp::QoS qos = rclcpp::QoS(10); + m_cloud_sub.subscribe(this, m_node_config.cloud_topic, qos.get_rmw_qos_profile()); + m_odom_sub.subscribe(this, m_node_config.odom_topic, qos.get_rmw_qos_profile()); + m_loop_marker_pub = this->create_publisher("/pgo/loop_markers", 10000); + m_tf_broadcaster = std::make_shared(*this); + m_sync = std::make_shared>>(message_filters::sync_policies::ApproximateTime(10), m_cloud_sub, m_odom_sub); + m_sync->setAgePenalty(0.1); + m_sync->registerCallback(std::bind(&PGONode::syncCB, this, std::placeholders::_1, std::placeholders::_2)); + m_timer = this->create_wall_timer(50ms, std::bind(&PGONode::timerCB, this)); + m_save_map_srv = this->create_service("/pgo/save_maps", std::bind(&PGONode::saveMapsCB, this, std::placeholders::_1, std::placeholders::_2)); + } + + void loadParameters() + { + this->declare_parameter("config_path", ""); + std::string config_path; + this->get_parameter("config_path", config_path); + YAML::Node config = YAML::LoadFile(config_path); + if (!config) + { + RCLCPP_WARN(this->get_logger(), "FAIL TO LOAD YAML FILE!"); + return; + } + RCLCPP_INFO(this->get_logger(), "LOAD FROM YAML CONFIG PATH: %s", config_path.c_str()); + m_node_config.cloud_topic = config["cloud_topic"].as(); + m_node_config.odom_topic = config["odom_topic"].as(); + m_node_config.map_frame = config["map_frame"].as(); + m_node_config.local_frame = config["local_frame"].as(); + + m_pgo_config.key_pose_delta_deg = config["key_pose_delta_deg"].as(); + m_pgo_config.key_pose_delta_trans = config["key_pose_delta_trans"].as(); + m_pgo_config.loop_search_radius = config["loop_search_radius"].as(); + m_pgo_config.loop_time_tresh = config["loop_time_tresh"].as(); + m_pgo_config.loop_score_tresh = config["loop_score_tresh"].as(); + m_pgo_config.loop_submap_half_range = config["loop_submap_half_range"].as(); + m_pgo_config.submap_resolution = config["submap_resolution"].as(); + m_pgo_config.min_loop_detect_duration = config["min_loop_detect_duration"].as(); + } + void syncCB(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &cloud_msg, const nav_msgs::msg::Odometry::ConstSharedPtr &odom_msg) + { + + std::lock_guard(m_state.message_mutex); + CloudWithPose cp; + cp.pose.setTime(cloud_msg->header.stamp.sec, cloud_msg->header.stamp.nanosec); + if (cp.pose.second < m_state.last_message_time) + { + RCLCPP_WARN(this->get_logger(), "Received out of order message"); + return; + } + m_state.last_message_time = cp.pose.second; + + cp.pose.r = Eigen::Quaterniond(odom_msg->pose.pose.orientation.w, + odom_msg->pose.pose.orientation.x, + odom_msg->pose.pose.orientation.y, + odom_msg->pose.pose.orientation.z) + .toRotationMatrix(); + cp.pose.t = V3D(odom_msg->pose.pose.position.x, odom_msg->pose.pose.position.y, odom_msg->pose.pose.position.z); + cp.cloud = CloudType::Ptr(new CloudType); + pcl::fromROSMsg(*cloud_msg, *cp.cloud); + m_state.cloud_buffer.push(cp); + } + + void sendBroadCastTF(builtin_interfaces::msg::Time &time) + { + geometry_msgs::msg::TransformStamped transformStamped; + transformStamped.header.frame_id = m_node_config.map_frame; + transformStamped.child_frame_id = m_node_config.local_frame; + transformStamped.header.stamp = time; + Eigen::Quaterniond q(m_pgo->offsetR()); + V3D t = m_pgo->offsetT(); + transformStamped.transform.translation.x = t.x(); + transformStamped.transform.translation.y = t.y(); + transformStamped.transform.translation.z = t.z(); + transformStamped.transform.rotation.x = q.x(); + transformStamped.transform.rotation.y = q.y(); + transformStamped.transform.rotation.z = q.z(); + transformStamped.transform.rotation.w = q.w(); + m_tf_broadcaster->sendTransform(transformStamped); + } + + void publishLoopMarkers(builtin_interfaces::msg::Time &time) + { + if (m_loop_marker_pub->get_subscription_count() == 0) + return; + if (m_pgo->historyPairs().size() == 0) + return; + + visualization_msgs::msg::MarkerArray marker_array; + visualization_msgs::msg::Marker nodes_marker; + visualization_msgs::msg::Marker edges_marker; + nodes_marker.header.frame_id = m_node_config.map_frame; + nodes_marker.header.stamp = time; + nodes_marker.ns = "pgo_nodes"; + nodes_marker.id = 0; + nodes_marker.type = visualization_msgs::msg::Marker::SPHERE_LIST; + nodes_marker.action = visualization_msgs::msg::Marker::ADD; + nodes_marker.pose.orientation.w = 1.0; + nodes_marker.scale.x = 0.3; + nodes_marker.scale.y = 0.3; + nodes_marker.scale.z = 0.3; + nodes_marker.color.r = 1.0; + nodes_marker.color.g = 0.8; + nodes_marker.color.b = 0.0; + nodes_marker.color.a = 1.0; + + edges_marker.header.frame_id = m_node_config.map_frame; + edges_marker.header.stamp = time; + edges_marker.ns = "pgo_edges"; + edges_marker.id = 1; + edges_marker.type = visualization_msgs::msg::Marker::LINE_LIST; + edges_marker.action = visualization_msgs::msg::Marker::ADD; + edges_marker.pose.orientation.w = 1.0; + edges_marker.scale.x = 0.1; + edges_marker.color.r = 0.0; + edges_marker.color.g = 0.8; + edges_marker.color.b = 0.0; + edges_marker.color.a = 1.0; + + std::vector &poses = m_pgo->keyPoses(); + std::vector> &pairs = m_pgo->historyPairs(); + for (size_t i = 0; i < pairs.size(); i++) + { + size_t i1 = pairs[i].first; + size_t i2 = pairs[i].second; + geometry_msgs::msg::Point p1, p2; + p1.x = poses[i1].t_global.x(); + p1.y = poses[i1].t_global.y(); + p1.z = poses[i1].t_global.z(); + + p2.x = poses[i2].t_global.x(); + p2.y = poses[i2].t_global.y(); + p2.z = poses[i2].t_global.z(); + + nodes_marker.points.push_back(p1); + nodes_marker.points.push_back(p2); + edges_marker.points.push_back(p1); + edges_marker.points.push_back(p2); + } + + marker_array.markers.push_back(nodes_marker); + marker_array.markers.push_back(edges_marker); + m_loop_marker_pub->publish(marker_array); + } + + void timerCB() + { + if (m_state.cloud_buffer.size() == 0) + return; + CloudWithPose cp = m_state.cloud_buffer.front(); + // 清理队列 + { + std::lock_guard(m_state.message_mutex); + while (!m_state.cloud_buffer.empty()) + { + m_state.cloud_buffer.pop(); + } + } + builtin_interfaces::msg::Time cur_time; + cur_time.sec = cp.pose.sec; + cur_time.nanosec = cp.pose.nsec; + if (!m_pgo->addKeyPose(cp)) + { + + sendBroadCastTF(cur_time); + return; + } + + m_pgo->searchForLoopPairs(); + + m_pgo->smoothAndUpdate(); + + sendBroadCastTF(cur_time); + + publishLoopMarkers(cur_time); + } + + void saveMapsCB(const std::shared_ptr request, std::shared_ptr response) + { + if (!std::filesystem::exists(request->file_path)) + { + response->success = false; + response->message = request->file_path + " IS NOT EXISTS!"; + return; + } + + if (m_pgo->keyPoses().size() == 0) + { + response->success = false; + response->message = "NO POSES!"; + return; + } + + std::filesystem::path p_dir(request->file_path); + std::filesystem::path patches_dir = p_dir / "patches"; + std::filesystem::path poses_txt_path = p_dir / "poses.txt"; + std::filesystem::path map_path = p_dir / "map.pcd"; + + if (request->save_patches) + { + if (std::filesystem::exists(patches_dir)) + { + std::filesystem::remove_all(patches_dir); + } + + std::filesystem::create_directories(patches_dir); + + if (std::filesystem::exists(poses_txt_path)) + { + std::filesystem::remove(poses_txt_path); + } + RCLCPP_INFO(this->get_logger(), "Patches Path: %s", patches_dir.string().c_str()); + } + RCLCPP_INFO(this->get_logger(), "SAVE MAP TO %s", map_path.string().c_str()); + + std::ofstream txt_file(poses_txt_path); + + CloudType::Ptr ret(new CloudType); + for (size_t i = 0; i < m_pgo->keyPoses().size(); i++) + { + + CloudType::Ptr body_cloud = m_pgo->keyPoses()[i].body_cloud; + if (request->save_patches) + { + std::string patch_name = std::to_string(i) + ".pcd"; + std::filesystem::path patch_path = patches_dir / patch_name; + pcl::io::savePCDFileBinary(patch_path.string(), *body_cloud); + Eigen::Quaterniond q(m_pgo->keyPoses()[i].r_global); + V3D t = m_pgo->keyPoses()[i].t_global; + txt_file << patch_name << " " << t.x() << " " << t.y() << " " << t.z() << " " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << std::endl; + } + CloudType::Ptr world_cloud(new CloudType); + pcl::transformPointCloud(*body_cloud, *world_cloud, m_pgo->keyPoses()[i].t_global, Eigen::Quaterniond(m_pgo->keyPoses()[i].r_global)); + *ret += *world_cloud; + } + txt_file.close(); + pcl::io::savePCDFileBinary(map_path.string(), *ret); + response->success = true; + response->message = "SAVE SUCCESS!"; + } + +private: + NodeConfig m_node_config; + Config m_pgo_config; + NodeState m_state; + std::shared_ptr m_pgo; + rclcpp::TimerBase::SharedPtr m_timer; + rclcpp::Publisher::SharedPtr m_loop_marker_pub; + rclcpp::Service::SharedPtr m_save_map_srv; + message_filters::Subscriber m_cloud_sub; + message_filters::Subscriber m_odom_sub; + std::shared_ptr m_tf_broadcaster; + std::shared_ptr>> m_sync; +}; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/pgo/src/pgos/commons.cpp b/src/FASTLIO2_ROS2/pgo/src/pgos/commons.cpp new file mode 100644 index 00000000..54fef5f6 --- /dev/null +++ b/src/FASTLIO2_ROS2/pgo/src/pgos/commons.cpp @@ -0,0 +1,8 @@ +#include "commons.h" + +void PoseWithTime::setTime(int32_t _sec, uint32_t _nsec) +{ + sec = _sec; + nsec = _nsec; + second = static_cast(sec) + static_cast(nsec) / 1e9; +} \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/pgo/src/pgos/commons.h b/src/FASTLIO2_ROS2/pgo/src/pgos/commons.h new file mode 100644 index 00000000..12de288f --- /dev/null +++ b/src/FASTLIO2_ROS2/pgo/src/pgos/commons.h @@ -0,0 +1,30 @@ +#pragma once +#include +#include +#include + +using PointType = pcl::PointXYZI; +using CloudType = pcl::PointCloud; +using PointVec = std::vector>; + +using M3D = Eigen::Matrix3d; +using V3D = Eigen::Vector3d; +using M3F = Eigen::Matrix3f; +using V3F = Eigen::Vector3f; +using M4F = Eigen::Matrix4f; +using V4F = Eigen::Vector4f; + +struct PoseWithTime { + V3D t; + M3D r; + int32_t sec; + uint32_t nsec; + double second; + void setTime(int32_t sec, uint32_t nsec); + // double second() const; +}; + +struct CloudWithPose { + CloudType::Ptr cloud; + PoseWithTime pose; +}; \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/pgo/src/pgos/simple_pgo.cpp b/src/FASTLIO2_ROS2/pgo/src/pgos/simple_pgo.cpp new file mode 100644 index 00000000..ee0f73bf --- /dev/null +++ b/src/FASTLIO2_ROS2/pgo/src/pgos/simple_pgo.cpp @@ -0,0 +1,211 @@ +#include "simple_pgo.h" + +SimplePGO::SimplePGO(const Config &config) : m_config(config) +{ + gtsam::ISAM2Params isam2_params; + isam2_params.relinearizeThreshold = 0.01; + isam2_params.relinearizeSkip = 1; + m_isam2 = std::make_shared(isam2_params); + m_initial_values.clear(); + m_graph.resize(0); + m_r_offset.setIdentity(); + m_t_offset.setZero(); + + m_icp.setMaximumIterations(50); + m_icp.setMaxCorrespondenceDistance(10); + m_icp.setTransformationEpsilon(1e-6); + m_icp.setEuclideanFitnessEpsilon(1e-6); + m_icp.setRANSACIterations(0); +} + +bool SimplePGO::isKeyPose(const PoseWithTime &pose) +{ + if (m_key_poses.size() == 0) + return true; + const KeyPoseWithCloud &last_item = m_key_poses.back(); + double delta_trans = (pose.t - last_item.t_local).norm(); + double delta_deg = Eigen::Quaterniond(pose.r).angularDistance(Eigen::Quaterniond(last_item.r_local)) * 57.324; + if (delta_trans > m_config.key_pose_delta_trans || delta_deg > m_config.key_pose_delta_deg) + return true; + return false; +} +bool SimplePGO::addKeyPose(const CloudWithPose &cloud_with_pose) +{ + bool is_key_pose = isKeyPose(cloud_with_pose.pose); + if (!is_key_pose) + return false; + size_t idx = m_key_poses.size(); + M3D init_r = m_r_offset * cloud_with_pose.pose.r; + V3D init_t = m_r_offset * cloud_with_pose.pose.t + m_t_offset; + // 添加初始值 + m_initial_values.insert(idx, gtsam::Pose3(gtsam::Rot3(init_r), gtsam::Point3(init_t))); + if (idx == 0) + { + // 添加先验约束 + gtsam::noiseModel::Diagonal::shared_ptr noise = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector6::Ones() * 1e-12); + m_graph.add(gtsam::PriorFactor(idx, gtsam::Pose3(gtsam::Rot3(init_r), gtsam::Point3(init_t)), noise)); + } + else + { + // 添加里程计约束 + const KeyPoseWithCloud &last_item = m_key_poses.back(); + M3D r_between = last_item.r_local.transpose() * cloud_with_pose.pose.r; + V3D t_between = last_item.r_local.transpose() * (cloud_with_pose.pose.t - last_item.t_local); + gtsam::noiseModel::Diagonal::shared_ptr noise = gtsam::noiseModel::Diagonal::Variances((gtsam::Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-6).finished()); + m_graph.add(gtsam::BetweenFactor(idx - 1, idx, gtsam::Pose3(gtsam::Rot3(r_between), gtsam::Point3(t_between)), noise)); + } + KeyPoseWithCloud item; + item.time = cloud_with_pose.pose.second; + item.r_local = cloud_with_pose.pose.r; + item.t_local = cloud_with_pose.pose.t; + item.body_cloud = cloud_with_pose.cloud; + item.r_global = init_r; + item.t_global = init_t; + m_key_poses.push_back(item); + return true; +} + +CloudType::Ptr SimplePGO::getSubMap(int idx, int half_range, double resolution) +{ + assert(idx >= 0 && idx < static_cast(m_key_poses.size())); + int min_idx = std::max(0, idx - half_range); + int max_idx = std::min(static_cast(m_key_poses.size()) - 1, idx + half_range); + + CloudType::Ptr ret(new CloudType); + for (int i = min_idx; i <= max_idx; i++) + { + + CloudType::Ptr body_cloud = m_key_poses[i].body_cloud; + CloudType::Ptr global_cloud(new CloudType); + pcl::transformPointCloud(*body_cloud, *global_cloud, m_key_poses[i].t_global, Eigen::Quaterniond(m_key_poses[i].r_global)); + *ret += *global_cloud; + } + if (resolution > 0) + { + pcl::VoxelGrid voxel_grid; + voxel_grid.setLeafSize(resolution, resolution, resolution); + voxel_grid.setInputCloud(ret); + voxel_grid.filter(*ret); + } + return ret; +} + +void SimplePGO::searchForLoopPairs() +{ + if (m_key_poses.size() < 10) + return; + if (m_config.min_loop_detect_duration > 0.0) + { + if (m_history_pairs.size() > 0) + { + double current_time = m_key_poses.back().time; + double last_time = m_key_poses[m_history_pairs.back().second].time; + if (current_time - last_time < m_config.min_loop_detect_duration) + return; + } + } + + size_t cur_idx = m_key_poses.size() - 1; + const KeyPoseWithCloud &last_item = m_key_poses.back(); + pcl::PointXYZ last_pose_pt; + last_pose_pt.x = last_item.t_global(0); + last_pose_pt.y = last_item.t_global(1); + last_pose_pt.z = last_item.t_global(2); + + pcl::PointCloud::Ptr key_poses_cloud(new pcl::PointCloud); + for (size_t i = 0; i < m_key_poses.size() - 1; i++) + { + pcl::PointXYZ pt; + pt.x = m_key_poses[i].t_global(0); + pt.y = m_key_poses[i].t_global(1); + pt.z = m_key_poses[i].t_global(2); + key_poses_cloud->push_back(pt); + } + pcl::KdTreeFLANN kdtree; + kdtree.setInputCloud(key_poses_cloud); + std::vector ids; + std::vector sqdists; + int neighbors = kdtree.radiusSearch(last_pose_pt, m_config.loop_search_radius, ids, sqdists); + if (neighbors == 0) + return; + + int loop_idx = -1; + for (size_t i = 0; i < ids.size(); i++) + { + int idx = ids[i]; + if (std::abs(last_item.time - m_key_poses[idx].time) > m_config.loop_time_tresh) + { + loop_idx = idx; + break; + } + } + + if (loop_idx == -1) + return; + + CloudType::Ptr target_cloud = getSubMap(loop_idx, m_config.loop_submap_half_range, m_config.submap_resolution); + CloudType::Ptr source_cloud = getSubMap(m_key_poses.size() - 1, 0, m_config.submap_resolution); + CloudType::Ptr align_cloud(new CloudType); + + m_icp.setInputSource(source_cloud); + m_icp.setInputTarget(target_cloud); + m_icp.align(*align_cloud); + + if (!m_icp.hasConverged() || m_icp.getFitnessScore() > m_config.loop_score_tresh) + return; + + M4F loop_transform = m_icp.getFinalTransformation(); + + LoopPair one_pair; + one_pair.source_id = cur_idx; + one_pair.target_id = loop_idx; + one_pair.score = m_icp.getFitnessScore(); + M3D r_refined = loop_transform.block<3, 3>(0, 0).cast() * m_key_poses[cur_idx].r_global; + V3D t_refined = loop_transform.block<3, 3>(0, 0).cast() * m_key_poses[cur_idx].t_global + loop_transform.block<3, 1>(0, 3).cast(); + one_pair.r_offset = m_key_poses[loop_idx].r_global.transpose() * r_refined; + one_pair.t_offset = m_key_poses[loop_idx].r_global.transpose() * (t_refined - m_key_poses[loop_idx].t_global); + m_cache_pairs.push_back(one_pair); + m_history_pairs.emplace_back(one_pair.target_id, one_pair.source_id); +} + +void SimplePGO::smoothAndUpdate() +{ + bool has_loop = !m_cache_pairs.empty(); + // 添加回环因子 + if (has_loop) + { + for (LoopPair &pair : m_cache_pairs) + { + m_graph.add(gtsam::BetweenFactor(pair.target_id, pair.source_id, + gtsam::Pose3(gtsam::Rot3(pair.r_offset), + gtsam::Point3(pair.t_offset)), + gtsam::noiseModel::Diagonal::Variances(gtsam::Vector6::Ones() * pair.score))); + } + std::vector().swap(m_cache_pairs); + } + // smooth and mapping + m_isam2->update(m_graph, m_initial_values); + m_isam2->update(); + if (has_loop) + { + m_isam2->update(); + m_isam2->update(); + m_isam2->update(); + m_isam2->update(); + } + m_graph.resize(0); + m_initial_values.clear(); + + // update key poses + gtsam::Values estimate_values = m_isam2->calculateBestEstimate(); + for (size_t i = 0; i < m_key_poses.size(); i++) + { + gtsam::Pose3 pose = estimate_values.at(i); + m_key_poses[i].r_global = pose.rotation().matrix().cast(); + m_key_poses[i].t_global = pose.translation().matrix().cast(); + } + // update offset + const KeyPoseWithCloud &last_item = m_key_poses.back(); + m_r_offset = last_item.r_global * last_item.r_local.transpose(); + m_t_offset = last_item.t_global - m_r_offset * last_item.t_local; +} \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/pgo/src/pgos/simple_pgo.h b/src/FASTLIO2_ROS2/pgo/src/pgos/simple_pgo.h new file mode 100644 index 00000000..fce824ad --- /dev/null +++ b/src/FASTLIO2_ROS2/pgo/src/pgos/simple_pgo.h @@ -0,0 +1,78 @@ +#pragma once +#include "commons.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +struct KeyPoseWithCloud +{ + M3D r_local; + V3D t_local; + M3D r_global; + V3D t_global; + double time; + CloudType::Ptr body_cloud; +}; +struct LoopPair +{ + size_t source_id; + size_t target_id; + M3D r_offset; + V3D t_offset; + double score; +}; + +struct Config +{ + double key_pose_delta_deg = 10; + double key_pose_delta_trans = 1.0; + double loop_search_radius = 1.0; + double loop_time_tresh = 60.0; + double loop_score_tresh = 0.15; + int loop_submap_half_range = 5; + double submap_resolution = 0.1; + double min_loop_detect_duration = 10.0; +}; + +class SimplePGO +{ +public: + SimplePGO(const Config &config); + + bool isKeyPose(const PoseWithTime &pose); + + bool addKeyPose(const CloudWithPose &cloud_with_pose); + + bool hasLoop(){return m_cache_pairs.size() > 0;} + + void searchForLoopPairs(); + + void smoothAndUpdate(); + + CloudType::Ptr getSubMap(int idx, int half_range, double resolution); + std::vector> &historyPairs() { return m_history_pairs; } + std::vector &keyPoses() { return m_key_poses; } + + M3D offsetR() { return m_r_offset; } + V3D offsetT() { return m_t_offset; } + +private: + Config m_config; + std::vector m_key_poses; + std::vector> m_history_pairs; + std::vector m_cache_pairs; + M3D m_r_offset; + V3D m_t_offset; + std::shared_ptr m_isam2; + gtsam::Values m_initial_values; + gtsam::NonlinearFactorGraph m_graph; + pcl::IterativeClosestPoint m_icp; +}; \ No newline at end of file From 21395a063bee4064071982f8c336b203eb9fba08 Mon Sep 17 00:00:00 2001 From: alexlin2 Date: Sun, 21 Dec 2025 01:14:17 -0500 Subject: [PATCH 2/9] moved fasltio2 --- src/{ => slam}/FASTLIO2_ROS2/README.md | 0 src/{ => slam}/FASTLIO2_ROS2/fastlio2/CMakeLists.txt | 0 src/{ => slam}/FASTLIO2_ROS2/fastlio2/LICENSE | 0 src/{ => slam}/FASTLIO2_ROS2/fastlio2/config/lio.yaml | 0 src/{ => slam}/FASTLIO2_ROS2/fastlio2/launch/lio_launch.py | 0 src/{ => slam}/FASTLIO2_ROS2/fastlio2/package.xml | 0 src/{ => slam}/FASTLIO2_ROS2/fastlio2/rviz/fastlio2.rviz | 0 src/{ => slam}/FASTLIO2_ROS2/fastlio2/src/lio_node.cpp | 0 src/{ => slam}/FASTLIO2_ROS2/fastlio2/src/map_builder/commons.cpp | 0 src/{ => slam}/FASTLIO2_ROS2/fastlio2/src/map_builder/commons.h | 0 src/{ => slam}/FASTLIO2_ROS2/fastlio2/src/map_builder/ieskf.cpp | 0 src/{ => slam}/FASTLIO2_ROS2/fastlio2/src/map_builder/ieskf.h | 0 .../FASTLIO2_ROS2/fastlio2/src/map_builder/ikd_Tree.cpp | 0 src/{ => slam}/FASTLIO2_ROS2/fastlio2/src/map_builder/ikd_Tree.h | 0 .../FASTLIO2_ROS2/fastlio2/src/map_builder/imu_processor.cpp | 0 .../FASTLIO2_ROS2/fastlio2/src/map_builder/imu_processor.h | 0 .../FASTLIO2_ROS2/fastlio2/src/map_builder/lidar_processor.cpp | 0 .../FASTLIO2_ROS2/fastlio2/src/map_builder/lidar_processor.h | 0 .../FASTLIO2_ROS2/fastlio2/src/map_builder/map_builder.cpp | 0 .../FASTLIO2_ROS2/fastlio2/src/map_builder/map_builder.h | 0 src/{ => slam}/FASTLIO2_ROS2/fastlio2/src/utils.cpp | 0 src/{ => slam}/FASTLIO2_ROS2/fastlio2/src/utils.h | 0 src/{ => slam}/FASTLIO2_ROS2/hba/CMakeLists.txt | 0 src/{ => slam}/FASTLIO2_ROS2/hba/LICENSE | 0 src/{ => slam}/FASTLIO2_ROS2/hba/config/hba.yaml | 0 src/{ => slam}/FASTLIO2_ROS2/hba/launch/hba_launch.py | 0 src/{ => slam}/FASTLIO2_ROS2/hba/package.xml | 0 src/{ => slam}/FASTLIO2_ROS2/hba/rviz/hba.rviz | 0 src/{ => slam}/FASTLIO2_ROS2/hba/src/hba/blam.cpp | 0 src/{ => slam}/FASTLIO2_ROS2/hba/src/hba/blam.h | 0 src/{ => slam}/FASTLIO2_ROS2/hba/src/hba/commons.cpp | 0 src/{ => slam}/FASTLIO2_ROS2/hba/src/hba/commons.h | 0 src/{ => slam}/FASTLIO2_ROS2/hba/src/hba/hba.cpp | 0 src/{ => slam}/FASTLIO2_ROS2/hba/src/hba/hba.h | 0 src/{ => slam}/FASTLIO2_ROS2/hba/src/hba_node copy.cpp | 0 src/{ => slam}/FASTLIO2_ROS2/hba/src/hba_node.cpp | 0 src/{ => slam}/FASTLIO2_ROS2/interface/CMakeLists.txt | 0 src/{ => slam}/FASTLIO2_ROS2/interface/LICENSE | 0 src/{ => slam}/FASTLIO2_ROS2/interface/package.xml | 0 src/{ => slam}/FASTLIO2_ROS2/interface/srv/IsValid.srv | 0 src/{ => slam}/FASTLIO2_ROS2/interface/srv/RefineMap.srv | 0 src/{ => slam}/FASTLIO2_ROS2/interface/srv/Relocalize.srv | 0 src/{ => slam}/FASTLIO2_ROS2/interface/srv/SaveMaps.srv | 0 src/{ => slam}/FASTLIO2_ROS2/interface/srv/SavePoses.srv | 0 src/{ => slam}/FASTLIO2_ROS2/localizer/CMakeLists.txt | 0 src/{ => slam}/FASTLIO2_ROS2/localizer/LICENSE | 0 src/{ => slam}/FASTLIO2_ROS2/localizer/config/localizer.yaml | 0 src/{ => slam}/FASTLIO2_ROS2/localizer/launch/localizer_launch.py | 0 src/{ => slam}/FASTLIO2_ROS2/localizer/package.xml | 0 src/{ => slam}/FASTLIO2_ROS2/localizer/rviz/localizer.rviz | 0 src/{ => slam}/FASTLIO2_ROS2/localizer/src/localizer_node.cpp | 0 src/{ => slam}/FASTLIO2_ROS2/localizer/src/localizers/commons.cpp | 0 src/{ => slam}/FASTLIO2_ROS2/localizer/src/localizers/commons.h | 0 .../FASTLIO2_ROS2/localizer/src/localizers/icp_localizer.cpp | 0 .../FASTLIO2_ROS2/localizer/src/localizers/icp_localizer.h | 0 src/{ => slam}/FASTLIO2_ROS2/pgo/CMakeLists.txt | 0 src/{ => slam}/FASTLIO2_ROS2/pgo/LICENSE | 0 src/{ => slam}/FASTLIO2_ROS2/pgo/config/pgo.yaml | 0 src/{ => slam}/FASTLIO2_ROS2/pgo/launch/pgo_launch.py | 0 src/{ => slam}/FASTLIO2_ROS2/pgo/package.xml | 0 src/{ => slam}/FASTLIO2_ROS2/pgo/rviz/pgo.rviz | 0 src/{ => slam}/FASTLIO2_ROS2/pgo/src/pgo_node.cpp | 0 src/{ => slam}/FASTLIO2_ROS2/pgo/src/pgos/commons.cpp | 0 src/{ => slam}/FASTLIO2_ROS2/pgo/src/pgos/commons.h | 0 src/{ => slam}/FASTLIO2_ROS2/pgo/src/pgos/simple_pgo.cpp | 0 src/{ => slam}/FASTLIO2_ROS2/pgo/src/pgos/simple_pgo.h | 0 66 files changed, 0 insertions(+), 0 deletions(-) rename src/{ => slam}/FASTLIO2_ROS2/README.md (100%) rename src/{ => slam}/FASTLIO2_ROS2/fastlio2/CMakeLists.txt (100%) rename src/{ => slam}/FASTLIO2_ROS2/fastlio2/LICENSE (100%) rename src/{ => slam}/FASTLIO2_ROS2/fastlio2/config/lio.yaml (100%) rename src/{ => slam}/FASTLIO2_ROS2/fastlio2/launch/lio_launch.py (100%) rename src/{ => slam}/FASTLIO2_ROS2/fastlio2/package.xml (100%) rename src/{ => slam}/FASTLIO2_ROS2/fastlio2/rviz/fastlio2.rviz (100%) rename src/{ => slam}/FASTLIO2_ROS2/fastlio2/src/lio_node.cpp (100%) rename src/{ => slam}/FASTLIO2_ROS2/fastlio2/src/map_builder/commons.cpp (100%) rename src/{ => slam}/FASTLIO2_ROS2/fastlio2/src/map_builder/commons.h (100%) rename src/{ => slam}/FASTLIO2_ROS2/fastlio2/src/map_builder/ieskf.cpp (100%) rename src/{ => slam}/FASTLIO2_ROS2/fastlio2/src/map_builder/ieskf.h (100%) rename src/{ => slam}/FASTLIO2_ROS2/fastlio2/src/map_builder/ikd_Tree.cpp (100%) rename src/{ => slam}/FASTLIO2_ROS2/fastlio2/src/map_builder/ikd_Tree.h (100%) rename src/{ => slam}/FASTLIO2_ROS2/fastlio2/src/map_builder/imu_processor.cpp (100%) rename src/{ => slam}/FASTLIO2_ROS2/fastlio2/src/map_builder/imu_processor.h (100%) rename src/{ => slam}/FASTLIO2_ROS2/fastlio2/src/map_builder/lidar_processor.cpp (100%) rename src/{ => slam}/FASTLIO2_ROS2/fastlio2/src/map_builder/lidar_processor.h (100%) rename src/{ => slam}/FASTLIO2_ROS2/fastlio2/src/map_builder/map_builder.cpp (100%) rename src/{ => slam}/FASTLIO2_ROS2/fastlio2/src/map_builder/map_builder.h (100%) rename src/{ => slam}/FASTLIO2_ROS2/fastlio2/src/utils.cpp (100%) rename src/{ => slam}/FASTLIO2_ROS2/fastlio2/src/utils.h (100%) rename src/{ => slam}/FASTLIO2_ROS2/hba/CMakeLists.txt (100%) rename src/{ => slam}/FASTLIO2_ROS2/hba/LICENSE (100%) rename src/{ => slam}/FASTLIO2_ROS2/hba/config/hba.yaml (100%) rename src/{ => slam}/FASTLIO2_ROS2/hba/launch/hba_launch.py (100%) rename src/{ => slam}/FASTLIO2_ROS2/hba/package.xml (100%) rename src/{ => slam}/FASTLIO2_ROS2/hba/rviz/hba.rviz (100%) rename src/{ => slam}/FASTLIO2_ROS2/hba/src/hba/blam.cpp (100%) rename src/{ => slam}/FASTLIO2_ROS2/hba/src/hba/blam.h (100%) rename src/{ => slam}/FASTLIO2_ROS2/hba/src/hba/commons.cpp (100%) rename src/{ => slam}/FASTLIO2_ROS2/hba/src/hba/commons.h (100%) rename src/{ => slam}/FASTLIO2_ROS2/hba/src/hba/hba.cpp (100%) rename src/{ => slam}/FASTLIO2_ROS2/hba/src/hba/hba.h (100%) rename src/{ => slam}/FASTLIO2_ROS2/hba/src/hba_node copy.cpp (100%) rename src/{ => slam}/FASTLIO2_ROS2/hba/src/hba_node.cpp (100%) rename src/{ => slam}/FASTLIO2_ROS2/interface/CMakeLists.txt (100%) rename src/{ => slam}/FASTLIO2_ROS2/interface/LICENSE (100%) rename src/{ => slam}/FASTLIO2_ROS2/interface/package.xml (100%) rename src/{ => slam}/FASTLIO2_ROS2/interface/srv/IsValid.srv (100%) rename src/{ => slam}/FASTLIO2_ROS2/interface/srv/RefineMap.srv (100%) rename src/{ => slam}/FASTLIO2_ROS2/interface/srv/Relocalize.srv (100%) rename src/{ => slam}/FASTLIO2_ROS2/interface/srv/SaveMaps.srv (100%) rename src/{ => slam}/FASTLIO2_ROS2/interface/srv/SavePoses.srv (100%) rename src/{ => slam}/FASTLIO2_ROS2/localizer/CMakeLists.txt (100%) rename src/{ => slam}/FASTLIO2_ROS2/localizer/LICENSE (100%) rename src/{ => slam}/FASTLIO2_ROS2/localizer/config/localizer.yaml (100%) rename src/{ => slam}/FASTLIO2_ROS2/localizer/launch/localizer_launch.py (100%) rename src/{ => slam}/FASTLIO2_ROS2/localizer/package.xml (100%) rename src/{ => slam}/FASTLIO2_ROS2/localizer/rviz/localizer.rviz (100%) rename src/{ => slam}/FASTLIO2_ROS2/localizer/src/localizer_node.cpp (100%) rename src/{ => slam}/FASTLIO2_ROS2/localizer/src/localizers/commons.cpp (100%) rename src/{ => slam}/FASTLIO2_ROS2/localizer/src/localizers/commons.h (100%) rename src/{ => slam}/FASTLIO2_ROS2/localizer/src/localizers/icp_localizer.cpp (100%) rename src/{ => slam}/FASTLIO2_ROS2/localizer/src/localizers/icp_localizer.h (100%) rename src/{ => slam}/FASTLIO2_ROS2/pgo/CMakeLists.txt (100%) rename src/{ => slam}/FASTLIO2_ROS2/pgo/LICENSE (100%) rename src/{ => slam}/FASTLIO2_ROS2/pgo/config/pgo.yaml (100%) rename src/{ => slam}/FASTLIO2_ROS2/pgo/launch/pgo_launch.py (100%) rename src/{ => slam}/FASTLIO2_ROS2/pgo/package.xml (100%) rename src/{ => slam}/FASTLIO2_ROS2/pgo/rviz/pgo.rviz (100%) rename src/{ => slam}/FASTLIO2_ROS2/pgo/src/pgo_node.cpp (100%) rename src/{ => slam}/FASTLIO2_ROS2/pgo/src/pgos/commons.cpp (100%) rename src/{ => slam}/FASTLIO2_ROS2/pgo/src/pgos/commons.h (100%) rename src/{ => slam}/FASTLIO2_ROS2/pgo/src/pgos/simple_pgo.cpp (100%) rename src/{ => slam}/FASTLIO2_ROS2/pgo/src/pgos/simple_pgo.h (100%) diff --git a/src/FASTLIO2_ROS2/README.md b/src/slam/FASTLIO2_ROS2/README.md similarity index 100% rename from src/FASTLIO2_ROS2/README.md rename to src/slam/FASTLIO2_ROS2/README.md diff --git a/src/FASTLIO2_ROS2/fastlio2/CMakeLists.txt b/src/slam/FASTLIO2_ROS2/fastlio2/CMakeLists.txt similarity index 100% rename from src/FASTLIO2_ROS2/fastlio2/CMakeLists.txt rename to src/slam/FASTLIO2_ROS2/fastlio2/CMakeLists.txt diff --git a/src/FASTLIO2_ROS2/fastlio2/LICENSE b/src/slam/FASTLIO2_ROS2/fastlio2/LICENSE similarity index 100% rename from src/FASTLIO2_ROS2/fastlio2/LICENSE rename to src/slam/FASTLIO2_ROS2/fastlio2/LICENSE diff --git a/src/FASTLIO2_ROS2/fastlio2/config/lio.yaml b/src/slam/FASTLIO2_ROS2/fastlio2/config/lio.yaml similarity index 100% rename from src/FASTLIO2_ROS2/fastlio2/config/lio.yaml rename to src/slam/FASTLIO2_ROS2/fastlio2/config/lio.yaml diff --git a/src/FASTLIO2_ROS2/fastlio2/launch/lio_launch.py b/src/slam/FASTLIO2_ROS2/fastlio2/launch/lio_launch.py similarity index 100% rename from src/FASTLIO2_ROS2/fastlio2/launch/lio_launch.py rename to src/slam/FASTLIO2_ROS2/fastlio2/launch/lio_launch.py diff --git a/src/FASTLIO2_ROS2/fastlio2/package.xml b/src/slam/FASTLIO2_ROS2/fastlio2/package.xml similarity index 100% rename from src/FASTLIO2_ROS2/fastlio2/package.xml rename to src/slam/FASTLIO2_ROS2/fastlio2/package.xml diff --git a/src/FASTLIO2_ROS2/fastlio2/rviz/fastlio2.rviz b/src/slam/FASTLIO2_ROS2/fastlio2/rviz/fastlio2.rviz similarity index 100% rename from src/FASTLIO2_ROS2/fastlio2/rviz/fastlio2.rviz rename to src/slam/FASTLIO2_ROS2/fastlio2/rviz/fastlio2.rviz diff --git a/src/FASTLIO2_ROS2/fastlio2/src/lio_node.cpp b/src/slam/FASTLIO2_ROS2/fastlio2/src/lio_node.cpp similarity index 100% rename from src/FASTLIO2_ROS2/fastlio2/src/lio_node.cpp rename to src/slam/FASTLIO2_ROS2/fastlio2/src/lio_node.cpp diff --git a/src/FASTLIO2_ROS2/fastlio2/src/map_builder/commons.cpp b/src/slam/FASTLIO2_ROS2/fastlio2/src/map_builder/commons.cpp similarity index 100% rename from src/FASTLIO2_ROS2/fastlio2/src/map_builder/commons.cpp rename to src/slam/FASTLIO2_ROS2/fastlio2/src/map_builder/commons.cpp diff --git a/src/FASTLIO2_ROS2/fastlio2/src/map_builder/commons.h b/src/slam/FASTLIO2_ROS2/fastlio2/src/map_builder/commons.h similarity index 100% rename from src/FASTLIO2_ROS2/fastlio2/src/map_builder/commons.h rename to src/slam/FASTLIO2_ROS2/fastlio2/src/map_builder/commons.h diff --git a/src/FASTLIO2_ROS2/fastlio2/src/map_builder/ieskf.cpp b/src/slam/FASTLIO2_ROS2/fastlio2/src/map_builder/ieskf.cpp similarity index 100% rename from src/FASTLIO2_ROS2/fastlio2/src/map_builder/ieskf.cpp rename to src/slam/FASTLIO2_ROS2/fastlio2/src/map_builder/ieskf.cpp diff --git a/src/FASTLIO2_ROS2/fastlio2/src/map_builder/ieskf.h b/src/slam/FASTLIO2_ROS2/fastlio2/src/map_builder/ieskf.h similarity index 100% rename from src/FASTLIO2_ROS2/fastlio2/src/map_builder/ieskf.h rename to src/slam/FASTLIO2_ROS2/fastlio2/src/map_builder/ieskf.h diff --git a/src/FASTLIO2_ROS2/fastlio2/src/map_builder/ikd_Tree.cpp b/src/slam/FASTLIO2_ROS2/fastlio2/src/map_builder/ikd_Tree.cpp similarity index 100% rename from src/FASTLIO2_ROS2/fastlio2/src/map_builder/ikd_Tree.cpp rename to src/slam/FASTLIO2_ROS2/fastlio2/src/map_builder/ikd_Tree.cpp diff --git a/src/FASTLIO2_ROS2/fastlio2/src/map_builder/ikd_Tree.h b/src/slam/FASTLIO2_ROS2/fastlio2/src/map_builder/ikd_Tree.h similarity index 100% rename from src/FASTLIO2_ROS2/fastlio2/src/map_builder/ikd_Tree.h rename to src/slam/FASTLIO2_ROS2/fastlio2/src/map_builder/ikd_Tree.h diff --git a/src/FASTLIO2_ROS2/fastlio2/src/map_builder/imu_processor.cpp b/src/slam/FASTLIO2_ROS2/fastlio2/src/map_builder/imu_processor.cpp similarity index 100% rename from src/FASTLIO2_ROS2/fastlio2/src/map_builder/imu_processor.cpp rename to src/slam/FASTLIO2_ROS2/fastlio2/src/map_builder/imu_processor.cpp diff --git a/src/FASTLIO2_ROS2/fastlio2/src/map_builder/imu_processor.h b/src/slam/FASTLIO2_ROS2/fastlio2/src/map_builder/imu_processor.h similarity index 100% rename from src/FASTLIO2_ROS2/fastlio2/src/map_builder/imu_processor.h rename to src/slam/FASTLIO2_ROS2/fastlio2/src/map_builder/imu_processor.h diff --git a/src/FASTLIO2_ROS2/fastlio2/src/map_builder/lidar_processor.cpp b/src/slam/FASTLIO2_ROS2/fastlio2/src/map_builder/lidar_processor.cpp similarity index 100% rename from src/FASTLIO2_ROS2/fastlio2/src/map_builder/lidar_processor.cpp rename to src/slam/FASTLIO2_ROS2/fastlio2/src/map_builder/lidar_processor.cpp diff --git a/src/FASTLIO2_ROS2/fastlio2/src/map_builder/lidar_processor.h b/src/slam/FASTLIO2_ROS2/fastlio2/src/map_builder/lidar_processor.h similarity index 100% rename from src/FASTLIO2_ROS2/fastlio2/src/map_builder/lidar_processor.h rename to src/slam/FASTLIO2_ROS2/fastlio2/src/map_builder/lidar_processor.h diff --git a/src/FASTLIO2_ROS2/fastlio2/src/map_builder/map_builder.cpp b/src/slam/FASTLIO2_ROS2/fastlio2/src/map_builder/map_builder.cpp similarity index 100% rename from src/FASTLIO2_ROS2/fastlio2/src/map_builder/map_builder.cpp rename to src/slam/FASTLIO2_ROS2/fastlio2/src/map_builder/map_builder.cpp diff --git a/src/FASTLIO2_ROS2/fastlio2/src/map_builder/map_builder.h b/src/slam/FASTLIO2_ROS2/fastlio2/src/map_builder/map_builder.h similarity index 100% rename from src/FASTLIO2_ROS2/fastlio2/src/map_builder/map_builder.h rename to src/slam/FASTLIO2_ROS2/fastlio2/src/map_builder/map_builder.h diff --git a/src/FASTLIO2_ROS2/fastlio2/src/utils.cpp b/src/slam/FASTLIO2_ROS2/fastlio2/src/utils.cpp similarity index 100% rename from src/FASTLIO2_ROS2/fastlio2/src/utils.cpp rename to src/slam/FASTLIO2_ROS2/fastlio2/src/utils.cpp diff --git a/src/FASTLIO2_ROS2/fastlio2/src/utils.h b/src/slam/FASTLIO2_ROS2/fastlio2/src/utils.h similarity index 100% rename from src/FASTLIO2_ROS2/fastlio2/src/utils.h rename to src/slam/FASTLIO2_ROS2/fastlio2/src/utils.h diff --git a/src/FASTLIO2_ROS2/hba/CMakeLists.txt b/src/slam/FASTLIO2_ROS2/hba/CMakeLists.txt similarity index 100% rename from src/FASTLIO2_ROS2/hba/CMakeLists.txt rename to src/slam/FASTLIO2_ROS2/hba/CMakeLists.txt diff --git a/src/FASTLIO2_ROS2/hba/LICENSE b/src/slam/FASTLIO2_ROS2/hba/LICENSE similarity index 100% rename from src/FASTLIO2_ROS2/hba/LICENSE rename to src/slam/FASTLIO2_ROS2/hba/LICENSE diff --git a/src/FASTLIO2_ROS2/hba/config/hba.yaml b/src/slam/FASTLIO2_ROS2/hba/config/hba.yaml similarity index 100% rename from src/FASTLIO2_ROS2/hba/config/hba.yaml rename to src/slam/FASTLIO2_ROS2/hba/config/hba.yaml diff --git a/src/FASTLIO2_ROS2/hba/launch/hba_launch.py b/src/slam/FASTLIO2_ROS2/hba/launch/hba_launch.py similarity index 100% rename from src/FASTLIO2_ROS2/hba/launch/hba_launch.py rename to src/slam/FASTLIO2_ROS2/hba/launch/hba_launch.py diff --git a/src/FASTLIO2_ROS2/hba/package.xml b/src/slam/FASTLIO2_ROS2/hba/package.xml similarity index 100% rename from src/FASTLIO2_ROS2/hba/package.xml rename to src/slam/FASTLIO2_ROS2/hba/package.xml diff --git a/src/FASTLIO2_ROS2/hba/rviz/hba.rviz b/src/slam/FASTLIO2_ROS2/hba/rviz/hba.rviz similarity index 100% rename from src/FASTLIO2_ROS2/hba/rviz/hba.rviz rename to src/slam/FASTLIO2_ROS2/hba/rviz/hba.rviz diff --git a/src/FASTLIO2_ROS2/hba/src/hba/blam.cpp b/src/slam/FASTLIO2_ROS2/hba/src/hba/blam.cpp similarity index 100% rename from src/FASTLIO2_ROS2/hba/src/hba/blam.cpp rename to src/slam/FASTLIO2_ROS2/hba/src/hba/blam.cpp diff --git a/src/FASTLIO2_ROS2/hba/src/hba/blam.h b/src/slam/FASTLIO2_ROS2/hba/src/hba/blam.h similarity index 100% rename from src/FASTLIO2_ROS2/hba/src/hba/blam.h rename to src/slam/FASTLIO2_ROS2/hba/src/hba/blam.h diff --git a/src/FASTLIO2_ROS2/hba/src/hba/commons.cpp b/src/slam/FASTLIO2_ROS2/hba/src/hba/commons.cpp similarity index 100% rename from src/FASTLIO2_ROS2/hba/src/hba/commons.cpp rename to src/slam/FASTLIO2_ROS2/hba/src/hba/commons.cpp diff --git a/src/FASTLIO2_ROS2/hba/src/hba/commons.h b/src/slam/FASTLIO2_ROS2/hba/src/hba/commons.h similarity index 100% rename from src/FASTLIO2_ROS2/hba/src/hba/commons.h rename to src/slam/FASTLIO2_ROS2/hba/src/hba/commons.h diff --git a/src/FASTLIO2_ROS2/hba/src/hba/hba.cpp b/src/slam/FASTLIO2_ROS2/hba/src/hba/hba.cpp similarity index 100% rename from src/FASTLIO2_ROS2/hba/src/hba/hba.cpp rename to src/slam/FASTLIO2_ROS2/hba/src/hba/hba.cpp diff --git a/src/FASTLIO2_ROS2/hba/src/hba/hba.h b/src/slam/FASTLIO2_ROS2/hba/src/hba/hba.h similarity index 100% rename from src/FASTLIO2_ROS2/hba/src/hba/hba.h rename to src/slam/FASTLIO2_ROS2/hba/src/hba/hba.h diff --git a/src/FASTLIO2_ROS2/hba/src/hba_node copy.cpp b/src/slam/FASTLIO2_ROS2/hba/src/hba_node copy.cpp similarity index 100% rename from src/FASTLIO2_ROS2/hba/src/hba_node copy.cpp rename to src/slam/FASTLIO2_ROS2/hba/src/hba_node copy.cpp diff --git a/src/FASTLIO2_ROS2/hba/src/hba_node.cpp b/src/slam/FASTLIO2_ROS2/hba/src/hba_node.cpp similarity index 100% rename from src/FASTLIO2_ROS2/hba/src/hba_node.cpp rename to src/slam/FASTLIO2_ROS2/hba/src/hba_node.cpp diff --git a/src/FASTLIO2_ROS2/interface/CMakeLists.txt b/src/slam/FASTLIO2_ROS2/interface/CMakeLists.txt similarity index 100% rename from src/FASTLIO2_ROS2/interface/CMakeLists.txt rename to src/slam/FASTLIO2_ROS2/interface/CMakeLists.txt diff --git a/src/FASTLIO2_ROS2/interface/LICENSE b/src/slam/FASTLIO2_ROS2/interface/LICENSE similarity index 100% rename from src/FASTLIO2_ROS2/interface/LICENSE rename to src/slam/FASTLIO2_ROS2/interface/LICENSE diff --git a/src/FASTLIO2_ROS2/interface/package.xml b/src/slam/FASTLIO2_ROS2/interface/package.xml similarity index 100% rename from src/FASTLIO2_ROS2/interface/package.xml rename to src/slam/FASTLIO2_ROS2/interface/package.xml diff --git a/src/FASTLIO2_ROS2/interface/srv/IsValid.srv b/src/slam/FASTLIO2_ROS2/interface/srv/IsValid.srv similarity index 100% rename from src/FASTLIO2_ROS2/interface/srv/IsValid.srv rename to src/slam/FASTLIO2_ROS2/interface/srv/IsValid.srv diff --git a/src/FASTLIO2_ROS2/interface/srv/RefineMap.srv b/src/slam/FASTLIO2_ROS2/interface/srv/RefineMap.srv similarity index 100% rename from src/FASTLIO2_ROS2/interface/srv/RefineMap.srv rename to src/slam/FASTLIO2_ROS2/interface/srv/RefineMap.srv diff --git a/src/FASTLIO2_ROS2/interface/srv/Relocalize.srv b/src/slam/FASTLIO2_ROS2/interface/srv/Relocalize.srv similarity index 100% rename from src/FASTLIO2_ROS2/interface/srv/Relocalize.srv rename to src/slam/FASTLIO2_ROS2/interface/srv/Relocalize.srv diff --git a/src/FASTLIO2_ROS2/interface/srv/SaveMaps.srv b/src/slam/FASTLIO2_ROS2/interface/srv/SaveMaps.srv similarity index 100% rename from src/FASTLIO2_ROS2/interface/srv/SaveMaps.srv rename to src/slam/FASTLIO2_ROS2/interface/srv/SaveMaps.srv diff --git a/src/FASTLIO2_ROS2/interface/srv/SavePoses.srv b/src/slam/FASTLIO2_ROS2/interface/srv/SavePoses.srv similarity index 100% rename from src/FASTLIO2_ROS2/interface/srv/SavePoses.srv rename to src/slam/FASTLIO2_ROS2/interface/srv/SavePoses.srv diff --git a/src/FASTLIO2_ROS2/localizer/CMakeLists.txt b/src/slam/FASTLIO2_ROS2/localizer/CMakeLists.txt similarity index 100% rename from src/FASTLIO2_ROS2/localizer/CMakeLists.txt rename to src/slam/FASTLIO2_ROS2/localizer/CMakeLists.txt diff --git a/src/FASTLIO2_ROS2/localizer/LICENSE b/src/slam/FASTLIO2_ROS2/localizer/LICENSE similarity index 100% rename from src/FASTLIO2_ROS2/localizer/LICENSE rename to src/slam/FASTLIO2_ROS2/localizer/LICENSE diff --git a/src/FASTLIO2_ROS2/localizer/config/localizer.yaml b/src/slam/FASTLIO2_ROS2/localizer/config/localizer.yaml similarity index 100% rename from src/FASTLIO2_ROS2/localizer/config/localizer.yaml rename to src/slam/FASTLIO2_ROS2/localizer/config/localizer.yaml diff --git a/src/FASTLIO2_ROS2/localizer/launch/localizer_launch.py b/src/slam/FASTLIO2_ROS2/localizer/launch/localizer_launch.py similarity index 100% rename from src/FASTLIO2_ROS2/localizer/launch/localizer_launch.py rename to src/slam/FASTLIO2_ROS2/localizer/launch/localizer_launch.py diff --git a/src/FASTLIO2_ROS2/localizer/package.xml b/src/slam/FASTLIO2_ROS2/localizer/package.xml similarity index 100% rename from src/FASTLIO2_ROS2/localizer/package.xml rename to src/slam/FASTLIO2_ROS2/localizer/package.xml diff --git a/src/FASTLIO2_ROS2/localizer/rviz/localizer.rviz b/src/slam/FASTLIO2_ROS2/localizer/rviz/localizer.rviz similarity index 100% rename from src/FASTLIO2_ROS2/localizer/rviz/localizer.rviz rename to src/slam/FASTLIO2_ROS2/localizer/rviz/localizer.rviz diff --git a/src/FASTLIO2_ROS2/localizer/src/localizer_node.cpp b/src/slam/FASTLIO2_ROS2/localizer/src/localizer_node.cpp similarity index 100% rename from src/FASTLIO2_ROS2/localizer/src/localizer_node.cpp rename to src/slam/FASTLIO2_ROS2/localizer/src/localizer_node.cpp diff --git a/src/FASTLIO2_ROS2/localizer/src/localizers/commons.cpp b/src/slam/FASTLIO2_ROS2/localizer/src/localizers/commons.cpp similarity index 100% rename from src/FASTLIO2_ROS2/localizer/src/localizers/commons.cpp rename to src/slam/FASTLIO2_ROS2/localizer/src/localizers/commons.cpp diff --git a/src/FASTLIO2_ROS2/localizer/src/localizers/commons.h b/src/slam/FASTLIO2_ROS2/localizer/src/localizers/commons.h similarity index 100% rename from src/FASTLIO2_ROS2/localizer/src/localizers/commons.h rename to src/slam/FASTLIO2_ROS2/localizer/src/localizers/commons.h diff --git a/src/FASTLIO2_ROS2/localizer/src/localizers/icp_localizer.cpp b/src/slam/FASTLIO2_ROS2/localizer/src/localizers/icp_localizer.cpp similarity index 100% rename from src/FASTLIO2_ROS2/localizer/src/localizers/icp_localizer.cpp rename to src/slam/FASTLIO2_ROS2/localizer/src/localizers/icp_localizer.cpp diff --git a/src/FASTLIO2_ROS2/localizer/src/localizers/icp_localizer.h b/src/slam/FASTLIO2_ROS2/localizer/src/localizers/icp_localizer.h similarity index 100% rename from src/FASTLIO2_ROS2/localizer/src/localizers/icp_localizer.h rename to src/slam/FASTLIO2_ROS2/localizer/src/localizers/icp_localizer.h diff --git a/src/FASTLIO2_ROS2/pgo/CMakeLists.txt b/src/slam/FASTLIO2_ROS2/pgo/CMakeLists.txt similarity index 100% rename from src/FASTLIO2_ROS2/pgo/CMakeLists.txt rename to src/slam/FASTLIO2_ROS2/pgo/CMakeLists.txt diff --git a/src/FASTLIO2_ROS2/pgo/LICENSE b/src/slam/FASTLIO2_ROS2/pgo/LICENSE similarity index 100% rename from src/FASTLIO2_ROS2/pgo/LICENSE rename to src/slam/FASTLIO2_ROS2/pgo/LICENSE diff --git a/src/FASTLIO2_ROS2/pgo/config/pgo.yaml b/src/slam/FASTLIO2_ROS2/pgo/config/pgo.yaml similarity index 100% rename from src/FASTLIO2_ROS2/pgo/config/pgo.yaml rename to src/slam/FASTLIO2_ROS2/pgo/config/pgo.yaml diff --git a/src/FASTLIO2_ROS2/pgo/launch/pgo_launch.py b/src/slam/FASTLIO2_ROS2/pgo/launch/pgo_launch.py similarity index 100% rename from src/FASTLIO2_ROS2/pgo/launch/pgo_launch.py rename to src/slam/FASTLIO2_ROS2/pgo/launch/pgo_launch.py diff --git a/src/FASTLIO2_ROS2/pgo/package.xml b/src/slam/FASTLIO2_ROS2/pgo/package.xml similarity index 100% rename from src/FASTLIO2_ROS2/pgo/package.xml rename to src/slam/FASTLIO2_ROS2/pgo/package.xml diff --git a/src/FASTLIO2_ROS2/pgo/rviz/pgo.rviz b/src/slam/FASTLIO2_ROS2/pgo/rviz/pgo.rviz similarity index 100% rename from src/FASTLIO2_ROS2/pgo/rviz/pgo.rviz rename to src/slam/FASTLIO2_ROS2/pgo/rviz/pgo.rviz diff --git a/src/FASTLIO2_ROS2/pgo/src/pgo_node.cpp b/src/slam/FASTLIO2_ROS2/pgo/src/pgo_node.cpp similarity index 100% rename from src/FASTLIO2_ROS2/pgo/src/pgo_node.cpp rename to src/slam/FASTLIO2_ROS2/pgo/src/pgo_node.cpp diff --git a/src/FASTLIO2_ROS2/pgo/src/pgos/commons.cpp b/src/slam/FASTLIO2_ROS2/pgo/src/pgos/commons.cpp similarity index 100% rename from src/FASTLIO2_ROS2/pgo/src/pgos/commons.cpp rename to src/slam/FASTLIO2_ROS2/pgo/src/pgos/commons.cpp diff --git a/src/FASTLIO2_ROS2/pgo/src/pgos/commons.h b/src/slam/FASTLIO2_ROS2/pgo/src/pgos/commons.h similarity index 100% rename from src/FASTLIO2_ROS2/pgo/src/pgos/commons.h rename to src/slam/FASTLIO2_ROS2/pgo/src/pgos/commons.h diff --git a/src/FASTLIO2_ROS2/pgo/src/pgos/simple_pgo.cpp b/src/slam/FASTLIO2_ROS2/pgo/src/pgos/simple_pgo.cpp similarity index 100% rename from src/FASTLIO2_ROS2/pgo/src/pgos/simple_pgo.cpp rename to src/slam/FASTLIO2_ROS2/pgo/src/pgos/simple_pgo.cpp diff --git a/src/FASTLIO2_ROS2/pgo/src/pgos/simple_pgo.h b/src/slam/FASTLIO2_ROS2/pgo/src/pgos/simple_pgo.h similarity index 100% rename from src/FASTLIO2_ROS2/pgo/src/pgos/simple_pgo.h rename to src/slam/FASTLIO2_ROS2/pgo/src/pgos/simple_pgo.h From 7487d71b679737bee8d6bf16f428e53e49dc07b2 Mon Sep 17 00:00:00 2001 From: alexlin2 Date: Sun, 21 Dec 2025 23:25:15 -0500 Subject: [PATCH 3/9] added fastlio2 relocalization --- .../launch/system_bagfile.launch.py | 19 +- ...bagfile_with_exploration_planner.launch.py | 19 +- ...ystem_bagfile_with_route_planner.launch.py | 18 +- .../launch/system_real_robot.launch.py | 19 +- ...l_robot_with_exploration_planner.launch.py | 19 +- ...em_real_robot_with_route_planner.launch.py | 18 +- .../launch/system_real_slam_only.launch.py | 23 +- .../FASTLIO2_ROS2/fastlio2/config/lio.yaml | 6 +- .../fastlio2/config/lio_autonomy.yaml | 55 ++++ .../FASTLIO2_ROS2/fastlio2/src/lio_node.cpp | 103 ++++++- .../fastlio2/src/map_builder/ieskf.cpp | 40 ++- .../localizer/src/localizer_node.cpp | 19 ++ .../fastlio2_autonomy_bridge/CMakeLists.txt | 43 +++ .../launch/fastlio2_autonomy.launch.py | 110 +++++++ src/slam/fastlio2_autonomy_bridge/package.xml | 25 ++ .../src/fastlio2_autonomy_bridge_node.cpp | 282 ++++++++++++++++++ 16 files changed, 802 insertions(+), 16 deletions(-) create mode 100644 src/slam/FASTLIO2_ROS2/fastlio2/config/lio_autonomy.yaml create mode 100644 src/slam/fastlio2_autonomy_bridge/CMakeLists.txt create mode 100644 src/slam/fastlio2_autonomy_bridge/launch/fastlio2_autonomy.launch.py create mode 100644 src/slam/fastlio2_autonomy_bridge/package.xml create mode 100644 src/slam/fastlio2_autonomy_bridge/src/fastlio2_autonomy_bridge_node.cpp diff --git a/src/base_autonomy/vehicle_simulator/launch/system_bagfile.launch.py b/src/base_autonomy/vehicle_simulator/launch/system_bagfile.launch.py index fc78986c..cfdccf2f 100644 --- a/src/base_autonomy/vehicle_simulator/launch/system_bagfile.launch.py +++ b/src/base_autonomy/vehicle_simulator/launch/system_bagfile.launch.py @@ -6,8 +6,10 @@ from launch.launch_description_sources import FrontendLaunchDescriptionSource, PythonLaunchDescriptionSource from launch_ros.actions import Node from launch.substitutions import LaunchConfiguration +from launch.conditions import IfCondition, UnlessCondition def generate_launch_description(): + use_fastlio2 = LaunchConfiguration('use_fastlio2') world_name = LaunchConfiguration('world_name') sensorOffsetX = LaunchConfiguration('sensorOffsetX') sensorOffsetY = LaunchConfiguration('sensorOffsetY') @@ -16,6 +18,11 @@ def generate_launch_description(): vehicleY = LaunchConfiguration('vehicleY') checkTerrainConn = LaunchConfiguration('checkTerrainConn') + declare_use_fastlio2 = DeclareLaunchArgument( + 'use_fastlio2', + default_value='false', + description='Use FASTLIO2 (with PGO/localizer bridge) instead of arise_slam_mid360 (true/false)' + ) declare_world_name = DeclareLaunchArgument('world_name', default_value='real_world', description='') declare_sensorOffsetX = DeclareLaunchArgument('sensorOffsetX', default_value='0.05', description='') declare_sensorOffsetY = DeclareLaunchArgument('sensorOffsetY', default_value='0.0', description='') @@ -62,7 +69,15 @@ def generate_launch_description(): start_arise_slam = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join( get_package_share_directory('arise_slam_mid360'), 'launch', 'arize_slam.launch.py') - ) + ), + condition=UnlessCondition(use_fastlio2) + ) + + start_fastlio2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join( + get_package_share_directory('fastlio2_autonomy_bridge'), 'launch', 'fastlio2_autonomy.launch.py') + ), + condition=IfCondition(use_fastlio2) ) start_visualization_tools = IncludeLaunchDescription( @@ -89,6 +104,7 @@ def generate_launch_description(): ld = LaunchDescription() # Add the actions + ld.add_action(declare_use_fastlio2) ld.add_action(declare_world_name) ld.add_action(declare_sensorOffsetX) ld.add_action(declare_sensorOffsetY) @@ -102,6 +118,7 @@ def generate_launch_description(): ld.add_action(start_terrain_analysis_ext) ld.add_action(start_sensor_scan_generation) ld.add_action(start_arise_slam) + ld.add_action(start_fastlio2) ld.add_action(start_visualization_tools) ld.add_action(start_joy) diff --git a/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_exploration_planner.launch.py b/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_exploration_planner.launch.py index 9f8328aa..c688ded3 100644 --- a/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_exploration_planner.launch.py +++ b/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_exploration_planner.launch.py @@ -6,8 +6,10 @@ from launch.launch_description_sources import FrontendLaunchDescriptionSource, PythonLaunchDescriptionSource from launch_ros.actions import Node from launch.substitutions import LaunchConfiguration +from launch.conditions import IfCondition, UnlessCondition def generate_launch_description(): + use_fastlio2 = LaunchConfiguration('use_fastlio2') exploration_planner_config = LaunchConfiguration('exploration_planner_config') world_name = LaunchConfiguration('world_name') sensorOffsetX = LaunchConfiguration('sensorOffsetX') @@ -17,6 +19,11 @@ def generate_launch_description(): vehicleY = LaunchConfiguration('vehicleY') checkTerrainConn = LaunchConfiguration('checkTerrainConn') + declare_use_fastlio2 = DeclareLaunchArgument( + 'use_fastlio2', + default_value='false', + description='Use FASTLIO2 (with PGO/localizer bridge) instead of arise_slam_mid360 (true/false)' + ) declare_exploration_planner_config = DeclareLaunchArgument('exploration_planner_config', default_value='indoor_small', description='') declare_world_name = DeclareLaunchArgument('world_name', default_value='real_world', description='') declare_sensorOffsetX = DeclareLaunchArgument('sensorOffsetX', default_value='0.05', description='') @@ -64,7 +71,15 @@ def generate_launch_description(): start_arise_slam = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join( get_package_share_directory('arise_slam_mid360'), 'launch', 'arize_slam.launch.py') - ) + ), + condition=UnlessCondition(use_fastlio2) + ) + + start_fastlio2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join( + get_package_share_directory('fastlio2_autonomy_bridge'), 'launch', 'fastlio2_autonomy.launch.py') + ), + condition=IfCondition(use_fastlio2) ) start_visualization_tools = IncludeLaunchDescription( @@ -99,6 +114,7 @@ def generate_launch_description(): ld = LaunchDescription() # Add the actions + ld.add_action(declare_use_fastlio2) ld.add_action(declare_exploration_planner_config) ld.add_action(declare_world_name) ld.add_action(declare_sensorOffsetX) @@ -113,6 +129,7 @@ def generate_launch_description(): ld.add_action(start_terrain_analysis_ext) ld.add_action(start_sensor_scan_generation) ld.add_action(start_arise_slam) + ld.add_action(start_fastlio2) ld.add_action(start_visualization_tools) ld.add_action(start_joy) ld.add_action(start_tare_planner) diff --git a/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_route_planner.launch.py b/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_route_planner.launch.py index 1c3d3d15..fa6d2aee 100644 --- a/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_route_planner.launch.py +++ b/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_route_planner.launch.py @@ -10,6 +10,7 @@ from launch_ros.substitutions import FindPackageShare def generate_launch_description(): + use_fastlio2 = LaunchConfiguration('use_fastlio2') use_pct_planner = LaunchConfiguration('use_pct_planner') route_planner_config = LaunchConfiguration('route_planner_config') world_name = LaunchConfiguration('world_name') @@ -20,6 +21,11 @@ def generate_launch_description(): vehicleY = LaunchConfiguration('vehicleY') checkTerrainConn = LaunchConfiguration('checkTerrainConn') + declare_use_fastlio2 = DeclareLaunchArgument( + 'use_fastlio2', + default_value='false', + description='Use FASTLIO2 (with PGO/localizer bridge) instead of arise_slam_mid360 (true/false)' + ) declare_use_pct_planner = DeclareLaunchArgument( 'use_pct_planner', default_value='true', @@ -72,7 +78,15 @@ def generate_launch_description(): start_arise_slam = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join( get_package_share_directory('arise_slam_mid360'), 'launch', 'arize_slam.launch.py') - ) + ), + condition=UnlessCondition(use_fastlio2) + ) + + start_fastlio2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join( + get_package_share_directory('fastlio2_autonomy_bridge'), 'launch', 'fastlio2_autonomy.launch.py') + ), + condition=IfCondition(use_fastlio2) ) start_visualization_tools = IncludeLaunchDescription( @@ -117,6 +131,7 @@ def generate_launch_description(): ld = LaunchDescription() # Add the actions + ld.add_action(declare_use_fastlio2) ld.add_action(declare_use_pct_planner) ld.add_action(declare_route_planner_config) ld.add_action(declare_world_name) @@ -132,6 +147,7 @@ def generate_launch_description(): ld.add_action(start_terrain_analysis_ext) ld.add_action(start_sensor_scan_generation) ld.add_action(start_arise_slam) + ld.add_action(start_fastlio2) ld.add_action(start_visualization_tools) ld.add_action(start_joy) ld.add_action(start_far_planner) diff --git a/src/base_autonomy/vehicle_simulator/launch/system_real_robot.launch.py b/src/base_autonomy/vehicle_simulator/launch/system_real_robot.launch.py index 38fbf22c..259dff5e 100644 --- a/src/base_autonomy/vehicle_simulator/launch/system_real_robot.launch.py +++ b/src/base_autonomy/vehicle_simulator/launch/system_real_robot.launch.py @@ -6,8 +6,10 @@ from launch.launch_description_sources import FrontendLaunchDescriptionSource, PythonLaunchDescriptionSource from launch_ros.actions import Node from launch.substitutions import LaunchConfiguration +from launch.conditions import IfCondition, UnlessCondition def generate_launch_description(): + use_fastlio2 = LaunchConfiguration('use_fastlio2') world_name = LaunchConfiguration('world_name') sensorOffsetX = LaunchConfiguration('sensorOffsetX') sensorOffsetY = LaunchConfiguration('sensorOffsetY') @@ -16,6 +18,11 @@ def generate_launch_description(): vehicleY = LaunchConfiguration('vehicleY') checkTerrainConn = LaunchConfiguration('checkTerrainConn') + declare_use_fastlio2 = DeclareLaunchArgument( + 'use_fastlio2', + default_value='true', + description='Use FASTLIO2 (with PGO/localizer bridge) instead of arise_slam_mid360 (true/false)' + ) declare_world_name = DeclareLaunchArgument('world_name', default_value='real_world', description='') declare_sensorOffsetX = DeclareLaunchArgument('sensorOffsetX', default_value='0.05', description='') declare_sensorOffsetY = DeclareLaunchArgument('sensorOffsetY', default_value='0.0', description='') @@ -62,7 +69,15 @@ def generate_launch_description(): start_arise_slam = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join( get_package_share_directory('arise_slam_mid360'), 'launch', 'arize_slam.launch.py') - ) + ), + condition=UnlessCondition(use_fastlio2) + ) + + start_fastlio2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join( + get_package_share_directory('fastlio2_autonomy_bridge'), 'launch', 'fastlio2_autonomy.launch.py') + ), + condition=IfCondition(use_fastlio2) ) start_visualization_tools = IncludeLaunchDescription( @@ -94,6 +109,7 @@ def generate_launch_description(): ld = LaunchDescription() # Add the actions + ld.add_action(declare_use_fastlio2) ld.add_action(declare_world_name) ld.add_action(declare_sensorOffsetX) ld.add_action(declare_sensorOffsetY) @@ -107,6 +123,7 @@ def generate_launch_description(): ld.add_action(start_terrain_analysis_ext) ld.add_action(start_sensor_scan_generation) ld.add_action(start_arise_slam) + ld.add_action(start_fastlio2) ld.add_action(start_visualization_tools) ld.add_action(start_joy) ld.add_action(start_mid360) diff --git a/src/base_autonomy/vehicle_simulator/launch/system_real_robot_with_exploration_planner.launch.py b/src/base_autonomy/vehicle_simulator/launch/system_real_robot_with_exploration_planner.launch.py index 0a554c86..2418dffa 100644 --- a/src/base_autonomy/vehicle_simulator/launch/system_real_robot_with_exploration_planner.launch.py +++ b/src/base_autonomy/vehicle_simulator/launch/system_real_robot_with_exploration_planner.launch.py @@ -6,8 +6,10 @@ from launch.launch_description_sources import FrontendLaunchDescriptionSource, PythonLaunchDescriptionSource from launch_ros.actions import Node from launch.substitutions import LaunchConfiguration +from launch.conditions import IfCondition, UnlessCondition def generate_launch_description(): + use_fastlio2 = LaunchConfiguration('use_fastlio2') exploration_planner_config = LaunchConfiguration('exploration_planner_config') world_name = LaunchConfiguration('world_name') sensorOffsetX = LaunchConfiguration('sensorOffsetX') @@ -17,6 +19,11 @@ def generate_launch_description(): vehicleY = LaunchConfiguration('vehicleY') checkTerrainConn = LaunchConfiguration('checkTerrainConn') + declare_use_fastlio2 = DeclareLaunchArgument( + 'use_fastlio2', + default_value='false', + description='Use FASTLIO2 (with PGO/localizer bridge) instead of arise_slam_mid360 (true/false)' + ) declare_exploration_planner_config = DeclareLaunchArgument('exploration_planner_config', default_value='indoor_small', description='') declare_world_name = DeclareLaunchArgument('world_name', default_value='real_world', description='') declare_sensorOffsetX = DeclareLaunchArgument('sensorOffsetX', default_value='0.05', description='') @@ -64,7 +71,15 @@ def generate_launch_description(): start_arise_slam = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join( get_package_share_directory('arise_slam_mid360'), 'launch', 'arize_slam.launch.py') - ) + ), + condition=UnlessCondition(use_fastlio2) + ) + + start_fastlio2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join( + get_package_share_directory('fastlio2_autonomy_bridge'), 'launch', 'fastlio2_autonomy.launch.py') + ), + condition=IfCondition(use_fastlio2) ) start_visualization_tools = IncludeLaunchDescription( @@ -104,6 +119,7 @@ def generate_launch_description(): ld = LaunchDescription() # Add the actions + ld.add_action(declare_use_fastlio2) ld.add_action(declare_exploration_planner_config) ld.add_action(declare_world_name) ld.add_action(declare_sensorOffsetX) @@ -118,6 +134,7 @@ def generate_launch_description(): ld.add_action(start_terrain_analysis_ext) ld.add_action(start_sensor_scan_generation) ld.add_action(start_arise_slam) + ld.add_action(start_fastlio2) ld.add_action(start_visualization_tools) ld.add_action(start_joy) ld.add_action(start_tare_planner) diff --git a/src/base_autonomy/vehicle_simulator/launch/system_real_robot_with_route_planner.launch.py b/src/base_autonomy/vehicle_simulator/launch/system_real_robot_with_route_planner.launch.py index bf394c7a..b19466d9 100644 --- a/src/base_autonomy/vehicle_simulator/launch/system_real_robot_with_route_planner.launch.py +++ b/src/base_autonomy/vehicle_simulator/launch/system_real_robot_with_route_planner.launch.py @@ -10,6 +10,7 @@ from launch_ros.substitutions import FindPackageShare def generate_launch_description(): + use_fastlio2 = LaunchConfiguration('use_fastlio2') use_pct_planner = LaunchConfiguration('use_pct_planner') route_planner_config = LaunchConfiguration('route_planner_config') world_name = LaunchConfiguration('world_name') @@ -20,6 +21,11 @@ def generate_launch_description(): vehicleY = LaunchConfiguration('vehicleY') checkTerrainConn = LaunchConfiguration('checkTerrainConn') + declare_use_fastlio2 = DeclareLaunchArgument( + 'use_fastlio2', + default_value='false', + description='Use FASTLIO2 (with PGO/localizer bridge) instead of arise_slam_mid360 (true/false)' + ) declare_use_pct_planner = DeclareLaunchArgument( 'use_pct_planner', default_value='false', @@ -72,7 +78,15 @@ def generate_launch_description(): start_arise_slam = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join( get_package_share_directory('arise_slam_mid360'), 'launch', 'arize_slam.launch.py') - ) + ), + condition=UnlessCondition(use_fastlio2) + ) + + start_fastlio2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join( + get_package_share_directory('fastlio2_autonomy_bridge'), 'launch', 'fastlio2_autonomy.launch.py') + ), + condition=IfCondition(use_fastlio2) ) start_visualization_tools = IncludeLaunchDescription( @@ -122,6 +136,7 @@ def generate_launch_description(): ld = LaunchDescription() # Add the actions + ld.add_action(declare_use_fastlio2) ld.add_action(declare_use_pct_planner) ld.add_action(declare_route_planner_config) ld.add_action(declare_world_name) @@ -137,6 +152,7 @@ def generate_launch_description(): ld.add_action(start_terrain_analysis_ext) ld.add_action(start_sensor_scan_generation) ld.add_action(start_arise_slam) + ld.add_action(start_fastlio2) ld.add_action(start_visualization_tools) ld.add_action(start_joy) ld.add_action(start_far_planner) diff --git a/src/base_autonomy/vehicle_simulator/launch/system_real_slam_only.launch.py b/src/base_autonomy/vehicle_simulator/launch/system_real_slam_only.launch.py index 2ba3f268..2a06e5db 100644 --- a/src/base_autonomy/vehicle_simulator/launch/system_real_slam_only.launch.py +++ b/src/base_autonomy/vehicle_simulator/launch/system_real_slam_only.launch.py @@ -2,15 +2,32 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition, UnlessCondition from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration def generate_launch_description(): + use_fastlio2 = LaunchConfiguration('use_fastlio2') + + declare_use_fastlio2 = DeclareLaunchArgument( + 'use_fastlio2', + default_value='false', + description='Use FASTLIO2 (with PGO/localizer bridge) instead of arise_slam_mid360 (true/false)' + ) start_arise_slam = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join( get_package_share_directory('arise_slam_mid360'), 'launch', 'arize_slam.launch.py') - ) + ), + condition=UnlessCondition(use_fastlio2) + ) + + start_fastlio2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join( + get_package_share_directory('fastlio2_autonomy_bridge'), 'launch', 'fastlio2_autonomy.launch.py') + ), + condition=IfCondition(use_fastlio2) ) start_mid360 = IncludeLaunchDescription( @@ -21,7 +38,9 @@ def generate_launch_description(): ld = LaunchDescription() # Add the actions + ld.add_action(declare_use_fastlio2) ld.add_action(start_arise_slam) + ld.add_action(start_fastlio2) ld.add_action(start_mid360) return ld \ No newline at end of file diff --git a/src/slam/FASTLIO2_ROS2/fastlio2/config/lio.yaml b/src/slam/FASTLIO2_ROS2/fastlio2/config/lio.yaml index 5d77ae3c..0ace2f20 100644 --- a/src/slam/FASTLIO2_ROS2/fastlio2/config/lio.yaml +++ b/src/slam/FASTLIO2_ROS2/fastlio2/config/lio.yaml @@ -1,9 +1,9 @@ imu_topic: /livox/imu lidar_topic: /livox/lidar -body_frame: body -world_frame: lidar +body_frame: sensor +world_frame: map print_time_cost: false -print_odom: true +print_odom: false odom_log_interval: 0.5 # seconds between odom logs (2Hz) lidar_filter_num: 6 diff --git a/src/slam/FASTLIO2_ROS2/fastlio2/config/lio_autonomy.yaml b/src/slam/FASTLIO2_ROS2/fastlio2/config/lio_autonomy.yaml new file mode 100644 index 00000000..88b74c51 --- /dev/null +++ b/src/slam/FASTLIO2_ROS2/fastlio2/config/lio_autonomy.yaml @@ -0,0 +1,55 @@ +imu_topic: /imu/data +lidar_topic: /lidar/scan + +# Frames: +# - world_frame: FASTLIO2 local drift frame (PGO/localizer provide map->world_frame correction TF) +# - body_frame: robot body frame (child frame in odometry) +body_frame: sensor +world_frame: map + +print_time_cost: false +print_odom: false +odom_log_interval: 0.5 # seconds between odom logs (2Hz) + +publish_body_aligned: true + +auto_pitch_align: true +auto_pitch_align_samples: 30 +# Example for a MID360 pitched down by 30deg relative to base (tilt about +Y in ROS ENU): +# r_bi: [ 0.8660254, 0.0, -0.5, +# 0.0, 1.0, 0.0, +# 0.5, 0.0, 0.8660254 ] +r_bi: [1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0] + +lidar_filter_num: 6 +lidar_min_range: 0.5 +lidar_max_range: 30.0 +scan_resolution: 0.15 +map_resolution: 0.3 + +cube_len: 300 +det_range: 60 +move_thresh: 1.5 + +na: 0.01 +ng: 0.01 +nba: 0.0001 +nbg: 0.0001 + +imu_init_num: 20 +near_search_num: 5 +ieskf_max_iter: 5 + +gravity_align: true +esti_il: false + +r_il: [1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0] +t_il: [-0.011, -0.02329, 0.04412] + +lidar_cov_inv: 1000.0 + + diff --git a/src/slam/FASTLIO2_ROS2/fastlio2/src/lio_node.cpp b/src/slam/FASTLIO2_ROS2/fastlio2/src/lio_node.cpp index ef006ffa..7fc747de 100644 --- a/src/slam/FASTLIO2_ROS2/fastlio2/src/lio_node.cpp +++ b/src/slam/FASTLIO2_ROS2/fastlio2/src/lio_node.cpp @@ -20,6 +20,8 @@ #include #include #include +#include +#include using namespace std::chrono_literals; struct NodeConfig @@ -31,6 +33,19 @@ struct NodeConfig bool print_time_cost = false; bool print_odom = false; double odom_log_interval = 5.0; // seconds between odom logs + + // If your IMU is mounted with a fixed tilt relative to your robot base, + // you typically want /state_estimation orientation to represent the robot base (level when robot is level), + // not the raw IMU frame. Set publish_body_aligned=true and provide r_bi (rotation from IMU frame -> body/base frame). + bool publish_body_aligned = false; + Eigen::Matrix3d r_bi = Eigen::Matrix3d::Identity(); // body^R_imu + + // Auto-estimate a pitch-only mounting offset at startup (useful for integrated LiDAR+IMU units like MID360). + // Assumption: robot starts on level ground; the observed IMU pitch is the mounting pitch offset. + // When enabled, FASTLIO2 will compute r_bi = Ry(pitch0) using the first N mapping outputs and then + // publish odom/TF/body_cloud with that pitch removed. (Yaw/roll are untouched; only pitch is compensated.) + bool auto_pitch_align = false; + int auto_pitch_align_samples = 30; }; struct StateData { @@ -96,6 +111,33 @@ class LIONode : public rclcpp::Node if (config["odom_log_interval"]) m_node_config.odom_log_interval = config["odom_log_interval"].as(); + if (config["publish_body_aligned"]) + m_node_config.publish_body_aligned = config["publish_body_aligned"].as(); + if (config["r_bi"]) + { + std::vector r_bi_vec = config["r_bi"].as>(); + if (r_bi_vec.size() == 9) + { + m_node_config.r_bi << r_bi_vec[0], r_bi_vec[1], r_bi_vec[2], + r_bi_vec[3], r_bi_vec[4], r_bi_vec[5], + r_bi_vec[6], r_bi_vec[7], r_bi_vec[8]; + } + } + + if (config["auto_pitch_align"]) + m_node_config.auto_pitch_align = config["auto_pitch_align"].as(); + if (config["auto_pitch_align_samples"]) + m_node_config.auto_pitch_align_samples = config["auto_pitch_align_samples"].as(); + + if (m_node_config.auto_pitch_align) + { + if (m_node_config.auto_pitch_align_samples < 1) + m_node_config.auto_pitch_align_samples = 1; + RCLCPP_WARN(this->get_logger(), + "[AUTO_PITCH_ALIGN] Enabled. Will estimate pitch offset from first %d frames.", + m_node_config.auto_pitch_align_samples); + } + m_builder_config.lidar_filter_num = config["lidar_filter_num"].as(); m_builder_config.lidar_min_range = config["lidar_min_range"].as(); m_builder_config.lidar_max_range = config["lidar_max_range"].as(); @@ -193,10 +235,20 @@ class LIONode : public rclcpp::Node odom.header.frame_id = frame_id; odom.header.stamp = Utils::getTime(time); odom.child_frame_id = child_frame; + + // State is IMU pose in world: R_wi, t_wi. + // If publish_body_aligned is enabled, publish body/base pose: R_wb = R_wi * (R_bi)^T + // where R_bi maps IMU vectors into body vectors (body^R_imu). + const Eigen::Matrix3d &R_wi = m_kf->x().r_wi; + Eigen::Matrix3d R_w_out = R_wi; + if (m_node_config.publish_body_aligned) + R_w_out = R_wi * m_node_config.r_bi.transpose(); + odom.pose.pose.position.x = m_kf->x().t_wi.x(); odom.pose.pose.position.y = m_kf->x().t_wi.y(); odom.pose.pose.position.z = m_kf->x().t_wi.z(); - Eigen::Quaterniond q(m_kf->x().r_wi); + + Eigen::Quaterniond q(R_w_out); odom.pose.pose.orientation.x = q.x(); odom.pose.pose.orientation.y = q.y(); odom.pose.pose.orientation.z = q.z(); @@ -250,7 +302,12 @@ class LIONode : public rclcpp::Node transformStamped.header.frame_id = frame_id; transformStamped.child_frame_id = child_frame; transformStamped.header.stamp = Utils::getTime(time); - Eigen::Quaterniond q(m_kf->x().r_wi); + const Eigen::Matrix3d &R_wi = m_kf->x().r_wi; + Eigen::Matrix3d R_w_out = R_wi; + if (m_node_config.publish_body_aligned) + R_w_out = R_wi * m_node_config.r_bi.transpose(); + + Eigen::Quaterniond q(R_w_out); V3D t = m_kf->x().t_wi; transformStamped.transform.translation.x = t.x(); transformStamped.transform.translation.y = t.y(); @@ -279,11 +336,47 @@ class LIONode : public rclcpp::Node if (m_builder->status() != BuilderStatus::MAPPING) return; + // Auto pitch-only mounting offset estimation (publishing only) + if (m_node_config.auto_pitch_align && !pitch_align_done_) + { + const Eigen::Matrix3d &R_wi = m_kf->x().r_wi; + Eigen::Quaterniond q_wi(R_wi); + tf2::Quaternion q_tf(q_wi.x(), q_wi.y(), q_wi.z(), q_wi.w()); + double roll = 0.0, pitch = 0.0, yaw = 0.0; + tf2::Matrix3x3(q_tf).getRPY(roll, pitch, yaw); + + pitch_sum_ += pitch; + pitch_samples_ += 1; + + if (pitch_samples_ >= m_node_config.auto_pitch_align_samples) + { + const double pitch0 = pitch_sum_ / static_cast(pitch_samples_); + // r_bi is body^R_imu. Using a pure pitch rotation cancels the startup IMU pitch when publishing: + // R_w_body = R_w_imu * (r_bi)^T = R_w_imu * Ry(-pitch0) + const Eigen::AngleAxisd aa(pitch0, Eigen::Vector3d::UnitY()); + m_node_config.r_bi = aa.toRotationMatrix(); + m_node_config.publish_body_aligned = true; + pitch_align_done_ = true; + RCLCPP_WARN(this->get_logger(), + "[AUTO_PITCH_ALIGN] Done. pitch0=%.3f rad (%.1f deg). publish_body_aligned=true", + pitch0, pitch0 * 180.0 / M_PI); + } + } + broadCastTF(m_tf_broadcaster, m_node_config.world_frame, m_node_config.body_frame, m_package.cloud_end_time); publishOdometry(m_odom_pub, m_node_config.world_frame, m_node_config.body_frame, m_package.cloud_end_time); - CloudType::Ptr body_cloud = m_builder->lidar_processor()->transformCloud(m_package.cloud, m_kf->x().r_il, m_kf->x().t_il); + // "body_cloud" is typically expected in the robot base frame. If publish_body_aligned is enabled, + // rotate IMU-frame points into body/base frame using r_bi. + Eigen::Matrix3d r_out = m_kf->x().r_il; + Eigen::Vector3d t_out = m_kf->x().t_il; + if (m_node_config.publish_body_aligned) + { + r_out = m_node_config.r_bi * r_out; + t_out = m_node_config.r_bi * t_out; + } + CloudType::Ptr body_cloud = m_builder->lidar_processor()->transformCloud(m_package.cloud, r_out, t_out); publishCloud(m_body_cloud_pub, body_cloud, m_node_config.body_frame, m_package.cloud_end_time); @@ -311,6 +404,10 @@ class LIONode : public rclcpp::Node std::shared_ptr m_kf; std::shared_ptr m_builder; std::shared_ptr m_tf_broadcaster; + + bool pitch_align_done_ = false; + int pitch_samples_ = 0; + double pitch_sum_ = 0.0; }; int main(int argc, char **argv) diff --git a/src/slam/FASTLIO2_ROS2/fastlio2/src/map_builder/ieskf.cpp b/src/slam/FASTLIO2_ROS2/fastlio2/src/map_builder/ieskf.cpp index 53b5ef95..b840f94c 100644 --- a/src/slam/FASTLIO2_ROS2/fastlio2/src/map_builder/ieskf.cpp +++ b/src/slam/FASTLIO2_ROS2/fastlio2/src/map_builder/ieskf.cpp @@ -4,11 +4,47 @@ double State::gravity = 9.81; M3D Jr(const V3D &inp) { - return Sophus::SO3d::leftJacobian(inp).transpose(); + // Left Jacobian of SO(3) for rotation vector inp (body->world convention depends on usage). + // This implementation avoids Sophus API differences across distros. + const double theta = inp.norm(); + const M3D I = M3D::Identity(); + const M3D A = Sophus::SO3d::hat(inp); + + if (theta < 1e-5) + { + // Series: J ≈ I - 0.5*A + 1/6*A^2 + return (I - 0.5 * A + (1.0 / 6.0) * (A * A)).transpose(); + } + + const double theta2 = theta * theta; + const double theta3 = theta2 * theta; + const double s = std::sin(theta); + const double c = std::cos(theta); + + const M3D J = I - ((1.0 - c) / theta2) * A + ((theta - s) / theta3) * (A * A); + return J.transpose(); } M3D JrInv(const V3D &inp) { - return Sophus::SO3d::leftJacobianInverse(inp).transpose(); + // Inverse left Jacobian of SO(3) + const double theta = inp.norm(); + const M3D I = M3D::Identity(); + const M3D A = Sophus::SO3d::hat(inp); + + if (theta < 1e-5) + { + // Series: J^{-1} ≈ I + 0.5*A + 1/12*A^2 + return (I + 0.5 * A + (1.0 / 12.0) * (A * A)).transpose(); + } + + const double theta2 = theta * theta; + const double s = std::sin(theta); + const double c = std::cos(theta); + const double cot_half = (1.0 + c) / s; // cot(theta/2) + const double k = (1.0 / theta2) * (1.0 - 0.5 * theta * cot_half); + + const M3D Jinv = I + 0.5 * A + k * (A * A); + return Jinv.transpose(); } void State::operator+=(const V21D &delta) diff --git a/src/slam/FASTLIO2_ROS2/localizer/src/localizer_node.cpp b/src/slam/FASTLIO2_ROS2/localizer/src/localizer_node.cpp index cd883e71..4609e0ac 100644 --- a/src/slam/FASTLIO2_ROS2/localizer/src/localizer_node.cpp +++ b/src/slam/FASTLIO2_ROS2/localizer/src/localizer_node.cpp @@ -123,6 +123,25 @@ class LocalizerNode : public rclcpp::Node if (config["initial_roll"]) m_config.initial_roll = config["initial_roll"].as(); + // Allow launch-time override via ROS parameters (keeps YAML as defaults) + this->declare_parameter("auto_relocalize", m_config.auto_relocalize); + this->declare_parameter("default_pcd_path", m_config.default_pcd_path); + this->declare_parameter("initial_x", m_config.initial_x); + this->declare_parameter("initial_y", m_config.initial_y); + this->declare_parameter("initial_z", m_config.initial_z); + this->declare_parameter("initial_yaw", m_config.initial_yaw); + this->declare_parameter("initial_pitch", m_config.initial_pitch); + this->declare_parameter("initial_roll", m_config.initial_roll); + + this->get_parameter("auto_relocalize", m_config.auto_relocalize); + this->get_parameter("default_pcd_path", m_config.default_pcd_path); + this->get_parameter("initial_x", m_config.initial_x); + this->get_parameter("initial_y", m_config.initial_y); + this->get_parameter("initial_z", m_config.initial_z); + this->get_parameter("initial_yaw", m_config.initial_yaw); + this->get_parameter("initial_pitch", m_config.initial_pitch); + this->get_parameter("initial_roll", m_config.initial_roll); + if (m_config.auto_relocalize) { RCLCPP_INFO(this->get_logger(), "[AUTO-RELOC] Enabled with map: %s", m_config.default_pcd_path.c_str()); diff --git a/src/slam/fastlio2_autonomy_bridge/CMakeLists.txt b/src/slam/fastlio2_autonomy_bridge/CMakeLists.txt new file mode 100644 index 00000000..880a04f8 --- /dev/null +++ b/src/slam/fastlio2_autonomy_bridge/CMakeLists.txt @@ -0,0 +1,43 @@ +cmake_minimum_required(VERSION 3.8) +project(fastlio2_autonomy_bridge) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(interface REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_sensor_msgs REQUIRED) + +add_executable(fastlio2_autonomy_bridge_node src/fastlio2_autonomy_bridge_node.cpp) +ament_target_dependencies( + fastlio2_autonomy_bridge_node + rclcpp + nav_msgs + sensor_msgs + geometry_msgs + interface + tf2 + tf2_ros + tf2_geometry_msgs + tf2_sensor_msgs +) + +install(TARGETS fastlio2_autonomy_bridge_node + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + +ament_package() + + diff --git a/src/slam/fastlio2_autonomy_bridge/launch/fastlio2_autonomy.launch.py b/src/slam/fastlio2_autonomy_bridge/launch/fastlio2_autonomy.launch.py new file mode 100644 index 00000000..423ee6ce --- /dev/null +++ b/src/slam/fastlio2_autonomy_bridge/launch/fastlio2_autonomy.launch.py @@ -0,0 +1,110 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + map_path_env = os.environ.get("MAP_PATH", "") + + # If MAP_PATH is set, default to localization mode (localizer provides map->lidar correction TF) + default_local_mode = "true" if map_path_env else "false" + if map_path_env.endswith(".pcd"): + default_map_path = map_path_env + else: + default_map_path = (map_path_env + ".pcd") if map_path_env else "" + + local_mode = LaunchConfiguration("local_mode") + relocalization_map_path = LaunchConfiguration("relocalization_map_path") + + declare_local_mode = DeclareLaunchArgument( + "local_mode", + default_value=default_local_mode, + description="Use localization mode (localizer) instead of mapping mode (pgo). Auto from MAP_PATH env.", + ) + declare_relocalization_map_path = DeclareLaunchArgument( + "relocalization_map_path", + default_value=default_map_path, + description="Map file for relocalization (.pcd). Auto from MAP_PATH env.", + ) + + # FASTLIO2 LIO + lio_config_path = os.path.join( + get_package_share_directory("fastlio2"), "config", "lio_autonomy.yaml" + ) + lio_node = Node( + package="fastlio2", + namespace="fastlio2", + executable="lio_node", + name="lio_node", + output="screen", + parameters=[{"config_path": lio_config_path}], + ) + + # Mapping mode: PGO provides map->lidar correction TF (identity initially, then loop closure offsets) + pgo_config_path = os.path.join( + get_package_share_directory("pgo"), "config", "pgo.yaml" + ) + pgo_node = Node( + package="pgo", + namespace="pgo", + executable="pgo_node", + name="pgo_node", + output="screen", + parameters=[{"config_path": pgo_config_path}], + condition=UnlessCondition(local_mode), + ) + + # Localization mode: localizer provides map->lidar correction TF (ICP relocalization) + localizer_config_path = os.path.join( + get_package_share_directory("localizer"), "config", "localizer.yaml" + ) + localizer_node = Node( + package="localizer", + namespace="localizer", + executable="localizer_node", + name="localizer_node", + output="screen", + parameters=[ + {"config_path": localizer_config_path}, + {"default_pcd_path": relocalization_map_path}, + ], + condition=IfCondition(local_mode), + ) + + # Bridge: publish autonomy stack topics in map frame + bridge_node = Node( + package="fastlio2_autonomy_bridge", + executable="fastlio2_autonomy_bridge_node", + name="fastlio2_autonomy_bridge", + output="screen", + parameters=[ + {"input_odom_topic": "/fastlio2/lio_odom"}, + {"input_cloud_topic": "/fastlio2/world_cloud"}, + {"output_odom_topic": "/state_estimation"}, + {"output_cloud_topic": "/registered_scan"}, + {"map_frame": "map"}, + {"tf_timeout_sec": 0.02}, + # In localization mode we subscribe to RViz /initialpose and call /localizer/relocalize. + # Before convergence, we fall back to raw LIO on the same output topics. + {"local_mode": local_mode}, + {"relocalization_map_path": relocalization_map_path}, + ], + ) + + return LaunchDescription( + [ + declare_local_mode, + declare_relocalization_map_path, + lio_node, + pgo_node, + localizer_node, + bridge_node, + ] + ) + + diff --git a/src/slam/fastlio2_autonomy_bridge/package.xml b/src/slam/fastlio2_autonomy_bridge/package.xml new file mode 100644 index 00000000..8b657e97 --- /dev/null +++ b/src/slam/fastlio2_autonomy_bridge/package.xml @@ -0,0 +1,25 @@ + + + fastlio2_autonomy_bridge + 0.0.1 + Bridge FASTLIO2 (optionally with PGO/localizer correction TF) into autonomy stack topics (/state_estimation, /registered_scan). + alexlin + MIT + + ament_cmake + + rclcpp + nav_msgs + sensor_msgs + geometry_msgs + interface + tf2 + tf2_ros + tf2_geometry_msgs + tf2_sensor_msgs + + + ament_cmake + + + diff --git a/src/slam/fastlio2_autonomy_bridge/src/fastlio2_autonomy_bridge_node.cpp b/src/slam/fastlio2_autonomy_bridge/src/fastlio2_autonomy_bridge_node.cpp new file mode 100644 index 00000000..0b7ffdcc --- /dev/null +++ b/src/slam/fastlio2_autonomy_bridge/src/fastlio2_autonomy_bridge_node.cpp @@ -0,0 +1,282 @@ +#include + +#include +#include +#include +#include + +#include "interface/srv/is_valid.hpp" +#include "interface/srv/relocalize.hpp" + +#include +#include +#include +#include +#include +#include +#include + +class Fastlio2AutonomyBridge : public rclcpp::Node +{ +public: + Fastlio2AutonomyBridge() + : Node("fastlio2_autonomy_bridge"), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_) + { + input_odom_topic_ = this->declare_parameter("input_odom_topic", "/fastlio2/lio_odom"); + input_cloud_topic_ = this->declare_parameter("input_cloud_topic", "/fastlio2/world_cloud"); + output_odom_topic_ = this->declare_parameter("output_odom_topic", "/state_estimation"); + output_cloud_topic_ = this->declare_parameter("output_cloud_topic", "/registered_scan"); + + // Publishing contract for autonomy stack + map_frame_ = this->declare_parameter("map_frame", "map"); + // If non-empty, we will look up TF from map_frame -> local_frame. If empty, we auto-detect local_frame + // from the incoming odometry header.frame_id. + local_frame_ = this->declare_parameter("local_frame", ""); + tf_timeout_sec_ = this->declare_parameter("tf_timeout_sec", 0.02); + + // Localization mode support: RViz /initialpose triggers localizer service. Until converged, we fall back to raw LIO. + local_mode_ = this->declare_parameter("local_mode", false); + relocalization_map_path_ = this->declare_parameter("relocalization_map_path", ""); + + pub_odom_ = this->create_publisher(output_odom_topic_, rclcpp::QoS(10)); + pub_cloud_ = this->create_publisher(output_cloud_topic_, rclcpp::QoS(10)); + + sub_odom_ = this->create_subscription( + input_odom_topic_, + rclcpp::QoS(50), + std::bind(&Fastlio2AutonomyBridge::odomCB, this, std::placeholders::_1)); + + sub_cloud_ = this->create_subscription( + input_cloud_topic_, + rclcpp::SensorDataQoS(), + std::bind(&Fastlio2AutonomyBridge::cloudCB, this, std::placeholders::_1)); + + if (local_mode_) + { + reloc_client_ = this->create_client("/localizer/relocalize"); + reloc_check_client_ = this->create_client("/localizer/relocalize_check"); + sub_initialpose_ = this->create_subscription( + "/initialpose", + rclcpp::QoS(10), + std::bind(&Fastlio2AutonomyBridge::initialposeCB, this, std::placeholders::_1)); + + check_timer_ = this->create_wall_timer( + std::chrono::milliseconds(250), + std::bind(&Fastlio2AutonomyBridge::checkValidCB, this)); + } + + RCLCPP_INFO(this->get_logger(), + "FASTLIO2 bridge: odom %s -> %s, cloud %s -> %s, map_frame=%s local_frame=%s local_mode=%d", + input_odom_topic_.c_str(), output_odom_topic_.c_str(), + input_cloud_topic_.c_str(), output_cloud_topic_.c_str(), + map_frame_.c_str(), local_frame_.empty() ? "(auto)" : local_frame_.c_str(), + local_mode_ ? 1 : 0); + } + +private: + geometry_msgs::msg::TransformStamped lookupMapToLocal(const builtin_interfaces::msg::Time & stamp) + { + const rclcpp::Duration timeout = rclcpp::Duration::from_seconds(tf_timeout_sec_); + const rclcpp::Time t_msg(stamp.sec, stamp.nanosec, this->get_clock()->get_clock_type()); + const rclcpp::Time t_latest(0, 0, this->get_clock()->get_clock_type()); + + // Always use latest TF for robustness in real-time pipelines. + (void)t_msg; + return tf_buffer_.lookupTransform(map_frame_, local_frame_, t_latest, timeout); + } + + void odomCB(const nav_msgs::msg::Odometry::SharedPtr msg) + { + if (!local_frame_ready_) + { + // Auto-detect local frame from upstream odometry. This should match localizer's behavior too. + if (local_frame_.empty()) + { + local_frame_ = msg->header.frame_id; + } + local_frame_ready_ = !local_frame_.empty(); + } + + geometry_msgs::msg::TransformStamped tf_map_local; + try { + // In localization mode, only publish corrected once the localizer reports valid. + // Otherwise, fall back to raw LIO (still published to keep autonomy running). + if (local_mode_ && !localization_valid_) + throw tf2::TransformException("localization not valid yet"); + tf_map_local = lookupMapToLocal(msg->header.stamp); + } catch (const tf2::TransformException & ex) { + nav_msgs::msg::Odometry out = *msg; + out.header.frame_id = map_frame_; + pub_odom_->publish(out); + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000, + "Publishing raw odom (no correction): %s", ex.what()); + return; + } + + tf2::Transform T_map_local; + tf2::fromMsg(tf_map_local.transform, T_map_local); + + tf2::Transform T_local_body; + tf2::fromMsg(msg->pose.pose, T_local_body); + + const tf2::Transform T_map_body = T_map_local * T_local_body; + + nav_msgs::msg::Odometry out = *msg; + out.header.frame_id = map_frame_; + out.pose.pose.position.x = T_map_body.getOrigin().x(); + out.pose.pose.position.y = T_map_body.getOrigin().y(); + out.pose.pose.position.z = T_map_body.getOrigin().z(); + const tf2::Quaternion q = T_map_body.getRotation(); + out.pose.pose.orientation = tf2::toMsg(q); + pub_odom_->publish(out); + } + + void cloudCB(const sensor_msgs::msg::PointCloud2::SharedPtr msg) + { + if (!local_frame_ready_) + { + // If cloud arrives before odom and user provided no local_frame, we can't TF-correct; publish raw. + sensor_msgs::msg::PointCloud2 out = *msg; + out.header.frame_id = map_frame_; + pub_cloud_->publish(out); + return; + } + + geometry_msgs::msg::TransformStamped tf_map_local; + try { + if (local_mode_ && !localization_valid_) + throw tf2::TransformException("localization not valid yet"); + tf_map_local = lookupMapToLocal(msg->header.stamp); + } catch (const tf2::TransformException & ex) { + sensor_msgs::msg::PointCloud2 out = *msg; + out.header.frame_id = map_frame_; + pub_cloud_->publish(out); + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000, + "Publishing raw cloud (no correction): %s", ex.what()); + return; + } + + sensor_msgs::msg::PointCloud2 out; + tf2::doTransform(*msg, out, tf_map_local); + out.header.frame_id = map_frame_; + pub_cloud_->publish(out); + } + + void initialposeCB(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) + { + if (!local_mode_) + return; + if (relocalization_map_path_.empty()) + { + RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 2000, + "Received /initialpose but relocalization_map_path is empty."); + return; + } + + tf2::Quaternion q; + tf2::fromMsg(msg->pose.pose.orientation, q); + double roll = 0.0, pitch = 0.0, yaw = 0.0; + tf2::Matrix3x3(q).getRPY(roll, pitch, yaw); + + auto req = std::make_shared(); + req->pcd_path = relocalization_map_path_; + req->x = static_cast(msg->pose.pose.position.x); + req->y = static_cast(msg->pose.pose.position.y); + req->z = static_cast(msg->pose.pose.position.z); + req->yaw = static_cast(yaw); + req->pitch = static_cast(pitch); + req->roll = static_cast(roll); + + has_initialpose_ = true; + localization_valid_ = false; + pending_relocalize_ = true; + + if (!reloc_client_ || !reloc_client_->service_is_ready()) + { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000, + "Relocalize service not ready yet."); + return; + } + + (void)reloc_client_->async_send_request( + req, + [this](rclcpp::Client::SharedFuture fut) { + const auto resp = fut.get(); + if (!resp->success) + { + RCLCPP_ERROR(this->get_logger(), "[RELOCALIZE] Service failed: %s", resp->message.c_str()); + return; + } + RCLCPP_INFO(this->get_logger(), "[RELOCALIZE] Initial pose sent. Waiting for convergence..."); + }); + } + + void checkValidCB() + { + if (!local_mode_) + return; + if (!has_initialpose_) + return; + if (!pending_relocalize_) + return; + if (!reloc_check_client_ || !reloc_check_client_->service_is_ready()) + return; + + auto req = std::make_shared(); + req->code = 0; + + (void)reloc_check_client_->async_send_request( + req, + [this](rclcpp::Client::SharedFuture fut) { + const auto resp = fut.get(); + if (resp->valid) + { + localization_valid_ = true; + pending_relocalize_ = false; + RCLCPP_WARN(this->get_logger(), "[RELOCALIZE] Converged. Switching to corrected /state_estimation and /registered_scan."); + } + }); + } + +private: + std::string input_odom_topic_; + std::string input_cloud_topic_; + std::string output_odom_topic_; + std::string output_cloud_topic_; + std::string map_frame_; + std::string local_frame_; + double tf_timeout_sec_; + + bool local_mode_; + bool local_frame_ready_ = false; + rclcpp::Subscription::SharedPtr sub_odom_; + rclcpp::Subscription::SharedPtr sub_cloud_; + rclcpp::Publisher::SharedPtr pub_odom_; + rclcpp::Publisher::SharedPtr pub_cloud_; + + std::string relocalization_map_path_; + + bool has_initialpose_ = false; + bool localization_valid_ = false; + bool pending_relocalize_ = false; + + rclcpp::Subscription::SharedPtr sub_initialpose_; + rclcpp::Client::SharedPtr reloc_client_; + rclcpp::Client::SharedPtr reloc_check_client_; + rclcpp::TimerBase::SharedPtr check_timer_; + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} + + From c5bd6d19b91e5fd1b022e1b6a1be3b26cfa14129 Mon Sep 17 00:00:00 2001 From: Bona Date: Tue, 27 Jan 2026 22:50:28 -0800 Subject: [PATCH 4/9] feat: add use_sim_time support to FASTLIO2 launch file --- .../launch/fastlio2_autonomy.launch.py | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/slam/fastlio2_autonomy_bridge/launch/fastlio2_autonomy.launch.py b/src/slam/fastlio2_autonomy_bridge/launch/fastlio2_autonomy.launch.py index 423ee6ce..14268be8 100644 --- a/src/slam/fastlio2_autonomy_bridge/launch/fastlio2_autonomy.launch.py +++ b/src/slam/fastlio2_autonomy_bridge/launch/fastlio2_autonomy.launch.py @@ -2,7 +2,7 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument +from launch.actions import DeclareLaunchArgument, SetParameter from launch.conditions import IfCondition, UnlessCondition from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node @@ -20,7 +20,13 @@ def generate_launch_description(): local_mode = LaunchConfiguration("local_mode") relocalization_map_path = LaunchConfiguration("relocalization_map_path") + use_sim_time = LaunchConfiguration("use_sim_time") + declare_use_sim_time = DeclareLaunchArgument( + "use_sim_time", + default_value="false", + description="Use simulation clock for bagfile playback", + ) declare_local_mode = DeclareLaunchArgument( "local_mode", default_value=default_local_mode, @@ -98,8 +104,10 @@ def generate_launch_description(): return LaunchDescription( [ + declare_use_sim_time, declare_local_mode, declare_relocalization_map_path, + SetParameter(name="use_sim_time", value=use_sim_time), lio_node, pgo_node, localizer_node, From 6f697ad4c4c21063822c7b631963309fd4f2aa10 Mon Sep 17 00:00:00 2001 From: Bona Date: Tue, 27 Jan 2026 23:08:35 -0800 Subject: [PATCH 5/9] feat: add use_sim_time support to bagfile launch files Enable proper TF synchronization when playing back bagfiles with ros2 bag play --clock. Defaults to true for bagfile mode. --- .../launch/system_bagfile.launch.py | 12 ++++++++++-- ...system_bagfile_with_exploration_planner.launch.py | 12 ++++++++++-- .../system_bagfile_with_route_planner.launch.py | 10 +++++++++- 3 files changed, 29 insertions(+), 5 deletions(-) diff --git a/src/base_autonomy/vehicle_simulator/launch/system_bagfile.launch.py b/src/base_autonomy/vehicle_simulator/launch/system_bagfile.launch.py index cfdccf2f..1961138b 100644 --- a/src/base_autonomy/vehicle_simulator/launch/system_bagfile.launch.py +++ b/src/base_autonomy/vehicle_simulator/launch/system_bagfile.launch.py @@ -2,13 +2,14 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, TimerAction +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, SetParameter, TimerAction from launch.launch_description_sources import FrontendLaunchDescriptionSource, PythonLaunchDescriptionSource from launch_ros.actions import Node -from launch.substitutions import LaunchConfiguration +from launch.substitutions import LaunchConfiguration from launch.conditions import IfCondition, UnlessCondition def generate_launch_description(): + use_sim_time = LaunchConfiguration('use_sim_time') use_fastlio2 = LaunchConfiguration('use_fastlio2') world_name = LaunchConfiguration('world_name') sensorOffsetX = LaunchConfiguration('sensorOffsetX') @@ -18,6 +19,11 @@ def generate_launch_description(): vehicleY = LaunchConfiguration('vehicleY') checkTerrainConn = LaunchConfiguration('checkTerrainConn') + declare_use_sim_time = DeclareLaunchArgument( + 'use_sim_time', + default_value='true', + description='Use simulation clock for bagfile playback (default true for bagfile mode)' + ) declare_use_fastlio2 = DeclareLaunchArgument( 'use_fastlio2', default_value='false', @@ -104,6 +110,8 @@ def generate_launch_description(): ld = LaunchDescription() # Add the actions + ld.add_action(declare_use_sim_time) + ld.add_action(SetParameter(name='use_sim_time', value=use_sim_time)) ld.add_action(declare_use_fastlio2) ld.add_action(declare_world_name) ld.add_action(declare_sensorOffsetX) diff --git a/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_exploration_planner.launch.py b/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_exploration_planner.launch.py index c688ded3..429aa913 100644 --- a/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_exploration_planner.launch.py +++ b/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_exploration_planner.launch.py @@ -2,13 +2,14 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, TimerAction +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, SetParameter, TimerAction from launch.launch_description_sources import FrontendLaunchDescriptionSource, PythonLaunchDescriptionSource from launch_ros.actions import Node -from launch.substitutions import LaunchConfiguration +from launch.substitutions import LaunchConfiguration from launch.conditions import IfCondition, UnlessCondition def generate_launch_description(): + use_sim_time = LaunchConfiguration('use_sim_time') use_fastlio2 = LaunchConfiguration('use_fastlio2') exploration_planner_config = LaunchConfiguration('exploration_planner_config') world_name = LaunchConfiguration('world_name') @@ -19,6 +20,11 @@ def generate_launch_description(): vehicleY = LaunchConfiguration('vehicleY') checkTerrainConn = LaunchConfiguration('checkTerrainConn') + declare_use_sim_time = DeclareLaunchArgument( + 'use_sim_time', + default_value='true', + description='Use simulation clock for bagfile playback (default true for bagfile mode)' + ) declare_use_fastlio2 = DeclareLaunchArgument( 'use_fastlio2', default_value='false', @@ -114,6 +120,8 @@ def generate_launch_description(): ld = LaunchDescription() # Add the actions + ld.add_action(declare_use_sim_time) + ld.add_action(SetParameter(name='use_sim_time', value=use_sim_time)) ld.add_action(declare_use_fastlio2) ld.add_action(declare_exploration_planner_config) ld.add_action(declare_world_name) diff --git a/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_route_planner.launch.py b/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_route_planner.launch.py index fa6d2aee..08fe28c7 100644 --- a/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_route_planner.launch.py +++ b/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_route_planner.launch.py @@ -2,7 +2,7 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, TimerAction +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, SetParameter, TimerAction from launch.launch_description_sources import FrontendLaunchDescriptionSource, PythonLaunchDescriptionSource from launch_ros.actions import Node from launch.substitutions import LaunchConfiguration, PathJoinSubstitution @@ -10,6 +10,7 @@ from launch_ros.substitutions import FindPackageShare def generate_launch_description(): + use_sim_time = LaunchConfiguration('use_sim_time') use_fastlio2 = LaunchConfiguration('use_fastlio2') use_pct_planner = LaunchConfiguration('use_pct_planner') route_planner_config = LaunchConfiguration('route_planner_config') @@ -21,6 +22,11 @@ def generate_launch_description(): vehicleY = LaunchConfiguration('vehicleY') checkTerrainConn = LaunchConfiguration('checkTerrainConn') + declare_use_sim_time = DeclareLaunchArgument( + 'use_sim_time', + default_value='true', + description='Use simulation clock for bagfile playback (default true for bagfile mode)' + ) declare_use_fastlio2 = DeclareLaunchArgument( 'use_fastlio2', default_value='false', @@ -131,6 +137,8 @@ def generate_launch_description(): ld = LaunchDescription() # Add the actions + ld.add_action(declare_use_sim_time) + ld.add_action(SetParameter(name='use_sim_time', value=use_sim_time)) ld.add_action(declare_use_fastlio2) ld.add_action(declare_use_pct_planner) ld.add_action(declare_route_planner_config) From 3f203de31293c8252fbed7951ca0a8540e3bde9d Mon Sep 17 00:00:00 2001 From: Bona Date: Wed, 28 Jan 2026 16:16:29 -0800 Subject: [PATCH 6/9] fix: import SetParameter from launch_ros.actions for Jazzy compatibility launch.actions.SetParameter only exists in Humble; Jazzy requires launch_ros.actions.SetParameter. The launch_ros import works on both. --- .../vehicle_simulator/launch/system_bagfile.launch.py | 4 ++-- .../launch/system_bagfile_with_exploration_planner.launch.py | 4 ++-- .../launch/system_bagfile_with_route_planner.launch.py | 4 ++-- .../launch/fastlio2_autonomy.launch.py | 4 ++-- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/base_autonomy/vehicle_simulator/launch/system_bagfile.launch.py b/src/base_autonomy/vehicle_simulator/launch/system_bagfile.launch.py index 1961138b..86515bc9 100644 --- a/src/base_autonomy/vehicle_simulator/launch/system_bagfile.launch.py +++ b/src/base_autonomy/vehicle_simulator/launch/system_bagfile.launch.py @@ -2,9 +2,9 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, SetParameter, TimerAction +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, TimerAction from launch.launch_description_sources import FrontendLaunchDescriptionSource, PythonLaunchDescriptionSource -from launch_ros.actions import Node +from launch_ros.actions import Node, SetParameter from launch.substitutions import LaunchConfiguration from launch.conditions import IfCondition, UnlessCondition diff --git a/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_exploration_planner.launch.py b/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_exploration_planner.launch.py index 429aa913..91c645d6 100644 --- a/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_exploration_planner.launch.py +++ b/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_exploration_planner.launch.py @@ -2,9 +2,9 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, SetParameter, TimerAction +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, TimerAction from launch.launch_description_sources import FrontendLaunchDescriptionSource, PythonLaunchDescriptionSource -from launch_ros.actions import Node +from launch_ros.actions import Node, SetParameter from launch.substitutions import LaunchConfiguration from launch.conditions import IfCondition, UnlessCondition diff --git a/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_route_planner.launch.py b/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_route_planner.launch.py index 08fe28c7..dca9a4fe 100644 --- a/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_route_planner.launch.py +++ b/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_route_planner.launch.py @@ -2,9 +2,9 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, SetParameter, TimerAction +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, TimerAction from launch.launch_description_sources import FrontendLaunchDescriptionSource, PythonLaunchDescriptionSource -from launch_ros.actions import Node +from launch_ros.actions import Node, SetParameter from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch.conditions import IfCondition, UnlessCondition from launch_ros.substitutions import FindPackageShare diff --git a/src/slam/fastlio2_autonomy_bridge/launch/fastlio2_autonomy.launch.py b/src/slam/fastlio2_autonomy_bridge/launch/fastlio2_autonomy.launch.py index 14268be8..b03009d2 100644 --- a/src/slam/fastlio2_autonomy_bridge/launch/fastlio2_autonomy.launch.py +++ b/src/slam/fastlio2_autonomy_bridge/launch/fastlio2_autonomy.launch.py @@ -2,10 +2,10 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, SetParameter +from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition, UnlessCondition from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node +from launch_ros.actions import Node, SetParameter def generate_launch_description(): From 08bec80b43bdc6839eb7357b36e2adfc7478baad Mon Sep 17 00:00:00 2001 From: bona Date: Wed, 4 Feb 2026 20:13:05 -0800 Subject: [PATCH 7/9] feat: add Docker build and deployment setup - Multi-stage Dockerfile (build + runtime) for all ROS 2 packages - docker-compose.yml for deployment (Docker 24.x compatible) - build.sh to build, tag, and push to iserverobotics/nav_autonomy - .env for hardware configuration (lidar, motor, network) - .dockerignore to exclude build artifacts from context --- .dockerignore | 12 ++ docker/.env | 69 ++++++++++ docker/Dockerfile | 278 ++++++++++++++++++++++++++++++++++++++ docker/build.sh | 83 ++++++++++++ docker/docker-compose.yml | 82 +++++++++++ 5 files changed, 524 insertions(+) create mode 100644 .dockerignore create mode 100644 docker/.env create mode 100644 docker/Dockerfile create mode 100755 docker/build.sh create mode 100644 docker/docker-compose.yml diff --git a/.dockerignore b/.dockerignore new file mode 100644 index 00000000..cfc33870 --- /dev/null +++ b/.dockerignore @@ -0,0 +1,12 @@ +.git +build +install +log +docker +map +base_station +chrony_conf +desktop_buttons +img +*.md +LICENSE diff --git a/docker/.env b/docker/.env new file mode 100644 index 00000000..722fcf41 --- /dev/null +++ b/docker/.env @@ -0,0 +1,69 @@ +# Hardware Configuration Environment Variables +# Customize for your hardware setup + +# ============================================ +# Docker Runtime +# ============================================ +# Set to nvidia for GPU support (requires nvidia-container-toolkit) +#DOCKER_RUNTIME=nvidia + +# Image tag (default: latest) +IMAGE_TAG=latest + +# ============================================ +# ROS Configuration +# ============================================ +# ROS domain ID for multi-robot setups +ROS_DOMAIN_ID=42 + +# ============================================ +# Mid-360 Lidar Configuration +# ============================================ +# Network interface connected to the lidar (e.g., eth0, enp0s3) +# Find with: ip addr show +LIDAR_INTERFACE=eth0 + +# Processing computer IP address on the lidar subnet +# Must be on the same subnet as the lidar (e.g., 192.168.1.5) +LIDAR_COMPUTER_IP=192.168.1.5 + +# Gateway IP address for the lidar subnet +LIDAR_GATEWAY=192.168.1.1 + +# Full IP address of your Mid-360 lidar +# Common pattern: 192.168.1.1XX where XX = last 2 digits of serial +LIDAR_IP=192.168.1.116 + +# ============================================ +# Motor Controller Configuration +# ============================================ +# Serial device for motor controller +# Check with: ls /dev/ttyACM* or ls /dev/ttyUSB* +MOTOR_SERIAL_DEVICE=/dev/ttyACM0 + +# ============================================ +# Network Communication +# ============================================ +# Enable WiFi buffer optimization for wireless data transmission +ENABLE_WIFI_BUFFER=false + +# ============================================ +# Navigation Options +# ============================================ +# Enable RViz visualization (set to true for debugging) +USE_RVIZ=false + +# Map path for localization mode (leave empty for SLAM/mapping mode) +# Set to file prefix (no .pcd extension), e.g., /ros2_ws/maps/warehouse +MAP_PATH= + +# ============================================ +# Device Group IDs +# ============================================ +# Group ID for /dev/input devices (joystick) +# Find with: getent group input | cut -d: -f3 +INPUT_GID=995 + +# Group ID for serial devices +# Find with: getent group dialout | cut -d: -f3 +DIALOUT_GID=20 diff --git a/docker/Dockerfile b/docker/Dockerfile new file mode 100644 index 00000000..95945b45 --- /dev/null +++ b/docker/Dockerfile @@ -0,0 +1,278 @@ +# ============================================================================= +# Navigation Autonomy Stack Docker Image +# ============================================================================= +# +# Multi-stage build for ROS 2 navigation with SLAM support. +# Includes arise_slam and FASTLIO2. +# +# Build: +# ./docker/build.sh # Build for ROS 2 Jazzy (default) +# ./docker/build.sh --humble # Build for ROS 2 Humble +# +# ============================================================================= + +ARG ROS_DISTRO=jazzy +ARG TARGETARCH + +# Platform-specific base images +FROM osrf/ros:${ROS_DISTRO}-desktop-full AS base-amd64 +FROM ros:${ROS_DISTRO}-ros-base AS base-arm64 + +# ============================================================================= +# STAGE 1: Build +# ============================================================================= +FROM base-${TARGETARCH} AS builder + +ARG ROS_DISTRO +ENV DEBIAN_FRONTEND=noninteractive +ENV ROS_DISTRO=${ROS_DISTRO} +ENV WORKSPACE=/ros2_ws + +# Install build dependencies +RUN apt-get update && apt-get install -y --no-install-recommends \ + git \ + cmake \ + build-essential \ + python3-colcon-common-extensions \ + libpcl-dev \ + libgoogle-glog-dev \ + libgflags-dev \ + libatlas-base-dev \ + libeigen3-dev \ + libsuitesparse-dev \ + ros-${ROS_DISTRO}-pcl-ros \ + ros-${ROS_DISTRO}-cv-bridge \ + && rm -rf /var/lib/apt/lists/* + +# On arm64, ros-base doesn't include rviz2; install separately for building rviz plugins +ARG TARGETARCH +RUN if [ "${TARGETARCH}" = "arm64" ]; then \ + apt-get update && apt-get install -y --no-install-recommends \ + ros-${ROS_DISTRO}-rviz2 \ + && rm -rf /var/lib/apt/lists/*; \ + fi + +# On arm64, build or-tools from source (pre-built binaries are x86_64 only) +ENV OR_TOOLS_VERSION=9.8 +RUN if [ "${TARGETARCH}" = "arm64" ]; then \ + echo "Building or-tools v${OR_TOOLS_VERSION} from source for arm64..." && \ + apt-get update && apt-get install -y --no-install-recommends \ + lsb-release \ + wget \ + && rm -rf /var/lib/apt/lists/* && \ + cd /tmp && \ + wget -q https://github.com/google/or-tools/archive/refs/tags/v${OR_TOOLS_VERSION}.tar.gz && \ + tar xzf v${OR_TOOLS_VERSION}.tar.gz && \ + cd or-tools-${OR_TOOLS_VERSION} && \ + cmake -S . -B build \ + -DCMAKE_BUILD_TYPE=Release \ + -DBUILD_DEPS=ON \ + -DBUILD_SAMPLES=OFF \ + -DBUILD_EXAMPLES=OFF \ + -DBUILD_FLATZINC=OFF \ + -DUSE_SCIP=OFF \ + -DUSE_COINOR=OFF && \ + cmake --build build --config Release -j$(($(nproc) > 4 ? 4 : $(nproc))) && \ + cmake --install build --prefix /opt/or-tools && \ + rm -rf /tmp/or-tools-${OR_TOOLS_VERSION} /tmp/v${OR_TOOLS_VERSION}.tar.gz; \ + fi + +# Create workspace and copy source +RUN mkdir -p ${WORKSPACE}/src +COPY src ${WORKSPACE}/src + +# On arm64, replace x86_64 or-tools with arm64 build +RUN if [ "${TARGETARCH}" = "arm64" ] && [ -d "/opt/or-tools" ]; then \ + echo "Replacing x86_64 or-tools with arm64 build..." && \ + OR_TOOLS_DIR=${WORKSPACE}/src/exploration_planner/tare_planner/or-tools && \ + if [ -d "${OR_TOOLS_DIR}" ]; then \ + rm -rf ${OR_TOOLS_DIR}/lib/*.so* ${OR_TOOLS_DIR}/lib/*.a && \ + cp -r /opt/or-tools/lib/* ${OR_TOOLS_DIR}/lib/ && \ + rm -rf ${OR_TOOLS_DIR}/include && \ + cp -r /opt/or-tools/include ${OR_TOOLS_DIR}/ && \ + ldconfig; \ + fi; \ + fi + +# Compatibility fix: In Humble, cv_bridge uses .h extension; Jazzy uses .hpp +RUN if [ "${ROS_DISTRO}" = "humble" ]; then \ + CV_BRIDGE_DIR=$(find /opt/ros/humble/include -name "cv_bridge.h" -printf "%h\n" 2>/dev/null | head -1) && \ + if [ -n "$CV_BRIDGE_DIR" ]; then \ + ln -sf "$CV_BRIDGE_DIR/cv_bridge.h" "$CV_BRIDGE_DIR/cv_bridge.hpp"; \ + fi; \ + fi + +# Build Livox-SDK2 +RUN cd ${WORKSPACE}/src/utilities/livox_ros_driver2/Livox-SDK2 && \ + mkdir -p build && cd build && \ + cmake .. && make -j$(nproc) && make install && ldconfig && \ + rm -rf ${WORKSPACE}/src/utilities/livox_ros_driver2/Livox-SDK2/build + +# Build Sophus +RUN cd ${WORKSPACE}/src/slam/dependency/Sophus && \ + mkdir -p build && cd build && \ + cmake .. -DBUILD_TESTS=OFF && make -j$(nproc) && make install && \ + rm -rf ${WORKSPACE}/src/slam/dependency/Sophus/build + +# Build Ceres Solver +RUN cd ${WORKSPACE}/src/slam/dependency/ceres-solver && \ + mkdir -p build && cd build && \ + cmake .. && make -j$(nproc) && make install && \ + rm -rf ${WORKSPACE}/src/slam/dependency/ceres-solver/build + +# Build GTSAM +RUN cd ${WORKSPACE}/src/slam/dependency/gtsam && \ + mkdir -p build && cd build && \ + cmake .. -DGTSAM_USE_SYSTEM_EIGEN=ON -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF && \ + make -j$(nproc) && make install && ldconfig && \ + rm -rf ${WORKSPACE}/src/slam/dependency/gtsam/build + +# Build ROS workspace +RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash && \ + cd ${WORKSPACE} && \ + colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release" + +# ============================================================================= +# STAGE 2: Runtime +# ============================================================================= +ARG ROS_DISTRO +ARG TARGETARCH +FROM base-${TARGETARCH} AS runtime + +ARG ROS_DISTRO +ENV DEBIAN_FRONTEND=noninteractive +ENV ROS_DISTRO=${ROS_DISTRO} +ENV WORKSPACE=/ros2_ws +ENV RMW_IMPLEMENTATION=rmw_fastrtps_cpp + +# Install runtime dependencies +RUN apt-get update && apt-get install -y --no-install-recommends \ + ros-${ROS_DISTRO}-pcl-ros \ + ros-${ROS_DISTRO}-cv-bridge \ + ros-${ROS_DISTRO}-rviz2 \ + ros-${ROS_DISTRO}-joy \ + ros-${ROS_DISTRO}-rmw-fastrtps-cpp \ + libpcl-dev \ + libgoogle-glog-dev \ + libgflags-dev \ + libatlas-base-dev \ + libeigen3-dev \ + libsuitesparse-dev \ + libx11-6 \ + libxext6 \ + libxrender1 \ + libgl1 \ + libglib2.0-0 \ + iputils-ping \ + net-tools \ + iproute2 \ + usbutils \ + joystick \ + && rm -rf /var/lib/apt/lists/* + +# Copy installed libraries from builder (Livox-SDK2, Sophus, Ceres, GTSAM) +COPY --from=builder /usr/local/lib /usr/local/lib +COPY --from=builder /usr/local/include /usr/local/include +RUN ldconfig + +# Copy built ROS workspace +COPY --from=builder ${WORKSPACE}/install ${WORKSPACE}/install + +# Copy rviz config files from source +COPY --from=builder ${WORKSPACE}/src/base_autonomy/vehicle_simulator/rviz ${WORKSPACE}/src/base_autonomy/vehicle_simulator/rviz +COPY --from=builder ${WORKSPACE}/src/route_planner/far_planner/rviz ${WORKSPACE}/src/route_planner/far_planner/rviz +COPY --from=builder ${WORKSPACE}/src/exploration_planner/tare_planner/rviz ${WORKSPACE}/src/exploration_planner/tare_planner/rviz + +# Copy lidar config +COPY --from=builder ${WORKSPACE}/src/utilities/livox_ros_driver2/config ${WORKSPACE}/src/utilities/livox_ros_driver2/config + +# Copy SLAM config files (arise_slam + FASTLIO2) +RUN --mount=from=builder,source=${WORKSPACE}/src,target=/tmp/src \ + mkdir -p ${WORKSPACE}/src/slam/arise_slam_mid360 && \ + cp -r /tmp/src/slam/arise_slam_mid360/config ${WORKSPACE}/src/slam/arise_slam_mid360/ 2>/dev/null || true && \ + mkdir -p ${WORKSPACE}/src/slam/FASTLIO2_ROS2 && \ + for pkg in fastlio2 localizer pgo hba; do \ + if [ -d "/tmp/src/slam/FASTLIO2_ROS2/$pkg/config" ]; then \ + mkdir -p ${WORKSPACE}/src/slam/FASTLIO2_ROS2/$pkg && \ + cp -r /tmp/src/slam/FASTLIO2_ROS2/$pkg/config ${WORKSPACE}/src/slam/FASTLIO2_ROS2/$pkg/; \ + fi; \ + if [ -d "/tmp/src/slam/FASTLIO2_ROS2/$pkg/rviz" ]; then \ + cp -r /tmp/src/slam/FASTLIO2_ROS2/$pkg/rviz ${WORKSPACE}/src/slam/FASTLIO2_ROS2/$pkg/; \ + fi; \ + done + +# Copy system launch scripts +COPY system_real_robot_with_route_planner.sh ${WORKSPACE}/ +COPY system_real_robot.sh ${WORKSPACE}/ +COPY system_real_robot_with_exploration_planner.sh ${WORKSPACE}/ +COPY system_bagfile.sh ${WORKSPACE}/ +COPY system_bagfile_with_route_planner.sh ${WORKSPACE}/ +COPY system_bagfile_with_exploration_planner.sh ${WORKSPACE}/ + +# Create directories for maps and logs +RUN mkdir -p ${WORKSPACE}/maps ${WORKSPACE}/logs + +# Set up shell environment +RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> ~/.bashrc && \ + echo "source ${WORKSPACE}/install/setup.bash" >> ~/.bashrc && \ + echo "export RMW_IMPLEMENTATION=rmw_fastrtps_cpp" >> ~/.bashrc + +# Entrypoint script +RUN cat > /ros_entrypoint.sh <<'ENTRYPOINT_EOF' +#!/bin/bash +set -e + +# Source ROS environment +source /opt/ros/${ROS_DISTRO}/setup.bash +source ${WORKSPACE}/install/setup.bash +export RMW_IMPLEMENTATION=rmw_fastrtps_cpp + +# Configure lidar network interface if specified +if [ -n "${LIDAR_INTERFACE}" ] && [ -n "${LIDAR_COMPUTER_IP}" ]; then + ip addr add ${LIDAR_COMPUTER_IP}/24 dev ${LIDAR_INTERFACE} 2>/dev/null || true + ip link set ${LIDAR_INTERFACE} up 2>/dev/null || true +fi + +# Generate MID360_config.json if lidar IPs are set +if [ -n "${LIDAR_COMPUTER_IP}" ] && [ -n "${LIDAR_IP}" ]; then + cat > ${WORKSPACE}/src/utilities/livox_ros_driver2/config/MID360_config.json </dev/null || true + echo "Generated MID360_config.json (LIDAR_IP=${LIDAR_IP}, COMPUTER_IP=${LIDAR_COMPUTER_IP})" +fi + +# WiFi buffer optimization +if [ "${ENABLE_WIFI_BUFFER}" = "true" ]; then + sysctl -w net.core.rmem_max=67108864 net.core.rmem_default=67108864 2>/dev/null || true + sysctl -w net.core.wmem_max=67108864 net.core.wmem_default=67108864 2>/dev/null || true +fi + +exec "$@" +ENTRYPOINT_EOF +RUN chmod +x /ros_entrypoint.sh + +WORKDIR ${WORKSPACE} +ENTRYPOINT ["/ros_entrypoint.sh"] +CMD ["bash"] diff --git a/docker/build.sh b/docker/build.sh new file mode 100755 index 00000000..a3eab92d --- /dev/null +++ b/docker/build.sh @@ -0,0 +1,83 @@ +#!/bin/bash + +set -e + +GREEN='\033[0;32m' +YELLOW='\033[1;33m' +RED='\033[0;31m' +NC='\033[0m' + +IMAGE_NAME="iserverobotics/nav_autonomy" +ROS_DISTRO="jazzy" +PUSH=true + +# Parse command line arguments +while [[ $# -gt 0 ]]; do + case $1 in + --humble) + ROS_DISTRO="humble" + shift + ;; + --jazzy) + ROS_DISTRO="jazzy" + shift + ;; + --no-push) + PUSH=false + shift + ;; + --help|-h) + echo "Usage: $0 [OPTIONS]" + echo "" + echo "Options:" + echo " --humble Build with ROS 2 Humble" + echo " --jazzy Build with ROS 2 Jazzy (default)" + echo " --no-push Build only, don't push to Docker Hub" + echo " --help, -h Show this help message" + exit 0 + ;; + *) + echo -e "${RED}Unknown option: $1${NC}" + echo "Run '$0 --help' for usage information" + exit 1 + ;; + esac +done + +IMAGE_TAG="${ROS_DISTRO}" + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +REPO_ROOT="$(cd "$SCRIPT_DIR/.." && pwd)" + +echo -e "${GREEN}================================================${NC}" +echo -e "${GREEN}Building Navigation Autonomy Stack Docker Image${NC}" +echo -e "${GREEN} Image: ${IMAGE_NAME}:${IMAGE_TAG}${NC}" +echo -e "${GREEN} ROS: ${ROS_DISTRO}${NC}" +echo -e "${GREEN} Context: ${REPO_ROOT}${NC}" +echo -e "${GREEN}================================================${NC}" +echo "" + +docker build \ + -f "${SCRIPT_DIR}/Dockerfile" \ + --build-arg ROS_DISTRO=${ROS_DISTRO} \ + -t ${IMAGE_NAME}:${IMAGE_TAG} \ + -t ${IMAGE_NAME}:latest \ + "${REPO_ROOT}" + +echo "" +echo -e "${GREEN}Build complete!${NC}" + +if [ "${PUSH}" = true ]; then + echo -e "${YELLOW}Pushing ${IMAGE_NAME}...${NC}" + docker push ${IMAGE_NAME}:${IMAGE_TAG} + docker push ${IMAGE_NAME}:latest + echo -e "${GREEN}Push complete!${NC}" +fi + +echo "" +echo -e "${GREEN}================================================${NC}" +echo -e "${GREEN}Done!${NC}" +echo -e "${GREEN}================================================${NC}" +echo "" +echo "To deploy:" +echo -e "${YELLOW} cd docker && docker compose up -d${NC}" diff --git a/docker/docker-compose.yml b/docker/docker-compose.yml new file mode 100644 index 00000000..f209abe0 --- /dev/null +++ b/docker/docker-compose.yml @@ -0,0 +1,82 @@ +version: "3.8" + +services: + nav_autonomy: + image: iserverobotics/nav_autonomy:${IMAGE_TAG:-latest} + container_name: nav_autonomy + + # Shared memory for ROS 2 FastDDS + shm_size: '8gb' + + stdin_open: true + tty: true + + # Host network required for ROS 2 DDS discovery and hardware access + network_mode: host + + # Privileged for hardware device access + privileged: true + + # GPU runtime (set DOCKER_RUNTIME=nvidia in .env if needed) + runtime: ${DOCKER_RUNTIME:-runc} + + restart: unless-stopped + + # Load hardware configuration from .env + env_file: + - .env + + environment: + - DISPLAY=${DISPLAY:-:0} + - QT_X11_NO_MITSHM=1 + - NVIDIA_VISIBLE_DEVICES=all + - NVIDIA_DRIVER_CAPABILITIES=all + - ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-42} + - RMW_IMPLEMENTATION=rmw_fastrtps_cpp + - LIDAR_INTERFACE=${LIDAR_INTERFACE:-} + - LIDAR_COMPUTER_IP=${LIDAR_COMPUTER_IP:-192.168.1.5} + - LIDAR_GATEWAY=${LIDAR_GATEWAY:-192.168.1.1} + - LIDAR_IP=${LIDAR_IP:-192.168.1.116} + - MOTOR_SERIAL_DEVICE=${MOTOR_SERIAL_DEVICE:-/dev/ttyACM0} + - ENABLE_WIFI_BUFFER=${ENABLE_WIFI_BUFFER:-false} + - USE_RVIZ=${USE_RVIZ:-false} + - MAP_PATH=${MAP_PATH:-} + + volumes: + # X11 for GUI (RViz) + - /tmp/.X11-unix:/tmp/.X11-unix:rw + - ${HOME}/.Xauthority:/root/.Xauthority:rw + # Time zone + - /etc/localtime:/etc/localtime:ro + - /etc/timezone:/etc/timezone:ro + # Persistent maps and logs + - ./maps:/ros2_ws/maps:rw + - ./logs:/ros2_ws/logs:rw + + devices: + - /dev/input:/dev/input + - /dev/dri:/dev/dri + - ${MOTOR_SERIAL_DEVICE:-/dev/ttyACM0}:${MOTOR_SERIAL_DEVICE:-/dev/ttyACM0} + + group_add: + - ${INPUT_GID:-995} + - ${DIALOUT_GID:-20} + + cap_add: + - NET_ADMIN + - SYS_ADMIN + - SYS_TIME + + command: + - bash + - -c + - | + echo "Starting real robot system with route planner..." + ros2 launch vehicle_simulator system_real_robot_with_route_planner.launch.py & + NAV_PID=$$! + sleep 2 + if [ "$$USE_RVIZ" = "true" ]; then + echo "Starting RViz2..." + ros2 run rviz2 rviz2 -d /ros2_ws/src/route_planner/far_planner/rviz/default.rviz & + fi + wait $$NAV_PID From e9893df1a933eae1ece26ddd2c65562050862904 Mon Sep 17 00:00:00 2001 From: bona Date: Wed, 4 Feb 2026 21:02:56 -0800 Subject: [PATCH 8/9] feat: add foxglove bridge support and default to FASTLIO2 - Add ros-foxglove-bridge to Dockerfile runtime dependencies - Launch foxglove_bridge on port 8765 in docker-compose command - Default to use_fastlio2:=true in launch command --- docker/Dockerfile | 1 + docker/docker-compose.yml | 4 +++- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/docker/Dockerfile b/docker/Dockerfile index 95945b45..6369f969 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -152,6 +152,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ ros-${ROS_DISTRO}-rviz2 \ ros-${ROS_DISTRO}-joy \ ros-${ROS_DISTRO}-rmw-fastrtps-cpp \ + ros-${ROS_DISTRO}-foxglove-bridge \ libpcl-dev \ libgoogle-glog-dev \ libgflags-dev \ diff --git a/docker/docker-compose.yml b/docker/docker-compose.yml index f209abe0..6a953609 100644 --- a/docker/docker-compose.yml +++ b/docker/docker-compose.yml @@ -72,9 +72,11 @@ services: - -c - | echo "Starting real robot system with route planner..." - ros2 launch vehicle_simulator system_real_robot_with_route_planner.launch.py & + ros2 launch vehicle_simulator system_real_robot_with_route_planner.launch.py use_fastlio2:=true & NAV_PID=$$! sleep 2 + echo "Starting Foxglove Bridge on port 8765..." + ros2 launch foxglove_bridge foxglove_bridge_launch.xml port:=8765 & if [ "$$USE_RVIZ" = "true" ]; then echo "Starting RViz2..." ros2 run rviz2 rviz2 -d /ros2_ws/src/route_planner/far_planner/rviz/default.rviz & From 3aa49ffc78ebd1b1efb39503fe1b681023f71830 Mon Sep 17 00:00:00 2001 From: bona Date: Sun, 8 Feb 2026 20:48:33 -0800 Subject: [PATCH 9/9] add lifecycle node conversion and foxglove utilities - Convert all 7 SLAM nodes to lifecycle nodes (FASTLIO2 + arise_slam) - Add handle_once=True to prevent re-activation loops - Add foxglove relay scripts (twist, goal/autonomy) - Update Docker and launch files --- docker/.env | 12 + docker/Dockerfile | 9 + docker/docker-compose.yml | 13 +- docker/fix.txt | 50 +++ .../foxglove_utility/goal_autonomy_relay.py | 84 +++++ docker/foxglove_utility/twist_relay.py | 62 ++++ .../launch/system_bagfile.launch.py | 26 ++ ...bagfile_with_exploration_planner.launch.py | 26 ++ ...ystem_bagfile_with_route_planner.launch.py | 26 ++ .../launch/system_real_robot.launch.py | 26 ++ ...l_robot_with_exploration_planner.launch.py | 26 ++ ...em_real_robot_with_route_planner.launch.py | 26 ++ .../launch/system_simulation.launch.py | 27 ++ ...ulation_with_exploration_planner.launch.py | 27 ++ ...em_simulation_with_route_planner.launch.py | 26 ++ .../FASTLIO2_ROS2/fastlio2/CMakeLists.txt | 3 +- src/slam/FASTLIO2_ROS2/fastlio2/package.xml | 1 + .../FASTLIO2_ROS2/fastlio2/src/lio_node.cpp | 93 +++++- .../FASTLIO2_ROS2/localizer/CMakeLists.txt | 3 +- src/slam/FASTLIO2_ROS2/localizer/package.xml | 1 + .../localizer/src/localizer_node.cpp | 70 +++- src/slam/FASTLIO2_ROS2/pgo/CMakeLists.txt | 3 +- src/slam/FASTLIO2_ROS2/pgo/package.xml | 1 + src/slam/FASTLIO2_ROS2/pgo/src/pgo_node.cpp | 70 +++- .../FASTLIO2_ROS2/pgo/src/pgo_node.cpp.bak | 307 ++++++++++++++++++ src/slam/arise_slam_mid360/CMakeLists.txt | 8 +- .../FeatureExtraction/featureExtraction.h | 29 +- .../ImuPreintegration/imuPreintegration.h | 21 +- .../LaserMapping/laserMapping.h | 50 +-- .../LidarProcess/LidarSlam.h | 5 +- .../arise_slam_mid360/config/parameter.h | 5 +- .../launch/arize_slam.launch.py | 79 ++++- src/slam/arise_slam_mid360/package.xml | 1 + .../FeatureExtraction/featureExtraction.cpp | 69 +++- .../ImuPreintegration/imuPreintegration.cpp | 73 ++++- .../src/LaserMapping/laserMapping.cpp | 80 ++++- .../src/LidarProcess/LidarSlam.cpp | 2 +- .../src/featureExtraction_node.cpp | 10 +- .../src/imuPreintegration_node.cpp | 8 +- .../src/laserMapping_node.cpp | 5 +- .../src/parameter/parameter.cpp | 8 +- .../fastlio2_autonomy_bridge/CMakeLists.txt | 2 + .../launch/fastlio2_autonomy.launch.py | 97 +++++- src/slam/fastlio2_autonomy_bridge/package.xml | 1 + .../src/fastlio2_autonomy_bridge_node.cpp | 86 ++++- 45 files changed, 1524 insertions(+), 133 deletions(-) create mode 100644 docker/fix.txt create mode 100644 docker/foxglove_utility/goal_autonomy_relay.py create mode 100644 docker/foxglove_utility/twist_relay.py create mode 100644 src/slam/FASTLIO2_ROS2/pgo/src/pgo_node.cpp.bak diff --git a/docker/.env b/docker/.env index 722fcf41..e2c8f4cf 100644 --- a/docker/.env +++ b/docker/.env @@ -57,6 +57,18 @@ USE_RVIZ=false # Set to file prefix (no .pcd extension), e.g., /ros2_ws/maps/warehouse MAP_PATH= +# ============================================ +# ROS Bridge Server (TCP API for programmatic control) +# ============================================ +# Bind address (0.0.0.0 = all interfaces) +#ROS_BRIDGE_HOST=0.0.0.0 + +# TCP port for the bridge API +#ROS_BRIDGE_PORT=9090 + +# Default autonomy speed (0.0 to 1.0) +#ROS_BRIDGE_AUTONOMY_SPEED=1.0 + # ============================================ # Device Group IDs # ============================================ diff --git a/docker/Dockerfile b/docker/Dockerfile index 6369f969..cc90db21 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -210,6 +210,11 @@ COPY system_bagfile.sh ${WORKSPACE}/ COPY system_bagfile_with_route_planner.sh ${WORKSPACE}/ COPY system_bagfile_with_exploration_planner.sh ${WORKSPACE}/ +# Copy foxglove relay scripts +COPY docker/foxglove_utility/twist_relay.py /usr/local/bin/twist_relay.py +COPY docker/foxglove_utility/goal_autonomy_relay.py /usr/local/bin/goal_autonomy_relay.py +RUN chmod +x /usr/local/bin/twist_relay.py /usr/local/bin/goal_autonomy_relay.py + # Create directories for maps and logs RUN mkdir -p ${WORKSPACE}/maps ${WORKSPACE}/logs @@ -270,6 +275,10 @@ if [ "${ENABLE_WIFI_BUFFER}" = "true" ]; then sysctl -w net.core.wmem_max=67108864 net.core.wmem_default=67108864 2>/dev/null || true fi +# Launch foxglove relay scripts in background +python3 /usr/local/bin/twist_relay.py & +python3 /usr/local/bin/goal_autonomy_relay.py & + exec "$@" ENTRYPOINT_EOF RUN chmod +x /ros_entrypoint.sh diff --git a/docker/docker-compose.yml b/docker/docker-compose.yml index 6a953609..96a1e8d0 100644 --- a/docker/docker-compose.yml +++ b/docker/docker-compose.yml @@ -41,6 +41,10 @@ services: - ENABLE_WIFI_BUFFER=${ENABLE_WIFI_BUFFER:-false} - USE_RVIZ=${USE_RVIZ:-false} - MAP_PATH=${MAP_PATH:-} + # ROS Bridge Server (TCP API for programmatic control, port 9090) + - ROS_BRIDGE_HOST=${ROS_BRIDGE_HOST:-0.0.0.0} + - ROS_BRIDGE_PORT=${ROS_BRIDGE_PORT:-9090} + - ROS_BRIDGE_AUTONOMY_SPEED=${ROS_BRIDGE_AUTONOMY_SPEED:-1.0} volumes: # X11 for GUI (RViz) @@ -72,7 +76,14 @@ services: - -c - | echo "Starting real robot system with route planner..." - ros2 launch vehicle_simulator system_real_robot_with_route_planner.launch.py use_fastlio2:=true & + echo " ROS Bridge Server: port $${ROS_BRIDGE_PORT:-9090} (TCP API)" + echo " Foxglove Bridge: port 8765 (WebSocket)" + ros2 launch vehicle_simulator system_real_robot_with_route_planner.launch.py \ + use_fastlio2:=true \ + enable_bridge:=true \ + bridge_host:=$${ROS_BRIDGE_HOST:-0.0.0.0} \ + bridge_port:=$${ROS_BRIDGE_PORT:-9090} \ + bridge_autonomy_speed:=$${ROS_BRIDGE_AUTONOMY_SPEED:-1.0} & NAV_PID=$$! sleep 2 echo "Starting Foxglove Bridge on port 8765..." diff --git a/docker/fix.txt b/docker/fix.txt new file mode 100644 index 00000000..8b403aac --- /dev/null +++ b/docker/fix.txt @@ -0,0 +1,50 @@ +# Fixes needed for nav_autonomy Docker image +# Apply these changes to the Dockerfile, then rebuild and push + +# ============================================================================ +# FIX 1: Add unitree_webrtc_connect to Dockerfile +# ============================================================================ +# Add after line ~357 (after portaudio19-dev install): + +# Install unitree_webrtc_connect for Unitree Go2 robot control +# Clone, fix missing __init__.py files (upstream bug), then install +RUN cd /tmp && \ + git clone https://github.com/VectorRobotics/unitree_webrtc_connect.git && \ + touch /tmp/unitree_webrtc_connect/go2_webrtc_driver/msgs/__init__.py && \ + touch /tmp/unitree_webrtc_connect/go2_webrtc_driver/lidar/__init__.py && \ + pip3 install --break-system-packages /tmp/unitree_webrtc_connect && \ + rm -rf /tmp/unitree_webrtc_connect + +# ============================================================================ +# FIX 2: Add relay scripts to Dockerfile +# ============================================================================ +# Add after the COPY commands for config files (around line ~387): + +COPY docker/navigation/foxglove_utility/twist_relay.py /usr/local/bin/twist_relay.py +COPY docker/navigation/foxglove_utility/goal_autonomy_relay.py /usr/local/bin/goal_autonomy_relay.py +RUN chmod +x /usr/local/bin/twist_relay.py /usr/local/bin/goal_autonomy_relay.py + +# ============================================================================ +# FIX 3: Ensure pip3 is available for installing Python packages +# ============================================================================ +# Add to the apt-get install section (around line ~25): + +python3-pip \ + +# ============================================================================ +# Summary of files to modify: +# ============================================================================ +# 1. docker/navigation/Dockerfile - add the above changes +# 2. Ensure docker/navigation/foxglove_utility/twist_relay.py exists +# 3. Ensure docker/navigation/foxglove_utility/goal_autonomy_relay.py exists + +# ============================================================================ +# Rebuild commands (run from dimos repo root): +# ============================================================================ +# cd /path/to/dimos +# docker compose -f docker/navigation/docker-compose.yml build +# docker tag dimos_autonomy_stack:jazzy iserverobotics/nav_autonomy:latest +# docker push iserverobotics/nav_autonomy:latest + +# Or for multi-arch: +# docker buildx build --platform linux/arm64 -t iserverobotics/nav_autonomy:latest --push -f docker/navigation/Dockerfile . diff --git a/docker/foxglove_utility/goal_autonomy_relay.py b/docker/foxglove_utility/goal_autonomy_relay.py new file mode 100644 index 00000000..4e075035 --- /dev/null +++ b/docker/foxglove_utility/goal_autonomy_relay.py @@ -0,0 +1,84 @@ +#!/usr/bin/env python3 +""" +Relay node that publishes Joy message to enable autonomy mode when goal_pose is received. +Mimics the behavior of the goalpoint_rviz_plugin for Foxglove compatibility. +""" + +from geometry_msgs.msg import PointStamped, PoseStamped +import rclpy +from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy +from sensor_msgs.msg import Joy + + +class GoalAutonomyRelay(Node): + def __init__(self): + super().__init__("goal_autonomy_relay") + + # QoS for goal topics (match foxglove_bridge) + goal_qos = QoSProfile( + reliability=ReliabilityPolicy.RELIABLE, + history=HistoryPolicy.KEEP_LAST, + durability=DurabilityPolicy.VOLATILE, + depth=5, + ) + + # Subscribe to goal_pose (PoseStamped from Foxglove) + self.pose_sub = self.create_subscription( + PoseStamped, "/goal_pose", self.goal_pose_callback, goal_qos + ) + + # Subscribe to way_point (PointStamped from Foxglove) + self.point_sub = self.create_subscription( + PointStamped, "/way_point", self.way_point_callback, goal_qos + ) + + # Publisher for Joy message to enable autonomy + self.joy_pub = self.create_publisher(Joy, "/joy", 5) + + self.get_logger().info( + "Goal autonomy relay started - will publish Joy to enable autonomy when goals are received" + ) + + def publish_autonomy_joy(self): + """Publish Joy message that enables autonomy mode (mimics goalpoint_rviz_plugin)""" + joy = Joy() + joy.header.stamp = self.get_clock().now().to_msg() + joy.header.frame_id = "goal_autonomy_relay" + + # axes[2] = -1.0 enables autonomy mode in pathFollower + # axes[4] = 1.0 sets forward speed + # axes[5] = 1.0 enables obstacle checking + joy.axes = [0.0, 0.0, -1.0, 0.0, 1.0, 1.0, 0.0, 0.0] + + # buttons[7] = 1 (same as RViz plugin) + joy.buttons = [0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0] + + self.joy_pub.publish(joy) + self.get_logger().info("Published Joy message to enable autonomy mode") + + def goal_pose_callback(self, msg: PoseStamped): + self.get_logger().info( + f"Received goal_pose at ({msg.pose.position.x:.2f}, {msg.pose.position.y:.2f})" + ) + self.publish_autonomy_joy() + + def way_point_callback(self, msg: PointStamped): + self.get_logger().info(f"Received way_point at ({msg.point.x:.2f}, {msg.point.y:.2f})") + self.publish_autonomy_joy() + + +def main(args=None): + rclpy.init(args=args) + node = GoalAutonomyRelay() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/docker/foxglove_utility/twist_relay.py b/docker/foxglove_utility/twist_relay.py new file mode 100644 index 00000000..9596e78f --- /dev/null +++ b/docker/foxglove_utility/twist_relay.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python3 +""" +Simple relay node that converts geometry_msgs/Twist to geometry_msgs/TwistStamped. +Used for Foxglove Teleop panel which only publishes Twist. +""" + +from geometry_msgs.msg import Twist, TwistStamped +import rclpy +from rclpy.node import Node +from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy + + +class TwistRelay(Node): + def __init__(self): + super().__init__("twist_relay") + + # Declare parameters + self.declare_parameter("input_topic", "/foxglove_teleop") + self.declare_parameter("output_topic", "/cmd_vel") + self.declare_parameter("frame_id", "vehicle") + + input_topic = self.get_parameter("input_topic").value + output_topic = self.get_parameter("output_topic").value + self.frame_id = self.get_parameter("frame_id").value + + # QoS for real-time control + qos = QoSProfile( + reliability=ReliabilityPolicy.BEST_EFFORT, history=HistoryPolicy.KEEP_LAST, depth=1 + ) + + # Subscribe to Twist (from Foxglove Teleop) + self.subscription = self.create_subscription(Twist, input_topic, self.twist_callback, qos) + + # Publish TwistStamped + self.publisher = self.create_publisher(TwistStamped, output_topic, qos) + + self.get_logger().info( + f"Twist relay: {input_topic} (Twist) -> {output_topic} (TwistStamped)" + ) + + def twist_callback(self, msg: Twist): + stamped = TwistStamped() + stamped.header.stamp = self.get_clock().now().to_msg() + stamped.header.frame_id = self.frame_id + stamped.twist = msg + self.publisher.publish(stamped) + + +def main(args=None): + rclpy.init(args=args) + node = TwistRelay() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/base_autonomy/vehicle_simulator/launch/system_bagfile.launch.py b/src/base_autonomy/vehicle_simulator/launch/system_bagfile.launch.py index 86515bc9..a4764933 100644 --- a/src/base_autonomy/vehicle_simulator/launch/system_bagfile.launch.py +++ b/src/base_autonomy/vehicle_simulator/launch/system_bagfile.launch.py @@ -18,6 +18,10 @@ def generate_launch_description(): vehicleX = LaunchConfiguration('vehicleX') vehicleY = LaunchConfiguration('vehicleY') checkTerrainConn = LaunchConfiguration('checkTerrainConn') + enable_bridge = LaunchConfiguration('enable_bridge') + bridge_host = LaunchConfiguration('bridge_host') + bridge_port = LaunchConfiguration('bridge_port') + bridge_autonomy_speed = LaunchConfiguration('bridge_autonomy_speed') declare_use_sim_time = DeclareLaunchArgument( 'use_sim_time', @@ -36,6 +40,10 @@ def generate_launch_description(): declare_vehicleX = DeclareLaunchArgument('vehicleX', default_value='0.0', description='') declare_vehicleY = DeclareLaunchArgument('vehicleY', default_value='0.0', description='') declare_checkTerrainConn = DeclareLaunchArgument('checkTerrainConn', default_value='true', description='') + declare_enable_bridge = DeclareLaunchArgument('enable_bridge', default_value='true', description='Enable ROS bridge server') + declare_bridge_host = DeclareLaunchArgument('bridge_host', default_value='0.0.0.0', description='ROS bridge bind address') + declare_bridge_port = DeclareLaunchArgument('bridge_port', default_value='9090', description='ROS bridge port') + declare_bridge_autonomy_speed = DeclareLaunchArgument('bridge_autonomy_speed', default_value='1.0', description='Default autonomy speed') start_local_planner = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join( @@ -107,6 +115,19 @@ def generate_launch_description(): }] ) + start_ros_bridge = Node( + package='ros_bridge_server', + executable='ros_bridge_server', + name='ros_bridge_server', + output='screen', + arguments=[ + '--host', bridge_host, + '--port', bridge_port, + '--autonomy-speed', bridge_autonomy_speed, + ], + condition=IfCondition(enable_bridge), + ) + ld = LaunchDescription() # Add the actions @@ -120,6 +141,10 @@ def generate_launch_description(): ld.add_action(declare_vehicleX) ld.add_action(declare_vehicleY) ld.add_action(declare_checkTerrainConn) + ld.add_action(declare_enable_bridge) + ld.add_action(declare_bridge_host) + ld.add_action(declare_bridge_port) + ld.add_action(declare_bridge_autonomy_speed) ld.add_action(start_local_planner) ld.add_action(start_terrain_analysis) @@ -129,5 +154,6 @@ def generate_launch_description(): ld.add_action(start_fastlio2) ld.add_action(start_visualization_tools) ld.add_action(start_joy) + ld.add_action(start_ros_bridge) return ld diff --git a/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_exploration_planner.launch.py b/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_exploration_planner.launch.py index 91c645d6..4bcf8557 100644 --- a/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_exploration_planner.launch.py +++ b/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_exploration_planner.launch.py @@ -19,6 +19,10 @@ def generate_launch_description(): vehicleX = LaunchConfiguration('vehicleX') vehicleY = LaunchConfiguration('vehicleY') checkTerrainConn = LaunchConfiguration('checkTerrainConn') + enable_bridge = LaunchConfiguration('enable_bridge') + bridge_host = LaunchConfiguration('bridge_host') + bridge_port = LaunchConfiguration('bridge_port') + bridge_autonomy_speed = LaunchConfiguration('bridge_autonomy_speed') declare_use_sim_time = DeclareLaunchArgument( 'use_sim_time', @@ -38,6 +42,10 @@ def generate_launch_description(): declare_vehicleX = DeclareLaunchArgument('vehicleX', default_value='0.0', description='') declare_vehicleY = DeclareLaunchArgument('vehicleY', default_value='0.0', description='') declare_checkTerrainConn = DeclareLaunchArgument('checkTerrainConn', default_value='true', description='') + declare_enable_bridge = DeclareLaunchArgument('enable_bridge', default_value='true', description='Enable ROS bridge server') + declare_bridge_host = DeclareLaunchArgument('bridge_host', default_value='0.0.0.0', description='ROS bridge bind address') + declare_bridge_port = DeclareLaunchArgument('bridge_port', default_value='9090', description='ROS bridge port') + declare_bridge_autonomy_speed = DeclareLaunchArgument('bridge_autonomy_speed', default_value='1.0', description='Default autonomy speed') start_local_planner = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join( @@ -117,6 +125,19 @@ def generate_launch_description(): }.items() ) + start_ros_bridge = Node( + package='ros_bridge_server', + executable='ros_bridge_server', + name='ros_bridge_server', + output='screen', + arguments=[ + '--host', bridge_host, + '--port', bridge_port, + '--autonomy-speed', bridge_autonomy_speed, + ], + condition=IfCondition(enable_bridge), + ) + ld = LaunchDescription() # Add the actions @@ -131,6 +152,10 @@ def generate_launch_description(): ld.add_action(declare_vehicleX) ld.add_action(declare_vehicleY) ld.add_action(declare_checkTerrainConn) + ld.add_action(declare_enable_bridge) + ld.add_action(declare_bridge_host) + ld.add_action(declare_bridge_port) + ld.add_action(declare_bridge_autonomy_speed) ld.add_action(start_local_planner) ld.add_action(start_terrain_analysis) @@ -141,5 +166,6 @@ def generate_launch_description(): ld.add_action(start_visualization_tools) ld.add_action(start_joy) ld.add_action(start_tare_planner) + ld.add_action(start_ros_bridge) return ld diff --git a/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_route_planner.launch.py b/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_route_planner.launch.py index dca9a4fe..f5af79e4 100644 --- a/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_route_planner.launch.py +++ b/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_route_planner.launch.py @@ -21,6 +21,10 @@ def generate_launch_description(): vehicleX = LaunchConfiguration('vehicleX') vehicleY = LaunchConfiguration('vehicleY') checkTerrainConn = LaunchConfiguration('checkTerrainConn') + enable_bridge = LaunchConfiguration('enable_bridge') + bridge_host = LaunchConfiguration('bridge_host') + bridge_port = LaunchConfiguration('bridge_port') + bridge_autonomy_speed = LaunchConfiguration('bridge_autonomy_speed') declare_use_sim_time = DeclareLaunchArgument( 'use_sim_time', @@ -45,6 +49,10 @@ def generate_launch_description(): declare_vehicleX = DeclareLaunchArgument('vehicleX', default_value='0.0', description='') declare_vehicleY = DeclareLaunchArgument('vehicleY', default_value='0.0', description='') declare_checkTerrainConn = DeclareLaunchArgument('checkTerrainConn', default_value='true', description='') + declare_enable_bridge = DeclareLaunchArgument('enable_bridge', default_value='true', description='Enable ROS bridge server') + declare_bridge_host = DeclareLaunchArgument('bridge_host', default_value='0.0.0.0', description='ROS bridge bind address') + declare_bridge_port = DeclareLaunchArgument('bridge_port', default_value='9090', description='ROS bridge port') + declare_bridge_autonomy_speed = DeclareLaunchArgument('bridge_autonomy_speed', default_value='1.0', description='Default autonomy speed') start_local_planner = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join( @@ -134,6 +142,19 @@ def generate_launch_description(): condition=IfCondition(use_pct_planner) ) + start_ros_bridge = Node( + package='ros_bridge_server', + executable='ros_bridge_server', + name='ros_bridge_server', + output='screen', + arguments=[ + '--host', bridge_host, + '--port', bridge_port, + '--autonomy-speed', bridge_autonomy_speed, + ], + condition=IfCondition(enable_bridge), + ) + ld = LaunchDescription() # Add the actions @@ -149,6 +170,10 @@ def generate_launch_description(): ld.add_action(declare_vehicleX) ld.add_action(declare_vehicleY) ld.add_action(declare_checkTerrainConn) + ld.add_action(declare_enable_bridge) + ld.add_action(declare_bridge_host) + ld.add_action(declare_bridge_port) + ld.add_action(declare_bridge_autonomy_speed) ld.add_action(start_local_planner) ld.add_action(start_terrain_analysis) @@ -160,5 +185,6 @@ def generate_launch_description(): ld.add_action(start_joy) ld.add_action(start_far_planner) ld.add_action(start_pct_planner) + ld.add_action(start_ros_bridge) return ld diff --git a/src/base_autonomy/vehicle_simulator/launch/system_real_robot.launch.py b/src/base_autonomy/vehicle_simulator/launch/system_real_robot.launch.py index 259dff5e..d1aa0bf5 100644 --- a/src/base_autonomy/vehicle_simulator/launch/system_real_robot.launch.py +++ b/src/base_autonomy/vehicle_simulator/launch/system_real_robot.launch.py @@ -17,6 +17,10 @@ def generate_launch_description(): vehicleX = LaunchConfiguration('vehicleX') vehicleY = LaunchConfiguration('vehicleY') checkTerrainConn = LaunchConfiguration('checkTerrainConn') + enable_bridge = LaunchConfiguration('enable_bridge') + bridge_host = LaunchConfiguration('bridge_host') + bridge_port = LaunchConfiguration('bridge_port') + bridge_autonomy_speed = LaunchConfiguration('bridge_autonomy_speed') declare_use_fastlio2 = DeclareLaunchArgument( 'use_fastlio2', @@ -30,6 +34,10 @@ def generate_launch_description(): declare_vehicleX = DeclareLaunchArgument('vehicleX', default_value='0.0', description='') declare_vehicleY = DeclareLaunchArgument('vehicleY', default_value='0.0', description='') declare_checkTerrainConn = DeclareLaunchArgument('checkTerrainConn', default_value='true', description='') + declare_enable_bridge = DeclareLaunchArgument('enable_bridge', default_value='true', description='Enable ROS bridge server') + declare_bridge_host = DeclareLaunchArgument('bridge_host', default_value='0.0.0.0', description='ROS bridge bind address') + declare_bridge_port = DeclareLaunchArgument('bridge_port', default_value='9090', description='ROS bridge port') + declare_bridge_autonomy_speed = DeclareLaunchArgument('bridge_autonomy_speed', default_value='1.0', description='Default autonomy speed') start_local_planner = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join( @@ -106,6 +114,19 @@ def generate_launch_description(): [get_package_share_directory('livox_ros_driver2'), '/launch_ROS2/msg_MID360_launch.py']), ) + start_ros_bridge = Node( + package='ros_bridge_server', + executable='ros_bridge_server', + name='ros_bridge_server', + output='screen', + arguments=[ + '--host', bridge_host, + '--port', bridge_port, + '--autonomy-speed', bridge_autonomy_speed, + ], + condition=IfCondition(enable_bridge), + ) + ld = LaunchDescription() # Add the actions @@ -117,6 +138,10 @@ def generate_launch_description(): ld.add_action(declare_vehicleX) ld.add_action(declare_vehicleY) ld.add_action(declare_checkTerrainConn) + ld.add_action(declare_enable_bridge) + ld.add_action(declare_bridge_host) + ld.add_action(declare_bridge_port) + ld.add_action(declare_bridge_autonomy_speed) ld.add_action(start_local_planner) ld.add_action(start_terrain_analysis) @@ -127,5 +152,6 @@ def generate_launch_description(): ld.add_action(start_visualization_tools) ld.add_action(start_joy) ld.add_action(start_mid360) + ld.add_action(start_ros_bridge) return ld diff --git a/src/base_autonomy/vehicle_simulator/launch/system_real_robot_with_exploration_planner.launch.py b/src/base_autonomy/vehicle_simulator/launch/system_real_robot_with_exploration_planner.launch.py index 2418dffa..71214e50 100644 --- a/src/base_autonomy/vehicle_simulator/launch/system_real_robot_with_exploration_planner.launch.py +++ b/src/base_autonomy/vehicle_simulator/launch/system_real_robot_with_exploration_planner.launch.py @@ -18,6 +18,10 @@ def generate_launch_description(): vehicleX = LaunchConfiguration('vehicleX') vehicleY = LaunchConfiguration('vehicleY') checkTerrainConn = LaunchConfiguration('checkTerrainConn') + enable_bridge = LaunchConfiguration('enable_bridge') + bridge_host = LaunchConfiguration('bridge_host') + bridge_port = LaunchConfiguration('bridge_port') + bridge_autonomy_speed = LaunchConfiguration('bridge_autonomy_speed') declare_use_fastlio2 = DeclareLaunchArgument( 'use_fastlio2', @@ -32,6 +36,10 @@ def generate_launch_description(): declare_vehicleX = DeclareLaunchArgument('vehicleX', default_value='0.0', description='') declare_vehicleY = DeclareLaunchArgument('vehicleY', default_value='0.0', description='') declare_checkTerrainConn = DeclareLaunchArgument('checkTerrainConn', default_value='true', description='') + declare_enable_bridge = DeclareLaunchArgument('enable_bridge', default_value='true', description='Enable ROS bridge server') + declare_bridge_host = DeclareLaunchArgument('bridge_host', default_value='0.0.0.0', description='ROS bridge bind address') + declare_bridge_port = DeclareLaunchArgument('bridge_port', default_value='9090', description='ROS bridge port') + declare_bridge_autonomy_speed = DeclareLaunchArgument('bridge_autonomy_speed', default_value='1.0', description='Default autonomy speed') start_local_planner = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join( @@ -116,6 +124,19 @@ def generate_launch_description(): [get_package_share_directory('livox_ros_driver2'), '/launch_ROS2/msg_MID360_launch.py']), ) + start_ros_bridge = Node( + package='ros_bridge_server', + executable='ros_bridge_server', + name='ros_bridge_server', + output='screen', + arguments=[ + '--host', bridge_host, + '--port', bridge_port, + '--autonomy-speed', bridge_autonomy_speed, + ], + condition=IfCondition(enable_bridge), + ) + ld = LaunchDescription() # Add the actions @@ -128,6 +149,10 @@ def generate_launch_description(): ld.add_action(declare_vehicleX) ld.add_action(declare_vehicleY) ld.add_action(declare_checkTerrainConn) + ld.add_action(declare_enable_bridge) + ld.add_action(declare_bridge_host) + ld.add_action(declare_bridge_port) + ld.add_action(declare_bridge_autonomy_speed) ld.add_action(start_local_planner) ld.add_action(start_terrain_analysis) @@ -139,5 +164,6 @@ def generate_launch_description(): ld.add_action(start_joy) ld.add_action(start_tare_planner) ld.add_action(start_mid360) + ld.add_action(start_ros_bridge) return ld diff --git a/src/base_autonomy/vehicle_simulator/launch/system_real_robot_with_route_planner.launch.py b/src/base_autonomy/vehicle_simulator/launch/system_real_robot_with_route_planner.launch.py index b19466d9..a02952f0 100644 --- a/src/base_autonomy/vehicle_simulator/launch/system_real_robot_with_route_planner.launch.py +++ b/src/base_autonomy/vehicle_simulator/launch/system_real_robot_with_route_planner.launch.py @@ -20,6 +20,10 @@ def generate_launch_description(): vehicleX = LaunchConfiguration('vehicleX') vehicleY = LaunchConfiguration('vehicleY') checkTerrainConn = LaunchConfiguration('checkTerrainConn') + enable_bridge = LaunchConfiguration('enable_bridge') + bridge_host = LaunchConfiguration('bridge_host') + bridge_port = LaunchConfiguration('bridge_port') + bridge_autonomy_speed = LaunchConfiguration('bridge_autonomy_speed') declare_use_fastlio2 = DeclareLaunchArgument( 'use_fastlio2', @@ -39,6 +43,10 @@ def generate_launch_description(): declare_vehicleX = DeclareLaunchArgument('vehicleX', default_value='0.0', description='') declare_vehicleY = DeclareLaunchArgument('vehicleY', default_value='0.0', description='') declare_checkTerrainConn = DeclareLaunchArgument('checkTerrainConn', default_value='true', description='') + declare_enable_bridge = DeclareLaunchArgument('enable_bridge', default_value='true', description='Enable ROS bridge server') + declare_bridge_host = DeclareLaunchArgument('bridge_host', default_value='0.0.0.0', description='ROS bridge bind address') + declare_bridge_port = DeclareLaunchArgument('bridge_port', default_value='9090', description='ROS bridge port') + declare_bridge_autonomy_speed = DeclareLaunchArgument('bridge_autonomy_speed', default_value='1.0', description='Default autonomy speed') start_local_planner = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join( @@ -133,6 +141,19 @@ def generate_launch_description(): [get_package_share_directory('livox_ros_driver2'), '/launch_ROS2/msg_MID360_launch.py']), ) + start_ros_bridge = Node( + package='ros_bridge_server', + executable='ros_bridge_server', + name='ros_bridge_server', + output='screen', + arguments=[ + '--host', bridge_host, + '--port', bridge_port, + '--autonomy-speed', bridge_autonomy_speed, + ], + condition=IfCondition(enable_bridge), + ) + ld = LaunchDescription() # Add the actions @@ -146,6 +167,10 @@ def generate_launch_description(): ld.add_action(declare_vehicleX) ld.add_action(declare_vehicleY) ld.add_action(declare_checkTerrainConn) + ld.add_action(declare_enable_bridge) + ld.add_action(declare_bridge_host) + ld.add_action(declare_bridge_port) + ld.add_action(declare_bridge_autonomy_speed) ld.add_action(start_local_planner) ld.add_action(start_terrain_analysis) @@ -158,5 +183,6 @@ def generate_launch_description(): ld.add_action(start_far_planner) ld.add_action(start_pct_planner) ld.add_action(start_mid360) + ld.add_action(start_ros_bridge) return ld diff --git a/src/base_autonomy/vehicle_simulator/launch/system_simulation.launch.py b/src/base_autonomy/vehicle_simulator/launch/system_simulation.launch.py index 7da08415..3aaa6560 100644 --- a/src/base_autonomy/vehicle_simulator/launch/system_simulation.launch.py +++ b/src/base_autonomy/vehicle_simulator/launch/system_simulation.launch.py @@ -3,6 +3,7 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, TimerAction +from launch.conditions import IfCondition from launch.launch_description_sources import FrontendLaunchDescriptionSource, PythonLaunchDescriptionSource from launch_ros.actions import Node from launch.substitutions import LaunchConfiguration @@ -16,6 +17,10 @@ def generate_launch_description(): terrainZ = LaunchConfiguration('terrainZ') vehicleYaw = LaunchConfiguration('vehicleYaw') checkTerrainConn = LaunchConfiguration('checkTerrainConn') + enable_bridge = LaunchConfiguration('enable_bridge') + bridge_host = LaunchConfiguration('bridge_host') + bridge_port = LaunchConfiguration('bridge_port') + bridge_autonomy_speed = LaunchConfiguration('bridge_autonomy_speed') declare_world_name = DeclareLaunchArgument('world_name', default_value='unity', description='') declare_vehicleHeight = DeclareLaunchArgument('vehicleHeight', default_value='0.75', description='') @@ -25,6 +30,10 @@ def generate_launch_description(): declare_terrainZ = DeclareLaunchArgument('terrainZ', default_value='0.0', description='') declare_vehicleYaw = DeclareLaunchArgument('vehicleYaw', default_value='0.0', description='') declare_checkTerrainConn = DeclareLaunchArgument('checkTerrainConn', default_value='true', description='') + declare_enable_bridge = DeclareLaunchArgument('enable_bridge', default_value='true', description='Enable ROS bridge server') + declare_bridge_host = DeclareLaunchArgument('bridge_host', default_value='0.0.0.0', description='ROS bridge bind address') + declare_bridge_port = DeclareLaunchArgument('bridge_port', default_value='9090', description='ROS bridge port') + declare_bridge_autonomy_speed = DeclareLaunchArgument('bridge_autonomy_speed', default_value='1.0', description='Default autonomy speed') start_local_planner = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join( @@ -119,6 +128,19 @@ def generate_launch_description(): }] ) + start_ros_bridge = Node( + package='ros_bridge_server', + executable='ros_bridge_server', + name='ros_bridge_server', + output='screen', + arguments=[ + '--host', bridge_host, + '--port', bridge_port, + '--autonomy-speed', bridge_autonomy_speed, + ], + condition=IfCondition(enable_bridge), + ) + ld = LaunchDescription() # Add the actions @@ -130,6 +152,10 @@ def generate_launch_description(): ld.add_action(declare_terrainZ) ld.add_action(declare_vehicleYaw) ld.add_action(declare_checkTerrainConn) + ld.add_action(declare_enable_bridge) + ld.add_action(declare_bridge_host) + ld.add_action(declare_bridge_port) + ld.add_action(declare_bridge_autonomy_speed) ld.add_action(start_local_planner) ld.add_action(start_terrain_analysis) @@ -140,5 +166,6 @@ def generate_launch_description(): ld.add_action(start_sensor_scan_generation) ld.add_action(start_visualization_tools) ld.add_action(start_joy) + ld.add_action(start_ros_bridge) return ld diff --git a/src/base_autonomy/vehicle_simulator/launch/system_simulation_with_exploration_planner.launch.py b/src/base_autonomy/vehicle_simulator/launch/system_simulation_with_exploration_planner.launch.py index 6725f501..c1432a17 100644 --- a/src/base_autonomy/vehicle_simulator/launch/system_simulation_with_exploration_planner.launch.py +++ b/src/base_autonomy/vehicle_simulator/launch/system_simulation_with_exploration_planner.launch.py @@ -3,6 +3,7 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, TimerAction +from launch.conditions import IfCondition from launch.launch_description_sources import FrontendLaunchDescriptionSource, PythonLaunchDescriptionSource from launch_ros.actions import Node from launch.substitutions import LaunchConfiguration @@ -17,6 +18,10 @@ def generate_launch_description(): terrainZ = LaunchConfiguration('terrainZ') vehicleYaw = LaunchConfiguration('vehicleYaw') checkTerrainConn = LaunchConfiguration('checkTerrainConn') + enable_bridge = LaunchConfiguration('enable_bridge') + bridge_host = LaunchConfiguration('bridge_host') + bridge_port = LaunchConfiguration('bridge_port') + bridge_autonomy_speed = LaunchConfiguration('bridge_autonomy_speed') declare_exploration_planner_config = DeclareLaunchArgument('exploration_planner_config', default_value='indoor_small', description='') declare_world_name = DeclareLaunchArgument('world_name', default_value='unity', description='') @@ -27,6 +32,10 @@ def generate_launch_description(): declare_terrainZ = DeclareLaunchArgument('terrainZ', default_value='0.0', description='') declare_vehicleYaw = DeclareLaunchArgument('vehicleYaw', default_value='0.0', description='') declare_checkTerrainConn = DeclareLaunchArgument('checkTerrainConn', default_value='true', description='') + declare_enable_bridge = DeclareLaunchArgument('enable_bridge', default_value='true', description='Enable ROS bridge server') + declare_bridge_host = DeclareLaunchArgument('bridge_host', default_value='0.0.0.0', description='ROS bridge bind address') + declare_bridge_port = DeclareLaunchArgument('bridge_port', default_value='9090', description='ROS bridge port') + declare_bridge_autonomy_speed = DeclareLaunchArgument('bridge_autonomy_speed', default_value='1.0', description='Default autonomy speed') start_local_planner = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join( @@ -129,6 +138,19 @@ def generate_launch_description(): }.items() ) + start_ros_bridge = Node( + package='ros_bridge_server', + executable='ros_bridge_server', + name='ros_bridge_server', + output='screen', + arguments=[ + '--host', bridge_host, + '--port', bridge_port, + '--autonomy-speed', bridge_autonomy_speed, + ], + condition=IfCondition(enable_bridge), + ) + ld = LaunchDescription() # Add the actions @@ -141,6 +163,10 @@ def generate_launch_description(): ld.add_action(declare_terrainZ) ld.add_action(declare_vehicleYaw) ld.add_action(declare_checkTerrainConn) + ld.add_action(declare_enable_bridge) + ld.add_action(declare_bridge_host) + ld.add_action(declare_bridge_port) + ld.add_action(declare_bridge_autonomy_speed) ld.add_action(start_local_planner) ld.add_action(start_terrain_analysis) @@ -152,5 +178,6 @@ def generate_launch_description(): ld.add_action(start_visualization_tools) ld.add_action(start_joy) ld.add_action(start_tare_planner) + ld.add_action(start_ros_bridge) return ld diff --git a/src/base_autonomy/vehicle_simulator/launch/system_simulation_with_route_planner.launch.py b/src/base_autonomy/vehicle_simulator/launch/system_simulation_with_route_planner.launch.py index 33a10792..2701bc41 100644 --- a/src/base_autonomy/vehicle_simulator/launch/system_simulation_with_route_planner.launch.py +++ b/src/base_autonomy/vehicle_simulator/launch/system_simulation_with_route_planner.launch.py @@ -20,6 +20,10 @@ def generate_launch_description(): terrainZ = LaunchConfiguration('terrainZ') vehicleYaw = LaunchConfiguration('vehicleYaw') checkTerrainConn = LaunchConfiguration('checkTerrainConn') + enable_bridge = LaunchConfiguration('enable_bridge') + bridge_host = LaunchConfiguration('bridge_host') + bridge_port = LaunchConfiguration('bridge_port') + bridge_autonomy_speed = LaunchConfiguration('bridge_autonomy_speed') declare_use_pct_planner = DeclareLaunchArgument( 'use_pct_planner', @@ -35,6 +39,10 @@ def generate_launch_description(): declare_terrainZ = DeclareLaunchArgument('terrainZ', default_value='0.0', description='') declare_vehicleYaw = DeclareLaunchArgument('vehicleYaw', default_value='0.0', description='') declare_checkTerrainConn = DeclareLaunchArgument('checkTerrainConn', default_value='true', description='') + declare_enable_bridge = DeclareLaunchArgument('enable_bridge', default_value='true', description='Enable ROS bridge server') + declare_bridge_host = DeclareLaunchArgument('bridge_host', default_value='0.0.0.0', description='ROS bridge bind address') + declare_bridge_port = DeclareLaunchArgument('bridge_port', default_value='9090', description='ROS bridge port') + declare_bridge_autonomy_speed = DeclareLaunchArgument('bridge_autonomy_speed', default_value='1.0', description='Default autonomy speed') start_local_planner = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join( @@ -147,6 +155,19 @@ def generate_launch_description(): condition=IfCondition(use_pct_planner) ) + start_ros_bridge = Node( + package='ros_bridge_server', + executable='ros_bridge_server', + name='ros_bridge_server', + output='screen', + arguments=[ + '--host', bridge_host, + '--port', bridge_port, + '--autonomy-speed', bridge_autonomy_speed, + ], + condition=IfCondition(enable_bridge), + ) + ld = LaunchDescription() # Add the actions @@ -160,6 +181,10 @@ def generate_launch_description(): ld.add_action(declare_terrainZ) ld.add_action(declare_vehicleYaw) ld.add_action(declare_checkTerrainConn) + ld.add_action(declare_enable_bridge) + ld.add_action(declare_bridge_host) + ld.add_action(declare_bridge_port) + ld.add_action(declare_bridge_autonomy_speed) ld.add_action(start_local_planner) ld.add_action(start_terrain_analysis) @@ -172,5 +197,6 @@ def generate_launch_description(): ld.add_action(start_joy) ld.add_action(start_far_planner) ld.add_action(start_pct_planner) + ld.add_action(start_ros_bridge) return ld diff --git a/src/slam/FASTLIO2_ROS2/fastlio2/CMakeLists.txt b/src/slam/FASTLIO2_ROS2/fastlio2/CMakeLists.txt index 7fd37a73..93e686d2 100644 --- a/src/slam/FASTLIO2_ROS2/fastlio2/CMakeLists.txt +++ b/src/slam/FASTLIO2_ROS2/fastlio2/CMakeLists.txt @@ -22,6 +22,7 @@ add_definitions(-DMP_PROC_NUM=2) # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) find_package(std_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) @@ -50,7 +51,7 @@ set(SRC_LIST src/map_builder/commons.cpp src/utils.cpp) add_executable(lio_node src/lio_node.cpp ${SRC_LIST}) -ament_target_dependencies(lio_node rclcpp std_msgs tf2 tf2_ros nav_msgs sensor_msgs livox_ros_driver2 pcl_conversions geometry_msgs) +ament_target_dependencies(lio_node rclcpp rclcpp_lifecycle std_msgs tf2 tf2_ros nav_msgs sensor_msgs livox_ros_driver2 pcl_conversions geometry_msgs) target_link_libraries(lio_node yaml-cpp ${PCL_LIBRARIES} diff --git a/src/slam/FASTLIO2_ROS2/fastlio2/package.xml b/src/slam/FASTLIO2_ROS2/fastlio2/package.xml index 4bd70f57..e8407bd4 100644 --- a/src/slam/FASTLIO2_ROS2/fastlio2/package.xml +++ b/src/slam/FASTLIO2_ROS2/fastlio2/package.xml @@ -10,6 +10,7 @@ ament_cmake rclcpp + rclcpp_lifecycle std_msgs tf2 tf2_ros diff --git a/src/slam/FASTLIO2_ROS2/fastlio2/src/lio_node.cpp b/src/slam/FASTLIO2_ROS2/fastlio2/src/lio_node.cpp index 7fc747de..1bb903cf 100644 --- a/src/slam/FASTLIO2_ROS2/fastlio2/src/lio_node.cpp +++ b/src/slam/FASTLIO2_ROS2/fastlio2/src/lio_node.cpp @@ -6,6 +6,8 @@ #include // #include #include +#include +#include #include #include @@ -24,6 +26,8 @@ #include using namespace std::chrono_literals; +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + struct NodeConfig { std::string imu_topic = "/livox/imu"; @@ -60,13 +64,18 @@ struct StateData nav_msgs::msg::Path path; }; -class LIONode : public rclcpp::Node +class LIONode : public rclcpp_lifecycle::LifecycleNode { public: - LIONode() : Node("lio_node") + LIONode() : LifecycleNode("lio_node") { - RCLCPP_INFO(this->get_logger(), "LIO Node Started"); + RCLCPP_INFO(this->get_logger(), "LIO Node created (lifecycle)"); loadParameters(); + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State &) + { + RCLCPP_INFO(this->get_logger(), "Configuring LIO Node..."); m_imu_sub = this->create_subscription(m_node_config.imu_topic, 10, std::bind(&LIONode::imuCB, this, std::placeholders::_1)); m_lidar_sub = this->create_subscription(m_node_config.lidar_topic, 10, std::bind(&LIONode::lidarCB, this, std::placeholders::_1)); @@ -82,7 +91,64 @@ class LIONode : public rclcpp::Node m_kf = std::make_shared(); m_builder = std::make_shared(m_builder_config, m_kf); + + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_activate(const rclcpp_lifecycle::State &) + { + RCLCPP_INFO(this->get_logger(), "Activating LIO Node..."); + m_body_cloud_pub->on_activate(); + m_world_cloud_pub->on_activate(); + m_path_pub->on_activate(); + m_odom_pub->on_activate(); m_timer = this->create_wall_timer(20ms, std::bind(&LIONode::timerCB, this)); + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) + { + RCLCPP_INFO(this->get_logger(), "Deactivating LIO Node..."); + m_timer.reset(); + m_body_cloud_pub->on_deactivate(); + m_world_cloud_pub->on_deactivate(); + m_path_pub->on_deactivate(); + m_odom_pub->on_deactivate(); + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) + { + RCLCPP_INFO(this->get_logger(), "Cleaning up LIO Node..."); + m_timer.reset(); + m_imu_sub.reset(); + m_lidar_sub.reset(); + m_body_cloud_pub.reset(); + m_world_cloud_pub.reset(); + m_path_pub.reset(); + m_odom_pub.reset(); + m_tf_broadcaster.reset(); + m_kf.reset(); + m_builder.reset(); + m_state_data.lidar_pushed = false; + m_state_data.last_lidar_time = -1.0; + m_state_data.last_imu_time = -1.0; + m_state_data.last_odom_log_time = -1.0; + m_state_data.imu_buffer.clear(); + m_state_data.lidar_buffer.clear(); + m_state_data.path.poses.clear(); + m_package = SyncPackage{}; + pitch_align_done_ = false; + pitch_samples_ = 0; + pitch_sum_ = 0.0; + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_shutdown(const rclcpp_lifecycle::State &) + { + RCLCPP_INFO(this->get_logger(), "Shutting down LIO Node..."); + on_cleanup(rclcpp_lifecycle::State()); + return CallbackReturn::SUCCESS; } void loadParameters() @@ -218,7 +284,7 @@ class LIONode : public rclcpp::Node return true; } - void publishCloud(rclcpp::Publisher::SharedPtr pub, CloudType::Ptr cloud, std::string frame_id, const double &time) + void publishCloud(rclcpp_lifecycle::LifecyclePublisher::SharedPtr pub, CloudType::Ptr cloud, std::string frame_id, const double &time) { if (pub->get_subscription_count() <= 0) return; @@ -229,7 +295,7 @@ class LIONode : public rclcpp::Node pub->publish(cloud_msg); } - void publishOdometry(rclcpp::Publisher::SharedPtr odom_pub, std::string frame_id, std::string child_frame, const double &time) + void publishOdometry(rclcpp_lifecycle::LifecyclePublisher::SharedPtr odom_pub, std::string frame_id, std::string child_frame, const double &time) { nav_msgs::msg::Odometry odom; odom.header.frame_id = frame_id; @@ -277,7 +343,7 @@ class LIONode : public rclcpp::Node odom_pub->publish(odom); } - void publishPath(rclcpp::Publisher::SharedPtr path_pub, std::string frame_id, const double &time) + void publishPath(rclcpp_lifecycle::LifecyclePublisher::SharedPtr path_pub, std::string frame_id, const double &time) { if (path_pub->get_subscription_count() <= 0) return; @@ -391,10 +457,10 @@ class LIONode : public rclcpp::Node rclcpp::Subscription::SharedPtr m_lidar_sub; rclcpp::Subscription::SharedPtr m_imu_sub; - rclcpp::Publisher::SharedPtr m_body_cloud_pub; - rclcpp::Publisher::SharedPtr m_world_cloud_pub; - rclcpp::Publisher::SharedPtr m_path_pub; - rclcpp::Publisher::SharedPtr m_odom_pub; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr m_body_cloud_pub; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr m_world_cloud_pub; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr m_path_pub; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr m_odom_pub; rclcpp::TimerBase::SharedPtr m_timer; StateData m_state_data; @@ -413,7 +479,10 @@ class LIONode : public rclcpp::Node int main(int argc, char **argv) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + auto node = std::make_shared(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + executor.spin(); rclcpp::shutdown(); return 0; -} \ No newline at end of file +} diff --git a/src/slam/FASTLIO2_ROS2/localizer/CMakeLists.txt b/src/slam/FASTLIO2_ROS2/localizer/CMakeLists.txt index 52baf9cc..73669ca3 100644 --- a/src/slam/FASTLIO2_ROS2/localizer/CMakeLists.txt +++ b/src/slam/FASTLIO2_ROS2/localizer/CMakeLists.txt @@ -15,6 +15,7 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) find_package(std_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(nav_msgs REQUIRED) @@ -40,7 +41,7 @@ set(SRC_LIST src/localizers/commons.cpp src/localizers/icp_localizer.cpp) add_executable(localizer_node src/localizer_node.cpp ${SRC_LIST}) -ament_target_dependencies(localizer_node rclcpp std_msgs sensor_msgs nav_msgs message_filters pcl_conversions tf2_ros geometry_msgs interface) +ament_target_dependencies(localizer_node rclcpp rclcpp_lifecycle std_msgs sensor_msgs nav_msgs message_filters pcl_conversions tf2_ros geometry_msgs interface) target_link_libraries(localizer_node ${PCL_LIBRARIES} yaml-cpp) diff --git a/src/slam/FASTLIO2_ROS2/localizer/package.xml b/src/slam/FASTLIO2_ROS2/localizer/package.xml index c7b30624..a3a0da53 100644 --- a/src/slam/FASTLIO2_ROS2/localizer/package.xml +++ b/src/slam/FASTLIO2_ROS2/localizer/package.xml @@ -9,6 +9,7 @@ ament_cmake + rclcpp_lifecycle std_msgs sensor_msgs nav_msgs diff --git a/src/slam/FASTLIO2_ROS2/localizer/src/localizer_node.cpp b/src/slam/FASTLIO2_ROS2/localizer/src/localizer_node.cpp index 4609e0ac..60f267d3 100644 --- a/src/slam/FASTLIO2_ROS2/localizer/src/localizer_node.cpp +++ b/src/slam/FASTLIO2_ROS2/localizer/src/localizer_node.cpp @@ -2,6 +2,8 @@ #include #include #include +#include +#include #include #include #include @@ -19,6 +21,7 @@ #include using namespace std::chrono_literals; +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; struct NodeConfig { @@ -59,13 +62,18 @@ struct NodeState M4F initial_guess = M4F::Identity(); }; -class LocalizerNode : public rclcpp::Node +class LocalizerNode : public rclcpp_lifecycle::LifecycleNode { public: - LocalizerNode() : Node("localizer_node") + LocalizerNode() : LifecycleNode("localizer_node") { RCLCPP_INFO(this->get_logger(), "Localizer Node Started"); loadParameters(); + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State &) + { + RCLCPP_INFO(this->get_logger(), "Configuring..."); rclcpp::QoS qos = rclcpp::QoS(10); m_cloud_sub.subscribe(this, m_config.cloud_topic, qos.get_rmw_qos_profile()); m_odom_sub.subscribe(this, m_config.odom_topic, qos.get_rmw_qos_profile()); @@ -83,7 +91,54 @@ class LocalizerNode : public rclcpp::Node m_map_cloud_pub = this->create_publisher("map_cloud", 10); + RCLCPP_INFO(this->get_logger(), "Configured successfully"); + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_activate(const rclcpp_lifecycle::State &) + { + RCLCPP_INFO(this->get_logger(), "Activating..."); + m_map_cloud_pub->on_activate(); m_timer = this->create_wall_timer(10ms, std::bind(&LocalizerNode::timerCB, this)); + RCLCPP_INFO(this->get_logger(), "Activated successfully"); + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) + { + RCLCPP_INFO(this->get_logger(), "Deactivating..."); + if (m_timer) { m_timer->cancel(); m_timer.reset(); } + m_map_cloud_pub->on_deactivate(); + RCLCPP_INFO(this->get_logger(), "Deactivated successfully"); + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) + { + RCLCPP_INFO(this->get_logger(), "Cleaning up..."); + m_timer.reset(); + m_sync.reset(); + m_tf_broadcaster.reset(); + m_localizer.reset(); + m_reloc_srv.reset(); + m_reloc_check_srv.reset(); + m_map_cloud_pub.reset(); + RCLCPP_INFO(this->get_logger(), "Cleaned up successfully"); + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_shutdown(const rclcpp_lifecycle::State &) + { + RCLCPP_INFO(this->get_logger(), "Shutting down..."); + m_timer.reset(); + m_sync.reset(); + m_tf_broadcaster.reset(); + m_localizer.reset(); + m_reloc_srv.reset(); + m_reloc_check_srv.reset(); + m_map_cloud_pub.reset(); + RCLCPP_INFO(this->get_logger(), "Shut down successfully"); + return CallbackReturn::SUCCESS; } void loadParameters() @@ -412,19 +467,22 @@ class LocalizerNode : public rclcpp::Node ICPConfig m_localizer_config; std::shared_ptr m_localizer; - message_filters::Subscriber m_cloud_sub; - message_filters::Subscriber m_odom_sub; + message_filters::Subscriber m_cloud_sub; + message_filters::Subscriber m_odom_sub; rclcpp::TimerBase::SharedPtr m_timer; std::shared_ptr>> m_sync; std::shared_ptr m_tf_broadcaster; rclcpp::Service::SharedPtr m_reloc_srv; rclcpp::Service::SharedPtr m_reloc_check_srv; - rclcpp::Publisher::SharedPtr m_map_cloud_pub; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr m_map_cloud_pub; }; int main(int argc, char **argv) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + auto node = std::make_shared(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + executor.spin(); rclcpp::shutdown(); return 0; } diff --git a/src/slam/FASTLIO2_ROS2/pgo/CMakeLists.txt b/src/slam/FASTLIO2_ROS2/pgo/CMakeLists.txt index d2e123c9..11969295 100644 --- a/src/slam/FASTLIO2_ROS2/pgo/CMakeLists.txt +++ b/src/slam/FASTLIO2_ROS2/pgo/CMakeLists.txt @@ -16,6 +16,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(std_msgs REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) find_package(nav_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(pcl_conversions REQUIRED) @@ -43,7 +44,7 @@ set(SRC_LIST src/pgos/commons.cpp src/pgos/simple_pgo.cpp) add_executable(pgo_node src/pgo_node.cpp ${SRC_LIST}) -ament_target_dependencies(pgo_node rclcpp std_msgs sensor_msgs nav_msgs message_filters pcl_conversions tf2_ros geometry_msgs visualization_msgs interface GTSAM) +ament_target_dependencies(pgo_node rclcpp rclcpp_lifecycle std_msgs sensor_msgs nav_msgs message_filters pcl_conversions tf2_ros geometry_msgs visualization_msgs interface GTSAM) target_link_libraries(pgo_node ${PCL_LIBRARIES} gtsam yaml-cpp) diff --git a/src/slam/FASTLIO2_ROS2/pgo/package.xml b/src/slam/FASTLIO2_ROS2/pgo/package.xml index e077f042..7217d2bf 100644 --- a/src/slam/FASTLIO2_ROS2/pgo/package.xml +++ b/src/slam/FASTLIO2_ROS2/pgo/package.xml @@ -11,6 +11,7 @@ std_msgs rclcpp + rclcpp_lifecycle nav_msgs tf2_ros pcl_conversions diff --git a/src/slam/FASTLIO2_ROS2/pgo/src/pgo_node.cpp b/src/slam/FASTLIO2_ROS2/pgo/src/pgo_node.cpp index 4f8a10f1..4bbe40e8 100644 --- a/src/slam/FASTLIO2_ROS2/pgo/src/pgo_node.cpp +++ b/src/slam/FASTLIO2_ROS2/pgo/src/pgo_node.cpp @@ -1,4 +1,6 @@ #include +#include +#include #include #include #include @@ -20,6 +22,7 @@ #include using namespace std::chrono_literals; +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; struct NodeConfig { @@ -36,13 +39,18 @@ struct NodeState double last_message_time; }; -class PGONode : public rclcpp::Node +class PGONode : public rclcpp_lifecycle::LifecycleNode { public: - PGONode() : Node("pgo_node") + PGONode() : LifecycleNode("pgo_node") { RCLCPP_INFO(this->get_logger(), "PGO node started"); loadParameters(); + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State &) + { + RCLCPP_INFO(this->get_logger(), "Configuring..."); m_pgo = std::make_shared(m_pgo_config); rclcpp::QoS qos = rclcpp::QoS(10); m_cloud_sub.subscribe(this, m_node_config.cloud_topic, qos.get_rmw_qos_profile()); @@ -52,8 +60,53 @@ class PGONode : public rclcpp::Node m_sync = std::make_shared>>(message_filters::sync_policies::ApproximateTime(10), m_cloud_sub, m_odom_sub); m_sync->setAgePenalty(0.1); m_sync->registerCallback(std::bind(&PGONode::syncCB, this, std::placeholders::_1, std::placeholders::_2)); - m_timer = this->create_wall_timer(50ms, std::bind(&PGONode::timerCB, this)); m_save_map_srv = this->create_service("/pgo/save_maps", std::bind(&PGONode::saveMapsCB, this, std::placeholders::_1, std::placeholders::_2)); + RCLCPP_INFO(this->get_logger(), "Configured successfully"); + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_activate(const rclcpp_lifecycle::State &) + { + RCLCPP_INFO(this->get_logger(), "Activating..."); + m_loop_marker_pub->on_activate(); + m_timer = this->create_wall_timer(50ms, std::bind(&PGONode::timerCB, this)); + RCLCPP_INFO(this->get_logger(), "Activated successfully"); + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) + { + RCLCPP_INFO(this->get_logger(), "Deactivating..."); + if (m_timer) { m_timer->cancel(); m_timer.reset(); } + m_loop_marker_pub->on_deactivate(); + RCLCPP_INFO(this->get_logger(), "Deactivated successfully"); + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) + { + RCLCPP_INFO(this->get_logger(), "Cleaning up..."); + m_timer.reset(); + m_sync.reset(); + m_tf_broadcaster.reset(); + m_pgo.reset(); + m_save_map_srv.reset(); + m_loop_marker_pub.reset(); + RCLCPP_INFO(this->get_logger(), "Cleaned up successfully"); + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_shutdown(const rclcpp_lifecycle::State &) + { + RCLCPP_INFO(this->get_logger(), "Shutting down..."); + m_timer.reset(); + m_sync.reset(); + m_tf_broadcaster.reset(); + m_pgo.reset(); + m_save_map_srv.reset(); + m_loop_marker_pub.reset(); + RCLCPP_INFO(this->get_logger(), "Shut down successfully"); + return CallbackReturn::SUCCESS; } void loadParameters() @@ -290,10 +343,10 @@ class PGONode : public rclcpp::Node NodeState m_state; std::shared_ptr m_pgo; rclcpp::TimerBase::SharedPtr m_timer; - rclcpp::Publisher::SharedPtr m_loop_marker_pub; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr m_loop_marker_pub; rclcpp::Service::SharedPtr m_save_map_srv; - message_filters::Subscriber m_cloud_sub; - message_filters::Subscriber m_odom_sub; + message_filters::Subscriber m_cloud_sub; + message_filters::Subscriber m_odom_sub; std::shared_ptr m_tf_broadcaster; std::shared_ptr>> m_sync; }; @@ -301,7 +354,10 @@ class PGONode : public rclcpp::Node int main(int argc, char **argv) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + auto node = std::make_shared(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + executor.spin(); rclcpp::shutdown(); return 0; } \ No newline at end of file diff --git a/src/slam/FASTLIO2_ROS2/pgo/src/pgo_node.cpp.bak b/src/slam/FASTLIO2_ROS2/pgo/src/pgo_node.cpp.bak new file mode 100644 index 00000000..4f8a10f1 --- /dev/null +++ b/src/slam/FASTLIO2_ROS2/pgo/src/pgo_node.cpp.bak @@ -0,0 +1,307 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "pgos/commons.h" +#include "pgos/simple_pgo.h" +#include "interface/srv/save_maps.hpp" +#include +#include +#include + +using namespace std::chrono_literals; + +struct NodeConfig +{ + std::string cloud_topic = "/lio/body_cloud"; + std::string odom_topic = "/lio/odom"; + std::string map_frame = "map"; + std::string local_frame = "lidar"; +}; + +struct NodeState +{ + std::mutex message_mutex; + std::queue cloud_buffer; + double last_message_time; +}; + +class PGONode : public rclcpp::Node +{ +public: + PGONode() : Node("pgo_node") + { + RCLCPP_INFO(this->get_logger(), "PGO node started"); + loadParameters(); + m_pgo = std::make_shared(m_pgo_config); + rclcpp::QoS qos = rclcpp::QoS(10); + m_cloud_sub.subscribe(this, m_node_config.cloud_topic, qos.get_rmw_qos_profile()); + m_odom_sub.subscribe(this, m_node_config.odom_topic, qos.get_rmw_qos_profile()); + m_loop_marker_pub = this->create_publisher("/pgo/loop_markers", 10000); + m_tf_broadcaster = std::make_shared(*this); + m_sync = std::make_shared>>(message_filters::sync_policies::ApproximateTime(10), m_cloud_sub, m_odom_sub); + m_sync->setAgePenalty(0.1); + m_sync->registerCallback(std::bind(&PGONode::syncCB, this, std::placeholders::_1, std::placeholders::_2)); + m_timer = this->create_wall_timer(50ms, std::bind(&PGONode::timerCB, this)); + m_save_map_srv = this->create_service("/pgo/save_maps", std::bind(&PGONode::saveMapsCB, this, std::placeholders::_1, std::placeholders::_2)); + } + + void loadParameters() + { + this->declare_parameter("config_path", ""); + std::string config_path; + this->get_parameter("config_path", config_path); + YAML::Node config = YAML::LoadFile(config_path); + if (!config) + { + RCLCPP_WARN(this->get_logger(), "FAIL TO LOAD YAML FILE!"); + return; + } + RCLCPP_INFO(this->get_logger(), "LOAD FROM YAML CONFIG PATH: %s", config_path.c_str()); + m_node_config.cloud_topic = config["cloud_topic"].as(); + m_node_config.odom_topic = config["odom_topic"].as(); + m_node_config.map_frame = config["map_frame"].as(); + m_node_config.local_frame = config["local_frame"].as(); + + m_pgo_config.key_pose_delta_deg = config["key_pose_delta_deg"].as(); + m_pgo_config.key_pose_delta_trans = config["key_pose_delta_trans"].as(); + m_pgo_config.loop_search_radius = config["loop_search_radius"].as(); + m_pgo_config.loop_time_tresh = config["loop_time_tresh"].as(); + m_pgo_config.loop_score_tresh = config["loop_score_tresh"].as(); + m_pgo_config.loop_submap_half_range = config["loop_submap_half_range"].as(); + m_pgo_config.submap_resolution = config["submap_resolution"].as(); + m_pgo_config.min_loop_detect_duration = config["min_loop_detect_duration"].as(); + } + void syncCB(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &cloud_msg, const nav_msgs::msg::Odometry::ConstSharedPtr &odom_msg) + { + + std::lock_guard(m_state.message_mutex); + CloudWithPose cp; + cp.pose.setTime(cloud_msg->header.stamp.sec, cloud_msg->header.stamp.nanosec); + if (cp.pose.second < m_state.last_message_time) + { + RCLCPP_WARN(this->get_logger(), "Received out of order message"); + return; + } + m_state.last_message_time = cp.pose.second; + + cp.pose.r = Eigen::Quaterniond(odom_msg->pose.pose.orientation.w, + odom_msg->pose.pose.orientation.x, + odom_msg->pose.pose.orientation.y, + odom_msg->pose.pose.orientation.z) + .toRotationMatrix(); + cp.pose.t = V3D(odom_msg->pose.pose.position.x, odom_msg->pose.pose.position.y, odom_msg->pose.pose.position.z); + cp.cloud = CloudType::Ptr(new CloudType); + pcl::fromROSMsg(*cloud_msg, *cp.cloud); + m_state.cloud_buffer.push(cp); + } + + void sendBroadCastTF(builtin_interfaces::msg::Time &time) + { + geometry_msgs::msg::TransformStamped transformStamped; + transformStamped.header.frame_id = m_node_config.map_frame; + transformStamped.child_frame_id = m_node_config.local_frame; + transformStamped.header.stamp = time; + Eigen::Quaterniond q(m_pgo->offsetR()); + V3D t = m_pgo->offsetT(); + transformStamped.transform.translation.x = t.x(); + transformStamped.transform.translation.y = t.y(); + transformStamped.transform.translation.z = t.z(); + transformStamped.transform.rotation.x = q.x(); + transformStamped.transform.rotation.y = q.y(); + transformStamped.transform.rotation.z = q.z(); + transformStamped.transform.rotation.w = q.w(); + m_tf_broadcaster->sendTransform(transformStamped); + } + + void publishLoopMarkers(builtin_interfaces::msg::Time &time) + { + if (m_loop_marker_pub->get_subscription_count() == 0) + return; + if (m_pgo->historyPairs().size() == 0) + return; + + visualization_msgs::msg::MarkerArray marker_array; + visualization_msgs::msg::Marker nodes_marker; + visualization_msgs::msg::Marker edges_marker; + nodes_marker.header.frame_id = m_node_config.map_frame; + nodes_marker.header.stamp = time; + nodes_marker.ns = "pgo_nodes"; + nodes_marker.id = 0; + nodes_marker.type = visualization_msgs::msg::Marker::SPHERE_LIST; + nodes_marker.action = visualization_msgs::msg::Marker::ADD; + nodes_marker.pose.orientation.w = 1.0; + nodes_marker.scale.x = 0.3; + nodes_marker.scale.y = 0.3; + nodes_marker.scale.z = 0.3; + nodes_marker.color.r = 1.0; + nodes_marker.color.g = 0.8; + nodes_marker.color.b = 0.0; + nodes_marker.color.a = 1.0; + + edges_marker.header.frame_id = m_node_config.map_frame; + edges_marker.header.stamp = time; + edges_marker.ns = "pgo_edges"; + edges_marker.id = 1; + edges_marker.type = visualization_msgs::msg::Marker::LINE_LIST; + edges_marker.action = visualization_msgs::msg::Marker::ADD; + edges_marker.pose.orientation.w = 1.0; + edges_marker.scale.x = 0.1; + edges_marker.color.r = 0.0; + edges_marker.color.g = 0.8; + edges_marker.color.b = 0.0; + edges_marker.color.a = 1.0; + + std::vector &poses = m_pgo->keyPoses(); + std::vector> &pairs = m_pgo->historyPairs(); + for (size_t i = 0; i < pairs.size(); i++) + { + size_t i1 = pairs[i].first; + size_t i2 = pairs[i].second; + geometry_msgs::msg::Point p1, p2; + p1.x = poses[i1].t_global.x(); + p1.y = poses[i1].t_global.y(); + p1.z = poses[i1].t_global.z(); + + p2.x = poses[i2].t_global.x(); + p2.y = poses[i2].t_global.y(); + p2.z = poses[i2].t_global.z(); + + nodes_marker.points.push_back(p1); + nodes_marker.points.push_back(p2); + edges_marker.points.push_back(p1); + edges_marker.points.push_back(p2); + } + + marker_array.markers.push_back(nodes_marker); + marker_array.markers.push_back(edges_marker); + m_loop_marker_pub->publish(marker_array); + } + + void timerCB() + { + if (m_state.cloud_buffer.size() == 0) + return; + CloudWithPose cp = m_state.cloud_buffer.front(); + // 清理队列 + { + std::lock_guard(m_state.message_mutex); + while (!m_state.cloud_buffer.empty()) + { + m_state.cloud_buffer.pop(); + } + } + builtin_interfaces::msg::Time cur_time; + cur_time.sec = cp.pose.sec; + cur_time.nanosec = cp.pose.nsec; + if (!m_pgo->addKeyPose(cp)) + { + + sendBroadCastTF(cur_time); + return; + } + + m_pgo->searchForLoopPairs(); + + m_pgo->smoothAndUpdate(); + + sendBroadCastTF(cur_time); + + publishLoopMarkers(cur_time); + } + + void saveMapsCB(const std::shared_ptr request, std::shared_ptr response) + { + if (!std::filesystem::exists(request->file_path)) + { + response->success = false; + response->message = request->file_path + " IS NOT EXISTS!"; + return; + } + + if (m_pgo->keyPoses().size() == 0) + { + response->success = false; + response->message = "NO POSES!"; + return; + } + + std::filesystem::path p_dir(request->file_path); + std::filesystem::path patches_dir = p_dir / "patches"; + std::filesystem::path poses_txt_path = p_dir / "poses.txt"; + std::filesystem::path map_path = p_dir / "map.pcd"; + + if (request->save_patches) + { + if (std::filesystem::exists(patches_dir)) + { + std::filesystem::remove_all(patches_dir); + } + + std::filesystem::create_directories(patches_dir); + + if (std::filesystem::exists(poses_txt_path)) + { + std::filesystem::remove(poses_txt_path); + } + RCLCPP_INFO(this->get_logger(), "Patches Path: %s", patches_dir.string().c_str()); + } + RCLCPP_INFO(this->get_logger(), "SAVE MAP TO %s", map_path.string().c_str()); + + std::ofstream txt_file(poses_txt_path); + + CloudType::Ptr ret(new CloudType); + for (size_t i = 0; i < m_pgo->keyPoses().size(); i++) + { + + CloudType::Ptr body_cloud = m_pgo->keyPoses()[i].body_cloud; + if (request->save_patches) + { + std::string patch_name = std::to_string(i) + ".pcd"; + std::filesystem::path patch_path = patches_dir / patch_name; + pcl::io::savePCDFileBinary(patch_path.string(), *body_cloud); + Eigen::Quaterniond q(m_pgo->keyPoses()[i].r_global); + V3D t = m_pgo->keyPoses()[i].t_global; + txt_file << patch_name << " " << t.x() << " " << t.y() << " " << t.z() << " " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << std::endl; + } + CloudType::Ptr world_cloud(new CloudType); + pcl::transformPointCloud(*body_cloud, *world_cloud, m_pgo->keyPoses()[i].t_global, Eigen::Quaterniond(m_pgo->keyPoses()[i].r_global)); + *ret += *world_cloud; + } + txt_file.close(); + pcl::io::savePCDFileBinary(map_path.string(), *ret); + response->success = true; + response->message = "SAVE SUCCESS!"; + } + +private: + NodeConfig m_node_config; + Config m_pgo_config; + NodeState m_state; + std::shared_ptr m_pgo; + rclcpp::TimerBase::SharedPtr m_timer; + rclcpp::Publisher::SharedPtr m_loop_marker_pub; + rclcpp::Service::SharedPtr m_save_map_srv; + message_filters::Subscriber m_cloud_sub; + message_filters::Subscriber m_odom_sub; + std::shared_ptr m_tf_broadcaster; + std::shared_ptr>> m_sync; +}; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/slam/arise_slam_mid360/CMakeLists.txt b/src/slam/arise_slam_mid360/CMakeLists.txt index d4a45d29..8c8c8da0 100755 --- a/src/slam/arise_slam_mid360/CMakeLists.txt +++ b/src/slam/arise_slam_mid360/CMakeLists.txt @@ -30,6 +30,7 @@ endif (POLICY CMP0048) find_package(ament_cmake REQUIRED) find_package(ament_cmake_auto REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) find_package(std_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) @@ -99,6 +100,7 @@ target_include_directories(AriseSlamLib PUBLIC ament_target_dependencies(AriseSlamLib rclcpp + rclcpp_lifecycle std_msgs geometry_msgs nav_msgs @@ -122,7 +124,7 @@ target_link_libraries(AriseSlamLib # IMU PreIntegration node add_executable(imu_preintegration_node src/imuPreintegration_node.cpp) -ament_target_dependencies(imu_preintegration_node rclcpp nav_msgs sensor_msgs tf2_ros) +ament_target_dependencies(imu_preintegration_node rclcpp rclcpp_lifecycle nav_msgs sensor_msgs tf2_ros) target_link_libraries(imu_preintegration_node gtsam AriseSlamLib @@ -132,7 +134,7 @@ target_link_libraries(imu_preintegration_node # Feature Extraction node add_executable(feature_extraction_node src/featureExtraction_node.cpp) -ament_target_dependencies(feature_extraction_node rclcpp nav_msgs sensor_msgs arise_slam_mid360_msgs livox_ros_driver2) +ament_target_dependencies(feature_extraction_node rclcpp rclcpp_lifecycle nav_msgs sensor_msgs arise_slam_mid360_msgs livox_ros_driver2) target_link_libraries(feature_extraction_node ${PCL_LIBRARIES} ${TBB_LIBRARIES} @@ -143,7 +145,7 @@ target_link_libraries(feature_extraction_node # LiDAR Mapping node add_executable(laser_mapping_node src/laserMapping_node.cpp) -ament_target_dependencies(laser_mapping_node rclcpp std_msgs nav_msgs sensor_msgs geometry_msgs tf2 tf2_ros) +ament_target_dependencies(laser_mapping_node rclcpp rclcpp_lifecycle std_msgs nav_msgs sensor_msgs geometry_msgs tf2 tf2_ros) target_link_libraries(laser_mapping_node ${PCL_LIBRARIES} ${CERES_LIBRARIES} diff --git a/src/slam/arise_slam_mid360/include/arise_slam_mid360/FeatureExtraction/featureExtraction.h b/src/slam/arise_slam_mid360/include/arise_slam_mid360/FeatureExtraction/featureExtraction.h index 4d64ddd6..9f18ca6d 100755 --- a/src/slam/arise_slam_mid360/include/arise_slam_mid360/FeatureExtraction/featureExtraction.h +++ b/src/slam/arise_slam_mid360/include/arise_slam_mid360/FeatureExtraction/featureExtraction.h @@ -23,6 +23,8 @@ #include #include "rclcpp/rclcpp.hpp" +#include +#include #include #include #include @@ -107,8 +109,15 @@ namespace arise_slam { typedef feature_extraction_config feature_extraction_config; - class featureExtraction : public rclcpp::Node { + class featureExtraction : public rclcpp_lifecycle::LifecycleNode { public: + using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + + CallbackReturn on_configure(const rclcpp_lifecycle::State &); + CallbackReturn on_activate(const rclcpp_lifecycle::State &); + CallbackReturn on_deactivate(const rclcpp_lifecycle::State &); + CallbackReturn on_cleanup(const rclcpp_lifecycle::State &); + CallbackReturn on_shutdown(const rclcpp_lifecycle::State &); /* TODO: return this as a parameter */ @@ -166,7 +175,7 @@ namespace arise_slam { void assignTimeforPointCloud(pcl::PointCloud::Ptr laserCloudIn_ptr_); template - sensor_msgs::msg::PointCloud2 publishCloud(rclcpp::Publisher::SharedPtr thisPub, typename pcl::PointCloud::Ptr thisCloud, rclcpp::Time thisStamp, std::string thisFrame); + sensor_msgs::msg::PointCloud2 publishCloud(rclcpp_lifecycle::LifecyclePublisher::SharedPtr thisPub, typename pcl::PointCloud::Ptr thisCloud, rclcpp::Time thisStamp, std::string thisFrame); void convert_velodyne_scan_order(pcl::PointCloud &laserCloudIn); @@ -217,14 +226,14 @@ namespace arise_slam { rclcpp::Subscription::SharedPtr subLivoxCloud; // Publishers - rclcpp::Publisher::SharedPtr pubLaserCloud; - rclcpp::Publisher::SharedPtr pubEdgePoints; - rclcpp::Publisher::SharedPtr pubPlannerPoints; - rclcpp::Publisher::SharedPtr pubBobPoints; - rclcpp::Publisher::SharedPtr pubLaserFeatureInfo; - rclcpp::Publisher::SharedPtr pubDepthUpPoints; - rclcpp::Publisher::SharedPtr pubDepthDownPoints; - std::vector::SharedPtr> pubEachScan; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr pubLaserCloud; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr pubEdgePoints; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr pubPlannerPoints; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr pubBobPoints; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr pubLaserFeatureInfo; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr pubDepthUpPoints; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr pubDepthDownPoints; + std::vector::SharedPtr> pubEachScan; rclcpp::CallbackGroup::SharedPtr cb_group_; diff --git a/src/slam/arise_slam_mid360/include/arise_slam_mid360/ImuPreintegration/imuPreintegration.h b/src/slam/arise_slam_mid360/include/arise_slam_mid360/ImuPreintegration/imuPreintegration.h index b2f5de3b..750f4369 100755 --- a/src/slam/arise_slam_mid360/include/arise_slam_mid360/ImuPreintegration/imuPreintegration.h +++ b/src/slam/arise_slam_mid360/include/arise_slam_mid360/ImuPreintegration/imuPreintegration.h @@ -6,6 +6,8 @@ #define IMUPREINTEGRATION_H #include "rclcpp/rclcpp.hpp" +#include +#include #include #include #include @@ -65,11 +67,18 @@ namespace arise_slam { double imu_acc_z_limit; }; - class imuPreintegration : public rclcpp::Node { + class imuPreintegration : public rclcpp_lifecycle::LifecycleNode { public: + using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; imuPreintegration(const rclcpp::NodeOptions & options); + CallbackReturn on_configure(const rclcpp_lifecycle::State &); + CallbackReturn on_activate(const rclcpp_lifecycle::State &); + CallbackReturn on_deactivate(const rclcpp_lifecycle::State &); + CallbackReturn on_cleanup(const rclcpp_lifecycle::State &); + CallbackReturn on_shutdown(const rclcpp_lifecycle::State &); + static constexpr double delta_t = 0; static constexpr double imu_laser_timedelay= 0.8; @@ -157,12 +166,12 @@ namespace arise_slam { rclcpp::Subscription::SharedPtr subVisualOdometry; // Publishers - rclcpp::Publisher::SharedPtr pubImuOdometry; - rclcpp::Publisher::SharedPtr pubImuOdometry2; - rclcpp::Publisher::SharedPtr pubHealthStatus; - rclcpp::Publisher::SharedPtr pubImuPath; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr pubImuOdometry; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr pubImuOdometry2; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr pubHealthStatus; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr pubImuPath; - rclcpp::Publisher::SharedPtr pubImuOdometrySmooth; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr pubImuOdometrySmooth; rclcpp::CallbackGroup::SharedPtr cb_group_; public: diff --git a/src/slam/arise_slam_mid360/include/arise_slam_mid360/LaserMapping/laserMapping.h b/src/slam/arise_slam_mid360/include/arise_slam_mid360/LaserMapping/laserMapping.h index f0040f55..2dab726f 100644 --- a/src/slam/arise_slam_mid360/include/arise_slam_mid360/LaserMapping/laserMapping.h +++ b/src/slam/arise_slam_mid360/include/arise_slam_mid360/LaserMapping/laserMapping.h @@ -27,6 +27,8 @@ #include #include #include "rclcpp/rclcpp.hpp" +#include +#include #include #include #include @@ -84,13 +86,21 @@ namespace arise_slam { }; - class laserMapping : public rclcpp::Node { + class laserMapping : public rclcpp_lifecycle::LifecycleNode { public: + using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + laserMapping(const rclcpp::NodeOptions & options); void initInterface(); + CallbackReturn on_configure(const rclcpp_lifecycle::State &); + CallbackReturn on_activate(const rclcpp_lifecycle::State &); + CallbackReturn on_deactivate(const rclcpp_lifecycle::State &); + CallbackReturn on_cleanup(const rclcpp_lifecycle::State &); + CallbackReturn on_shutdown(const rclcpp_lifecycle::State &); + void initializationParam(); @@ -217,25 +227,25 @@ namespace arise_slam { // rclcpp::Subscription<>::SharedPtr subTakeoffAlignment; // Publisher - rclcpp::Publisher::SharedPtr pubLaserCloudSurround; - rclcpp::Publisher::SharedPtr pubLaserCloudMap; - rclcpp::Publisher::SharedPtr pubLaserCloudPrior; - rclcpp::Publisher::SharedPtr pubLaserCloudFullRes; - rclcpp::Publisher::SharedPtr pubLaserCloudFullRes_rot; - rclcpp::Publisher::SharedPtr pubLaserCloudFullResOusterWithFeatures; - rclcpp::Publisher::SharedPtr pubLaserCloudFullResOuster; - rclcpp::Publisher::SharedPtr pubLaserCloudRawRes; - rclcpp::Publisher::SharedPtr pubOdomAftMapped; - rclcpp::Publisher::SharedPtr pubOdomAftMapped_rot; - rclcpp::Publisher::SharedPtr pubOdomAftMappedHighFrec; - rclcpp::Publisher::SharedPtr pubLaserAfterMappedPath; - rclcpp::Publisher::SharedPtr pubOptimizationStats; - rclcpp::Publisher::SharedPtr pubLaserOdometryIncremental; - rclcpp::Publisher::SharedPtr pubPreviousCloud; - rclcpp::Publisher::SharedPtr pubPreviousPose; - rclcpp::Publisher::SharedPtr pubprediction_source; - rclcpp::Publisher::SharedPtr pubVIOPrediction; - rclcpp::Publisher::SharedPtr pubLIOPrediction; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr pubLaserCloudSurround; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr pubLaserCloudMap; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr pubLaserCloudPrior; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr pubLaserCloudFullRes; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr pubLaserCloudFullRes_rot; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr pubLaserCloudFullResOusterWithFeatures; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr pubLaserCloudFullResOuster; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr pubLaserCloudRawRes; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr pubOdomAftMapped; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr pubOdomAftMapped_rot; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr pubOdomAftMappedHighFrec; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr pubLaserAfterMappedPath; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr pubOptimizationStats; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr pubLaserOdometryIncremental; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr pubPreviousCloud; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr pubPreviousPose; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr pubprediction_source; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr pubVIOPrediction; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr pubLIOPrediction; rclcpp::TimerBase::SharedPtr process_timer_; diff --git a/src/slam/arise_slam_mid360/include/arise_slam_mid360/LidarProcess/LidarSlam.h b/src/slam/arise_slam_mid360/include/arise_slam_mid360/LidarProcess/LidarSlam.h index ac48e02d..db5d2ded 100755 --- a/src/slam/arise_slam_mid360/include/arise_slam_mid360/LidarProcess/LidarSlam.h +++ b/src/slam/arise_slam_mid360/include/arise_slam_mid360/LidarProcess/LidarSlam.h @@ -11,6 +11,7 @@ #include #include +#include #include "arise_slam_mid360/sensor_data/pointcloud/LidarPoint.h" #include "arise_slam_mid360/LidarProcess/LocalMap.h" @@ -381,7 +382,7 @@ namespace arise_slam { double fisrt_time = 0; - rclcpp::Node::SharedPtr node_; + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; public: @@ -410,7 +411,7 @@ namespace arise_slam { void MannualYawCorrection(); - void initROSInterface(rclcpp::Node::SharedPtr); + void initROSInterface(rclcpp_lifecycle::LifecycleNode::SharedPtr); }; // class lidarslam diff --git a/src/slam/arise_slam_mid360/include/arise_slam_mid360/config/parameter.h b/src/slam/arise_slam_mid360/include/arise_slam_mid360/config/parameter.h index 6a43b53d..ad903682 100755 --- a/src/slam/arise_slam_mid360/include/arise_slam_mid360/config/parameter.h +++ b/src/slam/arise_slam_mid360/include/arise_slam_mid360/config/parameter.h @@ -6,6 +6,7 @@ #include #include #include "rclcpp/rclcpp.hpp" +#include #include "arise_slam_mid360/utils/Twist.h" #include @@ -135,8 +136,8 @@ extern float yaw_ratio; // extern bool start_from_previous_map; -bool readGlobalparam(rclcpp::Node::SharedPtr); -bool readCalibration(rclcpp::Node::SharedPtr); +bool readGlobalparam(rclcpp_lifecycle::LifecycleNode::SharedPtr); +bool readCalibration(rclcpp_lifecycle::LifecycleNode::SharedPtr); // the following are UBUNTU/LINUX ONLY terminal color codes. // #define RESET "\033[0m" diff --git a/src/slam/arise_slam_mid360/launch/arize_slam.launch.py b/src/slam/arise_slam_mid360/launch/arize_slam.launch.py index aab38b47..741380e8 100644 --- a/src/slam/arise_slam_mid360/launch/arize_slam.launch.py +++ b/src/slam/arise_slam_mid360/launch/arize_slam.launch.py @@ -2,11 +2,15 @@ from ament_index_python import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument +from launch.actions import DeclareLaunchArgument, EmitEvent, RegisterEventHandler, TimerAction from launch.substitutions import LaunchConfiguration, PythonExpression -from launch_ros.actions import Node +from launch_ros.actions import LifecycleNode from launch_ros.substitutions import FindPackageShare +from launch_ros.event_handlers import OnStateTransition +from launch_ros.events.lifecycle import ChangeState +import launch.events import launch_ros +import lifecycle_msgs.msg def get_share_file(package_name, file_name): return os.path.join(get_package_share_directory(package_name), file_name) @@ -80,9 +84,11 @@ def generate_launch_description(): init_pitch_arg = DeclareLaunchArgument("init_pitch", default_value="0.0") init_yaw_arg = DeclareLaunchArgument("init_yaw", default_value="0.0") - feature_extraction_node = Node( + feature_extraction_node = LifecycleNode( package="arise_slam_mid360", + namespace="", executable="feature_extraction_node", + name="feature_extraction_node", output={ "stdout": "screen", "stderr": "screen", @@ -97,9 +103,11 @@ def generate_launch_description(): }], ) - laser_mapping_node = Node( + laser_mapping_node = LifecycleNode( package="arise_slam_mid360", + namespace="", executable="laser_mapping_node", + name="laser_mapping_node", output={ "stdout": "screen", #"stderr": "screen", @@ -121,9 +129,11 @@ def generate_launch_description(): ] ) - imu_preintegration_node = Node( + imu_preintegration_node = LifecycleNode( package="arise_slam_mid360", + namespace="", executable="imu_preintegration_node", + name="imu_preintegration_node", output={ "stdout": "screen", "stderr": "screen", @@ -138,6 +148,57 @@ def generate_launch_description(): }], ) + # Auto-configure after small delay + configure_feature = TimerAction(period=0.5, actions=[ + EmitEvent(event=ChangeState( + lifecycle_node_matcher=launch.events.matches_action(feature_extraction_node), + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE, + )) + ]) + + activate_feature = RegisterEventHandler(OnStateTransition( + target_lifecycle_node=feature_extraction_node, + goal_state='inactive', + entities=[EmitEvent(event=ChangeState( + lifecycle_node_matcher=launch.events.matches_action(feature_extraction_node), + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE, + ))], + handle_once=True, + )) + + configure_mapping = TimerAction(period=1.5, actions=[ + EmitEvent(event=ChangeState( + lifecycle_node_matcher=launch.events.matches_action(laser_mapping_node), + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE, + )) + ]) + + activate_mapping = RegisterEventHandler(OnStateTransition( + target_lifecycle_node=laser_mapping_node, + goal_state='inactive', + entities=[EmitEvent(event=ChangeState( + lifecycle_node_matcher=launch.events.matches_action(laser_mapping_node), + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE, + ))], + handle_once=True, + )) + + configure_imu = TimerAction(period=2.5, actions=[ + EmitEvent(event=ChangeState( + lifecycle_node_matcher=launch.events.matches_action(imu_preintegration_node), + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE, + )) + ]) + + activate_imu = RegisterEventHandler(OnStateTransition( + target_lifecycle_node=imu_preintegration_node, + goal_state='inactive', + entities=[EmitEvent(event=ChangeState( + lifecycle_node_matcher=launch.events.matches_action(imu_preintegration_node), + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE, + ))], + handle_once=True, + )) return LaunchDescription([ launch_ros.actions.SetParameter(name='use_sim_time', value='false'), @@ -160,4 +221,12 @@ def generate_launch_description(): feature_extraction_node, laser_mapping_node, imu_preintegration_node, + # Auto-transition event handlers + activate_feature, + activate_mapping, + activate_imu, + # Timed configure triggers + configure_feature, + configure_mapping, + configure_imu, ]) diff --git a/src/slam/arise_slam_mid360/package.xml b/src/slam/arise_slam_mid360/package.xml index 6b428a98..b9390a12 100755 --- a/src/slam/arise_slam_mid360/package.xml +++ b/src/slam/arise_slam_mid360/package.xml @@ -18,6 +18,7 @@ amend_cmake rclcpp + rclcpp_lifecycle rclpy tf2 std_msgs diff --git a/src/slam/arise_slam_mid360/src/FeatureExtraction/featureExtraction.cpp b/src/slam/arise_slam_mid360/src/FeatureExtraction/featureExtraction.cpp index fb5c6668..e50db97f 100644 --- a/src/slam/arise_slam_mid360/src/FeatureExtraction/featureExtraction.cpp +++ b/src/slam/arise_slam_mid360/src/FeatureExtraction/featureExtraction.cpp @@ -22,10 +22,16 @@ namespace arise_slam { featureExtraction::featureExtraction(const rclcpp::NodeOptions & options) - : Node("feature_extraction_node", options) { + : LifecycleNode("feature_extraction_node", options) { } - void featureExtraction::initInterface() { + featureExtraction::CallbackReturn featureExtraction::on_configure(const rclcpp_lifecycle::State &) { + RCLCPP_INFO(this->get_logger(), "Configuring feature_extraction_node..."); + + imuBuf.allocate(2000); + visualOdomBuf.allocate(2000); + lidarBuf.allocate(50); + //! Callback Groups cb_group_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant); rclcpp::SubscriptionOptions sub_options; @@ -133,6 +139,63 @@ namespace arise_slam { m_imuPeriod = 1.0/200; + return CallbackReturn::SUCCESS; + } + + featureExtraction::CallbackReturn featureExtraction::on_activate(const rclcpp_lifecycle::State &) { + RCLCPP_INFO(this->get_logger(), "Activating feature_extraction_node..."); + if (pubLaserCloud) pubLaserCloud->on_activate(); + if (pubEdgePoints) pubEdgePoints->on_activate(); + if (pubPlannerPoints) pubPlannerPoints->on_activate(); + if (pubBobPoints) pubBobPoints->on_activate(); + if (pubLaserFeatureInfo) pubLaserFeatureInfo->on_activate(); + if (pubDepthUpPoints) pubDepthUpPoints->on_activate(); + if (pubDepthDownPoints) pubDepthDownPoints->on_activate(); + for (auto & p : pubEachScan) if (p) p->on_activate(); + return CallbackReturn::SUCCESS; + } + + featureExtraction::CallbackReturn featureExtraction::on_deactivate(const rclcpp_lifecycle::State &) { + RCLCPP_INFO(this->get_logger(), "Deactivating feature_extraction_node..."); + if (pubLaserCloud) pubLaserCloud->on_deactivate(); + if (pubEdgePoints) pubEdgePoints->on_deactivate(); + if (pubPlannerPoints) pubPlannerPoints->on_deactivate(); + if (pubBobPoints) pubBobPoints->on_deactivate(); + if (pubLaserFeatureInfo) pubLaserFeatureInfo->on_deactivate(); + if (pubDepthUpPoints) pubDepthUpPoints->on_deactivate(); + if (pubDepthDownPoints) pubDepthDownPoints->on_deactivate(); + for (auto & p : pubEachScan) if (p) p->on_deactivate(); + return CallbackReturn::SUCCESS; + } + + featureExtraction::CallbackReturn featureExtraction::on_cleanup(const rclcpp_lifecycle::State &) { + RCLCPP_INFO(this->get_logger(), "Cleaning up feature_extraction_node..."); + subLaserCloud.reset(); + subLivoxCloud.reset(); + subImu.reset(); + subOdom.reset(); + sub_depth_up.reset(); + sub_depth_down.reset(); + pubLaserCloud.reset(); + pubEdgePoints.reset(); + pubPlannerPoints.reset(); + pubBobPoints.reset(); + pubLaserFeatureInfo.reset(); + pubDepthUpPoints.reset(); + pubDepthDownPoints.reset(); + pubEachScan.clear(); + return CallbackReturn::SUCCESS; + } + + featureExtraction::CallbackReturn featureExtraction::on_shutdown(const rclcpp_lifecycle::State &) { + RCLCPP_INFO(this->get_logger(), "Shutting down feature_extraction_node..."); + on_cleanup(rclcpp_lifecycle::State()); + return CallbackReturn::SUCCESS; + } + + void featureExtraction::initInterface() { + // Legacy shim - calls on_configure for backward compatibility + on_configure(rclcpp_lifecycle::State()); } void featureExtraction::depthupHandler(const sensor_msgs::msg::PointCloud2::SharedPtr depthupMsg) @@ -791,7 +854,7 @@ namespace arise_slam { template sensor_msgs::msg::PointCloud2 - featureExtraction::publishCloud(rclcpp::Publisher::SharedPtr thisPub, typename pcl::PointCloud::Ptr thisCloud, + featureExtraction::publishCloud(rclcpp_lifecycle::LifecyclePublisher::SharedPtr thisPub, typename pcl::PointCloud::Ptr thisCloud, rclcpp::Time thisStamp, std::string thisFrame) { sensor_msgs::msg::PointCloud2 tempCloud; diff --git a/src/slam/arise_slam_mid360/src/ImuPreintegration/imuPreintegration.cpp b/src/slam/arise_slam_mid360/src/ImuPreintegration/imuPreintegration.cpp index 5262f4db..13871ef8 100644 --- a/src/slam/arise_slam_mid360/src/ImuPreintegration/imuPreintegration.cpp +++ b/src/slam/arise_slam_mid360/src/ImuPreintegration/imuPreintegration.cpp @@ -10,10 +10,16 @@ static Eigen::Affine3f lidarOdomAffine; namespace arise_slam { imuPreintegration::imuPreintegration(const rclcpp::NodeOptions & options) - : Node("imu_preintegration_node", options) { + : LifecycleNode("imu_preintegration_node", options) { } - - void imuPreintegration::initInterface() { + + imuPreintegration::CallbackReturn imuPreintegration::on_configure(const rclcpp_lifecycle::State &) { + RCLCPP_INFO(this->get_logger(), "Configuring..."); + + // Allocate buffers + imuBuf.allocate(1000); + lidarOdomBuf.allocate(100); + visualOdomBuf.allocate(5000); //! Callback Groups cb_group_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant); rclcpp::SubscriptionOptions sub_options; @@ -112,6 +118,67 @@ namespace arise_slam { lidar2Imu = imu2Lidar.inverse(); } + RCLCPP_INFO(this->get_logger(), "Configured successfully"); + return CallbackReturn::SUCCESS; + } + + imuPreintegration::CallbackReturn imuPreintegration::on_activate(const rclcpp_lifecycle::State &) { + RCLCPP_INFO(this->get_logger(), "Activating..."); + pubImuOdometry2->on_activate(); + pubHealthStatus->on_activate(); + pubImuPath->on_activate(); + RCLCPP_INFO(this->get_logger(), "Activated successfully"); + return CallbackReturn::SUCCESS; + } + + imuPreintegration::CallbackReturn imuPreintegration::on_deactivate(const rclcpp_lifecycle::State &) { + RCLCPP_INFO(this->get_logger(), "Deactivating..."); + pubImuOdometry2->on_deactivate(); + pubHealthStatus->on_deactivate(); + pubImuPath->on_deactivate(); + RCLCPP_INFO(this->get_logger(), "Deactivated successfully"); + return CallbackReturn::SUCCESS; + } + + imuPreintegration::CallbackReturn imuPreintegration::on_cleanup(const rclcpp_lifecycle::State &) { + RCLCPP_INFO(this->get_logger(), "Cleaning up..."); + subImu.reset(); + subLaserOdometry.reset(); + subVisualOdometry.reset(); + pubImuOdometry.reset(); + pubImuOdometry2.reset(); + pubHealthStatus.reset(); + pubImuPath.reset(); + pubImuOdometrySmooth.reset(); + tfMap2Odom.reset(); + tfOdom2BaseLink.reset(); + imuIntegratorOpt_.reset(); + imuIntegratorImu_.reset(); + imuQueOpt.clear(); + imuQueImu.clear(); + resetParams(); + RCLCPP_INFO(this->get_logger(), "Cleaned up successfully"); + return CallbackReturn::SUCCESS; + } + + imuPreintegration::CallbackReturn imuPreintegration::on_shutdown(const rclcpp_lifecycle::State &) { + RCLCPP_INFO(this->get_logger(), "Shutting down..."); + subImu.reset(); + subLaserOdometry.reset(); + subVisualOdometry.reset(); + pubImuOdometry.reset(); + pubImuOdometry2.reset(); + pubHealthStatus.reset(); + pubImuPath.reset(); + pubImuOdometrySmooth.reset(); + tfMap2Odom.reset(); + tfOdom2BaseLink.reset(); + RCLCPP_INFO(this->get_logger(), "Shut down successfully"); + return CallbackReturn::SUCCESS; + } + + void imuPreintegration::initInterface() { + on_configure(rclcpp_lifecycle::State()); } diff --git a/src/slam/arise_slam_mid360/src/LaserMapping/laserMapping.cpp b/src/slam/arise_slam_mid360/src/LaserMapping/laserMapping.cpp index 0dafac56..ddf29980 100644 --- a/src/slam/arise_slam_mid360/src/LaserMapping/laserMapping.cpp +++ b/src/slam/arise_slam_mid360/src/LaserMapping/laserMapping.cpp @@ -12,11 +12,13 @@ Eigen::Map q_w_curr(parameters+3); namespace arise_slam { laserMapping::laserMapping(const rclcpp::NodeOptions & options) - : Node("laser_mapping_node", options) { + : LifecycleNode("laser_mapping_node", options) { this->get_logger().set_level(rclcpp::Logger::Level::Debug); } - void laserMapping::initInterface() { + laserMapping::CallbackReturn laserMapping::on_configure(const rclcpp_lifecycle::State &) { + RCLCPP_INFO(this->get_logger(), "Configuring laser_mapping_node..."); + //! Callback Groups cb_group_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant); rclcpp::SubscriptionOptions sub_options; @@ -109,10 +111,6 @@ namespace arise_slam { pubprediction_source = this->create_publisher( ProjectName+"/prediction_source", 1); - process_timer_ = this->create_wall_timer( - std::chrono::milliseconds(static_cast(100.)), - std::bind(&laserMapping::process, this)); - slam.initROSInterface(shared_from_this()); slam.localMap.lineRes_ = config_.lineRes; slam.localMap.planeRes_ = config_.planeRes; @@ -149,6 +147,76 @@ namespace arise_slam { initializationParam(); + return CallbackReturn::SUCCESS; + } + + laserMapping::CallbackReturn laserMapping::on_activate(const rclcpp_lifecycle::State &) { + RCLCPP_INFO(this->get_logger(), "Activating laser_mapping_node..."); + if (pubLaserCloudSurround) pubLaserCloudSurround->on_activate(); + if (pubLaserCloudMap) pubLaserCloudMap->on_activate(); + if (pubLaserCloudPrior) pubLaserCloudPrior->on_activate(); + if (pubLaserCloudFullRes) pubLaserCloudFullRes->on_activate(); + if (pubOdomAftMapped) pubOdomAftMapped->on_activate(); + if (pubLaserOdometryIncremental) pubLaserOdometryIncremental->on_activate(); + if (pubVIOPrediction) pubVIOPrediction->on_activate(); + if (pubLIOPrediction) pubLIOPrediction->on_activate(); + if (pubLaserAfterMappedPath) pubLaserAfterMappedPath->on_activate(); + if (pubOptimizationStats) pubOptimizationStats->on_activate(); + if (pubprediction_source) pubprediction_source->on_activate(); + + process_timer_ = this->create_wall_timer( + std::chrono::milliseconds(static_cast(100.)), + std::bind(&laserMapping::process, this)); + + return CallbackReturn::SUCCESS; + } + + laserMapping::CallbackReturn laserMapping::on_deactivate(const rclcpp_lifecycle::State &) { + RCLCPP_INFO(this->get_logger(), "Deactivating laser_mapping_node..."); + process_timer_.reset(); + if (pubLaserCloudSurround) pubLaserCloudSurround->on_deactivate(); + if (pubLaserCloudMap) pubLaserCloudMap->on_deactivate(); + if (pubLaserCloudPrior) pubLaserCloudPrior->on_deactivate(); + if (pubLaserCloudFullRes) pubLaserCloudFullRes->on_deactivate(); + if (pubOdomAftMapped) pubOdomAftMapped->on_deactivate(); + if (pubLaserOdometryIncremental) pubLaserOdometryIncremental->on_deactivate(); + if (pubVIOPrediction) pubVIOPrediction->on_deactivate(); + if (pubLIOPrediction) pubLIOPrediction->on_deactivate(); + if (pubLaserAfterMappedPath) pubLaserAfterMappedPath->on_deactivate(); + if (pubOptimizationStats) pubOptimizationStats->on_deactivate(); + if (pubprediction_source) pubprediction_source->on_deactivate(); + return CallbackReturn::SUCCESS; + } + + laserMapping::CallbackReturn laserMapping::on_cleanup(const rclcpp_lifecycle::State &) { + RCLCPP_INFO(this->get_logger(), "Cleaning up laser_mapping_node..."); + process_timer_.reset(); + subLaserFeatureInfo.reset(); + subIMUOdometry.reset(); + subVisualOdometry.reset(); + pubLaserCloudSurround.reset(); + pubLaserCloudMap.reset(); + pubLaserCloudPrior.reset(); + pubLaserCloudFullRes.reset(); + pubOdomAftMapped.reset(); + pubLaserOdometryIncremental.reset(); + pubVIOPrediction.reset(); + pubLIOPrediction.reset(); + pubLaserAfterMappedPath.reset(); + pubOptimizationStats.reset(); + pubprediction_source.reset(); + return CallbackReturn::SUCCESS; + } + + laserMapping::CallbackReturn laserMapping::on_shutdown(const rclcpp_lifecycle::State &) { + RCLCPP_INFO(this->get_logger(), "Shutting down laser_mapping_node..."); + on_cleanup(rclcpp_lifecycle::State()); + return CallbackReturn::SUCCESS; + } + + void laserMapping::initInterface() { + // Legacy shim - calls on_configure for backward compatibility + on_configure(rclcpp_lifecycle::State()); } void laserMapping::initializationParam() { diff --git a/src/slam/arise_slam_mid360/src/LidarProcess/LidarSlam.cpp b/src/slam/arise_slam_mid360/src/LidarProcess/LidarSlam.cpp index 17325c3f..f423291e 100755 --- a/src/slam/arise_slam_mid360/src/LidarProcess/LidarSlam.cpp +++ b/src/slam/arise_slam_mid360/src/LidarProcess/LidarSlam.cpp @@ -18,7 +18,7 @@ namespace arise_slam { } - void LidarSLAM::initROSInterface(rclcpp::Node::SharedPtr node) { + void LidarSLAM::initROSInterface(rclcpp_lifecycle::LifecycleNode::SharedPtr node) { node_ = node; } diff --git a/src/slam/arise_slam_mid360/src/featureExtraction_node.cpp b/src/slam/arise_slam_mid360/src/featureExtraction_node.cpp index 15d32d0d..6c1f8547 100755 --- a/src/slam/arise_slam_mid360/src/featureExtraction_node.cpp +++ b/src/slam/arise_slam_mid360/src/featureExtraction_node.cpp @@ -9,19 +9,13 @@ int main(int argc, char **argv) rclcpp::init(argc,argv); rclcpp::NodeOptions options; options.arguments({"feature_extraction_node"}); - + std::shared_ptr featureExtraction = std::make_shared(options); - featureExtraction->imuBuf.allocate(2000); - featureExtraction->visualOdomBuf.allocate(2000); - featureExtraction->lidarBuf.allocate(50); - featureExtraction->initInterface(); - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(featureExtraction); + executor.add_node(featureExtraction->get_node_base_interface()); executor.spin(); - // rclcpp::spin(featureExtraction->get_node_base_interface()); rclcpp::shutdown(); return 0; diff --git a/src/slam/arise_slam_mid360/src/imuPreintegration_node.cpp b/src/slam/arise_slam_mid360/src/imuPreintegration_node.cpp index 82f441e4..1a88056f 100755 --- a/src/slam/arise_slam_mid360/src/imuPreintegration_node.cpp +++ b/src/slam/arise_slam_mid360/src/imuPreintegration_node.cpp @@ -13,15 +13,9 @@ int main(int argc, char **argv) std::shared_ptr imuPreintegration = std::make_shared(options); - imuPreintegration->imuBuf.allocate(1000); - imuPreintegration->lidarOdomBuf.allocate(100); - imuPreintegration->visualOdomBuf.allocate(5000); - imuPreintegration->initInterface(); - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(imuPreintegration); + executor.add_node(imuPreintegration->get_node_base_interface()); executor.spin(); - // rclcpp::spin(imuPreintegration->get_node_base_interface()); rclcpp::shutdown(); return 0; diff --git a/src/slam/arise_slam_mid360/src/laserMapping_node.cpp b/src/slam/arise_slam_mid360/src/laserMapping_node.cpp index e2a21a7b..d058f52a 100755 --- a/src/slam/arise_slam_mid360/src/laserMapping_node.cpp +++ b/src/slam/arise_slam_mid360/src/laserMapping_node.cpp @@ -13,12 +13,9 @@ int main(int argc, char **argv) std::shared_ptr laserMapping = std::make_shared(options); - laserMapping->initInterface(); - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(laserMapping); + executor.add_node(laserMapping->get_node_base_interface()); executor.spin(); - // rclcpp::spin(laserMapping->get_node_base_interface()); rclcpp::shutdown(); return 0; diff --git a/src/slam/arise_slam_mid360/src/parameter/parameter.cpp b/src/slam/arise_slam_mid360/src/parameter/parameter.cpp index a5f4cee5..c2338d48 100755 --- a/src/slam/arise_slam_mid360/src/parameter/parameter.cpp +++ b/src/slam/arise_slam_mid360/src/parameter/parameter.cpp @@ -120,7 +120,7 @@ float yaw_ratio; // bool start_from_previous_map; template -T readParam(rclcpp::Node::SharedPtr node, std::string name) +T readParam(rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::string name) { T ans; // node->declare_parameter(name); @@ -129,12 +129,12 @@ T readParam(rclcpp::Node::SharedPtr node, std::string name) } else { RCLCPP_ERROR(node->get_logger(), "Failed to load %s", name.c_str()); - rclcpp::shutdown(); + throw std::runtime_error("Failed to load parameter: " + name); } return ans; } -bool readCalibration(rclcpp::Node::SharedPtr node) +bool readCalibration(rclcpp_lifecycle::LifecycleNode::SharedPtr node) { RCLCPP_INFO(node->get_logger(), "[arise_slam] read parameter"); std::string calib_file; @@ -290,7 +290,7 @@ bool readCalibration(rclcpp::Node::SharedPtr node) return true; } -bool readGlobalparam(rclcpp::Node::SharedPtr node) +bool readGlobalparam(rclcpp_lifecycle::LifecycleNode::SharedPtr node) { node->declare_parameter("imu_topic","imu/data"); node->declare_parameter("laser_topic","velodyne_points"); diff --git a/src/slam/fastlio2_autonomy_bridge/CMakeLists.txt b/src/slam/fastlio2_autonomy_bridge/CMakeLists.txt index 880a04f8..e3d66dfd 100644 --- a/src/slam/fastlio2_autonomy_bridge/CMakeLists.txt +++ b/src/slam/fastlio2_autonomy_bridge/CMakeLists.txt @@ -7,6 +7,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) find_package(nav_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(geometry_msgs REQUIRED) @@ -20,6 +21,7 @@ add_executable(fastlio2_autonomy_bridge_node src/fastlio2_autonomy_bridge_node.c ament_target_dependencies( fastlio2_autonomy_bridge_node rclcpp + rclcpp_lifecycle nav_msgs sensor_msgs geometry_msgs diff --git a/src/slam/fastlio2_autonomy_bridge/launch/fastlio2_autonomy.launch.py b/src/slam/fastlio2_autonomy_bridge/launch/fastlio2_autonomy.launch.py index b03009d2..92c4eeb7 100644 --- a/src/slam/fastlio2_autonomy_bridge/launch/fastlio2_autonomy.launch.py +++ b/src/slam/fastlio2_autonomy_bridge/launch/fastlio2_autonomy.launch.py @@ -2,10 +2,14 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument +from launch.actions import DeclareLaunchArgument, EmitEvent, RegisterEventHandler, TimerAction from launch.conditions import IfCondition, UnlessCondition from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node, SetParameter +from launch_ros.actions import LifecycleNode, SetParameter +from launch_ros.event_handlers import OnStateTransition +from launch_ros.events.lifecycle import ChangeState +import launch.events +import lifecycle_msgs.msg def generate_launch_description(): @@ -42,7 +46,7 @@ def generate_launch_description(): lio_config_path = os.path.join( get_package_share_directory("fastlio2"), "config", "lio_autonomy.yaml" ) - lio_node = Node( + lio_node = LifecycleNode( package="fastlio2", namespace="fastlio2", executable="lio_node", @@ -55,7 +59,7 @@ def generate_launch_description(): pgo_config_path = os.path.join( get_package_share_directory("pgo"), "config", "pgo.yaml" ) - pgo_node = Node( + pgo_node = LifecycleNode( package="pgo", namespace="pgo", executable="pgo_node", @@ -69,7 +73,7 @@ def generate_launch_description(): localizer_config_path = os.path.join( get_package_share_directory("localizer"), "config", "localizer.yaml" ) - localizer_node = Node( + localizer_node = LifecycleNode( package="localizer", namespace="localizer", executable="localizer_node", @@ -83,8 +87,9 @@ def generate_launch_description(): ) # Bridge: publish autonomy stack topics in map frame - bridge_node = Node( + bridge_node = LifecycleNode( package="fastlio2_autonomy_bridge", + namespace="", executable="fastlio2_autonomy_bridge_node", name="fastlio2_autonomy_bridge", output="screen", @@ -102,6 +107,76 @@ def generate_launch_description(): ], ) + # Auto-configure after small delay + configure_lio = TimerAction(period=0.5, actions=[ + EmitEvent(event=ChangeState( + lifecycle_node_matcher=launch.events.matches_action(lio_node), + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE, + )) + ]) + + # Auto-activate after configure succeeds (node reaches 'inactive') + activate_lio = RegisterEventHandler(OnStateTransition( + target_lifecycle_node=lio_node, + goal_state='inactive', + entities=[EmitEvent(event=ChangeState( + lifecycle_node_matcher=launch.events.matches_action(lio_node), + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE, + ))], + handle_once=True, + )) + + configure_pgo = TimerAction(period=1.5, actions=[ + EmitEvent(event=ChangeState( + lifecycle_node_matcher=launch.events.matches_action(pgo_node), + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE, + )) + ]) + + activate_pgo = RegisterEventHandler(OnStateTransition( + target_lifecycle_node=pgo_node, + goal_state='inactive', + entities=[EmitEvent(event=ChangeState( + lifecycle_node_matcher=launch.events.matches_action(pgo_node), + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE, + ))], + handle_once=True, + )) + + configure_localizer = TimerAction(period=1.5, actions=[ + EmitEvent(event=ChangeState( + lifecycle_node_matcher=launch.events.matches_action(localizer_node), + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE, + )) + ]) + + activate_localizer = RegisterEventHandler(OnStateTransition( + target_lifecycle_node=localizer_node, + goal_state='inactive', + entities=[EmitEvent(event=ChangeState( + lifecycle_node_matcher=launch.events.matches_action(localizer_node), + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE, + ))], + handle_once=True, + )) + + configure_bridge = TimerAction(period=2.5, actions=[ + EmitEvent(event=ChangeState( + lifecycle_node_matcher=launch.events.matches_action(bridge_node), + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE, + )) + ]) + + activate_bridge = RegisterEventHandler(OnStateTransition( + target_lifecycle_node=bridge_node, + goal_state='inactive', + entities=[EmitEvent(event=ChangeState( + lifecycle_node_matcher=launch.events.matches_action(bridge_node), + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE, + ))], + handle_once=True, + )) + return LaunchDescription( [ declare_use_sim_time, @@ -112,6 +187,16 @@ def generate_launch_description(): pgo_node, localizer_node, bridge_node, + # Auto-transition event handlers + activate_lio, + activate_pgo, + activate_localizer, + activate_bridge, + # Timed configure triggers + configure_lio, + configure_pgo, + configure_localizer, + configure_bridge, ] ) diff --git a/src/slam/fastlio2_autonomy_bridge/package.xml b/src/slam/fastlio2_autonomy_bridge/package.xml index 8b657e97..6424d155 100644 --- a/src/slam/fastlio2_autonomy_bridge/package.xml +++ b/src/slam/fastlio2_autonomy_bridge/package.xml @@ -9,6 +9,7 @@ ament_cmake rclcpp + rclcpp_lifecycle nav_msgs sensor_msgs geometry_msgs diff --git a/src/slam/fastlio2_autonomy_bridge/src/fastlio2_autonomy_bridge_node.cpp b/src/slam/fastlio2_autonomy_bridge/src/fastlio2_autonomy_bridge_node.cpp index 0b7ffdcc..70961908 100644 --- a/src/slam/fastlio2_autonomy_bridge/src/fastlio2_autonomy_bridge_node.cpp +++ b/src/slam/fastlio2_autonomy_bridge/src/fastlio2_autonomy_bridge_node.cpp @@ -1,6 +1,8 @@ #include #include +#include +#include #include #include #include @@ -16,11 +18,13 @@ #include #include -class Fastlio2AutonomyBridge : public rclcpp::Node +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +class Fastlio2AutonomyBridge : public rclcpp_lifecycle::LifecycleNode { public: Fastlio2AutonomyBridge() - : Node("fastlio2_autonomy_bridge"), + : LifecycleNode("fastlio2_autonomy_bridge"), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { @@ -31,15 +35,20 @@ class Fastlio2AutonomyBridge : public rclcpp::Node // Publishing contract for autonomy stack map_frame_ = this->declare_parameter("map_frame", "map"); - // If non-empty, we will look up TF from map_frame -> local_frame. If empty, we auto-detect local_frame - // from the incoming odometry header.frame_id. local_frame_ = this->declare_parameter("local_frame", ""); tf_timeout_sec_ = this->declare_parameter("tf_timeout_sec", 0.02); - // Localization mode support: RViz /initialpose triggers localizer service. Until converged, we fall back to raw LIO. + // Localization mode support local_mode_ = this->declare_parameter("local_mode", false); relocalization_map_path_ = this->declare_parameter("relocalization_map_path", ""); + RCLCPP_INFO(this->get_logger(), "FASTLIO2 bridge created (lifecycle)"); + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State &) + { + RCLCPP_INFO(this->get_logger(), "Configuring FASTLIO2 bridge..."); + pub_odom_ = this->create_publisher(output_odom_topic_, rclcpp::QoS(10)); pub_cloud_ = this->create_publisher(output_cloud_topic_, rclcpp::QoS(10)); @@ -61,10 +70,6 @@ class Fastlio2AutonomyBridge : public rclcpp::Node "/initialpose", rclcpp::QoS(10), std::bind(&Fastlio2AutonomyBridge::initialposeCB, this, std::placeholders::_1)); - - check_timer_ = this->create_wall_timer( - std::chrono::milliseconds(250), - std::bind(&Fastlio2AutonomyBridge::checkValidCB, this)); } RCLCPP_INFO(this->get_logger(), @@ -73,6 +78,58 @@ class Fastlio2AutonomyBridge : public rclcpp::Node input_cloud_topic_.c_str(), output_cloud_topic_.c_str(), map_frame_.c_str(), local_frame_.empty() ? "(auto)" : local_frame_.c_str(), local_mode_ ? 1 : 0); + + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_activate(const rclcpp_lifecycle::State &) + { + RCLCPP_INFO(this->get_logger(), "Activating FASTLIO2 bridge..."); + pub_odom_->on_activate(); + pub_cloud_->on_activate(); + + if (local_mode_) + { + check_timer_ = this->create_wall_timer( + std::chrono::milliseconds(250), + std::bind(&Fastlio2AutonomyBridge::checkValidCB, this)); + } + + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) + { + RCLCPP_INFO(this->get_logger(), "Deactivating FASTLIO2 bridge..."); + check_timer_.reset(); + pub_odom_->on_deactivate(); + pub_cloud_->on_deactivate(); + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) + { + RCLCPP_INFO(this->get_logger(), "Cleaning up FASTLIO2 bridge..."); + check_timer_.reset(); + sub_odom_.reset(); + sub_cloud_.reset(); + sub_initialpose_.reset(); + pub_odom_.reset(); + pub_cloud_.reset(); + reloc_client_.reset(); + reloc_check_client_.reset(); + local_frame_ready_ = false; + has_initialpose_ = false; + localization_valid_ = false; + pending_relocalize_ = false; + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_shutdown(const rclcpp_lifecycle::State &) + { + RCLCPP_INFO(this->get_logger(), "Shutting down FASTLIO2 bridge..."); + on_cleanup(rclcpp_lifecycle::State()); + return CallbackReturn::SUCCESS; } private: @@ -253,8 +310,8 @@ class Fastlio2AutonomyBridge : public rclcpp::Node bool local_frame_ready_ = false; rclcpp::Subscription::SharedPtr sub_odom_; rclcpp::Subscription::SharedPtr sub_cloud_; - rclcpp::Publisher::SharedPtr pub_odom_; - rclcpp::Publisher::SharedPtr pub_cloud_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr pub_odom_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr pub_cloud_; std::string relocalization_map_path_; @@ -274,9 +331,10 @@ class Fastlio2AutonomyBridge : public rclcpp::Node int main(int argc, char ** argv) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + auto node = std::make_shared(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + executor.spin(); rclcpp::shutdown(); return 0; } - -