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
|
||||
|
||||
# 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_start_pose: [0.0, 0.0, 0.0]
|
||||
map_file_name: /home/guoch/test_ws/src/gc_navigation2_slamtoolbox/maps/my_map.posegraph
|
||||
map_start_pose: [0.0, 0.0, 0.0]
|
||||
|
||||
debug_logging: false
|
||||
throttle_scans: 1
|
||||
|
||||
@@ -26,7 +26,7 @@ def generate_launch_description():
|
||||
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, 'params', 'gc_navigation.yaml'))
|
||||
gc_navigation_fish_dir, 'params', 'gc_navigation_amcl.yaml'))
|
||||
|
||||
# =============================3.声明启动launch文件，传入：地图路径、是否使用仿真时间以及nav2参数文件==============
|
||||
nav2_bringup_launch = IncludeLaunchDescription(
|
||||
@@ -38,6 +38,7 @@ def generate_launch_description():
|
||||
'params_file': nav2_param_path}.items(),
|
||||
)
|
||||
|
||||
|
||||
# =============================4.Gazebo仿真设置==========================================================
|
||||
robot_name_in_model = 'mycar'
|
||||
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