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)