forked from zbw/yiliao2026
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
This commit is contained in:
52
bashes/README.md
Normal file
52
bashes/README.md
Normal file
@@ -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 雷达驱动自动切换程序`命令即可查看
|
||||||
125
bashes/radar-driver-switch.sh
Normal file
125
bashes/radar-driver-switch.sh
Normal file
@@ -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
|
||||||
5
dependencies/dependencies.txt
vendored
Normal file
5
dependencies/dependencies.txt
vendored
Normal file
@@ -0,0 +1,5 @@
|
|||||||
|
ros-humble-serial-driver
|
||||||
|
ros-humble-ackermann-msgs
|
||||||
|
# 从源码编译serial库
|
||||||
|
ros-humble-navigation2
|
||||||
|
ros-humble-slam-toolbox
|
||||||
44
src/LSLIDAR_X_ROS2-20240228/src/README.md
Normal file
44
src/LSLIDAR_X_ROS2-20240228/src/README.md
Normal file
@@ -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
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
@@ -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()
|
||||||
@@ -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 <http://www.gnu.org/licenses/>.
|
||||||
|
*
|
||||||
|
* 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 <unistd.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <pcap.h>
|
||||||
|
#include <netinet/in.h>
|
||||||
|
#include "rclcpp/rclcpp.hpp"
|
||||||
|
#include <lslidar_msgs/msg/lslidar_packet.hpp>
|
||||||
|
#include <string>
|
||||||
|
#include <sstream>
|
||||||
|
#include <sys/socket.h>
|
||||||
|
#include <arpa/inet.h>
|
||||||
|
#include <poll.h>
|
||||||
|
#include <errno.h>
|
||||||
|
#include <fcntl.h>
|
||||||
|
#include <sys/file.h>
|
||||||
|
#include <signal.h>
|
||||||
|
#include <sensor_msgs/msg/time_reference.hpp>
|
||||||
|
#include <std_msgs/msg/int8.hpp>
|
||||||
|
#include <cmath>
|
||||||
|
|
||||||
|
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
|
||||||
@@ -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 <sys/types.h>
|
||||||
|
#include <sys/stat.h>
|
||||||
|
#include <fcntl.h>
|
||||||
|
#include <termios.h>
|
||||||
|
#include <errno.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <fstream>
|
||||||
|
#include <iostream>
|
||||||
|
|
||||||
|
//波特率
|
||||||
|
#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
|
||||||
|
|
||||||
@@ -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 <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef LSLIDAR_DRIVER_H
|
||||||
|
#define LSLIDAR_DRIVER_H
|
||||||
|
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <netinet/in.h>
|
||||||
|
#include <string>
|
||||||
|
|
||||||
|
#include <boost/shared_ptr.hpp>
|
||||||
|
#include <boost/date_time/posix_time/posix_time.hpp>
|
||||||
|
#include <boost/thread.hpp>
|
||||||
|
#include "rclcpp/rclcpp.hpp"
|
||||||
|
#include <thread>
|
||||||
|
#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<LslidarDriver> LslidarDriverPtr;
|
||||||
|
typedef std::shared_ptr<const LslidarDriver> 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<ScanPoint> &points, rclcpp::Time &scan_time, float &scan_duration);
|
||||||
|
|
||||||
|
boost::thread *pubscan_thread_ ;
|
||||||
|
boost::shared_ptr<Input> 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<ScanPoint> scan_points_;
|
||||||
|
std::vector<ScanPoint> scan_points_bak_;
|
||||||
|
// Diagnostics updater
|
||||||
|
diagnostic_updater::Updater diagnostics;
|
||||||
|
std::shared_ptr<diagnostic_updater::TopicDiagnostic> diag_topic;
|
||||||
|
double diag_min_freq;
|
||||||
|
double diag_max_freq;
|
||||||
|
rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan_pub;
|
||||||
|
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr point_cloud_pub;
|
||||||
|
rclcpp::Subscription<std_msgs::msg::Int8>::SharedPtr difop_switch;
|
||||||
|
LSIOSR * serial_;
|
||||||
|
};
|
||||||
|
typedef PointXYZIT VPoint;
|
||||||
|
typedef pcl::PointCloud<VPoint> 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_
|
||||||
@@ -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,
|
||||||
|
])
|
||||||
|
|
||||||
@@ -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,
|
||||||
|
])
|
||||||
|
|
||||||
@@ -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,
|
||||||
|
])
|
||||||
|
|
||||||
@@ -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,
|
||||||
|
])
|
||||||
|
|
||||||
@@ -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,
|
||||||
|
])
|
||||||
|
|
||||||
@@ -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,
|
||||||
|
])
|
||||||
|
|
||||||
@@ -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,
|
||||||
|
])
|
||||||
|
|
||||||
@@ -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,
|
||||||
|
])
|
||||||
|
|
||||||
@@ -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,
|
||||||
|
])
|
||||||
|
|
||||||
@@ -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,
|
||||||
|
])
|
||||||
|
|
||||||
36
src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/package.xml
Normal file
36
src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/package.xml
Normal file
@@ -0,0 +1,36 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<package format="2">
|
||||||
|
<name>lslidar_driver</name>
|
||||||
|
<version>1.2.0</version>
|
||||||
|
<description>ROS device driver for Leishen lidar.</description>
|
||||||
|
<maintainer email="shaohuashu@lslidar.com">Nick Shu</maintainer>
|
||||||
|
<author>Nick Shu</author>
|
||||||
|
<license>GNU General Public License V3.0</license>
|
||||||
|
|
||||||
|
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||||
|
|
||||||
|
<build_depend>rclcpp</build_depend>
|
||||||
|
<build_depend>std_msgs</build_depend>
|
||||||
|
<build_depend>lslidar_msgs</build_depend>
|
||||||
|
<build_depend>pcl_conversions</build_depend>
|
||||||
|
<build_depend>rclpy</build_depend>
|
||||||
|
<build_depend>libpcap</build_depend>
|
||||||
|
<build_depend>libpcl-all-dev</build_depend>
|
||||||
|
<build_depend>pluginlib</build_depend>
|
||||||
|
<build_depend>sensor_msgs</build_depend>
|
||||||
|
|
||||||
|
<exec_depend>rclcpp</exec_depend>
|
||||||
|
<exec_depend>std_msgs</exec_depend>
|
||||||
|
<exec_depend>lslidar_msgs</exec_depend>
|
||||||
|
<exec_depend>pcl_conversions</exec_depend>
|
||||||
|
<exec_depend>rclpy</exec_depend>
|
||||||
|
<exec_depend>libpcap</exec_depend>
|
||||||
|
<exec_depend>libpcl-all</exec_depend>
|
||||||
|
<exec_depend>pluginlib</exec_depend>
|
||||||
|
<exec_depend>sensor_msgs</exec_depend>
|
||||||
|
|
||||||
|
<depend>diagnostic_updater</depend>
|
||||||
|
<export>
|
||||||
|
<build_type>ament_cmake</build_type>
|
||||||
|
</export>
|
||||||
|
</package>
|
||||||
@@ -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包读取功能
|
||||||
@@ -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包读取功能
|
||||||
@@ -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文件读取功能
|
||||||
@@ -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文件读取功能
|
||||||
@@ -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文件读取功能
|
||||||
@@ -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文件读取功能
|
||||||
@@ -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文件读取功能
|
||||||
@@ -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文件读取功能
|
||||||
161
src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/rviz/lslidar.rviz
Normal file
161
src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/rviz/lslidar.rviz
Normal file
@@ -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: <Fixed 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: <Fixed 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
|
||||||
398
src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/input.cc
Normal file
398
src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/input.cc
Normal file
@@ -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<std::string>("device_ip","192.168.1.102");
|
||||||
|
private_nh->declare_parameter<std::string>("device_ip_difop","192.168.1.200");
|
||||||
|
private_nh->declare_parameter<bool>("add_multicast",false);
|
||||||
|
private_nh->declare_parameter<std::string>("group_ip","224.1.1.2");
|
||||||
|
private_nh->declare_parameter<int>("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
|
||||||
400
src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lsiosr.cpp
Normal file
400
src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lsiosr.cpp
Normal file
@@ -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;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
1423
src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver.cc
Normal file
1423
src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver.cc
Normal file
File diff suppressed because it is too large
Load Diff
@@ -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 <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#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<lslidar_driver::LslidarDriver>();
|
||||||
|
|
||||||
|
while (rclcpp::ok() && node->polling()) {
|
||||||
|
rclcpp::spin_some(node);
|
||||||
|
}
|
||||||
|
//rclcpp::spin(node);
|
||||||
|
rclcpp::shutdown();
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
39
src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/CMakeLists.txt
Normal file
39
src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/CMakeLists.txt
Normal file
@@ -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()
|
||||||
@@ -0,0 +1,2 @@
|
|||||||
|
int64 temperature
|
||||||
|
int64 rpm
|
||||||
@@ -0,0 +1,5 @@
|
|||||||
|
# Raw Leishen LIDAR packet.
|
||||||
|
|
||||||
|
builtin_interfaces/Time stamp # packet timestamp
|
||||||
|
uint8[2000] data # packet contents
|
||||||
|
|
||||||
@@ -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
|
||||||
@@ -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
|
||||||
@@ -0,0 +1,4 @@
|
|||||||
|
std_msgs/Header header
|
||||||
|
|
||||||
|
# The 0th scan is at the bottom
|
||||||
|
LslidarScan[16] scans
|
||||||
25
src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/package.xml
Normal file
25
src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/package.xml
Normal file
@@ -0,0 +1,25 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<package format="3">
|
||||||
|
<name>lslidar_msgs</name>
|
||||||
|
<version>1.2.0</version>
|
||||||
|
<description>ROS message definitions for Leishen LIDARs.</description>
|
||||||
|
<maintainer email="shaohuashu@lslidar.com">Nick Shu</maintainer>
|
||||||
|
<author>Nick Shu</author>
|
||||||
|
<license>GNU General Public License V3.0</license>
|
||||||
|
|
||||||
|
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||||
|
<buildtool_depend>rosidl_default_generators</buildtool_depend>
|
||||||
|
|
||||||
|
<exec_depend>rosidl_default_runtime</exec_depend>
|
||||||
|
<exec_depend>builtin_interfaces</exec_depend>
|
||||||
|
|
||||||
|
<depend>std_msgs</depend>
|
||||||
|
<test_depend>ament_lint_auto</test_depend>
|
||||||
|
<test_depend>ament_lint_common</test_depend>
|
||||||
|
|
||||||
|
<member_of_group>rosidl_interface_packages</member_of_group>
|
||||||
|
|
||||||
|
<export>
|
||||||
|
<build_type>ament_cmake</build_type>
|
||||||
|
</export>
|
||||||
|
</package>
|
||||||
20
src/LSLIDAR_X_ROS2-20240228/src/version.txt
Normal file
20
src/LSLIDAR_X_ROS2-20240228/src/version.txt
Normal file
@@ -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
|
||||||
|
***************************************************************/
|
||||||
|
|
||||||
33
src/LSLIDAR_X_ROS2-20240228/src/wheeltec_udev.sh
Normal file
33
src/LSLIDAR_X_ROS2-20240228/src/wheeltec_udev.sh
Normal file
@@ -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
|
||||||
|
|
||||||
|
|
||||||
BIN
src/LSLIDAR_X_ROS2-20240228/src/镭神Lsx雷达旋转角度.png
Normal file
BIN
src/LSLIDAR_X_ROS2-20240228/src/镭神Lsx雷达旋转角度.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 6.5 KiB |
62
src/LSLIDAR_X_ROS2-20240228/wheeltec_lidar.launch.py
Normal file
62
src/LSLIDAR_X_ROS2-20240228/wheeltec_lidar.launch.py
Normal file
@@ -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
|
||||||
|
|
||||||
29
src/cyy_navigation2/CMakeLists.txt
Normal file
29
src/cyy_navigation2/CMakeLists.txt
Normal file
@@ -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()
|
||||||
|
|
||||||
21
src/cyy_navigation2/bt/follow_point.xml
Normal file
21
src/cyy_navigation2/bt/follow_point.xml
Normal file
@@ -0,0 +1,21 @@
|
|||||||
|
<!--
|
||||||
|
This Behavior Tree follows a dynamic pose to a certain distance
|
||||||
|
-->
|
||||||
|
|
||||||
|
<root main_tree_to_execute="MainTree">
|
||||||
|
<BehaviorTree ID="MainTree">
|
||||||
|
<PipelineSequence name="NavigateWithReplanning">
|
||||||
|
<RateController hz="1.0">
|
||||||
|
<Sequence>
|
||||||
|
<GoalUpdater input_goal="{goal}" output_goal="{updated_goal}">
|
||||||
|
<ComputePathToPose goal="{updated_goal}" path="{path}" planner_id="GridBased"/>
|
||||||
|
</GoalUpdater>
|
||||||
|
<TruncatePath distance="1.0" input_path="{path}" output_path="{truncated_path}"/>
|
||||||
|
</Sequence>
|
||||||
|
</RateController>
|
||||||
|
<KeepRunningUntilFailure>
|
||||||
|
<FollowPath path="{truncated_path}" controller_id="FollowPath"/>
|
||||||
|
</KeepRunningUntilFailure>
|
||||||
|
</PipelineSequence>
|
||||||
|
</BehaviorTree>
|
||||||
|
</root>
|
||||||
@@ -0,0 +1,46 @@
|
|||||||
|
<!--
|
||||||
|
This Behavior Tree replans the global path once every 15 seconds or if the path becomes invalid. It also has
|
||||||
|
recovery actions specific to planning / control as well as general system issues.
|
||||||
|
This will be continuous if a kinematically valid planner is selected.
|
||||||
|
-->
|
||||||
|
<root main_tree_to_execute="MainTree">
|
||||||
|
<BehaviorTree ID="MainTree">
|
||||||
|
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
|
||||||
|
<PipelineSequence name="NavigateWithReplanning">
|
||||||
|
<RateController hz="2.0">
|
||||||
|
<RecoveryNode number_of_retries="1" name="ComputePathToPose">
|
||||||
|
<Fallback>
|
||||||
|
<ReactiveSequence>
|
||||||
|
<Inverter>
|
||||||
|
<PathExpiringTimer seconds="10" path="{path}"/>
|
||||||
|
</Inverter>
|
||||||
|
<Inverter>
|
||||||
|
<GlobalUpdatedGoal/>
|
||||||
|
</Inverter>
|
||||||
|
<IsPathValid path="{path}"/>
|
||||||
|
</ReactiveSequence>
|
||||||
|
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||||
|
</Fallback>
|
||||||
|
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||||
|
</RecoveryNode>
|
||||||
|
</RateController>
|
||||||
|
<RecoveryNode number_of_retries="1" name="FollowPath">
|
||||||
|
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||||
|
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||||
|
</RecoveryNode>
|
||||||
|
</PipelineSequence>
|
||||||
|
<ReactiveFallback name="RecoveryFallback">
|
||||||
|
<GoalUpdated/>
|
||||||
|
<RoundRobin name="RecoveryActions">
|
||||||
|
<Sequence name="ClearingActions">
|
||||||
|
<ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||||
|
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||||
|
</Sequence>
|
||||||
|
<Spin spin_dist="1.57"/>
|
||||||
|
<Wait wait_duration="5"/>
|
||||||
|
<BackUp backup_dist="0.30" backup_speed="0.05"/>
|
||||||
|
</RoundRobin>
|
||||||
|
</ReactiveFallback>
|
||||||
|
</RecoveryNode>
|
||||||
|
</BehaviorTree>
|
||||||
|
</root>
|
||||||
@@ -0,0 +1,38 @@
|
|||||||
|
|
||||||
|
<!--
|
||||||
|
This Behavior Tree replans the global path periodically at 1 Hz through an array of poses continuously
|
||||||
|
and it also has recovery actions specific to planning / control as well as general system issues.
|
||||||
|
-->
|
||||||
|
<root main_tree_to_execute="MainTree">
|
||||||
|
<BehaviorTree ID="MainTree">
|
||||||
|
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
|
||||||
|
<PipelineSequence name="NavigateWithReplanning">
|
||||||
|
<RateController hz="0.333">
|
||||||
|
<RecoveryNode number_of_retries="1" name="ComputePathThroughPoses">
|
||||||
|
<ReactiveSequence>
|
||||||
|
<RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7"/>
|
||||||
|
<ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="GridBased"/>
|
||||||
|
</ReactiveSequence>
|
||||||
|
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||||
|
</RecoveryNode>
|
||||||
|
</RateController>
|
||||||
|
<RecoveryNode number_of_retries="1" name="FollowPath">
|
||||||
|
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||||
|
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||||
|
</RecoveryNode>
|
||||||
|
</PipelineSequence>
|
||||||
|
<ReactiveFallback name="RecoveryFallback">
|
||||||
|
<GoalUpdated/>
|
||||||
|
<RoundRobin name="RecoveryActions">
|
||||||
|
<Sequence name="ClearingActions">
|
||||||
|
<ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||||
|
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||||
|
</Sequence>
|
||||||
|
<Spin spin_dist="1.57"/>
|
||||||
|
<Wait wait_duration="5"/>
|
||||||
|
<BackUp backup_dist="0.30" backup_speed="0.05"/>
|
||||||
|
</RoundRobin>
|
||||||
|
</ReactiveFallback>
|
||||||
|
</RecoveryNode>
|
||||||
|
</BehaviorTree>
|
||||||
|
</root>
|
||||||
@@ -0,0 +1,36 @@
|
|||||||
|
|
||||||
|
<!--
|
||||||
|
This Behavior Tree replans the global path periodically at 1 Hz and it also has
|
||||||
|
recovery actions specific to planning / control as well as general system issues.
|
||||||
|
This will be continuous if a kinematically valid planner is selected.
|
||||||
|
-->
|
||||||
|
<root main_tree_to_execute="MainTree">
|
||||||
|
<BehaviorTree ID="MainTree">
|
||||||
|
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
|
||||||
|
<PipelineSequence name="NavigateWithReplanning">
|
||||||
|
<RateController hz="1.0">
|
||||||
|
<RecoveryNode number_of_retries="1" name="ComputePathToPose">
|
||||||
|
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||||
|
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||||
|
</RecoveryNode>
|
||||||
|
</RateController>
|
||||||
|
<RecoveryNode number_of_retries="1" name="FollowPath">
|
||||||
|
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||||
|
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||||
|
</RecoveryNode>
|
||||||
|
</PipelineSequence>
|
||||||
|
<ReactiveFallback name="RecoveryFallback">
|
||||||
|
<GoalUpdated/>
|
||||||
|
<RoundRobin name="RecoveryActions">
|
||||||
|
<Sequence name="ClearingActions">
|
||||||
|
<ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||||
|
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||||
|
</Sequence>
|
||||||
|
<Spin spin_dist="1.57"/>
|
||||||
|
<Wait wait_duration="5"/>
|
||||||
|
<BackUp backup_dist="0.30" backup_speed="0.05"/>
|
||||||
|
</RoundRobin>
|
||||||
|
</ReactiveFallback>
|
||||||
|
</RecoveryNode>
|
||||||
|
</BehaviorTree>
|
||||||
|
</root>
|
||||||
@@ -0,0 +1,47 @@
|
|||||||
|
|
||||||
|
<!--
|
||||||
|
This BT has all the functionalities of navigate_to_pose_w_replanning_and_recovery.xml,
|
||||||
|
with an additional feature to cancel the control closer to the goal proximity and
|
||||||
|
make the robot wait for a specific time, to see if the obstacle clears out before
|
||||||
|
navigating along a significantly longer path to reach the goal location.
|
||||||
|
-->
|
||||||
|
<root main_tree_to_execute="MainTree">
|
||||||
|
<BehaviorTree ID="MainTree">
|
||||||
|
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
|
||||||
|
<PipelineSequence name="NavigateWithReplanning">
|
||||||
|
<RateController hz="1.0">
|
||||||
|
<RecoveryNode number_of_retries="1" name="ComputePathToPose">
|
||||||
|
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||||
|
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||||
|
</RecoveryNode>
|
||||||
|
</RateController>
|
||||||
|
<ReactiveSequence name="MonitorAndFollowPath">
|
||||||
|
<PathLongerOnApproach path="{path}" prox_len="3.0" length_factor="2.0">
|
||||||
|
<RetryUntilSuccessful num_attempts="1">
|
||||||
|
<SequenceStar name="CancelingControlAndWait">
|
||||||
|
<CancelControl name="ControlCancel"/>
|
||||||
|
<Wait wait_duration="5"/>
|
||||||
|
</SequenceStar>
|
||||||
|
</RetryUntilSuccessful>
|
||||||
|
</PathLongerOnApproach>
|
||||||
|
<RecoveryNode number_of_retries="1" name="FollowPath">
|
||||||
|
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||||
|
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||||
|
</RecoveryNode>
|
||||||
|
</ReactiveSequence>
|
||||||
|
</PipelineSequence>
|
||||||
|
<ReactiveFallback name="RecoveryFallback">
|
||||||
|
<GoalUpdated/>
|
||||||
|
<RoundRobin name="RecoveryActions">
|
||||||
|
<Sequence name="ClearingActions">
|
||||||
|
<ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||||
|
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||||
|
</Sequence>
|
||||||
|
<Spin spin_dist="1.57"/>
|
||||||
|
<Wait wait_duration="5"/>
|
||||||
|
<BackUp backup_dist="0.30" backup_speed="0.05"/>
|
||||||
|
</RoundRobin>
|
||||||
|
</ReactiveFallback>
|
||||||
|
</RecoveryNode>
|
||||||
|
</BehaviorTree>
|
||||||
|
</root>
|
||||||
@@ -0,0 +1,44 @@
|
|||||||
|
|
||||||
|
<!--
|
||||||
|
This Behavior Tree replans the global path only if the path becomes invalid and it also has
|
||||||
|
recovery actions specific to planning / control as well as general system issues.
|
||||||
|
This will be continuous if a kinematically valid planner is selected.
|
||||||
|
-->
|
||||||
|
<root main_tree_to_execute="MainTree">
|
||||||
|
<BehaviorTree ID="MainTree">
|
||||||
|
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
|
||||||
|
<PipelineSequence>
|
||||||
|
<RateController hz="1.0" name="RateControllerComputePathToPose">
|
||||||
|
<RecoveryNode number_of_retries="1" name="RecoveryComputePathToPose">
|
||||||
|
<Fallback name="FallbackComputePathToPose">
|
||||||
|
<ReactiveSequence name="CheckIfNewPathNeeded">
|
||||||
|
<Inverter>
|
||||||
|
<GlobalUpdatedGoal/>
|
||||||
|
</Inverter>
|
||||||
|
<IsPathValid path="{path}"/>
|
||||||
|
</ReactiveSequence>
|
||||||
|
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||||
|
</Fallback>
|
||||||
|
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||||
|
</RecoveryNode>
|
||||||
|
</RateController>
|
||||||
|
<RecoveryNode number_of_retries="1" name="RecoveryFollowPath">
|
||||||
|
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||||
|
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||||
|
</RecoveryNode>
|
||||||
|
</PipelineSequence>
|
||||||
|
<ReactiveFallback name="FallbackRecoveries">
|
||||||
|
<GoalUpdated/>
|
||||||
|
<RoundRobin name="RecoveryActions">
|
||||||
|
<Sequence name="ClearingActions">
|
||||||
|
<ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||||
|
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||||
|
</Sequence>
|
||||||
|
<Spin name="SpinRecovery" spin_dist="1.57"/>
|
||||||
|
<Wait name="WaitRecovery" wait_duration="5"/>
|
||||||
|
<BackUp name="BackUpRecovery" backup_dist="0.30" backup_speed="0.05"/>
|
||||||
|
</RoundRobin>
|
||||||
|
</ReactiveFallback>
|
||||||
|
</RecoveryNode>
|
||||||
|
</BehaviorTree>
|
||||||
|
</root>
|
||||||
14
src/cyy_navigation2/bt/navigate_w_replanning_distance.xml
Normal file
14
src/cyy_navigation2/bt/navigate_w_replanning_distance.xml
Normal file
@@ -0,0 +1,14 @@
|
|||||||
|
<!--
|
||||||
|
This Behavior Tree replans the global path after every 1m.
|
||||||
|
-->
|
||||||
|
|
||||||
|
<root main_tree_to_execute="MainTree">
|
||||||
|
<BehaviorTree ID="MainTree">
|
||||||
|
<PipelineSequence name="NavigateWithReplanning">
|
||||||
|
<DistanceController distance="1.0">
|
||||||
|
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||||
|
</DistanceController>
|
||||||
|
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||||
|
</PipelineSequence>
|
||||||
|
</BehaviorTree>
|
||||||
|
</root>
|
||||||
@@ -0,0 +1,14 @@
|
|||||||
|
<!--
|
||||||
|
This Behavior Tree replans the global path only when the goal is updated.
|
||||||
|
-->
|
||||||
|
|
||||||
|
<root main_tree_to_execute="MainTree">
|
||||||
|
<BehaviorTree ID="MainTree">
|
||||||
|
<PipelineSequence name="NavigateWithReplanning">
|
||||||
|
<GoalUpdatedController>
|
||||||
|
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||||
|
</GoalUpdatedController>
|
||||||
|
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||||
|
</PipelineSequence>
|
||||||
|
</BehaviorTree>
|
||||||
|
</root>
|
||||||
@@ -0,0 +1,21 @@
|
|||||||
|
<!--
|
||||||
|
This Behavior Tree replans the global path only if the path becomes invalid
|
||||||
|
-->
|
||||||
|
<root main_tree_to_execute="MainTree">
|
||||||
|
<BehaviorTree ID="MainTree">
|
||||||
|
<PipelineSequence name="NavigateWithReplanning">
|
||||||
|
<RateController hz="1.0">
|
||||||
|
<Fallback>
|
||||||
|
<ReactiveSequence>
|
||||||
|
<Inverter>
|
||||||
|
<GlobalUpdatedGoal/>
|
||||||
|
</Inverter>
|
||||||
|
<IsPathValid path="{path}"/>
|
||||||
|
</ReactiveSequence>
|
||||||
|
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||||
|
</Fallback>
|
||||||
|
</RateController>
|
||||||
|
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||||
|
</PipelineSequence>
|
||||||
|
</BehaviorTree>
|
||||||
|
</root>
|
||||||
14
src/cyy_navigation2/bt/navigate_w_replanning_speed.xml
Normal file
14
src/cyy_navigation2/bt/navigate_w_replanning_speed.xml
Normal file
@@ -0,0 +1,14 @@
|
|||||||
|
<!--
|
||||||
|
This Behavior Tree replans the global path periodically proprortional to speed.
|
||||||
|
-->
|
||||||
|
|
||||||
|
<root main_tree_to_execute="MainTree">
|
||||||
|
<BehaviorTree ID="MainTree">
|
||||||
|
<PipelineSequence name="NavigateWithReplanning">
|
||||||
|
<SpeedController min_rate="0.1" max_rate="1.0" min_speed="0.0" max_speed="0.26">
|
||||||
|
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||||
|
</SpeedController>
|
||||||
|
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||||
|
</PipelineSequence>
|
||||||
|
</BehaviorTree>
|
||||||
|
</root>
|
||||||
14
src/cyy_navigation2/bt/navigate_w_replanning_time.xml
Normal file
14
src/cyy_navigation2/bt/navigate_w_replanning_time.xml
Normal file
@@ -0,0 +1,14 @@
|
|||||||
|
<!--
|
||||||
|
This Behavior Tree replans the global path periodically at 1 Hz.
|
||||||
|
-->
|
||||||
|
|
||||||
|
<root main_tree_to_execute="MainTree">
|
||||||
|
<BehaviorTree ID="MainTree">
|
||||||
|
<PipelineSequence name="NavigateWithReplanning">
|
||||||
|
<RateController hz="1.0">
|
||||||
|
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||||
|
</RateController>
|
||||||
|
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||||
|
</PipelineSequence>
|
||||||
|
</BehaviorTree>
|
||||||
|
</root>
|
||||||
20
src/cyy_navigation2/bt/odometry_calibration.xml
Normal file
20
src/cyy_navigation2/bt/odometry_calibration.xml
Normal file
@@ -0,0 +1,20 @@
|
|||||||
|
<!--
|
||||||
|
his Behavior Tree drives in a square for odometry calibration experiments
|
||||||
|
-->
|
||||||
|
|
||||||
|
<root main_tree_to_execute="MainTree">
|
||||||
|
<BehaviorTree ID="MainTree">
|
||||||
|
<Repeat num_cycles="3">
|
||||||
|
<Sequence name="Drive in a square">
|
||||||
|
<DriveOnHeading dist_to_travel="2.0" speed="0.2" time_allowance="12"/>
|
||||||
|
<Spin spin_dist="1.570796" is_recovery="false"/>
|
||||||
|
<DriveOnHeading dist_to_travel="2.0" speed="0.2" time_allowance="12"/>
|
||||||
|
<Spin spin_dist="1.570796" is_recovery="false"/>
|
||||||
|
<DriveOnHeading dist_to_travel="2.0" speed="0.2" time_allowance="12"/>
|
||||||
|
<Spin spin_dist="1.570796" is_recovery="false"/>
|
||||||
|
<DriveOnHeading dist_to_travel="2.0" speed="0.2" time_allowance="12"/>
|
||||||
|
<Spin spin_dist="1.570796" is_recovery="false"/>
|
||||||
|
</Sequence>
|
||||||
|
</Repeat>
|
||||||
|
</BehaviorTree>
|
||||||
|
</root>
|
||||||
348
src/cyy_navigation2/config/nav2_params.yaml
Normal file
348
src/cyy_navigation2/config/nav2_params.yaml
Normal file
@@ -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
|
||||||
108
src/cyy_navigation2/launch/car_bringup.launch.py
Normal file
108
src/cyy_navigation2/launch/car_bringup.launch.py
Normal file
@@ -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
|
||||||
41
src/cyy_navigation2/launch/cyy_nav.launch.py
Normal file
41
src/cyy_navigation2/launch/cyy_nav.launch.py
Normal file
@@ -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])
|
||||||
86
src/cyy_navigation2/launch/cyy_nav_box.launch.py
Normal file
86
src/cyy_navigation2/launch/cyy_nav_box.launch.py
Normal file
@@ -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
|
||||||
BIN
src/cyy_navigation2/maps/cyy_map.data
Normal file
BIN
src/cyy_navigation2/maps/cyy_map.data
Normal file
Binary file not shown.
BIN
src/cyy_navigation2/maps/cyy_map.pgm
Normal file
BIN
src/cyy_navigation2/maps/cyy_map.pgm
Normal file
Binary file not shown.
7
src/cyy_navigation2/maps/cyy_map.yaml
Normal file
7
src/cyy_navigation2/maps/cyy_map.yaml
Normal file
@@ -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
|
||||||
BIN
src/cyy_navigation2/maps/cyy_map1.pgm
Normal file
BIN
src/cyy_navigation2/maps/cyy_map1.pgm
Normal file
Binary file not shown.
7
src/cyy_navigation2/maps/cyy_map1.yaml
Normal file
7
src/cyy_navigation2/maps/cyy_map1.yaml
Normal file
@@ -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
|
||||||
20
src/cyy_navigation2/package.xml
Normal file
20
src/cyy_navigation2/package.xml
Normal file
@@ -0,0 +1,20 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||||
|
<package format="3">
|
||||||
|
<name>cyy_navigation2</name>
|
||||||
|
<version>0.0.0</version>
|
||||||
|
<description>TODO: Package description</description>
|
||||||
|
<maintainer email="root@todo.todo">root</maintainer>
|
||||||
|
<license>TODO: License declaration</license>
|
||||||
|
|
||||||
|
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||||
|
|
||||||
|
<depend>nav2_bringup</depend>
|
||||||
|
|
||||||
|
<test_depend>ament_lint_auto</test_depend>
|
||||||
|
<test_depend>ament_lint_common</test_depend>
|
||||||
|
|
||||||
|
<export>
|
||||||
|
<build_type>ament_cmake</build_type>
|
||||||
|
</export>
|
||||||
|
</package>
|
||||||
353
src/cyy_navigation2/param/nav2_params.yaml
Normal file
353
src/cyy_navigation2/param/nav2_params.yaml
Normal file
@@ -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
|
||||||
54
src/cyy_navigation2/param/slam_toolbox_localization.yaml
Normal file
54
src/cyy_navigation2/param/slam_toolbox_localization.yaml
Normal file
@@ -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
|
||||||
61
src/cyy_slamtoolbox/CMakeLists.txt
Normal file
61
src/cyy_slamtoolbox/CMakeLists.txt
Normal file
@@ -0,0 +1,61 @@
|
|||||||
|
# 设置CMake最低版本要求
|
||||||
|
cmake_minimum_required(VERSION 3.8)
|
||||||
|
# 定义项目名称（应与package.xml中的<name>一致）
|
||||||
|
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/<package_name>）
|
||||||
|
# install(TARGETS origincar_slam_node
|
||||||
|
# DESTINATION lib/${PROJECT_NAME})
|
||||||
|
|
||||||
|
# 安装launch文件到share/<package_name>/launch目录
|
||||||
|
install(DIRECTORY launch
|
||||||
|
DESTINATION share/${PROJECT_NAME}
|
||||||
|
PATTERN "*.py"
|
||||||
|
)
|
||||||
|
|
||||||
|
# 安装配置文件（如YAML参数文件）到share/<package_name>/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()
|
||||||
9
src/cyy_slamtoolbox/config/angular_filter_example.yaml
Normal file
9
src/cyy_slamtoolbox/config/angular_filter_example.yaml
Normal file
@@ -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
|
||||||
|
|
||||||
15
src/cyy_slamtoolbox/config/box_filter_example.yaml
Normal file
15
src/cyy_slamtoolbox/config/box_filter_example.yaml
Normal file
@@ -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
|
||||||
7
src/cyy_slamtoolbox/config/footprint_filter_example.yaml
Normal file
7
src/cyy_slamtoolbox/config/footprint_filter_example.yaml
Normal file
@@ -0,0 +1,7 @@
|
|||||||
|
scan_to_scan_filter_chain:
|
||||||
|
ros__parameters:
|
||||||
|
filter1:
|
||||||
|
name: footprint_filter
|
||||||
|
type: laser_filters/LaserScanFootprintFilter
|
||||||
|
params:
|
||||||
|
inscribed_radius: 0.325
|
||||||
9
src/cyy_slamtoolbox/config/intensity_filter_example.yaml
Normal file
9
src/cyy_slamtoolbox/config/intensity_filter_example.yaml
Normal file
@@ -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
|
||||||
19
src/cyy_slamtoolbox/config/laser_filter_config.yaml
Normal file
19
src/cyy_slamtoolbox/config/laser_filter_config.yaml
Normal file
@@ -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)
|
||||||
87
src/cyy_slamtoolbox/config/mapper_params_lifelong.yaml
Normal file
87
src/cyy_slamtoolbox/config/mapper_params_lifelong.yaml
Normal file
@@ -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
|
||||||
70
src/cyy_slamtoolbox/config/mapper_params_localization.yaml
Normal file
70
src/cyy_slamtoolbox/config/mapper_params_localization.yaml
Normal file
@@ -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
|
||||||
69
src/cyy_slamtoolbox/config/mapper_params_offline.yaml
Normal file
69
src/cyy_slamtoolbox/config/mapper_params_offline.yaml
Normal file
@@ -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
|
||||||
78
src/cyy_slamtoolbox/config/mapper_params_online_async.yaml
Normal file
78
src/cyy_slamtoolbox/config/mapper_params_online_async.yaml
Normal file
@@ -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
|
||||||
77
src/cyy_slamtoolbox/config/mapper_params_online_sync.yaml
Normal file
77
src/cyy_slamtoolbox/config/mapper_params_online_sync.yaml
Normal file
@@ -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
|
||||||
16
src/cyy_slamtoolbox/config/mask_filter_example.yaml
Normal file
16
src/cyy_slamtoolbox/config/mask_filter_example.yaml
Normal file
@@ -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.
|
||||||
20
src/cyy_slamtoolbox/config/median_filter_example.yaml
Normal file
20
src/cyy_slamtoolbox/config/median_filter_example.yaml
Normal file
@@ -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
|
||||||
@@ -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
|
||||||
42
src/cyy_slamtoolbox/config/multiple_filters_example.yaml
Normal file
42
src/cyy_slamtoolbox/config/multiple_filters_example.yaml
Normal file
@@ -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
|
||||||
10
src/cyy_slamtoolbox/config/polygon_filter_example.yaml
Normal file
10
src/cyy_slamtoolbox/config/polygon_filter_example.yaml
Normal file
@@ -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
|
||||||
11
src/cyy_slamtoolbox/config/range_filter_example.yaml
Normal file
11
src/cyy_slamtoolbox/config/range_filter_example.yaml
Normal file
@@ -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
|
||||||
8
src/cyy_slamtoolbox/config/scan_blob_filter_example.yaml
Normal file
8
src/cyy_slamtoolbox/config/scan_blob_filter_example.yaml
Normal file
@@ -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
|
||||||
12
src/cyy_slamtoolbox/config/sector_filter_example.yaml
Normal file
12
src/cyy_slamtoolbox/config/sector_filter_example.yaml
Normal file
@@ -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
|
||||||
18
src/cyy_slamtoolbox/config/shadow_filter_example.yaml
Normal file
18
src/cyy_slamtoolbox/config/shadow_filter_example.yaml
Normal file
@@ -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
|
||||||
|
|
||||||
137
src/cyy_slamtoolbox/config/slam_toolbox_default.rviz
Normal file
137
src/cyy_slamtoolbox/config/slam_toolbox_default.rviz
Normal file
@@ -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: <Fixed 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: <Fixed 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
|
||||||
19
src/cyy_slamtoolbox/config/speckle_filter_example.yaml
Normal file
19
src/cyy_slamtoolbox/config/speckle_filter_example.yaml
Normal file
@@ -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
|
||||||
24
src/cyy_slamtoolbox/launch/cyy_slam_toolbox_launch.launch.py
Normal file
24
src/cyy_slamtoolbox/launch/cyy_slam_toolbox_launch.launch.py
Normal file
@@ -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
|
||||||
|
])
|
||||||
@@ -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
|
||||||
|
])
|
||||||
24
src/cyy_slamtoolbox/launch/filter.launch.py
Normal file
24
src/cyy_slamtoolbox/launch/filter.launch.py
Normal file
@@ -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])
|
||||||
18
src/cyy_slamtoolbox/package.xml
Normal file
18
src/cyy_slamtoolbox/package.xml
Normal file
@@ -0,0 +1,18 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||||
|
<package format="3">
|
||||||
|
<name>cyy_slamtoolbox</name>
|
||||||
|
<version>0.0.0</version>
|
||||||
|
<description>TODO: Package description</description>
|
||||||
|
<maintainer email="root@todo.todo">root</maintainer>
|
||||||
|
<license>TODO: License declaration</license>
|
||||||
|
|
||||||
|
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||||
|
|
||||||
|
<test_depend>ament_lint_auto</test_depend>
|
||||||
|
<test_depend>ament_lint_common</test_depend>
|
||||||
|
|
||||||
|
<export>
|
||||||
|
<build_type>ament_cmake</build_type>
|
||||||
|
</export>
|
||||||
|
</package>
|
||||||
29
src/gc_navigation_fish/CMakeLists.txt
Normal file
29
src/gc_navigation_fish/CMakeLists.txt
Normal file
@@ -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(<dependency> 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()
|
||||||
34
src/gc_navigation_fish/launch/gc_navigation.launch.py
Normal file
34
src/gc_navigation_fish/launch/gc_navigation.launch.py
Normal file
@@ -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])
|
||||||
BIN
src/gc_navigation_fish/maps/test_map.pgm
Normal file
BIN
src/gc_navigation_fish/maps/test_map.pgm
Normal file
Binary file not shown.
7
src/gc_navigation_fish/maps/test_map.yaml
Normal file
7
src/gc_navigation_fish/maps/test_map.yaml
Normal file
@@ -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
|
||||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user