Skip to content

Commit

Permalink
Merge pull request #982 from lf-lang/carla-intersection-add-delay
Browse files Browse the repository at this point in the history 
Carla intersection refactoring
  • Loading branch information
@housengw
housengw authored Mar 5, 2022
2 parents 59c3135 + 2d9c4bf commit f4c638f
Show file tree
Hide file tree
Showing 21 changed files with 1,035 additions and 920 deletions.
333 changes: 46 additions & 287 deletions experimental/Python/src/Intersection/Carla/CarlaIntersection.lf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Loading

0 comments on commit f4c638f

Please sign in to comment.