def _check_town(self, world): if CarlaDataProvider.get_map().name != self._town: print("The CARLA server uses the wrong map!") print("This scenario requires to use map {}".format(self._town)) raise Exception("The CARLA server uses the wrong map!")
def get_distance_along_route(route, target_location): """ Calculate the distance of the given location along the route Note: If the location is not along the route, the route length will be returned """ wmap = CarlaDataProvider.get_map() covered_distance = 0 prev_position = None found = False # Don't use the input location, use the corresponding wp as location target_location_from_wp = wmap.get_waypoint(target_location).transform.location for position, _ in route: location = target_location_from_wp # Don't perform any calculations for the first route point if not prev_position: prev_position = position continue # Calculate distance between previous and current route point interval_length_squared = ((prev_position.x - position.x) ** 2) + ((prev_position.y - position.y) ** 2) distance_squared = ((location.x - prev_position.x) ** 2) + ((location.y - prev_position.y) ** 2) # Close to the current position? Stop calculation if distance_squared < 0.01: break if distance_squared < 400 and not distance_squared < interval_length_squared: # Check if a neighbor lane is closer to the route # Do this only in a close distance to correct route interval, otherwise the computation load is too high starting_wp = wmap.get_waypoint(location) wp = starting_wp.get_left_lane() while wp is not None: new_location = wp.transform.location new_distance_squared = ((new_location.x - prev_position.x) ** 2) + ( (new_location.y - prev_position.y) ** 2) if np.sign(starting_wp.lane_id) != np.sign(wp.lane_id): break if new_distance_squared < distance_squared: distance_squared = new_distance_squared location = new_location else: break wp = wp.get_left_lane() wp = starting_wp.get_right_lane() while wp is not None: new_location = wp.transform.location new_distance_squared = ((new_location.x - prev_position.x) ** 2) + ( (new_location.y - prev_position.y) ** 2) if np.sign(starting_wp.lane_id) != np.sign(wp.lane_id): break if new_distance_squared < distance_squared: distance_squared = new_distance_squared location = new_location else: break wp = wp.get_right_lane() if distance_squared < interval_length_squared: # The location could be inside the current route interval, if route/lane ids match # Note: This assumes a sufficiently small route interval # An alternative is to compare orientations, however, this also does not work for # long route intervals curr_wp = wmap.get_waypoint(position) prev_wp = wmap.get_waypoint(prev_position) wp = wmap.get_waypoint(location) if prev_wp and curr_wp and wp: if wp.road_id == prev_wp.road_id or wp.road_id == curr_wp.road_id: # Roads match, now compare the sign of the lane ids if (np.sign(wp.lane_id) == np.sign(prev_wp.lane_id) or np.sign(wp.lane_id) == np.sign(curr_wp.lane_id)): # The location is within the current route interval covered_distance += math.sqrt(distance_squared) found = True break covered_distance += math.sqrt(interval_length_squared) prev_position = position return covered_distance, found
def convert_position_to_transform(position, actor_list=None): """ Convert an OpenScenario position into a CARLA transform Not supported: Road, RelativeRoad, Lane, RelativeLane as the PythonAPI currently does not provide sufficient access to OpenDrive information Also not supported is Route. This can be added by checking additional route information """ if position.find('World') is not None: world_pos = position.find('World') x = float(world_pos.attrib.get('x', 0)) y = float(world_pos.attrib.get('y', 0)) z = float(world_pos.attrib.get('z', 0)) yaw = math.degrees(float(world_pos.attrib.get('h', 0))) pitch = math.degrees(float(world_pos.attrib.get('p', 0))) roll = math.degrees(float(world_pos.attrib.get('r', 0))) if not OpenScenarioParser.use_carla_coordinate_system: y = y * (-1.0) yaw = yaw * (-1.0) return carla.Transform(carla.Location(x=x, y=y, z=z), carla.Rotation(yaw=yaw, pitch=pitch, roll=roll)) elif ((position.find('RelativeWorld') is not None) or (position.find('RelativeObject') is not None) or (position.find('RelativeLane') is not None)): rel_pos = position.find('RelativeWorld') or position.find('RelativeObject') or position.find('RelativeLane') # get relative object and relative position obj = rel_pos.attrib.get('object') obj_actor = None actor_transform = None if actor_list is not None: for actor in actor_list: if actor.rolename == obj: obj_actor = actor actor_transform = actor.transform else: for actor in CarlaDataProvider.get_world().get_actors(): if 'role_name' in actor.attributes and actor.attributes['role_name'] == obj: obj_actor = actor actor_transform = obj_actor.get_transform() break if obj_actor is None: raise AttributeError("Object '{}' provided as position reference is not known".format(obj)) # calculate orientation h, p, r is_absolute = False if rel_pos.find('Orientation') is not None: orientation = rel_pos.find('Orientation') is_absolute = (orientation.attrib.get('type') == "absolute") dyaw = math.degrees(float(orientation.attrib.get('h', 0))) dpitch = math.degrees(float(orientation.attrib.get('p', 0))) droll = math.degrees(float(orientation.attrib.get('r', 0))) if not OpenScenarioParser.use_carla_coordinate_system: dyaw = dyaw * (-1.0) yaw = actor_transform.rotation.yaw pitch = actor_transform.rotation.pitch roll = actor_transform.rotation.roll if not is_absolute: yaw = yaw + dyaw pitch = pitch + dpitch roll = roll + droll else: yaw = dyaw pitch = dpitch roll = droll # calculate location x, y, z # dx, dy, dz if (position.find('RelativeWorld') is not None) or (position.find('RelativeObject') is not None): dx = float(rel_pos.attrib.get('dx', 0)) dy = float(rel_pos.attrib.get('dy', 0)) dz = float(rel_pos.attrib.get('dz', 0)) if not OpenScenarioParser.use_carla_coordinate_system: dy = dy * (-1.0) x = actor_transform.location.x + dx y = actor_transform.location.y + dy z = actor_transform.location.z + dz # dLane, ds, offset elif position.find('RelativeLane') is not None: dlane = float(rel_pos.attrib.get('dLane')) ds = float(rel_pos.attrib.get('ds')) offset = float(rel_pos.attrib.get('offset')) carla_map = CarlaDataProvider.get_map() relative_waypoint = carla_map.get_waypoint(actor_transform.location) if dlane == 0: wp = relative_waypoint elif dlane == -1: wp = relative_waypoint.get_left_lane() elif dlane == 1: wp = relative_waypoint.get_right_lane() if wp is None: raise AttributeError("Object '{}' position with dLane={} is not valid".format(obj, dlane)) wp = wp.next(ds)[-1] # Adapt transform according to offset h = math.radians(wp.transform.rotation.yaw) x_offset = math.sin(h) * offset y_offset = math.cos(h) * offset if OpenScenarioParser.use_carla_coordinate_system: x_offset = x_offset * (-1.0) y_offset = y_offset * (-1.0) x = wp.transform.location.x + x_offset y = wp.transform.location.y + y_offset z = wp.transform.location.z return carla.Transform(carla.Location(x=x, y=y, z=z), carla.Rotation(yaw=yaw, pitch=pitch, roll=roll)) # Not implemented elif position.find('Road') is not None: raise NotImplementedError("Road positions are not yet supported") elif position.find('RelativeRoad') is not None: raise NotImplementedError("RelativeRoad positions are not yet supported") elif position.find('Lane') is not None: lane_pos = position.find('Lane') road_id = int(lane_pos.attrib.get('roadId', 0)) lane_id = int(lane_pos.attrib.get('laneId', 0)) offset = float(lane_pos.attrib.get('offset', 0)) s = float(lane_pos.attrib.get('s', 0)) is_absolute = True waypoint = CarlaDataProvider.get_map().get_waypoint_xodr(road_id, lane_id, s) if waypoint is None: raise AttributeError("Lane position cannot be found") transform = waypoint.transform if lane_pos.find('Orientation') is not None: orientation = rel_pos.find('Orientation') dyaw = math.degrees(float(orientation.attrib.get('h', 0))) dpitch = math.degrees(float(orientation.attrib.get('p', 0))) droll = math.degrees(float(orientation.attrib.get('r', 0))) if not OpenScenarioParser.use_carla_coordinate_system: dyaw = dyaw * (-1.0) transform.rotation.yaw = transform.rotation.yaw + dyaw transform.rotation.pitch = transform.rotation.pitch + dpitch transform.rotation.roll = transform.rotation.roll + droll if offset != 0: forward_vector = transform.rotation.get_forward_vector() orthogonal_vector = carla.Vector3D(x=-forward_vector.y, y=forward_vector.x, z=forward_vector.z) transform.location.x = transform.location.x + offset * orthogonal_vector.x transform.location.y = transform.location.y + offset * orthogonal_vector.y return transform elif position.find('Route') is not None: raise NotImplementedError("Route positions are not yet supported") else: raise AttributeError("Unknown position")
def _create_behavior(self): """ Hero vehicle is turning left in an urban area, at a signalized intersection, while other actor coming straight .The hero actor may turn left either before other actor passes intersection or later, without any collision. After 80 seconds, a timeout stops the scenario. """ sequence = py_trees.composites.Sequence("Sequence Behavior") # Selecting straight path at intersection target_waypoint = generate_target_waypoint( CarlaDataProvider.get_map().get_waypoint( self.other_actors[0].get_location()), 0, ) # Generating waypoint list till next intersection plan = [] wp_choice = target_waypoint.next(1.0) while not wp_choice[0].is_intersection: target_waypoint = wp_choice[0] plan.append((target_waypoint, RoadOption.LANEFOLLOW)) wp_choice = target_waypoint.next(1.0) # adding flow of actors actor_source = ActorSource( ["vehicle.tesla.model3"], self._other_actor_transform, 15, self._blackboard_queue_name, ) # destroying flow of actors actor_sink = ActorSink(plan[-1][0].transform.location, 10) # follow waypoints untill next intersection move_actor = WaypointFollower( self.other_actors[0], self._target_vel, plan=plan, blackboard_queue_name=self._blackboard_queue_name, avoid_collision=True, ) # wait wait = DriveDistance(self.ego_vehicles[0], self._ego_distance) # actor_source2 = ActorSource(['vehicle.audi.tt'], # self._other_actor_transform2, 10, # self._blackboard_queue_name2) # # destroying flow of actors # actor_sink2 = ActorSink(carla.Location(x=-74.9, y=30), 20) # # follow waypoints untill next intersection # move_actor2 = WaypointFollower( # self.other_actors[1], # self._target_vel, # blackboard_queue_name=self._blackboard_queue_name2, # avoid_collision=True) distance_to_vehicle = InTriggerDistanceToVehicle( self.other_actors[1], self.ego_vehicles[0], 20) actor1_drive = KeepVelocity(self.other_actors[1], 8, distance=90) # Behavior tree root1 = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) # root1.add_child(wait) root1.add_child(actor_source) root1.add_child(actor_sink) root1.add_child(move_actor) # root2 = py_trees.composites.Parallel( # policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) root2 = py_trees.composites.Sequence( "Sequence_dist", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL) # root2.add_child(actor_source2) # root2.add_child(actor_sink2) # root2.add_child(move_actor2) root2.add_child(distance_to_vehicle) root2.add_child(actor1_drive) root = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) root.add_child(root1) root.add_child(root2) root.add_child(wait) sequence.add_child( ActorTransformSetter(self.other_actors[0], self._other_actor_transform)) sequence.add_child(root) sequence.add_child(ActorDestroy(self.other_actors[0])) sequence.add_child(ActorDestroy(self.other_actors[1])) return sequence
def _set_new_velocity(self, next_location): """ Calculate and set the new actor veloctiy given the current actor location and the _next_location_ If _consider_obstacles is true, the speed is adapted according to the closest obstacle in front of the actor, if it is within the _proximity_threshold distance. Args: next_location (carla.Location): Next target location of the actor returns: direction (carla.Vector3D): Length of direction vector of the actor """ current_time = GameTime.get_time() target_speed = self._target_speed if not self._last_update: self._last_update = current_time if self._consider_obstacles: # If distance is less than the proximity threshold, adapt velocity if self._obstacle_distance < self._proximity_threshold: distance = max(self._obstacle_distance, 0) if distance > 0: current_speed = math.sqrt(self._actor.get_velocity().x**2 + self._actor.get_velocity().y**2) current_speed_other = math.sqrt( self._obstacle_actor.get_velocity().x**2 + self._obstacle_actor.get_velocity().y**2) if current_speed_other < current_speed: acceleration = -0.5 * ( current_speed - current_speed_other)**2 / distance target_speed = max( acceleration * (current_time - self._last_update) + current_speed, 0) else: target_speed = 0 # set new linear velocity velocity = carla.Vector3D(0, 0, 0) direction = next_location - CarlaDataProvider.get_location(self._actor) direction_norm = math.sqrt(direction.x**2 + direction.y**2) velocity.x = direction.x / direction_norm * target_speed velocity.y = direction.y / direction_norm * target_speed self._actor.set_velocity(velocity) # set new angular velocity current_yaw = CarlaDataProvider.get_transform(self._actor).rotation.yaw new_yaw = CarlaDataProvider.get_map().get_waypoint( next_location).transform.rotation.yaw delta_yaw = new_yaw - current_yaw if math.fabs(delta_yaw) > 360: delta_yaw = delta_yaw % 360 if delta_yaw > 180: delta_yaw = delta_yaw - 360 elif delta_yaw < -180: delta_yaw = delta_yaw + 360 angular_velocity = carla.Vector3D(0, 0, 0) if target_speed == 0: angular_velocity.z = 0 else: angular_velocity.z = delta_yaw / (direction_norm / target_speed) self._actor.set_angular_velocity(angular_velocity) self._last_update = current_time return direction_norm
def _create_behavior(self): """ Hero vehicle is turning right in an urban area, at a signalized intersection, while other actor coming straight from left.The hero actor may turn right either before other actor passes intersection or later, without any collision. After 80 seconds, a timeout stops the scenario. """ location_of_collision_dynamic = get_geometric_linear_intersection( self.ego_vehicles[0], self.other_actors[0]) crossing_point_dynamic = get_crossing_point(self.other_actors[0]) sync_arrival = SyncArrival(self.other_actors[0], self.ego_vehicles[0], location_of_collision_dynamic) sync_arrival_stop = InTriggerDistanceToLocation( self.other_actors[0], crossing_point_dynamic, 5) sync_arrival_parallel = py_trees.composites.Parallel( "Synchronize arrival times", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) sync_arrival_parallel.add_child(sync_arrival) sync_arrival_parallel.add_child(sync_arrival_stop) # Selecting straight path at intersection target_waypoint = generate_target_waypoint( CarlaDataProvider.get_map().get_waypoint( self.other_actors[0].get_location()), 0) # Generating waypoint list till next intersection plan = [] wp_choice = target_waypoint.next(1.0) while not wp_choice[0].is_intersection: target_waypoint = wp_choice[0] plan.append((target_waypoint, RoadOption.LANEFOLLOW)) wp_choice = target_waypoint.next(1.0) move_actor = WaypointFollower(self.other_actors[0], self._target_vel, plan=plan) waypoint_follower_end = InTriggerDistanceToLocation( self.other_actors[0], plan[-1][0].transform.location, 10) move_actor_parallel = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) move_actor_parallel.add_child(move_actor) move_actor_parallel.add_child(waypoint_follower_end) # stop other actor stop = StopVehicle(self.other_actors[0], self._brake_value) # end condition end_condition = DriveDistance(self.ego_vehicles[0], self._ego_distance) # Behavior tree sequence = py_trees.composites.Sequence() sequence.add_child( ActorTransformSetter(self.other_actors[0], self._other_actor_transform)) sequence.add_child(sync_arrival_parallel) sequence.add_child(move_actor_parallel) sequence.add_child(stop) sequence.add_child(end_condition) sequence.add_child(ActorDestroy(self.other_actors[0])) return sequence
def __call__(self): return {'opendrive': CarlaDataProvider.get_map().to_opendrive()}
def _load_and_wait_for_world(self, town, ego_vehicles=None): """ Load a new CARLA world and provide data to CarlaDataProvider """ if self._args.reloadWorld: self.world = self.client.load_world(town) else: # if the world should not be reloaded, wait at least until all ego vehicles are ready ego_vehicle_found = False if self._args.waitForEgo: while not ego_vehicle_found and not self._shutdown_requested: vehicles = self.client.get_world().get_actors().filter( 'vehicle.*') for ego_vehicle in ego_vehicles: ego_vehicle_found = False for vehicle in vehicles: if vehicle.attributes[ 'role_name'] == ego_vehicle.rolename: ego_vehicle_found = True break if not ego_vehicle_found: print("Not all ego vehicles ready. Waiting ... ") time.sleep(1) break self.world = self.client.get_world() for speed_sign in self.world.get_actors().filter( 'traffic.speed_limit.*'): if speed_sign.type_id == 'traffic.speed_limit.30': speed_sign.destroy() if self._args.sync: settings = self.world.get_settings() settings.synchronous_mode = True settings.fixed_delta_seconds = 1.0 / self.frame_rate self.world.apply_settings(settings) self.traffic_manager.set_synchronous_mode(True) self.traffic_manager.set_random_device_seed( int(self._args.trafficManagerSeed)) CarlaDataProvider.set_client(self.client) CarlaDataProvider.set_world(self.world) CarlaDataProvider.set_traffic_manager_port( int(self._args.trafficManagerPort)) # Wait for the world to be ready if CarlaDataProvider.is_sync_mode(): self.world.tick() else: self.world.wait_for_tick() if CarlaDataProvider.get_map( ).name != town and CarlaDataProvider.get_map().name != "OpenDriveMap": print("The CARLA server uses the wrong map: {}".format( CarlaDataProvider.get_map().name)) print("This scenario requires to use map: {}".format(town)) return False return True
def convert_maneuver_to_atomic(action, actor): """ Convert an OpenSCENARIO maneuver action into a Behavior atomic Note not all OpenSCENARIO actions are currently supported """ maneuver_name = action.attrib.get('name', 'unknown') if action.find('Global') is not None: global_action = action.find('Global') if global_action.find('Infrastructure') is not None: infrastructure_action = global_action.find( 'Infrastructure').find('Signal') if infrastructure_action.find('SetState') is not None: traffic_light_action = infrastructure_action.find( 'SetState') traffic_light_id = traffic_light_action.attrib.get('name') traffic_light_state = traffic_light_action.attrib.get( 'state') atomic = TrafficLightStateSetter(traffic_light_id, traffic_light_state) else: raise NotImplementedError( "TrafficLights can only be influenced via SetState") else: raise NotImplementedError( "Global actions are not yet supported") elif action.find('UserDefined') is not None: user_defined_action = action.find('UserDefined') if user_defined_action.find('Command') is not None: command = user_defined_action.find('Command').text.replace( " ", "") if command == 'Idle': atomic = Idle() else: raise AttributeError( "Unknown user command action: {}".format(command)) else: raise NotImplementedError( "UserDefined script actions are not yet supported") elif action.find('Private') is not None: private_action = action.find('Private') if private_action.find('Longitudinal') is not None: private_action = private_action.find('Longitudinal') if private_action.find('Speed') is not None: long_maneuver = private_action.find('Speed') # duration and distance duration = float( long_maneuver.find("Dynamics").attrib.get( 'time', float("inf"))) distance = float( long_maneuver.find("Dynamics").attrib.get( 'distance', float("inf"))) # absolute velocity with given target speed if long_maneuver.find("Target").find( "Absolute") is not None: target_speed = float( long_maneuver.find("Target").find( "Absolute").attrib.get('value', 0)) atomic = KeepVelocity(actor, target_speed, distance=distance, duration=duration, name=maneuver_name) # relative velocity to given actor if long_maneuver.find("Target").find( "Relative") is not None: relative_speed = long_maneuver.find("Target").find( "Relative") obj = relative_speed.attrib.get('object') value = float(relative_speed.attrib.get('value', 0)) value_type = relative_speed.attrib.get('valueType') continuous = relative_speed.attrib.get('continuous') for traffic_actor in CarlaDataProvider.get_world( ).get_actors(): if 'role_name' in traffic_actor.attributes and traffic_actor.attributes[ 'role_name'] == obj: obj_actor = traffic_actor atomic = SetRelativeOSCVelocity( actor, obj_actor, value, value_type, continuous, duration, distance) elif private_action.find('Distance') is not None: raise NotImplementedError( "Longitudinal distance actions are not yet supported") else: raise AttributeError("Unknown longitudinal action") elif private_action.find('Lateral') is not None: private_action = private_action.find('Lateral') if private_action.find('LaneChange') is not None: lat_maneuver = private_action.find('LaneChange') target_lane_rel = float( lat_maneuver.find("Target").find( "Relative").attrib.get('value', 0)) distance = float( lat_maneuver.find("Dynamics").attrib.get( 'distance', float("inf"))) atomic = LaneChange( actor, None, direction="left" if target_lane_rel < 0 else "right", distance_lane_change=distance, name=maneuver_name) else: raise AttributeError("Unknown lateral action") elif private_action.find('Visibility') is not None: raise NotImplementedError( "Visibility actions are not yet supported") elif private_action.find('Meeting') is not None: raise NotImplementedError( "Meeting actions are not yet supported") elif private_action.find('Autonomous') is not None: private_action = private_action.find('Autonomous') activate = strtobool(private_action.attrib.get('activate')) atomic = ChangeAutoPilot(actor, activate, name=maneuver_name) elif private_action.find('Controller') is not None: raise NotImplementedError( "Controller actions are not yet supported") elif private_action.find('Position') is not None: position = private_action.find('Position') atomic = ActorTransformSetterToOSCPosition(actor, position, name=maneuver_name) elif private_action.find('Routing') is not None: target_speed = 5.0 private_action = private_action.find('Routing') if private_action.find('FollowRoute') is not None: private_action = private_action.find('FollowRoute') if private_action.find('Route') is not None: route = private_action.find('Route') plan = [] if route.find('ParameterDeclaration') is not None: if route.find('ParameterDeclaration').find( 'Parameter') is not None: parameter = route.find( 'ParameterDeclaration').find('Parameter') if parameter.attrib.get('name') == "Speed": target_speed = float( parameter.attrib.get('value', 5.0)) for waypoint in route.iter('Waypoint'): position = waypoint.find('Position') transform = OpenScenarioParser.convert_position_to_transform( position) waypoint = CarlaDataProvider.get_map( ).get_waypoint(transform.location) plan.append((waypoint, RoadOption.LANEFOLLOW)) atomic = WaypointFollower(actor, target_speed=target_speed, plan=plan, name=maneuver_name) elif private_action.find('CatalogReference') is not None: raise NotImplementedError( "CatalogReference private actions are not yet supported" ) else: raise AttributeError( "Unknown private FollowRoute action") elif private_action.find('FollowTrajectory') is not None: raise NotImplementedError( "Private FollowTrajectory actions are not yet supported" ) elif private_action.find('AcquirePosition') is not None: raise NotImplementedError( "Private AcquirePosition actions are not yet supported" ) else: raise AttributeError("Unknown private routing action") else: raise AttributeError("Unknown private action") else: raise AttributeError("Unknown action") return atomic
def __init__(self, world, ego_vehicles, config, test_num, rep, randomize=False, debug_mode=False, criteria_enable=True, timeout=80): self._map = CarlaDataProvider.get_map() self._world = world # specify test run number self._test = int(test_num) self._rep = int(rep) # location of the ego vehicle self._reference_waypoint = self._map.get_waypoint( config.trigger_points[0].location) self._other_actor_max_brake = 1.0 self._other_actor_stop_in_front_intersection = 10 self._other_actor_transform = [] self.timeout = timeout # Timeout of scenario in seconds self._num_scenarios = 3 #random.randint(1, 5) self.scenario_list = [] # return all possible scenarios and trigger points potential_scenarios = list_potential_scenarios() potential_triggers = list_potential_triggers(self._map) # if self._test >= 0 and self._test <= 4: # if self._rep != 0: # print("\nrunning repeated scenarios\n")#{}\n".format(str(self._test))) # self.selected_scenarios = self.select_scenarios(potential_scenarios, self._test) # self.selected_triggers = self.select_triggers(potential_triggers, self._test) # else: # i = 0 # while i < int(generate): if self._rep == 0: # i += 1 # print("\nrandomly generating scenario {}\n".format(i)) # randomly select specified number of scenarios and their trigger points self.selected_scenarios = self.select_scenarios_random( potential_scenarios, self._num_scenarios) self.selected_triggers = random.sample(potential_triggers, self._num_scenarios) for i in range(len(self.selected_scenarios)): trigger = self.selected_triggers[i] scenario = self.selected_scenarios[i] print(scenario + " | ") print(str(trigger) + " | ") print("\n") if scenario == "VehiclesAhead": actor_dict = LeadVehicleDict(trigger, self._map, None) # elif scenario == "StationaryObstaclesAhead": # actor_dict = StationaryObstaclesDict(trigger, self._map) elif scenario == "CutIn": actor_dict = CutInDict(trigger, self._map, None) elif scenario == "DynamicObstaclesAhead": off_highway_1 = self._map.get_waypoint( carla.Location(203, -341, 3)) if (trigger != off_highway_1): trigger = off_highway_1 actor_dict = DynamicObstaclesDict(trigger, self._map, None) else: print("actor dict class not implemented") exit(1) if not actor_dict.failed: self.scenario_list.append((scenario, actor_dict)) # for trigger in self.selected_triggers: # print(str(trigger) + " | ") # print("\n") # self.actor_list = dict() # if randomize: # self._first_vehicle_location = random.randint(20, 150) data = "\n" for ind in range(len(self.scenario_list)): (scenario, actor_dict) = self.scenario_list[ind] # scenario = self.selected_scenarios[ind] # trigger = self.selected_triggers[ind] x = actor_dict.start_transform.location.x y = actor_dict.start_transform.location.y z = actor_dict.start_transform.location.z transform_str = "{}|{}|{}".format(x, y, z) x = actor_dict.trigger.transform.location.x y = actor_dict.trigger.transform.location.y z = actor_dict.trigger.transform.location.z trigger_str = "{}|{}|{}".format(x, y, z) #test.rep.scenario test_info = "{}.{}.{}".format(self._test, self._rep, ind) if scenario == "VehiclesAhead": data += "{},VehiclesAhead,{},{},{},{},{},{},{},".format( test_info, #actor_dict.start_transform.location, #actor_dict.trigger.transform.location, transform_str, trigger_str, actor_dict.start_dist, actor_dict.speed, actor_dict.stop, "", "") elif scenario == "DynamicObstaclesAhead": # self._initialize_dynamic_obstacles_ahead() data += "{},DynamicObstaclesAhead,{},{},{},{},{},{},{},".format( test_info, #actor_dict.start_transform.location, #actor_dict.trigger.transform.location, transform_str, trigger_str, actor_dict.start_dist, actor_dict.speed, actor_dict.time_to_reach, "", "") else: #CutIn data += "{},CutIn,{},{},{},{},{},{},{},".format( test_info, #actor_dict.start_transform.location, #actor_dict.trigger.transform.location, transform_str, trigger_str, actor_dict.start_dist, actor_dict.speed, actor_dict.delta_speed, actor_dict.trigger_distance, actor_dict.direction) # 20% chance that there is extreme weather # extreme_weather = (random.randint(0, 5) < 1) extreme_weather = False if extreme_weather: data += "True," else: data += "False," # data += "\n" # f = open('test.txt', 'a') # f.write(data) f = open('test_config.txt', 'w') f.write(data) f.close() # agent = "" # while not agent: # f = open('test_agent.txt','r') # agent = f.read() # f.close() # f = open('test_agent.txt','w') # f.write("") # f.close() f = open('test_log.txt', 'a') f.write(data) f.close() # if int(generate) < 0: # retrieve test config from test_config.txt else: f = open('test_config.txt', 'r') config_info = f.read() f.close() # print("test {}".format(test_num)) # config_lines = file_content.split("\n") # config_info = config_lines[test_num] config_list = config_info.split(",") # agent = "" # while not agent: # f = open('test_agent.txt','r') # agent = f.read() # f.close() f = open('test_log.txt', 'a') for i in range(self._num_scenarios): test_info = config_list[0 + i * 9].split('.') test = test_info[0] rep = int(test_info[1]) scenario = test_info[2] rep += 1 # rep += 1 test_info[1] = str(rep) test_info_str = ".".join(test_info) config_list[0 + i * 9] = test_info_str config_info = ",".join(config_list) f.write(config_info) f.close() # update scenario config file f = open('test_config.txt', 'w') f.write(config_info) f.close() for i in range(self._num_scenarios): scenario_name = config_list[1 + i * 9] # start_transform = config_list[3] trigger = config_list[3 + i * 9].split( "|") #NEEDS TO BE PARSED --> x.y.z x = float(trigger[0]) y = float(trigger[1]) z = float(trigger[2]) trigger_waypoint = self._map.get_waypoint( carla.Location(x, y, z)) print(scenario_name + " | ") print(str(trigger_waypoint) + " | ") print("\n") start_dist = int(config_list[4 + i * 9]) speed = int(config_list[5 + i * 9]) if scenario_name == "VehiclesAhead": parsed_dict = dict() parsed_dict["start_dist"] = start_dist parsed_dict["trigger"] = trigger_waypoint parsed_dict["speed"] = speed if config_list[6 + i * 9] == "True": parsed_dict["stop"] = True else: parsed_dict["stop"] = False actor_dict = LeadVehicleDict(None, self._map, parsed_dict) elif scenario_name == "DynamicObstaclesAhead": time_to_reach = config_list[6 + i * 9] parsed_dict = dict() parsed_dict["start_dist"] = start_dist parsed_dict["trigger"] = trigger_waypoint parsed_dict["speed"] = speed parsed_dict["time_to_reach"] = int(time_to_reach) actor_dict = DynamicObstaclesDict(None, self._map, parsed_dict) elif scenario_name == "CutIn": delta_speed = int(config_list[6 + i * 9]) trigger_dist = int(config_list[7 + i * 9]) direction = config_list[8 + i * 9] parsed_dict = dict() parsed_dict["start_dist"] = start_dist parsed_dict["trigger"] = trigger_waypoint parsed_dict["speed"] = speed parsed_dict["delta_speed"] = delta_speed parsed_dict["trigger_distance"] = trigger_dist parsed_dict["direction"] = direction actor_dict = CutInDict(None, self._map, parsed_dict) else: print("ERROR: Invalid scenario name") raise RuntimeError self.scenario_list.append((scenario_name, actor_dict)) #PARSE CONFIG HERE f = open('test.log', 'a') # log test configurations # row = [] # for ind in range(len(self.selected_scenarios)): # scenario = self.selected_scenarios[ind] # trigger = self.selected_triggers[ind] # row.append(scenario + " at: \n" + str(trigger.transform.location)) # scenario_headers = [ # "Scenario One", "Scenario Two", "Scenario Three" # ] f.write( "\n\n\n\n##########################################################\n" + "\nTest Number: {}\nRep: {}\n\n".format(test_num, rep) + "##########################################################\n") # f.write(tabulate([row], headers=scenario_headers)) # f.write("\n-----------------------------------------------------------------------------------------------------------------------------------------") # f.write("\n<----- Test Configurations ----->") for (scenario, actor_dict) in self.scenario_list: log_header = [scenario] row = actor_dict._print_config() f.write(tabulate([row])) f.write("\n") f.close() super(RandomTest, self).__init__("RandomTest", ego_vehicles, config, world, debug_mode, criteria_enable=criteria_enable)
def _create_behavior(self): """ Scenario behavior: The other vehicle waits until the ego vehicle is close enough to the intersection and that its own traffic light is red. Then, it will start driving and 'illegally' cross the intersection. After a short distance it should stop again, outside of the intersection. The ego vehicle has to avoid the crash, but continue driving after the intersection is clear. If this does not happen within 120 seconds, a timeout stops the scenario """ crossing_point_dynamic = get_crossing_point(self.ego_vehicles[0]) # start condition startcondition = InTriggerDistanceToLocation( self.ego_vehicles[0], crossing_point_dynamic, self._ego_distance_to_traffic_light, name="Waiting for start position") sync_arrival_parallel = py_trees.composites.Parallel( "Synchronize arrival times", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) location_of_collision_dynamic = get_geometric_linear_intersection( self.ego_vehicles[0], self.other_actors[0]) sync_arrival = SyncArrival(self.other_actors[0], self.ego_vehicles[0], location_of_collision_dynamic) sync_arrival_stop = InTriggerDistanceToNextIntersection( self.other_actors[0], 5) sync_arrival_parallel.add_child(sync_arrival) sync_arrival_parallel.add_child(sync_arrival_stop) # Generate plan for WaypointFollower turn = 0 # drive straight ahead plan = [] # generating waypoints until intersection (target_waypoint) plan, target_waypoint = generate_target_waypoint_list( CarlaDataProvider.get_map().get_waypoint( self.other_actors[0].get_location()), turn) # Generating waypoint list till next intersection wp_choice = target_waypoint.next(5.0) while len(wp_choice) == 1: target_waypoint = wp_choice[0] plan.append((target_waypoint, RoadOption.LANEFOLLOW)) wp_choice = target_waypoint.next(5.0) continue_driving = py_trees.composites.Parallel( "ContinueDriving", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) continue_driving_waypoints = WaypointFollower( self.other_actors[0], self._other_actor_target_velocity, plan=plan, avoid_collision=False) continue_driving_distance = DriveDistance(self.other_actors[0], self._other_actor_distance, name="Distance") continue_driving_timeout = TimeOut(10) continue_driving.add_child(continue_driving_waypoints) continue_driving.add_child(continue_driving_distance) continue_driving.add_child(continue_driving_timeout) # finally wait that ego vehicle drove a specific distance wait = DriveDistance(self.ego_vehicles[0], self._ego_distance_to_drive, name="DriveDistance") # Build behavior tree sequence = py_trees.composites.Sequence("Sequence Behavior") sequence.add_child( ActorTransformSetter(self.other_actors[0], self._other_actor_transform)) sequence.add_child(startcondition) sequence.add_child(sync_arrival_parallel) sequence.add_child(continue_driving) sequence.add_child(wait) sequence.add_child(ActorDestroy(self.other_actors[0])) return sequence
def run_step(self): """ Execute on tick of the controller's control loop If _waypoints are provided, the vehicle moves towards the next waypoint with the given _target_speed, until reaching the final waypoint. Upon reaching the final waypoint, _reached_goal is set to True. If _waypoints is empty, the vehicle moves in its current direction with the given _target_speed. If _consider_obstacles is true, the speed is adapted according to the closest obstacle in front of the actor, if it is within the _proximity_threshold distance. """ if self._cv_image is not None: cv2.imshow("", self._cv_image) cv2.waitKey(1) if self._reached_goal: # Reached the goal, so stop velocity = carla.Vector3D(0, 0, 0) self._actor.set_target_velocity(velocity) return self._reached_goal = False if not self._waypoints: # No waypoints are provided, so we have to create a list of waypoints internally # get next waypoints from map, to avoid leaving the road self._reached_goal = False map_wp = None if not self._generated_waypoint_list: map_wp = CarlaDataProvider.get_map().get_waypoint( CarlaDataProvider.get_location(self._actor)) else: map_wp = CarlaDataProvider.get_map().get_waypoint( self._generated_waypoint_list[-1].location) while len(self._generated_waypoint_list) < 50: map_wps = map_wp.next(3.0) if map_wps: self._generated_waypoint_list.append(map_wps[0].transform) map_wp = map_wps[0] else: break direction_norm = self._set_new_velocity( self._generated_waypoint_list[0].location) if direction_norm < 2.0: self._generated_waypoint_list = self._generated_waypoint_list[ 1:] else: # When changing from "free" driving without pre-defined waypoints to a defined route with waypoints # it may happen that the first few waypoints are too close to the ego vehicle for obtaining a # reasonable control command. Therefore, we drop these waypoints first. while self._waypoints and self._waypoints[0].location.distance( self._actor.get_location()) < 0.5: self._waypoints = self._waypoints[1:] self._reached_goal = False direction_norm = self._set_new_velocity( self._waypoints[0].location) if direction_norm < 4.0: self._waypoints = self._waypoints[1:] if not self._waypoints: self._reached_goal = True
def _set_new_velocity(self, next_location): """ Calculate and set the new actor veloctiy given the current actor location and the _next_location_ If _consider_obstacles is true, the speed is adapted according to the closest obstacle in front of the actor, if it is within the _proximity_threshold distance. If _consider_trafficlights is true, the vehicle will enforce a stop at a red traffic light. If _max_deceleration is set, the vehicle will reduce its speed according to the given deceleration value. If the vehicle reduces its speed, braking lights will be activated. Args: next_location (carla.Location): Next target location of the actor returns: direction (carla.Vector3D): Length of direction vector of the actor """ current_time = GameTime.get_time() target_speed = self._target_speed if not self._last_update: self._last_update = current_time current_speed = math.sqrt(self._actor.get_velocity().x**2 + self._actor.get_velocity().y**2) if self._consider_obstacles: # If distance is less than the proximity threshold, adapt velocity if self._obstacle_distance < self._proximity_threshold: distance = max(self._obstacle_distance, 0) if distance > 0: current_speed_other = math.sqrt( self._obstacle_actor.get_velocity().x**2 + self._obstacle_actor.get_velocity().y**2) if current_speed_other < current_speed: acceleration = -0.5 * (current_speed - current_speed_other)**2 / distance target_speed = max(acceleration * (current_time - self._last_update) + current_speed, 0) else: target_speed = 0 if self._consider_traffic_lights: if (self._actor.is_at_traffic_light() and self._actor.get_traffic_light_state() == carla.TrafficLightState.Red): target_speed = 0 if target_speed < current_speed: if not self._brake_lights_active: self._brake_lights_active = True light_state = self._actor.get_light_state() light_state |= carla.VehicleLightState.Brake self._actor.set_light_state(carla.VehicleLightState(light_state)) if self._max_deceleration is not None: target_speed = max(target_speed, current_speed - (current_time - self._last_update) * self._max_deceleration) else: if self._brake_lights_active: self._brake_lights_active = False light_state = self._actor.get_light_state() light_state &= ~carla.VehicleLightState.Brake self._actor.set_light_state(carla.VehicleLightState(light_state)) if self._max_acceleration is not None: tmp_speed = min(target_speed, current_speed + (current_time - self._last_update) * self._max_acceleration) # If the tmp_speed is < 0.5 the vehicle may not properly accelerate. # Therefore, we bump the speed to 0.5 m/s if target_speed allows. target_speed = max(tmp_speed, min(0.5, target_speed)) # set new linear velocity velocity = carla.Vector3D(0, 0, 0) direction = next_location - CarlaDataProvider.get_location(self._actor) direction_norm = math.sqrt(direction.x**2 + direction.y**2) velocity.x = direction.x / direction_norm * target_speed velocity.y = direction.y / direction_norm * target_speed self._actor.set_target_velocity(velocity) # set new angular velocity current_yaw = CarlaDataProvider.get_transform(self._actor).rotation.yaw # When we have a waypoint list, use the direction between the waypoints to calculate the heading (change) # otherwise use the waypoint heading directly if self._waypoints: delta_yaw = math.degrees(math.atan2(direction.y, direction.x)) - current_yaw else: new_yaw = CarlaDataProvider.get_map().get_waypoint(next_location).transform.rotation.yaw delta_yaw = new_yaw - current_yaw if math.fabs(delta_yaw) > 360: delta_yaw = delta_yaw % 360 if delta_yaw > 180: delta_yaw = delta_yaw - 360 elif delta_yaw < -180: delta_yaw = delta_yaw + 360 angular_velocity = carla.Vector3D(0, 0, 0) if target_speed == 0: angular_velocity.z = 0 else: angular_velocity.z = delta_yaw / (direction_norm / target_speed) self._actor.set_target_angular_velocity(angular_velocity) self._last_update = current_time return direction_norm
def run_step(self): """ Execute on tick of the controller's control loop If _waypoints are provided, the vehicle moves towards the next waypoint with the given _target_speed, until reaching the final waypoint. Upon reaching the final waypoint, _reached_goal is set to True. If _waypoints is empty, the vehicle moves in its current direction with the given _target_speed. For further details see :func:`_set_new_velocity` """ if self._reached_goal: # Reached the goal, so stop velocity = carla.Vector3D(0, 0, 0) self._actor.set_target_velocity(velocity) return if self._visualizer: self._visualizer.render() self._reached_goal = False if not self._waypoints: # No waypoints are provided, so we have to create a list of waypoints internally # get next waypoints from map, to avoid leaving the road self._reached_goal = False map_wp = None if not self._generated_waypoint_list: map_wp = CarlaDataProvider.get_map().get_waypoint(CarlaDataProvider.get_location(self._actor)) else: map_wp = CarlaDataProvider.get_map().get_waypoint(self._generated_waypoint_list[-1].location) while len(self._generated_waypoint_list) < 50: map_wps = map_wp.next(2.0) if map_wps: self._generated_waypoint_list.append(map_wps[0].transform) map_wp = map_wps[0] else: break # Remove all waypoints that are too close to the vehicle while (self._generated_waypoint_list and self._generated_waypoint_list[0].location.distance(self._actor.get_location()) < 0.5): self._generated_waypoint_list = self._generated_waypoint_list[1:] direction_norm = self._set_new_velocity(self._offset_waypoint(self._generated_waypoint_list[0])) if direction_norm < 2.0: self._generated_waypoint_list = self._generated_waypoint_list[1:] else: # When changing from "free" driving without pre-defined waypoints to a defined route with waypoints # it may happen that the first few waypoints are too close to the ego vehicle for obtaining a # reasonable control command. Therefore, we drop these waypoints first. while self._waypoints and self._waypoints[0].location.distance(self._actor.get_location()) < 0.5: self._waypoints = self._waypoints[1:] self._reached_goal = False if not self._waypoints: self._reached_goal = True else: direction_norm = self._set_new_velocity(self._offset_waypoint(self._waypoints[0])) if direction_norm < 4.0: self._waypoints = self._waypoints[1:] if not self._waypoints: self._reached_goal = True
def _initialize_actors(self, config): """ Custom initialization """ ego_location = config.trigger_points[0].location ego_wp = CarlaDataProvider.get_map().get_waypoint(ego_location) # Get the junction starting_wp = ego_wp while not starting_wp.is_junction: starting_wps = starting_wp.next(1.0) if len(starting_wps) == 0: raise ValueError( "Failed to find junction as a waypoint with no next was detected" ) starting_wp = starting_wps[0] junction = starting_wp.get_junction() # Get the opposite entry lane wp possible_directions = ['right', 'left'] self._rng.shuffle(possible_directions) for direction in possible_directions: entry_wps, _ = get_junction_topology(junction) source_entry_wps = filter_junction_wp_direction( starting_wp, entry_wps, direction) if source_entry_wps: self._direction = direction break if not self._direction: raise ValueError( "Trying to find a lane to spawn the opposite actor but none was found" ) # Get the source transform spawn_wp = source_entry_wps[0] added_dist = 0 while added_dist < self._source_dist: spawn_wps = spawn_wp.previous(1.0) if len(spawn_wps) == 0: raise ValueError( "Failed to find a source location as a waypoint with no previous was detected" ) if spawn_wps[0].is_junction: break spawn_wp = spawn_wps[0] added_dist += 1 self._spawn_wp = spawn_wp source_transform = spawn_wp.transform self._spawn_location = carla.Transform( source_transform.location + carla.Location(z=0.1), source_transform.rotation) # Spawn the actor and move it below ground opposite_bp_wildcard = self._rng.choice(self._opposite_bp_wildcards) opposite_actor = CarlaDataProvider.request_new_actor( opposite_bp_wildcard, self._spawn_location) if not opposite_actor: raise Exception("Couldn't spawn the actor") opposite_actor.set_light_state( carla.VehicleLightState(carla.VehicleLightState.Special1 | carla.VehicleLightState.Special2)) self.other_actors.append(opposite_actor) opposite_transform = carla.Transform( source_transform.location - carla.Location(z=500), source_transform.rotation) opposite_actor.set_transform(opposite_transform) opposite_actor.set_simulate_physics(enabled=False) # Get the sink location sink_exit_wp = generate_target_waypoint( self._map.get_waypoint(source_transform.location), 0) sink_wps = sink_exit_wp.next(self._sink_dist) if len(sink_wps) == 0: raise ValueError( "Failed to find a sink location as a waypoint with no next was detected" ) self._sink_wp = sink_wps[0] # get the collision location self._collision_location = get_geometric_linear_intersection( starting_wp.transform.location, source_entry_wps[0].transform.location) if not self._collision_location: raise ValueError("Couldn't find an intersection point") # Get the relevant traffic lights tls = self._world.get_traffic_lights_in_junction(junction.id) ego_tl = get_closest_traffic_light(ego_wp, tls) source_tl = get_closest_traffic_light( self._spawn_wp, tls, ) self._tl_dict = {} for tl in tls: if tl in (ego_tl, source_tl): self._tl_dict[tl] = carla.TrafficLightState.Green else: self._tl_dict[tl] = carla.TrafficLightState.Red
def _initialize_actors(self, config): """ Default initialization of other actors. Override this method in child class to provide custom initialization. """ ego_location = config.trigger_points[0].location ego_wp = CarlaDataProvider.get_map().get_waypoint(ego_location) # Get the junction starting_wp = ego_wp while not starting_wp.is_junction: starting_wps = starting_wp.next(1.0) if len(starting_wps) == 0: raise ValueError( "Failed to find junction as a waypoint with no next was detected" ) starting_wp = starting_wps[0] junction = starting_wp.get_junction() # Get the opposite entry lane wp entry_wps, _ = get_junction_topology(junction) source_entry_wps = filter_junction_wp_direction( starting_wp, entry_wps, self._direction) if not source_entry_wps: raise ValueError( "Trying to find a lane in the {} direction but none was found". format(self._direction)) # Get the source transform source_entry_wp = self._rng.choice(source_entry_wps) # Get the source transform source_wp = source_entry_wp accum_dist = 0 while accum_dist < self._source_dist: source_wps = source_wp.previous(5) if len(source_wps) == 0: raise ValueError( "Failed to find a source location as a waypoint with no previous was detected" ) if source_wps[0].is_junction: break source_wp = source_wps[0] accum_dist += 5 self._source_wp = source_wp source_transform = self._source_wp.transform # Get the sink location sink_exit_wp = generate_target_waypoint( self._map.get_waypoint(source_transform.location), 0) sink_wps = sink_exit_wp.next(self._sink_dist) if len(sink_wps) == 0: raise ValueError( "Failed to find a sink location as a waypoint with no next was detected" ) self._sink_wp = sink_wps[0] # get traffic lights tls = self._world.get_traffic_lights_in_junction(junction.id) ego_tl = get_closest_traffic_light(ego_wp, tls) source_tl = get_closest_traffic_light(self._source_wp, tls) self._flow_tl_dict = {} self._init_tl_dict = {} for tl in tls: if tl == ego_tl: self._flow_tl_dict[tl] = carla.TrafficLightState.Green self._init_tl_dict[tl] = carla.TrafficLightState.Red elif tl == source_tl: self._flow_tl_dict[tl] = carla.TrafficLightState.Green self._init_tl_dict[tl] = carla.TrafficLightState.Green else: self._flow_tl_dict[tl] = carla.TrafficLightState.Red self._init_tl_dict[tl] = carla.TrafficLightState.Red