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
