def get_main_callback(sim, ped, npc1, npc2, gps_sensor, recorders=None):
ego_trigger_point = Vector(-15, 0, 0)
ped_trigger_thrs = 25
npc2_trigger_thrs = 30
ped_waypoints = get_pedesrian_waypoints(ped)
ped_event = saze.get_pedestrian_event(ped, ped_waypoints)
npc1_end_pos = Vector(-45, 0, 16)
npc1_event = saze.get_npc_event(sim, npc1, [npc1_end_pos], [15])
npc2_wp1 = Vector(-13, 0, 37.5)
npc2_wp2 = Vector(-17.5, 0, 30)
npc2_wp3 = Vector(-17.5, 0, -30)
npc2_event = saze.get_npc_event(sim, npc2, [npc2_wp1, npc2_wp2, npc2_wp3],
[15, 10, 15])
def callback():
gps_data = gps_sensor.data
ego_tr = sim.map_from_gps(latitude = gps_data.latitude,\
longitude = gps_data.longitude,\
)
dist = (ego_tr.position - ego_trigger_point).norm()
if dist < ped_trigger_thrs:
ped_event.trigger()
npc1_event.trigger()
if dist < npc2_trigger_thrs:
npc2_event.trigger()
if recorders:
for rec in recorders:
rec.capture_img()
return callback
def get_sedan1_event(sim, npc):
waypoint_vecs = []
waypoint_vecs.append(Vector(9,0,14))
waypoint_vecs.append(Vector(-48,0,15))
speeds = [5, 10]
return saze.get_npc_event(sim, npc, waypoint_vecs, speeds)
def get_jeep1_event(sim, npc):
waypoint_vecs = []
waypoint_vecs.append(Vector(26.6, 0, -48))
#waypoint_vecs.append(Vector(32, 0, -36))
waypoint_vecs.append(Vector(28, 0, -36))
waypoint_vecs.append(Vector(28,0,5.5))
waypoint_vecs.append(Vector(9,0,14))
waypoint_vecs.append(Vector(-48,0,15))
speeds = [10, 10, 5, 10, 10]
return saze.get_npc_event(sim, npc, waypoint_vecs, speeds)
def spawn_pedestrian(sim,
pos,
name,
offset=Vector(0, 0, 0),
rotation=Vector(0, 0, 0)):
state = lgsvl.AgentState()
spawns = sim.get_spawn()
state.transform = sim.map_point_on_lane(pos)
state.transform.position += offset
state.transform.rotation += rotation
ped = sim.add_agent(name, lgsvl.AgentType.PEDESTRIAN, state)
return ped
def _generate_initial_ego_state(pedestrian_behavior: _PedestrianBehavior, ego_distance: Optional[float],
test_place: Location, ego_speed: float, pedestrian_direction: bool) \
-> Optional[Tuple[AgentState, float]]:
from common.geometry import rotate_around_y
from common.scene import generate_initial_state
from lgsvl.geometry import Transform
if ego_distance is None:
pedestrian_crash_distance = (
pedestrian_behavior.initial_state.position -
test_place.ped_crash_pos).magnitude()
time_to_crash_point = pedestrian_crash_distance / (
PEDESTRIAN_SPEED / 3.6) # in seconds
ego_crash_distance = (ego_speed / 3.6) * time_to_crash_point
else:
ego_crash_distance = ego_distance
time_to_crash_point = ego_crash_distance / (ego_speed / 3.6)
if ego_crash_distance <= test_place.max_ego_distance:
if test_place.ego_approach_rotation is None:
# Move in a 90° offset relative to the pedestrian movement
ego_rotation_offset = -90 if pedestrian_direction else 90
ego_rotation = pedestrian_behavior.initial_state.rotation.y + ego_rotation_offset
else:
ego_rotation = -test_place.ego_approach_rotation if pedestrian_direction else test_place.ego_approach_rotation
ego_start_pos = test_place.ped_crash_pos \
- rotate_around_y(UNIT_VECTOR * (ego_crash_distance + EGO_BBOX_OFFSET), -ego_rotation)
ego_spawn = Transform(position=ego_start_pos,
rotation=Vector(0, ego_rotation, 0))
return generate_initial_state(ego_spawn,
ego_speed), time_to_crash_point
else:
return None
def get_main_callback(sim, sedan1, truck1, jeep1, gps_sensor, recorders = None):
event_s1 = get_sedan1_event(sim, sedan1)
event_t1 = get_truck1_event(sim, truck1)
event_j1 = get_jeep1_event(sim, jeep1)
ego_trigger_point = Vector(32,0,-23)
event_s1_thrs = 10
def callback():
gps_data = gps_sensor.data
ego_tr = sim.map_from_gps(latitude = gps_data.latitude,\
longitude = gps_data.longitude,\
)
#print(ego_tr)
#event_s1.trigger()
event_t1.trigger()
event_j1.trigger()
dist = (ego_tr.position - ego_trigger_point).norm()
if dist < event_s1_thrs:
event_s1.trigger()
pass
if recorders:
for rec in recorders:
rec.capture_img()
return callback
def rotate_around_y(a: Vector, angle: float) -> Vector:
"""
:param angle: Clockwise angle in degrees
"""
from math import cos, sin, radians
acw_angle = -radians(angle)
return Vector(a.x * cos(acw_angle) + a.z * sin(acw_angle), a.y,
-a.x * sin(acw_angle) + a.z * cos(acw_angle))
def get_directional_angle(a: Vector, b: Vector) -> float:
"""
:param a: A vector in the XZ plane
:param b: Another vector in the XZ plane
:return: The clockwise angle between a and b on the XZ plane in degrees
"""
from math import atan2, degrees
cross_ab = _cross_product(a, b)
dot_ab = _dot_product(a, b)
return -degrees(atan2(_dot_product(cross_ab, Vector(0, 1, 0)), dot_ab))
def send_next_destination(agent):
global i
print(f"{i}: {agent.name} reached destination")
i += 1
if i >= len(ros_destinations):
print("Iterated all destinations successfully. Stopping...")
sim.stop()
return
ros_dst = ros_destinations[i]
next_dst = sim.map_from_nav(Vector(*ros_dst[0]), Quaternion(*ros_dst[1]))
ego.set_destination(next_dst)
def main(args):
map_name = "Shalun"
app_tag = "Scenario2"
ego_spawn_pos = Vector(-75, 0, -40)
#ego_spawn_pos = Vector(-10, 0, -47)
#ego_spawn_pos = Vector(28, 0, -36)
sedan1_spawn_pos = Vector(28, 0, 5.5)
sedan2_spawn_pos = Vector(23, 0, 27)
truck1_spawn_pos = Vector(28, 0, -15)
jeep1_spawn_pos = Vector(11, 0, -48)
sim = saze.open_simulator(map_name)
saze.print_msg(app_tag, "Simulator opened")
ego = saze.spawn_ego(sim, ego_spawn_pos)
saze.print_msg(app_tag, "Ego vehicle spawned")
gps_sensor = saze.get_gps_sensor(ego)
saze.print_msg(app_tag,"GPS sensor ready")
sedan1 = saze.spawn_npc(sim, sedan1_spawn_pos, car_type = "Sedan")
sedan2 = saze.spawn_npc(sim, sedan2_spawn_pos, car_type = "Sedan")
truck1 = saze.spawn_npc(sim, truck1_spawn_pos, car_type = "DeliveryTruck")
#truck2 = None
jeep1 = saze.spawn_npc(sim, jeep1_spawn_pos, car_type = "Jeep")
recorders = None
if args.uname:
main_cam = saze.get_main_camera_sensor(ego)
main_dir_name = saze.get_img_dir_name(args.uname, app_tag, "main")
main_rec = saze.CamRecoder(main_dir_name, main_cam)
seg_cam = saze.get_seg_camera_sensor(ego)
seg_dir_name = saze.get_img_dir_name(args.uname, app_tag, "seg")
seg_rec = saze.CamRecoder(seg_dir_name, seg_cam)
recorders = [main_rec, seg_rec]
callback = get_main_callback(sim, sedan1, truck1, jeep1, gps_sensor, recorders = recorders)
sim.run_with_callback(callback)
def get_npc_event(sim, npc):
waypoints_vec = []
waypoints_vec.append(Vector(46.3, 0, -103))
# Setup waypoints for the NPC to follow
speed = 30
waypoints = []
for v in waypoints_vec:
waypoints.append(lgsvl.DriveWaypoint(v, speed))
# This function will be triggered
def npc_event_func():
npc.follow(waypoints)
npc_event = saze.Event(func=npc_event_func, params=None, only_once=True)
return npc_event
def get_main_callback(sim, npc, gps_sensor, recorders=None):
ego_trigger_point = Vector(-27, 0, -78)
dist_thrs = 25
npc_event = get_npc_event(sim, npc)
def callback():
gps_data = gps_sensor.data
ego_tr = sim.map_from_gps(latitude = gps_data.latitude,\
longitude = gps_data.longitude)
dist = (ego_tr.position - ego_trigger_point).norm()
if dist < dist_thrs:
npc_event.trigger()
if recorders:
for rec in recorders:
rec.capture_img()
return callback
def _generate_initial_pedestrian_behavior(
test_place: Location,
pedestrian_direction: bool) -> _PedestrianBehavior:
from common.geometry import get_directional_angle
from common.scene import generate_initial_state
from lgsvl.geometry import Transform
waypoints = [WalkWaypoint(test_place.ped_crash_pos, 0)]
if pedestrian_direction:
start = test_place.ped_pos_a
finish = test_place.ped_pos_b
else:
start = test_place.ped_pos_b
finish = test_place.ped_pos_a
rotation = get_directional_angle(finish - start, UNIT_VECTOR)
spawn = Transform(position=start, rotation=Vector(0, rotation, 0))
waypoints.append(WalkWaypoint(finish, 0))
return _PedestrianBehavior(
generate_initial_state(spawn, PEDESTRIAN_SPEED), waypoints)
def main(args):
map_name = "Shalun"
app_tag = "Scenario1"
ego_spawn_pos = Vector(-75, 0, -40)
npc1_spawn_pos = Vector(7, 0, 16)
npc2_spawn_pos = Vector(10, 0, 48)
ped1_spawn_pos = Vector(-7, 0, 8)
ped1_offset = Vector(0, 0, -3.5)
ped1_rotation = Vector(0, 270, 0)
sim = saze.open_simulator(map_name)
saze.print_msg(app_tag, "Simulator opened")
ego = saze.spawn_ego(sim, ego_spawn_pos)
saze.print_msg(app_tag, "Ego vehicle spawned")
gps_sensor = saze.get_gps_sensor(ego)
saze.print_msg(app_tag, "GPS sensor ready")
recorders = None
if args.uname:
main_cam = saze.get_main_camera_sensor(ego)
main_dir_name = saze.get_img_dir_name(args.uname, app_tag, "main")
main_rec = saze.CamRecoder(main_dir_name, main_cam)
seg_cam = saze.get_seg_camera_sensor(ego)
seg_dir_name = saze.get_img_dir_name(args.uname, app_tag, "seg")
seg_rec = saze.CamRecoder(seg_dir_name, seg_cam)
recorders = [main_rec, seg_rec]
npc1 = saze.spawn_npc(sim, npc1_spawn_pos, car_type="Sedan")
npc2 = saze.spawn_npc(sim, npc2_spawn_pos, car_type="Sedan")
ped = saze.spawn_pedestrian(sim,
ped1_spawn_pos,
name="Presley",
offset=ped1_offset,
rotation=ped1_rotation)
callback = get_main_callback(sim,
ped,
npc1,
npc2,
gps_sensor,
recorders=recorders)
sim.run_with_callback(callback)
def spawn_npc(sim):
npc_pos = Vector(-48, 0, -103)
npc = saze.spawn_npc(sim, npc_pos, "Sedan")
return npc
def _cross_product(a: Vector, b: Vector) -> Vector:
return Vector(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z,
a.x * b.y - a.y * a.x)
from lgsvl import Vector
@dataclass
class Location:
map_name: str
ped_pos_a: Vector
ped_crash_pos: Vector
ped_pos_b: Vector
max_ego_distance: float # Ensure AVs are not placed outside the map
# The following can be used to enforce an approach direction (e.g. in case of non orthogonal cross walks)
ego_approach_rotation: Optional[float] = None # In degree
LOC_1_VARA = Location("SanFrancisco", Vector(-185, 10.25, 139),
Vector(-202, 10.25, 139), Vector(-205, 10.25, 139), 500)
LOC_1_VARB = Location("SanFrancisco", Vector(-205, 10.25, 139),
Vector(-190, 10.25, 139), Vector(-185, 10.25, 139), 500)
LOC_2_VARA = Location("SanFrancisco", Vector(-185, 10.25, 585),
Vector(-202, 10.25, 587), Vector(-204, 10.25, 587), 500,
180)
LOC_2_VARB = Location("SanFrancisco", Vector(-204, 10.25, 587),
Vector(-189.5, 10.25, 586), Vector(-185, 10.25, 585),
500, 0)
LOC_3_VARA = Location("CubeTown", Vector(-7, 0, 42), Vector(3, 0, 42),
Vector(7, 0, 42), 80)
LOC_4_VARA = Location("CubeTown", Vector(7, 0, -42), Vector(-3, 0, -42),
from typing import Tuple, List, Optional
from lgsvl import AgentState, WalkWaypoint, Vector, Simulator
from test_case_06.config import TestConfig
from test_case_06.locations import Location
# Test case fixed settings
UNIT_VECTOR: Vector = Vector(0, 0, 1) # The unit vector with 0°
# NOTE Due to different bounding box sizes the crash positions of agents differ slightly
EGO_BBOX_OFFSET: float = 3 # in m
# NOTE The pedestrian speed is just an observation
PEDESTRIAN_SPEED: float = 4.4 # km/h # FIXME It seems like the pedestrian speed can not be changed
class _TestResult:
successful: bool = True
class _PedestrianBehavior:
def __init__(self, initial_state: AgentState,
waypoints: List[WalkWaypoint]):
self.initial_state = initial_state
self.waypoints = waypoints
initial_state: AgentState
waypoints: List[WalkWaypoint]
class TestCase6:
@staticmethod
def spawn_npc(sim, pos, car_type, offset=Vector(0, 0, 0)):
state = lgsvl.AgentState()
state.transform = sim.map_point_on_lane(pos)
state.transform.position += offset
npc = sim.add_agent(car_type, lgsvl.AgentType.NPC, state)
return npc
#!/usr/bin/env python3
#
# Copyright (c) 2019 LG Electronics, Inc.
#
# This software contains code licensed as described in LICENSE.
#
from lgsvl import Transform, Vector
from lgsvl.utils import (
transform_to_matrix,
matrix_inverse,
matrix_multiply,
vector_multiply,
)
# global "world" transform (for example, car position)
world = Transform(Vector(10, 20, 30), Vector(11, 22, 33))
# "local" transform, relative to world transform (for example, Lidar sensor)
local = Transform(Vector(0, 2, -0.5), Vector(0, 0, 0))
# combine them in one transform matrix
mtx = matrix_multiply(transform_to_matrix(local), transform_to_matrix(world))
# compute inverse for transforming points
mtx = matrix_inverse(mtx)
# now transform some point from world position into coordinate system of local transform of Lidar
pt = vector_multiply(Vector(15, 20, 30), mtx)
print(pt)
((2.965, 11.190, 0), (0, 0, -0.707, 0.707)),
((2.687, -0.399, 0), (0, 0, -0.0036, 0.999)),
]
ego.set_initial_pose()
sim.run(5)
def send_next_destination(agent):
global i
print(f"{i}: {agent.name} reached destination")
i += 1
if i >= len(ros_destinations):
print("Iterated all destinations successfully. Stopping...")
sim.stop()
return
ros_dst = ros_destinations[i]
next_dst = sim.map_from_nav(Vector(*ros_dst[0]), Quaternion(*ros_dst[1]))
ego.set_destination(next_dst)
ros_dst = ros_destinations[0]
first_dst = sim.map_from_nav(Vector(*ros_dst[0]), Quaternion(*ros_dst[1]))
ego.set_destination(first_dst)
ego.on_destination_reached(send_next_destination)
sim.run()
print("Done!")