rospy.Subscriber('map', OccupancyGrid, MapCallback) #rospy.Subscriber('map_metadata', MapMetaData, MapMetaCallback) #rospy.Subscriber('move_base_simple/goal', PoseStamped, GoalCallback) #rospy.Subscriber('initialpose', PoseWithCovarianceStamped, InitialPoseCallback) odom_list = tf.TransformListener() rospy.sleep(rospy.Duration(1, 0)) print "Starting Lab 3" i = 0 while not mapReady: sleep(.1) start = Point() goal = Point() start.x = 7 start.y = 1 start.z = 0 goal.x = 20 goal.y = 20 goal.z = 0 path = AStar.GetPath(occupancyGrid, start, goal) waypoints.Waypoints(path) #AStar.SearchForGoal(occupancyGrid, start, goal) print "Lab 3 complete!"
while 1: while not goalReady: time.sleep(.3) print "waiting" goalReady = 0 expandedMap, lowerResMap = ObstacleExpansion.ExpandMap(occupancyGrid) resPub.publish(lowerResMap) expPub.publish(expandedMap) start.x = x start.y = y print "start", x, y print "goal", goal.x, goal.y stop = 0 try: path = AStar.GetPath(expandedMap, start, goal) waypoints = AStar.Waypoints(path) for i in range (1, len(waypoints)): newx, newy = waypoint_math.TranslateWaypoint(expandedMap, waypoints[i]) newPointList = [Point(newx, newy, 0)] newCellPublisher = rospy.Publisher("newCells", GridCells) print "new x and y", newx, newy newCellPublisher.publish(AStar.MakeGridCellsFromList(expandedMap, newPointList)) turnAngle = waypoint_math.ChooseTurnDirection(newx, newy, x, y, theta) print turnAngle Rotate(turnAngle) driveDistance = waypoint_math.ChooseDriveDistance (newx, newy, x, y, theta) print driveDistance DriveStraight(.4, driveDistance) if (stop == 1): break