1、环境搭建：

1、创建工作区间：
mkdir -p ~/A_ws/src
cd ~/ros2_ws/src

2、获取源码：
参考开源项目：https://github.com/AAArpan/Autonomous-Navigation-and-Path-planning-on-ROS2-using-Dijkstra-and-SLAM.git

3、清理冗余：
~/ros2_ws/src/Autonomous-Navigation-and-Path-planning-on-ROS2-using-Dijkstra-and-SLAM/my_py_pkg/package.xml文件中的依赖多了一个：
删除掉文件中的<depend>my_robot_interfaces</depend>

4、安装环境依赖

sudo apt update

# 安装 TurtleBot3 相关包和 SLAM 工具箱
sudo apt install ros-humble-turtlebot3* ros-humble-slam-toolbox

# 安装 Nav2 导航相关包（项目后期导航需要）
sudo apt install ros-humble-navigation2 ros-humble-nav2-bringup

5、编译工作区间：

# 1. 回到工作空间根目录
cd ~/ros2_ws

# 2. 编译
colcon build

2、运行仿真环境

1、修复启动环境代码：
~/A_ws/src/Autonomous-Navigation-and-Path-planning-on-ROS2-using-Dijkstra-and-SLAM/turtlbot3_custom/launch/turtlebot3_world.launch.py的启动文件的模型地址采用的是硬编码的形式写死了，我们改为ros2的自动搜索：
world_path = os.path.join(get_package_share_directory('turtlbot3_custom'), 'worlds', 'path2.world')

2、重新编译：
cd ~/A_ws
colcon build --packages-select turtlbot3_custom（只编译一个功能包）

3、加载环境变量：
source install/setup.bash（或者写到~/.bashrc的文件里面）
export TURTLEBOT3_MODEL=burger（设置模型变量）

4、启动仿真环境：
ros2 launch turtlbot3_custom turtlebot3_world.launch.py use_sim_time:=true
此时如果出现小车能加载，但是看不到迷宫，则是启动文件出现问题，检查worlds文件是否真有path2.world文件，然后修改launch.py文件

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

def generate_launch_description():
    # 1. 基础配置
    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
    
    # 获取各个包的路径
    pkg_share = get_package_share_directory('turtlbot3_custom')       # 你的自定义包
    gazebo_ros_dir = get_package_share_directory('gazebo_ros')        # Gazebo基础包
    turtlebot3_gazebo_dir = get_package_share_directory('turtlebot3_gazebo') # Turtlebot3官方包
    
    # 2. 设置地图路径 (保留我们之前的修复)
    world_path = os.path.join(pkg_share, 'worlds', 'path2.world')

    # 3. 配置 Gazebo 服务器 (加载地图)
    gzserver_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(gazebo_ros_dir, 'launch', 'gzserver.launch.py')
        ),
        launch_arguments={'world': world_path}.items()
    )

    # 4. 配置 Gazebo 客户端 (图形界面)
    gzclient_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(gazebo_ros_dir, 'launch', 'gzclient.launch.py')
        )
    )

    # 5. 配置 Robot State Publisher (这一步会读取环境变量中的 TURTLEBOT3_MODEL 并发布模型)
    # 我们直接复用官方写好的 launch 文件，这样最稳健
    robot_state_publisher_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(turtlebot3_gazebo_dir, 'launch', 'robot_state_publisher.launch.py')
        ),
        launch_arguments={'use_sim_time': use_sim_time}.items()
    )

    # 6. 配置 Spawn Entity (这一步把机器人“生”在地图里)
    # 同样复用官方的生成脚本
    spawn_turtlebot_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(turtlebot3_gazebo_dir, 'launch', 'spawn_turtlebot3.launch.py')
        ),
        launch_arguments={
            'x_pose': '-2.0',  # 设定初始位置x，避免撞墙
            'y_pose': '-0.5'   # 设定初始位置y
        }.items()
    )

    return LaunchDescription([
        gzserver_cmd,
        gzclient_cmd,
        robot_state_publisher_cmd,
        spawn_turtlebot_cmd
    ])

5、重新编译和运行：
cd ~/A_ws
colcon build --packages-select turtlbot3_custom
source install/setup.bash
export TURTLEBOT3_MODEL=burger（可以写到~/.bashrc，放到source /home/xk/A_ws/install/setup.bash下面）
ros2 launch turtlbot3_custom turtlebot3_world.launch.py use_sim_time:=true
能够看到Gazebo中出现迷宫和小车

3、Slam建图

总共分为五步骤：
打开仿真环境->启动SLAM -> 可视化 -> 遥控建图 -> 保存地图（五个终端）

3.1 打开仿真环境：

source install/setup.bash（或者写到~/.bashrc的文件里面）
export TURTLEBOT3_MODEL=burger（设置模型变量）
ros2 launch turtlbot3_custom turtlebot3_world.launch.py use_sim_time:=true

3.2 启动SLAM

ros2 launch slam_toolbox online_async_launch.py
使用 slam_toolbox 来处理机器人的激光雷达数据并构建地图

3.3 可视化Rviz

rviz2
SLAM 在后台运行，但需要“看到”地图建立的过程，才能知道哪里还没扫到，但是需要配置一下：

• 修改 Fixed Frame：在左侧 "Displays" 面板最上方，找到 Fixed Frame，将其从 map (如果报错) 或 base_link 修改为 odom 或 map (建议先输 map，如果报错变红就改成 odom)。

• 添加 Map 显示：点击左下角的 Add 按钮 -> 在弹窗中选择 By topic 标签页 -> 找到 /map 话题 -> 选择 Map -> 点击 OK。

• 添加机器人模型：点击 Add -> By display type -> 选择 RobotModel -> OK。

• 添加激光雷达数据 (可选)：点击 Add -> By topic -> /scan -> LaserSca

3.4 遥控建图：

export TURTLEBOT3_MODEL=burger
ros2 run turtlebot3_teleop teleop_keyboard
通过按键控制小车移动，rviz会通过map话题实时显示扫描过的地图

3.5 保存地图

cd A_ws/
ros2 run nav2_map_server map_saver_cli -f my_maze_map

~/A_ws 目录下会多出两个文件：

1. my_maze_map.pgm (地图图片，灰色)

2. my_maze_map.yaml (地图参数文件)

4、离线A*算法导航

1、安装Python依赖
pip3 install opencv-python

2、编写A*节点：
cd ~/A_ws
touch A_Star.py
gedit A_Star.py

import cv2
import numpy as np
import heapq

# --- A* 算法实现 ---
def heuristic(a, b):
    # 曼哈顿距离启发函数
    return abs(a[0] - b[0]) + abs(a[1] - b[1])

def a_star(grid, start, goal):
    rows, cols = grid.shape
    # 优先级队列，存储 (F值, G值, (r, c))
    # F = G + H
    pq = []
    heapq.heappush(pq, (0 + heuristic(start, goal), 0, start))
    
    # 记录从起点到该点的实际代价 G
    cost_so_far = {start: 0}
    # 记录路径来源，用于回溯
    came_from = {start: None}
    
    while pq:
        # 取出 F 值最小的节点
        current_f, current_g, current_node = heapq.heappop(pq)
        
        # 如果到达终点
        if current_node == goal:
            break
        
        r, c = current_node
        # 上下左右四个方向
        directions = [(-1, 0), (1, 0), (0, -1), (0, 1)]
        
        for dr, dc in directions:
            nr, nc = r + dr, c + dc
            
            # 检查边界
            if 0 <= nr < rows and 0 <= nc < cols:
                # 检查障碍物 (grid中 1 代表障碍物, 0 代表空地)
                if grid[nr][nc] == 0:
                    new_cost = current_g + 1 # 假设每步代价为 1
                    
                    if (nr, nc) not in cost_so_far or new_cost < cost_so_far[(nr, nc)]:
                        cost_so_far[(nr, nc)] = new_cost
                        priority = new_cost + heuristic((nr, nc), goal)
                        heapq.heappush(pq, (priority, new_cost, (nr, nc)))
                        came_from[(nr, nc)] = current_node
                        
    # 路径回溯
    path = []
    if goal not in came_from:
        return [] # 未找到路径
        
    current = goal
    while current:
        path.append(current)
        current = came_from[current]
    
    return path[::-1] # 反转路径，从起点到终点

# --- 交互与显示逻辑 ---
start_point = None
end_point = None

def click_event(event, x, y, flags, param):
    global start_point, end_point, maze_display
    
    if event == cv2.EVENT_LBUTTONDOWN:
        # OpenCV 坐标是 (x, y) -> (col, row)
        # 我们的 grid 索引是 (row, col)
        r, c = y, x
        
        if start_point is None:
            start_point = (r, c)
            cv2.circle(maze_display, (x, y), 3, (0, 255, 0), -1) # 绿色起点
            print(f"起点已选择: {start_point}")
            cv2.imshow("Map", maze_display)
        elif end_point is None:
            end_point = (r, c)
            cv2.circle(maze_display, (x, y), 3, (0, 0, 255), -1) # 红色终点
            print(f"终点已选择: {end_point}")
            
            # 开始计算路径
            print("正在使用 A* 算法计算路径...")
            path = a_star(maze, start_point, end_point)
            
            if path:
                print(f"路径规划成功！路径长度: {len(path)}")
                for node in path:
                    # 画路径点 (注意 draw 时坐标是 x,y 即 col,row)
                    maze_display[node[0], node[1]] = [255, 0, 0] # 蓝色路径
                cv2.imshow("Map", maze_display)
            else:
                print("未找到路径！请检查起点或终点是否在墙壁内。")
                
            cv2.imshow("Map", maze_display)

# --- 主程序 ---
if __name__ == "__main__":
    # 1. 读取地图 (请确保路径正确)
    map_path = '/home/xk/A_ws/my_maze_map.pgm'
    maze_img = cv2.imread(map_path, cv2.IMREAD_GRAYSCALE)
    
    if maze_img is None:
        print(f"错误: 无法读取地图文件，请检查路径: {map_path}")
        exit()

    # 2. 处理地图数据
    # SLAM地图中: 0是黑(墙), 254是白(空), 205是灰(未知)
    # 我们将像素值 < 10 的视为障碍物 (值为1)，其他视为通路 (值为0)
    maze = (maze_img < 10).astype(np.uint8)

    # 3. 创建用于显示的彩色地图
    maze_display = cv2.cvtColor(maze_img, cv2.COLOR_GRAY2BGR)
    
    print("------------------------------------------")
    print("A* 路径规划器已启动")
    print("1. 请用鼠标左键点击地图选择 [起点]")
    print("2. 再次点击选择 [终点]")
    print("3. 按任意键退出")
    print("------------------------------------------")

    cv2.imshow("Map", maze_display)
    cv2.setMouseCallback("Map", click_event)

    cv2.waitKey(0)
    cv2.destroyAllWindows()

3、赋予脚本权限：
chmod +x A_Star.py
python3 A_Star.py

以上是纯地图的情况下计算两个点的最短距离，也称为离线导航。

5、运行导航堆栈

5.1 开启Gazebo仿真环境

# Terminal 1
source ~/A_ws/install/setup.bash
export TURTLEBOT3_MODEL=burger
ros2 launch turtlbot3_custom turtlebot3_world.launch.py use_sim_time:=true

5.2 启动导航堆栈

source ~/A_ws/install/setup.bash
export TURTLEBOT3_MODEL=burger

# 启动 Nav2，加载你的地图 (路径确保正确)
ros2 launch turtlebot3_navigation2 navigation2.launch.py map:=/home/xk/A_ws/my_maze_map.yaml use_sim_time:=true params_file:=/home/xk/A_ws/real_astar.yaml

PS：这里第一次打开时可能不会出现代价地图和AMCL的粒子云，是因为RVIZ还不知道初始位置在哪，所以需要加一步操作：
在 Rviz 工具栏上方，找到并点击 2D Pose Estimate 按钮，看着 Gazebo 里小车的位置和朝向。在 Rviz 的地图上，点击对应的位置，按住鼠标拖动出箭头方向（和小车朝向一致），松开。现在状态正常了，可以点击 Nav2 Goal，在地图空白处点一下，小车就会动起来了。

1、一键启动脚本

编写启动脚本文件launch_all.sh，然后赋予脚本权限：chmod +x 

#!/bin/bash

# export LIBGL_ALWAYS_SOFTWARE=1
export TURTLEBOT3_MODEL=burger

# ================= 配置区域 =================
WORKSPACE_DIR=~/A_ws
MAP_YAML="${WORKSPACE_DIR}/my_maze_map.yaml"
NAV2_PARAMS="${WORKSPACE_DIR}/real_astar.yaml"
PY_ASTAR_SCRIPT="${WORKSPACE_DIR}/A_Star.py"
# ===========================================

# 定义清理函数：强制杀掉所有相关进程
function cleanup() {
    echo ""
    echo "=========================================="
    echo "正在执行清理操作 (WSL2 强力模式)..."
    echo "=========================================="
    
    # 杀掉脚本后台任务
    kill $(jobs -p) 2>/dev/null
    
    # 强制杀掉 ROS 2 和仿真相关进程
    killall -9 gzserver gzclient rviz2 robot_state_publisher nav2_lifecycle_manager nav2_planner nav2_controller 2>/dev/null
    
    echo "清理完成！环境已重置。"
    exit
}

# 捕获 Ctrl+C 信号 (SIGINT)，触发 cleanup 函数
trap cleanup SIGINT

# 1. 环境初始化
echo "[1/4] 初始化环境变量..."
source /opt/ros/humble/setup.bash
source "${WORKSPACE_DIR}/install/setup.bash"


# 2. 预清理（防止之前残留的僵尸进程）
killall -9 gzserver gzclient rviz2 2>/dev/null

# 3. (可选) A* 算法原理演示
echo "------------------------------------------"
read -t 10 -p "是否先运行 Python A* 算法原理演示? (y/N) [10秒自动跳过]: " run_demo
echo "------------------------------------------"

if [[ "$run_demo" == "y" || "$run_demo" == "Y" ]]; then
    if [ -f "$PY_ASTAR_SCRIPT" ]; then
        echo "正在启动 A* 算法演示脚本..."
        echo "请在演示窗口中点击起点和终点。完成后按任意键或关闭窗口继续..."
        python3 "$PY_ASTAR_SCRIPT"
    else
        echo "警告: 未找到 A_Star.py，跳过演示。"
    fi
fi

# 4. 启动仿真与导航
echo ""
echo "[2/4] 正在启动 Gazebo 仿真环境..."
ros2 launch turtlbot3_custom turtlebot3_world.launch.py use_sim_time:=true &
GAZEBO_PID=$!
sleep 20 # 等待 Gazebo 启动

echo ""
echo "[3/4] 正在启动 Navigation2 堆栈..."
echo "      - 加载地图: ${MAP_YAML}"
echo "      - 加载参数: ${NAV2_PARAMS} (已针对 A* 规划调优)"

# 检查文件是否存在
if [ ! -f "$MAP_YAML" ] || [ ! -f "$NAV2_PARAMS" ]; then
    echo "错误: 地图或参数文件缺失！请检查路径。"
    cleanup
fi

ros2 launch turtlebot3_navigation2 navigation2.launch.py \
    map:="${MAP_YAML}" \
    params_file:="${NAV2_PARAMS}" \
    use_sim_time:=true &
NAV2_PID=$!

echo ""
echo "[4/4] 系统启动完毕！"
echo "=========================================="
echo "操作指南:"
echo "1. 在 Rviz 中使用 '2D Pose Estimate' 初始化位置"
echo "2. 使用 'Nav2 Goal' 发送目标点 (底层规划器将使用 A*)"
echo "3. 按 'Ctrl + C' 结束所有进程并自动清理"
echo "=========================================="

# 挂起脚本，等待用户按 Ctrl+C
wait