diff --git a/examples/modd/followCarBehaviorMODD.scenic b/examples/modd/followCarBehaviorMODD.scenic index 77674b5..554dded 100644 --- a/examples/modd/followCarBehaviorMODD.scenic +++ b/examples/modd/followCarBehaviorMODD.scenic @@ -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. @@ -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 @@ -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 @@ -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: @@ -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: @@ -175,7 +178,7 @@ 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 @@ -183,8 +186,8 @@ behavior FollowCarBehaviorMODD(target_speed = 10, laneToFollow=None, is_opposite 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]))) @@ -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) diff --git a/examples/modd/followLeader_extracar.scenic b/examples/modd/followLeader_extracar.scenic index 356eea8..af9bc79 100644 --- a/examples/modd/followLeader_extracar.scenic +++ b/examples/modd/followLeader_extracar.scenic @@ -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) @@ -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) @@ -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] @@ -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) diff --git a/examples/modd/modd_learner_main.py b/examples/modd/modd_learner_main.py index 5d9a3b7..1cae3b4 100644 --- a/examples/modd/modd_learner_main.py +++ b/examples/modd/modd_learner_main.py @@ -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, @@ -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, ) @@ -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, @@ -201,6 +208,3 @@ def spec(simulation): print(modd.training_results) print('Evaluation results:') print(modd.evaluation_results) - - - diff --git a/src/verifai/modd/odd_data_generator.py b/src/verifai/modd/odd_data_generator.py index 0fc8a08..659143c 100644 --- a/src/verifai/modd/odd_data_generator.py +++ b/src/verifai/modd/odd_data_generator.py @@ -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 diff --git a/src/verifai/modd/odd_evaluator.py b/src/verifai/modd/odd_evaluator.py index 16e11f2..c3b5d92 100644 --- a/src/verifai/modd/odd_evaluator.py +++ b/src/verifai/modd/odd_evaluator.py @@ -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 @@ -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 diff --git a/src/verifai/modd/odd_learner.py b/src/verifai/modd/odd_learner.py index 025a6a7..b7744ac 100644 --- a/src/verifai/modd/odd_learner.py +++ b/src/verifai/modd/odd_learner.py @@ -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