Merge pull request #644 from Qi-Zha0/three_tasks

Three tasks follow_lane, follow_lane_traffic, follow_route

behavior_metrics/.gitignore

@@ -9,6 +9,7 @@
 bag_analysis/
 core
 tmp_*
+carla_birdeye_view/
 
 
 ### Python ###

behavior_metrics/brains/CARLA/brain_carla_segmentation_based_imitation_learning.py

@@ -1,17 +1,13 @@
-from torchvision import transforms
-from PIL import Image
 from brains.CARLA.utils.pilotnet_onehot import PilotNetOneHot
-from brains.CARLA.utils.test_utils import traffic_light_to_int, model_control
+from brains.CARLA.utils.test_utils import traffic_light_to_int, model_control, calculate_delta_yaw
 from utils.constants import PRETRAINED_MODELS_DIR, ROOT_PATH
+from brains.CARLA.utils.high_level_command import HighLevelCommandLoader
 from os import path
 
 import numpy as np
 
 import torch
-import torchvision
-import cv2
 import time
-import os
 import math
 import carla
 
@@ -27,12 +23,14 @@ def __init__(self, sensors, actuators, model=None, handler=None, config=None):
         self.inference_times = []
         self.gpu_inference = config['GPU']
         self.device = torch.device('cuda' if (torch.cuda.is_available() and self.gpu_inference) else 'cpu')
-
+        self.red_light_counter = 0
+        self.running_light = False
+
         client = carla.Client('localhost', 2000)
-        client.set_timeout(10.0)
+        client.set_timeout(100.0)
         world = client.get_world()
         self.map = world.get_map()
-
+        
         weather = carla.WeatherParameters.ClearNoon
         world.set_weather(weather)
 
@@ -57,8 +55,19 @@ def __init__(self, sensors, actuators, model=None, handler=None, config=None):
                 self.net.load_state_dict(torch.load(PRETRAINED_MODELS + model,map_location=self.device))
                 self.net.eval()
 
-        self.prev_hlc = 0
+        if 'Route' in config:
+            route = config['Route']
+            print('route: ', route)
+        else: 
+            route = None
 
+        self.hlc_loader = HighLevelCommandLoader(self.vehicle, self.map, route=route)
+        self.prev_hlc = 0
+        self.prev_yaw = None
+        self.delta_yaw = 0
+
+        self.target_point = None
+        self.termination_code = 0 # 0: not terminated; 1: arrived at target; 2: wrong turn
 
     def update_frame(self, frame_id, data):
         """Update the information to be shown in one of the GUI's frames.
@@ -91,47 +100,58 @@ def execute(self):
         self.update_frame('frame_0', rgb_image)
         self.update_frame('frame_1', seg_image)
 
+        start_time = time.time()
         try:
             # calculate speed
-            speed_m_s = self.vehicle.get_velocity()
-            speed = 3.6 * math.sqrt(speed_m_s.x**2 + speed_m_s.y**2 + speed_m_s.z**2)
+            velocity = self.vehicle.get_velocity()
+            speed_m_s = np.sqrt(velocity.x**2 + velocity.y**2 + velocity.z**2)
+            speed = 3.6 * speed_m_s #m/s to km/h 
 
-            # randomly choose high-level command if at junction
-            vehicle_location = self.vehicle.get_transform().location
-            vehicle_waypoint = self.map.get_waypoint(vehicle_location)
-            next_to_junction = False
-            for j in range(1, 11):
-                next_waypoint = vehicle_waypoint.next(j * 1.0)[0]
-                if next_waypoint.is_junction:
-                    next_to_junction = True
-                    next_waypoints = vehicle_waypoint.next(j * 1.0)
-                    break
-            if vehicle_waypoint.is_junction or next_to_junction:
+            # read next high-level command or choose a random direction
+            hlc = self.hlc_loader.get_next_hlc()
+            if hlc != 0:
                 if self.prev_hlc == 0:
-                    valid_turns = []
-                    for next_wp in next_waypoints:
-                        yaw_diff = next_wp.transform.rotation.yaw - vehicle_waypoint.transform.rotation.yaw
-                        yaw_diff = (yaw_diff + 180) % 360 - 180
-                        if -15 < yaw_diff < 15:
-                            valid_turns.append(3)  # Go Straight
-                        elif 15 < yaw_diff < 165: 
-                            valid_turns.append(1)  # Turn Left
-                        elif -165 < yaw_diff < -15:
-                            valid_turns.append(2)  # Turn Right
-                    hlc = np.random.choice(valid_turns)
+                    self.prev_yaw = self.vehicle.get_transform().rotation.yaw
                 else:
-                    hlc = self.prev_hlc
-            else:
-                hlc = 0
+                    cur_yaw = self.vehicle.get_transform().rotation.yaw
+                    self.delta_yaw += calculate_delta_yaw(self.prev_yaw, cur_yaw)
+                    self.prev_yaw = cur_yaw
+
+            # detect whether the vehicle made the correct turn
+            turning_infraction = False
+            if self.prev_hlc != 0 and hlc == 0:
+                print(f'turned {self.delta_yaw} degrees')
+                if 45 < np.abs(self.delta_yaw) < 180:
+                    if self.delta_yaw < 0 and self.prev_hlc != 1:
+                        turning_infraction = True
+                    elif self.delta_yaw > 0 and self.prev_hlc != 2:
+                        turning_infraction = True
+                elif self.prev_hlc != 3:
+                    turning_infraction = True
+                if turning_infraction:
+                    print('Wrong Turn!!!')
+                    self.termination_code = 2
+                self.delta_yaw = 0
+
+            self.prev_hlc = hlc
 
             # get traffic light status
             light_status = -1
+            vehicle_location = self.vehicle.get_transform().location
             if self.vehicle.is_at_traffic_light():
                 traffic_light = self.vehicle.get_traffic_light()
                 light_status = traffic_light.get_state()
+                traffic_light_location = traffic_light.get_transform().location
+                distance_to_traffic_light = np.sqrt((vehicle_location.x - traffic_light_location.x)**2 + (vehicle_location.y - traffic_light_location.y)**2)
+                if light_status == carla.libcarla.TrafficLightState.Red and distance_to_traffic_light < 6 and speed_m_s > 5:
+                    if not self.running_light:
+                        self.running_light = True
+                        self.red_light_counter += 1
+                else:
+                    self.running_light = False
 
-            print(f'hlc: {hlc}')
-            print(f'light: {light_status}')
+            print(f'high-level command: {hlc}')
+            #print(f'light: {light_status}')
             frame_data = {
                 'hlc': hlc,
                 'measurements': speed,
@@ -145,8 +165,22 @@ def execute(self):
                                     ignore_traffic_light=False, 
                                     device=self.device, 
                                     combined_control=False)
+
+            self.inference_times.append(time.time() - start_time)
+
             self.motors.sendThrottle(throttle)
             self.motors.sendSteer(steer)
             self.motors.sendBrake(brake)
+
+            # calculate distance to target point
+            # print(f'vehicle location: ({vehicle_location.x}, {-vehicle_location.y})')
+            # print(f'target point: ({self.target_point[0]}, {self.target_point[1]})')
+            if self.target_point != None:
+                distance_to_target = np.sqrt((self.target_point[0] - vehicle_location.x)**2 + (self.target_point[1] - (-vehicle_location.y))**2)
+                print(f'Euclidean distance to target: {distance_to_target}')
+                if distance_to_target < 1.5:
+                    self.termination_code = 1
+
+
         except Exception as err:
             print(err)

behavior_metrics/brains/CARLA/utils/high_level_command.py

@@ -0,0 +1,91 @@
+import numpy as np
+import carla
+
+class HighLevelCommandLoader:
+    def __init__(self, vehicle, map, route=None):
+        self.vehicle = vehicle
+        self.map = map
+        self.prev_hlc = 0
+        self.route = route
+
+    def _get_junction(self):
+        """determine whether vehicle is at junction"""
+        junction = None
+        vehicle_location = self.vehicle.get_transform().location
+        vehicle_waypoint = self.map.get_waypoint(vehicle_location)
+
+        # check whether vehicle is at junction
+        for j in range(1, 11):
+            next_waypoint = vehicle_waypoint.next(j * 1.0)[0]
+            if next_waypoint.is_junction:
+                junction = next_waypoint.get_junction()
+                break
+        if vehicle_waypoint.is_junction:
+            junction = vehicle_waypoint.get_junction()
+        return junction
+
+    def _command_to_int(self, command):
+        commands = {
+            'Left': 1,
+            'Right': 2,
+            'Straight': 3
+        }
+        return commands[command]
+
+    def get_random_hlc(self):
+        """select a random turn at junction"""
+        junction = self._get_junction()
+        vehicle_location = self.vehicle.get_transform().location
+        vehicle_waypoint = self.map.get_waypoint(vehicle_location)
+
+        # randomly select a turning direction
+        if junction is not None:
+            if self.prev_hlc == 0:
+                valid_turns = []
+                waypoints = junction.get_waypoints(carla.LaneType.Driving)
+                for next_wp, _ in waypoints:
+                    yaw_diff = next_wp.transform.rotation.yaw - vehicle_waypoint.transform.rotation.yaw
+                    yaw_diff = (yaw_diff + 180) % 360 - 180 # convert to [-180, 180]
+                    if -15 < yaw_diff < 15:
+                        valid_turns.append(3)  # Go Straight
+                    elif 15 < yaw_diff < 165: 
+                        valid_turns.append(1)  # Turn Left
+                    elif -165 < yaw_diff < -15:
+                        valid_turns.append(2)  # Turn Right
+                hlc = np.random.choice(valid_turns)
+            else:
+                hlc = self.prev_hlc
+        else:
+            hlc = 0
+
+        self.prev_hlc = hlc
+
+        return hlc
+
+
+    def get_next_hlc(self):
+        if self.route is not None and len(self.route) > 0:
+            return self.load_next_hlc()
+        return self.get_random_hlc()
+
+    def load_next_hlc(self):
+        """load the next high level command from pre-defined route"""
+        if self.prev_hlc is None:
+            return None
+
+        junction = self._get_junction()
+
+        if junction is not None:
+            if self.prev_hlc == 0:
+                if len(self.route) == 0:
+                    hlc = None
+                hlc = self._command_to_int(self.route.pop(0))
+            else:
+                hlc = self.prev_hlc
+        else:
+            hlc = 0
+
+        self.prev_hlc = hlc
+
+        return hlc
+

behavior_metrics/brains/CARLA/utils/test_utils.py

@@ -128,4 +128,12 @@ def traffic_light_to_int(light_status):
         carla.libcarla.TrafficLightState.Green: 2,
         carla.libcarla.TrafficLightState.Yellow: 3
     }
-    return light_dict[light_status]
+    return light_dict[light_status]
+
+def calculate_delta_yaw(prev_yaw, cur_yaw):
+    delta_yaw = cur_yaw - prev_yaw
+    if delta_yaw > 180:
+        delta_yaw -= 360
+    elif delta_yaw < -180:
+        delta_yaw += 360
+    return delta_yaw

behavior_metrics/configs/CARLA/CARLA_launch_files/test_suite_template.launch

@@ -0,0 +1,56 @@
+<!-- -->
+<launch>
+  <!-- CARLA connection -->
+  <arg name='host' default='localhost'/>
+  <arg name='port' default='2000'/>
+  <arg name='timeout' default='10'/>
+
+  <!-- Map to load on startup (either a predefined CARLA town (e.g. 'Town01'), or a OpenDRIVE map file) -->
+  <arg name="town"/>
+
+  <!-- Ego vehicle spawn point -->
+<arg name="spawn_point"/>
+
+  <!-- Enable/disable passive mode -->
+  <arg name='passive' default=''/>
+
+  <!-- Synchronous mode-->
+  <arg name='synchronous_mode' default='True'/>
+  <arg name='synchronous_mode_wait_for_vehicle_control_command' default='False'/>
+  <arg name='fixed_delta_seconds' default='0.05'/>
+
+  <!-- the role names of vehicles that are controllable from within ROS-->
+  <arg name='ego_vehicles' default='["ego_vehicle"]'/>
+
+  <!-- launch carla ros bridge node -->
+  <node pkg="carla_ros_bridge" name="carla_ros_bridge" type="bridge.py" output="screen" required="true">
+    <param name="host" value="$(arg host)" unless="$(eval host == '')"/>
+    <param name="port" value="$(arg port)" unless="$(eval port == '')"/>
+    <param name="timeout" value="$(arg timeout)" unless="$(eval timeout == '')"/>
+    <param name="passive" value="$(arg passive)"/>
+    <param name="synchronous_mode" value="$(arg synchronous_mode)"/>
+    <param name="synchronous_mode_wait_for_vehicle_control_command" value="$(arg synchronous_mode_wait_for_vehicle_control_command)"/>
+    <param name="fixed_delta_seconds" value="$(arg fixed_delta_seconds)"/>
+    <param name="register_all_sensors" value="true"/>
+    <param name="town" value="$(arg town)"/>
+    <param name="ego_vehicle_role_name" value="$(arg ego_vehicles)"/>
+  </node>
+
+  <!-- the object json file that configures objects to spawn -->
+  <arg name="objects_definition_file" default='$(env OBJECT_PATH)/main_car_custom_camera.json'/>
+
+  <!-- Ego Vehicle -->
+  <arg name='ego_vehicle_role_name' default='ego_vehicle'/>
+
+  <!-- spawn the objects -->
+  <node pkg="carla_spawn_objects" type="carla_spawn_objects.py" name="$(anon carla_spawn_objects)" output="screen">
+    <param name="objects_definition_file" value="$(arg objects_definition_file)"/>
+    <param name="spawn_point_$(arg ego_vehicle_role_name)" value="$(arg spawn_point)"/>
+    <param name="spawn_sensors_only" value="false"/>
+  </node>
+
+  <include file="$(find carla_manual_control)/launch/carla_manual_control.launch">
+    <arg name='role_name' value='$(arg ego_vehicle_role_name)'/>
+  </include>
+
+</launch>

behavior_metrics/configs/CARLA/CARLA_launch_files/town_02_anticlockwise_imitation_learning.launch

@@ -35,15 +35,16 @@
 
   <!-- the object json file that configures objects to spawn -->
   <arg name="objects_definition_file" default='$(env OBJECT_PATH)/main_car_custom_camera.json'/>
+  <!--<arg name="objects_definition_file" default='$(env OBJECT_PATH)/main_car_custom_camera_tcp_paper.json'/>-->
 
   <!-- Ego Vehicle -->
   <arg name='ego_vehicle_role_name' default='ego_vehicle'/>
-  <arg name="ego_spawn_point" default='55.3, -105.6, 1.37, 0.0, 0.0, 180.0'/>
+  <arg name="spawn_point" default='55.3, -105.6, 1.37, 0.0, 0.0, 180.0'/>
 
   <!-- spawn the objects -->
   <node pkg="carla_spawn_objects" type="carla_spawn_objects.py" name="$(anon carla_spawn_objects)" output="screen">
     <param name="objects_definition_file" value="$(arg objects_definition_file)"/>
-    <param name="spawn_point_$(arg ego_vehicle_role_name)" value="$(arg ego_spawn_point)"/>
+    <param name="spawn_point_$(arg ego_vehicle_role_name)" value="$(arg spawn_point)"/>
     <param name="spawn_sensors_only" value="false"/>
   </node>
 

behavior_metrics/configs/CARLA/default_carla_subjective_vision.yml

@@ -49,7 +49,7 @@ Behaviors:
     Simulation:
         World: configs/CARLA/CARLA_launch_files/town_02_anticlockwise_single_ad_low.launch
         RandomSpawnPoint: False
-        MultiCar: True
+        Task: "follow_lane_traffic"
         NumberOfVehicle: 0
     Dataset:
         In: '/tmp/my_bag.bag'