目前打算使用python写出一个Astar的全局路径算法，总体分为三个部分：接收地图数据，设计路径（当然是顺过来的），发布路径。
文章目录
一、建立功能包
新建功能包，并导入依赖: gmapping map_server amcl move_base roscpp rospy std_msgs
在功能包中新建一个python文件，运行这个文件就可以生成相应的路径。（相关配置略去）
二、接受地图数据（处理上游）
查看地图发布的话题和发布者
首先我们要查一下，地图数据是从哪发布的,使用什么类型发布的：
rostopic list #里面应该有一个 /map
rostopic info /map #查看发布的类型，和发布者
rosmsg info nav_msgs/OccupancyGrid
在同一个代码中接收和发布消息
下面我们先接受数据：
正常来说代码如下：
#! /usr/bin/env python
import rospy
from nav_msgs.msg import OccupancyGrid
#回调函数
def doMsg(msg):
rospy.loginfo(msg.data)
if __name__ == "__main__":
#初始化ROS节点
rospy.init_node("Astar_globel_path_planning",anonymous=True)
#接受数据
sub = rospy.Subscriber("/map",OccupancyGrid,doMsg,queue_size=10)
rospy.spin()
#spin函数一直循环，就不会往下进行
#print(1)
这么写虽然能成功获取消息，但是有一个问题，就是在rospy.spin()之后的代码不会进行了，说白了在ros中没有C++中的spinOnce。如果想要结束spin，就要关闭节点，这样我就没有办法在一个代码中又接受消息，又发布消息。
解决办法：
使用rospy.wait_for_message这个函数，功能类似于spinOnce()
#! /usr/bin/env python
import rospy
from nav_msgs.msg import OccupancyGrid
if __name__ == "__main__":
#初始化ROS节点
rospy.init_node("Astar_globel_path_planning",anonymous=True)
#接受数据
OGmap = rospy.wait_for_message("/map",OccupancyGrid,timeout=None)
#能否继续下面的代码
print(OGmap)
print("成功")
这样就能成功接受消息并且对数据进行处理发布。
python+ros在类中建立节点
秉着面向成双成对编程的思想，当然想把这个功能编成一个类来写，而且写成一个类更为方便，所以尝试将其放入类中。
在if __name__ == "__main__":里面使用类，将执行类的__init__(self)函数
比如：
class Test():
def __init__(self):
print("在__init__里面")
#self._print()
def _print(self):
print("使用_print")
if __name__ == '__main__':
a = Test()
会输出 ‘在__init__里面’ 如果想运行其他函数，只需要将其写进__init__(self)函数里面
PS：
说一下if __name__ == "__main__":的作用：
我们写的.py文件有的时候是直接运行，有的时候是当做一个功能，导入到其他的.py文件中。在if __name__ == "__main__":里面的代码只有文件是直接运行的时候才会运行，如果是当做功能包引入到其他文件是不会运行的。
我们将上一节代码修改如下:
#! /usr/bin/env python
import rospy
from nav_msgs.msg import OccupancyGrid
class pathPlanning():
def __init__(self):
#初始化ROS节点
rospy.init_node("Astar_globel_path_planning",anonymous=True)
self.doMap()
def doMap(self):
'''
获取数据
将数据处理成一个矩阵（未知:-1，可通行:0，不可通行:1）
'''
#接受数据
self.OGmap = rospy.wait_for_message("/map",OccupancyGrid,timeout=None)
if __name__ == "__main__":
getmap = pathPlanning()
理解地图数据
先了解一下OccupancyGrid这个数据类型
std_msgs/Header header
uint32 seq
time stamp
string frame_id
nav_msgs/MapMetaData info
time map_load_time
float32 resolution #这个是分辨率
uint32 width #矩阵的width
uint32 height #矩阵的height
geometry_msgs/Pose origin
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
int8[] data
里面最重要的是int8[] data这个数据，放置了地图信息，可走区域的数值为0，障碍物数值为100，未知领域数值为-1，但是坑爹的是他是一个列表，不是矩阵，所以我们需要把他变成矩阵再处理数据。
OccupancyGrid将矩阵的宽高放在了上面，我们使用numpy进行变换：
def doMap(self):
'''
获取数据
将数据处理成一个矩阵（未知:-1，可通行:0，不可通行:1）
'''
#获取地图数据
self.OGmap = rospy.wait_for_message("/map",OccupancyGrid,timeout=None)
#地图的宽度
self.width = self.OGmap.info.width
#地图的高度
self.height = self.OGmap.info.height
#地图的分辨率
self.resolution = self.OGmap.info.resolution
#获取地图的数据 可走区域的数值为0，障碍物数值为100，未知领域数值为-1
mapdata = np.array(self.OGmap.data,dtype=np.int8)
#将地图数据变成矩阵
self.map = mapdata.reshape((self.height,self.width))
#将地图中的障碍变成从100变成1
self.map[self.map == 100] = 1
一般的算法是将0表示为可行区域，1表示为不可通行，所以末尾需要加一行变换，将矩阵内所有100变成1
然后我们看一下地图是什么样子的：
#查看地图数据存储格式
plt.matshow(self.map, cmap=plt.cm.gray)
plt.show()
结果如下：
但是我建立的地图在rviz中是这样的：
也就是正好倒过来了，所以将矩阵也翻一下才能变成我们想要的结果，优化doMap函数：
def doMap(self):
'''
获取数据
将数据处理成一个矩阵（未知:-1，可通行:0，不可通行:1）
'''
#接受数据
self.OGmap = rospy.wait_for_message("/map",OccupancyGrid,timeout=None)
#地图的宽度
self.width = self.OGmap.info.width
#地图的高度
self.height = self.OGmap.info.height
#地图的分辨率
self.resolution = self.OGmap.info.resolution
#获取地图的数据 可走区域的数值为0，障碍物数值为100，未知领域数值为-1
mapdata = np.array(self.OGmap.data,dtype=np.int8)
#将地图数据变成矩阵
self.map = mapdata.reshape((self.height,self.width))
#将地图中的障碍变成从100变成1
self.map[self.map == 100] = 1
#列是逆序的，所以要将列顺序
self.map = self.map[:,::-1]
# #查看地图数据存储格式
# plt.matshow(self.map, cmap=plt.cm.gray)
# plt.show()
这样就变成我们想要的数据格式了。
地图膨胀
因为路径规划的时候我们都是把机器人当成一个质点，所以有必要进行地图的膨胀，防止其撞到周边。
制地图膨胀的思路如下：
- 首先找到矩阵中所有1的位置
- 遍历矩阵，如果当前位置和1的位置距离在设置的膨胀距离以内，那么就将其设置为1
在类中编写一个函数：
def obstacleMap(self,obsize):
indexList = np.where(self.map == 1)#将地图矩阵中1的位置找到
#遍历地图矩阵
for x in range(self.map.shape[0]):
for y in range(self.map.shape[1]):
if self.map[x][y] == 0:
for ox,oy in zip(indexList[0],indexList[1]):
#如果和有1的位置的距离小于等于膨胀系数，那就设为1
distance = math.sqrt((x-ox)**2+(y-oy)**2)
if distance <= obsize:
self.map[x][y] = 1
使用numba加速
但是当我运行这段代码的时候发现，他竟然运行了快3分钟，多运行几次，我的暗影精灵就开始咆哮了，之前知道python比较慢，但是这也太慢了，在网上查资料，发现有个不错的加速循环的包叫numba
使用很简单：
from numba import jit
@jit(nopython=True)
def fun():
pass
这样就能对其加速，但是还有一个问题，我发现他不能在类中使用，只能单独对函数使用，所以无奈要把这个函数单独拿出来对其加速
#将最慢算法的加速一下
@jit(nopython=True)
def _obstacleMap(map,obsize):
'''
给地图一个膨胀参数
'''
indexList = np.where(map == 1)#将地图矩阵中1的位置找到
#遍历地图矩阵
for x in range(map.shape[0]):
for y in range(map.shape[1]):
if map[x][y] == 0:
for ox,oy in zip(indexList[0],indexList[1]):
#如果和有1的位置的距离小于等于膨胀系数，那就设为1
distance = math.sqrt((x-ox)**2+(y-oy)**2)
if distance <= obsize:
map[x][y] = 1
class pathPlanning():
def __init__(self):
#初始化ROS节点
rospy.init_node("Astar_globel_path_planning",anonymous=True)
self.doMap()
#obsize是膨胀系数，是按照矩阵的距离，而不是真实距离，所以要进行一个换算
self.obsize=7 #15太大了
print("现在进行地图膨胀")
ob_time = time.time()
_obstacleMap(self.map,self.obsize)
print("膨胀地图所用时间是:{:.3f}".format(time.time()-ob_time))
从三分钟提速到了1.045s！！！！人间奇迹
PS： 关于ros没有numba包的问题
在一开始我也有这个问题，因为我不知道numba应该下载到哪里，直接使用
pip install numba
一个是网络问题，其次换了源导入的时候也是识别不到，所以应该查询ros使用的python包都在什么位置：
在vscode中有一个setting.json文件，那里给出了python包的地址：
/opt/ros/noetic/lib/python3/dist-packages
或者在终端中查看一下：
pip show rospy
也能查询到该地址
然后就可以从国内源下载numba到对应地址，在ros中使用numba了
sudo pip install --target=/opt/ros/noetic/lib/python3/dist-packages numba -i https://pypi.tuna.tsinghua.edu.cn/simple