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 get_pedesrian_waypoints(pedestrian):
waypoints = []
ped_pos = pedestrian.transform.position
wp1 = lgsvl.WalkWaypoint(lgsvl.Vector(ped_pos.x, ped_pos.y, ped_pos.z), 0)
wp2 = lgsvl.WalkWaypoint(
lgsvl.Vector(ped_pos.x - 40, ped_pos.y, ped_pos.z), 0)
waypoints.append(wp1)
waypoints.append(wp2)
return waypoints
def test_ped_circle_waypoints(self): # Check if pedestrians can follow waypoints
with SimConnection(60) as sim:
state = spawnState(sim)
forward = lgsvl.utils.transform_to_forward(state.transform)
right = lgsvl.utils.transform_to_right(state.transform)
state.transform.position = state.position - 4 * forward
sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state)
state = spawnState(sim)
state.transform.position = state.position + 10 * forward
radius = 5
count = 3
waypointCommands = []
waypoints = []
layer_mask = 0 | (1 << 0)
for i in range(count):
x = radius * math.cos(i * 2 * math.pi / count)
z = radius * math.sin(i * 2 * math.pi / count)
idle = 1 if i < count//2 else 0
hit = sim.raycast(state.position + z * forward + x * right, lgsvl.Vector(0, -1, 0), layer_mask)
waypointCommands.append(lgsvl.WalkWaypoint(hit.point, idle))
waypoints.append(hit.point)
state.transform.position = waypoints[0]
zoe = self.create_ped(sim, "Zoe", state)
def on_waypoint(agent,index):
msg = "Waypoint " + str(index)
mEqual(self, zoe.state.position, waypoints[index], msg)
if index == len(waypoints)-1:
sim.stop()
zoe.on_waypoint_reached(on_waypoint)
zoe.follow(waypointCommands, True)
sim.run()
def test_ped_circle_waypoints(
self): # Check if pedestrians can follow waypoints
with SimConnection(60) as sim:
sim.add_agent("XE_Rigged-apollo", lgsvl.AgentType.EGO,
spawnState(sim))
state = spawnState(sim)
sx = state.position.x - 20
sy = state.position.y
sz = state.position.z + 10
radius = 5
count = 3
waypointCommands = []
waypoints = []
for i in range(count):
x = radius * math.cos(i * 2 * math.pi / count)
z = radius * math.sin(i * 2 * math.pi / count)
idle = 1 if i < count // 2 else 0
waypointCommands.append(
lgsvl.WalkWaypoint(lgsvl.Vector(sx + x, sy, sz + z), idle))
waypoints.append(lgsvl.Vector(sx + x, sy, sz + z))
state.transform.position = waypoints[0]
zoe = self.create_ped(sim, "Zoe", state)
def on_waypoint(agent, index):
msg = "Waypoint " + str(index)
mEqual(self, zoe.state.position, waypoints[index], msg)
if index == len(waypoints) - 1:
sim.stop()
zoe.on_waypoint_reached(on_waypoint)
zoe.follow(waypointCommands, True)
sim.run()
def test_waypoint_idle_time(self):
with SimConnection(60) as sim:
sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO,
spawnState(sim))
state = spawnState(sim)
forward = lgsvl.utils.transform_to_forward(state.transform)
right = lgsvl.utils.transform_to_right(state.transform)
state.transform.position = state.position + 10 * forward
zoe = self.create_ped(sim, "Zoe", state)
def on_waypoint(agent, index):
sim.stop()
layer_mask = 0 | (1 << 0)
waypoints = []
hit = sim.raycast(state.position - 2 * right,
lgsvl.Vector(0, -1, 0), layer_mask)
waypoints.append(lgsvl.WalkWaypoint(hit.point, 0))
hit = sim.raycast(state.position - 5 * right,
lgsvl.Vector(0, -1, 0), layer_mask)
waypoints.append(lgsvl.WalkWaypoint(hit.point, 0))
zoe.on_waypoint_reached(on_waypoint)
zoe.follow(waypoints)
t0 = time.time()
sim.run() # reach the first waypoint
sim.run() # reach the second waypoint
t1 = time.time()
noIdleTime = t1 - t0
zoe.state = state
waypoints = []
hit = sim.raycast(state.position - 2 * right,
lgsvl.Vector(0, -1, 0), layer_mask)
waypoints.append(lgsvl.WalkWaypoint(hit.point, 2))
hit = sim.raycast(state.position - 5 * right,
lgsvl.Vector(0, -1, 0), layer_mask)
waypoints.append(lgsvl.WalkWaypoint(hit.point, 0))
zoe.follow(waypoints)
t2 = time.time()
sim.run() # reach the first waypoint
sim.run() # reach the second waypoint
t3 = time.time()
idleTime = t3 - t2
self.assertAlmostEqual(idleTime - noIdleTime, 2.0, delta=0.5)
def main():
sim_host_ip = "127.0.0.1" # of your local machine running sim
sim_host_port = 8181
sim_map = "Shalun"
ego_car_model = "Lexus2016RXHybrid (Autoware)" # Jaguar2015XE (Autoware)
ros_bridge_ip = "192.168.1.100" # 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 = 0.5 # 1 means normal speed, 1.5 means 1.5x speed
ego_pos = Vector(-32.6, 0, 45)
ego_roll = 0
ego_pitch = 0
ego_yaw = 160
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, spawn_tf)
pedestrian_pos = Vector(-35, 0, 30)
pedestrian_state = ObjectState(Transform(pedestrian_pos))
waypoints = [
lgsvl.WalkWaypoint(lgsvl.Vector(-16, 0, 34), 5, 0),
lgsvl.WalkWaypoint(lgsvl.Vector(-14, 0, 12), 5, 0),
lgsvl.WalkWaypoint(lgsvl.Vector(-30, 0, 12), 5, 0),
lgsvl.WalkWaypoint(lgsvl.Vector(-30, 0, 30), 5, 0),
]
createPedestrian(sim, pedestrian_state, waypoints)
input("Press Enter to run simulation")
sim.run(time_limit, time_scale)
def test_waypoint_idle_time(self):
with SimConnection(40) as sim:
sim.add_agent("XE_Rigged-apollo", lgsvl.AgentType.EGO,
spawnState(sim))
state = spawnState(sim)
sx = state.position.x - 20
sy = state.position.y
sz = state.position.z + 10
state.transform.position = lgsvl.Vector(sx, sy, sz)
zoe = self.create_ped(sim, "Zoe", state)
def on_waypoint(agent, index):
sim.stop()
waypoints = []
waypoints.append(
lgsvl.WalkWaypoint(lgsvl.Vector(sx - 2, sy, sz), 0))
waypoints.append(
lgsvl.WalkWaypoint(lgsvl.Vector(sx - 5, sy, sz), 0))
zoe.on_waypoint_reached(on_waypoint)
zoe.follow(waypoints)
t0 = time.time()
sim.run(5) # reach the first waypoint
sim.run(5) # reach the second waypoint
t1 = time.time()
noIdleTime = t1 - t0
zoe.state = state
waypoints = []
waypoints.append(
lgsvl.WalkWaypoint(lgsvl.Vector(sx - 2, sy, sz), 2))
waypoints.append(
lgsvl.WalkWaypoint(lgsvl.Vector(sx - 5, sy, sz), 0))
zoe.follow(waypoints)
t2 = time.time()
sim.run(5) # reach the first waypoint
sim.run(5) # reach the second waypoint
t3 = time.time()
idleTime = t3 - t2
self.assertAlmostEqual(idleTime - noIdleTime, 2.0, delta=0.5)
ego.set_destination(sim_dst)
res = ego.on_destination_reached(on_destination_reached)
right = lgsvl.utils.transform_to_right(egoState.transform)
forward = lgsvl.utils.transform_to_forward(egoState.transform)
pedState = lgsvl.AgentState()
pedState.transform = egoState.transform
pedState.transform.position += 2 * forward
ped = sim.add_agent("Robin", lgsvl.AgentType.PEDESTRIAN, pedState)
def on_collision(agent1, agent2, contact):
raise Exception("Failed: {} collided with {}".format(agent1.name, agent2.name))
ego.on_collision(on_collision)
ped.on_collision(on_collision)
waypoints = []
waypoints.append(lgsvl.WalkWaypoint(pedState.position, idle=0, trigger_distance=1.5))
waypoints.append(lgsvl.WalkWaypoint(pedState.position + 3 * forward, 0, speed=1))
ped.follow(waypoints)
print(f"Running simulation for {TIME_LIMIT} seconds...")
sim.run(TIME_LIMIT)
if is_reached:
print("PASSED")
else:
exit("FAILED: Ego did not reach the destination")
right = lgsvl.utils.transform_to_right(spawns[0])
state.velocity = dict[index % 3]['ego_speed'] * forward
ego = sim.add_agent("Lincoln2017MKZ (Apollo 5.0)", lgsvl.AgentType.EGO,
state)
# This will create waypoints in a circle for the pedestrian to follow
radius = 5
count = 8
wp = []
for i in range(count):
x = radius * math.cos(i * 2 * math.pi / count)
z = radius * math.sin(i * 2 * math.pi / count)
# idle is how much time the pedestrian will wait once it reaches the waypoint
idle = 1 if i < count // 2 else 0
wp.append(
lgsvl.WalkWaypoint(
spawns[1].position + (8 + x) * right + z * forward, idle))
state = lgsvl.AgentState()
state.transform = spawns[1]
state.transform.position = wp[0].position
p = sim.add_agent("Pamela", lgsvl.AgentType.PEDESTRIAN, state)
# This sends the list of waypoints to the pedestrian. The bool controls whether or not the pedestrian will continue walking (default false)
p.follow(wp, True)
# Even though the sedan is commanded to follow the lane, obstacle avoidance takes precedence and it will not drive into the bus
vehicles = {
ego: "EGO",
}
def main():
sim_host_ip = "127.0.0.1" # of your local machine running sim
sim_host_port = 8181
sim_map = "Shalun"
ego_car_model = "Jaguar2015XE (Autoware)" # Lexus2016RXHybrid (Autoware)
ros_bridge_ip = "192.168.2.18" # 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 = 5
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)
pedestrian_idle = 0
pedestrian_pos = Vector(-31, 0, 31)
pedestrian_state = ObjectState(Transform(pedestrian_pos))
waypoints = [
lgsvl.WalkWaypoint(lgsvl.Vector(-18, 0, 29), pedestrian_idle, 0),
lgsvl.WalkWaypoint(lgsvl.Vector(-12, 0, 12), pedestrian_idle, 0),
lgsvl.WalkWaypoint(lgsvl.Vector(-18, 0, 29), pedestrian_idle, 0),
lgsvl.WalkWaypoint(lgsvl.Vector(-31, 0, 2), pedestrian_idle, 0),
]
createPedestrian(sim, pedestrian_state, waypoints)
pedestrian_pos = Vector(-18, 0, 29)
pedestrian_state = ObjectState(Transform(pedestrian_pos))
waypoints = [
lgsvl.WalkWaypoint(lgsvl.Vector(-35, 0, 30), pedestrian_idle, 0),
lgsvl.WalkWaypoint(lgsvl.Vector(-18, 0, 29), pedestrian_idle, 0),
]
createPedestrian(sim, pedestrian_state, waypoints)
pedestrian_pos = Vector(-13, 0, 12)
pedestrian_state = ObjectState(Transform(pedestrian_pos))
waypoints = [
lgsvl.WalkWaypoint(lgsvl.Vector(-18, 0, 29), pedestrian_idle, 0),
lgsvl.WalkWaypoint(lgsvl.Vector(-35, 0, 30), pedestrian_idle, 0),
lgsvl.WalkWaypoint(lgsvl.Vector(-18, 0, 29), pedestrian_idle, 0),
lgsvl.WalkWaypoint(lgsvl.Vector(-12, 0, 12), pedestrian_idle, 0),
]
createPedestrian(sim, pedestrian_state, waypoints)
### OTHER SIDE
pedestrian_pos = Vector(11, 0, -18.5)
pedestrian_state = ObjectState(Transform(pedestrian_pos))
waypoints = [
lgsvl.WalkWaypoint(lgsvl.Vector(28, 0, -14), pedestrian_idle, 0),
lgsvl.WalkWaypoint(lgsvl.Vector(11, 0, -15), pedestrian_idle, 0),
]
createPedestrian(sim, pedestrian_state, waypoints)
pedestrian_pos = Vector(28, 0, -11.5)
pedestrian_state = ObjectState(Transform(pedestrian_pos))
waypoints = [
lgsvl.WalkWaypoint(lgsvl.Vector(11, 0, -19), pedestrian_idle, 0),
lgsvl.WalkWaypoint(lgsvl.Vector(28, 0, -12), pedestrian_idle, 0),
]
createPedestrian(sim, pedestrian_state, waypoints)
pedestrian_pos = Vector(30, 0, -13)
pedestrian_state = ObjectState(Transform(pedestrian_pos))
waypoints = [
lgsvl.WalkWaypoint(lgsvl.Vector(25, 0, -1), pedestrian_idle, 0),
lgsvl.WalkWaypoint(lgsvl.Vector(30, 0, -13), pedestrian_idle, 0),
]
createPedestrian(sim, pedestrian_state, waypoints)
pedestrian_pos = Vector(25, 0, -1)
pedestrian_state = ObjectState(Transform(pedestrian_pos))
waypoints = [
lgsvl.WalkWaypoint(lgsvl.Vector(30, 0, -13), pedestrian_idle, 0),
lgsvl.WalkWaypoint(lgsvl.Vector(25, 0, -1), pedestrian_idle, 0),
]
createPedestrian(sim, pedestrian_state, waypoints)
# PEDESTIRAN NEAR THE VEHICLE TO TEST AVOIDANCE
# pedestrian_pos = Vector(-32, 0, 57.5)
# pedestrian_state = ObjectState(Transform(pedestrian_pos))
# waypoints = [
# lgsvl.WalkWaypoint(lgsvl.Vector(-32, 0, 57.5), 1e10, 0),
# ]
# createPedestrian(sim, pedestrian_state, waypoints)
input("Press Enter to run simulation")
sim.run(time_limit, time_scale)
"Bob", "EntrepreneurFemale", "Howard", "Johny", "Pamela", "Presley",
"Robin", "Stephen", "Zoe"
]
for i in range(20 * 6):
# Create peds in a block
start = spawns[0].position + (5 +
(1.0 *
(i // 6))) * forward - (2 +
(1.0 *
(i % 6))) * right
end = start + 10 * forward
# Give waypoints for the spawn location and 10m ahead
wp = [
lgsvl.WalkWaypoint(start, 0),
lgsvl.WalkWaypoint(end, 0),
]
state = lgsvl.AgentState()
state.transform.position = start
state.transform.rotation = spawns[0].rotation
name = random.choice(names)
# Send the waypoints and make the pedestrian loop over the waypoints
p = sim.add_agent(name, lgsvl.AgentType.PEDESTRIAN, state)
p.follow(wp, True)
input("Press Enter to start")
sim.run()
sx = state.transform.position.x
sy = state.transform.position.y
sz = state.transform.position.z
names = [
"Bob", "Entrepreneur", "Howard", "Johnny", "Pamela", "Presley", "Robin",
"Stephen"
]
for i in range(20 * 8):
px = sx + 5 - (1.0 * (i // 8))
pz = sz + 6 + (1.0 * (i % 8))
wp = [
lgsvl.WalkWaypoint(lgsvl.Vector(px, sy, pz), 0),
lgsvl.WalkWaypoint(lgsvl.Vector(px - 10, sy, pz), 0),
]
state = lgsvl.AgentState()
state.transform = spawns[1]
state.transform.position.x = px
state.transform.position.z = pz
name = random.choice(names)
p = sim.add_agent(name, lgsvl.AgentType.PEDESTRIAN, state)
p.follow(wp, True)
input("enter to start")
sim.run()
# This block creates the list of waypoints that the pedestrian will follow
# Each waypoint is an position vector paired with the speed that the pedestrian will walk to
waypoints = []
trigger = None
speed = 3
hit = sim.raycast(
pedestrian_position + lgsvl.Vector(7.4, 0.0, -2.2),
lgsvl.Vector(0, -1, 0),
layer_mask,
)
effector = lgsvl.TriggerEffector("TimeToCollision", {})
trigger = lgsvl.WaypointTrigger([effector])
wp = lgsvl.WalkWaypoint(
position=hit.point, speed=speed, idle=0, trigger_distance=0, trigger=trigger
)
waypoints.append(wp)
hit = sim.raycast(
pedestrian_position + lgsvl.Vector(12.4, 0.0, -3.4),
lgsvl.Vector(0, -1, 0),
layer_mask,
)
wp = lgsvl.WalkWaypoint(
position=hit.point, speed=speed, idle=0, trigger_distance=0, trigger=None
)
waypoints.append(wp)
def on_waypoint(agent, index):
a = sim.add_agent("XE_Rigged-apollo", lgsvl.AgentType.EGO, state)
sx = state.position.x - 10
sy = state.position.y
sz = state.position.z + 10
# This will create waypoints in a circle for the pedestrian to follow
radius = 5
count = 8
wp = []
for i in range(count):
x = radius * math.cos(i * 2 * math.pi / count)
z = radius * math.sin(i * 2 * math.pi / count)
# idle is how much time the pedestrian will wait once it reaches the waypoint
idle = 1 if i < count // 2 else 0
wp.append(lgsvl.WalkWaypoint(lgsvl.Vector(sx + x, sy, sz + z), idle))
state = lgsvl.AgentState()
state.transform = spawns[1]
state.transform.position = wp[0].position
p = sim.add_agent("Bob", lgsvl.AgentType.PEDESTRIAN, state)
def on_waypoint(agent, index):
print("Waypoint {} reached".format(index))
p.on_waypoint_reached(on_waypoint)
# This sends the list of waypoints to the pedestrian. The bool controls whether or not the pedestrian will continue walking (default false)
sim.add_agent(
env.str("LGSVL__VEHICLE_0",
lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5),
lgsvl.AgentType.EGO, state)
# This will create waypoints in a circle for the pedestrian to follow
radius = 4
count = 8
wp = []
for i in range(count):
x = radius * math.cos(i * 2 * math.pi / count)
z = radius * math.sin(i * 2 * math.pi / count)
# idle is how much time the pedestrian will wait once it reaches the waypoint
idle = 1 if i < count // 2 else 0
wp.append(
lgsvl.WalkWaypoint(spawns[1].position + x * right + (z + 8) * forward,
idle))
state = lgsvl.AgentState()
state.transform = spawns[1]
state.transform.position = wp[0].position
p = sim.add_agent("Pamela", lgsvl.AgentType.PEDESTRIAN, state)
def on_waypoint(agent, index):
print("Waypoint {} reached".format(index))
p.on_waypoint_reached(on_waypoint)
# This sends the list of waypoints to the pedestrian. The bool controls whether or not the pedestrian will continue walking (default false)
forward = lgsvl.utils.transform_to_forward(egoState.transform)
pedState = lgsvl.AgentState()
pedState.transform.position = egoState.transform.position
pedState.transform.position += 5 * right + 5 * forward
ped = sim.add_agent("Robin", lgsvl.AgentType.PEDESTRIAN, pedState)
def on_collision(agent1, agent2, contact):
raise Exception("Failed: {} collided with {}".format(agent1, agent2))
ego.on_collision(on_collision)
ped.on_collision(on_collision)
waypoints = []
waypoints.append(
lgsvl.WalkWaypoint(pedState.position, idle=0, trigger_distance=6))
waypoints.append(
lgsvl.WalkWaypoint(pedState.transform.position - 5 * right, idle=15))
waypoints.append(
lgsvl.WalkWaypoint(pedState.transform.position - 10 * right, idle=0))
ped.follow(waypoints)
print(f"Running simulation for {TIME_LIMIT} seconds...")
sim.run(TIME_LIMIT)
if is_reached:
print("PASSED")
else:
exit("FAILED: Ego did not reach the destination")
def on_control_received(agent, kind, context):
global controlReceived
# There can be multiple custom callbacks defined, this checks for the appropriate kind
if kind == "checkControl":
# Stops the Simulator running, this will only interrupt the first sim.run(30) call
sim.stop()
controlReceived = True
ego.on_custom(on_control_received)
# Run Simulator for at most 30 seconds for the AD stack to to initialize
sim.run(30)
# If a Control message was not received, then the AD stack is not ready and the scenario should not continue
if not controlReceived:
raise Exception("AD stack is not ready")
sys.exit()
# Create the list of waypoints for the pedestrian to follow
waypoints = []
# Fist waypoint is the same position that the PED is spawned. The PED will wait here until the EGO is within 50m
waypoints.append(lgsvl.WalkWaypoint(position=pedState.position, idle=0, trigger_distance=50))
# Second waypoint is across the crosswalk
waypoints.append(lgsvl.WalkWaypoint(position=lgsvl.Vector(8.88, 0.2, 6.299), idle=0))
# List of waypoints is given to the pedestrian
ped.follow(waypoints)
# Simulation will run for 30 seconds
sim.run(30)
