def game_loop(args):
    """ Main loop for agent"""

    pygame.init()
    pygame.font.init()
    world = None
    tot_target_reached = 0
    num_min_waypoints = 21
    rand_int = np.random.randint(100)

    try:
        client = carla.Client(args.host, args.port)
        client.set_timeout(4.0)

        client.load_world('Town0%d' % args.town)
        client.reload_world()

        display = pygame.display.set_mode((args.width, args.height),
                                          pygame.HWSURFACE | pygame.DOUBLEBUF)

        hud = HUD(args.width, args.height)
        world = World(client.get_world(), hud, args)
        controller = KeyboardControl(world)

        # map_dir = "/home/yash/Downloads/CARLA_0.9.9/PythonAPI/examples/Recordings_%d/" % args.town
        map_dir = "D:\\CARLA\\WindowsNoEditor\\PythonAPI\\examples\\Recordings_%d\\" % args.town
        map_obj = ParseCarlaMap(map_dir)
        points = [46, 81, 132, 10, 40, 14, 110, 71, 58, 4]

        edges = list(permutations(points, 2))
        edge_table = np.zeros(shape=(len(edges), 3), dtype=np.float32)
        edge_table[:, 0:2] = edges

        for index in range(edge_table.shape[0]):
            spawn_index, dest_index = edge_table[index, 0:2].astype(np.int)
            spawn_loc = carla.Location(
                x=map_obj.node_dict["location"]["x"][spawn_index],
                y=map_obj.node_dict["location"]["y"][spawn_index],
                z=map_obj.node_dict["location"]["z"][spawn_index])
            spawn_rot = carla.Rotation(
                roll=map_obj.node_dict["rotation"]["roll"][spawn_index],
                pitch=map_obj.node_dict["rotation"]["pitch"][spawn_index],
                yaw=map_obj.node_dict["rotation"]["yaw"][spawn_index])
            spawn_pt = carla.Transform(location=spawn_loc, rotation=spawn_rot)

            dest_loc = carla.Location(
                x=map_obj.node_dict["location"]["x"][dest_index],
                y=map_obj.node_dict["location"]["y"][dest_index],
                z=map_obj.node_dict["location"]["z"][dest_index])
            # dest_rot = carla.Rotation(roll=map_obj.node_dict["rotation"]["roll"][dest_index],
            #                            pitch=map_obj.node_dict["rotation"]["pitch"][dest_index],
            #                            yaw=map_obj.node_dict["rotation"]["yaw"][dest_index])
            # dest_pt = carla.Transform(location=dest_loc, rotation=dest_rot)

            world.restart(args, spawn_pt=spawn_pt)

            if args.agent == "Roaming":
                agent = RoamingAgent(world.player)
            elif args.agent == "Basic":
                agent = BasicAgent(world.player)
                spawn_point = world.map.get_spawn_points()[0]
                agent.set_destination(
                    (spawn_point.location.x, spawn_point.location.y,
                     spawn_point.location.z))
            else:
                agent = BehaviorAgent(
                    world.player,
                    ignore_traffic_light=args.ignore_traffic_light,
                    behavior=args.behavior)
                agent.set_destination(agent.vehicle.get_location(),
                                      dest_loc,
                                      clean=True)

            clock = pygame.time.Clock()
            start_time = time.time()
            while True:
                clock.tick_busy_loop(60)
                if controller.parse_events():
                    return

                # As soon as the server is ready continue!
                if not world.world.wait_for_tick(10.0):
                    continue

                if args.agent == "Roaming" or args.agent == "Basic":
                    if controller.parse_events():
                        return

                    # as soon as the server is ready continue!
                    world.world.wait_for_tick(10.0)

                    world.tick(clock)
                    world.render(display)
                    pygame.display.flip()
                    control = agent.run_step()
                    control.manual_gear_shift = False
                    world.player.apply_control(control)
                else:
                    agent.update_information(world)

                    world.tick(clock)
                    world.render(display)
                    pygame.display.flip()

                    # Set new destination when target has been reached
                    if len(agent.get_local_planner().waypoints_queue
                           ) < num_min_waypoints and args.loop:
                        agent.reroute(spawn_points)
                        tot_target_reached += 1
                        world.hud.notification("The target has been reached " +
                                               str(tot_target_reached) +
                                               " times.",
                                               seconds=4.0)

                    elif len(agent.get_local_planner().waypoints_queue
                             ) == 0 and not args.loop:
                        print("Target reached, mission accomplished...")
                        break

                    speed_limit = world.player.get_speed_limit()
                    agent.get_local_planner().set_speed(speed_limit)

                    control = agent.run_step()
                    world.player.apply_control(control)
                if time.time() - start_time >= 280:
                    break

            edge_table[index, 2] = time.time() - start_time
            print("edge %d was completed in %3.3f seconds" %
                  (index, edge_table[index, 2]))
            np.savetxt('Carla_0%d_%d.csv' % (args.town, rand_int),
                       edge_table,
                       fmt="%3d,%3d,%3.3f")

    finally:
        if world is not None:
            world.destroy()

        pygame.quit()
Exemplo n.º 2
0
def game_loop(args):
    """ Main loop for agent"""

    pygame.init()
    pygame.font.init()
    world = None
    tot_target_reached = 0
    num_min_waypoints = 21

    try:
        client = carla.Client(args.host, args.port)
        client.set_timeout(4.0)

        display = pygame.display.set_mode((args.width, args.height),
                                          pygame.HWSURFACE | pygame.DOUBLEBUF)

        hud = HUD(args.width, args.height)
        world = World(client.get_world(), hud, args)
        controller = KeyboardControl(world)

        if args.agent == "Roaming":
            agent = RoamingAgent(world.player)
        elif args.agent == "Basic":
            agent = BasicAgent(world.player)
            spawn_point = world.map.get_spawn_points()[0]
            agent.set_destination(
                (spawn_point.location.x, spawn_point.location.y,
                 spawn_point.location.z))
        else:
            agent = BehaviorAgent(world.player, behavior=args.behavior)

            spawn_points = world.map.get_spawn_points()
            random.shuffle(spawn_points)

            if spawn_points[0].location != agent.vehicle.get_location():
                destination = spawn_points[0].location
            else:
                destination = spawn_points[1].location

            agent.set_destination(agent.vehicle.get_location(),
                                  destination,
                                  clean=True)

        clock = pygame.time.Clock()

        while True:
            clock.tick_busy_loop(60)
            if controller.parse_events():
                return

            # As soon as the server is ready continue!
            if not world.world.wait_for_tick(10.0):
                continue

            if args.agent == "Roaming" or args.agent == "Basic":
                if controller.parse_events():
                    return

                # as soon as the server is ready continue!
                world.world.wait_for_tick(10.0)

                world.tick(clock)
                world.render(display)
                pygame.display.flip()
                control = agent.run_step()
                control.manual_gear_shift = False
                world.player.apply_control(control)
            else:
                agent.update_information()

                world.tick(clock)
                world.render(display)
                pygame.display.flip()
                # if world.save_count<5:
                #     world.camera_manager.image.save_to_disk('_out/%08d' % image.frame)
                #     world.save_count+=1

                # Set new destination when target has been reached
                if len(agent.get_local_planner().waypoints_queue
                       ) < num_min_waypoints and args.loop:
                    agent.reroute(spawn_points)
                    tot_target_reached += 1
                    world.hud.notification("The target has been reached " +
                                           str(tot_target_reached) + " times.",
                                           seconds=4.0)

                elif len(agent.get_local_planner().waypoints_queue
                         ) == 0 and not args.loop:
                    print("Target reached, mission accomplished...")
                    break

                speed_limit = world.player.get_speed_limit()
                agent.get_local_planner().set_speed(speed_limit)

                control = agent.run_step()
                world.player.apply_control(control)

    finally:
        if world is not None:
            world.destroy1()

        pygame.quit()
Exemplo n.º 3
0
def game_loop(args):
    """ Main loop for agent"""

    pygame.init()
    pygame.font.init()
    world = None
    original_settings = None
    tot_target_reached = 0
    num_min_waypoints = 21

    try:
        client = carla.Client(args.host, args.port)
        client.set_timeout(4.0)

        display = pygame.display.set_mode((args.width, args.height),
                                          pygame.HWSURFACE | pygame.DOUBLEBUF)

        hud = HUD(args.width, args.height)
        world = World(client.get_world(), hud, args)
        # make synchronous and disable keyboard controller BEGIN
        logging.info("make synchronous and disable keyboard controller")
        original_settings = world.world.get_settings()
        settings = world.world.get_settings()
        traffic_manager = client.get_trafficmanager(8000)
        traffic_manager.set_synchronous_mode(True)

        delta = 0.06

        settings.fixed_delta_seconds = delta
        settings.synchronous_mode = True
        world.world.apply_settings(settings)
        # END
        controller = KeyboardControl(world)

        if args.agent == "Roaming":
            agent = RoamingAgent(world.player)
        elif args.agent == "Basic":
            agent = BasicAgent(world.player)
            spawn_point = world.map.get_spawn_points()[0]
            agent.set_destination(
                (spawn_point.location.x, spawn_point.location.y,
                 spawn_point.location.z))
        else:
            agent = BehaviorAgent(world.player, behavior=args.behavior)

            spawn_points = world.map.get_spawn_points()
            random.shuffle(spawn_points)

            if spawn_points[0].location != agent.vehicle.get_location():
                destination = spawn_points[0].location
            else:
                destination = spawn_points[1].location

            agent.set_destination(agent.vehicle.get_location(),
                                  destination,
                                  clean=True)

        clock = pygame.time.Clock()

        while True:
            clock.tick_busy_loop(60)
            world.world.tick()  # synchronous
            hud.on_world_tick(world.world.get_snapshot().timestamp)
            if controller.parse_events():
                return

            # As soon as the server is ready continue!
            # if not world.world.wait_for_tick(10.0):
            #     continue

            if args.agent == "Roaming" or args.agent == "Basic":
                if controller.parse_events():
                    return

                # as soon as the server is ready continue!
                # world.world.wait_for_tick(10.0)

                world.tick(clock)
                world.render(display)
                pygame.display.flip()
                control = agent.run_step()
                control.manual_gear_shift = False
                world.player.apply_control(control)
            else:
                agent.update_information()

                world.tick(clock)
                world.render(display)
                pygame.display.flip()

                # Set new destination when target has been reached
                if len(agent.get_local_planner().waypoints_queue
                       ) < num_min_waypoints and args.loop:
                    agent.reroute(spawn_points)
                    tot_target_reached += 1
                    world.hud.notification("The target has been reached " +
                                           str(tot_target_reached) + " times.",
                                           seconds=4.0)

                elif len(agent.get_local_planner().waypoints_queue
                         ) == 0 and not args.loop:
                    print("Target reached, mission accomplished...")
                    break

                speed_limit = world.player.get_speed_limit()
                agent.get_local_planner().set_speed(speed_limit)

                control = agent.run_step()
                world.player.apply_control(control)

    finally:
        if world is not None:
            if original_settings is not None:
                world.world.apply_settings(original_settings)
            world.destroy()

        pygame.quit()
Exemplo n.º 4
0
def game_loop(args):
    """ Main loop for agent"""

    pygame.init()
    pygame.font.init()
    world = None
    tot_target_reached = 0
    num_min_waypoints = 21

    try:
        client = carla.Client(args.host, args.port)
        client.set_timeout(4.0)

        display = pygame.display.set_mode((args.width, args.height),
                                          pygame.HWSURFACE | pygame.DOUBLEBUF)

        hud = HUD(args.width, args.height)
        world = World(client.get_world(), hud, args)
        controller = KeyboardControl(world)

        if args.agent == "Roaming":
            agent = RoamingAgent(world.player)
        elif args.agent == "Basic":
            agent = BasicAgent(world.player)
            spawn_point = world.map.get_spawn_points()[0]
            agent.set_destination(
                (spawn_point.location.x, spawn_point.location.y,
                 spawn_point.location.z))
        else:
            # start and destination
            agent = BehaviorAgent(world.player, behavior=args.behavior)
            spawn_points = world.map.get_spawn_points()  # 7 spawn points
            # --- !!! modify: start and destination --- #
            # start --> destination way-points
            start = agent.vehicle.get_location()
            # end 1 and end 2
            destination = spawn_points[1].location

            agent.set_destination(start, destination, clean=True)
            # --- draw the waypoints --- #
            waypoints = agent._local_planner.waypoints_queue
            for w in waypoints:
                world.world.debug.draw_string(w[0].transform.location,
                                              'O',
                                              draw_shadow=False,
                                              color=carla.Color(r=255,
                                                                g=0,
                                                                b=0),
                                              life_time=120.0,
                                              persistent_lines=True)
            # -------------------------------------------------------------------------- #
            # random.shuffle(spawn_points)
            # if spawn_points[0].location != agent.vehicle.get_location():
            #     destination = spawn_points[0].location
            # else:
            #     destination = spawn_points[1].location
            # agent.set_destination(agent.vehicle.get_location(), destination, clean=True)

        clock = pygame.time.Clock()
        time_step = 0

        with open('driving_log.csv', 'a') as csv_file:
            writer = csv.writer(csv_file,
                                delimiter=',',
                                quotechar=' ',
                                quoting=csv.QUOTE_MINIMAL,
                                lineterminator='\n')
            while True:
                time_step += 1
                clock.tick_busy_loop(10.0)
                if controller.parse_events():
                    return

                # As soon as the server is ready continue!
                if not world.world.wait_for_tick(10.0):
                    continue

                if args.agent == "Roaming" or args.agent == "Basic":
                    if controller.parse_events():
                        return

                    # as soon as the server is ready continue!
                    world.world.wait_for_tick(10.0)

                    world.tick(clock)
                    world.render(display)
                    pygame.display.flip()
                    control = agent.run_step()
                    control.manual_gear_shift = False
                    world.player.apply_control(control)
                else:
                    agent.update_information(world)
                    world.tick(clock)
                    world.render(display)
                    pygame.display.flip()

                    # Set new destination when target has been reached
                    if len(agent.get_local_planner().waypoints_queue
                           ) < num_min_waypoints and args.loop:
                        agent.reroute(spawn_points)
                        tot_target_reached += 1
                        world.hud.notification("The target has been reached " +
                                               str(tot_target_reached) +
                                               " times.",
                                               seconds=4.0)

                    elif len(agent.get_local_planner().waypoints_queue
                             ) == 0 and not args.loop:
                        print("Target reached, mission accomplished...")
                        break

                    # --- modify limit speed --- #
                    speed_limit = world.player.get_speed_limit()
                    agent.get_local_planner().set_speed(speed_limit)
                    control = agent.run_step()
                    world.player.apply_control(control)

                    # --- save the image and vehicle information --- #
                    # save the camera-rgb image
                    image_name = str(world.camera_manager.image_name) + '.png'
                    pos = agent.vehicle.get_location()  # position
                    vel = agent.vehicle.get_velocity()
                    velocity = 3.6 * math.sqrt(
                        vel.x**2 + vel.y**2 + vel.z**2)  # velocity
                    steer = round(control.steer, 5)  # steer
                    throttle = round(control.throttle, 5)
                    writer.writerow([image_name, steer, throttle])

    finally:
        if world is not None:
            world.destroy()

        pygame.quit()