Improve ped

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)
 

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

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