diff --git a/experimental/Python/src/Intersection/Carla/CarlaIntersection.lf b/experimental/Python/src/Intersection/Carla/CarlaIntersection.lf index e1d242467e..2bb3296e24 100644 --- a/experimental/Python/src/Intersection/Carla/CarlaIntersection.lf +++ b/experimental/Python/src/Intersection/Carla/CarlaIntersection.lf @@ -19,314 +19,73 @@ */ 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 }; -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} - =}) -) { +reactor Relay(duration_in_seconds(0)) { preamble {= - ## need vehicle status and gps - ## send target velocity - =} - - input command; - - // Set up the ticker - ticker = new Ticker(vehicle_bank_index = vehicle_bank_index); - - output status; - output position; - - state world; - state vehicle; - state gps; - state gps_queue; - - reaction(ticker.world) {= - print("Got the world") - self.world = ticker.world.value - - 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") - + from time import sleep =} - reaction(ticker.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), - vehicle_bank_index(0) - ){ - output world; - output tick; - state client; - state world; - reaction(startup) -> world {= - self.client=carla.Client("localhost", 2000) - self.client.set_timeout(10.0) # seconds - - # 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") - - 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)) + input i; + output o; + logical action t; - self.world.get_spectator().set_transform(transform) - - # Send the world to Carla interface reactors - world.set(self.world) - =} - 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(i) -> t {= + t.schedule(self.duration_in_seconds, i.value) =} - 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() + reaction(t) -> o {= + o.set(t.value) =} } -reactor ego_vehicle ( - bank_index(0), - positions({=[]=}), - initial_speeds({=[]=}) - ) { - input grant; - 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.request -> request; - grant -> vehicle.grant; - - vehicle.control -> carla.command; - carla.status -> vehicle.vehicle_stat; - carla.position -> vehicle.vehicle_pos; -} - 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 ); + + carla.velocity_ -> vehicle.velocity_; + carla.position_ -> vehicle.position_; + vehicle.control_ -> carla.command_; + + vehicle.request_ -> rsu.request_; + rsu.grant_ -> vehicle.grant_; + - - 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; } diff --git a/experimental/Python/src/Intersection/Carla/CarlaSim.lf b/experimental/Python/src/Intersection/Carla/CarlaSim.lf new file mode 100644 index 0000000000..88dd413142 --- /dev/null +++ b/experimental/Python/src/Intersection/Carla/CarlaSim.lf @@ -0,0 +1,72 @@ +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 +=} + +# 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), + 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"{get_logical_time()} - carla_sim_{self.vehicle_id}: ", args) + =} + + 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]), + 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(self.interval / SEC(1)) + 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() + 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]) + 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/carla_intersection/carla_sim.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/carla_intersection/carla_sim.py deleted file mode 100644 index 9d3d1e203b..0000000000 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/carla_intersection/carla_sim.py +++ /dev/null @@ -1,211 +0,0 @@ -import rclpy -from rclpy.node import Node -from carla_intersection_msgs.msg import VehicleCommand -from std_msgs.msg import Bool -from geometry_msgs.msg import Vector3 -import time -import glob -import os -import sys -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 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]) - 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: - 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) - - self.initialize_carla() - - # 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(): - return - self.vehicle.apply_control( \ - carla.VehicleControl( \ - throttle=command.throttle, \ - brake=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.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.get_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) - - 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.get_spawn_point() - # Spawn the vehicle - vehicle_bp = blueprint_library.find(self.get_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.get_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() - - rclpy.spin(ego_vehicle) - - # Destroy the node explicitly - ego_vehicle.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/carla_intersection/rsu.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/carla_intersection/rsu.py deleted file mode 100644 index db49ad62c7..0000000000 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/carla_intersection/rsu.py +++ /dev/null @@ -1,126 +0,0 @@ -import rclpy -from rclpy.node import Node - -from math import sin, cos, sqrt, atan2, radians, pi - -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) - -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]) - - 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) - - 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: - # 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.get_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)) - - 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.get_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.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)) - 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() - - rclpy.spin(rsu) - - # Destroy the node explicitly - rsu.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/carla_intersection/vehicle.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/carla_intersection/vehicle.py deleted file mode 100644 index 898d6cb7cf..0000000000 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/carla_intersection/vehicle.py +++ /dev/null @@ -1,233 +0,0 @@ -import rclpy -from rclpy.node import Node - -import random -from math import sin, cos, sqrt, atan2, radians, pi - -from std_msgs.msg import String - -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): - 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]) - - self.current_pos = self.get_initial_position() - self.granted_time_to_enter = 0 - self.intersection_position = None - self.goal_reached = False - self.velocity = 0.0 - 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.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) - - def update_velocity(self, vehicle_stat): - velocity_3d = Vector3(x=vehicle_stat.x, y=vehicle_stat.y, z=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 - # Record the speed - 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: - 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.get_vehicle_id() - request.speed = self.velocity - request.position = Vector3(x=self.current_pos.x, y=self.current_pos.y, z=self.current_pos.z) - self.request_.publish(request) - self.asking_for_grant = True - - # Stop the vehicle - cmd = VehicleCommand() - cmd.throttle = 0.0 - cmd.brake = 1.0 - 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.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)) - - 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.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)) - - 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.get_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.get_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)) - - - def grant_callback(self, grant): - if grant.requestor_id != self.get_vehicle_id(): - return - self.get_logger().info("Vehicle {} Granted access".format(self.get_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.asking_for_grant = False - - -def main(args=None): - rclpy.init(args=args) - - ego_vehicle = Vehicle() - - rclpy.spin(ego_vehicle) - - # Destroy the node explicitly - ego_vehicle.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() \ 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 51ca1ee74f..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 @@ -2,55 +2,22 @@ import launch.actions import launch.substitutions from launch_ros.actions import Node -from geometry_msgs.msg import Vector3 - +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, \ + INTERSECTION_WIDTH, NOMINAL_SPEED_IN_INTERSECTION, INTERSECTION_POSITION 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_speeds = [ - [ 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( package='carla_intersection', - executable='rsu', + 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} ] ) ) @@ -62,22 +29,22 @@ def generate_launch_description(): nodes.append( Node( package='carla_intersection', - executable='vehicle', + executable='vehicle_node', parameters=[ {"vehicle_id": i}, - {"initial_speed": initial_speeds[i]}, - {"initial_position": initial_positions[i]} + {"initial_velocity": INITIAL_VELOCITIES[i]}, + {"initial_position": INITIAL_POSITIONS[i]} ] ) ) nodes.append( Node( package='carla_intersection', - executable='carla_sim', + executable='carla_sim_node', parameters=[ {"vehicle_id": i}, - {"initial_speed": initial_speeds[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]} ] ) ) 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/src/carla_sim.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py new file mode 100644 index 0000000000..b5dd10e347 --- /dev/null +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py @@ -0,0 +1,144 @@ +# 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 +try: + import queue +except ImportError: + import Queue as queue + +# Constants +from 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_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_velocity = initial_velocity + self.spawn_point = spawn_point + self.logger = logger + + def connect_to_carla(self): + # initialize Carla + self.client = carla.Client("localhost", 2000) + self.client.set_timeout(10.0) # seconds + + def initialize_world(self, fixed_delta_seconds): + self.world = self.client.load_world("Town05") + settings = self.world.get_settings() + settings.fixed_delta_seconds = fixed_delta_seconds + 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=180)) + 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: float, brake: float): + 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 velocity + target_speed = self.initial_velocity + 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 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 new file mode 100644 index 0000000000..5cb1714529 --- /dev/null +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim_node.py @@ -0,0 +1,86 @@ +# 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 + +# Other libraries +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 +class CarlaSimNode(Node): + def __init__(self): + super().__init__("carla_sim_node") + + # Parameters declaration + self.declare_parameter('interval', 16) # msec + self.declare_parameter('vehicle_id', 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]) + + # 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_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 + + # pubsub for input and output ports + 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, + initial_velocity=self.initial_velocity, spawn_point=self.spawn_point, + logger=self.get_logger()) + 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.interval / 1000.0) + 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) + + # 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): + self.carla_sim.apply_control(command.throttle, command.brake) + + def timer_callback(self): + if not self.world_is_ready: + return + self.carla_sim.tick() + 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)) + + def world_is_ready_callback(self, _=None): + self.carla_sim.get_world() + self.world_is_ready = True + self.carla_sim.initialize_vehicle() + + +def main(args=None): + rclpy.init(args=args) + + ego_vehicle = CarlaSimNode() + + rclpy.spin(ego_vehicle) + + # Destroy the node explicitly + ego_vehicle.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() 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/src/launch_parameters.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/launch_parameters.py new file mode 100644 index 0000000000..1c30742721 --- /dev/null +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/launch_parameters.py @@ -0,0 +1,29 @@ +SPAWN_POINTS = [ + # 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 = [ + # 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], + [-8.0, 0.0, 0.0] +] + +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 new file mode 100644 index 0000000000..e85f5a5eca --- /dev/null +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/rsu.py @@ -0,0 +1,92 @@ +# 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 + +# Constants +from 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) -> 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: + # 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 new file mode 100644 index 0000000000..b83efbfe24 --- /dev/null +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/rsu_node.py @@ -0,0 +1,61 @@ +# ROS2 libraries +import rclpy +from rclpy.node import Node + +# ROS2 messages +from carla_intersection_msgs.msg import Request, Grant + +# Other libraries +from src.utils import make_coordinate, ROSClock +from src.ros_utils import make_Vector3 +from src.rsu import RSU + +class RSUNode(Node): + def __init__(self): + super().__init__("rsu") + + # 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) + + # 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): + 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) + + +def main(args=None): + rclpy.init(args=args) + + rsu = RSUNode() + + rclpy.spin(rsu) + + # Destroy the node explicitly + rsu.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file 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..cd89ff4955 --- /dev/null +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py @@ -0,0 +1,69 @@ +# 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 + +class dotdict(dict): + """dot.notation access to dictionary attributes""" + __getattr__ = dict.get + __setattr__ = dict.__setitem__ + __delattr__ = dict.__delitem__ + + +class Coordinate: + def __init__(self, x, y, z): + self.x = x + self.y = y + self.z = z + +class GenericClock: + def get_current_time_in_ns(self): + assert False, "subclass must override this method" + +class ROSClock(GenericClock): + 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 + 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 Coordinate(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_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..5488b1b23d --- /dev/null +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle.py @@ -0,0 +1,186 @@ +# 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 + +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: 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: Coordinate) -> None: + self.position = new_position + + def get_velocity(self) -> Coordinate: + ''' + Returns the current velocity of the vehicle as a coordinate. + ''' + return self.velocity + + def get_position(self) -> Coordinate: + ''' + Returns the current position of the vehicle as a coordinate. + ''' + return self.position + + def get_speed(self) -> float: + ''' + Returns the current speed of the vehicle. + ''' + return self.speed + + 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( + arrival_time + ) + ) + self.granted_time_to_enter = arrival_time + self.intersection_position = intersection_position + + 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 + - speed : float + - position : Coordinate + - cmd + - throttle : float + - brake : float + ''' + 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.speed = self.get_speed() + pub_packets.request.position = 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 new file mode 100644 index 0000000000..1ae94d2ec7 --- /dev/null +++ b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle_node.py @@ -0,0 +1,81 @@ +# ROS2 libraries +import rclpy +from rclpy.node import Node + +# ROS2 messages +from carla_intersection_msgs.msg import Request, Grant, VehicleCommand +from geometry_msgs.msg import Vector3 + +# Other libraries +from src.utils import make_coordinate, ROSClock +from src.ros_utils import make_Vector3 +from src.vehicle import Vehicle + +# Constants +from src.constants import BILLION + +class VehicleNode(Node): + def __init__(self): + super().__init__(f"vehicle") + + # 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 = 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.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 position_callback(self, new_position): + self.vehicle.set_position(new_position) + + 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 + cmd.brake = pub_packets.cmd.brake + self.control_.publish(cmd) + if pub_packets.request != None and not self.asking_for_grant: + request = Request() + request.requestor_id = self.vehicle_id + request.speed = pub_packets.request.speed + request.position = make_Vector3(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.vehicle.grant(grant.arrival_time, grant.intersection_position) + self.asking_for_grant = False + +def main(args=None): + rclpy.init(args=args) + + ego_vehicle = VehicleNode() + + rclpy.spin(ego_vehicle) + + # Destroy the node explicitly + ego_vehicle.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/experimental/Python/src/Intersection/Carla/RSU.lf b/experimental/Python/src/Intersection/Carla/RSU.lf new file mode 100644 index 0000000000..96194aea48 --- /dev/null +++ b/experimental/Python/src/Intersection/Carla/RSU.lf @@ -0,0 +1,53 @@ +target Python { + files: [ + "ROS/carla_intersection/src/rsu.py", + "ROS/carla_intersection/src/utils.py" + ] +}; + +preamble {= + from rsu import RSU + from utils import GenericClock, 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"{get_logical_time()} - 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 = self.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..fcde445ee6 --- /dev/null +++ b/experimental/Python/src/Intersection/Carla/Vehicle.lf @@ -0,0 +1,70 @@ +target Python { + files: [ + "ROS/carla_intersection/src/vehicle.py", + "ROS/carla_intersection/src/utils.py" + ] +}; + +preamble {= + from vehicle import Vehicle + from utils import GenericClock, 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"{get_logical_time()} - 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 = self.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.requestor_id = self.bank_index + 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 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() ),