Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
53 changes: 28 additions & 25 deletions examples/modd/followCarBehaviorMODD.scenic
Original file line number Diff line number Diff line change
Expand Up @@ -11,27 +11,30 @@ import scenic.domains.driving.model as _model
from scenic.domains.driving.roads import ManeuverType
from scenic.domains.driving.behaviors import concatenateCenterlines

LEADER_SPEED = TimeSeries(VerifaiRange(6,8))
target_speed_noise = TimeSeries(VerifaiRange(-1,1))
EGO_BRAKING_THRESHOLD = 6

def run_MODD(car, monitor_model, obstacle, leader, monitor_type="sklearn"):
current_lane = car.lane
nearby_intersection = current_lane.maneuvers[0].intersection
distIntersection = distance from car to nearby_intersection
distObstacle = distance from car to obstacle
visibleObstacle = int(car can see obstacle)
visibleLeader = int(car can see leader)

input_features = np.concatenate((car.weather, np.array([car.r,car.g,car.b, distIntersection, distObstacle, visibleObstacle, visibleLeader])))
input_features = np.expand_dims(input_features, axis=0)
if monitor_type == "sklearn":
car.isSafe = monitor_model.predict(input_features)[0]
if car._lane is None:
car.isSafe = 0
else:
x = torch.Tensor(input_features).unsqueeze(0).cuda()
car.isSafe = (torch.nn.Sigmoid()(monitor_model(x)).cpu().detach().numpy()[0][0] > 0.65).astype(int)
current_lane = car.lane
nearby_intersection = current_lane.maneuvers[0].intersection
distIntersection = distance from car to nearby_intersection
distObstacle = distance from car to obstacle
visibleObstacle = int(car can see obstacle)
visibleLeader = int(car can see leader)

input_features = np.concatenate((car.weather, np.array([car.r,car.g,car.b, distIntersection, distObstacle, visibleObstacle, visibleLeader])))
input_features = np.expand_dims(input_features, axis=0)
if monitor_type == "sklearn":
car.isSafe = monitor_model.predict(input_features)[0]
else:
x = torch.Tensor(input_features).unsqueeze(0).cuda()
car.isSafe = (torch.nn.Sigmoid()(monitor_model(x)).cpu().detach().numpy()[0][0] > 0.65).astype(int)


behavior FollowCarBehaviorMODD(target_speed = 10, laneToFollow=None, is_oppositeTraffic=False, leaderCar=None, addNoise=False, monitor_model=None):
behavior FollowCarBehaviorMODD(target_speed = 10, laneToFollow=None, is_oppositeTraffic=False, leaderCar=None, addNoise=False, monitor_model=None, obstacleCar=None):
"""
This implementation is heavily inspired by the FollowLaneBehavior.
The main difference is that this behavior is used to follow a leaderCar with the addition of an ODD monitor.
Expand All @@ -44,6 +47,8 @@ behavior FollowCarBehaviorMODD(target_speed = 10, laneToFollow=None, is_opposite
:param addNoise: When true, random noise is added to the steering and the speed.
:param monitor_model: ODD monitor to be used by the ego car. If existing, it is used to identify check if it is safe enough for the system to stop using the safe controller.
"""
noise = target_speed_noise.getSample()


past_steer_angle = 0
past_speed = 0 # making an assumption here that the agent starts from zero speed
Expand All @@ -60,9 +65,7 @@ behavior FollowCarBehaviorMODD(target_speed = 10, laneToFollow=None, is_opposite
intersection_passed = False
entering_intersection = False # assumption that the agent is not instantiated within an intersection
end_lane = None
if isinstance(target_speed, TimeSeriesParameter):
target_speed = target_speed.getSample()
original_target_speed = target_speed
original_target_speed = target_speed + noise
TARGET_SPEED_FOR_TURNING = 3 # KM/H
TRIGGER_DISTANCE_TO_SLOWDOWN = 20 # FOR TURNING AT INTERSECTIONS

Expand All @@ -82,7 +85,8 @@ behavior FollowCarBehaviorMODD(target_speed = 10, laneToFollow=None, is_opposite
steps_running = 0

while True:

if self._lane is None:
continue
if self.speed is not None:
current_speed = self.speed
else:
Expand Down Expand Up @@ -140,7 +144,6 @@ behavior FollowCarBehaviorMODD(target_speed = 10, laneToFollow=None, is_opposite
target_speed = TARGET_SPEED_FOR_TURNING

trajectory = current_centerline
target_speed = target_speed
if isinstance(trajectory, PolylineRegion):
trajectory_centerline = trajectory
else:
Expand Down Expand Up @@ -175,16 +178,16 @@ behavior FollowCarBehaviorMODD(target_speed = 10, laneToFollow=None, is_opposite
if leaderCar is None:
self.steps_speed += 1
if self.steps_speed > 20:
original_target_speed = LEADER_SPEED.getSample()
original_target_speed = target_speed + target_speed_noise.getSample()
self.steps_speed = 0
else:
steps_running += 1

if not leaderCar is None and not monitor_model is None and steps_running > 80:
# Input format: (weather, np.array([r,g,b, distIntersection, distObstacle, visibleObstacle, visibleLeader]))
distIntersection = distance from self to nearby_intersection
distObstacle = distance from self to obstacle
visibleObstacle = int(ego can see obstacle)
distObstacle = distance from self to obstacleCar
visibleObstacle = int(ego can see obstacleCar)
visibleLeader = int(ego can see leader)

input_features = np.concatenate((self.weather, np.array([self.r,self.g,self.b, distIntersection, distObstacle, visibleObstacle, visibleLeader])))
Expand Down Expand Up @@ -238,13 +241,13 @@ behavior FollowCarBehaviorMODD(target_speed = 10, laneToFollow=None, is_opposite
if leaderCar is None:
self.steps_speed += 1
if self.steps_speed > 20:
original_target_speed = LEADER_SPEED.getSample()
original_target_speed = target_speed + target_speed_noise.getSample()
self.steps_speed = 0

else:
steps_running += 1

if not leaderCar is None and monitor_model and steps_running > 80:
run_MODD(self, monitor_model, obstacle, leader)
run_MODD(self, monitor_model, obstacleCar, leaderCar)


31 changes: 19 additions & 12 deletions examples/modd/followLeader_extracar.scenic
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,7 @@ from followCarBehaviorMODD import (

#CONSTANTS
EGO_MODEL = "vehicle.tesla.model3"
LEADER_SPEED = TimeSeries(VerifaiRange(6,8))
LEADER_SPEED = 7
EGO_SPEED = 3
THROTTLE_ACTION = 0.5
EGO_TO_LEADER = Range(-15, -10)
Expand All @@ -58,21 +58,29 @@ if globalParameters.monitor != "":

behavior EgoBehavior(target_speed = 10, controller_path = None, leaderCar=None, obstacleCar=None, monitor_model=None):
if monitor_model is None:
do ControllerBehavior(target_speed, controller_path, leaderCar)
try:
do ControllerBehavior(target_speed, controller_path, leaderCar)
interrupt when self._lane is None:
take SetBrakeAction(1.0)
else:
try:
do ControllerBehavior(target_speed=target_speed, controller_path=controller_path, leaderCar=leaderCar, obstacleCar=obstacleCar)
interrupt when not self.isSafe:
print("-- Monitor: Not safe! Switching to SafeBehavior.")
take SetBrakeAction(1.0)
do EgoBehavior2(target_speed = target_speed, controller_path = controller_path, leaderCar=leaderCar, obstacleCar=obstacleCar, monitor_model=monitor_model)
interrupt when not self.isSafe or self._lane is None:
if self._lane is None:
take SetBrakeAction(1.0)
else:
print("-- Monitor: Not safe! Switching to SafeBehavior.")
do EgoBehavior2(target_speed = target_speed, controller_path = controller_path, leaderCar=leaderCar, obstacleCar=obstacleCar, monitor_model=monitor_model)

behavior EgoBehavior2(target_speed = 10, controller_path = None, leaderCar=None, obstacleCar=None, monitor_model=None):
try:
do FollowCarBehaviorMODD(target_speed=target_speed, leaderCar=leader, monitor_model=monitor_model)
interrupt when self.isSafe:
print("-- Monitor: Safe! Switching back to CNN Controller.")
do EgoBehavior(target_speed=target_speed, controller_path=controller_path, leaderCar=leaderCar, obstacleCar=obstacleCar, monitor_model=monitor_model)
do FollowCarBehaviorMODD(target_speed=target_speed, leaderCar=leader, monitor_model=monitor_model, obstacleCar=obstacleCar)
interrupt when self.isSafe or self._lane is None:
if self._lane is None:
take SetBrakeAction(1.0)
else:
print("-- Monitor: Safe! Switching back to CNN Controller.")
do EgoBehavior(target_speed=target_speed, controller_path=controller_path, leaderCar=leaderCar, obstacleCar=obstacleCar, monitor_model=monitor_model)



Expand All @@ -83,7 +91,7 @@ behavior ControllerBehavior(target_speed = 10, controller_path = None, leaderCar
past_speed = 0 # making an assumption here that the agent starts from zero speed

original_target_speed = target_speed

current_lane = self.lane
nearby_intersection = current_lane.centerline[-1]

Expand Down Expand Up @@ -138,7 +146,6 @@ behavior ControllerBehavior(target_speed = 10, controller_path = None, leaderCar

speed_error = target_speed - current_speed


# compute throttle : Longitudinal Control
throttle = _lon_controller.run_step(speed_error)

Expand Down
12 changes: 8 additions & 4 deletions examples/modd/modd_learner_main.py
Original file line number Diff line number Diff line change
Expand Up @@ -72,17 +72,21 @@ def labeler(train_data_samples):

if MODEL == "DT":
base_model = DecisionTreeClassifier(max_depth=10)
monitor_type = "sklearn"

if MODEL == "NN":
nn = MLPClassifier(solver='adam', alpha=1e-5,
hidden_layer_sizes=(100,100), max_iter=100, random_state=42, verbose=1)
base_model = Pipeline([('scaler', StandardScaler()), ('nn', nn)])
monitor_type = "sklearn"

if MODEL == "LR":
base_model = LogisticRegression(verbose=1)
monitor_type = "sklearn"

if MODEL == "NN_TORCH":
base_model = MLP()
monitor_type = "torch"

trainer_params = DotMap(
model=base_model,
Expand Down Expand Up @@ -111,6 +115,7 @@ def specification(trace):
scenes_save_dir=os.path.join(os.path.dirname(__file__), "out/scene_timeseries"),
datagen_nomon_save_dir=os.path.join(os.path.dirname(__file__), "out/eval_samples_timeseries_nomonitor"),
save_model_path=trainer_params.save_model_path,
monitor_type=monitor_type,
verbosity=VERBOSITY,
)

Expand Down Expand Up @@ -142,7 +147,9 @@ def spec(simulation):
"verbosity": 3,
"timeBound": 300,
"controller": os.path.join(os.path.dirname(__file__), 'models/controller_cte_dist_130.pth')},
eval_params={"seed": 42,
eval_params={"monitor" : trainer_params.save_model_path,
"monitor_type" : monitor_type,
"seed": 42,
"render" : 0,
"verbosity": 3,
"timeBound": 300,
Expand Down Expand Up @@ -201,6 +208,3 @@ def spec(simulation):
print(modd.training_results)
print('Evaluation results:')
print(modd.evaluation_results)



6 changes: 3 additions & 3 deletions src/verifai/modd/odd_data_generator.py
Original file line number Diff line number Diff line change
Expand Up @@ -67,9 +67,9 @@ def sample_simulations(self, num_simulations, num_steps, save_path):
print(f"Saving in {save_path}training_{i}.pkl")
with open(os.path.join(save_path + os.sep, f"training_{i}.pkl"), 'wb') as filehandler:
pickle.dump(self.samples, filehandler)
except TerminationException:
if self.datagen_params.verbosity >= 1:
print("Sampler has generated all possible samples")
except Exception as e:
if self.eval_params.verbosity >= 1:
print(f' Failed to create simulation: {e}')
break
except KeyboardInterrupt:
break
Expand Down
6 changes: 2 additions & 4 deletions src/verifai/modd/odd_evaluator.py
Original file line number Diff line number Diff line change
Expand Up @@ -30,8 +30,6 @@ def __init__(self, eval_params, sampling_params, global_params):

def sample_simulations(self, num_simulations, num_steps, save_datagen_path, save_scenes_path):
i = 0
print(save_datagen_path)
print(save_scenes_path)
self.total_sample_time = 0
self.total_simulate_time = 0
self.sampling_params.server.maxSteps = num_steps
Expand Down Expand Up @@ -84,9 +82,9 @@ def sample_simulations(self, num_simulations, num_steps, save_datagen_path, save
with open(filename, 'wb') as filehandler:
pickle.dump(self.samples, filehandler)

except TerminationException:
except Exception as e:
if self.eval_params.verbosity >= 1:
print("Sampler has generated all possible samples")
print(f' Failed to create simulation: {e}')
break
except KeyboardInterrupt:
break
Expand Down
2 changes: 1 addition & 1 deletion src/verifai/modd/odd_learner.py
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@ def __init__(self, datagen_params=DotMap(), trainer_params=DotMap(),
self.sampling_params = sampling_params
self.global_params = global_params

def run(self):
def generate_monitor(self):
pass


Expand Down