diff --git a/srunner/scenarios/behavior_trees.py b/srunner/scenarios/behavior_trees.py index da3a83e..7921ce7 100644 --- a/srunner/scenarios/behavior_trees.py +++ b/srunner/scenarios/behavior_trees.py @@ -50,7 +50,7 @@ def create_tree(self): elif self.turn=='right': self.turn_value = 1 plan = generate_target_waypoint(self.map.get_waypoint(self.start_location), self.turn_value) self.behavior = WaypointFollower(self.vehicle, self.vehicle_speed, plan=plan) - return self.behavior + return self.behavior, plan class DriveAndSharpStopAtNextIntersection(): def __init__(self, vehicle, vehicle_speed, distance_from_intersection): @@ -65,7 +65,28 @@ def create_tree(self): self.behavior.add_child(drive_until_intersection_subtree) self.behavior.add_child(StopVehicle(self.vehicle, 2.0)) return self.behavior + + +class DriveAndTurnAtNextIntersectionTwice(): + def __init__(self, vehicle, vehicle_speed, start_location, turn_1='left', turn_2='left'): + self.vehicle = vehicle + self.vehicle_speed = vehicle_speed + self.start_location = start_location + self.turn_1 = turn_1 + self.turn_2 = turn_2 + self.map = CarlaDataProvider.get_map() + self.turn_value_1 = -1 #Default to left turn + self.turn_value_2 = -1 + self.behavior = None + def create_tree(self): + seq = py_trees.composites.Sequence("Sequence Behavior") + behavior_tree_first_turn, plan = DriveAndTurnAtNextIntersection(self.vehicle, self.vehicle_speed, self.start_location, self.turn_1).create_tree() + behavior_tree_second_turn, plan = DriveAndTurnAtNextIntersection(self.vehicle, self.vehicle_speed, plan[-1][0].transform.location, self.turn_2).create_tree() + seq.add_child(behavior_tree_first_turn) + seq.add_child(behavior_tree_second_turn) + return seq + class DriveAndSlowDownAtNextIntersection(): def __init__(self, vehicle, vehicle_speed, slow_down_speed): self.vehicle = vehicle diff --git a/srunner/scenarios/object_crash_intersection.py b/srunner/scenarios/object_crash_intersection.py index a1c3037..6e2b710 100644 --- a/srunner/scenarios/object_crash_intersection.py +++ b/srunner/scenarios/object_crash_intersection.py @@ -153,7 +153,7 @@ def _initialize_actors(self, config): # Move a certain distance to the front start_distance = 8 - waypoint = waypoint.next(start_distance)[0] + waypoint = waypoint[-1][0].next(start_distance)[0] # Get the last driving lane to the right waypoint, self._num_lane_changes = get_right_driving_lane(waypoint) @@ -320,7 +320,7 @@ def _initialize_actors(self, config): # Move a certain distance to the front start_distance = 8 - waypoint = waypoint.next(start_distance)[0] + waypoint = waypoint[-1][0].next(start_distance)[0] # Get the last driving lane to the right waypoint, self._num_lane_changes = get_right_driving_lane(waypoint)