我试图从头开始编写一些python代码。
此代码需要使机器人(表示为节点)覆盖所有工作空间并避开障碍物(对障碍物的位置有先验知识)。
我发现工作空间可以表示为矩阵系统。
使用波前距离变换(如图所示:http://www.emeraldinsight.com/content_images/fig/0490390507009.png)
如何编写代码,使机器人从一个节点移动到下一个最高值的节点,同时跟踪被访问的节点?
由于
编辑:这是我上网的python代码(我有中级编程技巧),我试图通过波前距离变换来适应boustrophedon运动(来回)。代码当前卡在第一行(x = 0和y = 0),试图移动到下一行并运行最高节点值。
尝试: 导入numpy 除了: print“未安装numpy数学库。” 进口时间
class waveFrontPlanner:
def __init__(self, map, slow=False):
self.__slow = slow
self.__map = map
if str(type(map)).find("numpy") != -1:
#If its a numpy array
self.__height, self.__width = self.__map.shape
else:
self.__height, self.__width = len(self.__map), len(self.__map[0])
self.__nothing = 0
self.__wall = 999
self.__goal = 1
self.__path = "PATH"
#Robot value
self.__robot = 254
#Robot default Location
self.__robot_x = 0
self.__robot_y = 11
#default goal location
self.__goal_x = 18
self.__goal_y = 0
#temp variables
self.__counter = 0
self.__steps = 0 #determine how processor intensive the algorithm was
#when searching for a node with a lower value
self.__maximum_node = 250
self.__max_node_location = 250
self.__reset_max = 250 #above this number is a special (wall or robot)
###########################################################################
def run(self, prnt=False):
"""
The entry point for the robot algorithm to use wavefront propagation.
"""
path = []
while self.__map[self.__robot_x][self.__robot_y] != self.__goal:
if self.__steps > 20000:
print "Cannot find a path."
return
#find new location to go to
self.__new_state = self.propagateWavefront()
#update location of robot
if self.__new_state == 1:
self.__robot_x -= 1
if prnt:
print "Move to x=%d y=%d\n\n" % \
(self.__robot_x, self.__robot_y)
path.append((self.__robot_x, self.__robot_y))
if self.__new_state == 2:
self.__robot_y += 1
if prnt:
print "Move to x=%d y=%d\n\n" % \
(self.__robot_x, self.__robot_y)
path.append((self.__robot_x, self.__robot_y))
if self.__new_state == 3:
self.__robot_x += 1
if prnt:
print "Move to x=%d y=%d\n\n" % \
(self.__robot_x, self.__robot_y)
path.append((self.__robot_x, self.__robot_y))
if self.__new_state == 4:
self.__robot_y -= 1
if prnt:
print "Move to x=%d y=%d\n\n" % \
(self.__robot_x, self.__robot_y)
path.append((self.__robot_x, self.__robot_y))
self.__old_state = self.__new_state
msg = "Found the goal in %i steps:\n" % self.__steps
msg += "Map size= %i %i\n\n" % (self.__height, self.__width)
if prnt:
print msg
self.printMap()
return path
###########################################################################
def propagateWavefront(self, prnt=False):
"""
"""
#self.unpropagate()
#Old robot location was deleted, store new robot location in map
self.__map[self.__robot_x][self.__robot_y] = self.__robot
self.__path = self.__robot
#Start location to begin scan at goal location
self.__map[self.__goal_x][self.__goal_y] = self.__goal
counter = 0
while counter < 200: #allows for recycling until robot is found
x = 0
y = 0
time.sleep(0.00001)
#while the map hasnt been fully scanned
while x < self.__height and y < self.__width:
#if this location is a wall or the goal, just ignore it
if self.__map[x][y] != self.__wall and \
self.__map[x][y] != self.__goal:
#a full trail to the robot has been located, finished!
maxLoc = self.maxSurroundingNodeValue(x, y)
if maxLoc < self.__reset_max and \
self.__map[x][y] == self.__robot:
if prnt:
print "Finished Wavefront:\n"
self.printMap()
# Tell the robot to move after this return.
return self.__max_node_location
#record a value in to this node
elif self.__maximum_node != self.__reset_max:
#if this isnt here, 'nothing' will go in the location
self.__map[x][y] = self.__maximum_node + 1
#go to next node and/or row
y += 1
if y == self.__width and x != self.__height:
x += 1
y = 0
#print self.__robot_x, self.__robot_y
if prnt:
print "Sweep #: %i\n" % (counter + 1)
self.printMap()
self.__steps += 1
counter += 1
return 0
###########################################################################
#def unpropagate(self):
"""
clears old path to determine new path
stay within boundary
"""
#for x in range(0, self.__height):
#for y in range(0, self.__width):
#if self.__map[x][y] != self.__wall and self.__map[x][y] != self.__goal:
#self.__map[x][y] = self.__path
#if this location is a wall or goal, just ignore it
#clear that space
###########################################################################
def maxSurroundingNodeValue(self, x, y):
"""
this method looks at a node and returns the lowest value around that
node.
"""
#reset minimum
self.__maximum_node = self.__reset_max
if y > 0 and x < (self.__height - 1):
if self.__map[x][y-1] < self.__maximum_node and self.__map[x+1][y] < self.__maximum_node and\
self.__map[x][y-1] != self.__wall and self.__map[x+1][y] != self.__wall:
if self.__map[x][y-1] >= self.__map[x+1][y]:
self.__maximum_node = self.__map[x][y-1]
self.__max_node_location = 4
else:
self.__maximum_node = self.__map[x+1][y]
self.__max_node_location = 3
if y < self.__width - 1 and x < self.__height - 1:
if self.__map[x][y+1] < self.__maximum_node and self.__map[x+1][y] < self.__maximum_node:
if self.__map[x][y+1] >= self.__map[x+1][y]:
self.__maximum_node = self.__map[x][y+1]
self.__max_node_location = 2
else:
self.__maximum_node = self.__map[x+1][y]
self.__max_node_location = 3
if x > 0 and y > 0:
if self.__map[x-1][y] < self.__maximum_node and self.__map[x][y-1] < self.__maximum_node:
if self.__map[x-1][y] >= self.__map[x][y-1]:
self.__maximum_node = self.__map[x-1][y]
self.__max_node_location = 1
else:
self.__maximum_node = self.__map[x][y-1]
self.__max_node_location = 4
if y < self.__width - 1 and x > 0:
if self.__map[x-1][y] < self.__maximum_node and self.__map[x][y+1] < self.__maximum_node:
if self.__map[x-1][y] >= self.__map[x][y+1]:
self.__maximum_node = self.__map[x-1][y]
self.__max_node_location = 1
else:
self.__maximum_node = self.__map[x][y+1]
self.__max_node_location = 2
return self.__maximum_node
###########################################################################
def printMap(self):
"""
Prints out the map of this instance of the class.
"""
msg = ''
for temp_B in range(0, self.__height):
for temp_A in range(0, self.__width):
if self.__map[temp_B][temp_A] == self.__wall:
msg += "%04s" % "[X]"
elif self.__map[temp_B][temp_A] == self.__robot:
msg += "%04s" % "o"
elif self.__map[temp_B][temp_A] == self.__goal:
msg += "%04s" % "G"
else:
msg += "%04s" % str(self.__map[temp_B][temp_A])
msg += "\n\n"
msg += "\n\n"
print msg
#
if self.__slow == True:
time.sleep(0.05)
#
如果名称 ==“主要”: “”” X是垂直的,Y是水平的
"""
floormap = [[19,19,19,19,19,19,19,19,19,19,19,19], \
[18,18,18,18,18,18,18,18,18,18,18,18], \
[17,17,17,17,17,17,17,17,17,17,17,17], \
[16,16,16,16,16,16,16,16,16,16,16,16], \
[15,15,15,15,15,15,15,15,15,15,15,15], \
[14,14,14,14,14,14,14,14,14,14,14,14], \
[13,13,13,13,13,13,13,13,13,13,13,13], \
[12,12,12,12,12,12,12,12,12,12,12,12], \
[11,11,11,11,11,11,11,11,11,11,11,12], \
[10,10,10,10,10,10,10,10,10,10,11,12], \
[9,9,9,9,9,9,9,9,9,10,11,12], \
[8,8,8,8,8,8,8,8,9,10,11,12], \
[7,7,7,7,7,7,7,8,9,10,11,12], \
[6,6,6,6,6,6,7,8,9,10,11,12], \
[5,5,5,5,5,6,7,8,9,10,11,12], \
[4,4,4,4,5,6,7,8,9,10,11,12], \
[3,3,3,4,5,6,7,8,9,10,11,12], \
[2,2,3,4,5,6,7,8,9,10,11,12], \
[1,2,3,4,5,6,7,8,9,10,11,12]]
start = time.time()
planner = waveFrontPlanner(floormap, False)
#time.sleep(2)
planner.run(True)
end = time.time()
print "Took %f seconds to run wavefront simulation" % (end - start)
答案 0 :(得分:0)
将网格保持在矩阵内,每个节点都包含您需要的信息:值,是障碍等等。
一种方法是使用深度优先搜索来获取所有可能的路径,并从引导您到达目的地的路径中选择最高价值路线。
Rant:这个问题被标记为Node.js,你应该删除标签,因为它没有任何关系。