diff --git a/srunner/scenariomanager/actorcontrols/npc_vehicle_control.py b/srunner/scenariomanager/actorcontrols/npc_vehicle_control.py index c90d02b2f..a18204d0c 100644 --- a/srunner/scenariomanager/actorcontrols/npc_vehicle_control.py +++ b/srunner/scenariomanager/actorcontrols/npc_vehicle_control.py @@ -9,6 +9,7 @@ This module provides an example control for vehicles """ +from distutils.util import strtobool import math import carla @@ -43,8 +44,38 @@ def __init__(self, actor, args=None): if self._waypoints: self._update_plan() + self._obstacle_sensor = None + self._obstacle_distance = float('inf') + self._obstacle_actor = None + self._consider_obstacles = False + self._proximity_threshold = float('inf') self._brake_lights_active = False + if args and 'consider_obstacles' in args and strtobool(args['consider_obstacles']): + self._consider_obstacles = strtobool(args['consider_obstacles']) + bp = CarlaDataProvider.get_world().get_blueprint_library().find('sensor.other.obstacle') + bp.set_attribute('distance', '250') + if args and 'proximity_threshold' in args: + self._proximity_threshold = float(args['proximity_threshold']) + bp.set_attribute('distance', str(max(float(args['proximity_threshold']), 250))) + bp.set_attribute('hit_radius', '1') + bp.set_attribute('only_dynamics', 'True') + self._obstacle_sensor = CarlaDataProvider.get_world().spawn_actor( + bp, carla.Transform(carla.Location(x=self._actor.bounding_box.extent.x, z=1.0)), attach_to=self._actor) + self._obstacle_sensor.listen(lambda event: self._on_obstacle(event)) # pylint: disable=unnecessary-lambda + + def _on_obstacle(self, event): + """ + Callback for the obstacle sensor + + Sets _obstacle_distance and _obstacle_actor according to the closest obstacle + found by the sensor. + """ + if not event: + return + self._obstacle_distance = event.distance + self._obstacle_actor = event.other_actor + def _update_plan(self): """ Update the plan (waypoint list) of the LocalPlanner @@ -104,6 +135,13 @@ def run_step(self): if target_speed < 0: raise NotImplementedError("Negative target speeds are not yet supported") + # If distance is less than the proximity threshold, adapt velocity + if self._obstacle_distance < self._proximity_threshold: + if "vehicle" in self._obstacle_actor.type_id: + self._obstacle_distance = float('inf') + self._obstacle_actor = None + target_speed = 0 + self._local_planner.set_speed(target_speed * 3.6) control = self._local_planner.run_step(debug=False) diff --git a/srunner/scenariomanager/actorcontrols/pedestrian_control.py b/srunner/scenariomanager/actorcontrols/pedestrian_control.py index cd7554b62..b48c50273 100644 --- a/srunner/scenariomanager/actorcontrols/pedestrian_control.py +++ b/srunner/scenariomanager/actorcontrols/pedestrian_control.py @@ -14,6 +14,7 @@ import carla from srunner.scenariomanager.actorcontrols.basic_control import BasicControl +from srunner.scenariomanager.carla_data_provider import CarlaDataProvider class PedestrianControl(BasicControl): @@ -31,6 +32,17 @@ def __init__(self, actor, args=None): super(PedestrianControl, self).__init__(actor) + bp = CarlaDataProvider.get_world().get_blueprint_library().find('sensor.other.collision') + self._collision_sensor = CarlaDataProvider.get_world().spawn_actor( + bp, carla.Transform(carla.Location(x=self._actor.bounding_box.extent.x, z=1.0)), attach_to=self._actor) + self._collision_sensor.listen(lambda event: self._on_collision(event)) # pylint: disable=unnecessary-lambda + self._colliding_actor = None + + def _on_collision(self, event): + if not event: + return + self._colliding_actor = event.other_actor + def reset(self): """ Reset the controller @@ -66,12 +78,31 @@ def run_step(self): location = self._waypoints[0].location direction = location - self._actor.get_location() direction_norm = math.sqrt(direction.x**2 + direction.y**2) + # It may happen that a pedestrian gets stuck when stepping on a sidewalk + # Use an upwards direction to improve behavior + if self._colliding_actor is not None and self._colliding_actor.type_id == "static.sidewalk": + current_transform = self._actor.get_transform() + new_transform = current_transform + new_transform.location = new_transform.location + carla.Location(z=0.3) + self._actor.set_transform(new_transform) + self._colliding_actor = None + return + #direction = direction + carla.Location(z=0.3) control.direction = direction / direction_norm self._actor.apply_control(control) if direction_norm < 1.0: self._waypoints = self._waypoints[1:] if not self._waypoints: self._reached_goal = True + + #for wpt in self._waypoints: + # begin = wpt.location + carla.Location(z=1.0) + # angle = math.radians(wpt.rotation.yaw) + # end = begin + carla.Location(x=math.cos(angle), y=math.sin(angle)) + # CarlaDataProvider.get_world().debug.draw_arrow(begin, end, arrow_size=0.3, life_time=1.0) + else: control.direction = self._actor.get_transform().rotation.get_forward_vector() self._actor.apply_control(control) + + self._colliding_actor = None diff --git a/srunner/scenariomanager/carla_data_provider.py b/srunner/scenariomanager/carla_data_provider.py index f030aa28f..86955e88d 100644 --- a/srunner/scenariomanager/carla_data_provider.py +++ b/srunner/scenariomanager/carla_data_provider.py @@ -425,7 +425,7 @@ def generate_spawn_points(): CarlaDataProvider._spawn_index = 0 @staticmethod - def create_blueprint(model, rolename='scenario', color=None, actor_category="car", safe=False): + def create_blueprint(model, rolename='scenario', color=None, actor_category="car", actor_args={}, safe=False): """ Function to setup the blueprint of an actor given its model and other relevant parameters """ @@ -489,9 +489,12 @@ def create_blueprint(model, rolename='scenario', color=None, actor_category="car color = CarlaDataProvider._rng.choice(blueprint.get_attribute('color').recommended_values) blueprint.set_attribute('color', color) - # Make pedestrians mortal + # Make actor mortal or invincible if blueprint.has_attribute('is_invincible'): - blueprint.set_attribute('is_invincible', 'false') + if 'invincible' in actor_args and actor_args['invincible'] == "on": + blueprint.set_attribute('is_invincible', 'true') + else: + blueprint.set_attribute('is_invincible', 'false') # Set the rolename if blueprint.has_attribute('role_name'): @@ -536,7 +539,7 @@ def request_new_actor(model, spawn_point, rolename='scenario', autopilot=False, """ This method tries to create a new actor, returning it if successful (None otherwise). """ - blueprint = CarlaDataProvider.create_blueprint(model, rolename, color, actor_category, safe_blueprint) + blueprint = CarlaDataProvider.create_blueprint(model, rolename, color, actor_category, safe=safe_blueprint) if random_location: actor = None @@ -601,7 +604,7 @@ def request_new_actors(actor_list, safe_blueprint=False, tick=True): # Get the blueprint blueprint = CarlaDataProvider.create_blueprint( - actor.model, actor.rolename, actor.color, actor.category, safe_blueprint) + actor.model, actor.rolename, actor.color, actor.category, actor.args, safe_blueprint) # Get the spawn point transform = actor.transform