Extend API waypoint mode to include angle

Author: hadiTab

lgsvl/agent.py

@@ -13,7 +13,7 @@
 from collections.abc import Iterable, Callable
 import math
 
-DriveWaypoint = namedtuple("DriveWaypoint", "position speed")
+DriveWaypoint = namedtuple("DriveWaypoint", "position speed angle idle trigger_distance")
 WalkWaypoint = namedtuple("WalkWaypoint", "position idle")
 
 class AgentType(Enum):
@@ -202,7 +202,7 @@ def __init__(self, uid, simulator):
   def follow(self, waypoints, loop = False):
     self.remote.command("vehicle/follow_waypoints", {
       "uid": self.uid,
-      "waypoints": [{"position": wp.position.to_json(), "speed": wp.speed} for wp in waypoints],
+      "waypoints": [{"position": wp.position.to_json(), "speed": wp.speed, "angle": wp.angle.to_json(), "idle": wp.idle, "trigger_distance": wp.trigger_distance} for wp in waypoints],
       "loop": loop,
     })
 

quickstart/13-npc-follow-waypoints.py

@@ -8,6 +8,7 @@
 import os
 import lgsvl
 import math
+import copy
 
 sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181)
 if sim.current_scene == "BorregasAve":
@@ -21,7 +22,9 @@
 
 state = lgsvl.AgentState()
 state.transform = spawns[0]
-a = sim.add_agent("Jaguar2015XE (Apollo 5.0)", lgsvl.AgentType.EGO, state)
+state2 = copy.deepcopy(state)
+state2.transform.position.z += 50
+a = sim.add_agent("Jaguar2015XE (Apollo 5.0)", lgsvl.AgentType.EGO, state2)
 
 # NPC, 10 meters ahead
 
@@ -35,7 +38,20 @@
 state.transform.position.z = sz
 npc = sim.add_agent("Sedan", lgsvl.AgentType.NPC, state)
 
-# snake-drive
+vehicles = {
+  a: "EGO",
+  npc: "Sedan",
+}
+
+# Executed upon receiving collision callback -- NPC is expected to drive through colliding objects
+def on_collision(agent1, agent2, contact):
+  name1 = vehicles[agent1]
+  name2 = vehicles[agent2] if agent2 is not None else "OBSTACLE"
+  print("{} collided with {}".format(name1, name2))
+
+a.on_collision(on_collision)
+npc.on_collision(on_collision)
+
 # This block creates the list of waypoints that the NPC will follow
 # Each waypoint is an position vector paired with the speed that the NPC will drive to it
 waypoints = []
@@ -46,18 +62,19 @@
 layer_mask |= 1 << 0 # 0 is the layer for the road (default)
 
 for i in range(20):
-  speed = 6 if i % 2 == 0 else 12
-  px = x_max * (-1 if i % 2 == 0 else 1)
+  speed = 24# if i % 2 == 0 else 12
+  px = 0
   pz = (i + 1) * z_delta
-
+  # Waypoint angles are input as Euler angles (roll, pitch, yaw)
+  angle = lgsvl.Vector(0, 0, 0)
   # Raycast the points onto the ground because BorregasAve is not flat
   hit = sim.raycast(lgsvl.Vector(sx + px, sy, sz + pz), lgsvl.Vector(0,-1,0), layer_mask) 
 
-  wp = lgsvl.DriveWaypoint(hit.point, speed)
-  #wp = lgsvl.DriveWaypoint(lgsvl.Vector(sx + px, sy, sz + pz), speed)
+  # NPC will wait for 1 second at each waypoint
+  wp = lgsvl.DriveWaypoint(hit.point, speed, angle, 1, 0)
   waypoints.append(wp)
 
-# When the NPC is within 1m of the waypoint, this will be called
+# When the NPC is within 0.5m of the waypoint, this will be called
 def on_waypoint(agent, index):
   print("waypoint {} reached".format(index))
 

quickstart/25-waypoint-flying-npc.py

@@ -0,0 +1,78 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) 2019 LG Electronics, Inc.
+#
+# This software contains code licensed as described in LICENSE.
+#
+
+import os
+import lgsvl
+import math
+
+sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181)
+if sim.current_scene == "BorregasAve":
+  sim.reset()
+else:
+  sim.load("BorregasAve")
+
+spawns = sim.get_spawn()
+
+# EGO
+
+state = lgsvl.AgentState()
+state.transform = spawns[0]
+a = sim.add_agent("Jaguar2015XE (Apollo 5.0)", lgsvl.AgentType.EGO, state)
+
+# NPC, 10 meters ahead
+
+sx = spawns[0].position.x
+sy = spawns[0].position.y
+sz = spawns[0].position.z + 10.0
+
+state = lgsvl.AgentState()
+state.transform = spawns[0]
+state.transform.position.x = sx
+state.transform.position.z = sz
+npc = sim.add_agent("Sedan", lgsvl.AgentType.NPC, state)
+
+
+# This block creates the list of waypoints that the NPC will follow
+# Each waypoint consists of a position vector, speed, and an angular orientation vector
+waypoints = []
+x_max = 2
+y_max = 2
+z_delta = 12
+
+layer_mask = 0
+layer_mask |= 1 << 0 # 0 is the layer for the road (default)
+px = 0
+py = 0.5
+
+for i in range(75):
+  speed = 6
+  px = 0
+  py = ((i + 1) * 0.05) if (i < 50) else (50 * 0.05 + (50 - i) * 0.05)
+  pz = i * z_delta * 0.05
+
+  angle = lgsvl.Vector(i, 0, 3*i)
+
+  # Raycast the points onto the ground because BorregasAve is not flat
+  hit = sim.raycast(lgsvl.Vector(sx + px, sy, sz + pz), lgsvl.Vector(0,-1,0), layer_mask) 
+
+  wp = lgsvl.DriveWaypoint(lgsvl.Vector(sx + px, sy + py, sz + pz), speed, angle, 0, 0)
+  waypoints.append(wp)
+
+# When the NPC is within 1m of the waypoint, this will be called
+def on_waypoint(agent, index):
+  print("waypoint {} reached".format(index))
+
+# The above function needs to be added to the list of callbacks for the NPC
+npc.on_waypoint_reached(on_waypoint)
+
+# The NPC needs to be given the list of waypoints. 
+# A bool can be passed as the 2nd argument that controls whether or not the NPC loops over the waypoints (default false)
+npc.follow(waypoints)
+
+input("Press Enter to run")
+
+sim.run()

quickstart/26-npc-trigger-waypoints.py

@@ -0,0 +1,90 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) 2019 LG Electronics, Inc.
+#
+# This software contains code licensed as described in LICENSE.
+#
+
+import os
+import lgsvl
+import math
+
+sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181)
+if sim.current_scene == "BorregasAve":
+  sim.reset()
+else:
+  sim.load("BorregasAve")
+
+spawns = sim.get_spawn()
+
+# EGO
+
+state = lgsvl.AgentState()
+state.transform = spawns[0]
+a = sim.add_agent("Jaguar2015XE (Apollo 5.0)", lgsvl.AgentType.EGO, state)
+
+# NPC, 10 meters ahead
+
+sx = spawns[0].position.x
+sy = spawns[0].position.y
+sz = spawns[0].position.z + 10.0
+
+state = lgsvl.AgentState()
+state.transform = spawns[0]
+state.transform.position.x = sx
+state.transform.position.z = sz
+npc = sim.add_agent("Sedan", lgsvl.AgentType.NPC, state)
+
+vehicles = {
+  a: "EGO",
+  npc: "Sedan",
+}
+
+# Executed upon receiving collision callback -- NPC is expected to drive through colliding objects
+def on_collision(agent1, agent2, contact):
+  name1 = vehicles[agent1]
+  name2 = vehicles[agent2] if agent2 is not None else "OBSTACLE"
+  print("{} collided with {}".format(name1, name2))
+
+a.on_collision(on_collision)
+npc.on_collision(on_collision)
+
+# This block creates the list of waypoints that the NPC will follow
+# Each waypoint is an position vector paired with the speed that the NPC will drive to it
+waypoints = []
+x_max = 2
+z_delta = 12
+
+layer_mask = 0
+layer_mask |= 1 << 0 # 0 is the layer for the road (default)
+
+for i in range(20):
+  speed = 24# if i % 2 == 0 else 12
+  px = 0
+  pz = (i + 1) * z_delta
+  # Waypoint angles are input as Euler angles (roll, pitch, yaw)
+  angle = lgsvl.Vector(0, 0, 0)
+  # Raycast the points onto the ground because BorregasAve is not flat
+  hit = sim.raycast(lgsvl.Vector(sx + px, sy, sz + pz), lgsvl.Vector(0,-1,0), layer_mask) 
+
+  # Trigger is set to 10 meters for every other waypoint (0 means no trigger)
+  tr = 0
+  if (i % 2):
+    tr = 10 
+  wp = lgsvl.DriveWaypoint(hit.point, speed, angle, 0, tr)
+  waypoints.append(wp)
+
+# When the NPC is within 0.5m of the waypoint, this will be called
+def on_waypoint(agent, index):
+  print("waypoint {} reached".format(index))
+
+# The above function needs to be added to the list of callbacks for the NPC
+npc.on_waypoint_reached(on_waypoint)
+
+# The NPC needs to be given the list of waypoints. 
+# A bool can be passed as the 2nd argument that controls whether or not the NPC loops over the waypoints (default false)
+npc.follow(waypoints)
+
+input("Press Enter to run")
+
+sim.run()