From d23c839b30cf39200c919d7a2bd030d0fcf633e9 Mon Sep 17 00:00:00 2001 From: Hou Seng Wong Date: Fri, 4 Feb 2022 11:06:10 -0800 Subject: [PATCH 01/22] added relay reactors --- .../Intersection/Carla/CarlaIntersection.lf | 27 ++++++++++++++++--- 1 file changed, 24 insertions(+), 3 deletions(-) diff --git a/experimental/Python/src/Intersection/Carla/CarlaIntersection.lf b/experimental/Python/src/Intersection/Carla/CarlaIntersection.lf index e1d242467e..c8995bd7c4 100644 --- a/experimental/Python/src/Intersection/Carla/CarlaIntersection.lf +++ b/experimental/Python/src/Intersection/Carla/CarlaIntersection.lf @@ -281,7 +281,24 @@ reactor ego_vehicle ( carla.position -> vehicle.vehicle_pos; } -main reactor ( +reactor Relay(duration_in_seconds(0)) { + preamble {= + from time import sleep + =} + input i; + output o; + logical action t; + + reaction(i) -> t {= + t.schedule(self.duration_in_seconds, i.value) + =} + + reaction(t) -> o {= + o.set(t.value) + =} +} + +federated reactor ( num_entries(4), positions({= [{ \ "x": -122.0, \ @@ -323,10 +340,14 @@ main reactor ( nominal_speed_in_intersection = 14 ); + request_relays = new[num_entries] Relay(duration_in_seconds=0); + egos.request -> request_relays.i; + request_relays.o -> rsu.request; - egos.request -> rsu.request; // For the purposes of this simulations, we don't want the simulation // to abruptly end once the last vehicle has reached the intersection. // vehicles.goal_reached -> rsu.vehicle_reached_intersection; - rsu.grant -> egos.grant; + grant_relays = new[num_entries] Relay(duration_in_seconds=0); + rsu.grant -> grant_relays.i; + grant_relays.o -> egos.grant; } From 21f8b30052c6fabfd1b2879fcff834764eb4479f Mon Sep 17 00:00:00 2001 From: Hou Seng Wong Date: Fri, 4 Feb 2022 11:28:03 -0800 Subject: [PATCH 02/22] factor out Ticker --- .../Intersection/Carla/CarlaIntersection.lf | 82 +++++++++---------- 1 file changed, 41 insertions(+), 41 deletions(-) diff --git a/experimental/Python/src/Intersection/Carla/CarlaIntersection.lf b/experimental/Python/src/Intersection/Carla/CarlaIntersection.lf index c8995bd7c4..5b946a2a04 100644 --- a/experimental/Python/src/Intersection/Carla/CarlaIntersection.lf +++ b/experimental/Python/src/Intersection/Carla/CarlaIntersection.lf @@ -75,10 +75,8 @@ reactor Carla( =} input command; - - // Set up the ticker - ticker = new Ticker(vehicle_bank_index = vehicle_bank_index); - + input world_is_ready; + input tick; output status; output position; @@ -87,10 +85,10 @@ reactor Carla( state gps; state gps_queue; - reaction(ticker.world) {= + reaction(world_is_ready) {= print("Got the world") - self.world = ticker.world.value - + self.world = self.client.get_world() + blueprint_library = self.world.get_blueprint_library() sensors_bp = {} @@ -149,7 +147,7 @@ reactor Carla( print("Spawned vehicle") =} - reaction(ticker.tick) -> status, position {= + reaction(tick) -> status, position {= velocity = self.vehicle.get_velocity() stat = vehicle_status(Vector3D( x = velocity.x, @@ -188,49 +186,44 @@ reactor Carla( * practice, it seems to not work as expected. */ reactor Ticker ( - interval(16 msec), - vehicle_bank_index(0) + interval(16 msec) ){ - output world; + output world_is_ready; output tick; state client; state world; - reaction(startup) -> world {= + reaction(startup) -> world_is_ready {= self.client=carla.Client("localhost", 2000) self.client.set_timeout(10.0) # seconds + + # Set the world to town 5 + self.world = self.client.load_world("Town05") + + settings = self.world.get_settings() + settings.fixed_delta_seconds = self.interval / SEC(1) + settings.substepping = True + settings.max_substep_delta_time = settings.fixed_delta_seconds / 10 + settings.max_substeps = 10 + settings.synchronous_mode = True # Enables synchronous mode + + self.world.apply_settings(settings) - # Only one of the vehicles will load the world and set the settings - if self.vehicle_bank_index != 0: - self.world = self.client.get_world() - else : - # Set the world to town 5 - self.world = self.client.load_world("Town05") + # Set the weather + weather = carla.WeatherParameters( + cloudiness=80.0, + precipitation=30.0, + sun_altitude_angle=70.0) - settings = self.world.get_settings() - settings.fixed_delta_seconds = self.interval / SEC(1) - settings.substepping = True - settings.max_substep_delta_time = settings.fixed_delta_seconds / 10 - settings.max_substeps = 10 - settings.synchronous_mode = True # Enables synchronous mode - - self.world.apply_settings(settings) - - # Set the weather - weather = carla.WeatherParameters( - cloudiness=80.0, - precipitation=30.0, - sun_altitude_angle=70.0) - - self.world.set_weather(weather) - - # Set the spectator (camera) position - transform = carla.Transform(carla.Location(x=-126.163864, y=3, z=67), \ - carla.Rotation(pitch=-90, yaw=-180, roll=0)) + self.world.set_weather(weather) + + # Set the spectator (camera) position + transform = carla.Transform(carla.Location(x=-126.163864, y=3, z=67), \ + carla.Rotation(pitch=-90, yaw=-180, roll=0)) - self.world.get_spectator().set_transform(transform) + self.world.get_spectator().set_transform(transform) # Send the world to Carla interface reactors - world.set(self.world) + world_is_ready.set(True) =} timer t(1 nsec, interval); reaction(t) -> tick {= @@ -260,6 +253,8 @@ reactor ego_vehicle ( initial_speeds({=[]=}) ) { input grant; + input tick; + input world_is_ready; output request; carla = new Carla( @@ -272,10 +267,11 @@ reactor ego_vehicle ( ); vehicle = new Vehicle(); - vehicle.request -> request; grant -> vehicle.grant; + world_is_ready -> carla.world_is_ready; + tick -> carla.tick; vehicle.control -> carla.command; carla.status -> vehicle.vehicle_stat; carla.position -> vehicle.vehicle_pos; @@ -339,6 +335,10 @@ federated reactor ( intersection_width = 40, nominal_speed_in_intersection = 14 ); + + ticker = new Ticker(); + (ticker.world_is_ready)+ -> egos.world_is_ready; + (ticker.tick)+ -> egos.tick; request_relays = new[num_entries] Relay(duration_in_seconds=0); egos.request -> request_relays.i; From 4f8c04d53260852cef8617059ac3e0c483f1fbe8 Mon Sep 17 00:00:00 2001 From: Steven Date: Mon, 7 Feb 2022 20:57:53 +0000 Subject: [PATCH 03/22] comment out relays --- .../Intersection/Carla/CarlaIntersection.lf | 20 +++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/experimental/Python/src/Intersection/Carla/CarlaIntersection.lf b/experimental/Python/src/Intersection/Carla/CarlaIntersection.lf index 5b946a2a04..fdacdfd700 100644 --- a/experimental/Python/src/Intersection/Carla/CarlaIntersection.lf +++ b/experimental/Python/src/Intersection/Carla/CarlaIntersection.lf @@ -340,14 +340,22 @@ federated reactor ( (ticker.world_is_ready)+ -> egos.world_is_ready; (ticker.tick)+ -> egos.tick; - request_relays = new[num_entries] Relay(duration_in_seconds=0); - egos.request -> request_relays.i; - request_relays.o -> rsu.request; + // Use Relays + // request_relays = new[num_entries] Relay(duration_in_seconds=0); + // egos.request -> request_relays.i; + // request_relays.o -> rsu.request; + + // Don't use Relays + egos.request -> rsu.request; // For the purposes of this simulations, we don't want the simulation // to abruptly end once the last vehicle has reached the intersection. // vehicles.goal_reached -> rsu.vehicle_reached_intersection; - grant_relays = new[num_entries] Relay(duration_in_seconds=0); - rsu.grant -> grant_relays.i; - grant_relays.o -> egos.grant; + // Use Relays + // grant_relays = new[num_entries] Relay(duration_in_seconds=0); + // rsu.grant -> grant_relays.i; + // grant_relays.o -> egos.grant; + + // Don't use Relays + rsu.grant -> egos.grant; } From 711ed9d68fa77e913af143b8f3112b3626bf8a60 Mon Sep 17 00:00:00 2001 From: Hou Seng Wong Date: Mon, 7 Feb 2022 14:01:26 -0800 Subject: [PATCH 04/22] add client to Carla reactor --- .../Python/src/Intersection/Carla/CarlaIntersection.lf | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/experimental/Python/src/Intersection/Carla/CarlaIntersection.lf b/experimental/Python/src/Intersection/Carla/CarlaIntersection.lf index fdacdfd700..e442f1c68d 100644 --- a/experimental/Python/src/Intersection/Carla/CarlaIntersection.lf +++ b/experimental/Python/src/Intersection/Carla/CarlaIntersection.lf @@ -81,12 +81,14 @@ reactor Carla( output position; state world; + state client; state vehicle; state gps; state gps_queue; reaction(world_is_ready) {= print("Got the world") + self.client = carla.Client("localhost", 2000) self.world = self.client.get_world() blueprint_library = self.world.get_blueprint_library() @@ -193,7 +195,7 @@ reactor Ticker ( state client; state world; reaction(startup) -> world_is_ready {= - self.client=carla.Client("localhost", 2000) + self.client = carla.Client("localhost", 2000) self.client.set_timeout(10.0) # seconds # Set the world to town 5 From 79087231a16bcfcb4cb83ca642075a88d0966b4c Mon Sep 17 00:00:00 2001 From: Hou Seng Wong Date: Tue, 8 Feb 2022 15:13:52 -0800 Subject: [PATCH 05/22] fix vehicle id in print messages --- .../Intersection/Carla/CarlaIntersection.lf | 6 +++--- .../Python/src/Intersection/Intersection.lf | 20 +++++++++---------- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/experimental/Python/src/Intersection/Carla/CarlaIntersection.lf b/experimental/Python/src/Intersection/Carla/CarlaIntersection.lf index e442f1c68d..0c328723dc 100644 --- a/experimental/Python/src/Intersection/Carla/CarlaIntersection.lf +++ b/experimental/Python/src/Intersection/Carla/CarlaIntersection.lf @@ -20,6 +20,7 @@ target Python { timeout: 40 sec, // fast: true // You can enable fast to get a much faster simulation + // logging: DEBUG }; import Vehicle, RSU from "../Intersection.lf"; @@ -219,7 +220,7 @@ reactor Ticker ( self.world.set_weather(weather) # Set the spectator (camera) position - transform = carla.Transform(carla.Location(x=-126.163864, y=3, z=67), \ + transform = carla.Transform(carla.Location(x=-126.163864, y=3, z=90), \ carla.Rotation(pitch=-90, yaw=-180, roll=0)) self.world.get_spectator().set_transform(transform) @@ -235,7 +236,6 @@ reactor Ticker ( self.world.tick() # self.world.wait_for_tick() # Use this instead for asynchronous client mode - tick.set(True) =} reaction(shutdown) {= @@ -268,7 +268,7 @@ reactor ego_vehicle ( initial_speed = {=lambda self: self.initial_speeds[self.vehicle_bank_index]=} ); - vehicle = new Vehicle(); + vehicle = new Vehicle(vehicle_id = bank_index); vehicle.request -> request; grant -> vehicle.grant; diff --git a/experimental/Python/src/Intersection/Intersection.lf b/experimental/Python/src/Intersection/Intersection.lf index 9668eee0a5..4e487d8be2 100644 --- a/experimental/Python/src/Intersection/Intersection.lf +++ b/experimental/Python/src/Intersection/Intersection.lf @@ -140,7 +140,7 @@ goal_reached_threshold = 14.0 goal_reached_threshold_time = (goal_reached_threshold/speed_limit) =} -reactor Vehicle { +reactor Vehicle (vehicle_id(0)){ input vehicle_stat; input vehicle_pos; @@ -204,9 +204,9 @@ reactor Vehicle { time_remaining = (self.granted_time_to_enter - get_logical_time()) / (BILLION * 1.0) print("########################################") - print("Vehicle {}: Distance to intersection: {}m.".format(self.bank_index + 1, distance_remaining)) - print("Vehicle {}: Time to intersection: {}s.".format(self.bank_index + 1, time_remaining)) - print("Vehicle {}: Current speed: {}m/s.".format(self.bank_index + 1, self.velocity)) + print("Vehicle {}: Distance to intersection: {}m.".format(self.vehicle_id + 1, distance_remaining)) + print("Vehicle {}: Time to intersection: {}s.".format(self.vehicle_id + 1, time_remaining)) + print("Vehicle {}: Current speed: {}m/s.".format(self.vehicle_id + 1, self.velocity)) target_speed = 0.0 # target_speed = distance_remaining/time_remaining @@ -223,9 +223,9 @@ reactor Vehicle { # Simulation is over self.goal_reached = True - print("\n\n*************************************************************\n\n".format(self.bank_index + 1)) - print("************* Vehicle {}: Reached intersection! *************".format(self.bank_index + 1)) - print("\n\n*************************************************************\n\n".format(self.bank_index + 1)) + print("\n\n*************************************************************\n\n".format(self.vehicle_id + 1)) + print("************* Vehicle {}: Reached intersection! *************".format(self.vehicle_id + 1)) + print("\n\n*************************************************************\n\n".format(self.vehicle_id + 1)) goal_reached.set(True) elif time_remaining < (distance_remaining / speed_limit): @@ -240,7 +240,7 @@ reactor Vehicle { # target_speed = ((2 * distance_remaining) / (time_remaining)) - self.velocity target_speed = distance_remaining / time_remaining - print("Vehicle {}: Calculated target speed: {}m/s.".format(self.bank_index + 1, target_speed)) + print("Vehicle {}: Calculated target speed: {}m/s.".format(self.vehicle_id + 1, target_speed)) if (target_speed - speed_limit) > 0: print("Warning: target speed exceeds the speed limit") @@ -276,11 +276,11 @@ reactor Vehicle { cmd = vehicle_command(throttle = throttle, brake = brake) control.set(cmd) - print("Vehicle {}: Throttle: {}. Brake: {}".format(self.bank_index + 1, throttle, brake)) + print("Vehicle {}: Throttle: {}. Brake: {}".format(self.vehicle_id + 1, throttle, brake)) =} reaction(grant) {= - print("Vehicle {} Granted access".format(self.bank_index + 1), + print("Vehicle {} Granted access".format(self.vehicle_id + 1), "to enter the intersection at elapsed logical time {:d}.\n".format( int(grant.value.arrival_time) - get_start_time() ), From 610e64554652b51930f8bbb0b8c56b4b88b74896 Mon Sep 17 00:00:00 2001 From: Hou Seng Wong Date: Wed, 23 Feb 2022 15:18:39 -0800 Subject: [PATCH 06/22] refactor nodes --- .../launch/intersection_demo.launch.py | 6 +- .../Carla/ROS/carla_intersection/setup.py | 10 +- .../{carla_intersection => src}/__init__.py | 0 .../carla_sim.py => src/carla_sim_node.py} | 75 +++++------ .../ROS/carla_intersection/src/constants.py | 12 ++ .../rsu.py => src/rsu_node.py} | 75 ++++------- .../Carla/ROS/carla_intersection/src/utils.py | 39 ++++++ .../vehicle.py => src/vehicle_node.py} | 121 ++++++------------ 8 files changed, 156 insertions(+), 182 deletions(-) rename experimental/Python/src/Intersection/Carla/ROS/carla_intersection/{carla_intersection => src}/__init__.py (100%) rename experimental/Python/src/Intersection/Carla/ROS/carla_intersection/{carla_intersection/carla_sim.py => src/carla_sim_node.py} (82%) create mode 100644 experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/constants.py rename experimental/Python/src/Intersection/Carla/ROS/carla_intersection/{carla_intersection/rsu.py => src/rsu_node.py} (56%) create mode 100644 experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py rename experimental/Python/src/Intersection/Carla/ROS/carla_intersection/{carla_intersection/vehicle.py => src/vehicle_node.py} (66%) diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/launch/intersection_demo.launch.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/launch/intersection_demo.launch.py index 51ca1ee74f..2e706b0681 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/launch/intersection_demo.launch.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/launch/intersection_demo.launch.py @@ -46,7 +46,7 @@ def generate_launch_description(): nodes.append( Node( package='carla_intersection', - executable='rsu', + executable='rsu_node', parameters=[ {"intersection_position": [-0.000007632,-0.001124366,2.792485]}, {"intersection_width": 40}, @@ -62,7 +62,7 @@ def generate_launch_description(): nodes.append( Node( package='carla_intersection', - executable='vehicle', + executable='vehicle_node', parameters=[ {"vehicle_id": i}, {"initial_speed": initial_speeds[i]}, @@ -73,7 +73,7 @@ def generate_launch_description(): nodes.append( Node( package='carla_intersection', - executable='carla_sim', + executable='carla_sim_node', parameters=[ {"vehicle_id": i}, {"initial_speed": initial_speeds[i]}, diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/setup.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/setup.py index 697b78fd43..f82cee7a27 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/setup.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/setup.py @@ -1,11 +1,11 @@ -from setuptools import setup +from setuptools import setup, find_packages package_name = 'carla_intersection' setup( name=package_name, version='0.0.0', - packages=[package_name], + packages=find_packages(), data_files=[ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), @@ -20,9 +20,9 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'rsu = carla_intersection.rsu:main', - 'vehicle = carla_intersection.vehicle:main', - 'carla_sim = carla_intersection.carla_sim:main' + 'rsu_node = src.rsu_node:main', + 'vehicle_node = src.vehicle_node:main', + 'carla_sim_node = src.carla_sim_node:main' ], }, ) diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/carla_intersection/__init__.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/__init__.py similarity index 100% rename from experimental/Python/src/Intersection/Carla/ROS/carla_intersection/carla_intersection/__init__.py rename to experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/__init__.py diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/carla_intersection/carla_sim.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim_node.py similarity index 82% rename from experimental/Python/src/Intersection/Carla/ROS/carla_intersection/carla_intersection/carla_sim.py rename to experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim_node.py index 9d3d1e203b..271f7146ec 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/carla_intersection/carla_sim.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim_node.py @@ -1,13 +1,26 @@ +# ROS2 libraries import rclpy from rclpy.node import Node + +# ROS2 messages from carla_intersection_msgs.msg import VehicleCommand from std_msgs.msg import Bool from geometry_msgs.msg import Vector3 -import time + +# Other libraries import glob import os import sys -CARLA_INSTALL_DIR = "/opt/carla-simulator" +try: + import queue +except ImportError: + import Queue as queue +from src.utils import make_coordinate, make_spawn_point + +# Constants +from src.constants import CARLA_INSTALL_DIR + +# Set up Carla try: sys.path.append(glob.glob(CARLA_INSTALL_DIR + "/PythonAPI/carla/dist/carla-*%d.%d-%s.egg" % ( sys.version_info.major, @@ -15,40 +28,37 @@ "win-amd64" if os.name == "nt" else "linux-x86_64"))[0]) except IndexError: pass - try: import carla except ImportError: sys.stderr.write("ERROR: Could not find the Carla .egg file.\nPlease make sure that" " CARLA_INSTALL_DIR in CarlaIntersection.lf points to the correct location.\n") -try: - import queue -except ImportError: - import Queue as queue - -class dotdict(dict): - """dot.notation access to dictionary attributes""" - __getattr__ = dict.get - __setattr__ = dict.__setitem__ - __delattr__ = dict.__delitem__ - # The Carla Simulator class CarlaSim(Node): def __init__(self): super().__init__("carla_sim") - self.interval = int(self.declare_parameter('interval', 16).value) # msec - self.vehicle_id = self.declare_parameter('vehicle_id', 0) - self.initial_speed = self.declare_parameter('initial_speed', [0.0, 0.0, 0.0]) - self.vehicle_type = self.declare_parameter('vehicle_type', 'vehicle.tesla.model3') - self.spawn_point = self.declare_parameter('spawn_point', [0.0, 0.0, 0.0, 0.0]) + + # Parameters declaration + self.declare_parameter('interval', 16) # msec + self.declare_parameter('vehicle_id', 0) + self.declare_parameter('initial_speed', [0.0, 0.0, 0.0]) + self.declare_parameter('vehicle_type', 'vehicle.tesla.model3') + self.declare_parameter('spawn_point', [0.0, 0.0, 0.0, 0.0]) + + # State variables initialization + self.interval = self.get_parameter('interval').value + self.vehicle_id = self.get_parameter('vehicle_id').value + self.vehicle_type = self.get_parameter('vehicle_type').value + self.initial_speed = make_coordinate(self.get_parameter('initial_speed').value) + self.spawn_point = make_spawn_point(self.get_parameter('spawn_point').value) self.world_is_ready = False # pubsub for input and output ports self.status_ = self.create_publisher(Vector3, "status_to_vehicle_stats", 10) self.position_ = self.create_publisher(Vector3, "position_to_vehicle_pos", 10) self.command_ = self.create_subscription(VehicleCommand, "control_to_command", self.command_callback, 10) - if self.get_vehicle_id() == 0: + if self.vehicle_id == 0: self.world_is_ready_ = self.create_publisher(Bool, "world_is_ready", 10) else: self.world_is_ready_ = self.create_subscription(Bool, "world_is_ready", self.world_is_ready_callback, 10) @@ -58,23 +68,8 @@ def __init__(self): # timer (should be after initialize_carla() is called) self.timer_ = self.create_timer(self.interval / 1000.0, self.timer_callback) - - def get_spawn_point(self): - sp = self.spawn_point.value - return dotdict({"x": sp[0], "y": sp[1], "z": sp[2], "yaw": sp[3]}) - - def get_vehicle_type(self): - return str(self.vehicle_type.value) - - def get_initial_speed(self): - sp = self.initial_speed.value - return Vector3(x=sp[0], y=sp[1], z=sp[2]) - - def get_vehicle_id(self): - return int(self.vehicle_id.value) - def command_callback(self, command): - if command.vehicle_id != self.get_vehicle_id(): + if command.vehicle_id != self.vehicle_id: return self.vehicle.apply_control( \ carla.VehicleControl( \ @@ -104,7 +99,7 @@ def initialize_carla(self): # initialize Carla self.client=carla.Client("localhost", 2000) self.client.set_timeout(10.0) # seconds - if self.get_vehicle_id() == 0: + if self.vehicle_id == 0: self.world = self.client.load_world("Town05") self.initialize_world(self.world) self.world_is_ready = True @@ -146,9 +141,9 @@ def initialize_vehicle(self, world): "imu": "sensor.other.imu" } - spawn_point = self.get_spawn_point() + spawn_point = self.spawn_point # Spawn the vehicle - vehicle_bp = blueprint_library.find(self.get_vehicle_type()) + vehicle_bp = blueprint_library.find(self.vehicle_type) transform = carla.Transform(carla.Location( \ x = spawn_point.x, \ y = spawn_point.y, \ @@ -186,7 +181,7 @@ def initialize_vehicle(self, world): self.gps.listen(self.gps_queue.put) # Set the initial speed - target_speed = self.get_initial_speed() + target_speed = self.initial_speed for i in range(1): self.vehicle.set_target_velocity(carla.Vector3D(x=target_speed.x, y=target_speed.y, z=target_speed.z)) # self.vehicle.apply_control(carla.VehicleControl(throttle=1.0, steer=0.0)) diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/constants.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/constants.py new file mode 100644 index 0000000000..0cc92ce634 --- /dev/null +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/constants.py @@ -0,0 +1,12 @@ +BILLION = 1_000_000_000 + +CARLA_INSTALL_DIR = "/opt/carla-simulator" + +# The speed limit of vehicles in m/s +SPEED_LIMIT = 14.0 + +# The distance (in meters) at which the controller assumes it has reached its goal +GOAL_REACHED_THRESHOLD = 14.0 + +# The time threshold at which the vehicle has reached its time-based goal +GOAL_REACHED_THRESHOLD_TIME = (GOAL_REACHED_THRESHOLD / SPEED_LIMIT) diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/carla_intersection/rsu.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/rsu_node.py similarity index 56% rename from experimental/Python/src/Intersection/Carla/ROS/carla_intersection/carla_intersection/rsu.py rename to experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/rsu_node.py index db49ad62c7..899deadaba 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/carla_intersection/rsu.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/rsu_node.py @@ -1,50 +1,30 @@ +# ROS2 libraries import rclpy from rclpy.node import Node -from math import sin, cos, sqrt, atan2, radians, pi +# ROS2 messages +from carla_intersection_msgs.msg import Request, Grant -try: - from math import isclose -except ImportError: - def isclose(a, b, rel_tol=1e-09, abs_tol=0.0): - return abs(a-b) <= max(rel_tol * max(abs(a), abs(b)), abs_tol) +# Other libraries +from src.utils import distance, make_coordinate, make_Vector3 + +# Constants +from src.constants import BILLION -from carla_intersection_msgs.msg import Request, Grant -from geometry_msgs.msg import Vector3 - -BILLION = 1_000_000_000 - -def distance(coordinate1, coordinate2): - """ - Calculate the great circle distance between two points - on the earth (specified in decimal degrees) - Taken from: https://stackoverflow.com/a/15737218/783868 - """ - # Currently ignores altitude - # Convert decimal degrees to radians - lat1 = radians(coordinate1.x) - lon1 = radians(coordinate1.y) - lat2 = radians(coordinate2.x) - lon2 = radians(coordinate2.y) - - # Haversine formula - dlon = lon2 - lon1 - dlat = lat2 - lat1 - a = sin(dlat/2)**2 + cos(lat1) * cos(lat2) * sin(dlon/2)**2 - c = 2 * atan2(sqrt(a), sqrt(1 - a)) - # Radius of earth in kilometers is 6371 - km = 6371.0 * c - m = km * 1000.0 - return m class RSU(Node): def __init__(self): super().__init__("rsu") - self.intersection_width = self.declare_parameter('intersection_width', 0) - self.nominal_speed_in_intersection = self.declare_parameter('nominal_speed_in_intersection', 0.0) - self.intersection_position = self.declare_parameter('intersection_position', [0.0, 0.0, 0.0]) - + # Parameters declaration + self.declare_parameter('intersection_width', 0) + self.declare_parameter('nominal_speed_in_intersection', 0.0) + self.declare_parameter('intersection_position', [0.0, 0.0, 0.0]) + + # State variables initialization + self.intersection_width = self.get_parameter('intersection_width').value + self.nominal_speed_in_intersection = self.get_parameter('nominal_speed_in_intersection').value + self.intersection_position = make_coordinate(self.get_parameter('intersection_position').value) self.earliest_free = 0 # nsec self.active_participants = [0] * 20 @@ -52,17 +32,6 @@ def __init__(self): self.grant_ = self.create_publisher(Grant, "grant", 10) self.request_ = self.create_subscription(Request, "request", self.request_callback, 10) - def get_intersection_position(self): - p = self.intersection_position.value - return Vector3(x=p[0], y=p[1], z=p[2]) - - def get_nominal_speed_in_intersection(self): - return float(self.nominal_speed_in_intersection.value) - - def get_intersection_width(self): - return int(self.intersection_width.value) - - def request_callback(self, request): self.active_participants[request.requestor_id] = 1 if request.speed == 0.0: @@ -73,10 +42,10 @@ def request_callback(self, request): # time from the time the vehicle sends the message # according to the arriving vehicle's clock. speed_in_m_per_sec = request.speed - dr = distance(self.get_intersection_position(), request.position) + dr = distance(self.intersection_position, request.position) arrival_time_sec = dr / speed_in_m_per_sec - self.get_logger().info("*** RSU: Vehicle {}'s distance to intersection is {}. ".format(request.requestor_id+1, dr, self.get_intersection_position(), request.position, arrival_time_sec)) + self.get_logger().info("*** RSU: Vehicle {}'s distance to intersection is {}. ".format(request.requestor_id+1, dr, self.intersection_position, request.position, arrival_time_sec)) time_message_sent = self.get_clock().now().to_msg() @@ -90,16 +59,16 @@ def request_callback(self, request): response.arrival_time = arrival_time_ns else: # Could be smarter than this, but just send the nominal speed in intersection. - response.target_speed = self.get_nominal_speed_in_intersection() + response.target_speed = self.nominal_speed_in_intersection # Vehicle has to slow down and maybe stop. response.arrival_time = self.earliest_free - response.intersection_position = self.get_intersection_position() + response.intersection_position = make_Vector3(self.intersection_position) response.requestor_id = request.requestor_id self.grant_.publish(response) # Update earliest free on the assumption that the vehicle # maintains its target speed (on average) within the intersection. - time_in_intersection_ns = int((BILLION * self.get_intersection_width()) / (response.target_speed)) + time_in_intersection_ns = int((BILLION * self.intersection_width) / (response.target_speed)) self.earliest_free = response.arrival_time + time_in_intersection_ns self.get_logger().info("*** RSU: Granted access to vehicle {} to enter at " diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py new file mode 100644 index 0000000000..3ed591bcd8 --- /dev/null +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py @@ -0,0 +1,39 @@ +from math import sin, cos, sqrt, atan2, radians +from geometry_msgs.msg import Vector3 +class dotdict(dict): + """dot.notation access to dictionary attributes""" + __getattr__ = dict.get + __setattr__ = dict.__setitem__ + __delattr__ = dict.__delitem__ + +def distance(coordinate1, coordinate2): + """ + Calculate the great circle distance between two points + on the earth (specified in decimal degrees) + Taken from: https://stackoverflow.com/a/15737218/783868 + """ + # Currently ignores altitude + # Convert decimal degrees to radians + lat1 = radians(coordinate1.x) + lon1 = radians(coordinate1.y) + lat2 = radians(coordinate2.x) + lon2 = radians(coordinate2.y) + + # Haversine formula + dlon = lon2 - lon1 + dlat = lat2 - lat1 + a = sin(dlat/2)**2 + cos(lat1) * cos(lat2) * sin(dlon/2)**2 + c = 2 * atan2(sqrt(a), sqrt(1 - a)) + # Radius of earth in kilometers is 6371 + km = 6371.0 * c + m = km * 1000.0 + return m + +def make_coordinate(list): + return dotdict({"x": list[0], "y": list[1], "z": list[2]}) + +def make_spawn_point(list): + return dotdict({"x": list[0], "y": list[1], "z": list[2], "yaw": list[3]}) + +def make_Vector3(coordinate): + return Vector3(x=coordinate.x, y=coordinate.y, z=coordinate.z) diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/carla_intersection/vehicle.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle_node.py similarity index 66% rename from experimental/Python/src/Intersection/Carla/ROS/carla_intersection/carla_intersection/vehicle.py rename to experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle_node.py index 898d6cb7cf..484ca8fcec 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/carla_intersection/vehicle.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle_node.py @@ -1,62 +1,29 @@ +# ROS2 libraries import rclpy from rclpy.node import Node -import random -from math import sin, cos, sqrt, atan2, radians, pi - -from std_msgs.msg import String - +# ROS2 messages from carla_intersection_msgs.msg import Request, Grant, VehicleCommand from geometry_msgs.msg import Vector3 -BILLION = 1_000_000_000 - - -# The speed limit of vehicles in m/s -speed_limit = 14.0 -# The distance (in meters) at which the controller assumes it has reached its goal -goal_reached_threshold = 14.0 -# The time threshold at which the vehicle has reached its time-based goal -goal_reached_threshold_time = (goal_reached_threshold/speed_limit) - -def distance(coordinate1, coordinate2): - """ - Calculate the great circle distance between two points - on the earth (specified in decimal degrees) - Taken from: https://stackoverflow.com/a/15737218/783868 - """ - # Currently ignores altitude - # Convert decimal degrees to radians - lat1 = radians(coordinate1.x) - lon1 = radians(coordinate1.y) - lat2 = radians(coordinate2.x) - lon2 = radians(coordinate2.y) - - # Haversine formula - dlon = lon2 - lon1 - dlat = lat2 - lat1 - a = sin(dlat/2)**2 + cos(lat1) * cos(lat2) * sin(dlon/2)**2 - c = 2 * atan2(sqrt(a), sqrt(1 - a)) - # Radius of earth in kilometers is 6371 - km = 6371.0 * c - m = km * 1000.0 - return m - - -class dotdict(dict): - """dot.notation access to dictionary attributes""" - __getattr__ = dict.get - __setattr__ = dict.__setitem__ - __delattr__ = dict.__delitem__ - - -class Vehicle(Node): +# Other libraries +from math import sqrt +from src.utils import distance, make_coordinate, make_Vector3 + +# Constants +from src.constants import BILLION, SPEED_LIMIT, GOAL_REACHED_THRESHOLD, GOAL_REACHED_THRESHOLD_TIME + +class VehicleNode(Node): def __init__(self): - super().__init__(f"vehicle_{random.randint(0,1000)}") - self.vehicle_id = self.declare_parameter('vehicle_id', 0) - self.initial_position = self.declare_parameter('initial_position', [0.0, 0.0, 0.0]) + super().__init__(f"vehicle") - self.current_pos = self.get_initial_position() + # Parameters declaration + self.declare_parameter('vehicle_id', 0) + self.declare_parameter('initial_position', [0.0, 0.0, 0.0]) + + # State variables initialization + self.vehicle_id = self.get_parameter('vehicle_id').value + self.initial_position = self.current_pos = make_coordinate(self.get_parameter('initial_position').value) self.granted_time_to_enter = 0 self.intersection_position = None self.goal_reached = False @@ -69,26 +36,18 @@ def __init__(self): self.control_ = self.create_publisher(VehicleCommand, "control_to_command", 10) self.grant_ = self.create_subscription(Grant, "grant", self.grant_callback, 10) self.request_ = self.create_publisher(Request, "request", 10) - - def get_vehicle_id(self): - return int(self.vehicle_id.value) - - def get_initial_position(self): - sp = self.initial_position.value - return dotdict({"x": sp[0], "y": sp[1], "z": sp[2]}) def vehicle_pos_callback(self, vehicle_pos): - self.current_pos = Vector3(x=vehicle_pos.x, y=vehicle_pos.y, z=vehicle_pos.z) + self.current_pos = make_coordinate([vehicle_pos.x, vehicle_pos.y, vehicle_pos.z]) def update_velocity(self, vehicle_stat): - velocity_3d = Vector3(x=vehicle_stat.x, y=vehicle_stat.y, z=vehicle_stat.z) + velocity_3d = make_coordinate([vehicle_stat.x, vehicle_stat.y, vehicle_stat.z]) linear_speed = sqrt(velocity_3d.x**2 + velocity_3d.y**2 + velocity_3d.z**2) self.velocity = linear_speed if self.velocity == 0.0: # Prevent divisions by zero self.velocity = 0.001 - def vehicle_stat_callback(self, vehicle_stat): if self.goal_reached: return @@ -96,7 +55,7 @@ def vehicle_stat_callback(self, vehicle_stat): self.update_velocity(vehicle_stat) # Check if we have received an initial pos - if distance(self.current_pos, Vector3(x=0.0, y=0.0, z=0.0)) <= 0.00000001: + if distance(self.current_pos, make_coordinate([0, 0, 0])) <= 0.00000001: self.get_logger().info("Warning: Have not received initial pos yet.") return @@ -105,9 +64,9 @@ def vehicle_stat_callback(self, vehicle_stat): if self.granted_time_to_enter == 0: if not self.asking_for_grant: request = Request() - request.requestor_id = self.get_vehicle_id() + request.requestor_id = self.vehicle_id request.speed = self.velocity - request.position = Vector3(x=self.current_pos.x, y=self.current_pos.y, z=self.current_pos.z) + request.position = make_Vector3(self.current_pos) self.request_.publish(request) self.asking_for_grant = True @@ -128,30 +87,30 @@ def vehicle_stat_callback(self, vehicle_stat): time_remaining = (self.granted_time_to_enter - current_time.sec * BILLION - current_time.nanosec) / BILLION self.get_logger().info("########################################") - self.get_logger().info("Vehicle {}: Distance to intersection: {}m.".format(self.get_vehicle_id() + 1, distance_remaining)) - self.get_logger().info("Vehicle {}: Time to intersection: {}s.".format(self.get_vehicle_id() + 1, time_remaining)) - self.get_logger().info("Vehicle {}: Current speed: {}m/s.".format(self.get_vehicle_id() + 1, self.velocity)) + self.get_logger().info("Vehicle {}: Distance to intersection: {}m.".format(self.vehicle_id + 1, distance_remaining)) + self.get_logger().info("Vehicle {}: Time to intersection: {}s.".format(self.vehicle_id + 1, time_remaining)) + self.get_logger().info("Vehicle {}: Current speed: {}m/s.".format(self.vehicle_id + 1, self.velocity)) target_speed = 0.0 # target_speed = distance_remaining/time_remaining - if distance_remaining <= goal_reached_threshold and \ - time_remaining <= goal_reached_threshold_time : + if distance_remaining <= GOAL_REACHED_THRESHOLD and \ + time_remaining <= GOAL_REACHED_THRESHOLD_TIME : # Goal reached # At this point, a normal controller should stop the vehicle until # it receives a new goal. However, for the purposes of this demo, # it will set the target speed to the speed limit so that vehicles # can leave the intersection (otherwise, they will just stop at the # intersection). - target_speed = speed_limit + target_speed = SPEED_LIMIT # Simulation is over self.goal_reached = True - self.get_logger().info("\n\n*************************************************************\n\n".format(self.get_vehicle_id() + 1)) - self.get_logger().info("************* Vehicle {}: Reached intersection! *************".format(self.get_vehicle_id() + 1)) - self.get_logger().info("\n\n*************************************************************\n\n".format(self.get_vehicle_id() + 1)) + self.get_logger().info("\n\n*************************************************************\n\n".format(self.vehicle_id + 1)) + self.get_logger().info("************* Vehicle {}: Reached intersection! *************".format(self.vehicle_id + 1)) + self.get_logger().info("\n\n*************************************************************\n\n".format(self.vehicle_id + 1)) - elif time_remaining < (distance_remaining / speed_limit): + elif time_remaining < (distance_remaining / SPEED_LIMIT): # No time to make it to the intersection even if we # were going at the speed limit. # Ask the RSU again @@ -163,9 +122,9 @@ def vehicle_stat_callback(self, vehicle_stat): # target_speed = ((2 * distance_remaining) / (time_remaining)) - self.velocity target_speed = distance_remaining / time_remaining - self.get_logger().info("Vehicle {}: Calculated target speed: {}m/s.".format(self.get_vehicle_id() + 1, target_speed)) + self.get_logger().info("Vehicle {}: Calculated target speed: {}m/s.".format(self.vehicle_id + 1, target_speed)) - if (target_speed - speed_limit) > 0: + if (target_speed - SPEED_LIMIT) > 0: self.get_logger().info("Warning: target speed exceeds the speed limit") target_speed = 0 self.granted_time_to_enter = 0 @@ -197,17 +156,17 @@ def vehicle_stat_callback(self, vehicle_stat): # Prepare and send the target velocity as a vehicle command cmd = VehicleCommand() - cmd.vehicle_id = self.get_vehicle_id() + cmd.vehicle_id = self.vehicle_id cmd.throttle = throttle cmd.brake = brake self.control_.publish(cmd) - self.get_logger().info("Vehicle {}: Throttle: {}. Brake: {}".format(self.get_vehicle_id() + 1, throttle, brake)) + self.get_logger().info("Vehicle {}: Throttle: {}. Brake: {}".format(self.vehicle_id + 1, throttle, brake)) def grant_callback(self, grant): - if grant.requestor_id != self.get_vehicle_id(): + if grant.requestor_id != self.vehicle_id: return - self.get_logger().info("Vehicle {} Granted access".format(self.get_vehicle_id() + 1) + + self.get_logger().info("Vehicle {} Granted access".format(self.vehicle_id + 1) + "to enter the intersection at elapsed logical time {:d}.\n".format( int(grant.arrival_time) ) @@ -220,7 +179,7 @@ def grant_callback(self, grant): def main(args=None): rclpy.init(args=args) - ego_vehicle = Vehicle() + ego_vehicle = VehicleNode() rclpy.spin(ego_vehicle) From c8f604c64b5495fe1aa6ec83720eb1265a2c0fc0 Mon Sep 17 00:00:00 2001 From: Hou Seng Wong Date: Wed, 23 Feb 2022 18:12:25 -0800 Subject: [PATCH 07/22] create a pair of pubsub for each vehicle_id --- .../Carla/ROS/carla_intersection/src/carla_sim_node.py | 8 +++----- .../Carla/ROS/carla_intersection/src/vehicle_node.py | 6 +++--- 2 files changed, 6 insertions(+), 8 deletions(-) diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim_node.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim_node.py index 271f7146ec..2e6a18098a 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim_node.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim_node.py @@ -55,9 +55,9 @@ def __init__(self): self.world_is_ready = False # pubsub for input and output ports - self.status_ = self.create_publisher(Vector3, "status_to_vehicle_stats", 10) - self.position_ = self.create_publisher(Vector3, "position_to_vehicle_pos", 10) - self.command_ = self.create_subscription(VehicleCommand, "control_to_command", self.command_callback, 10) + self.status_ = self.create_publisher(Vector3, f"status_to_vehicle_stats_{self.vehicle_id}", 10) + self.position_ = self.create_publisher(Vector3, f"position_to_vehicle_pos_{self.vehicle_id}", 10) + self.command_ = self.create_subscription(VehicleCommand, f"control_to_command_{self.vehicle_id}", self.command_callback, 10) if self.vehicle_id == 0: self.world_is_ready_ = self.create_publisher(Bool, "world_is_ready", 10) else: @@ -69,8 +69,6 @@ def __init__(self): self.timer_ = self.create_timer(self.interval / 1000.0, self.timer_callback) def command_callback(self, command): - if command.vehicle_id != self.vehicle_id: - return self.vehicle.apply_control( \ carla.VehicleControl( \ throttle=command.throttle, \ diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle_node.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle_node.py index 484ca8fcec..b88364bb0d 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle_node.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle_node.py @@ -31,9 +31,9 @@ def __init__(self): self.asking_for_grant = False # pubsub for input output ports - self.vehicle_stat_ = self.create_subscription(Vector3, "status_to_vehicle_stats", self.vehicle_stat_callback, 10) - self.vehicle_pos_ = self.create_subscription(Vector3, "position_to_vehicle_pos", self.vehicle_pos_callback, 10) - self.control_ = self.create_publisher(VehicleCommand, "control_to_command", 10) + self.vehicle_stat_ = self.create_subscription(Vector3, f"status_to_vehicle_stats_{self.vehicle_id}", self.vehicle_stat_callback, 10) + self.vehicle_pos_ = self.create_subscription(Vector3, f"position_to_vehicle_pos_{self.vehicle_id}", self.vehicle_pos_callback, 10) + self.control_ = self.create_publisher(VehicleCommand, f"control_to_command_{self.vehicle_id}", 10) self.grant_ = self.create_subscription(Grant, "grant", self.grant_callback, 10) self.request_ = self.create_publisher(Request, "request", 10) From 3b5a2f36dce6f068ef83e7342a42a071325deed5 Mon Sep 17 00:00:00 2001 From: Hou Seng Wong Date: Wed, 23 Feb 2022 18:12:40 -0800 Subject: [PATCH 08/22] factor out carla_sim logic --- .../launch/intersection_demo.launch.py | 1 - .../ROS/carla_intersection/src/carla_sim.py | 143 +++++++++++++++ .../carla_intersection/src/carla_sim_node.py | 167 +++--------------- .../Carla/ROS/carla_intersection/src/utils.py | 1 + 4 files changed, 168 insertions(+), 144 deletions(-) create mode 100644 experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/launch/intersection_demo.launch.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/launch/intersection_demo.launch.py index 2e706b0681..57f0bbcfc9 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/launch/intersection_demo.launch.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/launch/intersection_demo.launch.py @@ -2,7 +2,6 @@ import launch.actions import launch.substitutions from launch_ros.actions import Node -from geometry_msgs.msg import Vector3 def generate_launch_description(): diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py new file mode 100644 index 0000000000..4de3688cfc --- /dev/null +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py @@ -0,0 +1,143 @@ +# Other libraries +import glob +import os +import sys +try: + import queue +except ImportError: + import Queue as queue +from src.utils import make_coordinate, make_spawn_point + +# Constants +from src.constants import CARLA_INSTALL_DIR + +# Set up Carla +try: + sys.path.append(glob.glob(CARLA_INSTALL_DIR + "/PythonAPI/carla/dist/carla-*%d.%d-%s.egg" % ( + sys.version_info.major, + sys.version_info.minor, + "win-amd64" if os.name == "nt" else "linux-x86_64"))[0]) +except IndexError: + pass +try: + import carla +except ImportError: + sys.stderr.write("ERROR: Could not find the Carla .egg file.\nPlease make sure that" + " CARLA_INSTALL_DIR in CarlaIntersection.lf points to the correct location.\n") + + +class CarlaSim: + def __init__(self, interval, vehicle_type, initial_speed, spawn_point, logger): + # State variables initialization + self.world = None + self.client = None + self.vehicle = None + self.interval = interval + self.vehicle_type = vehicle_type + self.initial_speed = initial_speed + self.spawn_point = spawn_point + self.logger = logger + + def initialize_carla(self): + # initialize Carla + self.client = carla.Client("localhost", 2000) + self.client.set_timeout(10.0) # seconds + + def initialize_world(self): + self.world = self.client.load_world("Town05") + settings = self.world.get_settings() + settings.fixed_delta_seconds = self.interval / 1000.0 + settings.substepping = True + settings.max_substep_delta_time = settings.fixed_delta_seconds / 10 + settings.max_substeps = 10 + settings.synchronous_mode = True # Enables synchronous mode + self.world.apply_settings(settings) + self.set_weather() + self.set_spectator_camera() + + def set_weather(self): + # Set the weather + weather = carla.WeatherParameters( + cloudiness=80.0, + precipitation=30.0, + sun_altitude_angle=70.0) + self.world.set_weather(weather) + + def set_spectator_camera(self): + # Set the spectator (camera) position + transform = carla.Transform(carla.Location(x=-126.163864, y=3, z=67), \ + carla.Rotation(pitch=-90, yaw=-180, roll=0)) + self.world.get_spectator().set_transform(transform) + + def get_vehicle_velocity(self): + return self.vehicle.get_velocity() + + def get_vehicle_position(self): + return self.gps_queue.get() + + def apply_control(self, throttle, brake): + self.vehicle.apply_control(carla.VehicleControl(throttle=throttle, brake=brake)) + + def get_world(self): + self.world = self.client.get_world() + + def tick(self): + self.world.tick() + + def initialize_vehicle(self): + blueprint_library = self.world.get_blueprint_library() + + sensors_bp = {} + sensors = {} + sensors_to_spawn = { \ + "gps": "sensor.other.gnss", \ + "imu": "sensor.other.imu" + } + + spawn_point = self.spawn_point + # Spawn the vehicle + vehicle_bp = blueprint_library.find(self.vehicle_type) + transform = carla.Transform(carla.Location( \ + x = spawn_point.x, \ + y = spawn_point.y, \ + z = spawn_point.z \ + ), carla.Rotation(yaw = spawn_point.yaw)) + self.vehicle = self.world.spawn_actor(vehicle_bp, transform) + + for key in sensors_to_spawn.keys(): + sensors_bp[key] = blueprint_library.find(sensors_to_spawn[key]) + if key == "gps": + # Spawn the GPS sensor that is attached to the vehicle + relative_transform = carla.Transform(carla.Location( \ + x = 1.0, \ + y = 0.0, \ + z = 2.0), carla.Rotation()) + + elif key == "imu": + # Spawn the imu unit + relative_transform = carla.Transform(carla.Location( \ + x = 2.0, \ + y = 0.0, \ + z = 2.0), carla.Rotation()) + else: + relative_transform = carla.Transform(carla.Location(), carla.Rotation()) + + sensors[key] = self.world.spawn_actor( \ + sensors_bp[key], \ + relative_transform, \ + attach_to=self.vehicle, \ + attachment_type=carla.AttachmentType.Rigid \ + ) + + self.gps = sensors["gps"] + self.gps_queue = queue.Queue() + self.gps.listen(self.gps_queue.put) + + # Set the initial speed + target_speed = self.initial_speed + for i in range(1): + self.vehicle.set_target_velocity(carla.Vector3D(x=target_speed.x, y=target_speed.y, z=target_speed.z)) + # self.vehicle.apply_control(carla.VehicleControl(throttle=1.0, steer=0.0)) + # self.world.tick() + + self.logger.info("Spawned vehicle") \ No newline at end of file diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim_node.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim_node.py index 2e6a18098a..dabcdff4ba 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim_node.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim_node.py @@ -8,36 +8,13 @@ from geometry_msgs.msg import Vector3 # Other libraries -import glob -import os -import sys -try: - import queue -except ImportError: - import Queue as queue -from src.utils import make_coordinate, make_spawn_point - -# Constants -from src.constants import CARLA_INSTALL_DIR - -# Set up Carla -try: - sys.path.append(glob.glob(CARLA_INSTALL_DIR + "/PythonAPI/carla/dist/carla-*%d.%d-%s.egg" % ( - sys.version_info.major, - sys.version_info.minor, - "win-amd64" if os.name == "nt" else "linux-x86_64"))[0]) -except IndexError: - pass -try: - import carla -except ImportError: - sys.stderr.write("ERROR: Could not find the Carla .egg file.\nPlease make sure that" - " CARLA_INSTALL_DIR in CarlaIntersection.lf points to the correct location.\n") +from src.utils import make_coordinate, make_spawn_point, make_Vector3 +from src.carla_sim import CarlaSim # The Carla Simulator -class CarlaSim(Node): +class CarlaSimNode(Node): def __init__(self): - super().__init__("carla_sim") + super().__init__("carla_sim_node") # Parameters declaration self.declare_parameter('interval', 16) # msec @@ -53,145 +30,49 @@ def __init__(self): self.initial_speed = make_coordinate(self.get_parameter('initial_speed').value) self.spawn_point = make_spawn_point(self.get_parameter('spawn_point').value) self.world_is_ready = False - + # pubsub for input and output ports self.status_ = self.create_publisher(Vector3, f"status_to_vehicle_stats_{self.vehicle_id}", 10) self.position_ = self.create_publisher(Vector3, f"position_to_vehicle_pos_{self.vehicle_id}", 10) self.command_ = self.create_subscription(VehicleCommand, f"control_to_command_{self.vehicle_id}", self.command_callback, 10) + + self.carla_sim = CarlaSim(interval=self.interval, vehicle_type=self.vehicle_type, + initial_speed=self.initial_speed, spawn_point=self.spawn_point, + logger=self.get_logger()) + self.carla_sim.initialize_carla() if self.vehicle_id == 0: self.world_is_ready_ = self.create_publisher(Bool, "world_is_ready", 10) + self.carla_sim.initialize_world() + self.world_is_ready_.publish(Bool()) + self.world_is_ready_callback() else: self.world_is_ready_ = self.create_subscription(Bool, "world_is_ready", self.world_is_ready_callback, 10) - self.initialize_carla() - # timer (should be after initialize_carla() is called) self.timer_ = self.create_timer(self.interval / 1000.0, self.timer_callback) def command_callback(self, command): - self.vehicle.apply_control( \ - carla.VehicleControl( \ - throttle=command.throttle, \ - brake=command.brake \ - ) \ - ) + self.carla_sim.apply_control(command.throttle, command.brake) def timer_callback(self): if not self.world_is_ready: return - self.world.tick() - velocity = self.vehicle.get_velocity() - status = Vector3(x=velocity.x, y=velocity.y, z=velocity.z) - self.status_.publish(status) - - gps_pos = self.gps_queue.get() - position = Vector3(x = gps_pos.latitude, y = gps_pos.longitude, z= gps_pos.altitude) - self.position_.publish(position) - - def world_is_ready_callback(self, _): - self.world = self.client.get_world() + self.carla_sim.tick() + self.status_.publish(make_Vector3(self.carla_sim.get_vehicle_velocity())) + position = self.carla_sim.get_vehicle_position() + coordinate = make_coordinate([position.latitude, position.longitude, position.altitude]) + self.position_.publish(make_Vector3(coordinate)) + + def world_is_ready_callback(self, _=None): + self.carla_sim.get_world() self.world_is_ready = True - self.initialize_vehicle(self.world) - - def initialize_carla(self): - # initialize Carla - self.client=carla.Client("localhost", 2000) - self.client.set_timeout(10.0) # seconds - if self.vehicle_id == 0: - self.world = self.client.load_world("Town05") - self.initialize_world(self.world) - self.world_is_ready = True - self.world_is_ready_.publish(Bool()) - self.initialize_vehicle(self.world) + self.carla_sim.initialize_vehicle() - def initialize_world(self, world): - settings = world.get_settings() - settings.fixed_delta_seconds = self.interval / 1000.0 - settings.substepping = True - settings.max_substep_delta_time = settings.fixed_delta_seconds / 10 - settings.max_substeps = 10 - settings.synchronous_mode = True # Enables synchronous mode - world.apply_settings(settings) - self.set_weather() - self.set_spectator_camera() - - def set_weather(self): - # Set the weather - weather = carla.WeatherParameters( - cloudiness=80.0, - precipitation=30.0, - sun_altitude_angle=70.0) - self.world.set_weather(weather) - - def set_spectator_camera(self): - # Set the spectator (camera) position - transform = carla.Transform(carla.Location(x=-126.163864, y=3, z=67), \ - carla.Rotation(pitch=-90, yaw=-180, roll=0)) - self.world.get_spectator().set_transform(transform) - - def initialize_vehicle(self, world): - blueprint_library = world.get_blueprint_library() - - sensors_bp = {} - sensors = {} - sensors_to_spawn = { \ - "gps": "sensor.other.gnss", \ - "imu": "sensor.other.imu" - } - - spawn_point = self.spawn_point - # Spawn the vehicle - vehicle_bp = blueprint_library.find(self.vehicle_type) - transform = carla.Transform(carla.Location( \ - x = spawn_point.x, \ - y = spawn_point.y, \ - z = spawn_point.z \ - ), carla.Rotation(yaw = spawn_point.yaw)) - self.vehicle = self.world.spawn_actor(vehicle_bp, transform) - - for key in sensors_to_spawn.keys(): - sensors_bp[key] = blueprint_library.find(sensors_to_spawn[key]) - if key == "gps": - # Spawn the GPS sensor that is attached to the vehicle - relative_transform = carla.Transform(carla.Location( \ - x = 1.0, \ - y = 0.0, \ - z = 2.0), carla.Rotation()) - - elif key == "imu": - # Spawn the imu unit - relative_transform = carla.Transform(carla.Location( \ - x = 2.0, \ - y = 0.0, \ - z = 2.0), carla.Rotation()) - else: - relative_transform = carla.Transform(carla.Location(), carla.Rotation()) - - sensors[key] = self.world.spawn_actor( \ - sensors_bp[key], \ - relative_transform, \ - attach_to=self.vehicle, \ - attachment_type=carla.AttachmentType.Rigid \ - ) - - self.gps = sensors["gps"] - self.gps_queue = queue.Queue() - self.gps.listen(self.gps_queue.put) - - # Set the initial speed - target_speed = self.initial_speed - for i in range(1): - self.vehicle.set_target_velocity(carla.Vector3D(x=target_speed.x, y=target_speed.y, z=target_speed.z)) - # self.vehicle.apply_control(carla.VehicleControl(throttle=1.0, steer=0.0)) - # self.world.tick() - - self.get_logger().info("Spawned vehicle") - def main(args=None): rclpy.init(args=args) - ego_vehicle = CarlaSim() + ego_vehicle = CarlaSimNode() rclpy.spin(ego_vehicle) diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py index 3ed591bcd8..d31a64efb7 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py @@ -1,5 +1,6 @@ from math import sin, cos, sqrt, atan2, radians from geometry_msgs.msg import Vector3 + class dotdict(dict): """dot.notation access to dictionary attributes""" __getattr__ = dict.get From a16293a6f37bee6eb00d5de917c7cdccfb9a45b8 Mon Sep 17 00:00:00 2001 From: Hou Seng Wong Date: Thu, 24 Feb 2022 13:03:56 -0800 Subject: [PATCH 09/22] factor out vehicle logic --- .../launch/intersection_demo.launch.py | 6 +- .../ROS/carla_intersection/src/carla_sim.py | 9 +- .../carla_intersection/src/carla_sim_node.py | 6 +- .../Carla/ROS/carla_intersection/src/utils.py | 7 + .../ROS/carla_intersection/src/vehicle.py | 149 ++++++++++++++++ .../carla_intersection/src/vehicle_node.py | 167 ++++-------------- 6 files changed, 199 insertions(+), 145 deletions(-) create mode 100644 experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle.py diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/launch/intersection_demo.launch.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/launch/intersection_demo.launch.py index 57f0bbcfc9..5490aecb71 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/launch/intersection_demo.launch.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/launch/intersection_demo.launch.py @@ -34,7 +34,7 @@ def generate_launch_description(): [0.000367,-0.001185,2.794846] # <- ] - initial_speeds = [ + initial_velocities = [ [ 0.0, -8.0, 0.0], [ 8.0, 0.0, 0.0], [ 0.0, 8.0, 0.0], @@ -64,7 +64,7 @@ def generate_launch_description(): executable='vehicle_node', parameters=[ {"vehicle_id": i}, - {"initial_speed": initial_speeds[i]}, + {"initial_velocity": initial_velocities[i]}, {"initial_position": initial_positions[i]} ] ) @@ -75,7 +75,7 @@ def generate_launch_description(): executable='carla_sim_node', parameters=[ {"vehicle_id": i}, - {"initial_speed": initial_speeds[i]}, + {"initial_velocity": initial_velocities[i]}, {"spawn_point": [spawn_points[i]["x"], spawn_points[i]["y"], spawn_points[i]["z"], spawn_points[i]["yaw"]]} ] ) diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py index 4de3688cfc..bea2709d59 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py @@ -6,7 +6,6 @@ import queue except ImportError: import Queue as queue -from src.utils import make_coordinate, make_spawn_point # Constants from src.constants import CARLA_INSTALL_DIR @@ -27,14 +26,14 @@ class CarlaSim: - def __init__(self, interval, vehicle_type, initial_speed, spawn_point, logger): + def __init__(self, interval, vehicle_type, initial_velocity, spawn_point, logger): # State variables initialization self.world = None self.client = None self.vehicle = None self.interval = interval self.vehicle_type = vehicle_type - self.initial_speed = initial_speed + self.initial_velocity = initial_velocity self.spawn_point = spawn_point self.logger = logger @@ -133,8 +132,8 @@ def initialize_vehicle(self): self.gps_queue = queue.Queue() self.gps.listen(self.gps_queue.put) - # Set the initial speed - target_speed = self.initial_speed + # Set the initial velocity + target_speed = self.initial_velocity for i in range(1): self.vehicle.set_target_velocity(carla.Vector3D(x=target_speed.x, y=target_speed.y, z=target_speed.z)) # self.vehicle.apply_control(carla.VehicleControl(throttle=1.0, steer=0.0)) diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim_node.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim_node.py index dabcdff4ba..a36b4a6392 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim_node.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim_node.py @@ -19,7 +19,7 @@ def __init__(self): # Parameters declaration self.declare_parameter('interval', 16) # msec self.declare_parameter('vehicle_id', 0) - self.declare_parameter('initial_speed', [0.0, 0.0, 0.0]) + self.declare_parameter('initial_velocity', [0.0, 0.0, 0.0]) self.declare_parameter('vehicle_type', 'vehicle.tesla.model3') self.declare_parameter('spawn_point', [0.0, 0.0, 0.0, 0.0]) @@ -27,7 +27,7 @@ def __init__(self): self.interval = self.get_parameter('interval').value self.vehicle_id = self.get_parameter('vehicle_id').value self.vehicle_type = self.get_parameter('vehicle_type').value - self.initial_speed = make_coordinate(self.get_parameter('initial_speed').value) + self.initial_velocity = make_coordinate(self.get_parameter('initial_velocity').value) self.spawn_point = make_spawn_point(self.get_parameter('spawn_point').value) self.world_is_ready = False @@ -37,7 +37,7 @@ def __init__(self): self.command_ = self.create_subscription(VehicleCommand, f"control_to_command_{self.vehicle_id}", self.command_callback, 10) self.carla_sim = CarlaSim(interval=self.interval, vehicle_type=self.vehicle_type, - initial_speed=self.initial_speed, spawn_point=self.spawn_point, + initial_velocity=self.initial_velocity, spawn_point=self.spawn_point, logger=self.get_logger()) self.carla_sim.initialize_carla() if self.vehicle_id == 0: diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py index d31a64efb7..675e46799b 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py @@ -7,6 +7,10 @@ class dotdict(dict): __setattr__ = dict.__setitem__ __delattr__ = dict.__delitem__ +class VehicleClock: + def get_current_time_in_ns(self): + assert False, "subclass must override this method" + def distance(coordinate1, coordinate2): """ Calculate the great circle distance between two points @@ -38,3 +42,6 @@ def make_spawn_point(list): def make_Vector3(coordinate): return Vector3(x=coordinate.x, y=coordinate.y, z=coordinate.z) + +def make_speed(velocity): + return sqrt(velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2) diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle.py new file mode 100644 index 0000000000..fed0657026 --- /dev/null +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle.py @@ -0,0 +1,149 @@ +from src.utils import make_speed, make_Vector3, dotdict, distance +from src.constants import BILLION, GOAL_REACHED_THRESHOLD, GOAL_REACHED_THRESHOLD_TIME, SPEED_LIMIT + +class Vehicle: + def __init__(self, vehicle_id, initial_position, initial_velocity, clock, logger): + self.vehicle_id = vehicle_id + self.position = initial_position + self.velocity = initial_velocity + self.speed = make_speed(initial_velocity) + self.goal_reached = False + self.granted_time_to_enter = 0 + self.intersection_position = None + self.clock = clock + self.logger = logger + + def set_velocity(self, new_velocity): + self.velocity = new_velocity + self.speed = make_speed(new_velocity) + # Prevent divisions by zero + if self.speed == 0.0: + self.speed = 0.001 + + def set_position(self, new_position): + self.position = new_position + + def get_velocity(self): + return self.velocity + + def get_position(self): + return self.position + + def get_speed(self): + return self.speed + + def grant(self, arrival_time, intersection_position): + self.logger.info("Vehicle {} Granted access".format(self.vehicle_id + 1) + + "to enter the intersection at elapsed logical time {:d}.\n".format( + int(arrival_time) + ) + ) + self.granted_time_to_enter = arrival_time + self.intersection_position = intersection_position + + def receive_velocity_from_simulator(self, vehicle_stat): + pub_packets = dotdict() + if self.goal_reached: + return pub_packets + # Record the speed + self.set_velocity(vehicle_stat) + + # Send a new request to the RSU if no time to enter + # the intersection is granted + if self.granted_time_to_enter == 0: + pub_packets.request = dotdict() + pub_packets.request.requestor_id = self.vehicle_id + pub_packets.request.speed = self.get_speed() + pub_packets.request.position = make_Vector3(self.get_position()) + + # Stop the vehicle + pub_packets.cmd = dotdict() + pub_packets.cmd.throttle = 0.0 + pub_packets.cmd.brake = 1.0 + else: + # We have a granted time from the RSU + # All we need to do is adjust our velocity + # to enter the intersection at the allocated + # time + + # First, how far are we from the intersection + distance_remaining = distance(self.intersection_position, self.get_position()) + current_time = self.clock.get_current_time_in_ns() + time_remaining = (self.granted_time_to_enter - current_time) / BILLION + + self.logger.info("########################################") + self.logger.info("Vehicle {}: Distance to intersection: {}m.".format(self.vehicle_id, distance_remaining)) + self.logger.info("Vehicle {}: Time to intersection: {}s.".format(self.vehicle_id, time_remaining)) + self.logger.info("Vehicle {}: Current speed: {}m/s.".format(self.vehicle_id, self.speed)) + + target_speed = 0.0 + # target_speed = distance_remaining/time_remaining + + if distance_remaining <= GOAL_REACHED_THRESHOLD and \ + time_remaining <= GOAL_REACHED_THRESHOLD_TIME : + # Goal reached + # At this point, a normal controller should stop the vehicle until + # it receives a new goal. However, for the purposes of this demo, + # it will set the target speed to the speed limit so that vehicles + # can leave the intersection (otherwise, they will just stop at the + # intersection). + target_speed = SPEED_LIMIT + # Simulation is over + self.goal_reached = True + + self.logger.info("\n\n*************************************************************\n\n".format(self.vehicle_id)) + self.logger.info("************* Vehicle {}: Reached intersection! *************".format(self.vehicle_id)) + self.logger.info("\n\n*************************************************************\n\n".format(self.vehicle_id)) + + elif time_remaining < (distance_remaining / SPEED_LIMIT): + # No time to make it to the intersection even if we + # were going at the speed limit. + # Ask the RSU again + self.granted_time_to_enter = 0 + # Apply the brake since we ran out of time + target_speed = 0 + else: + # Has not reached the goal + # target_speed = ((2 * distance_remaining) / (time_remaining)) - self.speed + target_speed = distance_remaining / time_remaining + + self.logger.info("Vehicle {}: Calculated target speed: {}m/s.".format(self.vehicle_id, target_speed)) + + if (target_speed - SPEED_LIMIT) > 0: + self.logger.info("Warning: target speed exceeds the speed limit") + target_speed = 0 + self.granted_time_to_enter = 0 + + if target_speed <= 0: + self.logger.info("Warning: target speed negative or zero") + target_speed = 0.001 + self.granted_time_to_enter = 0 + + brake = 0.0 + throttle = 0.0 + + if target_speed >= self.speed: + # Calculate a proportional throttle (0.0 < throttle < 1.0) + throttle = min((target_speed - self.speed)/target_speed, 1.0) + # throttle = 1.0 + brake = 0.0 + # throttle = min(abs(target_speed / self.speed), 1) + else: + # Need to apply the brake + brake = min((self.speed - target_speed)/self.speed, 1.0) + # brake = 1.0 + throttle = 0.0 + + # Check throttle boundaries + if throttle < 0: + self.logger.info("Error: negative throttle") + throttle = 0.0 + + # Prepare and send the target velocity as a vehicle command + pub_packets.cmd = dotdict() + pub_packets.cmd.throttle = throttle + pub_packets.cmd.brake = brake + self.logger.info("Vehicle {}: Throttle: {}. Brake: {}".format(self.vehicle_id, throttle, brake)) + return pub_packets + + diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle_node.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle_node.py index b88364bb0d..a11903462f 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle_node.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle_node.py @@ -8,10 +8,11 @@ # Other libraries from math import sqrt -from src.utils import distance, make_coordinate, make_Vector3 +from src.utils import distance, make_coordinate, make_Vector3, VehicleClock +from src.vehicle import Vehicle # Constants -from src.constants import BILLION, SPEED_LIMIT, GOAL_REACHED_THRESHOLD, GOAL_REACHED_THRESHOLD_TIME +from src.constants import BILLION class VehicleNode(Node): def __init__(self): @@ -19,16 +20,19 @@ def __init__(self): # Parameters declaration self.declare_parameter('vehicle_id', 0) + self.declare_parameter('initial_velocity', [0.0, 0.0, 0.0]) self.declare_parameter('initial_position', [0.0, 0.0, 0.0]) # State variables initialization self.vehicle_id = self.get_parameter('vehicle_id').value - self.initial_position = self.current_pos = make_coordinate(self.get_parameter('initial_position').value) - self.granted_time_to_enter = 0 - self.intersection_position = None - self.goal_reached = False - self.velocity = 0.0 + self.initial_position = make_coordinate(self.get_parameter('initial_position').value) + self.initial_velocity = make_coordinate(self.get_parameter('initial_velocity').value) self.asking_for_grant = False + self.vehicle = Vehicle(vehicle_id = self.vehicle_id, + initial_position = self.initial_position, + initial_velocity = self.initial_velocity, + clock = ROSClock(self.get_clock()), + logger = self.get_logger()) # pubsub for input output ports self.vehicle_stat_ = self.create_subscription(Vector3, f"status_to_vehicle_stats_{self.vehicle_id}", self.vehicle_stat_callback, 10) @@ -38,142 +42,37 @@ def __init__(self): self.request_ = self.create_publisher(Request, "request", 10) def vehicle_pos_callback(self, vehicle_pos): - self.current_pos = make_coordinate([vehicle_pos.x, vehicle_pos.y, vehicle_pos.z]) - - def update_velocity(self, vehicle_stat): - velocity_3d = make_coordinate([vehicle_stat.x, vehicle_stat.y, vehicle_stat.z]) - linear_speed = sqrt(velocity_3d.x**2 + velocity_3d.y**2 + velocity_3d.z**2) - self.velocity = linear_speed - if self.velocity == 0.0: - # Prevent divisions by zero - self.velocity = 0.001 + self.vehicle.set_position(vehicle_pos) def vehicle_stat_callback(self, vehicle_stat): - if self.goal_reached: - return - # Record the speed - self.update_velocity(vehicle_stat) - - # Check if we have received an initial pos - if distance(self.current_pos, make_coordinate([0, 0, 0])) <= 0.00000001: - self.get_logger().info("Warning: Have not received initial pos yet.") - return - - # Send a new request to the RSU if no time to enter - # the intersection is granted - if self.granted_time_to_enter == 0: - if not self.asking_for_grant: - request = Request() - request.requestor_id = self.vehicle_id - request.speed = self.velocity - request.position = make_Vector3(self.current_pos) - self.request_.publish(request) - self.asking_for_grant = True - - # Stop the vehicle + pub_packets = self.vehicle.receive_velocity_from_simulator(vehicle_stat) + if pub_packets.cmd != None: cmd = VehicleCommand() - cmd.throttle = 0.0 - cmd.brake = 1.0 + cmd.throttle = pub_packets.cmd.throttle + cmd.brake = pub_packets.cmd.brake self.control_.publish(cmd) - else: - # We have a granted time from the RSU - # All we need to do is adjust our velocity - # to enter the intersection at the allocated - # time - - # First, how far are we from the intersection - distance_remaining = distance(self.intersection_position, self.current_pos) - current_time = self.get_clock().now().to_msg() - time_remaining = (self.granted_time_to_enter - current_time.sec * BILLION - current_time.nanosec) / BILLION - - self.get_logger().info("########################################") - self.get_logger().info("Vehicle {}: Distance to intersection: {}m.".format(self.vehicle_id + 1, distance_remaining)) - self.get_logger().info("Vehicle {}: Time to intersection: {}s.".format(self.vehicle_id + 1, time_remaining)) - self.get_logger().info("Vehicle {}: Current speed: {}m/s.".format(self.vehicle_id + 1, self.velocity)) - - target_speed = 0.0 - # target_speed = distance_remaining/time_remaining - - if distance_remaining <= GOAL_REACHED_THRESHOLD and \ - time_remaining <= GOAL_REACHED_THRESHOLD_TIME : - # Goal reached - # At this point, a normal controller should stop the vehicle until - # it receives a new goal. However, for the purposes of this demo, - # it will set the target speed to the speed limit so that vehicles - # can leave the intersection (otherwise, they will just stop at the - # intersection). - target_speed = SPEED_LIMIT - # Simulation is over - self.goal_reached = True - - self.get_logger().info("\n\n*************************************************************\n\n".format(self.vehicle_id + 1)) - self.get_logger().info("************* Vehicle {}: Reached intersection! *************".format(self.vehicle_id + 1)) - self.get_logger().info("\n\n*************************************************************\n\n".format(self.vehicle_id + 1)) - - elif time_remaining < (distance_remaining / SPEED_LIMIT): - # No time to make it to the intersection even if we - # were going at the speed limit. - # Ask the RSU again - self.granted_time_to_enter = 0 - # Apply the brake since we ran out of time - target_speed = 0 - else: - # Has not reached the goal - # target_speed = ((2 * distance_remaining) / (time_remaining)) - self.velocity - target_speed = distance_remaining / time_remaining - - self.get_logger().info("Vehicle {}: Calculated target speed: {}m/s.".format(self.vehicle_id + 1, target_speed)) - - if (target_speed - SPEED_LIMIT) > 0: - self.get_logger().info("Warning: target speed exceeds the speed limit") - target_speed = 0 - self.granted_time_to_enter = 0 - - if target_speed <= 0: - self.get_logger().info("Warning: target speed negative or zero") - target_speed = 0.001 - self.granted_time_to_enter = 0 - - brake = 0.0 - throttle = 0.0 - - if target_speed >= self.velocity: - # Calculate a proportional throttle (0.0 < throttle < 1.0) - throttle = min((target_speed - self.velocity)/target_speed, 1.0) - # throttle = 1.0 - brake = 0.0 - # throttle = min(abs(target_speed / self.velocity), 1) - else: - # Need to apply the brake - brake = min((self.velocity - target_speed)/self.velocity, 1.0) - # brake = 1.0 - throttle = 0.0 - - # Check throttle boundaries - if throttle < 0: - self.get_logger().info("Error: negative throttle") - throttle = 0.0 - - # Prepare and send the target velocity as a vehicle command - cmd = VehicleCommand() - cmd.vehicle_id = self.vehicle_id - cmd.throttle = throttle - cmd.brake = brake - self.control_.publish(cmd) - self.get_logger().info("Vehicle {}: Throttle: {}. Brake: {}".format(self.vehicle_id + 1, throttle, brake)) - + if pub_packets.request != None and not self.asking_for_grant: + request = Request() + request.requestor_id = pub_packets.request.requestor_id + request.speed = pub_packets.request.speed + request.position = pub_packets.request.position + self.request_.publish(request) + self.asking_for_grant = True def grant_callback(self, grant): if grant.requestor_id != self.vehicle_id: return - self.get_logger().info("Vehicle {} Granted access".format(self.vehicle_id + 1) + - "to enter the intersection at elapsed logical time {:d}.\n".format( - int(grant.arrival_time) - ) - ) - self.granted_time_to_enter = grant.arrival_time - self.intersection_position = grant.intersection_position + self.vehicle.grant(grant.arrival_time, grant.intersection_position) self.asking_for_grant = False + + +class ROSClock(VehicleClock): + def __init__(self, clock): + self.clock = clock + + def get_current_time_in_ns(self): + current_time = self.clock.now().to_msg() + return current_time.sec * BILLION + current_time.nanosec def main(args=None): From 343474e1025abce020135be44d075192925f6474 Mon Sep 17 00:00:00 2001 From: Hou Seng Wong Date: Thu, 24 Feb 2022 15:00:04 -0800 Subject: [PATCH 10/22] factor out rsu logic --- .../Carla/ROS/carla_intersection/src/rsu.py | 63 ++++++++++++++++ .../ROS/carla_intersection/src/rsu_node.py | 73 +++++-------------- .../Carla/ROS/carla_intersection/src/utils.py | 15 ++++ .../ROS/carla_intersection/src/vehicle.py | 4 +- .../carla_intersection/src/vehicle_node.py | 14 +--- 5 files changed, 101 insertions(+), 68 deletions(-) create mode 100644 experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/rsu.py diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/rsu.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/rsu.py new file mode 100644 index 0000000000..00fc0e9132 --- /dev/null +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/rsu.py @@ -0,0 +1,63 @@ +# Other libraries +from src.utils import distance, dotdict + +# Constants +from src.constants import BILLION + +class RSU: + def __init__(self, intersection_width, nominal_speed_in_intersection, intersection_position, clock, logger): + self.intersection_width = intersection_width + self.nominal_speed_in_intersection = nominal_speed_in_intersection + self.intersection_position = intersection_position + self.earliest_free = 0 # nsec + self.active_participants = [0] * 20 + self.clock = clock + self.logger = logger + + def receive_request(self, request): + pub_packets = dotdict() + self.active_participants[request.requestor_id] = 1 + if request.speed == 0.0: + # Avoid division by zero + request.speed = 0.001 + # Calculate the time it will take the approaching vehicle to + # arrive at its current speed. Note that this is + # time from the time the vehicle sends the message + # according to the arriving vehicle's clock. + speed_in_m_per_sec = request.speed + dr = distance(self.intersection_position, request.position) + time_to_intersection = dr / speed_in_m_per_sec + + self.logger.info("*** RSU: Vehicle {}'s distance to intersection is {}. ".format(request.requestor_id, dr, self.intersection_position, request.position, time_to_intersection)) + + current_time = self.clock.get_current_time_in_ns() + + # Convert the time interval to nsec (it is in seconds). + arrival_time_ns = int(current_time + (time_to_intersection * BILLION)) + + pub_packets.grant = dotdict() + pub_packets.grant.requestor_id = request.requestor_id + pub_packets.grant.intersection_position = self.intersection_position + if arrival_time_ns >= self.earliest_free: + # Vehicle can maintain speed. + pub_packets.grant.target_speed = request.speed + pub_packets.grant.arrival_time = arrival_time_ns + else: + # Could be smarter than this, but just send the nominal speed in intersection. + pub_packets.grant.target_speed = self.nominal_speed_in_intersection + # Vehicle has to slow down and maybe stop. + pub_packets.grant.arrival_time = self.earliest_free + + # Update earliest free on the assumption that the vehicle + # maintains its target speed (on average) within the intersection. + time_in_intersection_ns = int((BILLION * self.intersection_width) / (pub_packets.grant.target_speed)) + self.earliest_free = pub_packets.grant.arrival_time + time_in_intersection_ns + + self.logger.info("*** RSU: Granted access to vehicle {} to enter at " + "time {} ns with average target velocity {} m/s. Next available time is {}".format( + pub_packets.grant.requestor_id, + pub_packets.grant.arrival_time, + pub_packets.grant.target_speed, + self.earliest_free) + ) + return pub_packets \ No newline at end of file diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/rsu_node.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/rsu_node.py index 899deadaba..22a7cd5264 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/rsu_node.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/rsu_node.py @@ -6,13 +6,10 @@ from carla_intersection_msgs.msg import Request, Grant # Other libraries -from src.utils import distance, make_coordinate, make_Vector3 +from src.utils import distance, make_coordinate, make_Vector3, ROSClock +from src.rsu import RSU -# Constants -from src.constants import BILLION - - -class RSU(Node): +class RSUNode(Node): def __init__(self): super().__init__("rsu") @@ -25,64 +22,32 @@ def __init__(self): self.intersection_width = self.get_parameter('intersection_width').value self.nominal_speed_in_intersection = self.get_parameter('nominal_speed_in_intersection').value self.intersection_position = make_coordinate(self.get_parameter('intersection_position').value) - self.earliest_free = 0 # nsec - self.active_participants = [0] * 20 - + # pubsub for input / output ports self.grant_ = self.create_publisher(Grant, "grant", 10) self.request_ = self.create_subscription(Request, "request", self.request_callback, 10) + self.rsu = RSU(intersection_width = self.intersection_width, + nominal_speed_in_intersection = self.nominal_speed_in_intersection, + intersection_position = self.intersection_position, + clock = ROSClock(self.get_clock()), + logger = self.get_logger()) + def request_callback(self, request): - self.active_participants[request.requestor_id] = 1 - if request.speed == 0.0: - # Avoid division by zero - request.speed = 0.001 - # Calculate the time it will take the approaching vehicle to - # arrive at its current speed. Note that this is - # time from the time the vehicle sends the message - # according to the arriving vehicle's clock. - speed_in_m_per_sec = request.speed - dr = distance(self.intersection_position, request.position) - arrival_time_sec = dr / speed_in_m_per_sec + pub_packets = self.rsu.receive_request(request) + if pub_packets.grant != None: + grant = Grant() + grant.requestor_id = pub_packets.grant.requestor_id + grant.intersection_position = make_Vector3(pub_packets.grant.intersection_position) + grant.target_speed = pub_packets.grant.target_speed + grant.arrival_time = pub_packets.grant.arrival_time + self.grant_.publish(grant) - self.get_logger().info("*** RSU: Vehicle {}'s distance to intersection is {}. ".format(request.requestor_id+1, dr, self.intersection_position, request.position, arrival_time_sec)) - - time_message_sent = self.get_clock().now().to_msg() - - # Convert the time interval to nsec (it is in seconds). - arrival_time_ns = int(time_message_sent.sec * BILLION + time_message_sent.nanosec + (arrival_time_sec * BILLION)) - - response = Grant() - if arrival_time_ns >= self.earliest_free: - # Vehicle can maintain speed. - response.target_speed = request.speed - response.arrival_time = arrival_time_ns - else: - # Could be smarter than this, but just send the nominal speed in intersection. - response.target_speed = self.nominal_speed_in_intersection - # Vehicle has to slow down and maybe stop. - response.arrival_time = self.earliest_free - - response.intersection_position = make_Vector3(self.intersection_position) - response.requestor_id = request.requestor_id - self.grant_.publish(response) - # Update earliest free on the assumption that the vehicle - # maintains its target speed (on average) within the intersection. - time_in_intersection_ns = int((BILLION * self.intersection_width) / (response.target_speed)) - self.earliest_free = response.arrival_time + time_in_intersection_ns - - self.get_logger().info("*** RSU: Granted access to vehicle {} to enter at " - "time {} ns with average target velocity {} m/s. Next available time is {}".format( - response.requestor_id + 1, - response.arrival_time, - response.target_speed, - self.earliest_free) - ) def main(args=None): rclpy.init(args=args) - rsu = RSU() + rsu = RSUNode() rclpy.spin(rsu) diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py index 675e46799b..4f4146e4c0 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py @@ -7,10 +7,21 @@ class dotdict(dict): __setattr__ = dict.__setitem__ __delattr__ = dict.__delitem__ + class VehicleClock: def get_current_time_in_ns(self): assert False, "subclass must override this method" + +class ROSClock(VehicleClock): + def __init__(self, clock): + self.clock = clock + + def get_current_time_in_ns(self): + current_time = self.clock.now().to_msg() + return current_time.sec * BILLION + current_time.nanosec + + def distance(coordinate1, coordinate2): """ Calculate the great circle distance between two points @@ -34,14 +45,18 @@ def distance(coordinate1, coordinate2): m = km * 1000.0 return m + def make_coordinate(list): return dotdict({"x": list[0], "y": list[1], "z": list[2]}) + def make_spawn_point(list): return dotdict({"x": list[0], "y": list[1], "z": list[2], "yaw": list[3]}) + def make_Vector3(coordinate): return Vector3(x=coordinate.x, y=coordinate.y, z=coordinate.z) + def make_speed(velocity): return sqrt(velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2) diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle.py index fed0657026..2238cc2975 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle.py @@ -1,4 +1,4 @@ -from src.utils import make_speed, make_Vector3, dotdict, distance +from src.utils import make_speed, dotdict, distance from src.constants import BILLION, GOAL_REACHED_THRESHOLD, GOAL_REACHED_THRESHOLD_TIME, SPEED_LIMIT class Vehicle: @@ -54,7 +54,7 @@ def receive_velocity_from_simulator(self, vehicle_stat): pub_packets.request = dotdict() pub_packets.request.requestor_id = self.vehicle_id pub_packets.request.speed = self.get_speed() - pub_packets.request.position = make_Vector3(self.get_position()) + pub_packets.request.position = self.get_position() # Stop the vehicle pub_packets.cmd = dotdict() diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle_node.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle_node.py index a11903462f..53464c8b56 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle_node.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle_node.py @@ -8,7 +8,7 @@ # Other libraries from math import sqrt -from src.utils import distance, make_coordinate, make_Vector3, VehicleClock +from src.utils import distance, make_coordinate, make_Vector3, ROSClock from src.vehicle import Vehicle # Constants @@ -55,7 +55,7 @@ def vehicle_stat_callback(self, vehicle_stat): request = Request() request.requestor_id = pub_packets.request.requestor_id request.speed = pub_packets.request.speed - request.position = pub_packets.request.position + request.position = make_Vector3(pub_packets.request.position) self.request_.publish(request) self.asking_for_grant = True @@ -64,16 +64,6 @@ def grant_callback(self, grant): return self.vehicle.grant(grant.arrival_time, grant.intersection_position) self.asking_for_grant = False - - -class ROSClock(VehicleClock): - def __init__(self, clock): - self.clock = clock - - def get_current_time_in_ns(self): - current_time = self.clock.now().to_msg() - return current_time.sec * BILLION + current_time.nanosec - def main(args=None): rclpy.init(args=args) From 25445d3714ae5cae6fc08190a7d36441d72e0398 Mon Sep 17 00:00:00 2001 From: Hou Seng Wong Date: Thu, 24 Feb 2022 15:24:12 -0800 Subject: [PATCH 11/22] add comments --- .../ROS/carla_intersection/src/carla_sim.py | 2 +- .../Carla/ROS/carla_intersection/src/rsu.py | 25 ++++++++- .../Carla/ROS/carla_intersection/src/utils.py | 8 ++- .../ROS/carla_intersection/src/vehicle.py | 52 +++++++++++++++---- 4 files changed, 74 insertions(+), 13 deletions(-) diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py index bea2709d59..67f566e94e 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py @@ -74,7 +74,7 @@ def get_vehicle_velocity(self): def get_vehicle_position(self): return self.gps_queue.get() - def apply_control(self, throttle, brake): + def apply_control(self, throttle: float, brake: float): self.vehicle.apply_control(carla.VehicleControl(throttle=throttle, brake=brake)) def get_world(self): diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/rsu.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/rsu.py index 00fc0e9132..b903438abf 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/rsu.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/rsu.py @@ -14,7 +14,30 @@ def __init__(self, intersection_width, nominal_speed_in_intersection, intersecti self.clock = clock self.logger = logger - def receive_request(self, request): + + def receive_request(self, request) -> dotdict: + ''' + Handles the request to enter the intersection from a vehicle. + + Parameters + ---------- + request: Class + A request to enter the intersection from a + vehicle with the following structure: + - requestor_id : int + - speed : float + - position : coordinate + + Returns + ---------- + dotdict + A dotted dictionary with the following structure: + - grant + - requestor_id : int + - intersection_position : coordinate + - target_speed : float + - arrival_time : int + ''' pub_packets = dotdict() self.active_participants[request.requestor_id] = 1 if request.speed == 0.0: diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py index 4f4146e4c0..ae5089b6e6 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py @@ -8,6 +8,12 @@ class dotdict(dict): __delattr__ = dict.__delitem__ +class Coordinate: + def __init__(self, x, y, z): + self.x = x + self.y = y + self.z = z + class VehicleClock: def get_current_time_in_ns(self): assert False, "subclass must override this method" @@ -47,7 +53,7 @@ def distance(coordinate1, coordinate2): def make_coordinate(list): - return dotdict({"x": list[0], "y": list[1], "z": list[2]}) + return Coordinate(x=list[0], y=list[1], z=list[2]) def make_spawn_point(list): diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle.py index 2238cc2975..97268761ca 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle.py @@ -1,4 +1,4 @@ -from src.utils import make_speed, dotdict, distance +from src.utils import make_speed, dotdict, distance, Coordinate from src.constants import BILLION, GOAL_REACHED_THRESHOLD, GOAL_REACHED_THRESHOLD_TIME, SPEED_LIMIT class Vehicle: @@ -13,35 +13,67 @@ def __init__(self, vehicle_id, initial_position, initial_velocity, clock, logger self.clock = clock self.logger = logger - def set_velocity(self, new_velocity): + def set_velocity(self, new_velocity: Coordinate) -> None: self.velocity = new_velocity self.speed = make_speed(new_velocity) # Prevent divisions by zero if self.speed == 0.0: self.speed = 0.001 - def set_position(self, new_position): + def set_position(self, new_position: Coordinate) -> None: self.position = new_position - def get_velocity(self): + def get_velocity(self) -> Coordinate: + ''' + Returns the current velocity of the vehicle as a coordinate. + ''' return self.velocity - def get_position(self): + def get_position(self) -> Coordinate: + ''' + Returns the current position of the vehicle as a coordinate. + ''' return self.position - def get_speed(self): + def get_speed(self) -> float: + ''' + Returns the current speed of the vehicle. + ''' return self.speed - def grant(self, arrival_time, intersection_position): - self.logger.info("Vehicle {} Granted access".format(self.vehicle_id + 1) + + def grant(self, arrival_time: int, intersection_position: Coordinate) -> None: + ''' + Grants the vehicle access to the intersection. + ''' + self.logger.info("Vehicle {} Granted access".format(self.vehicle_id) + "to enter the intersection at elapsed logical time {:d}.\n".format( - int(arrival_time) + arrival_time ) ) self.granted_time_to_enter = arrival_time self.intersection_position = intersection_position - def receive_velocity_from_simulator(self, vehicle_stat): + def receive_velocity_from_simulator(self, vehicle_stat: Coordinate) -> dotdict: + ''' + React to the velocity from the simulator + + Parameters + ---------- + vehicle_stat: Coordinate + The updated velocity of the vehicle. + + Returns + ---------- + dotdict + A dotted dictionary with the following structure: + - request + - requestor_id : int + - speed : float + - position : Coordinate + - cmd + - throttle : float + - brake : float + ''' pub_packets = dotdict() if self.goal_reached: return pub_packets From 1a03f0fa8df773df33e41f4a64cd8b79c8c41668 Mon Sep 17 00:00:00 2001 From: Hou Seng Wong Date: Thu, 24 Feb 2022 16:07:21 -0800 Subject: [PATCH 12/22] factor out launch parameters --- .../launch/intersection_demo.launch.py | 49 +++---------------- .../src/launch_parameters.py | 20 ++++++++ 2 files changed, 28 insertions(+), 41 deletions(-) create mode 100644 experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/launch_parameters.py diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/launch/intersection_demo.launch.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/launch/intersection_demo.launch.py index 5490aecb71..cd0064a97a 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/launch/intersection_demo.launch.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/launch/intersection_demo.launch.py @@ -2,45 +2,12 @@ import launch.actions import launch.substitutions from launch_ros.actions import Node - +import sys +from os import path +sys.path.insert(0, path.dirname(path.dirname(__file__))) +from src.launch_parameters import SPAWN_POINTS, INITIAL_POSITIONS, INITIAL_VELOCITIES def generate_launch_description(): - spawn_points = [{ \ - "x": -122.0, \ - "y": 39.6, \ - "z": 0.3, \ - "yaw": -90.0, \ - }, { \ - "x": -177.77, \ - "y": 6.48, \ - "z": 0.3, \ - "yaw": 0.0 \ - }, { \ - "x": -132.77, \ - "y": -40, \ - "z": 0.3, \ - "yaw": 90.0 \ - }, { \ - "x": -80.77, \ - "y": -4.5, \ - "z": 0.3, \ - "yaw": 180.0} \ - ] - - initial_positions = [ - [0.000038,-0.000674,2.794825], # /|\ - [-0.000501,-0.001084,2.794891], # -> - [-0.000060,-0.001510,2.794854], # \|/ - [0.000367,-0.001185,2.794846] # <- - ] - - initial_velocities = [ - [ 0.0, -8.0, 0.0], - [ 8.0, 0.0, 0.0], - [ 0.0, 8.0, 0.0], - [-8.0, 0.0, 0.0] - ] - nodes = [] nodes.append( Node( @@ -64,8 +31,8 @@ def generate_launch_description(): executable='vehicle_node', parameters=[ {"vehicle_id": i}, - {"initial_velocity": initial_velocities[i]}, - {"initial_position": initial_positions[i]} + {"initial_velocity": INITIAL_VELOCITIES[i]}, + {"initial_position": INITIAL_POSITIONS[i]} ] ) ) @@ -75,8 +42,8 @@ def generate_launch_description(): executable='carla_sim_node', parameters=[ {"vehicle_id": i}, - {"initial_velocity": initial_velocities[i]}, - {"spawn_point": [spawn_points[i]["x"], spawn_points[i]["y"], spawn_points[i]["z"], spawn_points[i]["yaw"]]} + {"initial_velocity": INITIAL_VELOCITIES[i]}, + {"spawn_point": [SPAWN_POINTS[i]["x"], SPAWN_POINTS[i]["y"], SPAWN_POINTS[i]["z"], SPAWN_POINTS[i]["yaw"]]} ] ) ) diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/launch_parameters.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/launch_parameters.py new file mode 100644 index 0000000000..f705d2e108 --- /dev/null +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/launch_parameters.py @@ -0,0 +1,20 @@ +SPAWN_POINTS = [ + {"x": -122.0, "y": 39.6, "z": 0.3, "yaw": -90.0}, + {"x": -177.77, "y": 6.48, "z": 0.3, "yaw": 0.0}, + {"x": -132.77, "y": -40, "z": 0.3, "yaw": 90.0}, + {"x": -80.77, "y": -4.5, "z": 0.3, "yaw": 180.0} +] + +INITIAL_POSITIONS = [ + [0.000038,-0.000674,2.794825], # /|\ + [-0.000501,-0.001084,2.794891], # -> + [-0.000060,-0.001510,2.794854], # \|/ + [0.000367,-0.001185,2.794846] # <- +] + +INITIAL_VELOCITIES = [ + [ 0.0, -8.0, 0.0], + [ 8.0, 0.0, 0.0], + [ 0.0, 8.0, 0.0], + [-8.0, 0.0, 0.0] +] \ No newline at end of file From 9d0a89a6e96a54b71c5be9f3372bbb91753a27fe Mon Sep 17 00:00:00 2001 From: Hou Seng Wong Date: Thu, 24 Feb 2022 16:58:51 -0800 Subject: [PATCH 13/22] rename topic --- .../ROS/carla_intersection/src/carla_sim_node.py | 6 +++--- .../Carla/ROS/carla_intersection/src/utils.py | 1 + .../Carla/ROS/carla_intersection/src/vehicle_node.py | 12 ++++++------ 3 files changed, 10 insertions(+), 9 deletions(-) diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim_node.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim_node.py index a36b4a6392..2f5e9d9189 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim_node.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim_node.py @@ -32,8 +32,8 @@ def __init__(self): self.world_is_ready = False # pubsub for input and output ports - self.status_ = self.create_publisher(Vector3, f"status_to_vehicle_stats_{self.vehicle_id}", 10) - self.position_ = self.create_publisher(Vector3, f"position_to_vehicle_pos_{self.vehicle_id}", 10) + self.velocity_ = self.create_publisher(Vector3, f"vehicle_velocity_{self.vehicle_id}", 10) + self.position_ = self.create_publisher(Vector3, f"vehicle_position_{self.vehicle_id}", 10) self.command_ = self.create_subscription(VehicleCommand, f"control_to_command_{self.vehicle_id}", self.command_callback, 10) self.carla_sim = CarlaSim(interval=self.interval, vehicle_type=self.vehicle_type, @@ -58,7 +58,7 @@ def timer_callback(self): if not self.world_is_ready: return self.carla_sim.tick() - self.status_.publish(make_Vector3(self.carla_sim.get_vehicle_velocity())) + self.velocity_.publish(make_Vector3(self.carla_sim.get_vehicle_velocity())) position = self.carla_sim.get_vehicle_position() coordinate = make_coordinate([position.latitude, position.longitude, position.altitude]) self.position_.publish(make_Vector3(coordinate)) diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py index ae5089b6e6..23ae8e30cb 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py @@ -1,5 +1,6 @@ from math import sin, cos, sqrt, atan2, radians from geometry_msgs.msg import Vector3 +from src.constants import BILLION class dotdict(dict): """dot.notation access to dictionary attributes""" diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle_node.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle_node.py index 53464c8b56..87edc9a154 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle_node.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle_node.py @@ -35,17 +35,17 @@ def __init__(self): logger = self.get_logger()) # pubsub for input output ports - self.vehicle_stat_ = self.create_subscription(Vector3, f"status_to_vehicle_stats_{self.vehicle_id}", self.vehicle_stat_callback, 10) - self.vehicle_pos_ = self.create_subscription(Vector3, f"position_to_vehicle_pos_{self.vehicle_id}", self.vehicle_pos_callback, 10) + self.velocity_ = self.create_subscription(Vector3, f"vehicle_velocity_{self.vehicle_id}", self.velocity_callback, 10) + self.position_ = self.create_subscription(Vector3, f"vehicle_position_{self.vehicle_id}", self.position_callback, 10) self.control_ = self.create_publisher(VehicleCommand, f"control_to_command_{self.vehicle_id}", 10) self.grant_ = self.create_subscription(Grant, "grant", self.grant_callback, 10) self.request_ = self.create_publisher(Request, "request", 10) - def vehicle_pos_callback(self, vehicle_pos): - self.vehicle.set_position(vehicle_pos) + def position_callback(self, new_position): + self.vehicle.set_position(new_position) - def vehicle_stat_callback(self, vehicle_stat): - pub_packets = self.vehicle.receive_velocity_from_simulator(vehicle_stat) + def velocity_callback(self, new_velocity): + pub_packets = self.vehicle.receive_velocity_from_simulator(new_velocity) if pub_packets.cmd != None: cmd = VehicleCommand() cmd.throttle = pub_packets.cmd.throttle From 6cc3a602f39c09674d6dcce0a0b113f8df40a54d Mon Sep 17 00:00:00 2001 From: Hou Seng Wong Date: Fri, 25 Feb 2022 11:38:12 -0800 Subject: [PATCH 14/22] first attempt toward making LF use the same logic as ros --- .../Intersection/Carla/CarlaIntersection.lf | 342 ++---------------- .../Python/src/Intersection/Carla/CarlaSim.lf | 65 ++++ .../launch/intersection_demo.launch.py | 9 +- .../ROS/carla_intersection/src/carla_sim.py | 4 +- .../carla_intersection/src/carla_sim_node.py | 7 +- .../src/launch_parameters.py | 8 +- .../ROS/carla_intersection/src/ros_utils.py | 4 + .../Carla/ROS/carla_intersection/src/rsu.py | 4 +- .../ROS/carla_intersection/src/rsu_node.py | 3 +- .../Carla/ROS/carla_intersection/src/utils.py | 16 +- .../ROS/carla_intersection/src/vehicle.py | 6 +- .../carla_intersection/src/vehicle_node.py | 6 +- .../Python/src/Intersection/Carla/RSU.lf | 49 +++ .../Python/src/Intersection/Carla/Vehicle.lf | 65 ++++ 14 files changed, 251 insertions(+), 337 deletions(-) create mode 100644 experimental/Python/src/Intersection/Carla/CarlaSim.lf create mode 100644 experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/ros_utils.py create mode 100644 experimental/Python/src/Intersection/Carla/RSU.lf create mode 100644 experimental/Python/src/Intersection/Carla/Vehicle.lf diff --git a/experimental/Python/src/Intersection/Carla/CarlaIntersection.lf b/experimental/Python/src/Intersection/Carla/CarlaIntersection.lf index 0c328723dc..cc4c502e1f 100644 --- a/experimental/Python/src/Intersection/Carla/CarlaIntersection.lf +++ b/experimental/Python/src/Intersection/Carla/CarlaIntersection.lf @@ -19,266 +19,23 @@ */ target Python { timeout: 40 sec, + files: [ + "ROS/carla_intersection/src/launch_parameters.py", + + ] // fast: true // You can enable fast to get a much faster simulation // logging: DEBUG }; -import Vehicle, RSU from "../Intersection.lf"; +import Vehicle from "Vehicle.lf"; +import RSU from "RSU.lf"; +import CarlaSim from "CarlaSim.lf"; preamble {= -import glob -import os -CARLA_INSTALL_DIR = "/opt/carla-simulator" -try: - sys.path.append(glob.glob(CARLA_INSTALL_DIR + "/PythonAPI/carla/dist/carla-*%d.%d-%s.egg" % ( - sys.version_info.major, - sys.version_info.minor, - "win-amd64" if os.name == "nt" else "linux-x86_64"))[0]) -except IndexError: - pass - -try: - import carla -except ImportError: - sys.stderr.write("ERROR: Could not find the Carla .egg file.\nPlease make sure that" - " CARLA_INSTALL_DIR in CarlaIntersection.lf points to the correct location.\n") - -try: - import queue -except ImportError: - import Queue as queue - -class Vector3D: - def __init__(self, x=0, y=0, z=0): - self.x = x - self.y = y - self.z = z - + from launch_parameters import SPAWN_POINTS, INITIAL_POSITIONS, INITIAL_VELOCITIES, \ + INTERSECTION_WIDTH, NOMINAL_SPEED_IN_INTERSECTION, INTERSECTION_POSITION =} -reactor Carla( - vehicle_bank_index(0), - vehicle_name(""), - initial_speeds({=[]=}), - initial_speed({=Vector3D(x = 12.0)=}), - vehicle_type("vehicle.tesla.model3"), - positions({=[]=}), - spawn_point({= { \ - "x": -122.0, \ - "y": 39.6, \ - "z": 0.3, \ - "yaw": -90.0} - =}) -) { - preamble {= - ## need vehicle status and gps - ## send target velocity - =} - - input command; - input world_is_ready; - input tick; - output status; - output position; - - state world; - state client; - state vehicle; - state gps; - state gps_queue; - - reaction(world_is_ready) {= - print("Got the world") - self.client = carla.Client("localhost", 2000) - self.world = self.client.get_world() - - blueprint_library = self.world.get_blueprint_library() - - sensors_bp = {} - sensors = {} - sensors_to_spawn = { \ - "gps": "sensor.other.gnss", \ - "imu": "sensor.other.imu" - } - - spawn_point = self.spawn_point(self) - # Spawn the vehicle - vehicle_bp = blueprint_library.find(self.vehicle_type) - transform = carla.Transform(carla.Location( \ - x = spawn_point["x"], \ - y = spawn_point["y"], \ - z = spawn_point["z"] \ - ), carla.Rotation(yaw = spawn_point["yaw"])) - self.vehicle = self.world.spawn_actor(vehicle_bp, transform) - - for key in sensors_to_spawn.keys(): - sensors_bp[key] = blueprint_library.find(sensors_to_spawn[key]) - if key == "gps": - # Spawn the GPS sensor that is attached to the vehicle - relative_transform = carla.Transform(carla.Location( \ - x = 1.0, \ - y = 0.0, \ - z = 2.0), carla.Rotation()) - - elif key == "imu": - # Spawn the imu unit - relative_transform = carla.Transform(carla.Location( \ - x = 2.0, \ - y = 0.0, \ - z = 2.0), carla.Rotation()) - else: - relative_transform = carla.Transform(carla.Location(), carla.Rotation()) - - sensors[key] = self.world.spawn_actor( \ - sensors_bp[key], \ - relative_transform, \ - attach_to=self.vehicle, \ - attachment_type=carla.AttachmentType.Rigid \ - ) - - self.gps = sensors["gps"] - self.gps_queue = queue.Queue() - self.gps.listen(self.gps_queue.put) - - # Set the initial speed - target_speed = self.initial_speed(self) - for i in range(1): - self.vehicle.set_target_velocity(carla.Vector3D(x=target_speed.x, y=target_speed.y, z=target_speed.z)) - # self.vehicle.apply_control(carla.VehicleControl(throttle=1.0, steer=0.0)) - # self.world.tick() - - print("Spawned vehicle") - - =} - reaction(tick) -> status, position {= - velocity = self.vehicle.get_velocity() - stat = vehicle_status(Vector3D( - x = velocity.x, - y = velocity.y, - z = velocity.z - )) - status.set(stat) - - gps_pos = self.gps_queue.get() - # print(gps_pos.latitude, gps_pos.longitude, gps_pos.altitude) - pos = vehicle_position(coordinate(x = gps_pos.latitude, y = gps_pos.longitude, z= gps_pos.altitude)) - position.set(pos) - =} - reaction(command) {= - # print("Applying control: throttle: ", command.value.throttle) - self.vehicle.apply_control( \ - carla.VehicleControl( \ - throttle=command.value.throttle, \ - brake=command.value.brake \ - ) \ - ) - =} -} - -/** - * A centralized ticker that sets up the world and - * the client in synchronous mode. - * - * Outputs: - * world: The Carla world retrieved from the client - * tick: The periodic tick of the simulator - * - * Note: In theory, it should be possible to have multiple - * synchronous clients (in which, the Carla simulator server - * waits for a command from both clients before ticking). In - * practice, it seems to not work as expected. - */ -reactor Ticker ( - interval(16 msec) - ){ - output world_is_ready; - output tick; - state client; - state world; - reaction(startup) -> world_is_ready {= - self.client = carla.Client("localhost", 2000) - self.client.set_timeout(10.0) # seconds - - # Set the world to town 5 - self.world = self.client.load_world("Town05") - - settings = self.world.get_settings() - settings.fixed_delta_seconds = self.interval / SEC(1) - settings.substepping = True - settings.max_substep_delta_time = settings.fixed_delta_seconds / 10 - settings.max_substeps = 10 - settings.synchronous_mode = True # Enables synchronous mode - - self.world.apply_settings(settings) - - # Set the weather - weather = carla.WeatherParameters( - cloudiness=80.0, - precipitation=30.0, - sun_altitude_angle=70.0) - - self.world.set_weather(weather) - - # Set the spectator (camera) position - transform = carla.Transform(carla.Location(x=-126.163864, y=3, z=90), \ - carla.Rotation(pitch=-90, yaw=-180, roll=0)) - - self.world.get_spectator().set_transform(transform) - - # Send the world to Carla interface reactors - world_is_ready.set(True) - =} - timer t(1 nsec, interval); - reaction(t) -> tick {= - # print("Ticking") - - # Synchronous client mode - self.world.tick() - - # self.world.wait_for_tick() # Use this instead for asynchronous client mode - tick.set(True) - =} - reaction(shutdown) {= - # Set the world to async mode to stop it from freezing - settings = self.world.get_settings() - settings.synchronous_mode = False # Disable synchronous mode - self.world.apply_settings(settings) - - # Reload the world to destroy all actors - self.client.reload_world() - =} -} - -reactor ego_vehicle ( - bank_index(0), - positions({=[]=}), - initial_speeds({=[]=}) - ) { - input grant; - input tick; - input world_is_ready; - output request; - - carla = new Carla( - vehicle_bank_index = bank_index, - vehicle_name = "ego_vehicle", - positions = positions, - spawn_point = {=lambda self: self.positions[self.vehicle_bank_index]=}, - initial_speeds = initial_speeds, - initial_speed = {=lambda self: self.initial_speeds[self.vehicle_bank_index]=} - ); - - vehicle = new Vehicle(vehicle_id = bank_index); - vehicle.request -> request; - grant -> vehicle.grant; - - world_is_ready -> carla.world_is_ready; - tick -> carla.tick; - vehicle.control -> carla.command; - carla.status -> vehicle.vehicle_stat; - carla.position -> vehicle.vehicle_pos; -} - reactor Relay(duration_in_seconds(0)) { preamble {= from time import sleep @@ -296,68 +53,39 @@ reactor Relay(duration_in_seconds(0)) { =} } -federated reactor ( +main reactor ( num_entries(4), - positions({= [{ \ - "x": -122.0, \ - "y": 39.6, \ - "z": 0.3, \ - "yaw": -90.0, \ - }, { \ - "x": -177.77, \ - "y": 6.48, \ - "z": 0.3, \ - "yaw": 0.0 \ - }, { \ - "x": -132.77, \ - "y": -40, \ - "z": 0.3, \ - "yaw": 90.0 \ - }, { \ - "x": -80.77, \ - "y": -4.5, \ - "z": 0.3, \ - "yaw": 180.0} \ - ]=}), - initial_speeds({= \ - [ \ - Vector3D(y = -8), Vector3D(x = 8), \ - Vector3D(y = 8), Vector3D(x = -8) \ - ] - =}) + spawn_points({=SPAWN_POINTS=}), + initial_velocities({=INITIAL_VELOCITIES=}), + initial_positions({=INITIAL_POSITIONS=}), + intersection_width({=INTERSECTION_WIDTH=}), + nominal_speed_in_intersection({=NOMINAL_SPEED_IN_INTERSECTION=}), + intersection_position({=INTERSECTION_POSITION=}) ) { - egos = new[num_entries] ego_vehicle( - positions = positions, - initial_speeds = initial_speeds + carla = new[num_entries] CarlaSim( + interval = 16 msec, + initial_velocities = initial_velocities, + spawn_points = spawn_points + ); + + vehicle = new[num_entries] Vehicle( + initial_velocities = initial_velocities, + initial_positions = initial_positions ); rsu = new RSU( - num_entries = num_entries, - intersection_pos = {=coordinate(-0.000007632,-0.001124366,2.792485)=}, - intersection_width = 40, - nominal_speed_in_intersection = 14 + num_entries = num_entries, + intersection_width = intersection_width, + nominal_speed_in_intersection = nominal_speed_in_intersection, + intersection_position = intersection_position ); - ticker = new Ticker(); - (ticker.world_is_ready)+ -> egos.world_is_ready; - (ticker.tick)+ -> egos.tick; - - // Use Relays - // request_relays = new[num_entries] Relay(duration_in_seconds=0); - // egos.request -> request_relays.i; - // request_relays.o -> rsu.request; + carla.velocity_ -> vehicle.velocity_; + carla.position_ -> vehicle.position_; + vehicle.control_ -> carla.command_; - // Don't use Relays - egos.request -> rsu.request; - - // For the purposes of this simulations, we don't want the simulation - // to abruptly end once the last vehicle has reached the intersection. - // vehicles.goal_reached -> rsu.vehicle_reached_intersection; - // Use Relays - // grant_relays = new[num_entries] Relay(duration_in_seconds=0); - // rsu.grant -> grant_relays.i; - // grant_relays.o -> egos.grant; + vehicle.request_ -> rsu.request_; + rsu.grant_ -> vehicle.grant_; - // Don't use Relays - rsu.grant -> egos.grant; + } diff --git a/experimental/Python/src/Intersection/Carla/CarlaSim.lf b/experimental/Python/src/Intersection/Carla/CarlaSim.lf new file mode 100644 index 0000000000..5acb3c46d6 --- /dev/null +++ b/experimental/Python/src/Intersection/Carla/CarlaSim.lf @@ -0,0 +1,65 @@ +target Python { + files: [ + "ROS/carla_intersection/src/carla_sim.py", + "ROS/carla_intersection/src/utils.py" + ] +}; + +preamble {= + from utils import make_coordinate, make_spawn_point + from carla_sim import CarlaSim +=} + +reactor CarlaSim( + interval(16 msec), + bank_index(0), + initial_velocities({=None=}), + vehicle_type("vehicle.tesla.model3"), + spawn_points({=None=}) +) { + input command_; + output velocity_; + output position_; + logical action world_is_ready; + state carla_sim; + timer timer_(10 msec, interval); + + preamble {= + class Logger: + def __init__(self, vehicle_id): + self.vehicle_id = vehicle_id + + def info(self, *args): + print(f"carla_sim_{self.vehicle_id}: ", args) + =} + + reaction(startup) -> world_is_ready {= + self.carla_sim = CarlaSim(interval=self.interval, + vehicle_type=self.vehicle_type, + initial_velocity=make_coordinate(self.initial_velocities[self.bank_index]), + spawn_point=make_spawn_point(self.spawn_points[self.bank_index]), + logger=self.Logger(self.bank_index)) + self.carla_sim.connect_to_carla() + if self.bank_index == 0: + self.carla_sim.initialize_world() + world_is_ready.schedule(MSEC(5)) + =} + + reaction(world_is_ready) {= + self.carla_sim.get_world() + self.carla_sim.initialize_vehicle() + =} + + reaction(timer_) -> velocity_, position_ {= + self.carla_sim.tick() + velocity_.set(self.carla_sim.get_vehicle_velocity()) + p = self.carla_sim.get_vehicle_position() + coordinate = make_coordinate([p.latitude, p.longitude, p.altitude]) + position_.set(coordinate) + =} + + reaction(command_) {= + cmd = command_.value + self.carla_sim.apply_control(cmd.throttle, cmd.brake) + =} +} \ No newline at end of file diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/launch/intersection_demo.launch.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/launch/intersection_demo.launch.py index cd0064a97a..2cbfa9d6a9 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/launch/intersection_demo.launch.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/launch/intersection_demo.launch.py @@ -5,7 +5,8 @@ import sys from os import path sys.path.insert(0, path.dirname(path.dirname(__file__))) -from src.launch_parameters import SPAWN_POINTS, INITIAL_POSITIONS, INITIAL_VELOCITIES +from src.launch_parameters import SPAWN_POINTS, INITIAL_POSITIONS, INITIAL_VELOCITIES, \ + INTERSECTION_WIDTH, NOMINAL_SPEED_IN_INTERSECTION, INTERSECTION_POSITION def generate_launch_description(): nodes = [] @@ -14,9 +15,9 @@ def generate_launch_description(): package='carla_intersection', executable='rsu_node', parameters=[ - {"intersection_position": [-0.000007632,-0.001124366,2.792485]}, - {"intersection_width": 40}, - {"nominal_speed_in_intersection": 14.0} + {"intersection_position": INTERSECTION_POSITION}, + {"intersection_width": INTERSECTION_WIDTH}, + {"nominal_speed_in_intersection": NOMINAL_SPEED_IN_INTERSECTION} ] ) ) diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py index 67f566e94e..41c9c8df8a 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py @@ -8,7 +8,7 @@ import Queue as queue # Constants -from src.constants import CARLA_INSTALL_DIR +from constants import CARLA_INSTALL_DIR # Set up Carla try: @@ -37,7 +37,7 @@ def __init__(self, interval, vehicle_type, initial_velocity, spawn_point, logger self.spawn_point = spawn_point self.logger = logger - def initialize_carla(self): + def connect_to_carla(self): # initialize Carla self.client = carla.Client("localhost", 2000) self.client.set_timeout(10.0) # seconds diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim_node.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim_node.py index 2f5e9d9189..d7c602edbe 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim_node.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim_node.py @@ -8,7 +8,8 @@ from geometry_msgs.msg import Vector3 # Other libraries -from src.utils import make_coordinate, make_spawn_point, make_Vector3 +from src.utils import make_coordinate, make_spawn_point +from src.ros_utils import make_Vector3 from src.carla_sim import CarlaSim # The Carla Simulator @@ -39,7 +40,7 @@ def __init__(self): self.carla_sim = CarlaSim(interval=self.interval, vehicle_type=self.vehicle_type, initial_velocity=self.initial_velocity, spawn_point=self.spawn_point, logger=self.get_logger()) - self.carla_sim.initialize_carla() + self.carla_sim.connect_to_carla() if self.vehicle_id == 0: self.world_is_ready_ = self.create_publisher(Bool, "world_is_ready", 10) self.carla_sim.initialize_world() @@ -48,7 +49,7 @@ def __init__(self): else: self.world_is_ready_ = self.create_subscription(Bool, "world_is_ready", self.world_is_ready_callback, 10) - # timer (should be after initialize_carla() is called) + # timer (should be after connect_to_carla() is called) self.timer_ = self.create_timer(self.interval / 1000.0, self.timer_callback) def command_callback(self, command): diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/launch_parameters.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/launch_parameters.py index f705d2e108..d0bfa973b6 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/launch_parameters.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/launch_parameters.py @@ -17,4 +17,10 @@ [ 8.0, 0.0, 0.0], [ 0.0, 8.0, 0.0], [-8.0, 0.0, 0.0] -] \ No newline at end of file +] + +INTERSECTION_WIDTH = 40 + +NOMINAL_SPEED_IN_INTERSECTION = 14.0 + +INTERSECTION_POSITION = [-0.000007632,-0.001124366,2.792485] \ No newline at end of file diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/ros_utils.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/ros_utils.py new file mode 100644 index 0000000000..b744bf823b --- /dev/null +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/ros_utils.py @@ -0,0 +1,4 @@ +from geometry_msgs.msg import Vector3 + +def make_Vector3(coordinate): + return Vector3(x=coordinate.x, y=coordinate.y, z=coordinate.z) \ No newline at end of file diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/rsu.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/rsu.py index b903438abf..ae9272b42a 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/rsu.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/rsu.py @@ -1,8 +1,8 @@ # Other libraries -from src.utils import distance, dotdict +from utils import distance, dotdict # Constants -from src.constants import BILLION +from constants import BILLION class RSU: def __init__(self, intersection_width, nominal_speed_in_intersection, intersection_position, clock, logger): diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/rsu_node.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/rsu_node.py index 22a7cd5264..b83efbfe24 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/rsu_node.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/rsu_node.py @@ -6,7 +6,8 @@ from carla_intersection_msgs.msg import Request, Grant # Other libraries -from src.utils import distance, make_coordinate, make_Vector3, ROSClock +from src.utils import make_coordinate, ROSClock +from src.ros_utils import make_Vector3 from src.rsu import RSU class RSUNode(Node): diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py index 23ae8e30cb..a1957e3966 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py @@ -1,6 +1,5 @@ from math import sin, cos, sqrt, atan2, radians -from geometry_msgs.msg import Vector3 -from src.constants import BILLION +from constants import BILLION class dotdict(dict): """dot.notation access to dictionary attributes""" @@ -15,12 +14,14 @@ def __init__(self, x, y, z): self.y = y self.z = z -class VehicleClock: +class GenericClock: def get_current_time_in_ns(self): assert False, "subclass must override this method" +class LFClock(GenericClock): + def get_current_time_in_ns(self): + return get_logical_time() - -class ROSClock(VehicleClock): +class ROSClock(GenericClock): def __init__(self, clock): self.clock = clock @@ -60,10 +61,5 @@ def make_coordinate(list): def make_spawn_point(list): return dotdict({"x": list[0], "y": list[1], "z": list[2], "yaw": list[3]}) - -def make_Vector3(coordinate): - return Vector3(x=coordinate.x, y=coordinate.y, z=coordinate.z) - - def make_speed(velocity): return sqrt(velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2) diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle.py index 97268761ca..7c2d1778e7 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle.py @@ -1,5 +1,5 @@ -from src.utils import make_speed, dotdict, distance, Coordinate -from src.constants import BILLION, GOAL_REACHED_THRESHOLD, GOAL_REACHED_THRESHOLD_TIME, SPEED_LIMIT +from utils import make_speed, dotdict, distance, Coordinate +from constants import BILLION, GOAL_REACHED_THRESHOLD, GOAL_REACHED_THRESHOLD_TIME, SPEED_LIMIT class Vehicle: def __init__(self, vehicle_id, initial_position, initial_velocity, clock, logger): @@ -67,7 +67,6 @@ def receive_velocity_from_simulator(self, vehicle_stat: Coordinate) -> dotdict: dotdict A dotted dictionary with the following structure: - request - - requestor_id : int - speed : float - position : Coordinate - cmd @@ -84,7 +83,6 @@ def receive_velocity_from_simulator(self, vehicle_stat: Coordinate) -> dotdict: # the intersection is granted if self.granted_time_to_enter == 0: pub_packets.request = dotdict() - pub_packets.request.requestor_id = self.vehicle_id pub_packets.request.speed = self.get_speed() pub_packets.request.position = self.get_position() diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle_node.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle_node.py index 87edc9a154..1ae94d2ec7 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle_node.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle_node.py @@ -7,8 +7,8 @@ from geometry_msgs.msg import Vector3 # Other libraries -from math import sqrt -from src.utils import distance, make_coordinate, make_Vector3, ROSClock +from src.utils import make_coordinate, ROSClock +from src.ros_utils import make_Vector3 from src.vehicle import Vehicle # Constants @@ -53,7 +53,7 @@ def velocity_callback(self, new_velocity): self.control_.publish(cmd) if pub_packets.request != None and not self.asking_for_grant: request = Request() - request.requestor_id = pub_packets.request.requestor_id + request.requestor_id = self.vehicle_id request.speed = pub_packets.request.speed request.position = make_Vector3(pub_packets.request.position) self.request_.publish(request) diff --git a/experimental/Python/src/Intersection/Carla/RSU.lf b/experimental/Python/src/Intersection/Carla/RSU.lf new file mode 100644 index 0000000000..d206798a54 --- /dev/null +++ b/experimental/Python/src/Intersection/Carla/RSU.lf @@ -0,0 +1,49 @@ +target Python { + files: [ + "ROS/carla_intersection/src/rsu.py", + "ROS/carla_intersection/src/utils.py" + ] +}; + +preamble {= + from rsu import RSU + from utils import LFClock, dotdict, make_coordinate +=} + +reactor RSU( + num_entries(0), + intersection_width(0), + nominal_speed_in_intersection(0.0), + intersection_position(0, 0, 0) +) { + input[num_entries] request_; + output[num_entries] grant_; + + preamble {= + class Logger: + def info(self, *args): + print(f"rsu: ", args) + =} + + reaction(startup) {= + self.rsu = RSU(intersection_width = self.intersection_width, + nominal_speed_in_intersection = self.nominal_speed_in_intersection, + intersection_position = make_coordinate(self.intersection_position), + clock = LFClock(), + logger = self.Logger()) + =} + + reaction(request_) -> grant_ {= + for i in range(self.num_entries): + if not request_[i].is_present(): + continue + request = request_[i].value + pub_packets = self.rsu.receive_request(request) + if pub_packets.grant != None: + grant = dotdict() + grant.intersection_position = pub_packets.grant.intersection_position + grant.target_speed = pub_packets.grant.target_speed + grant.arrival_time = pub_packets.grant.arrival_time + grant_[i].set(grant) + =} +} \ No newline at end of file diff --git a/experimental/Python/src/Intersection/Carla/Vehicle.lf b/experimental/Python/src/Intersection/Carla/Vehicle.lf new file mode 100644 index 0000000000..092439a41e --- /dev/null +++ b/experimental/Python/src/Intersection/Carla/Vehicle.lf @@ -0,0 +1,65 @@ +target Python { + files: [ + "ROS/carla_intersection/src/vehicle.py", + "ROS/carla_intersection/src/utils.py" + ] +}; + +preamble {= + from vehicle import Vehicle + from utils import LFClock, dotdict, make_coordinate +=} + +reactor Vehicle( + bank_index(0), + initial_velocities({=None=}), + initial_positions({=None=}) +) { + input velocity_; + input position_; + input grant_; + output control_; + output request_; + state vehicle; + + preamble {= + class Logger: + def __init__(self, vehicle_id): + self.vehicle_id = vehicle_id + + def info(self, *args): + print(f"vehicle_{self.vehicle_id}: ", args) + =} + + reaction(startup) {= + self.vehicle = Vehicle(vehicle_id = self.bank_index, + initial_position = make_coordinate(self.initial_positions[self.bank_index]), + initial_velocity = make_coordinate(self.initial_velocities[self.bank_index]), + clock = LFClock(), + logger = self.Logger(self.bank_index)) + =} + + reaction(position_) {= + self.vehicle.set_position(position_.value) + =} + + reaction(velocity_) -> control_, request_ {= + new_velocity = velocity_.value + pub_packets = self.vehicle.receive_velocity_from_simulator(new_velocity) + if pub_packets.cmd != None: + cmd = dotdict() + cmd.throttle = pub_packets.cmd.throttle + cmd.brake = pub_packets.cmd.brake + control_.set(cmd) + if pub_packets.request != None: + request = dotdict() + request.speed = pub_packets.request.speed + request.position = pub_packets.request.position + request_.set(request) + =} + + reaction(grant_) {= + grant = grant_.value + self.vehicle.grant(grant.arrival_time, grant.intersection_position) + =} +} \ No newline at end of file From 892bd2157ced03968695fb28d03036a7a9e29dbb Mon Sep 17 00:00:00 2001 From: Hou Seng Wong Date: Fri, 25 Feb 2022 11:47:42 -0800 Subject: [PATCH 15/22] fix path issues for ROS --- .../Carla/ROS/carla_intersection/src/carla_sim.py | 7 ++++++- .../Intersection/Carla/ROS/carla_intersection/src/rsu.py | 6 ++++++ .../Intersection/Carla/ROS/carla_intersection/src/utils.py | 7 +++++++ .../Carla/ROS/carla_intersection/src/vehicle.py | 7 +++++++ 4 files changed, 26 insertions(+), 1 deletion(-) diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py index 41c9c8df8a..75f7a0a611 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py @@ -1,7 +1,12 @@ +# Set up sys.path such that imports work across LF and ROS +import sys +from os import path +if path.dirname(__file__) not in sys.path: + sys.path.insert(0, path.dirname(__file__)) + # Other libraries import glob import os -import sys try: import queue except ImportError: diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/rsu.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/rsu.py index ae9272b42a..e85f5a5eca 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/rsu.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/rsu.py @@ -1,3 +1,9 @@ +# Set up sys.path such that imports work across LF and ROS +import sys +from os import path +if path.dirname(__file__) not in sys.path: + sys.path.insert(0, path.dirname(__file__)) + # Other libraries from utils import distance, dotdict diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py index a1957e3966..c272654047 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py @@ -1,3 +1,10 @@ +# Set up sys.path such that imports work across LF and ROS +import sys +from os import path +if path.dirname(__file__) not in sys.path: + sys.path.insert(0, path.dirname(__file__)) + +# Other libraries from math import sin, cos, sqrt, atan2, radians from constants import BILLION diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle.py index 7c2d1778e7..5488b1b23d 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle.py @@ -1,3 +1,10 @@ +# Set up sys.path such that imports work across LF and ROS +import sys +from os import path +if path.dirname(__file__) not in sys.path: + sys.path.insert(0, path.dirname(__file__)) + +# Other libraries from utils import make_speed, dotdict, distance, Coordinate from constants import BILLION, GOAL_REACHED_THRESHOLD, GOAL_REACHED_THRESHOLD_TIME, SPEED_LIMIT From 95abbcb92faa27673ad04f604ba4686145fde80e Mon Sep 17 00:00:00 2001 From: Hou Seng Wong Date: Fri, 25 Feb 2022 13:01:11 -0800 Subject: [PATCH 16/22] use list for spawn_points --- .../launch/intersection_demo.launch.py | 2 +- .../src/launch_parameters.py | 19 +++++++++++-------- 2 files changed, 12 insertions(+), 9 deletions(-) diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/launch/intersection_demo.launch.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/launch/intersection_demo.launch.py index 2cbfa9d6a9..6845a36579 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/launch/intersection_demo.launch.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/launch/intersection_demo.launch.py @@ -44,7 +44,7 @@ def generate_launch_description(): parameters=[ {"vehicle_id": i}, {"initial_velocity": INITIAL_VELOCITIES[i]}, - {"spawn_point": [SPAWN_POINTS[i]["x"], SPAWN_POINTS[i]["y"], SPAWN_POINTS[i]["z"], SPAWN_POINTS[i]["yaw"]]} + {"spawn_point": SPAWN_POINTS[i]} ] ) ) diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/launch_parameters.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/launch_parameters.py index d0bfa973b6..1c30742721 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/launch_parameters.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/launch_parameters.py @@ -1,18 +1,21 @@ SPAWN_POINTS = [ - {"x": -122.0, "y": 39.6, "z": 0.3, "yaw": -90.0}, - {"x": -177.77, "y": 6.48, "z": 0.3, "yaw": 0.0}, - {"x": -132.77, "y": -40, "z": 0.3, "yaw": 90.0}, - {"x": -80.77, "y": -4.5, "z": 0.3, "yaw": 180.0} + # x y z yaw + [-122.0, 39.6, 0.3, -90.0], + [-177.77, 6.48, 0.3, 0.0], + [-132.77, -40, 0.3, 90.0], + [-80.77, -4.5, 0.3, 180.0] ] INITIAL_POSITIONS = [ - [0.000038,-0.000674,2.794825], # /|\ - [-0.000501,-0.001084,2.794891], # -> - [-0.000060,-0.001510,2.794854], # \|/ - [0.000367,-0.001185,2.794846] # <- + # x y z + [0.000038, -0.000674, 2.794825], # /|\ + [-0.000501, -0.001084, 2.794891], # -> + [-0.000060, -0.001510, 2.794854], # \|/ + [0.000367, -0.001185, 2.794846] # <- ] INITIAL_VELOCITIES = [ + # x y z [ 0.0, -8.0, 0.0], [ 8.0, 0.0, 0.0], [ 0.0, 8.0, 0.0], From c966eef53450da0ae532e3060c4581c49af40653 Mon Sep 17 00:00:00 2001 From: Hou Seng Wong Date: Fri, 25 Feb 2022 13:08:01 -0800 Subject: [PATCH 17/22] intersection runs but crashes shortly --- .../Python/src/Intersection/Carla/CarlaIntersection.lf | 2 +- .../Carla/ROS/carla_intersection/src/utils.py | 3 --- experimental/Python/src/Intersection/Carla/RSU.lf | 10 +++++++--- experimental/Python/src/Intersection/Carla/Vehicle.lf | 9 +++++++-- 4 files changed, 15 insertions(+), 9 deletions(-) diff --git a/experimental/Python/src/Intersection/Carla/CarlaIntersection.lf b/experimental/Python/src/Intersection/Carla/CarlaIntersection.lf index cc4c502e1f..2bb3296e24 100644 --- a/experimental/Python/src/Intersection/Carla/CarlaIntersection.lf +++ b/experimental/Python/src/Intersection/Carla/CarlaIntersection.lf @@ -21,7 +21,7 @@ target Python { timeout: 40 sec, files: [ "ROS/carla_intersection/src/launch_parameters.py", - + "ROS/carla_intersection/src/constants.py" ] // fast: true // You can enable fast to get a much faster simulation // logging: DEBUG diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py index c272654047..cd89ff4955 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py @@ -24,9 +24,6 @@ def __init__(self, x, y, z): class GenericClock: def get_current_time_in_ns(self): assert False, "subclass must override this method" -class LFClock(GenericClock): - def get_current_time_in_ns(self): - return get_logical_time() class ROSClock(GenericClock): def __init__(self, clock): diff --git a/experimental/Python/src/Intersection/Carla/RSU.lf b/experimental/Python/src/Intersection/Carla/RSU.lf index d206798a54..5594cc7192 100644 --- a/experimental/Python/src/Intersection/Carla/RSU.lf +++ b/experimental/Python/src/Intersection/Carla/RSU.lf @@ -7,7 +7,7 @@ target Python { preamble {= from rsu import RSU - from utils import LFClock, dotdict, make_coordinate + from utils import GenericClock, dotdict, make_coordinate =} reactor RSU( @@ -23,19 +23,23 @@ reactor RSU( class Logger: def info(self, *args): print(f"rsu: ", args) + + class LFClock(GenericClock): + def get_current_time_in_ns(self): + return get_logical_time() =} reaction(startup) {= self.rsu = RSU(intersection_width = self.intersection_width, nominal_speed_in_intersection = self.nominal_speed_in_intersection, intersection_position = make_coordinate(self.intersection_position), - clock = LFClock(), + clock = self.LFClock(), logger = self.Logger()) =} reaction(request_) -> grant_ {= for i in range(self.num_entries): - if not request_[i].is_present(): + if not request_[i].is_present: continue request = request_[i].value pub_packets = self.rsu.receive_request(request) diff --git a/experimental/Python/src/Intersection/Carla/Vehicle.lf b/experimental/Python/src/Intersection/Carla/Vehicle.lf index 092439a41e..5e168ab220 100644 --- a/experimental/Python/src/Intersection/Carla/Vehicle.lf +++ b/experimental/Python/src/Intersection/Carla/Vehicle.lf @@ -7,7 +7,7 @@ target Python { preamble {= from vehicle import Vehicle - from utils import LFClock, dotdict, make_coordinate + from utils import GenericClock, dotdict, make_coordinate =} reactor Vehicle( @@ -29,13 +29,17 @@ reactor Vehicle( def info(self, *args): print(f"vehicle_{self.vehicle_id}: ", args) + + class LFClock(GenericClock): + def get_current_time_in_ns(self): + return get_logical_time() =} reaction(startup) {= self.vehicle = Vehicle(vehicle_id = self.bank_index, initial_position = make_coordinate(self.initial_positions[self.bank_index]), initial_velocity = make_coordinate(self.initial_velocities[self.bank_index]), - clock = LFClock(), + clock = self.LFClock(), logger = self.Logger(self.bank_index)) =} @@ -53,6 +57,7 @@ reactor Vehicle( control_.set(cmd) if pub_packets.request != None: request = dotdict() + request.requestor_id = self.bank_index request.speed = pub_packets.request.speed request.position = pub_packets.request.position request_.set(request) From 97853396290f86e950ffebfa60640251607697b9 Mon Sep 17 00:00:00 2001 From: Hou Seng Wong Date: Fri, 25 Feb 2022 13:25:54 -0800 Subject: [PATCH 18/22] add logical time to LF log messages --- experimental/Python/src/Intersection/Carla/CarlaSim.lf | 2 +- experimental/Python/src/Intersection/Carla/RSU.lf | 2 +- experimental/Python/src/Intersection/Carla/Vehicle.lf | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/experimental/Python/src/Intersection/Carla/CarlaSim.lf b/experimental/Python/src/Intersection/Carla/CarlaSim.lf index 5acb3c46d6..eb4d6006a3 100644 --- a/experimental/Python/src/Intersection/Carla/CarlaSim.lf +++ b/experimental/Python/src/Intersection/Carla/CarlaSim.lf @@ -30,7 +30,7 @@ reactor CarlaSim( self.vehicle_id = vehicle_id def info(self, *args): - print(f"carla_sim_{self.vehicle_id}: ", args) + print(f"{get_logical_time()} - carla_sim_{self.vehicle_id}: ", args) =} reaction(startup) -> world_is_ready {= diff --git a/experimental/Python/src/Intersection/Carla/RSU.lf b/experimental/Python/src/Intersection/Carla/RSU.lf index 5594cc7192..96194aea48 100644 --- a/experimental/Python/src/Intersection/Carla/RSU.lf +++ b/experimental/Python/src/Intersection/Carla/RSU.lf @@ -22,7 +22,7 @@ reactor RSU( preamble {= class Logger: def info(self, *args): - print(f"rsu: ", args) + print(f"{get_logical_time()} - rsu: ", args) class LFClock(GenericClock): def get_current_time_in_ns(self): diff --git a/experimental/Python/src/Intersection/Carla/Vehicle.lf b/experimental/Python/src/Intersection/Carla/Vehicle.lf index 5e168ab220..fcde445ee6 100644 --- a/experimental/Python/src/Intersection/Carla/Vehicle.lf +++ b/experimental/Python/src/Intersection/Carla/Vehicle.lf @@ -28,7 +28,7 @@ reactor Vehicle( self.vehicle_id = vehicle_id def info(self, *args): - print(f"vehicle_{self.vehicle_id}: ", args) + print(f"{get_logical_time()} - vehicle_{self.vehicle_id}: ", args) class LFClock(GenericClock): def get_current_time_in_ns(self): From 44a21b1d36412967848f0911543ee48ba228d111 Mon Sep 17 00:00:00 2001 From: Hou Seng Wong Date: Fri, 25 Feb 2022 14:29:06 -0800 Subject: [PATCH 19/22] clean up code --- .../Carla/ROS/carla_intersection/src/carla_sim.py | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py index 75f7a0a611..ffb97f649b 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py @@ -139,9 +139,6 @@ def initialize_vehicle(self): # Set the initial velocity target_speed = self.initial_velocity - for i in range(1): - self.vehicle.set_target_velocity(carla.Vector3D(x=target_speed.x, y=target_speed.y, z=target_speed.z)) - # self.vehicle.apply_control(carla.VehicleControl(throttle=1.0, steer=0.0)) - # self.world.tick() + self.vehicle.set_target_velocity(carla.Vector3D(x=target_speed.x, y=target_speed.y, z=target_speed.z)) self.logger.info("Spawned vehicle") \ No newline at end of file From 1e470e89d005f650e2cb081e1997e441270873af Mon Sep 17 00:00:00 2001 From: Hou Seng Wong Date: Fri, 25 Feb 2022 14:39:15 -0800 Subject: [PATCH 20/22] add description about the CARLA bug --- experimental/Python/src/Intersection/Carla/CarlaSim.lf | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/experimental/Python/src/Intersection/Carla/CarlaSim.lf b/experimental/Python/src/Intersection/Carla/CarlaSim.lf index eb4d6006a3..2e781a7f33 100644 --- a/experimental/Python/src/Intersection/Carla/CarlaSim.lf +++ b/experimental/Python/src/Intersection/Carla/CarlaSim.lf @@ -10,6 +10,11 @@ preamble {= from carla_sim import CarlaSim =} +# TODO: Figure out why this warning persists | +# v +# WARNING: World::ApplySettings: After 30 attemps, the settings were not correctly set. Please check that everything is consistent. +# +# as well as why the velocity of the vehicle shoots through the roof... which causes CARLA to crash. reactor CarlaSim( interval(16 msec), bank_index(0), @@ -34,6 +39,7 @@ reactor CarlaSim( =} reaction(startup) -> world_is_ready {= + print("initial velocity: ", self.initial_velocities[self.bank_index]) self.carla_sim = CarlaSim(interval=self.interval, vehicle_type=self.vehicle_type, initial_velocity=make_coordinate(self.initial_velocities[self.bank_index]), @@ -52,6 +58,7 @@ reactor CarlaSim( reaction(timer_) -> velocity_, position_ {= self.carla_sim.tick() + print("setting velocity to ", self.carla_sim.get_vehicle_velocity()) velocity_.set(self.carla_sim.get_vehicle_velocity()) p = self.carla_sim.get_vehicle_position() coordinate = make_coordinate([p.latitude, p.longitude, p.altitude]) From dcf985647681db7dbc6e8437af3d667ff0ac7a3c Mon Sep 17 00:00:00 2001 From: Hou Seng Wong Date: Mon, 28 Feb 2022 16:02:04 -0800 Subject: [PATCH 21/22] fix LF version crash bug --- experimental/Python/src/Intersection/Carla/CarlaSim.lf | 2 +- .../Carla/ROS/carla_intersection/src/carla_sim.py | 4 ++-- .../Carla/ROS/carla_intersection/src/carla_sim_node.py | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/experimental/Python/src/Intersection/Carla/CarlaSim.lf b/experimental/Python/src/Intersection/Carla/CarlaSim.lf index 2e781a7f33..88dd413142 100644 --- a/experimental/Python/src/Intersection/Carla/CarlaSim.lf +++ b/experimental/Python/src/Intersection/Carla/CarlaSim.lf @@ -47,7 +47,7 @@ reactor CarlaSim( logger=self.Logger(self.bank_index)) self.carla_sim.connect_to_carla() if self.bank_index == 0: - self.carla_sim.initialize_world() + self.carla_sim.initialize_world(self.interval / SEC(1)) world_is_ready.schedule(MSEC(5)) =} diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py index ffb97f649b..fcc68c7478 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py @@ -47,10 +47,10 @@ def connect_to_carla(self): self.client = carla.Client("localhost", 2000) self.client.set_timeout(10.0) # seconds - def initialize_world(self): + def initialize_world(self, fixed_delta_seconds): self.world = self.client.load_world("Town05") settings = self.world.get_settings() - settings.fixed_delta_seconds = self.interval / 1000.0 + settings.fixed_delta_seconds = fixed_delta_seconds settings.substepping = True settings.max_substep_delta_time = settings.fixed_delta_seconds / 10 settings.max_substeps = 10 diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim_node.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim_node.py index d7c602edbe..5cb1714529 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim_node.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim_node.py @@ -43,7 +43,7 @@ def __init__(self): self.carla_sim.connect_to_carla() if self.vehicle_id == 0: self.world_is_ready_ = self.create_publisher(Bool, "world_is_ready", 10) - self.carla_sim.initialize_world() + self.carla_sim.initialize_world(self.interval / 1000.0) self.world_is_ready_.publish(Bool()) self.world_is_ready_callback() else: From 4eca2af5914ce22fa19fcc90d5a7fa24a52bcca5 Mon Sep 17 00:00:00 2001 From: Hou Seng Wong Date: Mon, 28 Feb 2022 16:18:02 -0800 Subject: [PATCH 22/22] change roll to make camera angle more intuitive --- .../Intersection/Carla/ROS/carla_intersection/src/carla_sim.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py index fcc68c7478..b5dd10e347 100644 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py @@ -70,7 +70,7 @@ def set_weather(self): def set_spectator_camera(self): # Set the spectator (camera) position transform = carla.Transform(carla.Location(x=-126.163864, y=3, z=67), \ - carla.Rotation(pitch=-90, yaw=-180, roll=0)) + carla.Rotation(pitch=-90, yaw=-180, roll=180)) self.world.get_spectator().set_transform(transform) def get_vehicle_velocity(self):