MinimumSnap代码大致讲解

引言

        发现大家对这份代码的内容不太熟悉，对于初学者来说确实较难理解，下面对这个功能包的代码内容做一个大致介绍以及如何启动。大家要是想要详细了解每一部分的具体用途可以结合代码注释和理论知识来进行理解

一、功能包内容介绍

        打开提供的MinimumSnap功能包我们会发现下面还分成了三个功能包，分别对应完成不同功能，下面介绍每一项的大致内容内容以及功能包启动方法。

在这先提一下，假如不了解这个功能包的理论知识，先去看看下面这篇文章，不要求大家看懂每一个数学公式是如何推导的，但需要大概知道这个做了一个什么样的事情。

【附源码和详细的公式推导】Minimum Snap轨迹生成，闭式求解Minimum Snap问题，机器人轨迹优化，多项式轨迹路径生成与优化-CSDN博客

        （1）min_snap：

       这个功能包下面的min_snap_generator.cpp是我们后面最经常用到的，在这份cpp文件里面包含了轨迹的预设点位数组“list_original”用于存放我们事先预设的点位，轨迹执行的触发函数“eight_goal_cb”，重规划执行函数“getCircleCenter”。

        这里提及一下重规划以及全局规划的概念，全局规划就是在生成轨迹之前给出所有的点，生成一条全局轨迹。这条轨迹可以是准确的也可以是大概的，准确的意思是我们给出的预先点都是我们实际要执行的航路点，后续无需对其进行修正；大概的意思是航路点是有偏差的，后续需要通过视觉获取点或其它方法来更新轨迹。

        大家在看的时候可以大致翻译一下那些变量名，大概也能知道它的作用。

        我们先来看“eight_goal_cb(const geometry_msgs::PoseStamped target_info)”这个函数，通过它的形参以及话题的定义我们可以了解到它是通过rviz的3DNavGoal来触发执行的，即rviz中3DNavGoal拉一下，后续可以根据我们的实际需求对它的触发机制进行改变。这个函数的内容为把我们设定的预先点拉取到“goal_list”中，然后把这些点位的信息通过pub发送给可视化部分去进行一个可视化展示（在这里请大家思考一下，怎么知道它是发送给谁的！），接着通过调用solve_min_snap()去解算轨迹。

        再来看看getCircleCenter（），这个函数是被放在了一个定时器里的，它在隔一段时间里就会被调用一次（请大家结合重规划的概念思考一下为什么要放在定时器里）。重规划这个函数和生成全局轨迹函数的不同点在于它的第一个点位是当前的自身点位，接着去计算出当前位置与最近一个圆环的距离distance，这个距离用于判断是否已经穿过圆环，从而决定把什么点位放入新的航路点数组中去解算新的轨迹。

        min_snap_closeform.cpp这个文件中包含的是关于MinimumSnap轨迹解算的数学操作，这里面的数学操作大多在min_snap_generator.cpp中被调用，所以这一部分不是大家需要去看的重点。

        （2）my_visualization

        这一部分实现的内容主要是把生成的轨迹以及与其相关的实时数据定义成rviz能够展示的可视化形状然后把可视化信息发送给rviz展示出来。这些东西都是事先写好一劳永逸的，大家只需大概看一下里面的内容是什么，无需过度关注。

        （3）traj_server

        由于我们想要让无人机根据我们生成的轨迹去飞行，所以我们需要计算出来无人机在每一个时刻应该在什么位置、速度、加速度以及yaw角的状态是怎么样的，并且发布出去让控制功能包接收。这个部分学习了解的过程中需要大家去查看和对比“quadrotor_msgs::PositionCommand”这个消息类型与模拟器中“角速度推力控制”话题中的消息类型的内容之间的差异，相信大家就能理解我之前为什么说需要一个上层控制算法去进行一个操作信息的转换。

        大致了解了三个功能包的内容后，应该对这部分代码有所了解了。这时候你打开这份代码看看自己能不能懂其中每一个变量的含义，至少你需要知道输入和输出的信息是什么，关键的函数做了什么样的操作，所以这里建议大家先完成思维导图后再去部署，我相信是没有什么难度的。

二、功能包启动方法

catkin_make    //工作空间下打开终端编译整个工作空间
source devel/setup.bash    //刷新环境变量
roslaunch min_snap real.launch    //启动代码并打开rviz

在此处顺带提一下real.launch里面的内容

在real.launch的第一行是“<include file="$(find min_snap)/launch/rviz.launch" />”

假如这一行被注释了将无法引入这个功能包编写好的运行rviz的launch文件，无法打开rviz

在这个launch文件中还包含了启动别的功能包、引入参数的作用

其中“mean_vel”用于调整无人机的平均飞行速度

<arg name="odom_topic" value="/vins_fusion/imu_propagate" />用于引入里程计数据（即定位信息）

<node pkg="min_snap" name="min_snap_generator" type="min_snap_generator" output="screen">启动min_snap

<node pkg="my_visualization" name="min_snap_visual" type="min_snap_visual" output="screen">启动轨迹视觉化部分

<node pkg="traj_server" name="my_traj_server" type="my_traj_server" output="screen">启动运动轨迹控制信息部分