From 9aaad18d0e37b78d6cc58f0e2f9140c9229e71cd Mon Sep 17 00:00:00 2001 From: Joshua Whitley Date: Sat, 1 May 2021 11:32:10 -0500 Subject: [PATCH 01/14] Add files required to make a ROS package --- package.xml | 18 ++++++++++++++++++ resource/lgsvl | 0 setup.cfg | 4 ++++ setup.py | 19 +++++++++++++++---- 4 files changed, 37 insertions(+), 4 deletions(-) create mode 100644 package.xml create mode 100644 resource/lgsvl create mode 100644 setup.cfg diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..20ed2e2 --- /dev/null +++ b/package.xml @@ -0,0 +1,18 @@ + + + + lgsvl + 2021.1.0 + A Python API for SVL Simulator + Hadi Tabatabaee + Other + + python3-environs-pip + python3-numpy + python3-websocket + python3-websockets + + + ament_python + + diff --git a/resource/lgsvl b/resource/lgsvl new file mode 100644 index 0000000..e69de29 diff --git a/setup.cfg b/setup.cfg new file mode 100644 index 0000000..34a94f7 --- /dev/null +++ b/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/lgsvl +[install] +install_scripts=$base/lib/lgsvl diff --git a/setup.py b/setup.py index 16c29f2..3719c6b 100755 --- a/setup.py +++ b/setup.py @@ -76,9 +76,16 @@ def get_version(git_tag): return public_version_identifier + local_version_label_with_plus +package_name = 'lgsvl' + setup( - name="lgsvl", + name=package_name, version=get_version('2021.1'), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], description="Python API for SVL Simulator", author="LGSVL", author_email="contact@svlsimulator.com", @@ -87,8 +94,9 @@ def get_version(git_tag): packages=["lgsvl", "lgsvl.dreamview", "lgsvl.evaluator", "lgsvl.wise"], install_requires=[ "environs", - "numpy", # for evaluator - "websocket-client", # for dreamview + "numpy", + "setuptools", + "websocket-client", "websockets" ], setup_requires=[ @@ -99,7 +107,10 @@ def get_version(git_tag): "flake8>=3.7.0" ], }, - license="Other", + zip_safe=True, + maintainer='Hadi Tabatabaee', + maintainer_email='hadi.tabatabaee@lge.com', + license='Other', classifiers=[ "License :: Other/Proprietary License", "Programming Language :: Python :: 3", From dfc93b7597a02411eeadbf3004946dd0ad3ef706 Mon Sep 17 00:00:00 2001 From: Hadi Tabatabaee Date: Thu, 12 Aug 2021 22:20:07 -0700 Subject: [PATCH 02/14] Invoke flake8 unambiguously --- .gitlab-ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index 07c1e3f..65833ea 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -9,4 +9,4 @@ stages: flake8: stage: Style Guide Enforcement script: - - flake8 + - python3 -m flake8 From 04f3df82a73d5fb3224a7e2d20e611f625ab7f4c Mon Sep 17 00:00:00 2001 From: "karol.byczkowski" Date: Thu, 19 Aug 2021 13:56:24 -0700 Subject: [PATCH 03/14] [VSE] [AUTO-7412] Added acceleration parameter to drive and walk waypoints. --- lgsvl/agent.py | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/lgsvl/agent.py b/lgsvl/agent.py index 6c8e578..b4b1c56 100644 --- a/lgsvl/agent.py +++ b/lgsvl/agent.py @@ -18,6 +18,7 @@ def __init__( self, position, speed, + acceleration=0, angle=Vector(0, 0, 0), idle=0, deactivate=False, @@ -27,6 +28,7 @@ def __init__( ): self.position = position self.speed = speed + self.acceleration = acceleration self.angle = angle self.idle = idle self.deactivate = deactivate @@ -36,9 +38,10 @@ def __init__( class WalkWaypoint: - def __init__(self, position, idle, trigger_distance=0, speed=1, trigger=None): + def __init__(self, position, idle, trigger_distance=0, speed=1, acceleration=0, trigger=None): self.position = position self.speed = speed + self.acceleration = acceleration self.idle = idle self.trigger_distance = trigger_distance self.trigger = trigger @@ -228,7 +231,7 @@ def follow(self, waypoints, loop=False, waypoints_path_type="Linear"): Parameters ---------- waypoints : list of DriveWaypoints - DriveWaypoint : Class (position, speed, angle, idle, trigger_distance) + DriveWaypoint : Class (position, speed, acceleration, angle, idle, trigger_distance) position : lgsvl.Vector() Unity coordinates of waypoint @@ -236,6 +239,9 @@ def follow(self, waypoints, loop=False, waypoints_path_type="Linear"): speed : float how fast the NPC should drive to the waypoint + acceleration : float + how fast the NPC will increase the speed + angle : lgsvl.Vector() Unity rotation of the NPC at the waypoint @@ -270,6 +276,7 @@ def follow(self, waypoints, loop=False, waypoints_path_type="Linear"): { "position": wp.position.to_json(), "speed": wp.speed, + "acceleration": wp.acceleration, "angle": wp.angle.to_json(), "idle": wp.idle, "deactivate": wp.deactivate, @@ -362,7 +369,7 @@ def follow(self, waypoints, loop=False, waypoints_path_type="Linear"): Parameters ---------- waypoints : list of WalkWaypoints - WalkWaypoint : Class (position, idle, trigger_distance, speed) + WalkWaypoint : Class (position, idle, trigger_distance, speed, acceleration) position : lgsvl.Vector() Unity coordinates of waypoint @@ -376,6 +383,9 @@ def follow(self, waypoints, loop=False, waypoints_path_type="Linear"): speed : float how fast the pedestrian should drive to the waypoint (default value 1) + acceleration : float + how fast the pedestrian will increase the speed + loop : bool whether the pedestrian should loop through the waypoints after reaching the final one """ @@ -389,6 +399,7 @@ def follow(self, waypoints, loop=False, waypoints_path_type="Linear"): "idle": wp.idle, "trigger_distance": wp.trigger_distance, "speed": wp.speed, + "acceleration": wp.acceleration, "trigger": ( None if wp.trigger is None else wp.trigger.to_json() ), From 469151941ef767d34782fa9c91e59cfb4963cff9 Mon Sep 17 00:00:00 2001 From: David Uhm Date: Fri, 20 Aug 2021 03:07:20 -0700 Subject: [PATCH 04/14] [AUTO-7425] Add an API call for canceling destination --- lgsvl/agent.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/lgsvl/agent.py b/lgsvl/agent.py index b4b1c56..064d65a 100644 --- a/lgsvl/agent.py +++ b/lgsvl/agent.py @@ -214,6 +214,14 @@ def apply_control(self, control, sticky=False): def on_custom(self, fn): self.simulator._add_callback(self, "custom", fn) + def cancel_destination(self): + self.remote.command( + "vehicle/cancel_destination", + { + "uid": self.uid, + } + ) + class NpcVehicle(Vehicle): def __init__(self, uid, simulator): From 9132959c220a16e098af31d9690cb7691b191587 Mon Sep 17 00:00:00 2001 From: Hadi Tabatabaee Date: Sun, 29 Aug 2021 00:27:19 -0700 Subject: [PATCH 05/14] Fix env var getting erros in SampleTestCases --- examples/SampleTestCases/cut-in.py | 2 +- examples/SampleTestCases/ped-crossing.py | 2 +- examples/SampleTestCases/red-light-runner.py | 2 +- examples/SampleTestCases/sudden-braking.py | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/examples/SampleTestCases/cut-in.py b/examples/SampleTestCases/cut-in.py index 1447b7c..9a9006c 100755 --- a/examples/SampleTestCases/cut-in.py +++ b/examples/SampleTestCases/cut-in.py @@ -28,7 +28,7 @@ BRIDGE_HOST = os.environ.get("LGSVL__AUTOPILOT_0_HOST", "127.0.0.1") BRIDGE_PORT = int(os.environ.get("LGSVL__AUTOPILOT_0_PORT", 9090)) -scene_name = env.str("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_sanfrancisco) +scene_name = os.environ.get("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_sanfrancisco) sim = lgsvl.Simulator(SIMULATOR_HOST, SIMULATOR_PORT) if sim.current_scene == scene_name: diff --git a/examples/SampleTestCases/ped-crossing.py b/examples/SampleTestCases/ped-crossing.py index 703f94f..06bef16 100755 --- a/examples/SampleTestCases/ped-crossing.py +++ b/examples/SampleTestCases/ped-crossing.py @@ -27,7 +27,7 @@ BRIDGE_HOST = os.environ.get("LGSVL__AUTOPILOT_0_HOST", "127.0.0.1") BRIDGE_PORT = int(os.environ.get("LGSVL__AUTOPILOT_0_PORT", 9090)) -scene_name = env.str("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_straight1lanepedestriancrosswalk) +scene_name = os.environ.get("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_straight1lanepedestriancrosswalk) sim = lgsvl.Simulator(SIMULATOR_HOST, SIMULATOR_PORT) if sim.current_scene == scene_name: sim.reset() diff --git a/examples/SampleTestCases/red-light-runner.py b/examples/SampleTestCases/red-light-runner.py index 45118fd..a974ba1 100755 --- a/examples/SampleTestCases/red-light-runner.py +++ b/examples/SampleTestCases/red-light-runner.py @@ -27,7 +27,7 @@ BRIDGE_HOST = os.environ.get("LGSVL__AUTOPILOT_0_HOST", "127.0.0.1") BRIDGE_PORT = int(os.environ.get("LGSVL__AUTOPILOT_0_PORT", 9090)) -scene_name = env.str("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_borregasave) +scene_name = os.environ.get("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_borregasave) sim = lgsvl.Simulator(SIMULATOR_HOST, SIMULATOR_PORT) if sim.current_scene == scene_name: sim.reset() diff --git a/examples/SampleTestCases/sudden-braking.py b/examples/SampleTestCases/sudden-braking.py index 8931d6e..802747d 100755 --- a/examples/SampleTestCases/sudden-braking.py +++ b/examples/SampleTestCases/sudden-braking.py @@ -27,7 +27,7 @@ BRIDGE_HOST = os.environ.get("LGSVL__AUTOPILOT_0_HOST", "127.0.0.1") BRIDGE_PORT = int(os.environ.get("LGSVL__AUTOPILOT_0_PORT", 9090)) -scene_name = env.str("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_singlelaneroad) +scene_name = os.environ.get("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_singlelaneroad) sim = lgsvl.Simulator(SIMULATOR_HOST, SIMULATOR_PORT) if sim.current_scene == scene_name: sim.reset() From a6739c7916a40706db1da1c4eb0c12f4baf005e1 Mon Sep 17 00:00:00 2001 From: "david.uhm" Date: Wed, 8 Sep 2021 13:45:38 -0700 Subject: [PATCH 06/14] [AUTO-7321] Add python API functions to send destination to Nav2 --- lgsvl/__init__.py | 2 +- lgsvl/agent.py | 24 +++++++- lgsvl/geometry.py | 18 ++++++ lgsvl/simulator.py | 15 ++++- quickstart/36-send-destination-to-nav2.py | 70 +++++++++++++++++++++++ 5 files changed, 126 insertions(+), 3 deletions(-) create mode 100755 quickstart/36-send-destination-to-nav2.py diff --git a/lgsvl/__init__.py b/lgsvl/__init__.py index 8d0d31e..be4e2ef 100644 --- a/lgsvl/__init__.py +++ b/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 ( diff --git a/lgsvl/agent.py b/lgsvl/agent.py index 064d65a..9797b60 100644 --- a/lgsvl/agent.py +++ b/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): diff --git a/lgsvl/geometry.py b/lgsvl/geometry.py index 9b129ce..d914d50 100644 --- a/lgsvl/geometry.py +++ b/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) diff --git a/lgsvl/simulator.py b/lgsvl/simulator.py index 9343fae..3499b04 100644 --- a/lgsvl/simulator.py +++ b/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", [{ diff --git a/quickstart/36-send-destination-to-nav2.py b/quickstart/36-send-destination-to-nav2.py new file mode 100755 index 0000000..6a8681b --- /dev/null +++ b/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!") From 6a58a3b946417f1a64324cf19387bb536e72467d Mon Sep 17 00:00:00 2001 From: "david.uhm" Date: Wed, 8 Sep 2021 14:14:44 -0700 Subject: [PATCH 07/14] [AUTO-7690] Implement API function to set NavOrigin in scene --- lgsvl/simulator.py | 10 ++++++++++ quickstart/36-send-destination-to-nav2.py | 2 ++ 2 files changed, 12 insertions(+) diff --git a/lgsvl/simulator.py b/lgsvl/simulator.py index 3499b04..6e203bf 100644 --- a/lgsvl/simulator.py +++ b/lgsvl/simulator.py @@ -294,6 +294,16 @@ def map_from_nav(self, position, orientation): ) return Transform.from_json(res) + @accepts(Transform, Vector) + def set_nav_origin(self, transform, offset=Vector()): + self.remote.command( + "navigation/set_origin", + { + "transform": transform.to_json(), + "offset": offset.to_json(), + } + ) + @accepts(Vector, Vector, int, float) def raycast(self, origin, direction, layer_mask=-1, max_distance=float("inf")): hit = self.remote.command("simulator/raycast", [{ diff --git a/quickstart/36-send-destination-to-nav2.py b/quickstart/36-send-destination-to-nav2.py index 6a8681b..7589d09 100755 --- a/quickstart/36-send-destination-to-nav2.py +++ b/quickstart/36-send-destination-to-nav2.py @@ -47,6 +47,7 @@ ego.set_initial_pose() sim.run(5) + def send_next_destination(agent): global i print(f"{i}: {agent.name} reached destination") @@ -60,6 +61,7 @@ def send_next_destination(agent): 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) From 1d9ff0b90453e8a71e0d10a61093f8136f516161 Mon Sep 17 00:00:00 2001 From: "david.uhm" Date: Wed, 8 Sep 2021 14:21:10 -0700 Subject: [PATCH 08/14] [AUTO-7691] Add API call to get NavOrigin from scene --- lgsvl/simulator.py | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/lgsvl/simulator.py b/lgsvl/simulator.py index 6e203bf..f486f7a 100644 --- a/lgsvl/simulator.py +++ b/lgsvl/simulator.py @@ -304,6 +304,16 @@ def set_nav_origin(self, transform, offset=Vector()): } ) + def get_nav_origin(self): + res = self.remote.command("navigation/get_origin") + nav_origin = None + if res: + nav_origin = { + "transform": Transform.from_json(res), + "offset": res["offset"] + } + return nav_origin + @accepts(Vector, Vector, int, float) def raycast(self, origin, direction, layer_mask=-1, max_distance=float("inf")): hit = self.remote.command("simulator/raycast", [{ From 0a6dd34fb2639388fda75dc50479f23f7779b5ae Mon Sep 17 00:00:00 2001 From: "karol.byczkowski" Date: Wed, 15 Sep 2021 23:48:11 -0700 Subject: [PATCH 09/14] Quickstart 20-enable-sensors now checks if LiDAR sensor is available + Quickstart 23-npc-callbacks now runs for a fixed time --- quickstart/20-enable-sensors.py | 19 +++++++++++++------ quickstart/23-npc-callbacks.py | 3 ++- 2 files changed, 15 insertions(+), 7 deletions(-) diff --git a/quickstart/20-enable-sensors.py b/quickstart/20-enable-sensors.py index d764147..12dfc64 100755 --- a/quickstart/20-enable-sensors.py +++ b/quickstart/20-enable-sensors.py @@ -28,14 +28,21 @@ # Sensors have an enabled/disabled state that can be set # By default all sensors are enabled # Disabling sensor will prevent it to send or receive messages to ROS or Cyber bridges +lidarAvailable = False for s in sensors: print(type(s), s.enabled) + if isinstance(s, lgsvl.LidarSensor): + lidarAvailable = True -input("Press Enter to disable lidar") +if lidarAvailable: + input("Press Enter to disable LiDAR") -for s in sensors: - if isinstance(s, lgsvl.LidarSensor): - s.enabled = False + for s in sensors: + if isinstance(s, lgsvl.LidarSensor): + s.enabled = False -for s in sensors: - print(type(s), s.enabled) + for s in sensors: + print(type(s), s.enabled) +else: + print("LiDAR sensor is not included in the selected sensor configuration. Please check the \"ego_lincoln2017mkz_apollo5\" sensor configuration in the default assets.") + input("Press Enter to stop the simulation") diff --git a/quickstart/23-npc-callbacks.py b/quickstart/23-npc-callbacks.py index 7b8ffaa..03c362d 100755 --- a/quickstart/23-npc-callbacks.py +++ b/quickstart/23-npc-callbacks.py @@ -61,4 +61,5 @@ def on_lane_change(agent): npc.on_lane_change(on_lane_change) npc.on_stop_line(on_stop_line) -sim.run() +print("Running the simulation for 30 seconds.") +sim.run(30) From 334db7a3118b8b073341234caa558ae9832e0490 Mon Sep 17 00:00:00 2001 From: Hadi Tabatabaee Date: Wed, 15 Sep 2021 23:46:49 -0700 Subject: [PATCH 10/14] Fix waypoints in quickstart 13 --- quickstart/13-npc-follow-waypoints.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/quickstart/13-npc-follow-waypoints.py b/quickstart/13-npc-follow-waypoints.py index d2ef498..9f47752 100755 --- a/quickstart/13-npc-follow-waypoints.py +++ b/quickstart/13-npc-follow-waypoints.py @@ -73,7 +73,7 @@ def on_collision(agent1, agent2, contact): ) # NPC will wait for 1 second at each waypoint - wp = lgsvl.DriveWaypoint(hit.point, speed, angle, 1) + wp = lgsvl.DriveWaypoint(hit.point, speed, angle=angle, idle=1) waypoints.append(wp) From 5d0db92870842059f98376e374b0377ccec512a6 Mon Sep 17 00:00:00 2001 From: David Uhm Date: Thu, 23 Sep 2021 15:55:10 -0700 Subject: [PATCH 11/14] Revert "[AUTO-7425] Add an API call for canceling destination" This reverts commit 469151941ef767d34782fa9c91e59cfb4963cff9. --- lgsvl/agent.py | 8 -------- 1 file changed, 8 deletions(-) diff --git a/lgsvl/agent.py b/lgsvl/agent.py index 9797b60..74e0575 100644 --- a/lgsvl/agent.py +++ b/lgsvl/agent.py @@ -214,14 +214,6 @@ def apply_control(self, control, sticky=False): def on_custom(self, fn): self.simulator._add_callback(self, "custom", fn) - def cancel_destination(self): - self.remote.command( - "vehicle/cancel_destination", - { - "uid": self.uid, - } - ) - def set_initial_pose(self): self.remote.command( "vehicle/set_initial_pose", From c6affda4354a19fb8d23c9eb959a8a649b1457bb Mon Sep 17 00:00:00 2001 From: Hadi Tabatabaee Date: Fri, 24 Sep 2021 14:53:20 -0700 Subject: [PATCH 12/14] Add get_bridge_type for ego agents --- lgsvl/agent.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/lgsvl/agent.py b/lgsvl/agent.py index 74e0575..f9ca63e 100644 --- a/lgsvl/agent.py +++ b/lgsvl/agent.py @@ -177,6 +177,9 @@ def connect_bridge(self, address, port): {"uid": self.uid, "address": address, "port": port}, ) + def get_bridge_type(self): + return self.remote.command("vehicle/bridge/type", {"uid": self.uid}) + def get_sensors(self): j = self.remote.command("vehicle/sensors/get", {"uid": self.uid}) return [Sensor.create(self.remote, sensor) for sensor in j] From 05f610bd6f3fab37b6523c1775cfcbf361261c0b Mon Sep 17 00:00:00 2001 From: "david.uhm" Date: Mon, 27 Sep 2021 14:54:47 -0700 Subject: [PATCH 13/14] [AUTO-7652] Add example python API script for spawning multi robots --- lgsvl/wise/wise.py | 20 ++++++++++ quickstart/35-spawn-multi-robots.py | 48 +++++++++++++++++++++++ quickstart/36-send-destination-to-nav2.py | 5 +-- 3 files changed, 70 insertions(+), 3 deletions(-) create mode 100755 quickstart/35-spawn-multi-robots.py diff --git a/lgsvl/wise/wise.py b/lgsvl/wise/wise.py index 8d21906..29bf94a 100644 --- a/lgsvl/wise/wise.py +++ b/lgsvl/wise/wise.py @@ -57,6 +57,10 @@ class DefaultAssets: # https://wise.svlsimulator.com/maps/profile/671868be-44f9-44a1-913c-cb0f29d12634 map_straight2laneopposing = "671868be-44f9-44a1-913c-cb0f29d12634" + # Map loaded in the quickstart scripts. Changing the map may lead to unexpected behaviour in the quickstart scripts. Default value is "LGSeocho". + # https://wise.svlsimulator.com/maps/profile/26546191-86e8-4b53-9432-1cecbbd95c87 + map_lgseocho = "26546191-86e8-4b53-9432-1cecbbd95c87" + # Ego vehicle that is loaded in most of the quickstart scripts. Default value is "Lincoln2017MKZ" using the "Apollo 5.0" sensor configuration. # This includes a bridge connection if needed and also bunch of sensors including LIDAR. # https://wise.svlsimulator.com/vehicles/profile/73805704-1e46-4eb6-b5f9-ec2244d5951e/edit/configuration/47b529db-0593-4908-b3e7-4b24a32a0f70 @@ -84,3 +88,19 @@ class DefaultAssets: # This has sensors for modular testing. # https://wise.svlsimulator.com/vehicles/profile/73805704-1e46-4eb6-b5f9-ec2244d5951e/edit/configuration/2e9095fa-c9b9-4f3f-8d7d-65fa2bb03921 ego_lincoln2017mkz_apollo6_modular = "2e9095fa-c9b9-4f3f-8d7d-65fa2bb03921" + + # Cloi robot that is loaded in quickstart/tests scripts. Default value is "LGCloi" using the "Navigation2" sensor configuration. + # https://wise.svlsimulator.com/vehicles/profile/20609b67-6dbd-40ad-9b46-e6bc455278ed/edit/configuration/c2207cd4-c8d0-4a12-b5b7-c79ab748becc + ego_lgcloi_navigation2 = "c2207cd4-c8d0-4a12-b5b7-c79ab748becc" + + # Cloi robot that is loaded in quickstart/tests scripts. Default value is "LGCloi" using the "Nav2_Multi_Robot1" sensor configuration. + # https://wise.svlsimulator.com/vehicles/profile/20609b67-6dbd-40ad-9b46-e6bc455278ed/edit/configuration/eee61d18-c6e3-4292-988d-445802aaee97 + ego_lgcloi_navigation2_multi_robot1 = "eee61d18-c6e3-4292-988d-445802aaee97" + + # Cloi robot that is loaded in quickstart/tests scripts. Default value is "LGCloi" using the "Nav2_Multi_Robot2" sensor configuration. + # https://wise.svlsimulator.com/vehicles/profile/20609b67-6dbd-40ad-9b46-e6bc455278ed/edit/configuration/f9c5ace0-969a-4ade-8208-87d09d1a53f8 + ego_lgcloi_navigation2_multi_robot2 = "f9c5ace0-969a-4ade-8208-87d09d1a53f8" + + # Cloi robot that is loaded in quickstart/tests scripts. Default value is "LGCloi" using the "Nav2_Multi_Robot3" sensor configuration. + # https://wise.svlsimulator.com/vehicles/profile/20609b67-6dbd-40ad-9b46-e6bc455278ed/edit/configuration/cfdb1484-91b7-4f27-b729-e313cc31ed8e + ego_lgcloi_navigation2_multi_robot3 = "cfdb1484-91b7-4f27-b729-e313cc31ed8e" diff --git a/quickstart/35-spawn-multi-robots.py b/quickstart/35-spawn-multi-robots.py new file mode 100755 index 0000000..f4a9727 --- /dev/null +++ b/quickstart/35-spawn-multi-robots.py @@ -0,0 +1,48 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +import time +from environs import Env +import lgsvl + +print("Python API Quickstart #35: Spawning multi robots with bridge connections") +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)) + +map_seocho = lgsvl.wise.DefaultAssets.map_lgseocho +robots = [ + lgsvl.wise.DefaultAssets.ego_lgcloi_navigation2_multi_robot1, + lgsvl.wise.DefaultAssets.ego_lgcloi_navigation2_multi_robot2, + lgsvl.wise.DefaultAssets.ego_lgcloi_navigation2_multi_robot3, +] + +if sim.current_scene == map_seocho: + sim.reset() +else: + sim.load(map_seocho) + +spawns = sim.get_spawn() +spawn = spawns[1] +right = lgsvl.utils.transform_to_right(spawn) + +for i, robot in enumerate(robots): + state = lgsvl.AgentState() + state.transform.position = spawn.position + (1.0 * i * right) + state.transform.rotation = spawn.rotation + ego = sim.add_agent(robot, lgsvl.AgentType.EGO, state) + print("Spawned a robot at:", state.transform.position) + + 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)) + print("Waiting for connection...") + while not ego.bridge_connected: + time.sleep(1) + + print("Bridge connected:", ego.bridge_connected) + +print("Running the simulation...") +sim.run() diff --git a/quickstart/36-send-destination-to-nav2.py b/quickstart/36-send-destination-to-nav2.py index 7589d09..7c4362d 100755 --- a/quickstart/36-send-destination-to-nav2.py +++ b/quickstart/36-send-destination-to-nav2.py @@ -14,9 +14,8 @@ 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" +map_seocho = lgsvl.wise.DefaultAssets.map_lgseocho +ego_cloi = lgsvl.wise.DefaultAssets.ego_lgcloi_navigation2 if sim.current_scene == map_seocho: sim.reset() From 9bc29c9aee5e98d4d99e6a410c04b153b8f7feef Mon Sep 17 00:00:00 2001 From: David Uhm Date: Tue, 28 Sep 2021 15:42:10 -0700 Subject: [PATCH 14/14] [AUTO-7654] Add example scripts for Navigation2 stack with Cloi --- examples/Nav2/single-robot-cut-in.py | 81 ++++++++++++++++++++ examples/Nav2/single-robot-freemode.py | 60 +++++++++++++++ examples/Nav2/single-robot-sudden-braking.py | 80 +++++++++++++++++++ examples/Nav2/single-robot-traffic-cones.py | 81 ++++++++++++++++++++ 4 files changed, 302 insertions(+) create mode 100755 examples/Nav2/single-robot-cut-in.py create mode 100755 examples/Nav2/single-robot-freemode.py create mode 100755 examples/Nav2/single-robot-sudden-braking.py create mode 100755 examples/Nav2/single-robot-traffic-cones.py diff --git a/examples/Nav2/single-robot-cut-in.py b/examples/Nav2/single-robot-cut-in.py new file mode 100755 index 0000000..5ab9719 --- /dev/null +++ b/examples/Nav2/single-robot-cut-in.py @@ -0,0 +1,81 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +import os +import lgsvl + +print("Navigation2 Example: Single Robot Cut In") + +TIME_DELAY = 5 +TIME_LIMIT = 70 +NAV_DST = ((10.9, -0, 0), (0, 0, 0, 1.0)) + +SIMULATOR_HOST = os.environ.get("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host) +SIMULATOR_PORT = int(os.environ.get("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +BRIDGE_HOST = os.environ.get("LGSVL__AUTOPILOT_0_HOST", lgsvl.wise.SimulatorSettings.bridge_host) +BRIDGE_PORT = int(os.environ.get("LGSVL__AUTOPILOT_0_PORT", lgsvl.wise.SimulatorSettings.bridge_port)) + +SCENE = os.environ.get("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_lgseocho) +ROBOT = os.environ.get("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lgcloi_navigation2) + +sim = lgsvl.Simulator(SIMULATOR_HOST, SIMULATOR_PORT) +if sim.current_scene == SCENE: + sim.reset() +else: + sim.load(SCENE, seed=0) + +spawns = sim.get_spawn() +egoState = lgsvl.AgentState() +egoState.transform = spawns[0] + +ego = sim.add_agent(ROBOT, lgsvl.AgentType.EGO, egoState) +ego.connect_bridge(BRIDGE_HOST, BRIDGE_PORT) +ego.set_initial_pose() +sim.run(TIME_DELAY) +is_reached = False + + +def on_destination_reached(agent): + global is_reached + is_reached = True + print(f"Robot reached destination") + sim.stop() + return + + +sim_dst = sim.map_from_nav(lgsvl.Vector(*NAV_DST[0]), lgsvl.Quaternion(*NAV_DST[1])) +ego.set_destination(sim_dst) +res = ego.on_destination_reached(on_destination_reached) + +right = lgsvl.utils.transform_to_right(egoState.transform) +forward = lgsvl.utils.transform_to_forward(egoState.transform) + +pedState = lgsvl.AgentState() +pedState.transform.position = egoState.transform.position +pedState.transform.position += 5 * right + 5 * forward +ped = sim.add_agent("Robin", lgsvl.AgentType.PEDESTRIAN, pedState) + + +def on_collision(agent1, agent2, contact): + raise Exception("Failed: {} collided with {}".format(agent1, agent2)) + + +ego.on_collision(on_collision) +ped.on_collision(on_collision) +waypoints = [] +waypoints.append(lgsvl.WalkWaypoint(pedState.position, idle=0, trigger_distance=6)) +waypoints.append(lgsvl.WalkWaypoint(pedState.transform.position - 5 * right, idle=15)) +waypoints.append(lgsvl.WalkWaypoint(pedState.transform.position - 10 * right, idle=0)) +ped.follow(waypoints) + +print(f"Running simulation for {TIME_LIMIT} seconds...") +sim.run(TIME_LIMIT) + +if is_reached: + print("PASSED") +else: + exit("FAILED: Ego did not reach the destination") diff --git a/examples/Nav2/single-robot-freemode.py b/examples/Nav2/single-robot-freemode.py new file mode 100755 index 0000000..843c0f2 --- /dev/null +++ b/examples/Nav2/single-robot-freemode.py @@ -0,0 +1,60 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +import os +import lgsvl + +print("Navigation2 Example: Single Robot Freemode") + +TIME_DELAY = 5 +TIME_LIMIT = 70 +NAV_DST = ((10.9, -0, 0), (0, 0, 0, 1.0)) + +SIMULATOR_HOST = os.environ.get("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host) +SIMULATOR_PORT = int(os.environ.get("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +BRIDGE_HOST = os.environ.get("LGSVL__AUTOPILOT_0_HOST", lgsvl.wise.SimulatorSettings.bridge_host) +BRIDGE_PORT = int(os.environ.get("LGSVL__AUTOPILOT_0_PORT", lgsvl.wise.SimulatorSettings.bridge_port)) + +SCENE = os.environ.get("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_lgseocho) +ROBOT = os.environ.get("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lgcloi_navigation2) + +sim = lgsvl.Simulator(SIMULATOR_HOST, SIMULATOR_PORT) +if sim.current_scene == SCENE: + sim.reset() +else: + sim.load(SCENE, seed=0) + +spawns = sim.get_spawn() +state = lgsvl.AgentState() +state.transform = spawns[0] + +ego = sim.add_agent(ROBOT, lgsvl.AgentType.EGO, state) +ego.connect_bridge(BRIDGE_HOST, BRIDGE_PORT) +ego.set_initial_pose() +sim.run(TIME_DELAY) +is_reached = False + + +def on_destination_reached(agent): + global is_reached + is_reached = True + print(f"Robot reached destination") + sim.stop() + return + + +sim_dst = sim.map_from_nav(lgsvl.Vector(*NAV_DST[0]), lgsvl.Quaternion(*NAV_DST[1])) +ego.set_destination(sim_dst) +res = ego.on_destination_reached(on_destination_reached) + +print(f"Running simulation for {TIME_LIMIT} seconds...") +sim.run(TIME_LIMIT) + +if is_reached: + print("PASSED") +else: + exit("FAILED: Ego did not reach the destination") diff --git a/examples/Nav2/single-robot-sudden-braking.py b/examples/Nav2/single-robot-sudden-braking.py new file mode 100755 index 0000000..3adda33 --- /dev/null +++ b/examples/Nav2/single-robot-sudden-braking.py @@ -0,0 +1,80 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +import os +import lgsvl + +print("Navigation2 Example: Single Robot Sudden Braking") + +TIME_DELAY = 5 +TIME_LIMIT = 70 +NAV_DST = ((10.9, -0, 0), (0, 0, 0, 1.0)) + +SIMULATOR_HOST = os.environ.get("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host) +SIMULATOR_PORT = int(os.environ.get("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +BRIDGE_HOST = os.environ.get("LGSVL__AUTOPILOT_0_HOST", lgsvl.wise.SimulatorSettings.bridge_host) +BRIDGE_PORT = int(os.environ.get("LGSVL__AUTOPILOT_0_PORT", lgsvl.wise.SimulatorSettings.bridge_port)) + +SCENE = os.environ.get("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_lgseocho) +ROBOT = os.environ.get("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lgcloi_navigation2) + +sim = lgsvl.Simulator(SIMULATOR_HOST, SIMULATOR_PORT) +if sim.current_scene == SCENE: + sim.reset() +else: + sim.load(SCENE, seed=0) + +spawns = sim.get_spawn() +egoState = lgsvl.AgentState() +egoState.transform = spawns[0] + +ego = sim.add_agent(ROBOT, lgsvl.AgentType.EGO, egoState) +ego.connect_bridge(BRIDGE_HOST, BRIDGE_PORT) +ego.set_initial_pose() +sim.run(TIME_DELAY) +is_reached = False + + +def on_destination_reached(agent): + global is_reached + is_reached = True + print(f"Robot reached destination") + sim.stop() + return + + +sim_dst = sim.map_from_nav(lgsvl.Vector(*NAV_DST[0]), lgsvl.Quaternion(*NAV_DST[1])) +ego.set_destination(sim_dst) +res = ego.on_destination_reached(on_destination_reached) + +right = lgsvl.utils.transform_to_right(egoState.transform) +forward = lgsvl.utils.transform_to_forward(egoState.transform) + +pedState = lgsvl.AgentState() +pedState.transform = egoState.transform +pedState.transform.position += 2 * forward +ped = sim.add_agent("Robin", lgsvl.AgentType.PEDESTRIAN, pedState) + + +def on_collision(agent1, agent2, contact): + raise Exception("Failed: {} collided with {}".format(agent1.name, agent2.name)) + + +ego.on_collision(on_collision) +ped.on_collision(on_collision) +waypoints = [] +waypoints.append(lgsvl.WalkWaypoint(pedState.position, idle=0, trigger_distance=1.5)) +waypoints.append(lgsvl.WalkWaypoint(pedState.position + 3 * forward, 0, speed=1)) +ped.follow(waypoints) + +print(f"Running simulation for {TIME_LIMIT} seconds...") +sim.run(TIME_LIMIT) + +if is_reached: + print("PASSED") +else: + exit("FAILED: Ego did not reach the destination") diff --git a/examples/Nav2/single-robot-traffic-cones.py b/examples/Nav2/single-robot-traffic-cones.py new file mode 100755 index 0000000..85f7a59 --- /dev/null +++ b/examples/Nav2/single-robot-traffic-cones.py @@ -0,0 +1,81 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +import os +import lgsvl + +print("Navigation2 Example: Single Robot Traffic Cones") + +TIME_DELAY = 5 +TIME_LIMIT = 90 +NAV_DST = ((10.9, -0, 0), (0, 0, 0, 1.0)) + +SIMULATOR_HOST = os.environ.get("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host) +SIMULATOR_PORT = int(os.environ.get("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +BRIDGE_HOST = os.environ.get("LGSVL__AUTOPILOT_0_HOST", lgsvl.wise.SimulatorSettings.bridge_host) +BRIDGE_PORT = int(os.environ.get("LGSVL__AUTOPILOT_0_PORT", lgsvl.wise.SimulatorSettings.bridge_port)) + +SCENE = os.environ.get("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_lgseocho) +ROBOT = os.environ.get("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lgcloi_navigation2) + +sim = lgsvl.Simulator(SIMULATOR_HOST, SIMULATOR_PORT) +if sim.current_scene == SCENE: + sim.reset() +else: + sim.load(SCENE, seed=0) + +spawns = sim.get_spawn() +state = lgsvl.AgentState() +state.transform = spawns[0] + +ego = sim.add_agent(ROBOT, lgsvl.AgentType.EGO, state) +ego.connect_bridge(BRIDGE_HOST, BRIDGE_PORT) +ego.set_initial_pose() +sim.run(TIME_DELAY) +is_reached = False + + +def on_destination_reached(agent): + global is_reached + is_reached = True + print(f"Robot reached destination") + sim.stop() + return + + +sim_dst = sim.map_from_nav(lgsvl.Vector(*NAV_DST[0]), lgsvl.Quaternion(*NAV_DST[1])) +ego.set_destination(sim_dst) +res = ego.on_destination_reached(on_destination_reached) + +right = lgsvl.utils.transform_to_right(state.transform) +forward = lgsvl.utils.transform_to_forward(state.transform) + +sign = 1 +for j in range(2, 11, 2): + sign *= -1 + for i in range(5): + objState = lgsvl.ObjectState() + objState.transform.position = state.transform.position + offset = sign * 0.5 * i * right + j * forward + objState.transform.position += offset + sim.controllable_add("TrafficCone", objState) + + +def on_collision(agent1, agent2, contact): + name1 = "traffic cone" if agent1 is None else agent1.name + name2 = "traffic cone" if agent2 is None else agent2.name + raise Exception("Failed: {} collided with {}".format(name1, name2)) + + +ego.on_collision(on_collision) +print(f"Running simulation for {TIME_LIMIT} seconds...") +sim.run(TIME_LIMIT) + +if is_reached: + print("PASSED") +else: + exit("FAILED: Ego did not reach the destination")