diff --git a/.gitignore b/.gitignore index e7a814b..a007fea 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1 @@ -Log/* build/* diff --git a/CMakeLists.txt b/CMakeLists.txt index 0652aab..79cd478 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,9 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.10) project(fast_livo) +find_package(fmt REQUIRED) +find_package(Boost REQUIRED COMPONENTS filesystem system) + set(CMAKE_BUILD_TYPE "Release") message(STATUS "Build Type: ${CMAKE_BUILD_TYPE}") @@ -75,58 +78,93 @@ else() message(STATUS "mimalloc not found, proceeding without it") endif() -# Find catkin and required dependencies -find_package(catkin REQUIRED COMPONENTS - geometry_msgs - nav_msgs - sensor_msgs - roscpp - rospy - std_msgs - pcl_ros - tf - message_generation - eigen_conversions - vikit_common - vikit_ros - cv_bridge - image_transport -) - +# Find ament and required dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclpy REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(pcl_ros REQUIRED) +find_package(pcl_conversions REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(livox_ros_driver2 REQUIRED) +find_package(vikit_common REQUIRED) +find_package(vikit_ros REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(image_transport REQUIRED) find_package(Eigen3 REQUIRED) find_package(PCL REQUIRED) find_package(OpenCV REQUIRED) find_package(Sophus REQUIRED) find_package(Boost REQUIRED COMPONENTS thread) -set(Sophus_LIBRARIES libSophus.so) - -# Define the catkin package -catkin_package( - CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime cv_bridge vikit_common vikit_ros image_transport - DEPENDS EIGEN3 PCL OpenCV Sophus -) - # Include directories for dependencies include_directories( - ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ${PCL_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ${Sophus_INCLUDE_DIRS} + ${vikit_common_INCLUDE_DIRS} + ${vikit_ros_INCLUDE_DIRS} include ) +set(dependencies + rclcpp + rclpy + geometry_msgs + nav_msgs + sensor_msgs + visualization_msgs + cv_bridge + vikit_common + vikit_ros + image_transport + pcl_ros + pcl_conversions + tf2_ros + livox_ros_driver2 +) + +set(COMMON_DEPENDENCIES OpenMP::OpenMP_CXX) + +# link_directories(${COMMON_DEPENDENCIES} +# ${vikit_common_LIBRARIES}/libvikit_common.so +# ${vikit_ros_LIBRARIES}/libvikit_ros.so +# ) + # Add libraries add_library(vio src/vio.cpp src/frame.cpp src/visual_point.cpp) add_library(lio src/voxel_map.cpp) add_library(pre src/preprocess.cpp) add_library(imu_proc src/IMU_Processing.cpp) add_library(laser_mapping src/LIVMapper.cpp) +add_library(utils src/utils.cpp) + +ament_target_dependencies(vio ${dependencies} ) +ament_target_dependencies(lio ${dependencies}) +ament_target_dependencies(pre ${dependencies}) +ament_target_dependencies(imu_proc ${dependencies}) +ament_target_dependencies(laser_mapping ${dependencies}) + +# linking libraries or executables to public dependencies +target_link_libraries(laser_mapping + ${CMAKE_SOURCE_DIR}/../../install/vikit_common/lib/libvikit_common.so + ${CMAKE_SOURCE_DIR}/../../install/vikit_ros/lib/libvikit_ros.so + ${COMMON_DEPENDENCIES} +) +target_link_libraries(vio ${COMMON_DEPENDENCIES}) +target_link_libraries(lio utils ${COMMON_DEPENDENCIES}) +target_link_libraries(pre ${COMMON_DEPENDENCIES}) +target_link_libraries(imu_proc ${COMMON_DEPENDENCIES}) # Add the main executable add_executable(fastlivo_mapping src/main.cpp) +ament_target_dependencies(fastlivo_mapping ${dependencies}) + # Link libraries to the executable target_link_libraries(fastlivo_mapping laser_mapping @@ -134,14 +172,45 @@ target_link_libraries(fastlivo_mapping lio pre imu_proc - ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} ${Sophus_LIBRARIES} ${Boost_LIBRARIES} + fmt::fmt + Boost::filesystem ) # Link mimalloc if found if(mimalloc_FOUND) target_link_libraries(fastlivo_mapping mimalloc) -endif() \ No newline at end of file +endif() + +# Install the executable +install(TARGETS + fastlivo_mapping + DESTINATION lib/${PROJECT_NAME} +) + +install( + DIRECTORY config launch rviz_cfg + DESTINATION share/${PROJECT_NAME} +) + +# Export dependencies +ament_export_dependencies( + rclcpp + rclpy + geometry_msgs + nav_msgs + sensor_msgs + pcl_ros + pcl_conversions + tf2_ros + livox_ros_driver2 + Eigen3 + PCL + OpenCV + Sophus +) + +ament_package() diff --git a/Log/Colmap/sparse/0/cameras.txt b/Log/Colmap/sparse/0/cameras.txt index 98f766c..ef35c1a 100644 --- a/Log/Colmap/sparse/0/cameras.txt +++ b/Log/Colmap/sparse/0/cameras.txt @@ -1,3 +1,3 @@ # Camera list with one line of data per camera: # CAMERA_ID, MODEL, WIDTH, HEIGHT, PARAMS[] -1 PINHOLE 640 512 588.143715 588.107927 296.059369 254.543215 +1 EQUIDISTANT 640 512 646.784720 646.657750 313.456795 261.399612 diff --git a/Log/PCD/.gitkeep b/Log/PCD/.gitkeep deleted file mode 100644 index e69de29..0000000 diff --git a/README.md b/README.md index 4abb623..bbf67a2 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,14 @@ -# FAST-LIVO2 +# FAST-LIVO2 ROS2 HUMBLE ## FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry +### :star: Acknowledgments + +This project extends the following open-source contributions: + +- **Original work**: [FAST-LIVO2](https://github.com/hku-mars/FAST-LIVO2) by Chunran Zheng (HKU MARS Lab). +- **ROS2 Humble port**: [FAST-LIVO2/tree/humble](https://github.com/Robotic-Developer-Road/FAST-LIVO2/tree/humble) by Robotic-Developer-Road. + ### ๐Ÿ“ข News - ๐Ÿ”“ **2025-01-23**: Code released! @@ -37,14 +44,17 @@ We open-source our handheld device, including CAD files, synchronization scheme, ### 1.4 Our associate dataset: FAST-LIVO2-Dataset Our associate dataset [**FAST-LIVO2-Dataset**](https://connecthkuhk-my.sharepoint.com/:f:/g/personal/zhengcr_connect_hku_hk/ErdFNQtjMxZOorYKDTtK4ugBkogXfq1OfDm90GECouuIQA?e=KngY9Z) used for evaluation is also available online. +### MARS-LVIG dataset +[**MARS-LVIG dataset**](https://mars.hku.hk/dataset.html)๏ผšA multi-sensor aerial robots SLAM dataset for LiDAR-visual-inertial-GNSS fusion. + ### 1.5 Our LiDAR-camera calibration method -The [**FAST-Calib**](https://github.com/hku-mars/FAST-Calib) toolkit is recommended. Its output extrinsic parameters can be directly filled into the YAML file. +The [**FAST-Calib**](https://github.com/hku-mars/FAST-Calib) toolkit is recommended. Its output extrinsic parameters can be directly filled into the YAML file. ## 2. Prerequisited ### 2.1 Ubuntu and ROS -Ubuntu 18.04~20.04. [ROS Installation](http://wiki.ros.org/ROS/Installation). +Ubuntu 22.04. [ROS Installation](http://wiki.ros.org/ROS/Installation). ### 2.2 PCL && Eigen && OpenCV @@ -56,12 +66,11 @@ OpenCV>=4.2, Follow [Opencv Installation](http://opencv.org/). ### 2.3 Sophus -Sophus Installation for the non-templated/double-only version. - +#### Binary installation ```bash -git clone https://github.com/strasdat/Sophus.git +cd ~ +git clone https://github.com/strasdat/Sophus.git -b 1.22.10 cd Sophus -git checkout a621ff mkdir build && cd build && cmake .. make sudo make install @@ -69,35 +78,101 @@ sudo make install ### 2.4 Vikit -Vikit contains camera models, some math and interpolation functions that we need. Vikit is a catkin project, therefore, download it into your catkin workspace source folder. +Vikit provides essential camera models, math utilities, and interpolation functions. As an ament package for ROS2, download its source into your colcon workspace's src folder. Additionally, I've added OpenCV fisheye distortion correction to the equidistant camera model in vikit_common. ```bash # Different from the one used in fast-livo1 -cd catkin_ws/src -git clone https://github.com/xuankuzcr/rpg_vikit.git +cd ~ +git clone https://github.com/Rhymer-Lcy/rpg_vikit_ros2_fisheye.git +mkdir -p ~/fast/src/ +cp -r ./rpg_vikit_ros2_fisheye/{vikit_common,vikit_ros} ~/fast_ws/src/ +``` + +Thanks to the following repositories for the code reference: + +- [xuankuzcr/rpg_vikit](https://github.com/xuankuzcr/rpg_vikit) +- [integralrobotics/rpg_vikit](https://github.com/integralrobotics/rpg_vikit) + +### 2.5 **livox_ros_driver2** + +Follow [livox_ros_driver2 Installation](https://github.com/Livox-SDK/livox_ros_driver2). + +```bash +cd ~/fast_ws/src/ +git clone https://github.com/Livox-SDK/livox_ros_driver2.git ws_livox/src/livox_ros_driver2 ``` +why not use `livox_ros_driver`? Because it is not compatible with ROS2 directly. actually i am not think there s any difference between [livox ros driver](https://github.com/Livox-SDK/livox_ros_driver.git) and [livox ros driver2](https://github.com/Livox-SDK/livox_ros_driver2.git) 's `CustomMsg`, the latter 's ros2 version is sufficient. + ## 3. Build -Clone the repository and catkin_make: +Clone the repository and colcon build: ``` -cd ~/catkin_ws/src -git clone https://github.com/hku-mars/FAST-LIVO2 -cd ../ -catkin_make -source ~/catkin_ws/devel/setup.bash +git clone https://github.com/Rhymer-Lcy/FAST-LIVO2-ROS2-MID360-Fisheye.git +cd livox_ros_driver2 +./build.sh humble +source ~/fast_ws/install/setup.bash ``` ## 4. Run our examples Download our collected rosbag files via OneDrive ([**FAST-LIVO2-Dataset**](https://connecthkuhk-my.sharepoint.com/:f:/g/personal/zhengcr_connect_hku_hk/ErdFNQtjMxZOorYKDTtK4ugBkogXfq1OfDm90GECouuIQA?e=KngY9Z)). +### convert rosbag + +convert ROS1 rosbag to ROS2 rosbag +```bash +pip install rosbags +rosbags-convert --src Retail_Street.bag --dst Retail_Street ``` -roslaunch fast_livo mapping_avia.launch -rosbag play YOUR_DOWNLOADED.bag +- [gitlab rosbags](https://gitlab.com/ternaris/rosbags) +- [pypi rosbags](https://pypi.org/project/rosbags/) + +### change the msg type on rosbag + +Such as dataset `Retail_Street.db3`, because we use `livox_ros2_driver2`'s `CustomMsg`, we need to change the msg type in the rosbag file. +1. use `rosbags-convert` to convert rosbag from ROS1 to ROS2. +2. change the msg type of msg type in **metadata.yaml** as follows: + +**metadata.yaml** +```diff +rosbag2_bagfile_information: + compression_format: '' + compression_mode: '' + custom_data: {} + duration: + nanoseconds: 135470252209 + files: + - duration: + nanoseconds: 135470252209 + message_count: 30157 + path: Retail_Street.db3 + .............. + topic_metadata: + name: /livox/lidar + offered_qos_profiles: '' + serialization_format: cdr +- type: livox_ros_driver/msg/CustomMsg ++ type: livox_ros_driver2/msg/CustomMsg + type_description_hash: RIHS01_94041b4794f52c1d81def2989107fc898a62dacb7a39d5dbe80d4b55e538bf6d + ............... +..... ``` +### Run the demo + +Do not forget to `source` your ROS2 workspace before running the following command. + +```bash +ros2 launch fast_livo mapping_aviz.launch.py use_rviz:=True use_sim_time:=True +ros2 bag play -p Retail_Street # Use space bar to play/pause +``` + +```bash +ros2 launch fast_livo mapping_aviz_metacamedu.launch.py use_rviz:=True use_sim_time:=True # Configuration for MID360-Fisheye dataset +ros2 bag play -p $BAG_PATH # Use space bar to play/pause (self-collected MID360-Fisheye dataset) +``` ## 5. License diff --git a/config/HILTI22.yaml b/config/HILTI22.yaml deleted file mode 100644 index fc763bc..0000000 --- a/config/HILTI22.yaml +++ /dev/null @@ -1,95 +0,0 @@ -common: - img_topic: "/alphasense/cam0/image_raw" - lid_topic: "/hesai/pandar" - imu_topic: "/alphasense/imu" - img_en: 1 - lidar_en: 1 - ros_driver_bug_fix: false - -extrin_calib: - # Hilti-2022 - extrinsic_T: [-0.001, -0.00855, 0.055] - extrinsic_R: [0, -1, 0, -1, 0, 0, 0, 0, -1] - - # Hilti-2023 - # extrinsic_T: [-0.006730146149038548, -0.006897049862999071, 0.049898628062256645] - # extrinsic_R: [0.006609639848469365, -0.9999773650294649, 0.0012578115132016717, - # -0.9999762249571927, -0.006612093869054189, -0.0019569708811106104, - # 0.001965243352927244, -0.0012448467359610184, -0.9999972940839232] - - # Hilti - Rcl: [ -0.999926, -0.00670802, 0.0101073, - -0.0100912, -0.00242564, -0.999946, - 0.00673218,-0.999975, 0.00235777 ] - Pcl: [ -0.0549762, 0.0675401, -0.0520599 ] - -time_offset: - imu_time_offset: 0.0 - img_time_offset: 0.0 - exposure_time_init: 0.0 - -preprocess: - hilti_en: true - point_filter_num: 1 - filter_size_surf: 0.1 # 0.2 - lidar_type: 5 # HesaiXT32 - scan_line: 32 - blind: 0.6 # 0.1 0.3 - -vio: - max_iterations: 5 - outlier_threshold: 500 - img_point_cov: 1000 - patch_size: 8 - patch_pyrimid_level: 4 - normal_en: true - raycast_en: false - inverse_composition_en: false - exposure_estimate_en: true - inv_expo_cov: 0.1 - -imu: - imu_en: true - imu_int_frame: 30 - acc_cov: 0.5 # 0.1 - gyr_cov: 0.01 - b_acc_cov: 0.0001 # 0.1 - b_gyr_cov: 0.0001 # 0.1 - -lio: - max_iterations: 5 - dept_err: 0.02 - beam_err: 0.05 - min_eigen_value: 0.0001 # 0.0025 - voxel_size: 0.4 - max_layer: 2 - max_points_num: 100 - layer_init_num: [5, 5, 5, 5, 5] - -local_map: - map_sliding_en: false - half_map_size: 100 - sliding_thresh: 8 - -uav: - imu_rate_odom: false - gravity_align_en: false - -publish: - dense_map_en: true - pub_effect_point_en: false - pub_plane_en: false - pub_scan_num: 1 - blind_rgb_points: 0.0 - -evo: - seq_name: "exp09_cupola" - pose_output_en: true - -pcd_save: - pcd_save_en: false - colmap_output_en: false # need to set interval = -1 - filter_size_pcd: 0.15 - interval: -1 - # how many LiDAR frames saved in each pcd file; - # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. diff --git a/config/MARS_LVIG.yaml b/config/MARS_LVIG.yaml index 34b84fc..5c7c9e1 100644 --- a/config/MARS_LVIG.yaml +++ b/config/MARS_LVIG.yaml @@ -1,111 +1,113 @@ -common: - img_topic: "/left_camera/image" - lid_topic: "/livox/lidar" - imu_topic: "/livox/imu" - img_en: 1 - lidar_en: 1 - ros_driver_bug_fix: false +/**: + ros__parameters: + common: + img_topic: "/left_camera/image" + lid_topic: "/livox/lidar" + imu_topic: "/livox/imu" + img_en: 1 + lidar_en: 1 + ros_driver_bug_fix: false -extrin_calib: - extrinsic_T: [0.04165, 0.02326, -0.0284] - extrinsic_R: [1, 0, 0, 0, 1, 0, 0, 0, 1] - # MARS_LVIG HKisland HKairport - Rcl: [0.00438814,-0.999807,-0.0191582, - -0.00978695,0.0191145,-0.999769, - 0.999942,0.00457463,-0.00970118] - Pcl: [0.016069, 0.0871753, -0.0718021] - # MARS_LVIG AMtown AMvalley - # Rcl: [ -0.0022464, -0.9997299, -0.0231319, - # -0.0084211, 0.0231501, -0.9996966, - # 0.9999620, -0.0020509, -0.0084708] - # Pcl: [-0.0025563, 0.0567484, -0.0512149] + extrin_calib: + extrinsic_T: [0.04165, 0.02326, -0.0284] + extrinsic_R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] + # MARS_LVIG HKisland HKairport + Rcl: [0.00438814,-0.999807,-0.0191582, + -0.00978695,0.0191145,-0.999769, + 0.999942,0.00457463,-0.00970118] + Pcl: [0.016069, 0.0871753, -0.0718021] + # MARS_LVIG AMtown AMvalley + # Rcl: [ -0.0022464, -0.9997299, -0.0231319, + # -0.0084211, 0.0231501, -0.9996966, + # 0.9999620, -0.0020509, -0.0084708] + # Pcl: [-0.0025563, 0.0567484, -0.0512149] -time_offset: - imu_time_offset: 0.0 - img_time_offset: -0.1 - exposure_time_init: 0.0 -# โ•”โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•— -# โ•‘ Configuration Settings โ•‘ -# โ• โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•ฃ -# โ•‘ Series โ”‚ ID โ”‚ img_time_offset โ”‚ exposure_time_init โ”‚ -s (start hover) โ•‘ -# โ• โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•ฃ -# โ•‘ HKairport โ”‚ HKairport01 โ”‚ 0.1 โ”‚ 0.0 โ”‚ 75 โ•‘ -# โ•‘ โ”‚ HKairport02 โ”‚ -0.1 โ”‚ 0.0 โ”‚ 60 โ•‘ -# โ•‘ โ”‚ HKairport03 โ”‚ -0.1 โ”‚ 0.0 โ”‚ 62 โ•‘ -# โ• โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•ฃ -# โ•‘ HKisland โ”‚ HKisland01 โ”‚ 0.0 โ”‚ 0.0 โ”‚ 118 โ•‘ -# โ•‘ โ”‚ HKisland02 โ”‚ 0.1 โ”‚ 0.0 โ”‚ 80 โ•‘ -# โ•‘ โ”‚ HKisland03 โ”‚ -0.1 โ”‚ 0.0 โ”‚ 72 โ•‘ -# โ• โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•ฃ -# โ•‘ AMtown โ”‚ AMtown01 โ”‚ -0.1 โ”‚ 0.0285 โ”‚ 75 โ•‘ -# โ•‘ โ”‚ AMtown02 โ”‚ -0.1 โ”‚ 0.0285 โ”‚ 50 โ•‘ -# โ•‘ โ”‚ AMtown03 โ”‚ -0.1 โ”‚ 0.0285 โ”‚ 106 โ•‘ -# โ• โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•ฃ -# โ•‘ AMvalley โ”‚ AMvalley01 โ”‚ -0.1 โ”‚ 0.0132 โ”‚ 70 โ•‘ -# โ•‘ โ”‚ AMvalley02 โ”‚ -0.1 โ”‚ 0.0132 โ”‚ 65 โ•‘ -# โ•‘ โ”‚ AMvalley03 โ”‚ -0.1 โ”‚ 0.0132 โ”‚ 68 โ•‘ -# โ•šโ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ• -preprocess: - point_filter_num: 1 - filter_size_surf: 0.1 - lidar_type: 1 # Livox Avia LiDAR - scan_line: 6 - blind: 0.8 + time_offset: + imu_time_offset: 0.0 + img_time_offset: -0.1 + exposure_time_init: 0.0 + # โ•”โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•— + # โ•‘ Configuration Settings โ•‘ + # โ• โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•ฃ + # โ•‘ Series โ”‚ ID โ”‚ img_time_offset โ”‚ exposure_time_init โ”‚ -s (start hover) โ•‘ + # โ• โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•ฃ + # โ•‘ HKairport โ”‚ HKairport01 โ”‚ 0.1 โ”‚ 0.0 โ”‚ 75 โ•‘ + # โ•‘ โ”‚ HKairport02 โ”‚ -0.1 โ”‚ 0.0 โ”‚ 60 โ•‘ + # โ•‘ โ”‚ HKairport03 โ”‚ -0.1 โ”‚ 0.0 โ”‚ 62 โ•‘ + # โ• โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•ฃ + # โ•‘ HKisland โ”‚ HKisland01 โ”‚ 0.0 โ”‚ 0.0 โ”‚ 118 โ•‘ + # โ•‘ โ”‚ HKisland02 โ”‚ 0.1 โ”‚ 0.0 โ”‚ 80 โ•‘ + # โ•‘ โ”‚ HKisland03 โ”‚ -0.1 โ”‚ 0.0 โ”‚ 72 โ•‘ + # โ• โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•ฃ + # โ•‘ AMtown โ”‚ AMtown01 โ”‚ -0.1 โ”‚ 0.0285 โ”‚ 75 โ•‘ + # โ•‘ โ”‚ AMtown02 โ”‚ -0.1 โ”‚ 0.0285 โ”‚ 50 โ•‘ + # โ•‘ โ”‚ AMtown03 โ”‚ -0.1 โ”‚ 0.0285 โ”‚ 106 โ•‘ + # โ• โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•ฃ + # โ•‘ AMvalley โ”‚ AMvalley01 โ”‚ -0.1 โ”‚ 0.0132 โ”‚ 70 โ•‘ + # โ•‘ โ”‚ AMvalley02 โ”‚ -0.1 โ”‚ 0.0132 โ”‚ 65 โ•‘ + # โ•‘ โ”‚ AMvalley03 โ”‚ -0.1 โ”‚ 0.0132 โ”‚ 68 โ•‘ + # โ•šโ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ•โ• + preprocess: + point_filter_num: 1 + filter_size_surf: 0.1 + lidar_type: 1 # Livox Avia LiDAR + scan_line: 6 + blind: 0.8 -vio: - max_iterations: 5 - outlier_threshold: 1000 # 78 100 156 #100 200 500 700 infinite - img_point_cov: 1000 # 100 1000 - patch_size: 8 - patch_pyrimid_level: 4 - normal_en: true - raycast_en: false - inverse_composition_en: false - exposure_estimate_en: true - inv_expo_cov: 0.1 + vio: + max_iterations: 5 + outlier_threshold: 1000 # 78 100 156 #100 200 500 700 infinite + img_point_cov: 1000 # 100 1000 + patch_size: 8 + patch_pyrimid_level: 4 + normal_en: true + raycast_en: false + inverse_composition_en: false + exposure_estimate_en: true + inv_expo_cov: 0.1 -imu: - imu_en: true - imu_int_frame: 30 - acc_cov: 2.0 # 0.5 - gyr_cov: 0.1 # 0.3 - b_acc_cov: 0.0001 # 0.1 - b_gyr_cov: 0.0001 # 0.1 + imu: + imu_en: true + imu_int_frame: 30 + acc_cov: 2.0 # 0.5 + gyr_cov: 0.1 # 0.3 + b_acc_cov: 0.0001 # 0.1 + b_gyr_cov: 0.0001 # 0.1 -lio: - max_iterations: 5 - dept_err: 0.02 - beam_err: 0.05 - min_eigen_value: 0.005 - voxel_size: 2.0 # 1.0 - max_layer: 2 - max_points_num: 50 - layer_init_num: [5, 5, 5, 5, 5] + lio: + max_iterations: 5 + dept_err: 0.02 + beam_err: 0.05 + min_eigen_value: 0.005 + voxel_size: 2.0 # 1.0 + max_layer: 2 + max_points_num: 50 + layer_init_num: [5, 5, 5, 5, 5] -local_map: - map_sliding_en: false - half_map_size: 100 - sliding_thresh: 8 + local_map: + map_sliding_en: false + half_map_size: 100 + sliding_thresh: 8.0 -uav: - imu_rate_odom: false - gravity_align_en: false + uav: + imu_rate_odom: false + gravity_align_en: false -publish: - dense_map_en: true - pub_effect_point_en: false - pub_plane_en: false - pub_scan_num: 1 - blind_rgb_points: 0.0 + publish: + dense_map_en: true + pub_effect_point_en: false + pub_plane_en: false + pub_scan_num: 1 + blind_rgb_points: 0.0 -evo: - seq_name: "HKisland03" - pose_output_en: false + evo: + seq_name: "HKisland03" + pose_output_en: false -pcd_save: - pcd_save_en: false - colmap_output_en: false # need to set interval = -1 - filter_size_pcd: 0.15 - interval: -1 - # how many LiDAR frames saved in each pcd file; - # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. \ No newline at end of file + pcd_save: + pcd_save_en: false + colmap_output_en: false # need to set interval = -1 + filter_size_pcd: 0.15 + interval: -1 + # how many LiDAR frames saved in each pcd file; + # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. \ No newline at end of file diff --git a/config/NTU_VIRAL.yaml b/config/NTU_VIRAL.yaml index b1f8a43..89964f6 100644 --- a/config/NTU_VIRAL.yaml +++ b/config/NTU_VIRAL.yaml @@ -1,87 +1,89 @@ -common: - img_topic: "/left/image_raw" - lid_topic: "/os1_cloud_node1/points" - imu_topic: "/imu/imu" - img_en: 1 - lidar_en: 1 - ros_driver_bug_fix: false +/**: + ros__parameters: + common: + img_topic: "/left/image_raw" + lid_topic: "/os1_cloud_node1/points" + imu_topic: "/imu/imu" + img_en: 1 + lidar_en: 1 + ros_driver_bug_fix: false -extrin_calib: - extrinsic_T: [-0.050, 0.000, 0.055] - extrinsic_R: [1, 0, 0, 0, 1, 0, 0, 0, 1] - # NTU_VIRAL - Rcl: [0.0218308, 0.99976, -0.00201407, - -0.0131205, 0.00230088, 0.999911, - 0.999676, -0.0218025, 0.0131676] - Pcl: [0.122993, 0.0398643, -0.0577101] + extrin_calib: + extrinsic_T: [-0.050, 0.000, 0.055] + extrinsic_R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] + # NTU_VIRAL + Rcl: [0.0218308, 0.99976, -0.00201407, + -0.0131205, 0.00230088, 0.999911, + 0.999676, -0.0218025, 0.0131676] + Pcl: [0.122993, 0.0398643, -0.0577101] -time_offset: - lidar_time_offset: -0.1 - imu_time_offset: 0.0 - img_time_offset: 0.0 - exposure_time_init: 0.0 + time_offset: + lidar_time_offset: -0.1 + imu_time_offset: 0.0 + img_time_offset: 0.0 + exposure_time_init: 0.0 -preprocess: - point_filter_num: 3 - filter_size_surf: 0.1 - lidar_type: 3 # Ouster - scan_line: 16 - blind: 1.0 + preprocess: + point_filter_num: 3 + filter_size_surf: 0.1 + lidar_type: 3 # Ouster + scan_line: 16 + blind: 1.0 -vio: - max_iterations: 5 - outlier_threshold: 1000 # 78 100 156 #100 200 500 700 infinite - img_point_cov: 100 # 100 1000 - patch_size: 8 - patch_pyrimid_level: 3 - normal_en: true - raycast_en: false - inverse_composition_en: false - exposure_estimate_en: true - inv_expo_cov: 0.1 + vio: + max_iterations: 5 + outlier_threshold: 1000 # 78 100 156 #100 200 500 700 infinite + img_point_cov: 100 # 100 1000 + patch_size: 8 + patch_pyrimid_level: 3 + normal_en: true + raycast_en: false + inverse_composition_en: false + exposure_estimate_en: true + inv_expo_cov: 0.1 -imu: - imu_en: true - imu_int_frame: 30 - acc_cov: 0.5 # 0.2 - gyr_cov: 0.3 # 0.5 - b_acc_cov: 0.0001 # 0.1 - b_gyr_cov: 0.0001 # 0.1 + imu: + imu_en: true + imu_int_frame: 30 + acc_cov: 0.5 # 0.2 + gyr_cov: 0.3 # 0.5 + b_acc_cov: 0.0001 # 0.1 + b_gyr_cov: 0.0001 # 0.1 -lio: - max_iterations: 5 - dept_err: 0.02 - beam_err: 0.01 - min_eigen_value: 0.0025 # 0.0025 - voxel_size: 0.5 - max_layer: 2 - max_points_num: 50 - layer_init_num: [5, 5, 5, 5, 5] + lio: + max_iterations: 5 + dept_err: 0.02 + beam_err: 0.01 + min_eigen_value: 0.0025 # 0.0025 + voxel_size: 0.5 + max_layer: 2 + max_points_num: 50 + layer_init_num: [5, 5, 5, 5, 5] -local_map: - map_sliding_en: false - half_map_size: 100 - sliding_thresh: 8 + local_map: + map_sliding_en: false + half_map_size: 100 + sliding_thresh: 8.0 -uav: - imu_rate_odom: false - gravity_align_en: false + uav: + imu_rate_odom: false + gravity_align_en: false -publish: - dense_map_en: true - pub_effect_point_en: false - pub_plane_en: false - pub_scan_num: 1 - blind_rgb_points: 0.0 + publish: + dense_map_en: true + pub_effect_point_en: false + pub_plane_en: false + pub_scan_num: 1 + blind_rgb_points: 0.0 -evo: - seq_name: "eee_01" - pose_output_en: true + evo: + seq_name: "eee_01" + pose_output_en: true -pcd_save: - pcd_save_en: false - colmap_output_en: false # need to set interval = -1 - filter_size_pcd: 0.15 - interval: -1 - # how many LiDAR frames saved in each pcd file; - # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. \ No newline at end of file + pcd_save: + pcd_save_en: false + colmap_output_en: false # need to set interval = -1 + filter_size_pcd: 0.15 + interval: -1 + # how many LiDAR frames saved in each pcd file; + # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. \ No newline at end of file diff --git a/config/avia.yaml b/config/avia.yaml index 04c24ba..61d3934 100755 --- a/config/avia.yaml +++ b/config/avia.yaml @@ -1,85 +1,89 @@ -common: - img_topic: "/left_camera/image" - lid_topic: "/livox/lidar" - imu_topic: "/livox/imu" - img_en: 1 - lidar_en: 1 - ros_driver_bug_fix: false +/**: + ros__parameters: + common: + img_topic: "/left_camera/image" + lid_topic: "/livox/lidar" + imu_topic: "/livox/imu" + img_en: 1 + lidar_en: 1 + ros_driver_bug_fix: false -extrin_calib: - extrinsic_T: [0.04165, 0.02326, -0.0284] - extrinsic_R: [1, 0, 0, 0, 1, 0, 0, 0, 1] - Rcl: [0.00610193,-0.999863,-0.0154172, - -0.00615449,0.0153796,-0.999863, - 0.999962,0.00619598,-0.0060598] - Pcl: [0.0194384, 0.104689,-0.0251952] + extrin_calib: + extrinsic_T: [0.04165, 0.02326, -0.0284] + extrinsic_R: [1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0] + Rcl: [0.00610193,-0.999863,-0.0154172, + -0.00615449,0.0153796,-0.999863, + 0.999962,0.00619598,-0.0060598] + Pcl: [0.0194384, 0.104689,-0.0251952] -time_offset: - imu_time_offset: 0.0 - img_time_offset: 0.1 - exposure_time_init: 0.0 + time_offset: + imu_time_offset: 0.0 + img_time_offset: 0.1 + exposure_time_init: 0.0 -preprocess: - point_filter_num: 1 - filter_size_surf: 0.1 - lidar_type: 1 # Livox Avia LiDAR - scan_line: 6 - blind: 0.8 + preprocess: + point_filter_num: 1 + filter_size_surf: 0.1 + lidar_type: 1 # Livox Avia LiDAR + scan_line: 6 + blind: 0.8 -vio: - max_iterations: 5 - outlier_threshold: 1000 # 78 100 156 #100 200 500 700 infinite - img_point_cov: 100 # 100 1000 - patch_size: 8 - patch_pyrimid_level: 4 - normal_en: true - raycast_en: false - inverse_composition_en: false - exposure_estimate_en: true - inv_expo_cov: 0.1 + vio: + max_iterations: 5 + outlier_threshold: 1000 # 78 100 156 #100 200 500 700 infinite + img_point_cov: 100 # 100 1000 + patch_size: 8 + patch_pyrimid_level: 4 + normal_en: true + raycast_en: false + inverse_composition_en: false + exposure_estimate_en: true + inv_expo_cov: 0.1 -imu: - imu_en: true - imu_int_frame: 30 - acc_cov: 0.5 # 0.2 - gyr_cov: 0.3 # 0.5 - b_acc_cov: 0.0001 # 0.1 - b_gyr_cov: 0.0001 # 0.1 + imu: + imu_en: true + imu_int_frame: 30 + acc_cov: 0.5 # 0.2 + gyr_cov: 0.3 # 0.5 + b_acc_cov: 0.0001 # 0.1 + b_gyr_cov: 0.0001 # 0.1 -lio: - max_iterations: 5 - dept_err: 0.02 - beam_err: 0.05 - min_eigen_value: 0.0025 # 0.005 - voxel_size: 0.5 - max_layer: 2 - max_points_num: 50 - layer_init_num: [5, 5, 5, 5, 5] + lio: + max_iterations: 5 + dept_err: 0.02 + beam_err: 0.05 + min_eigen_value: 0.0025 # 0.005 + voxel_size: 0.5 + max_layer: 2 + max_points_num: 50 + layer_init_num: [5, 5, 5, 5, 5] -local_map: - map_sliding_en: false - half_map_size: 100 - sliding_thresh: 8 + local_map: + map_sliding_en: false + half_map_size: 100 + sliding_thresh: 8.0 -uav: - imu_rate_odom: false - gravity_align_en: false + uav: + imu_rate_odom: false + gravity_align_en: false -publish: - dense_map_en: true - pub_effect_point_en: false - pub_plane_en: false - pub_scan_num: 1 - blind_rgb_points: 0.0 + publish: + dense_map_en: true + pub_effect_point_en: false + pub_plane_en: false + pub_scan_num: 1 + blind_rgb_points: 0.0 -evo: - seq_name: "CBD_Building_01" - pose_output_en: false + evo: + seq_name: "CBD_Building_01" + pose_output_en: false -pcd_save: - pcd_save_en: false - colmap_output_en: false # need to set interval = -1 - filter_size_pcd: 0.15 - interval: -1 - # how many LiDAR frames saved in each pcd file; - # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. + pcd_save: + pcd_save_en: false + colmap_output_en: false # need to set interval = -1 + filter_size_pcd: 0.15 + interval: -1 + # how many LiDAR frames saved in each pcd file; + # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. diff --git a/config/avia_metacamedu.yaml b/config/avia_metacamedu.yaml new file mode 100755 index 0000000..15e687d --- /dev/null +++ b/config/avia_metacamedu.yaml @@ -0,0 +1,85 @@ +/**: + ros__parameters: + common: + img_topic: "/camera/left/jpeg" + lid_topic: "/livox/lidar" + imu_topic: "/livox/imu" + img_en: 1 + lidar_en: 1 + ros_driver_bug_fix: false + enable_image_processing: true + + extrin_calib: + extrinsic_T: [0.011, 0.02329, -0.04412] + extrinsic_R: [1., 0., 0., 0., 1., 0., 0., 0., 1.] + Rcl: [0.8459084992484703, -0.4535552772229826, 0.28058228989439776, 0.3274101197085882, 0.026335829096533758, -0.9445152394843643, 0.4210005040514897, 0.8908389498719628, 0.17077628927755173] + Pcl: [0.022648260370124056, -0.09188984649611615, -0.03436178897905727] + time_offset: + imu_time_offset: 0.0 + img_time_offset: 0.0 + exposure_time_init: 0.0 + + preprocess: + point_filter_num: 1 + filter_size_surf: 0.001 + lidar_type: 7 # Livox Mid-360 LiDAR + scan_line: 4 + blind: 0.5 + + vio: + max_iterations: 5 + outlier_threshold: 1000 # 78 100 156 #100 200 500 700 infinite + img_point_cov: 100 # 100 1000 + patch_size: 8 + patch_pyrimid_level: 4 + normal_en: true + raycast_en: false + inverse_composition_en: false + exposure_estimate_en: true + inv_expo_cov: 0.1 + + imu: + imu_en: true + imu_int_frame: 30 + acc_cov: 0.8 # 0.2 + gyr_cov: 0.5 # 0.5 + b_acc_cov: 0.001 # 0.1 + b_gyr_cov: 0.001 # 0.1 + + lio: + max_iterations: 5 + dept_err: 0.02 + beam_err: 0.15 + min_eigen_value: 0.005 # 0.005 + voxel_size: 0.5 + max_layer: 2 + max_points_num: 50 + layer_init_num: [5, 5, 5, 5, 5] + + local_map: + map_sliding_en: false + half_map_size: 100 + sliding_thresh: 8.0 + + uav: + imu_rate_odom: false + gravity_align_en: true + + publish: + dense_map_en: true + pub_effect_point_en: false + pub_plane_en: false + pub_scan_num: 1 + blind_rgb_points: 0.0 + + evo: + seq_name: "CBD_Building_01" + pose_output_en: false + + pcd_save: + pcd_save_en: false + colmap_output_en: false # need to set interval = -1 + filter_size_pcd: 0.15 + interval: -1 + # how many LiDAR frames saved in each pcd file; + # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. diff --git a/config/camera_MARS_LVIG.yaml b/config/camera_MARS_LVIG.yaml index 698d648..b5ba94e 100644 --- a/config/camera_MARS_LVIG.yaml +++ b/config/camera_MARS_LVIG.yaml @@ -1,26 +1,29 @@ -cam_model: Pinhole -# HKisland HKairport -cam_width: 2448 -cam_height: 2048 -scale: 0.25 -cam_fx: 1444.431662789634 -cam_fy: 1444.343536688358 -cam_cx: 1177.801079401826 -cam_cy: 1043.601026568268 -cam_d0: -0.05729528706141188 -cam_d1: 0.1210407244166642 -cam_d2: 0.001274128378760289 -cam_d3: 0.0004389741530109464 +/**: + ros__parameters: + camera: + model: Pinhole + # HKisland HKairport + width: 2448 + height: 2048 + scale: 0.25 + fx: 1444.431662789634 + fy: 1444.343536688358 + cx: 1177.801079401826 + cy: 1043.601026568268 + d0: -0.05729528706141188 + d1: 0.1210407244166642 + d2: 0.001274128378760289 + d3: 0.0004389741530109464 -# AMtown AMvalley -# cam_width: 2448 -# cam_height: 2048 -# scale: 0.25 -# cam_fx: 1453.88 -# cam_fy: 1452.85 -# cam_cx: 1182.53 -# cam_cy: 1045.82 -# cam_d0: -0.052 -# cam_d1: 0.1168 -# cam_d2: 0.0015 -# cam_d3: 0.00013 \ No newline at end of file + # AMtown AMvalley + # width: 2448 + # height: 2048 + # scale: 0.25 + # fx: 1453.88 + # fy: 1452.85 + # cx: 1182.53 + # cy: 1045.82 + # d0: -0.052 + # d1: 0.1168 + # d2: 0.0015 + # d3: 0.00013 \ No newline at end of file diff --git a/config/camera_NTU_VIRAL.yaml b/config/camera_NTU_VIRAL.yaml index d097d8d..bf1308d 100644 --- a/config/camera_NTU_VIRAL.yaml +++ b/config/camera_NTU_VIRAL.yaml @@ -1,12 +1,14 @@ -cam_model: Pinhole -cam_width: 752 -cam_height: 480 -scale: 1.0 -cam_fx: 4.250258563372763e+02 -cam_fy: 4.267976260903337e+02 -cam_cx: 3.860151866550880e+02 -cam_cy: 2.419130336743440e+02 -cam_d0: -0.288105327549552 -cam_d1: 0.074578284234601 -cam_d2: 7.784489598138802e-04 -cam_d3: -2.277853975035461e-04 \ No newline at end of file +/**: + ros__parameters: + cam_model: Pinhole + cam_width: 752 + cam_height: 480 + scale: 1.0 + cam_fx: 4.250258563372763e+02 + cam_fy: 4.267976260903337e+02 + cam_cx: 3.860151866550880e+02 + cam_cy: 2.419130336743440e+02 + cam_d0: -0.288105327549552 + cam_d1: 0.074578284234601 + cam_d2: 7.784489598138802e-04 + cam_d3: -2.277853975035461e-04 \ No newline at end of file diff --git a/config/camera_fisheye_HILTI22.yaml b/config/camera_fisheye_HILTI22.yaml deleted file mode 100644 index c9314ee..0000000 --- a/config/camera_fisheye_HILTI22.yaml +++ /dev/null @@ -1,12 +0,0 @@ -cam_model: EquidistantCamera -cam_width: 720 -cam_height: 540 -scale: 1.0 -cam_fx: 351.31400364193297 -cam_fy: 351.4911744656785 -cam_cx: 367.8522793375995 -cam_cy: 253.8402144980996 -k1: -0.03696737352869157 -k2: -0.008917880497032812 -k3: 0.008912969593422046 -k4: -0.0037685977496087313 diff --git a/config/camera_metacamedu.yaml b/config/camera_metacamedu.yaml new file mode 100644 index 0000000..b470171 --- /dev/null +++ b/config/camera_metacamedu.yaml @@ -0,0 +1,15 @@ +/**: + ros__parameters: + camera: + model: EquidistantCamera + width: 3040 + height: 4032 + scale: 0.25 + fx: 1192.3693958329181 + fy: 1192.2974161347324 + cx: 1482.8936759196167 + cy: 1991.7309822067498 + k1: -0.009368926563676605 + k2: -0.0028549907638105074 + k3: 2.2388592029118634e-05 + k4: -0.0003622868223374491 diff --git a/config/camera_pinhole.yaml b/config/camera_pinhole.yaml index 83fd89e..4e883bf 100644 --- a/config/camera_pinhole.yaml +++ b/config/camera_pinhole.yaml @@ -1,12 +1,15 @@ -cam_model: Pinhole -cam_width: 1280 -cam_height: 1024 -scale: 0.5 -cam_fx: 1293.56944 -cam_fy: 1293.3155 -cam_cx: 626.91359 -cam_cy: 522.799224 -cam_d0: -0.076160 -cam_d1: 0.123001 -cam_d2: -0.00113 -cam_d3: 0.000251 \ No newline at end of file +/**: + ros__parameters: + camera: + model: Pinhole + width: 1280 + height: 1024 + scale: 0.5 + fx: 1293.56944 + fy: 1293.3155 + cx: 626.91359 + cy: 522.799224 + d0: -0.076160 + d1: 0.123001 + d2: -0.00113 + d3: 0.000251 \ No newline at end of file diff --git a/include/IMU_Processing.h b/include/IMU_Processing.h index fbc815e..2a045fa 100644 --- a/include/IMU_Processing.h +++ b/include/IMU_Processing.h @@ -14,12 +14,14 @@ which is included as part of this source code package. #define IMU_PROCESSING_H #include +#include #include "common_lib.h" #include -#include +#include #include -#include -const bool time_list(PointType &x, PointType &y) { return (x.curvature < y.curvature); } + +const bool time_list(PointType &x, + PointType &y); //{return (x.curvature < y.curvature);}; /// *************IMU Process and undistortion class ImuProcess @@ -31,7 +33,7 @@ class ImuProcess ~ImuProcess(); void Reset(); - void Reset(double start_timestamp, const sensor_msgs::ImuConstPtr &lastimu); + void Reset(double start_timestamp, const sensor_msgs::msg::Imu::ConstSharedPtr &lastimu); void set_extrinsic(const V3D &transl, const M3D &rot); void set_extrinsic(const V3D &transl); void set_extrinsic(const MD(4, 4) & T); @@ -68,7 +70,7 @@ class ImuProcess void IMU_init(const MeasureGroup &meas, StatesGroup &state, int &N); void Forward_without_imu(LidarMeasureGroup &meas, StatesGroup &state_inout, PointCloudXYZI &pcl_out); PointCloudXYZI pcl_wait_proc; - sensor_msgs::ImuConstPtr last_imu; + sensor_msgs::msg::Imu::ConstSharedPtr last_imu; PointCloudXYZI::Ptr cur_pcl_un_; vector IMUpose; M3D Lid_rot_to_IMU; diff --git a/include/LIVMapper.h b/include/LIVMapper.h index c810179..990beea 100644 --- a/include/LIVMapper.h +++ b/include/LIVMapper.h @@ -17,19 +17,21 @@ which is included as part of this source code package. #include "vio.h" #include "preprocess.h" #include -#include -#include +#include +#include +#include +#include #include class LIVMapper { public: - LIVMapper(ros::NodeHandle &nh); + LIVMapper(rclcpp::Node::SharedPtr &node, std::string node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); ~LIVMapper(); - void initializeSubscribersAndPublishers(ros::NodeHandle &nh, image_transport::ImageTransport &it); - void initializeComponents(); + void initializeSubscribersAndPublishers(rclcpp::Node::SharedPtr &nh, image_transport::ImageTransport &it_); + void initializeComponents(rclcpp::Node::SharedPtr &node); void initializeFiles(); - void run(); + void run(rclcpp::Node::SharedPtr &node); void gravityAlignment(); void handleFirstFrame(); void stateEstimationAndMapping(); @@ -40,27 +42,27 @@ class LIVMapper bool sync_packages(LidarMeasureGroup &meas); void prop_imu_once(StatesGroup &imu_prop_state, const double dt, V3D acc_avr, V3D angvel_avr); - void imu_prop_callback(const ros::TimerEvent &e); + void imu_prop_callback(); void transformLidar(const Eigen::Matrix3d rot, const Eigen::Vector3d t, const PointCloudXYZI::Ptr &input_cloud, PointCloudXYZI::Ptr &trans_cloud); void pointBodyToWorld(const PointType &pi, PointType &po); void RGBpointBodyToWorld(PointType const *const pi, PointType *const po); - void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg); - void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg_in); - void imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in); - void img_cbk(const sensor_msgs::ImageConstPtr &msg_in); + void standard_pcl_cbk(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); + void livox_pcl_cbk(const livox_ros_driver2::msg::CustomMsg::ConstSharedPtr &msg_in); + void imu_cbk(const sensor_msgs::msg::Imu::ConstSharedPtr &msg_in); + void img_cbk(const sensor_msgs::msg::Image::ConstSharedPtr &msg_in); void publish_img_rgb(const image_transport::Publisher &pubImage, VIOManagerPtr vio_manager); - void publish_frame_world(const ros::Publisher &pubLaserCloudFullRes, VIOManagerPtr vio_manager); - void publish_visual_sub_map(const ros::Publisher &pubSubVisualMap); - void publish_effect_world(const ros::Publisher &pubLaserCloudEffect, const std::vector &ptpl_list); - void publish_odometry(const ros::Publisher &pubOdomAftMapped); - void publish_mavros(const ros::Publisher &mavros_pose_publisher); - void publish_path(const ros::Publisher pubPath); - void readParameters(ros::NodeHandle &nh); + void publish_frame_world(const rclcpp::Publisher::SharedPtr &pubLaserCloudFullRes, VIOManagerPtr vio_manager); + void publish_visual_sub_map(const rclcpp::Publisher::SharedPtr &pubSubVisualMap); + void publish_effect_world(const rclcpp::Publisher::SharedPtr &pubLaserCloudEffect, const std::vector &ptpl_list); + void publish_odometry(const rclcpp::Publisher::SharedPtr &pmavros_pose_publisherubOdomAftMapped); + void publish_mavros(const rclcpp::Publisher::SharedPtr &mavros_pose_publisher); + void publish_path(const rclcpp::Publisher::SharedPtr &pubPath); + void readParameters(rclcpp::Node::SharedPtr &node); template void set_posestamp(T &out); template void pointBodyToWorld(const Eigen::Matrix &pi, Eigen::Matrix &po); template Eigen::Matrix pointBodyToWorld(const Eigen::Matrix &pi); - cv::Mat getImageFromMsg(const sensor_msgs::ImageConstPtr &img_msg); + cv::Mat getImageFromMsg(const sensor_msgs::msg::Image::ConstSharedPtr &img_msg); std::mutex mtx_buffer, mtx_buffer_imu_prop; std::condition_variable sig_buffer; @@ -84,18 +86,18 @@ class LIVMapper double _first_lidar_time = 0.0; double match_time = 0, solve_time = 0, solve_const_H_time = 0; - bool lidar_map_inited = false, pcd_save_en = false, pub_effect_point_en = false, pose_output_en = false, ros_driver_fix_en = false, hilti_en = false; + bool lidar_map_inited = false, pcd_save_en = false, pub_effect_point_en = false, pose_output_en = false, ros_driver_fix_en = false; int pcd_save_interval = -1, pcd_index = 0; int pub_scan_num = 1; StatesGroup imu_propagate, latest_ekf_state; bool new_imu = false, state_update_flg = false, imu_prop_enable = true, ekf_finish_once = false; - deque prop_imu_buffer; - sensor_msgs::Imu newest_imu; + deque prop_imu_buffer; + sensor_msgs::msg::Imu newest_imu; double latest_ekf_time; - nav_msgs::Odometry imu_prop_odom; - ros::Publisher pubImuPropOdom; + nav_msgs::msg::Odometry imu_prop_odom; + rclcpp::Publisher::SharedPtr pubImuPropOdom; double imu_time_offset = 0.0; double lidar_time_offset = 0.0; @@ -114,13 +116,13 @@ class LIVMapper int lidar_en = 1; bool is_first_frame = false; int grid_size, patch_size, grid_n_width, grid_n_height, patch_pyrimid_level; - double outlier_threshold; + int outlier_threshold; double plot_time; int frame_cnt; double img_time_offset = 0.0; deque lid_raw_data_buffer; deque lid_header_time_buffer; - deque imu_buffer; + deque imu_buffer; deque img_buffer; deque img_time_buffer; vector _pv_list; @@ -128,7 +130,7 @@ class LIVMapper vector extrinR; vector cameraextrinT; vector cameraextrinR; - double IMG_POINT_COV; + int IMG_POINT_COV; PointCloudXYZI::Ptr visual_sub_map; PointCloudXYZI::Ptr feats_undistort; @@ -149,39 +151,45 @@ class LIVMapper StatesGroup _state; StatesGroup state_propagat; - nav_msgs::Path path; - nav_msgs::Odometry odomAftMapped; - geometry_msgs::Quaternion geoQuat; - geometry_msgs::PoseStamped msg_body_pose; + nav_msgs::msg::Path path; + nav_msgs::msg::Odometry odomAftMapped; + geometry_msgs::msg::Quaternion geoQuat; + geometry_msgs::msg::PoseStamped msg_body_pose; PreprocessPtr p_pre; ImuProcessPtr p_imu; VoxelMapManagerPtr voxelmap_manager; VIOManagerPtr vio_manager; - ros::Publisher plane_pub; - ros::Publisher voxel_pub; - ros::Subscriber sub_pcl; - ros::Subscriber sub_imu; - ros::Subscriber sub_img; - ros::Publisher pubLaserCloudFullRes; - ros::Publisher pubNormal; - ros::Publisher pubSubVisualMap; - ros::Publisher pubLaserCloudEffect; - ros::Publisher pubLaserCloudMap; - ros::Publisher pubOdomAftMapped; - ros::Publisher pubPath; - ros::Publisher pubLaserCloudDyn; - ros::Publisher pubLaserCloudDynRmed; - ros::Publisher pubLaserCloudDynDbg; + rclcpp::Publisher::SharedPtr plane_pub; + rclcpp::Publisher::SharedPtr voxel_pub; + std::shared_ptr sub_pcl; + rclcpp::Subscription::SharedPtr sub_imu; + rclcpp::Subscription::SharedPtr sub_img; + // Compressed image subscriber + rclcpp::Subscription::SharedPtr sub_img_compressed; + rclcpp::Publisher::SharedPtr pubLaserCloudFullRes; + rclcpp::Publisher::SharedPtr pubNormal; + rclcpp::Publisher::SharedPtr pubSubVisualMap; + rclcpp::Publisher::SharedPtr pubLaserCloudEffect; + rclcpp::Publisher::SharedPtr pubLaserCloudMap; + rclcpp::Publisher::SharedPtr pubOdomAftMapped; + rclcpp::Publisher::SharedPtr pubPath; + rclcpp::Publisher::SharedPtr pubLaserCloudDyn; + rclcpp::Publisher::SharedPtr pubLaserCloudDynRmed; + rclcpp::Publisher::SharedPtr pubLaserCloudDynDbg; image_transport::Publisher pubImage; - ros::Publisher mavros_pose_publisher; - ros::Timer imu_prop_timer; + rclcpp::Publisher::SharedPtr mavros_pose_publisher; + rclcpp::TimerBase::SharedPtr imu_prop_timer; + rclcpp::Node::SharedPtr node; int frame_num = 0; double aver_time_consu = 0; double aver_time_icp = 0; double aver_time_map_inre = 0; bool colmap_output_en = false; + bool enable_image_processing = false; + // JPEG decoding callback function + void jpeg_callback(const sensor_msgs::msg::CompressedImage::SharedPtr msg); }; #endif \ No newline at end of file diff --git a/include/common_lib.h b/include/common_lib.h index 9de8a90..fa1ed2a 100755 --- a/include/common_lib.h +++ b/include/common_lib.h @@ -16,13 +16,16 @@ which is included as part of this source code package. #include #include #include +#include #include -#include -#include -#include +#include +#include +#include +#include +#include using namespace std; -using namespace Eigen; +// using namespace Eigen; // avoid cmake error: reference to โ€˜Matrixโ€™ is ambiguous using namespace Sophus; #define print_line std::cout << __FILE__ << ", " << __LINE__ << std::endl; @@ -43,7 +46,7 @@ enum LID_TYPE L515 = 4, XT32 = 5, PANDAR128 = 6, - ROBOSENSE = 7 + MID360 = 7 }; enum SLAM_MODE { @@ -63,7 +66,7 @@ struct MeasureGroup { double vio_time; double lio_time; - deque imu; + deque imu; cv::Mat img; MeasureGroup() { diff --git a/include/feature.h b/include/feature.h index 08a17f8..a94eea7 100644 --- a/include/feature.h +++ b/include/feature.h @@ -33,13 +33,13 @@ struct Feature int level_; //!< Image pyramid level where patch feature was extracted. VisualPoint *point_; //!< Pointer to 3D point which corresponds to the patch feature. Vector2d grad_; //!< Dominant gradient direction for edglets, normalized. - SE3 T_f_w_; //!< Pose of the frame where the patch feature was extracted. + SE3 T_f_w_; //!< Pose of the frame where the patch feature was extracted. float *patch_; //!< Pointer to the image patch data. float score_; //!< Score of the patch feature. float mean_; //!< Mean intensity of the image patch feature, used for normalization. double inv_expo_time_; //!< Inverse exposure time of the image where the patch feature was extracted. - Feature(VisualPoint *_point, float *_patch, const Vector2d &_px, const Vector3d &_f, const SE3 &_T_f_w, int _level) + Feature(VisualPoint *_point, float *_patch, const Vector2d &_px, const Vector3d &_f, const SE3 &_T_f_w, int _level) : type_(CORNER), px_(_px), f_(_f), T_f_w_(_T_f_w), mean_(0), score_(0), level_(_level), patch_(_patch), point_(_point) { } diff --git a/include/frame.h b/include/frame.h index 8172dcd..ea8a534 100644 --- a/include/frame.h +++ b/include/frame.h @@ -31,8 +31,8 @@ class Frame : boost::noncopyable static int frame_counter_; //!< Counts the number of created frames. Used to set the unique id. int id_; //!< Unique id of the frame. vk::AbstractCamera *cam_; //!< Camera model. - SE3 T_f_w_; //!< Transform (f)rame from (w)orld. - SE3 T_f_w_prior_; //!< Transform (f)rame from (w)orld provided by the IMU prior. + SE3 T_f_w_; //!< Transform (f)rame from (w)orld. + SE3 T_f_w_prior_; //!< Transform (f)rame from (w)orld provided by the IMU prior. cv::Mat img_; //!< Image of the frame. Features fts_; //!< List of features in the image. diff --git a/include/preprocess.h b/include/preprocess.h index bf74f60..501fc32 100755 --- a/include/preprocess.h +++ b/include/preprocess.h @@ -14,7 +14,7 @@ which is included as part of this source code package. #define PREPROCESS_H_ #include "common_lib.h" -#include +#include #include using namespace std; @@ -70,13 +70,13 @@ struct EIGEN_ALIGN16 Point { PCL_ADD_POINT4D; float intensity; - float time; + std::uint32_t t; std::uint16_t ring; EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace velodyne_ros POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point, - (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(float, time, time)(std::uint16_t, ring, ring)) + (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint32_t, t, t)(std::uint16_t, ring, ring)) /****************/ /*** Ouster ***/ @@ -121,32 +121,30 @@ namespace Pandar128_ros struct EIGEN_ALIGN16 Point { PCL_ADD_POINT4D; - uint8_t intensity; - double timestamp; - uint16_t ring; + float timestamp; + uint8_t ring; EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace Pandar128_ros POINT_CLOUD_REGISTER_POINT_STRUCT(Pandar128_ros::Point, - (float, x, x)(float, y, y)(float, z, z)(std::uint8_t, intensity, intensity)(double, timestamp, timestamp)(std::uint16_t, ring, ring)) + (float, x, x)(float, y, y)(float, z, z)(float, timestamp, timestamp)) /*****************/ -/*** Robosense_Airy ***/ -namespace robosense_ros -{ -struct EIGEN_ALIGN16 Point -{ +/*** LIVOX_MID-360 ***/ +namespace mid360_ros { +struct EIGEN_ALIGN16 Point { PCL_ADD_POINT4D; float intensity; + std::uint8_t tag; + std::uint8_t line; double timestamp; - uint16_t ring; EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} // namespace robosense_ros -POINT_CLOUD_REGISTER_POINT_STRUCT(robosense_ros::Point, - (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(double, timestamp, timestamp)(std::uint16_t, ring, ring)) + } // namespace mid360_ros + POINT_CLOUD_REGISTER_POINT_STRUCT(mid360_ros::Point, + (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity) + (std::uint8_t, tag, tag)(std::uint8_t, line, line)(double, timestamp, timestamp)) /*****************/ - class Preprocess { public: @@ -155,11 +153,11 @@ class Preprocess Preprocess(); ~Preprocess(); - void process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out); - void process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out); + void process(const livox_ros_driver2::msg::CustomMsg::SharedPtr &msg, PointCloudXYZI::Ptr &pcl_out); + void process(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg, PointCloudXYZI::Ptr &pcl_out); void set(bool feat_en, int lid_type, double bld, int pfilt_num); - // sensor_msgs::PointCloud2::ConstPtr pointcloud; + // sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud; PointCloudXYZI pl_full, pl_corn, pl_surf; PointCloudXYZI pl_buff[128]; // maximum 128 line lidar vector typess[128]; // maximum 128 line lidar @@ -167,18 +165,20 @@ class Preprocess double blind, blind_sqr; bool feature_enabled, given_offset_time; - ros::Publisher pub_full, pub_surf, pub_corn; + std::shared_ptr> pub_full; + std::shared_ptr> pub_surf; + std::shared_ptr> pub_corn; private: - void avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg); - void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); - void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); - void xt32_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); - void Pandar128_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); - void robosense_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); - void l515_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); + void avia_handler(const livox_ros_driver2::msg::CustomMsg::SharedPtr &msg); + void oust64_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); + void velodyne_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); + void mid360_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); + void xt32_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); + void Pandar128_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); + void l515_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); void give_feature(PointCloudXYZI &pl, vector &types); - void pub_func(PointCloudXYZI &pl, const ros::Time &ct); + void pub_func(PointCloudXYZI &pl, const rclcpp::Time &ct); int plane_judge(const PointCloudXYZI &pl, vector &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct); bool small_plane(const PointCloudXYZI &pl, vector &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct); bool edge_jump_judge(const PointCloudXYZI &pl, vector &types, uint i, Surround nor_dir); diff --git a/include/utils/utils.h b/include/utils/utils.h new file mode 100644 index 0000000..9555f70 --- /dev/null +++ b/include/utils/utils.h @@ -0,0 +1,76 @@ +#ifndef UTILS_H +#define UTILS_H + +#include +#include // for int64_t +#include // for std::numeric_limits +#include // for std::out_of_range +#include +#include +#include +#include +#include +#include + +std::vector convertToIntVectorSafe(const std::vector& int64_vector); + +inline double stamp2Sec(const builtin_interfaces::msg::Time& stamp) +{ + return rclcpp::Time(stamp).seconds(); +} + +inline rclcpp::Time sec2Stamp(double timestamp) +{ + int32_t sec = std::floor(timestamp); + auto nanosec_d = (timestamp - std::floor(timestamp)) * 1e9; + uint32_t nanosec = nanosec_d; + return rclcpp::Time(sec, nanosec); +} + +namespace tf +{ + +inline geometry_msgs::msg::Quaternion createQuaternionMsgFromYaw(double yaw) +{ + tf2::Quaternion q; + q.setRPY(0, 0, yaw); + return tf2::toMsg(q); +} + +inline geometry_msgs::msg::Quaternion createQuaternionMsgFromRollPitchYaw(double roll, double pitch, double yaw) +{ + tf2::Quaternion q; + q.setRPY(roll, pitch, yaw); + return tf2::toMsg(q); +} + +inline tf2::Quaternion createQuaternionFromYaw(double yaw) +{ + tf2::Quaternion q; + q.setRPY(0, 0, yaw); + return q; +} + +inline tf2::Quaternion createQuaternionFromRPY(double roll, double pitch, double yaw) +{ + tf2::Quaternion q; + q.setRPY(roll, pitch, yaw); + return q; +} +} + +inline geometry_msgs::msg::TransformStamped createTransformStamped( + const tf2::Transform &transform, + const builtin_interfaces::msg::Time &stamp, + const std::string &frame_id, + const std::string &child_frame_id) +{ + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped.header.stamp = stamp; + transform_stamped.header.frame_id = frame_id; + transform_stamped.child_frame_id = child_frame_id; + transform_stamped.transform = tf2::toMsg(transform); + return transform_stamped; +} + +#endif // UTILS_H \ No newline at end of file diff --git a/include/vio.h b/include/vio.h index d5600b1..32cb213 100755 --- a/include/vio.h +++ b/include/vio.h @@ -21,7 +21,7 @@ which is included as part of this source code package. #include #include #include -#include +#include struct SubSparseMap { @@ -85,7 +85,8 @@ class VIOManager public: int grid_size; vk::AbstractCamera *cam; - vk::PinholeCamera *pinhole_cam; + vk::EquidistantCamera *equidistant_cam; + // vk::PinholeCamera *pinhole_cam; StatesGroup *state; StatesGroup *state_propagat; M3D Rli, Rci, Rcl, Rcw, Jdphi_dR, Jdp_dt, Jdp_dR; @@ -119,8 +120,8 @@ class VIOManager int frame_count = 0; bool plot_flag; - Matrix G, H_T_H; - MatrixXd K, H_sub_inv; + Eigen::Matrix G, H_T_H; + Eigen::MatrixXd K, H_sub_inv; ofstream fout_camera, fout_colmap; unordered_map feat_map; @@ -153,11 +154,11 @@ class VIOManager void computeJacobianAndUpdateEKF(cv::Mat img); void resetGrid(); void updateVisualMapPoints(cv::Mat img); - void getWarpMatrixAffine(const vk::AbstractCamera &cam, const Vector2d &px_ref, const Vector3d &f_ref, const double depth_ref, const SE3 &T_cur_ref, + void getWarpMatrixAffine(const vk::AbstractCamera &cam, const Vector2d &px_ref, const Vector3d &f_ref, const double depth_ref, const SE3 &T_cur_ref, const int level_ref, const int pyramid_level, const int halfpatch_size, Matrix2d &A_cur_ref); void getWarpMatrixAffineHomography(const vk::AbstractCamera &cam, const V2D &px_ref, - const V3D &xyz_ref, const V3D &normal_ref, const SE3 &T_cur_ref, const int level_ref, Matrix2d &A_cur_ref); + const V3D &xyz_ref, const V3D &normal_ref, const SE3 &T_cur_ref, const int level_ref, Matrix2d &A_cur_ref); void warpAffine(const Matrix2d &A_cur_ref, const cv::Mat &img_ref, const Vector2d &px_ref, const int level_ref, const int search_level, const int pyramid_level, const int halfpatch_size, float *patch); void insertPointIntoVoxelMap(VisualPoint *pt_new); diff --git a/include/voxel_map.h b/include/voxel_map.h index 21afaa3..2f28d80 100644 --- a/include/voxel_map.h +++ b/include/voxel_map.h @@ -20,12 +20,12 @@ which is included as part of this source code package. #include #include #include -#include +#include #include #include #include -#include -#include +#include +#include #define VOXELMAP_HASH_P 116101 #define VOXELMAP_MAX_N 10000000000 @@ -37,7 +37,7 @@ typedef struct VoxelMapConfig double max_voxel_size_; int max_layer_; int max_iterations_; - std::vector layer_init_num_; + std::vector layer_init_num_; int max_points_num_; double planner_threshold_; double beam_err_; @@ -182,7 +182,7 @@ class VoxelOctoTree VoxelOctoTree *Insert(const pointWithVar &pv); }; -void loadVoxelConfig(ros::NodeHandle &nh, VoxelMapConfig &voxel_config); +void loadVoxelConfig(rclcpp::Node::SharedPtr &node, VoxelMapConfig &voxel_config); class VoxelMapManager { @@ -190,7 +190,7 @@ class VoxelMapManager VoxelMapManager() = default; VoxelMapConfig config_setting_; int current_frame_id_ = 0; - ros::Publisher voxel_map_pub_; + rclcpp::Publisher::SharedPtr voxel_map_pub_; std::unordered_map voxel_map_; PointCloudXYZI::Ptr feats_undistort_; @@ -208,7 +208,7 @@ class VoxelMapManager V3D last_slide_position = {0,0,0}; - geometry_msgs::Quaternion geoQuat_; + geometry_msgs::msg::Quaternion geoQuat_; int feats_down_size_; int effct_feat_num_; @@ -248,9 +248,9 @@ class VoxelMapManager private: void GetUpdatePlane(const VoxelOctoTree *current_octo, const int pub_max_voxel_layer, std::vector &plane_list); - void pubSinglePlane(visualization_msgs::MarkerArray &plane_pub, const std::string plane_ns, const VoxelPlane &single_plane, const float alpha, + void pubSinglePlane(visualization_msgs::msg::MarkerArray &plane_pub, const std::string plane_ns, const VoxelPlane &single_plane, const float alpha, const Eigen::Vector3d rgb); - void CalcVectQuation(const Eigen::Vector3d &x_vec, const Eigen::Vector3d &y_vec, const Eigen::Vector3d &z_vec, geometry_msgs::Quaternion &q); + void CalcVectQuation(const Eigen::Vector3d &x_vec, const Eigen::Vector3d &y_vec, const Eigen::Vector3d &z_vec, geometry_msgs::msg::Quaternion &q); void mapJet(double v, double vmin, double vmax, uint8_t &r, uint8_t &g, uint8_t &b); }; diff --git a/launch/mapping_avia.launch b/launch/mapping_avia.launch deleted file mode 100755 index 1e5c360..0000000 --- a/launch/mapping_avia.launch +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - -launch-prefix="gdb -ex run --args" -launch-prefix="valgrind --leak-check=full" - diff --git a/launch/mapping_avia_marslvig.launch b/launch/mapping_avia_marslvig.launch deleted file mode 100755 index 2fa90d3..0000000 --- a/launch/mapping_avia_marslvig.launch +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - -launch-prefix="gdb -ex run --args" -launch-prefix="valgrind --leak-check=full" - diff --git a/launch/mapping_avia_marslvig.launch.py b/launch/mapping_avia_marslvig.launch.py new file mode 100755 index 0000000..7e78750 --- /dev/null +++ b/launch/mapping_avia_marslvig.launch.py @@ -0,0 +1,111 @@ +#!/usr/bin/python3 +# -- coding: utf-8 --** + +import os +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node + +def generate_launch_description(): + + # Find path + config_file_dir = os.path.join(get_package_share_directory("fast_livo"), "config") + rviz_config_file = os.path.join(get_package_share_directory("fast_livo"), "rviz_cfg", "M300.rviz") + + #Load parameters + avia_config_cmd = os.path.join(config_file_dir, "MARS_LVIG.yaml") + camera_config_cmd = os.path.join(config_file_dir, "camera_MARS_LVIG.yaml") + + # Param use_rviz + use_rviz_arg = DeclareLaunchArgument( + "use_rviz", + default_value="False", + description="Whether to launch Rviz2", + ) + + avia_config_arg = DeclareLaunchArgument( + 'avia_params_file', + default_value=avia_config_cmd, + description='Full path to the ROS2 parameters file to use for fast_livo2 nodes', + ) + + camera_config_arg = DeclareLaunchArgument( + 'camera_params_file', + default_value=camera_config_cmd, + description='Full path to the ROS2 parameters file to use for vikit_ros nodes', + ) + + # https://github.com/ros-navigation/navigation2/blob/1c68c212db01f9f75fcb8263a0fbb5dfa711bdea/nav2_bringup/launch/navigation_launch.py#L40 + use_respawn_arg = DeclareLaunchArgument( + 'use_respawn', + default_value='True', + description='Whether to respawn if a node crashes. Applied when composition is disabled.') + + avia_params_file = LaunchConfiguration('avia_params_file') + camera_params_file = LaunchConfiguration('camera_params_file') + use_respawn = LaunchConfiguration('use_respawn') + + return LaunchDescription([ + use_rviz_arg, + avia_config_arg, + camera_config_arg, + use_respawn_arg, + + # use parameter_blackboard as global parameters server and load camera params + Node( + package='demo_nodes_cpp', + executable='parameter_blackboard', + name='parameter_blackboard', + # namespace='laserMapping', + parameters=[ + camera_params_file, + ], + output='screen' + ), + + # republish compressed image to raw image + # https://robotics.stackexchange.com/questions/110939/how-do-i-remap-compressed-video-to-raw-video-in-ros2 + # ros2 run image_transport republish compressed raw --ros-args --remap in:=/left_camera/image --remap out:=/left_camera/image + Node( + package="image_transport", + executable="republish", + name="republish", + arguments=[ # Array of strings/parametric arguments that will end up in process's argv + 'compressed', + 'raw', + ], + remappings=[ + ("in", "/left_camera/image"), + ("out", "/left_camera/image") + ], + output="screen", + respawn=use_respawn, + ), + + Node( + package="fast_livo", + executable="fastlivo_mapping", + name="laserMapping", + parameters=[ + avia_params_file, + ], + # https://docs.ros.org/en/humble/How-To-Guides/Getting-Backtraces-in-ROS-2.html + prefix=[ + # ("gdb -ex run --args"), + # ("valgrind --log-file=./valgrind_report.log --tool=memcheck --leak-check=full --show-leak-kinds=all -s --track-origins=yes --show-reachable=yes --undef-value-errors=yes --track-fds=yes") + ], + output="screen" + ), + + Node( + condition=IfCondition(LaunchConfiguration("use_rviz")), + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=["-d", rviz_config_file], + output="screen" + ), + ]) diff --git a/launch/mapping_aviz.launch.py b/launch/mapping_aviz.launch.py new file mode 100644 index 0000000..ab76830 --- /dev/null +++ b/launch/mapping_aviz.launch.py @@ -0,0 +1,106 @@ +#!/usr/bin/python3 +# -- coding: utf-8 --** + +import os +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node + +def generate_launch_description(): + + # Find path + config_file_dir = os.path.join(get_package_share_directory("fast_livo"), "config") + rviz_config_file = os.path.join(get_package_share_directory("fast_livo"), "rviz_cfg", "fast_livo2.rviz") + + #Load parameters + avia_config_cmd = os.path.join(config_file_dir, "avia.yaml") + camera_config_cmd = os.path.join(config_file_dir, "camera_pinhole.yaml") + + # Param use_rviz + use_rviz_arg = DeclareLaunchArgument( + "use_rviz", + default_value="False", + description="Whether to launch Rviz2", + ) + + avia_config_arg = DeclareLaunchArgument( + 'avia_params_file', + default_value=avia_config_cmd, + description='Full path to the ROS2 parameters file to use for fast_livo2 nodes', + ) + + camera_config_arg = DeclareLaunchArgument( + 'camera_params_file', + default_value=camera_config_cmd, + description='Full path to the ROS2 parameters file to use for vikit_ros nodes', + ) + + # https://github.com/ros-navigation/navigation2/blob/1c68c212db01f9f75fcb8263a0fbb5dfa711bdea/nav2_bringup/launch/navigation_launch.py#L40 + use_respawn_arg = DeclareLaunchArgument( + 'use_respawn', + default_value='True', + description='Whether to respawn if a node crashes. Applied when composition is disabled.') + + avia_params_file = LaunchConfiguration('avia_params_file') + camera_params_file = LaunchConfiguration('camera_params_file') + use_respawn = LaunchConfiguration('use_respawn') + + return LaunchDescription([ + use_rviz_arg, + avia_config_arg, + camera_config_arg, + use_respawn_arg, + + # play ros2 bag + # ExecuteProcess( + # cmd=[['ros2 bag play ', '~/datasets/Retail_Street ', '--clock ', "-l"]], + # shell=True + # ), + + # republish compressed image to raw image + # https://robotics.stackexchange.com/questions/110939/how-do-i-remap-compressed-video-to-raw-video-in-ros2 + # ros2 run image_transport republish compressed raw --ros-args --remap in:=/left_camera/image --remap out:=/left_camera/image + Node( + package="image_transport", + executable="republish", + name="republish", + arguments=[ # Array of strings/parametric arguments that will end up in process's argv + 'compressed', + 'raw', + ], + remappings=[ + ("in", "/left_camera/image"), + ("out", "/left_camera/image") + ], + output="screen", + respawn=use_respawn, + ), + + Node( + package="fast_livo", + executable="fastlivo_mapping", + name="laserMapping", + parameters=[ + avia_params_file, + camera_params_file, + ], + # https://docs.ros.org/en/humble/How-To-Guides/Getting-Backtraces-in-ROS-2.html + prefix=[ + # ("gdb -ex run --args"), + # ("valgrind --log-file=./valgrind_report.log --tool=memcheck --leak-check=full --show-leak-kinds=all -s --track-origins=yes --show-reachable=yes --undef-value-errors=yes --track-fds=yes") + ], + output="screen" + ), + + Node( + condition=IfCondition(LaunchConfiguration("use_rviz")), + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=["-d", rviz_config_file], + output="screen" + ), + ]) diff --git a/launch/mapping_aviz_metacamedu.launch.py b/launch/mapping_aviz_metacamedu.launch.py new file mode 100644 index 0000000..39adbb1 --- /dev/null +++ b/launch/mapping_aviz_metacamedu.launch.py @@ -0,0 +1,95 @@ +#!/usr/bin/python3 +# -- coding: utf-8 --** + +import os +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node + +def generate_launch_description(): + + # Find path + config_file_dir = os.path.join(get_package_share_directory("fast_livo"), "config") + rviz_config_file = os.path.join(get_package_share_directory("fast_livo"), "rviz_cfg", "fast_livo2.rviz") + + #Load parameters + avia_config_cmd = os.path.join(config_file_dir, "avia_metacamedu.yaml") + camera_config_cmd = os.path.join(config_file_dir, "camera_metacamedu.yaml") + + # Param use_rviz + use_rviz_arg = DeclareLaunchArgument( + "use_rviz", + default_value="False", + description="Whether to launch Rviz2", + ) + + avia_config_arg = DeclareLaunchArgument( + 'avia_params_file', + default_value=avia_config_cmd, + description='Full path to the ROS2 parameters file to use for fast_livo2 nodes', + ) + + camera_config_arg = DeclareLaunchArgument( + 'camera_params_file', + default_value=camera_config_cmd, + description='Full path to the ROS2 parameters file to use for vikit_ros nodes', + ) + + # https://github.com/ros-navigation/navigation2/blob/1c68c212db01f9f75fcb8263a0fbb5dfa711bdea/nav2_bringup/launch/navigation_launch.py#L40 + use_respawn_arg = DeclareLaunchArgument( + 'use_respawn', + default_value='True', + description='Whether to respawn if a node crashes. Applied when composition is disabled.') + + avia_params_file = LaunchConfiguration('avia_params_file') + camera_params_file = LaunchConfiguration('camera_params_file') + use_respawn = LaunchConfiguration('use_respawn') + + return LaunchDescription([ + use_rviz_arg, + avia_config_arg, + camera_config_arg, + use_respawn_arg, + + # play ros2 bag + # ExecuteProcess( + # cmd=[['ros2 bag play ', '~/datasets/Retail_Street ', '--clock ', "-l"]], + # shell=True + # ), + + # Node( + # package="image_decoder", + # executable="jpeg_decoder_node", + # name="jpeg_decoder", + # output="screen", + # respawn=use_respawn, + # ), + + Node( + package="fast_livo", + executable="fastlivo_mapping", + name="laserMapping", + parameters=[ + avia_params_file, + camera_params_file, + ], + # https://docs.ros.org/en/humble/How-To-Guides/Getting-Backtraces-in-ROS-2.html + prefix=[ + # ("gdb -ex run --args"), + # ("valgrind --log-file=./valgrind_report.log --tool=memcheck --leak-check=full --show-leak-kinds=all -s --track-origins=yes --show-reachable=yes --undef-value-errors=yes --track-fds=yes") + ], + output="screen" + ), + + Node( + condition=IfCondition(LaunchConfiguration("use_rviz")), + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=["-d", rviz_config_file], + output="screen" + ), + ]) diff --git a/launch/mapping_hesaixt32_hilti22.launch b/launch/mapping_hesaixt32_hilti22.launch deleted file mode 100644 index 59257b0..0000000 --- a/launch/mapping_hesaixt32_hilti22.launch +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - - diff --git a/launch/mapping_ouster_ntu.launch b/launch/mapping_ouster_ntu.launch deleted file mode 100755 index a0530d9..0000000 --- a/launch/mapping_ouster_ntu.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/launch/mapping_ouster_ntu.launch.py b/launch/mapping_ouster_ntu.launch.py new file mode 100644 index 0000000..1b1328f --- /dev/null +++ b/launch/mapping_ouster_ntu.launch.py @@ -0,0 +1,117 @@ +#!/usr/bin/python3 +# -- coding: utf-8 --** + +import os +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node + +def generate_launch_description(): + + # Find path + config_file_dir = os.path.join(get_package_share_directory("fast_livo"), "config") + rviz_config_file = os.path.join(get_package_share_directory("fast_livo"), "rviz_cfg", "ntu_viral.rviz") + + #Load parameters + avia_config_cmd = os.path.join(config_file_dir, "NTU_VIRAL.yaml") + camera_config_cmd = os.path.join(config_file_dir, "camera_NTU_VIRAL.yaml") + + # Param use_rviz + use_rviz_arg = DeclareLaunchArgument( + "use_rviz", + default_value="False", + description="Whether to launch Rviz2", + ) + + avia_config_arg = DeclareLaunchArgument( + 'avia_params_file', + default_value=avia_config_cmd, + description='Full path to the ROS2 parameters file to use for fast_livo2 nodes', + ) + + camera_config_arg = DeclareLaunchArgument( + 'camera_params_file', + default_value=camera_config_cmd, + description='Full path to the ROS2 parameters file to use for vikit_ros nodes', + ) + + # https://github.com/ros-navigation/navigation2/blob/1c68c212db01f9f75fcb8263a0fbb5dfa711bdea/nav2_bringup/launch/navigation_launch.py#L40 + use_respawn_arg = DeclareLaunchArgument( + 'use_respawn', + default_value='True', + description='Whether to respawn if a node crashes. Applied when composition is disabled.') + + avia_params_file = LaunchConfiguration('avia_params_file') + camera_params_file = LaunchConfiguration('camera_params_file') + use_respawn = LaunchConfiguration('use_respawn') + + return LaunchDescription([ + use_rviz_arg, + avia_config_arg, + camera_config_arg, + use_respawn_arg, + + # play ros2 bag + # ExecuteProcess( + # cmd=[['ros2 bag play ', '~/datasets/Retail_Street ', '--clock ', "-l"]], + # shell=True + # ), + + # use parameter_blackboard as global parameters server and load camera params + Node( + package='demo_nodes_cpp', + executable='parameter_blackboard', + name='parameter_blackboard', + # namespace='laserMapping', + parameters=[ + camera_params_file, + ], + output='screen' + ), + + # republish compressed image to raw image + # https://robotics.stackexchange.com/questions/110939/how-do-i-remap-compressed-video-to-raw-video-in-ros2 + # ros2 run image_transport republish compressed raw --ros-args --remap in:=/left_camera/image --remap out:=/left_camera/image + Node( + package="image_transport", + executable="republish", + name="republish", + arguments=[ # Array of strings/parametric arguments that will end up in process's argv + 'compressed', + 'raw', + ], + remappings=[ + ("in", "/left_camera/image"), + ("out", "/left_camera/image") + ], + output="screen", + respawn=use_respawn, + ), + + Node( + package="fast_livo", + executable="fastlivo_mapping", + name="laserMapping", + parameters=[ + avia_params_file, + ], + # https://docs.ros.org/en/humble/How-To-Guides/Getting-Backtraces-in-ROS-2.html + prefix=[ + # ("gdb -ex run --args"), + # ("valgrind --log-file=./valgrind_report.log --tool=memcheck --leak-check=full --show-leak-kinds=all -s --track-origins=yes --show-reachable=yes --undef-value-errors=yes --track-fds=yes") + ], + output="screen" + ), + + Node( + condition=IfCondition(LaunchConfiguration("use_rviz")), + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=["-d", rviz_config_file], + output="screen" + ), + ]) diff --git a/package.xml b/package.xml index c2eedfa..84a4020 100755 --- a/package.xml +++ b/package.xml @@ -1,5 +1,5 @@ - + fast_livo 0.0.0 @@ -16,38 +16,39 @@ Ji Zhang - catkin + ament_cmake + + rclcpp + rclpy + sensor_msgs geometry_msgs + visualization_msgs nav_msgs - roscpp - rospy std_msgs - sensor_msgs - tf + tf2_ros pcl_ros - message_generation + pcl_conversions + livox_ros_driver2 vikit_common vikit_ros cv_bridge + image_transport + libopencv-dev + + cv_bridge + image_transport + libopencv-dev + sensor_msgs + std_msgs + + rosidl_interface_packages - geometry_msgs - nav_msgs - sensor_msgs - roscpp - rospy - std_msgs - tf - pcl_ros - message_runtime - vikit_common - vikit_ros - cv_bridge - image_transport - image_transport - - rostest - rosbag + ament_lint_auto + ament_lint_common + ament_cmake + + diff --git a/pics/debug_error.png b/pics/debug_error.png new file mode 100644 index 0000000..3baa521 Binary files /dev/null and b/pics/debug_error.png differ diff --git a/pics/rosgraph.png b/pics/rosgraph.png new file mode 100644 index 0000000..26ba96f Binary files /dev/null and b/pics/rosgraph.png differ diff --git a/rviz_cfg/M300.rviz b/rviz_cfg/M300.rviz index a504963..84a33a6 100755 --- a/rviz_cfg/M300.rviz +++ b/rviz_cfg/M300.rviz @@ -1,5 +1,5 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 0 Name: Displays Property Tree Widget: @@ -25,21 +25,21 @@ Panels: - /Image1 Splitter Ratio: 0.34272301197052 Tree Height: 538 - - Class: rviz/Selection + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: rviz_common/Time Name: Time SyncMode: 0 SyncSource: surround @@ -52,7 +52,7 @@ Visualization Manager: Displays: - Alpha: 1 Cell Size: 1 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: false Line Style: @@ -69,7 +69,7 @@ Visualization Manager: Reference Frame: Value: false - Alpha: 1 - Class: rviz/Axes + Class: rviz_default_plugins/Axes Enabled: true Length: 4 Name: Axes @@ -77,7 +77,7 @@ Visualization Manager: Reference Frame: Show Trail: false Value: true - - Class: rviz/Group + - Class: rviz_common/Group Displays: - Alpha: 1 Autocompute Intensity Bounds: true @@ -87,7 +87,7 @@ Visualization Manager: Value: false Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 199; 228; 247 Color Transformer: FlatColor Decay Time: 0 @@ -115,7 +115,7 @@ Visualization Manager: Value: false Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 238; 238; 236 Color Transformer: RGB8 Decay Time: 10000 @@ -145,7 +145,7 @@ Visualization Manager: Value: true Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 239; 41; 41 Color Transformer: "" Decay Time: 0 @@ -167,10 +167,10 @@ Visualization Manager: Value: false Enabled: true Name: mapping - - Class: rviz/Group + - Class: rviz_common/Group Displays: - Angle Tolerance: 0.009999999776482582 - Class: rviz/Odometry + Class: rviz_default_plugins/Odometry Covariance: Orientation: Alpha: 0.5 @@ -208,7 +208,7 @@ Visualization Manager: Name: Odometry - Alpha: 0 Buffer Length: 2 - Class: rviz/Path + Class: rviz_default_plugins/Path Color: 25; 255; 255 Enabled: true Head Diameter: 0 @@ -238,7 +238,7 @@ Visualization Manager: Value: false Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 239; 41; 41 Color Transformer: Intensity Decay Time: 1000 @@ -258,7 +258,7 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: false - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /planner_normal Name: Marker @@ -266,7 +266,7 @@ Visualization Manager: {} Queue Size: 100 Value: true - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /voxels Name: MarkerArray @@ -282,7 +282,7 @@ Visualization Manager: Value: true Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 245; 121; 0 Color Transformer: FlatColor Decay Time: 0 @@ -302,7 +302,7 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: false - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /visualization_marker Name: MarkerArray @@ -318,7 +318,7 @@ Visualization Manager: Value: true Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 92; 53; 102 Color Transformer: FlatColor Decay Time: 99999 @@ -346,7 +346,7 @@ Visualization Manager: Value: true Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 115; 210; 22 Color Transformer: FlatColor Decay Time: 0 @@ -374,7 +374,7 @@ Visualization Manager: Value: false Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 237; 212; 0 Color Transformer: FlatColor Decay Time: 0 @@ -402,7 +402,7 @@ Visualization Manager: Value: true Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 239; 41; 41 Color Transformer: FlatColor Decay Time: 99999 @@ -430,7 +430,7 @@ Visualization Manager: Value: true Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 92; 53; 102 Color Transformer: FlatColor Decay Time: 0 @@ -451,7 +451,7 @@ Visualization Manager: Use rainbow: true Value: false - Angle Tolerance: 0 - Class: rviz/Odometry + Class: rviz_default_plugins/Odometry Covariance: Orientation: Alpha: 0.5 @@ -485,7 +485,7 @@ Visualization Manager: Topic: /aft_mapped_to_init Unreliable: false Value: false - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /waypoint_planner/visualize Name: MarkerArray @@ -493,7 +493,7 @@ Visualization Manager: {} Queue Size: 100 Value: false - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: true Marker Topic: /fsm_node/visualization/exp_traj Name: MarkerArray @@ -501,7 +501,7 @@ Visualization Manager: {} Queue Size: 100 Value: true - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /fsm_node/visualization/exp_sfcs Name: MarkerArray @@ -509,7 +509,7 @@ Visualization Manager: {} Queue Size: 100 Value: false - - Class: rviz/Image + - Class: rviz_default_plugins/Image Enabled: true Image Topic: /rgb_img Max Value: 1 @@ -529,26 +529,26 @@ Visualization Manager: Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + - Class: rviz_default_plugins/SetInitialPose Theta std deviation: 0.2617993950843811 Topic: /initialpose X std deviation: 0.5 Y std deviation: 0.5 - - Class: rviz/SetGoal + - Class: rviz_default_plugins/SetGoal Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: - Class: rviz/ThirdPersonFollower + Class: rviz_default_plugins/ThirdPersonFollower Distance: 582.7694702148438 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 @@ -569,7 +569,7 @@ Visualization Manager: Target Frame: drone Yaw: 3.1317780017852783 Saved: - - Class: rviz/Orbit + - Class: rviz_default_plugins/Orbit Distance: 117.53474426269531 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 @@ -589,7 +589,7 @@ Visualization Manager: Pitch: 0.19539840519428253 Target Frame: Yaw: 0.17540442943572998 - - Class: rviz/Orbit + - Class: rviz_default_plugins/Orbit Distance: 109.3125 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 @@ -609,7 +609,7 @@ Visualization Manager: Pitch: 0.035398442298173904 Target Frame: Yaw: 5.793589115142822 - - Class: rviz/Orbit + - Class: rviz_default_plugins/Orbit Distance: 85.65605163574219 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 @@ -629,7 +629,7 @@ Visualization Manager: Pitch: 0.5653983950614929 Target Frame: Yaw: 0.9104044437408447 - - Class: rviz/Orbit + - Class: rviz_default_plugins/Orbit Distance: 60.1053581237793 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 diff --git a/rviz_cfg/fast_livo2.rviz b/rviz_cfg/fast_livo2.rviz index da2892e..f89870f 100755 --- a/rviz_cfg/fast_livo2.rviz +++ b/rviz_cfg/fast_livo2.rviz @@ -1,9 +1,10 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 0 Name: Displays Property Tree Widget: Expanded: + - /Global Options1 - /Status1 - /Axes1 - /mapping1 @@ -16,6 +17,7 @@ Panels: - /Odometry1/Odometry1/Shape1 - /Path1 - /currPoints1/Autocompute Value Bounds1 + - /Marker1 - /MarkerArray1/Namespaces1 - /currPoints2/Autocompute Value Bounds1 - /Odometry2/Shape1 @@ -23,36 +25,32 @@ Panels: - /MarkerArray4 - /MarkerArray5 - /Image1 - Splitter Ratio: 0.34272301197052 - Tree Height: 538 - - Class: rviz/Selection + Splitter Ratio: 0.5394402146339417 + Tree Height: 360 + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - /2D Pose Estimate1 - - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: rviz_common/Time + Experimental: false Name: Time SyncMode: 0 SyncSource: surround -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 Visualization Manager: Class: "" Displays: - Alpha: 1 Cell Size: 1 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: false Line Style: @@ -68,16 +66,14 @@ Visualization Manager: Plane Cell Count: 160 Reference Frame: Value: false - - Alpha: 1 - Class: rviz/Axes + - Class: rviz_default_plugins/Axes Enabled: true Length: 0.699999988079071 Name: Axes Radius: 0.10000000149011612 - Reference Frame: - Show Trail: false + Reference Frame: camera_init Value: true - - Class: rviz/Group + - Class: rviz_common/Group Displays: - Alpha: 1 Autocompute Intensity Bounds: true @@ -87,23 +83,29 @@ Visualization Manager: Value: false Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 239; 41; 41 Color Transformer: FlatColor Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 + Max Intensity: 4096 Min Color: 0; 0; 0 + Min Intensity: 0 Name: currPoints Position Transformer: XYZ - Queue Size: 100000 Selectable: true Size (Pixels): 4 Size (m): 0.009999999776482582 Style: Points - Topic: /cloud_registered - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloud_registered Use Fixed Frame: true Use rainbow: false Value: true @@ -115,7 +117,7 @@ Visualization Manager: Value: false Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 238; 238; 236 Color Transformer: RGB8 Decay Time: 10000 @@ -127,13 +129,17 @@ Visualization Manager: Min Intensity: 5 Name: surround Position Transformer: XYZ - Queue Size: 1 Selectable: false Size (Pixels): 1 Size (m): 0.004999999888241291 Style: Points - Topic: /cloud_registered - Unreliable: true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloud_registered Use Fixed Frame: true Use rainbow: true Value: true @@ -145,32 +151,38 @@ Visualization Manager: Value: true Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 239; 41; 41 Color Transformer: "" Decay Time: 0 Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 + Max Intensity: 4096 Min Color: 0; 0; 0 + Min Intensity: 0 Name: PointCloud2 Position Transformer: XYZ - Queue Size: 10 Selectable: true Size (Pixels): 5 Size (m): 0.019999999552965164 Style: Squares - Topic: /cloud_effected - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloud_effected Use Fixed Frame: true Use rainbow: true Value: false Enabled: true Name: mapping - - Class: rviz/Group + - Class: rviz_common/Group Displays: - Angle Tolerance: 0.009999999776482582 - Class: rviz/Odometry + Class: rviz_default_plugins/Odometry Covariance: Orientation: Alpha: 0.5 @@ -190,7 +202,6 @@ Visualization Manager: Keep: 1 Name: Odometry Position Tolerance: 0.0010000000474974513 - Queue Size: 10 Shape: Alpha: 1 Axes Length: 0.5 @@ -201,14 +212,19 @@ Visualization Manager: Shaft Length: 0.800000011920929 Shaft Radius: 0.5 Value: Axes - Topic: /aft_mapped_to_init - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /aft_mapped_to_init Value: true Enabled: true Name: Odometry - Alpha: 0 Buffer Length: 2 - Class: rviz/Path + Class: rviz_default_plugins/Path Color: 25; 255; 255 Enabled: true Head Diameter: 0 @@ -223,12 +239,16 @@ Visualization Manager: Z: 0 Pose Color: 25; 255; 255 Pose Style: None - Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.4000000059604645 Shaft Length: 0.4000000059604645 - Topic: /path - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /path Value: true - Alpha: 0.10000000149011612 Autocompute Intensity Bounds: true @@ -238,41 +258,56 @@ Visualization Manager: Value: false Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 239; 41; 41 Color Transformer: Intensity Decay Time: 1000 Enabled: false Invert Rainbow: true Max Color: 255; 255; 255 + Max Intensity: 4096 Min Color: 0; 0; 0 + Min Intensity: 0 Name: currPoints Position Transformer: XYZ - Queue Size: 1 Selectable: true Size (Pixels): 2 Size (m): 0.009999999776482582 Style: Points - Topic: /cloud_voxel - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloud_voxel Use Fixed Frame: true Use rainbow: true Value: false - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true - Marker Topic: /planner_normal Name: Marker Namespaces: {} - Queue Size: 100 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planner_normal Value: true - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false - Marker Topic: /voxels Name: MarkerArray Namespaces: {} - Queue Size: 100 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: visualization_marker_array Value: false - Alpha: 1 Autocompute Intensity Bounds: true @@ -282,33 +317,43 @@ Visualization Manager: Value: true Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 245; 121; 0 Color Transformer: FlatColor Decay Time: 0 Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 + Max Intensity: 4096 Min Color: 0; 0; 0 + Min Intensity: 0 Name: PointCloud2 Position Transformer: XYZ - Queue Size: 1 Selectable: true Size (Pixels): 15 Size (m): 0.009999999776482582 Style: Points - Topic: /cloud_ray_sub_map - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloud_ray_sub_map Use Fixed Frame: true Use rainbow: true Value: false - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false - Marker Topic: /visualization_marker Name: MarkerArray Namespaces: {} - Queue Size: 100 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: visualization_marker_array Value: false - Alpha: 1 Autocompute Intensity Bounds: true @@ -318,23 +363,29 @@ Visualization Manager: Value: true Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 92; 53; 102 Color Transformer: FlatColor Decay Time: 99999 Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 + Max Intensity: 4096 Min Color: 0; 0; 0 + Min Intensity: 0 Name: PointCloud2 Position Transformer: XYZ - Queue Size: 10 Selectable: true Size (Pixels): 10 Size (m): 0.009999999776482582 Style: Points - Topic: /cloud_visual_map - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloud_visual_map Use Fixed Frame: true Use rainbow: true Value: false @@ -346,23 +397,29 @@ Visualization Manager: Value: true Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 115; 210; 22 Color Transformer: FlatColor Decay Time: 0 Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 + Max Intensity: 4096 Min Color: 0; 0; 0 + Min Intensity: 0 Name: surround Position Transformer: XYZ - Queue Size: 1 Selectable: false Size (Pixels): 12 Size (m): 0.05000000074505806 Style: Points - Topic: /cloud_visual_sub_map - Unreliable: true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloud_visual_sub_map Use Fixed Frame: true Use rainbow: true Value: false @@ -374,23 +431,29 @@ Visualization Manager: Value: false Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 237; 212; 0 Color Transformer: FlatColor Decay Time: 0 Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 + Max Intensity: 4096 Min Color: 0; 0; 0 + Min Intensity: 0 Name: currPoints Position Transformer: XYZ - Queue Size: 100000 Selectable: true Size (Pixels): 5 Size (m): 0.009999999776482582 Style: Points - Topic: /cloud_sample_points - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloud_sample_points Use Fixed Frame: true Use rainbow: false Value: false @@ -402,23 +465,29 @@ Visualization Manager: Value: true Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 239; 41; 41 Color Transformer: FlatColor Decay Time: 99999 Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 + Max Intensity: 4096 Min Color: 239; 41; 41 + Min Intensity: 0 Name: PointCloud2 Position Transformer: XYZ - Queue Size: 10 Selectable: true Size (Pixels): 4 Size (m): 0.009999999776482582 Style: Points - Topic: /cloud_visual_map - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloud_visual_map Use Fixed Frame: true Use rainbow: true Value: false @@ -430,28 +499,34 @@ Visualization Manager: Value: true Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 92; 53; 102 Color Transformer: FlatColor Decay Time: 0 Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 + Max Intensity: 4096 Min Color: 0; 0; 0 + Min Intensity: 0 Name: PointCloud2 Position Transformer: XYZ - Queue Size: 10 Selectable: true Size (Pixels): 20 Size (m): 0.009999999776482582 Style: Points - Topic: /cloud_ray_sub_map_fov - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloud_ray_sub_map_fov Use Fixed Frame: true Use rainbow: true Value: false - Angle Tolerance: 0 - Class: rviz/Odometry + Class: rviz_default_plugins/Odometry Covariance: Orientation: Alpha: 0.5 @@ -471,7 +546,6 @@ Visualization Manager: Keep: 1 Name: Odometry Position Tolerance: 0 - Queue Size: 10 Shape: Alpha: 1 Axes Length: 0.699999988079071 @@ -482,80 +556,116 @@ Visualization Manager: Shaft Length: 1 Shaft Radius: 0.05000000074505806 Value: Axes - Topic: /aft_mapped_to_init - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /aft_mapped_to_init Value: false - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false - Marker Topic: /waypoint_planner/visualize Name: MarkerArray Namespaces: {} - Queue Size: 100 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: visualization_marker_array Value: false - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: true - Marker Topic: /fsm_node/visualization/exp_traj Name: MarkerArray Namespaces: {} - Queue Size: 100 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: visualization_marker_array Value: true - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false - Marker Topic: /fsm_node/visualization/exp_sfcs Name: MarkerArray Namespaces: {} - Queue Size: 100 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: visualization_marker_array Value: false - - Class: rviz/Image + - Class: rviz_default_plugins/Image Enabled: true - Image Topic: /rgb_img Max Value: 1 Median window: 5 Min Value: 0 Name: Image Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rgb_img Value: true Enabled: true Global Options: Background Color: 0; 0; 0 - Default Light: true Fixed Frame: camera_init Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - 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: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint Single click: true - Topic: /clicked_point + 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/ThirdPersonFollower - Distance: 25.669620513916016 + Class: rviz_default_plugins/ThirdPersonFollower + Distance: 50.66728210449219 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false - Field of View: 0.7853981852531433 Focal Point: X: 5.083343982696533 Y: -9.8687162399292 @@ -565,18 +675,18 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: -0.10520334541797638 + Pitch: 0.3547965884208679 Target Frame: drone - Yaw: 3.161787748336792 + Value: ThirdPersonFollower (rviz_default_plugins) + Yaw: 2.5417795181274414 Saved: - - Class: rviz/Orbit + - Class: rviz_default_plugins/Orbit Distance: 117.53474426269531 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false - Field of View: 0.7853981852531433 Focal Point: X: -35.713138580322266 Y: 36.932613372802734 @@ -588,15 +698,15 @@ Visualization Manager: Near Clip Distance: 0.009999999776482582 Pitch: 0.19539840519428253 Target Frame: + Value: Orbit (rviz_default_plugins) Yaw: 0.17540442943572998 - - Class: rviz/Orbit + - Class: rviz_default_plugins/Orbit Distance: 109.3125 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false - Field of View: 0.7853981852531433 Focal Point: X: -22.092714309692383 Y: 63.322662353515625 @@ -608,15 +718,15 @@ Visualization Manager: Near Clip Distance: 0.009999999776482582 Pitch: 0.035398442298173904 Target Frame: + Value: Orbit (rviz_default_plugins) Yaw: 5.793589115142822 - - Class: rviz/Orbit + - Class: rviz_default_plugins/Orbit Distance: 85.65605163574219 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false - Field of View: 0.7853981852531433 Focal Point: X: 28.252656936645508 Y: -35.49672317504883 @@ -628,15 +738,15 @@ Visualization Manager: Near Clip Distance: 0.009999999776482582 Pitch: 0.5653983950614929 Target Frame: + Value: Orbit (rviz_default_plugins) Yaw: 0.9104044437408447 - - Class: rviz/Orbit + - Class: rviz_default_plugins/Orbit Distance: 60.1053581237793 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false - Field of View: 0.7853981852531433 Focal Point: X: 30.61589241027832 Y: 29.98663330078125 @@ -648,16 +758,17 @@ Visualization Manager: Near Clip Distance: 0.009999999776482582 Pitch: 0.315398633480072 Target Frame: + Value: Orbit (rviz_default_plugins) Yaw: 5.788588047027588 Window Geometry: Displays: collapsed: false - Height: 1376 + Height: 1016 Hide Left Dock: false Hide Right Dock: false Image: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Time: @@ -666,6 +777,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 2488 - X: 72 + Width: 1920 + X: 0 Y: 27 diff --git a/rviz_cfg/hilti.rviz b/rviz_cfg/hilti.rviz deleted file mode 100755 index b84c64c..0000000 --- a/rviz_cfg/hilti.rviz +++ /dev/null @@ -1,672 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /Axes1 - - /mapping1 - - /mapping1/currPoints1 - - /mapping1/surround1 - - /mapping1/surround1/Autocompute Value Bounds1 - - /mapping1/PointCloud21 - - /Odometry1 - - /Odometry1/Odometry1 - - /Odometry1/Odometry1/Shape1 - - /Path1 - - /currPoints1/Autocompute Value Bounds1 - - /MarkerArray1/Namespaces1 - - /currPoints2/Autocompute Value Bounds1 - - /Odometry2/Shape1 - - /MarkerArray3 - - /MarkerArray4 - - /MarkerArray5 - - /Image1 - Splitter Ratio: 0.34272301197052 - Tree Height: 538 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Name: Time - SyncMode: 0 - SyncSource: surround -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 1 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: false - 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: 160 - Reference Frame: - Value: false - - Alpha: 1 - Class: rviz/Axes - Enabled: true - Length: 0.699999988079071 - Name: Axes - Radius: 0.10000000149011612 - Reference Frame: - Show Trail: false - Value: true - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 20 - Min Value: -3 - Value: false - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 239; 41; 41 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: currPoints - Position Transformer: XYZ - Queue Size: 100000 - Selectable: true - Size (Pixels): 4 - Size (m): 0.009999999776482582 - Style: Points - Topic: /cloud_registered - Unreliable: false - Use Fixed Frame: true - Use rainbow: false - Value: true - - Alpha: 0.10000000149011612 - Autocompute Intensity Bounds: false - Autocompute Value Bounds: - Max Value: 15 - Min Value: -5 - Value: false - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 238; 238; 236 - Color Transformer: RGB8 - Decay Time: 10000 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 159 - Min Color: 0; 0; 0 - Min Intensity: 5 - Name: surround - Position Transformer: XYZ - Queue Size: 1 - Selectable: false - Size (Pixels): 1 - Size (m): 0.004999999888241291 - Style: Points - Topic: /cloud_registered - Unreliable: true - 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/PointCloud2 - Color: 239; 41; 41 - Color Transformer: "" - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: PointCloud2 - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 5 - Size (m): 0.019999999552965164 - Style: Squares - Topic: /cloud_effected - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - Enabled: true - Name: mapping - - Class: rviz/Group - Displays: - - Angle Tolerance: 0.009999999776482582 - Class: rviz/Odometry - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: true - Keep: 1 - Name: Odometry - Position Tolerance: 0.0010000000474974513 - Queue Size: 10 - Shape: - Alpha: 1 - Axes Length: 0.5 - Axes Radius: 0.15000000596046448 - Color: 255; 85; 0 - Head Length: 0 - Head Radius: 0 - Shaft Length: 0.800000011920929 - Shaft Radius: 0.5 - Value: Axes - Topic: /aft_mapped_to_init - Unreliable: false - Value: true - Enabled: true - Name: Odometry - - Alpha: 0 - Buffer Length: 2 - Class: rviz/Path - Color: 25; 255; 255 - Enabled: true - Head Diameter: 0 - Head Length: 0 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.03999999910593033 - Name: Path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 25; 255; 255 - Pose Style: None - Queue Size: 10 - Radius: 0.029999999329447746 - Shaft Diameter: 0.4000000059604645 - Shaft Length: 0.4000000059604645 - Topic: /path - Unreliable: false - Value: true - - Alpha: 0.10000000149011612 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 20 - Min Value: -3 - Value: false - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 239; 41; 41 - Color Transformer: Intensity - Decay Time: 1000 - Enabled: false - Invert Rainbow: true - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: currPoints - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 2 - Size (m): 0.009999999776482582 - Style: Points - Topic: /cloud_voxel - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /planner_normal - Name: Marker - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: false - Marker Topic: /voxels - Name: MarkerArray - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 245; 121; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: PointCloud2 - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 15 - Size (m): 0.009999999776482582 - Style: Points - Topic: /cloud_ray_sub_map - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz/MarkerArray - Enabled: false - Marker Topic: /visualization_marker - Name: MarkerArray - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 92; 53; 102 - Color Transformer: FlatColor - Decay Time: 99999 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: PointCloud2 - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 10 - Size (m): 0.009999999776482582 - Style: Points - Topic: /cloud_visual_map - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 115; 210; 22 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: surround - Position Transformer: XYZ - Queue Size: 1 - Selectable: false - Size (Pixels): 12 - Size (m): 0.05000000074505806 - Style: Points - Topic: /cloud_visual_sub_map - Unreliable: true - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 20 - Min Value: -3 - Value: false - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 237; 212; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: currPoints - Position Transformer: XYZ - Queue Size: 100000 - Selectable: true - Size (Pixels): 5 - Size (m): 0.009999999776482582 - Style: Points - Topic: /cloud_sample_points - Unreliable: false - Use Fixed Frame: true - Use rainbow: false - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 239; 41; 41 - Color Transformer: FlatColor - Decay Time: 99999 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 239; 41; 41 - Name: PointCloud2 - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 4 - Size (m): 0.009999999776482582 - Style: Points - Topic: /cloud_visual_map - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 92; 53; 102 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: PointCloud2 - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 20 - Size (m): 0.009999999776482582 - Style: Points - Topic: /cloud_ray_sub_map_fov - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Angle Tolerance: 0 - Class: rviz/Odometry - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: false - Keep: 1 - Name: Odometry - Position Tolerance: 0 - Queue Size: 10 - Shape: - Alpha: 1 - Axes Length: 0.699999988079071 - Axes Radius: 0.20000000298023224 - Color: 255; 25; 0 - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Value: Axes - Topic: /aft_mapped_to_init - Unreliable: false - Value: false - - Class: rviz/MarkerArray - Enabled: false - Marker Topic: /waypoint_planner/visualize - Name: MarkerArray - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /fsm_node/visualization/exp_traj - Name: MarkerArray - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: false - Marker Topic: /fsm_node/visualization/exp_sfcs - Name: MarkerArray - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Image - Enabled: true - Image Topic: /rgb_img - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - Enabled: true - Global Options: - Background Color: 238; 238; 236 - Default Light: true - Fixed Frame: camera_init - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/ThirdPersonFollower - Distance: 44.92388153076172 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.7853981852531433 - Focal Point: - X: -2.7518839836120605 - Y: 2.672811508178711 - Z: -5.34896862518508e-05 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: true - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.1397969275712967 - Target Frame: drone - Yaw: 1.5631110668182373 - Saved: - - Class: rviz/Orbit - Distance: 117.53474426269531 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.7853981852531433 - Focal Point: - X: -35.713138580322266 - Y: 36.932613372802734 - Z: 4.459701061248779 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: far1 - Near Clip Distance: 0.009999999776482582 - Pitch: 0.19539840519428253 - Target Frame: - Yaw: 0.17540442943572998 - - Class: rviz/Orbit - Distance: 109.3125 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.7853981852531433 - Focal Point: - X: -22.092714309692383 - Y: 63.322662353515625 - Z: 14.125411987304688 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: far2 - Near Clip Distance: 0.009999999776482582 - Pitch: 0.035398442298173904 - Target Frame: - Yaw: 5.793589115142822 - - Class: rviz/Orbit - Distance: 85.65605163574219 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.7853981852531433 - Focal Point: - X: 28.252656936645508 - Y: -35.49672317504883 - Z: -36.31112289428711 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: near1 - Near Clip Distance: 0.009999999776482582 - Pitch: 0.5653983950614929 - Target Frame: - Yaw: 0.9104044437408447 - - Class: rviz/Orbit - Distance: 60.1053581237793 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.7853981852531433 - Focal Point: - X: 30.61589241027832 - Y: 29.98663330078125 - Z: -12.290168762207031 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: near2 - Near Clip Distance: 0.009999999776482582 - Pitch: 0.315398633480072 - Target Frame: - Yaw: 5.788588047027588 -Window Geometry: - Displays: - collapsed: false - Height: 1376 - Hide Left Dock: false - Hide Right Dock: false - Image: - collapsed: false - QMainWindow State: 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 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 2488 - X: 72 - Y: 27 diff --git a/rviz_cfg/ntu_viral.rviz b/rviz_cfg/ntu_viral.rviz index a9a6088..265b245 100755 --- a/rviz_cfg/ntu_viral.rviz +++ b/rviz_cfg/ntu_viral.rviz @@ -1,5 +1,5 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 0 Name: Displays Property Tree Widget: @@ -25,21 +25,21 @@ Panels: - /Image1 Splitter Ratio: 0.34272301197052 Tree Height: 538 - - Class: rviz/Selection + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: rviz_common/Time Name: Time SyncMode: 0 SyncSource: surround @@ -52,7 +52,7 @@ Visualization Manager: Displays: - Alpha: 1 Cell Size: 1 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: false Line Style: @@ -69,7 +69,7 @@ Visualization Manager: Reference Frame: Value: false - Alpha: 1 - Class: rviz/Axes + Class: rviz_default_plugins/Axes Enabled: true Length: 0.699999988079071 Name: Axes @@ -77,7 +77,7 @@ Visualization Manager: Reference Frame: Show Trail: false Value: true - - Class: rviz/Group + - Class: rviz_common/Group Displays: - Alpha: 1 Autocompute Intensity Bounds: true @@ -87,7 +87,7 @@ Visualization Manager: Value: false Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 239; 41; 41 Color Transformer: FlatColor Decay Time: 0 @@ -115,7 +115,7 @@ Visualization Manager: Value: false Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 238; 238; 236 Color Transformer: RGB8 Decay Time: 10000 @@ -145,7 +145,7 @@ Visualization Manager: Value: true Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 239; 41; 41 Color Transformer: "" Decay Time: 0 @@ -167,10 +167,10 @@ Visualization Manager: Value: false Enabled: true Name: mapping - - Class: rviz/Group + - Class: rviz_common/Group Displays: - Angle Tolerance: 0.009999999776482582 - Class: rviz/Odometry + Class: rviz_default_plugins/Odometry Covariance: Orientation: Alpha: 0.5 @@ -208,7 +208,7 @@ Visualization Manager: Name: Odometry - Alpha: 0 Buffer Length: 2 - Class: rviz/Path + Class: rviz_default_plugins/Path Color: 25; 255; 255 Enabled: true Head Diameter: 0 @@ -238,7 +238,7 @@ Visualization Manager: Value: false Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 239; 41; 41 Color Transformer: Intensity Decay Time: 1000 @@ -258,7 +258,7 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: false - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /planner_normal Name: Marker @@ -266,7 +266,7 @@ Visualization Manager: {} Queue Size: 100 Value: true - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /voxels Name: MarkerArray @@ -282,7 +282,7 @@ Visualization Manager: Value: true Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 245; 121; 0 Color Transformer: FlatColor Decay Time: 0 @@ -302,7 +302,7 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: false - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /visualization_marker Name: MarkerArray @@ -318,7 +318,7 @@ Visualization Manager: Value: true Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 92; 53; 102 Color Transformer: FlatColor Decay Time: 99999 @@ -346,7 +346,7 @@ Visualization Manager: Value: true Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 115; 210; 22 Color Transformer: FlatColor Decay Time: 0 @@ -374,7 +374,7 @@ Visualization Manager: Value: false Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 237; 212; 0 Color Transformer: FlatColor Decay Time: 0 @@ -402,7 +402,7 @@ Visualization Manager: Value: true Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 239; 41; 41 Color Transformer: FlatColor Decay Time: 99999 @@ -430,7 +430,7 @@ Visualization Manager: Value: true Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 92; 53; 102 Color Transformer: FlatColor Decay Time: 0 @@ -451,7 +451,7 @@ Visualization Manager: Use rainbow: true Value: false - Angle Tolerance: 0 - Class: rviz/Odometry + Class: rviz_default_plugins/Odometry Covariance: Orientation: Alpha: 0.5 @@ -485,7 +485,7 @@ Visualization Manager: Topic: /aft_mapped_to_init Unreliable: false Value: false - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /waypoint_planner/visualize Name: MarkerArray @@ -493,7 +493,7 @@ Visualization Manager: {} Queue Size: 100 Value: false - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: true Marker Topic: /fsm_node/visualization/exp_traj Name: MarkerArray @@ -501,7 +501,7 @@ Visualization Manager: {} Queue Size: 100 Value: true - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /fsm_node/visualization/exp_sfcs Name: MarkerArray @@ -509,7 +509,7 @@ Visualization Manager: {} Queue Size: 100 Value: false - - Class: rviz/Image + - Class: rviz_default_plugins/Image Enabled: true Image Topic: /rgb_img Max Value: 1 @@ -529,26 +529,26 @@ Visualization Manager: Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + - Class: rviz_default_plugins/SetInitialPose Theta std deviation: 0.2617993950843811 Topic: /initialpose X std deviation: 0.5 Y std deviation: 0.5 - - Class: rviz/SetGoal + - Class: rviz_default_plugins/SetGoal Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: - Class: rviz/ThirdPersonFollower + Class: rviz_default_plugins/ThirdPersonFollower Distance: 65.96137237548828 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 @@ -569,7 +569,7 @@ Visualization Manager: Target Frame: drone Yaw: 3.251800537109375 Saved: - - Class: rviz/Orbit + - Class: rviz_default_plugins/Orbit Distance: 117.53474426269531 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 @@ -589,7 +589,7 @@ Visualization Manager: Pitch: 0.19539840519428253 Target Frame: Yaw: 0.17540442943572998 - - Class: rviz/Orbit + - Class: rviz_default_plugins/Orbit Distance: 109.3125 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 @@ -609,7 +609,7 @@ Visualization Manager: Pitch: 0.035398442298173904 Target Frame: Yaw: 5.793589115142822 - - Class: rviz/Orbit + - Class: rviz_default_plugins/Orbit Distance: 85.65605163574219 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 @@ -629,7 +629,7 @@ Visualization Manager: Pitch: 0.5653983950614929 Target Frame: Yaw: 0.9104044437408447 - - Class: rviz/Orbit + - Class: rviz_default_plugins/Orbit Distance: 60.1053581237793 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 diff --git a/scripts/colmap_output.sh b/scripts/colmap_output.sh index f0b35d5..5a1b3f0 100755 --- a/scripts/colmap_output.sh +++ b/scripts/colmap_output.sh @@ -1,8 +1,9 @@ #!/bin/bash +# Use 'ros2 pkg prefix --share' instead of 'rospack find' TARGET_DIRS=( - "$(rospack find fast_livo)/Log/Colmap/images" - "$(rospack find fast_livo)/Log/Colmap/sparse/0" + "$(ros2 pkg prefix --share fast_livo)/Log/Colmap/images" + "$(ros2 pkg prefix --share fast_livo)/Log/Colmap/sparse/0" ) for dir in "${TARGET_DIRS[@]}"; do @@ -22,4 +23,3 @@ for dir in "${TARGET_DIRS[@]}"; do echo "Exists: $dir" fi done - diff --git a/src/IMU_Processing.cpp b/src/IMU_Processing.cpp index f3e4c96..7985552 100755 --- a/src/IMU_Processing.cpp +++ b/src/IMU_Processing.cpp @@ -11,6 +11,9 @@ which is included as part of this source code package. */ #include "IMU_Processing.h" +#include + +const bool time_list(PointType &x, PointType &y) { return (x.curvature < y.curvature); } ImuProcess::ImuProcess() : Eye3d(M3D::Identity()), Zero3d(0, 0, 0), b_first_frame(true), imu_need_init(true) @@ -27,7 +30,7 @@ ImuProcess::ImuProcess() : Eye3d(M3D::Identity()), acc_s_last = Zero3d; Lid_offset_to_IMU = Zero3d; Lid_rot_to_IMU = Eye3d; - last_imu.reset(new sensor_msgs::Imu()); + last_imu.reset(new sensor_msgs::msg::Imu()); cur_pcl_un_.reset(new PointCloudXYZI()); } @@ -35,14 +38,14 @@ ImuProcess::~ImuProcess() {} void ImuProcess::Reset() { - ROS_WARN("Reset ImuProcess"); + RCLCPP_WARN(rclcpp::get_logger(""), "Reset ImuProcess"); mean_acc = V3D(0, 0, -1.0); mean_gyr = V3D(0, 0, 0); angvel_last = Zero3d; imu_need_init = true; init_iter_num = 1; IMUpose.clear(); - last_imu.reset(new sensor_msgs::Imu()); + last_imu.reset(new sensor_msgs::msg::Imu()); cur_pcl_un_.reset(new PointCloudXYZI()); } @@ -105,7 +108,7 @@ void ImuProcess::IMU_init(const MeasureGroup &meas, StatesGroup &state_inout, in { /** 1. initializing the gravity, gyro bias, acc and gyro covariance ** 2. normalize the acceleration measurenments to unit gravity **/ - ROS_INFO("IMU Initializing: %.1f %%", double(N) / MAX_INI_COUNT * 100); + RCLCPP_INFO(rclcpp::get_logger(""),"IMU Initializing: %.1f %%", double(N) / MAX_INI_COUNT * 100); V3D cur_acc, cur_gyr; if (b_first_frame) @@ -243,8 +246,8 @@ void ImuProcess::UndistortPcl(LidarMeasureGroup &lidar_meas, StatesGroup &state_ // cout<<"meas.imu.size: "<header.stamp.toSec(); - const double &imu_end_time = v_imu.back()->header.stamp.toSec(); + const double &imu_beg_time = stamp2Sec(v_imu.front()->header.stamp); + const double &imu_end_time = stamp2Sec(v_imu.back()->header.stamp); const double prop_beg_time = last_prop_end_time; // printf("[ IMU ] undistort input size: %zu \n", lidar_meas.pcl_proc_cur->points.size()); // printf("[ IMU ] IMU data sequence size: %zu \n", meas.imu.size()); @@ -305,7 +308,7 @@ void ImuProcess::UndistortPcl(LidarMeasureGroup &lidar_meas, StatesGroup &state_ double tau; if (!imu_time_init) { - // imu_time = v_imu.front()->header.stamp.toSec() - first_lidar_time; + // imu_time = stamp2Sec(v_imu.front()->header.stamp) - first_lidar_time; // tau = 1.0 / (0.25 * sin(2 * CV_PI * 0.5 * imu_time) + 0.75); tau = 1.0; imu_time_init = true; @@ -313,11 +316,10 @@ void ImuProcess::UndistortPcl(LidarMeasureGroup &lidar_meas, StatesGroup &state_ else { tau = state_inout.inv_expo_time; - // ROS_ERROR("tau: %.6f !!!!!!", tau); + // RCLCPP_ERROR_STREAM(rclcpp::get_logger(""),"tau: %.6f !!!!!!", tau); } // state_inout.cov(6, 6) = 0.01; - - // ROS_ERROR("lidar_meas.lio_vio_flg"); + // RCLCPP_ERROR_STREAM(rclcpp::get_logger(""),"lidar_meas.lio_vio_flg"); // cout<<"lidar_meas.lio_vio_flg: "<header.stamp.toSec() < prop_beg_time) continue; + if (stamp2Sec(tail->header.stamp) < prop_beg_time) continue; angvel_avr << 0.5 * (head->angular_velocity.x + tail->angular_velocity.x), 0.5 * (head->angular_velocity.y + tail->angular_velocity.y), 0.5 * (head->angular_velocity.z + tail->angular_velocity.z); @@ -344,30 +346,30 @@ void ImuProcess::UndistortPcl(LidarMeasureGroup &lidar_meas, StatesGroup &state_ // cout<<"acc_avr: "<header.stamp.toSec() - first_lidar_time << " " << angvel_avr.transpose() << " " << acc_avr.transpose() << endl; + fout_imu << setw(10) << stamp2Sec(head->header.stamp) - first_lidar_time << " " << angvel_avr.transpose() << " " << acc_avr.transpose() << endl; // #endif - // imu_time = head->header.stamp.toSec() - first_lidar_time; + // imu_time = stamp2Sec(head->header.stamp) - first_lidar_time; angvel_avr -= state_inout.bias_g; acc_avr = acc_avr * G_m_s2 / mean_acc.norm() - state_inout.bias_a; - if (head->header.stamp.toSec() < prop_beg_time) + if (stamp2Sec(head->header.stamp) < prop_beg_time) { // printf("00 \n"); - dt = tail->header.stamp.toSec() - last_prop_end_time; - offs_t = tail->header.stamp.toSec() - prop_beg_time; + dt = stamp2Sec(tail->header.stamp) - last_prop_end_time; + offs_t = stamp2Sec(tail->header.stamp) - prop_beg_time; } else if (i != v_imu.size() - 2) { // printf("11 \n"); - dt = tail->header.stamp.toSec() - head->header.stamp.toSec(); - offs_t = tail->header.stamp.toSec() - prop_beg_time; + dt = stamp2Sec(tail->header.stamp) - stamp2Sec(head->header.stamp); + offs_t = stamp2Sec(tail->header.stamp) - prop_beg_time; } else { // printf("22 \n"); - dt = prop_end_time - head->header.stamp.toSec(); + dt = prop_end_time - stamp2Sec(head->header.stamp); offs_t = prop_end_time - prop_beg_time; } @@ -424,8 +426,8 @@ void ImuProcess::UndistortPcl(LidarMeasureGroup &lidar_meas, StatesGroup &state_ angvel_last = angvel_avr; acc_s_last = acc_imu; - // cout<header.stamp.toSec(): - // "<header.stamp.toSec()<header.stamp): + // "<header.stamp)< +#include -LIVMapper::LIVMapper(ros::NodeHandle &nh) - : extT(0, 0, 0), +using namespace Sophus; +LIVMapper::LIVMapper(rclcpp::Node::SharedPtr &node, std::string node_name, const rclcpp::NodeOptions & options) + : node(std::make_shared(node_name, options)), + extT(0, 0, 0), extR(M3D::Identity()) { extrinT.assign(3, 0.0); extrinR.assign(9, 0.0); cameraextrinT.assign(3, 0.0); cameraextrinR.assign(9, 0.0); - + p_pre.reset(new Preprocess()); p_imu.reset(new ImuProcess()); - readParameters(nh); + readParameters(this->node); VoxelMapConfig voxel_config; - loadVoxelConfig(nh, voxel_config); + loadVoxelConfig(this->node, voxel_config); visual_sub_map.reset(new PointCloudXYZI()); feats_undistort.reset(new PointCloudXYZI()); @@ -40,88 +44,164 @@ LIVMapper::LIVMapper(ros::NodeHandle &nh) vio_manager.reset(new VIOManager()); root_dir = ROOT_DIR; initializeFiles(); - initializeComponents(); - path.header.stamp = ros::Time::now(); + initializeComponents(this->node); // initialize components errors + path.header.stamp = this->node->now(); path.header.frame_id = "camera_init"; } LIVMapper::~LIVMapper() {} -void LIVMapper::readParameters(ros::NodeHandle &nh) +void LIVMapper::readParameters(rclcpp::Node::SharedPtr &node) { - nh.param("common/lid_topic", lid_topic, "/livox/lidar"); - nh.param("common/imu_topic", imu_topic, "/livox/imu"); - nh.param("common/ros_driver_bug_fix", ros_driver_fix_en, false); - nh.param("common/img_en", img_en, 1); - nh.param("common/lidar_en", lidar_en, 1); - nh.param("common/img_topic", img_topic, "/left_camera/image"); - - nh.param("vio/normal_en", normal_en, true); - nh.param("vio/inverse_composition_en", inverse_composition_en, false); - nh.param("vio/max_iterations", max_iterations, 5); - nh.param("vio/img_point_cov", IMG_POINT_COV, 100); - nh.param("vio/raycast_en", raycast_en, false); - nh.param("vio/exposure_estimate_en", exposure_estimate_en, true); - nh.param("vio/inv_expo_cov", inv_expo_cov, 0.2); - nh.param("vio/grid_size", grid_size, 5); - nh.param("vio/grid_n_height", grid_n_height, 17); - nh.param("vio/patch_pyrimid_level", patch_pyrimid_level, 3); - nh.param("vio/patch_size", patch_size, 8); - nh.param("vio/outlier_threshold", outlier_threshold, 1000); - - nh.param("time_offset/exposure_time_init", exposure_time_init, 0.0); - nh.param("time_offset/img_time_offset", img_time_offset, 0.0); - nh.param("time_offset/imu_time_offset", imu_time_offset, 0.0); - nh.param("time_offset/lidar_time_offset", lidar_time_offset, 0.0); - nh.param("uav/imu_rate_odom", imu_prop_enable, false); - nh.param("uav/gravity_align_en", gravity_align_en, false); - - nh.param("evo/seq_name", seq_name, "01"); - nh.param("evo/pose_output_en", pose_output_en, false); - nh.param("imu/gyr_cov", gyr_cov, 1.0); - nh.param("imu/acc_cov", acc_cov, 1.0); - nh.param("imu/imu_int_frame", imu_int_frame, 3); - nh.param("imu/imu_en", imu_en, false); - nh.param("imu/gravity_est_en", gravity_est_en, true); - nh.param("imu/ba_bg_est_en", ba_bg_est_en, true); - - nh.param("preprocess/blind", p_pre->blind, 0.01); - nh.param("preprocess/filter_size_surf", filter_size_surf_min, 0.5); - nh.param("preprocess/hilti_en", hilti_en, false); - nh.param("preprocess/lidar_type", p_pre->lidar_type, AVIA); - nh.param("preprocess/scan_line", p_pre->N_SCANS, 6); - nh.param("preprocess/point_filter_num", p_pre->point_filter_num, 3); - nh.param("preprocess/feature_extract_enabled", p_pre->feature_enabled, false); - - nh.param("pcd_save/interval", pcd_save_interval, -1); - nh.param("pcd_save/pcd_save_en", pcd_save_en, false); - nh.param("pcd_save/colmap_output_en", colmap_output_en, false); - nh.param("pcd_save/filter_size_pcd", filter_size_pcd, 0.5); - nh.param>("extrin_calib/extrinsic_T", extrinT, vector()); - nh.param>("extrin_calib/extrinsic_R", extrinR, vector()); - nh.param>("extrin_calib/Pcl", cameraextrinT, vector()); - nh.param>("extrin_calib/Rcl", cameraextrinR, vector()); - nh.param("debug/plot_time", plot_time, -10); - nh.param("debug/frame_cnt", frame_cnt, 6); - - nh.param("publish/blind_rgb_points", blind_rgb_points, 0.01); - nh.param("publish/pub_scan_num", pub_scan_num, 1); - nh.param("publish/pub_effect_point_en", pub_effect_point_en, false); - nh.param("publish/dense_map_en", dense_map_en, false); - - p_pre->blind_sqr = p_pre->blind * p_pre->blind; + // declare parameters + auto try_declare = [node](const std::string & name, + const ParameterT & default_value) + { + if (!node->has_parameter(name)) + { + return node->declare_parameter(name, default_value); + } + else + { + return node->get_parameter(name).get_value(); + } + }; + + // declare parameter + try_declare.template operator()("common.lid_topic", "/livox/lidar"); + try_declare.template operator()("common.imu_topic", "/livox/imu"); + try_declare.template operator()("common.ros_driver_bug_fix", false); + try_declare.template operator()("common.img_en", 1); + try_declare.template operator()("common.lidar_en", 1); + try_declare.template operator()("common.img_topic", "/left_camera/image"); + try_declare.template operator()("common.enable_image_processing", false); + + try_declare.template operator()("vio.normal_en", true); + try_declare.template operator()("vio.inverse_composition_en", false); + try_declare.template operator()("vio.max_iterations", 5); + try_declare.template operator()("vio.img_point_cov", 100); + try_declare.template operator()("vio.raycast_en", false); + try_declare.template operator()("vio.exposure_estimate_en", true); + try_declare.template operator()("vio.inv_expo_cov", 0.1); + try_declare.template operator()("vio.grid_size", 5); + try_declare.template operator()("vio.grid_n_height", 17); + try_declare.template operator()("vio.patch_pyrimid_level", 4); + try_declare.template operator()("vio.patch_size", 8); + try_declare.template operator()("vio.outlier_threshold", 100); + try_declare.template operator()("time_offset.exposure_time_init", 0.0); + try_declare.template operator()("time_offset.img_time_offset", 0.0); + try_declare.template operator()("uav.imu_rate_odom", false); + try_declare.template operator()("uav.gravity_align_en", false); + + try_declare.template operator()("evo.seq_name", "01"); + try_declare.template operator()("evo.pose_output_en", false); + try_declare.template operator()("imu.gyr_cov", 1.0); + try_declare.template operator()("imu.acc_cov", 1.0); + try_declare.template operator()("imu.imu_int_frame", 30); + try_declare.template operator()("imu.imu_en", true); + try_declare.template operator()("imu.gravity_est_en", true); + try_declare.template operator()("imu.ba_bg_est_en", true); + + try_declare.template operator()("preprocess.blind", 0.01); + try_declare.template operator()("preprocess.filter_size_surf", 0.5); + try_declare.template operator()("preprocess.lidar_type", AVIA); + try_declare.template operator()("preprocess.scan_line",6); + try_declare.template operator()("preprocess.point_filter_num", 3); + try_declare.template operator()("preprocess.feature_extract_enabled", false); + + try_declare.template operator()("pcd_save.interval", -1); + try_declare.template operator()("pcd_save.pcd_save_en", false); + try_declare.template operator()("pcd_save.colmap_output_en", false); + try_declare.template operator()("pcd_save.filter_size_pcd", 0.5); + try_declare.template operator()>("extrin_calib.extrinsic_T", vector{}); + try_declare.template operator()>("extrin_calib.extrinsic_R", vector{}); + try_declare.template operator()>("extrin_calib.Pcl", vector{}); + try_declare.template operator()>("extrin_calib.Rcl", vector{}); + try_declare.template operator()("debug.plot_time", -10); + try_declare.template operator()("debug.frame_cnt", 6); + + try_declare.template operator()("publish.blind_rgb_points", 0.01); + try_declare.template operator()("publish.pub_scan_num", 1); + try_declare.template operator()("publish.pub_effect_point_en", false); + try_declare.template operator()("publish.dense_map_en", false); + + // get parameter + this->node->get_parameter("common.lid_topic", lid_topic); + this->node->get_parameter("common.imu_topic", imu_topic); + this->node->get_parameter("common.ros_driver_bug_fix", ros_driver_fix_en); + this->node->get_parameter("common.img_en", img_en); + this->node->get_parameter("common.lidar_en", lidar_en); + this->node->get_parameter("common.img_topic", img_topic); + this->node->get_parameter("common.enable_image_processing", enable_image_processing); + + this->node->get_parameter("vio.normal_en", normal_en); + this->node->get_parameter("vio.inverse_composition_en", inverse_composition_en); + this->node->get_parameter("vio.max_iterations", max_iterations); + this->node->get_parameter("vio.img_point_cov", IMG_POINT_COV); + this->node->get_parameter("vio.raycast_en", raycast_en); + this->node->get_parameter("vio.exposure_estimate_en", exposure_estimate_en); + this->node->get_parameter("vio.inv_expo_cov", inv_expo_cov); + this->node->get_parameter("vio.grid_size", grid_size); + this->node->get_parameter("vio.grid_n_height", grid_n_height); + this->node->get_parameter("vio.patch_pyrimid_level", patch_pyrimid_level); + this->node->get_parameter("vio.patch_size", patch_size); + this->node->get_parameter("vio.outlier_threshold", outlier_threshold); + this->node->get_parameter("time_offset.exposure_time_init", exposure_time_init); + this->node->get_parameter("time_offset.img_time_offset", img_time_offset); + this->node->get_parameter("time_offset.imu_time_offset", imu_time_offset); + this->node->get_parameter("time_offset.lidar_time_offset", lidar_time_offset); + this->node->get_parameter("uav.imu_rate_odom", imu_prop_enable); + this->node->get_parameter("uav.gravity_align_en", gravity_align_en); + + this->node->get_parameter("evo.seq_name", seq_name); + this->node->get_parameter("evo.pose_output_en", pose_output_en); + this->node->get_parameter("imu.gyr_cov", gyr_cov); + this->node->get_parameter("imu.acc_cov", acc_cov); + this->node->get_parameter("imu.imu_int_frame", imu_int_frame); + this->node->get_parameter("imu.imu_en", imu_en); + this->node->get_parameter("imu.gravity_est_en", gravity_est_en); + this->node->get_parameter("imu.ba_bg_est_en", ba_bg_est_en); + + this->node->get_parameter("preprocess.blind", p_pre->blind); + this->node->get_parameter("preprocess.filter_size_surf", filter_size_surf_min); + this->node->get_parameter("preprocess.lidar_type", p_pre->lidar_type); + this->node->get_parameter("preprocess.scan_line", p_pre->N_SCANS); + this->node->get_parameter("preprocess.point_filter_num", p_pre->point_filter_num); + this->node->get_parameter("preprocess.feature_extract_enabled", p_pre->feature_enabled); + + this->node->get_parameter("pcd_save.interval", pcd_save_interval); + this->node->get_parameter("pcd_save.pcd_save_en", pcd_save_en); + this->node->get_parameter("pcd_save.colmap_output_en", colmap_output_en); + this->node->get_parameter("pcd_save.filter_size_pcd", filter_size_pcd); + this->node->get_parameter("extrin_calib.extrinsic_T", extrinT); + this->node->get_parameter("extrin_calib.extrinsic_R", extrinR); + this->node->get_parameter("extrin_calib.Pcl", cameraextrinT); + this->node->get_parameter("extrin_calib.Rcl", cameraextrinR); + this->node->get_parameter("debug.plot_time", plot_time); + this->node->get_parameter("debug.frame_cnt", frame_cnt); + + this->node->get_parameter("publish.blind_rgb_points", blind_rgb_points); + this->node->get_parameter("publish.pub_scan_num", pub_scan_num); + this->node->get_parameter("publish.pub_effect_point_en", pub_effect_point_en); + this->node->get_parameter("publish.dense_map_en", dense_map_en); } -void LIVMapper::initializeComponents() +void LIVMapper::initializeComponents(rclcpp::Node::SharedPtr &node) { downSizeFilterSurf.setLeafSize(filter_size_surf_min, filter_size_surf_min, filter_size_surf_min); + + // extrinT.assign({0.04165, 0.02326, -0.0284}); + // extrinR.assign({1, 0, 0, 0, 1, 0, 0, 0, 1}); + // cameraextrinT.assign({0.0194384, 0.104689,-0.0251952}); + // cameraextrinR.assign({0.00610193,-0.999863,-0.0154172,-0.00615449,0.0153796,-0.999863,0.999962,0.00619598,-0.0060598}); + extT << VEC_FROM_ARRAY(extrinT); extR << MAT_FROM_ARRAY(extrinR); voxelmap_manager->extT_ << VEC_FROM_ARRAY(extrinT); voxelmap_manager->extR_ << MAT_FROM_ARRAY(extrinR); - if (!vk::camera_loader::loadFromRosNs("laserMapping", vio_manager->cam)) throw std::runtime_error("Camera model not correctly specified."); + if (!vk::camera_loader::loadFromRosNs(this->node, "camera", vio_manager->cam)) throw std::runtime_error("Camera model not correctly specified."); vio_manager->grid_size = grid_size; vio_manager->patch_size = patch_size; @@ -160,55 +240,137 @@ void LIVMapper::initializeComponents() void LIVMapper::initializeFiles() { - if (pcd_save_en && colmap_output_en) - { - const std::string folderPath = std::string(ROOT_DIR) + "/scripts/colmap_output.sh"; - - std::string chmodCommand = "chmod +x " + folderPath; - - int chmodRet = system(chmodCommand.c_str()); - if (chmodRet != 0) { - std::cerr << "Failed to set execute permissions for the script." << std::endl; - return; - } + namespace fs = boost::filesystem; + // Define target directories for Colmap outputs and point clouds + std::vector target_dirs = { + std::string(ROOT_DIR) + "Log/Colmap/images", + std::string(ROOT_DIR) + "Log/Colmap/sparse/0", + std::string(ROOT_DIR) + "Log/PCD" + }; + + // Conditionally clear and recreate directories when both flags are enabled + if (pcd_save_en && colmap_output_en) + { + for (const auto& dir_path : target_dirs) { + fs::path dir(dir_path); + // Remove existing directory and all contents + if (fs::exists(dir)) { + try { + uintmax_t removed_count = fs::remove_all(dir); + std::cout << "Removed " << removed_count << " items from " << dir_path << std::endl; + } catch (const fs::filesystem_error& e) { + // Handle potential permission errors during deletion + std::cerr << "Failed to remove " << dir_path << ": " << e.what() << std::endl; + continue; + } + } + + // Recreate the directory structure + try { + if (fs::create_directories(dir)) { + std::cout << "Created directory: " << dir_path << std::endl; + } + } catch (const fs::filesystem_error& e) { + // Handle directory creation failures (e.g., permission issues) + std::cerr << "Failed to create " << dir_path << ": " << e.what() << std::endl; + } + } + } - int executionRet = system(folderPath.c_str()); - if (executionRet != 0) { - std::cerr << "Failed to execute the script." << std::endl; - return; - } - } - if(colmap_output_en) fout_points.open(std::string(ROOT_DIR) + "Log/Colmap/sparse/0/points3D.txt", std::ios::out); - if(pcd_save_interval > 0) fout_pcd_pos.open(std::string(ROOT_DIR) + "Log/PCD/scans_pos.json", std::ios::out); - fout_pre.open(DEBUG_FILE_DIR("mat_pre.txt"), std::ios::out); - fout_out.open(DEBUG_FILE_DIR("mat_out.txt"), std::ios::out); + // Ensure all target directories exist regardless of flags + for (const auto& dir_path : target_dirs) { + fs::path dir(dir_path); + if (!fs::exists(dir)) { + try { + if (fs::create_directories(dir)) { + std::cout << "Created directory: " << dir_path << std::endl; + } + } catch (const fs::filesystem_error& e) { + // Note: This may fail if parent directories lack write permissions + std::cerr << "Failed to create " << dir_path << ": " << e.what() << std::endl; + } + } + } + if(colmap_output_en) fout_points.open(std::string(ROOT_DIR) + "Log/Colmap/sparse/0/points3D.txt", std::ios::out); + if(pcd_save_interval > 0) fout_pcd_pos.open(std::string(ROOT_DIR) + "Log/PCD/scans_pos.json", std::ios::out); + fout_pre.open(DEBUG_FILE_DIR("mat_pre.txt"), std::ios::out); + fout_out.open(DEBUG_FILE_DIR("mat_out.txt"), std::ios::out); } -void LIVMapper::initializeSubscribersAndPublishers(ros::NodeHandle &nh, image_transport::ImageTransport &it) +void LIVMapper::initializeSubscribersAndPublishers(rclcpp::Node::SharedPtr &node, image_transport::ImageTransport &it_) { - sub_pcl = p_pre->lidar_type == AVIA ? - nh.subscribe(lid_topic, 200000, &LIVMapper::livox_pcl_cbk, this): - nh.subscribe(lid_topic, 200000, &LIVMapper::standard_pcl_cbk, this); - sub_imu = nh.subscribe(imu_topic, 200000, &LIVMapper::imu_cbk, this); - sub_img = nh.subscribe(img_topic, 200000, &LIVMapper::img_cbk, this); - - pubLaserCloudFullRes = nh.advertise("/cloud_registered", 100); - pubNormal = nh.advertise("visualization_marker", 100); - pubSubVisualMap = nh.advertise("/cloud_visual_sub_map_before", 100); - pubLaserCloudEffect = nh.advertise("/cloud_effected", 100); - pubLaserCloudMap = nh.advertise("/Laser_map", 100); - pubOdomAftMapped = nh.advertise("/aft_mapped_to_init", 10); - pubPath = nh.advertise("/path", 10); - plane_pub = nh.advertise("/planner_normal", 1); - voxel_pub = nh.advertise("/voxels", 1); - pubLaserCloudDyn = nh.advertise("/dyn_obj", 100); - pubLaserCloudDynRmed = nh.advertise("/dyn_obj_removed", 100); - pubLaserCloudDynDbg = nh.advertise("/dyn_obj_dbg_hist", 100); - mavros_pose_publisher = nh.advertise("/mavros/vision_pose/pose", 10); + image_transport::ImageTransport it(this->node); + if (p_pre->lidar_type == AVIA) { + sub_pcl = this->node->create_subscription(lid_topic, 200000, std::bind(&LIVMapper::livox_pcl_cbk, this, std::placeholders::_1)); + } else { + sub_pcl = this->node->create_subscription(lid_topic, 200000, std::bind(&LIVMapper::standard_pcl_cbk, this, std::placeholders::_1)); + } + sub_imu = this->node->create_subscription(imu_topic, 200000, std::bind(&LIVMapper::imu_cbk, this, std::placeholders::_1)); + if (enable_image_processing) { + sub_img_compressed = this->node->create_subscription( + img_topic, 200000, std::bind(&LIVMapper::jpeg_callback, this, std::placeholders::_1)); + } else { + sub_img = this->node->create_subscription( + img_topic, 200000, std::bind(&LIVMapper::img_cbk, this, std::placeholders::_1)); + } + pubLaserCloudFullRes = this->node->create_publisher("/cloud_registered", 100); + pubNormal = this->node->create_publisher("/visualization_marker", 100); + pubSubVisualMap = this->node->create_publisher("/cloud_visual_sub_map_before", 100); + pubLaserCloudEffect = this->node->create_publisher("/cloud_effected", 100); + pubLaserCloudMap = this->node->create_publisher("/Laser_map", 100); + pubOdomAftMapped = this->node->create_publisher("/aft_mapped_to_init", 10); + pubPath = this->node->create_publisher("/path", 10); + plane_pub = this->node->create_publisher("/planner_normal", 1); + voxel_pub = this->node->create_publisher("/voxels", 1); + pubLaserCloudDyn = this->node->create_publisher("/dyn_obj", 100); + pubLaserCloudDynRmed = this->node->create_publisher("/dyn_obj_removed", 100); + pubLaserCloudDynDbg = this->node->create_publisher("/dyn_obj_dbg_hist", 100); + mavros_pose_publisher = this->node->create_publisher("/mavros/vision_pose/pose", 10); pubImage = it.advertise("/rgb_img", 1); - pubImuPropOdom = nh.advertise("/LIVO2/imu_propagate", 10000); - imu_prop_timer = nh.createTimer(ros::Duration(0.004), &LIVMapper::imu_prop_callback, this); - voxelmap_manager->voxel_map_pub_= nh.advertise("/planes", 10000); + pubImuPropOdom = this->node->create_publisher("/LIVO2/imu_propagate", 10000); + imu_prop_timer = this->node->create_wall_timer(0.004s, std::bind(&LIVMapper::imu_prop_callback, this)); + voxelmap_manager->voxel_map_pub_= this->node->create_publisher("/planes", 10000); +} + +void LIVMapper::jpeg_callback(const sensor_msgs::msg::CompressedImage::SharedPtr msg) +{ + // Skip processing if image processing is globally disabled + if (!img_en) return; + if (!enable_image_processing) { + RCLCPP_DEBUG(node->get_logger(), "Image processing disabled in config"); + return; + } + try { + // Decode JPEG image + cv::Mat image = cv::imdecode(cv::Mat(msg->data), cv::IMREAD_COLOR); + if (image.empty()) { + RCLCPP_ERROR(node->get_logger(), "Failed to decode JPEG image"); + return; + } + // Calculate adjusted timestamp + double msg_header_time = stamp2Sec(msg->header.stamp) + img_time_offset; + // Skip duplicate or out-of-order images + if (abs(msg_header_time - last_timestamp_img) < 0.001) return; + if (msg_header_time < last_timestamp_img) { + RCLCPP_ERROR(node->get_logger(), "image loop back."); + return; + } + // Skip images arriving too fast + if (msg_header_time - last_timestamp_img < 0.02) { + RCLCPP_WARN(node->get_logger(), "Image need Jumps: %.6f", msg_header_time); + return; + } + // Add to processing buffers + mtx_buffer.lock(); + img_buffer.push_back(image); + img_time_buffer.push_back(msg_header_time); + last_timestamp_img = msg_header_time; + mtx_buffer.unlock(); + // Notify processing thread + sig_buffer.notify_all(); + } catch (const std::exception& e) { + RCLCPP_ERROR(node->get_logger(), "Error decoding JPEG: %s", e.what()); + } } void LIVMapper::handleFirstFrame() @@ -228,7 +390,7 @@ void LIVMapper::gravityAlignment() { std::cout << "Gravity Alignment Starts" << std::endl; V3D ez(0, 0, -1), gz(_state.gravity); - Quaterniond G_q_I0 = Quaterniond::FromTwoVectors(gz, ez); + Eigen::Quaterniond G_q_I0 = Eigen::Quaterniond::FromTwoVectors(gz, ez); M3D G_R_I0 = G_q_I0.toRotationMatrix(); _state.pos_end = G_R_I0 * _state.pos_end; @@ -385,12 +547,12 @@ void LIVMapper::handleLIO() { evoFile.open(std::string(ROOT_DIR) + "Log/result/" + seq_name + ".txt", std::ios::out); pos_opend = true; - if (!evoFile.is_open()) ROS_ERROR("open fail\n"); + if (!evoFile.is_open()) RCLCPP_ERROR(this->node->get_logger(), "open fail\n"); } else { evoFile.open(std::string(ROOT_DIR) + "Log/result/" + seq_name + ".txt", std::ios::app); - if (!evoFile.is_open()) ROS_ERROR("open fail\n"); + if (!evoFile.is_open()) RCLCPP_ERROR(this->node->get_logger(), "open fail\n"); } Eigen::Matrix4d outT; Eigen::Quaterniond q(_state.rot_end); @@ -526,12 +688,12 @@ void LIVMapper::savePCD() } } -void LIVMapper::run() +void LIVMapper::run(rclcpp::Node::SharedPtr &node) { - ros::Rate rate(5000); - while (ros::ok()) + rclcpp::Rate rate(5000); + while (rclcpp::ok()) { - ros::spinOnce(); + rclcpp::spin_some(this->node); if (!sync_packages(LidarMeasures)) { rate.sleep(); @@ -568,11 +730,11 @@ void LIVMapper::prop_imu_once(StatesGroup &imu_prop_state, const double dt, V3D imu_prop_state.vel_end = imu_prop_state.vel_end + acc_imu * dt; } -void LIVMapper::imu_prop_callback(const ros::TimerEvent &e) +void LIVMapper::imu_prop_callback() { if (p_imu->imu_need_init || !new_imu || !ekf_finish_once) { return; } mtx_buffer_imu_prop.lock(); - new_imu = false; // ๆŽงๅˆถpropagate้ข‘็Ž‡ๅ’ŒIMU้ข‘็Ž‡ไธ€่‡ด + new_imu = false; // ๆŽงๅˆถ propagate ้ข‘็Ž‡ๅ’Œ IMU ้ข‘็Ž‡ไธ€่‡ด if (imu_prop_enable && !prop_imu_buffer.empty()) { static double last_t_from_lidar_end_time = 0; @@ -580,14 +742,14 @@ void LIVMapper::imu_prop_callback(const ros::TimerEvent &e) { imu_propagate = latest_ekf_state; // drop all useless imu pkg - while ((!prop_imu_buffer.empty() && prop_imu_buffer.front().header.stamp.toSec() < latest_ekf_time)) + while ((!prop_imu_buffer.empty() && stamp2Sec(prop_imu_buffer.front().header.stamp) < latest_ekf_time)) { prop_imu_buffer.pop_front(); } last_t_from_lidar_end_time = 0; for (int i = 0; i < prop_imu_buffer.size(); i++) { - double t_from_lidar_end_time = prop_imu_buffer[i].header.stamp.toSec() - latest_ekf_time; + double t_from_lidar_end_time = stamp2Sec(prop_imu_buffer[i].header.stamp) - latest_ekf_time; double dt = t_from_lidar_end_time - last_t_from_lidar_end_time; // cout << "prop dt" << dt << ", " << t_from_lidar_end_time << ", " << last_t_from_lidar_end_time << endl; V3D acc_imu(prop_imu_buffer[i].linear_acceleration.x, prop_imu_buffer[i].linear_acceleration.y, prop_imu_buffer[i].linear_acceleration.z); @@ -601,7 +763,7 @@ void LIVMapper::imu_prop_callback(const ros::TimerEvent &e) { V3D acc_imu(newest_imu.linear_acceleration.x, newest_imu.linear_acceleration.y, newest_imu.linear_acceleration.z); V3D omg_imu(newest_imu.angular_velocity.x, newest_imu.angular_velocity.y, newest_imu.angular_velocity.z); - double t_from_lidar_end_time = newest_imu.header.stamp.toSec() - latest_ekf_time; + double t_from_lidar_end_time = stamp2Sec(newest_imu.header.stamp) - latest_ekf_time; double dt = t_from_lidar_end_time - last_t_from_lidar_end_time; prop_imu_once(imu_propagate, dt, acc_imu, omg_imu); last_t_from_lidar_end_time = t_from_lidar_end_time; @@ -624,7 +786,7 @@ void LIVMapper::imu_prop_callback(const ros::TimerEvent &e) imu_prop_odom.twist.twist.linear.x = vel_i.x(); imu_prop_odom.twist.twist.linear.y = vel_i.y(); imu_prop_odom.twist.twist.linear.z = vel_i.z(); - pubImuPropOdom.publish(imu_prop_odom); + pubImuPropOdom->publish(imu_prop_odom); } mtx_buffer_imu_prop.unlock(); } @@ -670,7 +832,7 @@ template Matrix LIVMapper::pointBodyToWorld(const Matrix po(p[0], p[1], p[2]); + Eigen::Matrix po(p[0], p[1], p[2]); return po; } @@ -684,19 +846,19 @@ void LIVMapper::RGBpointBodyToWorld(PointType const *const pi, PointType *const po->intensity = pi->intensity; } -void LIVMapper::standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg) +void LIVMapper::standard_pcl_cbk(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) { if (!lidar_en) return; mtx_buffer.lock(); - double cur_head_time = msg->header.stamp.toSec() + lidar_time_offset; + double cur_head_time = stamp2Sec(msg->header.stamp) + lidar_time_offset; // cout<<"got feature"<node->get_logger(),"lidar loop back, clear buffer"); lid_raw_data_buffer.clear(); } - // ROS_INFO("get point cloud at time: %.6f", msg->header.stamp.toSec()); + // ROS_INFO("get point cloud at time: %.6f", stamp2Sec(msg->header.stamp)); PointCloudXYZI::Ptr ptr(new PointCloudXYZI()); p_pre->process(msg, ptr); lid_raw_data_buffer.push_back(ptr); @@ -707,37 +869,37 @@ void LIVMapper::standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg) sig_buffer.notify_all(); } -void LIVMapper::livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg_in) +void LIVMapper::livox_pcl_cbk(const livox_ros_driver2::msg::CustomMsg::ConstSharedPtr &msg_in) { if (!lidar_en) return; mtx_buffer.lock(); - livox_ros_driver::CustomMsg::Ptr msg(new livox_ros_driver::CustomMsg(*msg_in)); - // if ((abs(msg->header.stamp.toSec() - last_timestamp_lidar) > 0.2 && last_timestamp_lidar > 0) || sync_jump_flag) + livox_ros_driver2::msg::CustomMsg::SharedPtr msg(new livox_ros_driver2::msg::CustomMsg(*msg_in)); + // if ((abs(stamp2Sec(msg->header.stamp) - last_timestamp_lidar) > 0.2 && last_timestamp_lidar > 0) || sync_jump_flag) // { - // ROS_WARN("lidar jumps %.3f\n", msg->header.stamp.toSec() - last_timestamp_lidar); + // ROS_WARN("lidar jumps %.3f\n", stamp2Sec(msg->header.stamp) - last_timestamp_lidar); // sync_jump_flag = true; - // msg->header.stamp = ros::Time().fromSec(last_timestamp_lidar + 0.1); + // msg->header.stamp = rclcpp::Time().fromSec(last_timestamp_lidar + 0.1); // } - if (abs(last_timestamp_imu - msg->header.stamp.toSec()) > 1.0 && !imu_buffer.empty()) + if (abs(last_timestamp_imu - stamp2Sec(msg->header.stamp)) > 1.0 && !imu_buffer.empty()) { - double timediff_imu_wrt_lidar = last_timestamp_imu - msg->header.stamp.toSec(); - printf("\033[95mSelf sync IMU and LiDAR, HARD time lag is %.10lf \n\033[0m", timediff_imu_wrt_lidar - 0.100); + double timediff_imu_wrt_lidar = last_timestamp_imu - stamp2Sec(msg->header.stamp); + RCLCPP_INFO(this->node->get_logger(), "\033[95mSelf sync IMU and LiDAR, HARD time lag is %.10lf \n\033[0m", timediff_imu_wrt_lidar - 0.100); // imu_time_offset = timediff_imu_wrt_lidar; } - double cur_head_time = msg->header.stamp.toSec(); - ROS_INFO("Get LiDAR, its header time: %.6f", cur_head_time); + double cur_head_time = stamp2Sec(msg->header.stamp); + RCLCPP_INFO(this->node->get_logger(), "Get LiDAR, its header time: %.6f", cur_head_time); if (cur_head_time < last_timestamp_lidar) { - ROS_ERROR("lidar loop back, clear buffer"); + RCLCPP_ERROR(this->node->get_logger(), "lidar loop back, clear buffer"); lid_raw_data_buffer.clear(); } - // ROS_INFO("get point cloud at time: %.6f", msg->header.stamp.toSec()); + RCLCPP_INFO(this->node->get_logger(), "get point cloud at time: %.6f", stamp2Sec(msg->header.stamp)); PointCloudXYZI::Ptr ptr(new PointCloudXYZI()); p_pre->process(msg, ptr); if (!ptr || ptr->empty()) { - ROS_ERROR("Received an empty point cloud"); + RCLCPP_ERROR(this->node->get_logger(), "Received an empty point cloud"); mtx_buffer.unlock(); return; } @@ -750,23 +912,23 @@ void LIVMapper::livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg_i sig_buffer.notify_all(); } -void LIVMapper::imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in) +void LIVMapper::imu_cbk(const sensor_msgs::msg::Imu::ConstSharedPtr &msg_in) { if (!imu_en) return; if (last_timestamp_lidar < 0.0) return; - // ROS_INFO("get imu at time: %.6f", msg_in->header.stamp.toSec()); - sensor_msgs::Imu::Ptr msg(new sensor_msgs::Imu(*msg_in)); - msg->header.stamp = ros::Time().fromSec(msg->header.stamp.toSec() - imu_time_offset); - double timestamp = msg->header.stamp.toSec(); + RCLCPP_INFO(this->node->get_logger(), "get imu at time: %.6f", stamp2Sec(msg_in->header.stamp)); + sensor_msgs::msg::Imu::SharedPtr msg(new sensor_msgs::msg::Imu(*msg_in)); + msg->header.stamp = sec2Stamp(stamp2Sec(msg->header.stamp) - imu_time_offset); + double timestamp = stamp2Sec(msg->header.stamp); if (fabs(last_timestamp_lidar - timestamp) > 0.5 && (!ros_driver_fix_en)) { - ROS_WARN("IMU and LiDAR not synced! delta time: %lf .\n", last_timestamp_lidar - timestamp); + RCLCPP_WARN(this->node->get_logger(), "IMU and LiDAR not synced! delta time: %lf .\n", last_timestamp_lidar - timestamp); } if (ros_driver_fix_en) timestamp += std::round(last_timestamp_lidar - timestamp); - msg->header.stamp = ros::Time().fromSec(timestamp); + msg->header.stamp = sec2Stamp(timestamp); mtx_buffer.lock(); @@ -774,23 +936,22 @@ void LIVMapper::imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in) { mtx_buffer.unlock(); sig_buffer.notify_all(); - ROS_ERROR("imu loop back, offset: %lf \n", last_timestamp_imu - timestamp); + RCLCPP_ERROR(this->node->get_logger(), "imu loop back, offset: %lf \n", last_timestamp_imu - timestamp); return; } - // if (last_timestamp_imu > 0.0 && timestamp > last_timestamp_imu + 0.2) - // { - - // ROS_WARN("imu time stamp Jumps %0.4lf seconds \n", timestamp - last_timestamp_imu); - // mtx_buffer.unlock(); - // sig_buffer.notify_all(); - // return; - // } + if (last_timestamp_imu > 0.0 && timestamp > last_timestamp_imu + 0.2) + { + RCLCPP_WARN(this->node->get_logger(), "imu time stamp Jumps %0.4lf seconds \n", timestamp - last_timestamp_imu); + mtx_buffer.unlock(); + sig_buffer.notify_all(); + return; + } last_timestamp_imu = timestamp; imu_buffer.push_back(msg); - // cout<<"got imu: "<image; return img; } -void LIVMapper::img_cbk(const sensor_msgs::ImageConstPtr &msg_in) +// static int i = 0; +void LIVMapper::img_cbk(const sensor_msgs::msg::Image::ConstSharedPtr &msg_in) { if (!img_en) return; - sensor_msgs::Image::Ptr msg(new sensor_msgs::Image(*msg_in)); - // if ((abs(msg->header.stamp.toSec() - last_timestamp_img) > 0.2 && last_timestamp_img > 0) || sync_jump_flag) + if (enable_image_processing) { + RCLCPP_DEBUG(node->get_logger(), "Skipping raw image processing"); + return; + } + sensor_msgs::msg::Image::SharedPtr msg(new sensor_msgs::msg::Image(*msg_in)); + // if ((abs(stamp2Sec(msg->header.stamp) - last_timestamp_img) > 0.2 && last_timestamp_img > 0) || sync_jump_flag) // { - // ROS_WARN("img jumps %.3f\n", msg->header.stamp.toSec() - last_timestamp_img); + // RCLCPP_WARN(this->node->get_logger(), "img jumps %.3f\n", stamp2Sec(msg->header.stamp) - last_timestamp_img); // sync_jump_flag = true; - // msg->header.stamp = ros::Time().fromSec(last_timestamp_img + 0.1); + // msg->header.stamp = rclcpp::Time().fromSec(last_timestamp_img + 0.1); // } // Hiliti2022 40Hz - if (hilti_en) - { - static int frame_counter = 0; - if (++frame_counter % 4 != 0) return; - } - // double msg_header_time = msg->header.stamp.toSec(); - double msg_header_time = msg->header.stamp.toSec() + img_time_offset; + // if (hilti_en) + // { + // i++; + // if (i % 4 != 0) return; + // } + // double msg_header_time = stamp2Sec(msg->header.stamp); + double msg_header_time = stamp2Sec(msg->header.stamp) + img_time_offset; if (abs(msg_header_time - last_timestamp_img) < 0.001) return; - ROS_INFO("Get image, its header time: %.6f", msg_header_time); + RCLCPP_INFO(this->node->get_logger(), "Get image, its header time: %.6f", msg_header_time); if (last_timestamp_lidar < 0) return; if (msg_header_time < last_timestamp_img) { - ROS_ERROR("image loop back. \n"); + RCLCPP_ERROR(this->node->get_logger(), "image loop back. \n"); return; } @@ -845,7 +1011,7 @@ void LIVMapper::img_cbk(const sensor_msgs::ImageConstPtr &msg_in) if (img_time_correct - last_timestamp_img < 0.02) { - ROS_WARN("Image need Jumps: %.6f", img_time_correct); + RCLCPP_WARN(this->node->get_logger(), "Image need Jumps: %.6f", img_time_correct); mtx_buffer.unlock(); sig_buffer.notify_all(); return; @@ -903,7 +1069,7 @@ bool LIVMapper::sync_packages(LidarMeasureGroup &meas) mtx_buffer.lock(); while (!imu_buffer.empty()) { - if (imu_buffer.front()->header.stamp.toSec() > meas.lidar_frame_end_time) break; + if (stamp2Sec(imu_buffer.front()->header.stamp) > meas.lidar_frame_end_time) break; m.imu.push_back(imu_buffer.front()); imu_buffer.pop_front(); } @@ -944,19 +1110,19 @@ bool LIVMapper::sync_packages(LidarMeasureGroup &meas) // meas.last_lio_update_time); double lid_newest_time = lid_header_time_buffer.back() + lid_raw_data_buffer.back()->points.back().curvature / double(1000); - double imu_newest_time = imu_buffer.back()->header.stamp.toSec(); + double imu_newest_time = stamp2Sec(imu_buffer.back()->header.stamp); if (img_capture_time < meas.last_lio_update_time + 0.00001) { img_buffer.pop_front(); img_time_buffer.pop_front(); - ROS_ERROR("[ Data Cut ] Throw one image frame! \n"); + RCLCPP_ERROR(this->node->get_logger(), "[ Data Cut ] Throw one image frame! \n"); return false; } if (img_capture_time > lid_newest_time || img_capture_time > imu_newest_time) { - // ROS_ERROR("lost first camera frame"); + // RCLCPP_ERROR(this->node->get_logger(), "lost first camera frame"); // printf("img_capture_time, lid_newest_time, imu_newest_time: %lf , %lf // , %lf \n", img_capture_time, lid_newest_time, imu_newest_time); return false; @@ -971,13 +1137,13 @@ bool LIVMapper::sync_packages(LidarMeasureGroup &meas) mtx_buffer.lock(); while (!imu_buffer.empty()) { - if (imu_buffer.front()->header.stamp.toSec() > m.lio_time) break; + if (stamp2Sec(imu_buffer.front()->header.stamp) > m.lio_time) break; - if (imu_buffer.front()->header.stamp.toSec() > meas.last_lio_update_time) m.imu.push_back(imu_buffer.front()); + if (stamp2Sec(imu_buffer.front()->header.stamp) > meas.last_lio_update_time) m.imu.push_back(imu_buffer.front()); imu_buffer.pop_front(); // printf("[ Data Cut ] imu time: %lf \n", - // imu_buffer.front()->header.stamp.toSec()); + // stamp2Sec(imu_buffer.front()->header.stamp)); } mtx_buffer.unlock(); sig_buffer.notify_all(); @@ -1032,7 +1198,7 @@ bool LIVMapper::sync_packages(LidarMeasureGroup &meas) meas.lio_vio_flg = VIO; // printf("[ Data Cut ] VIO \n"); meas.measures.clear(); - double imu_time = imu_buffer.front()->header.stamp.toSec(); + double imu_time = stamp2Sec(imu_buffer.front()->header.stamp); struct MeasureGroup m; m.vio_time = img_capture_time; @@ -1041,12 +1207,12 @@ bool LIVMapper::sync_packages(LidarMeasureGroup &meas) mtx_buffer.lock(); // while ((!imu_buffer.empty() && (imu_time < img_capture_time))) // { - // imu_time = imu_buffer.front()->header.stamp.toSec(); + // imu_time = stamp2Sec(imu_buffer.front()->header.stamp); // if (imu_time > img_capture_time) break; // m.imu.push_back(imu_buffer.front()); // imu_buffer.pop_front(); // printf("[ Data Cut ] imu time: %lf \n", - // imu_buffer.front()->header.stamp.toSec()); + // stamp2Sec(imu_buffer.front()->header.stamp)); // } img_buffer.pop_front(); img_time_buffer.pop_front(); @@ -1099,21 +1265,21 @@ bool LIVMapper::sync_packages(LidarMeasureGroup &meas) return false; } } - ROS_ERROR("out sync"); + RCLCPP_ERROR(this->node->get_logger(), "out sync"); } void LIVMapper::publish_img_rgb(const image_transport::Publisher &pubImage, VIOManagerPtr vio_manager) { cv::Mat img_rgb = vio_manager->img_cp; cv_bridge::CvImage out_msg; - out_msg.header.stamp = ros::Time::now(); + out_msg.header.stamp = this->node->get_clock()->now(); // out_msg.header.frame_id = "camera_init"; out_msg.encoding = sensor_msgs::image_encodings::BGR8; out_msg.image = img_rgb; pubImage.publish(out_msg.toImageMsg()); } -void LIVMapper::publish_frame_world(const ros::Publisher &pubLaserCloudFullRes, VIOManagerPtr vio_manager) +void LIVMapper::publish_frame_world(const rclcpp::Publisher::SharedPtr &pubLaserCloudFullRes, VIOManagerPtr vio_manager) { if (pcl_w_wait_pub->empty()) return; PointCloudXYZRGB::Ptr laserCloudWorldRGB(new PointCloudXYZRGB()); @@ -1163,7 +1329,7 @@ void LIVMapper::publish_frame_world(const ros::Publisher &pubLaserCloudFullRes, } /*** Publish Frame ***/ - sensor_msgs::PointCloud2 laserCloudmsg; + sensor_msgs::msg::PointCloud2 laserCloudmsg; if (img_en) { // cout << "RGB pointcloud size: " << laserCloudWorldRGB->size() << endl; @@ -1173,9 +1339,9 @@ void LIVMapper::publish_frame_world(const ros::Publisher &pubLaserCloudFullRes, { pcl::toROSMsg(*pcl_w_wait_pub, laserCloudmsg); } - laserCloudmsg.header.stamp = ros::Time::now(); //.fromSec(last_timestamp_lidar); + laserCloudmsg.header.stamp = this->node->get_clock()->now(); //.fromSec(last_timestamp_lidar); laserCloudmsg.header.frame_id = "camera_init"; - pubLaserCloudFullRes.publish(laserCloudmsg); + pubLaserCloudFullRes->publish(laserCloudmsg); /**************** save map ****************/ /* 1. make sure you have enough memories @@ -1225,7 +1391,7 @@ void LIVMapper::publish_frame_world(const ros::Publisher &pubLaserCloudFullRes, PointCloudXYZI().swap(*pcl_w_wait_pub); } -void LIVMapper::publish_visual_sub_map(const ros::Publisher &pubSubVisualMap) +void LIVMapper::publish_visual_sub_map(const rclcpp::Publisher::SharedPtr &pubSubVisualMap) { PointCloudXYZI::Ptr laserCloudFullRes(visual_sub_map); int size = laserCloudFullRes->points.size(); if (size == 0) return; @@ -1233,15 +1399,15 @@ void LIVMapper::publish_visual_sub_map(const ros::Publisher &pubSubVisualMap) *sub_pcl_visual_map_pub = *laserCloudFullRes; if (1) { - sensor_msgs::PointCloud2 laserCloudmsg; + sensor_msgs::msg::PointCloud2 laserCloudmsg; pcl::toROSMsg(*sub_pcl_visual_map_pub, laserCloudmsg); - laserCloudmsg.header.stamp = ros::Time::now(); + laserCloudmsg.header.stamp = this->node->get_clock()->now(); laserCloudmsg.header.frame_id = "camera_init"; - pubSubVisualMap.publish(laserCloudmsg); + pubSubVisualMap->publish(laserCloudmsg); } } -void LIVMapper::publish_effect_world(const ros::Publisher &pubLaserCloudEffect, const std::vector &ptpl_list) +void LIVMapper::publish_effect_world(const rclcpp::Publisher::SharedPtr &pubLaserCloudEffect, const std::vector &ptpl_list) { int effect_feat_num = ptpl_list.size(); PointCloudXYZI::Ptr laserCloudWorld(new PointCloudXYZI(effect_feat_num, 1)); @@ -1251,11 +1417,11 @@ void LIVMapper::publish_effect_world(const ros::Publisher &pubLaserCloudEffect, laserCloudWorld->points[i].y = ptpl_list[i].point_w_[1]; laserCloudWorld->points[i].z = ptpl_list[i].point_w_[2]; } - sensor_msgs::PointCloud2 laserCloudFullRes3; + sensor_msgs::msg::PointCloud2 laserCloudFullRes3; pcl::toROSMsg(*laserCloudWorld, laserCloudFullRes3); - laserCloudFullRes3.header.stamp = ros::Time::now(); + laserCloudFullRes3.header.stamp = this->node->get_clock()->now(); laserCloudFullRes3.header.frame_id = "camera_init"; - pubLaserCloudEffect.publish(laserCloudFullRes3); + pubLaserCloudEffect->publish(laserCloudFullRes3); } template void LIVMapper::set_posestamp(T &out) @@ -1269,39 +1435,40 @@ template void LIVMapper::set_posestamp(T &out) out.orientation.w = geoQuat.w; } -void LIVMapper::publish_odometry(const ros::Publisher &pubOdomAftMapped) +void LIVMapper::publish_odometry(const rclcpp::Publisher::SharedPtr &pubOdomAftMapped) { odomAftMapped.header.frame_id = "camera_init"; odomAftMapped.child_frame_id = "aft_mapped"; - odomAftMapped.header.stamp = ros::Time::now(); //.ros::Time()fromSec(last_timestamp_lidar); + odomAftMapped.header.stamp = this->node->get_clock()->now(); //.ros::Time()fromSec(last_timestamp_lidar); set_posestamp(odomAftMapped.pose.pose); - static tf::TransformBroadcaster br; - tf::Transform transform; - tf::Quaternion q; - transform.setOrigin(tf::Vector3(_state.pos_end(0), _state.pos_end(1), _state.pos_end(2))); + static std::shared_ptr br; + br = std::make_shared(this->node); + tf2::Transform transform; + tf2::Quaternion q; + transform.setOrigin(tf2::Vector3(_state.pos_end(0), _state.pos_end(1), _state.pos_end(2))); q.setW(geoQuat.w); q.setX(geoQuat.x); q.setY(geoQuat.y); q.setZ(geoQuat.z); transform.setRotation(q); - br.sendTransform( tf::StampedTransform(transform, odomAftMapped.header.stamp, "camera_init", "aft_mapped") ); - pubOdomAftMapped.publish(odomAftMapped); + br->sendTransform(geometry_msgs::msg::TransformStamped(createTransformStamped(transform, odomAftMapped.header.stamp, "camera_init", "aft_mapped"))); + pubOdomAftMapped->publish(odomAftMapped); } -void LIVMapper::publish_mavros(const ros::Publisher &mavros_pose_publisher) +void LIVMapper::publish_mavros(const rclcpp::Publisher::SharedPtr &mavros_pose_publisher) { - msg_body_pose.header.stamp = ros::Time::now(); + msg_body_pose.header.stamp = this->node->get_clock()->now(); msg_body_pose.header.frame_id = "camera_init"; set_posestamp(msg_body_pose.pose); - mavros_pose_publisher.publish(msg_body_pose); + mavros_pose_publisher->publish(msg_body_pose); } -void LIVMapper::publish_path(const ros::Publisher pubPath) +void LIVMapper::publish_path(const rclcpp::Publisher::SharedPtr &pubPath) { set_posestamp(msg_body_pose.pose); - msg_body_pose.header.stamp = ros::Time::now(); + msg_body_pose.header.stamp = this->node->get_clock()->now(); msg_body_pose.header.frame_id = "camera_init"; path.poses.push_back(msg_body_pose); - pubPath.publish(path); + pubPath->publish(path); } \ No newline at end of file diff --git a/src/frame.cpp b/src/frame.cpp index 2a0b126..643dd10 100644 --- a/src/frame.cpp +++ b/src/frame.cpp @@ -10,7 +10,7 @@ This file is subject to the terms and conditions outlined in the 'LICENSE' file, which is included as part of this source code package. */ -#include +#include #include "feature.h" #include "frame.h" #include "visual_point.h" diff --git a/src/main.cpp b/src/main.cpp index 135b336..7770749 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -2,11 +2,15 @@ int main(int argc, char **argv) { - ros::init(argc, argv, "laserMapping"); - ros::NodeHandle nh; - image_transport::ImageTransport it(nh); - LIVMapper mapper(nh); - mapper.initializeSubscribersAndPublishers(nh, it); - mapper.run(); + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + options.allow_undeclared_parameters(true); + options.automatically_declare_parameters_from_overrides(true); + rclcpp::Node::SharedPtr nh; + image_transport::ImageTransport it_(nh); + LIVMapper mapper(nh, "laserMapping", options); + mapper.initializeSubscribersAndPublishers(nh, it_); + mapper.run(nh); + rclcpp::shutdown(); return 0; } \ No newline at end of file diff --git a/src/preprocess.cpp b/src/preprocess.cpp index afe3414..a9248d7 100755 --- a/src/preprocess.cpp +++ b/src/preprocess.cpp @@ -51,13 +51,13 @@ void Preprocess::set(bool feat_en, int lid_type, double bld, int pfilt_num) point_filter_num = pfilt_num; } -void Preprocess::process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out) +void Preprocess::process(const livox_ros_driver2::msg::CustomMsg::SharedPtr &msg, PointCloudXYZI::Ptr &pcl_out) { avia_handler(msg); *pcl_out = pl_surf; } -void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out) +void Preprocess::process(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg, PointCloudXYZI::Ptr &pcl_out) { switch (lidar_type) { @@ -81,8 +81,8 @@ void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointClo Pandar128_handler(msg); break; - case ROBOSENSE: - robosense_handler(msg); + case MID360: + mid360_handler(msg); break; default: @@ -92,7 +92,7 @@ void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointClo *pcl_out = pl_surf; } -void Preprocess::avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg) +void Preprocess::avia_handler(const livox_ros_driver2::msg::CustomMsg::SharedPtr &msg) { pl_surf.clear(); pl_corn.clear(); @@ -199,8 +199,42 @@ void Preprocess::avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg) } printf("[ Preprocess ] Output point number: %zu \n", pl_surf.points.size()); } +void Preprocess::mid360_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) { + pl_surf.clear(); + pl_corn.clear(); + pl_full.clear(); -void Preprocess::l515_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) + pcl::PointCloud pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + int plsize = pl_orig.points.size(); + pl_surf.reserve(plsize); + + // Convert ROS timestamp to seconds (double precision) + double msg_header_time = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9; + + for (int i = 0; i < plsize; i++) { + // if((pl_orig.points[i].line < num_scans_) && ((pl_orig.points[i].tag & 0x30) == 0x10 || (pl_orig.points[i].tag & 0x30) == 0x00)){ + PointType added_pt; + + added_pt.normal_x = 0; + added_pt.normal_y = 0; + added_pt.normal_z = 0; + added_pt.x = pl_orig.points[i].x; + added_pt.y = pl_orig.points[i].y; + added_pt.z = pl_orig.points[i].z; + added_pt.intensity = pl_orig.points[i].intensity; + added_pt.curvature = float(pl_orig.points[i].timestamp*1e-9 - msg_header_time)*1e3; // curvature unit: ms + // std::cout<<"pl_orig.points[i].timestamp: "< (blind * blind)) { + pl_surf.points.push_back(added_pt); + } + } + // } + } +} +void Preprocess::l515_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) { pl_surf.clear(); pl_corn.clear(); @@ -211,7 +245,7 @@ void Preprocess::l515_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) pl_corn.reserve(plsize); pl_surf.reserve(plsize); - double time_stamp = msg->header.stamp.toSec(); + double time_stamp = stamp2Sec(msg->header.stamp); // cout << "===================================" << endl; // printf("Pt size = %d, N_SCANS = %d\r\n", plsize, N_SCANS); for (int i = 0; i < pl_orig.points.size(); i++) @@ -240,7 +274,7 @@ void Preprocess::l515_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) // pub_func(pl_surf, pub_corn, msg->header.stamp); } -void Preprocess::oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) +void Preprocess::oust64_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) { pl_surf.clear(); pl_corn.clear(); @@ -302,7 +336,7 @@ void Preprocess::oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) } else { - double time_stamp = msg->header.stamp.toSec(); + double time_stamp = stamp2Sec(msg->header.stamp); // cout << "===================================" << endl; // printf("Pt size = %d, N_SCANS = %d\r\n", plsize, N_SCANS); for (int i = 0; i < pl_orig.points.size(); i++) @@ -333,9 +367,6 @@ void Preprocess::oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) pl_surf.points.push_back(added_pt); } - std::sort(pl_surf.points.begin(), pl_surf.points.end(), [](const PointType &a, const PointType &b) { - return a.curvature < b.curvature; - }); } // pub_func(pl_surf, pub_full, msg->header.stamp); // pub_func(pl_surf, pub_corn, msg->header.stamp); @@ -343,7 +374,7 @@ void Preprocess::oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) #define MAX_LINE_NUM 64 -void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) +void Preprocess::velodyne_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) { pl_surf.clear(); pl_corn.clear(); @@ -352,7 +383,6 @@ void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) pcl::PointCloud pl_orig; pcl::fromROSMsg(*msg, pl_orig); int plsize = pl_orig.points.size(); - if (plsize == 0) return; pl_surf.reserve(plsize); bool is_first[MAX_LINE_NUM]; @@ -361,7 +391,7 @@ void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) float yaw_last[MAX_LINE_NUM] = {0.0}; // yaw of last scan point float time_last[MAX_LINE_NUM] = {0.0}; // last offset time - if (pl_orig.points[plsize - 1].time > 0) { given_offset_time = true; } + if (pl_orig.points[plsize - 1].t > 0) { given_offset_time = true; } else { given_offset_time = false; @@ -399,7 +429,7 @@ void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) added_pt.y = pl_orig.points[i].y; added_pt.z = pl_orig.points[i].z; added_pt.intensity = pl_orig.points[i].intensity; - added_pt.curvature = pl_orig.points[i].time / 1000.0; // units: ms + added_pt.curvature = pl_orig.points[i].t / 1000.0; // units: ms if (!given_offset_time) { @@ -462,7 +492,7 @@ void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) added_pt.y = pl_orig.points[i].y; added_pt.z = pl_orig.points[i].z; added_pt.intensity = pl_orig.points[i].intensity; - added_pt.curvature = pl_orig.points[i].time / 1000.0; + added_pt.curvature = pl_orig.points[i].t / 1000.0; if (!given_offset_time) { @@ -511,7 +541,7 @@ void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) // pub_func(pl_surf, pub_corn, msg->header.stamp); } -void Preprocess::Pandar128_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) +void Preprocess::Pandar128_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) { pl_surf.clear(); @@ -520,7 +550,7 @@ void Preprocess::Pandar128_handler(const sensor_msgs::PointCloud2::ConstPtr &msg int plsize = pl_orig.points.size(); pl_surf.reserve(plsize); - double time_head = pl_orig.points[0].timestamp; + // double time_head = pl_orig.points[0].timestamp; for (int i = 0; i < plsize; i++) { PointType added_pt; @@ -531,8 +561,7 @@ void Preprocess::Pandar128_handler(const sensor_msgs::PointCloud2::ConstPtr &msg added_pt.x = pl_orig.points[i].x; added_pt.y = pl_orig.points[i].y; added_pt.z = pl_orig.points[i].z; - added_pt.intensity = static_cast(pl_orig.points[i].intensity) / 255.0f; - added_pt.curvature = (pl_orig.points[i].timestamp - time_head) * 1000.f; + added_pt.curvature = pl_orig.points[i].timestamp * 1000.f; if (i % point_filter_num == 0) { @@ -563,7 +592,7 @@ void Preprocess::Pandar128_handler(const sensor_msgs::PointCloud2::ConstPtr &msg // cout << GREEN << "pl_surf.points[31000].timestamp: " << pl_surf.points[31000].curvature << RESET << endl; } -void Preprocess::xt32_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) +void Preprocess::xt32_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) { pl_surf.clear(); pl_corn.clear(); @@ -707,42 +736,6 @@ void Preprocess::xt32_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) // pub_func(pl_surf, pub_corn, msg->header.stamp); } -void Preprocess::robosense_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) -{ - pl_surf.clear(); - - pcl::PointCloud pl_orig; - pcl::fromROSMsg(*msg, pl_orig); - int plsize = pl_orig.size(); - pl_surf.reserve(plsize); - - double time_head = pl_orig.points[0].timestamp; - for (int i = 0; i < plsize; ++i) - { - if (i % point_filter_num != 0) continue; - - const auto& pt = pl_orig.points[i]; - const double x = pt.x, y = pt.y, z = pt.z; - const double dist_sqr = x * x + y * y + z * z; - const bool is_valid = (dist_sqr >= blind_sqr) && !std::isnan(x) && !std::isnan(y) && !std::isnan(z); - if (!is_valid) continue; - - PointType added_pt; - added_pt.normal_x = 0; - added_pt.normal_y = 0; - added_pt.normal_z = 0; - added_pt.x = pt.x; - added_pt.y = pt.y; - added_pt.z = pt.z; - added_pt.intensity = pt.intensity; - added_pt.curvature = (pt.timestamp - time_head) * 1000.0; - pl_surf.points.push_back(added_pt); - } - std::sort(pl_surf.points.begin(), pl_surf.points.end(), [](const PointType &a, const PointType &b) { - return a.curvature < b.curvature; - }); -} - void Preprocess::give_feature(pcl::PointCloud &pl, vector &types) { int plsize = pl.size(); @@ -920,8 +913,6 @@ void Preprocess::give_feature(pcl::PointCloud &pl, vector &t { if (types[i].range < blind_sqr || types[i - 1].range < blind_sqr || types[i + 1].range < blind_sqr) { continue; } - if (types[i - 1].dista < 1e-8 || types[i].dista < 1e-8) { continue; } - if (types[i].ftype == Nor) { if (types[i - 1].dista > types[i].dista) { ratio = types[i - 1].dista / types[i].dista; } @@ -979,11 +970,11 @@ void Preprocess::give_feature(pcl::PointCloud &pl, vector &t } } -void Preprocess::pub_func(PointCloudXYZI &pl, const ros::Time &ct) +void Preprocess::pub_func(PointCloudXYZI &pl, const rclcpp::Time &ct) { pl.height = 1; pl.width = pl.size(); - sensor_msgs::PointCloud2 output; + sensor_msgs::msg::PointCloud2 output; pcl::toROSMsg(pl, output); output.header.frame_id = "livox"; output.header.stamp = ct; diff --git a/src/utils.cpp b/src/utils.cpp new file mode 100644 index 0000000..1e453c8 --- /dev/null +++ b/src/utils.cpp @@ -0,0 +1,19 @@ +// utils.cpp +#include +#include // for int64_t +#include // for std::numeric_limits +#include // for std::out_of_range + +std::vector convertToIntVectorSafe(const std::vector& int64_vector) { + std::vector int_vector; + int_vector.reserve(int64_vector.size()); // ้ข„็•™็ฉบ้—ดไปฅๆ้ซ˜ๆ•ˆ็Ž‡ + + for (int64_t value : int64_vector) { + if (value < std::numeric_limits::min() || value > std::numeric_limits::max()) { + throw std::out_of_range("Value is out of range for int"); + } + int_vector.push_back(static_cast(value)); + } + + return int_vector; +} diff --git a/src/vio.cpp b/src/vio.cpp index 2ff8fd0..727e813 100755 --- a/src/vio.cpp +++ b/src/vio.cpp @@ -12,6 +12,7 @@ which is included as part of this source code package. #include "vio.h" +using namespace Eigen; VIOManager::VIOManager() { // downSizeFilter.setLeafSize(0.2, 0.2, 0.2); @@ -127,7 +128,7 @@ void VIOManager::initializeVIO() if(colmap_output_en) { - pinhole_cam = dynamic_cast(cam); + equidistant_cam = dynamic_cast(cam); fout_colmap.open(DEBUG_FILE_DIR("Colmap/sparse/0/images.txt"), ios::out); fout_colmap << "# Image list with two lines of data per image:\n"; fout_colmap << "# IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME\n"; @@ -135,7 +136,7 @@ void VIOManager::initializeVIO() fout_camera.open(DEBUG_FILE_DIR("Colmap/sparse/0/cameras.txt"), ios::out); fout_camera << "# Camera list with one line of data per camera:\n"; fout_camera << "# CAMERA_ID, MODEL, WIDTH, HEIGHT, PARAMS[]\n"; - fout_camera << "1 PINHOLE " << width << " " << height << " " + fout_camera << "1 EQUIDISTANT " << width << " " << height << " " << std::fixed << std::setprecision(6) // ๆŽงๅˆถๆตฎ็‚นๆ•ฐ็ฒพๅบฆไธบ10ไฝ << fx << " " << fy << " " << cx << " " << cy << std::endl; @@ -250,12 +251,12 @@ void VIOManager::insertPointIntoVoxelMap(VisualPoint *pt_new) } void VIOManager::getWarpMatrixAffineHomography(const vk::AbstractCamera &cam, const V2D &px_ref, const V3D &xyz_ref, const V3D &normal_ref, - const SE3 &T_cur_ref, const int level_ref, Matrix2d &A_cur_ref) + const SE3 &T_cur_ref, const int level_ref, Matrix2d &A_cur_ref) { // create homography matrix const V3D t = T_cur_ref.inverse().translation(); const Eigen::Matrix3d H_cur_ref = - T_cur_ref.rotation_matrix() * (normal_ref.dot(xyz_ref) * Eigen::Matrix3d::Identity() - t * normal_ref.transpose()); + T_cur_ref.rotationMatrix() * (normal_ref.dot(xyz_ref) * Eigen::Matrix3d::Identity() - t * normal_ref.transpose()); // Compute affine warp matrix A_ref_cur using homography projection const int kHalfPatchSize = 4; V3D f_du_ref(cam.cam2world(px_ref + Eigen::Vector2d(kHalfPatchSize, 0) * (1 << level_ref))); @@ -273,7 +274,7 @@ void VIOManager::getWarpMatrixAffineHomography(const vk::AbstractCamera &cam, co } void VIOManager::getWarpMatrixAffine(const vk::AbstractCamera &cam, const Vector2d &px_ref, const Vector3d &f_ref, const double depth_ref, - const SE3 &T_cur_ref, const int level_ref, const int pyramid_level, const int halfpatch_size, + const SE3 &T_cur_ref, const int level_ref, const int pyramid_level, const int halfpatch_size, Matrix2d &A_cur_ref) { // Compute affine warp matrix A_ref_cur @@ -457,7 +458,7 @@ void VIOManager::retrieveFromVisualSparseMap(cv::Mat img, vector & if (pt == nullptr) continue; if (pt->obs_.size() == 0) continue; - V3D norm_vec(new_frame_->T_f_w_.rotation_matrix() * pt->normal_); + V3D norm_vec(new_frame_->T_f_w_.rotationMatrix() * pt->normal_); V3D dir(new_frame_->T_f_w_ * pt->pos_); if (dir[2] < 0) continue; // dir.normalize(); @@ -535,7 +536,7 @@ void VIOManager::retrieveFromVisualSparseMap(cv::Mat img, vector & // sub_map_ray.push_back(pt); // cloud_visual_sub_map // add_sample = true; - V3D norm_vec(new_frame_->T_f_w_.rotation_matrix() * pt->normal_); + V3D norm_vec(new_frame_->T_f_w_.rotationMatrix() * pt->normal_); V3D dir(new_frame_->T_f_w_ * pt->pos_); if (dir[2] < 0) continue; dir.normalize(); @@ -698,7 +699,7 @@ void VIOManager::retrieveFromVisualSparseMap(cv::Mat img, vector & if (normal_en) { - V3D norm_vec = (ref_ftr->T_f_w_.rotation_matrix() * pt->normal_).normalized(); + V3D norm_vec = (ref_ftr->T_f_w_.rotationMatrix() * pt->normal_).normalized(); V3D pf(ref_ftr->T_f_w_ * pt->pos_); // V3D pf_norm = pf.normalized(); @@ -864,7 +865,7 @@ void VIOManager::generateVisualMapPoints(cv::Mat img, vector &pg) pointWithVar pt_var = append_voxel_points[i]; V3D pt = pt_var.point_w; - V3D norm_vec(new_frame_->T_f_w_.rotation_matrix() * pt_var.normal); + V3D norm_vec(new_frame_->T_f_w_.rotationMatrix() * pt_var.normal); V3D dir(new_frame_->T_f_w_ * pt); dir.normalize(); double cos_theta = dir.dot(norm_vec); @@ -935,7 +936,7 @@ void VIOManager::updateVisualMapPoints(cv::Mat img) SE3 pose_ref = last_feature->T_f_w_; SE3 delta_pose = pose_ref * pose_cur.inverse(); double delta_p = delta_pose.translation().norm(); - double delta_theta = (delta_pose.rotation_matrix().trace() > 3.0 - 1e-6) ? 0.0 : std::acos(0.5 * (delta_pose.rotation_matrix().trace() - 1)); + double delta_theta = (delta_pose.rotationMatrix().trace() > 3.0 - 1e-6) ? 0.0 : std::acos(0.5 * (delta_pose.rotationMatrix().trace() - 1)); if (delta_p > 0.5 || delta_theta > 0.3) add_flag = true; // 0.5 || 0.3 // Step 3: pixel distance @@ -1046,7 +1047,7 @@ void VIOManager::updateReferencePatch(const unordered_mapT_f_w_ * pt->pos_; - V3D norm_vec = ref_patch_temp->T_f_w_.rotation_matrix() * pt->normal_; + V3D norm_vec = ref_patch_temp->T_f_w_.rotationMatrix() * pt->normal_; pf.normalize(); double cos_angle = pf.dot(norm_vec); // if(fabs(cos_angle) < 0.86) continue; // 20 degree @@ -1136,7 +1137,7 @@ void VIOManager::projectPatchFromRefToCur(const unordered_mapw2c(pt->pos_)); V2D pc_prior(new_frame_->w2c_prior(pt->pos_)); - V3D norm_vec(ref_ftr->T_f_w_.rotation_matrix() * pt->normal_); + V3D norm_vec(ref_ftr->T_f_w_.rotationMatrix() * pt->normal_); V3D pf(ref_ftr->T_f_w_ * pt->pos_); if (pf.dot(norm_vec) < 0) norm_vec = -norm_vec; @@ -1350,7 +1351,7 @@ void VIOManager::precomputeReferencePatches(int level) double depth((pt->pos_ - pt->ref_patch->pos()).norm()); V3D pf = pt->ref_patch->f_ * depth; V2D pc = pt->ref_patch->px_; - M3D R_ref_w = pt->ref_patch->T_f_w_.rotation_matrix(); + M3D R_ref_w = pt->ref_patch->T_f_w_.rotationMatrix(); computeProjectionJacobian(pf, Jdpi); p_w_hat << SKEW_SYM_MATRX(pt->pos_); @@ -1693,7 +1694,7 @@ void VIOManager::updateFrameState(StatesGroup state) V3D Pwi(state.pos_end); Rcw = Rci * Rwi.transpose(); Pcw = -Rci * Rwi.transpose() * Pwi + Pci; - new_frame_->T_f_w_ = SE3(Rcw, Pcw); + new_frame_->T_f_w_ = SE3(Eigen::Quaterniond(Rcw).normalized().toRotationMatrix(), Pcw); // avoid R is not orthogonal } void VIOManager::plotTrackedPoints() @@ -1768,10 +1769,16 @@ void VIOManager::dumpDataForColmap() std::string image_path = std::string(ROOT_DIR) + "Log/Colmap/images/" + cnt_str + ".png"; cv::Mat img_rgb_undistort; - pinhole_cam->undistortImage(img_rgb, img_rgb_undistort); + if(equidistant_cam) { + equidistant_cam->undistortImage(img_rgb, img_rgb_undistort); + } else { + RCLCPP_WARN(rclcpp::get_logger("vio"), "Camera model is not equidistant, skipping undistortion"); + img_rgb_undistort = img_rgb.clone(); + } + // pinhole_cam->undistortImage(img_rgb, img_rgb_undistort); cv::imwrite(image_path, img_rgb_undistort); - Eigen::Quaterniond q(new_frame_->T_f_w_.rotation_matrix()); + Eigen::Quaterniond q(new_frame_->T_f_w_.rotationMatrix()); Eigen::Vector3d t = new_frame_->T_f_w_.translation(); fout_colmap << cnt << " " << std::fixed << std::setprecision(6) // ไฟ่ฏๆตฎ็‚นๆ•ฐ็ฒพๅบฆไธบ6ไฝ diff --git a/src/voxel_map.cpp b/src/voxel_map.cpp index 01923cc..3dcd32e 100644 --- a/src/voxel_map.cpp +++ b/src/voxel_map.cpp @@ -11,7 +11,7 @@ which is included as part of this source code package. */ #include "voxel_map.h" - +using namespace Eigen; void calcBodyCov(Eigen::Vector3d &pb, const float range_inc, const float degree_inc, Eigen::Matrix3d &cov) { if (pb[2] == 0) pb[2] = 0.0001; @@ -33,23 +33,52 @@ void calcBodyCov(Eigen::Vector3d &pb, const float range_inc, const float degree_ cov = direction * range_var * direction.transpose() + A * direction_var * A.transpose(); } -void loadVoxelConfig(ros::NodeHandle &nh, VoxelMapConfig &voxel_config) +void loadVoxelConfig(rclcpp::Node::SharedPtr &node, VoxelMapConfig &voxel_config) { - nh.param("publish/pub_plane_en", voxel_config.is_pub_plane_map_, false); - - nh.param("lio/max_layer", voxel_config.max_layer_, 1); - nh.param("lio/voxel_size", voxel_config.max_voxel_size_, 0.5); - nh.param("lio/min_eigen_value", voxel_config.planner_threshold_, 0.01); - nh.param("lio/sigma_num", voxel_config.sigma_num_, 3); - nh.param("lio/beam_err", voxel_config.beam_err_, 0.02); - nh.param("lio/dept_err", voxel_config.dept_err_, 0.05); - nh.param>("lio/layer_init_num", voxel_config.layer_init_num_, vector{5,5,5,5,5}); - nh.param("lio/max_points_num", voxel_config.max_points_num_, 50); - nh.param("lio/max_iterations", voxel_config.max_iterations_, 5); - - nh.param("local_map/map_sliding_en", voxel_config.map_sliding_en, false); - nh.param("local_map/half_map_size", voxel_config.half_map_size, 100); - nh.param("local_map/sliding_thresh", voxel_config.sliding_thresh, 8); + // declare parameter + auto try_declare = [node](const std::string & name, + const ParameterT & default_value) + { + if (!node->has_parameter(name)) + { + return node->declare_parameter(name, default_value); + } + else + { + return node->get_parameter(name).get_value(); + } + }; + // declare parameter + try_declare.template operator()("publish.pub_plane_en", false); + try_declare.template operator()("lio.max_layer", 1); + try_declare.template operator()("lio.voxel_size", 0.5); + try_declare.template operator()("lio.min_eigen_value", 0.01); + try_declare.template operator()("lio.sigma_num", 3); + try_declare.template operator()("lio.beam_err", 0.02); + try_declare.template operator()("lio.dept_err", 0.05); + + // Declaration of parameter of type std::vector won't build, https://github.com/ros2/rclcpp/issues/1585 + try_declare.template operator()>("lio.layer_init_num", std::vector{5,5,5,5,5}); + try_declare.template operator()("lio.max_points_num", 50); + try_declare.template operator()("lio.min_iterations", 5); + try_declare.template operator()("local_map.map_sliding_en", false); + try_declare.template operator()("local_map.half_map_size", 100); + try_declare.template operator()("local_map.sliding_thresh", 8.0); + + // get parameter + node->get_parameter("publish.pub_plane_en", voxel_config.is_pub_plane_map_); + node->get_parameter("lio.max_layer", voxel_config.max_layer_); + node->get_parameter("lio.voxel_size", voxel_config.max_voxel_size_); + node->get_parameter("lio.min_eigen_value", voxel_config.planner_threshold_); + node->get_parameter("lio.sigma_num", voxel_config.sigma_num_); + node->get_parameter("lio.beam_err", voxel_config.beam_err_); + node->get_parameter("lio.dept_err", voxel_config.dept_err_); + node->get_parameter("lio.layer_init_num", voxel_config.layer_init_num_); + node->get_parameter("lio.max_points_num", voxel_config.max_points_num_); + node->get_parameter("lio.min_iterations", voxel_config.max_iterations_); + node->get_parameter("local_map.map_sliding_en", voxel_config.map_sliding_en); + node->get_parameter("local_map.half_map_size", voxel_config.half_map_size); + node->get_parameter("local_map.sliding_thresh", voxel_config.sliding_thresh); } void VoxelOctoTree::init_plane(const std::vector &points, VoxelPlane *plane) @@ -535,7 +564,7 @@ void VoxelMapManager::BuildVoxelMap() float planer_threshold = config_setting_.planner_threshold_; int max_layer = config_setting_.max_layer_; int max_points_num = config_setting_.max_points_num_; - std::vector layer_init_num = config_setting_.layer_init_num_; + std::vector layer_init_num = convertToIntVectorSafe(config_setting_.layer_init_num_); std::vector input_points; @@ -612,7 +641,7 @@ void VoxelMapManager::UpdateVoxelMap(const std::vector &input_poin float planer_threshold = config_setting_.planner_threshold_; int max_layer = config_setting_.max_layer_; int max_points_num = config_setting_.max_points_num_; - std::vector layer_init_num = config_setting_.layer_init_num_; + std::vector layer_init_num = convertToIntVectorSafe(config_setting_.layer_init_num_); uint plsize = input_points.size(); for (uint i = 0; i < plsize; i++) { @@ -789,9 +818,9 @@ void VoxelMapManager::pubVoxelMap() { double max_trace = 0.25; double pow_num = 0.2; - ros::Rate loop(500); + rclcpp::Rate loop(500); float use_alpha = 0.8; - visualization_msgs::MarkerArray voxel_plane; + visualization_msgs::msg::MarkerArray voxel_plane; voxel_plane.markers.reserve(1000000); std::vector pub_plane_list; for (auto iter = voxel_map_.begin(); iter != voxel_map_.end(); iter++) @@ -813,7 +842,7 @@ void VoxelMapManager::pubVoxelMap() else { alpha = 0; } pubSinglePlane(voxel_plane, "plane", pub_plane_list[i], alpha, plane_rgb); } - voxel_map_pub_.publish(voxel_plane); + voxel_map_pub_->publish(voxel_plane); loop.sleep(); } @@ -834,20 +863,20 @@ void VoxelMapManager::GetUpdatePlane(const VoxelOctoTree *current_octo, const in return; } -void VoxelMapManager::pubSinglePlane(visualization_msgs::MarkerArray &plane_pub, const std::string plane_ns, const VoxelPlane &single_plane, +void VoxelMapManager::pubSinglePlane(visualization_msgs::msg::MarkerArray &plane_pub, const std::string plane_ns, const VoxelPlane &single_plane, const float alpha, const Eigen::Vector3d rgb) { - visualization_msgs::Marker plane; + visualization_msgs::msg::Marker plane; plane.header.frame_id = "camera_init"; - plane.header.stamp = ros::Time(); + plane.header.stamp = rclcpp::Time(); plane.ns = plane_ns; plane.id = single_plane.id_; - plane.type = visualization_msgs::Marker::CYLINDER; - plane.action = visualization_msgs::Marker::ADD; + plane.type = visualization_msgs::msg::Marker::CYLINDER; + plane.action = visualization_msgs::msg::Marker::ADD; plane.pose.position.x = single_plane.center_[0]; plane.pose.position.y = single_plane.center_[1]; plane.pose.position.z = single_plane.center_[2]; - geometry_msgs::Quaternion q; + geometry_msgs::msg::Quaternion q; CalcVectQuation(single_plane.x_normal_, single_plane.y_normal_, single_plane.normal_, q); plane.pose.orientation = q; plane.scale.x = 3 * sqrt(single_plane.max_eigen_value_); @@ -857,12 +886,12 @@ void VoxelMapManager::pubSinglePlane(visualization_msgs::MarkerArray &plane_pub, plane.color.r = rgb(0); plane.color.g = rgb(1); plane.color.b = rgb(2); - plane.lifetime = ros::Duration(); + plane.lifetime = rclcpp::Duration::from_seconds(0.01); plane_pub.markers.push_back(plane); } void VoxelMapManager::CalcVectQuation(const Eigen::Vector3d &x_vec, const Eigen::Vector3d &y_vec, const Eigen::Vector3d &z_vec, - geometry_msgs::Quaternion &q) + geometry_msgs::msg::Quaternion &q) { Eigen::Matrix3d rot; rot << x_vec(0), x_vec(1), x_vec(2), y_vec(0), y_vec(1), y_vec(2), z_vec(0), z_vec(1), z_vec(2);