7_astar路径规划实验

前言

实验目标

在仿真环境中给出终点坐标，让无人机在octomap建立的二维全局地图上，使用A* 算法进行路径规划抵达终点。一是学习如果使用建好的图来进行规划；二是在一个文件运行所有内容的基础上，学习分模块的项目结构。

但是注意本项目中的路径规划还是非常初级，即目标点限制在已知地图中，且对于一个目标点，执行一次路径规划，无人机按照该路径走完才进行下一次规划。

实验配置

Ubuntu 20.04；
zsh；
ROS noetic；
PX4；
Gazebo；
RVIZ；

项目结构

之前简单起飞/绕圈的实验中，所有内容都写在一个cpp文件中。而在更大型的项目中，通常采用模块化的设计，将不同功能的实现放在不同的文件中，且为了多个文件之间的编译，采用.h和.cpp分离的格式

ws
/build
/devel
/src
	/ros_pkg
		/config
			params.yaml
		/include
			mavros_utils.h  // 封装好的mavros沟通工具
			astar_planner.h  // 规划器
			mission_fsm.h  // 状态机
		/src
			mavros_utils.cpp 
			astar_planner.cpp
			mission_fsm.cpp
			main_node.cpp  // 入口节点
		/launch
			mapping.launch  // 启动octomap_server
			sim.launch  // 启动仿真环境
			super.launch // 启动main
		/world
		CMakeLists.txt
		package.xml

关键模块可分为如下部分：mavros_utils飞控沟通模块，mission_fsm有限状态机，astar_plannerA* 规划器，main_node入口节点。

实验步骤

构建工作空间

安装工具

1 2	sudo apt update sudp apt install python3-catkin-tools

创建工作空间

mkdir -p ~/astar_ws/src
cd astar_ws

catkin init
catkin config --extend /opt/ros/$ROS_DISTRO
catkin config --merge-devel
catkin config --mkdirs
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release
catkin config --cmake-args -DCMAKE_CXX_FLAGS=-fdiagnostics-color

创建工作包

cd ~/astar_ws/src
catkin_create_pkg astar_uav roscpp std_msgs geometry_msgs mavros_msgs nav_msgs octomap_msgs tf2_ros eigen

cd ~/astar_ws/src/astar uav
mkdir -p include/astar_nav src launch config worlds scripts

git初始化
1
git init

工作空间说明

为什么区分.h和.cpp
首先.h文件会声明类的基础结构，例如有哪些变量，函数，而.cpp则具体写出内容，这样一是便于后续在规定的结构上修改，二是在编译时提前提供类的接口，而不必在编译文件时才知道。在一般使用时，.h与.cpp文件成对使用，分别放在include\和src\下。详细使用可见[[自定义库文件模块化设计]]

单个功能包与多个功能包
虽然相较于以前将不同功能放在不同的cpp文件中，已经实现了基础的模块化。但是更大的项目中一般将不同功能的文件放在不同的功能包中，例如fsm，plan分别在一个功能包，这样可以实现单独对某个功能的文件进行编译。

关键模块说明
在开始写关键部分代码之前，先对接下来写的内容进行一个结构上的梳理。首先是mavros_utils替代了之前代码中所有底层通信部分，被其他文件调用，所以优先构写。其次是mission_fsm，是运转的主体，连接其他所有部分。第三是main_node，用于实例化fsm，作为入口文件。第四是astar_planner，规划器订阅地图和目标点，然后被fsm调用规划。

为了节约篇章，以下只对关键内容进行记录。

飞控通信工具 mavros_utils

对于所有需要与飞控通信的mavros相关部分进行封装，使得在调用时屏蔽与mavros相关的部分，.h内容如下。

#ifndef MAVROS_UTILS_H
#define MAVROS_UTILS_H

#include<ros/ros.h>
#include<geometry_msgs/PoseStamped.h>
#include<mavros_msgs/State.h>
#include<mavros_msgs/SetMode.h>
#include<mavros_msgs/CommandBool.h>

class MavrosUtils
{
public:
    // 构造函数
    MavrosUtils(ros::NodeHandle& nh);

    // 核心接口
    bool isConnected();
    bool isArmed();
    std::string getMode();

    bool setOffboard();
    bool arm();

    // 返回当前位姿
    geometry_msgs::PoseStamped getCurrentPose();

    // 发送位置指令
    void sendPositionSetpoint(const geometry_msgs::PoseStamped& pose);

private:
    // 回调函数
    void statecb(const mavros_msgs::State::ConstPtr& msg);
    void poseCb(const geometry_msgs::PoseStamped::ConstPtr& msg);

    // 内部订阅者和发布者
    ros::Subscriber state_sub_;
    ros::Subscriber local_pos_sub_;
    ros::Publisher local_pos_pub_;

    // 服务客户端
    ros::ServiceClient arming_client_;
    ros::ServiceClient set_mode_client_;

    // 存储当前状态
    mavros_msgs::State current_state_;
    geometry_msgs::PoseStamped current_pose_;
};  

#endif // MAVROS_UTILS_H

有限状态机 mission_fsm

主控部分，通过状态机来控制整个节点的运作。

模式切换

对外界的接口，main_node通过在主循环中运行run来驱动

void MissionFSM::run()
{
    switch (current_state_)
    {
        case State::INIT:    execInit();      break;
        case State::TAKEOFF:  execTakeoff();   break;
        case State::WAIT_GOAL: execWaitGoal(); break;
        case State::EXECUTE:  execExecute();   break;
        case State::LAND:     execLand();      break;
        default: break;
    }
}

init取代之前代码中发送心跳点、解锁和切换模式的部分，完成后切换到takeoff模式。takeoff取代之前起飞到定高的部分，完成后切换到wait_goal模式。以上都是之前的代码中存在的部分，而wait_goal和execute则是新的部分。
wait_goal在接受到目标点后，先进行合法性判断，然后调用规划器去寻找一条路径，找到后进入execute模式。在execute模式下，无人机逐个提取出路径中的路径点，转换为标准的格式发布。
此处有一个细节，A* 找到的只有路径点，并不包含无人机的朝向，如果直接发布，无人机会固定朝着一个方向。需要根据当前位置和目标位置计算一个无人机的朝向，添加到路径点信息中。

入口节点 main_node

实例化所有类，并进行主循环。注意其中三个类都传递了句柄，说明三者都算是ros节点，即使不被主动调用函数时，也会订阅/发布话题信息

#include <ros/ros.h>
#include "astar_nav/mavros_utils.h"
#include "astar_nav/astar_planner.h"
#include "astar_nav/mission_fsm.h"

int main(int argc, char** argv)
{
    ros::init(argc, argv, "main_node");
    ros::NodeHandle nh;

    MavrosUtils mavros_utils(nh);
    AstarPlanner planner(nh);
    MissionFSM mission_fsm(nh, mavros_utils, planner);

    ros::Rate rate(20.0);
    while (ros::ok())
    {
        mission_fsm.run();
        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}

Astar规划器 astar_planner

由状态机调用，根据已有的地图返回一条路径。

地图订阅-膨胀地图

首先需要订阅二维地图

1	map_sub_ = nh.subscribe("/projected_map", 1, &AstarPlanner::mapCb, this);

在回调函数中，首先是将地图存储在临时变量中，其次是对地图进行膨胀，即遍历地图，对于可能是障碍物的点周围的点也判断为障碍物。而关于如何遍历地图，可见[[占据栅格图 Octomap]]

对外接口

在fsm的wait_goal中，一旦接收到了目标点则会调用接口，寻找路径。而接口函数需要将世界坐标系转换到栅格坐标系，以及判断起点/终点位置是否合法

Astar搜索

在膨胀后的地图上，使用已知的起点与终点进行A* 路径规划，这里的启发函数使用欧式距离，而A* 算法的具体实现不再列出。需要注意的是最终返回的数据结构std::vector<geometry_msgs::Point> path;，其中geometry_msgs::Point数据结构只有x, y, z三个元素，之后发表前必须转换为标准格式。

其他代码细节

地图创建

在地图中随机生成圆柱体，以将三维的路径规划简化为二维的平面上的。同时为了能够提前在rviz提前看到障碍物的地图，生成了一张具有真值的地图，然后作为话题发布。

建图模块

使用octomap进行建图，深度相机的点云话题作为输入，输出为三维占据栅格图-用于rviz展示，二维占据栅格图-用于实际路径规划。
注意tf的完整性，需要手动添加base_link到camera_link的变换

路线展示

为了在rviz中显示规划的路径，需要将搜索得到的路径作为话题发布

// 发布者结构
path_pub_ = nh.advertise<nav_msgs::Path>("/planned_path", 1);
// 发布消息
path_pub_.publish(path);

实验结果

使用rviz来查看结果。点击2D nav goal或者按G之后，使用鼠标左键在已知的地图中点击然后拖动。可以看到无人机会缓慢地飞过去

后续改进

首先是目标点只能在已知地图上，要让目标点能在已知的地图外，需要无人机边飞边规划。
其次是当前地图为全局地图，随着飞行会不断扩大，增大计算的压力，可以在上面的基础上修改为局部地图。