路径规划学习笔记
第一章 环境配置
1 Ubuntu16.04安装ROS Kinetic（20220315亲测可行）
1.1 配置Ubuntu软件仓库
打开Ubuntu Software
选择Software & Updates
设置仓库更新
修改镜像源为清华大学
1.2 设置 sources.list
sudo sh -c '. /etc/lsb-release && echo "deb http://mirrors.tuna.tsinghua.edu.cn/ros/ubuntu/ `lsb_release -cs` main" > /etc/apt/sources.list.d/ros-latest.list'
1.3 设置秘钥
sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
1.4 安装
更新软件包
sudo apt-get update
安装完整版(若长时间不动，可 Ctrl+c 停止，重新开始)
sudo apt-get install ros-kinetic-desktop-full
1.5 初始化和更新rosdep
sudo rosdep init
如果初始化报错
sudo gedit /etc/hosts
在弹出的框中，回车添加以下内容名，并保存
199.232.28.133 raw.githubusercontent.com
初始化成功后，更新rosdep
rosdep update
若出现错误，则使用rosdepc
sudo pip install rosdepc（若不行试试sudo pip3 install rosdepc）
还是不行则
sudo apt-get install python3-pipsudo pip install rosdepc
安装完成后，则可初始化和更新
sudo rosdepc initrosdepc update
1.6 环境配置
echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc
source ~/.bashrc
1.7 构建工厂依赖
sudo apt-get install python-rosinstall python-rosinstall-generator python-wstool build-essential
1.8 检验是否安装成功
roswtf
可以看到 0 warning , 0 fault
到此环境配置成功
2 作业总结
2.1 下载并解压课程包hw_1
2.2 创建工作空间
Ctrl+Alt+T，开启终端，在home目录下创建工作路径，并打开创建的工作路径
// mkdir filename 创建一个文件夹
// mkdir -p ~/filename1/filename2 这里需要创建一个工作路径，故用该命令
mkdir -p ~/catkin_ws/src
打开创建的路径（Tab键命令补齐，Ctrl+c终止程序，Ctrl+d退出终端，Ctrl+l清屏）
// cd ~/filename1/filename2 打开文件夹
cd ~/catkin_ws/src/
复制压缩包hw_1/src里的三个文件夹（功能包）到创建的路径下
# sudo mv filename target_work_path 移动一个文件夹到另一个文件夹
# sudo cp -r filename target_work_path 移动一个文件夹到另一个文件夹
# 当前工作路径应是要移动的文件夹所在路径
sudo cp -r grid_path_searcher ~/catkin_ws/src
sudo cp -r rviz_plugins ~/catkin_ws/src
sudo cp -r waypoint_generator ~/catkin_ws/src
终端执行以下命令
//生成CMakeList.txt，将上述复制的三个文件夹看做员工，则新生成的txt看做是员工表
catkin_init_workspace
cd ~/catkin_ws/
//编译上述三个功能包，生成两个文件夹：devel（执行文件）、build（编译文件）
catkin_make
//告诉ROS工作空间路径
source devel/setup.bash
2.3 打开 rviz
//启动ROS自带的节点
roscore
Ctrl+Alt+t，开启一个新终端（Ctrl+Shift+t），执行以下命令
//告诉ROS工作空间路径
cd ~/catkin_ws/
source devel/setup.bash
// 如果，没有以上两条，可能会出错
rviz
出现 rviz 初始界面
2.4 打开 rviz 配置文件
open config 配置文件路径为
~/catkin_ws/src/grid_path_searcher/launch/rviz_config/demo.rviz
得到以下画面
2.5 配置 rviz 插件
点击“+”，添加 Goal3DTool插件
点击“-”，删除 2D Nav Goal 和 2D Pose Estimate
2.6 载入地图
Ctrl+Alt+t，开启一个新终端，执行以下命令
//告诉ROS工作空间路径
cd ~/catkin_ws/
source devel/setup.bash
//运行src中第一个功能包的demo.lanch文件，导入地图
roslaunch grid_path_searcher demo.launch
可以看到 rviz 自动载入了点云地图
点击3D New Goal，在此地图中按住左键鼠标，出现绿色箭头，移动鼠标用于调整箭头方向，再 按住鼠标右键，移动鼠标调整箭头的高度，最后松开鼠标左右键，能够看到 在 3 维地图中规划出的一条从终点到达地图中心的一条路径。绿色的为Astar规划出的路径，红的的为JPS规划的路径。
最后一个终端可以看到打印数据。
出现问题可以从创建文件夹开始再试一次，有意想不到的效果。
2.7 rqt_graph工具使用
显示ROS此时有哪些节点node（椭圆形）在工作，发布了哪些话题topic（方形）。
/goal 生成起点
/random_complex 生成随机地图和终点
/demo_node 获取 /goal 和 /random_complex 生成的信息
/demo_node/grid_map_vis 在rviz中显示地图
/demo_node/grid_path_vis 在rviz中显示路径点
第二章
1 图搜索基础
工作空间：
配置空间：
膨胀检测：
图由节点和边构成，可分为有向图、无向图和权重图。
图搜索算法框架
- 维护两个容器，一个容器存放将访问的节点，一个存放已访问的节点
- 访问容器首先存放的是起点节点
- 循环
- 访问：从访问容器中根据某种规则弹出一个节点（对于Astar是 f(n) 值最小的节点）
- 扩展：扩展当前节点的所有邻居节点
- 装入容器：将所有邻居装入到访问容器中
- 结束循坏：访问到目标节点或容器中无节点弹出
广度优先搜索BFS：先进先出
深度优先DFS：后进先出
贪心算法：用一个函数估计当前点到终点的距离
2 Astar算法
在图搜索算法中，若在搜索的每一步都利用估价函数 f(n) = g(n) + h(n) 对容器中的节点进行排序，则该算法为Astar算法。Astar算法是一种图搜索算法，在图中求最短路径的搜索方法。
Astar算法的估价函数：f(n) = g(n) + h(n)
f(n)：总搜索代价
g(n)：起点到当前节点 n 的代价和
h(n)：启发函数，当前节点 n 到目标节点的实际距离的猜测，指引搜索的正确方向。
欧几里得启发函数：
曼哈顿启发函数：
Astar算法伪代码
- 维护容器，这个容器桨扩展的点按从小到大的顺序进行排列
- 将起始节点 A 放入这个容器
- 定义 g(A)=0, g(n)= ∞
- 循环
- if 容器是空，return False; break
- 从容器中弹出 f 值最小的节点 n
- 把这个节点 n 放到另一个放到被访问节点的容器
- if 节点 n 是目标节点，return True; break
- 扩展节点 n 的所有邻居节点 m
- if g(m)=∞（即节点 m 未被扩展过）
- g(m)=g(n)+Cnm（Cnm为节点 n 到节点 m 的距离）
- 将节点 m 放到容器中
- if g(m)>g(n)+Cnm（节点 m 被扩展过，此时需要更新 g(m) 值）
- g(m)=g(n)+Cnm
- 结束
- 结束循环