def __init__(self, client, city_name=None):
self.display = None
self.clock = None
self.controller = None
self.client = client
self.city_name = city_name
self.settings = None
self.main_view = None
self.second_view = None
self.third_view = None
self.tpp_view = None
self.map = CarlaMap(city_name, 16.43,
50.0) if city_name is not None else None
self.map_view = self.map.get_map(WINDOW_HEIGHT //
2) if city_name is not None else None
self.map_shape = self.map.map_image.shape if city_name is not None else None
self.info = None
self.positions = None
self.episode_data_dir = None
self.recording = None
self.data = None
self.connect = None
def __init__(
self,
city_name='Town01',
name_to_save='Test',
continue_experiment=False,
save_images=False,
distance_for_success=2.0
):
self.__metaclass__ = abc.ABCMeta
self._city_name = city_name
self._base_name = name_to_save
# The minimum distance for arriving into the goal point in
# order to consider ir a success
self._distance_for_success = distance_for_success
# The object used to record the benchmark and to able to continue after
self._recording = Recording(name_to_save=name_to_save,
continue_experiment=continue_experiment,
save_images=save_images
)
# We have a default planner instantiated that produces high level commands
self._planner = Planner(city_name)
self.carla_map = CarlaMap(city_name, 0.1653, 50)
def __init__(self, carla_client, args, wrapper):
self.client = carla_client
self._carla_settings = make_carla_settings(args)
self._timer = None
self._display = None
self._main_image = None
self._mini_view_image1 = None
self._mini_view_image2 = None
self._enable_autopilot = True
self._lidar_measurement = None
self._map_view = None
self._is_on_reverse = False
self._city_name = args.map_name
self._map = CarlaMap(self._city_name, 0.1643,
50.0) if self._city_name is not None else None
self._map_shape = self._map.map_image.shape if self._city_name is not None else None
self._map_view = self._map.get_map(
WINDOW_HEIGHT) if self._city_name is not None else None
self._position = None
self._agent_positions = None
self.should_display = True
random.seed(datetime.datetime.now())
self.manual = True
self.manual_control = (random.randint(1, 1000) == 1)
self.cnt = 0
self.history_collision = 0
self.ucnt = 0
self.prev_control = None
self.endnow = False
self.carla_wrapper = wrapper
def __init__(self, carla_client, args):
self.client = carla_client
self._carla_settings = make_carla_settings(args)
self._display = None
self._enable_autopilot = args.autopilot
self._is_on_reverse = False
self._city_name = args.map_name
self._map = CarlaMap(self._city_name, 16.43,
50.0) if self._city_name is not None else None
self._map_shape = self._map.map_image.shape if self._city_name is not None else None
#self._map_view = self._map.get_map(WINDOW_HEIGHT) if self._city_name is not None else None
self._map_view = None
self._position = None
self._control = VehicleControl()
self._control.throttle = 0.58
self._location = [0, 0, 0]
self._started = False
self._startID = [
0, 2, 4, 6, 11, 13, 17, 19, 21, 24, 30, 39, 55, 57, 66, 70
]
self._speed = 0
self._offroad = 0
self._terminal = False
self._error_dis = 0
self._error_yaw = 0
self._road_left, self._road, self._road_right = self._load_road(
'waypoints', 15)
def __init__(self, carla_client, nn_agent, saveid, args):
self.client = carla_client
self.NNagent = nn_agent
self._carla_settings = make_carla_settings(args)
self._timer = None
self._display = None
self._main_image = None
self._mini_view_image1 = None
self._mini_view_image2 = None
self._enable_autopilot = args.autopilot
self._NNagent_drives = False
self._lidar_measurement = None
self._map_view = None
self._is_on_reverse = False
self._city_name = args.map_name
self._map = CarlaMap(self._city_name, 0.1643, 50.0) if self._city_name is not None else None
self._map_shape = self._map.map_image.shape if self._city_name is not None else None
self._map_view = self._map.get_map(WINDOW_HEIGHT) if self._city_name is not None else None
self._position = None
self._agent_positions = None
self._at_intesection = False
self.brake_count = 0
self.saveid = saveid
self._savecount = 0
# ADDITION: Limit H5 files to 200 images each, keep track of image and measurement arrays
self._countFrames = 0
self._globalCount = 0
#self._sizeBatch = 200
self._numTarget = 28
self._h5_image = None
self._save_holder = []
self._save_holder_stop = []
self._cmd = 5
self._drivestate = 0
def __init__(self, city_name):
self.timer = Timer()
# Map setup
self._map = CarlaMap(city_name)
self._centerlines = Centerlines(city_name)
# Agent Setup
Agent.__init__(self)
self._neural_net = CAL_network()
self._seq_len = 5 #self._neural_net.model.max_input_shape
self._state = VehicleState()
self._agents_present = False
# Controller setup
param_path = os.path.dirname(__file__) + '/controller/params/'
cruise_params = get_params_from_txt(param_path + 'cruise_params.txt')
self._PID_cruise = PID(*cruise_params)
follow_params = get_params_from_txt(param_path + 'follow_params.txt')
self._PID_follow = PID(*follow_params)
# General Parameter setup
general_params = get_params_from_txt(param_path + 'general_params.txt')
self.c, self.d = general_params[0], general_params[1]
self.Kl_STANLEY = general_params[2]
self.Kr_STANLEY = general_params[3]
self.K0_STANLEY = general_params[4]
self.curve_slowdown = general_params[5]
self.DELTAl = general_params[6]
self.DELTAr = general_params[7]
self.DELTA0 = general_params[8]
self.EXP_DECAY = general_params[9]
def __init__(self, carla_client, args):
self.client = carla_client
self._carla_settings = make_carla_settings(args)
self._timer = None
self._display = None
self._main_image = None
self._mini_view_image1 = None
self._mini_view_image2 = None
self._enable_autopilot = args.autopilot
self._lidar_measurement = None
self._map_view = None
self._is_on_reverse = 0
self._city_name = args.map_name
self._map = CarlaMap(self._city_name, 16.43, 50.0) if self._city_name is not None else None
self._map_shape = self._map.map_image.shape if self._city_name is not None else None
self._map_view = self._map.get_map(WINDOW_HEIGHT) if self._city_name is not None else None
self._position = None
self._agent_positions = None
self.rev_count = 0
# ADDITION: Limit H5 files to 200 images each, keep track of image and measurement arrays
self._countFrames = 0
self._globalCount = 0
self._sizeBatch = 200
self._numTarget = 28
self._h5_image = None
self._image_array = np.zeros((self._sizeBatch,88,200,3))
self._meas_array = np.zeros((self._sizeBatch,self._numTarget))
def initialize_game(self, map_name, render_mode=True):
"""
Initialize the windows
Args:
map_name: The map that is going to be used (If it was up to display)
Returns:
None
"""
self._render_mode = render_mode
self._map_name = map_name
self._map = CarlaMap(map_name, 0.1643, 50.0)
self._map_shape = self._map.map_image.shape
self._map_view = self._map.get_map(self._window_height)
if self._render_mode:
if self._display_map:
extra_width = int(
(self._window_height / float(self._map_shape[0])) *
self._map_shape[1])
self._display = pygame.display.set_mode(
(self._window_width + extra_width, self._window_height),
pygame.HWSURFACE | pygame.DOUBLEBUF)
else:
self._display = pygame.display.set_mode(
(self._window_width, self._window_height),
pygame.HWSURFACE | pygame.DOUBLEBUF)
logging.debug('pygame started')
def __init__(self, carla_client, args):
self.client = carla_client
self._carla_settings, self._intrinsic, self._camera_to_car_transform, self._lidar_to_car_transform = make_carla_settings(
args)
self._timer = None
self._display = None
self._main_image = None
self._mini_view_image1 = None
self._mini_view_image2 = None
self._enable_autopilot = args.autopilot
self._lidar_measurement = None
self._map_view = None
self._is_on_reverse = False
self._city_name = args.map_name
self._map = CarlaMap(self._city_name, 16.43,
50.0) if self._city_name is not None else None
self._map_shape = self._map.map_image.shape if self._city_name is not None else None
self._map_view = self._map.get_map(
WINDOW_HEIGHT) if self._city_name is not None else None
self._position = None
self._agent_positions = None
self.captured_frame_no = self.current_captured_frame_num()
self._measurements = None
self._extrinsic = None
# To keep track of how far the car has driven since the last capture of data
self._agent_location_on_last_capture = None
self._frames_since_last_capture = 0
# How many frames we have captured since reset
self._captured_frames_since_restart = 0
def __init__(self, carla_client):
self.client = carla_client
self._carla_settings = self.make_carla_settings()
self._timer = None
self._display = None
self._main_image = None
self._mini_view_image1 = None
self._mini_view_image2 = None
self._enable_autopilot = True
self._lidar_measurement = None
self._map_view = None
self._is_on_reverse = False
self._city_name = MAP_NAME
self._map = CarlaMap(self._city_name, 0.1643,
50.0) if self._city_name is not None else None
self._map_shape = self._map.map_image.shape if self._city_name is not None else None
self._map_view = self._map.get_map(
WINDOW_HEIGHT) if self._city_name is not None else None
self._position = None
self._agent_positions = None
self.for_save = False
self.should_display = True
random.seed(datetime.datetime.now())
self.manual = (random.randint(1, 2) != 1)
self.step = 0
self.history_collision = 0
self.prev_image = None
def __init__(self, map_name, topic='/map'):
self.map_name = map_name
self.carla_map = CarlaMap(map_name)
self.map_pub = rospy.Publisher(topic,
OccupancyGrid,
queue_size=10,
latch=True)
self.build_map_message()
def loc_to_pix_01_02(self, location, town_name):
if self.carla_map is None:
from carla.planner.map import CarlaMap
self.carla_map = {"01": CarlaMap("Town01", 0.1643, 50.0),
"02": CarlaMap("Town02", 0.1643, 50.0)}
cur = self.carla_map[town_name].convert_to_pixel([location[0], location[1], .22])
cur = [int(cur[1]), int(cur[0])]
return cur
def view_start_positions(args):
# We assume the CARLA server is already waiting for a client to connect at
# host:port. The same way as in the client example.
with make_carla_client(args.host, args.port) as client:
print('CarlaClient connected')
# We load the default settings to the client.
scene = client.load_settings(CarlaSettings())
print("Received the start positions")
# We get the number of player starts, in order to detect the city.
number_of_player_starts = len(scene.player_start_spots)
if number_of_player_starts > 100: # WARNING: unsafe way to check for city, see issue #313
image = mpimg.imread("carla/planner/Town01.png")
carla_map = CarlaMap('Town01', 0.1653, 50)
else:
image = mpimg.imread("carla/planner/Town02.png")
carla_map = CarlaMap('Town02', 0.1653, 50)
fig, ax = plt.subplots(1)
ax.imshow(image)
if args.positions == 'all':
positions_to_plot = range(len(scene.player_start_spots))
else:
positions_to_plot = map(int, args.positions.split(','))
for position in positions_to_plot:
# Check if position is valid
if position >= len(scene.player_start_spots):
raise RuntimeError('Selected position is invalid')
# Convert world to pixel coordinates
pixel = carla_map.convert_to_pixel([
scene.player_start_spots[position].location.x,
scene.player_start_spots[position].location.y,
scene.player_start_spots[position].location.z
])
circle = Circle((pixel[0], pixel[1]),
12,
color='r',
label='A point')
ax.add_patch(circle)
if not args.no_labels:
plt.text(pixel[0], pixel[1], str(position), size='x-small')
plt.axis('off')
plt.show()
fig.savefig('town_positions.pdf',
orientation='landscape',
bbox_inches='tight')
def view_start_positions(args):
# We assume the CARLA server is already waiting for a client to connect at
# host:port. The same way as in the client example.
with make_carla_client(args.host, args.port) as client:
print('CarlaClient connected')
# We load the default settings to the client.
scene = client.load_settings(CarlaSettings())
print("Received the start positions")
try:
print(scene.map_name)
image = mpimg.imread('./environment/%s.png' % scene.map_name)
carla_map = CarlaMap(scene.map_name, 0.1653, 50)
except IOError as exception:
logging.error(exception)
logging.error('Cannot find map "%s"', scene.map_name)
sys.exit(1)
fig, ax = plt.subplots(1)
ax.imshow(image)
if args.positions == 'all':
positions_to_plot = range(len(scene.player_start_spots))
else:
positions_to_plot = map(int, args.positions.split(','))
for position in positions_to_plot:
# Check if position is valid
if position >= len(scene.player_start_spots):
raise RuntimeError('Selected position is invalid')
# Convert world to pixel coordinates
pixel = carla_map.convert_to_pixel([
scene.player_start_spots[position].location.x,
scene.player_start_spots[position].location.y,
scene.player_start_spots[position].location.z
])
circle = Circle((pixel[0], pixel[1]),
12,
color='r',
label='A point')
ax.add_patch(circle)
if not args.no_labels:
plt.text(pixel[0], pixel[1], str(position), size='x-small')
plt.axis('off')
plt.show()
fig.savefig('town_positions.pdf',
orientation='landscape',
bbox_inches='tight')
def __init__(self, city_name):
self._map = CarlaMap(city_name)
self._astar = AStar()
# Refers to the start position of the previous route computation
self._previous_node = []
# The current computed route
self._route = None
def __init__(self, carla_client, city_name=None):
self.client = carla_client
self._timer = None
self._display = None
self._main_image = None
self._mini_view_image1 = None
self._mini_view_image2 = None
self._map_view = None
self._is_on_reverse = False
self._city_name = city_name
self._map = CarlaMap(city_name) if city_name is not None else None
self._map_shape = self._map.map_image.shape if city_name is not None else None
self._map_view = self._map.get_map(WINDOW_HEIGHT) if city_name is not None else None
def __init__(self, carla_client, args):
self.client = carla_client
self._carla_settings = make_carla_settings(args)
self._timer = None
self._display = None
self._main_image = None
self._mini_view_image1 = None
self._mini_view_image2 = None
self._enable_autopilot = args.autopilot
self._lidar_measurement = None
self._map_view = None
self._is_on_reverse = False
self._city_name = args.map_name
self._map = CarlaMap(self._city_name, 0.1643,
50.0) if self._city_name is not None else None
self._map_shape = self._map.map_image.shape if self._city_name is not None else None
self._map_view = self._map.get_map(
WINDOW_HEIGHT) if self._city_name is not None else None
self._position = None
self._agent_positions = None
#Wheelchair addition
self._data_collection = False
self._time_stamp = float(0)
self._frame = 0
self._replay_frame = 1
self._AI_frame = 0
self._input_control = "Manual"
self._AI_steer = 0
self._Manual_steer = 0
self._player_start = args.start
self._ai_validation = args.checkai
#controller
self._xbox_cont = args.xboxcontroller
if args.xboxcontroller:
pygame.init()
pygame.joystick.init()
self._joystick = pygame.joystick.Joystick(0)
self._joystick.init()
self._takeovers = 0
self._distance = 0.01
#Real Time Display
self._realtimedisplay = args.realtime
self._rtdtoggle = False
self._rtddisplay = None
self.x_dim = 0
self.y_dim = 0
def __init__(self, city_name):
# These values are fixed for every city.
self._node_density = 50.0
self._pixel_density = 0.1643
self._map = CarlaMap(city_name, self._pixel_density, self._node_density)
self._astar = AStar()
# Refers to the start position of the previous route computation
self._previous_node = []
# The current computed route
self._route = None
def view_start_positions(args):
# We assume the CARLA server is already waiting for a client to connect at
# host:port. The same way as in the client example.
with make_carla_client(args.host, args.port) as client:
print('CarlaClient connected')
# We load the default settings to the client.
scene = client.load_settings(CarlaSettings())
print("Received the start positions")
try:
image = mpimg.imread('carla/planner/%s.png' % scene.map_name)
carla_map = CarlaMap(scene.map_name, 0.1653, 50)
except IOError as exception:
logging.error(exception)
logging.error('Cannot find map "%s"', scene.map_name)
sys.exit(1)
fig, ax = plt.subplots(1)
ax.imshow(image)
if args.positions == 'all':
positions_to_plot = range(len(scene.player_start_spots))
else:
positions_to_plot = map(int, args.positions.split(','))
for position in positions_to_plot:
# Check if position is valid
if position >= len(scene.player_start_spots):
raise RuntimeError('Selected position is invalid')
# Convert world to pixel coordinates
pixel = carla_map.convert_to_pixel([scene.player_start_spots[position].location.x,
scene.player_start_spots[position].location.y,
scene.player_start_spots[position].location.z])
circle = Circle((pixel[0], pixel[1]), 12, color='r', label='A point')
ax.add_patch(circle)
if not args.no_labels:
plt.text(pixel[0], pixel[1], str(position), size='x-small')
plt.axis('off')
plt.show()
fig.savefig('town_positions.pdf', orientation='landscape', bbox_inches='tight')
class MapHandler(object):
"""
Convert CarlaMap lane image to ROS OccupancyGrid message
"""
def __init__(self, map_name, topic='/map'):
self.map_name = map_name
self.carla_map = CarlaMap(map_name)
self.map_pub = rospy.Publisher(topic,
OccupancyGrid,
queue_size=10,
latch=True)
self.build_map_message()
def build_map_message(self):
self.map_msg = map_msg = OccupancyGrid()
# form array for map
map_img = self.carla_map.get_map_lanes()
# extract green channel, invert, scale to range 0..100, convert to int8
map_img = (100 - map_img[..., 1] * 100.0 / 255).astype(np.int8)
map_msg.data = map_img.ravel().tolist()
# set up general info
map_msg.info.resolution = self.carla_map._pixel_density
map_msg.info.width = map_img.shape[1]
map_msg.info.height = map_img.shape[0]
# set up origin orientation
quat = tf.transformations.quaternion_from_euler(0, 0, np.pi)
map_msg.info.origin.orientation.x = quat[0]
map_msg.info.origin.orientation.y = quat[1]
map_msg.info.origin.orientation.z = quat[2]
map_msg.info.origin.orientation.w = quat[3]
# set up origin position
top_right_corner = float(map_img.shape[1]), 0.0
to_world = self.carla_map.convert_to_world(top_right_corner)
map_msg.info.origin.position.x = to_world[0]
map_msg.info.origin.position.y = -to_world[1]
map_msg.info.origin.position.z = to_world[2]
# FIXME: height for Town01 is still in centimeters (issue #541)
if self.map_name == 'Town01':
map_msg.info.origin.position.z *= 100
def send_map(self):
self.map_pub.publish(self.map_msg)
def __init__(self, carla_client, args):
self.client = carla_client
self._carla_settings = make_carla_settings(args)
self._timer = None
self._display = None
self._main_image = None
self._mini_view_image2 = None
self._enable_autopilot = args.autopilot
self._lidar_measurement = None
self._map_view = None
self._is_on_reverse = False
self._city_name = args.map_name
self._map = CarlaMap(self._city_name, 0.1643, 50.0) if self._city_name is not None else None
self._map_shape = self._map.map_image.shape if self._city_name is not None else None
self._map_view = self._map.get_map(WINDOW_HEIGHT) if self._city_name is not None else None
self._position = None
self._agent_positions = None
def _initialize_game(self):
self._on_new_episode()
if self._city_name is not None:
self._map = CarlaMap(self._city_name, 0.1643, 50.0)
self._map_shape = self._map.map_image.shape
self._map_view = self._map.get_map(WINDOW_HEIGHT)
extra_width = int((WINDOW_HEIGHT/float(self._map_shape[0]))*self._map_shape[1])
self._display = pygame.display.set_mode(
(WINDOW_WIDTH + extra_width, WINDOW_HEIGHT),
pygame.HWSURFACE | pygame.DOUBLEBUF)
else:
self._display = pygame.display.set_mode(
(WINDOW_WIDTH, WINDOW_HEIGHT),
pygame.HWSURFACE | pygame.DOUBLEBUF)
logging.debug('pygame started')
def __init__(self, carla_client, enable_autopilot=False, enable_lidar=False, city_name=None):
self.client = carla_client
self._timer = None
self._display = None
self._main_image = None
self._mini_view_image1 = None
self._mini_view_image2 = None
self._enable_autopilot = enable_autopilot
self._enable_lidar = enable_lidar
self._lidar_measurement = None
self._map_view = None
self._is_on_reverse = False
self._city_name = city_name
self._map = CarlaMap(city_name, 16.43, 50.0) if city_name is not None else None
self._map_shape = self._map.map_image.shape if city_name is not None else None
self._map_view = self._map.get_map(WINDOW_HEIGHT) if city_name is not None else None
self._position = None
self._agent_positions = None
def __init__(self, carla_client, city_name=None, save_images_to_disk=False):
self.client = carla_client
self._timer = None
self._display = None
self._main_image = None
self._mini_view_image1 = None
self._mini_view_image2 = None
self._map_view = None
self._is_on_reverse = False
self._city_name = city_name
self._map = CarlaMap(city_name) if city_name is not None else None
self._map_shape = self._map.map_image.shape if city_name is not None else None
self._map_view = self._map.get_map(WINDOW_HEIGHT) if city_name is not None else None
self._save_images_to_disk = save_images_to_disk
self._image_filename_format='_images/episode_{:0>3d}/{:s}/image_{:0>5d}.png'
self._lidar_filename_format='_lidars/episode_{:0>3d}/{:s}/lidar_{:0>5d}.json'
self._episode = 0
self._frame = 0
def plot_position(scene, start, end):
try:
image = mpimg.imread('./environment/%s.png' % scene.map_name)
print("Plot")
carla_map = CarlaMap(scene.map_name, 0.1653, 50)
except IOError as exception:
logging.error(exception)
logging.error('Cannot find map "%s"', scene.map_name)
sys.exit(1)
_, ax = plt.subplots(1)
ax.imshow(image)
positions_to_plot = (start, end)
for position in positions_to_plot:
# Check if position is valid
if position >= len(scene.player_start_spots):
raise RuntimeError('Selected position is invalid')
# Convert world to pixel coordinates
pixel = carla_map.convert_to_pixel([
scene.player_start_spots[position].location.x,
scene.player_start_spots[position].location.y,
scene.player_start_spots[position].location.z
])
circle = Circle((pixel[0], pixel[1]),
12,
color='r',
label='A point')
ax.add_patch(circle)
plt.text(pixel[0], pixel[1], str(position), size='x-small')
plt.axis('off')
plt.show()
def view_start_positions(args):
# We assume the CARLA server is already waiting for a client to connect at
# host:port. The same way as in the client example.
with make_carla_client(args.host, args.port) as client:
print('CarlaClient connected')
# We load the default settings to the client.
scene = client.load_settings(CarlaSettings())
print("Received the start positions")
try:
image = mpimg.imread('carla/planner/%s.png' % scene.map_name)
carla_map = CarlaMap(scene.map_name, 0.1653, 50)
except IOError as exception:
logging.error(exception)
logging.error('Cannot find map "%s"', scene.map_name)
sys.exit(1)
fig, ax = plt.subplots(1)
plt.ion()
ax.imshow(image)
if args.positions == 'all':
positions_to_plot = range(len(scene.player_start_spots))
else:
positions_to_plot = map(int, args.positions.split(','))
while True:
measurements, sensor_data = client.read_data()
current_x = measurements.player_measurements.transform.location.x
current_y = measurements.player_measurements.transform.location.y
pixel = carla_map.convert_to_pixel([current_x,current_y,0])
circle = Circle((pixel[0], pixel[1]), 12, color='k', label='car')
ax.add_patch(circle)
plt.pause(0.05)
def __init__(self, carla_client, planner_obj, map_name, city_name,
save_data, is_noisy, collection_mode):
self.client = carla_client
self._timer = None
self._display = None
self._main_image = None
self._mini_view_image1 = None
self._mini_view_image2 = None
self._enable_lidar = False
self._lidar_measurement = None
self._map_view = None
self._is_on_reverse = False
self._city_name = city_name
self._map_name = map_name
self._save_data = save_data
self._map = CarlaMap(map_name, 16.43,
50.0) if map_name is not None else None
self._map_shape = self._map.map_image.shape if map_name is not None else None
# self._map_view = self._map.get_map(WINDOW_HEIGHT) if map_name is not None else None
self._position = None
self._agent_positions = None
self.planner_obj = planner_obj
self.is_noisy = is_noisy
self.collection_mode = collection_mode
def view_start_positions(args):
# We assume the CARLA server is already waiting for a client to connect at
# host:port. The same way as in the client example.
with make_carla_client(args.host, args.port) as client:
print('CarlaClient connected')
# We load the default settings to the client.
scene = client.load_settings(CarlaSettings())
print("Received the start positions")
try:
image = mpimg.imread('carla/planner/%s.png' % scene.map_name)
carla_map = CarlaMap(scene.map_name, 0.1653, 50)
except IOError as exception:
logging.error(exception)
logging.error('Cannot find map "%s"', scene.map_name)
sys.exit(1)
fig, ax = plt.subplots(1)
plt.ion()
ax.imshow(image)
if args.positions == 'all':
positions_to_plot = range(len(scene.player_start_spots))
else:
positions_to_plot = map(int, args.positions.split(','))
while True:
measurements, sensor_data = client.read_data()
current_x = measurements.player_measurements.transform.location.x
current_y = measurements.player_measurements.transform.location.y
pixel = carla_map.convert_to_pixel([current_x, current_y, 0])
circle = Circle((pixel[0], pixel[1]), 12, color='k', label='car')
ax.add_patch(circle)
plt.pause(0.05)
def __init__(self, carla_client, args): """ Initialize all of the parameters to keep track of the state of the CARLA simulation. Note most of the params here have to do with the map (hardcoded to be map01 up top). """ self.client = carla_client self._carla_settings = make_carla_settings() self._enable_autopilot = True self._is_on_reverse = False self._city_name = MAP_NAME self._position = None self._agent_positions = None self._map = CarlaMap(self._city_name, 0.1643, 50.0) if self._city_name is not None else None self.measurements = None self.enabled_commands = [] self.vehicle_distance_threshold = 30 self.traffic_light_distance_threshold = 30 self.pedestrian_distance_threshold = 10 self.speed_limit_distance_threshold = 20 self.lane_tolerance = 0.00002
def main(_):
global flags_obj
flags_obj = flags.FLAGS
#assert (flags_obj.map is not None), "Must provide the map path for LPE"
if flags_obj.dataset == 'CARLA':
from dataset import CarlaImageDataset as Dataset
labels = np.loadtxt(os.path.join(flags_obj.data_dir, 'label.txt'))
img_ids = np.expand_dims(labels[:, 0], 1)
labels = labels[:, 1:]
# transfer from cm to meter.
labels[:, [Dataset.POS_X, Dataset.POS_Y]] /= 100.0
# parse useful properties
labels = labels[:, [
Dataset.POS_X, Dataset.POS_Y, Dataset.ORIENT_X, Dataset.
ORIENT_Y, Dataset.ORIENT_Z
]]
labels = np.concatenate((img_ids, labels), axis=1)
print('=> using CARLA published data')
elif flags_obj.dataset == 'CARLA_SIM':
from dataset import CarlaSimDataset as Dataset
print('=> using self-collected CARLA data')
else:
print('=> using HUAWEI data, the function is built-in the dataset')
return
print(labels.shape, "first 10 labels", labels[:10])
# parse map for LPE
if flags_obj.dataset.startswith('CARLA'):
from carla.planner.map import CarlaMap
PIXEL_DENSITY = 0.1653
NODE_DENSITY = 50
carla_map = CarlaMap('Town01', PIXEL_DENSITY, NODE_DENSITY)
map_image = mpimg.imread(flags_obj.map)
parse_carla_dataset(labels, carla_map, map_image)
else:
pass
def start_game(args):
pygame.init()
with make_carla_client(args.host, args.port) as client:
settings = CarlaSettings()
settings.set(
SynchronousMode=False,
SendNonPlayerAgentsInfo=True,
NumberOfVehicles=3,
NumberOfPedestrians=40,
WeatherId=1,
QualityLevel="Low")
settings.randomize_seeds()
scene = client.load_settings(settings)
screen, img, dim, scale = setup_game_window(scene)
town_map = CarlaMap(scene.map_name, 0.1653, 50)
client.start_episode(7)
while running:
manage_events(pygame.event.get())
screen.blit(img, (0, 0))
play_frame(client, town_map, screen, dim, scale)
pygame.display.flip()
class CityTrack(object):
def __init__(self, city_name):
# These values are fixed for every city.
self._node_density = 50.0
self._pixel_density = 0.1643
self._map = CarlaMap(city_name, self._pixel_density,
self._node_density)
self._astar = AStar()
# Refers to the start position of the previous route computation
self._previous_node = []
# The current computed route
self._route = None
def project_node(self, position):
"""
Projecting the graph node into the city road
"""
node = self._map.convert_to_node(position)
# To change the orientation with respect to the map standards
node = tuple([int(x) for x in node])
# Set to zero if it is less than zero.
node = (max(0, node[0]), max(0, node[1]))
node = (min(self._map.get_graph_resolution()[0] - 1, node[0]),
min(self._map.get_graph_resolution()[1] - 1, node[1]))
node = self._map.search_on_grid(node)
return node
def get_intersection_nodes(self):
return self._map.get_intersection_nodes()
def get_pixel_density(self):
return self._pixel_density
def get_node_density(self):
return self._node_density
def is_at_goal(self, source, target):
return source == target
def is_at_new_node(self, current_node):
return current_node != self._previous_node
def is_away_from_intersection(self, current_node):
return self._closest_intersection_position(current_node) > 1
def is_far_away_from_route_intersection(self, current_node):
# CHECK FOR THE EMPTY CASE
if self._route is None:
raise RuntimeError(
'Impossible to find route' + ' Current planner is limited' +
' Try to select start points away from intersections')
return self._closest_intersection_route_position(
current_node, self._route) > 4
def compute_route(self, node_source, source_ori, node_target, target_ori):
self._previous_node = node_source
a_star = AStar()
a_star.init_grid(
self._map.get_graph_resolution()[0],
self._map.get_graph_resolution()[1],
self._map.get_walls_directed(node_source, source_ori, node_target,
target_ori), node_source, node_target)
route = a_star.solve()
# JuSt a Corner Case
# Clean this to avoid having to use this function
if route is None:
a_star = AStar()
a_star.init_grid(self._map.get_graph_resolution()[0],
self._map.get_graph_resolution()[1],
self._map.get_walls(), node_source, node_target)
route = a_star.solve()
self._route = route
return route
def get_distance_closest_node_route(self, pos, route):
distance = []
for node_iter in route:
if node_iter in self._map.get_intersection_nodes():
distance.append(sldist(node_iter, pos))
if not distance:
return sldist(route[-1], pos)
return sorted(distance)[0]
def _closest_intersection_position(self, current_node):
distance_vector = []
for node_iterator in self._map.get_intersection_nodes():
distance_vector.append(sldist(node_iterator, current_node))
return sorted(distance_vector)[0]
def _closest_intersection_route_position(self, current_node, route):
distance_vector = []
for _ in route:
for node_iterator in self._map.get_intersection_nodes():
distance_vector.append(sldist(node_iterator, current_node))
return sorted(distance_vector)[0]
class CityTrack(object):
def __init__(self, city_name):
# These values are fixed for every city.
self._node_density = 50.0
self._pixel_density = 0.1643
self._map = CarlaMap(city_name, self._pixel_density, self._node_density)
self._astar = AStar()
# Refers to the start position of the previous route computation
self._previous_node = []
# The current computed route
self._route = None
def project_node(self, position):
"""
Projecting the graph node into the city road
"""
node = self._map.convert_to_node(position)
# To change the orientation with respect to the map standards
node = tuple([int(x) for x in node])
# Set to zero if it is less than zero.
node = (max(0, node[0]), max(0, node[1]))
node = (min(self._map.get_graph_resolution()[0] - 1, node[0]),
min(self._map.get_graph_resolution()[1] - 1, node[1]))
node = self._map.search_on_grid(node)
return node
def get_intersection_nodes(self):
return self._map.get_intersection_nodes()
def get_pixel_density(self):
return self._pixel_density
def get_node_density(self):
return self._node_density
def is_at_goal(self, source, target):
return source == target
def is_at_new_node(self, current_node):
return current_node != self._previous_node
def is_away_from_intersection(self, current_node):
return self._closest_intersection_position(current_node) > 1
def is_far_away_from_route_intersection(self, current_node):
# CHECK FOR THE EMPTY CASE
if self._route is None:
raise RuntimeError('Impossible to find route'
+ ' Current planner is limited'
+ ' Try to select start points away from intersections')
return self._closest_intersection_route_position(current_node,
self._route) > 4
def compute_route(self, node_source, source_ori, node_target, target_ori):
self._previous_node = node_source
a_star = AStar()
a_star.init_grid(self._map.get_graph_resolution()[0],
self._map.get_graph_resolution()[1],
self._map.get_walls_directed(node_source, source_ori,
node_target, target_ori), node_source,
node_target)
route = a_star.solve()
# JuSt a Corner Case
# Clean this to avoid having to use this function
if route is None:
a_star = AStar()
a_star.init_grid(self._map.get_graph_resolution()[0],
self._map.get_graph_resolution()[1], self._map.get_walls(),
node_source, node_target)
route = a_star.solve()
self._route = route
return route
def get_distance_closest_node_route(self, pos, route):
distance = []
for node_iter in route:
if node_iter in self._map.get_intersection_nodes():
distance.append(sldist(node_iter, pos))
if not distance:
return sldist(route[-1], pos)
return sorted(distance)[0]
def _closest_intersection_position(self, current_node):
distance_vector = []
for node_iterator in self._map.get_intersection_nodes():
distance_vector.append(sldist(node_iterator, current_node))
return sorted(distance_vector)[0]
def _closest_intersection_route_position(self, current_node, route):
distance_vector = []
for _ in route:
for node_iterator in self._map.get_intersection_nodes():
distance_vector.append(sldist(node_iterator, current_node))
return sorted(distance_vector)[0]
