def setPathMarkers(finalTrajectory, came_from):
(sourcePoint, goalPoint, neighbourPoint,
finalPath) = init.initPathMarkers()
# neighbourPoint
searchedPoints = []
for key in came_from:
searchedPoints.append(came_from[key])
searchedPoints.remove(None)
for i in range(len(searchedPoints)):
tempPoint = Point()
tempPoint.x = searchedPoints[i][0]
tempPoint.y = searchedPoints[i][1]
tempPoint.z = searchedPoints[i][2]
neighbourPoint.points.append(tempPoint)
# finalPath
for i in range(len(finalTrajectory)):
tempPoint = Point()
tempPoint.x = finalTrajectory[i][0]
tempPoint.y = finalTrajectory[i][1]
tempPoint.z = finalTrajectory[i][2]
finalPath.points.append(tempPoint)
# sourcePoint
tempPoint = Point()
tempPoint.x = finalTrajectory[0][0]
tempPoint.y = finalTrajectory[0][1]
tempPoint.z = finalTrajectory[0][2]
sourcePoint.points.append(tempPoint)
sourcePoint.pose.position.x = finalTrajectory[0][0]
sourcePoint.pose.position.y = finalTrajectory[0][1]
sourcePoint.pose.position.z = finalTrajectory[0][2]
# goalPoint
tempPoint = Point()
tempPoint.x = finalTrajectory[len(finalTrajectory) - 1][0]
tempPoint.y = finalTrajectory[len(finalTrajectory) - 1][1]
tempPoint.z = finalTrajectory[len(finalTrajectory) - 1][2]
goalPoint.points.append(tempPoint)
goalPoint.pose.position.x = finalTrajectory[len(finalTrajectory) - 1][0]
goalPoint.pose.position.y = finalTrajectory[len(finalTrajectory) - 1][1]
goalPoint.pose.position.z = finalTrajectory[len(finalTrajectory) - 1][2]
return (sourcePoint, goalPoint, neighbourPoint, finalPath)
def publishknownmaze():
global maze
global mazestart
global mazegoal
global pathPoint
# rospy.logwarn('Publishing known maze...')
pathPoint = init.initPathMarkers()
knownObstPoint = init.initObstMarkers()
knownObstPoint.color.r = 0.8
knownObstPoint.color.g = 0.4
knownObstPoint.id = 31
for x in range(inc.MAZELENGTH):
for y in range(inc.MAZEWIDTH):
for z in range(inc.MAZEHEIGHT):
for d in range(inc.DIRECTIONS):
if maze[x][y][z].move[d]:
break
elif maze[x][y][z].obstacle:
tempPoint = Point()
tempPoint.x = x
tempPoint.y = y
tempPoint.z = z
knownObstPoint.points.append(tempPoint)
tmpcell = mazegoal
while tmpcell != mazestart:
tempPoint = Point()
tempPoint.x = tmpcell.x
tempPoint.y = tmpcell.y
tempPoint.z = tmpcell.z
pathPoint.points.append(tempPoint)
tmpcell = tmpcell.searchtree
tempPoint = Point()
tempPoint.x = mazestart.x
tempPoint.y = mazestart.y
tempPoint.z = mazestart.z
pathPoint.points.append(tempPoint)
pathPoint.text = '1'
rospy.logwarn('path length: ' + str(len(pathPoint.points)))
pathPub.publish(pathPoint)
obstPub.publish(knownObstPoint)
''' Intialize markers ''' currentPoint = init.initPointMarkers() currentPoint.color.r = 1.0 currentPoint.color.g = 1.0 currentPoint.id = 3 temptuple = tuple(init.gridalize(current_position, scale)) tempPoint = Point() tempPoint.x = temptuple[0] tempPoint.y = temptuple[1] tempPoint.z = temptuple[2] currentPoint.points.append(tempPoint) currentPoint.pose.position.x = temptuple[0] currentPoint.pose.position.y = temptuple[1] currentPoint.pose.position.z = temptuple[2] pathPoint = init.initPathMarkers() knownObstPoint = init.initObstMarkers() knownObstPoint.color.r = 0.8 knownObstPoint.color.g = 0.4 knownObstPoint.id = 31 #ifdef RANDOMIZESUCCS createpermutations() #endif keymodifier = 0 print '=====================================================================' initEnv.newEmptyEnv() # initEnv.newdfsmaze(inc.WALLSTOREMOVE)