commit 7fbddb15c211a2fda64f067b4a1c2324da229f95 Author: Orange <2314753575@qq.com> Date: Thu Mar 5 10:27:00 2026 +0800 Please enter the commit message for your changes. Lines starting with '#' will be ignored, and an empty message aborts the commit. On branch master Initial commit Changes to be committed: new file: bashes/README.md new file: bashes/radar-driver-switch.sh 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_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_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/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/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_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\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\357\200\272Zone.Identifier" new file: src/origincar_description/meshes/down_left_Link.STL 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\357\200\272Zone.Identifier" new file: src/origincar_description/meshes/up_left_Link.STL 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\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_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: zbw_slamtoolbox/CMakeLists.txt new file: zbw_slamtoolbox/config/mapper_params_online_async.yaml new file: zbw_slamtoolbox/config/mapper_params_online_sync copy.yaml new file: zbw_slamtoolbox/config/navigation.yaml new file: zbw_slamtoolbox/launch/navigation.launch.py new file: zbw_slamtoolbox/launch/slamtoolbox.launch.py new file: zbw_slamtoolbox/package.xml Changes not staged for commit: deleted: zbw_slamtoolbox/CMakeLists.txt deleted: zbw_slamtoolbox/config/mapper_params_online_async.yaml deleted: zbw_slamtoolbox/config/mapper_params_online_sync copy.yaml deleted: zbw_slamtoolbox/config/navigation.yaml deleted: zbw_slamtoolbox/launch/navigation.launch.py deleted: zbw_slamtoolbox/launch/slamtoolbox.launch.py deleted: zbw_slamtoolbox/package.xml Untracked files: .gitignore 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..4faab12 --- /dev/null +++ b/bashes/radar-driver-switch.sh @@ -0,0 +1,125 @@ +#!/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/ttyACM1" # 替换为你要等待的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=$(date +%s) +current_time=0 +qinheng_id="" +usb_address="" +vendor_id="" +product_id="" +cdc_acm_sub_addr="" + + +# ----------------- 1. 等待TTY设备就绪 ----------------- +# 循环等待tty设备就绪（检查设备节点是否存在+是否可读写） +while true; do + if [ -c "${TTY_DEVICE}" ] && [ -r "${TTY_DEVICE}" ] && [ -w "${TTY_DEVICE}" ]; then + echo "[雷达驱动自动切换程序] 设备 ${TTY_DEVICE} 已就绪！" > /dev/kmsg + break + fi + + if [ ${MAX_WAIT_SECONDS} -gt 0 ]; then + current_time=$(date +%s) + 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..c22c0f7 --- /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 = 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 0000000..aa24785 Binary files /dev/null and b/src/LSLIDAR_X_ROS2-20240228/src/镭神Lsx雷达旋转角度.png differ diff --git a/src/LSLIDAR_X_ROS2-20240228/wheeltec_lidar.launch.py b/src/LSLIDAR_X_ROS2-20240228/wheeltec_lidar.launch.py new file mode 100644 index 0000000..f38b80a --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/wheeltec_lidar.launch.py @@ -0,0 +1,62 @@ +import os +from pathlib import Path +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import (DeclareLaunchArgument, GroupAction, + IncludeLaunchDescription, SetEnvironmentVariable) +from launch.launch_description_sources import PythonLaunchDescriptionSource + +def generate_launch_description(): + Lslidar_dir = get_package_share_directory('lslidar_driver') + Lslidar_launch_dir = os.path.join(Lslidar_dir, 'launch') + + + Ld14_dir = get_package_share_directory('ldlidar_sl_ros2') + Ld14_launch_dir = os.path.join(Ld14_dir, 'launch') + + Ld06_dir = get_package_share_directory('ldlidar_stl_ros2') + Ld06_launch_dir = os.path.join(Ld14_dir, 'launch') + + + Lsm10_m10_uart = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(Lslidar_launch_dir, 'lsm10_uart_launch.py')),) + + Lsm10_m10_net = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(Lslidar_launch_dir, 'lsm10_net_launch.py')),) + + Lsm10_m10p_uart = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(Lslidar_launch_dir, 'lsm10p_uart_launch.py')),) + + Lsm10_m10p_net = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(Lslidar_launch_dir, 'lsm10p_net_launch.py')),) + + Lsn10 = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(Lslidar_launch_dir, 'lsn10_launch.py')),) + Lsn10p = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(Lslidar_launch_dir, 'lsn10p_launch.py')),) + + Lsn10_net = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(Lslidar_launch_dir, 'lsn10_net_launch.py')),) + Lsn10p_net = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(Lslidar_launch_dir, 'lsn10p_net_launch.py')),) + + Ld14 = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(Ld14_launch_dir, 'ld14.launch.py')),) + Ld06 = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(Ld06_launch_dir, 'ld06.launch.py')),) + + # Create the launch description and populate + ld = LaunchDescription() + ''' + Please select your lidar here, options include: + Lsm10_m10p_uart、Lsm10_m10p_net、Lsm10_m10_uart、Lsm10_m10_net、Lsn10、Lsn10p,Lsn10_net、Lsn10p_net,ld14、Ld06. + 1.If you are using LS* lidar (including lsn10, lsm10*), please don't forget to + modify the tf conversion parameters of robot_mode_description.launch.py + according to the user guide file. + 2.If you are using m10 lidar, please pay attention to distinguish whether it is m10p or not. + ''' + ld.add_action(Lsn10) + + return ld + diff --git a/src/LSLIDAR_X_ROS2-20240228/wheeltec_lidar.launch.py仅在WHEELTEC镜像中使用 b/src/LSLIDAR_X_ROS2-20240228/wheeltec_lidar.launch.py仅在WHEELTEC镜像中使用 new file mode 100644 index 0000000..e69de29 diff --git a/src/cyy_navigation2/CMakeLists.txt b/src/cyy_navigation2/CMakeLists.txt new file mode 100644 index 0000000..884ac6a --- /dev/null +++ b/src/cyy_navigation2/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.8) +project(cyy_navigation2) + +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(nav2_bringup REQUIRED) +install( + DIRECTORY launch param maps + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() + diff --git a/src/cyy_navigation2/bt/follow_point.xml b/src/cyy_navigation2/bt/follow_point.xml new file mode 100644 index 0000000..ffae8d9 --- /dev/null +++ b/src/cyy_navigation2/bt/follow_point.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + diff --git a/src/cyy_navigation2/bt/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml b/src/cyy_navigation2/bt/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml new file mode 100644 index 0000000..f4ed518 --- /dev/null +++ b/src/cyy_navigation2/bt/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml @@ -0,0 +1,46 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/cyy_navigation2/bt/navigate_through_poses_w_replanning_and_recovery.xml b/src/cyy_navigation2/bt/navigate_through_poses_w_replanning_and_recovery.xml new file mode 100644 index 0000000..1378863 --- /dev/null +++ b/src/cyy_navigation2/bt/navigate_through_poses_w_replanning_and_recovery.xml @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/cyy_navigation2/bt/navigate_to_pose_w_replanning_and_recovery.xml b/src/cyy_navigation2/bt/navigate_to_pose_w_replanning_and_recovery.xml new file mode 100644 index 0000000..2396b84 --- /dev/null +++ b/src/cyy_navigation2/bt/navigate_to_pose_w_replanning_and_recovery.xml @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/cyy_navigation2/bt/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml b/src/cyy_navigation2/bt/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml new file mode 100644 index 0000000..6b5883b --- /dev/null +++ b/src/cyy_navigation2/bt/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/cyy_navigation2/bt/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml b/src/cyy_navigation2/bt/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml new file mode 100644 index 0000000..d21efa3 --- /dev/null +++ b/src/cyy_navigation2/bt/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/cyy_navigation2/bt/navigate_w_replanning_distance.xml b/src/cyy_navigation2/bt/navigate_w_replanning_distance.xml new file mode 100644 index 0000000..8ca5dbb --- /dev/null +++ b/src/cyy_navigation2/bt/navigate_w_replanning_distance.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + diff --git a/src/cyy_navigation2/bt/navigate_w_replanning_only_if_goal_is_updated.xml b/src/cyy_navigation2/bt/navigate_w_replanning_only_if_goal_is_updated.xml new file mode 100644 index 0000000..3e88944 --- /dev/null +++ b/src/cyy_navigation2/bt/navigate_w_replanning_only_if_goal_is_updated.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + diff --git a/src/cyy_navigation2/bt/navigate_w_replanning_only_if_path_becomes_invalid.xml b/src/cyy_navigation2/bt/navigate_w_replanning_only_if_path_becomes_invalid.xml new file mode 100644 index 0000000..e921db5 --- /dev/null +++ b/src/cyy_navigation2/bt/navigate_w_replanning_only_if_path_becomes_invalid.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/cyy_navigation2/bt/navigate_w_replanning_speed.xml b/src/cyy_navigation2/bt/navigate_w_replanning_speed.xml new file mode 100644 index 0000000..f0f47c4 --- /dev/null +++ b/src/cyy_navigation2/bt/navigate_w_replanning_speed.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + diff --git a/src/cyy_navigation2/bt/navigate_w_replanning_time.xml b/src/cyy_navigation2/bt/navigate_w_replanning_time.xml new file mode 100644 index 0000000..33c2c41 --- /dev/null +++ b/src/cyy_navigation2/bt/navigate_w_replanning_time.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + diff --git a/src/cyy_navigation2/bt/odometry_calibration.xml b/src/cyy_navigation2/bt/odometry_calibration.xml new file mode 100644 index 0000000..49000e1 --- /dev/null +++ b/src/cyy_navigation2/bt/odometry_calibration.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + diff --git a/src/cyy_navigation2/config/nav2_params.yaml b/src/cyy_navigation2/config/nav2_params.yaml new file mode 100644 index 0000000..295058a --- /dev/null +++ b/src/cyy_navigation2/config/nav2_params.yaml @@ -0,0 +1,348 @@ +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.5 + alpha2: 0.5 + alpha3: 0.5 + alpha4: 0.5 + alpha5: 0.5 + base_frame_id: "base_link" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: true + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 4.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 120 + max_particles: 4000 + min_particles: 1000 + odom_frame_id: "odom" + pf_err: 0.01 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 2 + robot_model_type: "nav2_amcl::DifferentialMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.9 + z_max: 0.05 + z_rand: 0.1 + z_short: 0.05 + scan_topic: scan + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.50 + max_vel_y: 0.0 + max_vel_theta: 3.0 + min_speed_xy: 0.0 + max_speed_xy: 0.50 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.20 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.30 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.20 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.30 + always_send_full_costmap: True + +map_server: + ros__parameters: + use_sim_time: True + # Overridden in launch by the "map" launch configuration or provided default value. + # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. + yaml_filename: "" + +map_saver: + ros__parameters: + use_sim_time: True + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["backup", "drive_on_heading", "assisted_teleop", "wait"] + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.3 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/src/cyy_navigation2/launch/car_bringup.launch.py b/src/cyy_navigation2/launch/car_bringup.launch.py new file mode 100644 index 0000000..ebed688 --- /dev/null +++ b/src/cyy_navigation2/launch/car_bringup.launch.py @@ -0,0 +1,108 @@ +#!/usr/bin/env python3 +""" +小车导航系统启动文件 +启动所有必要的硬件和导航节点 +""" + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, TimerAction, LogInfo +from launch.launch_description_sources import PythonLaunchDescriptionSource, FrontendLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from launch.conditions import IfCondition, UnlessCondition + +def generate_launch_description(): + # 获取包路径 + cyy_navigation2_dir = get_package_share_directory('cyy_navigation2') + + # 声明启动参数 + use_base = LaunchConfiguration('use_base', default='true') + use_lidar = LaunchConfiguration('use_lidar', default='true') + use_description = LaunchConfiguration('use_description', default='true') + use_bridge = LaunchConfiguration('use_bridge', default='true') + + # 启动参数列表 + launch_arguments = [ + DeclareLaunchArgument( + 'use_base', + default_value='true', + description='启动底盘控制' + ), + DeclareLaunchArgument( + 'use_lidar', + default_value='true', + description='启动激光雷达' + ), + DeclareLaunchArgument( + 'use_description', + default_value='true', + description='启动URDF显示' + ), + DeclareLaunchArgument( + 'use_bridge', + default_value='true', + description='启动ROS Bridge' + ), + ] + + # 创建启动描述 + ld = LaunchDescription(launch_arguments) + + # 1. 启动底盘控制 + base_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + FindPackageShare('origincar_base'), '/launch/base_serial.launch.py' + ]), + condition=IfCondition(use_base) + ) + + # 添加延迟，确保底盘先启动 + ld.add_action(TimerAction( + period=1.0, + actions=[LogInfo(msg='启动底盘控制...'), base_launch] + )) + + # 2. 启动激光雷达 + lidar_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + FindPackageShare('lslidar_driver'), '/launch/lsn10_launch.py' + ]), + condition=IfCondition(use_lidar) + ) + + ld.add_action(TimerAction( + period=2.0, + actions=[LogInfo(msg='启动激光雷达...'), lidar_launch] + )) + + # 3. 启动URDF显示 + description_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + FindPackageShare('origincar_description'), '/launch/display.launch.py' + ]), + condition=IfCondition(use_description) + ) + + ld.add_action(TimerAction( + period=3.0, + actions=[LogInfo(msg='启动URDF显示...'), description_launch] + )) + + # 4. 启动ROS Bridge + bridge_launch = IncludeLaunchDescription( + FrontendLaunchDescriptionSource([ + FindPackageShare('rosbridge_server'), '/launch/rosbridge_websocket_launch.xml' + ]), + condition=IfCondition(use_bridge) + ) + + ld.add_action(TimerAction( + period=4.0, + actions=[LogInfo(msg='启动ROS Bridge...'), bridge_launch] + )) + + + return ld diff --git a/src/cyy_navigation2/launch/cyy_nav.launch.py b/src/cyy_navigation2/launch/cyy_nav.launch.py new file mode 100644 index 0000000..26f4de1 --- /dev/null +++ b/src/cyy_navigation2/launch/cyy_nav.launch.py @@ -0,0 +1,41 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + # =============================1.λĵַ============================================================= + cyy_navigation2_dir = get_package_share_directory('cyy_navigation2') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + slam = LaunchConfiguration('slam', default='True') + # =============================2.ȡļ·=================================================== + # use_sim_time Ҫótrue,ΪgazeboǷ滷ʱͨ/clockȡϵͳʱ + use_sim_time = LaunchConfiguration('use_sim_time', default='false') + map_yaml_path = LaunchConfiguration('map', + default=os.path.join(cyy_navigation2_dir, 'maps', 'cyy_map1.yaml')) + nav2_param_path = LaunchConfiguration('params_file', + default=os.path.join(cyy_navigation2_dir, 'param', 'nav2_params.yaml')) + rviz_config_dir = os.path.join(nav2_bringup_dir, 'rviz', 'nav2_default_view.rviz') + + # =============================3.launchļ룺ͼ·Ƿʹ÷ʱԼnav2ļ============== + nav2_bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([nav2_bringup_dir, '/launch', '/bringup_launch.py']), + launch_arguments={ + 'map': map_yaml_path, + 'use_sim_time': use_sim_time, + 'params_file': nav2_param_path, + 'slam': slam}.items(), + ) + rviz_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', rviz_config_dir], + parameters=[{'use_sim_time': use_sim_time}], + output='screen') + + return LaunchDescription([nav2_bringup_launch]) \ No newline at end of file diff --git a/src/cyy_navigation2/launch/cyy_nav_box.launch.py b/src/cyy_navigation2/launch/cyy_nav_box.launch.py new file mode 100644 index 0000000..c6fd3f2 --- /dev/null +++ b/src/cyy_navigation2/launch/cyy_nav_box.launch.py @@ -0,0 +1,86 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +def generate_launch_description(): + # 获取包路径 + cyy_navigation2_dir = get_package_share_directory('cyy_navigation2') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + + # 声明参数 + use_sim_time = LaunchConfiguration('use_sim_time', default='False') + slam = LaunchConfiguration('slam', default='False') + localization = LaunchConfiguration('localization', default='False') + + # 定义文件路径 + map_yaml_path = os.path.join(cyy_navigation2_dir, 'maps', 'cyy_map.yaml') + nav2_param_path = os.path.join(cyy_navigation2_dir, 'param', 'nav2_params.yaml') + slam_params_file = os.path.join(cyy_navigation2_dir, 'param', 'slam_toolbox_localization.yaml') + + # 定义地图基础路径（用于 .posegraph 文件） + map_base_path = os.path.join(cyy_navigation2_dir, 'maps', 'cyy_map') + + # 检查 .posegraph 文件是否存在 + posegraph_path = map_base_path + '.posegraph' + if not os.path.exists(posegraph_path): + print(f"警告: .posegraph 文件不存在: {posegraph_path}") + print("将使用 AMCL 替代") + use_slam_toolbox = False + else: + use_slam_toolbox = True + print(f"找到 .posegraph 文件: {posegraph_path}") + + # 创建启动描述 + ld = LaunchDescription([ + DeclareLaunchArgument('use_sim_time', default_value='False'), + DeclareLaunchArgument('slam', default_value='False'), + DeclareLaunchArgument('localization', default_value='False'), + ]) + + # 如果使用 slam_toolbox + if use_slam_toolbox: + slam_toolbox_node = Node( + package='slam_toolbox', + executable='localization_slam_toolbox_node', + name='slam_toolbox', + output='screen', + parameters=[ + slam_params_file, + {'use_sim_time': use_sim_time}, + {'map_file_name': map_base_path}, + {'map_start_pose': [0.0, 0.0, 0.0]} + ], + remappings=[('/scan', '/scan')] + ) + ld.add_action(slam_toolbox_node) + # 使用 slam_toolbox 时，Nav2 不启用定位 + localization_arg = 'False' + else: + # 使用 AMCL + localization_arg = 'True' + + # Nav2 启动 - 使用正确的路径 + nav2_launch_path = os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py') + if not os.path.exists(nav2_launch_path): + print(f"错误: 找不到 Nav2 启动文件: {nav2_launch_path}") + return ld + + nav2_bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(nav2_launch_path), + launch_arguments={ + 'map': map_yaml_path, + 'use_sim_time': use_sim_time, + 'params_file': nav2_param_path, + 'slam': slam, + 'localization': localization_arg, + 'autostart': 'True', + }.items(), + ) + + ld.add_action(nav2_bringup_launch) + + return ld \ No newline at end of file diff --git a/src/cyy_navigation2/maps/cyy_map.data b/src/cyy_navigation2/maps/cyy_map.data new file mode 100644 index 0000000..f1ee3aa Binary files /dev/null and b/src/cyy_navigation2/maps/cyy_map.data differ diff --git a/src/cyy_navigation2/maps/cyy_map.pgm b/src/cyy_navigation2/maps/cyy_map.pgm new file mode 100644 index 0000000..294e35a Binary files /dev/null and b/src/cyy_navigation2/maps/cyy_map.pgm differ diff --git a/src/cyy_navigation2/maps/cyy_map.yaml b/src/cyy_navigation2/maps/cyy_map.yaml new file mode 100644 index 0000000..8d57c8b --- /dev/null +++ b/src/cyy_navigation2/maps/cyy_map.yaml @@ -0,0 +1,7 @@ +image: cyy_map.pgm +mode: trinary +resolution: 0.01 +origin: [-2.27, -1.63, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.25 \ No newline at end of file diff --git a/src/cyy_navigation2/maps/cyy_map1.pgm b/src/cyy_navigation2/maps/cyy_map1.pgm new file mode 100644 index 0000000..c1da83d Binary files /dev/null and b/src/cyy_navigation2/maps/cyy_map1.pgm differ diff --git a/src/cyy_navigation2/maps/cyy_map1.yaml b/src/cyy_navigation2/maps/cyy_map1.yaml new file mode 100644 index 0000000..097f099 --- /dev/null +++ b/src/cyy_navigation2/maps/cyy_map1.yaml @@ -0,0 +1,7 @@ +image: cyy_map1.pgm +mode: trinary +resolution: 0.01 +origin: [-3.49, -3.24, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.25 \ No newline at end of file diff --git a/src/cyy_navigation2/package.xml b/src/cyy_navigation2/package.xml new file mode 100644 index 0000000..8d7370f --- /dev/null +++ b/src/cyy_navigation2/package.xml @@ -0,0 +1,20 @@ + + + + cyy_navigation2 + 0.0.0 + TODO: Package description + root + TODO: License declaration + + ament_cmake + + nav2_bringup + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/cyy_navigation2/param/nav2_params.yaml b/src/cyy_navigation2/param/nav2_params.yaml new file mode 100644 index 0000000..02c388c --- /dev/null +++ b/src/cyy_navigation2/param/nav2_params.yaml @@ -0,0 +1,353 @@ +amcl: + ros__parameters: + enabled: False + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_link" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::DifferentialMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 6.28 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: -0.50 + min_vel_y: 0.0 + max_vel_x: 0.50 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.50 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 0 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.01 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + footprint: "[[0.2, 0.15], [0.2, -0.15], [-0.2, -0.15], [-0.2, 0.15]]" + robot_radius: 0.00 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.10 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + footprint: "[[0.2, 0.15], [0.2, -0.15], [-0.2, -0.15], [-0.2, 0.15]]" + robot_radius: 0.0 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.30 + always_send_full_costmap: True + +map_server: + ros__parameters: + use_sim_time: True + # Overridden in launch by the "map" launch configuration or provided default value. + # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. + yaml_filename: "" + +map_saver: + ros__parameters: + use_sim_time: True + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.3 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/src/cyy_navigation2/param/slam_toolbox_localization.yaml b/src/cyy_navigation2/param/slam_toolbox_localization.yaml new file mode 100644 index 0000000..cd751e1 --- /dev/null +++ b/src/cyy_navigation2/param/slam_toolbox_localization.yaml @@ -0,0 +1,54 @@ +slam_toolbox: + ros__parameters: + # 核心模式设置 + mode: "localization" # 重要：设置为定位模式 + map_file_name: "" # 由启动参数传入 + map_start_pose: [0.0, 0.0, 0.0] + # ROS框架设置 + odom_frame: "odom" + map_frame: "map" + base_frame: "base_link" + scan_topic: "/scan" + + # 求解器设置 + solver_plugin: "solver_plugins::CeresSolver" + ceres_linear_solver: "SPARSE_NORMAL_CHOLESKY" + ceres_preconditioner: "SCHUR_JACOBI" + ceres_trust_strategy: "LEVENBERG_MARQUARDT" + ceres_dogleg_type: "TRADITIONAL_DOGLEG" + ceres_loss_function: "None" + + # 定位优化参数 + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.05 + minimum_travel_heading: 0.087 + + # 扫描处理 + scan_buffer_size: 20 + throttle_scans: 1 + + # 回环检测（定位模式可关闭） + do_loop_closing: false + + # 搜索参数 + link_match_minimum_response_fine: 0.2 + link_scan_maximum_distance: 2.0 + loop_search_maximum_distance: 5.0 + + # 性能参数 + minimum_time_interval: 0.1 + transform_publish_period: 0.02 + map_update_interval: 0.5 + + # 地图参数 + resolution: 0.05 + max_laser_range: 15.0 + min_laser_range: 0.2 + + # 初始位姿（可选，可以通过RViz设置） + # map_start_pose: [0.0, 0.0, 0.0] + + # 调试 + debug_logging: false + enable_interactive_mode: true \ No newline at end of file diff --git a/src/cyy_slamtoolbox/CMakeLists.txt b/src/cyy_slamtoolbox/CMakeLists.txt new file mode 100644 index 0000000..305da3e --- /dev/null +++ b/src/cyy_slamtoolbox/CMakeLists.txt @@ -0,0 +1,61 @@ +# 设置CMake最低版本要求 +cmake_minimum_required(VERSION 3.8) +# 定义项目名称（应与package.xml中的一致） +project(cyy_slamtoolbox) + +# 如果未设置，默认使用C++17标准 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +# 查找编译依赖 +find_package(ament_cmake REQUIRED) +# 查找slam_toolbox包 +find_package(slam_toolbox REQUIRED) +# 查找其他可能的依赖，如传感器消息、几何库等 +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(geometry_msgs REQUIRED) + +# 消息输出，用于调试 +message(STATUS "开始配置项目: ${PROJECT_NAME}") + +# 包含头文件目录 +include_directories( + include + ${ament_INCLUDE_DIRS} +) + +# 如果有自定义的C++节点源文件，在此添加可执行文件 +# 例如，如果您有自定义的SLAM处理节点： +# add_executable(origincar_slam_node src/origincar_slam_node.cpp) +# ament_target_dependencies(origincar_slam_node +# rclcpp std_msgs sensor_msgs nav_msgs tf2 tf2_ros geometry_msgs slam_toolbox) +# +# 安装目标（将可执行文件安装到lib/） +# install(TARGETS origincar_slam_node +# DESTINATION lib/${PROJECT_NAME}) + +# 安装launch文件到share//launch目录 +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} + PATTERN "*.py" +) + +# 安装配置文件（如YAML参数文件）到share//config目录 +install(DIRECTORY config + DESTINATION share/${PROJECT_NAME} + PATTERN "*.yaml" + PATTERN "*.yml" +) + + +# 声明ament包依赖关系（确保在package.xml中已声明这些依赖） +ament_export_dependencies(slam_toolbox rclcpp std_msgs sensor_msgs nav_msgs tf2 tf2_ros geometry_msgs) + +# 必须调用此函数来生成必要的CMake配置 +ament_package() \ No newline at end of file diff --git a/src/cyy_slamtoolbox/config/angular_filter_example.yaml b/src/cyy_slamtoolbox/config/angular_filter_example.yaml new file mode 100644 index 0000000..26ef078 --- /dev/null +++ b/src/cyy_slamtoolbox/config/angular_filter_example.yaml @@ -0,0 +1,9 @@ +scan_to_scan_filter_chain: + ros__parameters: + filter1: + name: angle + type: laser_filters/LaserScanAngularBoundsFilter + params: + lower_angle: -1.52 + upper_angle: 1.52 + \ No newline at end of file diff --git a/src/cyy_slamtoolbox/config/box_filter_example.yaml b/src/cyy_slamtoolbox/config/box_filter_example.yaml new file mode 100644 index 0000000..ae63991 --- /dev/null +++ b/src/cyy_slamtoolbox/config/box_filter_example.yaml @@ -0,0 +1,15 @@ +scan_to_scan_filter_chain: + ros__parameters: + filter1: + name: box_filter + type: laser_filters/LaserScanBoxFilter + params: + box_frame: base_link + max_x: 0.5 + max_y: 0.5 + max_z: 0.5 + min_x: -0.5 + min_y: -0.5 + min_z: -0.5 + + invert: false # activate to remove all points outside of the box diff --git a/src/cyy_slamtoolbox/config/footprint_filter_example.yaml b/src/cyy_slamtoolbox/config/footprint_filter_example.yaml new file mode 100644 index 0000000..aa3ae16 --- /dev/null +++ b/src/cyy_slamtoolbox/config/footprint_filter_example.yaml @@ -0,0 +1,7 @@ +scan_to_scan_filter_chain: + ros__parameters: + filter1: + name: footprint_filter + type: laser_filters/LaserScanFootprintFilter + params: + inscribed_radius: 0.325 diff --git a/src/cyy_slamtoolbox/config/intensity_filter_example.yaml b/src/cyy_slamtoolbox/config/intensity_filter_example.yaml new file mode 100644 index 0000000..05d0827 --- /dev/null +++ b/src/cyy_slamtoolbox/config/intensity_filter_example.yaml @@ -0,0 +1,9 @@ +scan_to_scan_filter_chain: + ros__parameters: + filter1: + name: intensity + type: laser_filters/LaserScanIntensityFilter + params: + lower_threshold: 8000. + upper_threshold: 100000. + disp_histogram: 0 diff --git a/src/cyy_slamtoolbox/config/laser_filter_config.yaml b/src/cyy_slamtoolbox/config/laser_filter_config.yaml new file mode 100644 index 0000000..05730e4 --- /dev/null +++ b/src/cyy_slamtoolbox/config/laser_filter_config.yaml @@ -0,0 +1,19 @@ +scan_filter_chain: + - name: angle_bounds + type: laser_filters/LaserScanAngularBoundsFilter + params: + # 示例：如果您只想处理前方180度的数据，可以设置为： + lower_angle: -1.57 # 约-90度 + upper_angle: 1.57 # 约90度 + + - name: range_filter + type: laser_filters/LaserScanRangeFilter + params: + # 过滤掉无效的距离值，这对于SLAM非常重要 + lower_threshold: 0.2 # 忽略小于0.2米的测量（如雷达自身的噪声） + upper_threshold: 20.0 # 忽略大于20.0米的测量（通常是不稳定的数据） + use_message_range_limits: false + + - name: interpolation + type: laser_filters/InterpolationFilter + # 此滤波器没有参数，它会自动对因过滤产生的无效值（如NaN）进行插值修复[1](@ref) \ No newline at end of file diff --git a/src/cyy_slamtoolbox/config/mapper_params_lifelong.yaml b/src/cyy_slamtoolbox/config/mapper_params_lifelong.yaml new file mode 100644 index 0000000..fe39dc2 --- /dev/null +++ b/src/cyy_slamtoolbox/config/mapper_params_lifelong.yaml @@ -0,0 +1,87 @@ + +slam_toolbox: + ros__parameters: + + # Plugin params + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ROS Parameters + odom_frame: odom + map_frame: map + base_frame: base_footprint + scan_topic: /scan + use_map_saver: true + mode: mapping + + # lifelong params + lifelong_search_use_tree: false + lifelong_minimum_score: 0.1 + lifelong_iou_match: 0.85 + lifelong_node_removal_score: 0.04 + lifelong_overlap_score_scale: 0.06 + lifelong_constraint_multiplier: 0.08 + lifelong_nearby_penalty: 0.001 + lifelong_candidates_scale: 0.03 + + # if you'd like to immediately start continuing a map at a given pose + # or at the dock, but they are mutually exclusive, if pose is given + # will use pose + #map_file_name: test_steve + #map_start_pose: [0.0, 0.0, 0.0] + #map_start_at_dock: true + + debug_logging: false + throttle_scans: 1 + transform_publish_period: 0.02 #if 0 never publishes odometry + map_update_interval: 5.0 + resolution: 0.05 + min_laser_range: 0.0 #for rastering images + max_laser_range: 20.0 #for rastering images + minimum_time_interval: 0.5 + transform_timeout: 0.2 + tf_buffer_duration: 10. + stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + + # General Parameters + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.5 + minimum_travel_heading: 0.5 + scan_buffer_size: 10 + scan_buffer_maximum_scan_distance: 10.0 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + loop_search_maximum_distance: 3.0 + do_loop_closing: true + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + + # Correlation Parameters - Correlation Parameters + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + + # Correlation Parameters - Loop Closure Parameters + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + + # Scan Matcher Parameters + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true + min_pass_through: 2 + occupancy_threshold: 0.1 diff --git a/src/cyy_slamtoolbox/config/mapper_params_localization.yaml b/src/cyy_slamtoolbox/config/mapper_params_localization.yaml new file mode 100644 index 0000000..176f2e2 --- /dev/null +++ b/src/cyy_slamtoolbox/config/mapper_params_localization.yaml @@ -0,0 +1,70 @@ +slam_toolbox: + ros__parameters: + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ROS Parameters + odom_frame: odom + map_frame: map + base_frame: base_link + scan_topic: /scan + mode: localization #localization + + # if you'd like to start localizing on bringup in a map and pose + #map_file_name: test_steve + #map_start_pose: [5.0, 1.0, 0.0] + + debug_logging: false + throttle_scans: 1 + transform_publish_period: 0.02 #if 0 never publishes odometry + map_update_interval: 5.0 + resolution: 0.05 + min_laser_range: 0.0 #for rastering images + max_laser_range: 20.0 #for rastering images + minimum_time_interval: 0.5 + transform_timeout: 0.2 + tf_buffer_duration: 30. + stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + + # General Parameters + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.5 + minimum_travel_heading: 0.5 + scan_buffer_size: 3 + scan_buffer_maximum_scan_distance: 10.0 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + do_loop_closing: true + loop_match_minimum_chain_size: 3 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + + # Correlation Parameters - Correlation Parameters + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + + # Correlation Parameters - Loop Closure Parameters + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + loop_search_maximum_distance: 3.0 + + # Scan Matcher Parameters + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true + min_pass_through: 2 + occupancy_threshold: 0.1 diff --git a/src/cyy_slamtoolbox/config/mapper_params_offline.yaml b/src/cyy_slamtoolbox/config/mapper_params_offline.yaml new file mode 100644 index 0000000..d7d8587 --- /dev/null +++ b/src/cyy_slamtoolbox/config/mapper_params_offline.yaml @@ -0,0 +1,69 @@ +slam_toolbox: + ros__parameters: + + # Plugin params + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ROS Parameters + odom_frame: odom + map_frame: map + base_frame: base_footprint + scan_topic: /scan + use_map_saver: true + mode: mapping #localization + debug_logging: false + throttle_scans: 1 + transform_publish_period: 0.02 #if 0 never publishes odometry + map_update_interval: 10.0 + resolution: 0.05 + min_laser_range: 0.0 #for rastering images + max_laser_range: 20.0 #for rastering images + minimum_time_interval: 0.5 + transform_timeout: 0.2 + tf_buffer_duration: 14400. + stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + enable_interactive_mode: true + + # General Parameters + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.5 + minimum_travel_heading: 0.5 + scan_buffer_size: 10 + scan_buffer_maximum_scan_distance: 10.0 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + loop_search_maximum_distance: 3.0 + do_loop_closing: true + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + + # Correlation Parameters - Correlation Parameters + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + + # Correlation Parameters - Loop Closure Parameters + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + + # Scan Matcher Parameters + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true + min_pass_through: 2 + occupancy_threshold: 0.1 diff --git a/src/cyy_slamtoolbox/config/mapper_params_online_async.yaml b/src/cyy_slamtoolbox/config/mapper_params_online_async.yaml new file mode 100644 index 0000000..8a392d5 --- /dev/null +++ b/src/cyy_slamtoolbox/config/mapper_params_online_async.yaml @@ -0,0 +1,78 @@ +slam_toolbox: + ros__parameters: + + # Plugin params + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ROS Parameters + odom_frame: odom + map_frame: map + base_frame: base_link + scan_topic: /scan + use_map_saver: true + mode: mapping #localization + samples: 440 + # if you'd like to immediately start continuing a map at a given pose + # or at the dock, but they are mutually exclusive, if pose is given + # will use pose + #map_file_name: test_steve + # map_start_pose: [0.0, 0.0, 0.0] + #map_start_at_dock: true + + debug_logging: false + throttle_scans: 5 + transform_publish_period: 0.02 #if 0 never publishes odometry + map_update_interval: 0.01 + resolution: 0.01 + min_laser_range: 0.21 #for rastering images + max_laser_range: 15.0 #for rastering images + minimum_time_interval: 0.1 + transform_timeout: 2.0 + tf_buffer_duration: 30. + stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + enable_interactive_mode: true + + # General Parameters + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.2 + minimum_travel_heading: 0.2 + scan_buffer_size: 10 + samples: 440 + scan_buffer_maximum_scan_distance: 10.0 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + loop_search_maximum_distance: 3.0 + do_loop_closing: true + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + + # Correlation Parameters - Correlation Parameters + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.05 + correlation_search_space_smear_deviation: 0.1 + + # Correlation Parameters - Loop Closure Parameters + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + + # Scan Matcher Parameters + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true + min_pass_through: 2 + occupancy_threshold: 0.1 diff --git a/src/cyy_slamtoolbox/config/mapper_params_online_sync.yaml b/src/cyy_slamtoolbox/config/mapper_params_online_sync.yaml new file mode 100644 index 0000000..4ccc68c --- /dev/null +++ b/src/cyy_slamtoolbox/config/mapper_params_online_sync.yaml @@ -0,0 +1,77 @@ +slam_toolbox: + ros__parameters: + + # Plugin params + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ROS Parameters + odom_frame: odom + map_frame: map + base_frame: base_footprint + scan_topic: /scan + use_map_saver: true + mode: mapping #localization + + # if you'd like to immediately start continuing a map at a given pose + # or at the dock, but they are mutually exclusive, if pose is given + # will use pose + #map_file_name: test_steve + #map_start_pose: [0.0, 0.0, 0.0] + #map_start_at_dock: true + + debug_logging: false + throttle_scans: 1 + transform_publish_period: 0.02 #if 0 never publishes odometry + map_update_interval: 5.0 + resolution: 0.05 + min_laser_range: 0.0 #for rastering images + max_laser_range: 20.0 #for rastering images + minimum_time_interval: 0.5 + transform_timeout: 0.2 + tf_buffer_duration: 30. + stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + enable_interactive_mode: true + + # General Parameters + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.5 + minimum_travel_heading: 0.5 + scan_buffer_size: 10 + scan_buffer_maximum_scan_distance: 10.0 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + loop_search_maximum_distance: 3.0 + do_loop_closing: true + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + + # Correlation Parameters - Correlation Parameters + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + + # Correlation Parameters - Loop Closure Parameters + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + + # Scan Matcher Parameters + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true + min_pass_through: 2 + occupancy_threshold: 0.1 diff --git a/src/cyy_slamtoolbox/config/mask_filter_example.yaml b/src/cyy_slamtoolbox/config/mask_filter_example.yaml new file mode 100644 index 0000000..17751d1 --- /dev/null +++ b/src/cyy_slamtoolbox/config/mask_filter_example.yaml @@ -0,0 +1,16 @@ +scan_to_scan_filter_chain: + ros__parameters: + filter1: + name: mask + type: laser_filters/LaserScanMaskFilter + params: + masks: + laser: + - 100. + - 101. + - 102. + - 103. + - 104. + - 1000. + - 1001. + - 1002. diff --git a/src/cyy_slamtoolbox/config/median_filter_example.yaml b/src/cyy_slamtoolbox/config/median_filter_example.yaml new file mode 100644 index 0000000..ea6c447 --- /dev/null +++ b/src/cyy_slamtoolbox/config/median_filter_example.yaml @@ -0,0 +1,20 @@ +scan_to_scan_filter_chain: + ros__parameters: + filter1: + type: laser_filters/LaserArrayFilter + name: median + params: + range_filter_chain: + filter1: + name: median + type: filters/MultiChannelMedianFilterFloat + params: + number_of_observations: 5 + unused: 10 + intensity_filter_chain: + filter1: + name: median + type: filters/MultiChannelMedianFilterFloat + params: + number_of_observations: 5 + unused: 10 diff --git a/src/cyy_slamtoolbox/config/median_spatial_filter_example.yaml b/src/cyy_slamtoolbox/config/median_spatial_filter_example.yaml new file mode 100644 index 0000000..bbe95e0 --- /dev/null +++ b/src/cyy_slamtoolbox/config/median_spatial_filter_example.yaml @@ -0,0 +1,17 @@ +scan_to_scan_filter_chain: + ros__parameters: + filter1: + name: median_spatial + type: laser_filters/LaserScanMedianSpatialFilter + params: + window_size: 31 + filter2: + name: median_filter + type: laser_filters/LaserArrayFilter + params: + range_filter_chain: + filter1: + name: median + type: filters/MultiChannelMedianFilterFloat + params: + number_of_observations: 3 \ No newline at end of file diff --git a/src/cyy_slamtoolbox/config/multiple_filters_example.yaml b/src/cyy_slamtoolbox/config/multiple_filters_example.yaml new file mode 100644 index 0000000..e992449 --- /dev/null +++ b/src/cyy_slamtoolbox/config/multiple_filters_example.yaml @@ -0,0 +1,42 @@ +scan_to_scan_filter_chain: + ros__parameters: + filter1: + type: laser_filters/LaserArrayFilter + name: laser_median_5 + params: + range_filter_chain: + filter1: + name: median_5 + type: filters/MultiChannelMedianFilterFloat + params: + number_of_observations: 5 + unused: 10 + intensity_filter_chain: + filter2: + name: median_5 + type: filters/MultiChannelMedianFilterFloat + params: + number_of_observations: 5 + unused: 10 + filter2: + name: intensity + type: laser_filters/LaserScanIntensityFilter + params: + lower_threshold: 8000. + upper_threshold: 100000. + disp_histogram: 0 + filter3: + name: shadows + type: laser_filters/ScanShadowsFilter + params: + min_angle: 10. + max_angle: 170. + neighbors: 20 + window: 0 + filter4: + name: dark_shadows + type: laser_filters/LaserScanIntensityFilter + params: + lower_threshold: 100. + upper_threshold: 10000. + disp_histogram: 0 diff --git a/src/cyy_slamtoolbox/config/pass_through_example.yaml b/src/cyy_slamtoolbox/config/pass_through_example.yaml new file mode 100644 index 0000000..e69de29 diff --git a/src/cyy_slamtoolbox/config/polygon_filter_example.yaml b/src/cyy_slamtoolbox/config/polygon_filter_example.yaml new file mode 100644 index 0000000..90bf7d9 --- /dev/null +++ b/src/cyy_slamtoolbox/config/polygon_filter_example.yaml @@ -0,0 +1,10 @@ +scan_to_scan_filter_chain: + ros__parameters: + filter1: + name: polygon_filter + type: laser_filters/LaserScanPolygonFilter + params: + polygon_frame: base_link + polygon: '[[0.0, 0.0], [0.1, 0.1], [0.1, 0.0], [0.0, -0.1]]' + invert: false + footprint_topic: base_footprint_exclude diff --git a/src/cyy_slamtoolbox/config/range_filter_example.yaml b/src/cyy_slamtoolbox/config/range_filter_example.yaml new file mode 100644 index 0000000..27869ed --- /dev/null +++ b/src/cyy_slamtoolbox/config/range_filter_example.yaml @@ -0,0 +1,11 @@ +scan_to_scan_filter_chain: + ros__parameters: + filter1: + name: box_filter + type: laser_filters/LaserScanRangeFilter + params: + use_message_range_limits: false # if not specified defaults to false + lower_threshold: 0.5 # if not specified defaults to 0.0 + upper_threshold: 1.0 # if not specified defaults to 100000.0 + lower_replacement_value: -.inf # if not specified defaults to NaN + upper_replacement_value: .inf # if not specified defaults to NaN diff --git a/src/cyy_slamtoolbox/config/scan_blob_filter_example.yaml b/src/cyy_slamtoolbox/config/scan_blob_filter_example.yaml new file mode 100644 index 0000000..eb9f4ab --- /dev/null +++ b/src/cyy_slamtoolbox/config/scan_blob_filter_example.yaml @@ -0,0 +1,8 @@ +scan_to_scan_filter_chain: + ros__parameters: + filter1: + name: scan_blob_filter + type: laser_filters/ScanBlobFilter + params: + max_radius: 0.25 # maximum radius to be considered as blob object + min_points: 6 # min scan points to be considered as blob object diff --git a/src/cyy_slamtoolbox/config/sector_filter_example.yaml b/src/cyy_slamtoolbox/config/sector_filter_example.yaml new file mode 100644 index 0000000..e278235 --- /dev/null +++ b/src/cyy_slamtoolbox/config/sector_filter_example.yaml @@ -0,0 +1,12 @@ +scan_to_scan_filter_chain: + ros__parameters: + filter1: + name: scan_filter + type: laser_filters/LaserScanSectorFilter + params: + angle_min: 2.54 # if not specified defaults to 0.0 + angle_max: -2.54 # if not specified defaults to 0.0 + range_min: 0.2 # if not specified defaults to 0.0 + range_max: 2.0 # if not specified defaults to 100000.0 + clear_inside: true # if not specified defaults to true + invert: false # (!clear_inside) if not specified defaults to false diff --git a/src/cyy_slamtoolbox/config/shadow_filter_example.yaml b/src/cyy_slamtoolbox/config/shadow_filter_example.yaml new file mode 100644 index 0000000..82504fe --- /dev/null +++ b/src/cyy_slamtoolbox/config/shadow_filter_example.yaml @@ -0,0 +1,18 @@ +scan_to_scan_filter_chain: + ros__parameters: + filter1: + name: shadows + type: laser_filters/ScanShadowsFilter + params: + min_angle: 10. + max_angle: 170. + neighbors: 20 + window: 1 + filter2: + name: dark_shadows + type: laser_filters/LaserScanIntensityFilter + params: + lower_threshold: 100. + upper_threshold: 10000. + disp_histogram: 0 + diff --git a/src/cyy_slamtoolbox/config/slam_toolbox_default.rviz b/src/cyy_slamtoolbox/config/slam_toolbox_default.rviz new file mode 100644 index 0000000..c079a07 --- /dev/null +++ b/src/cyy_slamtoolbox/config/slam_toolbox_default.rviz @@ -0,0 +1,137 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 154 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" + - Class: slam_toolbox::SlamToolboxPlugin + Name: SlamToolboxPlugin +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 + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 10 + 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.785398006439209 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.785398006439209 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000217000002b0fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000125000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000220053006c0061006d0054006f006f006c0062006f00780050006c007500670069006e0100000168000001850000018500ffffff000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000017e000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + SlamToolboxPlugin: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 72 + Y: 60 diff --git a/src/cyy_slamtoolbox/config/speckle_filter_example.yaml b/src/cyy_slamtoolbox/config/speckle_filter_example.yaml new file mode 100644 index 0000000..6f8c6a7 --- /dev/null +++ b/src/cyy_slamtoolbox/config/speckle_filter_example.yaml @@ -0,0 +1,19 @@ +scan_filter_chain: +- name: speckle_filter + type: laser_filters/LaserScanSpeckleFilter + params: + # Select which filter type to use. + # 0: Range based filtering (distance between consecutive points) + # 1: Euclidean filtering based on radius outlier search + filter_type: 0 + + # Only ranges smaller than this range are taken into account + max_range: 2.0 + + # filter_type[0] (Distance): max distance between consecutive points + # filter_type[1] (RadiusOutlier): max distance between points + max_range_difference: 0.1 + + # filter_type[0] (Distance): Number of consecutive ranges that will be tested for max_distance + # filter_type[1] (RadiusOutlier): Minimum number of neighbors + filter_window: 2 diff --git a/src/cyy_slamtoolbox/launch/cyy_slam_toolbox_launch.launch.py b/src/cyy_slamtoolbox/launch/cyy_slam_toolbox_launch.launch.py new file mode 100644 index 0000000..aa4ff01 --- /dev/null +++ b/src/cyy_slamtoolbox/launch/cyy_slam_toolbox_launch.launch.py @@ -0,0 +1,24 @@ +#!/usr/bin/env python3 +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + # 获取参数文件路径 + pkg_share = get_package_share_directory('cyy_slamtoolbox') + params_file = os.path.join(pkg_share, 'config', 'mapper_params_online_async.yaml') + + # 创建SLAM Toolbox节点 + slam_toolbox_node = Node( + package='slam_toolbox', + executable='async_slam_toolbox_node', # 或 sync_slam_toolbox_node + name='slam_toolbox', + output='screen', + parameters=[params_file] + ) + + + return LaunchDescription([ + slam_toolbox_node + ]) \ No newline at end of file diff --git a/src/cyy_slamtoolbox/launch/cyy_slam_toolbox_location.launch.py b/src/cyy_slamtoolbox/launch/cyy_slam_toolbox_location.launch.py new file mode 100644 index 0000000..c8c784a --- /dev/null +++ b/src/cyy_slamtoolbox/launch/cyy_slam_toolbox_location.launch.py @@ -0,0 +1,24 @@ +#!/usr/bin/env python3 +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + # ȡļ· + pkg_share = get_package_share_directory('cyy_slamtoolbox') + params_file = os.path.join(pkg_share, 'config', 'mapper_params_localization.yaml') + + # SLAM Toolboxڵ + slam_toolbox_node = Node( + package='slam_toolbox', + executable='localization_slam_toolbox_node', # sync_slam_toolbox_node + name='slam_toolbox', + output='screen', + parameters=[params_file] + ) + + + return LaunchDescription([ + slam_toolbox_node + ]) \ No newline at end of file diff --git a/src/cyy_slamtoolbox/launch/filter.launch.py b/src/cyy_slamtoolbox/launch/filter.launch.py new file mode 100644 index 0000000..19f7450 --- /dev/null +++ b/src/cyy_slamtoolbox/launch/filter.launch.py @@ -0,0 +1,24 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + # 假设配置文件路径，你需要根据实际情况修改 + config_path = os.path.join( + get_package_share_directory('cyy_slamtoolbox'), + 'config', + 'angular_filter_example.yaml' + ) + + laser_filter_node = Node( + package='laser_filters', + executable='scan_to_scan_filter_chain', + name='laser_filter', + output='screen', + parameters=[config_path], + # 重新映射话题：从/base_scan原始数据过滤后发布到/scan + remappings=[('scan', '/base_scan'), ('scan_filtered', '/scan')] + ) + + return LaunchDescription([laser_filter_node]) \ No newline at end of file diff --git a/src/cyy_slamtoolbox/package.xml b/src/cyy_slamtoolbox/package.xml new file mode 100644 index 0000000..18ae51b --- /dev/null +++ b/src/cyy_slamtoolbox/package.xml @@ -0,0 +1,18 @@ + + + + cyy_slamtoolbox + 0.0.0 + TODO: Package description + root + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/gc_navigation_fish/CMakeLists.txt b/src/gc_navigation_fish/CMakeLists.txt new file mode 100644 index 0000000..21e4ee1 --- /dev/null +++ b/src/gc_navigation_fish/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.8) +project(gc_navigation_fish) + +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) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) +install( + DIRECTORY launch param maps + DESTINATION share/${PROJECT_NAME} +) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/gc_navigation_fish/launch/gc_navigation.launch.py b/src/gc_navigation_fish/launch/gc_navigation.launch.py new file mode 100644 index 0000000..2419737 --- /dev/null +++ b/src/gc_navigation_fish/launch/gc_navigation.launch.py @@ -0,0 +1,34 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + # =============================1.定位到包的地址============================================================= + gc_navigation_fish_dir = get_package_share_directory( + 'gc_navigation_fish') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + + # =============================2.声明参数，获取配置文件路径=================================================== + # use_sim_time 这里要设置成true,因为gazebo是仿真环境，其时间是通过/clock话题获取，而不是系统时间 + use_sim_time = LaunchConfiguration('use_sim_time', default='false') + map_yaml_path = LaunchConfiguration('map', default=os.path.join( + gc_navigation_fish_dir, 'maps', 'test_map.yaml')) + nav2_param_path = LaunchConfiguration('params_file', default=os.path.join( + gc_navigation_fish_dir, 'param', 'gc_navigation.yaml')) + # =============================3.声明启动launch文件，传入：地图路径、是否使用仿真时间以及nav2参数文件============== + nav2_bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [nav2_bringup_dir, '/launch', '/bringup_launch.py']), + launch_arguments={ + 'map': map_yaml_path, + 'use_sim_time': use_sim_time, + 'params_file': nav2_param_path}.items(), + ) + + + return LaunchDescription([nav2_bringup_launch]) diff --git a/src/gc_navigation_fish/maps/test_map.pgm b/src/gc_navigation_fish/maps/test_map.pgm new file mode 100644 index 0000000..079a090 Binary files /dev/null and b/src/gc_navigation_fish/maps/test_map.pgm differ diff --git a/src/gc_navigation_fish/maps/test_map.yaml b/src/gc_navigation_fish/maps/test_map.yaml new file mode 100644 index 0000000..2d524cd --- /dev/null +++ b/src/gc_navigation_fish/maps/test_map.yaml @@ -0,0 +1,7 @@ +image: test_map.pgm +mode: trinary +resolution: 0.05 +origin: [-2.03, -6.12, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.25 \ No newline at end of file diff --git a/src/gc_navigation_fish/package.xml b/src/gc_navigation_fish/package.xml new file mode 100644 index 0000000..f54f961 --- /dev/null +++ b/src/gc_navigation_fish/package.xml @@ -0,0 +1,18 @@ + + + + gc_navigation_fish + 0.0.0 + TODO: Package description + guoch + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + nav2_bringup + + ament_cmake + + diff --git a/src/gc_navigation_fish/param/gc_navigation.yaml b/src/gc_navigation_fish/param/gc_navigation.yaml new file mode 100644 index 0000000..0b6f53a --- /dev/null +++ b/src/gc_navigation_fish/param/gc_navigation.yaml @@ -0,0 +1,408 @@ +amcl: + ros__parameters: + use_sim_time: False + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_link" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::DifferentialMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 10.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + +amcl_map_client: + ros__parameters: + use_sim_time: False + +amcl_rclcpp_node: + ros__parameters: + use_sim_time: False + +bt_navigator: + ros__parameters: + use_sim_time: False + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + +bt_navigator_rclcpp_node: + ros__parameters: + use_sim_time: False + +controller_server: + ros__parameters: + controller_frequency: 30.0 + FollowPath: + plugin: "nav2_mppi_controller::MPPIController" + time_steps: 56 + model_dt: 0.05 + batch_size: 2000 + vx_std: 0.2 + vy_std: 0.0 + wz_std: 0.4 + vx_max: 0.5 + vx_min: -0.35 + vy_max: 0.0 + wz_max: 1.9 + iteration_count: 1 + temperature: 0.3 + gamma: 0.015 + motion_model: "Ackermann" + visualize: false + TrajectoryVisualizer: + trajectory_step: 5 + time_step: 3 + AckermannConstraints: + min_turning_r: 0.4 + critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"] + ConstraintCritic: + enabled: true + cost_power: 1 + cost_weight: 4.0 + GoalCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 1.4 + GoalAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + threshold_to_consider: 0.5 + PreferForwardCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 0.5 + # Option to replace Cost and use Obstacles instead + # ObstaclesCritic: + # enabled: true + # cost_power: 1 + # repulsion_weight: 1.5 + # critical_weight: 20.0 + # consider_footprint: false + # collision_cost: 10000.0 + # collision_margin_distance: 0.1 + # near_goal_distance: 0.5 + CostCritic: + enabled: true + cost_power: 1 + cost_weight: 3.81 + critical_cost: 300.0 + consider_footprint: true + collision_cost: 1000000.0 + near_goal_distance: 1.0 + trajectory_point_step: 2 + PathAlignCritic: + enabled: true + cost_power: 1 + cost_weight: 14.0 + max_path_occupancy_ratio: 0.05 + trajectory_point_step: 4 + threshold_to_consider: 0.5 + offset_from_furthest: 20 + use_path_orientations: false + PathFollowCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + offset_from_furthest: 5 + threshold_to_consider: 1.4 + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + offset_from_furthest: 4 + threshold_to_consider: 0.5 + max_angle_to_furthest: 1.0 + forward_preference: true + # VelocityDeadbandCritic: + # enabled: true + # cost_power: 1 + # cost_weight: 35.0 + # deadband_velocities: [0.05, 0.05, 0.05] + # TwirlingCritic: + # enabled: true + # twirling_cost_power: 1 + # twirling_cost_weight: 10.0 + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: False + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: False + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + footprint: "[[0.14, 0.085], + [0.14, -0.085], + [-0.14, -0.085], + [-0.14, 0.085]]" + footprint_padding: 0.02 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + map_subscribe_transient_local: True + always_send_full_costmap: True + local_costmap_client: + ros__parameters: + use_sim_time: False + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: False + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: False + footprint: "[[0.14, 0.085], + [0.14, -0.085], + [-0.14, -0.085], + [-0.14, 0.085]]" + footprint_padding: 0.02 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + global_costmap_client: + ros__parameters: + use_sim_time: False + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: False + +map_server: + ros__parameters: + use_sim_time: False + yaml_filename: "turtlebot3_world.yaml" + +map_saver: + ros__parameters: + use_sim_time: False + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + planner_plugins: ["GridBased"] + use_sim_time: False + + GridBased: + plugin: "nav2_smac_planner/SmacPlannerHybrid" + downsample_costmap: false # whether or not to downsample the map + downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) + tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found. + allow_unknown: true # allow traveling in unknown space + max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable + max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution + max_planning_time: 5.0 # max time in s for planner to plan, smooth + motion_model_for_search: "REEDS_SHEPP" # or REEDS_SHEPP 可以倒车 + angle_quantization_bins: 72 # Number of angle bins for search + analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach. + analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting + minimum_turning_radius: 0.40 # minimum turning radius in m of path / vehicle + reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1 + change_penalty: 0.0 # Penalty to apply if motion is changing directions (L to R), must be >= 0 + non_straight_penalty: 1.2 # Penalty to apply if motion is non-straight, must be => 1 + cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. + retrospective_penalty: 0.015 + lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. + cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. + viz_expansions: false # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning). WARNING: heavy to compute and to display, for debug only as it degrades the performance. + smooth_path: True # If true, does a simple and quick smoothing post-processing to the path + + smoother: + max_iterations: 1000 + w_smooth: 0.3 + w_data: 0.2 + tolerance: 1.0e-10 + do_refinement: true + refinement_num: 2 + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: False + +smoother_server: + ros__parameters: + use_sim_time: False + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: False + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: False + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 diff --git a/src/gc_navigation_fish/param/navigation_test.yaml b/src/gc_navigation_fish/param/navigation_test.yaml new file mode 100644 index 0000000..86160da --- /dev/null +++ b/src/gc_navigation_fish/param/navigation_test.yaml @@ -0,0 +1,173 @@ +# ========================= +# 🚫 删除 amcl（SLAM模式不需要） +# ========================= +# ❌ 已移除 amcl 整块 +# ❌ 已移除 amcl_map_client +# ❌ 已移除 amcl_rclcpp_node + + +# ========================= +# 🚫 删除 map_server（由 slam_toolbox 提供 map） +# ========================= +# ❌ 已移除 map_server +# ❌ 已移除 map_saver + + +# ========================= +# BT Navigator +# ========================= +bt_navigator: + ros__parameters: + use_sim_time: false # ✅ 改为 false（真车） + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_navigate_to_pose_action_bt_node + +bt_navigator_rclcpp_node: + ros__parameters: + use_sim_time: false # ✅ 改为 false + + +# ========================= +# Controller Server（保持你的MPPI） +# ========================= +controller_server: + ros__parameters: + use_sim_time: false # ✅ 改为 false + controller_frequency: 30.0 + controller_plugins: ["FollowPath"] # ✅ 确保声明插件 + + FollowPath: + plugin: "nav2_mppi_controller::MPPIController" + motion_model: "Ackermann" + time_steps: 56 + model_dt: 0.05 + batch_size: 2000 + vx_max: 0.5 + vx_min: -0.35 + wz_max: 1.9 + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: false # ✅ 改为 false + + +# ========================= +# Planner Server（Smac Hybrid A*） +# ========================= +planner_server: + ros__parameters: + use_sim_time: false # ✅ 改为 false + planner_plugins: ["GridBased"] + + GridBased: + plugin: "nav2_smac_planner/SmacPlannerHybrid" + motion_model_for_search: "REEDS_SHEPP" + minimum_turning_radius: 0.40 + allow_unknown: true + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: false # ✅ 改为 false + + +# ========================= +# Local Costmap +# ========================= +local_costmap: + local_costmap: + ros__parameters: + use_sim_time: false # ✅ 改为 false + global_frame: odom + robot_base_frame: base_link + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + plugins: ["voxel_layer", "inflation_layer"] + + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + observation_sources: scan + scan: + topic: /scan + data_type: "LaserScan" + marking: true + clearing: true + + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + inflation_radius: 0.55 + + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: false # ✅ 改为 false + + +# ========================= +# Global Costmap +# ========================= +global_costmap: + global_costmap: + ros__parameters: + use_sim_time: false # ✅ 改为 false + global_frame: map + robot_base_frame: base_link + resolution: 0.05 + track_unknown_space: true + plugins: ["obstacle_layer", "inflation_layer"] # ✅ 删除 static_layer（SLAM模式不需要） + + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + observation_sources: scan + scan: + topic: /scan + data_type: "LaserScan" + marking: true + clearing: true + + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + inflation_radius: 0.55 + + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: false # ✅ 改为 false + + +# ========================= +# Behavior Server +# ========================= +behavior_server: + ros__parameters: + use_sim_time: false # ✅ 改为 false + global_frame: odom + robot_base_frame: base_link + behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"] + + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + + +# ========================= +# Waypoint Follower +# ========================= +waypoint_follower: + ros__parameters: + use_sim_time: false # ✅ 改为 false + loop_rate: 20 \ No newline at end of file diff --git a/src/gc_slam_toolbox_fish/CMakeLists.txt b/src/gc_slam_toolbox_fish/CMakeLists.txt new file mode 100644 index 0000000..262d40e --- /dev/null +++ b/src/gc_slam_toolbox_fish/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.8) +project(gc_slam_toolbox_fish) + +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) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) +install( + DIRECTORY config launch rviz + DESTINATION share/${PROJECT_NAME} +) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/gc_slam_toolbox_fish/config/gc_2d.lua b/src/gc_slam_toolbox_fish/config/gc_2d.lua new file mode 100644 index 0000000..329a2ee --- /dev/null +++ b/src/gc_slam_toolbox_fish/config/gc_2d.lua @@ -0,0 +1,63 @@ +include "map_builder.lua" +include "trajectory_builder.lua" + +options = { + map_builder = MAP_BUILDER, + trajectory_builder = TRAJECTORY_BUILDER, + map_frame = "map", + tracking_frame = "base_link", + -- base_link改为odom,发布map到odom之间的位姿态 + published_frame = "odom", + odom_frame = "odom", + -- true改为false，不用提供里程计数据 + provide_odom_frame = false, + -- false改为true，仅发布2D位资 + publish_frame_projected_to_2d = true, + -- false改为true，使用里程计数据 + use_odometry = true, + use_nav_sat = false, + use_landmarks = false, + -- 0改为1,使用一个雷达 + num_laser_scans = 1, + -- 1改为0，不使用多波雷达 + num_multi_echo_laser_scans = 0, + -- 10改为1，1/1=1等于不分割 + num_subdivisions_per_laser_scan = 1, + num_point_clouds = 0, + lookup_transform_timeout_sec = 0.2, + submap_publish_period_sec = 0.3, + pose_publish_period_sec = 5e-3, + trajectory_publish_period_sec = 30e-3, + rangefinder_sampling_ratio = 1., + odometry_sampling_ratio = 1., + fixed_frame_pose_sampling_ratio = 1., + imu_sampling_ratio = 1., + landmarks_sampling_ratio = 1., +} + + +-- false改为true，启动2D SLAM +MAP_BUILDER.use_trajectory_builder_2d = true + +-- 0改成0.10,比机器人半径小的都忽略 +TRAJECTORY_BUILDER_2D.min_range = 0.10 +-- 30改成3.5,限制在雷达最大扫描范围内，越小一般越精确些 +TRAJECTORY_BUILDER_2D.max_range = 3.5 +-- 5改成3,传感器数据超出有效范围最大值 +TRAJECTORY_BUILDER_2D.missing_data_ray_length = 3. +-- true改成false,不使用IMU数据，大家可以开启，然后对比下效果 +TRAJECTORY_BUILDER_2D.use_imu_data = false +-- false改成true,使用实时回环检测来进行前端的扫描匹配 +TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true +-- 1.0改成0.1,提高对运动的敏感度 +TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1) + +-- 0.55改成0.65,Fast csm的最低分数，高于此分数才进行优化。 +POSE_GRAPH.constraint_builder.min_score = 0.65 +--0.6改成0.7,全局定位最小分数，低于此分数则认为目前全局定位不准确 +POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7 + +-- 设置0可关闭全局SLAM +-- POSE_GRAPH.optimize_every_n_nodes = 0 + +return options diff --git a/src/gc_slam_toolbox_fish/launch/catograph.launch.py b/src/gc_slam_toolbox_fish/launch/catograph.launch.py new file mode 100644 index 0000000..ff51eec --- /dev/null +++ b/src/gc_slam_toolbox_fish/launch/catograph.launch.py @@ -0,0 +1,63 @@ + +import os +from launch import LaunchDescription +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + # 定位到功能包的地址 + pkg_share = FindPackageShare( + package='gc_slam_toolbox_fish').find('gc_slam_toolbox_fish') + + # =====================运行节点需要的配置======================================================================= + # 是否使用仿真时间，我们用gazebo，这里设置成true + use_sim_time = LaunchConfiguration('use_sim_time', default='true') + # 地图的分辨率 + resolution = LaunchConfiguration('resolution', default='0.05') + # 地图的发布周期 + publish_period_sec = LaunchConfiguration( + 'publish_period_sec', default='1.0') + # 配置文件夹路径 + configuration_directory = LaunchConfiguration( + 'configuration_directory', default=os.path.join(pkg_share, 'config')) + # 配置文件 + configuration_basename = LaunchConfiguration( + 'configuration_basename', default='gc_2d.lua') + rviz_config_dir = os.path.join(pkg_share, 'config')+"/cartographer.rviz" + print(f"rviz config in {rviz_config_dir}") + + # =====================声明三个节点，cartographer/occupancy_grid_node/rviz_node================================= + cartographer_node = Node( + package='cartographer_ros', + executable='cartographer_node', + name='cartographer_node', + output='screen', + parameters=[{'use_sim_time': use_sim_time}], + arguments=['-configuration_directory', configuration_directory, + '-configuration_basename', configuration_basename]) + + cartographer_occupancy_grid_node = Node( + package='cartographer_ros', + executable='cartographer_occupancy_grid_node', + name='cartographer_occupancy_grid_node', + output='screen', + parameters=[{'use_sim_time': use_sim_time}], + arguments=['-resolution', resolution, '-publish_period_sec', publish_period_sec]) + + rviz_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', rviz_config_dir], + parameters=[{'use_sim_time': use_sim_time}], + output='screen') + + # ===============================================定义启动文件======================================================== + ld = LaunchDescription() + ld.add_action(cartographer_node) + ld.add_action(cartographer_occupancy_grid_node) + # ld.add_action(rviz_node) + + return ld diff --git a/src/gc_slam_toolbox_fish/package.xml b/src/gc_slam_toolbox_fish/package.xml new file mode 100644 index 0000000..81a27f4 --- /dev/null +++ b/src/gc_slam_toolbox_fish/package.xml @@ -0,0 +1,18 @@ + + + + gc_slam_toolbox_fish + 0.0.0 + TODO: Package description + guoch + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/origincar_base/CMakeLists.txt b/src/origincar_base/CMakeLists.txt new file mode 100644 index 0000000..e02128e --- /dev/null +++ b/src/origincar_base/CMakeLists.txt @@ -0,0 +1,89 @@ +cmake_minimum_required(VERSION 3.5) +project(origincar_base) + +# 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(actionlib REQUIRED) +#find_package(actionlib_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) + +find_package(nav2_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclpy REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(std_srvs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(ackermann_msgs REQUIRED) # CHANGE +find_package(origincar_msg REQUIRED) # CHANGE +find_package(serial REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(origincar_description REQUIRED) +find_package(robot_localization REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/Position.msg" + ) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +include_directories( + include + ${robot_localization_INCLUDE_DIRS} +) +set(origincar_base_node_SRCS + src/origincar_base.cpp + src/Quaternion_Solution.cpp +) + +add_executable(origincar_base_node src/origincar_base.cpp src/Quaternion_Solution.cpp) +ament_target_dependencies(origincar_base_node tf2_ros tf2 tf2_geometry_msgs rclcpp std_msgs geometry_msgs robot_localization nav_msgs std_srvs sensor_msgs ackermann_msgs serial origincar_msg origincar_description) + +#add_executable(testNode src/test.cpp src/Quaternion_Solution.cpp) +#ament_target_dependencies(testNode rclcpp std_msgs nav_msgs std_srvs sensor_msgs ackermann_msgs serial origincar_msg) + +install(PROGRAMS scripts/cmd_vel_to_ackermann_drive.py DESTINATION lib/${PROJECT_NAME}) + +install(TARGETS +origincar_base_node + +DESTINATION lib/${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install( + DIRECTORY launch config + DESTINATION share/${PROJECT_NAME} +) +ament_package() diff --git a/src/origincar_base/config/ekf.yaml b/src/origincar_base/config/ekf.yaml new file mode 100644 index 0000000..4c15af1 --- /dev/null +++ b/src/origincar_base/config/ekf.yaml @@ -0,0 +1,222 @@ +### ekf config file ### +ekf_filter_node: + ros__parameters: +# ͨóʼλ + use_pose_with_covariance_stamped: true # ͨ + pose0: /set_pose # ָ + pose0_config: [true, true, false, # ʹ x, y + false, false, true] # ʹ yaw + pose0_differential: false +# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin +# computation until it receives at least one message from one of the inputs. It will then run continuously at the +# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified. + frequency: 30.0 + +# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict +# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the +# filter will generate new output. Defaults to 1 / frequency if not specified. + sensor_timeout: 2.0 + +# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is +# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar +# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected +# by, for example, an IMU. Defaults to false if unspecified. + two_d_mode: true + +# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for +# future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if +# unspecified. + transform_time_offset: 0.0 + +# Use this parameter to provide specify how long the tf listener should wait for a transform to become available. +# Defaults to 0.0 if unspecified. + transform_timeout: 0.2 + +# If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is +# unhappy with any settings or data. + print_diagnostics: false + +# Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by +# debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious +# effects on the performance of the node. Defaults to false if unspecified. + debug: false + +# Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path. + debug_out_file: /path/to/debug/file.txt + +# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified. + publish_tf: true + +# Whether to publish the acceleration state. Defaults to false if unspecified. + publish_acceleration: false + +# REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and +# earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames. +# The robot's position in the odom frame will drift over time, but is accurate in the short term and should be +# continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom +# frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your +# robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based +# localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame. +# ekf_localization_node and ukf_localization_node are not concerned with the earth frame. +# Here is how to use the following settings: +# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system. +# 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of +# odom_frame. +# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set +# "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes. +# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates +# from landmark observations) then: +# 3a. Set your "world_frame" to your map_frame value +# 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state +# estimation node from robot_localization! However, that instance should *not* fuse the global data. + map_frame: map # Defaults to "map" if unspecified + odom_frame: odom_combined # Defaults to "odom" if unspecified + base_link_frame: base_footprint # Defaults to "base_link" if unspecified + world_frame: odom_combined # Defaults to the value of odom_frame if unspecified + +# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry, +# geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped, +# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0, +# odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no +# default values, and must be specified. + odom0: odom +# Each sensor reading updates some or all of the filter's state. These options give you greater control over which +# values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only +# want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the +# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types +# do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message +# has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false +# if unspecified, effectively making this parameter required for each sensor. + odom0_config: [true, false, false, + false, false, false, + true, true, false, + false, false, true, + false, false, false] + +# If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase +# the size of the subscription queue so that more measurements are fused. + odom0_queue_size: 10 + +# [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result +# of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's +# algorithm. + odom0_nodelay: false + +# [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under- +# report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they +# arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also +# measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't +# always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose +# data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then +# integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true +# for twist measurements has no effect. + odom0_differential: true + +# [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point" +# for all future measurements. While you can achieve the same effect with the differential paremeter, the key +# difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before +# integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true. + odom0_relative: false + +# [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to +# control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to +# numeric_limits::max() if unspecified. It is strongly recommended that these parameters be removed if not +# required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation. +# For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying +# the thresholds. +# odom0_pose_rejection_threshold: 5.0 +# odom0_twist_rejection_threshold: 1.0 + + imu0: /imu/data_raw + imu0_config: [false, false, false, + false, false, true, + false, false, false, + false, false, true, + false, false, false] + + imu0_nodelay: false + imu0_differential: false + imu0_relative: true + imu0_queue_size: 10 + imu0_pose_rejection_threshold: 20.0 # Note the difference in parameter names + imu0_twist_rejection_threshold: 1.542 # + imu0_linear_acceleration_rejection_threshold: 10.0 # + +# [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set +# this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame. + imu0_remove_gravitational_acceleration: true + +# [ADVANCED] The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no +# acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During +# correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be +# problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When +# this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially +# noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance +# for the velocity variable in question, or decrease the variance of the variable in question in the measurement +# itself. In addition, users can also take advantage of the control command being issued to the robot at the time we +# make the prediction. If control is used, it will get converted into an acceleration term, which will be used during +# predicition. Note that if an acceleration measurement for the variable in question is available from one of the +# inputs, the control term will be ignored. +# Whether or not we use the control input during predicition. Defaults to false. + use_control: false +# Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to +# false. + stamped_control: false +# The last issued control command will be used in prediction for this period. Defaults to 0.2. + control_timeout: 0.2 +# Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw. + control_config: [true, false, false, false, false, true] +# Places limits on how large the acceleration term will be. Should match your robot's kinematics. + acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4] +# Acceleration and deceleration limits are not always the same for robots. + deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5] +# If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these +# gains + acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9] +# If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these +# gains + deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0] +# [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is +# exposed as a configuration parameter. This matrix represents the noise we add to the total error after each +# prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be. +# However, if users find that a given variable is slow to converge, one approach is to increase the +# process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error +# to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are +# ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if +# unspecified. + process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015] +# [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal +# value (variance) to a large value will result in rapid convergence for initial measurements of the variable in +# question. Users should take care not to use large values for variables that will not be measured directly. The values +# are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below +#if unspecified. + initial_estimate_covariance: [1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9] + diff --git a/src/origincar_base/config/imu.yaml b/src/origincar_base/config/imu.yaml new file mode 100644 index 0000000..edbf7de --- /dev/null +++ b/src/origincar_base/config/imu.yaml @@ -0,0 +1,7 @@ +imu_filter_madgwick: + ros__parameters: + fixed_frame: "base_footprint" + use_mag: false + publish_tf: false + world_frame: "enu" + orientation_stddev: 0.05 diff --git a/src/origincar_base/include/origincar_base/Quaternion_Solution.h b/src/origincar_base/include/origincar_base/Quaternion_Solution.h new file mode 100644 index 0000000..b839cec --- /dev/null +++ b/src/origincar_base/include/origincar_base/Quaternion_Solution.h @@ -0,0 +1,10 @@ + +#ifndef __QUATERNION_SOLUTION_H_ +#define __QUATERNION_SOLUTION_H_ +#include "origincar_base.h" +float InvSqrt(float number); +void Quaternion_Solution(float gx, float gy, float gz, float ax, float ay, float az); +extern volatile float q0, q1, q2, q3; +#endif + + diff --git a/src/origincar_base/include/origincar_base/origincar_base.h b/src/origincar_base/include/origincar_base/origincar_base.h new file mode 100644 index 0000000..456ce18 --- /dev/null +++ b/src/origincar_base/include/origincar_base/origincar_base.h @@ -0,0 +1,228 @@ + +#ifndef _ORIGINCAR_BASE_H_ +#define _ORIGINCAR_BASE_H_ + +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/LinearMath/Transform.h" +#include "tf2/LinearMath/Quaternion.h" +#include +#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" +#include "origincar_msg/msg/data.hpp" +#include "origincar_msg/msg/sign.hpp" // 匹配信号发送 +#include +#include +using namespace std; + + +#define SEND_DATA_CHECK 1 //Send data check flag bits //发送数据校验标志位 +#define READ_DATA_CHECK 0 //Receive data to check flag bits //接收数据校验标志位 +#define FRAME_HEADER 0X7B //Frame head //帧头 +#define FRAME_TAIL 0X7D //Frame tail //帧尾 +#define RECEIVE_DATA_SIZE 24 //The length of the data sent by the lower computer //下位机发送过来的数据的长度 +#define SEND_DATA_SIZE 11 //The length of data sent by ROS to the lower machine //ROS向下位机发送的数据的长度 +#define PI 3.1415926f //PI //圆周率 + +#define GYROSCOPE_RATIO 0.00026644f + +#define ACCEl_RATIO 1671.84f + +extern sensor_msgs::msg::Imu Mpu6050; + +const double odom_pose_covariance[36] = {1e-3, 0, 0, 0, 0, 0, + 0, 1e-3, 0, 0, 0, 0, + 0, 0, 1e6, 0, 0, 0, + 0, 0, 0, 1e6, 0, 0, + 0, 0, 0, 0, 1e6, 0, + 0, 0, 0, 0, 0, 1e3 }; + +const double odom_pose_covariance2[36] = {1e-9, 0, 0, 0, 0, 0, + 0, 1e-3, 1e-9, 0, 0, 0, + 0, 0, 1e6, 0, 0, 0, + 0, 0, 0, 1e6, 0, 0, + 0, 0, 0, 0, 1e6, 0, + 0, 0, 0, 0, 0, 1e-9 }; + +const double odom_twist_covariance[36] = {1e-3, 0, 0, 0, 0, 0, + 0, 1e-3, 0, 0, 0, 0, + 0, 0, 1e6, 0, 0, 0, + 0, 0, 0, 1e6, 0, 0, + 0, 0, 0, 0, 1e6, 0, + 0, 0, 0, 0, 0, 1e3 }; + +const double odom_twist_covariance2[36] = {1e-9, 0, 0, 0, 0, 0, + 0, 1e-3, 1e-9, 0, 0, 0, + 0, 0, 1e6, 0, 0, 0, + 0, 0, 0, 1e6, 0, 0, + 0, 0, 0, 0, 1e6, 0, + 0, 0, 0, 0, 0, 1e-9} ; + +typedef struct __Vel_Pos_Data_ +{ + float X; + float Y; + float Z; + +} Vel_Pos_Data; + +typedef struct __MPU6050_DATA_ +{ + short accele_x_data; + short accele_y_data; + short accele_z_data; + short gyros_x_data; + short gyros_y_data; + short gyros_z_data; + +} MPU6050_DATA; + +typedef struct _SEND_DATA_ +{ + uint8_t tx[SEND_DATA_SIZE]; + float X_speed; + float Y_speed; + float Z_speed; + unsigned char Frame_Tail; +} SEND_DATA; + +typedef struct _RECEIVE_DATA_ +{ + uint8_t rx[RECEIVE_DATA_SIZE]; + uint8_t Flag_Stop; + unsigned char Frame_Header; + float X_speed; + float Y_speed; + float Z_speed; + float Power_Voltage; + unsigned char Frame_Tail; +} RECEIVE_DATA; + +class origincar_base : public rclcpp::Node + +{ +public: + origincar_base(); + ~origincar_base(); + void Control(); + void Publish_Odom(); + +public : + serial::Serial Stm32_Serial; + +private: + void declare_parameters(); + void get_parameters(); + + + void Cmd_Vel_Callback(const geometry_msgs::msg::Twist::SharedPtr twist_aux); + void Akm_Cmd_Vel_Callback(const ackermann_msgs::msg::AckermannDriveStamped::SharedPtr akm_ctl); + + void Publish_ImuSensor(); + void Publish_Voltage(); + auto createQuaternionMsgFromYaw(double yaw); + + bool Get_Sensor_Data(); + unsigned char Check_Sum(unsigned char Count_Number,unsigned char mode); + short IMU_Trans(uint8_t Data_High,uint8_t Data_Low); + float Odom_Trans(uint8_t Data_High,uint8_t Data_Low); + + void Sign_Switch_Callback(const std_msgs::msg::Int32::SharedPtr sign_switch); + +private: + void reset_filter() + { + auto msg = std::make_shared(); + + // 设置头信息 + msg->header.stamp = this->now(); + msg->header.frame_id = "odom_combined"; + + // 重置位姿 + msg->pose.pose.position.x = 0.54; + msg->pose.pose.position.y = 0.2; + msg->pose.pose.orientation.w = 1.0; + + // 设置关键协方差 + std::array cov = {0}; + cov[0] = 0.5; // x + cov[7] = 0.5; // y + cov[35] = 0.5; // yaw + msg->pose.covariance = cov; + + // 发布重置指令 + pose_pub_->publish(*msg); + RCLCPP_WARN(this->get_logger(), "FILTER RESET TRIGGERED!"); + } + + rclcpp::Publisher::SharedPtr pose_pub_; + rclcpp::Time _Now, _Last_Time; + float Sampling_Time; + rclcpp::Subscription::SharedPtr Cmd_Vel_Sub; + rclcpp::Subscription::SharedPtr Akm_Cmd_Vel_Sub; + + rclcpp::Publisher::SharedPtr odom_publisher; + rclcpp::Publisher::SharedPtr voltage_publisher; + rclcpp::Publisher::SharedPtr imu_publisher; + + rclcpp::Publisher::SharedPtr test_publisher; + + rclcpp::Publisher::SharedPtr robotpose_publisher; + rclcpp::Publisher::SharedPtr robotvel_publisher; + // rclcpp::Publisher::SharedPtr tf_pub_; + + std::shared_ptr tf_bro; + rclcpp::Publisher::SharedPtr tf_pub_; + rclcpp::TimerBase::SharedPtr test_timer; + + rclcpp::TimerBase::SharedPtr odom_timer; + rclcpp::TimerBase::SharedPtr imu_timer; + rclcpp::TimerBase::SharedPtr voltage_timer; + + rclcpp::TimerBase::SharedPtr robotpose_timer; + rclcpp::TimerBase::SharedPtr robotvel_timer; + + std::unique_ptr tf_broadcaster_; + + rclcpp::Subscription::SharedPtr Sign_Switch_Sub; + + string usart_port_name, robot_frame_id, gyro_frame_id, odom_frame_id, akm_cmd_vel, test; + std::string cmd_vel; + int serial_baud_rate; + RECEIVE_DATA Receive_Data; + SEND_DATA Send_Data; + + Vel_Pos_Data Robot_Pos; + Vel_Pos_Data Robot_Vel; + MPU6050_DATA Mpu6050_Data; + float Power_voltage; + size_t count_; +}; + + +#endif //_ORIGINCAR_BASE_H_ diff --git a/src/origincar_base/launch/__pycache__/testtwo.launch.cpython-38.pyc b/src/origincar_base/launch/__pycache__/testtwo.launch.cpython-38.pyc new file mode 100644 index 0000000..5150cce Binary files /dev/null and b/src/origincar_base/launch/__pycache__/testtwo.launch.cpython-38.pyc differ diff --git a/src/origincar_base/launch/base_serial.launch.py b/src/origincar_base/launch/base_serial.launch.py new file mode 100644 index 0000000..848e78d --- /dev/null +++ b/src/origincar_base/launch/base_serial.launch.py @@ -0,0 +1,47 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.conditions import IfCondition, UnlessCondition +import launch_ros.actions + +def generate_launch_description(): + akmcar = LaunchConfiguration('akmcar', default='false') + + robot_parameters = [ + {'usart_port_name': '/dev/ttyACM0', + 'serial_baud_rate': 115200, + 'robot_frame_id': 'base_link', + 'odom_frame_id': 'odom', + 'cmd_vel': 'cmd_vel', + 'product_number': 0} + ] + + return LaunchDescription([ + DeclareLaunchArgument( + 'akmcar', + default_value='false', + description='Use simulation (Gazebo) clock if true' + ), + + launch_ros.actions.Node( + condition=IfCondition(akmcar), + package='origincar_base', + executable='origincar_base_node', + parameters=robot_parameters + [{'akm_cmd_vel': 'ackermann_cmd'}], + remappings=[('/cmd_vel', 'cmd_vel')], + ), + + launch_ros.actions.Node( + condition=IfCondition(akmcar), + package='origincar_base', + executable='cmd_vel_to_ackermann_drive.py', + name='cmd_vel_to_ackermann_drive', + ), + + launch_ros.actions.Node( + condition=UnlessCondition(akmcar), + package='origincar_base', + executable='origincar_base_node', + parameters=robot_parameters + [{'akm_cmd_vel': 'none'}], + ) + ]) diff --git a/src/origincar_base/launch/ekf.launch.py b/src/origincar_base/launch/ekf.launch.py new file mode 100644 index 0000000..51713ba --- /dev/null +++ b/src/origincar_base/launch/ekf.launch.py @@ -0,0 +1,33 @@ +#!/usr/bin/python3 + +import os +import launch +import launch_ros +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + + package_name = 'origincar_base' + ld = launch.LaunchDescription() + pkg_share = launch_ros.substitutions.FindPackageShare(package=package_name).find(package_name) + robot_localization_file_path = os.path.join(pkg_share, 'config/ekf.yaml') + use_sim_time = LaunchConfiguration('use_sim_time') + + # Start robot localization using an Extended Kalman filter + robot_localization_node = Node( + package='robot_localization', + executable='ekf_node', + name='ekf_filter_node', + output='screen', + remappings=[("odometry/filtered", "odom_combined")], + parameters=[ + robot_localization_file_path, + {'use_sim_time': use_sim_time} + ]) + + ld.add_action(launch.actions.DeclareLaunchArgument(name='use_sim_time', default_value='false', + description='Flag to disable use_sim_time')) + ld.add_action(robot_localization_node) + return ld diff --git a/src/origincar_base/launch/origincar_bringup.launch.py b/src/origincar_base/launch/origincar_bringup.launch.py new file mode 100644 index 0000000..3d9fb0d --- /dev/null +++ b/src/origincar_base/launch/origincar_bringup.launch.py @@ -0,0 +1,90 @@ +import os +from pathlib import Path +import launch +from launch.actions import SetEnvironmentVariable +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import (DeclareLaunchArgument, GroupAction, + IncludeLaunchDescription, SetEnvironmentVariable) +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import PushRosNamespace +import launch_ros.actions +from launch.conditions import UnlessCondition + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory('origincar_base') + launch_dir = os.path.join(bringup_dir, 'launch') + ekf_config = Path(get_package_share_directory('origincar_base'), 'config', 'ekf.yaml') + imu_config = Path(get_package_share_directory('origincar_base'), 'config', 'imu.yaml') + + + carto_slam = LaunchConfiguration('carto_slam', default='false') + carto_slam_dec = DeclareLaunchArgument('carto_slam',default_value='false') + + origincar_base = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, 'base_serial.launch.py')), + launch_arguments={'akmcar': 'false'}.items(), + ) + + choose_car = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, 'robot_mode_description.launch.py')), + ) + + + base_to_link = launch_ros.actions.Node( + package='tf2_ros', + executable='static_transform_publisher', + name='base_to_link', + arguments=['0', '0', '0','0', '0','0','base_footprint','base_link'],#arguments=['0.41', '0.12', '0','0', '0','0','base_footprint','base_link'], + ) + + + base_to_gyro = launch_ros.actions.Node( + package='tf2_ros', + executable='static_transform_publisher', + name='base_to_gyro', + arguments=['0', '0', '0','0', '0','0','base_footprint','gyro_link'], + ) + + link_to_laser = launch_ros.actions.Node( + package='tf2_ros', + executable='static_transform_publisher', + name='link_to_laser', + arguments=['0', '0', '0','0', '0','0','base_link','laser'], + ) + + imu_filter_node = launch_ros.actions.Node( + package='imu_filter_madgwick', + executable='imu_filter_madgwick_node', + parameters=[imu_config] + ) + + robot_ekf = launch_ros.actions.Node( + condition=UnlessCondition(carto_slam), + package='robot_localization', + executable='ekf_node', + parameters=[ekf_config], + remappings=[("odometry/filtered", "odom_combined")] + ) + + joint_state_publisher_node = launch_ros.actions.Node( + package='joint_state_publisher', + executable='joint_state_publisher', + name='joint_state_publisher', + ) + + ld = LaunchDescription() + + ld.add_action(carto_slam_dec) + ld.add_action(origincar_base) + ld.add_action(base_to_link) + ld.add_action(base_to_gyro) + ld.add_action(joint_state_publisher_node) + ld.add_action(choose_car) + ld.add_action(imu_filter_node) + ld.add_action(robot_ekf) + + return ld + diff --git a/src/origincar_base/launch/robot_mode_description.launch.py b/src/origincar_base/launch/robot_mode_description.launch.py new file mode 100644 index 0000000..4bce9d9 --- /dev/null +++ b/src/origincar_base/launch/robot_mode_description.launch.py @@ -0,0 +1,24 @@ +import os +from pathlib import Path +import launch_ros.actions +import launch +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import (DeclareLaunchArgument, GroupAction,LogInfo, + IncludeLaunchDescription, SetEnvironmentVariable) +from launch.substitutions import LaunchConfiguration + +def generate_launch_description(): + origincar_description = GroupAction([ + launch_ros.actions.Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + arguments=[os.path.join(get_package_share_directory('origincar_description'),'urdf','origincar.urdf')] + ) + ]) + + ld = LaunchDescription() + + ld.add_action(origincar_description) + return ld \ No newline at end of file diff --git a/src/origincar_base/msg/Position.msg b/src/origincar_base/msg/Position.msg new file mode 100644 index 0000000..2fe2697 --- /dev/null +++ b/src/origincar_base/msg/Position.msg @@ -0,0 +1,3 @@ +float32 angle_x +float32 angle_y +float32 distance diff --git a/src/origincar_base/package.xml b/src/origincar_base/package.xml new file mode 100644 index 0000000..5e569f3 --- /dev/null +++ b/src/origincar_base/package.xml @@ -0,0 +1,44 @@ + + + + origincar_base + 0.0.0 + ROS2 origincar_base for origincar_base + ps-micro + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + tf2_geometry_msgs + + + rosidl_default_generators + rosidl_interface_packages + tf2_geometry_msgs + + rosidl_default_runtime + ament_index_cpp + geometry_msgs + rclcpp + rclcpp_action + sensor_msgs + std_msgs + std_srvs + tf2 + tf2_ros + rclpy + serial + nav2_msgs + nav_msgs + ackermann_msgs + origincar_msg + origincar_description + robot_localization + + boost + + ament_cmake + + diff --git a/src/origincar_base/scripts/cmd_vel_to_ackermann_drive.py b/src/origincar_base/scripts/cmd_vel_to_ackermann_drive.py new file mode 100644 index 0000000..bae5266 --- /dev/null +++ b/src/origincar_base/scripts/cmd_vel_to_ackermann_drive.py @@ -0,0 +1,47 @@ +#!/usr/bin/env python3 + +import math +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Twist +from ackermann_msgs.msg import AckermannDriveStamped +from rclpy.qos import QoSProfile + +class CmdVel2AckermannDriveNode(Node): + def __init__(self): + super().__init__('cmd_vel_to_ackermann_drive') + self.publisher = self.create_publisher(AckermannDriveStamped, '/ackermann_cmd', QoSProfile(depth=10)) + self.subscription = self.create_subscription(Twist, 'cmd_vel', self.cmd_callback, QoSProfile(depth=10)) + self.wheelbase = 0.143 #轮距 + self.frame_id = 'odom_combined' + self.cmd_angle_instead_rotvel = False + + def convert_trans_rot_vel_to_steering_angle(self, vel, omega): + if omega == 0 or vel == 0: + return 0 + radius = vel / omega + return math.atan(self.wheelbase / radius) + + def cmd_callback(self, data): + vel = data.linear.x + if self.cmd_angle_instead_rotvel: + steering = data.angular.z + else: + steering = 1.0 * self.convert_trans_rot_vel_to_steering_angle(vel, data.angular.z) + + msg = AckermannDriveStamped() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = self.frame_id + msg.drive.steering_angle = steering + msg.drive.speed = vel + self.publisher.publish(msg) + +def main(): + rclpy.init() + node = CmdVel2AckermannDriveNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/src/origincar_base/src/Quaternion_Solution.cpp b/src/origincar_base/src/Quaternion_Solution.cpp new file mode 100644 index 0000000..56cf830 --- /dev/null +++ b/src/origincar_base/src/Quaternion_Solution.cpp @@ -0,0 +1,91 @@ + +#include "origincar_base/Quaternion_Solution.h" +#define SAMPLING_FREQ 20.0f // 采样频率 +/************************************** +Date: May 31, 2020 +Function: 平方根倒数 求四元数用到 +***************************************/ +float InvSqrt(float number) +{ + volatile long i; + volatile float x, y; + volatile const float f = 1.5F; + x = number * 0.5F; + y = number; + i = * (( long * ) &y); + i = 0x5f375a86 - ( i >> 1 ); + y = * (( float * ) &i); + y = y * ( f - ( x * y * y ) ); + + return y; +} +/************************************** +Date: May 31, 2020 +Function: 四元数解算 +***************************************/ +volatile float twoKp = 1.0f; // 2 * proportional gain (Kp) +volatile float twoKi = 0.0f; // 2 * integral gain (Ki) +volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame +volatile float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki +void Quaternion_Solution(float gx, float gy, float gz, float ax, float ay, float az) +{ + float recipNorm; + float halfvx, halfvy, halfvz; + float halfex, halfey, halfez; + float qa, qb, qc; + // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) + if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { + // 首先把加速度计采集到的值(三维向量)转化为单位向量，即向量除以模 + recipNorm = InvSqrt(ax * ax + ay * ay + az * az); + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; + // 把四元数换算成方向余弦中的第三行的三个元素 + halfvx = q1 * q3 - q0 * q2; + halfvy = q0 * q1 + q2 * q3; + halfvz = q0 * q0 - 0.5f + q3 * q3; + //误差是估计的重力方向和测量的重力方向的交叉乘积之和 + halfex = (ay * halfvz - az * halfvy); + halfey = (az * halfvx - ax * halfvz); + halfez = (ax * halfvy - ay * halfvx); + // 计算并应用积分反馈（如果启用） + if(twoKi > 0.0f) { + integralFBx += twoKi * halfex * (1.0f / SAMPLING_FREQ); // integral error scaled by Ki + integralFBy += twoKi * halfey * (1.0f / SAMPLING_FREQ); + integralFBz += twoKi * halfez * (1.0f / SAMPLING_FREQ); + gx += integralFBx; // apply integral feedback + gy += integralFBy; + gz += integralFBz; + } + else { + integralFBx = 0.0f; // prevent integral windup + integralFBy = 0.0f; + integralFBz = 0.0f; + } + // Apply proportional feedback + gx += twoKp * halfex; + gy += twoKp * halfey; + gz += twoKp * halfez; + } + // Integrate rate of change of quaternion + gx *= (0.5f * (1.0f / SAMPLING_FREQ)); // pre-multiply common factors + gy *= (0.5f * (1.0f / SAMPLING_FREQ)); + gz *= (0.5f * (1.0f / SAMPLING_FREQ)); + qa = q0; + qb = q1; + qc = q2; + q0 += (-qb * gx - qc * gy - q3 * gz); + q1 += (qa * gx + qc * gz - q3 * gy); + q2 += (qa * gy - qb * gz + q3 * gx); + q3 += (qa * gz + qb * gy - qc * gx); + // Normalise quaternion + recipNorm = InvSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; + Mpu6050.orientation.w = q0; + Mpu6050.orientation.x = q1; + Mpu6050.orientation.y = q2; + Mpu6050.orientation.z = q3; +} diff --git a/src/origincar_base/src/origincar_base.cpp b/src/origincar_base/src/origincar_base.cpp new file mode 100644 index 0000000..4a39723 --- /dev/null +++ b/src/origincar_base/src/origincar_base.cpp @@ -0,0 +1,469 @@ +#include "origincar_base/origincar_base.h" +#include "rclcpp/rclcpp.hpp" +#include "origincar_base/Quaternion_Solution.h" +#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" +#include "origincar_msg/msg/data.hpp" +#include "robot_localization/srv/set_pose.hpp" +#include + +using std::placeholders::_1; +using namespace std; +void sigintHandler(int sig); +sensor_msgs::msg::Imu Mpu6050; +rclcpp::Node::SharedPtr node_handle = nullptr; +int Init_imu_num = 0; +float gyro_z_sum=0; + +int main(int argc, char *argv[]) +{ + rclcpp::init(argc, argv); + signal(SIGINT, sigintHandler); + origincar_base Robot_Control; + Robot_Control.Control(); + rclcpp::shutdown(); + return 0; +} + + + + +short origincar_base::IMU_Trans(uint8_t Data_High,uint8_t Data_Low) +{ + short transition_16; + transition_16 = 0; + transition_16 |= Data_High<<8; + transition_16 |= Data_Low; + return transition_16; +} + +float origincar_base::Odom_Trans(uint8_t Data_High,uint8_t Data_Low) +{ + float data_return; + short transition_16; + transition_16 = 0; + transition_16 |= Data_High<<8; + transition_16 |= Data_Low; + data_return = (transition_16 / 1000)+(transition_16 % 1000)*0.001; + return data_return; + } + +void origincar_base::Akm_Cmd_Vel_Callback(const ackermann_msgs::msg::AckermannDriveStamped::SharedPtr akm_ctl) +{ + short transition; + std::cout<<"linerx"<drive.speed<drive.steering_angle<drive.speed*1000; + Send_Data.tx[4] = transition; + Send_Data.tx[3] = transition>>8; + + transition=0; + transition = akm_ctl->drive.steering_angle*1000/2; + Send_Data.tx[8] = transition; + Send_Data.tx[7] = transition>>8; + + Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK); + Send_Data.tx[10]=FRAME_TAIL; + + try { + Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx)); + } catch (serial::IOException& e) { + RCLCPP_ERROR(this->get_logger(),("Unable to send data through serial port")); + } +} + +void origincar_base::Cmd_Vel_Callback(const geometry_msgs::msg::Twist::SharedPtr twist_aux) +{ + RCLCPP_INFO(this->get_logger(),"linarx: %.2f, angularz: %.2f ", twist_aux->linear.x, twist_aux->angular.z); + std::cout<<"linerx"<linear.x<angular.z<linear.x*1000; + Send_Data.tx[4] = transition; + Send_Data.tx[3] = transition>>8; + + transition=0; + transition = twist_aux->linear.y*1000; + Send_Data.tx[6] = transition; + Send_Data.tx[5] = transition>>8; + + transition=0; + transition = (twist_aux->angular.z)*1000; + Send_Data.tx[8] = transition; + Send_Data.tx[7] = transition>>8; + + Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK); + Send_Data.tx[10]=FRAME_TAIL; + + try + { + Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx)); + } catch (serial::IOException& e) { + RCLCPP_ERROR(this->get_logger(),("Unable to send data through serial port")); + } +} + +void origincar_base::Sign_Switch_Callback(const std_msgs::msg::Int32::SharedPtr sign_switch) +{ + (void)sign_switch; + if (sign_switch->data == -1) { + memset(&Robot_Pos, 0, sizeof(Robot_Pos)); + memset(&Mpu6050, 0, sizeof(Mpu6050)); + q0 = 0; + q1 = 0; + q2 = 0; + q3 = 0; + Robot_Pos.X = 0.54; + Robot_Pos.Y = 0.2; + Robot_Pos.Z = 0; + Robot_Vel.X = 0; + Robot_Vel.Y = 0; + Robot_Vel.Z = 0; + memset(&Robot_Vel, 0, sizeof(Robot_Vel)); + reset_filter(); + } + else if(sign_switch->data == -2) { + memset(&Robot_Pos, 0, sizeof(Robot_Pos)); + Robot_Pos.X = 2.54; + Robot_Pos.Y = 2.5; + Robot_Pos.Z = -1.5708; + Robot_Vel.X = 0; + Robot_Vel.Y = 0; + Robot_Vel.Z = 0; + memset(&Robot_Vel, 0, sizeof(Robot_Vel)); + reset_filter(); + } + /* + else if (sign_switch->data == 6) { + memset(&Robot_Pos, 0, sizeof(Robot_Pos)); + Robot_Pos.X = 2; + Robot_Pos.Y = 2; + memset(&Robot_Vel, 0, sizeof(Robot_Vel)); + }*/ + +} + +void origincar_base::Publish_ImuSensor() +{ + sensor_msgs::msg::Imu Imu_Data_Pub; + Imu_Data_Pub.header.stamp = rclcpp::Node::now(); + Imu_Data_Pub.header.frame_id = gyro_frame_id; + + Imu_Data_Pub.orientation.x = Mpu6050.orientation.x; + Imu_Data_Pub.orientation.y = Mpu6050.orientation.y; + Imu_Data_Pub.orientation.z = Mpu6050.orientation.z; + Imu_Data_Pub.orientation.w = Mpu6050.orientation.w; + Imu_Data_Pub.orientation_covariance[0] = 1e6; + Imu_Data_Pub.orientation_covariance[4] = 1e6; + Imu_Data_Pub.orientation_covariance[8] = 1e-6; + Imu_Data_Pub.angular_velocity.x = Mpu6050.angular_velocity.x; + Imu_Data_Pub.angular_velocity.y = Mpu6050.angular_velocity.y; + Imu_Data_Pub.angular_velocity.z = Mpu6050.angular_velocity.z; + Imu_Data_Pub.angular_velocity_covariance[0] = 1e6; + Imu_Data_Pub.angular_velocity_covariance[4] = 1e6; + Imu_Data_Pub.angular_velocity_covariance[8] = 1e-6; + Imu_Data_Pub.linear_acceleration.x = Mpu6050.linear_acceleration.x; + Imu_Data_Pub.linear_acceleration.y = Mpu6050.linear_acceleration.y; + Imu_Data_Pub.linear_acceleration.z = Mpu6050.linear_acceleration.z; + + imu_publisher->publish(Imu_Data_Pub); + +} + +void origincar_base::Publish_Odom() +{ + tf2::Quaternion q; + q.setRPY(0,0,Robot_Pos.Z); + geometry_msgs::msg::Quaternion odom_quat=tf2::toMsg(q); + + origincar_msg::msg::Data robotpose; + origincar_msg::msg::Data robotvel; + nav_msgs::msg::Odometry odom; + + odom.header.stamp = rclcpp::Node::now(); + odom.header.frame_id = odom_frame_id; + odom.child_frame_id = robot_frame_id; + + odom.pose.pose.position.x = Robot_Pos.X; + odom.pose.pose.position.y = Robot_Pos.Y; + + odom.pose.pose.position.z = 0.0; + odom.pose.pose.orientation = odom_quat; + //odom_quat; + + + odom.twist.twist.linear.x = Robot_Vel.X; + odom.twist.twist.linear.y = Robot_Vel.Y; + odom.twist.twist.angular.z = Robot_Vel.Z; + + robotpose.x = Robot_Pos.X; + robotpose.y = Robot_Pos.Y; + robotpose.z = Robot_Pos.Z; + + robotvel.x = Robot_Vel.X; + robotvel.y = Robot_Vel.Y; + robotvel.z = Robot_Vel.Z; + + geometry_msgs::msg::TransformStamped t; + + t.header.stamp = rclcpp::Node::now(); + t.header.frame_id = odom_frame_id; + t.child_frame_id = robot_frame_id; + + t.transform.translation.x = Robot_Pos.X; + t.transform.translation.y = Robot_Pos.Y; + t.transform.translation.z = 0.0; + + t.transform.rotation = odom_quat; + tf_broadcaster_->sendTransform(t); + odom_publisher->publish(odom); + robotpose_publisher->publish(robotpose); + robotvel_publisher->publish(robotvel); +} + +void origincar_base::Publish_Voltage() +{ + std_msgs::msg::Float32 voltage_msgs; + static float Count_Voltage_Pub = 0; + + if (Count_Voltage_Pub++ > 10) { + Count_Voltage_Pub = 0; + voltage_msgs.data = Power_voltage; + voltage_publisher->publish(voltage_msgs); + } +} + +unsigned char origincar_base::Check_Sum(unsigned char Count_Number,unsigned char mode) +{ + unsigned char check_sum = 0, k; + + if (mode == 0) { + for(k=0; k < Count_Number; k++) { + check_sum = check_sum^Receive_Data.rx[k]; + } + } else if (mode == 1) { + for (k=0; k < Count_Number; k++) { + check_sum = check_sum^Send_Data.tx[k]; + } + } + + return check_sum; +} + +bool origincar_base::Get_Sensor_Data() +{ + short transition_16 = 0, j = 0, Header_Pos = 0, Tail_Pos = 0; + uint8_t Receive_Data_Pr[RECEIVE_DATA_SIZE] = {0}; + Stm32_Serial.read(Receive_Data_Pr,sizeof (Receive_Data_Pr)); + for (j = 0; j < 24; j++) { + if (Receive_Data_Pr[j] == FRAME_HEADER) + Header_Pos=j; + else if (Receive_Data_Pr[j] == FRAME_TAIL) + Tail_Pos = j; + } + + if (Tail_Pos == (Header_Pos + 23)) { + memcpy(Receive_Data.rx, Receive_Data_Pr, sizeof(Receive_Data_Pr)); + } else if (Header_Pos == (1 + Tail_Pos)) { + for (j = 0;j < 24; j++) + Receive_Data.rx[j] = Receive_Data_Pr[(j+Header_Pos) % 24]; + } else { + return false; + } + + Receive_Data.Frame_Header = Receive_Data.rx[0]; + Receive_Data.Frame_Tail = Receive_Data.rx[23]; + if (Receive_Data.Frame_Header == FRAME_HEADER) { + if (Receive_Data.Frame_Tail == FRAME_TAIL) { + if (Receive_Data.rx[22] == Check_Sum(22,READ_DATA_CHECK)||(Header_Pos == (1 + Tail_Pos))) { + Receive_Data.Flag_Stop=Receive_Data.rx[1]; + Robot_Vel.X = Odom_Trans(Receive_Data.rx[2],Receive_Data.rx[3]); + + Robot_Vel.Y = Odom_Trans(Receive_Data.rx[4],Receive_Data.rx[5]); + + //Robot_Vel.Z = Odom_Trans(Receive_Data.rx[6],Receive_Data.rx[7]); + + Mpu6050_Data.accele_x_data = IMU_Trans(Receive_Data.rx[8],Receive_Data.rx[9]); + Mpu6050_Data.accele_y_data = IMU_Trans(Receive_Data.rx[10],Receive_Data.rx[11]); + Mpu6050_Data.accele_z_data = IMU_Trans(Receive_Data.rx[12],Receive_Data.rx[13]); + Mpu6050_Data.gyros_x_data = IMU_Trans(Receive_Data.rx[14],Receive_Data.rx[15]); + Mpu6050_Data.gyros_y_data = IMU_Trans(Receive_Data.rx[16],Receive_Data.rx[17]); + Mpu6050_Data.gyros_z_data = IMU_Trans(Receive_Data.rx[18],Receive_Data.rx[19]); + + Mpu6050.linear_acceleration.x = Mpu6050_Data.accele_x_data / ACCEl_RATIO; + Mpu6050.linear_acceleration.y = Mpu6050_Data.accele_y_data / ACCEl_RATIO; + Mpu6050.linear_acceleration.z = Mpu6050_Data.accele_z_data / ACCEl_RATIO; + + Mpu6050.angular_velocity.x = Mpu6050_Data.gyros_x_data * GYROSCOPE_RATIO; + Mpu6050.angular_velocity.y = Mpu6050_Data.gyros_y_data * GYROSCOPE_RATIO; + + if(Init_imu_num<100) + { + Init_imu_num += 1; + gyro_z_sum += Mpu6050_Data.gyros_z_data ; + Mpu6050.angular_velocity.z = 0; + } + else + { + Mpu6050.angular_velocity.z = (Mpu6050_Data.gyros_z_data - (gyro_z_sum / (float)Init_imu_num))*GYROSCOPE_RATIO; + //RCLCPP_INFO(this->get_logger(),"gyro_z_sum: %.2f, err: %.2f, gyroz: %.2f ", gyro_z_sum ,(gyro_z_sum / (float)Init_imu_num), Mpu6050.angular_velocity.z); + } + Robot_Vel.Z = Mpu6050.angular_velocity.z; + transition_16 = 0; + transition_16 |= Receive_Data.rx[20]<<8; + transition_16 |= Receive_Data.rx[21]; + Power_voltage = transition_16/1000+(transition_16 % 1000)*0.001; + + return true; + } + } + } + + return false; +} + +void origincar_base::Control() +{ + rclcpp::Time current_time, last_time; + current_time = rclcpp::Node::now(); + last_time = rclcpp::Node::now(); + while(rclcpp::ok()) { + current_time = rclcpp::Node::now(); + Sampling_Time = (current_time - last_time).seconds(); + if (true == Get_Sensor_Data()) { + Robot_Pos.X+=1.03*(Robot_Vel.X * cos(Robot_Pos.Z) - Robot_Vel.Y * sin(Robot_Pos.Z)) * Sampling_Time; + Robot_Pos.Y+=1.01*(Robot_Vel.X * sin(Robot_Pos.Z) + Robot_Vel.Y * cos(Robot_Pos.Z)) * Sampling_Time;//1.125 + Robot_Pos.Z+= Mpu6050.angular_velocity.z * Sampling_Time; + + Quaternion_Solution(Mpu6050.angular_velocity.x, Mpu6050.angular_velocity.y, Mpu6050.angular_velocity.z,\ + Mpu6050.linear_acceleration.x, Mpu6050.linear_acceleration.y, Mpu6050.linear_acceleration.z); + Publish_ImuSensor(); + Publish_Voltage(); + Publish_Odom(); + rclcpp::spin_some(this->get_node_base_interface()); + } + last_time = current_time; + } +} + +origincar_base::origincar_base() +: rclcpp::Node ("origincar_base") +{ + memset(&Robot_Pos, 0, sizeof(Robot_Pos)); + memset(&Robot_Vel, 0, sizeof(Robot_Vel)); + memset(&Receive_Data, 0, sizeof(Receive_Data)); + memset(&Send_Data, 0, sizeof(Send_Data)); + memset(&Mpu6050_Data, 0, sizeof(Mpu6050_Data)); + Robot_Pos.X = 0.54; + Robot_Pos.Y = 0.2; + + int serial_baud_rate = 115200; + + this->declare_parameter("usart_port_name", "/dev/ttyCH343USB0"); + this->declare_parameter("cmd_vel", "cmd_vel"); + this->declare_parameter("akm_cmd_vel", "ackermann_cmd"); + this->declare_parameter("odom_frame_id", "odom"); + this->declare_parameter("robot_frame_id", "base_link"); + this->declare_parameter("gyro_frame_id", "gyro_link"); + + this->get_parameter("serial_baud_rate", serial_baud_rate); + this->get_parameter("usart_port_name", usart_port_name); + this->get_parameter("cmd_vel", cmd_vel); + this->get_parameter("akm_cmd_vel", akm_cmd_vel); + this->get_parameter("odom_frame_id", odom_frame_id); + this->get_parameter("robot_frame_id", robot_frame_id); + this->get_parameter("gyro_frame_id", gyro_frame_id); + + odom_publisher = create_publisher("odom", 10); + + imu_publisher = create_publisher("imu/data_raw", 10); + + voltage_publisher = create_publisher("PowerVoltage", 1); + + robotpose_publisher = create_publisher("robotpose", 10); + + robotvel_publisher = create_publisher("robotvel", 10); + + pose_pub_ = create_publisher( + "/set_pose", + rclcpp::SystemDefaultsQoS().reliable()); + + tf_bro = std::make_shared(this); + tf_broadcaster_ = std::make_unique(*this); + Cmd_Vel_Sub = create_subscription( + cmd_vel, 1, std::bind(&origincar_base::Cmd_Vel_Callback, this, _1)); + Akm_Cmd_Vel_Sub = create_subscription( + akm_cmd_vel, 1, std::bind(&origincar_base::Akm_Cmd_Vel_Callback, this, _1)); + + Sign_Switch_Sub = create_subscription( + "/sign4return", 1, std::bind(&origincar_base::Sign_Switch_Callback, this, _1)); + try { + Stm32_Serial.setPort("/dev/ttyACM0"); + Stm32_Serial.setBaudrate(serial_baud_rate); + serial::Timeout _time = serial::Timeout::simpleTimeout(2000); + Stm32_Serial.setTimeout(_time); + Stm32_Serial.open(); + } catch (serial::IOException& e) { + RCLCPP_ERROR(this->get_logger(),"origincar_base can not open serial port,Please check the serial port cable! "); + } + if(Stm32_Serial.isOpen()) { + RCLCPP_INFO(this->get_logger(),"origincar_base serial port opened"); + } +} + + +void sigintHandler(int sig) +{ + sig = sig; + printf("OriginBot shutdown...\n"); + serial::Serial Stm32_Serial; + Stm32_Serial.setPort("/dev/ttyACM0"); + Stm32_Serial.setBaudrate(115200); + serial::Timeout _time = serial::Timeout::simpleTimeout(2000); + Stm32_Serial.setTimeout(_time); + Stm32_Serial.open(); + SEND_DATA Send_Data; + if (Stm32_Serial.isOpen()) { + Send_Data.tx[0]=FRAME_HEADER; + Send_Data.tx[1] = 0; + Send_Data.tx[2] = 0; + + Send_Data.tx[4] = 0; + Send_Data.tx[3] = 0; + + Send_Data.tx[6] = 0; + Send_Data.tx[5] = 0; + + Send_Data.tx[7] = 0; + Send_Data.tx[8] = 0; + int check_sum = 0; + for (int k = 0; k < 9; k++) { + check_sum = check_sum^Send_Data.tx[k]; + } + Send_Data.tx[9]=check_sum; + Send_Data.tx[10]=FRAME_TAIL; + + try { + Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx)); + } catch (serial::IOException& e) { + } + + } + // 关闭ROS2接口，清除资源 + rclcpp::shutdown(); +} + +origincar_base::~origincar_base() +{ + RCLCPP_INFO(this->get_logger(),"Shutting down"); +} \ No newline at end of file diff --git a/src/origincar_description/CMakeLists.txt b/src/origincar_description/CMakeLists.txt new file mode 100644 index 0000000..21934a5 --- /dev/null +++ b/src/origincar_description/CMakeLists.txt @@ -0,0 +1,40 @@ +cmake_minimum_required(VERSION 3.5) +project(origincar_description) + +# 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) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +install( + DIRECTORY launch urdf rviz meshes world + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/origincar_description/CMakeLists.txt.save b/src/origincar_description/CMakeLists.txt.save new file mode 100644 index 0000000..05033cf --- /dev/null +++ b/src/origincar_description/CMakeLists.txt.save @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 2.8.3) + +project(origincar_description) + +find_package(ament_cmake REQUIRED) + +find_package(roslaunch) + +foreach(dir config launch meshes urdf) + install(DIRECTORY ${dir}/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) +endforeach(dir) diff --git a/src/origincar_description/config/joint_names_origincar_description.yaml b/src/origincar_description/config/joint_names_origincar_description.yaml new file mode 100644 index 0000000..8d4a477 --- /dev/null +++ b/src/origincar_description/config/joint_names_origincar_description.yaml @@ -0,0 +1 @@ +controller_joint_names: ['', 'down_left_joint', 'down_right_joint', 'up_left_joint', ] diff --git a/src/origincar_description/config/joint_names_origincar_description.yamlZone.Identifier b/src/origincar_description/config/joint_names_origincar_description.yamlZone.Identifier new file mode 100644 index 0000000..d6c1ec6 Binary files /dev/null and b/src/origincar_description/config/joint_names_origincar_description.yamlZone.Identifier differ diff --git a/src/origincar_description/launch/display.launch b/src/origincar_description/launch/display.launch new file mode 100644 index 0000000..ef7821b --- /dev/null +++ b/src/origincar_description/launch/display.launch @@ -0,0 +1,20 @@ + + + + + + + \ No newline at end of file diff --git a/src/origincar_description/launch/display.launch.py b/src/origincar_description/launch/display.launch.py new file mode 100644 index 0000000..68eeeb8 --- /dev/null +++ b/src/origincar_description/launch/display.launch.py @@ -0,0 +1,63 @@ +#!/usr/bin/python3 + +from launch import LaunchDescription +from launch_ros.actions import Node +import os +from ament_index_python.packages import get_package_share_directory +from launch_ros.parameter_descriptions import ParameterValue +from launch.substitutions import Command, LaunchConfiguration +from launch.actions import DeclareLaunchArgument + + +def generate_launch_description(): + # * 这里是机器人模型路径之类的 + origincar_urdf_dir = get_package_share_directory('origincar_description') + default_model_path = os.path.join( + origincar_urdf_dir, "urdf", "origincar.urdf") + default_rviz_path = os.path.join( + origincar_urdf_dir, "rviz", "display.rviz") + model = DeclareLaunchArgument( + name="model", default_value=default_model_path) + use_sim_time = LaunchConfiguration('use_sim_time', default='true') + + # 加载机器人模型 + # 1.启动 robot_state_publisher 节点并以参数方式加载 urdf 文件 + robot_description = ParameterValue(Command(["xacro ", LaunchConfiguration("model")]), + value_type=str) + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + parameters=[{"robot_description": robot_description, + 'use_sim_time': True, 'publish_frequency': 30.0}] + ) + + joint_state_publisher_node = Node( + package='joint_state_publisher_gui', + executable='joint_state_publisher_gui', + name='joint_state_publisher_gui', + parameters=[{'use_sim_time': use_sim_time}], + arguments=[default_model_path] + ) + + # 2.启动 joint_state_publisher 节点发布非固定关节状态 + joint_state_publisher = Node( + package="joint_state_publisher", + executable="joint_state_publisher" + ) + + # rviz2 节点 + # *-d 为加载配置文件，这个不用看，因人而异 + rviz2 = Node( + package="rviz2", + executable="rviz2", + parameters=[{'use_sim_time': use_sim_time}], + arguments=["-d", default_rviz_path] + ) + + return LaunchDescription([ + model, + robot_state_publisher, + # joint_state_publisher_node, + joint_state_publisher, + rviz2 + ]) diff --git a/src/origincar_description/launch/gazebo.launch b/src/origincar_description/launch/gazebo.launch new file mode 100644 index 0000000..42b2bcd --- /dev/null +++ b/src/origincar_description/launch/gazebo.launch @@ -0,0 +1,20 @@ + + + + + + \ No newline at end of file diff --git a/src/origincar_description/launch/gazebo.launch.py b/src/origincar_description/launch/gazebo.launch.py new file mode 100644 index 0000000..384d4b8 --- /dev/null +++ b/src/origincar_description/launch/gazebo.launch.py @@ -0,0 +1,68 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +import os +from ament_index_python.packages import get_package_share_directory +from launch_ros.parameter_descriptions import ParameterValue +from launch.substitutions import Command, LaunchConfiguration +from launch.actions import DeclareLaunchArgument +from launch.actions import ExecuteProcess + + +def generate_launch_description(): + ld = LaunchDescription() + robot_name_in_model = 'mycar' + origincar_urdf_dir = get_package_share_directory( + 'origincar_description') + default_model_path = os.path.join( + origincar_urdf_dir, "urdf", "origincar.urdf") + default_rviz_path = os.path.join( + origincar_urdf_dir, "rviz", "display.rviz") + model = DeclareLaunchArgument( + name="model", default_value=default_model_path) + use_sim_time = LaunchConfiguration('use_sim_time', default='true') + # * ###################gazebo + gazebo_world_path = os.path.join(origincar_urdf_dir, 'world/test.world') + start_gazebo_cmd = ExecuteProcess( + cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so', + '-s', 'libgazebo_ros_factory.so', gazebo_world_path], + output='screen') + + spawn_entity_cmd = Node( + package='gazebo_ros', + executable='spawn_entity.py', + arguments=['-entity', robot_name_in_model, + '-file', default_model_path], + output='screen' + ) + + robot_description = ParameterValue(Command(["xacro ", LaunchConfiguration("model")]), + value_type=str) + + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + parameters=[{"robot_description": robot_description, + 'use_sim_time': True, 'publish_frequency': 30.0}] + ) + + # 2.启动 joint_state_publisher 节点发布非固定关节状态 + joint_state_publisher = Node( + package="joint_state_publisher", + executable="joint_state_publisher" + ) + # *###################rviz + # rviz2 节点 + # *-d 为加载配置文件，这个不用看，因人而异 + rviz2 = Node( + package="rviz2", + executable="rviz2", + arguments=["-d", default_rviz_path], + parameters=[{'use_sim_time': use_sim_time}] + ) + ld.add_action(model) + ld.add_action(start_gazebo_cmd) + ld.add_action(spawn_entity_cmd) + ld.add_action(robot_state_publisher) + # ld.add_action(joint_state_publisher) + # ld.add_action(rviz2) + return ld diff --git a/src/origincar_description/meshes/base_link.STL b/src/origincar_description/meshes/base_link.STL new file mode 100644 index 0000000..9588ff4 Binary files /dev/null and b/src/origincar_description/meshes/base_link.STL differ diff --git a/src/origincar_description/meshes/base_link.STLZone.Identifier b/src/origincar_description/meshes/base_link.STLZone.Identifier new file mode 100644 index 0000000..d6c1ec6 Binary files /dev/null and b/src/origincar_description/meshes/base_link.STLZone.Identifier differ diff --git a/src/origincar_description/meshes/down_left_Link.STL b/src/origincar_description/meshes/down_left_Link.STL new file mode 100644 index 0000000..746f229 Binary files /dev/null and b/src/origincar_description/meshes/down_left_Link.STL differ diff --git a/src/origincar_description/meshes/down_left_Link.STLZone.Identifier b/src/origincar_description/meshes/down_left_Link.STLZone.Identifier new file mode 100644 index 0000000..d6c1ec6 Binary files /dev/null and b/src/origincar_description/meshes/down_left_Link.STLZone.Identifier differ diff --git a/src/origincar_description/meshes/down_right_Link.STL b/src/origincar_description/meshes/down_right_Link.STL new file mode 100644 index 0000000..a2e315a Binary files /dev/null and b/src/origincar_description/meshes/down_right_Link.STL differ diff --git a/src/origincar_description/meshes/down_right_Link.STLZone.Identifier b/src/origincar_description/meshes/down_right_Link.STLZone.Identifier new file mode 100644 index 0000000..d6c1ec6 Binary files /dev/null and b/src/origincar_description/meshes/down_right_Link.STLZone.Identifier differ diff --git a/src/origincar_description/meshes/up_left_Link.STL b/src/origincar_description/meshes/up_left_Link.STL new file mode 100644 index 0000000..3367b35 Binary files /dev/null and b/src/origincar_description/meshes/up_left_Link.STL differ diff --git a/src/origincar_description/meshes/up_left_Link.STLZone.Identifier b/src/origincar_description/meshes/up_left_Link.STLZone.Identifier new file mode 100644 index 0000000..d6c1ec6 Binary files /dev/null and b/src/origincar_description/meshes/up_left_Link.STLZone.Identifier differ diff --git a/src/origincar_description/meshes/up_right_Link.STL b/src/origincar_description/meshes/up_right_Link.STL new file mode 100644 index 0000000..0174871 Binary files /dev/null and b/src/origincar_description/meshes/up_right_Link.STL differ diff --git a/src/origincar_description/meshes/up_right_Link.STLZone.Identifier b/src/origincar_description/meshes/up_right_Link.STLZone.Identifier new file mode 100644 index 0000000..d6c1ec6 Binary files /dev/null and b/src/origincar_description/meshes/up_right_Link.STLZone.Identifier differ diff --git a/src/origincar_description/package.xml b/src/origincar_description/package.xml new file mode 100644 index 0000000..493cafe --- /dev/null +++ b/src/origincar_description/package.xml @@ -0,0 +1,24 @@ + + + + origincar_description + 0.0.0 + TODO: Package description + ps-micro + TODO: License declaration + + ament_cmake + + rviz2 + xacro + robot_state_publisher + joint_state_publisher + ros2launch + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/origincar_description/rviz/README b/src/origincar_description/rviz/README new file mode 100644 index 0000000..1ba2ec8 --- /dev/null +++ b/src/origincar_description/rviz/README @@ -0,0 +1 @@ +## 此文件夹为配置RVIZ，暂时不使用 \ No newline at end of file diff --git a/src/origincar_description/rviz/display.rviz b/src/origincar_description/rviz/display.rviz new file mode 100644 index 0000000..57dd63a --- /dev/null +++ b/src/origincar_description/rviz/display.rviz @@ -0,0 +1,186 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + - /RobotModel1/Description Topic1 + Splitter Ratio: 0.5 + Tree Height: 490 + - 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 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + down_left_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + down_right_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + up_left_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + up_right_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + {} + Update Interval: 0 + Value: false + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + 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 + 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: 0.4845520257949829 + 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.7803981304168701 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.05039529129862785 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 719 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000015600000275fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000275000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000275fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000275000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000035a0000027500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1483 + X: 72 + Y: 27 diff --git a/src/origincar_description/urdf/origincar.urdf b/src/origincar_description/urdf/origincar.urdf new file mode 100644 index 0000000..a51ecbd --- /dev/null +++ b/src/origincar_description/urdf/origincar.urdf @@ -0,0 +1,475 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + / + cmd_vel:=cmd_vel + odom:=odom + distance:=distance + + 30.0 + fr_left_wheel_joint + fr_right_wheel_joint + re_left_wheel_joint + re_right_wheel_joint + fr_left_steer_joint + fr_right_steer_joint + virtual_steering_wheel_joint + 0.583972 + 1.570796 + 4 + + + 600 0 6 + 0 0 + 600 0 6 + 0 0 + 800 0 0 + 0 0 + odom + base_footprint + true + true + + + + + + 100 + fr_right_steer_joint + fr_right_wheel_joint + fr_left_steer_joint + fr_left_wheel_joint + re_right_wheel_joint + re_left_wheel_joint + virtual_steering_wheel_joint + + + + + + + + + / + ~/out:=imu + + false + + true + 100 + true + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + + + + + + true + true + 5 + 0 0 0.075 0 0 0 + + + + 360 + 1.000000 + 0.000000 + 6.280000 + + + + 0.120000 + 3.5 + 0.015000 + + + gaussian + 0.0 + 0.01 + + + + + + + ~/out:=scan + + sensor_msgs/LaserScan + laser_link + + + + \ No newline at end of file diff --git a/src/origincar_description/urdf/origincar.xacro b/src/origincar_description/urdf/origincar.xacro new file mode 100644 index 0000000..0aca376 --- /dev/null +++ b/src/origincar_description/urdf/origincar.xacro @@ -0,0 +1,135 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/origincar_description/world/fishbot.world b/src/origincar_description/world/fishbot.world new file mode 100644 index 0000000..e6e1c0a --- /dev/null +++ b/src/origincar_description/world/fishbot.world @@ -0,0 +1,626 @@ + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + 0 + 0 + 0 + + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + 0 + 0 + 0 + + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + 51 251000000 + 51 570048324 + 1767706562 783975173 + 51251 + + 0.005469 0.07101 0 0 -0 0 + 1 1 1 + + 2.33047 -1.27133 0 0 0 -2.0944 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 0.005469 -2.61367 0 0 -0 3.14159 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.31953 -1.27133 0 0 -0 2.0944 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.31953 1.41335 0 0 -0 1.0472 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 0.005469 2.75569 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 2.33047 1.41335 0 0 0 -1.0472 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -0.039643 1.36341 0.499995 0 1e-05 0 + 1 1 1 + + -0.039643 1.36341 0.499995 0 1e-05 0 + 0 0 0 0 -0 0 + 0.010615 -0.006191 -9.78231 0.012424 0.021225 1.8e-05 + 0.010615 -0.006191 -9.78231 0 -0 0 + + + + -1.0625 -0.503729 0.499993 -3e-06 -4e-06 -0 + 1 1 1 + + -1.0625 -0.503729 0.499993 -3e-06 -4e-06 -0 + 0 0 0 0 -0 0 + 0 0 -9.8 0 -0 0 + 0 0 -9.8 0 -0 0 + + + + 0.724857 -0.542454 0.499997 3e-06 4e-06 -0 + 1 1 1 + + 0.724857 -0.542454 0.499997 3e-06 4e-06 -0 + 0 0 0 0 -0 0 + 0 0 -9.8 0 -0 0 + 0 0 -9.8 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + 0.005469 0.07101 0 0 -0 0 + + + + + 3.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 3.25 0.15 2.5 + + + + + 0.996078 0.47451 0.0196078 1 + + + 0 + + + 2.325 -1.34234 0 0 -0 -2.0944 + 0 + 0 + 0 + + + + + + 3.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 3.25 0.15 2.5 + + + + + 0.996078 0.47451 0.0196078 1 + + + 0 + + + -0 -2.68468 0 0 -0 3.14159 + 0 + 0 + 0 + + + + + + 3.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 3.25 0.15 2.5 + + + + + 0.996078 0.47451 0.0196078 1 + + + 0 + + + -2.325 -1.34234 0 0 -0 2.0944 + 0 + 0 + 0 + + + + + + 3.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 3.25 0.15 2.5 + + + + + 0.996078 0.47451 0.0196078 1 + + + 0 + + + -2.325 1.34234 0 0 -0 1.0472 + 0 + 0 + 0 + + + + + + 3.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 3.25 0.15 2.5 + + + + + 0.996078 0.47451 0.0196078 1 + + + 0 + + + -0 2.68468 0 0 -0 0 + 0 + 0 + 0 + + + + + + 3.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 3.25 0.15 2.5 + + + + + 0.996078 0.47451 0.0196078 1 + + + 0 + + + 2.325 1.34234 0 0 -0 -1.0472 + 0 + 0 + 0 + + 1 + + + -0.039648 1.36341 0.5 0 -0 0 + + + 1 + + 0.166667 + 0 + 0 + 0.166667 + 0 + 0.166667 + + 0 0 0 0 -0 0 + + + + + 1 1 1 + + + 10 + + + + + + + + + + + + + + + + + 1 1 1 + + + + + + + 0 + 0 + 0 + + + + -1.0625 -0.503731 0.5 0 -0 0 + + + 1 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + 0 0 0 0 -0 0 + + + + + 0.5 + 1 + + + 10 + + + + + + + + + + + + + + + + + 0.5 + 1 + + + + + + + 0 + 0 + 0 + + + + 0.724855 -0.542452 0.5 0 -0 0 + + + 1 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + 0 0 0 0 -0 0 + + + + + 0.5 + 1 + + + 10 + + + + + + + + + + + + + + + + + 0.5 + 1 + + + + + + + 0 + 0 + 0 + + + + + 11.8556 -5.21446 33.9466 0 1.2498 2.56501 + orbit + perspective + + + + diff --git a/src/origincar_description/world/gc_world.world b/src/origincar_description/world/gc_world.world new file mode 100644 index 0000000..cc15c7b --- /dev/null +++ b/src/origincar_description/world/gc_world.world @@ -0,0 +1,893 @@ + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + 0 + 0 + 0 + + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + 0 + 0 + 0 + + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + -4.71646 -0.134166 0 0 -0 0 + + + + + 6.5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 6.5 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -10.5912 -3.10164 0 0 -0 0 + 0 + 0 + 0 + + + + + + 16 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 16 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -5.36881 -0.250968 0 0 -0 -1.5708 + 0 + 0 + 0 + + + + + + 19.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 19.25 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 4.18119 -8.17597 0 0 -0 0 + 0 + 0 + 0 + + + + + + 16.5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 16.5 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 13.7312 -0.000968 0 0 -0 1.5708 + 0 + 0 + 0 + + + + + + 19.2565 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 19.2565 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 4.18119 7.92403 0 0 -0 -3.11542 + 0 + 0 + 0 + + + + + + 10.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 10.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -1.01381 3.08403 0 0 -0 0 + 0 + 0 + 0 + + + + + + 5.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 5.25 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 4.28619 0.534032 0 0 -0 -1.5708 + 0 + 0 + 0 + + + + + + 5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 5 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -3.07881 -1.59597 0 0 -0 0 + 0 + 0 + 0 + + + + + + 5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 5 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 9.25619 -5.56097 0 0 -0 1.5708 + 0 + 0 + 0 + + + + + + 4.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 4.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 11.4662 3.66903 0 0 -0 0 + 0 + 0 + 0 + + 1 + + + -6.35985 -5.47044 0.5 0 -0 0 + + + 1 + + 0.166667 + 0 + 0 + 0.166667 + 0 + 0.166667 + + 0 0 0 0 -0 0 + + + + + 1 1 1 + + + 10 + + + + + + + + + + + + + + + + + 1 1 1 + + + + + + + 0 + 0 + 0 + + + + -2.08128 -5.46626 0.5 0 -0 0 + + + 1 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + 0 0 0 0 -0 0 + + + + + 0.5 + 1 + + + 10 + + + + + + + + + + + + + + + + + 0.5 + 1 + + + + + + + 0 + 0 + 0 + + + + 3.24338 0.112284 0.5 0 -0 0 + + + 1 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + 0 0 0 0 -0 0 + + + + + 0.5 + 1 + + + 10 + + + + + + + + + + + + + + + + + 0.5 + 1 + + + + + + + 0 + 0 + 0 + + + + 1.72376 5.13 0.5 0 -0 0 + + + 1 + + 0.166667 + 0 + 0 + 0.166667 + 0 + 0.166667 + + 0 0 0 0 -0 0 + + + + + 1 1 1 + + + 10 + + + + + + + + + + + + + + + + + 1 1 1 + + + + + + + 0 + 0 + 0 + + + + 148 245000000 + 149 497524618 + 1770372561 596283813 + 148245 + + -4.71646 -0.134166 0 0 -0 0 + 1 1 1 + + -15.3077 -3.23581 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -10.0853 -0.385134 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.53527 -8.31014 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.01474 -0.135134 0 0 -0 1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.53527 7.78986 0 0 0 -3.11542 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -5.73027 2.94986 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.43027 0.399866 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -7.79527 -1.73014 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 4.53973 -5.69514 0 0 -0 1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.74974 3.53486 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -6.35985 -5.47044 0.499995 -1e-05 -0 -0 + 1 1 1 + + -6.35985 -5.47044 0.499995 -1e-05 -0 -0 + 0 0 0 0 -0 0 + 0.004709 0.011055 -9.78158 -0.022108 0.009414 1e-06 + 0.004709 0.011055 -9.78158 0 -0 0 + + + + 1.72376 5.13 0.5 0 -0 0 + 1 1 1 + + 1.72376 5.13 0.5 0 -0 0 + 0 0 0 0 -0 0 + -0.004709 -9.78112 9.78158 0.712677 -0.009414 -4.3e-05 + -0.004709 -9.78112 9.78158 0 -0 0 + + + + -2.08128 -5.46626 0.499997 3e-06 4e-06 -0 + 1 1 1 + + -2.08128 -5.46626 0.499997 3e-06 4e-06 -0 + 0 0 0 0 -0 0 + 0 0 -9.8 0 -0 0 + 0 0 -9.8 0 -0 0 + + + + 3.24338 0.112286 0.499993 -3e-06 -4e-06 -0 + 1 1 1 + + 3.24338 0.112286 0.499993 -3e-06 -4e-06 -0 + 0 0 0 0 -0 0 + 0 0 -9.8 0 -0 0 + 0 0 -9.8 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + + 7.40665 -3.57041 31.6489 0 1.4618 3.1242 + orbit + perspective + + + + diff --git a/src/origincar_description/world/test.world b/src/origincar_description/world/test.world new file mode 100644 index 0000000..8cbcf19 --- /dev/null +++ b/src/origincar_description/world/test.world @@ -0,0 +1,828 @@ + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + 0 + 0 + 0 + + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + 0 + 0 + 0 + + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + 2.92199 0.94347 0 0 -0 0 + + + + + 5.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 5.25 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -4.8877 -4.19109 0 0 -0 -1.5708 + 0 + 0 + 0 + + + + + + 10 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 10 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 0.0373 -6.74109 0 0 -0 0 + 0 + 0 + 0 + + + + + + 3.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 3.25 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 1.06397 5.19109 0 0 -0 1.5708 + 0 + 0 + 0 + + + + + + 3.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 3.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 1.09029 -4.75661 0 0 -0 -1.5708 + 0 + 0 + 0 + + + + + + 13.5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 13.5 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 4.9623 -0.066086 0 0 -0 1.5708 + 0 + 0 + 0 + + + + + + 10 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 10 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 0.0373 6.60891 0 0 -0 3.14159 + 0 + 0 + 0 + + + + + + 9.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 9.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -4.8877 1.80891 0 0 -0 -1.5708 + 0 + 0 + 0 + + + + + + 3.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 3.25 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -3.4123 3.54932 0 0 -0 0 + 0 + 0 + 0 + + + + + + 3.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 3.25 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -1.8623 1.99932 0 0 -0 -1.5708 + 0 + 0 + 0 + + + + + + 3.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 3.25 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -0.312301 0.449317 0 0 -0 0 + 0 + 0 + 0 + + 1 + + + 0.15442 -3.50156 0.5 0 -0 0 + + + 1 + + 0.166667 + 0 + 0 + 0.166667 + 0 + 0.166667 + + 0 0 0 0 -0 0 + + + + + 1 1 1 + + + 10 + + + + + + + + + + + + + + + + + 1 1 1 + + + + + + + 0 + 0 + 0 + + + + 1.94616 -0.907636 0.5 0 -0 0 + + + 1 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + 0 0 0 0 -0 0 + + + + + 0.5 + 1 + + + 10 + + + + + + + + + + + + + + + + + 0.5 + 1 + + + + + + + 0 + 0 + 0 + + + + 5.80896 0.105775 0.5 0 -0 0 + + + 1 + + 0.166667 + 0 + 0 + 0.166667 + 0 + 0.166667 + + 0 0 0 0 -0 0 + + + + + 1 1 1 + + + 10 + + + + + + + + + + + + + + + + + 1 1 1 + + + + + + + 0 + 0 + 0 + + + + 77 768000000 + 78 15142337 + 1770533307 91681029 + 77768 + + 2.92199 0.94347 0 0 -0 0 + 1 1 1 + + -1.96571 -3.24762 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 2.95929 -5.79762 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.98596 6.13456 0 0 -0 1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 4.01228 -3.81314 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 7.88429 0.877384 0 0 -0 1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 2.95929 7.55238 0 0 -0 3.14159 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1.96571 2.75238 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.49031 4.49279 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.05969 2.94279 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 2.60969 1.39279 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0.15442 -3.50156 0.5 0 -0 0.691942 + 1 1 1 + + 0.15442 -3.50156 0.5 0 -0 0.691942 + 0 0 0 0 -0 0 + -0 0 -0 0 -0 0 + -0 0 -0 0 -0 0 + + + + 5.80896 0.105775 0.5 0 -0 0.665093 + 1 1 1 + + 5.80896 0.105775 0.5 0 -0 0.665093 + 0 0 0 0 -0 0 + 7.28216 6.1355 9.84639 -2.8462 1.14378 -3.14153 + 7.28216 6.1355 9.84639 0 -0 0 + + + + 1.94616 -0.907634 0.499993 -3e-06 -3e-06 0 + 1 1 1 + + 1.94616 -0.907634 0.499993 -3e-06 -3e-06 0 + 0 0 0 0 -0 0 + 0 0 -9.8 0 -0 0 + 0 0 -9.8 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + + 4.60766 -4.56851 22.292 -0 1.35564 1.44419 + orbit + perspective + + + + diff --git a/src/origincar_msg/CMakeLists.txt b/src/origincar_msg/CMakeLists.txt new file mode 100644 index 0000000..8697fbf --- /dev/null +++ b/src/origincar_msg/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 3.5) + +project(origincar_msg) + +# Default to C++14 +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_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +find_package(std_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/Data.msg" + "msg/Sign.msg" + DEPENDENCIES std_msgs + ADD_LINTER_TESTS +) + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() diff --git a/src/origincar_msg/msg/Data.msg b/src/origincar_msg/msg/Data.msg new file mode 100644 index 0000000..161f45e --- /dev/null +++ b/src/origincar_msg/msg/Data.msg @@ -0,0 +1,3 @@ +float32 x +float32 y +float32 z diff --git a/src/origincar_msg/msg/Sign.msg b/src/origincar_msg/msg/Sign.msg new file mode 100644 index 0000000..189d1f8 --- /dev/null +++ b/src/origincar_msg/msg/Sign.msg @@ -0,0 +1 @@ +int32 sign_data \ No newline at end of file diff --git a/src/origincar_msg/package.xml b/src/origincar_msg/package.xml new file mode 100644 index 0000000..bd08ee6 --- /dev/null +++ b/src/origincar_msg/package.xml @@ -0,0 +1,21 @@ + + + + origincar_msg + 0.0.0 + TODO: Package description + ps-micro + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + diff --git a/src/zbw_slamtoolbox/CMakeLists.txt b/src/zbw_slamtoolbox/CMakeLists.txt new file mode 100644 index 0000000..6ec9f44 --- /dev/null +++ b/src/zbw_slamtoolbox/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.8) +project(zbw_slamtoolbox) + +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) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +install( + DIRECTORY launch config + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/zbw_slamtoolbox/config/mapper_params_online_async.yaml b/src/zbw_slamtoolbox/config/mapper_params_online_async.yaml new file mode 100644 index 0000000..b1853e6 --- /dev/null +++ b/src/zbw_slamtoolbox/config/mapper_params_online_async.yaml @@ -0,0 +1,22 @@ +slam_toolbox: + ros__parameters: + + # Plugin params + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ROS Parameters + odom_frame: odom + map_frame: map + base_frame: base_link + scan_topic: /scan + + transform_timeout: 5.0 + tf_buffer_duration: 30.0 + + minimum_range: 0.2 # 雷达硬件最小测距 + queue_size: 50 \ No newline at end of file diff --git a/src/zbw_slamtoolbox/config/mapper_params_online_sync copy.yaml b/src/zbw_slamtoolbox/config/mapper_params_online_sync copy.yaml new file mode 100644 index 0000000..2a956f8 --- /dev/null +++ b/src/zbw_slamtoolbox/config/mapper_params_online_sync copy.yaml @@ -0,0 +1,77 @@ +slam_toolbox: + ros__parameters: + + # Plugin params + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ROS Parameters + odom_frame: odom + map_frame: map + base_frame: base_link + scan_topic: /scan + use_map_saver: true + mode: mapping #localization + + # if you'd like to immediately start continuing a map at a given pose + # or at the dock, but they are mutually exclusive, if pose is given + # will use pose + #map_file_name: test_steve + #map_start_pose: [0.0, 0.0, 0.0] + #map_start_at_dock: true + + debug_logging: false + throttle_scans: 2 + transform_publish_period: 0.02 #if 0 never publishes odometry + map_update_interval: 5.0 + resolution: 0.05 + min_laser_range: 0.0 #for rastering images + max_laser_range: 20.0 #for rastering images + minimum_time_interval: 0.5 + transform_timeout: 0.2 + tf_buffer_duration: 30. + stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + enable_interactive_mode: true + + # General Parameters + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.5 + minimum_travel_heading: 0.5 + scan_buffer_size: 10 + scan_buffer_maximum_scan_distance: 10.0 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + loop_search_maximum_distance: 3.0 + do_loop_closing: true + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + + # Correlation Parameters - Correlation Parameters + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + + # Correlation Parameters - Loop Closure Parameters + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + + # Scan Matcher Parameters + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true + min_pass_through: 2 + occupancy_threshold: 0.1 diff --git a/src/zbw_slamtoolbox/config/navigation.yaml b/src/zbw_slamtoolbox/config/navigation.yaml new file mode 100644 index 0000000..e69de29 diff --git a/src/zbw_slamtoolbox/launch/navigation.launch.py b/src/zbw_slamtoolbox/launch/navigation.launch.py new file mode 100644 index 0000000..8b28bf0 --- /dev/null +++ b/src/zbw_slamtoolbox/launch/navigation.launch.py @@ -0,0 +1,86 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +def generate_launch_description(): + # 获取包路径 + cyy_navigation2_dir = get_package_share_directory('cyy_navigation2') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + + # 声明参数 + use_sim_time = LaunchConfiguration('use_sim_time', default='False') + slam = LaunchConfiguration('slam', default='False') + localization = LaunchConfiguration('localization', default='False') + + # 定义文件路径 + map_yaml_path = os.path.join(cyy_navigation2_dir, 'maps', 'cyy_map.yaml') + nav2_param_path = os.path.join(cyy_navigation2_dir, 'param', 'nav2_params.yaml') + slam_params_file = os.path.join(cyy_navigation2_dir, 'param', 'slam_toolbox_localization.yaml') + + # 定义地图基础路径（用于 .posegraph 文件） + map_base_path = os.path.join(cyy_navigation2_dir, 'maps', 'cyy_map') + + # 检查 .posegraph 文件是否存在 + posegraph_path = map_base_path + '.posegraph' + if not os.path.exists(posegraph_path): + print(f"警告: .posegraph 文件不存在: {posegraph_path}") + print("将使用 AMCL 替代") + use_slam_toolbox = False + else: + use_slam_toolbox = True + print(f"找到 .posegraph 文件: {posegraph_path}") + + # 创建启动描述 + ld = LaunchDescription([ + DeclareLaunchArgument('use_sim_time', default_value='False'), + DeclareLaunchArgument('slam', default_value='False'), + DeclareLaunchArgument('localization', default_value='False'), + ]) + + # 如果使用 slam_toolbox + if use_slam_toolbox: + slam_toolbox_node = Node( + package='slam_toolbox', + executable='localization_slam_toolbox_node', + name='slam_toolbox', + output='screen', + parameters=[ + slam_params_file, + {'use_sim_time': use_sim_time}, + {'map_file_name': map_base_path}, + {'map_start_pose': [0.0, 0.0, 0.0]} + ], + remappings=[('/scan', '/scan')] + ) + ld.add_action(slam_toolbox_node) + # 使用 slam_toolbox 时，Nav2 不启用定位 + localization_arg = 'False' + else: + # 使用 AMCL + localization_arg = 'True' + + # Nav2 启动 - 使用正确的路径 + nav2_launch_path = os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py') + if not os.path.exists(nav2_launch_path): + print(f"错误: 找不到 Nav2 启动文件: {nav2_launch_path}") + return ld + + nav2_bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(nav2_launch_path), + launch_arguments={ + 'map': map_yaml_path, + 'use_sim_time': use_sim_time, + 'params_file': nav2_param_path, + 'slam': slam, + 'localization': localization_arg, + 'autostart': 'True', + }.items(), + ) + + ld.add_action(nav2_bringup_launch) + + return ld \ No newline at end of file diff --git a/src/zbw_slamtoolbox/launch/slamtoolbox.launch.py b/src/zbw_slamtoolbox/launch/slamtoolbox.launch.py new file mode 100644 index 0000000..be83830 --- /dev/null +++ b/src/zbw_slamtoolbox/launch/slamtoolbox.launch.py @@ -0,0 +1,27 @@ +#!/usr/bin/env python3 +from launch import LaunchDescription +from launch_ros.actions import Node, SetParameter +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + # 获取参数文件路径 + pkg_share = get_package_share_directory('zbw_slamtoolbox') + params_file = os.path.join(pkg_share, 'config', 'mapper_params_online_async.yaml') + + set_use_sim_time = SetParameter(name='use_sim_time', value=False) + + # 创建SLAM Toolbox节点 + slam_toolbox_node = Node( + package='slam_toolbox', + executable='async_slam_toolbox_node', # 或 sync_slam_toolbox_node + name='slam_toolbox', + output='screen', + parameters=[params_file] + ) + + + return LaunchDescription([ + set_use_sim_time, + slam_toolbox_node + ]) \ No newline at end of file diff --git a/src/zbw_slamtoolbox/package.xml b/src/zbw_slamtoolbox/package.xml new file mode 100644 index 0000000..0f1cc9c --- /dev/null +++ b/src/zbw_slamtoolbox/package.xml @@ -0,0 +1,18 @@ + + + + zbw_slamtoolbox + 0.0.0 + TODO: Package description + sunrise + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/zbw_slamtoolbox/CMakeLists.txt b/zbw_slamtoolbox/CMakeLists.txt new file mode 100644 index 0000000..6ec9f44 --- /dev/null +++ b/zbw_slamtoolbox/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.8) +project(zbw_slamtoolbox) + +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) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +install( + DIRECTORY launch config + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/zbw_slamtoolbox/config/mapper_params_online_async.yaml b/zbw_slamtoolbox/config/mapper_params_online_async.yaml new file mode 100644 index 0000000..b1853e6 --- /dev/null +++ b/zbw_slamtoolbox/config/mapper_params_online_async.yaml @@ -0,0 +1,22 @@ +slam_toolbox: + ros__parameters: + + # Plugin params + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ROS Parameters + odom_frame: odom + map_frame: map + base_frame: base_link + scan_topic: /scan + + transform_timeout: 5.0 + tf_buffer_duration: 30.0 + + minimum_range: 0.2 # 雷达硬件最小测距 + queue_size: 50 \ No newline at end of file diff --git a/zbw_slamtoolbox/config/mapper_params_online_sync copy.yaml b/zbw_slamtoolbox/config/mapper_params_online_sync copy.yaml new file mode 100644 index 0000000..2a956f8 --- /dev/null +++ b/zbw_slamtoolbox/config/mapper_params_online_sync copy.yaml @@ -0,0 +1,77 @@ +slam_toolbox: + ros__parameters: + + # Plugin params + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ROS Parameters + odom_frame: odom + map_frame: map + base_frame: base_link + scan_topic: /scan + use_map_saver: true + mode: mapping #localization + + # if you'd like to immediately start continuing a map at a given pose + # or at the dock, but they are mutually exclusive, if pose is given + # will use pose + #map_file_name: test_steve + #map_start_pose: [0.0, 0.0, 0.0] + #map_start_at_dock: true + + debug_logging: false + throttle_scans: 2 + transform_publish_period: 0.02 #if 0 never publishes odometry + map_update_interval: 5.0 + resolution: 0.05 + min_laser_range: 0.0 #for rastering images + max_laser_range: 20.0 #for rastering images + minimum_time_interval: 0.5 + transform_timeout: 0.2 + tf_buffer_duration: 30. + stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + enable_interactive_mode: true + + # General Parameters + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.5 + minimum_travel_heading: 0.5 + scan_buffer_size: 10 + scan_buffer_maximum_scan_distance: 10.0 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + loop_search_maximum_distance: 3.0 + do_loop_closing: true + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + + # Correlation Parameters - Correlation Parameters + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + + # Correlation Parameters - Loop Closure Parameters + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + + # Scan Matcher Parameters + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true + min_pass_through: 2 + occupancy_threshold: 0.1 diff --git a/zbw_slamtoolbox/config/navigation.yaml b/zbw_slamtoolbox/config/navigation.yaml new file mode 100644 index 0000000..e69de29 diff --git a/zbw_slamtoolbox/launch/navigation.launch.py b/zbw_slamtoolbox/launch/navigation.launch.py new file mode 100644 index 0000000..8b28bf0 --- /dev/null +++ b/zbw_slamtoolbox/launch/navigation.launch.py @@ -0,0 +1,86 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +def generate_launch_description(): + # 获取包路径 + cyy_navigation2_dir = get_package_share_directory('cyy_navigation2') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + + # 声明参数 + use_sim_time = LaunchConfiguration('use_sim_time', default='False') + slam = LaunchConfiguration('slam', default='False') + localization = LaunchConfiguration('localization', default='False') + + # 定义文件路径 + map_yaml_path = os.path.join(cyy_navigation2_dir, 'maps', 'cyy_map.yaml') + nav2_param_path = os.path.join(cyy_navigation2_dir, 'param', 'nav2_params.yaml') + slam_params_file = os.path.join(cyy_navigation2_dir, 'param', 'slam_toolbox_localization.yaml') + + # 定义地图基础路径（用于 .posegraph 文件） + map_base_path = os.path.join(cyy_navigation2_dir, 'maps', 'cyy_map') + + # 检查 .posegraph 文件是否存在 + posegraph_path = map_base_path + '.posegraph' + if not os.path.exists(posegraph_path): + print(f"警告: .posegraph 文件不存在: {posegraph_path}") + print("将使用 AMCL 替代") + use_slam_toolbox = False + else: + use_slam_toolbox = True + print(f"找到 .posegraph 文件: {posegraph_path}") + + # 创建启动描述 + ld = LaunchDescription([ + DeclareLaunchArgument('use_sim_time', default_value='False'), + DeclareLaunchArgument('slam', default_value='False'), + DeclareLaunchArgument('localization', default_value='False'), + ]) + + # 如果使用 slam_toolbox + if use_slam_toolbox: + slam_toolbox_node = Node( + package='slam_toolbox', + executable='localization_slam_toolbox_node', + name='slam_toolbox', + output='screen', + parameters=[ + slam_params_file, + {'use_sim_time': use_sim_time}, + {'map_file_name': map_base_path}, + {'map_start_pose': [0.0, 0.0, 0.0]} + ], + remappings=[('/scan', '/scan')] + ) + ld.add_action(slam_toolbox_node) + # 使用 slam_toolbox 时，Nav2 不启用定位 + localization_arg = 'False' + else: + # 使用 AMCL + localization_arg = 'True' + + # Nav2 启动 - 使用正确的路径 + nav2_launch_path = os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py') + if not os.path.exists(nav2_launch_path): + print(f"错误: 找不到 Nav2 启动文件: {nav2_launch_path}") + return ld + + nav2_bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(nav2_launch_path), + launch_arguments={ + 'map': map_yaml_path, + 'use_sim_time': use_sim_time, + 'params_file': nav2_param_path, + 'slam': slam, + 'localization': localization_arg, + 'autostart': 'True', + }.items(), + ) + + ld.add_action(nav2_bringup_launch) + + return ld \ No newline at end of file diff --git a/zbw_slamtoolbox/launch/slamtoolbox.launch.py b/zbw_slamtoolbox/launch/slamtoolbox.launch.py new file mode 100644 index 0000000..be83830 --- /dev/null +++ b/zbw_slamtoolbox/launch/slamtoolbox.launch.py @@ -0,0 +1,27 @@ +#!/usr/bin/env python3 +from launch import LaunchDescription +from launch_ros.actions import Node, SetParameter +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + # 获取参数文件路径 + pkg_share = get_package_share_directory('zbw_slamtoolbox') + params_file = os.path.join(pkg_share, 'config', 'mapper_params_online_async.yaml') + + set_use_sim_time = SetParameter(name='use_sim_time', value=False) + + # 创建SLAM Toolbox节点 + slam_toolbox_node = Node( + package='slam_toolbox', + executable='async_slam_toolbox_node', # 或 sync_slam_toolbox_node + name='slam_toolbox', + output='screen', + parameters=[params_file] + ) + + + return LaunchDescription([ + set_use_sim_time, + slam_toolbox_node + ]) \ No newline at end of file diff --git a/zbw_slamtoolbox/package.xml b/zbw_slamtoolbox/package.xml new file mode 100644 index 0000000..0f1cc9c --- /dev/null +++ b/zbw_slamtoolbox/package.xml @@ -0,0 +1,18 @@ + + + + zbw_slamtoolbox + 0.0.0 + TODO: Package description + sunrise + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + +