|
当上游和算法接上去之后，下面考虑如何将算法发布出去，并且现实在rviz中
发布路径
算法给出的格式
整理一下，举个例子
地图格式为：
[[-1,-1,-1,-1,-1,-1,-1]
[-1, 0, 0, 0, 0, 0,-1]
[-1, 0, 0, 1, 0, 0,-1]
[-1, 0, 0, 1, 0, 0,-1]
[-1, 0, 0, 1, 0, 0,-1]
[-1,-1,-1,-1,-1,-1,-1]]
起始点和目标点为：
[1,2],[4,4]
Astar算法参考：link
输出是一系列的点：
[[1,2],[2,2],[3,2],[4,2],[4,3],[4,4]]
查看rviz接收路径的数据类型
rviz接受数据类型是Path
> rosmsg info Path
[nav_msgs/Path]:
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/PoseStamped[] poses
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Pose pose
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
需要输入时间戳，frame_id，和pose消息。
如何发布
我一直以为路径的这些点会像发布地图一样，将所有信息储存在一个列表中，一起发送，但是现在看来，他是一个点一个点的发，所以我们要设置发送频率，让他在1秒钟多发送一些，甚至都发送完。
def sendAstarPath(self):
AstarPath = rospy.Publisher("AstarPath",Path,queue_size=15)
init_path = Path()
rate = rospy.Rate(200)
for i in range(len(self.pathList)):
init_path.header.stamp = rospy.Time.now()
init_path.header.frame_id = "map"
current_point = PoseStamped()
current_point.header.frame_id = "map"
current_point.pose.position.x = pixwidth - self.pathList[i][0]*self.resolution
current_point.pose.position.y = self.pathList[i][1]*self.resolution - pixheight
current_point.pose.position.z = 0
current_point.pose.orientation.x = 0
current_point.pose.orientation.y = 0
current_point.pose.orientation.z = 0
current_point.pose.orientation.w = 1
init_path.poses.append(current_point)
AstarPath.publish(init_path)
rate.sleep()
i += 1
time.sleep(0.5)
因为这里的x和y坐标是地图坐标，而Astar中的是栅格坐标，所以我们要给他转回去，这个就是前一节公式的逆。
在rviz中显示
消息发布后，在rviz中接收发布的AstarPath话题，就可以显示对应的话题了。
以下是所有代码：
import time
from numba import jit
import math
import rospy
import numpy as np
import matplotlib.pyplot as plt
from nav_msgs.msg import OccupancyGrid
from geometry_msgs.msg import PoseWithCovarianceStamped
from geometry_msgs.msg import PoseStamped
from nav_msgs.msg import Path
class MapMatrix:
"""
说明:
1.构造方法需要两个参数，即二维数组的宽和高
2.成员变量w和h是二维数组的宽和高
3.使用:对象[x][y]可以直接取到相应的值
4.数组的默认值都是0
"""
def __init__(self,map):
self.w=map.shape[0]
self.h=map.shape[1]
self.data=map
def showArrayD(self):
for y in range(self.h):
for x in range(self.w):
print(self.data[x][y],end=' ')
print("")
def __getitem__(self, item):
return self.data[item]
class Point:
"""
表示一个点
"""
def __init__(self,x,y):
self.x=x;self.y=y
def __eq__(self, other):
if self.x==other.x and self.y==other.y:
return True
return False
class AStar:
"""
AStar算法的Python3.x实现
"""
class Node:
def __init__(self, point, endPoint, g=0):
self.point = point
self.father = None
self.g = g
self.h = (abs(endPoint.x - point.x) + abs(endPoint.y - point.y)) * 10
def __init__(self, map2d, startPoint, endPoint, passTag=0):
"""
构造AStar算法的启动条件
:param map2d: ArrayD类型的寻路数组
:param startPoint: Point或二元组类型的寻路起点
:param endPoint: Point或二元组类型的寻路终点
:param passTag: int类型的可行走标记（若地图数据!=passTag即为障碍）
"""
self.openList = []
self.closeList = []
self.map2d = map2d
if isinstance(startPoint, Point) and isinstance(endPoint, Point):
self.startPoint = startPoint
self.endPoint = endPoint
else:
self.startPoint = Point(*startPoint)
self.endPoint = Point(*endPoint)
self.passTag = passTag
def getMinNode(self):
"""
获得openlist中F值最小的节点
:return: Node
"""
currentNode = self.openList[0]
for node in self.openList:
if node.g + node.h < currentNode.g + currentNode.h:
currentNode = node
return currentNode
def pointInCloseList(self, point):
for node in self.closeList:
if node.point == point:
return True
return False
def pointInOpenList(self, point):
for node in self.openList:
if node.point == point:
return node
return None
def endPointInCloseList(self):
for node in self.openList:
if node.point == self.endPoint:
return node
return None
def searchNear(self, minF, offsetX, offsetY):
"""
搜索节点周围的点
:param minF:F值最小的节点
:param offsetX:坐标偏移量
:param offsetY:
:return:
"""
if minF.point.x + offsetX < 0 or minF.point.x + offsetX > self.map2d.w - 1 or minF.point.y + offsetY < 0 or minF.point.y + offsetY > self.map2d.h - 1:
return
if self.map2d[minF.point.x + offsetX][minF.point.y + offsetY] != self.passTag:
return
currentPoint = Point(minF.point.x + offsetX, minF.point.y + offsetY)
if self.pointInCloseList(currentPoint):
return
if offsetX == 0 or offsetY == 0:
step = 10
else:
step = 14
currentNode = self.pointInOpenList(currentPoint)
if not currentNode:
currentNode = AStar.Node(currentPoint, self.endPoint, g=minF.g + step)
currentNode.father = minF
self.openList.append(currentNode)
return
if minF.g + step < currentNode.g:
currentNode.g = minF.g + step
currentNode.father = minF
def start(self):
"""
开始寻路
:return: None或Point列表（路径）
"""
if self.map2d[self.endPoint.x][self.endPoint.y] != self.passTag:
return None
startNode = AStar.Node(self.startPoint, self.endPoint)
self.openList.append(startNode)
while True:
minF = self.getMinNode()
self.closeList.append(minF)
self.openList.remove(minF)
self.searchNear(minF, 0, -1)
self.searchNear(minF, 0, 1)
self.searchNear(minF, -1, 0)
self.searchNear(minF, 1, 0)
point = self.endPointInCloseList()
if point:
cPoint = point
pathList = []
while True:
if cPoint.father:
pathList.append([cPoint.point.y,cPoint.point.x])
cPoint = cPoint.father
else:
return list(reversed(pathList))
if len(self.openList) == 0:
return None
pixwidth = 10.197194
pixheight = 4.625010
@jit(nopython=True)
def _obstacleMap(map,obsize):
'''
给地图一个膨胀参数
'''
indexList = np.where(map == 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]):
distance = math.sqrt((x-ox)**2+(y-oy)**2)
if distance <= obsize:
map[x][y] = 1
class pathPlanning():
def __init__(self):
'''
起点:[2,2]
终点:[2,4]
地图:（未知:-1，可通行:0，不可通行:1）
返回的内容:[(2,4),(1,4),(0,3),(1,2),(2,2)]
'''
rospy.init_node("Astar_globel_path_planning",anonymous=True)
self.doMap()
self.obsize=7
print("现在进行地图膨胀")
ob_time = time.time()
_obstacleMap(self.map,self.obsize)
print("膨胀地图所用时间是:{:.3f}".format(time.time()-ob_time))
self.getIniPose()
self.getTarPose()
print("已接收")
s_time = time.time()
self.map2d=MapMatrix(self.map)
aStar=AStar(self.map2d,Point(self.start_point[1],self.start_point[0]),Point(self.final_point[1],self.final_point[0]))
self.pathList=aStar.start()
print("Astar算法所用时间是:{:.3f}".format(time.time()-s_time))
self.sendAstarPath()
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
mapdata = np.array(self.OGmap.data,dtype=np.int8)
self.map = mapdata.reshape((self.height,self.width))
self.map[self.map == 100] = 1
self.map = self.map[:,::-1]
def getIniPose(self):
'''
获取初始坐标点
'''
self.IniPose = rospy.wait_for_message("/amcl_pose", PoseWithCovarianceStamped,timeout=None)
self.init_x = self.IniPose.pose.pose.position.x
self.init_y = self.IniPose.pose.pose.position.y
self.start_point = self.worldToMap(self.init_x,self.init_y)
self.init_quaternions_z = self.IniPose.pose.pose.orientation.z
self.init_quaternions_w = self.IniPose.pose.pose.orientation.w
def getTarPose(self):
'''
获取目标坐标点
'''
self.TarPose = rospy.wait_for_message("/move_base_simple/goal", PoseStamped,timeout=None)
self.tar_x = self.TarPose.pose.position.x
self.tar_y = self.TarPose.pose.position.y
self.final_point = self.worldToMap(self.tar_x,self.tar_y)
self.tar_quaternions_x = self.TarPose.pose.orientation.x
self.tar_quaternions_y = self.TarPose.pose.orientation.y
self.tar_quaternions_z = self.TarPose.pose.orientation.z
self.tar_quaternions_w = self.TarPose.pose.orientation.w
def sendAstarPath(self):
AstarPath = rospy.Publisher("AstarPath",Path,queue_size=15)
init_path = Path()
rate = rospy.Rate(200)
for i in range(len(self.pathList)):
init_path.header.stamp = rospy.Time.now()
init_path.header.frame_id = "map"
current_point = PoseStamped()
current_point.header.frame_id = "map"
current_point.pose.position.x = pixwidth - self.pathList[i][0]*self.resolution
current_point.pose.position.y = self.pathList[i][1]*self.resolution - pixheight
current_point.pose.position.z = 0
current_point.pose.orientation.x = 0
current_point.pose.orientation.y = 0
current_point.pose.orientation.z = 0
current_point.pose.orientation.w = 1
init_path.poses.append(current_point)
AstarPath.publish(init_path)
rate.sleep()
i += 1
time.sleep(0.5)
def worldToMap(self,x,y):
mx = (int)((pixwidth-x) /self.resolution)
my = (int)(-(-pixheight-y) /self.resolution)
return [mx,my]
if __name__ == "__main__":
getmap = pathPlanning()
|