本文整理汇总了Python中nav_msgs.msg.GridCells.append方法的典型用法代码示例。如果您正苦于以下问题：Python GridCells.append方法的具体用法？Python GridCells.append怎么用？Python GridCells.append使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类nav_msgs.msg.GridCells的用法示例。
在下文中一共展示了GridCells.append方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: PathFinder
# 需要导入模块: from nav_msgs.msg import GridCells [as 别名]
# 或者: from nav_msgs.msg.GridCells import append [as 别名]
class PathFinder(object):
'''
PathFinder will search the map for a valid path to the goal.
Args:
Map = ROS map file to be traversed
start = Initial location
goal = Desired location
Returns:
Path = An array of coordinates representing a path as a series of waypoints.
Resolution is, as of yet, unspecified.
'''
map = None
closed = None
open = None
start = None
goal = None
def __init__(start, goal, resolution, map):
self.map = map
self.start = start
self.goal = goal
self.closed = GridCells()
self.closed.cell_height = resolution
self.closed.cell_width = resolution
self.closed.header.frame_id = "map"
self.open = GridCells()
self.open.cell_height = resolution
self.open.cell_width = resolution
self.open.header.frame_id = "map"
def astar(self):
closedset = [] #nodes already evaluated
openset = [self.start] #nodes to be evaluated - contains start
came_from = self.map #navigated nodes
start.gscore = 0
start.heuristic = calcHeur(self.start, self.goal) + start.gscore
lastNode = start
while len(self.openset) > 0:
distance = 100000
for node in self.open:
if node.fscore < distance:
current = node
distance = node.fscore
if node.isSame(self.goal):
distance = 0
current = node
if current.isSame(self.goal):
return make_path(current)
self.open.remove(current)
self.closed.append(current)
currentDirection = direction(current, last)
upPoint = Point(current.x, current.y+resolution, 0)
leftPoint = Point(current.x - resolution, current.y,0)
rightPoint = Point(current.x+resolution, current.y, 0)
downPoint = Point(current.x, current.y-resolution, 0)
upNode = Node(upPoint, current.score + resolution, current)
leftNode = Node(leftPoint, current.score + resolution, current)
rightNode = Node(rightPoint, current.score + resolution, current)
downNode = Node(downPoint, current.score + resolution, current)
if currentDirection is Direction.up:
upNode.heuristic -= 2*resolution
elif currentDirection is Direction.right:
rightNode.heuristic -= 2*resolution
elif currentDirection is Direction.left:
leftNode.heuristic -= 2*resolution
elif currentDirection is Direction.down:
downNode.heuristic -= 2*resolution
possibleneighbors = [upNode, rightNode, leftNode, downNode]
validneighbors = []
#remove neighbors that aren't in c-space
for neighbor in possibleneighbors:
for tiles in botWorld:
if neighbor.isSame(item):
validneighbors.append(neighbor)
#remove neighbors that are already in the closedset
for neighbor in validneighbors:
for closedTiles in closedset:
if neighbor.isSame(item):
validneighbors.remove(neighbor)
for neighbor in validneighbors:
gscore = current.score + resolution
#.........这里部分代码省略.........