Minor improvements

PythonAPI/carla/agents/navigation/basic_agent.py

@@ -12,12 +12,11 @@ import carla
 from enum import Enum
 
 from agents.navigation.agent import Agent
-from agents.navigation.local_planner import LocalPlanner
+from agents.navigation.local_planner import LocalPlanner, RoadOption
 from agents.navigation.global_route_planner import GlobalRoutePlanner
 from agents.tools.misc import get_speed
 
 
-
 class AgentState(Enum):
     """
     AGENT_STATE represents the possible states of a roaming agent
@@ -76,6 +75,12 @@ class BasicAgent(Agent):
         """
         self._local_planner.set_speed(speed)
 
+    def follow_speed_limits(self, value=True):
+        """
+        (De)activates a flag to make the agent dynamically change the target speed according to the speed limits
+        """
+        self._local_planner.follow_speed_limits(value)
+
     def get_local_planner(self):
         """
         Get method for protected member local planner
@@ -104,6 +109,23 @@ class BasicAgent(Agent):
         route_trace = self.trace_route(start_waypoint, end_waypoint)
         self._local_planner.set_global_plan(route_trace, clean_queue=clean_queue)
 
+        for w in route_trace:
+            wp = w[0].transform.location + carla.Location(z=0.1)
+            if w[1] == RoadOption.LEFT:  # Yellow
+                color = carla.Color(255, 255, 0)
+            elif w[1] == RoadOption.RIGHT:  # Cyan
+                color = carla.Color(0, 255, 255)
+            elif w[1] == RoadOption.CHANGELANELEFT:  # Orange
+                color = carla.Color(255, 64, 0)
+            elif w[1] == RoadOption.CHANGELANERIGHT:  # Dark Cyan
+                color = carla.Color(0, 64, 255)
+            elif w[1] == RoadOption.STRAIGHT:  # Gray
+                color = carla.Color(128, 128, 128)
+            else:  # LANEFOLLOW
+                color = carla.Color(0, 255, 0) # Green
+            self._world.debug.draw_point(wp, size=0.08, color=color, life_time=100)
+
+
     def set_global_plan(self, plan, stop_waypoint_creation=True, clean_queue=True):
         """
         Adds a specific plan to the agent.
@@ -127,7 +149,7 @@ class BasicAgent(Agent):
             :param end_waypoint: final position
         """
         if self._grp is None:
-            self._grp = GlobalRoutePlanner(self._map, self._sampling_resolution, self._world)
+            self._grp = GlobalRoutePlanner(self._map, self._sampling_resolution)
         route = self._grp.trace_route(start_waypoint, end_waypoint)
         return route
 

PythonAPI/carla/agents/navigation/behavior_agent.py

@@ -12,9 +12,7 @@ import random
 import numpy as np
 import carla
 from agents.navigation.basic_agent import BasicAgent
-from agents.navigation.local_planner import LocalPlanner, RoadOption
-from agents.navigation.global_route_planner import GlobalRoutePlanner
-from agents.navigation.global_route_planner_dao import GlobalRoutePlannerDAO
+from agents.navigation.local_planner import RoadOption
 from agents.navigation.behavior_types import Cautious, Aggressive, Normal
 
 from agents.tools.misc import get_speed, positive, is_within_distance, compute_distance

PythonAPI/carla/agents/navigation/global_route_planner.py

@@ -24,14 +24,13 @@ class GlobalRoutePlanner(object):
     This class provides a high level route planner.
     """
 
-    def __init__(self, wmap, sampling_resolution, world=None):
+    def __init__(self, wmap, sampling_resolution):
         """
         Constructor
         """
         self._previous_decision = RoadOption.VOID
 
         self._sampling_resolution = sampling_resolution
-        self._world = world  # TODO: remove it after debugging
         self._wmap = wmap
         self._initialize_map()
 
@@ -79,27 +78,11 @@ class GlobalRoutePlanner(object):
         from origin to destination
         """
         route = trace_route(origin, destination, self._wmap, self._sampling_resolution)
+        if not route:
+            return []
 
         # Add options
         route_with_options = self.add_options_to_route(route)
-
-        # TODO: remove it
-        for w in route_with_options:
-            wp = w[0].transform.location + carla.Location(z=0.1)
-            if w[1] == RoadOption.LEFT:  # Yellow
-                color = carla.Color(255, 255, 0)
-            elif w[1] == RoadOption.RIGHT:  # Cyan
-                color = carla.Color(0, 255, 255)
-            elif w[1] == RoadOption.CHANGELANELEFT:  # Orange
-                color = carla.Color(255, 64, 0)
-            elif w[1] == RoadOption.CHANGELANERIGHT:  # Dark Cyan
-                color = carla.Color(0, 64, 255)
-            elif w[1] == RoadOption.STRAIGHT:  # Gray
-                color = carla.Color(128, 128, 128)
-            else:  # LANEFOLLOW
-                color = carla.Color(0, 255, 0) # Green
-            self._world.debug.draw_point(wp, size=0.08, color=color, life_time=100)
-
         return route_with_options
 
     def add_options_to_route(self, route):
@@ -169,8 +152,11 @@ class GlobalRoutePlanner(object):
             route_direction = next_waypoint.transform.location - waypoint.transform.location
             np_route_direction = np.array([route_direction.x, route_direction.y, route_direction.z])
 
-            dot = np.dot(np_direction, np_route_direction)
-            dot /= np.linalg.norm(np_direction) * np.linalg.norm(np_route_direction)
+            if np.linalg.norm(np_route_direction):
+                dot = np.dot(np_direction, np_route_direction)
+                dot /= np.linalg.norm(np_direction) * np.linalg.norm(np_route_direction)
+            else:
+                dot = 1
 
             if 0 < dot < math.cos(math.radians(45)):
                 cross = np.cross(np_direction, np_route_direction)