From f5b212913d3948c3eb6d5a4115503425a23b4f92 Mon Sep 17 00:00:00 2001 From: ishaan95 Date: Fri, 28 Jul 2023 09:54:24 -0700 Subject: [PATCH] drive and turn twice behavior. VehicleTurningRight changes have been added as well. --- srunner/scenarios/behavior_trees.py | 22 ++++++++++++++++++- .../scenarios/object_crash_intersection.py | 4 ++-- 2 files changed, 23 insertions(+), 3 deletions(-) diff --git a/srunner/scenarios/behavior_trees.py b/srunner/scenarios/behavior_trees.py index d7183d2..0919cd7 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,27 @@ 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 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)