add&fix：修改了文件名称以及添加了slam定位的卵巢文件
This commit is contained in:
陌欢醉
@@ -1,29 +0,0 @@
|
|||||||
cmake_minimum_required(VERSION 3.5)
|
|
||||||
project(gc_bringup)
|
|
||||||
|
|
||||||
if(NOT CMAKE_C_STANDARD)
|
|
||||||
set(CMAKE_C_STANDARD 99)
|
|
||||||
endif()
|
|
||||||
|
|
||||||
if(NOT CMAKE_CXX_STANDARD)
|
|
||||||
set(CMAKE_CXX_STANDARD 17)
|
|
||||||
endif()
|
|
||||||
|
|
||||||
find_package(ament_cmake REQUIRED)
|
|
||||||
|
|
||||||
install(
|
|
||||||
DIRECTORY launch config maps rviz
|
|
||||||
DESTINATION share/${PROJECT_NAME}
|
|
||||||
)
|
|
||||||
|
|
||||||
install(PROGRAMS
|
|
||||||
scripts/nav_goal.py
|
|
||||||
DESTINATION lib/${PROJECT_NAME}
|
|
||||||
)
|
|
||||||
|
|
||||||
if(BUILD_TESTING)
|
|
||||||
find_package(ament_lint_auto REQUIRED)
|
|
||||||
ament_lint_auto_find_test_dependencies()
|
|
||||||
endif()
|
|
||||||
|
|
||||||
ament_package()
|
|
||||||
@@ -1,77 +0,0 @@
|
|||||||
# gc_bringup
|
|
||||||
|
|
||||||
GC 机器人启动包 — 阿克曼底盘 + 激光雷达 + SLAM 建图 + AMCL 导航
|
|
||||||
|
|
||||||
## 📁 目录结构
|
|
||||||
|
|
||||||
```
|
|
||||||
gc_bringup/
|
|
||||||
├── launch/
|
|
||||||
│ ├── slam_mapping.launch.py # SLAM 在线建图
|
|
||||||
│ ├── navigation.launch.py # AMCL 定位 + Nav2 导航
|
|
||||||
│ └── navigation_slam.launch.py # slam_toolbox 定位 + Nav2 导航
|
|
||||||
├── config/
|
|
||||||
│ ├── nav2_params.yaml # Nav2 参数（阿克曼配置）
|
|
||||||
│ ├── mapper_params_online_async.yaml # slam_toolbox 建图参数
|
|
||||||
│ └── mapper_params_localization.yaml # slam_toolbox 定位参数
|
|
||||||
├── maps/ # 地图文件存放
|
|
||||||
├── rviz/ # RViz2 配置文件
|
|
||||||
├── CMakeLists.txt
|
|
||||||
└── package.xml
|
|
||||||
```
|
|
||||||
|
|
||||||
## 🚀 使用方式
|
|
||||||
|
|
||||||
### 1. 编译
|
|
||||||
```bash
|
|
||||||
cd ~/test_ws
|
|
||||||
colcon build --packages-select gc_bringup
|
|
||||||
source install/setup.bash
|
|
||||||
```
|
|
||||||
|
|
||||||
### 2. SLAM 建图
|
|
||||||
启动 Gazebo + 机器人 + slam_toolbox 在线异步建图：
|
|
||||||
```bash
|
|
||||||
ros2 launch gc_bringup slam_mapping.launch.py
|
|
||||||
```
|
|
||||||
|
|
||||||
在 RViz 中使用 "2D Nav Goal" 工具驱动机器人探索环境建图。
|
|
||||||
|
|
||||||
保存地图：
|
|
||||||
```bash
|
|
||||||
ros2 run nav2_map_server map_saver_cli -f ~/test_ws/src/gc_bringup/maps/my_map
|
|
||||||
```
|
|
||||||
|
|
||||||
### 3. AMCL 导航（推荐）
|
|
||||||
使用建好的地图进行 AMCL 定位 + Nav2 导航：
|
|
||||||
```bash
|
|
||||||
ros2 launch gc_bringup navigation.launch.py
|
|
||||||
```
|
|
||||||
|
|
||||||
操作步骤：
|
|
||||||
1. 在 RViz 中使用 "2D Pose Estimate" 设置初始位姿
|
|
||||||
2. 使用 "2D Nav Goal" 设置导航目标
|
|
||||||
|
|
||||||
### 4. slam_toolbox 定位导航
|
|
||||||
使用 slam_toolbox 定位模式（需要 .posegraph 文件）：
|
|
||||||
```bash
|
|
||||||
ros2 launch gc_bringup navigation_slam.launch.py
|
|
||||||
```
|
|
||||||
|
|
||||||
## 🔧 启动参数
|
|
||||||
|
|
||||||
| 参数 | 默认值 | 说明 |
|
|
||||||
|------|--------|------|
|
|
||||||
| `use_sim_time` | `true` | 使用仿真时间 |
|
|
||||||
| `use_gazebo` | `true` | 启动 Gazebo |
|
|
||||||
| `use_rviz` | `true` | 启动 RViz2 |
|
|
||||||
| `world` | `origincar_description/world/gc_world.world` | Gazebo 世界文件 |
|
|
||||||
| `map` | `gc_bringup/maps/my_map.yaml` | 导航用地图 |
|
|
||||||
|
|
||||||
## 🤖 机器人说明
|
|
||||||
|
|
||||||
- **底盘类型**: 阿克曼转向 (Ackermann)
|
|
||||||
- **传感器**: 360° 激光雷达 (LiDAR)、IMU
|
|
||||||
- **控制器**: MPPI (Ackermann 运动模型)
|
|
||||||
- **规划器**: Smac Hybrid (Reeds-Shepp，支持倒车)
|
|
||||||
- **定位**: AMCL (自适应蒙特卡洛定位) 或 slam_toolbox 定位模式
|
|
||||||
@@ -1,60 +0,0 @@
|
|||||||
# =============================================================================
|
|
||||||
# slam_toolbox 定位模式参数
|
|
||||||
# 用于在已有地图上进行定位（替代 AMCL）
|
|
||||||
# =============================================================================
|
|
||||||
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 参数
|
|
||||||
odom_frame: odom
|
|
||||||
map_frame: map
|
|
||||||
base_frame: base_footprint
|
|
||||||
scan_topic: /scan
|
|
||||||
use_sim_time: True
|
|
||||||
mode: localization
|
|
||||||
|
|
||||||
# 调试
|
|
||||||
debug_logging: false
|
|
||||||
throttle_scans: 1
|
|
||||||
transform_publish_period: 0.02
|
|
||||||
map_update_interval: 10.0
|
|
||||||
resolution: 0.05
|
|
||||||
max_laser_range: 3.5
|
|
||||||
minimum_time_interval: 0.5
|
|
||||||
transform_timeout: 0.2
|
|
||||||
tf_buffer_duration: 30.0
|
|
||||||
stack_size_to_use: 40000000
|
|
||||||
|
|
||||||
# 定位模式不需要回环检测
|
|
||||||
loop_search_maximum_distance: 3.0
|
|
||||||
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_search_space_dimension: 0.5
|
|
||||||
correlation_search_space_resolution: 0.01
|
|
||||||
correlation_search_space_smear_deviation: 0.1
|
|
||||||
|
|
||||||
# 回环闭合
|
|
||||||
loop_search_space_dimension: 8.0
|
|
||||||
loop_search_space_resolution: 0.05
|
|
||||||
loop_search_space_smear_deviation: 0.03
|
|
||||||
|
|
||||||
# 扫描匹配器
|
|
||||||
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
|
|
||||||
@@ -1,61 +0,0 @@
|
|||||||
# =============================================================================
|
|
||||||
# slam_toolbox 在线异步建图参数
|
|
||||||
# 用于实时 SLAM 建图
|
|
||||||
# =============================================================================
|
|
||||||
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 参数
|
|
||||||
odom_frame: odom
|
|
||||||
map_frame: map
|
|
||||||
base_frame: base_footprint
|
|
||||||
scan_topic: /scan
|
|
||||||
use_sim_time: True
|
|
||||||
mode: mapping
|
|
||||||
|
|
||||||
# 调试
|
|
||||||
debug_logging: false
|
|
||||||
throttle_scans: 1
|
|
||||||
transform_publish_period: 0.02
|
|
||||||
map_update_interval: 5.0
|
|
||||||
resolution: 0.05
|
|
||||||
max_laser_range: 3.5
|
|
||||||
minimum_time_interval: 0.5
|
|
||||||
transform_timeout: 0.2
|
|
||||||
tf_buffer_duration: 30.0
|
|
||||||
stack_size_to_use: 40000000
|
|
||||||
enable_interactive_mode: true
|
|
||||||
|
|
||||||
# 回环检测
|
|
||||||
loop_search_maximum_distance: 3.0
|
|
||||||
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_search_space_dimension: 0.5
|
|
||||||
correlation_search_space_resolution: 0.01
|
|
||||||
correlation_search_space_smear_deviation: 0.1
|
|
||||||
|
|
||||||
# 回环闭合
|
|
||||||
loop_search_space_dimension: 8.0
|
|
||||||
loop_search_space_resolution: 0.05
|
|
||||||
loop_search_space_smear_deviation: 0.03
|
|
||||||
|
|
||||||
# 扫描匹配器
|
|
||||||
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
|
|
||||||
@@ -1,411 +0,0 @@
|
|||||||
# =============================================================================
|
|
||||||
# Nav2 参数 — 阿克曼底盘 + AMCL + MPPI
|
|
||||||
# =============================================================================
|
|
||||||
|
|
||||||
amcl:
|
|
||||||
ros__parameters:
|
|
||||||
use_sim_time: True
|
|
||||||
alpha1: 0.2
|
|
||||||
alpha2: 0.2
|
|
||||||
alpha3: 0.2
|
|
||||||
alpha4: 0.2
|
|
||||||
alpha5: 0.2
|
|
||||||
base_frame_id: "base_footprint"
|
|
||||||
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
|
|
||||||
# ★ 自动设定初始位姿 — 不设置的话 map→odom 的 TF 不会发布
|
|
||||||
set_initial_pose: true
|
|
||||||
initial_pose:
|
|
||||||
x: 0.0
|
|
||||||
y: 0.0
|
|
||||||
z: 0.0
|
|
||||||
yaw: 0.0
|
|
||||||
robot_model_type: "nav2_amcl::DifferentialMotionModel"
|
|
||||||
save_pose_rate: 0.5
|
|
||||||
sigma_hit: 0.2
|
|
||||||
tf_broadcast: true
|
|
||||||
transform_tolerance: 10.0
|
|
||||||
update_min_a: 0.2
|
|
||||||
update_min_d: 0.25
|
|
||||||
z_hit: 0.5
|
|
||||||
z_max: 0.05
|
|
||||||
z_rand: 0.5
|
|
||||||
z_short: 0.05
|
|
||||||
scan_topic: scan
|
|
||||||
|
|
||||||
amcl_map_client:
|
|
||||||
ros__parameters:
|
|
||||||
use_sim_time: True
|
|
||||||
|
|
||||||
amcl_rclcpp_node:
|
|
||||||
ros__parameters:
|
|
||||||
use_sim_time: True
|
|
||||||
|
|
||||||
bt_navigator:
|
|
||||||
ros__parameters:
|
|
||||||
use_sim_time: True
|
|
||||||
global_frame: map
|
|
||||||
robot_base_frame: base_footprint
|
|
||||||
odom_topic: /odom
|
|
||||||
bt_loop_duration: 10
|
|
||||||
default_server_timeout: 20
|
|
||||||
plugin_lib_names:
|
|
||||||
- nav2_compute_path_to_pose_action_bt_node
|
|
||||||
- nav2_compute_path_through_poses_action_bt_node
|
|
||||||
- nav2_smooth_path_action_bt_node
|
|
||||||
- nav2_follow_path_action_bt_node
|
|
||||||
- nav2_spin_action_bt_node
|
|
||||||
- nav2_wait_action_bt_node
|
|
||||||
- nav2_back_up_action_bt_node
|
|
||||||
- nav2_drive_on_heading_bt_node
|
|
||||||
- nav2_clear_costmap_service_bt_node
|
|
||||||
- nav2_is_stuck_condition_bt_node
|
|
||||||
- nav2_goal_reached_condition_bt_node
|
|
||||||
- nav2_goal_updated_condition_bt_node
|
|
||||||
- nav2_globally_updated_goal_condition_bt_node
|
|
||||||
- nav2_is_path_valid_condition_bt_node
|
|
||||||
- nav2_initial_pose_received_condition_bt_node
|
|
||||||
- nav2_reinitialize_global_localization_service_bt_node
|
|
||||||
- nav2_rate_controller_bt_node
|
|
||||||
- nav2_distance_controller_bt_node
|
|
||||||
- nav2_speed_controller_bt_node
|
|
||||||
- nav2_truncate_path_action_bt_node
|
|
||||||
- nav2_truncate_path_local_action_bt_node
|
|
||||||
- nav2_goal_updater_node_bt_node
|
|
||||||
- nav2_recovery_node_bt_node
|
|
||||||
- nav2_pipeline_sequence_bt_node
|
|
||||||
- nav2_round_robin_node_bt_node
|
|
||||||
- nav2_transform_available_condition_bt_node
|
|
||||||
- nav2_time_expired_condition_bt_node
|
|
||||||
- nav2_path_expiring_timer_condition
|
|
||||||
- nav2_distance_traveled_condition_bt_node
|
|
||||||
- nav2_single_trigger_bt_node
|
|
||||||
- nav2_is_battery_low_condition_bt_node
|
|
||||||
- nav2_navigate_through_poses_action_bt_node
|
|
||||||
- nav2_navigate_to_pose_action_bt_node
|
|
||||||
- nav2_remove_passed_goals_action_bt_node
|
|
||||||
- nav2_planner_selector_bt_node
|
|
||||||
- nav2_controller_selector_bt_node
|
|
||||||
- nav2_goal_checker_selector_bt_node
|
|
||||||
- nav2_controller_cancel_bt_node
|
|
||||||
- nav2_path_longer_on_approach_bt_node
|
|
||||||
- nav2_wait_cancel_bt_node
|
|
||||||
- nav2_spin_cancel_bt_node
|
|
||||||
- nav2_back_up_cancel_bt_node
|
|
||||||
- nav2_drive_on_heading_cancel_bt_node
|
|
||||||
|
|
||||||
bt_navigator_rclcpp_node:
|
|
||||||
ros__parameters:
|
|
||||||
use_sim_time: True
|
|
||||||
|
|
||||||
controller_server:
|
|
||||||
ros__parameters:
|
|
||||||
use_sim_time: True
|
|
||||||
controller_frequency: 30.0
|
|
||||||
controller_plugins: ["FollowPath"]
|
|
||||||
progress_checker:
|
|
||||||
plugin: "nav2_controller::SimpleProgressChecker"
|
|
||||||
required_movement_radius: 0.5
|
|
||||||
movement_time_allowance: 10.0
|
|
||||||
goal_checker:
|
|
||||||
plugin: "nav2_controller::SimpleGoalChecker"
|
|
||||||
xy_goal_tolerance: 0.25
|
|
||||||
yaw_goal_tolerance: 0.25
|
|
||||||
FollowPath:
|
|
||||||
plugin: "nav2_mppi_controller::MPPIController"
|
|
||||||
time_steps: 56
|
|
||||||
model_dt: 0.05
|
|
||||||
batch_size: 2000
|
|
||||||
vx_std: 0.2
|
|
||||||
vy_std: 0.0
|
|
||||||
wz_std: 0.4
|
|
||||||
vx_max: 0.5
|
|
||||||
vx_min: -0.35
|
|
||||||
vy_max: 0.0
|
|
||||||
wz_max: 1.9
|
|
||||||
iteration_count: 1
|
|
||||||
temperature: 0.3
|
|
||||||
gamma: 0.015
|
|
||||||
motion_model: "Ackermann"
|
|
||||||
visualize: false
|
|
||||||
TrajectoryVisualizer:
|
|
||||||
trajectory_step: 5
|
|
||||||
time_step: 3
|
|
||||||
AckermannConstraints:
|
|
||||||
min_turning_r: 0.4
|
|
||||||
critics:
|
|
||||||
- "ConstraintCritic"
|
|
||||||
- "CostCritic"
|
|
||||||
- "GoalCritic"
|
|
||||||
- "GoalAngleCritic"
|
|
||||||
- "PathAlignCritic"
|
|
||||||
- "PathFollowCritic"
|
|
||||||
- "PathAngleCritic"
|
|
||||||
- "PreferForwardCritic"
|
|
||||||
ConstraintCritic:
|
|
||||||
enabled: true
|
|
||||||
cost_power: 1
|
|
||||||
cost_weight: 4.0
|
|
||||||
GoalCritic:
|
|
||||||
enabled: true
|
|
||||||
cost_power: 1
|
|
||||||
cost_weight: 5.0
|
|
||||||
threshold_to_consider: 1.4
|
|
||||||
GoalAngleCritic:
|
|
||||||
enabled: true
|
|
||||||
cost_power: 1
|
|
||||||
cost_weight: 3.0
|
|
||||||
threshold_to_consider: 0.5
|
|
||||||
PreferForwardCritic:
|
|
||||||
enabled: true
|
|
||||||
cost_power: 1
|
|
||||||
cost_weight: 5.0
|
|
||||||
threshold_to_consider: 0.5
|
|
||||||
CostCritic:
|
|
||||||
enabled: true
|
|
||||||
cost_power: 1
|
|
||||||
cost_weight: 3.81
|
|
||||||
critical_cost: 300.0
|
|
||||||
consider_footprint: true
|
|
||||||
collision_cost: 1000000.0
|
|
||||||
near_goal_distance: 1.0
|
|
||||||
trajectory_point_step: 2
|
|
||||||
PathAlignCritic:
|
|
||||||
enabled: true
|
|
||||||
cost_power: 1
|
|
||||||
cost_weight: 14.0
|
|
||||||
max_path_occupancy_ratio: 0.05
|
|
||||||
trajectory_point_step: 4
|
|
||||||
threshold_to_consider: 0.5
|
|
||||||
offset_from_furthest: 20
|
|
||||||
use_path_orientations: false
|
|
||||||
PathFollowCritic:
|
|
||||||
enabled: true
|
|
||||||
cost_power: 1
|
|
||||||
cost_weight: 5.0
|
|
||||||
offset_from_furthest: 5
|
|
||||||
threshold_to_consider: 1.4
|
|
||||||
PathAngleCritic:
|
|
||||||
enabled: true
|
|
||||||
cost_power: 1
|
|
||||||
cost_weight: 2.0
|
|
||||||
offset_from_furthest: 4
|
|
||||||
threshold_to_consider: 0.5
|
|
||||||
max_angle_to_furthest: 1.0
|
|
||||||
forward_preference: true
|
|
||||||
|
|
||||||
controller_server_rclcpp_node:
|
|
||||||
ros__parameters:
|
|
||||||
use_sim_time: True
|
|
||||||
|
|
||||||
planner_server:
|
|
||||||
ros__parameters:
|
|
||||||
planner_plugins: ["GridBased"]
|
|
||||||
use_sim_time: True
|
|
||||||
GridBased:
|
|
||||||
plugin: "nav2_smac_planner/SmacPlannerHybrid"
|
|
||||||
downsample_costmap: false
|
|
||||||
downsampling_factor: 1
|
|
||||||
tolerance: 0.25
|
|
||||||
allow_unknown: true
|
|
||||||
max_iterations: 1000000
|
|
||||||
max_on_approach_iterations: 1000
|
|
||||||
max_planning_time: 5.0
|
|
||||||
motion_model_for_search: "REEDS_SHEPP"
|
|
||||||
angle_quantization_bins: 72
|
|
||||||
analytic_expansion_ratio: 3.5
|
|
||||||
analytic_expansion_max_length: 3.0
|
|
||||||
minimum_turning_radius: 0.40
|
|
||||||
reverse_penalty: 2.0
|
|
||||||
change_penalty: 0.0
|
|
||||||
non_straight_penalty: 1.2
|
|
||||||
cost_penalty: 2.0
|
|
||||||
retrospective_penalty: 0.015
|
|
||||||
lookup_table_size: 20.0
|
|
||||||
cache_obstacle_heuristic: false
|
|
||||||
viz_expansions: false
|
|
||||||
smooth_path: True
|
|
||||||
smoother:
|
|
||||||
max_iterations: 1000
|
|
||||||
w_smooth: 0.3
|
|
||||||
w_data: 0.2
|
|
||||||
tolerance: 1.0e-10
|
|
||||||
do_refinement: true
|
|
||||||
refinement_num: 2
|
|
||||||
|
|
||||||
planner_server_rclcpp_node:
|
|
||||||
ros__parameters:
|
|
||||||
use_sim_time: 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", "wait"]
|
|
||||||
spin:
|
|
||||||
plugin: "nav2_behaviors/Spin"
|
|
||||||
backup:
|
|
||||||
plugin: "nav2_behaviors/BackUp"
|
|
||||||
wait:
|
|
||||||
plugin: "nav2_behaviors/Wait"
|
|
||||||
global_frame: odom
|
|
||||||
robot_base_frame: base_footprint
|
|
||||||
transform_tolerance: 0.5
|
|
||||||
use_sim_time: True
|
|
||||||
simulate_ahead_time: 2.0
|
|
||||||
max_rotational_vel: 1.0
|
|
||||||
min_rotational_vel: 0.4
|
|
||||||
rotational_acc_lim: 3.2
|
|
||||||
|
|
||||||
behavior_server_rclcpp_node:
|
|
||||||
ros__parameters:
|
|
||||||
use_sim_time: True
|
|
||||||
|
|
||||||
local_costmap:
|
|
||||||
local_costmap:
|
|
||||||
ros__parameters:
|
|
||||||
update_frequency: 5.0
|
|
||||||
publish_frequency: 2.0
|
|
||||||
transform_tolerance: 0.5
|
|
||||||
global_frame: odom
|
|
||||||
robot_base_frame: base_footprint
|
|
||||||
use_sim_time: True
|
|
||||||
rolling_window: true
|
|
||||||
width: 3
|
|
||||||
height: 3
|
|
||||||
resolution: 0.05
|
|
||||||
footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]"
|
|
||||||
footprint_padding: 0.02
|
|
||||||
plugins: ["voxel_layer", "inflation_layer"]
|
|
||||||
inflation_layer:
|
|
||||||
plugin: "nav2_costmap_2d::InflationLayer"
|
|
||||||
cost_scaling_factor: 3.0
|
|
||||||
inflation_radius: 0.55
|
|
||||||
voxel_layer:
|
|
||||||
plugin: "nav2_costmap_2d::VoxelLayer"
|
|
||||||
enabled: True
|
|
||||||
publish_voxel_map: True
|
|
||||||
origin_z: 0.0
|
|
||||||
z_resolution: 0.05
|
|
||||||
z_voxels: 16
|
|
||||||
max_obstacle_height: 2.0
|
|
||||||
mark_threshold: 0
|
|
||||||
observation_sources: scan
|
|
||||||
scan:
|
|
||||||
topic: /scan
|
|
||||||
max_obstacle_height: 2.0
|
|
||||||
clearing: True
|
|
||||||
marking: True
|
|
||||||
data_type: "LaserScan"
|
|
||||||
raytrace_max_range: 3.0
|
|
||||||
raytrace_min_range: 0.0
|
|
||||||
obstacle_max_range: 2.5
|
|
||||||
obstacle_min_range: 0.0
|
|
||||||
static_layer:
|
|
||||||
map_subscribe_transient_local: True
|
|
||||||
always_send_full_costmap: True
|
|
||||||
local_costmap_client:
|
|
||||||
ros__parameters:
|
|
||||||
use_sim_time: True
|
|
||||||
local_costmap_rclcpp_node:
|
|
||||||
ros__parameters:
|
|
||||||
use_sim_time: True
|
|
||||||
|
|
||||||
global_costmap:
|
|
||||||
global_costmap:
|
|
||||||
ros__parameters:
|
|
||||||
use_sim_time: True
|
|
||||||
transform_tolerance: 0.5
|
|
||||||
update_frequency: 1.0
|
|
||||||
publish_frequency: 1.0
|
|
||||||
global_frame: map
|
|
||||||
robot_base_frame: base_footprint
|
|
||||||
footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]"
|
|
||||||
footprint_padding: 0.02
|
|
||||||
resolution: 0.05
|
|
||||||
track_unknown_space: true
|
|
||||||
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
|
|
||||||
obstacle_layer:
|
|
||||||
plugin: "nav2_costmap_2d::ObstacleLayer"
|
|
||||||
enabled: True
|
|
||||||
observation_sources: scan
|
|
||||||
scan:
|
|
||||||
topic: /scan
|
|
||||||
max_obstacle_height: 2.0
|
|
||||||
clearing: True
|
|
||||||
marking: True
|
|
||||||
data_type: "LaserScan"
|
|
||||||
raytrace_max_range: 3.0
|
|
||||||
raytrace_min_range: 0.0
|
|
||||||
obstacle_max_range: 2.5
|
|
||||||
obstacle_min_range: 0.0
|
|
||||||
static_layer:
|
|
||||||
plugin: "nav2_costmap_2d::StaticLayer"
|
|
||||||
map_subscribe_transient_local: True
|
|
||||||
inflation_layer:
|
|
||||||
plugin: "nav2_costmap_2d::InflationLayer"
|
|
||||||
cost_scaling_factor: 3.0
|
|
||||||
inflation_radius: 0.55
|
|
||||||
always_send_full_costmap: True
|
|
||||||
global_costmap_client:
|
|
||||||
ros__parameters:
|
|
||||||
use_sim_time: True
|
|
||||||
global_costmap_rclcpp_node:
|
|
||||||
ros__parameters:
|
|
||||||
use_sim_time: True
|
|
||||||
|
|
||||||
map_server:
|
|
||||||
ros__parameters:
|
|
||||||
use_sim_time: True
|
|
||||||
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
|
|
||||||
|
|
||||||
robot_state_publisher:
|
|
||||||
ros__parameters:
|
|
||||||
use_sim_time: True
|
|
||||||
|
|
||||||
waypoint_follower:
|
|
||||||
ros__parameters:
|
|
||||||
loop_rate: 20
|
|
||||||
use_sim_time: True
|
|
||||||
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
|
|
||||||
@@ -1,211 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
# =============================================================================
|
|
||||||
# gc_bringup — AMCL 导航模式 (直接启动 Nav2 节点)
|
|
||||||
# 启动 Gazebo + 机器人 + Nav2 (AMCL 定位)
|
|
||||||
# =============================================================================
|
|
||||||
import os
|
|
||||||
from ament_index_python.packages import get_package_share_directory
|
|
||||||
from launch import LaunchDescription
|
|
||||||
from launch.actions import (DeclareLaunchArgument, ExecuteProcess,
|
|
||||||
TimerAction)
|
|
||||||
from launch.conditions import IfCondition
|
|
||||||
from launch.substitutions import LaunchConfiguration, Command
|
|
||||||
from launch_ros.actions import Node
|
|
||||||
from launch_ros.parameter_descriptions import ParameterValue
|
|
||||||
|
|
||||||
|
|
||||||
def generate_launch_description():
|
|
||||||
bringup_dir = get_package_share_directory('gc_bringup')
|
|
||||||
origincar_dir = get_package_share_directory('origincar_description')
|
|
||||||
|
|
||||||
# ==================== 参数 ====================
|
|
||||||
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
|
|
||||||
use_gazebo = LaunchConfiguration('use_gazebo', default='true')
|
|
||||||
use_rviz = LaunchConfiguration('use_rviz', default='false')
|
|
||||||
world = LaunchConfiguration('world', default=os.path.join(
|
|
||||||
origincar_dir, 'world', 'gc_world.world'))
|
|
||||||
map_yaml = LaunchConfiguration('map', default=os.path.join(
|
|
||||||
bringup_dir, 'maps', 'my_map.yaml'))
|
|
||||||
|
|
||||||
declare_use_sim_time = DeclareLaunchArgument('use_sim_time', default_value='true')
|
|
||||||
declare_use_gazebo = DeclareLaunchArgument('use_gazebo', default_value='true')
|
|
||||||
declare_use_rviz = DeclareLaunchArgument('use_rviz', default_value='false')
|
|
||||||
declare_world = DeclareLaunchArgument(
|
|
||||||
'world', default_value=os.path.join(origincar_dir, 'world', 'gc_world.world'))
|
|
||||||
declare_map = DeclareLaunchArgument(
|
|
||||||
'map', default_value=os.path.join(bringup_dir, 'maps', 'my_map.yaml'))
|
|
||||||
|
|
||||||
# ==================== 机器人模型 ====================
|
|
||||||
default_model_path = os.path.join(origincar_dir, 'urdf', 'origincar.urdf')
|
|
||||||
robot_description = ParameterValue(
|
|
||||||
Command(['xacro ', default_model_path]), value_type=str)
|
|
||||||
|
|
||||||
|
|
||||||
# ==================== Gazebo ====================
|
|
||||||
start_gazebo = ExecuteProcess(
|
|
||||||
condition=IfCondition(use_gazebo),
|
|
||||||
cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so',
|
|
||||||
'-s', 'libgazebo_ros_factory.so', world],
|
|
||||||
output='screen')
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
spawn_robot = Node(
|
|
||||||
condition=IfCondition(use_gazebo),
|
|
||||||
package='gazebo_ros',
|
|
||||||
executable='spawn_entity.py',
|
|
||||||
arguments=['-entity', 'mycar', '-file', default_model_path,
|
|
||||||
'-x', '0.0', '-y', '0.0', '-z', '0.1'],
|
|
||||||
output='screen')
|
|
||||||
|
|
||||||
# ==================== TF & 机器人状态 ====================
|
|
||||||
robot_state_publisher = Node(
|
|
||||||
package='robot_state_publisher',
|
|
||||||
executable='robot_state_publisher',
|
|
||||||
name='robot_state_publisher',
|
|
||||||
parameters=[{'robot_description': robot_description,
|
|
||||||
'use_sim_time': True, 'publish_frequency': 30.0}],
|
|
||||||
output='screen')
|
|
||||||
|
|
||||||
joint_state_publisher = Node(
|
|
||||||
package='joint_state_publisher',
|
|
||||||
executable='joint_state_publisher',
|
|
||||||
name='joint_state_publisher',
|
|
||||||
parameters=[{'use_sim_time': True}], output='screen')
|
|
||||||
|
|
||||||
# ==================== Nav2 参数文件路径 ====================
|
|
||||||
nav2_params = os.path.join(bringup_dir, 'config', 'nav2_params.yaml')
|
|
||||||
|
|
||||||
# ==================== Map Server ====================
|
|
||||||
map_server = Node(
|
|
||||||
package='nav2_map_server',
|
|
||||||
executable='map_server',
|
|
||||||
name='map_server',
|
|
||||||
output='screen',
|
|
||||||
parameters=[nav2_params,
|
|
||||||
{'use_sim_time': True, 'yaml_filename': map_yaml}],
|
|
||||||
ros_arguments=['-p', 'use_sim_time:=True'],
|
|
||||||
)
|
|
||||||
|
|
||||||
# ==================== AMCL ====================
|
|
||||||
amcl = Node(
|
|
||||||
package='nav2_amcl',
|
|
||||||
executable='amcl',
|
|
||||||
name='amcl',
|
|
||||||
output='screen',
|
|
||||||
parameters=[nav2_params, {'use_sim_time': True}],
|
|
||||||
remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')],
|
|
||||||
ros_arguments=['-p', 'use_sim_time:=True'],
|
|
||||||
)
|
|
||||||
|
|
||||||
# ==================== Planner Server ====================
|
|
||||||
planner_server = Node(
|
|
||||||
package='nav2_planner',
|
|
||||||
executable='planner_server',
|
|
||||||
name='planner_server',
|
|
||||||
output='screen',
|
|
||||||
parameters=[nav2_params, {'use_sim_time': True}],
|
|
||||||
remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')],
|
|
||||||
ros_arguments=['-p', 'use_sim_time:=True'],
|
|
||||||
)
|
|
||||||
|
|
||||||
# ==================== Controller Server ====================
|
|
||||||
controller_server = Node(
|
|
||||||
package='nav2_controller',
|
|
||||||
executable='controller_server',
|
|
||||||
name='controller_server',
|
|
||||||
output='screen',
|
|
||||||
parameters=[nav2_params, {'use_sim_time': True}],
|
|
||||||
remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')],
|
|
||||||
ros_arguments=['-p', 'use_sim_time:=True'],
|
|
||||||
)
|
|
||||||
|
|
||||||
# ==================== Behavior Server ====================
|
|
||||||
behavior_server = Node(
|
|
||||||
package='nav2_behaviors',
|
|
||||||
executable='behavior_server',
|
|
||||||
name='behavior_server',
|
|
||||||
output='screen',
|
|
||||||
parameters=[nav2_params, {'use_sim_time': True}],
|
|
||||||
remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')],
|
|
||||||
ros_arguments=['-p', 'use_sim_time:=True'],
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
# ==================== BT Navigator ====================
|
|
||||||
bt_navigator = Node(
|
|
||||||
package='nav2_bt_navigator',
|
|
||||||
executable='bt_navigator',
|
|
||||||
name='bt_navigator',
|
|
||||||
output='screen',
|
|
||||||
parameters=[nav2_params, {'use_sim_time': True}],
|
|
||||||
remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')],
|
|
||||||
ros_arguments=['-p', 'use_sim_time:=True'],
|
|
||||||
)
|
|
||||||
|
|
||||||
# ==================== Waypoint Follower ====================
|
|
||||||
waypoint_follower = Node(
|
|
||||||
package='nav2_waypoint_follower',
|
|
||||||
executable='waypoint_follower',
|
|
||||||
name='waypoint_follower',
|
|
||||||
output='screen',
|
|
||||||
parameters=[nav2_params, {'use_sim_time': True}],
|
|
||||||
remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')],
|
|
||||||
ros_arguments=['-p', 'use_sim_time:=True'],
|
|
||||||
)
|
|
||||||
|
|
||||||
# ==================== Lifecycle Manager (map + amcl) ====================
|
|
||||||
lifecycle_localization = Node(
|
|
||||||
package='nav2_lifecycle_manager',
|
|
||||||
executable='lifecycle_manager',
|
|
||||||
name='lifecycle_manager_localization',
|
|
||||||
output='screen',
|
|
||||||
parameters=[{'use_sim_time': True, 'autostart': True,
|
|
||||||
'node_names': ['map_server', 'amcl']}],
|
|
||||||
)
|
|
||||||
|
|
||||||
# ==================== Lifecycle Manager (nav nodes) ====================
|
|
||||||
lifecycle_navigation = Node(
|
|
||||||
package='nav2_lifecycle_manager',
|
|
||||||
executable='lifecycle_manager',
|
|
||||||
name='lifecycle_manager_navigation',
|
|
||||||
output='screen',
|
|
||||||
parameters=[{'use_sim_time': True, 'autostart': True,
|
|
||||||
'node_names': ['controller_server', 'planner_server',
|
|
||||||
'behavior_server', 'bt_navigator',
|
|
||||||
'waypoint_follower']}],
|
|
||||||
)
|
|
||||||
|
|
||||||
# ==================== RViz2 (可选) ====================
|
|
||||||
rviz_config = os.path.join(bringup_dir, 'rviz', 'navigation.rviz')
|
|
||||||
rviz_node = Node(
|
|
||||||
condition=IfCondition(use_rviz),
|
|
||||||
package='rviz2',
|
|
||||||
executable='rviz2',
|
|
||||||
name='rviz2',
|
|
||||||
arguments=['-d', rviz_config],
|
|
||||||
parameters=[{'use_sim_time': True}],
|
|
||||||
output='screen')
|
|
||||||
|
|
||||||
# ==================== 组装 ====================
|
|
||||||
ld = LaunchDescription()
|
|
||||||
ld.add_action(declare_use_sim_time)
|
|
||||||
ld.add_action(declare_use_gazebo)
|
|
||||||
ld.add_action(declare_use_rviz)
|
|
||||||
ld.add_action(declare_world)
|
|
||||||
ld.add_action(declare_map)
|
|
||||||
|
|
||||||
ld.add_action(start_gazebo)
|
|
||||||
ld.add_action(TimerAction(period=5.0, actions=[spawn_robot]))
|
|
||||||
ld.add_action(robot_state_publisher)
|
|
||||||
ld.add_action(joint_state_publisher)
|
|
||||||
|
|
||||||
ld.add_action(TimerAction(period=8.0, actions=[
|
|
||||||
map_server, amcl,
|
|
||||||
planner_server, controller_server, behavior_server, bt_navigator,
|
|
||||||
waypoint_follower,
|
|
||||||
lifecycle_localization, lifecycle_navigation,
|
|
||||||
rviz_node,
|
|
||||||
]))
|
|
||||||
|
|
||||||
return ld
|
|
||||||
@@ -1,150 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
# =============================================================================
|
|
||||||
# gc_bringup — slam_toolbox 定位 + 导航模式
|
|
||||||
# 使用 slam_toolbox 定位模式替代 AMCL，适用于有 posegraph 的场景
|
|
||||||
# =============================================================================
|
|
||||||
import os
|
|
||||||
from ament_index_python.packages import get_package_share_directory
|
|
||||||
from launch import LaunchDescription
|
|
||||||
from launch.actions import (DeclareLaunchArgument, ExecuteProcess,
|
|
||||||
IncludeLaunchDescription, TimerAction)
|
|
||||||
from launch.conditions import IfCondition
|
|
||||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
|
||||||
from launch.substitutions import LaunchConfiguration, Command
|
|
||||||
from launch_ros.actions import Node
|
|
||||||
from launch_ros.parameter_descriptions import ParameterValue
|
|
||||||
|
|
||||||
|
|
||||||
def generate_launch_description():
|
|
||||||
# ============================ 包路径 ============================
|
|
||||||
bringup_dir = get_package_share_directory('gc_bringup')
|
|
||||||
origincar_dir = get_package_share_directory('origincar_description')
|
|
||||||
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
|
|
||||||
|
|
||||||
# ============================ 启动参数 ============================
|
|
||||||
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
|
|
||||||
use_gazebo = LaunchConfiguration('use_gazebo', default='true')
|
|
||||||
use_rviz = LaunchConfiguration('use_rviz', default='true')
|
|
||||||
world = LaunchConfiguration('world', default=os.path.join(
|
|
||||||
origincar_dir, 'world', 'gc_world.world'))
|
|
||||||
map_yaml = LaunchConfiguration('map', default=os.path.join(
|
|
||||||
bringup_dir, 'maps', 'my_map.yaml'))
|
|
||||||
|
|
||||||
declare_use_sim_time = DeclareLaunchArgument('use_sim_time', default_value='true')
|
|
||||||
declare_use_gazebo = DeclareLaunchArgument('use_gazebo', default_value='true')
|
|
||||||
declare_use_rviz = DeclareLaunchArgument('use_rviz', default_value='true')
|
|
||||||
declare_world = DeclareLaunchArgument(
|
|
||||||
'world', default_value=os.path.join(origincar_dir, 'world', 'gc_world.world'))
|
|
||||||
declare_map = DeclareLaunchArgument(
|
|
||||||
'map', default_value=os.path.join(bringup_dir, 'maps', 'my_map.yaml'))
|
|
||||||
|
|
||||||
# ============================ 机器人模型 ============================
|
|
||||||
default_model_path = os.path.join(origincar_dir, 'urdf', 'origincar.urdf')
|
|
||||||
robot_description = ParameterValue(
|
|
||||||
Command(['xacro ', default_model_path]), value_type=str)
|
|
||||||
|
|
||||||
# ============================ Gazebo ============================
|
|
||||||
start_gazebo = ExecuteProcess(
|
|
||||||
condition=IfCondition(use_gazebo),
|
|
||||||
cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so',
|
|
||||||
'-s', 'libgazebo_ros_factory.so', world],
|
|
||||||
output='screen'
|
|
||||||
)
|
|
||||||
|
|
||||||
spawn_robot = Node(
|
|
||||||
condition=IfCondition(use_gazebo),
|
|
||||||
package='gazebo_ros',
|
|
||||||
executable='spawn_entity.py',
|
|
||||||
arguments=['-entity', 'mycar',
|
|
||||||
'-file', default_model_path,
|
|
||||||
'-x', '0.0', '-y', '0.0', '-z', '0.1'],
|
|
||||||
output='screen'
|
|
||||||
)
|
|
||||||
|
|
||||||
# ============================ TF & 机器人状态 ============================
|
|
||||||
robot_state_publisher = Node(
|
|
||||||
package='robot_state_publisher',
|
|
||||||
executable='robot_state_publisher',
|
|
||||||
name='robot_state_publisher',
|
|
||||||
parameters=[{
|
|
||||||
'robot_description': robot_description,
|
|
||||||
'use_sim_time': True,
|
|
||||||
'publish_frequency': 30.0
|
|
||||||
}],
|
|
||||||
output='screen'
|
|
||||||
)
|
|
||||||
|
|
||||||
joint_state_publisher = Node(
|
|
||||||
package='joint_state_publisher',
|
|
||||||
executable='joint_state_publisher',
|
|
||||||
name='joint_state_publisher',
|
|
||||||
parameters=[{'use_sim_time': True}],
|
|
||||||
output='screen'
|
|
||||||
)
|
|
||||||
|
|
||||||
# ============================ slam_toolbox 定位模式 ============================
|
|
||||||
map_base_path = os.path.join(bringup_dir, 'maps', 'my_map')
|
|
||||||
slam_localization_params = os.path.join(
|
|
||||||
bringup_dir, 'config', 'mapper_params_localization.yaml')
|
|
||||||
|
|
||||||
slam_toolbox_node = Node(
|
|
||||||
package='slam_toolbox',
|
|
||||||
executable='localization_slam_toolbox_node',
|
|
||||||
name='slam_toolbox',
|
|
||||||
output='screen',
|
|
||||||
parameters=[
|
|
||||||
slam_localization_params,
|
|
||||||
{
|
|
||||||
'map_file_name': map_base_path,
|
|
||||||
'use_sim_time': True
|
|
||||||
}
|
|
||||||
]
|
|
||||||
)
|
|
||||||
|
|
||||||
# ============================ Nav2 Bringup (关闭内部定位，用 slam_toolbox) ============================
|
|
||||||
nav2_params = os.path.join(bringup_dir, 'config', 'nav2_params.yaml')
|
|
||||||
nav2_launch_path = os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')
|
|
||||||
|
|
||||||
nav2_bringup = IncludeLaunchDescription(
|
|
||||||
PythonLaunchDescriptionSource(nav2_launch_path),
|
|
||||||
launch_arguments={
|
|
||||||
'map': map_yaml,
|
|
||||||
'use_sim_time': use_sim_time,
|
|
||||||
'params_file': nav2_params,
|
|
||||||
'slam': 'False',
|
|
||||||
'autostart': 'True',
|
|
||||||
'use_composition': 'False',
|
|
||||||
}.items(),
|
|
||||||
)
|
|
||||||
|
|
||||||
# ============================ RViz2 ============================
|
|
||||||
rviz_config = os.path.join(bringup_dir, 'rviz', 'navigation.rviz')
|
|
||||||
rviz_node = Node(
|
|
||||||
condition=IfCondition(use_rviz),
|
|
||||||
package='rviz2',
|
|
||||||
executable='rviz2',
|
|
||||||
name='rviz2',
|
|
||||||
arguments=['-d', rviz_config],
|
|
||||||
parameters=[{'use_sim_time': True}],
|
|
||||||
output='screen'
|
|
||||||
)
|
|
||||||
|
|
||||||
# ============================ 组装 ============================
|
|
||||||
ld = LaunchDescription()
|
|
||||||
|
|
||||||
ld.add_action(declare_use_sim_time)
|
|
||||||
ld.add_action(declare_use_gazebo)
|
|
||||||
ld.add_action(declare_use_rviz)
|
|
||||||
ld.add_action(declare_world)
|
|
||||||
ld.add_action(declare_map)
|
|
||||||
|
|
||||||
ld.add_action(start_gazebo)
|
|
||||||
ld.add_action(TimerAction(period=5.0, actions=[spawn_robot]))
|
|
||||||
ld.add_action(robot_state_publisher)
|
|
||||||
ld.add_action(joint_state_publisher)
|
|
||||||
ld.add_action(TimerAction(
|
|
||||||
period=8.0,
|
|
||||||
actions=[slam_toolbox_node, nav2_bringup, rviz_node]
|
|
||||||
))
|
|
||||||
|
|
||||||
return ld
|
|
||||||
@@ -1,153 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
# =============================================================================
|
|
||||||
# gc_bringup — SLAM 建图模式
|
|
||||||
# 启动 Gazebo + 机器人 + slam_toolbox 在线异步建图 + RViz
|
|
||||||
# =============================================================================
|
|
||||||
import os
|
|
||||||
from ament_index_python.packages import get_package_share_directory
|
|
||||||
from launch import LaunchDescription
|
|
||||||
from launch.actions import DeclareLaunchArgument, ExecuteProcess, TimerAction
|
|
||||||
from launch.conditions import IfCondition, UnlessCondition
|
|
||||||
from launch.substitutions import LaunchConfiguration, Command
|
|
||||||
from launch_ros.actions import Node
|
|
||||||
from launch_ros.parameter_descriptions import ParameterValue
|
|
||||||
|
|
||||||
|
|
||||||
def generate_launch_description():
|
|
||||||
# ============================ 包路径 ============================
|
|
||||||
bringup_dir = get_package_share_directory('gc_bringup')
|
|
||||||
origincar_dir = get_package_share_directory('origincar_description')
|
|
||||||
|
|
||||||
# ============================ 启动参数 ============================
|
|
||||||
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
|
|
||||||
use_gazebo = LaunchConfiguration('use_gazebo', default='true')
|
|
||||||
use_rviz = LaunchConfiguration('use_rviz', default='false')
|
|
||||||
world = LaunchConfiguration('world', default=os.path.join(
|
|
||||||
origincar_dir, 'world', 'gc_world.world'))
|
|
||||||
|
|
||||||
declare_use_sim_time = DeclareLaunchArgument(
|
|
||||||
'use_sim_time', default_value='true',
|
|
||||||
description='Use simulation (Gazebo) clock if true')
|
|
||||||
declare_use_gazebo = DeclareLaunchArgument(
|
|
||||||
'use_gazebo', default_value='true',
|
|
||||||
description='Start Gazebo simulation')
|
|
||||||
declare_use_rviz = DeclareLaunchArgument(
|
|
||||||
'use_rviz', default_value='false',
|
|
||||||
description='Start RViz2 (set true if you need local visualization)')
|
|
||||||
declare_world = DeclareLaunchArgument(
|
|
||||||
'world', default_value=os.path.join(origincar_dir, 'world', 'gc_world.world'),
|
|
||||||
description='Full path to Gazebo world file')
|
|
||||||
|
|
||||||
# ============================ 机器人模型 ============================
|
|
||||||
default_model_path = os.path.join(origincar_dir, 'urdf', 'origincar.urdf')
|
|
||||||
robot_description = ParameterValue(
|
|
||||||
Command(['xacro ', default_model_path]), value_type=str)
|
|
||||||
|
|
||||||
# ============================ Gazebo ============================
|
|
||||||
start_gazebo = ExecuteProcess(
|
|
||||||
condition=IfCondition(use_gazebo),
|
|
||||||
cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so',
|
|
||||||
'-s', 'libgazebo_ros_factory.so', world],
|
|
||||||
output='screen'
|
|
||||||
)
|
|
||||||
|
|
||||||
spawn_robot = Node(
|
|
||||||
condition=IfCondition(use_gazebo),
|
|
||||||
package='gazebo_ros',
|
|
||||||
executable='spawn_entity.py',
|
|
||||||
arguments=['-entity', 'mycar',
|
|
||||||
'-file', default_model_path,
|
|
||||||
'-x', '0.0', '-y', '0.0', '-z', '0.1'],
|
|
||||||
output='screen'
|
|
||||||
)
|
|
||||||
|
|
||||||
# ============================ TF & 机器人状态 ============================
|
|
||||||
robot_state_publisher = Node(
|
|
||||||
package='robot_state_publisher',
|
|
||||||
executable='robot_state_publisher',
|
|
||||||
name='robot_state_publisher',
|
|
||||||
parameters=[{
|
|
||||||
'robot_description': robot_description,
|
|
||||||
'use_sim_time': True,
|
|
||||||
'publish_frequency': 30.0
|
|
||||||
}],
|
|
||||||
output='screen'
|
|
||||||
)
|
|
||||||
|
|
||||||
joint_state_publisher = Node(
|
|
||||||
package='joint_state_publisher',
|
|
||||||
executable='joint_state_publisher',
|
|
||||||
name='joint_state_publisher',
|
|
||||||
parameters=[{'use_sim_time': True}],
|
|
||||||
output='screen'
|
|
||||||
)
|
|
||||||
|
|
||||||
# ============================ SLAM Toolbox (在线异步建图) ============================
|
|
||||||
slam_params_file = os.path.join(
|
|
||||||
bringup_dir, 'config', 'mapper_params_online_async.yaml')
|
|
||||||
|
|
||||||
slam_toolbox_node = Node(
|
|
||||||
package='slam_toolbox',
|
|
||||||
executable='async_slam_toolbox_node',
|
|
||||||
name='slam_toolbox',
|
|
||||||
output='screen',
|
|
||||||
parameters=[slam_params_file, {'use_sim_time': True}],
|
|
||||||
remappings=[('/scan', '/scan')]
|
|
||||||
)
|
|
||||||
|
|
||||||
# ============================ RViz2 ============================
|
|
||||||
rviz_config = os.path.join(bringup_dir, 'rviz', 'slam_mapping.rviz')
|
|
||||||
rviz_node = Node(
|
|
||||||
condition=IfCondition(use_rviz),
|
|
||||||
package='rviz2',
|
|
||||||
executable='rviz2',
|
|
||||||
name='rviz2',
|
|
||||||
arguments=['-d', rviz_config],
|
|
||||||
parameters=[{'use_sim_time': True}],
|
|
||||||
output='screen'
|
|
||||||
)
|
|
||||||
|
|
||||||
# ============================ 键盘遥控 (用于驱动机器人建图) ============================
|
|
||||||
use_teleop = LaunchConfiguration('use_teleop', default='true')
|
|
||||||
declare_use_teleop = DeclareLaunchArgument(
|
|
||||||
'use_teleop', default_value='true',
|
|
||||||
description='Start keyboard teleop for driving')
|
|
||||||
|
|
||||||
teleop_node = Node(
|
|
||||||
condition=IfCondition(use_teleop),
|
|
||||||
package='teleop_twist_keyboard',
|
|
||||||
executable='teleop_twist_keyboard',
|
|
||||||
name='teleop_twist_keyboard',
|
|
||||||
prefix='xterm -e',
|
|
||||||
output='screen',
|
|
||||||
remappings=[('/cmd_vel', '/cmd_vel')],
|
|
||||||
parameters=[{'use_sim_time': True}]
|
|
||||||
)
|
|
||||||
|
|
||||||
# ============================ 组装启动描述 ============================
|
|
||||||
ld = LaunchDescription()
|
|
||||||
|
|
||||||
ld.add_action(declare_use_sim_time)
|
|
||||||
ld.add_action(declare_use_gazebo)
|
|
||||||
ld.add_action(declare_use_rviz)
|
|
||||||
ld.add_action(declare_world)
|
|
||||||
ld.add_action(declare_use_teleop)
|
|
||||||
|
|
||||||
ld.add_action(start_gazebo)
|
|
||||||
|
|
||||||
# 等 Gazebo 启动后再生成机器人 (延迟 5 秒)
|
|
||||||
ld.add_action(TimerAction(
|
|
||||||
period=5.0,
|
|
||||||
actions=[spawn_robot]
|
|
||||||
))
|
|
||||||
|
|
||||||
ld.add_action(robot_state_publisher)
|
|
||||||
ld.add_action(joint_state_publisher)
|
|
||||||
|
|
||||||
# 等机器人生成后再启动 SLAM (延迟 3 秒)
|
|
||||||
ld.add_action(TimerAction(
|
|
||||||
period=8.0,
|
|
||||||
actions=[slam_toolbox_node, teleop_node, rviz_node]
|
|
||||||
))
|
|
||||||
|
|
||||||
return ld
|
|
||||||
Binary file not shown.
Binary file not shown.
@@ -1,7 +0,0 @@
|
|||||||
image: my_map.pgm
|
|
||||||
mode: trinary
|
|
||||||
resolution: 0.05
|
|
||||||
origin: [-9.95, -8.29, 0]
|
|
||||||
negate: 0
|
|
||||||
occupied_thresh: 0.65
|
|
||||||
free_thresh: 0.25
|
|
||||||
@@ -1,28 +0,0 @@
|
|||||||
<?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>gc_bringup</name>
|
|
||||||
<version>1.0.0</version>
|
|
||||||
<description>GC Robot Bringup: SLAM mapping and AMCL navigation for Ackermann robot with LiDAR</description>
|
|
||||||
<maintainer email="guoch@todo.todo">guoch</maintainer>
|
|
||||||
<license>Apache-2.0</license>
|
|
||||||
|
|
||||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
|
||||||
|
|
||||||
<exec_depend>gazebo_ros</exec_depend>
|
|
||||||
<exec_depend>robot_state_publisher</exec_depend>
|
|
||||||
<exec_depend>joint_state_publisher</exec_depend>
|
|
||||||
<exec_depend>rviz2</exec_depend>
|
|
||||||
<exec_depend>nav2_bringup</exec_depend>
|
|
||||||
<exec_depend>slam_toolbox</exec_depend>
|
|
||||||
<exec_depend>origincar_description</exec_depend>
|
|
||||||
<exec_depend>xacro</exec_depend>
|
|
||||||
<exec_depend>teleop_twist_keyboard</exec_depend>
|
|
||||||
|
|
||||||
<test_depend>ament_lint_auto</test_depend>
|
|
||||||
<test_depend>ament_lint_common</test_depend>
|
|
||||||
|
|
||||||
<export>
|
|
||||||
<build_type>ament_cmake</build_type>
|
|
||||||
</export>
|
|
||||||
</package>
|
|
||||||
@@ -1,133 +0,0 @@
|
|||||||
Panels:
|
|
||||||
- Class: rviz_common/Displays
|
|
||||||
Name: Displays
|
|
||||||
- Class: rviz_common/Views
|
|
||||||
Name: Views
|
|
||||||
Visualization Manager:
|
|
||||||
Class: ""
|
|
||||||
Displays:
|
|
||||||
- Alpha: 0.8
|
|
||||||
Class: rviz_default_plugins/RobotModel
|
|
||||||
Enabled: true
|
|
||||||
Name: RobotModel
|
|
||||||
Robot Description: robot_description
|
|
||||||
Visual Enabled: true
|
|
||||||
- Class: rviz_default_plugins/TF
|
|
||||||
Enabled: true
|
|
||||||
Name: TF
|
|
||||||
Frame Timeout: 15
|
|
||||||
Marker Scale: 0.3
|
|
||||||
- Class: rviz_default_plugins/Map
|
|
||||||
Enabled: true
|
|
||||||
Name: Map
|
|
||||||
Topic:
|
|
||||||
Depth: 1
|
|
||||||
History Policy: Keep Last
|
|
||||||
Reliability Policy: Reliable
|
|
||||||
Value: /map
|
|
||||||
Update Topic:
|
|
||||||
Depth: 1
|
|
||||||
History Policy: Keep Last
|
|
||||||
Reliability Policy: Reliable
|
|
||||||
Value: /map_updates
|
|
||||||
Color Scheme: map
|
|
||||||
Draw Behind: true
|
|
||||||
Use Timestamp: false
|
|
||||||
- Class: rviz_default_plugins/LaserScan
|
|
||||||
Enabled: true
|
|
||||||
Name: LaserScan
|
|
||||||
Topic:
|
|
||||||
Depth: 1
|
|
||||||
History Policy: Keep Last
|
|
||||||
Reliability Policy: Best Effort
|
|
||||||
Value: /scan
|
|
||||||
Size (m): 0.02
|
|
||||||
Style: Flat Squares
|
|
||||||
Decay Time: 0.3
|
|
||||||
Color Transformer: Intensity
|
|
||||||
- Class: rviz_default_plugins/Path
|
|
||||||
Enabled: true
|
|
||||||
Name: Global Plan
|
|
||||||
Topic:
|
|
||||||
Depth: 1
|
|
||||||
History Policy: Keep Last
|
|
||||||
Reliability Policy: Reliable
|
|
||||||
Value: /plan
|
|
||||||
Color: 25; 255; 0
|
|
||||||
Line Style: Dashed
|
|
||||||
- Class: rviz_default_plugins/Path
|
|
||||||
Enabled: true
|
|
||||||
Name: Local Plan
|
|
||||||
Topic:
|
|
||||||
Depth: 1
|
|
||||||
History Policy: Keep Last
|
|
||||||
Reliability Policy: Reliable
|
|
||||||
Value: /local_plan
|
|
||||||
Color: 255; 153; 0
|
|
||||||
- Class: rviz_default_plugins/Path
|
|
||||||
Enabled: true
|
|
||||||
Name: Smoothed Plan
|
|
||||||
Topic:
|
|
||||||
Depth: 1
|
|
||||||
History Policy: Keep Last
|
|
||||||
Reliability Policy: Reliable
|
|
||||||
Value: /smoothed_plan
|
|
||||||
Color: 255; 255; 0
|
|
||||||
- Class: rviz_default_plugins/Costmap
|
|
||||||
Enabled: true
|
|
||||||
Name: Local Costmap
|
|
||||||
Topic:
|
|
||||||
Depth: 1
|
|
||||||
History Policy: Keep Last
|
|
||||||
Reliability Policy: Reliable
|
|
||||||
Value: /local_costmap/costmap
|
|
||||||
Color Scheme: costmap
|
|
||||||
Draw Behind: false
|
|
||||||
- Class: rviz_default_plugins/Costmap
|
|
||||||
Enabled: false
|
|
||||||
Name: Global Costmap
|
|
||||||
Topic:
|
|
||||||
Depth: 1
|
|
||||||
History Policy: Keep Last
|
|
||||||
Reliability Policy: Reliable
|
|
||||||
Value: /global_costmap/costmap
|
|
||||||
Color Scheme: costmap
|
|
||||||
Draw Behind: false
|
|
||||||
- Alpha: 0.99
|
|
||||||
Class: rviz_default_plugins/ParticleCloud
|
|
||||||
Enabled: true
|
|
||||||
Name: AMCL Particle Swarm
|
|
||||||
Topic:
|
|
||||||
Depth: 1
|
|
||||||
History Policy: Keep Last
|
|
||||||
Reliability Policy: Reliable
|
|
||||||
Value: /particle_cloud
|
|
||||||
Color: 255; 0; 0
|
|
||||||
Style: Flat Squares
|
|
||||||
Enabled: true
|
|
||||||
Global Options:
|
|
||||||
Background Color: 48; 48; 48
|
|
||||||
Fixed Frame: map
|
|
||||||
Frame Rate: 30
|
|
||||||
Name: root
|
|
||||||
Tools:
|
|
||||||
- Class: rviz_default_plugins/Interact
|
|
||||||
- Class: rviz_default_plugins/MoveCamera
|
|
||||||
- Class: rviz_default_plugins/Select
|
|
||||||
- Class: rviz_default_plugins/SetInitialPose
|
|
||||||
Topic:
|
|
||||||
Value: /initialpose
|
|
||||||
- Class: rviz_default_plugins/SetGoal
|
|
||||||
Topic:
|
|
||||||
Value: /goal_pose
|
|
||||||
Views:
|
|
||||||
Current:
|
|
||||||
Class: rviz_default_plugins/Orbit
|
|
||||||
Distance: 5.0
|
|
||||||
Focal Point:
|
|
||||||
X: 0.0
|
|
||||||
Y: 0.0
|
|
||||||
Z: 0.5
|
|
||||||
Name: Current View
|
|
||||||
Pitch: 0.8
|
|
||||||
Yaw: 2.8
|
|
||||||
@@ -1,84 +0,0 @@
|
|||||||
Panels:
|
|
||||||
- Class: rviz_common/Displays
|
|
||||||
Name: Displays
|
|
||||||
- Class: rviz_common/Views
|
|
||||||
Name: Views
|
|
||||||
Visualization Manager:
|
|
||||||
Class: ""
|
|
||||||
Displays:
|
|
||||||
- Alpha: 0.8
|
|
||||||
Class: rviz_default_plugins/RobotModel
|
|
||||||
Enabled: true
|
|
||||||
Name: RobotModel
|
|
||||||
Robot Description: robot_description
|
|
||||||
Visual Enabled: true
|
|
||||||
- Class: rviz_default_plugins/TF
|
|
||||||
Enabled: true
|
|
||||||
Name: TF
|
|
||||||
Frame Timeout: 15
|
|
||||||
Marker Scale: 0.3
|
|
||||||
- Class: rviz_default_plugins/Map
|
|
||||||
Enabled: true
|
|
||||||
Name: Map
|
|
||||||
Topic:
|
|
||||||
Depth: 1
|
|
||||||
History Policy: Keep Last
|
|
||||||
Reliability Policy: Reliable
|
|
||||||
Value: /map
|
|
||||||
Update Topic:
|
|
||||||
Depth: 1
|
|
||||||
History Policy: Keep Last
|
|
||||||
Reliability Policy: Reliable
|
|
||||||
Value: /map_updates
|
|
||||||
Color Scheme: map
|
|
||||||
Draw Behind: true
|
|
||||||
Use Timestamp: false
|
|
||||||
- Class: rviz_default_plugins/LaserScan
|
|
||||||
Enabled: true
|
|
||||||
Name: LaserScan
|
|
||||||
Topic:
|
|
||||||
Depth: 1
|
|
||||||
History Policy: Keep Last
|
|
||||||
Reliability Policy: Best Effort
|
|
||||||
Value: /scan
|
|
||||||
Size (m): 0.02
|
|
||||||
Style: Flat Squares
|
|
||||||
Decay Time: 0.3
|
|
||||||
Color Transformer: Intensity
|
|
||||||
- Class: rviz_default_plugins/Path
|
|
||||||
Enabled: true
|
|
||||||
Name: Path (SLAM)
|
|
||||||
Topic:
|
|
||||||
Depth: 1
|
|
||||||
History Policy: Keep Last
|
|
||||||
Reliability Policy: Reliable
|
|
||||||
Value: /plan
|
|
||||||
Color: 25; 255; 0
|
|
||||||
Line Style: Dashed
|
|
||||||
Enabled: true
|
|
||||||
Global Options:
|
|
||||||
Background Color: 48; 48; 48
|
|
||||||
Fixed Frame: map
|
|
||||||
Frame Rate: 30
|
|
||||||
Name: root
|
|
||||||
Tools:
|
|
||||||
- Class: rviz_default_plugins/Interact
|
|
||||||
- Class: rviz_default_plugins/MoveCamera
|
|
||||||
- Class: rviz_default_plugins/Select
|
|
||||||
- Class: rviz_default_plugins/SetInitialPose
|
|
||||||
Topic:
|
|
||||||
Value: /initialpose
|
|
||||||
- Class: rviz_default_plugins/SetGoal
|
|
||||||
Topic:
|
|
||||||
Value: /goal_pose
|
|
||||||
Views:
|
|
||||||
Current:
|
|
||||||
Class: rviz_default_plugins/Orbit
|
|
||||||
Distance: 5.0
|
|
||||||
Focal Point:
|
|
||||||
X: 0.0
|
|
||||||
Y: 0.0
|
|
||||||
Z: 0.5
|
|
||||||
Name: Current View
|
|
||||||
Pitch: 0.8
|
|
||||||
Yaw: 2.8
|
|
||||||
@@ -1,58 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
"""
|
|
||||||
命令行发送导航目标点到 Nav2
|
|
||||||
|
|
||||||
用法:
|
|
||||||
ros2 run gc_bringup nav_goal 1.0 2.0 0.5 # 去 (1.0, 2.0), yaw=0.5
|
|
||||||
ros2 run gc_bringup nav_goal 1.0 2.0 # 去 (1.0, 2.0), yaw=0.0
|
|
||||||
ros2 run gc_bringup nav_goal -2.0 0.0 3.14 # 去 (-2.0, 0.0), 面朝后
|
|
||||||
|
|
||||||
# 或使用 topic pub
|
|
||||||
ros2 topic pub /goal_pose geometry_msgs/PoseStamped "{header: {frame_id: 'map'}, pose: {position: {x: 1.0, y: 0.0}, orientation: {z: 0.0, w: 1.0}}}"
|
|
||||||
"""
|
|
||||||
|
|
||||||
import sys
|
|
||||||
import rclpy
|
|
||||||
from rclpy.node import Node
|
|
||||||
from geometry_msgs.msg import PoseStamped
|
|
||||||
import math
|
|
||||||
|
|
||||||
|
|
||||||
class GoalSender(Node):
|
|
||||||
def __init__(self):
|
|
||||||
super().__init__('goal_sender')
|
|
||||||
self.publisher = self.create_publisher(PoseStamped, '/goal_pose', 10)
|
|
||||||
|
|
||||||
def send_goal(self, x: float, y: float, yaw: float = 0.0):
|
|
||||||
msg = PoseStamped()
|
|
||||||
msg.header.frame_id = 'map'
|
|
||||||
msg.header.stamp = self.get_clock().now().to_msg()
|
|
||||||
msg.pose.position.x = x
|
|
||||||
msg.pose.position.y = y
|
|
||||||
msg.pose.position.z = 0.0
|
|
||||||
msg.pose.orientation.z = math.sin(yaw / 2.0)
|
|
||||||
msg.pose.orientation.w = math.cos(yaw / 2.0)
|
|
||||||
self.publisher.publish(msg)
|
|
||||||
self.get_logger().info(f'发送导航目标: x={x:.2f}, y={y:.2f}, yaw={yaw:.2f}')
|
|
||||||
|
|
||||||
|
|
||||||
def main():
|
|
||||||
rclpy.init()
|
|
||||||
sender = GoalSender()
|
|
||||||
|
|
||||||
args = sys.argv[1:]
|
|
||||||
if len(args) < 2:
|
|
||||||
print("用法: ros2 run gc_bringup nav_goal <x> <y> [yaw]")
|
|
||||||
print("示例: ros2 run gc_bringup nav_goal 1.0 2.0 0.5")
|
|
||||||
sys.exit(1)
|
|
||||||
|
|
||||||
x = float(args[0])
|
|
||||||
y = float(args[1])
|
|
||||||
yaw = float(args[2]) if len(args) > 2 else 0.0
|
|
||||||
|
|
||||||
sender.send_goal(x, y, yaw)
|
|
||||||
rclpy.shutdown()
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
main()
|
|
||||||
@@ -1,18 +0,0 @@
|
|||||||
<?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>gc_navigation</name>
|
|
||||||
<version>0.0.0</version>
|
|
||||||
<description>TODO: Package description</description>
|
|
||||||
<maintainer email="guoch@todo.todo">guoch</maintainer>
|
|
||||||
<license>TODO: License declaration</license>
|
|
||||||
|
|
||||||
<test_depend>ament_copyright</test_depend>
|
|
||||||
<test_depend>ament_flake8</test_depend>
|
|
||||||
<test_depend>ament_pep257</test_depend>
|
|
||||||
<test_depend>python3-pytest</test_depend>
|
|
||||||
|
|
||||||
<export>
|
|
||||||
<build_type>ament_python</build_type>
|
|
||||||
</export>
|
|
||||||
</package>
|
|
||||||
@@ -1,4 +0,0 @@
|
|||||||
[develop]
|
|
||||||
script_dir=$base/lib/gc_navigation
|
|
||||||
[install]
|
|
||||||
install_scripts=$base/lib/gc_navigation
|
|
||||||
@@ -1,25 +0,0 @@
|
|||||||
from setuptools import find_packages, setup
|
|
||||||
|
|
||||||
package_name = 'gc_navigation'
|
|
||||||
|
|
||||||
setup(
|
|
||||||
name=package_name,
|
|
||||||
version='0.0.0',
|
|
||||||
packages=find_packages(exclude=['test']),
|
|
||||||
data_files=[
|
|
||||||
('share/ament_index/resource_index/packages',
|
|
||||||
['resource/' + package_name]),
|
|
||||||
('share/' + package_name, ['package.xml']),
|
|
||||||
],
|
|
||||||
install_requires=['setuptools'],
|
|
||||||
zip_safe=True,
|
|
||||||
maintainer='guoch',
|
|
||||||
maintainer_email='guoch@todo.todo',
|
|
||||||
description='TODO: Package description',
|
|
||||||
license='TODO: License declaration',
|
|
||||||
tests_require=['pytest'],
|
|
||||||
entry_points={
|
|
||||||
'console_scripts': [
|
|
||||||
],
|
|
||||||
},
|
|
||||||
)
|
|
||||||
@@ -1,25 +0,0 @@
|
|||||||
# Copyright 2015 Open Source Robotics Foundation, Inc.
|
|
||||||
#
|
|
||||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
|
||||||
# you may not use this file except in compliance with the License.
|
|
||||||
# You may obtain a copy of the License at
|
|
||||||
#
|
|
||||||
# http://www.apache.org/licenses/LICENSE-2.0
|
|
||||||
#
|
|
||||||
# Unless required by applicable law or agreed to in writing, software
|
|
||||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
|
||||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
||||||
# See the License for the specific language governing permissions and
|
|
||||||
# limitations under the License.
|
|
||||||
|
|
||||||
from ament_copyright.main import main
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
|
|
||||||
# Remove the `skip` decorator once the source file(s) have a copyright header
|
|
||||||
@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
|
|
||||||
@pytest.mark.copyright
|
|
||||||
@pytest.mark.linter
|
|
||||||
def test_copyright():
|
|
||||||
rc = main(argv=['.', 'test'])
|
|
||||||
assert rc == 0, 'Found errors'
|
|
||||||
@@ -1,25 +0,0 @@
|
|||||||
# Copyright 2017 Open Source Robotics Foundation, Inc.
|
|
||||||
#
|
|
||||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
|
||||||
# you may not use this file except in compliance with the License.
|
|
||||||
# You may obtain a copy of the License at
|
|
||||||
#
|
|
||||||
# http://www.apache.org/licenses/LICENSE-2.0
|
|
||||||
#
|
|
||||||
# Unless required by applicable law or agreed to in writing, software
|
|
||||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
|
||||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
||||||
# See the License for the specific language governing permissions and
|
|
||||||
# limitations under the License.
|
|
||||||
|
|
||||||
from ament_flake8.main import main_with_errors
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.flake8
|
|
||||||
@pytest.mark.linter
|
|
||||||
def test_flake8():
|
|
||||||
rc, errors = main_with_errors(argv=[])
|
|
||||||
assert rc == 0, \
|
|
||||||
'Found %d code style errors / warnings:\n' % len(errors) + \
|
|
||||||
'\n'.join(errors)
|
|
||||||
@@ -1,23 +0,0 @@
|
|||||||
# Copyright 2015 Open Source Robotics Foundation, Inc.
|
|
||||||
#
|
|
||||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
|
||||||
# you may not use this file except in compliance with the License.
|
|
||||||
# You may obtain a copy of the License at
|
|
||||||
#
|
|
||||||
# http://www.apache.org/licenses/LICENSE-2.0
|
|
||||||
#
|
|
||||||
# Unless required by applicable law or agreed to in writing, software
|
|
||||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
|
||||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
||||||
# See the License for the specific language governing permissions and
|
|
||||||
# limitations under the License.
|
|
||||||
|
|
||||||
from ament_pep257.main import main
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.linter
|
|
||||||
@pytest.mark.pep257
|
|
||||||
def test_pep257():
|
|
||||||
rc = main(argv=['.', 'test'])
|
|
||||||
assert rc == 0, 'Found code style errors / warnings'
|
|
||||||
@@ -15,8 +15,8 @@ slam_toolbox:
|
|||||||
mode: localization #localization
|
mode: localization #localization
|
||||||
|
|
||||||
# if you'd like to start localizing on bringup in a map and pose
|
# if you'd like to start localizing on bringup in a map and pose
|
||||||
# map_file_name: /home/guoch/test_ws/src/gc_navigation2_slamtoolbox/maps/my_map.posegraph
|
map_file_name: /home/guoch/test_ws/src/gc_navigation2_slamtoolbox/maps/my_map.posegraph
|
||||||
# map_start_pose: [0.0, 0.0, 0.0]
|
map_start_pose: [0.0, 0.0, 0.0]
|
||||||
|
|
||||||
debug_logging: false
|
debug_logging: false
|
||||||
throttle_scans: 1
|
throttle_scans: 1
|
||||||
|
|||||||
@@ -26,7 +26,7 @@ def generate_launch_description():
|
|||||||
map_yaml_path = LaunchConfiguration('map', default=os.path.join(
|
map_yaml_path = LaunchConfiguration('map', default=os.path.join(
|
||||||
gc_navigation_fish_dir, 'maps', 'my_map.yaml'))
|
gc_navigation_fish_dir, 'maps', 'my_map.yaml'))
|
||||||
nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
|
nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
|
||||||
gc_navigation_fish_dir, 'params', 'gc_navigation.yaml'))
|
gc_navigation_fish_dir, 'params', 'gc_navigation_amcl.yaml'))
|
||||||
|
|
||||||
# =============================3.声明启动launch文件，传入：地图路径、是否使用仿真时间以及nav2参数文件==============
|
# =============================3.声明启动launch文件，传入：地图路径、是否使用仿真时间以及nav2参数文件==============
|
||||||
nav2_bringup_launch = IncludeLaunchDescription(
|
nav2_bringup_launch = IncludeLaunchDescription(
|
||||||
@@ -38,6 +38,7 @@ def generate_launch_description():
|
|||||||
'params_file': nav2_param_path}.items(),
|
'params_file': nav2_param_path}.items(),
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
# =============================4.Gazebo仿真设置==========================================================
|
# =============================4.Gazebo仿真设置==========================================================
|
||||||
robot_name_in_model = 'mycar'
|
robot_name_in_model = 'mycar'
|
||||||
default_model_path = os.path.join(
|
default_model_path = os.path.join(
|
||||||
@@ -0,0 +1,113 @@
|
|||||||
|
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
|
||||||
|
from launch_ros.parameter_descriptions import ParameterValue
|
||||||
|
from launch.substitutions import Command
|
||||||
|
from launch.actions import ExecuteProcess
|
||||||
|
|
||||||
|
|
||||||
|
def generate_launch_description():
|
||||||
|
ld = LaunchDescription()
|
||||||
|
|
||||||
|
# =============================1.定位到包的地址=============================================================
|
||||||
|
pkg_dir = get_package_share_directory('gc_navigation2_slamtoolbox')
|
||||||
|
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
|
||||||
|
origincar_urdf_dir = get_package_share_directory('origincar_description')
|
||||||
|
|
||||||
|
# =============================2.声明参数===============================================================
|
||||||
|
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
|
||||||
|
map_yaml_path = LaunchConfiguration('map', default=os.path.join(
|
||||||
|
pkg_dir, 'maps', 'my_map.yaml'))
|
||||||
|
nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
|
||||||
|
pkg_dir, 'params', 'gc_navigation.yaml'))
|
||||||
|
slam_params_file = os.path.join(
|
||||||
|
pkg_dir, 'config', 'mapper_params_localization.yaml')
|
||||||
|
|
||||||
|
# =============================3.slam_toolbox 定位模式（替代 AMCL）=========================================
|
||||||
|
slam_toolbox_node = Node(
|
||||||
|
package='slam_toolbox',
|
||||||
|
executable='localization_slam_toolbox_node',
|
||||||
|
name='slam_toolbox',
|
||||||
|
output='screen',
|
||||||
|
parameters=[slam_params_file,
|
||||||
|
{'use_sim_time': True}],
|
||||||
|
# 关键：确保 slam_toolbox 作为定位器发布 map->odom 变换
|
||||||
|
)
|
||||||
|
|
||||||
|
# =============================4.仅启动 nav2 navigation（不含 localization）=================================
|
||||||
|
# 因为 slam_toolbox 已经替代了 AMCL，不需要再跑 localization_launch
|
||||||
|
navigation_launch = IncludeLaunchDescription(
|
||||||
|
PythonLaunchDescriptionSource(
|
||||||
|
[nav2_bringup_dir, '/launch', '/navigation_launch.py']),
|
||||||
|
launch_arguments={
|
||||||
|
'use_sim_time': use_sim_time,
|
||||||
|
'params_file': nav2_param_path}.items(),
|
||||||
|
)
|
||||||
|
|
||||||
|
# =============================5.map_server 单独启动（提供静态地图给 costmap）===============================
|
||||||
|
map_server_node = Node(
|
||||||
|
package='nav2_map_server',
|
||||||
|
executable='map_server',
|
||||||
|
name='map_server',
|
||||||
|
output='screen',
|
||||||
|
parameters=[nav2_param_path,
|
||||||
|
{'yaml_filename': map_yaml_path,
|
||||||
|
'use_sim_time': True}],
|
||||||
|
)
|
||||||
|
|
||||||
|
# map_server 生命周期自动管理
|
||||||
|
map_server_lifecycle_node = Node(
|
||||||
|
package='nav2_lifecycle_manager',
|
||||||
|
executable='lifecycle_manager',
|
||||||
|
name='lifecycle_manager_map',
|
||||||
|
output='screen',
|
||||||
|
parameters=[{'use_sim_time': True,
|
||||||
|
'autostart': True,
|
||||||
|
'node_names': ['map_server']}],
|
||||||
|
)
|
||||||
|
|
||||||
|
# =============================6.Gazebo仿真设置==========================================================
|
||||||
|
robot_name_in_model = 'mycar'
|
||||||
|
default_model_path = os.path.join(
|
||||||
|
origincar_urdf_dir, "urdf", "origincar.urdf")
|
||||||
|
model = DeclareLaunchArgument(
|
||||||
|
name="model", default_value=default_model_path)
|
||||||
|
|
||||||
|
gazebo_world_path = os.path.join(origincar_urdf_dir, 'world/test.world')
|
||||||
|
start_gazebo_cmd = ExecuteProcess(
|
||||||
|
cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so',
|
||||||
|
'-s', 'libgazebo_ros_factory.so', gazebo_world_path],
|
||||||
|
output='screen')
|
||||||
|
|
||||||
|
spawn_entity_cmd = Node(
|
||||||
|
package='gazebo_ros',
|
||||||
|
executable='spawn_entity.py',
|
||||||
|
arguments=['-entity', robot_name_in_model,
|
||||||
|
'-file', default_model_path],
|
||||||
|
output='screen'
|
||||||
|
)
|
||||||
|
|
||||||
|
robot_description = ParameterValue(
|
||||||
|
Command(["xacro ", LaunchConfiguration("model")]), value_type=str)
|
||||||
|
|
||||||
|
robot_state_publisher = Node(
|
||||||
|
package="robot_state_publisher",
|
||||||
|
executable="robot_state_publisher",
|
||||||
|
parameters=[{"robot_description": robot_description,
|
||||||
|
'use_sim_time': True, 'publish_frequency': 30.0}]
|
||||||
|
)
|
||||||
|
|
||||||
|
ld.add_action(model)
|
||||||
|
ld.add_action(start_gazebo_cmd)
|
||||||
|
ld.add_action(spawn_entity_cmd)
|
||||||
|
ld.add_action(robot_state_publisher)
|
||||||
|
ld.add_action(map_server_node)
|
||||||
|
ld.add_action(map_server_lifecycle_node)
|
||||||
|
ld.add_action(slam_toolbox_node)
|
||||||
|
ld.add_action(navigation_launch)
|
||||||
|
|
||||||
|
return ld
|
||||||
@@ -1,29 +0,0 @@
|
|||||||
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()
|
|
||||||
@@ -1,34 +0,0 @@
|
|||||||
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='true')
|
|
||||||
map_yaml_path = LaunchConfiguration('map', default=os.path.join(
|
|
||||||
gc_navigation_fish_dir, 'maps', 'my_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])
|
|
||||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,7 +0,0 @@
|
|||||||
image: map_1770592084.pgm
|
|
||||||
mode: trinary
|
|
||||||
resolution: 0.05
|
|
||||||
origin: [-2.15, -5.81, 0]
|
|
||||||
negate: 0
|
|
||||||
occupied_thresh: 0.65
|
|
||||||
free_thresh: 0.25
|
|
||||||
Binary file not shown.
@@ -1,7 +0,0 @@
|
|||||||
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
|
|
||||||
@@ -1,18 +0,0 @@
|
|||||||
<?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>gc_navigation_fish</name>
|
|
||||||
<version>0.0.0</version>
|
|
||||||
<description>TODO: Package description</description>
|
|
||||||
<maintainer email="guoch@todo.todo">guoch</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>
|
|
||||||
<exec_depend>nav2_bringup</exec_depend>
|
|
||||||
<export>
|
|
||||||
<build_type>ament_cmake</build_type>
|
|
||||||
</export>
|
|
||||||
</package>
|
|
||||||
@@ -1,408 +0,0 @@
|
|||||||
amcl:
|
|
||||||
ros__parameters:
|
|
||||||
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: 10.0
|
|
||||||
update_min_a: 0.2
|
|
||||||
update_min_d: 0.25
|
|
||||||
z_hit: 0.5
|
|
||||||
z_max: 0.05
|
|
||||||
z_rand: 0.5
|
|
||||||
z_short: 0.05
|
|
||||||
scan_topic: scan
|
|
||||||
|
|
||||||
amcl_map_client:
|
|
||||||
ros__parameters:
|
|
||||||
use_sim_time: True
|
|
||||||
|
|
||||||
amcl_rclcpp_node:
|
|
||||||
ros__parameters:
|
|
||||||
use_sim_time: True
|
|
||||||
|
|
||||||
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
|
|
||||||
# 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
|
|
||||||
# nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
|
|
||||||
# nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
|
|
||||||
# They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
|
|
||||||
plugin_lib_names:
|
|
||||||
- nav2_compute_path_to_pose_action_bt_node
|
|
||||||
- nav2_compute_path_through_poses_action_bt_node
|
|
||||||
- nav2_smooth_path_action_bt_node
|
|
||||||
- nav2_follow_path_action_bt_node
|
|
||||||
- nav2_spin_action_bt_node
|
|
||||||
- nav2_wait_action_bt_node
|
|
||||||
- nav2_back_up_action_bt_node
|
|
||||||
- nav2_drive_on_heading_bt_node
|
|
||||||
- nav2_clear_costmap_service_bt_node
|
|
||||||
- nav2_is_stuck_condition_bt_node
|
|
||||||
- nav2_goal_reached_condition_bt_node
|
|
||||||
- nav2_goal_updated_condition_bt_node
|
|
||||||
- nav2_globally_updated_goal_condition_bt_node
|
|
||||||
- nav2_is_path_valid_condition_bt_node
|
|
||||||
- nav2_initial_pose_received_condition_bt_node
|
|
||||||
- nav2_reinitialize_global_localization_service_bt_node
|
|
||||||
- nav2_rate_controller_bt_node
|
|
||||||
- nav2_distance_controller_bt_node
|
|
||||||
- nav2_speed_controller_bt_node
|
|
||||||
- nav2_truncate_path_action_bt_node
|
|
||||||
- nav2_truncate_path_local_action_bt_node
|
|
||||||
- nav2_goal_updater_node_bt_node
|
|
||||||
- nav2_recovery_node_bt_node
|
|
||||||
- nav2_pipeline_sequence_bt_node
|
|
||||||
- nav2_round_robin_node_bt_node
|
|
||||||
- nav2_transform_available_condition_bt_node
|
|
||||||
- nav2_time_expired_condition_bt_node
|
|
||||||
- nav2_path_expiring_timer_condition
|
|
||||||
- nav2_distance_traveled_condition_bt_node
|
|
||||||
- nav2_single_trigger_bt_node
|
|
||||||
- nav2_is_battery_low_condition_bt_node
|
|
||||||
- nav2_navigate_through_poses_action_bt_node
|
|
||||||
- nav2_navigate_to_pose_action_bt_node
|
|
||||||
- nav2_remove_passed_goals_action_bt_node
|
|
||||||
- nav2_planner_selector_bt_node
|
|
||||||
- nav2_controller_selector_bt_node
|
|
||||||
- nav2_goal_checker_selector_bt_node
|
|
||||||
- nav2_controller_cancel_bt_node
|
|
||||||
- nav2_path_longer_on_approach_bt_node
|
|
||||||
- nav2_wait_cancel_bt_node
|
|
||||||
- nav2_spin_cancel_bt_node
|
|
||||||
- nav2_back_up_cancel_bt_node
|
|
||||||
- nav2_drive_on_heading_cancel_bt_node
|
|
||||||
|
|
||||||
bt_navigator_rclcpp_node:
|
|
||||||
ros__parameters:
|
|
||||||
use_sim_time: True
|
|
||||||
|
|
||||||
controller_server:
|
|
||||||
ros__parameters:
|
|
||||||
controller_frequency: 30.0
|
|
||||||
FollowPath:
|
|
||||||
plugin: "nav2_mppi_controller::MPPIController"
|
|
||||||
time_steps: 56
|
|
||||||
model_dt: 0.05
|
|
||||||
batch_size: 2000
|
|
||||||
vx_std: 0.2
|
|
||||||
vy_std: 0.0
|
|
||||||
wz_std: 0.4
|
|
||||||
vx_max: 0.5
|
|
||||||
vx_min: -0.35
|
|
||||||
vy_max: 0.0
|
|
||||||
wz_max: 1.9
|
|
||||||
iteration_count: 1
|
|
||||||
temperature: 0.3
|
|
||||||
gamma: 0.015
|
|
||||||
motion_model: "Ackermann"
|
|
||||||
visualize: false
|
|
||||||
TrajectoryVisualizer:
|
|
||||||
trajectory_step: 5
|
|
||||||
time_step: 3
|
|
||||||
AckermannConstraints:
|
|
||||||
min_turning_r: 0.4
|
|
||||||
critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"]
|
|
||||||
ConstraintCritic:
|
|
||||||
enabled: true
|
|
||||||
cost_power: 1
|
|
||||||
cost_weight: 4.0
|
|
||||||
GoalCritic:
|
|
||||||
enabled: true
|
|
||||||
cost_power: 1
|
|
||||||
cost_weight: 5.0
|
|
||||||
threshold_to_consider: 1.4
|
|
||||||
GoalAngleCritic:
|
|
||||||
enabled: true
|
|
||||||
cost_power: 1
|
|
||||||
cost_weight: 3.0
|
|
||||||
threshold_to_consider: 0.5
|
|
||||||
PreferForwardCritic:
|
|
||||||
enabled: true
|
|
||||||
cost_power: 1
|
|
||||||
cost_weight: 5.0
|
|
||||||
threshold_to_consider: 0.5
|
|
||||||
# Option to replace Cost and use Obstacles instead
|
|
||||||
# ObstaclesCritic:
|
|
||||||
# enabled: true
|
|
||||||
# cost_power: 1
|
|
||||||
# repulsion_weight: 1.5
|
|
||||||
# critical_weight: 20.0
|
|
||||||
# consider_footprint: false
|
|
||||||
# collision_cost: 10000.0
|
|
||||||
# collision_margin_distance: 0.1
|
|
||||||
# near_goal_distance: 0.5
|
|
||||||
CostCritic:
|
|
||||||
enabled: true
|
|
||||||
cost_power: 1
|
|
||||||
cost_weight: 3.81
|
|
||||||
critical_cost: 300.0
|
|
||||||
consider_footprint: true
|
|
||||||
collision_cost: 1000000.0
|
|
||||||
near_goal_distance: 1.0
|
|
||||||
trajectory_point_step: 2
|
|
||||||
PathAlignCritic:
|
|
||||||
enabled: true
|
|
||||||
cost_power: 1
|
|
||||||
cost_weight: 14.0
|
|
||||||
max_path_occupancy_ratio: 0.05
|
|
||||||
trajectory_point_step: 4
|
|
||||||
threshold_to_consider: 0.5
|
|
||||||
offset_from_furthest: 20
|
|
||||||
use_path_orientations: false
|
|
||||||
PathFollowCritic:
|
|
||||||
enabled: true
|
|
||||||
cost_power: 1
|
|
||||||
cost_weight: 5.0
|
|
||||||
offset_from_furthest: 5
|
|
||||||
threshold_to_consider: 1.4
|
|
||||||
PathAngleCritic:
|
|
||||||
enabled: true
|
|
||||||
cost_power: 1
|
|
||||||
cost_weight: 2.0
|
|
||||||
offset_from_furthest: 4
|
|
||||||
threshold_to_consider: 0.5
|
|
||||||
max_angle_to_furthest: 1.0
|
|
||||||
forward_preference: true
|
|
||||||
# VelocityDeadbandCritic:
|
|
||||||
# enabled: true
|
|
||||||
# cost_power: 1
|
|
||||||
# cost_weight: 35.0
|
|
||||||
# deadband_velocities: [0.05, 0.05, 0.05]
|
|
||||||
# TwirlingCritic:
|
|
||||||
# enabled: true
|
|
||||||
# twirling_cost_power: 1
|
|
||||||
# twirling_cost_weight: 10.0
|
|
||||||
|
|
||||||
controller_server_rclcpp_node:
|
|
||||||
ros__parameters:
|
|
||||||
use_sim_time: True
|
|
||||||
|
|
||||||
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.14, 0.085],
|
|
||||||
[0.14, -0.085],
|
|
||||||
[-0.14, -0.085],
|
|
||||||
[-0.14, 0.085]]"
|
|
||||||
footprint_padding: 0.02
|
|
||||||
plugins: ["voxel_layer", "inflation_layer"]
|
|
||||||
inflation_layer:
|
|
||||||
plugin: "nav2_costmap_2d::InflationLayer"
|
|
||||||
cost_scaling_factor: 3.0
|
|
||||||
inflation_radius: 0.55
|
|
||||||
voxel_layer:
|
|
||||||
plugin: "nav2_costmap_2d::VoxelLayer"
|
|
||||||
enabled: True
|
|
||||||
publish_voxel_map: True
|
|
||||||
origin_z: 0.0
|
|
||||||
z_resolution: 0.05
|
|
||||||
z_voxels: 16
|
|
||||||
max_obstacle_height: 2.0
|
|
||||||
mark_threshold: 0
|
|
||||||
observation_sources: scan
|
|
||||||
scan:
|
|
||||||
topic: /scan
|
|
||||||
max_obstacle_height: 2.0
|
|
||||||
clearing: True
|
|
||||||
marking: True
|
|
||||||
data_type: "LaserScan"
|
|
||||||
raytrace_max_range: 3.0
|
|
||||||
raytrace_min_range: 0.0
|
|
||||||
obstacle_max_range: 2.5
|
|
||||||
obstacle_min_range: 0.0
|
|
||||||
static_layer:
|
|
||||||
map_subscribe_transient_local: True
|
|
||||||
always_send_full_costmap: True
|
|
||||||
local_costmap_client:
|
|
||||||
ros__parameters:
|
|
||||||
use_sim_time: True
|
|
||||||
local_costmap_rclcpp_node:
|
|
||||||
ros__parameters:
|
|
||||||
use_sim_time: 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.14, 0.085],
|
|
||||||
[0.14, -0.085],
|
|
||||||
[-0.14, -0.085],
|
|
||||||
[-0.14, 0.085]]"
|
|
||||||
footprint_padding: 0.02
|
|
||||||
resolution: 0.05
|
|
||||||
track_unknown_space: true
|
|
||||||
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
|
|
||||||
obstacle_layer:
|
|
||||||
plugin: "nav2_costmap_2d::ObstacleLayer"
|
|
||||||
enabled: True
|
|
||||||
observation_sources: scan
|
|
||||||
scan:
|
|
||||||
topic: /scan
|
|
||||||
max_obstacle_height: 2.0
|
|
||||||
clearing: True
|
|
||||||
marking: True
|
|
||||||
data_type: "LaserScan"
|
|
||||||
raytrace_max_range: 3.0
|
|
||||||
raytrace_min_range: 0.0
|
|
||||||
obstacle_max_range: 2.5
|
|
||||||
obstacle_min_range: 0.0
|
|
||||||
static_layer:
|
|
||||||
plugin: "nav2_costmap_2d::StaticLayer"
|
|
||||||
map_subscribe_transient_local: True
|
|
||||||
inflation_layer:
|
|
||||||
plugin: "nav2_costmap_2d::InflationLayer"
|
|
||||||
cost_scaling_factor: 3.0
|
|
||||||
inflation_radius: 0.55
|
|
||||||
always_send_full_costmap: True
|
|
||||||
global_costmap_client:
|
|
||||||
ros__parameters:
|
|
||||||
use_sim_time: True
|
|
||||||
global_costmap_rclcpp_node:
|
|
||||||
ros__parameters:
|
|
||||||
use_sim_time: True
|
|
||||||
|
|
||||||
map_server:
|
|
||||||
ros__parameters:
|
|
||||||
use_sim_time: True
|
|
||||||
yaml_filename: "turtlebot3_world.yaml"
|
|
||||||
|
|
||||||
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:
|
|
||||||
planner_plugins: ["GridBased"]
|
|
||||||
use_sim_time: True
|
|
||||||
|
|
||||||
GridBased:
|
|
||||||
plugin: "nav2_smac_planner/SmacPlannerHybrid"
|
|
||||||
downsample_costmap: false # whether or not to downsample the map
|
|
||||||
downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm)
|
|
||||||
tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found.
|
|
||||||
allow_unknown: true # allow traveling in unknown space
|
|
||||||
max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable
|
|
||||||
max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution
|
|
||||||
max_planning_time: 5.0 # max time in s for planner to plan, smooth
|
|
||||||
motion_model_for_search: "REEDS_SHEPP" # or REEDS_SHEPP 可以倒车
|
|
||||||
angle_quantization_bins: 72 # Number of angle bins for search
|
|
||||||
analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach.
|
|
||||||
analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting
|
|
||||||
minimum_turning_radius: 0.40 # minimum turning radius in m of path / vehicle
|
|
||||||
reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1
|
|
||||||
change_penalty: 0.0 # Penalty to apply if motion is changing directions (L to R), must be >= 0
|
|
||||||
non_straight_penalty: 1.2 # Penalty to apply if motion is non-straight, must be => 1
|
|
||||||
cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable.
|
|
||||||
retrospective_penalty: 0.015
|
|
||||||
lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters.
|
|
||||||
cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static.
|
|
||||||
viz_expansions: false # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning). WARNING: heavy to compute and to display, for debug only as it degrades the performance.
|
|
||||||
smooth_path: True # If true, does a simple and quick smoothing post-processing to the path
|
|
||||||
|
|
||||||
smoother:
|
|
||||||
max_iterations: 1000
|
|
||||||
w_smooth: 0.3
|
|
||||||
w_data: 0.2
|
|
||||||
tolerance: 1.0e-10
|
|
||||||
do_refinement: true
|
|
||||||
refinement_num: 2
|
|
||||||
|
|
||||||
planner_server_rclcpp_node:
|
|
||||||
ros__parameters:
|
|
||||||
use_sim_time: 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", "wait"]
|
|
||||||
spin:
|
|
||||||
plugin: "nav2_behaviors/Spin"
|
|
||||||
backup:
|
|
||||||
plugin: "nav2_behaviors/BackUp"
|
|
||||||
drive_on_heading:
|
|
||||||
plugin: "nav2_behaviors/DriveOnHeading"
|
|
||||||
wait:
|
|
||||||
plugin: "nav2_behaviors/Wait"
|
|
||||||
global_frame: odom
|
|
||||||
robot_base_frame: base_link
|
|
||||||
transform_tolerance: 0.1
|
|
||||||
use_sim_time: 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:
|
|
||||||
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
|
|
||||||
@@ -1,29 +0,0 @@
|
|||||||
cmake_minimum_required(VERSION 3.8)
|
|
||||||
project(gc_slam_toolbox_fish)
|
|
||||||
|
|
||||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
|
||||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
|
||||||
endif()
|
|
||||||
|
|
||||||
# find dependencies
|
|
||||||
find_package(ament_cmake REQUIRED)
|
|
||||||
# uncomment the following section in order to fill in
|
|
||||||
# further dependencies manually.
|
|
||||||
# find_package(<dependency> REQUIRED)
|
|
||||||
install(
|
|
||||||
DIRECTORY config launch rviz
|
|
||||||
DESTINATION share/${PROJECT_NAME}
|
|
||||||
)
|
|
||||||
if(BUILD_TESTING)
|
|
||||||
find_package(ament_lint_auto REQUIRED)
|
|
||||||
# the following line skips the linter which checks for copyrights
|
|
||||||
# comment the line when a copyright and license is added to all source files
|
|
||||||
set(ament_cmake_copyright_FOUND TRUE)
|
|
||||||
# the following line skips cpplint (only works in a git repo)
|
|
||||||
# comment the line when this package is in a git repo and when
|
|
||||||
# a copyright and license is added to all source files
|
|
||||||
set(ament_cmake_cpplint_FOUND TRUE)
|
|
||||||
ament_lint_auto_find_test_dependencies()
|
|
||||||
endif()
|
|
||||||
|
|
||||||
ament_package()
|
|
||||||
@@ -1,63 +0,0 @@
|
|||||||
include "map_builder.lua"
|
|
||||||
include "trajectory_builder.lua"
|
|
||||||
|
|
||||||
options = {
|
|
||||||
map_builder = MAP_BUILDER,
|
|
||||||
trajectory_builder = TRAJECTORY_BUILDER,
|
|
||||||
map_frame = "map",
|
|
||||||
tracking_frame = "base_link",
|
|
||||||
-- base_link改为odom,发布map到odom之间的位姿态
|
|
||||||
published_frame = "odom",
|
|
||||||
odom_frame = "odom",
|
|
||||||
-- true改为false，不用提供里程计数据
|
|
||||||
provide_odom_frame = false,
|
|
||||||
-- false改为true，仅发布2D位资
|
|
||||||
publish_frame_projected_to_2d = true,
|
|
||||||
-- false改为true，使用里程计数据
|
|
||||||
use_odometry = true,
|
|
||||||
use_nav_sat = false,
|
|
||||||
use_landmarks = false,
|
|
||||||
-- 0改为1,使用一个雷达
|
|
||||||
num_laser_scans = 1,
|
|
||||||
-- 1改为0，不使用多波雷达
|
|
||||||
num_multi_echo_laser_scans = 0,
|
|
||||||
-- 10改为1，1/1=1等于不分割
|
|
||||||
num_subdivisions_per_laser_scan = 1,
|
|
||||||
num_point_clouds = 0,
|
|
||||||
lookup_transform_timeout_sec = 0.2,
|
|
||||||
submap_publish_period_sec = 0.3,
|
|
||||||
pose_publish_period_sec = 5e-3,
|
|
||||||
trajectory_publish_period_sec = 30e-3,
|
|
||||||
rangefinder_sampling_ratio = 1.,
|
|
||||||
odometry_sampling_ratio = 1.,
|
|
||||||
fixed_frame_pose_sampling_ratio = 1.,
|
|
||||||
imu_sampling_ratio = 1.,
|
|
||||||
landmarks_sampling_ratio = 1.,
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
-- false改为true，启动2D SLAM
|
|
||||||
MAP_BUILDER.use_trajectory_builder_2d = true
|
|
||||||
|
|
||||||
-- 0改成0.10,比机器人半径小的都忽略
|
|
||||||
TRAJECTORY_BUILDER_2D.min_range = 0.10
|
|
||||||
-- 30改成3.5,限制在雷达最大扫描范围内，越小一般越精确些
|
|
||||||
TRAJECTORY_BUILDER_2D.max_range = 3.5
|
|
||||||
-- 5改成3,传感器数据超出有效范围最大值
|
|
||||||
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 3.
|
|
||||||
-- true改成false,不使用IMU数据，大家可以开启，然后对比下效果
|
|
||||||
TRAJECTORY_BUILDER_2D.use_imu_data = false
|
|
||||||
-- false改成true,使用实时回环检测来进行前端的扫描匹配
|
|
||||||
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
|
|
||||||
-- 1.0改成0.1,提高对运动的敏感度
|
|
||||||
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1)
|
|
||||||
|
|
||||||
-- 0.55改成0.65,Fast csm的最低分数，高于此分数才进行优化。
|
|
||||||
POSE_GRAPH.constraint_builder.min_score = 0.65
|
|
||||||
--0.6改成0.7,全局定位最小分数，低于此分数则认为目前全局定位不准确
|
|
||||||
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7
|
|
||||||
|
|
||||||
-- 设置0可关闭全局SLAM
|
|
||||||
-- POSE_GRAPH.optimize_every_n_nodes = 0
|
|
||||||
|
|
||||||
return options
|
|
||||||
@@ -1,63 +0,0 @@
|
|||||||
|
|
||||||
import os
|
|
||||||
from launch import LaunchDescription
|
|
||||||
from launch.substitutions import LaunchConfiguration
|
|
||||||
from launch_ros.actions import Node
|
|
||||||
from launch_ros.substitutions import FindPackageShare
|
|
||||||
|
|
||||||
|
|
||||||
def generate_launch_description():
|
|
||||||
# 定位到功能包的地址
|
|
||||||
pkg_share = FindPackageShare(
|
|
||||||
package='gc_slam_toolbox_fish').find('gc_slam_toolbox_fish')
|
|
||||||
|
|
||||||
# =====================运行节点需要的配置=======================================================================
|
|
||||||
# 是否使用仿真时间，我们用gazebo，这里设置成true
|
|
||||||
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
|
|
||||||
# 地图的分辨率
|
|
||||||
resolution = LaunchConfiguration('resolution', default='0.05')
|
|
||||||
# 地图的发布周期
|
|
||||||
publish_period_sec = LaunchConfiguration(
|
|
||||||
'publish_period_sec', default='1.0')
|
|
||||||
# 配置文件夹路径
|
|
||||||
configuration_directory = LaunchConfiguration(
|
|
||||||
'configuration_directory', default=os.path.join(pkg_share, 'config'))
|
|
||||||
# 配置文件
|
|
||||||
configuration_basename = LaunchConfiguration(
|
|
||||||
'configuration_basename', default='gc_2d.lua')
|
|
||||||
rviz_config_dir = os.path.join(pkg_share, 'config')+"/cartographer.rviz"
|
|
||||||
print(f"rviz config in {rviz_config_dir}")
|
|
||||||
|
|
||||||
# =====================声明三个节点，cartographer/occupancy_grid_node/rviz_node=================================
|
|
||||||
cartographer_node = Node(
|
|
||||||
package='cartographer_ros',
|
|
||||||
executable='cartographer_node',
|
|
||||||
name='cartographer_node',
|
|
||||||
output='screen',
|
|
||||||
parameters=[{'use_sim_time': use_sim_time}],
|
|
||||||
arguments=['-configuration_directory', configuration_directory,
|
|
||||||
'-configuration_basename', configuration_basename])
|
|
||||||
|
|
||||||
cartographer_occupancy_grid_node = Node(
|
|
||||||
package='cartographer_ros',
|
|
||||||
executable='cartographer_occupancy_grid_node',
|
|
||||||
name='cartographer_occupancy_grid_node',
|
|
||||||
output='screen',
|
|
||||||
parameters=[{'use_sim_time': use_sim_time}],
|
|
||||||
arguments=['-resolution', resolution, '-publish_period_sec', publish_period_sec])
|
|
||||||
|
|
||||||
rviz_node = Node(
|
|
||||||
package='rviz2',
|
|
||||||
executable='rviz2',
|
|
||||||
name='rviz2',
|
|
||||||
arguments=['-d', rviz_config_dir],
|
|
||||||
parameters=[{'use_sim_time': use_sim_time}],
|
|
||||||
output='screen')
|
|
||||||
|
|
||||||
# ===============================================定义启动文件========================================================
|
|
||||||
ld = LaunchDescription()
|
|
||||||
ld.add_action(cartographer_node)
|
|
||||||
ld.add_action(cartographer_occupancy_grid_node)
|
|
||||||
# ld.add_action(rviz_node)
|
|
||||||
|
|
||||||
return ld
|
|
||||||
@@ -1,18 +0,0 @@
|
|||||||
<?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>gc_slam_toolbox_fish</name>
|
|
||||||
<version>0.0.0</version>
|
|
||||||
<description>TODO: Package description</description>
|
|
||||||
<maintainer email="guoch@todo.todo">guoch</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>
|
|
||||||
@@ -1,63 +0,0 @@
|
|||||||
include "map_builder.lua"
|
|
||||||
include "trajectory_builder.lua"
|
|
||||||
|
|
||||||
options = {
|
|
||||||
map_builder = MAP_BUILDER,
|
|
||||||
trajectory_builder = TRAJECTORY_BUILDER,
|
|
||||||
map_frame = "map",
|
|
||||||
tracking_frame = "base_link",
|
|
||||||
-- base_link改为odom,发布map到odom之间的位姿态
|
|
||||||
published_frame = "odom",
|
|
||||||
odom_frame = "odom",
|
|
||||||
-- true改为false，不用提供里程计数据
|
|
||||||
provide_odom_frame = false,
|
|
||||||
-- false改为true，仅发布2D位资
|
|
||||||
publish_frame_projected_to_2d = true,
|
|
||||||
-- false改为true，使用里程计数据
|
|
||||||
use_odometry = true,
|
|
||||||
use_nav_sat = false,
|
|
||||||
use_landmarks = false,
|
|
||||||
-- 0改为1,使用一个雷达
|
|
||||||
num_laser_scans = 1,
|
|
||||||
-- 1改为0，不使用多波雷达
|
|
||||||
num_multi_echo_laser_scans = 0,
|
|
||||||
-- 10改为1，1/1=1等于不分割
|
|
||||||
num_subdivisions_per_laser_scan = 1,
|
|
||||||
num_point_clouds = 0,
|
|
||||||
lookup_transform_timeout_sec = 0.2,
|
|
||||||
submap_publish_period_sec = 0.3,
|
|
||||||
pose_publish_period_sec = 5e-3,
|
|
||||||
trajectory_publish_period_sec = 30e-3,
|
|
||||||
rangefinder_sampling_ratio = 1.,
|
|
||||||
odometry_sampling_ratio = 1.,
|
|
||||||
fixed_frame_pose_sampling_ratio = 1.,
|
|
||||||
imu_sampling_ratio = 1.,
|
|
||||||
landmarks_sampling_ratio = 1.,
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
-- false改为true，启动2D SLAM
|
|
||||||
MAP_BUILDER.use_trajectory_builder_2d = true
|
|
||||||
|
|
||||||
-- 0改成0.10,比机器人半径小的都忽略
|
|
||||||
TRAJECTORY_BUILDER_2D.min_range = 0.10
|
|
||||||
-- 30改成3.5,限制在雷达最大扫描范围内，越小一般越精确些
|
|
||||||
TRAJECTORY_BUILDER_2D.max_range = 3.5
|
|
||||||
-- 5改成3,传感器数据超出有效范围最大值
|
|
||||||
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 3.
|
|
||||||
-- true改成false,不使用IMU数据，大家可以开启，然后对比下效果
|
|
||||||
TRAJECTORY_BUILDER_2D.use_imu_data = false
|
|
||||||
-- false改成true,使用实时回环检测来进行前端的扫描匹配
|
|
||||||
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
|
|
||||||
-- 1.0改成0.1,提高对运动的敏感度
|
|
||||||
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1)
|
|
||||||
|
|
||||||
-- 0.55改成0.65,Fast csm的最低分数，高于此分数才进行优化。
|
|
||||||
POSE_GRAPH.constraint_builder.min_score = 0.65
|
|
||||||
--0.6改成0.7,全局定位最小分数，低于此分数则认为目前全局定位不准确
|
|
||||||
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7
|
|
||||||
|
|
||||||
-- 设置0可关闭全局SLAM
|
|
||||||
-- POSE_GRAPH.optimize_every_n_nodes = 0
|
|
||||||
|
|
||||||
return options
|
|
||||||
@@ -1,63 +0,0 @@
|
|||||||
|
|
||||||
import os
|
|
||||||
from launch import LaunchDescription
|
|
||||||
from launch.substitutions import LaunchConfiguration
|
|
||||||
from launch_ros.actions import Node
|
|
||||||
from launch_ros.substitutions import FindPackageShare
|
|
||||||
|
|
||||||
|
|
||||||
def generate_launch_description():
|
|
||||||
# 定位到功能包的地址
|
|
||||||
pkg_share = FindPackageShare(
|
|
||||||
package='gc_slamtoolbox').find('gc_slamtoolbox')
|
|
||||||
|
|
||||||
# =====================运行节点需要的配置=======================================================================
|
|
||||||
# 是否使用仿真时间，我们用gazebo，这里设置成true
|
|
||||||
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
|
|
||||||
# 地图的分辨率
|
|
||||||
resolution = LaunchConfiguration('resolution', default='0.05')
|
|
||||||
# 地图的发布周期
|
|
||||||
publish_period_sec = LaunchConfiguration(
|
|
||||||
'publish_period_sec', default='1.0')
|
|
||||||
# 配置文件夹路径
|
|
||||||
configuration_directory = LaunchConfiguration(
|
|
||||||
'configuration_directory', default=os.path.join(pkg_share, 'config'))
|
|
||||||
# 配置文件
|
|
||||||
configuration_basename = LaunchConfiguration(
|
|
||||||
'configuration_basename', default='nav2_costmap_2d.lua')
|
|
||||||
rviz_config_dir = os.path.join(pkg_share, 'config')+"/cartographer.rviz"
|
|
||||||
print(f"rviz config in {rviz_config_dir}")
|
|
||||||
|
|
||||||
# =====================声明三个节点，cartographer/occupancy_grid_node/rviz_node=================================
|
|
||||||
cartographer_node = Node(
|
|
||||||
package='cartographer_ros',
|
|
||||||
executable='cartographer_node',
|
|
||||||
name='cartographer_node',
|
|
||||||
output='screen',
|
|
||||||
parameters=[{'use_sim_time': use_sim_time}],
|
|
||||||
arguments=['-configuration_directory', configuration_directory,
|
|
||||||
'-configuration_basename', configuration_basename])
|
|
||||||
|
|
||||||
cartographer_occupancy_grid_node = Node(
|
|
||||||
package='cartographer_ros',
|
|
||||||
executable='cartographer_occupancy_grid_node',
|
|
||||||
name='cartographer_occupancy_grid_node',
|
|
||||||
output='screen',
|
|
||||||
parameters=[{'use_sim_time': use_sim_time}],
|
|
||||||
arguments=['-resolution', resolution, '-publish_period_sec', publish_period_sec])
|
|
||||||
|
|
||||||
rviz_node = Node(
|
|
||||||
package='rviz2',
|
|
||||||
executable='rviz2',
|
|
||||||
name='rviz2',
|
|
||||||
arguments=['-d', rviz_config_dir],
|
|
||||||
parameters=[{'use_sim_time': use_sim_time}],
|
|
||||||
output='screen')
|
|
||||||
|
|
||||||
# ===============================================定义启动文件========================================================
|
|
||||||
ld = LaunchDescription()
|
|
||||||
ld.add_action(cartographer_node)
|
|
||||||
ld.add_action(cartographer_occupancy_grid_node)
|
|
||||||
ld.add_action(rviz_node)
|
|
||||||
|
|
||||||
return ld
|
|
||||||
@@ -1,18 +0,0 @@
|
|||||||
<?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>gc_slamtoolbox</name>
|
|
||||||
<version>0.0.0</version>
|
|
||||||
<description>TODO: Package description</description>
|
|
||||||
<maintainer email="guoch@todo.todo">guoch</maintainer>
|
|
||||||
<license>TODO: License declaration</license>
|
|
||||||
|
|
||||||
<test_depend>ament_copyright</test_depend>
|
|
||||||
<test_depend>ament_flake8</test_depend>
|
|
||||||
<test_depend>ament_pep257</test_depend>
|
|
||||||
<test_depend>python3-pytest</test_depend>
|
|
||||||
|
|
||||||
<export>
|
|
||||||
<build_type>ament_python</build_type>
|
|
||||||
</export>
|
|
||||||
</package>
|
|
||||||
@@ -1,4 +0,0 @@
|
|||||||
[develop]
|
|
||||||
script_dir=$base/lib/gc_slamtoolbox
|
|
||||||
[install]
|
|
||||||
install_scripts=$base/lib/gc_slamtoolbox
|
|
||||||
@@ -1,25 +0,0 @@
|
|||||||
from setuptools import find_packages, setup
|
|
||||||
|
|
||||||
package_name = 'gc_slamtoolbox'
|
|
||||||
|
|
||||||
setup(
|
|
||||||
name=package_name,
|
|
||||||
version='0.0.0',
|
|
||||||
packages=find_packages(exclude=['test']),
|
|
||||||
data_files=[
|
|
||||||
('share/ament_index/resource_index/packages',
|
|
||||||
['resource/' + package_name]),
|
|
||||||
('share/' + package_name, ['package.xml']),
|
|
||||||
],
|
|
||||||
install_requires=['setuptools'],
|
|
||||||
zip_safe=True,
|
|
||||||
maintainer='guoch',
|
|
||||||
maintainer_email='guoch@todo.todo',
|
|
||||||
description='TODO: Package description',
|
|
||||||
license='TODO: License declaration',
|
|
||||||
tests_require=['pytest'],
|
|
||||||
entry_points={
|
|
||||||
'console_scripts': [
|
|
||||||
],
|
|
||||||
},
|
|
||||||
)
|
|
||||||
@@ -1,25 +0,0 @@
|
|||||||
# Copyright 2015 Open Source Robotics Foundation, Inc.
|
|
||||||
#
|
|
||||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
|
||||||
# you may not use this file except in compliance with the License.
|
|
||||||
# You may obtain a copy of the License at
|
|
||||||
#
|
|
||||||
# http://www.apache.org/licenses/LICENSE-2.0
|
|
||||||
#
|
|
||||||
# Unless required by applicable law or agreed to in writing, software
|
|
||||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
|
||||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
||||||
# See the License for the specific language governing permissions and
|
|
||||||
# limitations under the License.
|
|
||||||
|
|
||||||
from ament_copyright.main import main
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
|
|
||||||
# Remove the `skip` decorator once the source file(s) have a copyright header
|
|
||||||
@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
|
|
||||||
@pytest.mark.copyright
|
|
||||||
@pytest.mark.linter
|
|
||||||
def test_copyright():
|
|
||||||
rc = main(argv=['.', 'test'])
|
|
||||||
assert rc == 0, 'Found errors'
|
|
||||||
@@ -1,25 +0,0 @@
|
|||||||
# Copyright 2017 Open Source Robotics Foundation, Inc.
|
|
||||||
#
|
|
||||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
|
||||||
# you may not use this file except in compliance with the License.
|
|
||||||
# You may obtain a copy of the License at
|
|
||||||
#
|
|
||||||
# http://www.apache.org/licenses/LICENSE-2.0
|
|
||||||
#
|
|
||||||
# Unless required by applicable law or agreed to in writing, software
|
|
||||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
|
||||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
||||||
# See the License for the specific language governing permissions and
|
|
||||||
# limitations under the License.
|
|
||||||
|
|
||||||
from ament_flake8.main import main_with_errors
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.flake8
|
|
||||||
@pytest.mark.linter
|
|
||||||
def test_flake8():
|
|
||||||
rc, errors = main_with_errors(argv=[])
|
|
||||||
assert rc == 0, \
|
|
||||||
'Found %d code style errors / warnings:\n' % len(errors) + \
|
|
||||||
'\n'.join(errors)
|
|
||||||
@@ -1,23 +0,0 @@
|
|||||||
# Copyright 2015 Open Source Robotics Foundation, Inc.
|
|
||||||
#
|
|
||||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
|
||||||
# you may not use this file except in compliance with the License.
|
|
||||||
# You may obtain a copy of the License at
|
|
||||||
#
|
|
||||||
# http://www.apache.org/licenses/LICENSE-2.0
|
|
||||||
#
|
|
||||||
# Unless required by applicable law or agreed to in writing, software
|
|
||||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
|
||||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
||||||
# See the License for the specific language governing permissions and
|
|
||||||
# limitations under the License.
|
|
||||||
|
|
||||||
from ament_pep257.main import main
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.linter
|
|
||||||
@pytest.mark.pep257
|
|
||||||
def test_pep257():
|
|
||||||
rc = main(argv=['.', 'test'])
|
|
||||||
assert rc == 0, 'Found code style errors / warnings'
|
|
||||||
Reference in New Issue
Block a user