diff --git a/behavior_metrics/.gitignore b/behavior_metrics/.gitignore
index d6c9882c..af96b108 100644
--- a/behavior_metrics/.gitignore
+++ b/behavior_metrics/.gitignore
@@ -9,6 +9,7 @@
bag_analysis/
core
tmp_*
+carla_birdeye_view/
### Python ###
diff --git a/behavior_metrics/brains/CARLA/brain_carla_segmentation_based_imitation_learning.py b/behavior_metrics/brains/CARLA/brain_carla_segmentation_based_imitation_learning.py
index 2f9e2b61..d45c06d5 100644
--- a/behavior_metrics/brains/CARLA/brain_carla_segmentation_based_imitation_learning.py
+++ b/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)
\ No newline at end of file
diff --git a/behavior_metrics/brains/CARLA/utils/high_level_command.py b/behavior_metrics/brains/CARLA/utils/high_level_command.py
new file mode 100644
index 00000000..e27fb0c0
--- /dev/null
+++ b/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
+
\ No newline at end of file
diff --git a/behavior_metrics/brains/CARLA/utils/test_utils.py b/behavior_metrics/brains/CARLA/utils/test_utils.py
index 1c096138..f67f1af7 100644
--- a/behavior_metrics/brains/CARLA/utils/test_utils.py
+++ b/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]
\ No newline at end of file
+ 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
\ No newline at end of file
diff --git a/behavior_metrics/configs/CARLA/CARLA_launch_files/test_suite_template.launch b/behavior_metrics/configs/CARLA/CARLA_launch_files/test_suite_template.launch
new file mode 100644
index 00000000..8e1996cb
--- /dev/null
+++ b/behavior_metrics/configs/CARLA/CARLA_launch_files/test_suite_template.launch
@@ -0,0 +1,56 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/behavior_metrics/configs/CARLA/CARLA_launch_files/town_02_anticlockwise_imitation_learning.launch b/behavior_metrics/configs/CARLA/CARLA_launch_files/town_02_anticlockwise_imitation_learning.launch
index bbdac71d..f3d9ce7c 100644
--- a/behavior_metrics/configs/CARLA/CARLA_launch_files/town_02_anticlockwise_imitation_learning.launch
+++ b/behavior_metrics/configs/CARLA/CARLA_launch_files/town_02_anticlockwise_imitation_learning.launch
@@ -35,15 +35,16 @@
+
-
+
-
+
diff --git a/behavior_metrics/configs/CARLA/default_carla_subjective_vision.yml b/behavior_metrics/configs/CARLA/default_carla_subjective_vision.yml
index e9df8537..5f239fa6 100644
--- a/behavior_metrics/configs/CARLA/default_carla_subjective_vision.yml
+++ b/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'
diff --git a/behavior_metrics/configs/CARLA/default_carla_test_suite.yml b/behavior_metrics/configs/CARLA/default_carla_test_suite.yml
new file mode 100644
index 00000000..897b0f45
--- /dev/null
+++ b/behavior_metrics/configs/CARLA/default_carla_test_suite.yml
@@ -0,0 +1,79 @@
+Behaviors:
+ Robot:
+ Sensors:
+ Cameras:
+ Camera_0:
+ Name: 'camera_0'
+ Topic: '/carla/ego_vehicle/rgb_front/image'
+ Camera_1:
+ Name: 'camera_1'
+ Topic: '/carla/ego_vehicle/rgb_view/image'
+ Camera_2:
+ Name: 'camera_2'
+ Topic: '/carla/ego_vehicle/semantic_segmentation_front/image'
+ Camera_3:
+ Name: 'camera_3'
+ Topic: '/carla/ego_vehicle/dvs_front/image'
+ Pose3D:
+ Pose3D_0:
+ Name: 'pose3d_0'
+ Topic: '/carla/ego_vehicle/odometry'
+ BirdEyeView:
+ BirdEyeView_0:
+ Name: 'bird_eye_view_0'
+ Topic: ''
+ Speedometer:
+ Speedometer_0:
+ Name: 'speedometer_0'
+ Topic: '/carla/ego_vehicle/speedometer'
+ Actuators:
+ CARLA_Motors:
+ Motors_0:
+ Name: 'motors_0'
+ Topic: '/carla/ego_vehicle/vehicle_control_cmd'
+ MaxV: 3
+ MaxW: 0.3
+ BrainPath: ['brains/CARLA/brain_carla_segmentation_based_imitation_learning.py']
+ PilotTimeCycle: 50
+ AsyncMode: False
+ Parameters:
+ Model: ['pilotnet_v8.0.pth']
+ ImageCropped: False
+ ImageSize: [ 100,50 ]
+ ImageNormalized: True
+ PredictionsNormalized: True
+ GPU: True
+ UseOptimized: False
+ ImageTranform: ''
+ Type: 'CARLA'
+ Experiment:
+ Name: "Carla Town02 Test Suite"
+ Description: "Testing model in Town02 with each route consisting of two turns."
+ UseWorldTimeouts: False
+ Timeout: [500] # for each world!
+ Simulation:
+ Task: "follow_route"
+ World: [configs/CARLA/CARLA_launch_files/test_suite_template.launch]
+ TestSuite: 'Town02_two_turns'
+ RandomizeRoutes: False
+ NumRoutes: 2
+ RandomSpawnPoint: False
+ NumberOfVehicle: 50
+ NumberOfWalker: 0
+ PercentagePedestriansRunning: 0.5
+ PercentagePedestriansCrossing: 0.5
+ Dataset:
+ In: '/tmp/my_bag.bag'
+ Out: ''
+ Stats:
+ Out: './'
+ PerfectLap: './perfect_bags/lap-simple-circuit.bag'
+ Layout:
+ Frame_0:
+ Name: frame_0
+ Geometry: [0, 1, 1, 2]
+ Data: rgbimage
+ Frame_1:
+ Name: frame_1
+ Geometry: [1, 1, 1, 2]
+ Data: rgbimage
\ No newline at end of file
diff --git a/behavior_metrics/configs/CARLA/default_carla_torch.yml b/behavior_metrics/configs/CARLA/default_carla_torch.yml
index 6f9865d2..1cc1148e 100644
--- a/behavior_metrics/configs/CARLA/default_carla_torch.yml
+++ b/behavior_metrics/configs/CARLA/default_carla_torch.yml
@@ -33,11 +33,11 @@ Behaviors:
Topic: '/carla/ego_vehicle/vehicle_control_cmd'
MaxV: 3
MaxW: 0.3
- BrainPath: 'brains/CARLA/pytorch/brain_carla_bird_eye_deep_learning_torch.py'
+ BrainPath: 'brains/CARLA/brain_carla_segmentation_based_imitation_learning.py'
PilotTimeCycle: 50
AsyncMode: True
Parameters:
- Model: 'pilotnet_model.pth'
+ Model: 'pilotnet_v8.0.pth'
ImageCropped: True
ImageSize: [ 100,50 ]
ImageNormalized: True
@@ -47,7 +47,7 @@ Behaviors:
ImageTranform: ''
Type: 'CARLA'
Simulation:
- World: configs/CARLA/CARLA_launch_files/town_02_anticlockwise.launch
+ World: configs/CARLA/CARLA_launch_files/town_02_anticlockwise_imitation_learning.launch
RandomSpawnPoint: False
Dataset:
In: '/tmp/my_bag.bag'
diff --git a/behavior_metrics/configs/CARLA/default_carla_torch_segmentation_based_IM.yml b/behavior_metrics/configs/CARLA/default_carla_torch_segmentation_based_IM.yml
new file mode 100644
index 00000000..1cc1148e
--- /dev/null
+++ b/behavior_metrics/configs/CARLA/default_carla_torch_segmentation_based_IM.yml
@@ -0,0 +1,74 @@
+Behaviors:
+ Robot:
+ Sensors:
+ Cameras:
+ Camera_0:
+ Name: 'camera_0'
+ Topic: '/carla/ego_vehicle/rgb_front/image'
+ Camera_1:
+ Name: 'camera_1'
+ Topic: '/carla/ego_vehicle/rgb_view/image'
+ Camera_2:
+ Name: 'camera_2'
+ Topic: '/carla/ego_vehicle/semantic_segmentation_front/image'
+ Camera_3:
+ Name: 'camera_3'
+ Topic: '/carla/ego_vehicle/dvs_front/image'
+ Pose3D:
+ Pose3D_0:
+ Name: 'pose3d_0'
+ Topic: '/carla/ego_vehicle/odometry'
+ BirdEyeView:
+ BirdEyeView_0:
+ Name: 'bird_eye_view_0'
+ Topic: ''
+ Speedometer:
+ Speedometer_0:
+ Name: 'speedometer_0'
+ Topic: '/carla/ego_vehicle/speedometer'
+ Actuators:
+ CARLA_Motors:
+ Motors_0:
+ Name: 'motors_0'
+ Topic: '/carla/ego_vehicle/vehicle_control_cmd'
+ MaxV: 3
+ MaxW: 0.3
+ BrainPath: 'brains/CARLA/brain_carla_segmentation_based_imitation_learning.py'
+ PilotTimeCycle: 50
+ AsyncMode: True
+ Parameters:
+ Model: 'pilotnet_v8.0.pth'
+ ImageCropped: True
+ ImageSize: [ 100,50 ]
+ ImageNormalized: True
+ PredictionsNormalized: True
+ GPU: True
+ UseOptimized: False
+ ImageTranform: ''
+ Type: 'CARLA'
+ Simulation:
+ World: configs/CARLA/CARLA_launch_files/town_02_anticlockwise_imitation_learning.launch
+ RandomSpawnPoint: False
+ Dataset:
+ In: '/tmp/my_bag.bag'
+ Out: ''
+ Stats:
+ Out: './'
+ PerfectLap: './perfect_bags/lap-simple-circuit.bag'
+ Layout:
+ Frame_0:
+ Name: frame_0
+ Geometry: [1, 1, 1, 1]
+ Data: rgbimage
+ Frame_1:
+ Name: frame_1
+ Geometry: [0, 1, 1, 1]
+ Data: rgbimage
+ Frame_2:
+ Name: frame_2
+ Geometry: [0, 2, 1, 1]
+ Data: rgbimage
+ Frame_3:
+ Name: frame_3
+ Geometry: [1, 2, 1, 1]
+ Data: rgbimage
diff --git a/behavior_metrics/configs/CARLA/test_suites/Town02_two_turns.py b/behavior_metrics/configs/CARLA/test_suites/Town02_two_turns.py
new file mode 100644
index 00000000..8ce91401
--- /dev/null
+++ b/behavior_metrics/configs/CARLA/test_suites/Town02_two_turns.py
@@ -0,0 +1,91 @@
+TEST_ROUTES = [
+ {
+ "map": "Town02",
+ "start": "-3.68, -288.22, 0.5, 0.0, 0.0, 90.0",
+ "end": "41.39, -212.98, 0.5, 0.0, 0.0, -90.0",
+ "distance": 158,
+ "commands": ["Right", "Right"]
+ },
+ {
+ "map": "Town02",
+ "start":"136.11, -302.57, 0.5, 0.0, 0.0, 180.0",
+ "end":"45.89, -216.26, 0.5, 0.0, 0.0, 90.0",
+ "distance": 174,
+ "commands": ["Right", "Straight"]
+ },
+ {
+ "map": "Town02",
+ "start": "45.24, -225.59, 0.5, 0.0, 0.0, 90",
+ "end": "-7.53, -270.73, 0.5, 0.0, 0.0, -90.0",
+ "distance": 166,
+ "commands": ["Left", "Left"]
+ },
+ {
+ "map": "Town02",
+ "start": "41.39, -203.90, 0.5, 0.0, 0.0, -90.0",
+ "end": "136.11, -306.42, 0.5, 0.0, 0.0, 0.0",
+ "distance": 193,
+ "commands": ["Straight", "Left"]
+ },
+ {
+ "map": "Town02",
+ "start": "59.71, -241.28, 0.5, 0.0, 0.0, 0.0",
+ "end": "166.91, -191.77, 0.5, 0.0, 0.0, 0.0",
+ "distance": 148,
+ "commands": ["Left", "Right"]
+ },
+ {
+ "map": "Town02",
+ "start": "165.09, -187.12, 0.5, 0.0, 0.0, 180.0",
+ "end": "71.04, -237.43, 0.5, 0.0, 0.0, 180.0",
+ "distance": 136,
+ "commands": ["Left", "Right"]
+ },
+ {
+ "map": "Town02",
+ "start": "59.60, -306.42, 0.5, 0.0, 0.0, 0.0",
+ "end": "165.09, -187.12, 0.5, 0.0, 0.0, 180.0",
+ "distance": 272,
+ "commands": ["Straight", "Left"]
+ },
+ {
+ "map": "Town02",
+ "start": "166.91, -191.77, 0.5, 0.0, 0.0, 0.0",
+ "end": "71.53, -302.57, 0.5, 0.0, 0.0, 180.0",
+ "distance": 245,
+ "commands": ["Right", "Straight"]
+ },
+ {
+ "map": "Town02",
+ "start": "92.34, -187.92, 0.5, 0.0, 0.0, 180.0",
+ "end": "84.41, -109.30, 0.5, 0.0, 0.0, 0.0",
+ "distance": 255,
+ "commands": ["Straight", "Right"]
+ },
+ {
+ "map": "Town02",
+ "start": "84.41, -105.55, 0.5, 0.0, 0.0, 180.0",
+ "end": "92.34, -191.77, 0.5, 0.0, 0.0, 0.0",
+ "distance": 269,
+ "commands": ["Left", "Straight"]
+ },
+ {
+ "map": "Town02",
+ "start": "135.88, -226.05, 0.5, 0.0, 0.0, 90.0",
+ "end": "193.78, -121.21, 0.5, 0.0, 0.0, 90.0",
+ "distance": 155,
+ "commands": ["Right", "Left"]
+ },
+ {
+ "map": "Town02",
+ "start": "189.93, -121.21, 0.5, 0.0, 0.0, -90.0",
+ "end": "132.03, -211.0, 0.5, 0.0, 0.0, -90.0",
+ "distance": 140,
+ "commands": ["Right", "Left"]
+ }
+]
+
+if __name__ == "__main__":
+ # Quick test to see if the routes are loaded correctly
+ for route in TEST_ROUTES:
+ print(route)
\ No newline at end of file
diff --git a/behavior_metrics/driver_carla.py b/behavior_metrics/driver_carla.py
index 94a721d3..0f9c2ed7 100644
--- a/behavior_metrics/driver_carla.py
+++ b/behavior_metrics/driver_carla.py
@@ -296,6 +296,9 @@ def main():
config_data = check_args(sys.argv)
app_configuration = Config(config_data['config'][0])
if not config_data['script']:
+ if app_configuration.task not in ['follow_lane', 'follow_lane_traffic']:
+ logger.info('Selected task does not support --gui. Try use --script instead. Killing program...')
+ sys.exit(-1)
environment.launch_env(app_configuration.current_world, random_spawn_point=app_configuration.experiment_random_spawn_point, carla_simulator=True)
controller = ControllerCarla()
traffic_manager = TrafficManager(app_configuration.number_of_vehicle,
@@ -321,53 +324,97 @@ def main():
environment.close_ros_and_simulators()
else:
if is_config_correct(app_configuration):
- experiments_starting_time = time.time()
- experiment_counter = 0
- experiments_elapsed_times = {'experiment_counter': [], 'elapsed_time': []}
- experiments_information = {'world_counter': {}}
- for world_counter, world in enumerate(app_configuration.current_world):
- experiments_information['world_counter'][world_counter] = {'brain_counter': {}}
- for brain_counter, brain in enumerate(app_configuration.brain_path):
- experiments_information['world_counter'][world_counter]['brain_counter'][brain_counter] = {'repetition_counter': {}}
- for repetition_counter in range(app_configuration.experiment_repetitions):
- success = -1
- experiment_attempts = 0
- while success != 0:
- experiments_information['world_counter'][world_counter]['brain_counter'][brain_counter]['repetition_counter'][repetition_counter] = experiment_attempts
- logger.info("Launching: python3 script_manager_carla.py -c " + config_data['config'][0] + " -s -world_counter " + str(world_counter) + " -brain_counter " + str(brain_counter) + " -repetition_counter " + str(repetition_counter))
- logger.info("Experiment attempt: " + str(experiment_attempts+1))
- current_experiment_starting_time = time.time()
- success = os.system("python3 script_manager_carla.py -c " + config_data['config'][0] + " -s -world_counter " + str(world_counter) + " -brain_counter " + str(brain_counter) + " -repetition_counter " + str(repetition_counter))
- if success != 0:
- root = './'
- folders = list(os.walk(root))[1:]
- for folder in folders:
- if len(folder[0].split('/')) == 2 and not folder[1] and not folder[2]:
- logger.info("Removing empty folder: " + folder[0])
- os.rmdir(folder[0])
- if success == 2:
- logger.info('KeyboardInterrupt called! Killing program...')
- sys.exit(-1)
- elif success != 0 and experiment_attempts < 5:
- experiment_attempts += 1
- logger.info("Python process finished with error! Repeating experiment")
- elif success != 0 and experiment_attempts >= 5:
- success = 0
- logger.info("Too many failed attempts for this experiment.")
- else:
- experiments_elapsed_times['experiment_counter'].append(experiment_counter)
- experiments_elapsed_times['elapsed_time'].append(time.time() - current_experiment_starting_time)
- experiment_counter += 1
- logger.info("Python process finished.")
-
- logger.info('Experiments information: ')
- logger.info(experiments_information)
- logger.info('Last experiment folder: ')
- logger.info(max(glob.glob(os.path.join('./', '*/')), key=os.path.getmtime))
-
+ if app_configuration.task == 'follow_lane':
+ experiments_starting_time = time.time()
+ experiment_counter = 0
+ experiments_elapsed_times = {'experiment_counter': [], 'elapsed_time': []}
+ experiments_information = {'world_counter': {}}
+ for world_counter, world in enumerate(app_configuration.current_world):
+ experiments_information['world_counter'][world_counter] = {'brain_counter': {}}
+ for brain_counter, brain in enumerate(app_configuration.brain_path):
+ experiments_information['world_counter'][world_counter]['brain_counter'][brain_counter] = {'repetition_counter': {}}
+ for repetition_counter in range(app_configuration.experiment_repetitions):
+ success = -1
+ experiment_attempts = 0
+ while success != 0:
+ experiments_information['world_counter'][world_counter]['brain_counter'][brain_counter]['repetition_counter'][repetition_counter] = experiment_attempts
+ logger.info("Launching: python3 script_manager_carla.py -c " + config_data['config'][0] + " -s -world_counter " + str(world_counter) + " -brain_counter " + str(brain_counter) + " -repetition_counter " + str(repetition_counter))
+ logger.info("Experiment attempt: " + str(experiment_attempts+1))
+ current_experiment_starting_time = time.time()
+ success = os.system("python3 script_manager_carla.py -c " + config_data['config'][0] + " -s -world_counter " + str(world_counter) + " -brain_counter " + str(brain_counter) + " -repetition_counter " + str(repetition_counter))
+ if success != 0:
+ root = './'
+ folders = list(os.walk(root))[1:]
+ for folder in folders:
+ if len(folder[0].split('/')) == 2 and not folder[1] and not folder[2]:
+ logger.info("Removing empty folder: " + folder[0])
+ os.rmdir(folder[0])
+ if success == 2:
+ logger.info('KeyboardInterrupt called! Killing program...')
+ sys.exit(-1)
+ elif success != 0 and experiment_attempts < 5:
+ experiment_attempts += 1
+ logger.info("Python process finished with error! Repeating experiment")
+ elif success != 0 and experiment_attempts >= 5:
+ success = 0
+ logger.info("Too many failed attempts for this experiment.")
+ else:
+ experiments_elapsed_times['experiment_counter'].append(experiment_counter)
+ experiments_elapsed_times['elapsed_time'].append(time.time() - current_experiment_starting_time)
+ experiment_counter += 1
+ logger.info("Python process finished.")
+
+ logger.info('Experiments information: ')
+ logger.info(experiments_information)
+ logger.info('Last experiment folder: ')
+ logger.info(max(glob.glob(os.path.join('./', '*/')), key=os.path.getmtime))
+ elif app_configuration.task == 'follow_route':
+
+ experiments_starting_time = time.time()
+ experiment_counter = 0
+ experiments_elapsed_times = {'experiment_counter': [], 'elapsed_time': []}
+
+ for world_counter, world in enumerate(app_configuration.current_world):
+ for brain_counter, brain in enumerate(app_configuration.brain_path):
+ for route_counter in range(app_configuration.num_routes):
+ success = -1
+ experiment_attempts = 0
+ while success != 0:
+ logger.info("Launching: python3 test_suite_manager_carla.py -c " + config_data['config'][0])
+ logger.info("Experiment attempt: " + str(experiment_attempts+1))
+ current_experiment_starting_time = time.time()
+ success = os.system("python3 test_suite_manager_carla.py -c " + config_data['config'][0] + " -s -world_counter " + str(world_counter) + " -brain_counter " + str(brain_counter) + " -route_counter " + str(route_counter))
+ print('success: ', success)
+ if success != 0:
+ root = './'
+ folders = list(os.walk(root))[1:]
+ for folder in folders:
+ if len(folder[0].split('/')) == 2 and not folder[1] and not folder[2]:
+ logger.info("Removing empty folder: " + folder[0])
+ os.rmdir(folder[0])
+ if success == 2:
+ logger.info('KeyboardInterrupt called! Killing program...')
+ sys.exit(-1)
+ elif success != 0 and experiment_attempts < 5:
+ experiment_attempts += 1
+ logger.info("Python process finished with error! Repeating experiment")
+ elif success != 0 and experiment_attempts >= 5:
+ success = 0
+ logger.info("Too many failed attempts for this experiment.")
+ else:
+ experiments_elapsed_times['experiment_counter'].append(experiment_counter)
+ experiments_elapsed_times['elapsed_time'].append(time.time() - current_experiment_starting_time)
+ experiment_counter += 1
+ logger.info("Python process finished.")
+
+ logger.info('Last experiment folder: ')
+ logger.info(max(glob.glob(os.path.join('./', '*/')), key=os.path.getmtime))
+ else:
+ logger.info('Invalid task type. Try "follow_route", "follow_lane" or "follow_lane_traffic". Killing program...')
+ sys.exit(-1)
experiments_elapsed_times['total_experiments_elapsed_time'] = time.time() - experiments_starting_time
generate_agregated_experiments_metrics(experiments_starting_time, experiments_elapsed_times)
- if app_configuration.experiment_random_spawn_point == True:
+ if app_configuration.experiment_random_spawn_point == True or app_configuration.task == 'follow_route':
if os.path.isfile('tmp_circuit.launch'):
os.remove('tmp_circuit.launch')
logger.info('DONE! Bye, bye :)')
diff --git a/behavior_metrics/test_suite_manager_carla.py b/behavior_metrics/test_suite_manager_carla.py
new file mode 100644
index 00000000..972c8f8f
--- /dev/null
+++ b/behavior_metrics/test_suite_manager_carla.py
@@ -0,0 +1,194 @@
+import argparse
+import os
+import sys
+import threading
+import time
+import rospy
+import importlib
+import random
+
+from pilot_carla import PilotCarla
+from utils import environment
+from utils.colors import Colors
+from utils.configuration import Config
+from utils.controller_carla import ControllerCarla
+from utils.logger import logger
+from utils.constants import CARLA_TOWNS_TIMEOUTS
+from utils.traffic import TrafficManager
+from utils.constants import CARLA_TEST_SUITE_DIR, ROOT_PATH
+
+TESTSUITES = ROOT_PATH + '/' + CARLA_TEST_SUITE_DIR
+
+def check_args(argv):
+ parser = argparse.ArgumentParser(description='Testing suite runner.')
+
+ parser.add_argument('-c',
+ '--config',
+ type=str,
+ action='append',
+ required=True,
+ help='{}Path to the configuration file in YML format.{}'.format(
+ Colors.OKBLUE, Colors.ENDC))
+
+ group = parser.add_mutually_exclusive_group(required=True)
+ group.add_argument('-g',
+ '--gui',
+ action='store_true',
+ help='{}Load the GUI (Graphic User Interface). Requires PyQt5 installed{}'.format(
+ Colors.OKBLUE, Colors.ENDC))
+
+ group.add_argument('-t',
+ '--tui',
+ action='store_true',
+ help='{}Load the TUI (Terminal User Interface). Requires npyscreen installed{}'.format(
+ Colors.OKBLUE, Colors.ENDC))
+
+ group.add_argument('-s',
+ '--script',
+ action='store_true',
+ help='{}Run Behavior Metrics as script{}'.format(
+ Colors.OKBLUE, Colors.ENDC))
+
+ parser.add_argument('-r',
+ '--random',
+ action='store_true',
+ help='{}Run Behavior Metrics F1 with random spawning{}'.format(
+ Colors.OKBLUE, Colors.ENDC))
+
+ parser.add_argument('-world_counter',
+ type=str,
+ action='append',
+ help='{}World counter{}'.format(
+ Colors.OKBLUE, Colors.ENDC))
+
+ parser.add_argument('-brain_counter',
+ type=str,
+ action='append',
+ help='{}Brain counter{}'.format(
+ Colors.OKBLUE, Colors.ENDC))
+
+ parser.add_argument('-route_counter',
+ type=str,
+ action='append',
+ help='{}Route counter{}'.format(
+ Colors.OKBLUE, Colors.ENDC))
+
+ args = parser.parse_args()
+
+ config_data = {'config': None, 'gui': None, 'tui': None, 'script': None, 'random': False, 'world_counter': 0, 'brain_counter': 0, 'route_counter': 0}
+ if args.config:
+ config_data['config'] = []
+ for config_file in args.config:
+ if not os.path.isfile(config_file):
+ parser.error('{}No such file {} {}'.format(Colors.FAIL, config_file, Colors.ENDC))
+
+ config_data['config'] = args.config
+
+ if args.gui:
+ config_data['gui'] = args.gui
+
+ if args.tui:
+ config_data['tui'] = args.tui
+
+ if args.script:
+ config_data['script'] = args.script
+
+ if args.world_counter:
+ config_data['world_counter'] = args.world_counter
+
+ if args.brain_counter:
+ config_data['brain_counter'] = args.brain_counter
+
+ if args.route_counter:
+ config_data['route_counter'] = args.route_counter
+
+ return config_data
+
+def main():
+ config_data = check_args(sys.argv)
+ app_configuration = Config(config_data['config'][0])
+ world_counter = int(config_data['world_counter'][0])
+ brain_counter = int(config_data['brain_counter'][0])
+ route_counter = int(config_data['route_counter'][0])
+
+ logger.info(str(world_counter) + ' ' + str(brain_counter) + ' ' + str(route_counter))
+
+ world = app_configuration.current_world[world_counter]
+ brain = app_configuration.brain_path[brain_counter]
+ experiment_model = app_configuration.experiment_model[brain_counter]
+
+
+ test_suite_fname = TESTSUITES + app_configuration.test_suite + '.py'
+
+ if not os.path.exists(test_suite_fname):
+ logger.info('Test suite file does not exist! Killing program...')
+ sys.exit(-1)
+
+ if TESTSUITES not in sys.path:
+ sys.path.append(TESTSUITES)
+ test_routes_module = importlib.import_module(app_configuration.test_suite)
+ TEST_ROUTES = getattr(test_routes_module, 'TEST_ROUTES')
+ spawn_point = TEST_ROUTES[route_counter]['start']
+ target_point = TEST_ROUTES[route_counter]['end'].split(', ')
+ target_point = (float(target_point[0]), float(target_point[1]))
+ start_point = spawn_point.split(', ')
+ start_point = (float(start_point[0]), float(start_point[1]))
+ logger.info(f'-------from {start_point} to {target_point}-------')
+ town = TEST_ROUTES[route_counter]['map']
+ route = TEST_ROUTES[route_counter]['commands']
+ app_configuration.brain_kwargs['Route'] = route
+ route_length = TEST_ROUTES[route_counter]['distance']
+ environment.launch_env(world,
+ random_spawn_point=False,
+ carla_simulator=True,
+ config_spawn_point=spawn_point,
+ config_town=town)
+
+ controller = ControllerCarla()
+
+ # generate traffic
+ traffic_manager = TrafficManager(app_configuration.number_of_vehicle,
+ app_configuration.number_of_walker,
+ app_configuration.percentage_walker_running,
+ app_configuration.percentage_walker_crossing,
+ app_configuration.async_mode,
+ port=random.randint(8000, 9000)) # to avoid bind error
+ traffic_manager.generate_traffic()
+
+ # Launch control
+ pilot = PilotCarla(app_configuration, controller, brain, experiment_model=experiment_model)
+ pilot.brains.active_brain.target_point = target_point
+ pilot.daemon = True
+ pilot.start()
+ logger.info('Executing app')
+ controller.resume_pilot()
+ controller.unpause_carla_simulation()
+ controller.record_metrics(app_configuration.stats_out, world_counter=world_counter, brain_counter=brain_counter, repetition_counter=route_counter)
+ if app_configuration.use_world_timeouts:
+ experiment_timeout = CARLA_TOWNS_TIMEOUTS[controller.carla_map.name]
+ else:
+ experiment_timeout = app_configuration.experiment_timeouts[world_counter]
+
+ start_time = time.time()
+ termination_code = 3 # timeout
+ while (time.time() - start_time) < experiment_timeout:
+ rospy.sleep(0.1)
+ if pilot.brains.active_brain.termination_code != 0:
+ break
+
+ # rospy.sleep(experiment_timeout)
+ controller.stop_recording_metrics(termination_code=termination_code, route_length=route_length)
+ controller.pilot.stop()
+ controller.stop_pilot()
+ controller.pause_carla_simulation()
+
+ logger.info('closing all processes...')
+ controller.pilot.kill()
+ environment.close_ros_and_simulators()
+ while not controller.pilot.execution_completed:
+ time.sleep(1)
+
+
+if __name__ == '__main__':
+ main()
+ sys.exit(0)
\ No newline at end of file
diff --git a/behavior_metrics/utils/configuration.py b/behavior_metrics/utils/configuration.py
index 156d2376..96ab6fa2 100644
--- a/behavior_metrics/utils/configuration.py
+++ b/behavior_metrics/utils/configuration.py
@@ -84,6 +84,11 @@ def initialize_empty_configuration(self):
self.experiment_timeouts = None
+ self.task = None
+ self.test_suite = None
+ self.num_routes = None
+ self.randomize_routes = None
+
def initialize_configuration(self, config_data):
"""Initialize the configuration of the application based on a YAML profile file
@@ -95,6 +100,25 @@ def initialize_configuration(self, config_data):
self.robot_type = robot['Type']
self.pilot_time_cycle = robot['PilotTimeCycle']
self.current_world = config_data['Behaviors']['Simulation']['World']
+
+ if 'Task' in config_data['Behaviors']['Simulation']:
+ self.task = config_data['Behaviors']['Simulation']['Task']
+ else:
+ self.task = 'follow_lane'
+
+ if self.task == 'follow_route' and 'TestSuite' in config_data['Behaviors']['Simulation']:
+ self.test_suite = config_data['Behaviors']['Simulation']['TestSuite']
+
+ if self.task == 'follow_route' and 'NumRoutes' in config_data['Behaviors']['Simulation']:
+ self.num_routes = config_data['Behaviors']['Simulation']['NumRoutes']
+ else:
+ self.num_routes = 0
+
+ if self.task == 'follow_route' and 'RandomizeRoutes' in config_data['Behaviors']['Simulation']:
+ self.randomize_routes = config_data['Behaviors']['Simulation']['RandomizeRoutes']
+ else:
+ self.randomize_routes = True
+
if 'WaypointPublisher' in config_data['Behaviors']['Simulation']:
self.waypoint_publisher_path = config_data['Behaviors']['Simulation']['WaypointPublisher']
else:
@@ -132,7 +156,8 @@ def initialize_configuration(self, config_data):
if 'Timeout' in config_data['Behaviors']['Experiment']:
self.experiment_timeouts = config_data['Behaviors']['Experiment']['Timeout']
self.use_world_timeouts = config_data['Behaviors']['Experiment']['UseWorldTimeouts']
- self.experiment_repetitions = config_data['Behaviors']['Experiment']['Repetitions']
+ if 'Repetitions' in config_data['Behaviors']['Experiment']:
+ self.experiment_repetitions = config_data['Behaviors']['Experiment']['Repetitions']
if 'RandomSpawnPoint' in config_data['Behaviors']['Simulation']:
self.experiment_random_spawn_point = config_data['Behaviors']['Simulation']['RandomSpawnPoint']
@@ -141,10 +166,6 @@ def initialize_configuration(self, config_data):
else:
self.spawn_points = []
- if 'MultiCar' in config_data['Behaviors']['Simulation']:
- self.multicar = config_data['Behaviors']['Simulation']['MultiCar']
- else:
- self.multicar = False
if 'NumberOfVehicle' in config_data['Behaviors']['Simulation']:
self.number_of_vehicle = config_data['Behaviors']['Simulation']['NumberOfVehicle']
diff --git a/behavior_metrics/utils/constants.py b/behavior_metrics/utils/constants.py
index 25bf8fe6..3fe5af6c 100644
--- a/behavior_metrics/utils/constants.py
+++ b/behavior_metrics/utils/constants.py
@@ -2,6 +2,7 @@
PRETRAINED_MODELS_DIR = 'models/'
DATASETS_DIR = 'datasets_opencv/'
+CARLA_TEST_SUITE_DIR = 'configs/CARLA/test_suites/'
ROOT_PATH = str(Path(__file__).parent.parent)
MIN_EXPERIMENT_PERCENTAGE_COMPLETED = 3
# General timeout reference for each circuit extracted using explicit_brain
@@ -140,3 +141,12 @@
"Carla/Maps/Town06": 210,
"Carla/Maps/Town07": 120,
}
+
+CARLA_INFRACTION_PENALTIES = {
+ 'collision_walker': 0.5,
+ 'collision_vehicle': 0.6,
+ 'collision_static': 0.65,
+ 'wrong_turn': 0.7,
+ 'time_out': 0.7,
+ 'red_light': 0.7
+}
\ No newline at end of file
diff --git a/behavior_metrics/utils/controller_carla.py b/behavior_metrics/utils/controller_carla.py
index 408033e6..f9643d44 100644
--- a/behavior_metrics/utils/controller_carla.py
+++ b/behavior_metrics/utils/controller_carla.py
@@ -37,6 +37,7 @@
from datetime import datetime
from std_msgs.msg import String
from utils import metrics_carla
+from utils.constants import CARLA_INFRACTION_PENALTIES
try:
from carla_msgs.msg import CarlaLaneInvasionEvent
from carla_msgs.msg import CarlaCollisionEvent
@@ -72,7 +73,7 @@ def __init__(self):
client = carla.Client('localhost', 2000)
client.set_timeout(100.0) # seconds
self.world = client.get_world()
- # time.sleep(5)
+ time.sleep(5) # takes a few second for the correct map to finish loading
self.carla_map = self.world.get_map()
while len(self.world.get_actors().filter('vehicle.*')) == 0:
logger.info("Waiting for vehicles!")
@@ -296,7 +297,7 @@ def record_metrics(self, metrics_record_dir_path, world_counter=None, brain_coun
with open("logs/.roslaunch_stdout.log", "w") as out, open("logs/.roslaunch_stderr.log", "w") as err:
self.proc = subprocess.Popen(command, stdout=out, stderr=err)
- def stop_recording_metrics(self):
+ def stop_recording_metrics(self, termination_code=None, route_length=None):
logger.info("Stopping metrics bag recording")
end_time = time.time()
@@ -353,6 +354,51 @@ def stop_recording_metrics(self):
experiment_metrics_filename = self.metrics_record_dir_path + self.time_str + '/' + self.time_str
self.experiment_metrics = metrics_carla.get_metrics(self.experiment_metrics, self.experiment_metrics_bag_filename, self.map_waypoints, experiment_metrics_filename, self.pilot.configuration)
+ self.experiment_metrics['collisions_vehicle'] = 0
+ self.experiment_metrics['collisions_walker'] = 0
+ self.experiment_metrics['collisions_static'] = 0
+
+ collision_actor_ids = self.experiment_metrics['collision_actor_ids']
+ for actor_id in collision_actor_ids:
+ actor = self.world.get_actor(actor_id)
+ if actor:
+ actor_type = actor.type_id.split('.')[0]
+ if actor_type == 'vehicle':
+ self.experiment_metrics['collisions_vehicle'] += 1
+ elif actor_type == 'walker':
+ self.experiment_metrics['collisions_walker'] += 1
+ else:
+ self.experiment_metrics['collisions_static'] += 1
+ else:
+ print(f"No actor found with ID {actor_id}")
+
+ if hasattr(self.pilot.brains.active_brain, 'red_light_counter'):
+ self.experiment_metrics['traffic_light_infractions'] = self.pilot.brains.active_brain.red_light_counter
+ self.experiment_metrics['traffic_light_infractions_per_km'] = self.experiment_metrics['traffic_light_infractions'] / (self.experiment_metrics['effective_completed_distance']/1000)
+
+ if route_length is not None:
+ self.experiment_metrics['route_completion'] = self.experiment_metrics['effective_completed_distance'] / route_length
+
+ wrong_turn_counter = 0
+ time_out_counter = 0
+ if termination_code is not None:
+ if termination_code == 1:
+ self.experiment_metrics['termination cause'] = 'success'
+ elif termination_code == 2:
+ wrong_turn_counter += 1
+ self.experiment_metrics['termination cause'] = 'wrong turn'
+ elif termination_code == 3:
+ time_out_counter += 1
+ self.experiment_metrics['termination cause'] = 'time out'
+
+ if 'route_completion' in self.experiment_metrics.keys() and 'traffic_light_infractions' in self.experiment_metrics.keys() and termination_code is not None:
+ self.experiment_metrics['driving score'] = self.experiment_metrics['route_completion'] * \
+ CARLA_INFRACTION_PENALTIES['collision_vehicle']**self.experiment_metrics['collisions_vehicle'] * \
+ CARLA_INFRACTION_PENALTIES['collision_static']**self.experiment_metrics['collisions_static'] * \
+ CARLA_INFRACTION_PENALTIES['collision_walker']**self.experiment_metrics['collisions_walker'] * \
+ CARLA_INFRACTION_PENALTIES['red_light']**self.experiment_metrics['traffic_light_infractions'] * \
+ CARLA_INFRACTION_PENALTIES['wrong_turn']**wrong_turn_counter * \
+ CARLA_INFRACTION_PENALTIES['time_out']**time_out_counter
self.save_metrics(first_images, last_images)
for key, value in self.experiment_metrics.items():
diff --git a/behavior_metrics/utils/environment.py b/behavior_metrics/utils/environment.py
index 02bf0a71..8225c697 100644
--- a/behavior_metrics/utils/environment.py
+++ b/behavior_metrics/utils/environment.py
@@ -31,7 +31,7 @@
__license__ = 'GPLv3'
-def launch_env(launch_file, random_spawn_point=False, carla_simulator=False, config_spawn_point=None):
+def launch_env(launch_file, random_spawn_point=False, carla_simulator=False, config_spawn_point=None, config_town=None):
"""Launch the environmet specified by the launch_file given in command line at launch time.
Arguments:
launch_file {str} -- path of the launch file to be executed
@@ -55,6 +55,20 @@ def launch_env(launch_file, random_spawn_point=False, carla_simulator=False, con
else:
spawn_point.attrib['default'] = random.choice(CARLA_TOWNS_SPAWN_POINTS[town.attrib['default']])
tree.write('tmp_circuit.launch')
+ elif config_spawn_point is not None and config_town is not None:
+ tree = ET.parse(ROOT_PATH + '/' + launch_file)
+ root = tree.getroot()
+ town = root.find(".//*[@name=\"town\"]")
+ if town is None:
+ town=root.find(".//*[@name='town']")
+ town.attrib['default'] = config_town
+ spawn_point = root.find(".//*[@name=\"spawn_point\"]")
+ if spawn_point is None:
+ spawn_point=root.find(".//*[@name='spawn_point']")
+ spawn_point.attrib['default'] = config_spawn_point
+ tree.write('tmp_circuit.launch')
+ else:
+ spawn_point = None
with open("/tmp/.carlalaunch_stdout.log", "w") as out, open("/tmp/.carlalaunch_stderr.log", "w") as err:
tree = ET.parse(ROOT_PATH + '/' + launch_file)
root = tree.getroot()
@@ -67,7 +81,7 @@ def launch_env(launch_file, random_spawn_point=False, carla_simulator=False, con
logger.info("SimulatorEnv: launching simulator server.")
time.sleep(5)
with open("/tmp/.roslaunch_stdout.log", "w") as out, open("/tmp/.roslaunch_stderr.log", "w") as err:
- if random_spawn_point:
+ if random_spawn_point or (spawn_point is not None and town is not None):
child = subprocess.Popen(["roslaunch", ROOT_PATH + '/tmp_circuit.launch'], stdout=out, stderr=err)
else:
child = subprocess.Popen(["roslaunch", ROOT_PATH + '/' + launch_file], stdout=out, stderr=err)
diff --git a/behavior_metrics/utils/metrics_carla.py b/behavior_metrics/utils/metrics_carla.py
index 3fa87434..9ce3ad6d 100644
--- a/behavior_metrics/utils/metrics_carla.py
+++ b/behavior_metrics/utils/metrics_carla.py
@@ -72,7 +72,7 @@ def get_metrics(experiment_metrics, experiment_metrics_bag_filename, map_waypoin
for index, row in dataframe_pose.iterrows():
checkpoints.append(row)
- if config.multicar:
+ if config.task == 'follow_lane_traffic':
data_file = experiment_metrics_bag_filename.split('.bag')[0] + '/carla-npc_vehicle_1-odometry.csv'
dataframe_pose = pd.read_csv(data_file)
checkpoints_2 = []
@@ -124,7 +124,7 @@ def get_metrics(experiment_metrics, experiment_metrics_bag_filename, map_waypoin
experiment_metrics, collisions_checkpoints = get_collisions(experiment_metrics, collision_points, dataframe_pose)
experiment_metrics, lane_invasion_checkpoints = get_lane_invasions(experiment_metrics, lane_invasion_points, dataframe_pose)
experiment_metrics['experiment_total_simulated_time'] = seconds_end - seconds_start
- if config.multicar:
+ if config.task == 'follow_lane_traffic':
experiment_metrics = get_distance_other_vehicle(experiment_metrics, checkpoints, checkpoints_2)
if 'bird_eye_view_images' in experiment_metrics:
@@ -233,20 +233,24 @@ def get_suddenness_control_commands(experiment_metrics, vehicle_status_points):
def get_collisions(experiment_metrics, collision_points, df_checkpoints):
collisions_checkpoints = []
collisions_checkpoints_different = []
+ collisions_actors_different = []
previous_collisions_checkpoints_x, previous_collisions_checkpoints_y = 0, 0
for point in collision_points:
- collision_point = df_checkpoints.loc[df_checkpoints['Time'] == point['Time']]
+ idx = (df_checkpoints['Time'] - point['Time']).abs().idxmin()
+ collision_point = df_checkpoints.iloc[idx]
collisions_checkpoints.append(collision_point)
- point_1 = np.array([collision_point.iloc[0]['pose.pose.position.x'], collision_point.iloc[0]['pose.pose.position.y']])
+ point_1 = np.array([collision_point['pose.pose.position.x'], collision_point['pose.pose.position.y']])
point_2 = np.array([previous_collisions_checkpoints_x, previous_collisions_checkpoints_y])
dist = (point_2 - point_1) ** 2
dist = np.sum(dist, axis=0)
dist = np.sqrt(dist)
if dist > 1:
collisions_checkpoints_different.append(collision_point)
- previous_collisions_checkpoints_x, previous_collisions_checkpoints_y = collision_point.iloc[0]['pose.pose.position.x'], collision_point.iloc[0]['pose.pose.position.y']
+ collisions_actors_different.append(point['other_actor_id'])
+ previous_collisions_checkpoints_x, previous_collisions_checkpoints_y = collision_point['pose.pose.position.x'], collision_point['pose.pose.position.y']
experiment_metrics['collisions'] = len(collisions_checkpoints_different)
+ experiment_metrics['collision_actor_ids'] = collisions_actors_different
return experiment_metrics, collisions_checkpoints
def get_lane_invasions(experiment_metrics, lane_invasion_points, df_checkpoints):
@@ -255,9 +259,11 @@ def get_lane_invasions(experiment_metrics, lane_invasion_points, df_checkpoints)
previous_lane_invasion_checkpoints_x, previous_lane_invasion_checkpoints_y = 0, 0
previous_time = 0
for point in lane_invasion_points:
- lane_invasion_point = df_checkpoints.loc[df_checkpoints['Time'] == point['Time']]
+ idx = (df_checkpoints['Time'] - point['Time']).abs().idxmin()
+ lane_invasion_point = df_checkpoints.iloc[idx]
+
lane_invasion_checkpoints.append(lane_invasion_point)
- point_1 = np.array([lane_invasion_point.iloc[0]['pose.pose.position.x'], lane_invasion_point.iloc[0]['pose.pose.position.y']])
+ point_1 = np.array([lane_invasion_point['pose.pose.position.x'], lane_invasion_point['pose.pose.position.y']])
point_2 = np.array([previous_lane_invasion_checkpoints_x, previous_lane_invasion_checkpoints_y])
dist = (point_2 - point_1) ** 2
dist = np.sum(dist, axis=0)
@@ -265,7 +271,7 @@ def get_lane_invasions(experiment_metrics, lane_invasion_points, df_checkpoints)
if dist > 1 and point['Time'] - previous_time > 0.5:
lane_invasion_checkpoints_different.append(lane_invasion_point)
previous_time = point['Time']
- previous_lane_invasion_checkpoints_x, previous_lane_invasion_checkpoints_y = lane_invasion_point.iloc[0]['pose.pose.position.x'], lane_invasion_point.iloc[0]['pose.pose.position.y']
+ previous_lane_invasion_checkpoints_x, previous_lane_invasion_checkpoints_y = lane_invasion_point['pose.pose.position.x'], lane_invasion_point['pose.pose.position.y']
experiment_metrics['lane_invasions'] = len(lane_invasion_checkpoints_different)
return experiment_metrics, lane_invasion_checkpoints
@@ -312,7 +318,6 @@ def get_position_deviation_and_effective_completed_distance(experiment_metrics,
checkpoints_tuples_y.append(checkpoint_y)
checkpoints_speeds.append(current_checkpoint[2])
checkpoints_tuples.append((checkpoint_x, checkpoint_y, current_checkpoint[2]))
-
min_dists = []
best_checkpoint_points_x = []
best_checkpoint_points_y = []