From 261f533e1ea6767a5e8ed215f85344259db6c6a6 Mon Sep 17 00:00:00 2001
From: Orange <2314753575@qq.com>
Date: Sat, 13 Jun 2026 19:55:01 +0800
Subject: [PATCH] Please enter the commit message for your changes. Lines
starting with '#' will be ignored, and an empty message aborts the commit.
On branch master
Your branch is ahead of 'origin/master' by 31 commits.
(use "git push" to publish your local commits)
Changes to be committed:
new file: .gitignore
new file: .vscode/c_cpp_properties.json
new file: .vscode/launch.json
new file: .vscode/settings.json
new file: CLAUDE.md
new file: README.md
new file: bashes/README.md
new file: bashes/radar-driver-switch.sh
new file: bashes/radar-driver-switch.sh.bak
new file: dependencies/dependencies.txt
new file: src/LSLIDAR_X_ROS2-20240228/src/README.md
new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/CMakeLists.txt
new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/include/lslidar_driver/input.h
new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/include/lslidar_driver/lsiosr.h
new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/include/lslidar_driver/lslidar_driver.h
new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lslidar_double_launch.py
new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10_net_launch.py
new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10_uart_launch.py
new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10p_net_launch.py
new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10p_uart_launch.py
new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10_launch.py
new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10_net_launch.py
new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10p_launch.py
new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10p_net_launch.py
new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/viewer_scan_launch.py
new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/package.xml
new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsm10_net.yaml
new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsm10p_net.yaml
new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsn10_net.yaml
new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsn10p_net.yaml
new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsm10.yaml
new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsm10_p.yaml
new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsn10.yaml
new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsn10p.yaml
new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/rviz/lslidar.rviz
new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/input.cc
new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lsiosr.cpp
new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver.cc
new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver.cc.bak
new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver_node.cc
new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/CMakeLists.txt
new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarDifop.msg
new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarPacket.msg
new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarPoint.msg
new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarScan.msg
new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarSweep.msg
new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/package.xml
new file: src/LSLIDAR_X_ROS2-20240228/src/version.txt
new file: src/LSLIDAR_X_ROS2-20240228/src/wheeltec_udev.sh
new file: "src/LSLIDAR_X_ROS2-20240228/src/\351\225\255\347\245\236Lsx\351\233\267\350\276\276\346\227\213\350\275\254\350\247\222\345\272\246.png"
new file: src/LSLIDAR_X_ROS2-20240228/wheeltec_lidar.launch.py
new file: "src/LSLIDAR_X_ROS2-20240228/wheeltec_lidar.launch.py\344\273\205\345\234\250WHEELTEC\351\225\234\345\203\217\344\270\255\344\275\277\347\224\250"
new file: src/cyy_navigation2/CMakeLists.txt
new file: src/cyy_navigation2/bt/follow_point.xml
new file: src/cyy_navigation2/bt/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml
new file: src/cyy_navigation2/bt/navigate_through_poses_w_replanning_and_recovery.xml
new file: src/cyy_navigation2/bt/navigate_to_pose_w_replanning_and_recovery.xml
new file: src/cyy_navigation2/bt/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml
new file: src/cyy_navigation2/bt/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml
new file: src/cyy_navigation2/bt/navigate_w_replanning_distance.xml
new file: src/cyy_navigation2/bt/navigate_w_replanning_only_if_goal_is_updated.xml
new file: src/cyy_navigation2/bt/navigate_w_replanning_only_if_path_becomes_invalid.xml
new file: src/cyy_navigation2/bt/navigate_w_replanning_speed.xml
new file: src/cyy_navigation2/bt/navigate_w_replanning_time.xml
new file: src/cyy_navigation2/bt/odometry_calibration.xml
new file: src/cyy_navigation2/config/nav2_params.yaml
new file: src/cyy_navigation2/launch/car_bringup.launch.py
new file: src/cyy_navigation2/launch/cyy_nav.launch.py
new file: src/cyy_navigation2/launch/cyy_nav_box.launch.py
new file: src/cyy_navigation2/maps/cyy_map.data
new file: src/cyy_navigation2/maps/cyy_map.pgm
new file: src/cyy_navigation2/maps/cyy_map.yaml
new file: src/cyy_navigation2/maps/cyy_map1.pgm
new file: src/cyy_navigation2/maps/cyy_map1.yaml
new file: src/cyy_navigation2/package.xml
new file: src/cyy_navigation2/param/nav2_params.yaml
new file: src/cyy_navigation2/param/slam_toolbox_localization.yaml
new file: src/cyy_slamtoolbox/CMakeLists.txt
new file: src/cyy_slamtoolbox/config/angular_filter_example.yaml
new file: src/cyy_slamtoolbox/config/box_filter_example.yaml
new file: src/cyy_slamtoolbox/config/footprint_filter_example.yaml
new file: src/cyy_slamtoolbox/config/intensity_filter_example.yaml
new file: src/cyy_slamtoolbox/config/laser_filter_config.yaml
new file: src/cyy_slamtoolbox/config/mapper_params_lifelong.yaml
new file: src/cyy_slamtoolbox/config/mapper_params_localization.yaml
new file: src/cyy_slamtoolbox/config/mapper_params_offline.yaml
new file: src/cyy_slamtoolbox/config/mapper_params_online_async.yaml
new file: src/cyy_slamtoolbox/config/mapper_params_online_sync.yaml
new file: src/cyy_slamtoolbox/config/mask_filter_example.yaml
new file: src/cyy_slamtoolbox/config/median_filter_example.yaml
new file: src/cyy_slamtoolbox/config/median_spatial_filter_example.yaml
new file: src/cyy_slamtoolbox/config/multiple_filters_example.yaml
new file: src/cyy_slamtoolbox/config/pass_through_example.yaml
new file: src/cyy_slamtoolbox/config/polygon_filter_example.yaml
new file: src/cyy_slamtoolbox/config/range_filter_example.yaml
new file: src/cyy_slamtoolbox/config/scan_blob_filter_example.yaml
new file: src/cyy_slamtoolbox/config/sector_filter_example.yaml
new file: src/cyy_slamtoolbox/config/shadow_filter_example.yaml
new file: src/cyy_slamtoolbox/config/slam_toolbox_default.rviz
new file: src/cyy_slamtoolbox/config/speckle_filter_example.yaml
new file: src/cyy_slamtoolbox/launch/cyy_slam_toolbox_launch.launch.py
new file: src/cyy_slamtoolbox/launch/cyy_slam_toolbox_location.launch.py
new file: src/cyy_slamtoolbox/launch/filter.launch.py
new file: src/cyy_slamtoolbox/package.xml
new file: src/gc_navigation2_real/CMakeLists.txt
new file: src/gc_navigation2_real/behavior_tree/nav_to_pose_ackermann.xml
new file: src/gc_navigation2_real/config/slam_toolbox_async_real.yaml
new file: src/gc_navigation2_real/config/slam_toolbox_localization_real.yaml
new file: src/gc_navigation2_real/launch/real_bringup.launch.py
new file: src/gc_navigation2_real/launch/real_nav2_slam.launch.py
new file: src/gc_navigation2_real/launch/real_nav2_slam_online.launch.py
new file: src/gc_navigation2_real/launch/real_slam_mapping.launch.py
new file: src/gc_navigation2_real/maps/Readme.txt
new file: src/gc_navigation2_real/package.xml
new file: src/gc_navigation2_real/params/gc_navigation_slam_real.yaml
new file: src/gc_navigation2_slamtoolbox/CMakeLists.txt
new file: src/gc_navigation2_slamtoolbox/config/mapper_params_lifelong.yaml
new file: src/gc_navigation2_slamtoolbox/config/mapper_params_localization.yaml
new file: src/gc_navigation2_slamtoolbox/config/mapper_params_offline.yaml
new file: src/gc_navigation2_slamtoolbox/config/mapper_params_online_multi_async.yaml
new file: src/gc_navigation2_slamtoolbox/config/mapper_params_online_sync.yaml
new file: src/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml
new file: src/gc_navigation2_slamtoolbox/config/slam_toolbox_async.yaml
new file: src/gc_navigation2_slamtoolbox/config/slam_toolbox_localization.yaml
new file: src/gc_navigation2_slamtoolbox/config/slam_toolbox_mapping.yaml
new file: src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_amcl.launch.py
new file: src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam.launch.py
new file: src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online.launch.py
new file: src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online_real.launch.py
new file: src/gc_navigation2_slamtoolbox/launch/gc_nav_slamtoolbox_lifelong.launch.py
new file: src/gc_navigation2_slamtoolbox/launch/gc_nav_slamtoolbox_location.launch.py
new file: src/gc_navigation2_slamtoolbox/launch/gc_nav_slamtoolbox_on_async.launch.py
new file: src/gc_navigation2_slamtoolbox/launch/gc_slam_mapping.launch.py
new file: src/gc_navigation2_slamtoolbox/maps/my_map.data
new file: src/gc_navigation2_slamtoolbox/maps/my_map.pgm
new file: src/gc_navigation2_slamtoolbox/maps/my_map.posegraph
new file: src/gc_navigation2_slamtoolbox/maps/my_map.yaml
new file: src/gc_navigation2_slamtoolbox/maps/zhihui.data
new file: src/gc_navigation2_slamtoolbox/maps/zhihui.pgm
new file: src/gc_navigation2_slamtoolbox/maps/zhihui.posegraph
new file: src/gc_navigation2_slamtoolbox/maps/zhihui.yaml
new file: src/gc_navigation2_slamtoolbox/package.xml
new file: src/gc_navigation2_slamtoolbox/params/gc_navigation_amcl.yaml
new file: src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml
new file: src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml.bak
new file: src/gc_navigation2_slamtoolbox/world/zhihui/model.config
new file: src/gc_navigation2_slamtoolbox/world/zhihui/model.sdf
new file: src/gc_navigation_fish/CMakeLists.txt
new file: src/gc_navigation_fish/launch/gc_navigation.launch.py
new file: src/gc_navigation_fish/maps/test_map.pgm
new file: src/gc_navigation_fish/maps/test_map.yaml
new file: src/gc_navigation_fish/package.xml
new file: src/gc_navigation_fish/param/gc_navigation.yaml
new file: src/gc_navigation_fish/param/navigation_test.yaml
new file: src/gc_slam_toolbox_fish/CMakeLists.txt
new file: src/gc_slam_toolbox_fish/config/gc_2d.lua
new file: src/gc_slam_toolbox_fish/launch/catograph.launch.py
new file: src/gc_slam_toolbox_fish/package.xml
new file: src/ground_slam/.gitignore
new file: src/ground_slam/CMakeLists.txt
new file: src/ground_slam/LICENSE.md
new file: src/ground_slam/README.md
new file: src/ground_slam/cmake/FindEigen3.cmake
new file: src/ground_slam/cmake/FindFFTW3.cmake
new file: src/ground_slam/configs/config_HD.yaml
new file: src/ground_slam/configs/config_geekplus.yaml
new file: src/ground_slam/configs/config_ntu.yaml
new file: src/ground_slam/figures/data_association.jpg
new file: src/ground_slam/figures/features_small.jpg
new file: src/ground_slam/figures/fig1.jpg
new file: src/ground_slam/figures/loop_error.jpg
new file: src/ground_slam/figures/loop_trajectory.jpg
new file: src/ground_slam/figures/pipeline.png
new file: src/ground_slam/figures/rmse_curve.jpg
new file: src/ground_slam/figures/sample_images.jpg
new file: src/ground_slam/figures/trajectory.jpg
new file: src/ground_slam/figures/video.png
new file: src/ground_slam/include/camera.h
new file: src/ground_slam/include/circ_shift.h
new file: src/ground_slam/include/correlation_flow.h
new file: src/ground_slam/include/dataset.h
new file: src/ground_slam/include/edge.h
new file: src/ground_slam/include/frame.h
new file: src/ground_slam/include/loop_closure.h
new file: src/ground_slam/include/map.h
new file: src/ground_slam/include/map_builder.h
new file: src/ground_slam/include/map_stitcher.h
new file: src/ground_slam/include/optimization_2d/angle_local_parameterization.h
new file: src/ground_slam/include/optimization_2d/normalize_angle.h
new file: src/ground_slam/include/optimization_2d/pose_graph_2d.h
new file: src/ground_slam/include/optimization_2d/pose_graph_2d_error_term.h
new file: src/ground_slam/include/optimization_2d/types.h
new file: src/ground_slam/include/read_configs.h
new file: src/ground_slam/include/thread_publisher.h
new file: src/ground_slam/include/timer.h
new file: src/ground_slam/include/utils.h
new file: src/ground_slam/include/visualization.h
new file: src/ground_slam/main.cpp
new file: src/ground_slam/package.xml
new file: src/ground_slam/src/camera.cc
new file: src/ground_slam/src/correlation_flow.cc
new file: src/ground_slam/src/dataset.cc
new file: src/ground_slam/src/edge.cc
new file: src/ground_slam/src/frame.cc
new file: src/ground_slam/src/loop_closure.cc
new file: src/ground_slam/src/map.cc
new file: src/ground_slam/src/map_builder.cc
new file: src/ground_slam/src/map_stitcher.cc
new file: src/ground_slam/src/optimization_2d/pose_graph_2d.cc
new file: src/ground_slam/src/thread_publisher.cc
new file: src/ground_slam/src/timer.cc
new file: src/ground_slam/src/utils.cc
new file: src/ground_slam/src/visualization.cc
new file: src/ground_slam/supplementary/GroundSLAM_Supplement_Materia.pdf
new file: src/origincar_base/CMakeLists.txt
new file: src/origincar_base/config/ekf.yaml
new file: src/origincar_base/config/imu.yaml
new file: src/origincar_base/include/origincar_base/Quaternion_Solution.h
new file: src/origincar_base/include/origincar_base/origincar_base.h
new file: src/origincar_base/launch/__pycache__/testtwo.launch.cpython-38.pyc
new file: src/origincar_base/launch/base_serial.launch.py
new file: src/origincar_base/launch/ekf.launch.py
new file: src/origincar_base/launch/origincar_bringup.launch.py
new file: src/origincar_base/launch/robot_mode_description.launch.py
new file: src/origincar_base/msg/Position.msg
new file: src/origincar_base/package.xml
new file: src/origincar_base/scripts/__pycache__/cmd_vel_to_ackermann_drive.cpython-310.pyc
new file: src/origincar_base/scripts/cmd_vel_to_ackermann_drive.py
new file: src/origincar_base/src/Quaternion_Solution.cpp
new file: src/origincar_base/src/origincar_base.cpp
new file: src/origincar_base/src/origincar_base.cpp.bak
new file: src/origincar_description/CMakeLists.txt
new file: src/origincar_description/CMakeLists.txt.save
new file: src/origincar_description/config/joint_names_origincar_description.yaml
new file: src/origincar_description/config/joint_names_origincar_description.yaml:Zone.Identifier
new file: "src/origincar_description/config/joint_names_origincar_description.yaml\357\200\272Zone.Identifier"
new file: src/origincar_description/launch/display.launch
new file: src/origincar_description/launch/display.launch.py
new file: src/origincar_description/launch/gazebo.launch
new file: src/origincar_description/launch/gazebo.launch.py
new file: src/origincar_description/meshes/base_link.STL
new file: src/origincar_description/meshes/base_link.STL:Zone.Identifier
new file: "src/origincar_description/meshes/base_link.STL\357\200\272Zone.Identifier"
new file: src/origincar_description/meshes/down_left_Link.STL
new file: src/origincar_description/meshes/down_left_Link.STL:Zone.Identifier
new file: "src/origincar_description/meshes/down_left_Link.STL\357\200\272Zone.Identifier"
new file: src/origincar_description/meshes/down_right_Link.STL
new file: src/origincar_description/meshes/down_right_Link.STL:Zone.Identifier
new file: "src/origincar_description/meshes/down_right_Link.STL\357\200\272Zone.Identifier"
new file: src/origincar_description/meshes/up_left_Link.STL
new file: src/origincar_description/meshes/up_left_Link.STL:Zone.Identifier
new file: "src/origincar_description/meshes/up_left_Link.STL\357\200\272Zone.Identifier"
new file: src/origincar_description/meshes/up_right_Link.STL
new file: src/origincar_description/meshes/up_right_Link.STL:Zone.Identifier
new file: "src/origincar_description/meshes/up_right_Link.STL\357\200\272Zone.Identifier"
new file: src/origincar_description/package.xml
new file: src/origincar_description/rviz/README
new file: src/origincar_description/rviz/display.rviz
new file: src/origincar_description/urdf/origincar.urdf
new file: src/origincar_description/urdf/origincar.xacro
new file: src/origincar_description/world/fishbot.world
new file: src/origincar_description/world/gc_world.world
new file: src/origincar_description/world/test.world
new file: src/origincar_description/world/zhihui.world
new file: src/origincar_msg/CMakeLists.txt
new file: src/origincar_msg/msg/Data.msg
new file: src/origincar_msg/msg/Sign.msg
new file: src/origincar_msg/package.xml
new file: src/zbw_slamtoolbox/CMakeLists.txt
new file: src/zbw_slamtoolbox/config/mapper_params_online_async.yaml
new file: src/zbw_slamtoolbox/config/mapper_params_online_sync copy.yaml
new file: src/zbw_slamtoolbox/config/navigation.yaml
new file: src/zbw_slamtoolbox/launch/navigation.launch.py
new file: src/zbw_slamtoolbox/launch/slamtoolbox.launch.py
new file: src/zbw_slamtoolbox/package.xml
new file: "\347\253\236\350\265\233\346\226\271\346\241\210_\347\254\25421\345\261\212\346\231\272\350\203\275\346\261\275\350\275\246\347\253\236\350\265\233\345\234\260\347\223\234\346\234\272\345\231\250\344\272\272\350\265\233\351\241\271.md"
new file: "\350\260\203\350\257\225\350\256\260\345\275\225.log"
---
.gitignore | 12 +
.vscode/c_cpp_properties.json | 26 +
.vscode/launch.json | 28 +
.vscode/settings.json | 20 +
CLAUDE.md | 165 ++
README.md | 105 ++
bashes/README.md | 52 +
bashes/radar-driver-switch.sh | 101 ++
bashes/radar-driver-switch.sh.bak | 129 ++
dependencies/dependencies.txt | 5 +
src/LSLIDAR_X_ROS2-20240228/src/README.md | 44 +
.../src/lslidar_driver/CMakeLists.txt | 56 +
.../include/lslidar_driver/input.h | 134 ++
.../include/lslidar_driver/lsiosr.h | 88 +
.../include/lslidar_driver/lslidar_driver.h | 159 ++
.../launch/lslidar_double_launch.py | 50 +
.../lslidar_driver/launch/lsm10_net_launch.py | 27 +
.../launch/lsm10_uart_launch.py | 27 +
.../launch/lsm10p_net_launch.py | 27 +
.../launch/lsm10p_uart_launch.py | 27 +
.../src/lslidar_driver/launch/lsn10_launch.py | 28 +
.../lslidar_driver/launch/lsn10_net_launch.py | 28 +
.../lslidar_driver/launch/lsn10p_launch.py | 28 +
.../launch/lsn10p_net_launch.py | 28 +
.../launch/viewer_scan_launch.py | 26 +
.../src/lslidar_driver/package.xml | 36 +
.../params/lidar_net_ros2/lsm10_net.yaml | 22 +
.../params/lidar_net_ros2/lsm10p_net.yaml | 22 +
.../params/lidar_net_ros2/lsn10_net.yaml | 25 +
.../params/lidar_net_ros2/lsn10p_net.yaml | 25 +
.../params/lidar_uart_ros2/lsm10.yaml | 28 +
.../params/lidar_uart_ros2/lsm10_p.yaml | 28 +
.../params/lidar_uart_ros2/lsn10.yaml | 26 +
.../params/lidar_uart_ros2/lsn10p.yaml | 25 +
.../src/lslidar_driver/rviz/lslidar.rviz | 161 ++
.../src/lslidar_driver/src/input.cc | 398 +++++
.../src/lslidar_driver/src/lsiosr.cpp | 400 +++++
.../src/lslidar_driver/src/lslidar_driver.cc | 1423 +++++++++++++++++
.../lslidar_driver/src/lslidar_driver.cc.bak | 1423 +++++++++++++++++
.../lslidar_driver/src/lslidar_driver_node.cc | 35 +
.../src/lslidar_msgs/CMakeLists.txt | 39 +
.../src/lslidar_msgs/msg/LslidarDifop.msg | 2 +
.../src/lslidar_msgs/msg/LslidarPacket.msg | 5 +
.../src/lslidar_msgs/msg/LslidarPoint.msg | 12 +
.../src/lslidar_msgs/msg/LslidarScan.msg | 6 +
.../src/lslidar_msgs/msg/LslidarSweep.msg | 4 +
.../src/lslidar_msgs/package.xml | 25 +
src/LSLIDAR_X_ROS2-20240228/src/version.txt | 20 +
.../src/wheeltec_udev.sh | 33 +
.../src/镭神Lsx雷达旋转角度.png | Bin 0 -> 6646 bytes
.../wheeltec_lidar.launch.py | 62 +
...ltec_lidar.launch.py仅在WHEELTEC镜像中使用 | 0
src/cyy_navigation2/CMakeLists.txt | 29 +
src/cyy_navigation2/bt/follow_point.xml | 21 +
...replanning_and_if_path_becomes_invalid.xml | 46 +
...hrough_poses_w_replanning_and_recovery.xml | 38 +
...gate_to_pose_w_replanning_and_recovery.xml | 36 +
..._replanning_goal_patience_and_recovery.xml | 47 +
...eplanning_only_if_path_becomes_invalid.xml | 44 +
.../bt/navigate_w_replanning_distance.xml | 14 +
...e_w_replanning_only_if_goal_is_updated.xml | 14 +
...eplanning_only_if_path_becomes_invalid.xml | 21 +
.../bt/navigate_w_replanning_speed.xml | 14 +
.../bt/navigate_w_replanning_time.xml | 14 +
.../bt/odometry_calibration.xml | 20 +
src/cyy_navigation2/config/nav2_params.yaml | 348 ++++
.../launch/car_bringup.launch.py | 108 ++
src/cyy_navigation2/launch/cyy_nav.launch.py | 41 +
.../launch/cyy_nav_box.launch.py | 86 +
src/cyy_navigation2/maps/cyy_map.data | Bin 0 -> 758856 bytes
src/cyy_navigation2/maps/cyy_map.pgm | Bin 0 -> 253086 bytes
src/cyy_navigation2/maps/cyy_map.yaml | 7 +
src/cyy_navigation2/maps/cyy_map1.pgm | Bin 0 -> 2149551 bytes
src/cyy_navigation2/maps/cyy_map1.yaml | 7 +
src/cyy_navigation2/package.xml | 20 +
src/cyy_navigation2/param/nav2_params.yaml | 353 ++++
.../param/slam_toolbox_localization.yaml | 54 +
src/cyy_slamtoolbox/CMakeLists.txt | 61 +
.../config/angular_filter_example.yaml | 9 +
.../config/box_filter_example.yaml | 15 +
.../config/footprint_filter_example.yaml | 7 +
.../config/intensity_filter_example.yaml | 9 +
.../config/laser_filter_config.yaml | 19 +
.../config/mapper_params_lifelong.yaml | 87 +
.../config/mapper_params_localization.yaml | 70 +
.../config/mapper_params_offline.yaml | 69 +
.../config/mapper_params_online_async.yaml | 78 +
.../config/mapper_params_online_sync.yaml | 77 +
.../config/mask_filter_example.yaml | 16 +
.../config/median_filter_example.yaml | 20 +
.../config/median_spatial_filter_example.yaml | 17 +
.../config/multiple_filters_example.yaml | 42 +
.../config/pass_through_example.yaml | 0
.../config/polygon_filter_example.yaml | 10 +
.../config/range_filter_example.yaml | 11 +
.../config/scan_blob_filter_example.yaml | 8 +
.../config/sector_filter_example.yaml | 12 +
.../config/shadow_filter_example.yaml | 18 +
.../config/slam_toolbox_default.rviz | 137 ++
.../config/speckle_filter_example.yaml | 19 +
.../launch/cyy_slam_toolbox_launch.launch.py | 24 +
.../cyy_slam_toolbox_location.launch.py | 24 +
src/cyy_slamtoolbox/launch/filter.launch.py | 24 +
src/cyy_slamtoolbox/package.xml | 18 +
src/gc_navigation2_real/CMakeLists.txt | 23 +
.../behavior_tree/nav_to_pose_ackermann.xml | 37 +
.../config/slam_toolbox_async_real.yaml | 78 +
.../slam_toolbox_localization_real.yaml | 77 +
.../launch/real_bringup.launch.py | 54 +
.../launch/real_nav2_slam.launch.py | 110 ++
.../launch/real_nav2_slam_online.launch.py | 135 ++
.../launch/real_slam_mapping.launch.py | 103 ++
src/gc_navigation2_real/maps/Readme.txt | 3 +
src/gc_navigation2_real/package.xml | 31 +
.../params/gc_navigation_slam_real.yaml | 353 ++++
src/gc_navigation2_slamtoolbox/CMakeLists.txt | 31 +
.../config/mapper_params_lifelong.yaml | 89 ++
.../config/mapper_params_localization.yaml | 72 +
.../config/mapper_params_offline.yaml | 71 +
.../mapper_params_online_multi_async.yaml | 74 +
.../config/mapper_params_online_sync.yaml | 79 +
.../config/nav_to_pose_ackermann.xml | 36 +
.../config/slam_toolbox_async.yaml | 82 +
.../config/slam_toolbox_localization.yaml | 55 +
.../config/slam_toolbox_mapping.yaml | 73 +
.../launch/gc_nav2_with_amcl.launch.py | 156 ++
.../launch/gc_nav2_with_slam.launch.py | 196 +++
.../launch/gc_nav2_with_slam_online.launch.py | 181 +++
.../gc_nav2_with_slam_online_real.launch.py | 56 +
.../gc_nav_slamtoolbox_lifelong.launch.py | 25 +
.../gc_nav_slamtoolbox_location.launch.py | 25 +
.../gc_nav_slamtoolbox_on_async.launch.py | 25 +
.../launch/gc_slam_mapping.launch.py | 196 +++
.../maps/my_map.data | Bin 0 -> 1400948 bytes
.../maps/my_map.pgm | Bin 0 -> 52949 bytes
.../maps/my_map.posegraph | Bin 0 -> 6886670 bytes
.../maps/my_map.yaml | 7 +
.../maps/zhihui.data | Bin 0 -> 653622 bytes
.../maps/zhihui.pgm | Bin 0 -> 12550 bytes
.../maps/zhihui.posegraph | Bin 0 -> 6120972 bytes
.../maps/zhihui.yaml | 7 +
src/gc_navigation2_slamtoolbox/package.xml | 18 +
.../params/gc_navigation_amcl.yaml | 413 +++++
.../params/gc_navigation_slam.yaml | 343 ++++
.../params/gc_navigation_slam.yaml.bak | 343 ++++
.../world/zhihui/model.config | 11 +
.../world/zhihui/model.sdf | 471 ++++++
src/gc_navigation_fish/CMakeLists.txt | 29 +
.../launch/gc_navigation.launch.py | 34 +
src/gc_navigation_fish/maps/test_map.pgm | Bin 0 -> 57839 bytes
src/gc_navigation_fish/maps/test_map.yaml | 7 +
src/gc_navigation_fish/package.xml | 18 +
.../param/gc_navigation.yaml | 408 +++++
.../param/navigation_test.yaml | 173 ++
src/gc_slam_toolbox_fish/CMakeLists.txt | 29 +
src/gc_slam_toolbox_fish/config/gc_2d.lua | 63 +
.../launch/catograph.launch.py | 63 +
src/gc_slam_toolbox_fish/package.xml | 18 +
src/ground_slam/.gitignore | 51 +
src/ground_slam/CMakeLists.txt | 118 ++
src/ground_slam/LICENSE.md | 674 ++++++++
src/ground_slam/README.md | 138 ++
src/ground_slam/cmake/FindEigen3.cmake | 94 ++
src/ground_slam/cmake/FindFFTW3.cmake | 18 +
src/ground_slam/configs/config_HD.yaml | 49 +
src/ground_slam/configs/config_geekplus.yaml | 50 +
src/ground_slam/configs/config_ntu.yaml | 49 +
src/ground_slam/figures/data_association.jpg | Bin 0 -> 2787856 bytes
src/ground_slam/figures/features_small.jpg | Bin 0 -> 904199 bytes
src/ground_slam/figures/fig1.jpg | Bin 0 -> 1238765 bytes
src/ground_slam/figures/loop_error.jpg | Bin 0 -> 1067652 bytes
src/ground_slam/figures/loop_trajectory.jpg | Bin 0 -> 1285624 bytes
src/ground_slam/figures/pipeline.png | Bin 0 -> 1649958 bytes
src/ground_slam/figures/rmse_curve.jpg | Bin 0 -> 304448 bytes
src/ground_slam/figures/sample_images.jpg | Bin 0 -> 793805 bytes
src/ground_slam/figures/trajectory.jpg | Bin 0 -> 390283 bytes
src/ground_slam/figures/video.png | Bin 0 -> 117672 bytes
src/ground_slam/include/camera.h | 59 +
src/ground_slam/include/circ_shift.h | 252 +++
src/ground_slam/include/correlation_flow.h | 36 +
src/ground_slam/include/dataset.h | 30 +
src/ground_slam/include/edge.h | 31 +
src/ground_slam/include/frame.h | 45 +
src/ground_slam/include/loop_closure.h | 43 +
src/ground_slam/include/map.h | 81 +
src/ground_slam/include/map_builder.h | 83 +
src/ground_slam/include/map_stitcher.h | 42 +
.../angle_local_parameterization.h | 66 +
.../include/optimization_2d/normalize_angle.h | 52 +
.../include/optimization_2d/pose_graph_2d.h | 33 +
.../pose_graph_2d_error_term.h | 183 +++
.../include/optimization_2d/types.h | 116 ++
src/ground_slam/include/read_configs.h | 137 ++
src/ground_slam/include/thread_publisher.h | 32 +
src/ground_slam/include/timer.h | 27 +
src/ground_slam/include/utils.h | 82 +
src/ground_slam/include/visualization.h | 65 +
src/ground_slam/main.cpp | 110 ++
src/ground_slam/package.xml | 27 +
src/ground_slam/src/camera.cc | 243 +++
src/ground_slam/src/correlation_flow.cc | 228 +++
src/ground_slam/src/dataset.cc | 55 +
src/ground_slam/src/edge.cc | 9 +
src/ground_slam/src/frame.cc | 77 +
src/ground_slam/src/loop_closure.cc | 74 +
src/ground_slam/src/map.cc | 105 ++
src/ground_slam/src/map_builder.cc | 334 ++++
src/ground_slam/src/map_stitcher.cc | 149 ++
.../src/optimization_2d/pose_graph_2d.cc | 221 +++
src/ground_slam/src/thread_publisher.cc | 68 +
src/ground_slam/src/timer.cc | 35 +
src/ground_slam/src/utils.cc | 183 +++
src/ground_slam/src/visualization.cc | 201 +++
.../GroundSLAM_Supplement_Materia.pdf | Bin 0 -> 2374262 bytes
src/origincar_base/CMakeLists.txt | 89 ++
src/origincar_base/config/ekf.yaml | 48 +
src/origincar_base/config/imu.yaml | 7 +
.../origincar_base/Quaternion_Solution.h | 10 +
.../include/origincar_base/origincar_base.h | 230 +++
.../__pycache__/testtwo.launch.cpython-38.pyc | Bin 0 -> 580 bytes
.../launch/base_serial.launch.py | 51 +
src/origincar_base/launch/ekf.launch.py | 33 +
.../launch/origincar_bringup.launch.py | 99 ++
.../launch/robot_mode_description.launch.py | 27 +
src/origincar_base/msg/Position.msg | 3 +
src/origincar_base/package.xml | 44 +
...cmd_vel_to_ackermann_drive.cpython-310.pyc | Bin 0 -> 2003 bytes
.../scripts/cmd_vel_to_ackermann_drive.py | 47 +
.../src/Quaternion_Solution.cpp | 91 ++
src/origincar_base/src/origincar_base.cpp | 490 ++++++
src/origincar_base/src/origincar_base.cpp.bak | 477 ++++++
src/origincar_description/CMakeLists.txt | 44 +
src/origincar_description/CMakeLists.txt.save | 12 +
.../joint_names_origincar_description.yaml | 1 +
...origincar_description.yaml:Zone.Identifier | Bin 0 -> 25 bytes
...origincar_description.yamlZone.Identifier | Bin 0 -> 25 bytes
.../launch/display.launch | 20 +
.../launch/display.launch.py | 63 +
.../launch/gazebo.launch | 20 +
.../launch/gazebo.launch.py | 68 +
.../meshes/base_link.STL | Bin 0 -> 726384 bytes
.../meshes/base_link.STL:Zone.Identifier | Bin 0 -> 25 bytes
.../meshes/base_link.STLZone.Identifier | Bin 0 -> 25 bytes
.../meshes/down_left_Link.STL | Bin 0 -> 2165384 bytes
.../meshes/down_left_Link.STL:Zone.Identifier | Bin 0 -> 25 bytes
.../meshes/down_left_Link.STLZone.Identifier | Bin 0 -> 25 bytes
.../meshes/down_right_Link.STL | Bin 0 -> 2165384 bytes
.../down_right_Link.STL:Zone.Identifier | Bin 0 -> 25 bytes
.../down_right_Link.STLZone.Identifier | Bin 0 -> 25 bytes
.../meshes/up_left_Link.STL | Bin 0 -> 2165384 bytes
.../meshes/up_left_Link.STL:Zone.Identifier | Bin 0 -> 25 bytes
.../meshes/up_left_Link.STLZone.Identifier | Bin 0 -> 25 bytes
.../meshes/up_right_Link.STL | Bin 0 -> 2165384 bytes
.../meshes/up_right_Link.STL:Zone.Identifier | Bin 0 -> 25 bytes
.../meshes/up_right_Link.STLZone.Identifier | Bin 0 -> 25 bytes
src/origincar_description/package.xml | 24 +
src/origincar_description/rviz/README | 1 +
src/origincar_description/rviz/display.rviz | 186 +++
src/origincar_description/urdf/origincar.urdf | 471 ++++++
.../urdf/origincar.xacro | 135 ++
src/origincar_description/world/fishbot.world | 626 ++++++++
.../world/gc_world.world | 893 +++++++++++
src/origincar_description/world/test.world | 828 ++++++++++
src/origincar_description/world/zhihui.world | 791 +++++++++
src/origincar_msg/CMakeLists.txt | 34 +
src/origincar_msg/msg/Data.msg | 3 +
src/origincar_msg/msg/Sign.msg | 1 +
src/origincar_msg/package.xml | 21 +
src/zbw_slamtoolbox/CMakeLists.txt | 31 +
.../config/mapper_params_online_async.yaml | 22 +
.../mapper_params_online_sync copy.yaml | 77 +
src/zbw_slamtoolbox/config/navigation.yaml | 0
.../launch/navigation.launch.py | 86 +
.../launch/slamtoolbox.launch.py | 27 +
src/zbw_slamtoolbox/package.xml | 18 +
竞赛方案_第21届智能汽车竞赛地瓜机器人赛项.md | 222 +++
调试记录.log | 169 ++
277 files changed, 24464 insertions(+)
create mode 100644 .gitignore
create mode 100644 .vscode/c_cpp_properties.json
create mode 100644 .vscode/launch.json
create mode 100644 .vscode/settings.json
create mode 100644 CLAUDE.md
create mode 100644 README.md
create mode 100644 bashes/README.md
create mode 100644 bashes/radar-driver-switch.sh
create mode 100644 bashes/radar-driver-switch.sh.bak
create mode 100644 dependencies/dependencies.txt
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/README.md
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/CMakeLists.txt
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/include/lslidar_driver/input.h
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/include/lslidar_driver/lsiosr.h
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/include/lslidar_driver/lslidar_driver.h
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lslidar_double_launch.py
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10_net_launch.py
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10_uart_launch.py
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10p_net_launch.py
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10p_uart_launch.py
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10_launch.py
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10_net_launch.py
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10p_launch.py
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10p_net_launch.py
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/viewer_scan_launch.py
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/package.xml
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsm10_net.yaml
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsm10p_net.yaml
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsn10_net.yaml
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsn10p_net.yaml
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsm10.yaml
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsm10_p.yaml
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsn10.yaml
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsn10p.yaml
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/rviz/lslidar.rviz
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/input.cc
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lsiosr.cpp
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver.cc
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver.cc.bak
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver_node.cc
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/CMakeLists.txt
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarDifop.msg
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarPacket.msg
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarPoint.msg
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarScan.msg
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarSweep.msg
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/package.xml
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/version.txt
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/wheeltec_udev.sh
create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/镭神Lsx雷达旋转角度.png
create mode 100644 src/LSLIDAR_X_ROS2-20240228/wheeltec_lidar.launch.py
create mode 100644 src/LSLIDAR_X_ROS2-20240228/wheeltec_lidar.launch.py仅在WHEELTEC镜像中使用
create mode 100644 src/cyy_navigation2/CMakeLists.txt
create mode 100644 src/cyy_navigation2/bt/follow_point.xml
create mode 100644 src/cyy_navigation2/bt/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml
create mode 100644 src/cyy_navigation2/bt/navigate_through_poses_w_replanning_and_recovery.xml
create mode 100644 src/cyy_navigation2/bt/navigate_to_pose_w_replanning_and_recovery.xml
create mode 100644 src/cyy_navigation2/bt/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml
create mode 100644 src/cyy_navigation2/bt/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml
create mode 100644 src/cyy_navigation2/bt/navigate_w_replanning_distance.xml
create mode 100644 src/cyy_navigation2/bt/navigate_w_replanning_only_if_goal_is_updated.xml
create mode 100644 src/cyy_navigation2/bt/navigate_w_replanning_only_if_path_becomes_invalid.xml
create mode 100644 src/cyy_navigation2/bt/navigate_w_replanning_speed.xml
create mode 100644 src/cyy_navigation2/bt/navigate_w_replanning_time.xml
create mode 100644 src/cyy_navigation2/bt/odometry_calibration.xml
create mode 100644 src/cyy_navigation2/config/nav2_params.yaml
create mode 100644 src/cyy_navigation2/launch/car_bringup.launch.py
create mode 100644 src/cyy_navigation2/launch/cyy_nav.launch.py
create mode 100644 src/cyy_navigation2/launch/cyy_nav_box.launch.py
create mode 100644 src/cyy_navigation2/maps/cyy_map.data
create mode 100644 src/cyy_navigation2/maps/cyy_map.pgm
create mode 100644 src/cyy_navigation2/maps/cyy_map.yaml
create mode 100644 src/cyy_navigation2/maps/cyy_map1.pgm
create mode 100644 src/cyy_navigation2/maps/cyy_map1.yaml
create mode 100644 src/cyy_navigation2/package.xml
create mode 100644 src/cyy_navigation2/param/nav2_params.yaml
create mode 100644 src/cyy_navigation2/param/slam_toolbox_localization.yaml
create mode 100644 src/cyy_slamtoolbox/CMakeLists.txt
create mode 100644 src/cyy_slamtoolbox/config/angular_filter_example.yaml
create mode 100644 src/cyy_slamtoolbox/config/box_filter_example.yaml
create mode 100644 src/cyy_slamtoolbox/config/footprint_filter_example.yaml
create mode 100644 src/cyy_slamtoolbox/config/intensity_filter_example.yaml
create mode 100644 src/cyy_slamtoolbox/config/laser_filter_config.yaml
create mode 100644 src/cyy_slamtoolbox/config/mapper_params_lifelong.yaml
create mode 100644 src/cyy_slamtoolbox/config/mapper_params_localization.yaml
create mode 100644 src/cyy_slamtoolbox/config/mapper_params_offline.yaml
create mode 100644 src/cyy_slamtoolbox/config/mapper_params_online_async.yaml
create mode 100644 src/cyy_slamtoolbox/config/mapper_params_online_sync.yaml
create mode 100644 src/cyy_slamtoolbox/config/mask_filter_example.yaml
create mode 100644 src/cyy_slamtoolbox/config/median_filter_example.yaml
create mode 100644 src/cyy_slamtoolbox/config/median_spatial_filter_example.yaml
create mode 100644 src/cyy_slamtoolbox/config/multiple_filters_example.yaml
create mode 100644 src/cyy_slamtoolbox/config/pass_through_example.yaml
create mode 100644 src/cyy_slamtoolbox/config/polygon_filter_example.yaml
create mode 100644 src/cyy_slamtoolbox/config/range_filter_example.yaml
create mode 100644 src/cyy_slamtoolbox/config/scan_blob_filter_example.yaml
create mode 100644 src/cyy_slamtoolbox/config/sector_filter_example.yaml
create mode 100644 src/cyy_slamtoolbox/config/shadow_filter_example.yaml
create mode 100644 src/cyy_slamtoolbox/config/slam_toolbox_default.rviz
create mode 100644 src/cyy_slamtoolbox/config/speckle_filter_example.yaml
create mode 100644 src/cyy_slamtoolbox/launch/cyy_slam_toolbox_launch.launch.py
create mode 100644 src/cyy_slamtoolbox/launch/cyy_slam_toolbox_location.launch.py
create mode 100644 src/cyy_slamtoolbox/launch/filter.launch.py
create mode 100644 src/cyy_slamtoolbox/package.xml
create mode 100644 src/gc_navigation2_real/CMakeLists.txt
create mode 100644 src/gc_navigation2_real/behavior_tree/nav_to_pose_ackermann.xml
create mode 100644 src/gc_navigation2_real/config/slam_toolbox_async_real.yaml
create mode 100644 src/gc_navigation2_real/config/slam_toolbox_localization_real.yaml
create mode 100644 src/gc_navigation2_real/launch/real_bringup.launch.py
create mode 100644 src/gc_navigation2_real/launch/real_nav2_slam.launch.py
create mode 100644 src/gc_navigation2_real/launch/real_nav2_slam_online.launch.py
create mode 100644 src/gc_navigation2_real/launch/real_slam_mapping.launch.py
create mode 100644 src/gc_navigation2_real/maps/Readme.txt
create mode 100644 src/gc_navigation2_real/package.xml
create mode 100644 src/gc_navigation2_real/params/gc_navigation_slam_real.yaml
create mode 100644 src/gc_navigation2_slamtoolbox/CMakeLists.txt
create mode 100644 src/gc_navigation2_slamtoolbox/config/mapper_params_lifelong.yaml
create mode 100644 src/gc_navigation2_slamtoolbox/config/mapper_params_localization.yaml
create mode 100644 src/gc_navigation2_slamtoolbox/config/mapper_params_offline.yaml
create mode 100644 src/gc_navigation2_slamtoolbox/config/mapper_params_online_multi_async.yaml
create mode 100644 src/gc_navigation2_slamtoolbox/config/mapper_params_online_sync.yaml
create mode 100644 src/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml
create mode 100644 src/gc_navigation2_slamtoolbox/config/slam_toolbox_async.yaml
create mode 100644 src/gc_navigation2_slamtoolbox/config/slam_toolbox_localization.yaml
create mode 100644 src/gc_navigation2_slamtoolbox/config/slam_toolbox_mapping.yaml
create mode 100644 src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_amcl.launch.py
create mode 100644 src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam.launch.py
create mode 100644 src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online.launch.py
create mode 100644 src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online_real.launch.py
create mode 100644 src/gc_navigation2_slamtoolbox/launch/gc_nav_slamtoolbox_lifelong.launch.py
create mode 100644 src/gc_navigation2_slamtoolbox/launch/gc_nav_slamtoolbox_location.launch.py
create mode 100644 src/gc_navigation2_slamtoolbox/launch/gc_nav_slamtoolbox_on_async.launch.py
create mode 100644 src/gc_navigation2_slamtoolbox/launch/gc_slam_mapping.launch.py
create mode 100644 src/gc_navigation2_slamtoolbox/maps/my_map.data
create mode 100644 src/gc_navigation2_slamtoolbox/maps/my_map.pgm
create mode 100644 src/gc_navigation2_slamtoolbox/maps/my_map.posegraph
create mode 100644 src/gc_navigation2_slamtoolbox/maps/my_map.yaml
create mode 100644 src/gc_navigation2_slamtoolbox/maps/zhihui.data
create mode 100644 src/gc_navigation2_slamtoolbox/maps/zhihui.pgm
create mode 100644 src/gc_navigation2_slamtoolbox/maps/zhihui.posegraph
create mode 100644 src/gc_navigation2_slamtoolbox/maps/zhihui.yaml
create mode 100644 src/gc_navigation2_slamtoolbox/package.xml
create mode 100644 src/gc_navigation2_slamtoolbox/params/gc_navigation_amcl.yaml
create mode 100644 src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml
create mode 100644 src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml.bak
create mode 100644 src/gc_navigation2_slamtoolbox/world/zhihui/model.config
create mode 100644 src/gc_navigation2_slamtoolbox/world/zhihui/model.sdf
create mode 100644 src/gc_navigation_fish/CMakeLists.txt
create mode 100644 src/gc_navigation_fish/launch/gc_navigation.launch.py
create mode 100644 src/gc_navigation_fish/maps/test_map.pgm
create mode 100644 src/gc_navigation_fish/maps/test_map.yaml
create mode 100644 src/gc_navigation_fish/package.xml
create mode 100644 src/gc_navigation_fish/param/gc_navigation.yaml
create mode 100644 src/gc_navigation_fish/param/navigation_test.yaml
create mode 100644 src/gc_slam_toolbox_fish/CMakeLists.txt
create mode 100644 src/gc_slam_toolbox_fish/config/gc_2d.lua
create mode 100644 src/gc_slam_toolbox_fish/launch/catograph.launch.py
create mode 100644 src/gc_slam_toolbox_fish/package.xml
create mode 100755 src/ground_slam/.gitignore
create mode 100755 src/ground_slam/CMakeLists.txt
create mode 100644 src/ground_slam/LICENSE.md
create mode 100644 src/ground_slam/README.md
create mode 100755 src/ground_slam/cmake/FindEigen3.cmake
create mode 100755 src/ground_slam/cmake/FindFFTW3.cmake
create mode 100644 src/ground_slam/configs/config_HD.yaml
create mode 100644 src/ground_slam/configs/config_geekplus.yaml
create mode 100644 src/ground_slam/configs/config_ntu.yaml
create mode 100644 src/ground_slam/figures/data_association.jpg
create mode 100644 src/ground_slam/figures/features_small.jpg
create mode 100644 src/ground_slam/figures/fig1.jpg
create mode 100644 src/ground_slam/figures/loop_error.jpg
create mode 100644 src/ground_slam/figures/loop_trajectory.jpg
create mode 100644 src/ground_slam/figures/pipeline.png
create mode 100644 src/ground_slam/figures/rmse_curve.jpg
create mode 100644 src/ground_slam/figures/sample_images.jpg
create mode 100644 src/ground_slam/figures/trajectory.jpg
create mode 100644 src/ground_slam/figures/video.png
create mode 100644 src/ground_slam/include/camera.h
create mode 100644 src/ground_slam/include/circ_shift.h
create mode 100644 src/ground_slam/include/correlation_flow.h
create mode 100644 src/ground_slam/include/dataset.h
create mode 100644 src/ground_slam/include/edge.h
create mode 100644 src/ground_slam/include/frame.h
create mode 100644 src/ground_slam/include/loop_closure.h
create mode 100644 src/ground_slam/include/map.h
create mode 100644 src/ground_slam/include/map_builder.h
create mode 100644 src/ground_slam/include/map_stitcher.h
create mode 100644 src/ground_slam/include/optimization_2d/angle_local_parameterization.h
create mode 100644 src/ground_slam/include/optimization_2d/normalize_angle.h
create mode 100644 src/ground_slam/include/optimization_2d/pose_graph_2d.h
create mode 100644 src/ground_slam/include/optimization_2d/pose_graph_2d_error_term.h
create mode 100644 src/ground_slam/include/optimization_2d/types.h
create mode 100644 src/ground_slam/include/read_configs.h
create mode 100644 src/ground_slam/include/thread_publisher.h
create mode 100755 src/ground_slam/include/timer.h
create mode 100644 src/ground_slam/include/utils.h
create mode 100644 src/ground_slam/include/visualization.h
create mode 100755 src/ground_slam/main.cpp
create mode 100644 src/ground_slam/package.xml
create mode 100644 src/ground_slam/src/camera.cc
create mode 100644 src/ground_slam/src/correlation_flow.cc
create mode 100644 src/ground_slam/src/dataset.cc
create mode 100644 src/ground_slam/src/edge.cc
create mode 100644 src/ground_slam/src/frame.cc
create mode 100644 src/ground_slam/src/loop_closure.cc
create mode 100644 src/ground_slam/src/map.cc
create mode 100644 src/ground_slam/src/map_builder.cc
create mode 100644 src/ground_slam/src/map_stitcher.cc
create mode 100644 src/ground_slam/src/optimization_2d/pose_graph_2d.cc
create mode 100644 src/ground_slam/src/thread_publisher.cc
create mode 100755 src/ground_slam/src/timer.cc
create mode 100644 src/ground_slam/src/utils.cc
create mode 100644 src/ground_slam/src/visualization.cc
create mode 100644 src/ground_slam/supplementary/GroundSLAM_Supplement_Materia.pdf
create mode 100644 src/origincar_base/CMakeLists.txt
create mode 100644 src/origincar_base/config/ekf.yaml
create mode 100644 src/origincar_base/config/imu.yaml
create mode 100644 src/origincar_base/include/origincar_base/Quaternion_Solution.h
create mode 100644 src/origincar_base/include/origincar_base/origincar_base.h
create mode 100644 src/origincar_base/launch/__pycache__/testtwo.launch.cpython-38.pyc
create mode 100644 src/origincar_base/launch/base_serial.launch.py
create mode 100644 src/origincar_base/launch/ekf.launch.py
create mode 100644 src/origincar_base/launch/origincar_bringup.launch.py
create mode 100644 src/origincar_base/launch/robot_mode_description.launch.py
create mode 100644 src/origincar_base/msg/Position.msg
create mode 100644 src/origincar_base/package.xml
create mode 100644 src/origincar_base/scripts/__pycache__/cmd_vel_to_ackermann_drive.cpython-310.pyc
create mode 100755 src/origincar_base/scripts/cmd_vel_to_ackermann_drive.py
create mode 100644 src/origincar_base/src/Quaternion_Solution.cpp
create mode 100644 src/origincar_base/src/origincar_base.cpp
create mode 100644 src/origincar_base/src/origincar_base.cpp.bak
create mode 100644 src/origincar_description/CMakeLists.txt
create mode 100644 src/origincar_description/CMakeLists.txt.save
create mode 100644 src/origincar_description/config/joint_names_origincar_description.yaml
create mode 100644 src/origincar_description/config/joint_names_origincar_description.yaml:Zone.Identifier
create mode 100644 src/origincar_description/config/joint_names_origincar_description.yamlZone.Identifier
create mode 100644 src/origincar_description/launch/display.launch
create mode 100644 src/origincar_description/launch/display.launch.py
create mode 100644 src/origincar_description/launch/gazebo.launch
create mode 100644 src/origincar_description/launch/gazebo.launch.py
create mode 100644 src/origincar_description/meshes/base_link.STL
create mode 100644 src/origincar_description/meshes/base_link.STL:Zone.Identifier
create mode 100644 src/origincar_description/meshes/base_link.STLZone.Identifier
create mode 100644 src/origincar_description/meshes/down_left_Link.STL
create mode 100644 src/origincar_description/meshes/down_left_Link.STL:Zone.Identifier
create mode 100644 src/origincar_description/meshes/down_left_Link.STLZone.Identifier
create mode 100644 src/origincar_description/meshes/down_right_Link.STL
create mode 100644 src/origincar_description/meshes/down_right_Link.STL:Zone.Identifier
create mode 100644 src/origincar_description/meshes/down_right_Link.STLZone.Identifier
create mode 100644 src/origincar_description/meshes/up_left_Link.STL
create mode 100644 src/origincar_description/meshes/up_left_Link.STL:Zone.Identifier
create mode 100644 src/origincar_description/meshes/up_left_Link.STLZone.Identifier
create mode 100644 src/origincar_description/meshes/up_right_Link.STL
create mode 100644 src/origincar_description/meshes/up_right_Link.STL:Zone.Identifier
create mode 100644 src/origincar_description/meshes/up_right_Link.STLZone.Identifier
create mode 100644 src/origincar_description/package.xml
create mode 100644 src/origincar_description/rviz/README
create mode 100644 src/origincar_description/rviz/display.rviz
create mode 100644 src/origincar_description/urdf/origincar.urdf
create mode 100644 src/origincar_description/urdf/origincar.xacro
create mode 100644 src/origincar_description/world/fishbot.world
create mode 100644 src/origincar_description/world/gc_world.world
create mode 100644 src/origincar_description/world/test.world
create mode 100644 src/origincar_description/world/zhihui.world
create mode 100644 src/origincar_msg/CMakeLists.txt
create mode 100644 src/origincar_msg/msg/Data.msg
create mode 100644 src/origincar_msg/msg/Sign.msg
create mode 100644 src/origincar_msg/package.xml
create mode 100644 src/zbw_slamtoolbox/CMakeLists.txt
create mode 100644 src/zbw_slamtoolbox/config/mapper_params_online_async.yaml
create mode 100644 src/zbw_slamtoolbox/config/mapper_params_online_sync copy.yaml
create mode 100644 src/zbw_slamtoolbox/config/navigation.yaml
create mode 100644 src/zbw_slamtoolbox/launch/navigation.launch.py
create mode 100644 src/zbw_slamtoolbox/launch/slamtoolbox.launch.py
create mode 100644 src/zbw_slamtoolbox/package.xml
create mode 100644 竞赛方案_第21届智能汽车竞赛地瓜机器人赛项.md
create mode 100644 调试记录.log
diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..1dffd57
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,12 @@
+# Build artifacts
+build
+
+# Install artifacts
+install
+
+# Log artifacts
+log
+
+# VSCode database
+.vscode/browse.vc.db*
+
diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
new file mode 100644
index 0000000..753ea31
--- /dev/null
+++ b/.vscode/c_cpp_properties.json
@@ -0,0 +1,26 @@
+{
+ "configurations": [
+ {
+ "browse": {
+ "databaseFilename": "${workspaceFolder}/.vscode/browse.vc.db",
+ "limitSymbolsToIncludedHeaders": false
+ },
+ "includePath": [
+ "/home/sunrise/yiliao_ws/install/origincar_base/include/**",
+ "/home/sunrise/yiliao_ws/install/origincar_msg/include/**",
+ "/home/sunrise/yiliao_ws/install/lslidar_msgs/include/**",
+ "/opt/ros/humble/include/**",
+ "/home/sunrise/yiliao_ws/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/include/**",
+ "/home/sunrise/yiliao_ws/src/ground_slam/include/**",
+ "/home/sunrise/yiliao_ws/src/origincar_base/include/**",
+ "/usr/include/**"
+ ],
+ "name": "ros2",
+ "intelliSenseMode": "gcc-arm64",
+ "compilerPath": "/usr/bin/gcc",
+ "cStandard": "gnu11",
+ "cppStandard": "c++17"
+ }
+ ],
+ "version": 4
+}
\ No newline at end of file
diff --git a/.vscode/launch.json b/.vscode/launch.json
new file mode 100644
index 0000000..7f63e8f
--- /dev/null
+++ b/.vscode/launch.json
@@ -0,0 +1,28 @@
+{
+ // 使用 IntelliSense 了解相关属性。
+ // 悬停以查看现有属性的描述。
+ // 欲了解更多信息,请访问: https://go.microsoft.com/fwlink/?linkid=830387
+ "version": "0.2.0",
+ "configurations": [
+ {
+ "name": "ROS2 C++ Debug",
+ "request": "launch",
+ "type": "cppdbg",
+ "MIMode": "gdb",
+ "cwd": "${workspaceFolder}",
+ "program": "${workspaceFolder}/install/xxx"
+ },
+ {
+ "name": "ROS2: Launch grab_service",
+ "request": "launch",
+ "target": "${workspaceFolder}/install/xxx",
+ "launch": ["rviz", "gz", "gzserver", "gzclient"],
+ "type": "ros2"
+ },
+ {
+ "name": "ROS: Attach",
+ "request": "attach",
+ "type": "ros2"
+ }
+ ]
+}
\ No newline at end of file
diff --git a/.vscode/settings.json b/.vscode/settings.json
new file mode 100644
index 0000000..1124fb9
--- /dev/null
+++ b/.vscode/settings.json
@@ -0,0 +1,20 @@
+{
+ "yaml.schemas": {
+ "https://www.schemastore.org/package.json": "file:///home/guoch/test_ws/src/gc_navigation2_slamtoolbox/config/mapper_params_localization.yaml"
+ },
+ "ROS2.distro": "humble",
+ "python.autoComplete.extraPaths": [
+ "/home/sunrise/yiliao_ws/install/origincar_base/local/lib/python3.10/dist-packages",
+ "/home/sunrise/yiliao_ws/install/origincar_msg/local/lib/python3.10/dist-packages",
+ "/home/sunrise/yiliao_ws/install/lslidar_msgs/local/lib/python3.10/dist-packages",
+ "/opt/ros/humble/lib/python3.10/site-packages",
+ "/opt/ros/humble/local/lib/python3.10/dist-packages"
+ ],
+ "python.analysis.extraPaths": [
+ "/home/sunrise/yiliao_ws/install/origincar_base/local/lib/python3.10/dist-packages",
+ "/home/sunrise/yiliao_ws/install/origincar_msg/local/lib/python3.10/dist-packages",
+ "/home/sunrise/yiliao_ws/install/lslidar_msgs/local/lib/python3.10/dist-packages",
+ "/opt/ros/humble/lib/python3.10/site-packages",
+ "/opt/ros/humble/local/lib/python3.10/dist-packages"
+ ]
+}
\ No newline at end of file
diff --git a/CLAUDE.md b/CLAUDE.md
new file mode 100644
index 0000000..eab6c1c
--- /dev/null
+++ b/CLAUDE.md
@@ -0,0 +1,165 @@
+# CLAUDE.md
+
+## 项目概述
+
+这是一个 ROS 2 Humble 工作空间,用于 Ackermann 转向小车的 Gazebo 仿真、SLAM 建图和 Nav2 导航。同时支持实车部署(无 Gazebo)。
+
+## 工作空间结构
+
+```
+test_ws/
+├── src/
+│ ├── gc_navigation2_slamtoolbox/ # 仿真版:导航+SLAM 启动文件和配置
+│ │ ├── launch/ # Python launch 文件
+│ │ ├── config/ # slam_toolbox 参数
+│ │ ├── params/ # Nav2 参数
+│ │ ├── maps/ # 地图文件 (.pgm, .yaml, .posegraph)
+│ │ └── world/ # Gazebo 模型 (zhihui/)
+│ ├── gc_navigation2_real/ # 实车版:导航+SLAM 启动文件和配置(无 Gazebo)
+│ │ ├── launch/ # Python launch 文件
+│ │ ├── config/ # slam_toolbox 参数(实车调优)
+│ │ ├── params/ # Nav2 参数(use_sim_time=false)
+│ │ ├── maps/ # 实车地图存放目录
+│ │ └── behavior_tree/ # 阿克曼自定义行为树
+│ ├── origincar_description/ # 机器人 URDF 和 Gazebo world
+│ │ ├── urdf/ # origincar.urdf
+│ │ ├── world/ # .world 文件
+│ │ ├── meshes/ # 3D 模型
+│ │ ├── launch/ # display.launch.py
+│ │ └── rviz/ # RViz 配置
+│ ├── origincar_base/ # 实车底盘驱动(串口通信+EKF+IMU融合)
+│ │ ├── src/ # C++ 源码(origincar_base.cpp)
+│ │ ├── scripts/ # cmd_vel_to_ackermann_drive.py
+│ │ ├── launch/ # origincar_bringup / base_serial / ekf
+│ │ └── config/ # ekf.yaml / imu.yaml
+│ ├── origincar_msg/ # 自定义 ROS 2 消息
+│ │ └── msg/ # Data.msg / Sign.msg
+│ └── LSLIDAR_X_ROS2-20240228/ # 镭神激光雷达 ROS 2 驱动
+│ └── src/
+│ ├── lslidar_driver/ # 雷达驱动核心
+│ └── lslidar_msgs/ # 雷达消息定义
+├── build/ # colcon build 输出
+├── install/ # colcon install 输出 (含符号链接)
+└── log/ # 构建日志
+```
+
+## 构建命令
+
+```bash
+cd /home/guoch/test_ws
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install
+source install/setup.bash
+```
+
+## 常用 Launch 文件
+
+### 仿真版 (gc_navigation2_slamtoolbox)
+
+| 文件 | 功能 |
+|------|------|
+| `gc_slam_mapping.launch.py` | Gazebo + SLAM 建图模式 |
+| `gc_nav2_with_slam_online.launch.py` | 已知地图 + SLAM 在线更新 + Nav2 导航 |
+| `gc_nav2_with_slam.launch.py` | 已知地图 + SLAM 定位 + Nav2 导航 |
+| `gc_nav2_with_amcl.launch.py` | AMCL 定位 + Nav2 导航 |
+
+### 实车版 (gc_navigation2_real)
+
+| 文件 | 功能 |
+|------|------|
+| `real_bringup.launch.py` | 基础 bringup(底盘驱动+雷达+TF,无导航) |
+| `real_slam_mapping.launch.py` | 实车 SLAM 建图模式 |
+| `real_nav2_slam.launch.py` | 已知地图 + SLAM 定位 + Nav2 导航 |
+| `real_nav2_slam_online.launch.py` | 已知地图 + SLAM 在线更新 + Nav2 导航 |
+
+## 机器人参数 (origincar)
+
+- **类型**: Ackermann 转向小车
+- **尺寸**: 0.276 × 0.214 × 0.211 m (长×宽×高)
+- **轴距**: 0.143 m(URDF 中前轮 x=0.0715,后轮 x=-0.0715),**轮距**: 0.189 m
+- **轮子**: 半径 0.03 m,厚 0.025 m
+- **激光雷达**: 360°, 0.12~3.5 m, 5 Hz(仿真); 镭神 N10 0.15~12m(实车)
+- **IMU**: 100 Hz, 含高斯噪声
+- **Gazebo 插件**: `gazebo_ros_ackermann_drive`, `gazebo_ros_ray_sensor`, `gazebo_ros_imu_sensor`
+
+## 实车 TF 树
+
+```
+map ──→ odom_combined ──→ base_footprint ──→ base_link ──→ laser
+ ↑ ↑ ↑ ↑
+slam_toolbox EKF static TF URDF/static
+(定位) (融合odom+IMU) (origincar_bringup)
+```
+
+- slam_toolbox 发布 `map → odom_combined`
+- EKF 发布 `odom_combined → base_footprint`
+- origincar_bringup 发布 `base_footprint → base_link`(z=0)、`base_link → laser`
+- robot_state_publisher 发布 URDF 各连杆 TF
+
+### 仿真 vs 实车关键差异
+
+| 方面 | 仿真版 | 实车版 |
+|------|--------|--------|
+| use_sim_time | `true` | `false` |
+| odom_frame | `odom` | `odom_combined`(EKF 融合后) |
+| 底盘驱动 | Gazebo plugin | origincar_base 串口驱动 |
+| 激光雷达 | Gazebo ray plugin | lslidar_driver(镭神 N10) |
+| robot_state_publisher | launch 文件内启动 | origincar_bringup 已包含,勿重复启动 |
+| joint_state_publisher | launch 文件内启动 | origincar_bringup 已包含,勿重复启动 |
+
+## 阿克曼底盘模式
+
+通过 `akmcar` 参数控制,链路如下:
+
+```
+origincar_bringup.launch.py
+ └─ akmcar=LaunchConfiguration('akmcar', default='true')
+ └─ 传入 base_serial.launch.py
+ ├─ IfCondition(true) → origincar_base_node (akm_cmd_vel='ackermann_cmd')
+ │ + cmd_vel_to_ackermann_drive.py (Twist→Ackermann)
+ └─ UnlessCondition(false) → 差速模式(跳过)
+```
+
+数据流:`MPPI cmd_vel(Twist) → cmd_vel_to_ackermann_drive.py → ackermann_cmd(AckermannDriveStamped) → STM32`
+
+关键参数:
+- `cmd_vel_to_ackermann_drive.py`: `wheelbase = 0.143`(与 URDF 一致)
+- `akmcar=true` 时 STM32 接收 `speed + steering_angle`
+- `akmcar=false` 时 STM32 接收 `vx + vy + wz`(固件自行转换)
+
+## 关键设计
+
+1. **map_server** 提供全量静态底图(`/map`)
+2. **slam_toolbox** 负责定位和增量建图,重映射 `/map` → `/slam_map` 避免冲突
+3. **Nav2** 使用 `global_costmap/static_layer ← /map` + `local_costmap/obstacle_layer ← /scan`
+4. 保存地图: `ros2 service call /slam_toolbox/serialize_map ...`
+5. 实车版 launch 文件**不应**自行启动 `robot_state_publisher` 和 `joint_state_publisher`,`origincar_bringup` 已包含
+
+## 地图文件
+
+地图放在 `src/gc_navigation2_slamtoolbox/maps/`(仿真)和 `src/gc_navigation2_real/maps/`(实车),包含:
+- `xxx.pgm` — 地图图像
+- `xxx.yaml` — 地图元数据 (resolution, origin, thresholds)
+- `xxx.posegraph` — 序列化的位姿图
+
+## Gazebo World
+
+World 文件在 `src/origincar_description/world/`:
+- `zhihui.world` — 智慧楼墙体环境(内联模型)
+- `test.world` — 测试环境
+- `fishbot.world` / `gc_world.world` — 其他场景
+
+## 实车底盘驱动包 (origincar_base)
+
+- `origincar_base_node`(C++): 串口读写(/dev/ttyACM0, 115200bps)、航迹推算、四元数姿态解算(Mahony AHRS)
+- `cmd_vel_to_ackermann_drive.py`(Python): Twist → AckermannDriveStamped 转换,wheelbase=0.143
+- 帧协议: 24 字节收(帧头0x7B/帧尾0x7D)/ 11 字节发
+- EKF 融合: `/odom` + IMU → `/odom_combined`,`two_d_mode=true`
+
+## 注意事项
+
+- 使用 `--symlink-install` 构建,Python launch 文件修改后无需重新编译
+- `.world` 文件不应使用 `model://` 外部引用,应将模型内联定义
+- `robot_base_frame` 在 ackermann plugin 中应设为 `base_footprint`
+- 实车 `gc_navigation2_real` 的 launch 文件调用 `origincar_bringup.launch.py` 时,不再重复启动 `robot_state_publisher` 和 `joint_state_publisher`
+- 轴距参数在三处需保持一致:URDF(0.143m)、`cmd_vel_to_ackermann_drive.py`(0.143m)、Nav2 planner `minimum_turning_radius`(0.40m)
diff --git a/README.md b/README.md
new file mode 100644
index 0000000..4cf7969
--- /dev/null
+++ b/README.md
@@ -0,0 +1,105 @@
+# 智慧医疗2026年代码仓库
+
+# 一、文件夹说明
+## `dependencies`
+`dependencies.txt`存储依赖名称
+
+## `src`
+存放源代码
+
+## `bashes`
+自动脚本的存放路径
+
+---
+
+# 二、关于雷达驱动自动配置脚本的说明
+## 1. 配置说明:在设备上配置开机自启动(仅使用雷达的情况直接看第二点)
+1. 放到固定位置并授权
+ ```bash
+ sudo cp 你的脚本路径 /usr/local/bin/radar-driver-switch.sh
+ sudo chmod +x /usr/local/bin/radar-driver-switch.sh
+ sudo chown root:root /usr/local/bin/radar-driver-switch.sh
+ ```
+2. 创建 systemd 服务文件
+ ```bash
+ sudo vim /etc/systemd/system/radar-driver-switch.service
+ ```
+ 编辑内容:
+ ```ini
+ [Unit]
+ Description=雷达驱动自动切换程序
+ After=sysfs.target udev.target systemd-udevd.service
+ Before=multi-user.target
+
+ [Service]
+ Type=oneshot
+ ExecStart=/usr/local/bin/radar-driver-switch.sh
+ User=root
+ Group=root
+ StandardOutput=journal+console
+ StandardError=journal+console
+
+ [Install]
+ WantedBy=multi-user.target
+ ```
+3. 应用开机自启
+ ```bash
+ sudo systemctl daemon-reload
+ sudo systemctl enable radar-driver-switch.service
+ ```
+4. 测试与debug
+ - 测试一次
+ ```bash
+ sudo systemctl start radar-driver-switch.service
+ ```
+ - 查看状态 / 日志(排查必用)
+ ```bash
+ sudo systemctl status radar-driver-switch.service
+ ```
+ - 看 dmesg 日志
+ ```bash
+ dmesg | grep 雷达驱动自动切换程序
+ ```
+---
+### 特别说明
+1. 关于退出码对应的情况,详见sh文件开头
+2. 日志输出在`/dev/kmsg`文件中,使用`dmesg | grep 雷达驱动自动切换程序`命令即可查看
+
+## 2. 使用说明:
+1. 设备开机自动启动脚本连接雷达。若开机时雷达与设备没有物理连接,则需要手动启动脚本连接雷达
+2. 查看雷达是否物理连接
+ ```bash
+ lsusb
+ ```
+ 返回的数据中,如果有` ID 1a86:55d4 QinHeng Electronics USB Single Serial`,则物理连接成功。
+3. 查看是否成功驱动雷达
+ ```bash
+ ls /dev/tty*
+ ```
+ 看返回数据,默认串口连接设备为`ttyACM*`,名为`ttyACM0`的设备是IMU,如果存在`ttyACM1`,则雷达连接失败;
+
+ 如果出现`ttyCH340`,则说明雷达设备驱动成功
+
+---
+
+
+# 三、指令
+启动雷达:
+`ros2 launch lslidar_driver lsn10_launch.py `
+open new terminal
+```bash
+ros2 topic pub -1 /lslidar_order std_msgs/msg/Int8 data:\ 1\ # (open radar)
+ros2 topic pub -1 /lslidar_order std_msgs/msg/Int8 data:\ 0\ # (close radar)
+```
+
+# 四、其它说明
+我设置了一些自定义指令,方便终端调试:
+```bash
+rosbuild # 编译指定包
+build_debug # 以调试模式编译指定包
+foxglove # 启动foxbridge
+```
+启动键盘控制:
+`ros2 run teleop_twist_keyboard teleop_twist_keyboard `
+
+# 五、日志
diff --git a/bashes/README.md b/bashes/README.md
new file mode 100644
index 0000000..42b9182
--- /dev/null
+++ b/bashes/README.md
@@ -0,0 +1,52 @@
+## 关于雷达驱动自动配置脚本的说明
+### 在设备上配置开机自启动
+1. 放到固定位置并授权
+ ```bash
+ sudo cp 你的脚本路径 /usr/local/bin/radar-driver-switch.sh
+ sudo chmod +x /usr/local/bin/radar-driver-switch.sh
+ sudo chown root:root /usr/local/bin/radar-driver-switch.sh
+ ```
+2. 创建 systemd 服务文件
+ ```bash
+ sudo vim /etc/systemd/system/radar-driver-switch.service
+ ```
+ 编辑内容:
+ ```ini
+ [Unit]
+ Description=雷达驱动自动切换程序
+ After=sysfs.target udev.target systemd-udevd.service
+ Before=multi-user.target
+
+ [Service]
+ Type=oneshot
+ ExecStart=/usr/local/bin/radar-driver-switch.sh
+ User=root
+ Group=root
+ StandardOutput=journal+console
+ StandardError=journal+console
+
+ [Install]
+ WantedBy=multi-user.target
+ ```
+3. 应用开机自启
+ ```bash
+ sudo systemctl daemon-reload
+ sudo systemctl enable radar-driver-switch.service
+ ```
+4. 测试与debug
+ - 测试一次
+ ```bash
+ sudo systemctl start radar-driver-switch.service
+ ```
+ - 查看状态 / 日志(排查必用)
+ ```bash
+ sudo systemctl status radar-driver-switch.service
+ ```
+ - 看 dmesg 日志
+ ```bash
+ dmesg | grep 雷达驱动自动切换程序
+ ```
+---
+### 特别说明
+1. 关于退出码对应的情况,详见sh文件开头
+2. 日志输出在`/dev/kmsg`文件中,使用`dmesg | grep 雷达驱动自动切换程序`命令即可查看
\ No newline at end of file
diff --git a/bashes/radar-driver-switch.sh b/bashes/radar-driver-switch.sh
new file mode 100644
index 0000000..c64b9ce
--- /dev/null
+++ b/bashes/radar-driver-switch.sh
@@ -0,0 +1,101 @@
+#!/bin/bash
+
+# 退出码说明
+# 0 - 成功
+# 1 - 错误:等待TTY设备就绪超时
+# 2 - 错误:未找到CH343驱动
+# 3 - 错误:雷达端口上未找到CH34x设备
+# 4 - 错误:未从cdc_acm找到雷达接口
+# 5 - 错误:解绑cdc_acm驱动失败
+# 6 - 错误:绑定usb_ch343驱动失败
+
+# ── 配置 ────────────────────────────────────────────
+TTY_DEVICE="/dev/ttyACM*"
+CDC_ACM_PATH="/sys/bus/usb/drivers/cdc_acm"
+USB_CH343_PATH="/sys/bus/usb/drivers/usb_ch343"
+RADAR_USB_PORT="1-1.4" # 雷达 CH34x 的 USB 物理端口(固定不变)
+CHECK_INTERVAL=1
+MAX_WAIT_SECONDS=30
+
+# ── 初始化 ──────────────────────────────────────────
+start_time=$(cut -d. -f1 /proc/uptime)
+
+# ── 1. 等待 TTY 设备就绪 ────────────────────────────
+while true; do
+ for dev in /dev/ttyACM*; do
+ [ -e "$dev" ] || continue
+ if [ -c "$dev" ] && [ -r "$dev" ] && [ -w "$dev" ]; then
+ echo "[雷达驱动自动切换程序] 设备 $dev 已就绪!" > /dev/kmsg
+ TTY_DEVICE="$dev"
+ break 2
+ fi
+ done
+
+ if [ ${MAX_WAIT_SECONDS} -gt 0 ]; then
+ current_time=$(cut -d. -f1 /proc/uptime)
+ elapsed_seconds=$((current_time - start_time))
+ if [ ${elapsed_seconds} -ge ${MAX_WAIT_SECONDS} ]; then
+ echo "[雷达驱动自动切换程序] 错误:等待 ${TTY_DEVICE} 超时(${MAX_WAIT_SECONDS}秒)!" > /dev/kmsg
+ exit 1
+ fi
+ echo "[雷达驱动自动切换程序] 仍在等待${TTY_DEVICE}(已等待${elapsed_seconds}秒)..." > /dev/kmsg
+ fi
+ sleep ${CHECK_INTERVAL}
+done
+
+# ── 2. 确认 CH343 驱动已加载 ────────────────────────
+if [ -d "${USB_CH343_PATH}" ]; then
+ echo "[雷达驱动自动切换程序] CH343驱动已加载,继续..." > /dev/kmsg
+else
+ echo "[雷达驱动自动切换程序] 错误:未找到CH343驱动!" > /dev/kmsg
+ exit 2
+fi
+
+# ── 3. 通过物理端口定位雷达 CH34x ───────────────────
+radar_sysfs="/sys/bus/usb/devices/${RADAR_USB_PORT}"
+
+if [ ! -d "${radar_sysfs}" ]; then
+ echo "[雷达驱动自动切换程序] 错误:端口 ${RADAR_USB_PORT} 上无设备!" > /dev/kmsg
+ exit 3
+fi
+
+vendor_id=$(cat "${radar_sysfs}/idVendor" 2>/dev/null)
+product_id=$(cat "${radar_sysfs}/idProduct" 2>/dev/null)
+
+if [ "${vendor_id}" != "1a86" ] || [ "${product_id}" != "55d4" ]; then
+ echo "[雷达驱动自动切换程序] 错误:端口 ${RADAR_USB_PORT} 上 VID:PID=${vendor_id}:${product_id},非雷达CH34x!" > /dev/kmsg
+ exit 3
+fi
+
+echo "[雷达驱动自动切换程序] 雷达确认位于端口 ${RADAR_USB_PORT},VID:PID=${vendor_id}:${product_id}" > /dev/kmsg
+
+# ── 4. 查找 cdc_acm 下雷达的接口 ────────────────────
+cdc_acm_sub_addr=$(ls "${CDC_ACM_PATH}" 2>/dev/null | grep "^${RADAR_USB_PORT}:" | head -n 1)
+
+if [ -z "${cdc_acm_sub_addr}" ]; then
+ echo "[雷达驱动自动切换程序] 错误:cdc_acm驱动中未找到端口 ${RADAR_USB_PORT} 的接口" > /dev/kmsg
+ exit 4
+fi
+
+echo "[雷达驱动自动切换程序] 雷达 cdc_acm 接口:${cdc_acm_sub_addr}" > /dev/kmsg
+
+# ── 5. 解绑 cdc_acm ─────────────────────────────────
+echo "${cdc_acm_sub_addr}" | sudo tee "${CDC_ACM_PATH}/unbind" > /dev/null 2>&1
+if [ $? -eq 0 ]; then
+ echo "[雷达驱动自动切换程序] 已解绑 cdc_acm:${cdc_acm_sub_addr}" > /dev/kmsg
+else
+ echo "[雷达驱动自动切换程序] 错误:解绑 cdc_acm 失败" > /dev/kmsg
+ exit 5
+fi
+
+# ── 6. 绑定 ch343 ───────────────────────────────────
+echo "${cdc_acm_sub_addr}" | sudo tee "${USB_CH343_PATH}/bind" > /dev/null 2>&1
+if [ $? -eq 0 ]; then
+ echo "[雷达驱动自动切换程序] 已绑定 usb_ch343:${cdc_acm_sub_addr}" > /dev/kmsg
+else
+ echo "[雷达驱动自动切换程序] 错误:绑定 usb_ch343 失败" > /dev/kmsg
+ exit 6
+fi
+
+echo "[雷达驱动自动切换程序] 雷达驱动切换完毕!" > /dev/kmsg
+exit 0
diff --git a/bashes/radar-driver-switch.sh.bak b/bashes/radar-driver-switch.sh.bak
new file mode 100644
index 0000000..edf5f15
--- /dev/null
+++ b/bashes/radar-driver-switch.sh.bak
@@ -0,0 +1,129 @@
+#!/bin/bash
+
+# 退出码说明
+# 0 - 成功
+# 1 - 错误:等待TTY设备就绪超时
+# 2 - 错误:未找到CH343驱动
+# 3 - 错误:未找到含Qingheng的USB设备
+# 4 - 错误:未找到对应USB地址
+# 5 - 错误:未找到cdc_acm绑定的对应USB设备
+# 6 - 错误:解绑cdc_acm驱动失败
+# 7 - 错误:绑定usb_ch343驱动失败
+
+# 变量设置
+TTY_DEVICE="/dev/ttyACM*" # 替换为你要等待的tty设备路径
+CDC_ACM_PATH="/sys/bus/usb/drivers/cdc_acm"
+USB_CH343_PATH="/sys/bus/usb/drivers/usb_ch343"
+CHECK_INTERVAL=1 # 检查设备就绪的时间间隔(秒)
+MAX_WAIT_SECONDS=30 # 最大等待时间(秒),0表示无限等待
+
+# 初始化变量
+start_time=$(cut -d. -f1 /proc/uptime)
+current_time=0
+qinheng_id=""
+usb_address=""
+vendor_id=""
+product_id=""
+cdc_acm_sub_addr=""
+
+
+# ----------------- 1. 等待TTY设备就绪 -----------------
+# 循环等待tty设备就绪(检查设备节点是否存在+是否可读写)
+while true; do
+ for dev in /dev/ttyACM*; do
+ [ -e "$dev" ] || continue
+ if [ -c "$dev" ] && [ -r "$dev" ] && [ -w "$dev" ]; then
+ echo "[雷达驱动自动切换程序] 设备 $dev 已就绪!" > /dev/kmsg
+ TTY_DEVICE="$dev"
+ break 2 # 跳出两层循环
+ fi
+ done
+
+ if [ ${MAX_WAIT_SECONDS} -gt 0 ]; then
+ current_time=$(cut -d. -f1 /proc/uptime)
+ elapsed_seconds=$((current_time - start_time))
+ if [ ${elapsed_seconds} -ge ${MAX_WAIT_SECONDS} ]; then
+ echo "[雷达驱动自动切换程序] 错误:等待 ${TTY_DEVICE} 超时(${MAX_WAIT_SECONDS}秒)!" > /dev/kmsg
+ exit 1
+ fi
+ echo "[雷达驱动自动切换程序] 仍在等待${TTY_DEVICE}(已等待${elapsed_seconds}秒)..." > /dev/kmsg
+ fi
+
+ sleep ${CHECK_INTERVAL}
+done
+
+# ------ 2. 查看CH343是否存在 ------
+if ls /sys/bus/usb/drivers/usb_ch343 > /dev/null 2>&1; then
+ echo "[雷达驱动自动切换程序] 检测到CH343驱动存在,继续执行..." > /dev/kmsg
+else
+ echo "[雷达驱动自动切换程序] 错误:未找到CH343驱动!" > /dev/kmsg
+ exit 2
+fi
+
+
+# ------------- 3. 查找Qingheng对应的usb接口 -------------
+echo "[雷达驱动自动切换程序] 开始提取QinHeng设备ID..." > /dev/kmsg
+
+# 执行lsusb并过滤含QinHeng的行,提取ID(核心逻辑)
+qinheng_id=$(lsusb | grep "QinHeng" | awk '{print $6}' | head -n 1)
+
+# 检查是否提取到ID
+if [ -z "${qinheng_id}" ]; then
+ echo "[雷达驱动自动切换程序] 错误:未找到含QinHeng的USB设备!" > /dev/kmsg
+ exit 3
+else
+ echo "[雷达驱动自动切换程序] 成功提取QinHeng设备ID:${qinheng_id}" > /dev/kmsg
+ # 将ID拆分为vendor_id和product_id
+ vendor_id=$(echo "${qinheng_id}" | cut -d ':' -f 1)
+ product_id=$(echo "${qinheng_id}" | cut -d ':' -f 2)
+fi
+
+# -------------------- 4. 提取USB地址 --------------------
+echo "[雷达驱动自动切换程序] 开始查找idVendor=${vendor_id},idProduct=${product_id}的USB地址..." > /dev/kmsg
+# 核心命令:过滤dmesg日志,提取USB地址
+usb_address=$(dmesg | grep -E "idVendor=${vendor_id}, idProduct=${product_id}" | grep -oE "usb [0-9]+-[0-9.]+:" | awk '{print $2}' | sed 's/://' | tail -n 1)
+
+# 检查是否提取到USB地址
+if [ -z "${usb_address}" ]; then
+ echo "[雷达驱动自动切换程序] 错误:未找到对应USB地址" > /dev/kmsg
+ exit 4
+else
+ echo "[雷达驱动自动切换程序] 成功找到USB地址:${usb_address}" > /dev/kmsg
+fi
+
+# ---- 5. 找到cdc_acm驱动对应的usb设备 ----
+echo "[雷达驱动自动切换程序] 开始从${CDC_ACM_PATH}提取${usb_address}对应的子地址..." > /dev/kmsg
+
+cdc_acm_sub_addr=$(ls "${CDC_ACM_PATH}" 2>/dev/null | grep "^${usb_address}:" | head -n 1)
+
+if [ -z "${cdc_acm_sub_addr}" ]; then
+ echo "[雷达驱动自动切换程序] 错误:${CDC_ACM_PATH}中未找到${usb_address}绑定的对应USB设备" > /dev/kmsg
+ exit 5
+else
+ echo "[雷达驱动自动切换程序] 成功提取cdc_acm子地址:${cdc_acm_sub_addr}" > /dev/kmsg
+fi
+
+# 6. 切换驱动
+echo "[雷达驱动自动切换程序] 开始切换cdc_acm驱动到USB地址${usb_address}..." > /dev/kmsg
+
+echo "${cdc_acm_sub_addr}" | sudo tee "${CDC_ACM_PATH}/unbind" 2>/dev/null
+if [ $? -eq 0 ]; then
+ echo "[雷达驱动自动切换程序] 成功解绑cdc_acm驱动:${cdc_acm_sub_addr}" > /dev/kmsg
+else
+ echo "[雷达驱动自动切换程序] 错误:解绑cdc_acm驱动失败" > /dev/kmsg
+ exit 6
+fi
+
+echo "${cdc_acm_sub_addr}" | sudo tee "${USB_CH343_PATH}/bind" 2>/dev/null
+if [ $? -eq 0 ]; then
+ echo "[雷达驱动自动切换程序] 成功绑定usb_ch343驱动:${cdc_acm_sub_addr}" > /dev/kmsg
+else
+ echo "[雷达驱动自动切换程序] 错误:绑定usb_ch343驱动失败(检查驱动是否加载)" > /dev/kmsg
+ exit 7
+fi
+
+
+# 执行完成
+echo "[雷达驱动自动切换程序] 雷达驱动自动切换完毕!" > /dev/kmsg
+
+exit 0
\ No newline at end of file
diff --git a/dependencies/dependencies.txt b/dependencies/dependencies.txt
new file mode 100644
index 0000000..489f65e
--- /dev/null
+++ b/dependencies/dependencies.txt
@@ -0,0 +1,5 @@
+ros-humble-serial-driver
+ros-humble-ackermann-msgs
+# 从源码编译serial库
+ros-humble-navigation2
+ros-humble-slam-toolbox
\ No newline at end of file
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/README.md b/src/LSLIDAR_X_ROS2-20240228/src/README.md
new file mode 100644
index 0000000..9871ecc
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/README.md
@@ -0,0 +1,44 @@
+# lslidar
+
+## Description
+The `lslidar package is a linux ROS2 driver for lslidar M10 ,M10_GPS,M10_P,M10_PLUS and N10.
+The package is tested on Ubuntu 20.04 with ROS2 indigo.
+
+## Compling
+This is a Catkin package. Make sure the package is on `ROS_PACKAGE_PATH` after cloning the package to your workspace. And the normal procedure for compling a catkin package will work.
+
+```
+cd your_work_space
+colcon build
+source install/setup.bash
+ros2 launch lslidar_driver lslidar_launch.py
+```
+open new terminal
+ros2 topic pub -1 /lslidar_order std_msgs/msg/Int8 data:\ 1\ (open radar)
+ros2 topic pub -1 /lslidar_order std_msgs/msg/Int8 data:\ 0\ (close radar)
+
+
+
+
+
+ros2 launch lslidar_driver lslidar_launch.py
+
+```
+
+Note that this launch file launches both the driver, which is the only launch file needed to be used.
+
+
+## FAQ
+
+
+## Bug Report
+
+Prefer to open an issue. You can also send an E-mail to honghangli@lslidar.com
+
+
+
+
+RERTION
+
+
+
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/CMakeLists.txt b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/CMakeLists.txt
new file mode 100644
index 0000000..85e13f6
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/CMakeLists.txt
@@ -0,0 +1,56 @@
+cmake_minimum_required(VERSION 3.5)
+project(lslidar_driver)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+ set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+set(libpcap_LIBRARIES -lpcap)
+
+#set(FastRTPS_INCLUDE_DIR /opt/ros/foxy/include)
+#set(FastRTPS_LIBRARY_RELEASE /opt/ros/foxy/lib/libfastrtps.so)
+
+#find_package(Boost REQUIRED COMPONENTS )
+find_package(Boost REQUIRED thread)
+find_package(rclcpp REQUIRED)
+find_package(PCL REQUIRED)
+find_package(diagnostic_updater REQUIRED)
+find_package(lslidar_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(ament_cmake REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(pcl_conversions REQUIRED)
+find_package(sensor_msgs REQUIRED)
+#find_package(PCL REQUIRED COMPONENTS common io)
+
+include_directories(
+ include
+ ${PCL_INCLUDE_DIRS}
+ ${PCL_COMMON_INCLUDE_DIRS}
+ ${Boost_INCLUDE_DIRS}
+)
+
+# Node
+add_executable(lslidar_driver_node src/lslidar_driver_node.cc src/lslidar_driver.cc src/input.cc src/lsiosr.cpp)
+target_link_libraries(lslidar_driver_node ${rclcpp_LIBRARIES} ${libpcap_LIBRARIES} ${Boost_LIBRARIES} Boost::thread)
+ament_target_dependencies(lslidar_driver_node rclcpp std_msgs lslidar_msgs sensor_msgs diagnostic_updater pcl_conversions)
+
+
+install(DIRECTORY launch params rviz
+ DESTINATION share/${PROJECT_NAME})
+
+install(TARGETS
+ lslidar_driver_node
+ DESTINATION lib/${PROJECT_NAME}
+)
+
+ament_export_dependencies(rclcpp pluginlib lslidar_msgs sensor_msgs pcl_conversions)
+ament_export_include_directories(include ${PCL_COMMON_INCLUDE_DIRS})
+
+ament_package()
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/include/lslidar_driver/input.h b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/include/lslidar_driver/input.h
new file mode 100644
index 0000000..2715da4
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/include/lslidar_driver/input.h
@@ -0,0 +1,134 @@
+/*
+ * This file is part of lslidar_ch driver.
+ *
+ * The driver is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * The driver is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with the driver. If not, see .
+ *
+ * Input -- base class used to access the data independently of
+ * its source
+ *
+ * InputSocket -- derived class reads live data from the device
+ * via a UDP socket
+ *
+ * InputPCAP -- derived class provides a similar interface from a
+ * PCAP dump
+ */
+
+#ifndef __LSLIDAR_INPUT_H_
+#define __LSLIDAR_INPUT_H_
+
+#include
+#include
+#include
+#include
+#include "rclcpp/rclcpp.hpp"
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+namespace lslidar_driver
+{
+static uint16_t MSOP_DATA_PORT_NUMBER = 2368; // lslidar default data port on PC
+/**
+ * 从在线的网络数据或离线的网络抓包数据(pcap文件)中提取出lidar的原始数据,即packet数据包
+ * @brief The Input class,
+ *
+ * @param private_nh 一个NodeHandled,用于通过节点传递参数
+ * @param port
+ * @returns 0 if successful,
+ * -1 if end of file
+ * >0 if incomplete packet (is this possible?)
+ */
+class Input
+{
+public:
+ Input(rclcpp::Node* private_nh, uint16_t port);
+
+ virtual ~Input()
+ {
+ }
+
+ virtual int getPacket(lslidar_msgs::msg::LslidarPacket::UniquePtr &packet) = 0;
+
+ int getRpm(void);
+ int getReturnMode(void);
+ bool getUpdateFlag(void);
+ void clearUpdateFlag(void);
+ void UDP_order(const std_msgs::msg::Int8 msg);
+ void UDP_difop();
+protected:
+ rclcpp::Node* private_nh_;
+ uint16_t port_;
+ std::string devip_str_;
+ std::string lidar_name;
+ int cur_rpm_;
+ int return_mode_;
+ bool npkt_update_flag_;
+ bool add_multicast;
+ std::string group_ip;
+ int UDP_PORT_NUMBER_DIFOP;
+ int socket_id_difop;
+ int sockfd_;
+ std::string devip_str_difop;
+};
+
+/** @brief Live lslidar input from socket. */
+class InputSocket : public Input
+{
+public:
+ InputSocket(rclcpp::Node* private_nh, uint16_t port = MSOP_DATA_PORT_NUMBER);
+
+ virtual ~InputSocket();
+
+ virtual int getPacket(lslidar_msgs::msg::LslidarPacket::UniquePtr &packet);
+
+private:
+private:
+
+ in_addr devip_;
+ in_addr devip_difop;
+ //struct ip_mreq group;
+
+};
+class InputPCAP : public Input
+{
+public:
+ InputPCAP(rclcpp::Node* private_nh,uint16_t port = MSOP_DATA_PORT_NUMBER, double packet_rate = 0.0,
+ std::string filename="");
+ virtual ~InputPCAP();
+ virtual int getPacket(lslidar_msgs::msg::LslidarPacket::UniquePtr &pkt);
+private:
+
+ rclcpp::Rate packet_rate_;
+ std::string filename_;
+ pcap_t *pcap_;
+ bpf_program pcap_packet_filter_;
+ char errbuf_[PCAP_ERRBUF_SIZE];
+ bool empty_;
+ bool read_once_;
+ bool read_fast_;
+ double repeat_delay_;
+ };
+}
+
+#endif // __LSLIDAR_INPUT_H
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/include/lslidar_driver/lsiosr.h b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/include/lslidar_driver/lsiosr.h
new file mode 100644
index 0000000..6109497
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/include/lslidar_driver/lsiosr.h
@@ -0,0 +1,88 @@
+/*******************************************************
+@company: Copyright (C) 2021, Leishen Intelligent System
+@product: LSM10_N10
+@filename: lsiosr.cpp
+@brief:
+@version: date: author: comments:
+@v1.0 22-10-24 li new
+*******************************************************/
+#ifndef LSIOSR_H
+#define LSIOSR_H
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+//波特率
+#define BAUD_230400 230400
+#define BAUD_460800 460800
+#define BAUD_500000 500000
+#define BAUD_921600 921600
+
+//奇偶校验位
+#define PARITY_ODD 'O' //奇数
+#define PARITY_EVEN 'E' //偶数
+#define PARITY_NONE 'N' //无奇偶校验位
+
+//停止位
+#define STOP_BIT_1 1
+#define STOP_BIT_2 2
+
+//数据位
+#define DATA_BIT_7 7
+#define DATA_BIT_8 8
+
+namespace lslidar_driver
+{
+class LSIOSR{
+public:
+ static LSIOSR* instance(std::string name, int speed, int fd = 0);
+
+ ~LSIOSR();
+
+ /* 从串口中读取数据 */
+ int read(unsigned char *buffer, int length, int timeout = 30);
+
+ /* 向串口传数据 */
+ int send(const char* buffer, int length, int timeout = 30);
+
+ /* Empty serial port input buffer */
+ void flushinput();
+
+ /* 串口初始化 */
+ int init();
+
+ int close();
+
+ /* 获取串口号 */
+ std::string getPort();
+
+ /* 设置串口号 */
+ int setPortName(std::string name);
+
+private:
+ LSIOSR(std::string name, int speed, int fd);
+
+ int waitWritable(int millis);
+ int waitReadable(int millis);
+
+ /* 串口配置的函数 */
+ int setOpt(int nBits, uint8_t nEvent, int nStop);
+
+ std::string port_;
+ int baud_rate_;
+
+ int fd_;
+};
+}
+#endif
+
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/include/lslidar_driver/lslidar_driver.h b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/include/lslidar_driver/lslidar_driver.h
new file mode 100644
index 0000000..34bb6d1
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/include/lslidar_driver/lslidar_driver.h
@@ -0,0 +1,159 @@
+/*
+ * This file is part of lslidar driver.
+ *
+ * The driver is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * The driver is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with the driver. If not, see .
+ */
+
+#ifndef LSLIDAR_DRIVER_H
+#define LSLIDAR_DRIVER_H
+
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include "rclcpp/rclcpp.hpp"
+#include
+#include "diagnostic_updater/diagnostic_updater.hpp"
+#include "diagnostic_updater/publisher.hpp"
+#include "lslidar_msgs/msg/lslidar_packet.hpp"
+#include "std_msgs/msg/byte.hpp"
+
+#include "sensor_msgs/msg/point_cloud2.hpp"
+#include "pcl_conversions/pcl_conversions.h"
+#include "pcl/point_types.h"
+
+#include "time.h"
+#include "input.h"
+#include "lsiosr.h"
+#include "sensor_msgs/msg/laser_scan.hpp"
+namespace lslidar_driver {
+
+struct PointXYZIT {
+ PCL_ADD_POINT4D;
+ uint8_t intensity;
+ double timestamp;
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned
+} EIGEN_ALIGN16;
+
+typedef struct {
+ double degree;
+ double range;
+ double intensity;
+} ScanPoint;
+
+class LslidarDriver: public rclcpp::Node {
+public:
+ LslidarDriver();
+ LslidarDriver(const rclcpp::NodeOptions& options);
+ ~LslidarDriver();
+
+ bool initialize();
+ bool polling();
+
+ typedef std::shared_ptr LslidarDriverPtr;
+ typedef std::shared_ptr LslidarDriverConstPtr;
+
+private:
+ uint64_t get_gps_stamp(struct tm t);
+ uint8_t N10_CalCRC8(unsigned char * p, int len);
+ bool loadParameters();
+ bool createRosIO();
+ void open_serial();
+ void lidar_difop();
+ void lidar_order(const std_msgs::msg::Int8::SharedPtr msg);
+ void data_processing(unsigned char *packet_bytes,int len);
+ void data_processing_2(unsigned char *packet_bytes,int len);
+ void difop_processing(unsigned char *packet_bytes);
+ void pubScanThread();
+ void recvThread_crc(int &count,int &link_time);
+ int receive_data(unsigned char *packet_bytes);
+ int getScan(std::vector &points, rclcpp::Time &scan_time, float &scan_duration);
+
+ boost::thread *pubscan_thread_ ;
+ boost::shared_ptr msop_input_;
+ boost::mutex mutex_;
+ boost::mutex pubscan_mutex_;
+ boost::condition_variable pubscan_cond_;
+ int UDP_PORT_NUMBER;
+ int count_num;
+ int package_points;
+ int data_bits_start;
+ int degree_bits_start;
+ int end_degree_bits_start;
+ int rpm_bits_start;
+ int baud_rate_;
+ int points_size_;
+ int idx = 0;
+ int link_time = 0;
+ int fixed_array_length;//cyy_add
+
+ bool use_gps_ts;
+ bool is_start;
+ bool high_reflection;
+ bool compensation;
+ bool first_compensation = true;
+ bool pubScan;
+ bool pubPointCloud2;
+
+ double min_range;
+ double max_range;
+ double angle_disable_min;
+ double angle_disable_max;
+ double angle_able_min;
+ double angle_able_max;
+ double last_degree = 0.0;
+ double degree_compensation = 0.0;
+
+ uint16_t PACKET_SIZE ;
+ uint64_t sweep_end_time_gps;
+ uint64_t sweep_end_time_hardware;
+ uint64_t sub_second;
+
+ std::string frame_id;
+ std::string interface_selection;
+ std::string scan_topic;
+ std::string lidar_name;
+ std::string serial_port_;
+ std::string dump_file;
+ std::string pointcloud_topic;
+ std::string in_file_name;
+
+ tm pTime;
+ rclcpp::Time pre_time_;
+ rclcpp::Time time_;
+ std::vector scan_points_;
+ std::vector scan_points_bak_;
+ // Diagnostics updater
+ diagnostic_updater::Updater diagnostics;
+ std::shared_ptr diag_topic;
+ double diag_min_freq;
+ double diag_max_freq;
+ rclcpp::Publisher::SharedPtr scan_pub;
+ rclcpp::Publisher::SharedPtr point_cloud_pub;
+ rclcpp::Subscription::SharedPtr difop_switch;
+ LSIOSR * serial_;
+};
+typedef PointXYZIT VPoint;
+typedef pcl::PointCloud VPointCloud;
+
+} // namespace lslidar_driver
+POINT_CLOUD_REGISTER_POINT_STRUCT(lslidar_driver::PointXYZIT,
+ (float, x, x)(float, y, y)(float, z, z)(
+ std::uint8_t, intensity,
+ intensity)(double, timestamp, timestamp))
+#endif // _LSLIDAR_DRIVER_H_
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lslidar_double_launch.py b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lslidar_double_launch.py
new file mode 100644
index 0000000..40ad793
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lslidar_double_launch.py
@@ -0,0 +1,50 @@
+#!/usr/bin/python3
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import LifecycleNode
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+
+import lifecycle_msgs.msg
+import os
+
+def generate_launch_description():
+
+ driver_dir_1 = os.path.join(get_package_share_directory('lslidar_driver_n10p'), 'params', 'lsx10_1.yaml')
+ driver_dir_2 = os.path.join(get_package_share_directory('lslidar_driver_n10p'), 'params', 'lsx10_2.yaml')
+
+ driver_node_1 = LifecycleNode(package='lslidar_driver_n10p',
+ executable='lslidar_driver_node',
+ name='lslidar_driver_node', #设置激光数据topic名称
+ output='screen',
+ emulate_tty=True,
+ namespace='lidar_1',
+ parameters=[driver_dir_1],
+ )
+
+ driver_node_2 = LifecycleNode(package='lslidar_driver_n10p',
+ executable='lslidar_driver_node',
+ name='lslidar_driver_node', #设置激光数据topic名称
+ output='screen',
+ emulate_tty=True,
+ namespace='lidar_2',
+ parameters=[driver_dir_2],
+ )
+
+ rviz_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'rviz', 'lslidar.rviz')
+
+ rviz_node = Node(
+ package='rviz2',
+ namespace='',
+ executable='rviz2',
+ name='rviz2',
+ arguments=['-d', rviz_dir],
+ output='screen')
+
+ return LaunchDescription([
+ driver_node_1,
+ driver_node_2,
+ rviz_node,
+ ])
+
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10_net_launch.py b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10_net_launch.py
new file mode 100644
index 0000000..7659d3f
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10_net_launch.py
@@ -0,0 +1,27 @@
+#!/usr/bin/python3
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import LifecycleNode
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+
+import lifecycle_msgs.msg
+import os
+
+def generate_launch_description():
+
+ driver_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lidar_net_ros2','lsm10_net.yaml')
+
+ driver_node = LifecycleNode(package='lslidar_driver',
+ executable='lslidar_driver_node',
+ name='lslidar_driver_node',
+ output='screen',
+ emulate_tty=True,
+ namespace='',
+ parameters=[driver_dir],
+ )
+ return LaunchDescription([
+ driver_node,
+ ])
+
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10_uart_launch.py b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10_uart_launch.py
new file mode 100644
index 0000000..cd2fa16
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10_uart_launch.py
@@ -0,0 +1,27 @@
+#!/usr/bin/python3
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import LifecycleNode
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+
+import lifecycle_msgs.msg
+import os
+
+def generate_launch_description():
+
+ driver_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lidar_uart_ros2','lsm10.yaml')
+
+ driver_node = LifecycleNode(package='lslidar_driver',
+ executable='lslidar_driver_node',
+ name='lslidar_driver_node',
+ output='screen',
+ emulate_tty=True,
+ namespace='',
+ parameters=[driver_dir],
+ )
+ return LaunchDescription([
+ driver_node,
+ ])
+
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10p_net_launch.py b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10p_net_launch.py
new file mode 100644
index 0000000..e2468c8
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10p_net_launch.py
@@ -0,0 +1,27 @@
+#!/usr/bin/python3
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import LifecycleNode
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+
+import lifecycle_msgs.msg
+import os
+
+def generate_launch_description():
+
+ driver_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lidar_net_ros2','lsm10p_net.yaml')
+
+ driver_node = LifecycleNode(package='lslidar_driver',
+ executable='lslidar_driver_node',
+ name='lslidar_driver_node',
+ output='screen',
+ emulate_tty=True,
+ namespace='',
+ parameters=[driver_dir],
+ )
+ return LaunchDescription([
+ driver_node,
+ ])
+
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10p_uart_launch.py b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10p_uart_launch.py
new file mode 100644
index 0000000..10d1763
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10p_uart_launch.py
@@ -0,0 +1,27 @@
+#!/usr/bin/python3
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import LifecycleNode
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+
+import lifecycle_msgs.msg
+import os
+
+def generate_launch_description():
+
+ driver_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lidar_uart_ros2','lsm10_p.yaml')
+
+ driver_node = LifecycleNode(package='lslidar_driver',
+ executable='lslidar_driver_node',
+ name='lslidar_driver_node',
+ output='screen',
+ emulate_tty=True,
+ namespace='',
+ parameters=[driver_dir],
+ )
+ return LaunchDescription([
+ driver_node,
+ ])
+
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10_launch.py b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10_launch.py
new file mode 100644
index 0000000..ae9366e
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10_launch.py
@@ -0,0 +1,28 @@
+#!/usr/bin/python3
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import LifecycleNode
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+
+import lifecycle_msgs.msg
+import os
+
+def generate_launch_description():
+
+ driver_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params','lidar_uart_ros2', 'lsn10.yaml')
+
+ driver_node = LifecycleNode(package='lslidar_driver',
+ executable='lslidar_driver_node',
+ name='lslidar_driver_node', #设置激光数据topic名称
+ output='screen',
+ emulate_tty=True,
+ namespace='',
+ parameters=[driver_dir],
+ )
+
+ return LaunchDescription([
+ driver_node,
+ ])
+
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10_net_launch.py b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10_net_launch.py
new file mode 100644
index 0000000..9a6a4d8
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10_net_launch.py
@@ -0,0 +1,28 @@
+#!/usr/bin/python3
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import LifecycleNode
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+
+import lifecycle_msgs.msg
+import os
+
+def generate_launch_description():
+
+ driver_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params','lidar_net_ros2', 'lsn10_net.yaml')
+
+ driver_node = LifecycleNode(package='lslidar_driver',
+ executable='lslidar_driver_node',
+ name='lslidar_driver_node', #设置激光数据topic名称
+ output='screen',
+ emulate_tty=True,
+ namespace='',
+ parameters=[driver_dir],
+ )
+
+ return LaunchDescription([
+ driver_node,
+ ])
+
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10p_launch.py b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10p_launch.py
new file mode 100644
index 0000000..758b03b
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10p_launch.py
@@ -0,0 +1,28 @@
+#!/usr/bin/python3
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import LifecycleNode
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+
+import lifecycle_msgs.msg
+import os
+
+def generate_launch_description():
+
+ driver_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params','lidar_uart_ros2', 'lsn10p.yaml')
+
+ driver_node = LifecycleNode(package='lslidar_driver',
+ executable='lslidar_driver_node',
+ name='lslidar_driver_node', #设置激光数据topic名称
+ output='screen',
+ emulate_tty=True,
+ namespace='',
+ parameters=[driver_dir],
+ )
+
+ return LaunchDescription([
+ driver_node,
+ ])
+
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10p_net_launch.py b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10p_net_launch.py
new file mode 100644
index 0000000..966bda9
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10p_net_launch.py
@@ -0,0 +1,28 @@
+#!/usr/bin/python3
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import LifecycleNode
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+
+import lifecycle_msgs.msg
+import os
+
+def generate_launch_description():
+
+ driver_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params','lidar_net_ros2', 'lsn10p_net.yaml')
+
+ driver_node = LifecycleNode(package='lslidar_driver',
+ executable='lslidar_driver_node',
+ name='lslidar_driver_node', #设置激光数据topic名称
+ output='screen',
+ emulate_tty=True,
+ namespace='',
+ parameters=[driver_dir],
+ )
+
+ return LaunchDescription([
+ driver_node,
+ ])
+
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/viewer_scan_launch.py b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/viewer_scan_launch.py
new file mode 100644
index 0000000..c82e76d
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/viewer_scan_launch.py
@@ -0,0 +1,26 @@
+#!/usr/bin/python3
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import LifecycleNode
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+
+import lifecycle_msgs.msg
+import os
+
+def generate_launch_description():
+
+ rviz2_config = os.path.join(get_package_share_directory('lslidar_driver'),'rviz','lslidar.rviz')
+
+ rviz2_node = Node(
+ package='rviz2',
+ executable='rviz2',
+ name='rviz2',
+ arguments=['-d',rviz2_config],
+ output='screen')
+
+ return LaunchDescription([
+ rviz2_node,
+ ])
+
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/package.xml b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/package.xml
new file mode 100644
index 0000000..2d88422
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/package.xml
@@ -0,0 +1,36 @@
+
+
+ lslidar_driver
+ 1.2.0
+ ROS device driver for Leishen lidar.
+ Nick Shu
+ Nick Shu
+ GNU General Public License V3.0
+
+ ament_cmake
+
+ rclcpp
+ std_msgs
+ lslidar_msgs
+ pcl_conversions
+ rclpy
+ libpcap
+ libpcl-all-dev
+ pluginlib
+ sensor_msgs
+
+ rclcpp
+ std_msgs
+ lslidar_msgs
+ pcl_conversions
+ rclpy
+ libpcap
+ libpcl-all
+ pluginlib
+ sensor_msgs
+
+ diagnostic_updater
+
+ ament_cmake
+
+
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsm10_net.yaml b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsm10_net.yaml
new file mode 100644
index 0000000..6e91feb
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsm10_net.yaml
@@ -0,0 +1,22 @@
+/lslidar_driver_node:
+ ros__parameters:
+ frame_id: laser #激光坐标
+ group_ip: 224.1.1.2
+ add_multicast: false
+ device_ip: 192.168.1.200 #雷达目的ip
+ device_ip_difop: 192.168.1.102 #雷达源IP
+ msop_port: 2368 #雷达目的端口号
+ difop_port: 2369 #雷达源端口号
+ lidar_name: M10 #雷达选择:M10 M10_P M10_PLUS M10_GPS N10
+ ceil_increase: -1 #Lsm10*时改值应设置为-1
+ angle_disable_min: 0.0 #单角度裁剪开始值
+ angle_disable_max: 0.0 #单角度裁剪结束值
+ truncated_mode_: 0 #多角度裁剪开关:值为0时表示不使用多角度裁剪,默认为0
+ #值为1表示使用多角度裁剪,同时angle_disable_min与angle_disable_max设为 0,角度值在/lslidar_driver.cc中修改
+ min_range: 0.0 #雷达接收距离最小值
+ max_range: 200.0 #雷达接收距离最大值
+ use_gps_ts: false #雷达是否使用GPS授时
+ scan_topic: /scan #设置激光数据topic名称
+ interface_selection: net #接口选择:net 为网口,serial 为串口。
+ serial_port_: /dev/wheeltec_laser #串口连接时的串口号
+# pcap: /home/ls/work/2211/M10_P_gps.pcap #雷达是否使用pcap包读取功能
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsm10p_net.yaml b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsm10p_net.yaml
new file mode 100644
index 0000000..a8e30ce
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsm10p_net.yaml
@@ -0,0 +1,22 @@
+/lslidar_driver_node:
+ ros__parameters:
+ frame_id: laser #激光坐标
+ group_ip: 224.1.1.2
+ add_multicast: false
+ device_ip: 192.168.1.200 #雷达目的ip
+ device_ip_difop: 192.168.1.102 #雷达源IP
+ msop_port: 2368 #雷达目的端口号
+ difop_port: 2369 #雷达源端口号
+ lidar_name: M10_P #雷达选择:M10 M10_P M10_PLUS M10_GPS N10
+ ceil_increase: -1 #Lsm10*时改值应设置为-1
+ angle_disable_min: 0.0 #单角度裁剪开始值
+ angle_disable_max: 0.0 #单角度裁剪结束值
+ truncated_mode_: 0 #多角度裁剪开关:值为0时表示不使用多角度裁剪,默认为0
+ #值为1表示使用多角度裁剪,同时angle_disable_min与angle_disable_max设为 0,角度值在/lslidar_driver.cc中修改
+ min_range: 0.0 #雷达接收距离最小值
+ max_range: 200.0 #雷达接收距离最大值
+ use_gps_ts: false #雷达是否使用GPS授时
+ scan_topic: /scan #设置激光数据topic名称
+ interface_selection: net #接口选择:net 为网口,serial 为串口。
+ serial_port_: /dev/wheeltec_laser #串口连接时的串口号
+# pcap: /home/ls/work/2211/M10_P_gps.pcap #雷达是否使用pcap包读取功能
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsn10_net.yaml b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsn10_net.yaml
new file mode 100644
index 0000000..f211d48
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsn10_net.yaml
@@ -0,0 +1,25 @@
+/lslidar_driver_node:
+ ros__parameters:
+ frame_id: laser #激光坐标
+ group_ip: 224.1.1.2
+ add_multicast: false
+ device_ip: 192.168.1.200 #雷达源IP
+ device_ip_difop: 192.168.1.102 #雷达目的ip
+ msop_port: 2368 #雷达目的端口号
+ difop_port: 2369 #雷达源端口号
+ lidar_name: N10 #雷达选择:M10 M10_P M10_PLUS M10_GPS N10 L10 N10_P
+ angle_disable_min: 0.0 #角度裁剪开始值
+ angle_disable_max: 0.0 #角度裁剪结束值
+ min_range: 0.2 #雷达接收距离最小值
+ max_range: 200.0 #雷达接收距离最大值
+ use_gps_ts: false #雷达是否使用GPS授时
+ scan_topic: /scan #设置激光数据topic名称
+ interface_selection: net #接口选择:net 为网口,serial 为串口。
+ serial_port_: /dev/wheeltec_laser #串口连接时的串口号
+ high_reflection: false #M10_P雷达需填写该值,若不确定,请联系技术支持。
+ compensation: false #M10系列是否使用角度补偿功能
+ pubScan: true #是否发布scan话题
+ pubPointCloud2: false #是否发布pointcloud2话题
+ pointcloud_topic: /lslidar_point_cloud #设置激光数据topic名称
+# pcap: /home/ls/1.pcap #雷达是否使用pcap包读取功能
+# in_file_name: /home/ls/1.txt #雷达是否使用txt文件读取功能
\ No newline at end of file
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsn10p_net.yaml b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsn10p_net.yaml
new file mode 100644
index 0000000..ffd83bb
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsn10p_net.yaml
@@ -0,0 +1,25 @@
+/lslidar_driver_node:
+ ros__parameters:
+ frame_id: laser #激光坐标
+ group_ip: 224.1.1.2
+ add_multicast: false
+ device_ip: 192.168.1.200 #雷达源IP
+ device_ip_difop: 192.168.1.102 #雷达目的ip
+ msop_port: 2368 #雷达目的端口号
+ difop_port: 2369 #雷达源端口号
+ lidar_name: N10_P #雷达选择:M10 M10_P M10_PLUS M10_GPS N10 L10 N10_P
+ angle_disable_min: 120.0 #角度裁剪开始值
+ angle_disable_max: 230.0 #角度裁剪结束值
+ min_range: 0.2 #雷达接收距离最小值
+ max_range: 200.0 #雷达接收距离最大值
+ use_gps_ts: false #雷达是否使用GPS授时
+ scan_topic: /scan #设置激光数据topic名称
+ interface_selection: net #接口选择:net 为网口,serial 为串口。
+ serial_port_: /dev/wheeltec_laser #串口连接时的串口号
+ high_reflection: false #M10_P雷达需填写该值,若不确定,请联系技术支持。
+ compensation: false #M10系列是否使用角度补偿功能
+ pubScan: true #是否发布scan话题
+ pubPointCloud2: false #是否发布pointcloud2话题
+ pointcloud_topic: /lslidar_point_cloud #设置激光数据topic名称
+# pcap: /home/ls/1.pcap #雷达是否使用pcap包读取功能
+# in_file_name: /home/ls/1.txt #雷达是否使用txt文件读取功能
\ No newline at end of file
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsm10.yaml b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsm10.yaml
new file mode 100644
index 0000000..203f6b5
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsm10.yaml
@@ -0,0 +1,28 @@
+/lslidar_driver_node:
+ ros__parameters:
+ frame_id: laser #激光坐标
+ group_ip: 224.1.1.2
+ add_multicast: false
+ device_ip: 192.168.1.200 #雷达目的ip
+ device_ip_difop: 192.168.1.102 #雷达源IP
+ msop_port: 2368 #雷达目的端口号
+ difop_port: 2369 #雷达源端口号
+ lidar_name: M10 #雷达选择:M10 M10_P M10_PLUS M10_GPS N10
+ ceil_increase: -1 #Lsm10*时改值应设置为-1
+ angle_disable_min: 0.0 #单角度裁剪开始值
+ angle_disable_max: 0.0 #单角度裁剪结束值
+ truncated_mode_: 0 #多角度裁剪开关:值为0时表示不使用多角度裁剪,默认为0
+ #值为1表示使用多角度裁剪,同时angle_disable_min与angle_disable_max设为 0,角度值在/lslidar_driver.cc中修改
+ min_range: 0.0 #雷达接收距离最小值
+ max_range: 200.0 #雷达接收距离最大值
+ use_gps_ts: false #雷达是否使用GPS授时
+ scan_topic: /scan #设置激光数据topic名称
+ interface_selection: serial #接口选择:net 为网口,serial 为串口。
+ serial_port_: /dev/wheeltec_laser #串口连接时的串口号
+ high_reflection: false #M10_P雷达需填写该值,若不确定,请联系技术支持。
+ compensation: false #M10系列是否使用角度补偿功能
+ pubScan: true #是否发布scan话题
+ pubPointCloud2: false #是否发布pointcloud2话题
+ pointcloud_topic: /lslidar_point_cloud #设置激光数据topic名称
+# pcap: /home/ls/1.pcap #雷达是否使用pcap包读取功能
+# in_file_name: /home/ls/1.txt #雷达是否使用txt文件读取功能
\ No newline at end of file
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsm10_p.yaml b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsm10_p.yaml
new file mode 100644
index 0000000..009e5fb
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsm10_p.yaml
@@ -0,0 +1,28 @@
+/lslidar_driver_node:
+ ros__parameters:
+ frame_id: laser #激光坐标
+ group_ip: 224.1.1.2
+ add_multicast: false
+ device_ip: 192.168.1.200 #雷达目的ip
+ device_ip_difop: 192.168.1.102 #雷达源IP
+ msop_port: 2368 #雷达目的端口号
+ difop_port: 2369 #雷达源端口号
+ lidar_name: M10_P #雷达选择:M10 M10_P M10_PLUS M10_GPS N10
+ ceil_increase: -1 #Lsm10*时改值应设置为-1
+ angle_disable_min: 0.0 #单角度裁剪开始值
+ angle_disable_max: 0.0 #单角度裁剪结束值
+ truncated_mode_: 0 #多角度裁剪开关:值为0时表示不使用多角度裁剪,默认为0
+ #值为1表示使用多角度裁剪,同时angle_disable_min与angle_disable_max设为 0,角度值在/lslidar_driver.cc中修改
+ min_range: 0.0 #雷达接收距离最小值
+ max_range: 200.0 #雷达接收距离最大值
+ use_gps_ts: false #雷达是否使用GPS授时
+ scan_topic: /scan #设置激光数据topic名称
+ interface_selection: serial #接口选择:net 为网口,serial 为串口。
+ serial_port_: /dev/wheeltec_laser #串口连接时的串口号
+ high_reflection: false #M10_P雷达需填写该值,若不确定,请联系技术支持。
+ compensation: false #M10系列是否使用角度补偿功能
+ pubScan: true #是否发布scan话题
+ pubPointCloud2: false #是否发布pointcloud2话题
+ pointcloud_topic: /lslidar_point_cloud #设置激光数据topic名称
+# pcap: /home/ls/1.pcap #雷达是否使用pcap包读取功能
+# in_file_name: /home/ls/1.txt #雷达是否使用txt文件读取功能
\ No newline at end of file
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsn10.yaml b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsn10.yaml
new file mode 100644
index 0000000..ea0e78f
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsn10.yaml
@@ -0,0 +1,26 @@
+/lslidar_driver_node:
+ ros__parameters:
+ frame_id: laser_link #激光坐标
+ group_ip: 224.1.1.2
+ add_multicast: false
+ device_ip: 192.168.1.200 #雷达源IP
+ device_ip_difop: 192.168.1.102 #雷达目的ip
+ msop_port: 2368 #雷达目的端口号
+ difop_port: 2369 #雷达源端口号
+ lidar_name: N10 #雷达选择:M10 M10_P M10_PLUS M10_GPS N10 L10 N10_P
+ angle_disable_min: 0.0 #角度裁剪开始值
+ angle_disable_max: 0.0 #角度裁剪结束值
+ min_range: 0.15 #雷达接收距离最小值
+ max_range: 200.0 #雷达接收距离最大值
+ use_gps_ts: false #雷达是否使用GPS授时
+ scan_topic: /scan #设置激光数据topic名称
+ interface_selection: serial #接口选择:net 为网口,serial 为串口。
+ serial_port_: /dev/ttyCH343USB0 #串口连接时的串口号
+ high_reflection: false #M10_P雷达需填写该值,若不确定,请联系技术支持。
+ compensation: false #M10系列是否使用角度补偿功能
+ pubScan: true #是否发布scan话题
+ pubPointCloud2: false #是否发布pointcloud2话题
+ pointcloud_topic: /lslidar_point_cloud #设置激光数据topic名称
+ fixed_array_length: 450
+# pcap: /home/ls/1.pcap #雷达是否使用pcap包读取功能
+# in_file_name: /home/ls/1.txt #雷达是否使用txt文件读取功能
\ No newline at end of file
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsn10p.yaml b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsn10p.yaml
new file mode 100644
index 0000000..8f1c6c9
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsn10p.yaml
@@ -0,0 +1,25 @@
+/lslidar_driver_node:
+ ros__parameters:
+ frame_id: laser #激光坐标
+ group_ip: 224.1.1.2
+ add_multicast: false
+ device_ip: 192.168.1.200 #雷达源IP
+ device_ip_difop: 192.168.1.102 #雷达目的ip
+ msop_port: 2368 #雷达目的端口号
+ difop_port: 2369 #雷达源端口号
+ lidar_name: N10_P #雷达选择:M10 M10_P M10_PLUS M10_GPS N10 L10 N10_P
+ angle_disable_min: 120.0 #角度裁剪开始值
+ angle_disable_max: 230.0 #角度裁剪结束值
+ min_range: 0.2 #雷达接收距离最小值
+ max_range: 200.0 #雷达接收距离最大值
+ use_gps_ts: false #雷达是否使用GPS授时
+ scan_topic: /scan #设置激光数据topic名称
+ interface_selection: serial #接口选择:net 为网口,serial 为串口。
+ serial_port_: /dev/wheeltec_laser #串口连接时的串口号
+ high_reflection: false #M10_P雷达需填写该值,若不确定,请联系技术支持。
+ compensation: false #M10系列是否使用角度补偿功能
+ pubScan: true #是否发布scan话题
+ pubPointCloud2: false #是否发布pointcloud2话题
+ pointcloud_topic: /lslidar_point_cloud #设置激光数据topic名称
+# pcap: /home/ls/1.pcap #雷达是否使用pcap包读取功能
+# in_file_name: /home/ls/1.txt #雷达是否使用txt文件读取功能
\ No newline at end of file
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/rviz/lslidar.rviz b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/rviz/lslidar.rviz
new file mode 100644
index 0000000..4b104c7
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/rviz/lslidar.rviz
@@ -0,0 +1,161 @@
+Panels:
+ - Class: rviz_common/Displays
+ Help Height: 78
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Global Options1
+ - /Status1
+ - /LaserScan1
+ Splitter Ratio: 0.3441176414489746
+ Tree Height: 617
+ - Class: rviz_common/Selection
+ Name: Selection
+ - Class: rviz_common/Tool Properties
+ Expanded:
+ - /2D Goal Pose1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.5886790156364441
+ - Class: rviz_common/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz_default_plugins/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.029999999329447746
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 10
+ Reference Frame:
+ Value: true
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz_default_plugins/LaserScan
+ Color: 255; 255; 255
+ Color Transformer: Intensity
+ Decay Time: 0
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Max Intensity: 0
+ Min Color: 0; 0; 0
+ Min Intensity: 0
+ Name: LaserScan
+ Position Transformer: XYZ
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.009999999776482582
+ Style: Flat Squares
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ Filter size: 10
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: scan
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Fixed Frame: laser
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz_default_plugins/Interact
+ Hide Inactive Objects: true
+ - Class: rviz_default_plugins/MoveCamera
+ - Class: rviz_default_plugins/Select
+ - Class: rviz_default_plugins/FocusCamera
+ - Class: rviz_default_plugins/Measure
+ Line color: 128; 128; 0
+ - Class: rviz_default_plugins/SetInitialPose
+ Covariance x: 0.25
+ Covariance y: 0.25
+ Covariance yaw: 0.06853891909122467
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /initialpose
+ - Class: rviz_default_plugins/SetGoal
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /goal_pose
+ - Class: rviz_default_plugins/PublishPoint
+ Single click: true
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /clicked_point
+ Transformation:
+ Current:
+ Class: rviz_default_plugins/TF
+ Value: true
+ Views:
+ Current:
+ Class: rviz_default_plugins/Orbit
+ Distance: 3.635173797607422
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Focal Point:
+ X: 0
+ Y: 0
+ Z: 0
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.8653978705406189
+ Target Frame:
+ Value: Orbit (rviz)
+ Yaw: 3.095397710800171
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 846
+ Hide Left Dock: false
+ Hide Right Dock: false
+ QMainWindow State: 000000ff00000000fd000000040000000000000156000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002f4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000292000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: false
+ Width: 1283
+ X: 406
+ Y: 152
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/input.cc b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/input.cc
new file mode 100644
index 0000000..cfa40a8
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/input.cc
@@ -0,0 +1,398 @@
+#include "lslidar_driver/input.h"
+
+extern volatile sig_atomic_t flag;
+namespace lslidar_driver
+{
+ static const size_t packet_size_input = 400;
+ ////////////////////////////////////////////////////////////////////////
+ // Input base class implementation
+ ////////////////////////////////////////////////////////////////////////
+
+ /** @brief constructor
+ *
+ * @param private_nh ROS private handle for calling node.
+ * @param port UDP port number.
+ */
+ Input::Input(rclcpp::Node *private_nh, uint16_t port) : private_nh_(private_nh), port_(port) {
+ npkt_update_flag_ = false;
+ cur_rpm_ = 0;
+ return_mode_ = 1;
+ devip_str_difop = std::string("192.168.1.200");
+ devip_str_ = std::string("192.168.1.102");
+ lidar_name = std::string("M10");
+ add_multicast = false;
+ group_ip = std::string("224.1.1.2");
+ UDP_PORT_NUMBER_DIFOP = 2369;
+
+
+ private_nh->declare_parameter("device_ip","192.168.1.102");
+ private_nh->declare_parameter("device_ip_difop","192.168.1.200");
+ private_nh->declare_parameter("add_multicast",false);
+ private_nh->declare_parameter("group_ip","224.1.1.2");
+ private_nh->declare_parameter("difop_port",2369);
+
+
+ private_nh->get_parameter("lidar_name", lidar_name);
+ private_nh->get_parameter("device_ip", devip_str_);
+ private_nh->get_parameter("add_multicast", add_multicast);
+ private_nh->get_parameter("group_ip", group_ip);
+ private_nh->get_parameter("difop_port", UDP_PORT_NUMBER_DIFOP);
+ private_nh->get_parameter("device_ip_difop", devip_str_difop);
+
+ if (!devip_str_.empty())
+ RCLCPP_INFO(private_nh->get_logger(), "[driver][input] accepting packets from IP address: %s port: %d",
+ devip_str_.c_str(),port);
+ }
+
+ /** @brief constructor
+ *
+ * @param private_nh ROS private handle for calling node.
+ * @param port UDP port number
+ */
+ InputSocket::InputSocket(rclcpp::Node *private_nh, uint16_t port) : Input(private_nh, port) {
+ sockfd_ = -1;
+
+ if (!devip_str_.empty()) {
+ inet_aton(devip_str_.c_str(), &devip_);
+ inet_aton(devip_str_difop.c_str(), &devip_difop);
+ }
+
+ RCLCPP_INFO(private_nh_->get_logger(), "[driver][socket] Opening UDP socket: port %d", port);
+ sockfd_ = socket(PF_INET, SOCK_DGRAM, 0);
+ if (sockfd_ == -1) {
+ perror("socket"); // TODO: ROS_ERROR errno
+ return;
+ }
+
+ int opt = 1;
+ if (setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (const void *) &opt, sizeof(opt))) {
+ perror("setsockopt error!\n");
+ return;
+ }
+
+ sockaddr_in my_addr; // my address information
+ memset(&my_addr, 0, sizeof(my_addr)); // initialize to zeros
+ my_addr.sin_family = AF_INET; // host byte order
+ my_addr.sin_port = htons(port); // port in network byte order
+ my_addr.sin_addr.s_addr = INADDR_ANY; // automatically fill in my IP
+
+ if (bind(sockfd_, (sockaddr * ) & my_addr, sizeof(sockaddr)) == -1) {
+ perror("bind"); // TODO: ROS_ERROR errno
+ return;
+ }
+
+ if (add_multicast) {
+ struct ip_mreq group;
+ group.imr_multiaddr.s_addr = inet_addr(group_ip.c_str());
+ group.imr_interface.s_addr = htonl(INADDR_ANY);
+
+ if (setsockopt(sockfd_, IPPROTO_IP, IP_ADD_MEMBERSHIP, (char *) &group, sizeof(group)) < 0) {
+ perror("Adding multicast group error ");
+ close(sockfd_);
+ exit(1);
+ } else
+ printf("Adding multicast group...OK.\n");
+ }
+ if (fcntl(sockfd_, F_SETFL, O_NONBLOCK | FASYNC) < 0) {
+ perror("non-block");
+ return;
+ }
+ }
+
+ /** @brief destructor */
+ InputSocket::~InputSocket(void) {
+ (void) close(sockfd_);
+ }
+
+ void Input::UDP_difop()
+ {
+ sockaddr_in server_sai;
+ server_sai.sin_family = AF_INET; // IPV4 协议族
+ server_sai.sin_port = htons(UDP_PORT_NUMBER_DIFOP);
+ server_sai.sin_addr.s_addr = inet_addr(devip_str_.c_str());
+ for (int k = 0; k < 10; k++)
+ {
+ unsigned char data[188]= {0x00};
+ data[0] = 0xA5;
+ data[1] = 0x5A;
+ data[2] = 0x55;
+ data[184] = 0x08;
+ data[185] = 0x01;
+ data[186] = 0xFA;
+ data[187] = 0xFB;
+ int rtn = sendto(sockfd_, data, 188, 0, (struct sockaddr *)&server_sai, sizeof(struct sockaddr));
+ if (rtn < 0) printf("start scan error !\n");
+ else return;
+ }
+ return;
+ }
+
+ void Input::UDP_order(const std_msgs::msg::Int8 msg)
+ {
+ int i = msg.data;
+ sockaddr_in server_sai;
+ server_sai.sin_family = AF_INET; // IPV4 协议族
+ server_sai.sin_port = htons(UDP_PORT_NUMBER_DIFOP);
+ server_sai.sin_addr.s_addr = inet_addr(devip_str_.c_str());
+ int rtn = 0;
+ for (int k = 0; k < 10; k++)
+ {
+ unsigned char data[188]= {0x00};
+ data[0] = 0xA5;
+ data[1] = 0x5A;
+ data[2] = 0x55;
+ data[186] = 0xFA;
+ data[187] = 0xFB;
+ if(lidar_name == "M10" || lidar_name == "M10_GPS" || lidar_name == "M10_P"){
+ if (i <= 1){ //雷达启停
+ data[184] = 0x01;
+ data[185] = char(i);
+ }
+ else if (i == 2){ //雷达点云不滤波
+ data[181] = 0x0A;
+ data[184] = 0x06;
+ data[185] = 0x01;
+ }
+ else if (i == 3){ //雷达点云正常滤波
+ data[181] = 0x0B;
+ data[184] = 0x06;
+ data[185] = 0x01;
+ }
+ else if (i == 4){ //雷达近距离滤波
+ data[181] = 0x0C;
+ data[184] = 0x06;
+ data[185] = 0x01;
+ }
+ else if (i == 100){ //接收设备包
+ data[184] = 0x08;
+ data[185] = 0x01;
+ }
+ else return;
+ }
+ else if (lidar_name == "M10_PLUS"){
+ data[184] = 0x0A;
+ data[185] = 0x01;
+ if(i == 5) {
+ data[141] = 0x01;
+ data[142] = 0x2c;
+ }
+ else if(i == 6) {
+ data[141] = 0x01;
+ data[142] = 0x68;
+ }
+ else if(i == 8) {
+ data[141] = 0x01;
+ data[142] = 0xe0;
+ }
+ else if(i == 10) {
+ data[141] = 0x02;
+ data[142] = 0x58;
+ }
+ else if(i == 12) {
+ data[141] = 0x02;
+ data[142] = 0xd0;
+ }
+ else if(i == 15) {
+ data[141] = 0x03;
+ data[142] = 0x84;
+ }
+ else if(i == 20) {
+ data[141] = 0x04;
+ data[142] = 0xb0;
+ }
+ else if(i <= 1) {
+ data[184] = 0x01;
+ data[185] = char(i);
+ }
+ else if(i == 100) { //接收设备包
+ data[184] = 0x08;
+ data[185] = 0x01;
+ }
+ else return;
+ }
+ else if(lidar_name == "N10"){
+ if(i <= 1){
+ data[185] = char(i);
+ data[184] = 0x01;
+ }
+ else if(i>=6 && i<=12){
+ data[172] = char(i);
+ data[184] = 0x0a;
+ data[185] = 0X01;
+ }
+ else return;
+ }
+ rtn = sendto(sockfd_, data, 188, 0, (struct sockaddr *)&server_sai, sizeof(struct sockaddr));
+ if (rtn < 0)
+ {
+ printf("start scan error !\n");
+ }
+ else
+ {
+ if (i == 1)
+ usleep(3000000);
+ return;
+ }
+ }
+ return;
+ }
+
+
+
+ int InputSocket::getPacket(lslidar_msgs::msg::LslidarPacket::UniquePtr &packet)
+ {
+ int q = 0;
+ struct pollfd fds[1];
+ fds[0].fd = sockfd_;
+ fds[0].events = POLLIN;
+ static const int POLL_TIMEOUT = 2000; // one second (in msec)
+
+ sockaddr_in sender_address{};
+ socklen_t sender_address_len = sizeof(sender_address);
+ while (flag == 1)
+ {
+ // poll() until input available
+ do {
+ int retval = poll(fds, 1, POLL_TIMEOUT);
+ if (retval < 0) // poll() error?
+ {
+ if (errno != EINTR)
+ RCLCPP_ERROR(private_nh_->get_logger(), "[driver][socket] poll() error: %s", strerror(errno));
+ return 0;
+ }
+ if (retval == 0) // poll() timeout?
+ {
+ RCLCPP_WARN(private_nh_->get_logger(), "lslidar poll() timeout, port: %d",port_);
+ return 0;
+ }
+ if ((fds[0].revents & POLLERR) || (fds[0].revents & POLLHUP) || (fds[0].revents & POLLNVAL)) // device error?
+ {
+ RCLCPP_ERROR(private_nh_->get_logger(),"poll() reports lslidar error");
+ return 0;
+ }
+ } while ((fds[0].revents & POLLIN) == 0);
+
+ // Receive packets that should now be available from the
+ // socket using a blocking read.
+ ssize_t nbytes = recvfrom(sockfd_, &packet->data[0], packet_size_input, 0,
+ (sockaddr *)&sender_address, &sender_address_len);
+ // ROS_DEBUG_STREAM("incomplete lslidar packet read: "
+ // << nbytes << " bytes");
+ q = (int)nbytes;
+ if (nbytes < 0)
+ {
+ if (errno != EWOULDBLOCK)
+ {
+ perror("recvfail");
+ RCLCPP_ERROR(private_nh_->get_logger(),"recvfail");
+ return 1;
+ }
+ }
+ else if ((size_t)nbytes <= packet_size_input || (size_t)nbytes >= 50)
+ {
+
+ // read successful,
+ // if packet is not from the lidar scanner we selected by IP,
+ // continue otherwise we are done
+ if (devip_str_ != "" && sender_address.sin_addr.s_addr != devip_.s_addr)
+ continue;
+ else
+ break; // done
+ }
+
+ }
+ if (flag == 0)
+ {
+ abort();
+ }
+
+ return q;
+ }
+ InputPCAP::InputPCAP(rclcpp::Node *private_nh, uint16_t port, double packet_rate, std::string filename) : Input(private_nh, port),
+ packet_rate_(packet_rate),
+ filename_(filename)
+ {
+ pcap_ = NULL;
+ empty_ = true;
+ read_once_ = false;
+ read_fast_ = false;
+ repeat_delay_ = 0.0;
+ private_nh->get_parameter("read_once", read_once_);
+ private_nh->get_parameter("read_fast", read_fast_);
+ private_nh->get_parameter("repeat_delay", repeat_delay_);
+
+ if (read_once_)
+ RCLCPP_WARN(private_nh_->get_logger(),"Read input file only once.");
+ if (read_fast_)
+ RCLCPP_WARN(private_nh_->get_logger(),"Read input file as quickly as possible.");
+ if (repeat_delay_ > 0.0)
+ RCLCPP_WARN(private_nh_->get_logger(),"Delay %.3f seconds before repeating input file.", repeat_delay_);
+
+ RCLCPP_INFO(private_nh_->get_logger(),"Opening PCAP file %s",filename_.c_str());
+ if ((pcap_ = pcap_open_offline(filename_.c_str(), errbuf_)) == NULL)
+ {
+ RCLCPP_WARN(private_nh_->get_logger(),"Error opening lslidar socket dump file.");
+ return;
+ }
+ std::stringstream filter;
+ if (devip_str_ != "")
+ {
+ filter << "src host " << devip_str_ << "&&";
+ }
+ filter << "udp dst port " << port;
+ pcap_compile(pcap_, &pcap_packet_filter_, filter.str().c_str(), 1, PCAP_NETMASK_UNKNOWN);
+ }
+
+ InputPCAP::~InputPCAP(void)
+ {
+ pcap_close(pcap_);
+ }
+
+ int InputPCAP::getPacket(lslidar_msgs::msg::LslidarPacket::UniquePtr &pkt)
+ {
+ struct pcap_pkthdr *header;
+ const u_char *pkt_data;
+ while (flag == 1)
+ {
+ int res;
+ if ((res = pcap_next_ex(pcap_, &header, &pkt_data)) >= 0)
+ {
+ // skip packets not for the correct port and from the selected IP address
+ if (!devip_str_.empty() && (0 == pcap_offline_filter(&pcap_packet_filter_, header, pkt_data)))
+ continue;
+
+ if (read_fast_ == false)
+ packet_rate_.sleep();
+ mempcpy(&pkt->data[0], pkt_data + 42, packet_size_input);
+ empty_ = false;
+ return 0;
+ }
+ if (empty_)
+ {
+ RCLCPP_WARN(private_nh_->get_logger(),"Error %d reading lslidar packet: %s", res, pcap_geterr(pcap_));
+ return -1;
+ }
+ if (read_once_)
+ {
+ RCLCPP_WARN(private_nh_->get_logger(),"end of file reached -- done reading.");
+ return -1;
+ }
+ if (repeat_delay_ > 0.0)
+ {
+ RCLCPP_WARN(private_nh_->get_logger(),"end of file reached -- delaying %.3f seconds.", repeat_delay_);
+ usleep(rint(repeat_delay_ * 1000000.0));
+ }
+ RCLCPP_WARN(private_nh_->get_logger(),"replayding lsliar dump file");
+
+ pcap_close(pcap_);
+ pcap_ = pcap_open_offline(filename_.c_str(), errbuf_);
+ empty_ = true;
+ }
+ if (flag == 0)
+ {
+ abort();
+ }
+ return 0;
+ }
+
+} // namespace
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lsiosr.cpp b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lsiosr.cpp
new file mode 100644
index 0000000..d560b12
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lsiosr.cpp
@@ -0,0 +1,400 @@
+/*******************************************************
+@company: Copyright (C) 2022, Leishen Intelligent System
+@product: LSM10 and N10
+@filename: lsiosr.cpp
+@brief:
+@version: date: author: comments:
+@v1.0 21-2-4 yao new
+*******************************************************/
+#include "lslidar_driver/lsiosr.h"
+
+namespace lslidar_driver {
+
+LSIOSR * LSIOSR::instance(std::string name, int speed, int fd)
+{
+ static LSIOSR obj(name, speed, fd);
+ return &obj;
+}
+
+LSIOSR::LSIOSR(std::string port, int baud_rate, int fd):port_(port), baud_rate_(baud_rate), fd_(fd)
+{
+ printf("port = %s, baud_rate = %d\n", port.c_str(), baud_rate);
+}
+
+LSIOSR::~LSIOSR()
+{
+ close();
+}
+/* 串口配置的函数 */
+int LSIOSR::setOpt(int nBits, uint8_t nEvent, int nStop)
+{
+ struct termios newtio, oldtio;
+ /*保存测试现有串口参数设置,在这里如果串口号等出错,会有相关的出错信息*/
+ if (tcgetattr(fd_, &oldtio) != 0)
+ {
+ perror("SetupSerial 1");
+ return -1;
+ }
+ bzero(&newtio, sizeof(newtio));
+ /*步骤一,设置字符大小*/
+ newtio.c_cflag |= CLOCAL; //如果设置,modem 的控制线将会被忽略。如果没有设置,则 open()函数会阻塞直到载波检测线宣告 modem 处于摘机状态为止。
+ newtio.c_cflag |= CREAD; //使端口能读取输入的数据
+ /*设置每个数据的位数*/
+ switch (nBits)
+ {
+ case 7:
+ newtio.c_cflag |= CS7;
+ break;
+ case 8:
+ newtio.c_cflag |= CS8;
+ break;
+ }
+ /*设置奇偶校验位*/
+ switch (nEvent)
+ {
+ case 'O': //奇数
+ newtio.c_iflag |= (INPCK | ISTRIP);
+ newtio.c_cflag |= PARENB; //使能校验,如果不设PARODD则是偶校验
+ newtio.c_cflag |= PARODD; //奇校验
+ break;
+ case 'E': //偶数
+ newtio.c_iflag |= (INPCK | ISTRIP);
+ newtio.c_cflag |= PARENB;
+ newtio.c_cflag &= ~PARODD;
+ break;
+ case 'N': //无奇偶校验位
+ newtio.c_cflag &= ~PARENB;
+ break;
+ }
+ /*设置波特率*/
+ switch (baud_rate_)
+ {
+ case 230400:
+ cfsetispeed(&newtio, B230400);
+ cfsetospeed(&newtio, B230400);
+ break;
+ case 460800:
+ cfsetispeed(&newtio, B460800);
+ cfsetospeed(&newtio, B460800);
+ break;
+ case 500000:
+ cfsetispeed(&newtio, B500000);
+ cfsetospeed(&newtio, B500000);
+ break;
+ case 921600:
+ cfsetispeed(&newtio, B921600);
+ cfsetospeed(&newtio, B921600);
+ break;
+ default:
+ cfsetispeed(&newtio, B460800);
+ cfsetospeed(&newtio, B460800);
+ break;
+ }
+
+ /*
+ * 设置停止位
+ * 设置停止位的位数, 如果设置,则会在每帧后产生两个停止位, 如果没有设置,则产生一个
+ * 停止位。一般都是使用一位停止位。需要两位停止位的设备已过时了。
+ * */
+ if (nStop == 1)
+ newtio.c_cflag &= ~CSTOPB;
+ else if (nStop == 2)
+ newtio.c_cflag |= CSTOPB;
+ /*设置等待时间和最小接收字符*/
+ newtio.c_cc[VTIME] = 0;
+ newtio.c_cc[VMIN] = 0;
+ /*处理未接收字符*/
+ tcflush(fd_, TCIFLUSH);
+ /*激活新配置*/
+ if ((tcsetattr(fd_, TCSANOW, &newtio)) != 0)
+ {
+ perror("serial set error");
+ return -1;
+ }
+
+ return 0;
+}
+
+void LSIOSR::flushinput() {
+ tcflush(fd_, TCIFLUSH);
+}
+
+/* 从串口中读取数据 */
+int LSIOSR::read(unsigned char *buffer, int length, int timeout)
+{
+ memset(buffer, 0, length);
+
+ int totalBytesRead = 0;
+ int rc;
+ int unlink = 0;
+ unsigned char* pb = buffer;
+
+ if (timeout > 0)
+ {
+ rc = waitReadable(timeout);
+ if (rc <= 0)
+ {
+ return (rc == 0) ? 0 : -1;
+ }
+
+ int retry = 3;
+ while (length > 0)
+ {
+ rc = ::read(fd_, pb, (size_t)length);
+
+ if (rc > 0)
+ {
+ length -= rc;
+ pb += rc;
+ totalBytesRead += rc;
+
+ if (length == 0)
+ {
+ break;
+ }
+ }
+ else if (rc < 0)
+ {
+ printf("error \n");
+ retry--;
+ if (retry <= 0)
+ {
+ break;
+ }
+ }
+ unlink++;
+ rc = waitReadable(20);
+ if(unlink > 10)
+ return -1;
+
+ if (rc <= 0)
+ {
+ break;
+ }
+ }
+ }
+ else
+ {
+ rc = ::read(fd_, pb, (size_t)length);
+
+ if (rc > 0)
+ {
+ totalBytesRead += rc;
+ }
+ else if ((rc < 0) && (errno != EINTR) && (errno != EAGAIN))
+ {
+ printf("read error\n");
+ return -1;
+ }
+ }
+
+ return totalBytesRead;
+}
+
+int LSIOSR::waitReadable(int millis)
+{
+ if (fd_ < 0)
+ {
+ return -1;
+ }
+ int serial = fd_;
+
+ fd_set fdset;
+ struct timeval tv;
+ int rc = 0;
+
+ while (millis > 0)
+ {
+ if (millis < 5000)
+ {
+ tv.tv_usec = millis % 1000 * 1000;
+ tv.tv_sec = millis / 1000;
+
+ millis = 0;
+ }
+ else
+ {
+ tv.tv_usec = 0;
+ tv.tv_sec = 5;
+
+ millis -= 5000;
+ }
+
+ FD_ZERO(&fdset);
+ FD_SET(serial, &fdset);
+
+ rc = select(serial + 1, &fdset, NULL, NULL, &tv);
+ if (rc > 0)
+ {
+ rc = (FD_ISSET(serial, &fdset)) ? 1 : -1;
+ break;
+ }
+ else if (rc < 0)
+ {
+ rc = -1;
+ break;
+ }
+ }
+
+ return rc;
+}
+
+
+int LSIOSR::waitWritable(int millis)
+{
+ if (fd_ < 0)
+ {
+ return -1;
+ }
+ int serial = fd_;
+
+ fd_set fdset;
+ struct timeval tv;
+ int rc = 0;
+
+ while (millis > 0)
+ {
+ if (millis < 5000)
+ {
+ tv.tv_usec = millis % 1000 * 1000;
+ tv.tv_sec = millis / 1000;
+
+ millis = 0;
+ }
+ else
+ {
+ tv.tv_usec = 0;
+ tv.tv_sec = 5;
+
+ millis -= 5000;
+ }
+
+ FD_ZERO(&fdset);
+ FD_SET(serial, &fdset);
+
+ rc = select(serial + 1, NULL, &fdset, NULL, &tv);
+ if (rc > 0)
+ {
+ rc = (FD_ISSET(serial, &fdset)) ? 1 : -1;
+ break;
+ }
+ else if (rc < 0)
+ {
+ rc = -1;
+ break;
+ }
+ }
+
+ return rc;
+}
+
+/* 向串口中发送数据 */
+int LSIOSR::send(const char* buffer, int length, int timeout)
+{
+ if (fd_ < 0)
+ {
+ return -1;
+ }
+
+ if ((buffer == 0) || (length <= 0))
+ {
+ return -1;
+ }
+
+ int totalBytesWrite = 0;
+ int rc;
+ char* pb = (char*)buffer;
+
+
+ if (timeout > 0)
+ {
+ rc = waitWritable(timeout);
+ if (rc <= 0)
+ {
+ return (rc == 0) ? 0 : -1;
+ }
+
+ int retry = 3;
+ while (length > 0)
+ {
+ rc = write(fd_, pb, (size_t)length);
+ if (rc > 0)
+ {
+ length -= rc;
+ pb += rc;
+ totalBytesWrite += rc;
+
+ if (length == 0)
+ {
+ break;
+ }
+ }
+ else
+ {
+ retry--;
+ if (retry <= 0)
+ {
+ break;
+ }
+ }
+
+ rc = waitWritable(50);
+ if (rc <= 0)
+ {
+ break;
+ }
+ }
+ }
+ else
+ {
+ rc = write(fd_, pb, (size_t)length);
+ if (rc > 0)
+ {
+ totalBytesWrite += rc;
+ }
+ else if ((rc < 0) && (errno != EINTR) && (errno != EAGAIN))
+ {
+ return -1;
+ }
+ }
+
+ return totalBytesWrite;
+}
+
+int LSIOSR::init()
+{
+ int error_code = 0;
+
+ fd_ = open(port_.c_str(), O_RDWR|O_NOCTTY|O_NDELAY);
+ if (0 < fd_)
+ {
+ error_code = 0;
+ setOpt(DATA_BIT_8, PARITY_NONE, STOP_BIT_1);//设置串口参数
+ //printf("open_port %s OK !\n", port_.c_str());
+ }
+ else
+ {
+ error_code = -1;
+ }
+
+ return error_code;
+}
+
+int LSIOSR::close()
+{
+ ::close(fd_);
+ return 0;
+}
+
+std::string LSIOSR::getPort()
+{
+ return port_;
+}
+
+int LSIOSR::setPortName(std::string name)
+{
+ port_ = name;
+ return 0;
+}
+
+}
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver.cc b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver.cc
new file mode 100644
index 0000000..a141253
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver.cc
@@ -0,0 +1,1423 @@
+/*
+ * This file is part of lslidar driver.
+ *
+ * The driver is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * The driver is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with the driver. If not, see .
+ */
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include "rclcpp/rclcpp.hpp"
+#include "lslidar_driver/lslidar_driver.h"
+#include
+
+int truncated_mode_=0; //多角度屏蔽开关:默认为0,如果需要屏蔽多个角度,则truncated_mode_赋值为1。
+
+int scan_crop_min[]={0,180}; //雷达屏蔽角度,这里屏蔽角度为135°到225°,
+ //如果要多角度屏蔽,如10~30,50~60,改为:
+ //scan_angle_min[]={10,50};scan_angle_max[]={30,60};
+int scan_crop_max[]={90,270}; //修改后编译即可
+
+
+namespace lslidar_driver
+{
+
+ static void my_hander(int sig)
+ {
+ printf("sig: %d", sig);
+ abort();
+ }
+ LslidarDriver::LslidarDriver() : LslidarDriver(rclcpp::NodeOptions()) {}
+ LslidarDriver::LslidarDriver(const rclcpp::NodeOptions &options) : Node("lslidar_driver_node", options), diagnostics(this)
+ {
+ signal(SIGINT, my_hander);
+
+ if (!this->initialize())
+ RCLCPP_ERROR(this->get_logger(), "Could not initialize the driver...");
+ else
+ RCLCPP_INFO(this->get_logger(), "Successfully initialize driver...");
+ }
+
+ LslidarDriver::~LslidarDriver()
+ {
+ return;
+ }
+
+ bool LslidarDriver::loadParameters()
+ {
+ pubscan_thread_ = new boost::thread(boost::bind(&LslidarDriver::pubScanThread, this));
+ interface_selection = std::string("net");
+ frame_id = std::string("laser_link");
+ scan_topic = std::string("/scan");
+ lidar_name = std::string("M10");
+ pointcloud_topic = std::string("/lslidar_point_cloud");
+ is_start = true;
+ min_range = 0.3;
+ max_range = 100.0;
+ use_gps_ts = true;
+ compensation = true;
+ pubScan = true;
+ pubPointCloud2 = true;
+ angle_disable_min = 0.0;
+ angle_disable_max = 0.0;
+ // 添加恒定输出长度参数
+ fixed_array_length = 450; //cyy_addcyy_add
+ this->declare_parameter("fixed_array_length", 450);//cyy_addcyy_add
+
+
+ this->declare_parameter("lidar_name", "M10");
+ this->declare_parameter("frame_id", "laser_link");
+ this->declare_parameter("scan_topic", "/scan");
+ this->declare_parameter("pointcloud_topic", "/lslidar_point_cloud");
+ this->declare_parameter("min_range", 0.3);
+ this->declare_parameter("max_range", 100.0);
+ this->declare_parameter("use_gps_ts", false);
+ this->declare_parameter("high_reflection", false);
+ this->declare_parameter("compensation", false);
+ this->declare_parameter("pubScan", true);
+ this->declare_parameter("pubPointCloud2", false);
+ this->declare_parameter("angle_disable_min", 0.0);
+ this->declare_parameter("angle_disable_max", 0.0);
+ this->declare_parameter("interface_selection", "net");
+
+ this->get_parameter("fixed_array_length", fixed_array_length);//cyy_addcyy_add
+ this->get_parameter("lidar_name", lidar_name);
+ this->get_parameter("frame_id", frame_id);
+ this->get_parameter("high_reflection", high_reflection);
+ this->get_parameter("scan_topic", scan_topic);
+ this->get_parameter("min_range", min_range);
+ this->get_parameter("max_range", max_range);
+ this->get_parameter("use_gps_ts", use_gps_ts);
+ this->get_parameter("compensation", compensation);
+ this->get_parameter("pointcloud_topic", pointcloud_topic);
+ this->get_parameter("pubScan", pubScan);
+ this->get_parameter("pubPointCloud2", pubPointCloud2);
+ this->get_parameter("angle_disable_min", angle_disable_min);
+ this->get_parameter("angle_disable_max", angle_disable_max);
+ this->get_parameter("interface_selection", interface_selection);
+ while (angle_disable_min < 0)
+ angle_disable_min += 360;
+ while (angle_disable_max < 0)
+ angle_disable_max += 360;
+ while (angle_disable_min > 360)
+ angle_disable_min -= 360;
+ while (angle_disable_max > 360)
+ angle_disable_max -= 360;
+ if (angle_disable_max == angle_disable_min)
+ {
+ angle_able_min = 0;
+ angle_able_max = 360;
+ }
+ else
+ {
+ if (angle_disable_min < angle_disable_max && angle_disable_min != 0.0)
+ {
+ angle_able_min = angle_disable_max;
+ angle_able_max = angle_disable_min + 360;
+ }
+ if (angle_disable_min < angle_disable_max && angle_disable_min == 0.0)
+ {
+ angle_able_min = angle_disable_max;
+ angle_able_max = 360;
+ }
+ if (angle_disable_min > angle_disable_max)
+ {
+ angle_able_min = angle_disable_max;
+ angle_able_max = angle_disable_min;
+ }
+ }
+ count_num = 0;
+
+ scan_points_.resize(6000);
+
+ if (lidar_name == "M10")
+ {
+ use_gps_ts = false;
+ PACKET_SIZE = 92;
+ package_points = 42;
+ data_bits_start = 6;
+ degree_bits_start = 2;
+ rpm_bits_start = 4;
+ baud_rate_ = 460800;
+ points_size_ = 1008;
+ }
+ else if (lidar_name == "M10_P")
+ {
+ PACKET_SIZE = 160;
+ package_points = 70;
+ data_bits_start = 8;
+ degree_bits_start = 4;
+ rpm_bits_start = 6;
+ baud_rate_ = 500000;
+ points_size_ = 2000;
+ }
+ else if (lidar_name == "M10_PLUS")
+ {
+ PACKET_SIZE = 104;
+ package_points = 41;
+ data_bits_start = 8;
+ degree_bits_start = 4;
+ rpm_bits_start = 6;
+ points_size_ = 5000;
+ baud_rate_ = 921600;
+ }
+ else if (lidar_name == "M10_GPS")
+ {
+ PACKET_SIZE = 102;
+ package_points = 42;
+ data_bits_start = 6;
+ degree_bits_start = 2;
+ rpm_bits_start = 4;
+ baud_rate_ = 460800;
+ points_size_ = 1008;
+ }
+ else if (lidar_name == "N10")
+ {
+ PACKET_SIZE = 58;
+ package_points = 16;
+ data_bits_start = 7;
+ degree_bits_start = 5;
+ end_degree_bits_start = 55;
+ baud_rate_ = 230400;
+ points_size_ = 2000;
+ use_gps_ts = false;
+ compensation = false;
+ }
+ else if (lidar_name == "M10_DOUBLE")
+ {
+ PACKET_SIZE = 300;
+ package_points = 70;
+ data_bits_start = 8;
+ degree_bits_start = 4;
+ rpm_bits_start = 6;
+ points_size_ = 3000;
+ baud_rate_ = 921600;
+ }
+ else if (lidar_name == "N10_P")
+ {
+ PACKET_SIZE = 108;
+ package_points = 16;
+ data_bits_start = 7;
+ degree_bits_start = 5;
+ end_degree_bits_start = 105;
+ baud_rate_ = 460800;
+ points_size_ = 2000;
+ use_gps_ts = false;
+ compensation = false;
+ }
+ else if (lidar_name == "L10")
+ {
+ PACKET_SIZE = 58;
+ package_points = 16;
+ data_bits_start = 7;
+ degree_bits_start = 5;
+ end_degree_bits_start = 55;
+ baud_rate_ = 230400;
+ points_size_ = 2000;
+ use_gps_ts = false;
+ compensation = false;
+ }
+ RCLCPP_INFO_STREAM(this->get_logger(), "Lidar is " << lidar_name.c_str());
+
+ if (pubScan)
+ scan_pub = this->create_publisher(scan_topic, 10);
+ if (pubPointCloud2)
+ point_cloud_pub = this->create_publisher(pointcloud_topic, 10);
+ difop_switch = this->create_subscription("lslidar_order", 1, std::bind(&LslidarDriver::lidar_order, this, std::placeholders::_1)); // 转速输入
+ return true;
+ }
+
+ void LslidarDriver::lidar_difop()
+ {
+ if (lidar_name == "L10" || lidar_name == "N10" || lidar_name == "N10_P")
+ return;
+ if (interface_selection == "net")
+ msop_input_->UDP_difop();
+ else
+ {
+ for (int k = 0; k < 10; k++)
+ {
+ unsigned char data[188] = {0x00};
+ data[0] = 0xA5;
+ data[1] = 0x5A;
+ data[2] = 0x55;
+ data[184] = 0x08;
+ data[185] = 0x01;
+ data[186] = 0xFA;
+ data[187] = 0xFB;
+ int rtn = serial_->send((const char *)data, 188);
+ if (rtn < 0)
+ printf("start scan error !\n");
+ else
+ return;
+ }
+ }
+ return;
+ }
+
+ void LslidarDriver::lidar_order(const std_msgs::msg::Int8::SharedPtr msg)
+ {
+ if (lidar_name == "L10")
+ return;
+ int i = msg->data;
+ if (i == 0)
+ is_start = false;
+ else
+ is_start = true;
+ if (interface_selection == "net")
+ msop_input_->UDP_order(*msg);
+ else
+ {
+ int i = msg->data;
+ for (int k = 0; k < 10; k++)
+ {
+ int rtn;
+ unsigned char data[188] = {0x00};
+ data[0] = 0xA5;
+ data[1] = 0x5A;
+ data[2] = 0x55;
+ data[186] = 0xFA;
+ data[187] = 0xFB;
+
+ if (lidar_name == "M10" || lidar_name == "M10_GPS" || lidar_name == "M10_P" || lidar_name == "M10_DOUBLE")
+ {
+ if (i <= 1)
+ { // 雷达启停
+ data[184] = 0x01;
+ data[185] = char(i);
+ }
+ else if (i == 2)
+ { // 雷达点云不滤波
+ data[181] = 0x0A;
+ data[184] = 0x06;
+ if (is_start)
+ data[185] = 0x01;
+ }
+ else if (i == 3)
+ { // 雷达点云正常滤波
+ data[181] = 0x0B;
+ data[184] = 0x06;
+ if (is_start)
+ data[185] = 0x01;
+ }
+ else if (i == 4)
+ { // 雷达近距离滤波
+ data[181] = 0x0C;
+ data[184] = 0x06;
+ if (is_start)
+ data[185] = 0x01;
+ }
+ else if (i == 100)
+ { // 接收设备包
+ data[184] = 0x08;
+ data[185] = 0x01;
+ }
+ else
+ return;
+ }
+ else if (lidar_name == "M10_PLUS")
+ {
+ data[184] = 0x0A;
+ data[185] = 0x01;
+ if (i == 5)
+ {
+ data[141] = 0x01;
+ data[142] = 0x2c;
+ }
+ else if (i == 6)
+ {
+ data[141] = 0x01;
+ data[142] = 0x68;
+ }
+ else if (i == 8)
+ {
+ data[141] = 0x01;
+ data[142] = 0xe0;
+ }
+ else if (i == 10)
+ {
+ data[141] = 0x02;
+ data[142] = 0x58;
+ }
+ else if (i == 12)
+ {
+ data[141] = 0x02;
+ data[142] = 0xd0;
+ }
+ else if (i == 15)
+ {
+ data[141] = 0x03;
+ data[142] = 0x84;
+ }
+ else if (i == 20)
+ {
+ data[141] = 0x04;
+ data[142] = 0xb0;
+ }
+ else if (i <= 1)
+ {
+ data[184] = 0x01;
+ data[185] = char(i);
+ }
+ else if (i == 100) // 接收设备包
+ {
+ data[184] = 0x08;
+ data[185] = 0x01;
+ }
+ else
+ return;
+ }
+ else if (lidar_name == "N10" || lidar_name == "N10_P")
+ {
+ if (i <= 1)
+ {
+ data[185] = char(i);
+ data[184] = 0x01;
+ }
+ else if (i >= 6 && i <= 12)
+ {
+ data[172] = char(i);
+ data[184] = 0x0a;
+ data[185] = 0X01;
+ }
+ else
+ return;
+ }
+ rtn = serial_->send((const char *)data, 188);
+ if (rtn < 0)
+ printf("start scan error !\n");
+ else
+ {
+ if (i == 1)
+ usleep(1000000); // 1.0s
+ if (i == 0)
+ is_start = false;
+ if (i == 1)
+ is_start = true;
+ return;
+ }
+ }
+ return;
+ }
+ }
+
+ void LslidarDriver::open_serial()
+ {
+ diagnostics.setHardwareID("Lslidar");
+ int code = 0;
+ serial_port_ = std::string("/dev/ttyUSB0");
+ this->declare_parameter("serial_port_", "/dev/ttyUSB0");
+ this->get_parameter("serial_port_", serial_port_);
+ serial_ = LSIOSR::instance(serial_port_, baud_rate_);
+ code = serial_->init();
+ if (code != 0)
+ {
+ printf("open_port %s ERROR !\n", serial_port_.c_str());
+ rclcpp::shutdown();
+ exit(0);
+ }
+ printf("open_port %s OK !\n", serial_port_.c_str());
+ }
+
+ bool LslidarDriver::createRosIO()
+ {
+ UDP_PORT_NUMBER = 2368;
+ this->declare_parameter("msop_port", 2368);
+ this->get_parameter("msop_port", UDP_PORT_NUMBER);
+ RCLCPP_INFO_STREAM(this->get_logger(), "Opening UDP socket: port " << UDP_PORT_NUMBER);
+ dump_file = std::string("");
+ this->declare_parameter("pcap", "");
+ this->get_parameter("pcap", dump_file);
+ // ROS diagnostics
+ diagnostics.setHardwareID("Lslidar");
+
+ const double diag_freq = 12 * 24;
+ diag_max_freq = diag_freq;
+ diag_min_freq = diag_freq;
+ RCLCPP_INFO(this->get_logger(), "expected frequency: %.3f (Hz)", diag_freq);
+
+ using namespace diagnostic_updater;
+ diag_topic.reset(new TopicDiagnostic(
+ "lslidar_packets", diagnostics,
+ FrequencyStatusParam(&diag_min_freq, &diag_max_freq, 0.1, 10),
+ TimeStampStatusParam()));
+
+ int hz = 10;
+ if (lidar_name == "M10_P")
+ hz = 12;
+ else if (lidar_name == "M10_PLUS")
+ hz = 20;
+
+ double packet_rate = hz * 24;
+ if (dump_file != "")
+ {
+ msop_input_.reset(new lslidar_driver::InputPCAP(this, UDP_PORT_NUMBER, packet_rate, dump_file));
+ }
+ else
+ {
+ msop_input_.reset(new lslidar_driver::InputSocket(this, UDP_PORT_NUMBER));
+ }
+
+ // Output
+ return true;
+ }
+
+ int LslidarDriver::getScan(std::vector &points, rclcpp::Time &scan_time, float &scan_duration)
+ {
+ boost::unique_lock lock(mutex_);
+ points.assign(scan_points_bak_.begin(), scan_points_bak_.end());
+ scan_time = pre_time_;
+ scan_duration = time_.seconds() - pre_time_.seconds();
+ return 1;
+ }
+
+ uint64_t LslidarDriver::get_gps_stamp(struct tm t)
+ {
+
+ uint64_t ptime = static_cast(timegm(&t));
+ return ptime;
+ }
+
+ bool LslidarDriver::initialize()
+ {
+ if (!loadParameters())
+ {
+ RCLCPP_ERROR(this->get_logger(), "Cannot load all required ROS parameters...");
+ return false;
+ }
+ if (interface_selection == "net")
+ {
+ if (!createRosIO())
+ {
+ RCLCPP_ERROR(this->get_logger(), "Cannot create all ROS IO...");
+ return false;
+ }
+ }
+ else
+ {
+ in_file_name = std::string("");
+ this->declare_parameter("in_file_name", "");
+ this->get_parameter("in_file_name", in_file_name);
+ if (in_file_name == "")
+ open_serial();
+ else
+ {
+ RCLCPP_INFO_STREAM(this->get_logger(), "Opening txt file " << in_file_name.c_str());
+ std::ifstream file_reader(in_file_name);
+ if (!file_reader.is_open())
+ {
+ RCLCPP_ERROR(this->get_logger(), "Cannot open the file");
+ return false;
+ }
+ }
+ }
+ RCLCPP_INFO(this->get_logger(), "Initialised lslidar without error");
+ return true;
+ }
+
+ void LslidarDriver::recvThread_crc(int &count, int &link_time)
+ {
+ if (count <= 0)
+ link_time++;
+ else
+ link_time = 0;
+
+ if (link_time > 150)
+ {
+ serial_->close();
+ int ret = serial_->init();
+ if (ret < 0)
+ {
+ RCLCPP_ERROR(this->get_logger(), "serial open fail");
+ usleep(200000);
+ }
+ link_time = 0;
+ }
+ }
+
+ int LslidarDriver::receive_data(unsigned char *packet_bytes)
+ {
+ int link_time = 0;
+ int len_H = 0;
+ int len_L = 0;
+ int len = 0;
+ int count_2 = 0;
+ int count = 0;
+ while (count <= 0)
+ {
+ count = serial_->read(packet_bytes, 1);
+ LslidarDriver::recvThread_crc(count, link_time);
+ }
+ if (packet_bytes[0] != 0xA5)
+ return 0;
+
+ while (count_2 <= 0)
+ {
+ count_2 = serial_->read(packet_bytes + count, 1);
+ if (count_2 >= 0)
+ count += count_2;
+ LslidarDriver::recvThread_crc(count_2, link_time);
+ }
+
+ count_2 = 0;
+ if (packet_bytes[1] != 0x5A)
+ return 0;
+ while (count_2 <= 0)
+ {
+ count_2 = serial_->read(packet_bytes + count, 2);
+ if (count_2 >= 0)
+ count += count_2;
+ LslidarDriver::recvThread_crc(count_2, link_time);
+ }
+
+ count_2 = 0;
+
+ if (lidar_name == "M10")
+ len = 92;
+ else if (lidar_name == "M10_GPS")
+ len = 102;
+ else if (lidar_name == "N10_P")
+ len = 108;
+ else if (lidar_name == "N10" || lidar_name == "L10")
+ len = packet_bytes[2];
+ else
+ {
+ len_H = packet_bytes[2];
+ len_L = packet_bytes[3];
+ len = len_H * 256 + len_L;
+ }
+ if (lidar_name == "M10" || lidar_name == "M10_DOUBLE" || lidar_name == "M10_GPS" || lidar_name == "M10_P" || lidar_name == "M10_PLUS")
+ {
+ if (packet_bytes[2] == 0x55 && packet_bytes[3] == 0x00)
+ len = 188;
+ }
+ while (count < len)
+ {
+ count_2 = serial_->read(packet_bytes + count, len - count);
+ if (count_2 >= 0)
+ count += count_2;
+ LslidarDriver::recvThread_crc(count_2, link_time);
+ }
+ if (lidar_name == "N10" || lidar_name == "L10" || lidar_name == "N10_P")
+ {
+ if (packet_bytes[PACKET_SIZE - 1] != N10_CalCRC8(packet_bytes, PACKET_SIZE - 1))
+ return 0;
+ }
+ return len;
+ }
+
+ uint8_t LslidarDriver::N10_CalCRC8(unsigned char *p, int len)
+ {
+ uint8_t crc = 0;
+ int sum = 0;
+
+ for (int i = 0; i < len; i++)
+ {
+ sum += uint8_t(p[i]);
+ }
+ crc = sum & 0xff;
+ return crc;
+ }
+
+ void LslidarDriver::difop_processing(unsigned char *packet_bytes) // 处理设备包的数据
+ {
+ int s = packet_bytes[173];
+ int z = packet_bytes[174];
+ int degree_temp = s & 0x7F;
+ int sign_temp = s & 0x80;
+ degree_compensation = double(degree_temp * 256 + z) / 100.f;
+ if (sign_temp)
+ degree_compensation = -degree_compensation;
+ first_compensation = false;
+ printf("degree_compensation = %f\n", degree_compensation);
+ return;
+ }
+
+ void LslidarDriver::data_processing(unsigned char *packet_bytes, int len) // 处理每一包的数据
+ {
+ double degree;
+ double end_degree;
+ double degree_interval = 15.0;
+ boost::posix_time::ptime t1, t2;
+ t1 = boost::posix_time::microsec_clock::universal_time();
+
+ int s = packet_bytes[degree_bits_start];
+ int z = packet_bytes[degree_bits_start + 1];
+
+ degree = (s * 256 + z) / 100.f + degree_compensation;
+ degree = (degree < 0) ? degree + 360 : degree;
+ degree = (degree > 360) ? degree - 360 : degree;
+ if (lidar_name == "N10" || lidar_name == "L10")
+ {
+ int s_e = packet_bytes[end_degree_bits_start];
+ int z_e = packet_bytes[end_degree_bits_start + 1];
+
+ end_degree = (s_e * 256 + z_e) / 100.f;
+ end_degree = (end_degree > 360) ? end_degree - 360 : end_degree;
+
+ if (degree > end_degree)
+ degree_interval = end_degree + 360 - degree;
+ else
+ degree_interval = end_degree - degree;
+ }
+
+ // boost::unique_lock lock(mutex_);
+ if (lidar_name == "M10_PLUS" || lidar_name == "M10_P")
+ {
+ PACKET_SIZE = len;
+ package_points = (PACKET_SIZE - 20) / 2;
+ }
+ int invalidValue = 0;
+ int point_len = 2;
+ if (lidar_name == "N10" || lidar_name == "L10")
+ point_len = 3;
+
+ if (lidar_name == "M10_GPS" || lidar_name == "M10")
+ {
+ int err_data_84 = packet_bytes[84];
+ int err_data_85 = packet_bytes[85];
+ if ((err_data_84 * 256 + err_data_85) == 0xFFFF || packet_bytes[86] >= 0xF5)
+ {
+ packet_bytes[86] = 0xFF;
+ packet_bytes[87] = 0xFF;
+ }
+ }
+
+ for (int num = 0; num < point_len * package_points; num += point_len)
+ {
+ int s = packet_bytes[num + data_bits_start];
+ int z = packet_bytes[num + data_bits_start + 1];
+ if ((s * 256 + z) == 0xFFFF)
+ invalidValue++;
+ }
+
+ if (use_gps_ts && lidar_name != "N10")
+ {
+ pTime.tm_year = packet_bytes[PACKET_SIZE - 12] + 2000 - 1900; // x+2000
+ pTime.tm_mon = packet_bytes[PACKET_SIZE - 11] - 1; // 1-12
+ pTime.tm_mday = packet_bytes[PACKET_SIZE - 10]; // 1-31
+ pTime.tm_hour = packet_bytes[PACKET_SIZE - 9]; // 0-23
+ pTime.tm_min = packet_bytes[PACKET_SIZE - 8]; // 0-59
+ pTime.tm_sec = packet_bytes[PACKET_SIZE - 7]; // 0-59
+ sub_second = (packet_bytes[PACKET_SIZE - 6] * 256 + packet_bytes[PACKET_SIZE - 5]) * 1000000 + (packet_bytes[PACKET_SIZE - 4] * 256 + packet_bytes[PACKET_SIZE - 3]) * 1000;
+ sweep_end_time_gps = get_gps_stamp(pTime);
+ sweep_end_time_hardware = sub_second % 1000000000;
+ }
+ invalidValue = package_points - invalidValue;
+ if (lidar_name == "N10" || lidar_name == "L10")
+ invalidValue--;
+ if (invalidValue <= 1)
+ {
+ delete packet_bytes;
+ return;
+ }
+
+ for (int num = 0; num < package_points; num++)
+ {
+ int s = packet_bytes[num * point_len + data_bits_start];
+ int z = packet_bytes[num * point_len + data_bits_start + 1];
+ int y = 0;
+ if (lidar_name == "N10" || lidar_name == "L10")
+ y = packet_bytes[num * point_len + data_bits_start + 2];
+ int dist_temp = s & 0x7F;
+ int inten_temp = s & 0x80;
+
+ if ((s * 256 + z) != 0xFFFF)
+ {
+ if (lidar_name == "N10" || lidar_name == "L10")
+ {
+ scan_points_[idx].range = double(s * 256 + (z)) / 1000.f;
+ scan_points_[idx].intensity = int(y);
+ }
+ else if ((lidar_name == "M10_P" || lidar_name == "M10_PLUS") && !high_reflection)
+ {
+ scan_points_[idx].range = double(s * 256 + (z)) / 1000.f;
+ scan_points_[idx].intensity = 0;
+ }
+ else
+ {
+ scan_points_[idx].range = double(dist_temp * 256 + (z)) / 1000.f;
+ if (inten_temp)
+ scan_points_[idx].intensity = 255;
+ else
+ scan_points_[idx].intensity = 0;
+ }
+ if ((degree + (degree_interval / invalidValue * num)) > 360)
+ scan_points_[idx].degree = degree + (degree_interval / invalidValue * num) - 360;
+ else
+ scan_points_[idx].degree = degree + (degree_interval / invalidValue * num);
+ }
+ else
+ continue;
+
+ if ((scan_points_[idx].degree < last_degree && scan_points_[idx].degree < 5 && last_degree > 355) || idx >= points_size_)
+ {
+ last_degree = scan_points_[idx].degree;
+ count_num = idx;
+ idx = 0;
+ for (long unsigned int k = 0; k < scan_points_.size(); k++)
+ {
+ if (scan_points_[k].range < min_range || scan_points_[k].range > max_range)
+ scan_points_[k].range = 0;
+ }
+ boost::unique_lock lock(mutex_);
+ scan_points_bak_.resize(scan_points_.size());
+ scan_points_bak_.assign(scan_points_.begin(), scan_points_.end());
+ for (long unsigned int k = 0; k < scan_points_.size(); k++)
+ {
+ scan_points_[k].range = 0;
+ scan_points_[k].degree = 0;
+ scan_points_[k].intensity = 0;
+ }
+ pre_time_ = time_;
+ lock.unlock();
+ pubscan_cond_.notify_one();
+ time_ = get_clock()->now();
+ }
+ else
+ {
+ last_degree = scan_points_[idx].degree;
+ idx++;
+ }
+ }
+ packet_bytes = {0x00};
+ if (packet_bytes)
+ {
+ packet_bytes = NULL;
+ delete packet_bytes;
+ }
+ }
+
+ void LslidarDriver::data_processing_2(unsigned char *packet_bytes, int len) // 处理每一包的数据
+ {
+ double degree;
+ double end_degree;
+ double degree_interval = 15.0;
+ boost::posix_time::ptime t1, t2;
+ t1 = boost::posix_time::microsec_clock::universal_time();
+
+ int s = packet_bytes[degree_bits_start];
+ int z = packet_bytes[degree_bits_start + 1];
+
+ degree = (s * 256 + z) / 100.f + degree_compensation;
+ degree = (degree < 0) ? degree + 360 : degree;
+ degree = (degree > 360) ? degree - 360 : degree;
+ if (lidar_name == "N10_P")
+ {
+ int s_e = packet_bytes[end_degree_bits_start];
+ int z_e = packet_bytes[end_degree_bits_start + 1];
+
+ end_degree = (s_e * 256 + z_e) / 100.f;
+ end_degree = (end_degree > 360) ? end_degree - 360 : end_degree;
+
+ if (degree > end_degree)
+ degree_interval = end_degree + 360 - degree;
+ else
+ degree_interval = end_degree - degree;
+ }
+
+ // boost::unique_lock lock(mutex_);
+ if (lidar_name == "M10_DOUBLE")
+ {
+ PACKET_SIZE = len;
+ package_points = (PACKET_SIZE - 20) / 4;
+ }
+ int invalidValue = 0;
+ int point_len = 4;
+ if (lidar_name == "N10_P")
+ point_len = 6;
+
+ for (int num = 0; num < point_len * package_points; num += point_len)
+ {
+ int s = packet_bytes[num + data_bits_start];
+ int z = packet_bytes[num + data_bits_start + 1];
+ if ((s * 256 + z) == 0xFFFF)
+ invalidValue++;
+ }
+
+ if (use_gps_ts)
+ {
+ pTime.tm_year = packet_bytes[PACKET_SIZE - 12] + 2000 - 1900; // x+2000
+ pTime.tm_mon = packet_bytes[PACKET_SIZE - 11] - 1; // 1-12
+ pTime.tm_mday = packet_bytes[PACKET_SIZE - 10]; // 1-31
+ pTime.tm_hour = packet_bytes[PACKET_SIZE - 9]; // 0-23
+ pTime.tm_min = packet_bytes[PACKET_SIZE - 8]; // 0-59
+ pTime.tm_sec = packet_bytes[PACKET_SIZE - 7]; // 0-59
+ sub_second = (packet_bytes[PACKET_SIZE - 6] * 256 + packet_bytes[PACKET_SIZE - 5]) * 1000000 + (packet_bytes[PACKET_SIZE - 4] * 256 + packet_bytes[PACKET_SIZE - 3]) * 1000;
+ sweep_end_time_gps = get_gps_stamp(pTime);
+ sweep_end_time_hardware = sub_second % 1000000000;
+ }
+ invalidValue = package_points - invalidValue;
+ if (lidar_name == "N10_P")
+ invalidValue--;
+ if (invalidValue <= 1)
+ {
+ delete packet_bytes;
+ return;
+ }
+
+ for (int num = 0; num < package_points; num++)
+ {
+ int s = packet_bytes[num * point_len + data_bits_start];
+ int z = packet_bytes[num * point_len + data_bits_start + 1];
+ int y = 0;
+ if (lidar_name == "N10_P")
+ y = packet_bytes[num * point_len + data_bits_start + 2];
+
+ if ((s * 256 + z) != 0xFFFF)
+ {
+ scan_points_[idx].range = double(s * 256 + (z)) / 1000.f;
+ if (lidar_name == "N10_P")
+ scan_points_[idx].intensity = int(y);
+ else
+ scan_points_[idx].intensity = 0;
+ s = packet_bytes[num * point_len + data_bits_start + point_len / 2];
+ z = packet_bytes[num * point_len + data_bits_start + point_len / 2 + 1];
+ if (lidar_name == "N10_P")
+ y = packet_bytes[num * point_len + data_bits_start + point_len / 2 + 2];
+
+ scan_points_[idx + 3000].range = double(s * 256 + (z)) / 1000.f;
+ if (lidar_name == "N10_P")
+ scan_points_[idx + 3000].intensity = int(y);
+ else
+ scan_points_[idx + 3000].intensity = 0;
+
+ if ((degree + (degree_interval / invalidValue * num)) > 360)
+ scan_points_[idx].degree = degree + (degree_interval / invalidValue * num) - 360;
+ else
+ scan_points_[idx].degree = degree + (degree_interval / invalidValue * num);
+ }
+ else
+ continue;
+ if (((scan_points_[idx].degree < last_degree && scan_points_[idx].degree < 5 && last_degree > 355) || idx >= points_size_) && idx > 10)
+ {
+ last_degree = scan_points_[idx].degree;
+ count_num = idx;
+ idx = 0;
+ for (int k = 0; k < count_num; k++)
+ {
+ if (angle_able_max > 360)
+ {
+ if ((360 - scan_points_[k].degree) > (angle_able_max - 360) && (360 - scan_points_[k].degree) < angle_able_min)
+ {
+ scan_points_[k].range = 0;
+ scan_points_[k + 3000].range = 0;
+ }
+ }
+ else
+ {
+ if ((360 - scan_points_[k].degree) > angle_able_max || (360 - scan_points_[k].degree) < angle_able_min)
+ {
+ scan_points_[k].range = 0;
+ scan_points_[k + 3000].range = 0;
+ }
+ }
+ if (scan_points_[k].range < min_range || scan_points_[k].range > max_range)
+ scan_points_[k].range = 0;
+ if (scan_points_[k + 3000].range < min_range || scan_points_[k + 3000].range > max_range)
+ scan_points_[k + 3000].range = 0;
+ }
+ boost::unique_lock lock(mutex_);
+ scan_points_bak_.resize(scan_points_.size());
+ scan_points_bak_.assign(scan_points_.begin(), scan_points_.end());
+ for (long unsigned int k = 0; k < scan_points_.size(); k++)
+ {
+ scan_points_[k].range = 0;
+ scan_points_[k].degree = 0;
+ scan_points_[k].intensity = 0;
+ }
+ pre_time_ = time_;
+ lock.unlock();
+ pubscan_cond_.notify_one();
+ time_ = get_clock()->now();
+ }
+ else
+ {
+ last_degree = scan_points_[idx].degree;
+ idx++;
+ }
+ }
+ packet_bytes = {0x00};
+ if (packet_bytes)
+ {
+ packet_bytes = NULL;
+ delete packet_bytes;
+ }
+ }
+
+ void LslidarDriver::pubScanThread()
+ {
+ bool wait_for_wake = true;
+ boost::unique_lock lock(pubscan_mutex_);
+
+ while (rclcpp::ok())
+ {
+
+ while (wait_for_wake)
+ {
+ pubscan_cond_.wait(lock);
+ wait_for_wake = false;
+ }
+ if (lidar_name == "N10_P" || lidar_name == "M10_DOUBLE")
+ {
+ if (pubScan)
+ {
+ auto scan = sensor_msgs::msg::LaserScan::UniquePtr(new sensor_msgs::msg::LaserScan());
+ ////int scan_num = count_num * 2;
+ int scan_num = count_num ;
+
+ std::vector points;
+ rclcpp::Time start_time;
+ float scan_time;
+ this->getScan(points, start_time, scan_time);
+ scan->header.frame_id = frame_id;
+ if (use_gps_ts)
+ {
+ scan->header.stamp = rclcpp::Time(sweep_end_time_gps, sweep_end_time_hardware);
+ }
+ else
+ {
+ scan->header.stamp = this->now(); // timestamp will obtained from sweep data stamp
+ }
+
+ scan->angle_min = 0;
+ scan->angle_max = 2 * M_PI;
+ scan->angle_increment = 2 * M_PI / (double)(count_num);
+ scan->range_min = min_range;
+ scan->range_max = max_range;
+ scan->ranges.reserve(scan_num);
+ scan->ranges.assign(scan_num, std::numeric_limits::infinity());
+ scan->intensities.reserve(scan_num);
+ scan->intensities.assign(scan_num, std::numeric_limits::infinity());
+ // scan->scan_time = scan_time;
+ // scan->time_increment = scan_time / (double)(count_num);
+
+ for (int k = 0; k < scan_num; k++)
+ {
+ scan->ranges[k] = std::numeric_limits::infinity();
+ scan->intensities[k] = 0;
+ }
+
+ for (int i = 0; i < count_num; i++)
+ {
+ int point_idx = round((360 - points[i].degree) * count_num / 360);
+ if (points[i].range == 0.0)
+ {
+ scan->ranges[point_idx] = std::numeric_limits::infinity();
+ scan->intensities[point_idx] = 0;
+ }
+ else
+ {
+ double dist = points[i].range;
+ scan->ranges[point_idx] = (float)dist;
+ scan->intensities[point_idx] = points[i].intensity;
+ }
+
+ if(truncated_mode_){
+ int len=sizeof(scan_crop_max) / sizeof(scan_crop_max[0]) ;
+ for(int j=0;j=(scan_crop_min[j]*count_num / 360)) && (point_idx<=(scan_crop_max[j]*count_num / 360))){
+ scan->ranges[point_idx] = std::numeric_limits::infinity();
+ scan->intensities[point_idx] = 0;
+ }
+ }
+ }
+ /*
+ if (points[i + 3000].range == 0.0)
+ {
+ scan->ranges[point_idx + count_num] = std::numeric_limits::infinity();
+ scan->intensities[point_idx + count_num] = 0;
+ }
+ else
+ {
+ double dist = points[i+3000].range;
+ scan->ranges[point_idx + count_num] = (float)dist;
+ scan->intensities[point_idx + count_num] = points[i + 3000].intensity;
+ }*/
+ }
+ scan_pub->publish(std::move(scan));
+ }
+ if (pubPointCloud2)
+ {
+ std::vector points;
+ rclcpp::Time start_time;
+ float scan_time;
+ this->getScan(points, start_time, scan_time);
+ VPointCloud::Ptr point_cloud(new VPointCloud());
+ if (use_gps_ts)
+ {
+ start_time = rclcpp::Time(sweep_end_time_gps, sweep_end_time_hardware);
+ }
+ double timestamp = start_time.seconds();
+ point_cloud->header.stamp = static_cast(timestamp * 1e6);
+ point_cloud->header.frame_id = frame_id;
+ point_cloud->height = 1;
+ // printf("now = %f\n",timestamp);
+ for (uint16_t i = 0; i < count_num; i++)
+ {
+ // printf("degree = %f\n",points[i].degree);
+ double degree = 360.0 - points[i].degree;
+ bool pass_point = false;
+ if (angle_able_max < 360)
+ {
+ if (degree < angle_able_min || degree > angle_able_max)
+ pass_point = true;
+ }
+ else
+ {
+ if (degree < angle_able_min && degree > (angle_able_max - 360))
+ pass_point = true;
+ }
+ if (points[i].range < 0.001)
+ pass_point = true;
+ if (!pass_point)
+ {
+ // printf("degree = %f\n",degree);
+ // printf("angle_able_min = %f\nangle_able_max=%f\n",angle_able_min,angle_able_max);
+ VPoint point;
+ int point_idx = round(degree * count_num / 360);
+ point.timestamp = timestamp - point_idx * (scan_time / count_num);
+ // printf("timestamp = %f\n",point.timestamp);
+ point.x = points[i].range * cos(M_PI / 180 * points[i].degree);
+ point.y = -points[i].range * sin(M_PI / 180 * points[i].degree);
+ point.z = 0;
+ point.intensity = points[i].intensity;
+ point_cloud->points.push_back(point);
+ ++point_cloud->width;
+ }
+ if (points[i + 3000].range < 0.001)
+ pass_point = true;
+ if (!pass_point)
+ {
+ // printf("degree = %f\n",degree);
+ // printf("angle_able_min = %f\nangle_able_max=%f\n",angle_able_min,angle_able_max);
+ VPoint point;
+ int point_idx = round(degree * count_num / 360);
+ point.timestamp = timestamp - point_idx * (scan_time / count_num);
+ // printf("timestamp = %f\n",point.timestamp);
+ point.x = points[i + 3000].range * cos(M_PI / 180 * points[i].degree);
+ point.y = -points[i + 3000].range * sin(M_PI / 180 * points[i].degree);
+ point.z = 0;
+ point.intensity = points[i + 3000].intensity;
+ point_cloud->points.push_back(point);
+ ++point_cloud->width;
+ }
+ }
+ sensor_msgs::msg::PointCloud2 pc_msg;
+ pcl::toROSMsg(*point_cloud, pc_msg);
+ point_cloud_pub->publish(pc_msg);
+ }
+ }
+ else
+ {
+ if (pubScan)
+ {
+ auto scan = sensor_msgs::msg::LaserScan::UniquePtr(new sensor_msgs::msg::LaserScan());
+ //int scan_num = ceil((angle_able_max - angle_able_min) / 360 * count_num) + 1;
+ int scan_num = ceil((angle_able_max - angle_able_min) / 360 * count_num) + 1;
+
+ std::vector points;
+ rclcpp::Time start_time;
+ float scan_time;
+ this->getScan(points, start_time, scan_time);
+ scan->header.frame_id = frame_id;
+ if (use_gps_ts)
+ {
+ scan->header.stamp = rclcpp::Time(sweep_end_time_gps, sweep_end_time_hardware);
+ }
+ else
+ {
+ scan->header.stamp = this->now(); // timestamp will obtained from sweep data stamp
+ }
+
+ if (angle_able_max > 360)
+ {
+ scan->angle_min = 2 * M_PI * (angle_able_min - 360) / 360;
+ scan->angle_max = 2 * M_PI * (angle_able_max - 360) / 360;
+ }
+ else
+ {
+ scan->angle_min = 2 * M_PI * angle_able_min / 360;
+ scan->angle_max = 2 * M_PI * angle_able_max / 360;
+ }
+ scan->angle_increment = 2 * M_PI / (double)(scan_num - 1);
+
+ scan->range_min = min_range;
+ scan->range_max = max_range;
+ scan->ranges.reserve(scan_num);
+ scan->ranges.assign(scan_num, std::numeric_limits::infinity());
+ scan->intensities.reserve(scan_num);
+ scan->intensities.assign(scan_num, std::numeric_limits::infinity());
+ scan->scan_time = 0.1;
+ scan->time_increment = 0.1 / (double)(scan_num - 1);
+
+ int start_num = floor(angle_able_min * count_num / 360);
+ int end_num = floor(angle_able_max * count_num / 360);
+
+ for (int i = 0; i < count_num; i++)
+ {
+ int point_idx = round((360 - points[i].degree) * count_num / 360);
+ if (point_idx < (end_num - count_num))
+ point_idx += count_num;
+ point_idx = point_idx - start_num;
+ if (point_idx < 0 || point_idx >= scan_num)
+ continue;
+ if (points[i].range == 0.0)
+ {
+ scan->ranges[point_idx] = std::numeric_limits::infinity();
+ }
+ else
+ {
+ double dist = points[i].range;
+ scan->ranges[point_idx] = (float)dist;
+ }
+ scan->intensities[point_idx] = points[i].intensity;
+
+ if(truncated_mode_){
+ int len=sizeof(scan_crop_max) / sizeof(scan_crop_max[0]) ;
+ for(int j=0;j=(scan_crop_min[j]*count_num / 360)) && (point_idx<=(scan_crop_max[j]*count_num / 360))){
+ scan->ranges[point_idx] = std::numeric_limits::infinity();
+ scan->intensities[point_idx] = 0;
+ }
+ }
+ }
+ }
+
+
+ scan_pub->publish(std::move(scan));
+ }
+ if (pubPointCloud2)
+ {
+ std::vector points;
+ rclcpp::Time start_time;
+ float scan_time;
+ this->getScan(points, start_time, scan_time);
+ VPointCloud::Ptr point_cloud(new VPointCloud());
+ if (use_gps_ts)
+ {
+ start_time = rclcpp::Time(sweep_end_time_gps, sweep_end_time_hardware);
+ }
+ double timestamp = start_time.seconds();
+ point_cloud->header.stamp = static_cast(timestamp * 1e6);
+ point_cloud->header.frame_id = frame_id;
+ point_cloud->height = 1;
+ for (uint16_t i = 0; i < count_num; i++)
+ {
+ double degree = 360.0 - points[i].degree;
+ bool pass_point = false;
+ if (angle_able_max < 360)
+ {
+ if (degree < angle_able_min || degree > angle_able_max)
+ pass_point = true;
+ }
+ else
+ {
+ if (degree < angle_able_min && degree > (angle_able_max - 360))
+ pass_point = true;
+ }
+ if (points[i].range < 0.001)
+ pass_point = true;
+ if (!pass_point)
+ {
+ // printf("degree = %f\n",degree);
+ // printf("angle_able_min = %f\nangle_able_max=%f\n",angle_able_min,angle_able_max);
+ VPoint point;
+ int point_idx = round(degree * count_num / 360);
+ point.timestamp = timestamp - point_idx * (scan_time / count_num);
+ // printf("timestamp = %f\n",point.timestamp);
+ point.x = points[i].range * cos(M_PI / 180 * points[i].degree);
+ point.y = -points[i].range * sin(M_PI / 180 * points[i].degree);
+ point.z = 0;
+ point.intensity = points[i].intensity;
+ point_cloud->points.push_back(point);
+ ++point_cloud->width;
+ }
+ }
+ sensor_msgs::msg::PointCloud2 pc_msg;
+ pcl::toROSMsg(*point_cloud, pc_msg);
+ point_cloud_pub->publish(pc_msg);
+ }
+ }
+ count_num = 0;
+ wait_for_wake = true;
+ if (first_compensation && compensation)
+ {
+ lidar_difop();
+ }
+ }
+ }
+
+ bool LslidarDriver::polling()
+ {
+ if (!is_start)
+ return true;
+ // Allocate a new shared pointer for zero-copy sharing with other nodelets.
+ unsigned char *packet_bytes = new unsigned char[500];
+ int len = 0;
+ bool difop = false;
+ if (interface_selection == "net")
+ {
+ auto packet = lslidar_msgs::msg::LslidarPacket::UniquePtr(
+ new lslidar_msgs::msg::LslidarPacket());
+
+ std_msgs::msg::Byte msg;
+ while (true)
+ {
+ difop = false;
+ len = 0;
+ // keep reading until full packet received
+ len = msop_input_->getPacket(packet);
+ if (packet->data[0] == 0x5a)
+ {
+ if (lidar_name == "N10" || lidar_name == "L10")
+ len = 58;
+ else if (lidar_name == "M10")
+ len = 92;
+ else if (lidar_name == "N10_P")
+ len = 108;
+ else if (lidar_name == "M10_GPS")
+ len = 102;
+ else
+ {
+ int len_H = packet->data[1];
+ int len_L = packet->data[2];
+ len = len_H * 256 + len_L;
+ }
+ for (int i = len - 1; i > 0; i--)
+ packet->data[i] = packet->data[i - 1];
+ packet->data[0] = 0xa5;
+ }
+
+ if (lidar_name == "N10" || lidar_name == "L10")
+ len = 58;
+ else if (lidar_name == "M10")
+ len = 92;
+ else if (lidar_name == "N10_P")
+ len = 108;
+ else if (lidar_name == "M10_GPS")
+ len = 102;
+ else
+ {
+ int len_H = packet->data[2];
+ int len_L = packet->data[3];
+ len = len_H * 256 + len_L;
+ }
+ if ((lidar_name == "M10" || lidar_name == "M10_DOUBLE" || lidar_name == "M10_GPS" || lidar_name == "M10_P" || lidar_name == "M10_PLUS") && compensation)
+ {
+ if (packet->data[2] == 0x55 && packet->data[3] == 0x00 && packet->data[186] == 0xFA && packet->data[187] == 0xFB)
+ {
+ len = 188;
+ difop = true;
+ }
+ }
+
+ if (len <= 0 || len >= 1000 || packet->data[0] != 0xa5 || packet->data[1] != 0x5a)
+ continue;
+ for (int i = 0; i < len; i++)
+ {
+ packet_bytes[i] = packet->data[i];
+ }
+ if ((lidar_name == "N10" || lidar_name == "L10" || lidar_name == "N10_P") && packet_bytes[len - 1] != N10_CalCRC8(packet_bytes, len - 1))
+ continue;
+ break;
+ }
+ }
+ else
+ {
+ if (in_file_name != "") // 读txt文件功能
+ {
+ int usleep_time = round(1000000 / 10 / 24) - 135;
+ while (true)
+ {
+ std::ifstream file_reader(in_file_name);
+ while (file_reader.peek() != EOF)
+ {
+ std::string line;
+ std::getline(file_reader, line, '\n');
+ for (long unsigned int i = 0; i < line.size() - 1; i++)
+ {
+ line[i] = line[i] - 48;
+ if (line[i] > 9)
+ line[i] = line[i] - 39;
+ }
+
+ for (long unsigned int i = 0; i < (line.size() - 1) / 2; i++)
+ {
+ packet_bytes[i] = line[i * 2] * 16 + line[i * 2 + 1];
+ }
+ if (lidar_name == "N10" || lidar_name == "L10")
+ len = 58;
+ else if (lidar_name == "M10")
+ len = 92;
+ else if (lidar_name == "N10_P")
+ len = 108;
+ else if (lidar_name == "M10_GPS")
+ len = 102;
+ else
+ {
+ int len_H = packet_bytes[2];
+ int len_L = packet_bytes[3];
+ len = len_H * 256 + len_L;
+ }
+ if (lidar_name == "N10_P" || lidar_name == "M10_DOUBLE")
+ LslidarDriver::data_processing_2(packet_bytes, len);
+ else
+ LslidarDriver::data_processing(packet_bytes, len);
+ usleep(usleep_time);
+ }
+ }
+ return false;
+ }
+ else
+ {
+ while (true)
+ {
+ difop = false;
+ len = 0;
+ len = LslidarDriver::receive_data(packet_bytes);
+ if ((lidar_name == "M10" || lidar_name == "M10_DOUBLE" || lidar_name == "M10_GPS" || lidar_name == "M10_P" || lidar_name == "M10_PLUS") && compensation)
+ {
+ if (packet_bytes[2] == 0x55 && packet_bytes[3] == 0x00 && packet_bytes[186] == 0xFA && packet_bytes[187] == 0xFB)
+ difop = true;
+ }
+ if (len == 0)
+ continue;
+ break;
+ }
+ }
+ }
+ if (difop)
+ LslidarDriver::difop_processing(packet_bytes);
+ else
+ {
+ if (lidar_name == "N10_P" || lidar_name == "M10_DOUBLE")
+ LslidarDriver::data_processing_2(packet_bytes, len);
+ else
+ LslidarDriver::data_processing(packet_bytes, len);
+ }
+ delete packet_bytes;
+ return true;
+ }
+
+} // namespace lslidar_driver
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver.cc.bak b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver.cc.bak
new file mode 100644
index 0000000..c22c0f7
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver.cc.bak
@@ -0,0 +1,1423 @@
+/*
+ * This file is part of lslidar driver.
+ *
+ * The driver is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * The driver is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with the driver. If not, see .
+ */
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include "rclcpp/rclcpp.hpp"
+#include "lslidar_driver/lslidar_driver.h"
+#include
+
+int truncated_mode_=0; //多角度屏蔽开关:默认为0,如果需要屏蔽多个角度,则truncated_mode_赋值为1。
+
+int scan_crop_min[]={0,180}; //雷达屏蔽角度,这里屏蔽角度为135°到225°,
+ //如果要多角度屏蔽,如10~30,50~60,改为:
+ //scan_angle_min[]={10,50};scan_angle_max[]={30,60};
+int scan_crop_max[]={90,270}; //修改后编译即可
+
+
+namespace lslidar_driver
+{
+
+ static void my_hander(int sig)
+ {
+ printf("sig: %d", sig);
+ abort();
+ }
+ LslidarDriver::LslidarDriver() : LslidarDriver(rclcpp::NodeOptions()) {}
+ LslidarDriver::LslidarDriver(const rclcpp::NodeOptions &options) : Node("lslidar_driver_node", options), diagnostics(this)
+ {
+ signal(SIGINT, my_hander);
+
+ if (!this->initialize())
+ RCLCPP_ERROR(this->get_logger(), "Could not initialize the driver...");
+ else
+ RCLCPP_INFO(this->get_logger(), "Successfully initialize driver...");
+ }
+
+ LslidarDriver::~LslidarDriver()
+ {
+ return;
+ }
+
+ bool LslidarDriver::loadParameters()
+ {
+ pubscan_thread_ = new boost::thread(boost::bind(&LslidarDriver::pubScanThread, this));
+ interface_selection = std::string("net");
+ frame_id = std::string("laser_link");
+ scan_topic = std::string("/scan");
+ lidar_name = std::string("M10");
+ pointcloud_topic = std::string("/lslidar_point_cloud");
+ is_start = true;
+ min_range = 0.3;
+ max_range = 100.0;
+ use_gps_ts = true;
+ compensation = true;
+ pubScan = true;
+ pubPointCloud2 = true;
+ angle_disable_min = 0.0;
+ angle_disable_max = 0.0;
+ // 添加恒定输出长度参数
+ fixed_array_length = 450; //cyy_addcyy_add
+ this->declare_parameter("fixed_array_length", 450);//cyy_addcyy_add
+
+
+ this->declare_parameter("lidar_name", "M10");
+ this->declare_parameter("frame_id", "laser_link");
+ this->declare_parameter("scan_topic", "/scan");
+ this->declare_parameter("pointcloud_topic", "/lslidar_point_cloud");
+ this->declare_parameter("min_range", 0.3);
+ this->declare_parameter("max_range", 100.0);
+ this->declare_parameter("use_gps_ts", false);
+ this->declare_parameter("high_reflection", false);
+ this->declare_parameter("compensation", false);
+ this->declare_parameter("pubScan", true);
+ this->declare_parameter("pubPointCloud2", false);
+ this->declare_parameter("angle_disable_min", 0.0);
+ this->declare_parameter("angle_disable_max", 0.0);
+ this->declare_parameter("interface_selection", "net");
+
+ this->get_parameter("fixed_array_length", fixed_array_length);//cyy_addcyy_add
+ this->get_parameter("lidar_name", lidar_name);
+ this->get_parameter("frame_id", frame_id);
+ this->get_parameter("high_reflection", high_reflection);
+ this->get_parameter("scan_topic", scan_topic);
+ this->get_parameter("min_range", min_range);
+ this->get_parameter("max_range", max_range);
+ this->get_parameter("use_gps_ts", use_gps_ts);
+ this->get_parameter("compensation", compensation);
+ this->get_parameter("pointcloud_topic", pointcloud_topic);
+ this->get_parameter("pubScan", pubScan);
+ this->get_parameter("pubPointCloud2", pubPointCloud2);
+ this->get_parameter("angle_disable_min", angle_disable_min);
+ this->get_parameter("angle_disable_max", angle_disable_max);
+ this->get_parameter("interface_selection", interface_selection);
+ while (angle_disable_min < 0)
+ angle_disable_min += 360;
+ while (angle_disable_max < 0)
+ angle_disable_max += 360;
+ while (angle_disable_min > 360)
+ angle_disable_min -= 360;
+ while (angle_disable_max > 360)
+ angle_disable_max -= 360;
+ if (angle_disable_max == angle_disable_min)
+ {
+ angle_able_min = 0;
+ angle_able_max = 360;
+ }
+ else
+ {
+ if (angle_disable_min < angle_disable_max && angle_disable_min != 0.0)
+ {
+ angle_able_min = angle_disable_max;
+ angle_able_max = angle_disable_min + 360;
+ }
+ if (angle_disable_min < angle_disable_max && angle_disable_min == 0.0)
+ {
+ angle_able_min = angle_disable_max;
+ angle_able_max = 360;
+ }
+ if (angle_disable_min > angle_disable_max)
+ {
+ angle_able_min = angle_disable_max;
+ angle_able_max = angle_disable_min;
+ }
+ }
+ count_num = 0;
+
+ scan_points_.resize(6000);
+
+ if (lidar_name == "M10")
+ {
+ use_gps_ts = false;
+ PACKET_SIZE = 92;
+ package_points = 42;
+ data_bits_start = 6;
+ degree_bits_start = 2;
+ rpm_bits_start = 4;
+ baud_rate_ = 460800;
+ points_size_ = 1008;
+ }
+ else if (lidar_name == "M10_P")
+ {
+ PACKET_SIZE = 160;
+ package_points = 70;
+ data_bits_start = 8;
+ degree_bits_start = 4;
+ rpm_bits_start = 6;
+ baud_rate_ = 500000;
+ points_size_ = 2000;
+ }
+ else if (lidar_name == "M10_PLUS")
+ {
+ PACKET_SIZE = 104;
+ package_points = 41;
+ data_bits_start = 8;
+ degree_bits_start = 4;
+ rpm_bits_start = 6;
+ points_size_ = 5000;
+ baud_rate_ = 921600;
+ }
+ else if (lidar_name == "M10_GPS")
+ {
+ PACKET_SIZE = 102;
+ package_points = 42;
+ data_bits_start = 6;
+ degree_bits_start = 2;
+ rpm_bits_start = 4;
+ baud_rate_ = 460800;
+ points_size_ = 1008;
+ }
+ else if (lidar_name == "N10")
+ {
+ PACKET_SIZE = 58;
+ package_points = 16;
+ data_bits_start = 7;
+ degree_bits_start = 5;
+ end_degree_bits_start = 55;
+ baud_rate_ = 230400;
+ points_size_ = 2000;
+ use_gps_ts = false;
+ compensation = false;
+ }
+ else if (lidar_name == "M10_DOUBLE")
+ {
+ PACKET_SIZE = 300;
+ package_points = 70;
+ data_bits_start = 8;
+ degree_bits_start = 4;
+ rpm_bits_start = 6;
+ points_size_ = 3000;
+ baud_rate_ = 921600;
+ }
+ else if (lidar_name == "N10_P")
+ {
+ PACKET_SIZE = 108;
+ package_points = 16;
+ data_bits_start = 7;
+ degree_bits_start = 5;
+ end_degree_bits_start = 105;
+ baud_rate_ = 460800;
+ points_size_ = 2000;
+ use_gps_ts = false;
+ compensation = false;
+ }
+ else if (lidar_name == "L10")
+ {
+ PACKET_SIZE = 58;
+ package_points = 16;
+ data_bits_start = 7;
+ degree_bits_start = 5;
+ end_degree_bits_start = 55;
+ baud_rate_ = 230400;
+ points_size_ = 2000;
+ use_gps_ts = false;
+ compensation = false;
+ }
+ RCLCPP_INFO_STREAM(this->get_logger(), "Lidar is " << lidar_name.c_str());
+
+ if (pubScan)
+ scan_pub = this->create_publisher(scan_topic, 10);
+ if (pubPointCloud2)
+ point_cloud_pub = this->create_publisher(pointcloud_topic, 10);
+ difop_switch = this->create_subscription("lslidar_order", 1, std::bind(&LslidarDriver::lidar_order, this, std::placeholders::_1)); // 转速输入
+ return true;
+ }
+
+ void LslidarDriver::lidar_difop()
+ {
+ if (lidar_name == "L10" || lidar_name == "N10" || lidar_name == "N10_P")
+ return;
+ if (interface_selection == "net")
+ msop_input_->UDP_difop();
+ else
+ {
+ for (int k = 0; k < 10; k++)
+ {
+ unsigned char data[188] = {0x00};
+ data[0] = 0xA5;
+ data[1] = 0x5A;
+ data[2] = 0x55;
+ data[184] = 0x08;
+ data[185] = 0x01;
+ data[186] = 0xFA;
+ data[187] = 0xFB;
+ int rtn = serial_->send((const char *)data, 188);
+ if (rtn < 0)
+ printf("start scan error !\n");
+ else
+ return;
+ }
+ }
+ return;
+ }
+
+ void LslidarDriver::lidar_order(const std_msgs::msg::Int8::SharedPtr msg)
+ {
+ if (lidar_name == "L10")
+ return;
+ int i = msg->data;
+ if (i == 0)
+ is_start = false;
+ else
+ is_start = true;
+ if (interface_selection == "net")
+ msop_input_->UDP_order(*msg);
+ else
+ {
+ int i = msg->data;
+ for (int k = 0; k < 10; k++)
+ {
+ int rtn;
+ unsigned char data[188] = {0x00};
+ data[0] = 0xA5;
+ data[1] = 0x5A;
+ data[2] = 0x55;
+ data[186] = 0xFA;
+ data[187] = 0xFB;
+
+ if (lidar_name == "M10" || lidar_name == "M10_GPS" || lidar_name == "M10_P" || lidar_name == "M10_DOUBLE")
+ {
+ if (i <= 1)
+ { // 雷达启停
+ data[184] = 0x01;
+ data[185] = char(i);
+ }
+ else if (i == 2)
+ { // 雷达点云不滤波
+ data[181] = 0x0A;
+ data[184] = 0x06;
+ if (is_start)
+ data[185] = 0x01;
+ }
+ else if (i == 3)
+ { // 雷达点云正常滤波
+ data[181] = 0x0B;
+ data[184] = 0x06;
+ if (is_start)
+ data[185] = 0x01;
+ }
+ else if (i == 4)
+ { // 雷达近距离滤波
+ data[181] = 0x0C;
+ data[184] = 0x06;
+ if (is_start)
+ data[185] = 0x01;
+ }
+ else if (i == 100)
+ { // 接收设备包
+ data[184] = 0x08;
+ data[185] = 0x01;
+ }
+ else
+ return;
+ }
+ else if (lidar_name == "M10_PLUS")
+ {
+ data[184] = 0x0A;
+ data[185] = 0x01;
+ if (i == 5)
+ {
+ data[141] = 0x01;
+ data[142] = 0x2c;
+ }
+ else if (i == 6)
+ {
+ data[141] = 0x01;
+ data[142] = 0x68;
+ }
+ else if (i == 8)
+ {
+ data[141] = 0x01;
+ data[142] = 0xe0;
+ }
+ else if (i == 10)
+ {
+ data[141] = 0x02;
+ data[142] = 0x58;
+ }
+ else if (i == 12)
+ {
+ data[141] = 0x02;
+ data[142] = 0xd0;
+ }
+ else if (i == 15)
+ {
+ data[141] = 0x03;
+ data[142] = 0x84;
+ }
+ else if (i == 20)
+ {
+ data[141] = 0x04;
+ data[142] = 0xb0;
+ }
+ else if (i <= 1)
+ {
+ data[184] = 0x01;
+ data[185] = char(i);
+ }
+ else if (i == 100) // 接收设备包
+ {
+ data[184] = 0x08;
+ data[185] = 0x01;
+ }
+ else
+ return;
+ }
+ else if (lidar_name == "N10" || lidar_name == "N10_P")
+ {
+ if (i <= 1)
+ {
+ data[185] = char(i);
+ data[184] = 0x01;
+ }
+ else if (i >= 6 && i <= 12)
+ {
+ data[172] = char(i);
+ data[184] = 0x0a;
+ data[185] = 0X01;
+ }
+ else
+ return;
+ }
+ rtn = serial_->send((const char *)data, 188);
+ if (rtn < 0)
+ printf("start scan error !\n");
+ else
+ {
+ if (i == 1)
+ usleep(1000000); // 1.0s
+ if (i == 0)
+ is_start = false;
+ if (i == 1)
+ is_start = true;
+ return;
+ }
+ }
+ return;
+ }
+ }
+
+ void LslidarDriver::open_serial()
+ {
+ diagnostics.setHardwareID("Lslidar");
+ int code = 0;
+ serial_port_ = std::string("/dev/ttyUSB0");
+ this->declare_parameter("serial_port_", "/dev/ttyUSB0");
+ this->get_parameter("serial_port_", serial_port_);
+ serial_ = LSIOSR::instance(serial_port_, baud_rate_);
+ code = serial_->init();
+ if (code != 0)
+ {
+ printf("open_port %s ERROR !\n", serial_port_.c_str());
+ rclcpp::shutdown();
+ exit(0);
+ }
+ printf("open_port %s OK !\n", serial_port_.c_str());
+ }
+
+ bool LslidarDriver::createRosIO()
+ {
+ UDP_PORT_NUMBER = 2368;
+ this->declare_parameter("msop_port", 2368);
+ this->get_parameter("msop_port", UDP_PORT_NUMBER);
+ RCLCPP_INFO_STREAM(this->get_logger(), "Opening UDP socket: port " << UDP_PORT_NUMBER);
+ dump_file = std::string("");
+ this->declare_parameter("pcap", "");
+ this->get_parameter("pcap", dump_file);
+ // ROS diagnostics
+ diagnostics.setHardwareID("Lslidar");
+
+ const double diag_freq = 12 * 24;
+ diag_max_freq = diag_freq;
+ diag_min_freq = diag_freq;
+ RCLCPP_INFO(this->get_logger(), "expected frequency: %.3f (Hz)", diag_freq);
+
+ using namespace diagnostic_updater;
+ diag_topic.reset(new TopicDiagnostic(
+ "lslidar_packets", diagnostics,
+ FrequencyStatusParam(&diag_min_freq, &diag_max_freq, 0.1, 10),
+ TimeStampStatusParam()));
+
+ int hz = 10;
+ if (lidar_name == "M10_P")
+ hz = 12;
+ else if (lidar_name == "M10_PLUS")
+ hz = 20;
+
+ double packet_rate = hz * 24;
+ if (dump_file != "")
+ {
+ msop_input_.reset(new lslidar_driver::InputPCAP(this, UDP_PORT_NUMBER, packet_rate, dump_file));
+ }
+ else
+ {
+ msop_input_.reset(new lslidar_driver::InputSocket(this, UDP_PORT_NUMBER));
+ }
+
+ // Output
+ return true;
+ }
+
+ int LslidarDriver::getScan(std::vector &points, rclcpp::Time &scan_time, float &scan_duration)
+ {
+ boost::unique_lock lock(mutex_);
+ points.assign(scan_points_bak_.begin(), scan_points_bak_.end());
+ scan_time = pre_time_;
+ scan_duration = time_.seconds() - pre_time_.seconds();
+ return 1;
+ }
+
+ uint64_t LslidarDriver::get_gps_stamp(struct tm t)
+ {
+
+ uint64_t ptime = static_cast(timegm(&t));
+ return ptime;
+ }
+
+ bool LslidarDriver::initialize()
+ {
+ if (!loadParameters())
+ {
+ RCLCPP_ERROR(this->get_logger(), "Cannot load all required ROS parameters...");
+ return false;
+ }
+ if (interface_selection == "net")
+ {
+ if (!createRosIO())
+ {
+ RCLCPP_ERROR(this->get_logger(), "Cannot create all ROS IO...");
+ return false;
+ }
+ }
+ else
+ {
+ in_file_name = std::string("");
+ this->declare_parameter("in_file_name", "");
+ this->get_parameter("in_file_name", in_file_name);
+ if (in_file_name == "")
+ open_serial();
+ else
+ {
+ RCLCPP_INFO_STREAM(this->get_logger(), "Opening txt file " << in_file_name.c_str());
+ std::ifstream file_reader(in_file_name);
+ if (!file_reader.is_open())
+ {
+ RCLCPP_ERROR(this->get_logger(), "Cannot open the file");
+ return false;
+ }
+ }
+ }
+ RCLCPP_INFO(this->get_logger(), "Initialised lslidar without error");
+ return true;
+ }
+
+ void LslidarDriver::recvThread_crc(int &count, int &link_time)
+ {
+ if (count <= 0)
+ link_time++;
+ else
+ link_time = 0;
+
+ if (link_time > 150)
+ {
+ serial_->close();
+ int ret = serial_->init();
+ if (ret < 0)
+ {
+ RCLCPP_ERROR(this->get_logger(), "serial open fail");
+ usleep(200000);
+ }
+ link_time = 0;
+ }
+ }
+
+ int LslidarDriver::receive_data(unsigned char *packet_bytes)
+ {
+ int link_time = 0;
+ int len_H = 0;
+ int len_L = 0;
+ int len = 0;
+ int count_2 = 0;
+ int count = 0;
+ while (count <= 0)
+ {
+ count = serial_->read(packet_bytes, 1);
+ LslidarDriver::recvThread_crc(count, link_time);
+ }
+ if (packet_bytes[0] != 0xA5)
+ return 0;
+
+ while (count_2 <= 0)
+ {
+ count_2 = serial_->read(packet_bytes + count, 1);
+ if (count_2 >= 0)
+ count += count_2;
+ LslidarDriver::recvThread_crc(count_2, link_time);
+ }
+
+ count_2 = 0;
+ if (packet_bytes[1] != 0x5A)
+ return 0;
+ while (count_2 <= 0)
+ {
+ count_2 = serial_->read(packet_bytes + count, 2);
+ if (count_2 >= 0)
+ count += count_2;
+ LslidarDriver::recvThread_crc(count_2, link_time);
+ }
+
+ count_2 = 0;
+
+ if (lidar_name == "M10")
+ len = 92;
+ else if (lidar_name == "M10_GPS")
+ len = 102;
+ else if (lidar_name == "N10_P")
+ len = 108;
+ else if (lidar_name == "N10" || lidar_name == "L10")
+ len = packet_bytes[2];
+ else
+ {
+ len_H = packet_bytes[2];
+ len_L = packet_bytes[3];
+ len = len_H * 256 + len_L;
+ }
+ if (lidar_name == "M10" || lidar_name == "M10_DOUBLE" || lidar_name == "M10_GPS" || lidar_name == "M10_P" || lidar_name == "M10_PLUS")
+ {
+ if (packet_bytes[2] == 0x55 && packet_bytes[3] == 0x00)
+ len = 188;
+ }
+ while (count < len)
+ {
+ count_2 = serial_->read(packet_bytes + count, len - count);
+ if (count_2 >= 0)
+ count += count_2;
+ LslidarDriver::recvThread_crc(count_2, link_time);
+ }
+ if (lidar_name == "N10" || lidar_name == "L10" || lidar_name == "N10_P")
+ {
+ if (packet_bytes[PACKET_SIZE - 1] != N10_CalCRC8(packet_bytes, PACKET_SIZE - 1))
+ return 0;
+ }
+ return len;
+ }
+
+ uint8_t LslidarDriver::N10_CalCRC8(unsigned char *p, int len)
+ {
+ uint8_t crc = 0;
+ int sum = 0;
+
+ for (int i = 0; i < len; i++)
+ {
+ sum += uint8_t(p[i]);
+ }
+ crc = sum & 0xff;
+ return crc;
+ }
+
+ void LslidarDriver::difop_processing(unsigned char *packet_bytes) // 处理设备包的数据
+ {
+ int s = packet_bytes[173];
+ int z = packet_bytes[174];
+ int degree_temp = s & 0x7F;
+ int sign_temp = s & 0x80;
+ degree_compensation = double(degree_temp * 256 + z) / 100.f;
+ if (sign_temp)
+ degree_compensation = -degree_compensation;
+ first_compensation = false;
+ printf("degree_compensation = %f\n", degree_compensation);
+ return;
+ }
+
+ void LslidarDriver::data_processing(unsigned char *packet_bytes, int len) // 处理每一包的数据
+ {
+ double degree;
+ double end_degree;
+ double degree_interval = 15.0;
+ boost::posix_time::ptime t1, t2;
+ t1 = boost::posix_time::microsec_clock::universal_time();
+
+ int s = packet_bytes[degree_bits_start];
+ int z = packet_bytes[degree_bits_start + 1];
+
+ degree = (s * 256 + z) / 100.f + degree_compensation;
+ degree = (degree < 0) ? degree + 360 : degree;
+ degree = (degree > 360) ? degree - 360 : degree;
+ if (lidar_name == "N10" || lidar_name == "L10")
+ {
+ int s_e = packet_bytes[end_degree_bits_start];
+ int z_e = packet_bytes[end_degree_bits_start + 1];
+
+ end_degree = (s_e * 256 + z_e) / 100.f;
+ end_degree = (end_degree > 360) ? end_degree - 360 : end_degree;
+
+ if (degree > end_degree)
+ degree_interval = end_degree + 360 - degree;
+ else
+ degree_interval = end_degree - degree;
+ }
+
+ // boost::unique_lock lock(mutex_);
+ if (lidar_name == "M10_PLUS" || lidar_name == "M10_P")
+ {
+ PACKET_SIZE = len;
+ package_points = (PACKET_SIZE - 20) / 2;
+ }
+ int invalidValue = 0;
+ int point_len = 2;
+ if (lidar_name == "N10" || lidar_name == "L10")
+ point_len = 3;
+
+ if (lidar_name == "M10_GPS" || lidar_name == "M10")
+ {
+ int err_data_84 = packet_bytes[84];
+ int err_data_85 = packet_bytes[85];
+ if ((err_data_84 * 256 + err_data_85) == 0xFFFF || packet_bytes[86] >= 0xF5)
+ {
+ packet_bytes[86] = 0xFF;
+ packet_bytes[87] = 0xFF;
+ }
+ }
+
+ for (int num = 0; num < point_len * package_points; num += point_len)
+ {
+ int s = packet_bytes[num + data_bits_start];
+ int z = packet_bytes[num + data_bits_start + 1];
+ if ((s * 256 + z) == 0xFFFF)
+ invalidValue++;
+ }
+
+ if (use_gps_ts && lidar_name != "N10")
+ {
+ pTime.tm_year = packet_bytes[PACKET_SIZE - 12] + 2000 - 1900; // x+2000
+ pTime.tm_mon = packet_bytes[PACKET_SIZE - 11] - 1; // 1-12
+ pTime.tm_mday = packet_bytes[PACKET_SIZE - 10]; // 1-31
+ pTime.tm_hour = packet_bytes[PACKET_SIZE - 9]; // 0-23
+ pTime.tm_min = packet_bytes[PACKET_SIZE - 8]; // 0-59
+ pTime.tm_sec = packet_bytes[PACKET_SIZE - 7]; // 0-59
+ sub_second = (packet_bytes[PACKET_SIZE - 6] * 256 + packet_bytes[PACKET_SIZE - 5]) * 1000000 + (packet_bytes[PACKET_SIZE - 4] * 256 + packet_bytes[PACKET_SIZE - 3]) * 1000;
+ sweep_end_time_gps = get_gps_stamp(pTime);
+ sweep_end_time_hardware = sub_second % 1000000000;
+ }
+ invalidValue = package_points - invalidValue;
+ if (lidar_name == "N10" || lidar_name == "L10")
+ invalidValue--;
+ if (invalidValue <= 1)
+ {
+ delete packet_bytes;
+ return;
+ }
+
+ for (int num = 0; num < package_points; num++)
+ {
+ int s = packet_bytes[num * point_len + data_bits_start];
+ int z = packet_bytes[num * point_len + data_bits_start + 1];
+ int y = 0;
+ if (lidar_name == "N10" || lidar_name == "L10")
+ y = packet_bytes[num * point_len + data_bits_start + 2];
+ int dist_temp = s & 0x7F;
+ int inten_temp = s & 0x80;
+
+ if ((s * 256 + z) != 0xFFFF)
+ {
+ if (lidar_name == "N10" || lidar_name == "L10")
+ {
+ scan_points_[idx].range = double(s * 256 + (z)) / 1000.f;
+ scan_points_[idx].intensity = int(y);
+ }
+ else if ((lidar_name == "M10_P" || lidar_name == "M10_PLUS") && !high_reflection)
+ {
+ scan_points_[idx].range = double(s * 256 + (z)) / 1000.f;
+ scan_points_[idx].intensity = 0;
+ }
+ else
+ {
+ scan_points_[idx].range = double(dist_temp * 256 + (z)) / 1000.f;
+ if (inten_temp)
+ scan_points_[idx].intensity = 255;
+ else
+ scan_points_[idx].intensity = 0;
+ }
+ if ((degree + (degree_interval / invalidValue * num)) > 360)
+ scan_points_[idx].degree = degree + (degree_interval / invalidValue * num) - 360;
+ else
+ scan_points_[idx].degree = degree + (degree_interval / invalidValue * num);
+ }
+ else
+ continue;
+
+ if ((scan_points_[idx].degree < last_degree && scan_points_[idx].degree < 5 && last_degree > 355) || idx >= points_size_)
+ {
+ last_degree = scan_points_[idx].degree;
+ count_num = idx;
+ idx = 0;
+ for (long unsigned int k = 0; k < scan_points_.size(); k++)
+ {
+ if (scan_points_[k].range < min_range || scan_points_[k].range > max_range)
+ scan_points_[k].range = 0;
+ }
+ boost::unique_lock lock(mutex_);
+ scan_points_bak_.resize(scan_points_.size());
+ scan_points_bak_.assign(scan_points_.begin(), scan_points_.end());
+ for (long unsigned int k = 0; k < scan_points_.size(); k++)
+ {
+ scan_points_[k].range = 0;
+ scan_points_[k].degree = 0;
+ scan_points_[k].intensity = 0;
+ }
+ pre_time_ = time_;
+ lock.unlock();
+ pubscan_cond_.notify_one();
+ time_ = get_clock()->now();
+ }
+ else
+ {
+ last_degree = scan_points_[idx].degree;
+ idx++;
+ }
+ }
+ packet_bytes = {0x00};
+ if (packet_bytes)
+ {
+ packet_bytes = NULL;
+ delete packet_bytes;
+ }
+ }
+
+ void LslidarDriver::data_processing_2(unsigned char *packet_bytes, int len) // 处理每一包的数据
+ {
+ double degree;
+ double end_degree;
+ double degree_interval = 15.0;
+ boost::posix_time::ptime t1, t2;
+ t1 = boost::posix_time::microsec_clock::universal_time();
+
+ int s = packet_bytes[degree_bits_start];
+ int z = packet_bytes[degree_bits_start + 1];
+
+ degree = (s * 256 + z) / 100.f + degree_compensation;
+ degree = (degree < 0) ? degree + 360 : degree;
+ degree = (degree > 360) ? degree - 360 : degree;
+ if (lidar_name == "N10_P")
+ {
+ int s_e = packet_bytes[end_degree_bits_start];
+ int z_e = packet_bytes[end_degree_bits_start + 1];
+
+ end_degree = (s_e * 256 + z_e) / 100.f;
+ end_degree = (end_degree > 360) ? end_degree - 360 : end_degree;
+
+ if (degree > end_degree)
+ degree_interval = end_degree + 360 - degree;
+ else
+ degree_interval = end_degree - degree;
+ }
+
+ // boost::unique_lock lock(mutex_);
+ if (lidar_name == "M10_DOUBLE")
+ {
+ PACKET_SIZE = len;
+ package_points = (PACKET_SIZE - 20) / 4;
+ }
+ int invalidValue = 0;
+ int point_len = 4;
+ if (lidar_name == "N10_P")
+ point_len = 6;
+
+ for (int num = 0; num < point_len * package_points; num += point_len)
+ {
+ int s = packet_bytes[num + data_bits_start];
+ int z = packet_bytes[num + data_bits_start + 1];
+ if ((s * 256 + z) == 0xFFFF)
+ invalidValue++;
+ }
+
+ if (use_gps_ts)
+ {
+ pTime.tm_year = packet_bytes[PACKET_SIZE - 12] + 2000 - 1900; // x+2000
+ pTime.tm_mon = packet_bytes[PACKET_SIZE - 11] - 1; // 1-12
+ pTime.tm_mday = packet_bytes[PACKET_SIZE - 10]; // 1-31
+ pTime.tm_hour = packet_bytes[PACKET_SIZE - 9]; // 0-23
+ pTime.tm_min = packet_bytes[PACKET_SIZE - 8]; // 0-59
+ pTime.tm_sec = packet_bytes[PACKET_SIZE - 7]; // 0-59
+ sub_second = (packet_bytes[PACKET_SIZE - 6] * 256 + packet_bytes[PACKET_SIZE - 5]) * 1000000 + (packet_bytes[PACKET_SIZE - 4] * 256 + packet_bytes[PACKET_SIZE - 3]) * 1000;
+ sweep_end_time_gps = get_gps_stamp(pTime);
+ sweep_end_time_hardware = sub_second % 1000000000;
+ }
+ invalidValue = package_points - invalidValue;
+ if (lidar_name == "N10_P")
+ invalidValue--;
+ if (invalidValue <= 1)
+ {
+ delete packet_bytes;
+ return;
+ }
+
+ for (int num = 0; num < package_points; num++)
+ {
+ int s = packet_bytes[num * point_len + data_bits_start];
+ int z = packet_bytes[num * point_len + data_bits_start + 1];
+ int y = 0;
+ if (lidar_name == "N10_P")
+ y = packet_bytes[num * point_len + data_bits_start + 2];
+
+ if ((s * 256 + z) != 0xFFFF)
+ {
+ scan_points_[idx].range = double(s * 256 + (z)) / 1000.f;
+ if (lidar_name == "N10_P")
+ scan_points_[idx].intensity = int(y);
+ else
+ scan_points_[idx].intensity = 0;
+ s = packet_bytes[num * point_len + data_bits_start + point_len / 2];
+ z = packet_bytes[num * point_len + data_bits_start + point_len / 2 + 1];
+ if (lidar_name == "N10_P")
+ y = packet_bytes[num * point_len + data_bits_start + point_len / 2 + 2];
+
+ scan_points_[idx + 3000].range = double(s * 256 + (z)) / 1000.f;
+ if (lidar_name == "N10_P")
+ scan_points_[idx + 3000].intensity = int(y);
+ else
+ scan_points_[idx + 3000].intensity = 0;
+
+ if ((degree + (degree_interval / invalidValue * num)) > 360)
+ scan_points_[idx].degree = degree + (degree_interval / invalidValue * num) - 360;
+ else
+ scan_points_[idx].degree = degree + (degree_interval / invalidValue * num);
+ }
+ else
+ continue;
+ if (((scan_points_[idx].degree < last_degree && scan_points_[idx].degree < 5 && last_degree > 355) || idx >= points_size_) && idx > 10)
+ {
+ last_degree = scan_points_[idx].degree;
+ count_num = idx;
+ idx = 0;
+ for (int k = 0; k < count_num; k++)
+ {
+ if (angle_able_max > 360)
+ {
+ if ((360 - scan_points_[k].degree) > (angle_able_max - 360) && (360 - scan_points_[k].degree) < angle_able_min)
+ {
+ scan_points_[k].range = 0;
+ scan_points_[k + 3000].range = 0;
+ }
+ }
+ else
+ {
+ if ((360 - scan_points_[k].degree) > angle_able_max || (360 - scan_points_[k].degree) < angle_able_min)
+ {
+ scan_points_[k].range = 0;
+ scan_points_[k + 3000].range = 0;
+ }
+ }
+ if (scan_points_[k].range < min_range || scan_points_[k].range > max_range)
+ scan_points_[k].range = 0;
+ if (scan_points_[k + 3000].range < min_range || scan_points_[k + 3000].range > max_range)
+ scan_points_[k + 3000].range = 0;
+ }
+ boost::unique_lock lock(mutex_);
+ scan_points_bak_.resize(scan_points_.size());
+ scan_points_bak_.assign(scan_points_.begin(), scan_points_.end());
+ for (long unsigned int k = 0; k < scan_points_.size(); k++)
+ {
+ scan_points_[k].range = 0;
+ scan_points_[k].degree = 0;
+ scan_points_[k].intensity = 0;
+ }
+ pre_time_ = time_;
+ lock.unlock();
+ pubscan_cond_.notify_one();
+ time_ = get_clock()->now();
+ }
+ else
+ {
+ last_degree = scan_points_[idx].degree;
+ idx++;
+ }
+ }
+ packet_bytes = {0x00};
+ if (packet_bytes)
+ {
+ packet_bytes = NULL;
+ delete packet_bytes;
+ }
+ }
+
+ void LslidarDriver::pubScanThread()
+ {
+ bool wait_for_wake = true;
+ boost::unique_lock lock(pubscan_mutex_);
+
+ while (rclcpp::ok())
+ {
+
+ while (wait_for_wake)
+ {
+ pubscan_cond_.wait(lock);
+ wait_for_wake = false;
+ }
+ if (lidar_name == "N10_P" || lidar_name == "M10_DOUBLE")
+ {
+ if (pubScan)
+ {
+ auto scan = sensor_msgs::msg::LaserScan::UniquePtr(new sensor_msgs::msg::LaserScan());
+ ////int scan_num = count_num * 2;
+ int scan_num = count_num ;
+
+ std::vector points;
+ rclcpp::Time start_time;
+ float scan_time;
+ this->getScan(points, start_time, scan_time);
+ scan->header.frame_id = frame_id;
+ if (use_gps_ts)
+ {
+ scan->header.stamp = rclcpp::Time(sweep_end_time_gps, sweep_end_time_hardware);
+ }
+ else
+ {
+ scan->header.stamp = this->now(); // timestamp will obtained from sweep data stamp
+ }
+
+ scan->angle_min = 0;
+ scan->angle_max = 2 * M_PI;
+ scan->angle_increment = 2 * M_PI / (double)(count_num);
+ scan->range_min = min_range;
+ scan->range_max = max_range;
+ scan->ranges.reserve(scan_num);
+ scan->ranges.assign(scan_num, std::numeric_limits::infinity());
+ scan->intensities.reserve(scan_num);
+ scan->intensities.assign(scan_num, std::numeric_limits::infinity());
+ // scan->scan_time = scan_time;
+ // scan->time_increment = scan_time / (double)(count_num);
+
+ for (int k = 0; k < scan_num; k++)
+ {
+ scan->ranges[k] = std::numeric_limits::infinity();
+ scan->intensities[k] = 0;
+ }
+
+ for (int i = 0; i < count_num; i++)
+ {
+ int point_idx = round((360 - points[i].degree) * count_num / 360);
+ if (points[i].range == 0.0)
+ {
+ scan->ranges[point_idx] = std::numeric_limits::infinity();
+ scan->intensities[point_idx] = 0;
+ }
+ else
+ {
+ double dist = points[i].range;
+ scan->ranges[point_idx] = (float)dist;
+ scan->intensities[point_idx] = points[i].intensity;
+ }
+
+ if(truncated_mode_){
+ int len=sizeof(scan_crop_max) / sizeof(scan_crop_max[0]) ;
+ for(int j=0;j=(scan_crop_min[j]*count_num / 360)) && (point_idx<=(scan_crop_max[j]*count_num / 360))){
+ scan->ranges[point_idx] = std::numeric_limits::infinity();
+ scan->intensities[point_idx] = 0;
+ }
+ }
+ }
+ /*
+ if (points[i + 3000].range == 0.0)
+ {
+ scan->ranges[point_idx + count_num] = std::numeric_limits::infinity();
+ scan->intensities[point_idx + count_num] = 0;
+ }
+ else
+ {
+ double dist = points[i+3000].range;
+ scan->ranges[point_idx + count_num] = (float)dist;
+ scan->intensities[point_idx + count_num] = points[i + 3000].intensity;
+ }*/
+ }
+ scan_pub->publish(std::move(scan));
+ }
+ if (pubPointCloud2)
+ {
+ std::vector points;
+ rclcpp::Time start_time;
+ float scan_time;
+ this->getScan(points, start_time, scan_time);
+ VPointCloud::Ptr point_cloud(new VPointCloud());
+ if (use_gps_ts)
+ {
+ start_time = rclcpp::Time(sweep_end_time_gps, sweep_end_time_hardware);
+ }
+ double timestamp = start_time.seconds();
+ point_cloud->header.stamp = static_cast(timestamp * 1e6);
+ point_cloud->header.frame_id = frame_id;
+ point_cloud->height = 1;
+ // printf("now = %f\n",timestamp);
+ for (uint16_t i = 0; i < count_num; i++)
+ {
+ // printf("degree = %f\n",points[i].degree);
+ double degree = 360.0 - points[i].degree;
+ bool pass_point = false;
+ if (angle_able_max < 360)
+ {
+ if (degree < angle_able_min || degree > angle_able_max)
+ pass_point = true;
+ }
+ else
+ {
+ if (degree < angle_able_min && degree > (angle_able_max - 360))
+ pass_point = true;
+ }
+ if (points[i].range < 0.001)
+ pass_point = true;
+ if (!pass_point)
+ {
+ // printf("degree = %f\n",degree);
+ // printf("angle_able_min = %f\nangle_able_max=%f\n",angle_able_min,angle_able_max);
+ VPoint point;
+ int point_idx = round(degree * count_num / 360);
+ point.timestamp = timestamp - point_idx * (scan_time / count_num);
+ // printf("timestamp = %f\n",point.timestamp);
+ point.x = points[i].range * cos(M_PI / 180 * points[i].degree);
+ point.y = -points[i].range * sin(M_PI / 180 * points[i].degree);
+ point.z = 0;
+ point.intensity = points[i].intensity;
+ point_cloud->points.push_back(point);
+ ++point_cloud->width;
+ }
+ if (points[i + 3000].range < 0.001)
+ pass_point = true;
+ if (!pass_point)
+ {
+ // printf("degree = %f\n",degree);
+ // printf("angle_able_min = %f\nangle_able_max=%f\n",angle_able_min,angle_able_max);
+ VPoint point;
+ int point_idx = round(degree * count_num / 360);
+ point.timestamp = timestamp - point_idx * (scan_time / count_num);
+ // printf("timestamp = %f\n",point.timestamp);
+ point.x = points[i + 3000].range * cos(M_PI / 180 * points[i].degree);
+ point.y = -points[i + 3000].range * sin(M_PI / 180 * points[i].degree);
+ point.z = 0;
+ point.intensity = points[i + 3000].intensity;
+ point_cloud->points.push_back(point);
+ ++point_cloud->width;
+ }
+ }
+ sensor_msgs::msg::PointCloud2 pc_msg;
+ pcl::toROSMsg(*point_cloud, pc_msg);
+ point_cloud_pub->publish(pc_msg);
+ }
+ }
+ else
+ {
+ if (pubScan)
+ {
+ auto scan = sensor_msgs::msg::LaserScan::UniquePtr(new sensor_msgs::msg::LaserScan());
+ //int scan_num = ceil((angle_able_max - angle_able_min) / 360 * count_num) + 1;
+ int scan_num = fixed_array_length;//cyy_addcyy_add
+
+ std::vector points;
+ rclcpp::Time start_time;
+ float scan_time;
+ this->getScan(points, start_time, scan_time);
+ scan->header.frame_id = frame_id;
+ if (use_gps_ts)
+ {
+ scan->header.stamp = rclcpp::Time(sweep_end_time_gps, sweep_end_time_hardware);
+ }
+ else
+ {
+ scan->header.stamp = this->now(); // timestamp will obtained from sweep data stamp
+ }
+
+ if (angle_able_max > 360)
+ {
+ scan->angle_min = 2 * M_PI * (angle_able_min - 360) / 360;
+ scan->angle_max = 2 * M_PI * (angle_able_max - 360) / 360;
+ }
+ else
+ {
+ scan->angle_min = 2 * M_PI * angle_able_min / 360;
+ scan->angle_max = 2 * M_PI * angle_able_max / 360;
+ }
+ scan->angle_increment = 2 * M_PI / (double)(fixed_array_length - 1);
+
+ scan->range_min = min_range;
+ scan->range_max = max_range;
+ scan->ranges.reserve(scan_num);
+ scan->ranges.assign(scan_num, std::numeric_limits::infinity());
+ scan->intensities.reserve(scan_num);
+ scan->intensities.assign(scan_num, std::numeric_limits::infinity());
+ scan->scan_time = 0.1;
+ scan->time_increment = 0.1 / (double)(fixed_array_length - 1);
+
+ int start_num = floor(angle_able_min * count_num / 360);
+ int end_num = floor(angle_able_max * count_num / 360);
+
+ for (int i = 0; i < count_num; i++)
+ {
+ int point_idx = round((360 - points[i].degree) * count_num / 360);
+ if (point_idx < (end_num - count_num))
+ point_idx += count_num;
+ point_idx = point_idx - start_num;
+ if (point_idx < 0 || point_idx >= scan_num)
+ continue;
+ if (points[i].range == 0.0)
+ {
+ scan->ranges[point_idx] = std::numeric_limits::infinity();
+ }
+ else
+ {
+ double dist = points[i].range;
+ scan->ranges[point_idx] = (float)dist;
+ }
+ scan->intensities[point_idx] = points[i].intensity;
+
+ if(truncated_mode_){
+ int len=sizeof(scan_crop_max) / sizeof(scan_crop_max[0]) ;
+ for(int j=0;j=(scan_crop_min[j]*count_num / 360)) && (point_idx<=(scan_crop_max[j]*count_num / 360))){
+ scan->ranges[point_idx] = std::numeric_limits::infinity();
+ scan->intensities[point_idx] = 0;
+ }
+ }
+ }
+ }
+
+
+ scan_pub->publish(std::move(scan));
+ }
+ if (pubPointCloud2)
+ {
+ std::vector points;
+ rclcpp::Time start_time;
+ float scan_time;
+ this->getScan(points, start_time, scan_time);
+ VPointCloud::Ptr point_cloud(new VPointCloud());
+ if (use_gps_ts)
+ {
+ start_time = rclcpp::Time(sweep_end_time_gps, sweep_end_time_hardware);
+ }
+ double timestamp = start_time.seconds();
+ point_cloud->header.stamp = static_cast(timestamp * 1e6);
+ point_cloud->header.frame_id = frame_id;
+ point_cloud->height = 1;
+ for (uint16_t i = 0; i < count_num; i++)
+ {
+ double degree = 360.0 - points[i].degree;
+ bool pass_point = false;
+ if (angle_able_max < 360)
+ {
+ if (degree < angle_able_min || degree > angle_able_max)
+ pass_point = true;
+ }
+ else
+ {
+ if (degree < angle_able_min && degree > (angle_able_max - 360))
+ pass_point = true;
+ }
+ if (points[i].range < 0.001)
+ pass_point = true;
+ if (!pass_point)
+ {
+ // printf("degree = %f\n",degree);
+ // printf("angle_able_min = %f\nangle_able_max=%f\n",angle_able_min,angle_able_max);
+ VPoint point;
+ int point_idx = round(degree * count_num / 360);
+ point.timestamp = timestamp - point_idx * (scan_time / count_num);
+ // printf("timestamp = %f\n",point.timestamp);
+ point.x = points[i].range * cos(M_PI / 180 * points[i].degree);
+ point.y = -points[i].range * sin(M_PI / 180 * points[i].degree);
+ point.z = 0;
+ point.intensity = points[i].intensity;
+ point_cloud->points.push_back(point);
+ ++point_cloud->width;
+ }
+ }
+ sensor_msgs::msg::PointCloud2 pc_msg;
+ pcl::toROSMsg(*point_cloud, pc_msg);
+ point_cloud_pub->publish(pc_msg);
+ }
+ }
+ count_num = 0;
+ wait_for_wake = true;
+ if (first_compensation && compensation)
+ {
+ lidar_difop();
+ }
+ }
+ }
+
+ bool LslidarDriver::polling()
+ {
+ if (!is_start)
+ return true;
+ // Allocate a new shared pointer for zero-copy sharing with other nodelets.
+ unsigned char *packet_bytes = new unsigned char[500];
+ int len = 0;
+ bool difop = false;
+ if (interface_selection == "net")
+ {
+ auto packet = lslidar_msgs::msg::LslidarPacket::UniquePtr(
+ new lslidar_msgs::msg::LslidarPacket());
+
+ std_msgs::msg::Byte msg;
+ while (true)
+ {
+ difop = false;
+ len = 0;
+ // keep reading until full packet received
+ len = msop_input_->getPacket(packet);
+ if (packet->data[0] == 0x5a)
+ {
+ if (lidar_name == "N10" || lidar_name == "L10")
+ len = 58;
+ else if (lidar_name == "M10")
+ len = 92;
+ else if (lidar_name == "N10_P")
+ len = 108;
+ else if (lidar_name == "M10_GPS")
+ len = 102;
+ else
+ {
+ int len_H = packet->data[1];
+ int len_L = packet->data[2];
+ len = len_H * 256 + len_L;
+ }
+ for (int i = len - 1; i > 0; i--)
+ packet->data[i] = packet->data[i - 1];
+ packet->data[0] = 0xa5;
+ }
+
+ if (lidar_name == "N10" || lidar_name == "L10")
+ len = 58;
+ else if (lidar_name == "M10")
+ len = 92;
+ else if (lidar_name == "N10_P")
+ len = 108;
+ else if (lidar_name == "M10_GPS")
+ len = 102;
+ else
+ {
+ int len_H = packet->data[2];
+ int len_L = packet->data[3];
+ len = len_H * 256 + len_L;
+ }
+ if ((lidar_name == "M10" || lidar_name == "M10_DOUBLE" || lidar_name == "M10_GPS" || lidar_name == "M10_P" || lidar_name == "M10_PLUS") && compensation)
+ {
+ if (packet->data[2] == 0x55 && packet->data[3] == 0x00 && packet->data[186] == 0xFA && packet->data[187] == 0xFB)
+ {
+ len = 188;
+ difop = true;
+ }
+ }
+
+ if (len <= 0 || len >= 1000 || packet->data[0] != 0xa5 || packet->data[1] != 0x5a)
+ continue;
+ for (int i = 0; i < len; i++)
+ {
+ packet_bytes[i] = packet->data[i];
+ }
+ if ((lidar_name == "N10" || lidar_name == "L10" || lidar_name == "N10_P") && packet_bytes[len - 1] != N10_CalCRC8(packet_bytes, len - 1))
+ continue;
+ break;
+ }
+ }
+ else
+ {
+ if (in_file_name != "") // 读txt文件功能
+ {
+ int usleep_time = round(1000000 / 10 / 24) - 135;
+ while (true)
+ {
+ std::ifstream file_reader(in_file_name);
+ while (file_reader.peek() != EOF)
+ {
+ std::string line;
+ std::getline(file_reader, line, '\n');
+ for (long unsigned int i = 0; i < line.size() - 1; i++)
+ {
+ line[i] = line[i] - 48;
+ if (line[i] > 9)
+ line[i] = line[i] - 39;
+ }
+
+ for (long unsigned int i = 0; i < (line.size() - 1) / 2; i++)
+ {
+ packet_bytes[i] = line[i * 2] * 16 + line[i * 2 + 1];
+ }
+ if (lidar_name == "N10" || lidar_name == "L10")
+ len = 58;
+ else if (lidar_name == "M10")
+ len = 92;
+ else if (lidar_name == "N10_P")
+ len = 108;
+ else if (lidar_name == "M10_GPS")
+ len = 102;
+ else
+ {
+ int len_H = packet_bytes[2];
+ int len_L = packet_bytes[3];
+ len = len_H * 256 + len_L;
+ }
+ if (lidar_name == "N10_P" || lidar_name == "M10_DOUBLE")
+ LslidarDriver::data_processing_2(packet_bytes, len);
+ else
+ LslidarDriver::data_processing(packet_bytes, len);
+ usleep(usleep_time);
+ }
+ }
+ return false;
+ }
+ else
+ {
+ while (true)
+ {
+ difop = false;
+ len = 0;
+ len = LslidarDriver::receive_data(packet_bytes);
+ if ((lidar_name == "M10" || lidar_name == "M10_DOUBLE" || lidar_name == "M10_GPS" || lidar_name == "M10_P" || lidar_name == "M10_PLUS") && compensation)
+ {
+ if (packet_bytes[2] == 0x55 && packet_bytes[3] == 0x00 && packet_bytes[186] == 0xFA && packet_bytes[187] == 0xFB)
+ difop = true;
+ }
+ if (len == 0)
+ continue;
+ break;
+ }
+ }
+ }
+ if (difop)
+ LslidarDriver::difop_processing(packet_bytes);
+ else
+ {
+ if (lidar_name == "N10_P" || lidar_name == "M10_DOUBLE")
+ LslidarDriver::data_processing_2(packet_bytes, len);
+ else
+ LslidarDriver::data_processing(packet_bytes, len);
+ }
+ delete packet_bytes;
+ return true;
+ }
+
+} // namespace lslidar_driver
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver_node.cc b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver_node.cc
new file mode 100644
index 0000000..6a89f99
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver_node.cc
@@ -0,0 +1,35 @@
+/*
+ * This file is part of lslidar driver.
+ *
+ * The driver is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * The driver is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with the driver. If not, see .
+ */
+
+#include "rclcpp/rclcpp.hpp"
+#include "lslidar_driver/lslidar_driver.h"
+
+using namespace lslidar_driver;
+volatile sig_atomic_t flag = 1;
+
+int main(int argc, char* argv[])
+{
+ rclcpp::init(argc, argv);
+ auto node = std::make_shared();
+
+ while (rclcpp::ok() && node->polling()) {
+ rclcpp::spin_some(node);
+ }
+ //rclcpp::spin(node);
+ rclcpp::shutdown();
+ return 0;
+}
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/CMakeLists.txt b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/CMakeLists.txt
new file mode 100644
index 0000000..e6a603e
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/CMakeLists.txt
@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 3.5)
+project(lslidar_msgs)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+ set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+ set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+if(BUILD_TESTING)
+ find_package(ament_lint_auto REQUIRED)
+ ament_lint_auto_find_test_dependencies()
+endif()
+
+rosidl_generate_interfaces(lslidar_msgs
+ "msg/LslidarDifop.msg"
+ "msg/LslidarPacket.msg"
+ "msg/LslidarPoint.msg"
+ "msg/LslidarScan.msg"
+ "msg/LslidarSweep.msg"
+ DEPENDENCIES builtin_interfaces std_msgs
+ )
+
+ament_package()
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarDifop.msg b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarDifop.msg
new file mode 100644
index 0000000..f377c75
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarDifop.msg
@@ -0,0 +1,2 @@
+int64 temperature
+int64 rpm
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarPacket.msg b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarPacket.msg
new file mode 100644
index 0000000..d77ee45
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarPacket.msg
@@ -0,0 +1,5 @@
+# Raw Leishen LIDAR packet.
+
+builtin_interfaces/Time stamp # packet timestamp
+uint8[2000] data # packet contents
+
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarPoint.msg b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarPoint.msg
new file mode 100644
index 0000000..3132167
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarPoint.msg
@@ -0,0 +1,12 @@
+# Time when the point is captured
+float32 time
+
+# Converted distance in the sensor frame
+float64 x
+float64 y
+float64 z
+
+# Raw measurement from Leishen M10
+float64 azimuth
+float64 distance
+float64 intensity
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarScan.msg b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarScan.msg
new file mode 100644
index 0000000..0a3891c
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarScan.msg
@@ -0,0 +1,6 @@
+# Altitude of all the points within this scan
+float64 altitude
+
+# The valid points in this scan sorted by azimuth
+# from 0 to 359.99
+LslidarPoint[] points
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarSweep.msg b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarSweep.msg
new file mode 100644
index 0000000..9cfe4b7
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarSweep.msg
@@ -0,0 +1,4 @@
+std_msgs/Header header
+
+# The 0th scan is at the bottom
+LslidarScan[16] scans
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/package.xml b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/package.xml
new file mode 100644
index 0000000..8001ad5
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/package.xml
@@ -0,0 +1,25 @@
+
+
+ lslidar_msgs
+ 1.2.0
+ ROS message definitions for Leishen LIDARs.
+ Nick Shu
+ Nick Shu
+ GNU General Public License V3.0
+
+ ament_cmake
+ rosidl_default_generators
+
+ rosidl_default_runtime
+ builtin_interfaces
+
+ std_msgs
+ ament_lint_auto
+ ament_lint_common
+
+ rosidl_interface_packages
+
+
+ ament_cmake
+
+
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/version.txt b/src/LSLIDAR_X_ROS2-20240228/src/version.txt
new file mode 100644
index 0000000..7a2b9d4
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/version.txt
@@ -0,0 +1,20 @@
+版本变更
+/***************************************************************
+初始版本: LSLIDAR_M10_N10_V2.5.0_221104_ROS2
+变更内容:
+ 1.实现M10/M10_P/M10_PLUS/N10/M10_GPS网口和串口传输数据生成点云功能
+ 2.实现点云角度裁剪和距离过滤功能
+ 3.可以通过lslidar_order话题控制雷达启停
+ 4.支持读取pcap包
+
+更改日期: 2022-11-04
+***************************************************************/
+
+/***************************************************************
+初始版本: LSLIDAR_M10_N10_V2.5.0_221111_ROS2
+变更内容:
+ 1.针对M10和M10_GPS雷达出货后发现的点云问题进行驱动补救
+
+更改日期: 2022-11-11
+***************************************************************/
+
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/wheeltec_udev.sh b/src/LSLIDAR_X_ROS2-20240228/src/wheeltec_udev.sh
new file mode 100644
index 0000000..17aaa46
--- /dev/null
+++ b/src/LSLIDAR_X_ROS2-20240228/src/wheeltec_udev.sh
@@ -0,0 +1,33 @@
+#CP2102 串口号0002 设置别名为wheeltec_controller
+echo 'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60",ATTRS{serial}=="0002", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_controller"' >/etc/udev/rules.d/wheeltec_controller.rules
+#CH9102,同时系统安装了对应驱动 串口号0002 设置别名为wheeltec_controller
+echo 'KERNEL=="ttyCH343USB*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="55d4",ATTRS{serial}=="0002", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_controller"' >/etc/udev/rules.d/wheeltec_controller2.rules
+#CH9102,同时系统没有安装对应驱动 串口号0002 设置别名为wheeltec_controller
+echo 'KERNEL=="ttyACM*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="55d4",ATTRS{serial}=="0002", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_controller"' >/etc/udev/rules.d/wheeltec_controller3.rules
+
+#CP2102 串口号0001 设置别名为wheeltec_lidar
+echo 'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60",ATTRS{serial}=="0001", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_lidar"' >/etc/udev/rules.d/wheeltec_lidar.rules
+#CH9102,同时系统安装了对应驱动 串口号0001 设置别名为wheeltec_lidar
+echo 'KERNEL=="ttyCH343USB*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="55d4",ATTRS{serial}=="54B8001974", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_lidar"' >/etc/udev/rules.d/wheeltec_lidar2.rules
+#CH9102,同时系统没有安装对应驱动 串口号0001 设置别名为wheeltec_lidar
+echo 'KERNEL=="ttyACM*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="55d4",ATTRS{serial}=="0001", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_lidar"' >/etc/udev/rules.d/wheeltec_lidar3.rules
+
+#CP2102 串口号0003 设置别名为wheeltec_FDI_IMU_GNSS
+echo 'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60",ATTRS{serial}=="0003", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_FDI_IMU_GNSS"' >/etc/udev/rules.d/wheeltec_fdi_imu_gnss.rules
+#CH9102,同时系统安装了对应驱动 串口号0003 设置别名为wheeltec_FDI_IMU_GNSS
+echo 'KERNEL=="ttyCH343USB*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="55d4",ATTRS{serial}=="0003", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_FDI_IMU_GNSS"' >/etc/udev/rules.d/wheeltec_fdi_imu_gnss2.rules
+#CH9102,同时系统没有安装对应驱动 串口号0003 设置别名为wheeltec_FDI_IMU_GNSS
+echo 'KERNEL=="ttyACM*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="55d4",ATTRS{serial}=="0003", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_FDI_IMU_GNSS"' >/etc/udev/rules.d/wheeltec_fdi_imu_gnss3.rules
+
+echo 'SUBSYSTEM=="video4linux",ATTR{name}=="GENERAL WEBCAM",ATTR{index}=="0",MODE:="0777",SYMLINK+="RgbCam"' >>/etc/udev/rules.d/camera.rules
+echo 'SUBSYSTEM=="video4linux",ATTR{name}=="GENERAL WEBCAM: GENERAL WEBCAM",ATTR{index}=="0",MODE:="0777",SYMLINK+="RgbCam"' >>/etc/udev/rules.d/camera.rules
+echo 'SUBSYSTEM=="video4linux",ATTR{name}=="Astra Pro HD Camera: Astra Pro ",ATTR{index}=="0",MODE:="0777",SYMLINK+="Astra_Pro"' >>/etc/udev/rules.d/camera.rules
+echo 'SUBSYSTEM=="video4linux",ATTR{name}=="USB 2.0 Camera: USB Camera",ATTR{index}=="0",MODE:="0777",SYMLINK+="Astra_Dabai"' >>/etc/udev/rules.d/camera.rules
+echo 'SUBSYSTEM=="video4linux",ATTR{name}=="USB 2.0 Camera",ATTR{index}=="0",MODE:="0777",SYMLINK+="Astra_Gemini"' >>/etc/udev/rules.d/camera.rules
+echo 'SUBSYSTEM=="video4linux",ATTR{name}=="Intel(R) RealSense(TM) Depth Ca",ATTR{index}=="0",MODE:="0777",SYMLINK+="realsense"' >>/etc/udev/rules.d/camera.rules
+
+service udev reload
+sleep 2
+service udev restart
+
+
diff --git a/src/LSLIDAR_X_ROS2-20240228/src/镭神Lsx雷达旋转角度.png b/src/LSLIDAR_X_ROS2-20240228/src/镭神Lsx雷达旋转角度.png
new file mode 100644
index 0000000000000000000000000000000000000000..aa247855f25076075025218ad73d877a247427a8
GIT binary patch
literal 6646
zcmdT}dpMM9*Qc6@?UZ2)L$VW*;}&7aIgwK;#}Q^IBr^`f@J;W&zVCbc
zVX7^0oomck>E3#?75s7T3Qb12DbBCW&mzOlweyzunTA)J?1f<_)k~l{d7*-U?I(#-
zo0BtZuic=pOvRrHJ)xB7Yn@aG2d$tYr`nOcRd+m1qEu=mDodEuoSq@+i#<(Uy29Ww
za0W}q@rXsEJ>NQ0CD0J=rKBoOP$(V^da%+iRpPkvZIrSe%hNq#DJGOgq6k0dOazgr
z@7`?Low0@8c)b3x#WAz|F=M5=D!Jv(^5`(l5e%`&IOE!BIfK(BX=Y6O2u-Wne1+W@
z_VHGypbLcA@e@Z8B$V8JenM>IU@j|-1T3lB9$Rzcs&V&lzZ8*4k)uhw_}R}>iWnv(
zGYU63e(*@X?+3s8UdrjeZY}zpbDjk;(ir%MFOd$cqo{Acnlhiz9fP
zC#ujJ4Ph&u_2nfD4wdo|#Z25Rm_B#=dMkRE%ITcnXQ=pu$0_4XgpX%K4i+ZfC5;Nx
zVrHU)$>EKt$}bOzuK2HhkT40Fgp6>gu`4Mt+-247bQhKCKVag1d-