Skip to content

Commit 9bc29c9

Browse files
daviduhmdaviduhm
committed
[AUTO-7654] Add example scripts for Navigation2 stack with Cloi
1 parent 05f610b commit 9bc29c9

File tree

4 files changed

+302
-0
lines changed

4 files changed

+302
-0
lines changed

‎examples/Nav2/single-robot-cut-in.py

+81
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,81 @@
1+
#!/usr/bin/env python3
2+
#
3+
# Copyright (c) 2021 LG Electronics, Inc.
4+
#
5+
# This software contains code licensed as described in LICENSE.
6+
#
7+
8+
import os
9+
import lgsvl
10+
11+
print("Navigation2 Example: Single Robot Cut In")
12+
13+
TIME_DELAY = 5
14+
TIME_LIMIT = 70
15+
NAV_DST = ((10.9, -0, 0), (0, 0, 0, 1.0))
16+
17+
SIMULATOR_HOST = os.environ.get("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host)
18+
SIMULATOR_PORT = int(os.environ.get("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port))
19+
BRIDGE_HOST = os.environ.get("LGSVL__AUTOPILOT_0_HOST", lgsvl.wise.SimulatorSettings.bridge_host)
20+
BRIDGE_PORT = int(os.environ.get("LGSVL__AUTOPILOT_0_PORT", lgsvl.wise.SimulatorSettings.bridge_port))
21+
22+
SCENE = os.environ.get("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_lgseocho)
23+
ROBOT = os.environ.get("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lgcloi_navigation2)
24+
25+
sim = lgsvl.Simulator(SIMULATOR_HOST, SIMULATOR_PORT)
26+
if sim.current_scene == SCENE:
27+
sim.reset()
28+
else:
29+
sim.load(SCENE, seed=0)
30+
31+
spawns = sim.get_spawn()
32+
egoState = lgsvl.AgentState()
33+
egoState.transform = spawns[0]
34+
35+
ego = sim.add_agent(ROBOT, lgsvl.AgentType.EGO, egoState)
36+
ego.connect_bridge(BRIDGE_HOST, BRIDGE_PORT)
37+
ego.set_initial_pose()
38+
sim.run(TIME_DELAY)
39+
is_reached = False
40+
41+
42+
def on_destination_reached(agent):
43+
global is_reached
44+
is_reached = True
45+
print(f"Robot reached destination")
46+
sim.stop()
47+
return
48+
49+
50+
sim_dst = sim.map_from_nav(lgsvl.Vector(*NAV_DST[0]), lgsvl.Quaternion(*NAV_DST[1]))
51+
ego.set_destination(sim_dst)
52+
res = ego.on_destination_reached(on_destination_reached)
53+
54+
right = lgsvl.utils.transform_to_right(egoState.transform)
55+
forward = lgsvl.utils.transform_to_forward(egoState.transform)
56+
57+
pedState = lgsvl.AgentState()
58+
pedState.transform.position = egoState.transform.position
59+
pedState.transform.position += 5 * right + 5 * forward
60+
ped = sim.add_agent("Robin", lgsvl.AgentType.PEDESTRIAN, pedState)
61+
62+
63+
def on_collision(agent1, agent2, contact):
64+
raise Exception("Failed: {} collided with {}".format(agent1, agent2))
65+
66+
67+
ego.on_collision(on_collision)
68+
ped.on_collision(on_collision)
69+
waypoints = []
70+
waypoints.append(lgsvl.WalkWaypoint(pedState.position, idle=0, trigger_distance=6))
71+
waypoints.append(lgsvl.WalkWaypoint(pedState.transform.position - 5 * right, idle=15))
72+
waypoints.append(lgsvl.WalkWaypoint(pedState.transform.position - 10 * right, idle=0))
73+
ped.follow(waypoints)
74+
75+
print(f"Running simulation for {TIME_LIMIT} seconds...")
76+
sim.run(TIME_LIMIT)
77+
78+
if is_reached:
79+
print("PASSED")
80+
else:
81+
exit("FAILED: Ego did not reach the destination")

‎examples/Nav2/single-robot-freemode.py

+60
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,60 @@
1+
#!/usr/bin/env python3
2+
#
3+
# Copyright (c) 2021 LG Electronics, Inc.
4+
#
5+
# This software contains code licensed as described in LICENSE.
6+
#
7+
8+
import os
9+
import lgsvl
10+
11+
print("Navigation2 Example: Single Robot Freemode")
12+
13+
TIME_DELAY = 5
14+
TIME_LIMIT = 70
15+
NAV_DST = ((10.9, -0, 0), (0, 0, 0, 1.0))
16+
17+
SIMULATOR_HOST = os.environ.get("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host)
18+
SIMULATOR_PORT = int(os.environ.get("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port))
19+
BRIDGE_HOST = os.environ.get("LGSVL__AUTOPILOT_0_HOST", lgsvl.wise.SimulatorSettings.bridge_host)
20+
BRIDGE_PORT = int(os.environ.get("LGSVL__AUTOPILOT_0_PORT", lgsvl.wise.SimulatorSettings.bridge_port))
21+
22+
SCENE = os.environ.get("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_lgseocho)
23+
ROBOT = os.environ.get("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lgcloi_navigation2)
24+
25+
sim = lgsvl.Simulator(SIMULATOR_HOST, SIMULATOR_PORT)
26+
if sim.current_scene == SCENE:
27+
sim.reset()
28+
else:
29+
sim.load(SCENE, seed=0)
30+
31+
spawns = sim.get_spawn()
32+
state = lgsvl.AgentState()
33+
state.transform = spawns[0]
34+
35+
ego = sim.add_agent(ROBOT, lgsvl.AgentType.EGO, state)
36+
ego.connect_bridge(BRIDGE_HOST, BRIDGE_PORT)
37+
ego.set_initial_pose()
38+
sim.run(TIME_DELAY)
39+
is_reached = False
40+
41+
42+
def on_destination_reached(agent):
43+
global is_reached
44+
is_reached = True
45+
print(f"Robot reached destination")
46+
sim.stop()
47+
return
48+
49+
50+
sim_dst = sim.map_from_nav(lgsvl.Vector(*NAV_DST[0]), lgsvl.Quaternion(*NAV_DST[1]))
51+
ego.set_destination(sim_dst)
52+
res = ego.on_destination_reached(on_destination_reached)
53+
54+
print(f"Running simulation for {TIME_LIMIT} seconds...")
55+
sim.run(TIME_LIMIT)
56+
57+
if is_reached:
58+
print("PASSED")
59+
else:
60+
exit("FAILED: Ego did not reach the destination")

‎examples/Nav2/single-robot-sudden-braking.py

+80
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,80 @@
1+
#!/usr/bin/env python3
2+
#
3+
# Copyright (c) 2021 LG Electronics, Inc.
4+
#
5+
# This software contains code licensed as described in LICENSE.
6+
#
7+
8+
import os
9+
import lgsvl
10+
11+
print("Navigation2 Example: Single Robot Sudden Braking")
12+
13+
TIME_DELAY = 5
14+
TIME_LIMIT = 70
15+
NAV_DST = ((10.9, -0, 0), (0, 0, 0, 1.0))
16+
17+
SIMULATOR_HOST = os.environ.get("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host)
18+
SIMULATOR_PORT = int(os.environ.get("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port))
19+
BRIDGE_HOST = os.environ.get("LGSVL__AUTOPILOT_0_HOST", lgsvl.wise.SimulatorSettings.bridge_host)
20+
BRIDGE_PORT = int(os.environ.get("LGSVL__AUTOPILOT_0_PORT", lgsvl.wise.SimulatorSettings.bridge_port))
21+
22+
SCENE = os.environ.get("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_lgseocho)
23+
ROBOT = os.environ.get("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lgcloi_navigation2)
24+
25+
sim = lgsvl.Simulator(SIMULATOR_HOST, SIMULATOR_PORT)
26+
if sim.current_scene == SCENE:
27+
sim.reset()
28+
else:
29+
sim.load(SCENE, seed=0)
30+
31+
spawns = sim.get_spawn()
32+
egoState = lgsvl.AgentState()
33+
egoState.transform = spawns[0]
34+
35+
ego = sim.add_agent(ROBOT, lgsvl.AgentType.EGO, egoState)
36+
ego.connect_bridge(BRIDGE_HOST, BRIDGE_PORT)
37+
ego.set_initial_pose()
38+
sim.run(TIME_DELAY)
39+
is_reached = False
40+
41+
42+
def on_destination_reached(agent):
43+
global is_reached
44+
is_reached = True
45+
print(f"Robot reached destination")
46+
sim.stop()
47+
return
48+
49+
50+
sim_dst = sim.map_from_nav(lgsvl.Vector(*NAV_DST[0]), lgsvl.Quaternion(*NAV_DST[1]))
51+
ego.set_destination(sim_dst)
52+
res = ego.on_destination_reached(on_destination_reached)
53+
54+
right = lgsvl.utils.transform_to_right(egoState.transform)
55+
forward = lgsvl.utils.transform_to_forward(egoState.transform)
56+
57+
pedState = lgsvl.AgentState()
58+
pedState.transform = egoState.transform
59+
pedState.transform.position += 2 * forward
60+
ped = sim.add_agent("Robin", lgsvl.AgentType.PEDESTRIAN, pedState)
61+
62+
63+
def on_collision(agent1, agent2, contact):
64+
raise Exception("Failed: {} collided with {}".format(agent1.name, agent2.name))
65+
66+
67+
ego.on_collision(on_collision)
68+
ped.on_collision(on_collision)
69+
waypoints = []
70+
waypoints.append(lgsvl.WalkWaypoint(pedState.position, idle=0, trigger_distance=1.5))
71+
waypoints.append(lgsvl.WalkWaypoint(pedState.position + 3 * forward, 0, speed=1))
72+
ped.follow(waypoints)
73+
74+
print(f"Running simulation for {TIME_LIMIT} seconds...")
75+
sim.run(TIME_LIMIT)
76+
77+
if is_reached:
78+
print("PASSED")
79+
else:
80+
exit("FAILED: Ego did not reach the destination")

‎examples/Nav2/single-robot-traffic-cones.py

+81
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,81 @@
1+
#!/usr/bin/env python3
2+
#
3+
# Copyright (c) 2021 LG Electronics, Inc.
4+
#
5+
# This software contains code licensed as described in LICENSE.
6+
#
7+
8+
import os
9+
import lgsvl
10+
11+
print("Navigation2 Example: Single Robot Traffic Cones")
12+
13+
TIME_DELAY = 5
14+
TIME_LIMIT = 90
15+
NAV_DST = ((10.9, -0, 0), (0, 0, 0, 1.0))
16+
17+
SIMULATOR_HOST = os.environ.get("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host)
18+
SIMULATOR_PORT = int(os.environ.get("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port))
19+
BRIDGE_HOST = os.environ.get("LGSVL__AUTOPILOT_0_HOST", lgsvl.wise.SimulatorSettings.bridge_host)
20+
BRIDGE_PORT = int(os.environ.get("LGSVL__AUTOPILOT_0_PORT", lgsvl.wise.SimulatorSettings.bridge_port))
21+
22+
SCENE = os.environ.get("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_lgseocho)
23+
ROBOT = os.environ.get("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lgcloi_navigation2)
24+
25+
sim = lgsvl.Simulator(SIMULATOR_HOST, SIMULATOR_PORT)
26+
if sim.current_scene == SCENE:
27+
sim.reset()
28+
else:
29+
sim.load(SCENE, seed=0)
30+
31+
spawns = sim.get_spawn()
32+
state = lgsvl.AgentState()
33+
state.transform = spawns[0]
34+
35+
ego = sim.add_agent(ROBOT, lgsvl.AgentType.EGO, state)
36+
ego.connect_bridge(BRIDGE_HOST, BRIDGE_PORT)
37+
ego.set_initial_pose()
38+
sim.run(TIME_DELAY)
39+
is_reached = False
40+
41+
42+
def on_destination_reached(agent):
43+
global is_reached
44+
is_reached = True
45+
print(f"Robot reached destination")
46+
sim.stop()
47+
return
48+
49+
50+
sim_dst = sim.map_from_nav(lgsvl.Vector(*NAV_DST[0]), lgsvl.Quaternion(*NAV_DST[1]))
51+
ego.set_destination(sim_dst)
52+
res = ego.on_destination_reached(on_destination_reached)
53+
54+
right = lgsvl.utils.transform_to_right(state.transform)
55+
forward = lgsvl.utils.transform_to_forward(state.transform)
56+
57+
sign = 1
58+
for j in range(2, 11, 2):
59+
sign *= -1
60+
for i in range(5):
61+
objState = lgsvl.ObjectState()
62+
objState.transform.position = state.transform.position
63+
offset = sign * 0.5 * i * right + j * forward
64+
objState.transform.position += offset
65+
sim.controllable_add("TrafficCone", objState)
66+
67+
68+
def on_collision(agent1, agent2, contact):
69+
name1 = "traffic cone" if agent1 is None else agent1.name
70+
name2 = "traffic cone" if agent2 is None else agent2.name
71+
raise Exception("Failed: {} collided with {}".format(name1, name2))
72+
73+
74+
ego.on_collision(on_collision)
75+
print(f"Running simulation for {TIME_LIMIT} seconds...")
76+
sim.run(TIME_LIMIT)
77+
78+
if is_reached:
79+
print("PASSED")
80+
else:
81+
exit("FAILED: Ego did not reach the destination")

0 commit comments

Comments
 (0)