本文整理汇总了Python中nav_msgs.msg.OccupancyGrid.transform方法的典型用法代码示例。如果您正苦于以下问题：Python OccupancyGrid.transform方法的具体用法？Python OccupancyGrid.transform怎么用？Python OccupancyGrid.transform使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类nav_msgs.msg.OccupancyGrid的用法示例。
在下文中一共展示了OccupancyGrid.transform方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: BumpGuesser
# 需要导入模块: from nav_msgs.msg import OccupancyGrid [as 别名]
# 或者: from nav_msgs.msg.OccupancyGrid import transform [as 别名]
#.........这里部分代码省略.........
occupied. You shouldn't expect good results if you call this
on an occupied cell.
"""
xmax = self.mapData.og.info.width
ymax = self.mapData.og.info.height
if self.mapData.sampled:
# Fails on an occupied cell
assert self.mapData.mapArrayS[xi, yi] < 50
for x in range(max(xi - 1, 0), min(xi + 1, xmax)):
for y in range(max(yi - 1, 0), min(yi + 1, ymax)):
if self.mapData.mapArrayS[x, y] > 50:
return True
return False
else:
# Fails on an occupied cell
assert self.mapData.mapArray[xi, yi] < 50
for x in range(max(xi - 1, 0), min(xi + 1, xmax)):
for y in range(max(yi - 1, 0), min(yi + 1, ymax)):
if self.mapData.mapArray[x, y] > 50:
return True
return False
def addMap(self, newMapData):
""" Receives a new map. """
assert isinstance(newMapData, MapData)
self.mapData = newMapData
def handle_guess(self, newPointList):
"""
Takes an input guess (a list of Point objects) and filters it
against a map of points that seem probable because of a recent
bump. Then resamples per usual to return a list of equally
probable points.
"""
if not self.ready_to_publish:
return False
assert isinstance(newPointList, list)
assert isinstance(newPointList[0], Point)
# Find the limits of the input data.
xmax = -1.0e6
ymax = -1.0e6
xmin = 1.0e6
ymin = 1.0e6
for pt in newPointList:
xmax = max(xmax, pt.point[0])
ymax = max(ymax, pt.point[1])
xmin = min(xmin, pt.point[0])
ymin = min(ymin, pt.point[1])
# Shrink the map to accommodate the relevant area
self.mapData.sample((xmin, ymin), (xmax, ymax))
# Cruise through the map looking for empty cells next to occupied
# ones. These will be the likely cells when a bump is encountered.
#
# Because of the possibility of bumping an object that isn't on the
# map, any empty map cell is possible. Therefore, we run through
# the map, packing the map data into a list of Point objects, since
# that's what the perfesser wants for input. While we're running
# through, we keep a separate list of empty cells next to full ones.
wallPointList = []
emptyPointList = []
for xi in range(self.mapData.ogS.info.width):
for yi in range(self.mapData.ogS.info.height):
if self.mapData.mapArrayS[xi, yi] < 50:
p = Point()
p.point = self.mapData.transform((xi, yi))
emptyPointList.append(p)
if self.occupiedNeighbor(xi, yi):
newP = Point()
newP.point = (
p.point[0] + np.random.normal(0.0, self.mapData.ogS.info.resolution / 3.0),
p.point[1] + np.random.normal(0.0, self.mapData.ogS.info.resolution / 3.0),
p.point[2],
)
wallPointList.append(newP)
# Using the wallError, sample the two lists together to get a roughly
# correct distribution of points to feed to the perfesser.
self.mapPointList = []
for i in range(self.nPoints):
if i < self.wallError * self.nPoints:
self.mapPointList.append(random.choice(wallPointList))
else:
self.mapPointList.append(random.choice(emptyPointList))
self.guesser.newPoints(self.mapPointList)
pts = Belief()
pts.points = newPointList
self.guesser.update(pts)
self.pointList = self.guesser.outPoints()
self.ready_to_publish = False
return True