diff --git a/README.md b/README.md
index 73a4318..cd46063 100644
--- a/README.md
+++ b/README.md
@@ -82,7 +82,17 @@
---
-# 三、其它说明
+
+# 三、指令
+启动雷达：
+`ros2 launch lslidar_driver lsn10_launch.py `
+open new terminal
+```bash
+ros2 topic pub -1 /lslidar_order std_msgs/msg/Int8 data:\ 1\ # (open radar)
+ros2 topic pub -1 /lslidar_order std_msgs/msg/Int8 data:\ 0\ # (close radar)
+```
+
+# 四、其它说明
我设置了一些自定义指令，方便终端调试：
```bash
rosbuild # 编译指定包
@@ -90,3 +100,4 @@ build_debug # 以调试模式编译指定包
foxglove # 启动foxbridge
```
+# 五、日志
diff --git a/src/gc_navigation2_real/maps/Readme.txt b/src/gc_navigation2_real/maps/Readme.txt
new file mode 100644
index 0000000..75b15b6
--- /dev/null
+++ b/src/gc_navigation2_real/maps/Readme.txt
@@ -0,0 +1,3 @@
+# 实车地图存放目录
+# 使用 map_saver_cli 保存地图到此目录：
+# ros2 run nav2_map_server map_saver_cli -f ~/test_ws/src/gc_navigation2_real/maps/your_map_name
diff --git a/src/gc_navigation2_slamtoolbox/config/slam_toolbox_async.yaml b/src/gc_navigation2_slamtoolbox/config/slam_toolbox_async.yaml
index f0d2734..d0b1e16 100644
--- a/src/gc_navigation2_slamtoolbox/config/slam_toolbox_async.yaml
+++ b/src/gc_navigation2_slamtoolbox/config/slam_toolbox_async.yaml
@@ -12,7 +12,7 @@ slam_toolbox:
# ============================ ROS 基础参数 =========================================
odom_frame: odom # 里程计坐标系
map_frame: map # 地图坐标系
- base_frame: base_footprint # 机器人基座坐标系（与 URDF 根帧一致）
+ base_frame: base_link # 机器人基座坐标系（与 URDF 根帧一致）
scan_topic: /scan # 激光雷达话题
use_map_saver: true # 启用地图保存功能
mode: mapping # mapping = 定位 + 实时建图（地图可更新）
@@ -21,12 +21,12 @@ slam_toolbox:
# 启动时暂不加载（注释掉），通过服务在启动后手动加载：
# ros2 service call /slam_toolbox/deserialize_map slam_toolbox/srv/DeserializePoseGraph "{filename: '/home/guoch/test_ws/src/gc_navigation2_slamtoolbox/maps/my_map.posegraph', match_type: 1}"
# map_file_name: /home/guoch/test_ws/src/gc_navigation2_slamtoolbox/maps/my_map.posegraph
- map_start_pose: [0.0, 0.0, 0.0] # 初始位姿 [x, y, yaw]（map 坐标系下）
+ map_start_pose: [-2.0, -2.0, 0.0] # 初始位姿 [x, y, yaw]（map 坐标系下）
# map_start_at_dock: true # 或从 docking 位姿启动（与 pose 互斥）
# ============================ 调试与性能 ===========================================
debug_logging: false # 调试日志（生产环境关闭）
- throttle_scans: 1 # 每 N 帧激光处理一次（1=全部处理）
+ throttle_scans: 10 # 每 N 帧激光处理一次（1=全部处理）
transform_publish_period: 0.02 # TF 发布周期（秒），0=不发布里程计
map_update_interval: 3.0 # /map 话题更新间隔（秒），越小越实时
resolution: 0.05 # 地图分辨率（米/像素）
@@ -42,11 +42,11 @@ slam_toolbox:
# ============================ 通用建图参数 =========================================
use_scan_matching: true # 启用扫描匹配
use_scan_barycenter: true # 使用扫描重心
- minimum_travel_distance: 0.5 # 最小移动距离触发处理（米）
- minimum_travel_heading: 0.5 # 最小转向角度触发处理（弧度）
+ minimum_travel_distance: 0.1 # 最小移动距离触发处理（米）
+ minimum_travel_heading: 0.1 # 最小转向角度触发处理（弧度）
check_min_dist_and_heading_precisely: false
- scan_buffer_size: 10 # 扫描缓冲区大小（越大匹配越稳定）
- scan_buffer_maximum_scan_distance: 10.0 # 缓冲区最大扫描距离
+ scan_buffer_size: 5 # 扫描缓冲区大小（越大匹配越稳定）
+ scan_buffer_maximum_scan_distance: 5.0 # 缓冲区最大扫描距离
link_match_minimum_response_fine: 0.1 # 精细匹配最小响应
link_scan_maximum_distance: 1.5 # 链接扫描最大距离
diff --git a/src/gc_navigation2_slamtoolbox/config/slam_toolbox_mapping.yaml b/src/gc_navigation2_slamtoolbox/config/slam_toolbox_mapping.yaml
new file mode 100644
index 0000000..44a3d68
--- /dev/null
+++ b/src/gc_navigation2_slamtoolbox/config/slam_toolbox_mapping.yaml
@@ -0,0 +1,73 @@
+slam_toolbox:
+ ros__parameters:
+
+ # === Solver ===
+ 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_link
+ scan_topic: /scan
+ mode: mapping
+ use_map_saver: true
+
+ # === 调试与性能 ===
+ debug_logging: false
+ throttle_scans: 1
+ transform_publish_period: 0.02
+ map_update_interval: 3.0
+ resolution: 0.05
+ min_laser_range: 0.15
+ max_laser_range: 20.0
+ minimum_time_interval: 0.5
+ transform_timeout: 0.2
+ tf_buffer_duration: 30.0
+ stack_size_to_use: 40000000
+ enable_interactive_mode: true
+
+ # === 建图参数 ===
+ use_scan_matching: true
+ use_scan_barycenter: true
+ minimum_travel_distance: 0.5
+ minimum_travel_heading: 0.1
+ scan_buffer_size: 10
+ scan_buffer_maximum_scan_distance: 10.0
+ link_match_minimum_response_fine: 0.1
+ link_scan_maximum_distance: 1.5
+
+ # === 回环检测 ===
+ do_loop_closing: true
+ loop_match_minimum_chain_size: 10
+ loop_match_maximum_variance_coarse: 3.0
+ loop_match_minimum_response_coarse: 0.35
+ loop_match_minimum_response_fine: 0.45
+ loop_search_maximum_distance: 3.0
+
+ # === 扫描匹配 ===
+ 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
+ min_pass_through: 2
+ occupancy_threshold: 0.1
+
+ # MessageFilter queue size (default 1, too small for high-rate lidar)
+ scan_queue_size: 20
diff --git a/src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online_real.launch.py b/src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online_real.launch.py
new file mode 100644
index 0000000..a37bb2d
--- /dev/null
+++ b/src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online_real.launch.py
@@ -0,0 +1,80 @@
+# ============================================================================
+# gc_nav2_with_slam_online_real.launch.py
+# 功能：真机实时建图 — slam_toolbox 从零建图 + Nav2 导航
+# ============================================================================
+
+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
+
+
+def generate_launch_description():
+ """真机实时建图：slam_toolbox 从零建图 + Nav2 导航"""
+ 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='false')
+ nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
+ pkg_dir, 'params', 'gc_navigation_slam.yaml'))
+ slam_params_file = os.path.join(pkg_dir, 'config', 'slam_toolbox_mapping.yaml')
+
+ # === 3. slam_toolbox：实时建图 → 直接发布到 /map ===
+ slam_toolbox_node = Node(
+ package='slam_toolbox',
+ executable='async_slam_toolbox_node',
+ name='slam_toolbox',
+ output='screen',
+ parameters=[slam_params_file,
+ {'use_sim_time': use_sim_time}],
+ )
+
+ # === 4. Nav2 导航栈 ===
+ 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. 机器人模型 + TF ===
+ model = DeclareLaunchArgument(
+ name='model',
+ default_value=os.path.join(origincar_urdf_dir, 'urdf', 'origincar.urdf'))
+
+ robot_description = ParameterValue(
+ Command(['xacro ', LaunchConfiguration('model')]), value_type=str)
+ robot_state_publisher = Node(
+ package='robot_state_publisher',
+ executable='robot_state_publisher',
+ parameters=[{'robot_description': robot_description,
+ 'use_sim_time': use_sim_time, 'publish_frequency': 30.0}],
+ )
+
+ base_footprint_tf = Node(
+ package='tf2_ros',
+ executable='static_transform_publisher',
+ name='base_footprint_to_base_link',
+ arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'],
+ )
+
+ # === 6. 组装 ===
+ ld.add_action(model)
+ ld.add_action(robot_state_publisher)
+ ld.add_action(base_footprint_tf)
+ ld.add_action(slam_toolbox_node)
+ ld.add_action(navigation_launch)
+
+ return ld
diff --git a/src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml b/src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml
index 31f37c5..1384445 100644
--- a/src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml
+++ b/src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml
@@ -1,17 +1,17 @@
slam_toolbox:
ros__parameters:
- use_sim_time: True
+ use_sim_time: False
bt_navigator:
ros__parameters:
- use_sim_time: True
+ use_sim_time: False
global_frame: map
robot_base_frame: base_footprint
odom_topic: /odom
bt_loop_duration: 50
default_server_timeout: 20
# 阿克曼底盘专用 BT：移除 Spin，恢复行为 = 清代价地图 → 后退 → 等待
- default_nav_to_pose_bt_xml: /home/guoch/test_ws/install/gc_navigation2_slamtoolbox/share/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml
+ default_nav_to_pose_bt_xml: /home/sunrise/yiliao_ws/install/gc_navigation2_slamtoolbox/share/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml
plugin_lib_names:
- nav2_compute_path_to_pose_action_bt_node
- nav2_compute_path_through_poses_action_bt_node
@@ -59,11 +59,11 @@ bt_navigator:
bt_navigator_rclcpp_node:
ros__parameters:
- use_sim_time: True
+ use_sim_time: False
controller_server:
ros__parameters:
- use_sim_time: True
+ use_sim_time: False
controller_frequency: 20.0
FollowPath:
plugin: "nav2_mppi_controller::MPPIController"
@@ -142,7 +142,7 @@ controller_server:
controller_server_rclcpp_node:
ros__parameters:
- use_sim_time: True
+ use_sim_time: False
local_costmap:
local_costmap:
@@ -152,7 +152,7 @@ local_costmap:
transform_tolerance: 0.5
global_frame: odom
robot_base_frame: base_link
- use_sim_time: True
+ use_sim_time: False
rolling_window: true
width: 3
height: 3
@@ -192,21 +192,21 @@ local_costmap:
always_send_full_costmap: True
local_costmap_client:
ros__parameters:
- use_sim_time: True
+ use_sim_time: False
local_costmap_rclcpp_node:
ros__parameters:
- use_sim_time: True
+ use_sim_time: False
global_costmap:
global_costmap:
ros__parameters:
- use_sim_time: True
+ use_sim_time: False
transform_tolerance: 0.5
update_frequency: 1.0
publish_frequency: 1.0
global_frame: map
robot_base_frame: base_link
- use_sim_time: True
+ use_sim_time: False
footprint: "[[0.14, 0.085],
[0.14, -0.085],
[-0.14, -0.085],
@@ -239,14 +239,14 @@ global_costmap:
always_send_full_costmap: True
global_costmap_client:
ros__parameters:
- use_sim_time: True
+ use_sim_time: False
global_costmap_rclcpp_node:
ros__parameters:
- use_sim_time: True
+ use_sim_time: False
map_saver:
ros__parameters:
- use_sim_time: True
+ use_sim_time: False
save_map_timeout: 5.0
free_thresh_default: 0.25
occupied_thresh_default: 0.65
@@ -255,7 +255,7 @@ map_saver:
planner_server:
ros__parameters:
planner_plugins: ["GridBased"]
- use_sim_time: True
+ use_sim_time: False
GridBased:
plugin: "nav2_smac_planner/SmacPlannerHybrid"
@@ -291,11 +291,11 @@ planner_server:
planner_server_rclcpp_node:
ros__parameters:
- use_sim_time: True
+ use_sim_time: False
smoother_server:
ros__parameters:
- use_sim_time: True
+ use_sim_time: False
smoother_plugins: ["simple_smoother"]
simple_smoother:
plugin: "nav2_smoother::SimpleSmoother"
@@ -321,7 +321,7 @@ behavior_server:
global_frame: odom
robot_base_frame: base_link
transform_tolerance: 0.5
- use_sim_time: True
+ use_sim_time: False
simulate_ahead_time: 2.0
max_rotational_vel: 1.0
min_rotational_vel: 0.4
@@ -329,12 +329,12 @@ behavior_server:
robot_state_publisher:
ros__parameters:
- use_sim_time: True
+ use_sim_time: False
waypoint_follower:
ros__parameters:
loop_rate: 20
- use_sim_time: True
+ use_sim_time: False
stop_on_failure: false
waypoint_task_executor_plugin: "wait_at_waypoint"
wait_at_waypoint:
diff --git a/src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml.bak b/src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml.bak
new file mode 100644
index 0000000..31f37c5
--- /dev/null
+++ b/src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml.bak
@@ -0,0 +1,343 @@
+slam_toolbox:
+ 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: 50
+ default_server_timeout: 20
+ # 阿克曼底盘专用 BT：移除 Spin，恢复行为 = 清代价地图 → 后退 → 等待
+ default_nav_to_pose_bt_xml: /home/guoch/test_ws/install/gc_navigation2_slamtoolbox/share/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml
+ 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: 20.0
+ FollowPath:
+ plugin: "nav2_mppi_controller::MPPIController"
+ time_steps: 36
+ model_dt: 0.05
+ batch_size: 1000
+ 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
+
+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_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:
+ use_sim_time: True
+ transform_tolerance: 0.5
+ 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_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
+ 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: 1.3 # 后退惩罚，越小越愿意后退（默认 2.0，设为 1.3 允许灵活倒车）
+ 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" # 需保留以匹配默认 BT XML
+ backup:
+ plugin: "nav2_behaviors/BackUp"
+ backup_dist: 0.8
+ backup_speed: 0.18
+ wait:
+ plugin: "nav2_behaviors/Wait"
+ wait_duration: 0.5
+ global_frame: odom
+ robot_base_frame: base_link
+ 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
+
+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
diff --git a/src/origincar_base/config/ekf.yaml b/src/origincar_base/config/ekf.yaml
index 4c15af1..a0d236f 100644
--- a/src/origincar_base/config/ekf.yaml
+++ b/src/origincar_base/config/ekf.yaml
@@ -1,222 +1,48 @@
### ekf config file ###
ekf_filter_node:
ros__parameters:
-# ͨóʼλ
- use_pose_with_covariance_stamped: true # ͨ
- pose0: /set_pose # ָ
- pose0_config: [true, true, false, # ʹ x, y
- false, false, true] # ʹ yaw
- pose0_differential: false
-# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
-# computation until it receives at least one message from one of the inputs. It will then run continuously at the
-# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
frequency: 30.0
-
-# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict
-# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the
-# filter will generate new output. Defaults to 1 / frequency if not specified.
sensor_timeout: 2.0
-
-# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
-# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
-# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
-# by, for example, an IMU. Defaults to false if unspecified.
two_d_mode: true
-
-# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for
-# future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if
-# unspecified.
transform_time_offset: 0.0
-
-# Use this parameter to provide specify how long the tf listener should wait for a transform to become available.
-# Defaults to 0.0 if unspecified.
transform_timeout: 0.2
-
-# If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is
-# unhappy with any settings or data.
print_diagnostics: false
-
-# Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by
-# debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious
-# effects on the performance of the node. Defaults to false if unspecified.
debug: false
-
-# Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path.
- debug_out_file: /path/to/debug/file.txt
-
-# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
publish_tf: true
-
-# Whether to publish the acceleration state. Defaults to false if unspecified.
publish_acceleration: false
-# REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and
-# earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames.
-# The robot's position in the odom frame will drift over time, but is accurate in the short term and should be
-# continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom
-# frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your
-# robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based
-# localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame.
-# ekf_localization_node and ukf_localization_node are not concerned with the earth frame.
-# Here is how to use the following settings:
-# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
-# 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of
-# odom_frame.
-# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set
-# "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
-# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates
-# from landmark observations) then:
-# 3a. Set your "world_frame" to your map_frame value
-# 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state
-# estimation node from robot_localization! However, that instance should *not* fuse the global data.
- map_frame: map # Defaults to "map" if unspecified
- odom_frame: odom_combined # Defaults to "odom" if unspecified
- base_link_frame: base_footprint # Defaults to "base_link" if unspecified
- world_frame: odom_combined # Defaults to the value of odom_frame if unspecified
+ map_frame: map
+ odom_frame: odom
+ base_link_frame: base_link
+ world_frame: odom
-# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry,
-# geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped,
-# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0,
-# odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no
-# default values, and must be specified.
odom0: odom
-# Each sensor reading updates some or all of the filter's state. These options give you greater control over which
-# values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only
-# want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the
-# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types
-# do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message
-# has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false
-# if unspecified, effectively making this parameter required for each sensor.
odom0_config: [true, false, false,
false, false, false,
true, true, false,
false, false, true,
false, false, false]
-
-# If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase
-# the size of the subscription queue so that more measurements are fused.
odom0_queue_size: 10
-
-# [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result
-# of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's
-# algorithm.
odom0_nodelay: false
-
-# [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under-
-# report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they
-# arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also
-# measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't
-# always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose
-# data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then
-# integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true
-# for twist measurements has no effect.
odom0_differential: true
-
-# [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point"
-# for all future measurements. While you can achieve the same effect with the differential paremeter, the key
-# difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before
-# integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true.
odom0_relative: false
-# [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to
-# control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to
-# numeric_limits::max() if unspecified. It is strongly recommended that these parameters be removed if not
-# required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation.
-# For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying
-# the thresholds.
-# odom0_pose_rejection_threshold: 5.0
-# odom0_twist_rejection_threshold: 1.0
-
imu0: /imu/data_raw
imu0_config: [false, false, false,
false, false, true,
false, false, false,
false, false, true,
- false, false, false]
-
+ false, false, false]
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 10
- imu0_pose_rejection_threshold: 20.0 # Note the difference in parameter names
- imu0_twist_rejection_threshold: 1.542 #
- imu0_linear_acceleration_rejection_threshold: 10.0 #
-
-# [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set
-# this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame.
imu0_remove_gravitational_acceleration: true
-# [ADVANCED] The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no
-# acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During
-# correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be
-# problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When
-# this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially
-# noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance
-# for the velocity variable in question, or decrease the variance of the variable in question in the measurement
-# itself. In addition, users can also take advantage of the control command being issued to the robot at the time we
-# make the prediction. If control is used, it will get converted into an acceleration term, which will be used during
-# predicition. Note that if an acceleration measurement for the variable in question is available from one of the
-# inputs, the control term will be ignored.
-# Whether or not we use the control input during predicition. Defaults to false.
use_control: false
-# Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to
-# false.
stamped_control: false
-# The last issued control command will be used in prediction for this period. Defaults to 0.2.
control_timeout: 0.2
-# Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw.
control_config: [true, false, false, false, false, true]
-# Places limits on how large the acceleration term will be. Should match your robot's kinematics.
acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
-# Acceleration and deceleration limits are not always the same for robots.
deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
-# If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these
-# gains
acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
-# If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these
-# gains
- deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]
-# [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is
-# exposed as a configuration parameter. This matrix represents the noise we add to the total error after each
-# prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be.
-# However, if users find that a given variable is slow to converge, one approach is to increase the
-# process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error
-# to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are
-# ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if
-# unspecified.
- process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015]
-# [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal
-# value (variance) to a large value will result in rapid convergence for initial measurements of the variable in
-# question. Users should take care not to use large values for variables that will not be measured directly. The values
-# are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below
-#if unspecified.
- initial_estimate_covariance: [1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9]
-
diff --git a/src/origincar_base/scripts/__pycache__/cmd_vel_to_ackermann_drive.cpython-310.pyc b/src/origincar_base/scripts/__pycache__/cmd_vel_to_ackermann_drive.cpython-310.pyc
new file mode 100644
index 0000000..f404eaa
Binary files /dev/null and b/src/origincar_base/scripts/__pycache__/cmd_vel_to_ackermann_drive.cpython-310.pyc differ
diff --git a/src/origincar_base/scripts/cmd_vel_to_ackermann_drive.py b/src/origincar_base/scripts/cmd_vel_to_ackermann_drive.py
old mode 100644
new mode 100755