[AUTO-7321] Add python API functions to send destination to Nav2

Author: daviduhm

lgsvl/__init__.py

@@ -4,7 +4,7 @@
 # This software contains code licensed as described in LICENSE.
 #
 
-from .geometry import Vector, BoundingBox, Transform
+from .geometry import Vector, BoundingBox, Transform, Quaternion
 from .simulator import Simulator, RaycastHit, WeatherState
 from .sensor import Sensor, CameraSensor, LidarSensor, ImuSensor
 from .agent import (

lgsvl/agent.py

@@ -4,7 +4,7 @@
 # This software contains code licensed as described in LICENSE.
 #
 
-from .geometry import Vector, BoundingBox
+from .geometry import Vector, BoundingBox, Transform
 from .sensor import Sensor
 from .utils import accepts, ObjectState as AgentState
 
@@ -222,6 +222,28 @@ def cancel_destination(self):
             }
         )
 
+    def set_initial_pose(self):
+        self.remote.command(
+            "vehicle/set_initial_pose",
+            {
+                "uid": self.uid,
+            }
+        )
+
+    @accepts(Transform)
+    def set_destination(self, transform):
+        self.remote.command(
+            "vehicle/set_destination",
+            {
+                "uid": self.uid,
+                "transform": transform.to_json(),
+            }
+        )
+
+    def on_destination_reached(self, fn):
+        self.remote.command("agent/on_destination_reached", {"uid": self.uid})
+        self.simulator._add_callback(self, "destination_reached", fn)
+
 
 class NpcVehicle(Vehicle):
     def __init__(self, uid, simulator):

lgsvl/geometry.py

@@ -132,3 +132,21 @@ def __repr__(self):
         return "Spawn(position={}, rotation={}, destinations={})".format(
             self.position, self.rotation, self.destinations
         )
+
+
+class Quaternion:
+    def __init__(self, x=0.0, y=0.0, z=0.0, w=0.0):
+        self.x = x
+        self.y = y
+        self.z = z
+        self.w = w
+
+    @staticmethod
+    def from_json(j):
+        return Quaternion(j["x"], j["y"], j["z"], j["w"])
+
+    def to_json(self):
+        return {"x": self.x, "y": self.y, "z": self.z, "w": self.w}
+
+    def __repr__(self):
+        return "Quaternion({}, {}, {}, {})".format(self.x, self.y, self.z, self.w)

lgsvl/simulator.py

@@ -7,7 +7,7 @@
 from .remote import Remote
 from .agent import Agent, AgentType, AgentState
 from .sensor import GpsData
-from .geometry import Vector, Transform, Spawn
+from .geometry import Vector, Transform, Spawn, Quaternion
 from .utils import accepts, ObjectState
 from .controllable import Controllable
 
@@ -131,6 +131,8 @@ def _process_events(self, events):
                                 fn(agent)
                             elif event_type == "lane_change":
                                 fn(agent)
+                            elif event_type == "destination_reached":
+                                fn(agent)
                             elif event_type == "custom":
                                 fn(agent, ev["kind"], ev["context"])
                             if self.stopped:
@@ -281,6 +283,17 @@ def map_point_on_lane(self, point):
         j = self.remote.command("map/point_on_lane", {"point": point.to_json()})
         return Transform.from_json(j)
 
+    @accepts(Vector, Quaternion)
+    def map_from_nav(self, position, orientation):
+        res = self.remote.command(
+            "map/from_nav",
+            {
+                "position": position.to_json(),
+                "orientation": orientation.to_json()
+            }
+        )
+        return Transform.from_json(res)
+
     @accepts(Vector, Vector, int, float)
     def raycast(self, origin, direction, layer_mask=-1, max_distance=float("inf")):
         hit = self.remote.command("simulator/raycast", [{

quickstart/36-send-destination-to-nav2.py

@@ -0,0 +1,70 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) 2021 LG Electronics, Inc.
+#
+# This software contains code licensed as described in LICENSE.
+#
+
+import lgsvl
+from environs import Env
+from lgsvl import Vector, Quaternion
+
+print("Python API Quickstart #36: Sending destinations to Navigation2")
+env = Env()
+
+sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port))
+
+# TODO: Make public assets for LGSeocho with NavOrigin and cloi sensor config
+map_seocho = "9b494638-b3e9-4239-9f3f-51b2c68cd292"
+ego_cloi = "4717598d-6c67-48a1-9a02-988a89d8660c"
+
+if sim.current_scene == map_seocho:
+    sim.reset()
+else:
+    sim.load(map_seocho)
+
+sim.set_time_of_day(0, fixed=True)
+spawns = sim.get_spawn()
+state = lgsvl.AgentState()
+state.transform = spawns[0]
+
+ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", ego_cloi), lgsvl.AgentType.EGO, state)
+ego.connect_bridge(env.str("LGSVL__AUTOPILOT_0_HOST", lgsvl.wise.SimulatorSettings.bridge_host), env.int("LGSVL__AUTOPILOT_0_PORT", lgsvl.wise.SimulatorSettings.bridge_port))
+
+i = 0
+
+# List of destinations in Nav2 costmap coordinates
+ros_destinations = [
+    ((3.118, -0.016, 0), (0, 0, 0.0004, 0.999)),
+    ((3.126, -10.025, 0), (0, 0, 0.006, 0.999)),
+    ((11.171, -9.875, 0), (0, 0, 0.707, 0.707)),
+    ((11.882, -0.057, 0), (0, 0, 0.699, 0.715)),
+    ((12.351, 11.820, 0), (0, 0, -0.999, 0.0004)),
+    ((2.965, 11.190, 0), (0, 0, -0.707, 0.707)),
+    ((2.687, -0.399, 0), (0, 0, -0.0036, 0.999)),
+]
+
+ego.set_initial_pose()
+sim.run(5)
+
+def send_next_destination(agent):
+    global i
+    print(f"{i}: {agent.name} reached destination")
+    i += 1
+    if i >= len(ros_destinations):
+        print("Iterated all destinations successfully. Stopping...")
+        sim.stop()
+        return
+
+    ros_dst = ros_destinations[i]
+    next_dst = sim.map_from_nav(Vector(*ros_dst[0]), Quaternion(*ros_dst[1]))
+    ego.set_destination(next_dst)
+
+ros_dst = ros_destinations[0]
+first_dst = sim.map_from_nav(Vector(*ros_dst[0]), Quaternion(*ros_dst[1]))
+ego.set_destination(first_dst)
+ego.on_destination_reached(send_next_destination)
+
+sim.run()
+
+print("Done!")