206 lines
7.1 KiB
Python
Executable File
206 lines
7.1 KiB
Python
Executable File
#!/usr/bin/env python
|
|
|
|
# Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma de
|
|
# Barcelona (UAB).
|
|
#
|
|
# This work is licensed under the terms of the MIT license.
|
|
# For a copy, see <https://opensource.org/licenses/MIT>.
|
|
|
|
import glob
|
|
import os
|
|
import sys
|
|
|
|
try:
|
|
sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % (
|
|
sys.version_info.major,
|
|
sys.version_info.minor,
|
|
'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
|
|
except IndexError:
|
|
pass
|
|
|
|
import carla
|
|
|
|
import argparse
|
|
import random
|
|
import time
|
|
|
|
red = carla.Color(255, 0, 0)
|
|
green = carla.Color(0, 255, 0)
|
|
blue = carla.Color(47, 210, 231)
|
|
cyan = carla.Color(0, 255, 255)
|
|
yellow = carla.Color(255, 255, 0)
|
|
orange = carla.Color(255, 162, 0)
|
|
white = carla.Color(255, 255, 255)
|
|
|
|
trail_life_time = 10
|
|
waypoint_separation = 4
|
|
|
|
|
|
def draw_transform(debug, trans, col=carla.Color(255, 0, 0), lt=-1):
|
|
debug.draw_arrow(
|
|
trans.location, trans.location + trans.get_forward_vector(),
|
|
thickness=0.05, arrow_size=0.1, color=col, life_time=lt)
|
|
|
|
|
|
def draw_waypoint_union(debug, w0, w1, color=carla.Color(255, 0, 0), lt=5):
|
|
debug.draw_line(
|
|
w0.transform.location + carla.Location(z=0.25),
|
|
w1.transform.location + carla.Location(z=0.25),
|
|
thickness=0.1, color=color, life_time=lt, persistent_lines=False)
|
|
debug.draw_point(w1.transform.location + carla.Location(z=0.25), 0.1, color, lt, False)
|
|
|
|
|
|
def draw_waypoint_info(debug, w, lt=5):
|
|
w_loc = w.transform.location
|
|
debug.draw_string(w_loc + carla.Location(z=0.5), "lane: " + str(w.lane_id), False, yellow, lt)
|
|
debug.draw_string(w_loc + carla.Location(z=1.0), "road: " + str(w.road_id), False, blue, lt)
|
|
debug.draw_string(w_loc + carla.Location(z=-.5), str(w.lane_change), False, red, lt)
|
|
|
|
def draw_junction(debug, junction, l_time=10):
|
|
"""Draws a junction bounding box and the initial and final waypoint of every lane."""
|
|
# draw bounding box
|
|
box = junction.bounding_box
|
|
point1 = box.location + carla.Location(x=box.extent.x, y=box.extent.y, z=2)
|
|
point2 = box.location + carla.Location(x=-box.extent.x, y=box.extent.y, z=2)
|
|
point3 = box.location + carla.Location(x=-box.extent.x, y=-box.extent.y, z=2)
|
|
point4 = box.location + carla.Location(x=box.extent.x, y=-box.extent.y, z=2)
|
|
debug.draw_line(
|
|
point1, point2,
|
|
thickness=0.1, color=orange, life_time=l_time, persistent_lines=False)
|
|
debug.draw_line(
|
|
point2, point3,
|
|
thickness=0.1, color=orange, life_time=l_time, persistent_lines=False)
|
|
debug.draw_line(
|
|
point3, point4,
|
|
thickness=0.1, color=orange, life_time=l_time, persistent_lines=False)
|
|
debug.draw_line(
|
|
point4, point1,
|
|
thickness=0.1, color=orange, life_time=l_time, persistent_lines=False)
|
|
# draw junction pairs (begin-end) of every lane
|
|
junction_w = junction.get_waypoints(carla.LaneType.Any)
|
|
for pair_w in junction_w:
|
|
draw_transform(debug, pair_w[0].transform, orange, l_time)
|
|
debug.draw_point(
|
|
pair_w[0].transform.location + carla.Location(z=0.75), 0.1, orange, l_time, False)
|
|
draw_transform(debug, pair_w[1].transform, orange, l_time)
|
|
debug.draw_point(
|
|
pair_w[1].transform.location + carla.Location(z=0.75), 0.1, orange, l_time, False)
|
|
debug.draw_line(
|
|
pair_w[0].transform.location + carla.Location(z=0.75),
|
|
pair_w[1].transform.location + carla.Location(z=0.75), 0.1, white, l_time, False)
|
|
|
|
def main():
|
|
argparser = argparse.ArgumentParser()
|
|
argparser.add_argument(
|
|
'--host',
|
|
metavar='H',
|
|
default='127.0.0.1',
|
|
help='IP of the host server (default: 127.0.0.1)')
|
|
argparser.add_argument(
|
|
'-p', '--port',
|
|
metavar='P',
|
|
default=2000,
|
|
type=int,
|
|
help='TCP port to listen to (default: 2000)')
|
|
argparser.add_argument(
|
|
'-i', '--info',
|
|
action='store_true',
|
|
help='Show text information')
|
|
argparser.add_argument(
|
|
'-x',
|
|
default=0.0,
|
|
type=float,
|
|
help='X start position (default: 0.0)')
|
|
argparser.add_argument(
|
|
'-y',
|
|
default=0.0,
|
|
type=float,
|
|
help='Y start position (default: 0.0)')
|
|
argparser.add_argument(
|
|
'-z',
|
|
default=0.0,
|
|
type=float,
|
|
help='Z start position (default: 0.0)')
|
|
argparser.add_argument(
|
|
'-s', '--seed',
|
|
metavar='S',
|
|
default=os.getpid(),
|
|
type=int,
|
|
help='Seed for the random path (default: program pid)')
|
|
argparser.add_argument(
|
|
'-t', '--tick-time',
|
|
metavar='T',
|
|
default=0.2,
|
|
type=float,
|
|
help='Tick time between updates (forward velocity) (default: 0.2)')
|
|
args = argparser.parse_args()
|
|
|
|
try:
|
|
client = carla.Client(args.host, args.port)
|
|
client.set_timeout(2.0)
|
|
|
|
world = client.get_world()
|
|
m = world.get_map()
|
|
debug = world.debug
|
|
|
|
random.seed(args.seed)
|
|
print("Seed: ", args.seed)
|
|
|
|
loc = carla.Location(args.x, args.y, args.z)
|
|
print("Initial location: ", loc)
|
|
|
|
current_w = m.get_waypoint(loc)
|
|
|
|
# main loop
|
|
while True:
|
|
# list of potential next waypoints
|
|
potential_w = list(current_w.next(waypoint_separation))
|
|
|
|
# check for available right driving lanes
|
|
if current_w.lane_change & carla.LaneChange.Right:
|
|
right_w = current_w.get_right_lane()
|
|
if right_w and right_w.lane_type == carla.LaneType.Driving:
|
|
potential_w += list(right_w.next(waypoint_separation))
|
|
|
|
# check for available left driving lanes
|
|
if current_w.lane_change & carla.LaneChange.Left:
|
|
left_w = current_w.get_left_lane()
|
|
if left_w and left_w.lane_type == carla.LaneType.Driving:
|
|
potential_w += list(left_w.next(waypoint_separation))
|
|
|
|
# choose a random waypoint to be the next
|
|
next_w = random.choice(potential_w)
|
|
potential_w.remove(next_w)
|
|
|
|
# Render some nice information, notice that you can't see the strings if you are using an editor camera
|
|
if args.info:
|
|
draw_waypoint_info(debug, current_w, trail_life_time)
|
|
draw_waypoint_union(debug, current_w, next_w, cyan if current_w.is_junction else green, trail_life_time)
|
|
draw_transform(debug, current_w.transform, white, trail_life_time)
|
|
|
|
# print the remaining waypoints
|
|
for p in potential_w:
|
|
draw_waypoint_union(debug, current_w, p, red, trail_life_time)
|
|
draw_transform(debug, p.transform, white, trail_life_time)
|
|
|
|
# draw all junction waypoints and bounding box
|
|
if next_w.is_junction:
|
|
junction = next_w.get_junction()
|
|
draw_junction(debug, junction, trail_life_time)
|
|
|
|
# update the current waypoint and sleep for some time
|
|
current_w = next_w
|
|
time.sleep(args.tick_time)
|
|
|
|
finally:
|
|
pass
|
|
|
|
|
|
if __name__ == '__main__':
|
|
try:
|
|
main()
|
|
except KeyboardInterrupt:
|
|
print('\nExit by user.')
|
|
finally:
|
|
print('\nExit.')
|