diff --git a/PythonAPI/carla/agents/navigation/agent.py b/PythonAPI/carla/agents/navigation/agent.py index f6fcfa3b2..f998880a0 100644 --- a/PythonAPI/carla/agents/navigation/agent.py +++ b/PythonAPI/carla/agents/navigation/agent.py @@ -13,7 +13,10 @@ import math from enum import Enum import carla -from agents.tools.misc import is_within_distance_ahead, is_within_distance, compute_distance +from agents.tools.misc import (is_within_distance_ahead, + is_within_distance, + compute_distance, + get_trafficlight_trigger_location) class AgentState(Enum): """ @@ -27,7 +30,7 @@ class AgentState(Enum): class Agent(object): """Base class to define agents in CARLA""" - def __init__(self, vehicle): + def __init__(self, vehicle, debug=False): """ Constructor method. @@ -45,14 +48,15 @@ class Agent(object): print(' The server could not send the OpenDRIVE (.xodr) file:') print(' Make sure it exists, has the same name of your town, and is correct.') sys.exit(1) + self._debug = debug self._last_traffic_light = None + self._sampling_resolution = 2.0 def get_local_planner(self): """Get method for protected member local planner""" return self._local_planner - @staticmethod - def run_step(debug=False): + def run_step(self): """ Execute one step of navigation. @@ -61,12 +65,19 @@ class Agent(object): """ control = carla.VehicleControl() - if debug: - control.steer = 0.0 - control.throttle = 0.0 - control.brake = 0.0 - control.hand_brake = False - control.manual_gear_shift = False + return control + + def emergency_stop(self): + """ + Send an emergency stop command to the vehicle + + :return: control for braking + """ + control = carla.VehicleControl() + control.steer = 0.0 + control.throttle = 0.0 + control.brake = 1.0 + control.hand_brake = False return control @@ -86,7 +97,7 @@ class Agent(object): ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location) for traffic_light in lights_list: - object_location = self._get_trafficlight_trigger_location(traffic_light) + object_location = get_trafficlight_trigger_location(traffic_light) object_waypoint = self._map.get_waypoint(object_location) if object_waypoint.road_id != ego_vehicle_waypoint.road_id: @@ -107,29 +118,6 @@ class Agent(object): return (False, None) - def _get_trafficlight_trigger_location(self, traffic_light): # pylint: disable=no-self-use - """ - Calculates the yaw of the waypoint that represents the trigger volume of the traffic light - """ - def rotate_point(point, radians): - """ - rotate a given point by a given angle - """ - rotated_x = math.cos(radians) * point.x - math.sin(radians) * point.y - rotated_y = math.sin(radians) * point.x - math.cos(radians) * point.y - - return carla.Vector3D(rotated_x, rotated_y, point.z) - - base_transform = traffic_light.get_transform() - base_rot = base_transform.rotation.yaw - area_loc = base_transform.transform(traffic_light.trigger_volume.location) - area_ext = traffic_light.trigger_volume.extent - - point = rotate_point(carla.Vector3D(0, 0, area_ext.z), math.radians(base_rot)) - point_location = area_loc + carla.Location(x=point.x, y=point.y) - - return carla.Location(point_location.x, point_location.y, point_location.z) - def _bh_is_vehicle_hazard(self, ego_wpt, ego_loc, vehicle_list, proximity_th, up_angle_th, low_angle_th=0, lane_offset=0): """ @@ -233,19 +221,3 @@ class Agent(object): return (True, target_vehicle) return (False, None) - - - @staticmethod - def emergency_stop(): - """ - Send an emergency stop command to the vehicle - - :return: control for braking - """ - control = carla.VehicleControl() - control.steer = 0.0 - control.throttle = 0.0 - control.brake = 1.0 - control.hand_brake = False - - return control diff --git a/PythonAPI/carla/agents/navigation/basic_agent.py b/PythonAPI/carla/agents/navigation/basic_agent.py index 737d4af9f..d6ee74293 100644 --- a/PythonAPI/carla/agents/navigation/basic_agent.py +++ b/PythonAPI/carla/agents/navigation/basic_agent.py @@ -38,7 +38,7 @@ class BasicAgent(Agent): self._local_planner = LocalPlanner( self._vehicle, opt_dict={'target_speed' : target_speed, 'lateral_control_dict':args_lateral_dict}) - self._hop_resolution = 2.0 + self._sampling_resolution = 2.0 self._path_seperation_hop = 2 self._path_seperation_threshold = 0.5 self._target_speed = target_speed @@ -51,22 +51,24 @@ class BasicAgent(Agent): """ start_waypoint = self._map.get_waypoint(self._vehicle.get_location()) - end_waypoint = self._map.get_waypoint( - carla.Location(location[0], location[1], location[2])) + end_waypoint = self._map.get_waypoint(location) - route_trace = self._trace_route(start_waypoint, end_waypoint) + route_trace = self.trace_route(start_waypoint, end_waypoint) self._local_planner.set_global_plan(route_trace) - def _trace_route(self, start_waypoint, end_waypoint): + def trace_route(self, start_waypoint, end_waypoint): """ This method sets up a global router and returns the optimal route from start_waypoint to end_waypoint + + :param start_waypoint: initial position + :param end_waypoint: final position """ # Setting up global router if self._grp is None: - dao = GlobalRoutePlannerDAO(self._vehicle.get_world().get_map(), self._hop_resolution) + dao = GlobalRoutePlannerDAO(self._vehicle.get_world().get_map(), self._sampling_resolution) grp = GlobalRoutePlanner(dao) grp.setup() self._grp = grp diff --git a/PythonAPI/carla/agents/navigation/behavior_agent.py b/PythonAPI/carla/agents/navigation/behavior_agent.py index ea3cc60c5..4a0a95318 100644 --- a/PythonAPI/carla/agents/navigation/behavior_agent.py +++ b/PythonAPI/carla/agents/navigation/behavior_agent.py @@ -16,7 +16,7 @@ from agents.navigation.local_planner_behavior import LocalPlanner from agents.navigation.local_planner import RoadOption from agents.navigation.global_route_planner import GlobalRoutePlanner from agents.navigation.global_route_planner_dao import GlobalRoutePlannerDAO -from agents.navigation.types_behavior import Cautious, Aggressive, Normal +from agents.navigation.behavior_types import Cautious, Aggressive, Normal from agents.tools.misc import get_speed, positive @@ -110,12 +110,10 @@ class BehaviorAgent(Agent): :param end_location: final position :param clean: boolean to clean the waypoint queue """ - if clean: - self._local_planner.waypoints_queue.clear() self.start_waypoint = self._map.get_waypoint(start_location) self.end_waypoint = self._map.get_waypoint(end_location) - route_trace = self._trace_route(self.start_waypoint, self.end_waypoint) + route_trace = self.trace_route(self.start_waypoint, self.end_waypoint) self._local_planner.set_global_plan(route_trace, clean) @@ -135,19 +133,17 @@ class BehaviorAgent(Agent): self.set_destination(new_start, destination) - def _trace_route(self, start_waypoint, end_waypoint): + def trace_route(self, start_waypoint, end_waypoint): """ - This method sets up a global router and returns the - optimal route from start_waypoint to end_waypoint. + This method sets up a global router and returns the optimal route + from start_waypoint to end_waypoint :param start_waypoint: initial position :param end_waypoint: final position """ # Setting up global router if self._grp is None: - wld = self.vehicle.get_world() - dao = GlobalRoutePlannerDAO( - wld.get_map(), sampling_resolution=self._sampling_resolution) + dao = GlobalRoutePlannerDAO(self._vehicle.get_world().get_map(), self._sampling_resolution) grp = GlobalRoutePlanner(dao) grp.setup() self._grp = grp diff --git a/PythonAPI/carla/agents/navigation/types_behavior.py b/PythonAPI/carla/agents/navigation/behavior_types.py similarity index 100% rename from PythonAPI/carla/agents/navigation/types_behavior.py rename to PythonAPI/carla/agents/navigation/behavior_types.py diff --git a/PythonAPI/carla/agents/navigation/roaming_agent.py b/PythonAPI/carla/agents/navigation/roaming_agent.py deleted file mode 100644 index 579afa759..000000000 --- a/PythonAPI/carla/agents/navigation/roaming_agent.py +++ /dev/null @@ -1,74 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2018 Intel Labs. -# authors: German Ros (german.ros@intel.com) -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . - -""" This module implements an agent that roams around a track following random waypoints and avoiding other vehicles. -The agent also responds to traffic lights. """ - -from agents.navigation.agent import Agent, AgentState -from agents.navigation.local_planner import LocalPlanner - - -class RoamingAgent(Agent): - """ - RoamingAgent implements a basic agent that navigates scenes making random - choices when facing an intersection. - - This agent respects traffic lights and other vehicles. - """ - - def __init__(self, vehicle): - """ - - :param vehicle: actor to apply to local planner logic onto - """ - super(RoamingAgent, self).__init__(vehicle) - self._proximity_threshold = 10.0 # meters - self._state = AgentState.NAVIGATING - self._local_planner = LocalPlanner(self._vehicle) - - def run_step(self, debug=False): - """ - Execute one step of navigation. - :return: carla.VehicleControl - """ - - # is there an obstacle in front of us? - hazard_detected = False - - # retrieve relevant elements for safe navigation, i.e.: traffic lights - # and other vehicles - actor_list = self._world.get_actors() - vehicle_list = actor_list.filter("*vehicle*") - lights_list = actor_list.filter("*traffic_light*") - - # check possible obstacles - vehicle_state, vehicle = self._is_vehicle_hazard(vehicle_list) - if vehicle_state: - if debug: - print('!!! VEHICLE BLOCKING AHEAD [{}])'.format(vehicle.id)) - - self._state = AgentState.BLOCKED_BY_VEHICLE - hazard_detected = True - - # check for the state of the traffic lights - light_state, traffic_light = self._is_light_red(lights_list) - if light_state: - if debug: - print('=== RED LIGHT AHEAD [{}])'.format(traffic_light.id)) - - self._state = AgentState.BLOCKED_RED_LIGHT - hazard_detected = True - - if hazard_detected: - control = self.emergency_stop() - else: - self._state = AgentState.NAVIGATING - # standard local planner behavior - control = self._local_planner.run_step() - - return control diff --git a/PythonAPI/carla/agents/tools/misc.py b/PythonAPI/carla/agents/tools/misc.py index 34016fff0..ca5bbb281 100644 --- a/PythonAPI/carla/agents/tools/misc.py +++ b/PythonAPI/carla/agents/tools/misc.py @@ -39,6 +39,29 @@ def get_speed(vehicle): return 3.6 * math.sqrt(vel.x ** 2 + vel.y ** 2 + vel.z ** 2) +def get_trafficlight_trigger_location(traffic_light): + """ + Calculates the yaw of the waypoint that represents the trigger volume of the traffic light + """ + def rotate_point(point, radians): + """ + rotate a given point by a given angle + """ + rotated_x = math.cos(radians) * point.x - math.sin(radians) * point.y + rotated_y = math.sin(radians) * point.x - math.cos(radians) * point.y + + return carla.Vector3D(rotated_x, rotated_y, point.z) + + base_transform = traffic_light.get_transform() + base_rot = base_transform.rotation.yaw + area_loc = base_transform.transform(traffic_light.trigger_volume.location) + area_ext = traffic_light.trigger_volume.extent + + point = rotate_point(carla.Vector3D(0, 0, area_ext.z), math.radians(base_rot)) + point_location = area_loc + carla.Location(x=point.x, y=point.y) + + return carla.Location(point_location.x, point_location.y, point_location.z) + def is_within_distance_ahead(target_transform, current_transform, max_distance): """ Check if a target object is within a certain distance in front of a reference object. diff --git a/PythonAPI/examples/automatic_control.py b/PythonAPI/examples/automatic_control.py index 3bce2effc..92df01308 100755 --- a/PythonAPI/examples/automatic_control.py +++ b/PythonAPI/examples/automatic_control.py @@ -692,9 +692,7 @@ def game_loop(args): elif args.agent == "Basic": agent = BasicAgent(world.player) spawn_point = world.map.get_spawn_points()[0] - agent.set_destination((spawn_point.location.x, - spawn_point.location.y, - spawn_point.location.z)) + agent.set_destination(spawn_point) else: agent = BehaviorAgent(world.player, behavior=args.behavior)