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)