目前打算使用python写出一个Astar的全局路径算法,总体分为三个部分:接收地图数据,设计路径(当然是顺过来的),发布路径。
文章目录
- 一、建立功能包
- 二、接受地图数据(处理上游)
- 查看地图发布的话题和发布者
- 在同一个代码中接收和发布消息
- python+ros在类中建立节点
- 理解地图数据
- 地图膨胀
- 使用numba加速
- PS: 关于ros没有numba包的问题
一、建立功能包
新建功能包,并导入依赖: gmapping map_server amcl move_base roscpp rospy std_msgs
在功能包中新建一个python文件,运行这个文件就可以生成相应的路径。(相关配置略去)
二、接受地图数据(处理上游)
查看地图发布的话题和发布者
首先我们要查一下,地图数据是从哪发布的,使用什么类型发布的:
在同一个代码中接收和发布消息
下面我们先接受数据:
正常来说代码如下:
这么写虽然能成功获取消息,但是有一个问题,就是在rospy.spin()之后的代码不会进行了,说白了在ros中没有C++中的spinOnce。如果想要结束spin,就要关闭节点,这样我就没有办法在一个代码中又接受消息,又发布消息。
解决办法:
使用rospy.wait_for_message
这个函数,功能类似于spinOnce()
这样就能成功接受消息并且对数据进行处理发布。
python+ros在类中建立节点
秉着面向成双成对编程的思想,当然想把这个功能编成一个类来写,而且写成一个类更为方便,所以尝试将其放入类中。
在if __name__ == "__main__":
里面使用类,将执行类的__init__(self)
函数
比如:
会输出 ‘在__init__里面’ 如果想运行其他函数,只需要将其写进__init__(self)
函数里面
PS:
说一下if __name__ == "__main__":
的作用:
我们写的.py文件有的时候是直接运行,有的时候是当做一个功能,导入到其他的.py文件中。在if __name__ == "__main__":
里面的代码只有文件是直接运行的时候才会运行,如果是当做功能包引入到其他文件是不会运行的。
我们将上一节代码修改如下:
理解地图数据
先了解一下OccupancyGrid这个数据类型
里面最重要的是int8[] data这个数据,放置了地图信息,可走区域的数值为0,障碍物数值为100,未知领域数值为-1,但是坑爹的是他是一个列表,不是矩阵,所以我们需要把他变成矩阵再处理数据。
OccupancyGrid将矩阵的宽高放在了上面,我们使用numpy进行变换:
一般的算法是将0表示为可行区域,1表示为不可通行,所以末尾需要加一行变换,将矩阵内所有100变成1
然后我们看一下地图是什么样子的:
结果如下:
但是我建立的地图在rviz中是这样的:
也就是正好倒过来了,所以将矩阵也翻一下才能变成我们想要的结果,优化doMap函数:
这样就变成我们想要的数据格式了。
地图膨胀
因为路径规划的时候我们都是把机器人当成一个质点,所以有必要进行地图的膨胀,防止其撞到周边。
制地图膨胀的思路如下:
- 首先找到矩阵中所有1的位置
- 遍历矩阵,如果当前位置和1的位置距离在设置的膨胀距离以内,那么就将其设置为1
在类中编写一个函数:
使用numba加速
但是当我运行这段代码的时候发现,他竟然运行了快3分钟,多运行几次,我的暗影精灵就开始咆哮了,之前知道python比较慢,但是这也太慢了,在网上查资料,发现有个不错的加速循环的包叫numba
使用很简单:
这样就能对其加速,但是还有一个问题,我发现他不能在类中使用,只能单独对函数使用,所以无奈要把这个函数单独拿出来对其加速
从三分钟提速到了1.045s!!!!人间奇迹
PS: 关于ros没有numba包的问题
在一开始我也有这个问题,因为我不知道numba应该下载到哪里,直接使用
一个是网络问题,其次换了源导入的时候也是识别不到,所以应该查询ros使用的python包都在什么位置:
在vscode中有一个setting.json文件,那里给出了python包的地址:
或者在终端中查看一下:
也能查询到该地址
然后就可以从国内源下载numba到对应地址,在ros中使用numba了