capable of driving routes consisting of a few locations

custom_basic_agent.py

@@ -0,0 +1,149 @@
+#!/usr/bin/env python
+
+# Modified to produce routes with multiple high level waypoints
+
+# Copyright (c) 2018 Intel Labs.
+# authors: German Ros ([email protected])
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see <https://opensource.org/licenses/MIT>.
+
+""" This module implements an agent that roams around a track following random
+waypoints and avoiding other vehicles.
+The agent also responds to traffic lights. """
+
+import sys
+
+sys.path.append('../carla')
+import carla
+from agents.navigation.agent import Agent, AgentState
+from agents.navigation.local_planner import LocalPlanner
+from agents.navigation.global_route_planner import GlobalRoutePlanner
+from agents.navigation.global_route_planner_dao import GlobalRoutePlannerDAO
+
+class BasicAgent(Agent):
+    """
+    BasicAgent implements a basic agent that navigates scenes to reach a given
+    target destination. This agent respects traffic lights and other vehicles.
+    """
+
+    def __init__(self, vehicle, target_speed=20):
+        """
+
+        :param vehicle: actor to apply to local planner logic onto
+        """
+        super(BasicAgent, self).__init__(vehicle)
+
+        self._proximity_threshold = 10.0  # meters
+        self._state = AgentState.NAVIGATING
+        args_lateral_dict = {
+            'K_P': 1,
+            'K_D': 0.02,
+            'K_I': 0,
+            'dt': 1.0/20.0}
+        self._local_planner = LocalPlanner(
+            self._vehicle, opt_dict={'target_speed' : target_speed,
+            'lateral_control_dict':args_lateral_dict})
+        self._hop_resolution = 2.0
+        self._path_seperation_hop = 2
+        self._path_seperation_threshold = 0.5
+        self._target_speed = target_speed
+        self._grp = None
+
+    def set_destination_list(self, locations):
+        """
+        *** Modified ***
+        This method creates a list of waypoints from agent's position through multiple
+        destination locations based on the route returned by the global router
+        """
+
+        curr_waypoint = self._map.get_waypoint(self._vehicle.get_location())
+
+        # cycle through desired locations and append waypoints for each
+        full_route_trace = []
+        for location in locations:
+            next_waypoint = self._map.get_waypoint(
+                carla.Location(location[0], location[1], location[2]))
+            seg_trace = self._trace_route(curr_waypoint, next_waypoint)
+            assert seg_trace
+            full_route_trace += seg_trace
+            curr_waypoint = next_waypoint
+
+        self._local_planner.set_global_plan(full_route_trace)
+
+    def set_destination(self, location):
+        """
+        This method creates a list of waypoints from agent's position to destination location
+        based on the route returned by the global router
+        """
+
+        start_waypoint = self._map.get_waypoint(self._vehicle.get_location())
+        end_waypoint = self._map.get_waypoint(
+            carla.Location(location[0], location[1], location[2]))
+
+        route_trace = self._trace_route(start_waypoint, end_waypoint)
+        assert route_trace
+
+        self._local_planner.set_global_plan(route_trace)
+
+    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
+        """
+
+        # Setting up global router
+        if self._grp is None:
+            dao = GlobalRoutePlannerDAO(self._vehicle.get_world().get_map(), self._hop_resolution)
+            grp = GlobalRoutePlanner(dao)
+            grp.setup()
+            self._grp = grp
+
+        # Obtain route plan
+        route = self._grp.trace_route(
+            start_waypoint.transform.location,
+            end_waypoint.transform.location)
+
+        return route
+
+    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(debug=debug)
+
+        return control

route_follow.py

@@ -22,11 +22,25 @@
     pass
 
 import carla
+from custom_basic_agent import BasicAgent
 
 import random
 import time
 
 
+LOC_neighborhood_culdesac = [62, 60, 0]
+LOC_town_center = [30, -3, 0]
+LOC_highway_neighborhood_edge = [240, 130, 0] 
+LOC_center_roundabout_north = [20, 0, 0]
+LOC_center_roundabout_south = [-20, 0, 0]
+
+
+ROUTE_neighborhood_highway = [LOC_highway_neighborhood_edge, LOC_neighborhood_culdesac, LOC_highway_neighborhood_edge, LOC_neighborhood_culdesac]
+ROUTE_neighborhood_town_center = [LOC_town_center, LOC_neighborhood_culdesac, LOC_town_center, LOC_neighborhood_culdesac, LOC_town_center]
+
+ROUTE_short = [LOC_center_roundabout_north, LOC_center_roundabout_south, LOC_center_roundabout_north]
+ROUTE = ROUTE_short
+
 def main():
     actor_list = []
 
@@ -50,25 +64,43 @@ def main():
 
         # get the map of the world and the waypoint for the starting location
         world_map = world.get_map()
-        starting_wpt = world_map.get_waypoint(carla.Location(x=30, y=-8, z=0))
+        starting_loc = ROUTE[0]
+        starting_wpt = world_map.get_waypoint(carla.Location(x=starting_loc[0], y=starting_loc[1], z=starting_loc[2]))
 
         # spawn the vehicle at the chosen waypoint and add it to the actor list
         vehicle = world.spawn_actor(bp, starting_wpt.transform)
         actor_list.append(vehicle)
         print('created %s' % vehicle.type_id)
 
-        vehicle.set_autopilot(True)
+        # vehicle.set_autopilot(True)
 
         # move simulation view to center on vehicle, up high
         world.tick()
 
         world_snapshot = world.wait_for_tick()
         actor_snapshot = world_snapshot.find(vehicle.id)
         spectator_transform = actor_snapshot.get_transform()
-        spectator_transform.location += carla.Location(x=0.0, y=0.0, z=100.0) 
+        spectator_transform.location += carla.Location(x=0, y=0, z=100.0) 
+        spectator_transform.rotation.pitch = -89
+        spectator_transform.rotation.yaw = -179 
+        spectator_transform.rotation.roll = -1 
         spectator.set_transform(spectator_transform)
 
-        time.sleep(15)
+        # create a basic agent of the vehicle
+        agent = BasicAgent(vehicle, target_speed =40)
+        agent.set_destination_list(ROUTE)
+
+
+        # drive to waypoints until they are all gone
+        while len(agent._local_planner._waypoints_queue) > 0:
+            world.wait_for_tick(10.0)
+
+            control = agent.run_step()
+            control.manual_gear_shift = False
+            vehicle.apply_control(control)
+
+        print('Finished following waypoints')
+
 
     finally: