def addNpcAtFixedPoints(sim, spawn_idx: int):
spawns = sim.get_spawn()
state = lgsvl.AgentState()
forward = lgsvl.utils.transform_to_forward(spawns[0])
right = lgsvl.utils.transform_to_right(spawns[0])
# 10 meters ahead, on left lane
state.transform.position = spawns[0].position + 10.0 * forward
state.transform.rotation = spawns[0].rotation
npc1 = sim.add_agent("Sedan", lgsvl.AgentType.NPC, state)
state = lgsvl.AgentState()
# 10 meters ahead, on right lane
state.transform.position = spawns[0].position + \
4.0 * right + 10.0 * forward
state.transform.rotation = spawns[0].rotation
npc2 = sim.add_agent("SUV", lgsvl.AgentType.NPC, state,
lgsvl.Vector(1, 1, 0))
# If the passed bool is False, then the NPC will not moved
# The float passed is the maximum speed the NPC will drive
# 11.1 m/s is ~40 km/h
npc1.follow_closest_lane(True, 11.1)
# 5.6 m/s is ~20 km/h
npc2.follow_closest_lane(True, 5.6)
def addNpcRandomAtlanes(sim, no_of_npc: int, spawn_idx: int, tf: Transform, isDrive: bool):
spawns = sim.get_spawn()
state = lgsvl.AgentState()
spawn = tf
sx = spawn.position.x
sy = spawn.position.y
sz = spawn.position.z
mindist = 5.0
maxdist = 100.0
random.seed()
for i in range(no_of_npc):
# Creates a random point around the EGO
angle = random.uniform(0.0, 2*math.pi)
dist = random.uniform(mindist, maxdist)
point = lgsvl.Vector(sx + dist * math.cos(angle),
sy, sz + dist * math.sin(angle))
# Creates an NPC on a lane that is closest to the random point
state = lgsvl.AgentState()
state.transform = sim.map_point_on_lane(point)
new_npc = sim.add_agent("Sedan", lgsvl.AgentType.NPC, state)
new_npc.follow_closest_lane(isDrive, 6)
def set_environment(self):
'''start simulator, spawn EGO and NPC'''
###################### simulator ######################
if self.sim.current_scene == "BorregasAve":
self.sim.reset()
else:
self.sim.load("BorregasAve")
###################### EGO ######################
spawns = self.sim.get_spawn()
state = lgsvl.AgentState()
state.transform = spawns[0]
self.ego = self.sim.add_agent("Lincoln2017MKZ (Apollo 5.0)",
lgsvl.AgentType.EGO, state)
# sensors = self.ego.get_sensors()
# c = lgsvl.VehicleControl()
# c.turn_signal_left = True
# self.ego.apply_control(c, True)
###################### NPC ######################
sx = spawns[0].position.x - 8
sy = spawns[0].position.y
sz = spawns[0].position.z + 100
state = lgsvl.AgentState()
state.transform = spawns[0]
state.transform.position.x = sx
state.transform.position.z = sz
state.transform.rotation.y = 180.0
self.npc = self.sim.add_agent("Sedan", lgsvl.AgentType.NPC, state)
self.npc.on_waypoint_reached(self.on_waypoint)
self.vehicles = {
self.ego: "EGO",
self.npc: "Sedan",
}
self.ego.on_collision(self.on_collision)
self.npc.on_collision(self.on_collision)
###################### Traffic light is kept green ######################
controllables = self.sim.get_controllables()
# Pick a traffic light of intrest
signal = controllables[2]
# Get current controllable states
# print("\n# Current control policy:")
# print(signal.control_policy)
# Create a new control policy
control_policy = "trigger=100;green=100;yellow=0;red=0;loop"
# Control this traffic light with a new control policy
signal.control(control_policy)
self.z_position = sz
def test_lane_change_left(self):
with SimConnection(40) as sim:
state = lgsvl.AgentState()
state.transform = sim.map_point_on_lane(
lgsvl.Vector(4.49, -1.57, 40.85))
npc = sim.add_agent("SUV", lgsvl.AgentType.NPC, state)
state.transform = sim.map_point_on_lane(
lgsvl.Vector(9.82, -1.79, 42.02))
sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5,
lgsvl.AgentType.EGO, state)
forward = lgsvl.utils.transform_to_forward(state.transform)
target = state.position + 31 * forward
npc.follow_closest_lane(True, 10)
agents = []
def on_lane_change(agent):
agents.append(agent)
npc.on_lane_change(on_lane_change)
sim.run(2)
npc.change_lane(True)
sim.run(3)
self.assertTrue(npc == agents[0])
self.assertAlmostEqual((npc.state.position - target).magnitude(),
0,
delta=2)
def test_lane_change_right_missing_lane(self):
with SimConnection(40) as sim:
state = lgsvl.AgentState()
state.transform = sim.map_point_on_lane(
lgsvl.Vector(0.63, -1.57, 42.73))
npc = sim.add_agent("Hatchback", lgsvl.AgentType.NPC, state)
forward = lgsvl.utils.transform_to_forward(state.transform)
target = state.position + 42.75 * forward
state.transform = sim.map_point_on_lane(
lgsvl.Vector(4.49, -1.57, 40.85))
sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5,
lgsvl.AgentType.EGO, state)
npc.follow_closest_lane(True, 10)
agents = []
def on_lane_change(agent):
agents.append(agent)
npc.on_lane_change(on_lane_change)
sim.run(2)
npc.change_lane(False)
sim.run(3)
self.assertTrue(len(agents) == 0)
self.assertAlmostEqual((npc.state.position - target).magnitude(),
0,
delta=2)
def test_lane_change_left_opposing_traffic(self):
with SimConnection(40) as sim:
state = lgsvl.AgentState()
state.transform = sim.map_point_on_lane(
lgsvl.Vector(78.962, -3.363, -40.292))
npc = sim.add_agent("SUV", lgsvl.AgentType.NPC, state)
forward = lgsvl.utils.transform_to_forward(state.transform)
target = state.position + 42.75 * forward
state.transform.position = state.position - 10 * forward
sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5,
lgsvl.AgentType.EGO, state)
npc.follow_closest_lane(True, 10)
agents = []
def on_lane_change(agent):
agents.append(agent)
npc.on_lane_change(on_lane_change)
sim.run(2)
npc.change_lane(True)
sim.run(3)
self.assertTrue(len(agents) == 0)
self.assertAlmostEqual((npc.state.position - target).magnitude(),
0,
delta=2)
def createObjectInSimulator(self, obj):
# Figure out what type of LGSVL object this is
if not hasattr(obj, 'lgsvlName'):
raise RuntimeError(f'object {obj} does not have an lgsvlName property')
if not hasattr(obj, 'lgsvlAgentType'):
raise RuntimeError(f'object {obj} does not have an lgsvlAgentType property')
name = obj.lgsvlName
agentType = obj.lgsvlAgentType
# Set up position and orientation
state = lgsvl.AgentState()
elevation = obj.elevation
if elevation is None:
elevation = self.groundElevationAt(obj.position)
state.transform.position = utils.scenicToLGSVLPosition(obj.position, elevation)
state.transform.rotation = utils.scenicToLGSVLRotation(obj.heading)
# Create LGSVL object
lgsvlObj = self.client.add_agent(name, agentType, state)
obj.lgsvlObject = lgsvlObj
# Initialize Data
self.data[obj] = {}
# Initialize Apollo if needed
if getattr(obj, 'apolloVehicle', None):
self.initApolloFor(obj, lgsvlObj)
def test_waypoint_speed(self):
with SimConnection(60) as sim:
state = lgsvl.AgentState()
state.transform = sim.map_point_on_lane(
lgsvl.Vector(78.962, -3.363, -40.292))
sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5,
lgsvl.AgentType.EGO, state)
forward = lgsvl.utils.transform_to_forward(state.transform)
up = lgsvl.utils.transform_to_up(state.transform)
state.transform.position = state.position + 10 * forward
npc = sim.add_agent("Hatchback", lgsvl.AgentType.NPC, state)
waypoints = []
layer_mask = 0
layer_mask |= 1 << 0
hit = sim.raycast(state.position + 10 * forward + 50 * up,
lgsvl.Vector(0, -1, 0), layer_mask)
waypoints.append(
lgsvl.DriveWaypoint(
hit.point, 5, lgsvl.Vector(0, 0, 0), 0,
0)) # this waypoint to allow NPC to get up to speed
hit = sim.raycast(state.position + 30 * forward + 50 * up,
lgsvl.Vector(0, -1, 0), layer_mask)
waypoints.append(
lgsvl.DriveWaypoint(hit.point, 5, lgsvl.Vector(0, 0, 0), 0, 0))
def on_waypoint(agent, index):
sim.stop()
npc.on_waypoint_reached(on_waypoint)
npc.follow(waypoints)
sim.run()
t0 = time.time()
sim.run()
t1 = time.time()
waypoints = []
hit = sim.raycast(state.position + 40 * forward + 50 * up,
lgsvl.Vector(0, -1, 0), layer_mask)
waypoints.append(
lgsvl.DriveWaypoint(
hit.point, 20, lgsvl.Vector(0, 0, 0), 0,
0)) # this waypoint to allow NPC to get up to speed
hit = sim.raycast(state.position + 120 * forward + 50 * up,
lgsvl.Vector(0, -1, 0), layer_mask)
waypoints.append(
lgsvl.DriveWaypoint(hit.point, 20, lgsvl.Vector(0, 0, 0), 0,
0))
npc.follow(waypoints)
sim.run()
t2 = time.time()
sim.run()
t3 = time.time()
lowSpeed = t1 - t0
highSpeed = t3 - t2
self.assertAlmostEqual(lowSpeed, highSpeed, delta=0.5)
self.assertAlmostEqual(lowSpeed, 4.5, delta=0.5)
self.assertAlmostEqual(highSpeed, 4.5, delta=0.5)
def initScene(sim, sim_map: str, ego_car_model: str, ego_state: ObjectState, ros_bridge_ip: str,
ros_bridge_port: int, spawn_idx: int) -> None:
if sim.current_scene == sim_map:
sim.reset()
else:
sim.load(sim_map)
spawns = sim.get_spawn()
default_state = lgsvl.AgentState()
default_state.transform = spawns[spawn_idx]
forward = lgsvl.utils.transform_to_forward(spawns[spawn_idx])
right = lgsvl.utils.transform_to_right(spawns[spawn_idx])
default_state.transform.position += 0 * right
ego = sim.add_agent(ego_car_model, lgsvl.AgentType.EGO, default_state)
ego.connect_bridge(os.environ.get(
"BRIDGE_HOST", ros_bridge_ip), ros_bridge_port)
sensors = ego.get_sensors()
for s in sensors:
print(type(s), s.enabled)
print("Current time = ", sim.current_time)
print("Current frame = ", sim.current_frame)
def add_pedestrian_with_trigger(agent_type, offset, timing):
i = 0
spawns = sim.get_spawn()
forward = lgsvl.utils.transform_to_forward(spawns[i])
right = lgsvl.utils.transform_to_right(spawns[i])
up = lgsvl.utils.transform_to_up(spawns[i])
wp = [
lgsvl.WalkWaypoint(
project(spawns[0].position + 5 * right + offset * forward +
2 * up), 0, 100),
lgsvl.WalkWaypoint(
project(spawns[0].position + 5 * right + offset * forward +
2 * up), timing, 0),
lgsvl.WalkWaypoint(
project(spawns[0].position - 5 * right + offset * forward +
2 * up), 0),
lgsvl.WalkWaypoint(
project(spawns[0].position - 11 * right + offset * forward +
2 * up), 0),
]
state = lgsvl.AgentState()
state.transform = copy.deepcopy(spawns[0])
state.transform.position = copy.deepcopy(wp[0].position)
p = sim.add_agent(agent_type, lgsvl.AgentType.PEDESTRIAN, state)
p.follow(wp, False)
def addNpcVehicle(self, posVector, vehicleType="SUV"):
sim = self.sim
npcList = self.npcList
npcState = lgsvl.AgentState()
npcState.transform = sim.map_point_on_lane(posVector)
npc = sim.add_agent(vehicleType, lgsvl.AgentType.NPC, npcState)
npcList.append(npc)
def test_lane_change_right(self):
with SimConnection(40) as sim:
state = lgsvl.AgentState()
state.transform = sim.map_point_on_lane(
lgsvl.Vector(40.85, -1.57, -4.49))
npc = sim.add_agent("SUV", lgsvl.AgentType.NPC, state)
state.transform = sim.map_point_on_lane(
lgsvl.Vector(42.73, -1.57, -0.63))
sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO,
state)
forward = lgsvl.utils.transform_to_forward(state.transform)
target = state.position + 31 * forward
npc.follow_closest_lane(True, 10)
agents = []
def on_lane_change(agent):
agents.append(agent)
npc.on_lane_change(on_lane_change)
sim.run(2)
npc.change_lane(False)
sim.run(3)
self.assertTrue(npc == agents[0])
self.assertAlmostEqual((npc.state.position - target).magnitude(),
0,
delta=2)
def __init__(self, sim_host_ip: str, sim_host_port: int, sim_map: str,
ego_car_model: str, ros_bridge_ip: str, ros_bridge_port: int,
time_limit: float, time_scale: float) -> None:
# initialise simulator and global params
self.sim = lgsvl.Simulator(
os.environ.get("SIMULATOR_HOST", sim_host_ip), sim_host_port)
self.sim.load(sim_map)
self.ego_car_model = ego_car_model
self.ros_bridge_ip = ros_bridge_ip
self.ros_bridge_port = ros_bridge_port
self.time_limit = time_limit
self.time_scale = time_scale
# set default ego state
self.default_ego_state = lgsvl.AgentState()
self.default_ego_state.transform = self.sim.get_spawn()[0]
self.ego_state = copy.deepcopy(self.default_ego_state)
# set global direction based on map
self.forward_dir_map = lgsvl.utils.transform_to_forward(
self.default_ego_state.transform)
self.right_dir_map = lgsvl.utils.transform_to_right(
self.default_ego_state.transform)
# set pedestrians and npc types
self.pedestrians_names = [
"Bob", "EntrepreneurFemale", "Howard", "Johny", "Pamela",
"Presley", "Robin", "Stephen", "Zoe"
]
self.npc_types = ['Sedan', 'SUV', 'Jeep', 'Hatchback']
def test_AAA_NPC_no_scene(self):
with SimConnection(load_scene=False) as sim:
with self.assertRaises(Exception) as e:
state = lgsvl.AgentState()
agent = sim.add_agent("Jeep", lgsvl.AgentType.NPC, state)
agent.state.position
self.assertFalse(repr(e.exception).startswith(PROBLEM))
def test_GPS(self): # Check that the GPS sensor works
with SimConnection() as sim:
state = lgsvl.AgentState()
state.transform = sim.get_spawn()[0]
right = lgsvl.utils.transform_to_right(state.transform)
state.velocity = -50 * right
agent = sim.add_agent(
lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5,
lgsvl.AgentType.EGO, state)
sensors = agent.get_sensors()
initialGPSData = None
gps = None
for s in sensors:
if s.name == "GPS":
gps = s
initialGPSData = gps.data
sim.run(1)
finalGPSData = gps.data
latChanged = notAlmostEqual(initialGPSData.latitude,
finalGPSData.latitude)
lonChanged = notAlmostEqual(initialGPSData.longitude,
finalGPSData.longitude)
northingChanged = notAlmostEqual(initialGPSData.northing,
finalGPSData.northing)
eastingChanged = notAlmostEqual(initialGPSData.easting,
finalGPSData.easting)
self.assertTrue(latChanged or lonChanged)
self.assertTrue(northingChanged or eastingChanged)
self.assertNotAlmostEqual(gps.data.latitude, 0)
self.assertNotAlmostEqual(gps.data.longitude, 0)
self.assertNotAlmostEqual(gps.data.northing, 0)
self.assertNotAlmostEqual(gps.data.easting, 0)
self.assertNotAlmostEqual(gps.data.altitude, 0)
self.assertNotAlmostEqual(gps.data.orientation, 0)
def set_env(self, env):
# Set the map
MAP = env.get("map")
if self.sim.current_scene == MAP:
self.sim.reset()
self.log.debug(f"Reset the map: {MAP}")
else:
self.sim.load(MAP)
self.log.debug(f"Loaded the map: {MAP}")
# Set the time
self.sim.set_time_of_day(env.get("time"))
self.log.debug(f"Set the time to {env.get('time')}")
# Set the vehicle
vehicleState = lgsvl.AgentState()
vehicleState.transform = self.sim.get_spawn()[0]
self.vehicle = self.sim.add_agent(env.get("car"),
lgsvl.AgentType.EGO,
vehicleState)
self.log.debug("Created an EGO vehicle: {env.get('car')}")
self.vehiclePos = self.vehicle.state.position
self.vehicle.connect_bridge(self.config.get("BRIDGE_HOST"),
self.config.get("BRIDGE_PORT"))
self.log.debug("Connected the vehicle to the bridge: {self.config.get('BRIDGE_HOST')}:{self.config.get('BRIDGE_PORT')}")
self.vehicle.on_collision(self._on_collision)
self.log.debug("Connected a collision callback")
def waypoint_follow_test(waypoints, pos, rot):
state = lgsvl.AgentState()
state.transform.position = pos
state.transform.rotation = rot
npc = sim.add_agent("Sedan", lgsvl.AgentType.NPC, state)
npc.follow(waypoints)
sim.run(10)
def test_GPS(self):
with SimConnection() as sim:
state = lgsvl.AgentState()
state.transform = sim.get_spawn()[0]
state.velocity = lgsvl.Vector(-50, 0, 0)
agent = sim.add_agent("XE_Rigged-apollo", lgsvl.AgentType.EGO,
state)
sensors = agent.get_sensors()
initialGPSData = None
gps = None
for s in sensors:
if s.name == "GPS":
gps = s
initialGPSData = gps.data
sim.run(1)
finalGPSData = gps.data
latChanged = notAlmostEqual(initialGPSData.latitude,
finalGPSData.latitude)
lonChanged = notAlmostEqual(initialGPSData.longitude,
finalGPSData.longitude)
northingChanged = notAlmostEqual(initialGPSData.northing,
finalGPSData.northing)
eastingChanged = notAlmostEqual(initialGPSData.easting,
finalGPSData.easting)
self.assertTrue(latChanged or lonChanged)
self.assertTrue(northingChanged or eastingChanged)
self.assertNotAlmostEqual(gps.data.latitude, 0)
self.assertNotAlmostEqual(gps.data.longitude, 0)
self.assertNotAlmostEqual(gps.data.northing, 0)
self.assertNotAlmostEqual(gps.data.easting, 0)
self.assertNotAlmostEqual(gps.data.altitude, 0)
self.assertNotAlmostEqual(gps.data.orientation, 0)
def add_line_loop_car_with_underground_trigger(car_type,
spawn_id,
trigger_offset,
offsets,
trigger_distance,
delay,
angle_offset,
speed,
no_loop=False):
"""
offset is (forward, right)
1. wait underground of trigger_offsets to be triggered.
2. visit offsets[0] -> offsets[n-1] accordingly
3. back to the offsets[0] through the underground of offsets[n-1] -> that of offset[0]
4. play again from 2
"""
# not redefine spawns
rotation = spawns[spawn_id].rotation
angle = copy.deepcopy(rotation)
angle.y += angle_offset
speed_invisible = 100
underground_offset = 3
up = lgsvl.utils.transform_to_up(spawns[spawn_id])
underground = [
lgsvl.DriveWaypoint(
get_ground_point(spawn_id, trigger_offset) -
underground_offset * up, speed_invisible, angle, 0, 0,
trigger_distance),
lgsvl.DriveWaypoint(
get_ground_point(spawn_id, offsets[0]) - underground_offset * up,
speed_invisible, angle, delay),
]
forward = [
lgsvl.DriveWaypoint(get_ground_point(spawn_id, offset), speed, angle,
0) for offset in offsets
]
backward = [
lgsvl.DriveWaypoint(
get_ground_point(spawn_id, offsets[-1]) - underground_offset * up,
speed_invisible, angle, 0),
lgsvl.DriveWaypoint(
get_ground_point(spawn_id, offsets[0]) - underground_offset * up,
speed_invisible, angle, 0),
]
if no_loop:
waypoints = underground + forward + backward * 100
else:
loop = forward + backward
N = 50 # the number of loop iteration
waypoints = underground + loop * N
state = lgsvl.AgentState()
state.transform.position = get_ground_point(
spawn_id, trigger_offset) - underground_offset * up
state.transform.rotation = angle
npc = sim.add_agent(car_type, lgsvl.AgentType.NPC, state)
npc.follow(waypoints, loop=False)
def main():
sim_host_ip = "127.0.0.1" # of your local machine running sim
sim_host_port = 8181
sim_map = "CubeTown"
ego_car_model = "Jaguar2015XE (Autoware)" # Lexus2016RXHybrid (Autoware)
ros_bridge_ip = "192.168.1.78" # of the ros machine that runs the websocket_server
ros_bridge_port = 9090
time_limit = 0 # 0 means unlimited time otherwise sim will produce this much data in virtual seconds
time_scale = 1 # 1 means normal speed, 1.5 means 1.5x speed
ego_pos = Vector(-22, 0, 59.9)
ego_roll = 0
ego_pitch = 0
ego_yaw = 250
ego_rot = Vector(ego_pitch, ego_yaw, ego_roll)
ego_tf = Transform(ego_pos, ego_rot)
ego_v = Vector(0, 0, 0)
ego_ang_v = Vector(0, 0, 0)
ego_state = ObjectState(ego_tf, ego_v, ego_ang_v)
no_of_npc = 0
spawn_idx = 0
spawn_pos = Vector(-32.6, 0, 45)
spawn_tf = Transform(spawn_pos)
sim = lgsvl.Simulator(os.environ.get(
"SIMULATOR_HOST", sim_host_ip), sim_host_port)
initScene(sim, sim_map, ego_car_model, ego_state,
ros_bridge_ip, ros_bridge_port, spawn_idx)
# addNpcRandomAtlanes(sim, no_of_npc, spawn_idx, ego_tf)
spawns = sim.get_spawn()
state = lgsvl.AgentState()
forward = lgsvl.utils.transform_to_forward(spawns[0])
right = lgsvl.utils.transform_to_right(spawns[0])
state.transform.position = spawns[0].position + 30.0 * forward + 2 * right
state.transform.rotation = spawns[0].rotation
addNpcAtFixedPoints(sim, state, 0.0, False)
state.transform.position = spawns[0].position + 20.0 * forward + 30 * right
state.transform.rotation = spawns[0].rotation
addNpcAtFixedPoints(sim, state, 0.0, False)
state.transform.position = spawns[0].position + 60.0 * forward - 4.4 * right
state.transform.rotation = spawns[0].rotation + Vector(0, 180, 0)
addNpcAtFixedPoints(sim, state, 0.0, False)
# start the sim
input("Press Enter to run simulation")
sim.run(time_limit, time_scale)
def _setup_npc(self, npc_type = None, position = None, follow_lane = True,
speed = None, speed_upper = 25.0, speed_lower = 7.0,
randomize = False, min_dist = 10.0, max_dist = 40.0):
"""
Spawns an NPC vehicle of a specific type at a specific location with an
option to have it follow lane annotations in the Unity scene at a given
speed.
Not specifying any input results in a random selection of NPC type, a
random spawn location within the [min_dist, max_dist] range of the ego
vehicle, and a random speed selected within the [speed_lower, speed_upper]
range.
"""
npc_types = {"Sedan", "Hatchback", "SUV", "Jeep", "BoxTruck", "SchoolBus"}
if (not npc_type):
npc_type = random.sample(npc_types, 1)[0]
if (randomize or not position):
sx = self.ego.transform.position.x
sy = self.ego.transform.position.y
sz = self.ego.transform.position.z
while (not position):
angle = random.uniform(0.0, 2*math.pi)
dist = random.uniform(min_dist, max_dist)
point = lgsvl.Vector(sx + dist * math.cos(angle), sy, sz + dist * math.sin(angle))
transform = self.env.map_point_on_lane(point)
px = transform.position.x
py = transform.position.y
pz = transform.position.z
mindist = 0.0
maxdist = 10.0
dist = random.uniform(mindist, maxdist)
angle = math.radians(transform.rotation.y)
position = lgsvl.Vector(px - dist * math.cos(angle), py, pz + dist * math.sin(angle))
for pos in self._occupied:
if (position and self._proximity(position, pos) < 7):
position = None
state = lgsvl.AgentState()
state.transform = self.env.map_point_on_lane(position)
n = self.env.add_agent(npc_type, lgsvl.AgentType.NPC, state)
if (follow_lane):
if (not speed):
speed = random.uniform(speed_lower, speed_upper)
n.follow_closest_lane(True, speed)
self.vehicles[n] = npc_type
self._occupied.append(position)
def position_npc(self, transform):
npc_state = lgsvl.AgentState()
npc_state.transform = transform
available_npcs = ['Sedan', 'SUV', 'Jeep',
'Hatchback'] # 'SchoolBus', 'DeliveryTruck'
npc_type = available_npcs[random.randint(0, len(available_npcs) - 1)]
npc = self.sim.add_agent(npc_type, lgsvl.AgentType.NPC, npc_state)
self.npcs.append(npc)
self.npcs_state.append(npc_state)
def spawn_ego(sim, pos=None):
state = lgsvl.AgentState()
if pos:
state.transform = sim.map_point_on_lane(pos)
else:
spawns = sim.get_spawn()
state.transform = spawns[0]
ego = sim.add_agent("XE_Rigged-lgsvl", lgsvl.AgentType.EGO, state)
return ego
def generate_NPCs(self):
# Encroaching Vehicle in opposite lane
npcState = lgsvl.AgentState()
x = self.EGO_start.x
y = self.EGO_start.y
z = self.EGO_start.z
##### NPC1 #####
npcState.transform = self.sim.map_point_on_lane(
lgsvl.Vector(x + 7, y, z))
self.npc1 = self.sim.add_agent("Sedan", lgsvl.AgentType.NPC, npcState)
npc1Speed = 5
n1x = self.npc1.state.position.x
n1y = self.npc1.state.position.y
n1z = self.npc1.state.position.z
self.npc1Waypoints = []
self.npc1Waypoints.append(
lgsvl.DriveWaypoint(lgsvl.Vector(n1x - 12, n1y, n1z + 0.5),
npc1Speed))
self.npc1Waypoints.append(
lgsvl.DriveWaypoint(lgsvl.Vector(n1x - 14, n1y, n1z + 2.5),
npc1Speed))
self.npc1Waypoints.append(
lgsvl.DriveWaypoint(lgsvl.Vector(n1x - 16, n1y, n1z + 6),
npc1Speed))
self.npc1Waypoints.append(
lgsvl.DriveWaypoint(lgsvl.Vector(n1x - 18, n1y, n1z + 9),
npc1Speed))
self.npc1Waypoints.append(
lgsvl.DriveWaypoint(lgsvl.Vector(n1x - 20, n1y, n1z + 13),
npc1Speed))
##### NPC2 #####
npcState.transform = self.sim.map_point_on_lane(
lgsvl.Vector(x - INITIAL_HEADWAY, y, 1.0))
npcState.transform.rotation = lgsvl.Vector(0, 90, 0)
self.npc2 = self.sim.add_agent("Jeep", lgsvl.AgentType.NPC, npcState)
npc2Speed = 12
n2x = self.npc2.state.position.x
n2y = self.npc2.state.position.y
n2z = self.npc2.state.position.z
self.npc2Waypoints = []
self.npc2Waypoints.append(
lgsvl.DriveWaypoint(lgsvl.Vector(n2x + 50, n2y, n2z),
npc2Speed - 5)) # ready for left turn
self.npc2Waypoints.append(
lgsvl.DriveWaypoint(lgsvl.Vector(n2x + 65, n2y, n2z + 30),
npc2Speed))
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 test_multiple_lane_changes(self):
with SimConnection(120) as sim:
state = lgsvl.AgentState()
state.transform.position = lgsvl.Vector(-180, 10, 239)
state.transform.rotation = lgsvl.Vector(0, 90, 0)
sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguae2015xe_autowareai,
lgsvl.AgentType.EGO, state)
state = lgsvl.AgentState()
state.transform.position = lgsvl.Vector(-175, 10, 234.5)
state.transform.rotation = lgsvl.Vector(0, 90, 0.016)
npc = sim.add_agent("Sedan", lgsvl.AgentType.NPC, state)
npc.follow_closest_lane(True, 10)
agents = []
def on_lane_change(agent):
agents.append(agent)
npc.on_lane_change(on_lane_change)
sim.run(1)
npc.change_lane(True)
sim.run(10)
self.assertTrue(len(agents) == 1)
self.assertTrue(npc == agents[0])
self.assertAlmostEqual(npc.state.position.x, 238.5, delta=1.5)
npc.change_lane(True)
sim.run(13)
self.assertTrue(len(agents) == 2)
self.assertTrue(npc == agents[1])
self.assertAlmostEqual(npc.state.position.x, 242.5, delta=1.5)
npc.change_lane(False)
sim.run(10)
self.assertTrue(len(agents) == 3)
self.assertTrue(npc == agents[2])
self.assertAlmostEqual(npc.state.position.x, 238.5, delta=1.5)
def initEV(self):
sim = self.sim
egoState = lgsvl.AgentState()
egoState.transform = sim.map_point_on_lane(self.initEvPos)
ego = sim.add_agent("XE_Rigged-apollo_3_5", lgsvl.AgentType.EGO,
egoState)
sensors = ego.get_sensors()
for s in sensors:
if s.name in [
'velodyne', 'Main Camera', 'Telephoto Camera', 'GPS', 'IMU'
]:
s.enabled = True
self.ego = ego
def test_multiple_lane_changes(self):
with SimConnection(120) as sim:
state = lgsvl.AgentState()
state.transform.position = lgsvl.Vector(239, 10, 180)
state.transform.rotation = lgsvl.Vector(0, 180, 0)
sim.add_agent("XE_Rigged-apollo", lgsvl.AgentType.EGO, state)
state = lgsvl.AgentState()
state.transform.position = lgsvl.Vector(234.5, 10, 175)
state.transform.rotation = lgsvl.Vector(0.016, 180, 0)
npc = sim.add_agent("Sedan", lgsvl.AgentType.NPC, state)
npc.follow_closest_lane(True, 30)
agents = []
def on_lane_change(agent):
agents.append(agent)
npc.on_lane_change(on_lane_change)
sim.run(1)
npc.change_lane(True)
sim.run(10)
self.assertTrue(len(agents) == 1)
self.assertTrue(npc == agents[0])
self.assertAlmostEqual(npc.state.position.x, 238.5, delta=1.5)
npc.change_lane(True)
sim.run(13)
self.assertTrue(len(agents) == 2)
self.assertTrue(npc == agents[1])
self.assertAlmostEqual(npc.state.position.x, 242.5, delta=1.5)
npc.change_lane(False)
sim.run(10)
self.assertTrue(len(agents) == 3)
self.assertTrue(npc == agents[2])
self.assertAlmostEqual(npc.state.position.x, 238.5, delta=1.5)
def add_ego_car():
spawns = sim.get_spawn()
forward = lgsvl.utils.transform_to_forward(spawns[0])
right = lgsvl.utils.transform_to_right(spawns[0])
state = lgsvl.AgentState()
state.transform = spawns[0]
a = sim.add_agent(vehicle_name, lgsvl.AgentType.EGO, state)
a.connect_bridge(args.bridge, 9090)
print("Waiting for connection...")
while not a.bridge_connected:
time.sleep(1)
print("Bridge connected:", a.bridge_connected)
return a
def setup_collision(self, sim, mover_name, agent_type, still_name,
still_type):
state = spawnState(sim)
state.velocity = lgsvl.Vector(-50, 0, 0)
mover = sim.add_agent(mover_name, agent_type, state)
# school bus, 20m ahead, perpendicular to road, stopped
state = lgsvl.AgentState()
state.transform = sim.get_spawn()[0]
state.transform.position.x -= 20.0
state.transform.rotation.y = 0.0
still = sim.add_agent(still_name, still_type, state)
return mover, still
