Пример #1
0
    def _create_behavior(self):
        sequence = py_trees.composites.Sequence("Sequence Behavior")

        parallel = py_trees.composites.Parallel(
            policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL)
        parallel.add_child(StopVehicle(self.ego_vehicles[0], 1))
        parallel.add_child(StopVehicle(self.ego_vehicles[0], 1))
        sequence.add_child(parallel)

        # match all cars speed
        parallel = py_trees.composites.Parallel(
            policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL)
        parallel.add_child(KeepVelocity(self.ego_vehicles[0], 2, distance=7))
        parallel.add_child(KeepVelocity(self.other_actors[0], 2, distance=7))
        sequence.add_child(parallel)

        # turn target speed autopilot for all cars
        parallel = py_trees.composites.Parallel(
            policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL)
        parallel.add_child(
            WaypointFollower(self.ego_vehicles[0],
                             self._speed,
                             avoid_collision=False))
        parallel.add_child(
            WaypointFollower(self.other_actors[0],
                             self._speed,
                             avoid_collision=False))
        sequence.add_child(parallel)

        driveDistance = DriveDistance(self.ego_vehicles[0],
                                      self._drivenDistanceM,
                                      name="DriveDistance")
        sequence.add_child(driveDistance)

        return sequence
Пример #2
0
    def _create_behavior(self):
        """
        The scenario defined after is a "follow leading vehicle" scenario. After
        invoking this scenario, it will wait for the user controlled vehicle to
        enter the start region, then make the other actor to drive towards obstacle.
        Once obstacle clears the road, make the other actor to drive towards the
        next intersection. Finally, the user-controlled vehicle has to be close
        enough to the other actor to end the scenario.
        If this does not happen within 60 seconds, a timeout stops the scenario
        """

        # let the other actor drive until next intersection
        driving_to_next_intersection = py_trees.composites.Parallel(
            "Driving towards Intersection",
            policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)

        obstacle_clear_road = py_trees.composites.Parallel("Obstalce clearing road",
                                                           policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
        obstacle_clear_road.add_child(DriveDistance(self.other_actors[1], 4))
        obstacle_clear_road.add_child(KeepVelocity(self.other_actors[1], self._second_actor_speed))

        stop_near_intersection = py_trees.composites.Parallel(
            "Waiting for end position near Intersection",
            policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
        stop_near_intersection.add_child(WaypointFollower(self.other_actors[0], 10))
        stop_near_intersection.add_child(InTriggerDistanceToNextIntersection(self.other_actors[0], 20))

        driving_to_next_intersection.add_child(WaypointFollower(self.other_actors[0], self._first_actor_speed))
        driving_to_next_intersection.add_child(InTriggerDistanceToVehicle(self.other_actors[1],
                                                                          self.other_actors[0], 15))

        # end condition
        endcondition = py_trees.composites.Parallel("Waiting for end position",
                                                    policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL)
        endcondition_part1 = InTriggerDistanceToVehicle(self.other_actors[0],
                                                        self.ego_vehicles[0],
                                                        distance=20,
                                                        name="FinalDistance")
        endcondition_part2 = StandStill(self.ego_vehicles[0], name="FinalSpeed")
        endcondition.add_child(endcondition_part1)
        endcondition.add_child(endcondition_part2)

        # Build behavior tree
        sequence = py_trees.composites.Sequence("Sequence Behavior")
        sequence.add_child(ActorTransformSetter(self.other_actors[0], self._first_actor_transform))
        sequence.add_child(ActorTransformSetter(self.other_actors[1], self._second_actor_transform))
        sequence.add_child(driving_to_next_intersection)
        sequence.add_child(StopVehicle(self.other_actors[0], self._other_actor_max_brake))
        sequence.add_child(TimeOut(3))
        sequence.add_child(obstacle_clear_road)
        sequence.add_child(stop_near_intersection)
        sequence.add_child(StopVehicle(self.other_actors[0], self._other_actor_max_brake))
        sequence.add_child(endcondition)
        sequence.add_child(ActorDestroy(self.other_actors[0]))
        sequence.add_child(ActorDestroy(self.other_actors[1]))

        return sequence
    def _create_behavior(self):
        """
        The behavior tree returned by this method is as follows:
        The ego vehicle is trying to pass a leading vehicle in the same lane
        by moving onto the oncoming lane while another vehicle is moving in the
        opposite direction in the oncoming lane.
        """

        # Leaf nodes
        actor_source = ActorSource(
            [
                "vehicle.audi.tt", "vehicle.tesla.model3",
                "vehicle.nissan.micra"
            ],
            self._source_transform,
            self._source_gap,
            self._blackboard_queue_name,
        )
        actor_sink = ActorSink(self._sink_location, 10)
        ego_drive_distance = DriveDistance(self.ego_vehicles[0],
                                           self._ego_vehicle_drive_distance)
        waypoint_follower = WaypointFollower(
            self.other_actors[1],
            self._opposite_speed,
            blackboard_queue_name=self._blackboard_queue_name,
            avoid_collision=True,
        )

        pedestrian_walking = KeepVelocity(self.other_actors[2],
                                          1,
                                          distance=300)

        # Non-leaf nodes
        parallel_root = py_trees.composites.Parallel(
            policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)

        # Building tree
        parallel_root.add_child(ego_drive_distance)
        parallel_root.add_child(actor_source)
        parallel_root.add_child(actor_sink)
        parallel_root.add_child(waypoint_follower)
        parallel_root.add_child(pedestrian_walking)

        scenario_sequence = py_trees.composites.Sequence()
        scenario_sequence.add_child(
            ActorTransformSetter(self.other_actors[0],
                                 self._first_actor_transform))
        scenario_sequence.add_child(
            ActorTransformSetter(self.other_actors[1],
                                 self._second_actor_transform))
        # scenario_sequence.add_child(
        #     ActorTransformSetter(self.other_actors[2],
        #                          self._third_actor_transform))
        scenario_sequence.add_child(parallel_root)
        scenario_sequence.add_child(ActorDestroy(self.other_actors[0]))
        scenario_sequence.add_child(ActorDestroy(self.other_actors[1]))
        scenario_sequence.add_child(ActorDestroy(self.other_actors[2]))

        return scenario_sequence
Пример #4
0
    def _create_behavior(self):
        """
        The scenario defined after is a "follow leading vehicle" scenario. After
        invoking this scenario, it will wait for the user controlled vehicle to
        enter the start region, then make the other actor to drive until reaching
        the next intersection. Finally, the user-controlled vehicle has to be close
        enough to the other actor to end the scenario.
        If this does not happen within 60 seconds, a timeout stops the scenario
        """

        # to avoid the other actor blocking traffic, it was spawed elsewhere
        # reset its pose to the required one
        start_transform = ActorTransformSetter(self.other_actors[0],
                                               self._other_actor_transform)

        # let the other actor drive until next intersection
        # @todo: We should add some feedback mechanism to respond to ego_vehicle behavior
        driving_to_next_intersection = py_trees.composites.Parallel(
            "DrivingTowardsIntersection",
            policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)

        driving_to_next_intersection.add_child(
            WaypointFollower(self.other_actors[0], self._first_vehicle_speed))
        driving_to_next_intersection.add_child(
            InTriggerDistanceToNextIntersection(
                self.other_actors[0],
                self._other_actor_stop_in_front_intersection))

        # stop vehicle
        stop = StopVehicle(self.other_actors[0], self._other_actor_max_brake)

        # end condition
        endcondition = py_trees.composites.Parallel(
            "Waiting for end position",
            policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL)
        endcondition_part1 = InTriggerDistanceToVehicle(self.other_actors[0],
                                                        self.ego_vehicles[0],
                                                        distance=20,
                                                        name="FinalDistance")
        endcondition_part2 = StandStill(self.ego_vehicles[0],
                                        name="StandStill",
                                        duration=1)
        endcondition.add_child(endcondition_part1)
        endcondition.add_child(endcondition_part2)

        # Build behavior tree
        sequence = py_trees.composites.Sequence("Sequence Behavior")
        sequence.add_child(start_transform)
        sequence.add_child(driving_to_next_intersection)
        sequence.add_child(stop)
        sequence.add_child(endcondition)
        sequence.add_child(ActorDestroy(self.other_actors[0]))

        return sequence
    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', 'vehicle.audi.tt'],
                                   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)

        # Behavior tree
        root = py_trees.composites.Parallel(
            policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
        root.add_child(wait)
        root.add_child(actor_source)
        root.add_child(actor_sink)
        root.add_child(move_actor)

        sequence.add_child(
            ActorTransformSetter(self.other_actors[0],
                                 self._other_actor_transform))
        sequence.add_child(root)
        sequence.add_child(ActorDestroy(self.other_actors[0]))

        return sequence
Пример #6
0
    def _create_behavior(self):

        # sequence vw
        # make visible
        sequence_vw = py_trees.composites.Sequence("VW T2")
        vw_visible = ActorTransformSetter(self.other_actors[1], self.slow_car_visible)
        sequence_vw.add_child(vw_visible)

        # brake, avoid rolling backwarts
        brake = StopVehicle(self.other_actors[1], self._max_brake)
        sequence_vw.add_child(brake)
        sequence_vw.add_child(Idle())

        # sequence tesla
        # make visible
        sequence_tesla = py_trees.composites.Sequence("Tesla")
        tesla_visible = ActorTransformSetter(self.other_actors[0], self.fast_car_visible)
        sequence_tesla.add_child(tesla_visible)

        # drive fast towards slow vehicle
        just_drive = py_trees.composites.Parallel("DrivingTowardsSlowVehicle",
                                                  policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
        tesla_driving_fast = WaypointFollower(self.other_actors[0], self._fast_vehicle_velocity)
        just_drive.add_child(tesla_driving_fast)
        distance_to_vehicle = InTriggerDistanceToVehicle(
            self.other_actors[1], self.other_actors[0], self._trigger_distance)
        just_drive.add_child(distance_to_vehicle)
        sequence_tesla.add_child(just_drive)

        # change lane
        lane_change_atomic = LaneChange(self.other_actors[0], distance_other_lane=200)
        sequence_tesla.add_child(lane_change_atomic)
        sequence_tesla.add_child(Idle())

        # ego vehicle
        # end condition
        endcondition = py_trees.composites.Parallel("Waiting for end position of ego vehicle",
                                                    policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL)
        endcondition_part1 = InTriggerDistanceToVehicle(self.other_actors[1],
                                                        self.ego_vehicles[0],
                                                        distance=20,
                                                        name="FinalDistance")
        endcondition_part2 = StandStill(self.ego_vehicles[0], name="FinalSpeed")
        endcondition.add_child(endcondition_part1)
        endcondition.add_child(endcondition_part2)

        # build tree
        root = py_trees.composites.Parallel("Parallel Behavior", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
        root.add_child(sequence_vw)
        root.add_child(sequence_tesla)
        root.add_child(endcondition)
        return root
Пример #7
0
    def _create_behavior(self):
        """
        Order of sequence:
        - car_visible: spawn car at a visible transform
        - just_drive: drive until in trigger distance to ego_vehicle
        - accelerate: accelerate to catch up distance to ego_vehicle
        - lane_change: change the lane
        - endcondition: drive for a defined distance
        """

        # car_visible
        behaviour = py_trees.composites.Sequence("CarOn_{}_Lane" .format(self._direction))
        car_visible = ActorTransformSetter(self.other_actors[0], self._transform_visible)
        behaviour.add_child(car_visible)

        # just_drive
        just_drive = py_trees.composites.Parallel(
            "DrivingStraight", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)

        car_driving = WaypointFollower(self.other_actors[0], self._velocity)
        just_drive.add_child(car_driving)

        trigger_distance = InTriggerDistanceToVehicle(
            self.other_actors[0], self.ego_vehicles[0], self._trigger_distance)
        just_drive.add_child(trigger_distance)
        behaviour.add_child(just_drive)

        # accelerate
        accelerate = AccelerateToCatchUp(self.other_actors[0], self.ego_vehicles[0], throttle_value=1,
                                         delta_velocity=self._delta_velocity, trigger_distance=5, max_distance=500)
        behaviour.add_child(accelerate)

        # lane_change
        if self._direction == 'left':
            lane_change = LaneChange(
                self.other_actors[0], speed=None, direction='right', distance_same_lane=5, distance_other_lane=300)
            behaviour.add_child(lane_change)
        else:
            lane_change = LaneChange(
                self.other_actors[0], speed=None, direction='left', distance_same_lane=5, distance_other_lane=300)
            behaviour.add_child(lane_change)

        # endcondition
        endcondition = DriveDistance(self.other_actors[0], 200)

        # build tree
        root = py_trees.composites.Sequence("Behavior", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
        root.add_child(behaviour)
        root.add_child(endcondition)
        return root
Пример #8
0
    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 _create_behavior(self):
        """
        The scenario defined after is a "other leading vehicle" scenario. After
        invoking this scenario, the user controlled vehicle has to drive towards the
        moving other actors, then make the leading actor to decelerate when user controlled
        vehicle is at some close distance. Finally, the user-controlled vehicle has to change
        lane to avoid collision and follow other leading actor in other lane to end the scenario.
        If this does not happen within 90 seconds, a timeout stops the scenario or the ego vehicle
        drives certain distance and stops the scenario.
        """
        # start condition
        driving_in_same_direction = py_trees.composites.Parallel(
            "All actors driving in same direction",
            policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
        leading_actor_sequence_behavior = py_trees.composites.Sequence(
            "Decelerating actor sequence behavior")

        # both actors moving in same direction
        keep_velocity = py_trees.composites.Parallel(
            "Trigger condition for deceleration",
            policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
        keep_velocity.add_child(
            WaypointFollower(self.other_actors[0],
                             self._first_vehicle_speed,
                             avoid_collision=True))
        keep_velocity.add_child(
            InTriggerDistanceToVehicle(self.other_actors[0],
                                       self.ego_vehicles[0], 55))

        # Decelerating actor sequence behavior
        decelerate = self._first_vehicle_speed / 3.2
        leading_actor_sequence_behavior.add_child(keep_velocity)
        leading_actor_sequence_behavior.add_child(
            WaypointFollower(self.other_actors[0],
                             decelerate,
                             avoid_collision=True))
        # end condition
        ego_drive_distance = DriveDistance(self.ego_vehicles[0],
                                           self._ego_vehicle_drive_distance)

        # Build behavior tree
        sequence = py_trees.composites.Sequence("Scenario behavior")
        parallel_root = py_trees.composites.Parallel(
            policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)

        parallel_root.add_child(ego_drive_distance)
        parallel_root.add_child(driving_in_same_direction)
        driving_in_same_direction.add_child(leading_actor_sequence_behavior)
        driving_in_same_direction.add_child(
            WaypointFollower(self.other_actors[1],
                             self._second_vehicle_speed,
                             avoid_collision=True))

        sequence.add_child(
            ActorTransformSetter(self.other_actors[0],
                                 self._first_actor_transform))
        sequence.add_child(
            ActorTransformSetter(self.other_actors[1],
                                 self._second_actor_transform))
        sequence.add_child(parallel_root)
        sequence.add_child(ActorDestroy(self.other_actors[0]))
        sequence.add_child(ActorDestroy(self.other_actors[1]))

        return sequence
Пример #10
0
    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_id = None
                    traffic_light_action = infrastructure_action.find(
                        'SetState')
                    name = traffic_light_action.attrib.get('name')
                    if name.startswith("id="):
                        traffic_light_id = name[3:]
                    elif name.startswith("pos="):
                        position = name[4:]
                        pos = position.split(",")
                        for carla_actor in CarlaDataProvider.get_world(
                        ).get_actors().filter('traffic.traffic_light'):
                            carla_actor_loc = carla_actor.get_transform(
                            ).location
                            distance = carla_actor_loc.distance(
                                carla.Location(x=float(pos[0]),
                                               y=float(pos[1]),
                                               z=carla_actor_loc.z))
                            if distance < 2.0:
                                traffic_light_id = actor.id
                                break
                    if traffic_light_id is None:
                        raise AttributeError(
                            "Unknown  traffic light {}".format(name))
                    traffic_light_state = traffic_light_action.attrib.get(
                        'state')
                    atomic = TrafficLightStateSetter(traffic_light_id,
                                                     traffic_light_state,
                                                     name=maneuver_name + "_" +
                                                     str(traffic_light_id))
                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 convert_maneuver_to_atomic(action, actor, catalogs):
        """
        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('GlobalAction') is not None:
            global_action = action.find('GlobalAction')
            if global_action.find('InfrastructureAction') is not None:
                infrastructure_action = global_action.find('InfrastructureAction').find('TrafficSignalAction')
                if infrastructure_action.find('TrafficSignalStateAction') is not None:
                    traffic_light_id = None
                    traffic_light_action = infrastructure_action.find('TrafficSignalStateAction')
                    name = traffic_light_action.attrib.get('name')
                    if name.startswith("id="):
                        traffic_light_id = name[3:]
                    elif name.startswith("pos="):
                        position = name[4:]
                        pos = position.split(",")
                        for carla_actor in CarlaDataProvider.get_world().get_actors().filter('traffic.traffic_light'):
                            carla_actor_loc = carla_actor.get_transform().location
                            distance = carla_actor_loc.distance(carla.Location(x=float(pos[0]),
                                                                               y=float(pos[1]),
                                                                               z=carla_actor_loc.z))
                            if distance < 2.0:
                                traffic_light_id = carla_actor.id
                                break
                    if traffic_light_id is None:
                        raise AttributeError("Unknown  traffic light {}".format(name))
                    traffic_light_state = traffic_light_action.attrib.get('state')
                    atomic = TrafficLightStateSetter(
                        traffic_light_id, traffic_light_state, name=maneuver_name + "_" + str(traffic_light_id))
                else:
                    raise NotImplementedError("TrafficLights can only be influenced via TrafficSignalStateAction")
            elif global_action.find('EnvironmentAction') is not None:
                weather_behavior = UpdateWeather(
                    OpenScenarioParser.get_weather_from_env_action(global_action, catalogs))
                friction_behavior = UpdateRoadFriction(
                    OpenScenarioParser.get_friction_from_env_action(global_action, catalogs))

                env_behavior = py_trees.composites.Parallel(
                    policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, name=maneuver_name)

                env_behavior.add_child(
                    oneshot_behavior(variable_name=maneuver_name + ">WeatherUpdate", behaviour=weather_behavior))
                env_behavior.add_child(
                    oneshot_behavior(variable_name=maneuver_name + ">FrictionUpdate", behaviour=friction_behavior))

                return env_behavior

            else:
                raise NotImplementedError("Global actions are not yet supported")
        elif action.find('UserDefinedAction') is not None:
            user_defined_action = action.find('UserDefinedAction')
            if user_defined_action.find('CustomCommandAction') is not None:
                command = user_defined_action.find('CustomCommandAction').attrib.get('type')
                atomic = RunScript(command, name=maneuver_name)
        elif action.find('PrivateAction') is not None:
            private_action = action.find('PrivateAction')

            if private_action.find('LongitudinalAction') is not None:
                private_action = private_action.find('LongitudinalAction')

                if private_action.find('SpeedAction') is not None:
                    long_maneuver = private_action.find('SpeedAction')

                    # duration and distance
                    distance = float('inf')
                    duration = float('inf')
                    dimension = long_maneuver.find("SpeedActionDynamics").attrib.get('dynamicsDimension')
                    if dimension == "distance":
                        distance = float(long_maneuver.find("SpeedActionDynamics").attrib.get('value', float("inf")))
                    else:
                        duration = float(long_maneuver.find("SpeedActionDynamics").attrib.get('value', float("inf")))

                    # absolute velocity with given target speed
                    if long_maneuver.find("SpeedActionTarget").find("AbsoluteTargetSpeed") is not None:
                        target_speed = float(long_maneuver.find("SpeedActionTarget").find(
                            "AbsoluteTargetSpeed").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("SpeedActionTarget").find("RelativeTargetSpeed") is not None:
                        relative_speed = long_maneuver.find("SpeedActionTarget").find("RelativeTargetSpeed")
                        obj = relative_speed.attrib.get('entityRef')
                        value = float(relative_speed.attrib.get('value', 0))
                        value_type = relative_speed.attrib.get('speedTargetValueType')
                        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('LongitudinalDistanceAction') is not None:
                    raise NotImplementedError("Longitudinal distance actions are not yet supported")
                else:
                    raise AttributeError("Unknown longitudinal action")
            elif private_action.find('LateralAction') is not None:
                private_action = private_action.find('LateralAction')
                if private_action.find('LaneChangeAction') is not None:
                    # Note: LaneChangeActions are currently only supported for RelativeTargetLane
                    #       with +1 or -1 referring to the action actor
                    lat_maneuver = private_action.find('LaneChangeAction')
                    target_lane_rel = float(lat_maneuver.find("LaneChangeTarget").find(
                        "RelativeTargetLane").attrib.get('value', 0))
                    # duration and distance
                    distance = float('inf')
                    duration = float('inf')
                    dimension = lat_maneuver.find("LaneChangeActionDynamics").attrib.get('dynamicsDimension')
                    if dimension == "distance":
                        distance = float(
                            lat_maneuver.find("LaneChangeActionDynamics").attrib.get('value', float("inf")))
                    else:
                        duration = float(
                            lat_maneuver.find("LaneChangeActionDynamics").attrib.get('value', 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('VisibilityAction') is not None:
                raise NotImplementedError("Visibility actions are not yet supported")
            elif private_action.find('SynchronizeAction') is not None:
                raise NotImplementedError("Synchronization actions are not yet supported")
            elif private_action.find('ActivateControllerAction') is not None:
                private_action = private_action.find('ActivateControllerAction')
                activate = strtobool(private_action.attrib.get('longitudinal'))
                atomic = ChangeAutoPilot(actor, activate, name=maneuver_name)
            elif private_action.find('ControllerAction') is not None:
                raise NotImplementedError("Controller actions are not yet supported")
            elif private_action.find('TeleportAction') is not None:
                position = private_action.find('TeleportAction')
                atomic = ActorTransformSetterToOSCPosition(actor, position, name=maneuver_name)
            elif private_action.find('RoutingAction') is not None:
                target_speed = 5.0
                private_action = private_action.find('RoutingAction')
                if private_action.find('AssignRouteAction') is not None:
                    private_action = private_action.find('AssignRouteAction')
                    if private_action.find('Route') is not None:
                        route = private_action.find('Route')
                        plan = []
                        if route.find('ParameterDeclarations') is not None:
                            if route.find('ParameterDeclarations').find('Parameter') is not None:
                                parameter = route.find('ParameterDeclarations').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)
                            plan.append(transform.location)
                        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('FollowTrajectoryAction') is not None:
                    raise NotImplementedError("Private FollowTrajectory actions are not yet supported")
                elif private_action.find('AcquirePositionAction') 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:
            if action:
                raise AttributeError("Unknown action: {}".format(maneuver_name))
            else:
                return Idle(duration=0, name=maneuver_name)

        return atomic
    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)

        #manipulate traffic light
        manipulate_traffic_light = TrafficLightManipulator(
            self.ego_vehicles[0], "S7right")
        move_actor_parallel.add_child(manipulate_traffic_light)

        # 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
Пример #13
0
    def _create_behavior(self):
        """
        """
        # building the tree
        scenario_sequence = py_trees.composites.Sequence()
        waypoint_events = py_trees.composites.Parallel(
            policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL)
        destroy_actors = py_trees.composites.Sequence()

        reach_destination = InTriggerDistanceToLocation(
            self.ego_vehicles[0], self.customized_data['destination'], 2)

        scenario_sequence.add_child(waypoint_events)
        scenario_sequence.add_child(reach_destination)
        scenario_sequence.add_child(destroy_actors)

        for i in range(len(self.pedestrian_list)):
            pedestrian_actor, pedestrian_generated_transform = self.pedestrian_list[
                i]
            pedestrian_info = self.customized_data['pedestrian_list'][i]

            trigger_distance = InTriggerDistanceToVehicle(
                self.ego_vehicles[0], pedestrian_actor,
                pedestrian_info.trigger_distance)

            movement = py_trees.composites.Parallel(
                policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)

            actor_velocity = KeepVelocity(pedestrian_actor,
                                          pedestrian_info.speed)
            actor_traverse = DriveDistance(pedestrian_actor,
                                           pedestrian_info.dist_to_travel)

            movement.add_child(actor_velocity)
            movement.add_child(actor_traverse)

            if pedestrian_info.after_trigger_behavior == 'destroy':
                after_trigger_behavior = ActorDestroy(pedestrian_actor)
            elif pedestrian_info.after_trigger_behavior == 'stop':
                after_trigger_behavior = StopVehicle(pedestrian_actor,
                                                     brake_value=0.5)
                destroy_actor = ActorDestroy(pedestrian_actor)
                destroy_actors.add_child(destroy_actor)
            else:
                raise

            pedestrian_behaviors = py_trees.composites.Sequence()

            pedestrian_behaviors.add_child(trigger_distance)
            pedestrian_behaviors.add_child(movement)
            pedestrian_behaviors.add_child(after_trigger_behavior)

            waypoint_events.add_child(pedestrian_behaviors)

        for i in range(len(self.vehicle_list)):
            vehicle_actor, generated_transform = self.vehicle_list[i]
            vehicle_info = self.customized_data['vehicle_list'][i]

            keep_velocity = py_trees.composites.Parallel(
                "Trigger condition for changing behavior",
                policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
            keep_velocity.add_child(
                InTriggerDistanceToVehicle(self.ego_vehicles[0], vehicle_actor,
                                           vehicle_info.trigger_distance))
            keep_velocity.add_child(
                WaypointFollower(vehicle_actor,
                                 vehicle_info.initial_speed,
                                 avoid_collision=vehicle_info.avoid_collision))

            if vehicle_info.waypoint_follower:
                # interpolate current location and destination to find a path

                start_location = generated_transform.location
                end_location = vehicle_info.targeted_waypoint.location
                _, route = interpolate_trajectory(
                    self.world, [start_location, end_location])
                ds_ids = downsample_route(
                    route, self.customized_data['sample_factor'])
                route = [(route[x][0], route[x][1]) for x in ds_ids]

                # print('route', len(route))
                perturb_route(route, vehicle_info.waypoints_perturbation)
                # visualize_route(route)

                plan = []
                for transform, cmd in route:
                    wp = self._wmap.get_waypoint(transform.location,
                                                 project_to_road=False,
                                                 lane_type=carla.LaneType.Any)
                    if not wp:
                        wp = self._wmap.get_waypoint(
                            transform.location,
                            project_to_road=True,
                            lane_type=carla.LaneType.Any)
                        print('(', transform.location.x, transform.location.y,
                              ')', 'is replaced by', '(',
                              wp.transform.location.x, wp.transform.location.y,
                              ')')
                    plan.append((wp, cmd))

                movement = WaypointFollower(
                    actor=vehicle_actor,
                    target_speed=vehicle_info.targeted_speed,
                    plan=plan,
                    avoid_collision=vehicle_info.avoid_collision)
            else:
                movement = py_trees.composites.Parallel(
                    policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
                actor_velocity = KeepVelocity(
                    vehicle_actor,
                    vehicle_info.targeted_speed,
                    target_direction=vehicle_info.target_direction)
                actor_traverse = DriveDistance(vehicle_actor,
                                               vehicle_info.dist_to_travel)
                movement.add_child(actor_velocity)
                movement.add_child(actor_traverse)

            if vehicle_info.after_trigger_behavior == 'destroy':
                after_trigger_behavior = ActorDestroy(vehicle_actor)
            elif vehicle_info.after_trigger_behavior == 'stop':
                after_trigger_behavior = StopVehicle(vehicle_actor,
                                                     brake_value=0.5)
                destroy_actor = ActorDestroy(vehicle_actor)
                destroy_actors.add_child(destroy_actor)
            else:
                raise

            vehicle_behaviors = py_trees.composites.Sequence()

            vehicle_behaviors.add_child(keep_velocity)
            vehicle_behaviors.add_child(movement)
            vehicle_behaviors.add_child(after_trigger_behavior)

            waypoint_events.add_child(vehicle_behaviors)

        return scenario_sequence
Пример #14
0
    def _create_behavior(self):
        """
        The scenario defined after is a "follow leading vehicle" scenario. After
        invoking this scenario, it will wait for the user controlled vehicle to
        enter the start region, then make the other actor to drive until reaching
        the next intersection. Finally, the user-controlled vehicle has to be close
        enough to the other actor to end the scenario.
        If this does not happen within 60 seconds, a timeout stops the scenario
        """
        root = py_trees.composites.Parallel(
            "Running random scenario",
            policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)

        root.add_child(StandStill(self.ego_vehicles[0], "scenario ended", 3.0))
        root.add_child(
            InTriggerDistanceToLocation(self.ego_vehicles[0],
                                        carla.Location(200, -250, 0), 10))
        for (scenario, actor_dict) in self.scenario_list:
            if scenario == "VehiclesAhead":
                wait_for_trigger = InTriggerDistanceToLocation(
                    self.ego_vehicles[0],
                    actor_dict.trigger.transform.location, 10)
                start_transform = ActorTransformSetter(
                    self.other_actors[actor_dict.index],
                    actor_dict.start_transform)

                keep_driving = WaypointFollower(
                    self.other_actors[actor_dict.index],
                    actor_dict.speed,
                    avoid_collision=True)

                drive = py_trees.composites.Parallel(
                    "Driving for a distance",
                    policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
                drive_success = DriveDistance(
                    self.other_actors[actor_dict.index],
                    random.randint(50, 100))
                drive.add_child(drive_success)
                drive.add_child(keep_driving)
                stand = StandStill(self.other_actors[actor_dict.index],
                                   "stand still", random.randint(1, 5))
                stop = StopVehicle(self.other_actors[actor_dict.index], 1.0)
                accelerate = AccelerateToVelocity(
                    self.other_actors[actor_dict.index], 1.0, actor_dict.speed)
                # Build behavior tree
                sequence = py_trees.composites.Sequence(
                    "VehiclesAhead behavior sequence")
                sequence.add_child(wait_for_trigger)
                sequence.add_child(start_transform)

                # stop_condition = py_trees.composites.Parallel("stop condition",
                #                             policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
                # stop_condition.add_child(keep_driving)
                # stop_condition.add_child(drive_success)
                if actor_dict.stop:
                    sequence.add_child(drive)
                    sequence.add_child(stop)
                    sequence.add_child(accelerate)
                    sequence.add_child(keep_driving)  #stop_condition
                    print("lead vehicle stop behavior added")
                else:
                    sequence.add_child(keep_driving)  #stop_condition

                sequence.add_child(
                    ActorDestroy(self.other_actors[actor_dict.index]))
                root.add_child(sequence)

            elif scenario == "StationaryObstaclesAhead":
                lane_width = self.ego_vehicles[0].get_world().get_map(
                ).get_waypoint(self.ego_vehicles[0].get_location()).lane_width
                lane_width = lane_width + (1.25 * lane_width)
                sequence = py_trees.composites.Sequence(
                    "StationaryObstaclesAhead behavior sequence")
                # actor_stand = TimeOut(15)
                # actor_removed = ActorDestroy(self.other_actors[num_actors])
                # end_condition = DriveDistance(self.ego_vehicles[0], self._ego_vehicle_distance_driven)
                sequence.add_child(
                    ActorTransformSetter(self.other_actors[actor_dict.index],
                                         actor_dict.start_transform))
                sequence.add_child(TimeOut(120))
                root.add_child(sequence)

            elif scenario == "CutIn":
                sequence = py_trees.composites.Sequence("CarOn_{}_Lane".format(
                    actor_dict.direction))
                car_visible = ActorTransformSetter(
                    self.other_actors[actor_dict.index],
                    actor_dict.start_transform)

                sequence.add_child(
                    InTriggerDistanceToLocation(
                        self.ego_vehicles[0],
                        actor_dict.trigger.transform.location, 10))
                sequence.add_child(car_visible)

                # accelerate
                accelerate = AccelerateToCatchUp(
                    self.other_actors[actor_dict.index],
                    self.ego_vehicles[0],
                    throttle_value=1,
                    delta_velocity=actor_dict.delta_speed,
                    trigger_distance=actor_dict.trigger_distance,
                    max_distance=100)

                sequence.add_child(accelerate)

                # lane_change
                # lane_change = None
                if actor_dict.direction == 'left':
                    lane_change = LaneChange(
                        self.other_actors[actor_dict.index],
                        speed=actor_dict.lane_change_speed,
                        direction='right',
                        distance_same_lane=50,
                        distance_other_lane=10)
                    sequence.add_child(lane_change)
                else:
                    lane_change = LaneChange(
                        self.other_actors[actor_dict.index],
                        speed=actor_dict.lane_change_speed,
                        direction='left',
                        distance_same_lane=50,
                        distance_other_lane=10)
                    sequence.add_child(lane_change)
                sequence.add_child(
                    WaypointFollower(self.other_actors[actor_dict.index],
                                     target_speed=20,
                                     avoid_collision=True))
                endcondition = DriveDistance(
                    self.other_actors[actor_dict.index], 150)
                parallel = py_trees.composites.Sequence(
                    "Behavior",
                    policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
                parallel.add_child(sequence)
                parallel.add_child(endcondition)
                root.add_child(parallel)

            elif scenario == "DynamicObstaclesAhead":
                lane_width = actor_dict.trigger.lane_width
                lane_width = lane_width + (1.25 * lane_width *
                                           actor_dict.num_lane_changes)

                start_condition = InTimeToArrivalToVehicle(
                    self.other_actors[actor_dict.index], self.ego_vehicles[0],
                    actor_dict.time_to_reach)

                # actor_velocity = KeepVelocity(self.other_actors[actor_dict.index],
                #                         actor_dict.speed,
                #                         name="walker velocity")

                # actor_drive = DriveDistance(self.other_actors[actor_dict.index],
                #                         0.5 * lane_width,
                #                         name="walker drive distance")

                actor_start_cross_lane = AccelerateToVelocity(
                    self.other_actors[actor_dict.index],
                    1.0,
                    actor_dict.speed,
                    name="walker crossing lane accelerate velocity")
                actor_cross_lane = DriveDistance(
                    self.other_actors[actor_dict.index],
                    lane_width,
                    name="walker drive distance for lane crossing ")

                actor_stop_cross_lane = StopVehicle(
                    self.other_actors[actor_dict.index],
                    1.0,
                    name="walker stop")

                ego_pass_machine = DriveDistance(
                    self.ego_vehicles[0], 5, name="ego vehicle passed walker")

                # actor_remove = ActorDestroy(self.other_actors[actor_dict.index],
                #                             name="Destroying walker")

                scenario_sequence = py_trees.composites.Sequence()
                keep_velocity_other = py_trees.composites.Parallel(
                    policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE,
                    name="keep velocity other")
                keep_velocity = py_trees.composites.Parallel(
                    policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE,
                    name="keep velocity")

                scenario_sequence.add_child(
                    ActorTransformSetter(self.other_actors[actor_dict.index],
                                         actor_dict.start_transform,
                                         name='TransformSetterTS3walker',
                                         physics=False))
                # scenario_sequence.add_child(HandBrakeVehicle(self.other_actors[actor_dict.index], True))
                scenario_sequence.add_child(start_condition)
                # scenario_sequence.add_child(HandBrakeVehicle(self.other_actors[actor_dict.index], False))
                # scenario_sequence.add_child(keep_velocity)
                scenario_sequence.add_child(keep_velocity_other)
                scenario_sequence.add_child(actor_stop_cross_lane)

                # keep_velocity.add_child(actor_velocity)
                # keep_velocity.add_child(actor_drive)
                keep_velocity_other.add_child(actor_start_cross_lane)
                # keep_velocity_other.add_child(actor_cross_lane)
                # keep_velocity_other.add_child(ego_pass_machine)

                root.add_child(scenario_sequence)

        return root