def _create_behavior(self): """ After invoking this scenario, cyclist will wait for the user controlled vehicle to enter the in the trigger distance region, the cyclist starts crossing the road once the condition meets, ego vehicle has to avoid the crash after a right turn, but continue driving after the road is clear.If this does not happen within 90 seconds, a timeout stops the scenario. """ root = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="IntersectionRightTurn") lane_width = self._reference_waypoint.lane_width lane_width = lane_width + (1.10 * lane_width * self._num_lane_changes) if self._ego_route is not None: trigger_distance = InTriggerDistanceToLocationAlongRoute( self.ego_vehicles[0], self._ego_route, self._other_actor_transform.location, 20) else: trigger_distance = InTriggerDistanceToVehicle( self.other_actors[0], self.ego_vehicles[0], 20) actor_velocity = KeepVelocity(self.other_actors[0], self._other_actor_target_velocity) actor_traverse = DriveDistance(self.other_actors[0], 0.30 * lane_width) post_timer_velocity_actor = KeepVelocity( self.other_actors[0], self._other_actor_target_velocity) post_timer_traverse_actor = DriveDistance(self.other_actors[0], 0.70 * lane_width) end_condition = TimeOut(5) # non leaf nodes scenario_sequence = py_trees.composites.Sequence() actor_ego_sync = py_trees.composites.Parallel( "Synchronization of actor and ego vehicle", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) after_timer_actor = py_trees.composites.Parallel( "After timout actor will cross the remaining lane_width", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) # building the tree root.add_child(scenario_sequence) scenario_sequence.add_child( ActorTransformSetter(self.other_actors[0], self._other_actor_transform, name='TransformSetterTS4')) scenario_sequence.add_child(trigger_distance) scenario_sequence.add_child(actor_ego_sync) scenario_sequence.add_child(after_timer_actor) scenario_sequence.add_child(end_condition) scenario_sequence.add_child(ActorDestroy(self.other_actors[0])) actor_ego_sync.add_child(actor_velocity) actor_ego_sync.add_child(actor_traverse) after_timer_actor.add_child(post_timer_velocity_actor) after_timer_actor.add_child(post_timer_traverse_actor) return root
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
def _create_behavior(self): """ After invoking this scenario, it will wait for the user controlled vehicle to enter the start region, then make a traffic participant to accelerate until it is going fast enough to reach an intersection point. at the same time as the user controlled vehicle at the junction. Once the user controlled vehicle comes close to the junction, the traffic participant accelerates and passes through the junction. After 60 seconds, a timeout stops the scenario. """ # Creating leaf nodes start_other_trigger = InTriggerRegion(self.ego_vehicles[0], -80, -70, -75, -60) sync_arrival = SyncArrival(self.other_actors[0], self.ego_vehicles[0], carla.Location(x=-74.63, y=-136.34)) pass_through_trigger = InTriggerRegion(self.ego_vehicles[0], -90, -70, -124, -119) keep_velocity_other = KeepVelocity(self.other_actors[0], self._other_actor_target_velocity) stop_other_trigger = InTriggerRegion(self.other_actors[0], -45, -35, -140, -130) stop_other = StopVehicle(self.other_actors[0], self._other_actor_max_brake) end_condition = InTriggerRegion(self.ego_vehicles[0], -90, -70, -170, -156) # Creating non-leaf nodes root = py_trees.composites.Sequence() scenario_sequence = py_trees.composites.Sequence() sync_arrival_parallel = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) keep_velocity_other_parallel = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) # Building tree root.add_child(scenario_sequence) scenario_sequence.add_child( ActorTransformSetter(self.other_actors[0], self._other_actor_transform)) scenario_sequence.add_child(start_other_trigger) scenario_sequence.add_child(sync_arrival_parallel) scenario_sequence.add_child(keep_velocity_other_parallel) scenario_sequence.add_child(stop_other) scenario_sequence.add_child(end_condition) scenario_sequence.add_child(ActorDestroy(self.other_actors[0])) sync_arrival_parallel.add_child(sync_arrival) sync_arrival_parallel.add_child(pass_through_trigger) keep_velocity_other_parallel.add_child(keep_velocity_other) keep_velocity_other_parallel.add_child(stop_other_trigger) return root
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
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): """ After invoking this scenario, cyclist will wait for the user controlled vehicle to enter the in the trigger distance region, the cyclist starts crossing the road once the condition meets, ego vehicle has to avoid the crash after a turn, but continue driving after the road is clear.If this does not happen within 90 seconds, a timeout stops the scenario. """ sequence = py_trees.composites.Sequence() collision_location = self._collision_wp.transform.location collision_distance = collision_location.distance(self._adversary_transform.location) collision_duration = collision_distance / self._adversary_speed collision_time_trigger = collision_duration + self._reaction_time # On trigger behavior if self._ego_route is not None: sequence.add_child(Scenario4Manager(self._spawn_dist)) # First waiting behavior sequence.add_child(WaitEndIntersection(self.ego_vehicles[0])) # Adversary trigger behavior trigger_adversary = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="TriggerAdversaryStart") trigger_adversary.add_child(InTimeToArrivalToLocation( self.ego_vehicles[0], collision_time_trigger, collision_location)) trigger_adversary.add_child(InTriggerDistanceToLocation( self.ego_vehicles[0], collision_location, self._min_trigger_dist)) sequence.add_child(trigger_adversary) sequence.add_child(HandBrakeVehicle(self.other_actors[0], False)) # Move the adversary. speed_duration = 2.0 * collision_duration speed_distance = 2.0 * collision_distance sequence.add_child(KeepVelocity( self.other_actors[0], self._adversary_speed, True, speed_duration, speed_distance, name="AdversaryCrossing") ) # Remove everything sequence.add_child(ActorDestroy(self.other_actors[0], name="DestroyAdversary")) sequence.add_child(DriveDistance(self.ego_vehicles[0], self._ego_end_distance, name="EndCondition")) return sequence
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 _create_behavior(self): """ After invoking this scenario, cyclist will wait for the user controlled vehicle to enter trigger distance region, the cyclist starts crossing the road once the condition meets, then after 60 seconds, a timeout stops the scenario """ root = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="OccludedObjectCrossing") lane_width = self._reference_waypoint.lane_width lane_width = lane_width + (1.25 * lane_width * self._num_lane_changes) dist_to_trigger = 12 + self._num_lane_changes # leaf nodes if self._ego_route is not None: start_condition = InTriggerDistanceToLocationAlongRoute( self.ego_vehicles[0], self._ego_route, self.transform.location, dist_to_trigger) else: start_condition = InTimeToArrivalToVehicle(self.other_actors[0], self.ego_vehicles[0], self._time_to_reach) actor_velocity = KeepVelocity(self.other_actors[0], self._other_actor_target_velocity, name="walker velocity") actor_drive = DriveDistance(self.other_actors[0], 0.5 * lane_width, name="walker drive distance") actor_start_cross_lane = AccelerateToVelocity( self.other_actors[0], 1.0, self._other_actor_target_velocity, name="walker crossing lane accelerate velocity") actor_cross_lane = DriveDistance( self.other_actors[0], lane_width, name="walker drive distance for lane crossing ") actor_stop_crossed_lane = StopVehicle(self.other_actors[0], self._other_actor_max_brake, name="walker stop") ego_pass_machine = DriveDistance(self.ego_vehicles[0], 5, name="ego vehicle passed prop") actor_remove = ActorDestroy(self.other_actors[0], name="Destroying walker") static_remove = ActorDestroy(self.other_actors[1], name="Destroying Prop") end_condition = DriveDistance(self.ego_vehicles[0], self._ego_vehicle_distance_driven, name="End condition ego drive distance") # non leaf nodes 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") # building tree root.add_child(scenario_sequence) scenario_sequence.add_child( ActorTransformSetter(self.other_actors[0], self.transform, name='TransformSetterTS3walker', physics=False)) scenario_sequence.add_child( ActorTransformSetter(self.other_actors[1], self.transform2, name='TransformSetterTS3coca', physics=False)) scenario_sequence.add_child( HandBrakeVehicle(self.other_actors[0], True)) scenario_sequence.add_child(start_condition) scenario_sequence.add_child( HandBrakeVehicle(self.other_actors[0], False)) scenario_sequence.add_child(keep_velocity) scenario_sequence.add_child(keep_velocity_other) scenario_sequence.add_child(actor_stop_crossed_lane) scenario_sequence.add_child(actor_remove) scenario_sequence.add_child(static_remove) scenario_sequence.add_child(end_condition) 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) return root
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) move_pedestrian = KeepVelocity(self.other_actors[2], 1, distance=100) move_actor_parallel.add_child(move_pedestrian) # 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( ActorTransformSetter(self.other_actors[1], self._other_actor_transform2)) # sequence.add_child( # ActorTransformSetter(self.other_actors[2], # self._pedestrian_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
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
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"], 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) # actor_source2 = ActorSource(['vehicle.audi.tt'], # self._other_actor_transform2, 10, # self._blackboard_queue_name2) # # destroying flow of actors # actor_sink2 = ActorSink(carla.Location(x=-74.9, y=30), 20) # # follow waypoints untill next intersection # move_actor2 = WaypointFollower( # self.other_actors[1], # self._target_vel, # blackboard_queue_name=self._blackboard_queue_name2, # avoid_collision=True) distance_to_vehicle = InTriggerDistanceToVehicle( self.other_actors[1], self.ego_vehicles[0], 20) actor1_drive = KeepVelocity(self.other_actors[1], 8, distance=90) # Behavior tree root1 = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) # root1.add_child(wait) root1.add_child(actor_source) root1.add_child(actor_sink) root1.add_child(move_actor) # root2 = py_trees.composites.Parallel( # policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) root2 = py_trees.composites.Sequence( "Sequence_dist", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL) # root2.add_child(actor_source2) # root2.add_child(actor_sink2) # root2.add_child(move_actor2) root2.add_child(distance_to_vehicle) root2.add_child(actor1_drive) root = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) root.add_child(root1) root.add_child(root2) root.add_child(wait) sequence.add_child( ActorTransformSetter(self.other_actors[0], self._other_actor_transform)) sequence.add_child(root) sequence.add_child(ActorDestroy(self.other_actors[0])) sequence.add_child(ActorDestroy(self.other_actors[1])) return sequence