【Roban教程】行走实现

承接上篇的路径规划，我们将获得的路径传入path_controler包来实现机器人的行走。此包的主要功能是进一步分析接收到的路径信息以及向机器人发布行走的信息。

一、路径分析

首先，在接收到nav_msgs:: Path类型的路径信息之后，我们提取出来其中的数据信息。我们可以获得一串含有多个点的位置信息。其中每个点包含三个位置信息x, y, z。其中我们只取x, y分量作为此路径点在地图上的位置。

同时，我们通过startCallback回调函数来接收来自/initialpose话题的当前位置信息：

geometry _msgs:: Pose 类型的全局变量 start_pose＿代表实时的位置信息。除了可以直接接收当前的位置信息以外，变 start_pose＿还包含着机器人的以四元数方法表示的形态信息： X, y, Z, 。根据四元数与欧拉角的转换矩阵可得，偏航角 Yaw 与四元数的关系式为：

因此，在 pathCallback 回调函数中，我们使用如下代码进行当前方向信息的提取：

其中变量 walkFinish 表示当前行走是否结束，变量 reachGoal 则表示是否到达路径的 终点。在有了当前的位置坐标和朝向，再加上规划好的路径信息，我们便可以分析路径，获得真正符合机器人行走的线路。代码如下所示：

我们希望将地图上的路径信息转换成可以直接作用于机器人的例如右转 40r，直行一步的信息，首先我们通过初始机器人方向以及当前为止指向第个目标点的方向可以计算出在原地的转角，这便得到了第一个转动的角度。因为我们认为在后面的行走中，每走完一步以后的朝向是沿着上一个在路径信息的分析过程中，我们希望地图上的路径信息可以转换成简单明了的能够使机器人前进或者转弯的信息。基于这个目的，我们的路径分析过程旨在输出两个数组，一个数组是每一步所走的长度，另一个数组是每一步所旋 转的角度。其中，在求初始转动的角度与其他的转角不同，需要通过当前的朝向来求。我们计算当前位置的方向与当前位置指向下个目标点的差，作为第一次旋转的角度、另外，我们添加了每步的步长不应该小千于0.11 的限定。首先是因为 path 中的目标点过千密集，不适合将目标点直接作为行走的点。其次因为如果步长太小，将不能忽略路径中存在的锯齿形误差，从而影响机器人正常的前进。所以我们将满五次且每次步长大于 0.11 的行走的点发送出去，作为次行走，同时将 walkFinish 参数设0以代表开始这一次行走。

二、行走控制

行走控制直接和 Roban 机器人的步态控制挂钩，它接收来自 BodyHubeNode 节点的请求信息后，发送相应的行走信息以实现机器人行走。

到此为止，我们可以通过 img_package 包来发布 RGB 图、深度图以及点云图的消息 了。此时的 rqt_graph 界面如图2所示。

图2rqt_graph 界面