diff --git a/examples/driving/arya.scenic b/examples/driving/arya.scenic new file mode 100644 index 000000000..b5d14978b --- /dev/null +++ b/examples/driving/arya.scenic @@ -0,0 +1,45 @@ +''' +To run this file using the MetaDrive simulator: + scenic examples/driving/arya.scenic --2d --model scenic.simulators.metadrive.model --simulate + +To run this file using the Carla simulator: + scenic examples/driving/arya.scenic --2d --model scenic.simulators.carla.model --simulate + + +TODO: + Make some kinda waypoints that the car can follow + make the car follow the waypoints using pure pursuit. +''' + +param map = localPath('../../assets/maps/CARLA/Town05.xodr') +param carla_map = 'Town05' +param time_step = 1.0/10 + +model scenic.domains.driving.model + +behavior PullIntoRoad(): + while (distance from self to ego) > 15: + wait + do FollowLaneBehavior(laneToFollow=ego.lane) + +ego = new Car with behavior DriveAvoidingCollisions(avoidance_threshold=5) + +rightCurb = ego.laneGroup.curb +spot = new OrientedPoint on visible rightCurb +badAngle = Uniform(1.0, -1.0) * Range(10, 20) deg +#parkedCar = new Car left of spot by 0.5, + #facing badAngle relative to roadDirection, + #with behavior PullIntoRoad + +#require (distance to parkedCar) > 20 + +monitor StopAfterInteraction(): + for i in range(50): + wait + while ego.speed > 2: + wait + for i in range(50): + wait + terminate +require monitor StopAfterInteraction() +terminate after 30 seconds # in case ego never breaks diff --git a/src/scenic/domains/driving/behaviors.scenic b/src/scenic/domains/driving/behaviors.scenic index 173500190..a9282b2ec 100644 --- a/src/scenic/domains/driving/behaviors.scenic +++ b/src/scenic/domains/driving/behaviors.scenic @@ -4,10 +4,15 @@ These behaviors are automatically imported when using the driving domain. """ import math +import matplotlib.pyplot as plt +import csv from scenic.domains.driving.actions import * import scenic.domains.driving.model as _model from scenic.domains.driving.roads import ManeuverType +from shapely.geometry import Point as ShapelyPoint +from shapely.geometry import LineString +from shapely.geometry import MultiPoint def concatenateCenterlines(centerlines=[]): return PolylineRegion.unionAll(centerlines) @@ -16,14 +21,17 @@ behavior ConstantThrottleBehavior(x): while True: take SetThrottleAction(x), SetReverseAction(False), SetHandBrakeAction(False) -behavior DriveAvoidingCollisions(target_speed=25, avoidance_threshold=10): - try: - do FollowLaneBehavior(target_speed=target_speed) +behavior DriveAvoidingCollisions(lon_controller = None, lat_controller = None, target_speed=25, avoidance_threshold=10): + _lon_controller = lon_controller + _lat_controller = lat_controller + try: + do FollowLaneBehavior(_lon_controller, _lat_controller, target_speed=target_speed) interrupt when self.distanceToClosest(_model.Vehicle) <= avoidance_threshold: take SetThrottleAction(0), SetBrakeAction(1) behavior AccelerateForwardBehavior(): take SetReverseAction(False), SetHandBrakeAction(False), SetThrottleAction(0.5) + take SetThrottleAction(0), SetHandBrakeAction(False), SetThrottleAction(0.5) behavior WalkForwardBehavior(): """Walk forward behavior for pedestrians. @@ -39,7 +47,7 @@ behavior WalkForwardBehavior(): behavior ConstantThrottleBehavior(x): take SetThrottleAction(x) -behavior FollowLaneBehavior(target_speed = 10, laneToFollow=None, is_oppositeTraffic=False): +behavior FollowLaneBehavior(lon_controller = None, lat_controller = None, target_speed = 10, laneToFollow=None, is_oppositeTraffic=False): """ Follow's the lane on which the vehicle is at, unless the laneToFollow is specified. Once the vehicle reaches an intersection, by default, the vehicle will take the straight route. @@ -52,6 +60,17 @@ behavior FollowLaneBehavior(target_speed = 10, laneToFollow=None, is_oppositeTra :param target_speed: Its unit is in m/s. By default, it is set to 10 m/s :param laneToFollow: If the lane to follow is different from the lane that the vehicle is on, this parameter can be used to specify that lane. By default, this variable will be set to None, which means that the vehicle will follow the lane that it is currently on. """ + + #Initializing controllers to default if not specified + if lon_controller is None or lat_controller is None: + default_lon_controller, default_lat_controller = simulation().getPurePursuitControllers(self) + + if lon_controller is None: + lon_controller = default_lon_controller + + if lat_controller is None: + lat_controller = default_lat_controller + past_steer_angle = 0 past_speed = 0 # making an assumption here that the agent starts from zero speed @@ -76,9 +95,6 @@ behavior FollowLaneBehavior(target_speed = 10, laneToFollow=None, is_oppositeTra else: nearby_intersection = current_lane.centerline[-1] - # instantiate longitudinal and lateral controllers - _lon_controller, _lat_controller = simulation().getLaneFollowingControllers(self) - while True: if self.speed is not None: @@ -129,28 +145,33 @@ behavior FollowLaneBehavior(target_speed = 10, laneToFollow=None, is_oppositeTra in_turning_lane = False entering_intersection = False target_speed = original_target_speed - _lon_controller, _lat_controller = simulation().getLaneFollowingControllers(self) - nearest_line_points = current_centerline.nearestSegmentTo(self.position) - nearest_line_segment = PolylineRegion(nearest_line_points) - cte = nearest_line_segment.signedDistanceTo(self.position) - if is_oppositeTraffic: - cte = -cte + if lon_controller is None or lat_controller is None: + default_lon_controller, default_lat_controller = simulation().getPurePursuitControllers(self) + + if lon_controller is None: + lon_controller = default_lon_controller + + if lat_controller is None: + lat_controller = default_lat_controller + speed_error = target_speed - current_speed # compute throttle : Longitudinal Control - throttle = _lon_controller.run_step(speed_error) + throttle = lon_controller.run_step(speed_error) # compute steering : Lateral Control - current_steer_angle = _lat_controller.run_step(cte) + current_steer_angle = lat_controller.run_step(current_centerline, self, is_oppositeTraffic) take RegulatedControlAction(throttle, current_steer_angle, past_steer_angle) past_steer_angle = current_steer_angle past_speed = current_speed + #plot code starts here + -behavior FollowTrajectoryBehavior(target_speed = 10, trajectory = None, turn_speed=None): +behavior FollowTrajectoryBehavior(lon_controller = None, lat_controller = None, target_speed = 10, trajectory = None, turn_speed=None): """ Follows the given trajectory. The behavior terminates once the end of the trajectory is reached. @@ -169,40 +190,10 @@ behavior FollowTrajectoryBehavior(target_speed = 10, trajectory = None, turn_spe traj_centerline = [traj.centerline for traj in trajectory] trajectory_centerline = concatenateCenterlines(traj_centerline) - # instantiate longitudinal and lateral controllers - _lon_controller,_lat_controller = simulation().getLaneFollowingControllers(self) - past_steer_angle = 0 - - if trajectory[-1].maneuvers: - end_intersection = trajectory[-1].maneuvers[0].intersection - if end_intersection == None: - end_intersection = trajectory[-1].centerline[-1] - else: - end_intersection = trajectory[-1].centerline[-1] - - while True: - if self in _model.network.intersectionRegion: - do TurnBehavior(trajectory_centerline, target_speed=turn_speed) + _lon_controller = lon_controller + _lat_controller = lat_controller - if (distance from self to end_intersection) < distanceToEndpoint: - break - - if self.speed is not None: - current_speed = self.speed - else: - current_speed = 0 - - cte = trajectory_centerline.signedDistanceTo(self.position) - speed_error = target_speed - current_speed - - # compute throttle : Longitudinal Control - throttle = _lon_controller.run_step(speed_error) - - # compute steering : Latitudinal Control - current_steer_angle = _lat_controller.run_step(cte) - - take RegulatedControlAction(throttle, current_steer_angle, past_steer_angle) - past_steer_angle = current_steer_angle + do FollowLaneBehavior(lon_controller = _lon_controller, lat_controller = _lat_controller, target_speed = target_speed, laneToFollow = traj_centerline) @@ -219,7 +210,7 @@ behavior TurnBehavior(trajectory, target_speed=6): trajectory_centerline = concatenateCenterlines([traj.centerline for traj in trajectory]) # instantiate longitudinal and lateral controllers - _lon_controller, _lat_controller = simulation().getTurningControllers(self) + lon_controller, lat_controller = simulation().getTurningControllers(self) past_steer_angle = 0 @@ -233,10 +224,10 @@ behavior TurnBehavior(trajectory, target_speed=6): speed_error = target_speed - current_speed # compute throttle : Longitudinal Control - throttle = _lon_controller.run_step(speed_error) + throttle = lon_controller.run_step(speed_error) # compute steering : Latitudinal Control - current_steer_angle = _lat_controller.run_step(cte) + current_steer_angle = lat_controller.run_step(cte) take RegulatedControlAction(throttle, current_steer_angle, past_steer_angle) past_steer_angle = current_steer_angle @@ -265,7 +256,7 @@ behavior LaneChangeBehavior(laneSectionToSwitch, is_oppositeTraffic=False, targe nearby_intersection = current_lane.centerline[-1] # instantiate longitudinal and lateral controllers - _lon_controller, _lat_controller = simulation().getLaneChangingControllers(self) + lon_controller, lat_controller = simulation().getLaneChangingControllers(self) past_steer_angle = 0 @@ -310,10 +301,10 @@ behavior LaneChangeBehavior(laneSectionToSwitch, is_oppositeTraffic=False, targe speed_error = target_speed - current_speed # compute throttle : Longitudinal Control - throttle = _lon_controller.run_step(speed_error) + throttle = lon_controller.run_step(speed_error) # compute steering : Latitudinal Control - current_steer_angle = _lat_controller.run_step(cte) + current_steer_angle = lat_controller.run_step(cte) take RegulatedControlAction(throttle, current_steer_angle, past_steer_angle) past_steer_angle = current_steer_angle diff --git a/src/scenic/domains/driving/controllers.py b/src/scenic/domains/driving/controllers.py index ce7d46135..d8f45844b 100644 --- a/src/scenic/domains/driving/controllers.py +++ b/src/scenic/domains/driving/controllers.py @@ -13,9 +13,18 @@ """ from collections import deque +import math import numpy as np +from scenic.domains.driving.actions import * +from scenic.domains.driving.roads import ManeuverType +from shapely.geometry import Point as ShapelyPoint +from shapely.geometry import LineString +from shapely.geometry import MultiPoint +from scenic.core.regions import PolylineRegion +from scenic.core.regions import CircularRegion + class PIDLongitudinalController: """Longitudinal control using a PID to reach a target speed. @@ -80,7 +89,7 @@ def __init__(self, K_P=0.3, K_D=0.2, K_I=0, dt=0.1): self.windup_guard = 20.0 self.output = 0 - def run_step(self, cte): + def run_step(self, input_trajectory, ego, opposite_traffic): """Estimate the steering angle of the vehicle based on the PID equations. Arguments: @@ -89,6 +98,14 @@ def run_step(self, cte): Returns: a signal between -1 and 1, with -1 meaning maximum steering to the left. """ + + nearest_line_points = input_trajectory.nearestSegmentTo(ego.position) + nearest_line_segment = PolylineRegion(nearest_line_points) + cte = nearest_line_segment.signedDistanceTo(ego.position) + + if opposite_traffic: + cte = -1 * cte + error = cte delta_error = error - self.last_error self.PTerm = self.Kp * error @@ -107,3 +124,121 @@ def run_step(self, cte): self.output = self.PTerm + (self.Ki * self.ITerm) + (self.Kd * self.DTerm) return np.clip(self.output, -1, 1) + +class PurePursuitLateralController: + """ + Pure Pursuit Controller + + Arguments: + wb: wheelbase length + ld: lookahead distance + dt: time step + cl: car length + clwbr: car length to wheel base ratio + """ + + def __init__(self, cl, ld = 7, dt = 0.1, clwbr = 0.65): + """ + Todo: + find the actual wheelbase and update the default number + - it says car length is 4.5 meters, but im not sure if thats wheelbase + experiment with the lookahead distance to see what works the best + """ + self.dt = dt + self.wb = cl * clwbr + self.clwbr = clwbr + self.ld = ld + self.past_cte = 0 + self.max_steering_angle = np.arctan((2 * self.wb) / self.ld) + + def run_step(self, input_trajectory, ego, opposite_traffic): + """Estimate the steering angle of the vehicle based on the pure pursuit equations. + + Arguments: + cte: cross-track error (distance to right of desired trajectory) + + Returns: + a signal between -1 and 1, with -1 meaning maximum steering to the left. + + Todo: + Find if the equation needs to be updated to consider negative vs positive cte + find what maximum steering distance is in radians and scale the return value accordingly + -It is not listed in the documentation + + + #takes current position and the path + #add plot + """ + # Define variables + lookahead_distance = self.ld + circlular_region = CircularRegion(ego.position, lookahead_distance, resolution = 64) + polyline_circle = circlular_region.boundary + shapely_boundary = polyline_circle.lineString # extract shapely circle and shapely path + line = input_trajectory.lineString + distance = input_trajectory.lineString.project(ShapelyPoint(ego.position.coordinates[0], ego.position.coordinates[1])) + coords = [] + try: + coords = list(line.coords) # lineString case + except NotImplementedError: + for geom in line.geoms: # multilinestring case + coords.extend(list(geom.coords)) + + + # Splitting the path into two parts, the part in front of the ego and behind it + + split_trajectory = [] + for j, p in enumerate(coords): + pd = line.project(ShapelyPoint(p[0], p[1])) + if pd == distance: + split_trajectory = [ + LineString(coords[:j+1]), + LineString(coords[j:])] + break + if pd > distance: + cp = line.interpolate(distance) + split_trajectory = [ + LineString(coords[:j] + [(cp.x, cp.y, 0)]), + LineString([(cp.x, cp.y, 0)] + coords[j:])] + break + + + # Get intersection points of the circle with the second half of the path + + shapely_intersection = shapely_boundary.intersection(split_trajectory[1]) + + if isinstance(shapely_intersection, ShapelyPoint): + candidate_points = [shapely_intersection] + elif isinstance(shapely_intersection, MultiPoint): + candidate_points = list(shapely_intersection.geoms) + else: + candidate_points = [] + assert all(isinstance(p, ShapelyPoint) for p in candidate_points) + + + # Find lookahead point by traversing the path + + if len(candidate_points) == 0: + # print("No candidate point found") + # Sometimes no point is found, this is an error + # In that case, dont touch steering wheel + # Pure Pursuit is a self correcting algorithm so it should be fine + cte = self.past_cte + else: + candidate_points.sort(key=lambda point: split_trajectory[1].project(ShapelyPoint(point.x, point.y))) + lookahead_point = candidate_points[0] + + + # Find the theta value/cte to feed to the pure pursuit algorithm + + theta = math.atan2(lookahead_point.y - ego.position.coordinates[1], lookahead_point.x - ego.position.coordinates[0]) + cte = (ego.heading + math.pi/2) - theta + self.past_cte = cte + + if opposite_traffic: + cte = -1 * cte + + # perform pure pursuit calculation and normalize it from -1 to 1 + steering_angle = np.arctan((2 * self.wb * math.sin(cte)) / self.ld) + rv = steering_angle / self.max_steering_angle + + return rv diff --git a/src/scenic/domains/driving/simulators.py b/src/scenic/domains/driving/simulators.py index 1fa1c1b64..75ff38eff 100644 --- a/src/scenic/domains/driving/simulators.py +++ b/src/scenic/domains/driving/simulators.py @@ -4,6 +4,7 @@ from scenic.domains.driving.controllers import ( PIDLateralController, PIDLongitudinalController, + PurePursuitLateralController, ) @@ -43,7 +44,15 @@ def getLaneFollowingControllers(self, agent): ) lat_controller = PIDLateralController(K_P=0.2, K_D=0.1, K_I=0.0, dt=dt) return lon_controller, lat_controller + + def getPurePursuitControllers(self, agent, cl = 4.5, ld = 7, clwbr = 0.65): + dt = self.timestep + + lon_controller = PIDLongitudinalController(K_P=0.5, K_D=0.1, K_I=0.7, dt=dt) + lat_controller = PurePursuitLateralController(cl = agent.length, ld = 7, dt = dt, clwbr = 0.65) + return lon_controller, lat_controller + def getTurningControllers(self, agent): """Get longitudinal and lateral controllers for turning.""" dt = self.timestep