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 _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 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 _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():
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)
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
sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", sim_host_ip),
sim_host_port)
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)
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 + 88 * forward + 55 * right
state.transform.rotation = spawns[0].rotation - Vector(0, 90, 0)
npcSpeed = 10
waypoints = [
DriveWaypoint(state.transform.position, npcSpeed,
state.transform.rotation, 0, False, 0),
DriveWaypoint(state.transform.position, npcSpeed,
state.transform.rotation, 20, False, 0)
]
pos = state.transform.position
increment = 100
start = 55
end = -25
for i in range(increment):
pos += (end - start) / increment * right
waypoints.append(
DriveWaypoint(pos, npcSpeed, state.transform.rotation, 0, False,
0))
addNpcAFollowWaypoints(sim, state, waypoints, True)
state.transform.position = spawns[0].position + 94 * forward + -70 * right
state.transform.rotation = spawns[0].rotation + Vector(0, 90, 0)
npcSpeed = 10
waypoints = [
DriveWaypoint(state.transform.position, npcSpeed,
state.transform.rotation, 0, False, 0),
DriveWaypoint(state.transform.position, npcSpeed,
state.transform.rotation, 20, False, 0)
]
pos = state.transform.position
increment = 100
start = -70
end = 50
for i in range(increment):
pos += (end - start) / increment * right
waypoints.append(
DriveWaypoint(pos, npcSpeed, state.transform.rotation, 0, False,
0))
addNpcAFollowWaypoints(sim, state, waypoints, True)
# start the sim
input("Press Enter to run simulation")
sim.run(time_limit, time_scale)
# Simulation Configuration
SIMULATOR_HOST = "127.0.0.1"
BRIDGE_HOST = "127.0.0.1"
SIM_INITIALIZE_TIME = 2
SIM_TIME_LIMIT = 30
# scenario configuration
with open('config.json') as json_file:
scenario_config = json.load(json_file)
with open('parameters.json') as json_file:
scenario_parameters = json.load(json_file)
# Intersections
ego_intersection_transform = Transform.from_json(scenario_config["ego_intersection"]['transform'])
agent_1_intersection_transform = Transform.from_json(scenario_config["agent_1_intersection"]['transform'])
agent_2_intersection_transform = Transform.from_json(scenario_config["agent_2_intersection"]['transform'])
# Waypoints
with open('waypoints.json') as json_file:
scenario_waypoints = json.load(json_file)
# Agent 1 Way Points
agent_1_waypoints = []
for waypoint in scenario_waypoints['agent_1']:
agent_1_waypoints.append(lgsvl.agent.DriveWaypoint(Vector.from_json(waypoint['position']),
waypoint['speed'],
Vector.from_json(waypoint['angle']),
waypoint['idle'],
waypoint['deactivate'],