def draw(self, world: carla.World):
if self.yaw_agnostic:
end = utils.clone_location(self.transform.location)
end.z += 1.5
world.debug.draw_arrow(
begin=self.transform.location,
end=end,
thickness=0.2,
arrow_size=2,
life_time=0,
color=carla.Color(r=20, g=60, b=160),
)
else:
world.debug.draw_arrow(
begin=self.transform.location,
end=self.transform.location +
self.transform.get_forward_vector(),
thickness=0.1,
arrow_size=0.1,
life_time=0,
color=carla.Color(r=20, g=200, b=100),
)
world.debug.draw_string(self.transform.location,
text=str(self.id),
color=carla.Color(255, 255, 255),
life_time=0)
def tick(self, clock: pg.time.Clock):
lifetime = 5
now = self._timestamp_now_ms()
if now > self._last_update_time + (lifetime * 1000) - 50:
color = carla.Color(0, 0, 200, 0)
self._last_update_time = now
# draw_bbox(self.debug, veh)
# self._draw_position_vray(lifetime=lifetime)
if self._state_updater.goal:
g = self._state_updater.goal.pose
from_loc = carla.Location(g.position.x, -g.position.y,
g.position.z)
to_loc = carla.Location(from_loc.x, from_loc.y,
from_loc.z + 100)
self._debug_helper.draw_line(from_loc,
to_loc,
thickness=1.0,
color=carla.Color(40, 255, 0),
life_time=lifetime)
path = self._state_updater.path
for idx, wp in enumerate(path):
if idx % 10 == 0:
self._draw_waypoint(wp, color, lifetime=lifetime)
def get_color_validity(waypoint_transform, scenario_transform, scenario_type,
scenario_index, debug):
"""
Uses the same condition as in route_scenario to see if they will
be differentiated
"""
TRIGGER_THRESHOLD = 1.4 # Routes use 1.5 (+ an error margin)
TRIGGER_ANGLE_THRESHOLD = 10
dx = float(waypoint_transform.location.x) - scenario_transform.location.x
dy = float(waypoint_transform.location.y) - scenario_transform.location.y
dz = float(waypoint_transform.location.z) - scenario_transform.location.z
dpos = math.sqrt(dx * dx + dy * dy + dz * dz)
dyaw = (float(waypoint_transform.rotation.yaw) -
scenario_transform.rotation.yaw) % 360
if dpos > TRIGGER_THRESHOLD:
if not debug:
print("WARNING: Found a scenario with the wrong position "
"(Type: {}, Index: {})".format(scenario_type,
scenario_index))
return carla.Color(255, 0, 0)
if dyaw > TRIGGER_ANGLE_THRESHOLD and dyaw < (360 -
TRIGGER_ANGLE_THRESHOLD):
if not debug:
print("WARNING: Found a scenario with the wrong orientation "
"(Type: {}, Index: {})".format(scenario_type,
scenario_index))
return carla.Color(0, 0, 255)
return carla.Color(0, 255, 0)
def draw_shortest_path(world, planner, origin, destination):
"""Draws shortest feasible lines/arrows from origin to destination
Args:
world:
planner:
origin (tuple): (x, y, z)
destination (tuple): (x, y, z)
Returns:
next waypoint as a list of coordinates (x,y,z)
"""
hops = get_shortest_path_waypoints(world, planner, origin, destination)
for i in range(1, len(hops)):
hop1 = hops[i - 1][0].transform.location
hop2 = hops[i][0].transform.location
hop1.z = origin[2]
hop2.z = origin[2]
if i == len(hops) - 1:
world.debug.draw_arrow(hop1,
hop2,
life_time=1.0,
color=carla.Color(0, 255, 0),
thickness=0.5)
else:
world.debug.draw_line(hop1,
hop2,
life_time=1.0,
color=carla.Color(0, 255, 0),
thickness=0.5)
def draw_shortest_path_old(world, planner, origin, destination):
"""Draws shortest feasible lines/arrows from origin to destination
Args:
world:
planner:
origin (typle): (x, y, z)
destination:
Returns:
"""
xys = get_shortest_path_waypoints(planner, (origin[0], origin[1]),
destination)
if len(xys) > 2:
for i in range(len(xys) - 2):
world.debug.draw_line(carla.Location(*xys[i]),
carla.Location(*xys[i + 1]),
life_time=1.0,
color=carla.Color(0, 255, 0))
elif len(xys) == 2:
world.debug.draw_arrow(carla.Location(*xys[-2]),
carla.Location(*xys[-1]),
life_time=100.0,
color=carla.Color(0, 255, 0),
thickness=0.5)
def __init__(self, start_position, end_position, the_map, world):
# Create start and end node
self.the_map = the_map
self.world = world
self.start_node = Node()
self.end_node = Node()
self.initialization_star_end_positions(start_position, end_position,
the_map)
self.travel_step = SEARCH_STEP
# Initialize both open and closed list
self.open_list = []
self.closed_list = []
# Add the start node
self.open_list.append(self.start_node)
self.initial_path = []
self.smoothed_path = []
self.world.debug.draw_string(carla.Location(
x=self.start_node.position.x, y=self.start_node.position.y),
"<---- Start",
draw_shadow=False,
color=carla.Color(r=255, g=200, b=0),
life_time=40,
persistent_lines=True)
self.world.debug.draw_string(carla.Location(
x=self.end_node.position.x, y=self.end_node.position.y),
"<---- End",
draw_shadow=False,
color=carla.Color(r=255, g=200, b=0),
life_time=40,
persistent_lines=True)
def highlight_display(self, tfl_violators, lvd_pair):
# highlight traffic light violators
debug = self.world.debug
for id in tfl_violators:
actor = self.world.get_actor(id)
color = carla.Color(255, 0, 0)
lt = 1 # life-time
debug.draw_point(actor.get_location() + carla.Location(z=0.25),
0.1, color, lt, False)
if len(lvd_pair):
# highlight lvd pairs
leader = self.world.get_actor(lvd_pair[0])
follower = self.world.get_actor(lvd_pair[1])
color = carla.Color(0, 0, 255)
lt = 1 # life-time
debug.draw_arrow(
follower.get_location() + carla.Location(z=0.25),
leader.get_location() + carla.Location(z=0.25),
thickness=2,
arrow_size=2,
color=color,
life_time=lt,
)
def draw_arrow_ext(debug, src):
dest_x = src + carla.Location(x=2)
dest_y = src + carla.Location(y=2)
dest_z = src + carla.Location(z=2)
debug.draw_arrow(src, dest_x, color=carla.Color(255, 0, 0), life_time=0)
debug.draw_arrow(src, dest_y, color=carla.Color(0, 255, 0), life_time=0)
debug.draw_arrow(src, dest_z, color=carla.Color(0, 0, 255), life_time=0)
def main():
client = carla.Client('localhost', 2000)
client.set_timeout(10.0) # seconds
world = client.get_world()
the_map = world.get_map()
start = (10, -200)
end = (-10, 130)
start = (170, -200)
end = (random.randint(-300, 300), random.randint(30, 300))
a_star_algorithm = AStar(start, end, the_map, world)
smoothed_path = a_star_algorithm.find_path()
if smoothed_path is not None:
if True:
for p in smoothed_path:
sleep(0.001)
world.debug.draw_string(carla.Location(x=p.x, y=p.y),
"x",
draw_shadow=False,
color=carla.Color(r=20, g=200, b=220),
life_time=15,
persistent_lines=True)
else:
print("Path not found")
world.debug.draw_string(carla.Location(x=start[1], y=start[1]),
"Path not found",
draw_shadow=False,
color=carla.Color(r=20, g=200, b=20),
life_time=5,
persistent_lines=True)
def draw_waypoints(self, distance=3, all_lines=False):
"""
show the waypoints on the map
:return:
"""
waypoints = self.map.generate_waypoints(distance)
line1, line2 = list(), list()
for idx, w in enumerate(waypoints):
if idx % 2 == 0:
line1.append(w)
else:
line2.append(w)
if all_lines:
for w in waypoints:
self.world.debug.draw_string(w.transform.location,
'O',
draw_shadow=False,
color=carla.Color(r=255, g=0,
b=0),
life_time=10.0,
persistent_lines=True)
else:
for w in line1:
self.world.debug.draw_string(w.transform.location,
'O',
draw_shadow=False,
color=carla.Color(r=255, g=0,
b=0),
life_time=10.0,
persistent_lines=True)
def process_img(im_rgb, im_ss, text, actors_present, frame, SAVE_FLAG,
DISP_FLAG):
if SAVE_FLAG == 1:
#for bounding boxes
debug = world.debug
for actor in actors_present:
debug.draw_box(box=carla.BoundingBox(
actor.get_transform().location, actor.bounding_box.extent),
rotation=actor.get_transform().rotation,
thickness=0.1,
color=carla.Color(255, 0, 0, 0),
life_time=0.1)
im_rgb.save_to_disk("data\{}_rgb.jpg".format(frame))
im_ss.save_to_disk("data\{}_semseg.jpg".format(frame))
if DISP_FLAG == 1:
i = np.frombuffer(im_rgb.raw_data, dtype=np.dtype("uint8"))
i2 = im_rgb.reshape((IM_WIDTH, IM_HEIGHT, 4))
cv2.putText(i2, text, bottomLeftCornerOfText, font, fontScale,
(255, 255, 255), lineType, cv2.LINE_AA)
#for bounding boxes
debug = world.debug
for actor in actors_present:
debug.draw_box(box=carla.BoundingBox(
actor.get_transform().location, actor.bounding_box.extent),
rotation=actor.get_transform().rotation,
thickness=0.1,
color=carla.Color(255, 0, 0, 0),
life_time=0.1)
cv2.imshow("", i2)
cv2.waitKey(1)
return
def getWaypoint(self):
if len(self.waypoint_list) != 0:
location = carla.Location(self.waypoint_list[0][0],
self.waypoint_list[0][1],
self.waypoint_list[0][2])
rotation = self.map.get_waypoint(
location,
project_to_road=True,
lane_type=carla.LaneType.Driving).transform.rotation
box = carla.BoundingBox(location, carla.Vector3D(0, 6, 3))
if len(self.waypoint_list) == 1:
self.world.debug.draw_box(box,
rotation,
thickness=0.5,
color=carla.Color(255, 255, 0, 255),
life_time=0)
else:
self.world.debug.draw_box(box,
rotation,
thickness=0.5,
color=carla.Color(0, 255, 0, 255),
life_time=2)
return self.waypoint_list[0]
else:
return None
def _draw_path(self, life_time=60.0, skip=0):
"""
Draw a connected path from start of route to end.
Green node = start
Red node = point along path
Blue node = destination
"""
for i in range(0, len(self.route_waypoints) - 1, skip + 1):
w0 = self.route_waypoints[i][0]
w1 = self.route_waypoints[i + 1][0]
self.world.debug.draw_line(
w0.transform.location + carla.Location(z=0.25),
w1.transform.location + carla.Location(z=0.25),
thickness=0.1,
color=carla.Color(255, 0, 0),
life_time=life_time,
persistent_lines=False)
self.world.debug.draw_point(
w0.transform.location + carla.Location(z=0.25), 0.1,
carla.Color(0, 255, 0) if i == 0 else carla.Color(255, 0, 0),
life_time, False)
self.world.debug.draw_point(
self.route_waypoints[-1][0].transform.location +
carla.Location(z=0.25), 0.1, carla.Color(0, 0,
255), life_time, False)
def visualize_route(self, dangerous, route):
if not route:
return
right_lane_edges = dict()
left_lane_edges = dict()
for road_segment in route.roadSegments:
right_most_lane = road_segment.drivableLaneSegments[0]
if right_most_lane.laneInterval.laneId not in right_lane_edges:
edge = ad.map.route.getRightProjectedENUEdge(
right_most_lane.laneInterval)
right_lane_edges[right_most_lane.laneInterval.laneId] = edge
intersection_lane = ad.map.intersection.Intersection.isLanePartOfAnIntersection(
right_most_lane.laneInterval.laneId)
color = carla.Color(r=(128 if dangerous else 255))
if intersection_lane:
color.b = 128 if dangerous else 255
color = carla.Color(r=255, g=0, b=255)
self.visualize_enu_edge(edge, color,
self._player.get_location().z)
left_most_lane = road_segment.drivableLaneSegments[-1]
if left_most_lane.laneInterval.laneId not in left_lane_edges:
edge = ad.map.route.getLeftProjectedENUEdge(
left_most_lane.laneInterval)
left_lane_edges[left_most_lane.laneInterval.laneId] = edge
intersection_lane = ad.map.intersection.Intersection.isLanePartOfAnIntersection(
left_most_lane.laneInterval.laneId)
color = carla.Color(g=(128 if dangerous else 255))
if intersection_lane:
color.b = 128 if dangerous else 255
self.visualize_enu_edge(edge, color,
self._player.get_location().z)
def _build_scenario_instances(self, world, ego_vehicle, scenario_definitions, town,
scenarios_per_tick=5, timeout=300, debug_mode=False):
"""
Based on the parsed route and possible scenarios, build all the scenario classes.
"""
scenario_instance_vec = []
if debug_mode:
for scenario in scenario_definitions:
loc = carla.Location(scenario['trigger_position']['x'],
scenario['trigger_position']['y'],
scenario['trigger_position']['z']) + carla.Location(z=2.0)
world.debug.draw_point(loc, size=1.0, color=carla.Color(255, 0, 0), life_time=100000)
world.debug.draw_string(loc, str(scenario['name']), draw_shadow=False,
color=carla.Color(0, 0, 255), life_time=100000, persistent_lines=True)
for scenario_number, definition in enumerate(scenario_definitions):
# Get the class possibilities for this scenario number
scenario_class = NUMBER_CLASS_TRANSLATION[definition['name']]
# Create the other actors that are going to appear
if definition['other_actors'] is not None:
list_of_actor_conf_instances = self._get_actors_instances(definition['other_actors'])
else:
list_of_actor_conf_instances = []
# Create an actor configuration for the ego-vehicle trigger position
egoactor_trigger_position = convert_json_to_transform(definition['trigger_position'])
scenario_configuration = ScenarioConfiguration()
scenario_configuration.other_actors = list_of_actor_conf_instances
scenario_configuration.town = town
scenario_configuration.trigger_points = [egoactor_trigger_position]
scenario_configuration.subtype = definition['scenario_type']
scenario_configuration.ego_vehicles = [ActorConfigurationData('vehicle.lincoln.mkz2017',
ego_vehicle.get_transform(),
'hero')]
route_var_name = "ScenarioRouteNumber{}".format(scenario_number)
scenario_configuration.route_var_name = route_var_name
try:
scenario_instance = scenario_class(world, [ego_vehicle], scenario_configuration,
criteria_enable=False, timeout=timeout)
# Do a tick every once in a while to avoid spawning everything at the same time
if scenario_number % scenarios_per_tick == 0:
sync_mode = world.get_settings().synchronous_mode
if sync_mode:
world.tick()
else:
world.wait_for_tick()
scenario_number += 1
except Exception as e: # pylint: disable=broad-except
if debug_mode:
traceback.print_exc()
print("Skipping scenario '{}' due to setup error: {}".format(definition['name'], e))
continue
scenario_instance_vec.append(scenario_instance)
return scenario_instance_vec
def main():
client = carla.Client('localhost', 2000)
client.set_timeout(10)
world = client.get_world()
map = world.get_map()
vehicle = world.get_actor(87)
location1 = map.get_waypoint(vehicle.get_location())
print(location1)
#waypoint1 = vehicle.get_location()
#custom_controller = VehiclePIDController(vehicle, args_lateral = {'K_P': 1, 'K_D': 0.0, 'K_I': 0}, args_longitudinal = {'K_P': 1, 'K_D': 0.0, 'K_I': 0.0})
vehicle2 = world.get_actor(86)
vehicle2.set_simulate_physics(True)
location2 = map.get_waypoint(vehicle2.get_location())
print(location2)
#waypoint2 = vehicle.get_location()
#vehicle2.set_transform(waypoint.transform)
#control_signal = custom_controller.run_step(1, waypoint)
#vehicle.apply_control(control_signal)
#Routeplanner
amap = world.get_map()
sampling_resolution = 2
dao = GlobalRoutePlannerDAO(amap, sampling_resolution)
grp = GlobalRoutePlanner(dao)
grp.setup()
spawn_points = world.get_map().get_spawn_points()
spawn_point = spawn_points[50]
vehicle2.set_transform(spawn_point)
spawn_point2 = spawn_points[100]
vehicle.set_transform(spawn_point2)
a = carla.Location(spawn_points[50].location)
b = carla.Location(spawn_points[100].location)
w1 = grp.trace_route(a, b)
i = 0
for w in w1:
if i % 10 == 0:
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)
else:
world.debug.draw_string(w[0].transform.location,
'O',
draw_shadow=False,
color=carla.Color(r=0, g=0, b=255),
life_time=1000.0,
persistent_lines=True)
i += 1
def restart(self):
# Keep same camera config if the camera manager exists.
cam_index = self.camera_manager.index if self.camera_manager is not None else 0
cam_pos_index = self.camera_manager.transform_index if self.camera_manager is not None else 0
# Get a random blueprint.
# blueprint = random.choice(self.world.get_blueprint_library().filter(self._actor_filter))
blueprint = random.choice(
self.world.get_blueprint_library().filter("vehicle.*"))
blueprint.set_attribute('role_name', self.actor_role_name)
if blueprint.has_attribute('color'):
color = random.choice(
blueprint.get_attribute('color').recommended_values)
blueprint.set_attribute('color', color)
if blueprint.has_attribute('driver_id'):
driver_id = random.choice(
blueprint.get_attribute('driver_id').recommended_values)
blueprint.set_attribute('driver_id', driver_id)
if blueprint.has_attribute('is_invincible'):
blueprint.set_attribute('is_invincible', 'true')
# Spawn the player.
if self.player is not None:
spawn_point = self.player.get_transform()
spawn_point.location.z += 2.0
spawn_point.rotation.roll = 0.0
spawn_point.rotation.pitch = 0.0
self.destroy()
self.player = self.world.try_spawn_actor(blueprint, spawn_point)
while self.player is None:
spawn_points = self.map.get_spawn_points()
# spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform()
spawn_point = carla.Transform(
location=carla.Location(x=-88.646866, y=-103.266113, z=0),
rotation=carla.Rotation(pitch=-0.471802,
yaw=89.843742,
roll=0.000000))
print("spawn_point", spawn_point)
self.world.debug.draw_string(
spawn_point.location,
'OOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOO',
draw_shadow=True,
color=carla.Color(r=255, g=0, b=0),
life_time=120000.0,
persistent_lines=True)
self.world.debug.draw_point(spawn_point.location, 30,
carla.Color(255, 0, 0), 120000.0)
# end_point = carla.Transform(location=carla.Location(x=-88.646866, y=-120.266113, z=0), rotation=carla.Rotation(pitch=-0.471802, yaw=89.843742, roll=0.000000))
# self.world.debug.draw_arrow(spawn_point.location, end_point.location, 12.0, 8.0, carla.Color(255,0,0), -120000)
self.player = self.world.try_spawn_actor(blueprint, spawn_point)
# Set up the sensors.
self.collision_sensor = CollisionSensor(self.player, self.hud)
self.lane_invasion_sensor = LaneInvasionSensor(self.player, self.hud)
self.gnss_sensor = GnssSensor(self.player)
self.camera_manager = CameraManager(self.player, self.hud, self._gamma)
self.camera_manager.transform_index = cam_pos_index
self.camera_manager.set_sensor(cam_index, notify=False)
actor_type = get_actor_display_name(self.player)
self.hud.notification(actor_type)
def draw_target_and_source(self, with_arrow=True):
self.draw_box_with_arrow(self.source,
carla.Color(r=0, b=255, g=0, a=255),
'Source',
with_arrow=with_arrow)
self.draw_box_with_arrow(self.target,
carla.Color(r=0, b=0, g=255, a=255),
'Target',
with_arrow=with_arrow)
def draw_on_world(self, world):
import carla
for marking in self.left_markings:
world.debug.draw_point(marking.as_carla_location(),
size=0.1,
color=carla.Color(255, 255, 0))
for marking in self.right_markings:
world.debug.draw_point(marking.as_carla_location(),
size=0.1,
color=carla.Color(255, 255, 0))
def __init__(self, name, flags, log_file_name=None):
""" Initializes the WaypointVisualizerOperator with the given
parameters.
Args:
name: The name of the operator.
flags: A handle to the global flags instance to retrieve the
configuration.
log_file_name: The file to log the required information to.
"""
super(WaypointVisualizerOperator, self).__init__(name)
self._logger = setup_logging(self.name, log_file_name)
self._flags = flags
_, self._world = pylot.simulation.carla_utils.get_world(
self._flags.carla_host, self._flags.carla_port,
self._flags.carla_timeout)
if self._world is None:
raise ValueError("Error connecting to the simulator.")
self._colors = [
carla.Color(255, 0, 0),
carla.Color(0, 255, 0),
carla.Color(0, 0, 255),
carla.Color(128, 128, 0),
carla.Color(0, 128, 128),
carla.Color(128, 0, 128),
carla.Color(64, 64, 0),
carla.Color(64, 0, 64),
carla.Color(0, 64, 64)
]
def _frameUpdate(self):
# vel = self._getCarForwardVelocity()
trns = self.carla_vehicle.get_transform().location
rotsV = self.carla_vehicle.get_transform().rotation.get_forward_vector(
)
self.vehiclePose.translation = Translation2d(trns.x, trns.y)
self.vehiclePose.rotation = Rotation2d(rotsV.x, rotsV.y)
debug = False
dbg = self.carla_world.debug
for i in self.waypointList:
loc = carla.Location(i.x, i.y, 0.5)
if debug:
dbg.draw_point(loc, 0.1, carla.Color(0, 255, 0), 0.02)
pointPt = self.vehiclePose.translation + Translation2d(
5, 0).rotateByOrigin(self.vehiclePose.rotation)
loc = carla.Location(pointPt.x, pointPt.y, 0.5)
if debug:
dbg.draw_point(loc, 0.1, carla.Color(255, 0, 0), 0.01)
if len(self.waypointList) > 0:
loc = carla.Location(self.waypointList[0].x,
self.waypointList[0].y, 0.51)
if debug: dbg.draw_point(loc, 0.2, carla.Color(0, 0, 255), 0.01)
if self.drivingBehavior == AgentDrivingBehavior.FOLLOW_WAYPOINTS:
if self.collisionDetection:
a = self._getAnyCollisions()
if a is not None:
print(f"Solving for collision with {a.ssid}")
self._solveForProfileToAvoidCollision(a)
self.velocityReference = self.waypointFollowSpeed
self.angularVelocityReference = self._purePursuitAngleToAngularVelocity(
)
print(f"AM {self.name}, GOING {self.velocityReference}")
elif self.drivingBehavior == AgentDrivingBehavior.MAINTAIN_DISTANCE:
self.velocityReference = self._runFollowPDLoop()
self.angularVelocityReference = self._purePursuitAngleToAngularVelocity(
)
elif self.drivingBehavior == AgentDrivingBehavior.MERGING:
self.mergeStartTime += 0.01
if self.mergeStartTime >= self.mergeDwell:
self.drivingBehavior = AgentDrivingBehavior.MAINTAIN_DISTANCE
self.waypointList = deepcopy(
MeshNode.call(self.followTarget.port,
Request("get_waypoints")).response)
self.velocityReference = self._runFollowPDLoop()
self.angularVelocityReference = self._purePursuitAngleToAngularVelocity(
)
elif self.drivingBehavior == AgentDrivingBehavior.WAITING:
self.velocityReference = 0
self.angularVelocityReference = 0
else:
return
self.drive_vehicle()
def save_lidar_image(image, world, vehicle):
global waypoint
#Convert raw data to coordinates (x,y,z)
print("SCAN:")
points = np.frombuffer(image.raw_data, dtype=np.dtype('f4'))
print(image.horizontal_angle)
print(points.shape)
points = np.reshape(points, (int(points.shape[0] / 3), 3))
#reshape from list of 4d points to an array of 3d points
transform = vehicle.get_transform()
transform = [
transform.location.x, transform.location.y, transform.location.z
]
#Rotate into the car's frame
vehicle_rotation = vehicle.get_transform().rotation
roll = vehicle_rotation.roll
pitch = vehicle_rotation.pitch
yaw = vehicle_rotation.yaw + (np.pi / 2)
R = transforms3d.euler.euler2mat(roll, pitch, yaw).T
points = points.copy()
points[:] = [np.dot(R, point) for point in points]
points[:] = [np.add(transform, point)
for point in points] #Move location into car's frame
left_cluster, right_cluster, left_center, right_center = filter_scan(
points)
left = carla.Location(x=float(left_center[0]),
y=float(left_center[1]),
z=float(0))
world.debug.draw_point(left, life_time=1, color=carla.Color(0, 255, 255))
right = carla.Location(x=float(right_center[0]),
y=float(right_center[1]),
z=float(0))
world.debug.draw_point(left, life_time=1, color=carla.Color(0, 255, 255))
left_front = get_front_points(left_cluster,
vehicle.get_transform().location)
right_front = get_front_points(right_cluster,
vehicle.get_transform().location)
waypoint = waypoint_from_centers(get_center_cluster(left_front),
get_center_cluster(right_front))
left_location = carla.Location(x=float(waypoint[0]),
y=float(waypoint[1]),
z=float(0))
world.debug.draw_point(left_location,
life_time=1,
color=carla.Color(0, 255, 255))
def create_scenarios(world, town_data, tmap, scenarios, debug):
"""
Creates new S7 to S10 by moving 5 meters backwards from an already existing S4.
They have to be manually added to the json file but information is given to simplify that process
"""
for scenario_data in town_data:
# Get the desired scenario data
scenario_type = scenario_data["scenario_type"]
if scenario_type not in scenarios:
continue
number = float(scenario_type[8:])
debug_loc_height = carla.Location(z=DEBUG_HEIGHT_INTERVAL * number)
debug_str_height = carla.Location(z=DEBUG_HEIGHT_INTERVAL * number +
0.1)
color = SCENARIO_COLOR[scenario_type][0]
scenario_list = scenario_data["available_event_configurations"]
for i in range(len(scenario_list)):
# Get the individual scenario data
scenario_dict = scenario_list[i]
scenario_transform = get_scenario_transform(scenario_dict)
scenario_location = scenario_transform.location
world.debug.draw_point(scenario_location + debug_loc_height,
float(0.15), color)
world.debug.draw_string(scenario_location + debug_str_height,
str(i), False, carla.Color(0, 0, 0), 1000)
# Get the new scenario data
scenario_wp = tmap.get_waypoint(scenario_location)
new_transform = scenario_wp.previous(5)[0].transform
new_location = new_transform.location
world.debug.draw_point(new_location + debug_loc_height,
float(0.15), carla.Color(0, 0, 0))
world.debug.draw_string(new_location + debug_str_height, str(i),
False, carla.Color(0, 0, 0), 1000)
if debug:
spectator = world.get_spectator()
spectator.set_transform(
carla.Transform(new_location + carla.Location(z=50),
carla.Rotation(pitch=-90)))
input(
" New Scenario [{}/{}] at (x={}, y={}, z={}, yaw={}). Press Enter to continue"
.format(i,
len(scenario_list) - 1, round(new_location.x, 1),
round(new_location.y, 1), round(new_location.z, 1),
round(new_transform.rotation.yaw, 1)))
world.wait_for_tick()
def debug_display(self):
display_time = 40.0
for loc in self.controlled_junction_locations:
self.carla_world.debug.draw_string(
carlautil.ndarray_to_location(loc) + carla.Location(z=3.0),
'o',
color=carla.Color(r=255, g=0, b=0, a=100),
life_time=display_time)
for loc in self.uncontrolled_junction_locations:
self.carla_world.debug.draw_string(
carlautil.ndarray_to_location(loc) + carla.Location(z=3.0),
'o',
color=carla.Color(r=0, g=255, b=0, a=100),
life_time=display_time)
def _draw_waypoints(self, world, waypoints, vertical_shift, persistency=-1):
"""
Draw a list of waypoints at a certain height given in vertical_shift.
"""
for w in waypoints:
wp = w[0].location + carla.Location(z=vertical_shift)
size = 0.2
if w[1] == RoadOption.LEFT: # Yellow
color = carla.Color(255, 255, 0)
elif w[1] == RoadOption.RIGHT: # Cyan
color = carla.Color(0, 255, 255)
elif w[1] == RoadOption.CHANGELANELEFT: # Orange
color = carla.Color(255, 64, 0)
elif w[1] == RoadOption.CHANGELANERIGHT: # Dark Cyan
color = carla.Color(0, 64, 255)
elif w[1] == RoadOption.STRAIGHT: # Gray
color = carla.Color(128, 128, 128)
else: # LANEFOLLOW
color = carla.Color(0, 255, 0) # Green
size = 0.1
world.debug.draw_point(wp, size=size, color=color, life_time=persistency)
world.debug.draw_point(waypoints[0][0].location + carla.Location(z=vertical_shift), size=0.2,
color=carla.Color(0, 0, 255), life_time=persistency)
world.debug.draw_point(waypoints[-1][0].location + carla.Location(z=vertical_shift), size=0.2,
color=carla.Color(255, 0, 0), life_time=persistency)
def parse_lidar(self):
lidar = self.knowledge.get_lidar()
if lidar is not None:
bounding_box = self.knowledge.get_bounding_box()
location = self.knowledge.get_location()
lidar_location = self.knowledge.get_lidar_location()
sensor_position = carla.Location(location.x + lidar_location.x,
location.y + lidar_location.y,
lidar_location.z + location.z)
if self.knowledge.DEBUG:
self.knowledge.get_world().debug.draw_point(sensor_position,
color=carla.Color(
r=255,
g=0,
b=0),
life_time=1.0)
detections = []
if not lidar == None:
for point in lidar:
relative_location = carla.Location(
location.x + point.x, location.y + point.y,
1.5 + bounding_box.extent.z + location.z +
point.z) # 3 => Lidar height
valid_x = relative_location.x > bounding_box.extent.x and relative_location.x < -bounding_box.extent.x
valid_y = relative_location.y > bounding_box.extent.y and relative_location.y < -bounding_box.extent.y
valid_z = relative_location.z > 0.5
# Add only valid points
if valid_x and valid_y and valid_z and self.knowledge.distance(
relative_location,
location) < 3: # 3 => Max distance
detections.append(point)
if self.knowledge.DEBUG:
# Draw where the lidar points are close
self.knowledge.get_world().debug.draw_line(
relative_location,
sensor_position,
color=carla.Color(r=0, g=255, b=0),
life_time=1.0)
self.knowledge.update_data('lidar_close', detections)
def visualize_rss_results(self, state_snapshot):
for state in state_snapshot:
other_actor = state.get_actor(self._world)
if not other_actor:
print(
"Actor not found. Skip visualizing state {}".format(state))
continue
ego_point = self._player.get_location()
ego_point.z += 0.05
yaw = self._player.get_transform().rotation.yaw
cosine = math.cos(math.radians(yaw))
sine = math.sin(math.radians(yaw))
line_offset = carla.Location(-sine * 0.1, cosine * 0.1, 0.0)
point = other_actor.get_location()
point.z += 0.05
indicator_color = carla.Color(0, 255, 0)
dangerous = ad.rss.state.isDangerous(state.rss_state)
if dangerous:
indicator_color = carla.Color(255, 0, 0)
elif state.rss_state.situationType == ad.rss.situation.SituationType.NotRelevant:
indicator_color = carla.Color(150, 150, 150)
if self._visualization_mode == RssDebugVisualizationMode.All:
# the connection lines are only visualized if All is requested
lon_color = indicator_color
lat_l_color = indicator_color
lat_r_color = indicator_color
if not state.rss_state.longitudinalState.isSafe:
lon_color.r = 255
lon_color.g = 0 if dangerous else 255
if not state.rss_state.lateralStateLeft.isSafe:
lat_l_color.r = 255
lat_l_color.g = 0 if dangerous else 255
if not state.rss_state.lateralStateRight.isSafe:
lat_r_color.r = 255
lat_r_color.g = 0 if dangerous else 255
self._world.debug.draw_line(ego_point, point, 0.1, lon_color,
0.02, False)
self._world.debug.draw_line(ego_point - line_offset,
point - line_offset, 0.1,
lat_l_color, 0.02, False)
self._world.debug.draw_line(ego_point + line_offset,
point + line_offset, 0.1,
lat_r_color, 0.02, False)
point.z += 3.
self._world.debug.draw_point(point, 0.2, indicator_color, 0.02,
False)
def _Radar_callback(weak_self, radar_data):
self = weak_self()
if not self:
return
# To get a numpy [[vel, altitude, azimuth, depth],...[,,,]]:
# points = np.frombuffer(radar_data.raw_data, dtype=np.dtype('f4'))
# points = np.reshape(points, (len(radar_data), 4))
current_rot = radar_data.transform.rotation
for detect in radar_data:
azi = math.degrees(detect.azimuth)
alt = math.degrees(detect.altitude)
# The 0.25 adjusts a bit the distance so the dots can
# be properly seen
fw_vec = carla.Vector3D(x=detect.depth - 0.25)
carla.Transform(
carla.Location(),
carla.Rotation(
pitch=current_rot.pitch + alt,
yaw=current_rot.yaw + azi,
roll=current_rot.roll)).transform(fw_vec)
def clamp(min_v, max_v, value):
return max(min_v, min(value, max_v))
norm_velocity = detect.velocity / self.velocity_range # range [-1, 1]
r = int(clamp(0.0, 1.0, 1.0 - norm_velocity) * 255.0)
g = int(clamp(0.0, 1.0, 1.0 - abs(norm_velocity)) * 255.0)
b = int(abs(clamp(- 1.0, 0.0, - 1.0 - norm_velocity)) * 255.0)
self.debug.draw_point(
radar_data.transform.location + fw_vec,
size=0.075,
life_time=0.06,
persistent_lines=False,
color=carla.Color(r, g, b))
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_transform(debug, trans, col=carla.Color(255, 0, 0), lt=-1):
yaw_in_rad = math.radians(trans.rotation.yaw)
pitch_in_rad = math.radians(trans.rotation.pitch)
p1 = carla.Location(
x=trans.location.x + math.cos(pitch_in_rad) * math.cos(yaw_in_rad),
y=trans.location.y + math.cos(pitch_in_rad) * math.sin(yaw_in_rad),
z=trans.location.z + math.sin(pitch_in_rad))
