add carla_utils and robo_utils

carla_utils/__init__.py

@@ -0,0 +1,61 @@
+from . import system
+
+import importlib
+carla = importlib.import_module('carla')
+navigation = importlib.import_module('agents.navigation')
+importlib.import_module('agents.navigation.local_planner')
+importlib.import_module('agents.navigation.global_route_planner')
+importlib.import_module('agents.navigation.global_route_planner_dao')
+
+from .system import parse_json_file, parse_yaml_file, parse_yaml_file_unsafe, printVariable, NCQPipe
+from .system import kill_process
+
+from . import basic
+
+'''global'''
+from . import globv
+globv.init()
+
+'''examples'''
+from .examples import NPC
+
+'''augment'''
+from .augment import vector3DToArray
+from .augment import ActorVertices, CollisionCheck
+from .augment import *
+
+from . import trajectory
+
+from .world_map import *
+from .world_map import default_settings
+from .world_map import add_vehicle, add_vehicles
+from .world_map import get_spawn_points
+from .world_map import Role
+from .world_map import kill_all_servers
+from .world_map import get_topology, get_topology_origin
+
+'''sensor'''
+from .sensor import createSensorListMaster, createSensorListMasters, CarlaSensorMaster, CarlaSensorListMaster
+from .sensor import CameraParams, PesudoSensor
+
+from . import sensor
+
+
+'''agents'''
+from .agents import AgentsRoutePlanner, Controller
+from .agents import BaseAgent, BaseAgentObstacle
+from .agents import KeyboardAgent
+from .agents import NaiveAgent, IdmAgent
+from .agents import AgentListMaster
+
+from . import rl_template
+
+'''utils'''
+from .utils import PyGameInteraction
+
+
+'''perception'''
+from . import perception
+
+'''others'''
+from termcolor import cprint

carla_utils/agents/__init__.py

@@ -0,0 +1,18 @@
+
+from .tools import get_leading_agent_unsafe
+
+from .agents_route_planner import AgentsRoutePlanner, get_route_planner
+from .agents_master import AgentListMaster
+
+from .controller import Controller
+from .vehicle_model import RealModel, BicycleModel2D, BicycleModel2DParallel
+
+from .agent_abc import AgentABC
+from .agent_base import BaseAgent
+from .agent_keyboard import KeyboardAgent
+
+from .agent_naive import NaiveAgent
+from .agent_idm import IdmAgent
+
+from .agent_obstacle import BaseAgentObstacle
+

carla_utils/agents/agent_abc.py

@@ -0,0 +1,147 @@
+from carla_utils import carla
+DestroyActor = carla.command.DestroyActor
+
+from abc import ABC
+import threading
+import numpy as np
+
+from ..augment import GlobalPath
+from ..sensor import CarlaSensorListMaster
+from ..world_map import get_reference_route_wrt_waypoint, Core, Role
+from ..system import Clock
+from .controller import Controller
+from .tools import vehicle_wheelbase
+from .vehicle_model import RealModel, SteerModel
+
+
+class AgentABC(ABC):
+    def __init__(self, config, vehicle, sensors_master, global_path):
+        """
+        
+        
+        Args:
+            config: contains decision_frequency, control_frequency,
+                    max_velocity, max_acceleration, min_acceleration,
+                    max_throttle, max_brake, max_steer
+        
+        Returns:
+            None
+        """
+
+        '''config'''
+        self.config = config
+        self.real = config.real
+        self.core: Core = config.get('core', None)
+
+        self.decision_frequency = config.decision_frequency
+        self.control_frequency = config.control_frequency
+        self.skip_num = int(self.control_frequency // self.decision_frequency)
+        assert self.control_frequency % self.decision_frequency == 0
+
+        self.perception_range = float(config.perception_range)
+
+        self.tick_time = 0
+
+        self.clock = Clock(self.control_frequency)
+
+        '''vehicle'''
+        self.world, self.town_map, self.vehicle = self.core.world, self.core.town_map, vehicle
+        self.sensors_master: CarlaSensorListMaster = sensors_master
+        self.global_path: GlobalPath = global_path
+
+        self.id = vehicle.id
+        self.bounding_box = vehicle.bounding_box.extent
+
+        self.role_name: Role = Role.loads(vehicle.attributes['role_name'])
+        self.vi = self.role_name.vi
+
+        self.decision_dt, self.control_dt = 1.0/self.decision_frequency, 1.0/self.control_frequency
+        self.wheelbase = vehicle_wheelbase(self.vehicle)
+        self.controller = Controller(config, self.control_dt, self.wheelbase)
+        self.max_velocity = float(config.get('max_velocity', 8.34))
+        self.max_acceleration = self.controller.max_acceleration
+        self.min_acceleration = self.controller.min_acceleration
+        self.max_throttle = self.controller.max_throttle
+        self.max_brake = self.controller.max_brake
+        self.max_steer = self.controller.max_steer
+        self.max_curvature = np.tan(self.max_steer) / self.wheelbase
+
+        self.steer_model = SteerModel(self.control_dt, alpha=0.0)
+        self.vehicle_model = RealModel()
+        self.init()
+        return
+
+
+    def extend_route(self):
+        waypoint = self.global_path.carla_waypoints[-1]
+        sr = self.global_path.sampling_resolution
+        route = get_reference_route_wrt_waypoint(waypoint, sr, round(3*self.perception_range / sr))
+        self.global_path.extend(route[1:])
+        # self.global_path.draw(self.world, life_time=0)
+
+    def goal_reached(self, preview_distance):
+        return self.global_path.reached(preview_distance)
+
+    def destroy(self):
+        if self.real:
+            self.thread_stoped = True
+            self.thread_control.join()
+        self.sensors_master.destroy()
+        self.vehicle.destroy()
+
+    def destroy_commands(self):
+        '''
+            deprecated
+        '''
+        cmds = self.sensors_master.destroy_commands()
+        cmds.append(DestroyActor(self.vehicle))
+        return cmds
+
+
+    def check_collision(self):
+        return self.sensors_master[('sensor.other.collision', 'default')].get_raw_data() != None
+
+    def check_timeout(self, time_tolerance):
+        self.tick_time += 1
+        return self.tick_time >= time_tolerance
+
+
+    def init(self):
+        if self.real:
+            self.run_step = self._run_step_real
+            print('[{}] Agent is in real mode.'.format(self.__class__.__name__))
+
+            self.target = None
+            self.stop_control = carla.VehicleControl(brake=0.87654)
+            self.thread_stoped = False
+            self.thread_control = threading.Thread(target=self._run_control, args=(lambda: self.thread_stoped,))
+            self.thread_control.start()
+        else:
+            self.run_step = self._run_step_fast
+
+
+    def _run_step_fast(self, reference):
+        target = self.get_target(reference)
+        for _ in range(self.skip_num):
+            if self.goal_reached(self.perception_range *1.2): self.extend_route()
+            control = self.get_control(target)
+            self.forward(control)
+            self.core.tick()
+        return
+
+
+    def _run_step_real(self, reference):
+        self.target = self.get_target(reference)
+        return
+
+    def _run_control(self, stop):
+        while True:
+            self.clock.tick_begin()
+            if self.goal_reached(self.perception_range *1.2): self.extend_route()
+            control = self.get_control(self.target) if self.target != None else self.stop_control
+            self.forward(control)
+            if stop(): break
+            self.clock.tick_end()
+        self.vehicle_model(self.vehicle, self.stop_control)
+        print('[BaseAgent-Real] Exit control thread.')
+        return

carla_utils/agents/agent_base.py

@@ -0,0 +1,35 @@
+from carla_utils import carla
+
+from ..augment import cvt, vector3DNorm
+from .agent_abc import AgentABC
+
+
+class BaseAgent(AgentABC):
+    def get_target(self, reference):
+        return self.max_velocity
+
+    def get_transform(self):
+        return self.vehicle.get_transform()
+    def get_current_v(self):
+        return vector3DNorm(self.vehicle.get_velocity())
+    def get_state(self):
+        current_transform = self.get_transform()
+        current_v = self.get_current_v()
+        return cvt.CUAState.carla_transform(current_transform, v=current_v)
+
+
+    def get_control(self, target):
+        current_transform = self.get_transform()
+        target_waypoint, curvature = self.global_path.target_waypoint(current_transform)
+        # draw_arrow(self.world, target_waypoint.transform, life_time=0.1)
+
+        current_v = self.get_current_v()
+        current_state = cvt.CUAState.carla_transform(current_transform, v=current_v)
+        target_state = cvt.CUAState.carla_transform(target_waypoint.transform, v=target, k=curvature)
+        control = self.controller.run_step(current_state, target_state)
+        return control
+
+    def forward(self, control):
+        control.steer = self.steer_model(control.steer)
+        self.vehicle_model(self.vehicle, control)
+

carla_utils/agents/agent_idm.py

@@ -0,0 +1,88 @@
+from carla_utils import carla
+
+import time
+import numpy as np
+
+from .agent_base import BaseAgent
+
+from .tools import get_leading_agent_unsafe
+from .agent_naive import AgentState
+
+
+class IdmAgent(BaseAgent):
+    def __init__(self, config, vehicle, sensors_master, global_path):
+        BaseAgent.__init__(self, config, vehicle, sensors_master, global_path)
+
+        self.leading_range = 50.0
+        assert self.leading_range <= self.perception_range
+
+        self.desired_speed = self.max_velocity
+        self.time_gap = 1.0  ## 1.6
+        self.min_gap = 2.0
+        self.delta = 4.0
+        self.acceleration = 1.0
+        self.comfortable_deceleration = 1.5  ## 1.7
+
+
+    def get_target(self, reference):
+        """
+        
+        Args:
+            reference: list of BaseAgent
+        
+        Returns:
+            
+        """
+
+        agents = reference
+        self._state = AgentState.NAVIGATING
+
+        # actor_list = self.world.get_actors()
+        # lights_list = actor_list.filter("*traffic_light*")
+        # '''get traffic light'''
+        # light_state, traffic_light = self._is_light_red(lights_list)
+        # if light_state: self._state = AgentState.BLOCKED_RED_LIGHT
+
+        '''get leading agent'''
+        current_transform = self.vehicle.get_transform()
+        reference_waypoints, remaining_distance = self.global_path.remaining_waypoints(current_transform)
+        agent, distance = get_leading_agent_unsafe(self, agents, reference_waypoints, max_distance=self.leading_range)
+
+        if agent != None:
+            self._state = AgentState.BLOCKED_BY_VEHICLE
+
+        if self._state == AgentState.NAVIGATING:
+            target_v = self.max_velocity
+        elif self._state == AgentState.BLOCKED_RED_LIGHT:
+            target_v = -1.0
+        elif self._state == AgentState.BLOCKED_BY_VEHICLE:
+            target_v = self.intelligent_driver_model(agent, distance)
+        else: raise NotImplementedError
+        return target_v
+
+
+    def intelligent_driver_model(self, leading_agent: BaseAgent, leading_distance):
+        distance_c2c = leading_distance
+        length_two_half = leading_agent.vehicle.bounding_box.extent.x + self.vehicle.bounding_box.extent.x
+        distance_b2b = distance_c2c - length_two_half - 0.3   # bumper-to-bumper distance
+        distance_b2b_valid = max(0.001, distance_b2b)
+
+        leading_v = leading_agent.get_current_v()
+        current_v = self.get_current_v()
+        delta_v = current_v - leading_v
+
+        s = current_v*(self.time_gap+delta_v/(2*np.sqrt(self.acceleration*self.comfortable_deceleration)))
+        distance_desired = self.min_gap + max(0, s)
+
+        v_rational = (current_v / self.desired_speed)**self.delta
+        s_rational = (distance_desired / distance_b2b_valid) ** 2
+        acceleration_target = self.acceleration * (1 - v_rational - s_rational)
+        target_v = current_v + acceleration_target / self.decision_frequency
+
+
+        # a = (distance_b2b, (self.min_gap + current_v*self.time_gap) / (1-(current_v/self.desired_speed)**self.delta)*0.5)
+        # print(a, (target_v, current_v), (acceleration_target, 1-v_rational, s_rational) )
+        # print()
+
+        return target_v
+