迁移了原来的nav2包；新增基于obstacle_scanner构建costmap的nav2，暂未实际测试
This commit is contained in:
61
src/navigation/cyy_slamtoolbox/CMakeLists.txt
Normal file
61
src/navigation/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()
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
@@ -0,0 +1,7 @@
|
||||
scan_to_scan_filter_chain:
|
||||
ros__parameters:
|
||||
filter1:
|
||||
name: footprint_filter
|
||||
type: laser_filters/LaserScanFootprintFilter
|
||||
params:
|
||||
inscribed_radius: 0.325
|
||||
@@ -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
|
||||
@@ -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)
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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.
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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/navigation/cyy_slamtoolbox/config/slam_toolbox_default.rviz
Normal file
137
src/navigation/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
|
||||
@@ -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
|
||||
@@ -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/navigation/cyy_slamtoolbox/launch/filter.launch.py
Normal file
24
src/navigation/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/navigation/cyy_slamtoolbox/package.xml
Normal file
18
src/navigation/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>
|
||||
Reference in New Issue
Block a user