BasicAgent implements a basic agent that navigates scenes to reach a given target destination. This agent respects traffic lights and other vehicles.
| 18 | from agents.navigation.global_route_planner_dao import GlobalRoutePlannerDAO |
| 19 | |
| 20 | class BasicAgent(Agent): |
| 21 | """ |
| 22 | BasicAgent implements a basic agent that navigates scenes to reach a given |
| 23 | target destination. This agent respects traffic lights and other vehicles. |
| 24 | """ |
| 25 | |
| 26 | def __init__(self, vehicle, target_speed=20): |
| 27 | """ |
| 28 | |
| 29 | :param vehicle: actor to apply to local planner logic onto |
| 30 | """ |
| 31 | super(BasicAgent, self).__init__(vehicle) |
| 32 | |
| 33 | self._proximity_threshold = 10.0 # meters |
| 34 | self._state = AgentState.NAVIGATING |
| 35 | args_lateral_dict = { |
| 36 | 'K_P': 1, |
| 37 | 'K_D': 0.02, |
| 38 | 'K_I': 0, |
| 39 | 'dt': 1.0/20.0} |
| 40 | self._local_planner = LocalPlanner( |
| 41 | self._vehicle, opt_dict={'target_speed' : target_speed, |
| 42 | 'lateral_control_dict':args_lateral_dict}) |
| 43 | self._hop_resolution = 2.0 |
| 44 | self._path_seperation_hop = 2 |
| 45 | self._path_seperation_threshold = 0.5 |
| 46 | self._target_speed = target_speed |
| 47 | self._grp = None |
| 48 | |
| 49 | def set_destination(self, location): |
| 50 | """ |
| 51 | This method creates a list of waypoints from agent's position to destination location |
| 52 | based on the route returned by the global router |
| 53 | """ |
| 54 | |
| 55 | start_waypoint = self._map.get_waypoint(self._vehicle.get_location()) |
| 56 | end_waypoint = self._map.get_waypoint( |
| 57 | carla.Location(location[0], location[1], location[2])) |
| 58 | |
| 59 | route_trace = self._trace_route(start_waypoint, end_waypoint) |
| 60 | assert route_trace |
| 61 | |
| 62 | self._local_planner.set_global_plan(route_trace) |
| 63 | |
| 64 | def _trace_route(self, start_waypoint, end_waypoint): |
| 65 | """ |
| 66 | This method sets up a global router and returns the optimal route |
| 67 | from start_waypoint to end_waypoint |
| 68 | """ |
| 69 | |
| 70 | # Setting up global router |
| 71 | if self._grp is None: |
| 72 | dao = GlobalRoutePlannerDAO(self._vehicle.get_world().get_map(), self._hop_resolution) |
| 73 | grp = GlobalRoutePlanner(dao) |
| 74 | grp.setup() |
| 75 | self._grp = grp |
| 76 | |
| 77 | # Obtain route plan |