API controllables

Author: EricBoiseLGSVL

lgsvl/__init__.py

@@ -7,5 +7,6 @@
 from .geometry import Vector, BoundingBox, Transform
 from .simulator import Simulator, RaycastHit, WeatherState
 from .sensor import Sensor, CameraSensor, LidarSensor, ImuSensor
-from .agent import AgentType, VehicleControl, AgentState, Vehicle, EgoVehicle, NpcVehicle, Pedestrian, DriveWaypoint, WalkWaypoint, NPCControl
+from .agent import AgentType, AgentState, VehicleControl, Vehicle, EgoVehicle, NpcVehicle, Pedestrian, DriveWaypoint, WalkWaypoint, NPCControl
 from .controllable import Controllable
+from .utils import ObjectState

lgsvl/agent.py

@@ -6,7 +6,7 @@
 
 from .geometry import Vector, Transform, BoundingBox
 from .sensor import Sensor
-from .utils import accepts
+from .utils import accepts, ObjectState as AgentState
 
 from enum import Enum
 from collections.abc import Iterable, Callable
@@ -55,54 +55,6 @@ def __init__(self):
     self.turn_signal_left = None  # bool
     self.turn_signal_right = None # bool
 
-
-class AgentState:
-  def __init__(self, transform = None, velocity = None, angular_velocity = None):
-    if transform is None: transform = Transform()
-    if velocity is None: velocity = Vector()
-    if angular_velocity is None: angular_velocity = Vector()
-    self.transform = transform
-    self.velocity = velocity
-    self.angular_velocity = angular_velocity
-
-  @property
-  def position(self):
-    return self.transform.position
-
-  @property
-  def rotation(self):
-    return self.transform.rotation
-
-  @property
-  def speed(self):
-    return math.sqrt(
-      self.velocity.x * self.velocity.x +
-      self.velocity.y * self.velocity.y +
-      self.velocity.z * self.velocity.z)
-
-  @staticmethod
-  def from_json(j):
-    return AgentState(
-      Transform.from_json(j["transform"]),
-      Vector.from_json(j["velocity"]),
-      Vector.from_json(j["angular_velocity"]),
-    )
-
-  def to_json(self):
-    return {
-      "transform": self.transform.to_json(),
-      "velocity": self.velocity.to_json(),
-      "angular_velocity": self.angular_velocity.to_json(),
-    }
-
-  def __repr__(self):
-    return str({
-      "transform": str(self.transform),
-      "velocity": str(self.velocity),
-      "angular_velocity": str(self.angular_velocity),
-    })
-
-
 class Agent:
   def __init__(self, uid, simulator):
     self.uid = uid
@@ -335,3 +287,4 @@ def follow(self, waypoints, loop = False):
   def on_waypoint_reached(self, fn):
     self.remote.command("agent/on_waypoint_reached", {"uid": self.uid})
     self.simulator._add_callback(self, "waypoint_reached", fn)
+	

lgsvl/controllable.py

@@ -4,8 +4,8 @@
 # This software contains code licensed as described in LICENSE.
 #
 
-from .geometry import Transform
-from .utils import accepts
+from .geometry import Vector, Transform
+from .utils import accepts, ObjectState
 
 class Controllable:
   def __init__(self, remote, j):
@@ -16,6 +16,19 @@ def __init__(self, remote, j):
     self.valid_actions = j["valid_actions"]
     self.default_control_policy = j["default_control_policy"]
 
+  @property
+  def object_state(self):
+    j = self.remote.command("controllable/object_state/get", {"uid": self.uid})
+    return ObjectState.from_json(j)
+
+  @object_state.setter
+  @accepts(ObjectState)
+  def object_state(self, object_state):
+    self.remote.command("controllable/object_state/set", {
+      "uid": self.uid,
+      "state": object_state.to_json()
+    })
+
   @property
   def current_state(self):
     j = self.remote.command("controllable/current_state/get", {"uid": self.uid})

lgsvl/simulator.py

@@ -8,7 +8,7 @@
 from .agent import Agent, AgentType, AgentState
 from .sensor import GpsData
 from .geometry import Vector, Transform
-from .utils import accepts
+from .utils import accepts, ObjectState
 from .controllable import Controllable
 
 from collections import namedtuple
@@ -240,6 +240,20 @@ def raycast_batch(self, args):
 
     return results
 
+  @accepts(str, ObjectState)
+  def controllable_add(self, name, object_state = None):
+    if object_state is None: object_state = ObjectState()
+    args = {"name": name, "state": object_state.to_json()}
+    j = self.remote.command("simulator/controllable_add", args)
+    controllable = Controllable(self.remote, j)
+    controllable.name = name
+    return controllable
+
+  @accepts(Controllable)
+  def controllable_remove(self, controllable):
+    self.remote.command("simulator/controllable_remove", {"uid": controllable.uid})
+    del self.controllables[controllable.uid]
+
   @accepts(str)
   def get_controllables(self, control_type = None):
     j = self.remote.command("controllable/get/all", {

lgsvl/utils.py

@@ -23,6 +23,51 @@ def new_f(*args, **kwargs):
     return new_f
   return check_accepts
 
+class ObjectState:
+  def __init__(self, transform = None, velocity = None, angular_velocity = None):
+    if transform is None: transform = Transform()
+    if velocity is None: velocity = Vector()
+    if angular_velocity is None: angular_velocity = Vector()
+    self.transform = transform
+    self.velocity = velocity
+    self.angular_velocity = angular_velocity
+
+  @property
+  def position(self):
+    return self.transform.position
+
+  @property
+  def rotation(self):
+    return self.transform.rotation
+
+  @property
+  def speed(self):
+    return math.sqrt(
+      self.velocity.x * self.velocity.x +
+      self.velocity.y * self.velocity.y +
+      self.velocity.z * self.velocity.z)
+
+  @staticmethod
+  def from_json(j):
+    return ObjectState(
+      Transform.from_json(j["transform"]),
+      Vector.from_json(j["velocity"]),
+      Vector.from_json(j["angular_velocity"]),
+    )
+
+  def to_json(self):
+    return {
+      "transform": self.transform.to_json(),
+      "velocity": self.velocity.to_json(),
+      "angular_velocity": self.angular_velocity.to_json(),
+    }
+
+  def __repr__(self):
+    return str({
+      "transform": str(self.transform),
+      "velocity": str(self.velocity),
+      "angular_velocity": str(self.angular_velocity),
+    })
 
 def transform_to_matrix(tr):
   px = tr.position.x

quickstart/28-control-traffic-cone.py

@@ -0,0 +1,55 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) 2020 LG Electronics, Inc.
+#
+# This software contains code licensed as described in LICENSE.
+#
+
+import os
+import lgsvl
+
+sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181)
+
+scene_name = "CubeTown"
+
+if sim.current_scene == scene_name:
+  sim.reset()
+else:
+  sim.load(scene_name, 42)
+
+spawns = sim.get_spawn()
+
+state = lgsvl.AgentState()
+forward = lgsvl.utils.transform_to_forward(spawns[0])
+right = lgsvl.utils.transform_to_right(spawns[0])
+up = lgsvl.utils.transform_to_up(spawns[0])
+state.transform = spawns[0]
+
+ego = sim.add_agent("Lincoln2017MKZ (Apollo 5.0)", lgsvl.AgentType.EGO, state)
+
+print("Python API Quickstart #28: How to Add/Control Traffic Cone")
+
+for i in range(10*3):
+  # Create controllables in a block
+  start = spawns[0].position + (5 + (1.0 * (i//6))) * forward - (2 + (1.0 * (i % 6))) * right
+  end = start + 10 * forward
+
+  state = lgsvl.ObjectState()
+  state.transform.position = start
+  state.transform.rotation = spawns[0].rotation
+  # Set velocity and angular_velocity
+  state.velocity = 10 * up
+  state.angular_velocity = 6.5 * right
+  
+  # add controllable
+  o = sim.controllable_add("TrafficCone", state)
+  
+
+print("\nAdded {} Traffic Cones".format(i + 1))
+
+seconds = 10
+input("\nPress Enter to run simulation for {} seconds".format(seconds))
+print("\nRunning simulation for {} seconds...".format(seconds))
+sim.run(seconds)
+
+print("\nDone!")