Skip to content

Commit

Permalink
Merge pull request #644 from Qi-Zha0/three_tasks
Browse files Browse the repository at this point in the history
Three tasks follow_lane, follow_lane_traffic, follow_route
  • Loading branch information
sergiopaniego authored Oct 30, 2023
2 parents 5bbb37e + bdb46a3 commit 59db63e
Show file tree
Hide file tree
Showing 18 changed files with 881 additions and 109 deletions.
1 change: 1 addition & 0 deletions behavior_metrics/.gitignore
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@
bag_analysis/
core
tmp_*
carla_birdeye_view/


### Python ###
Expand Down
Original file line number Diff line number Diff line change
@@ -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

Expand All @@ -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)

Expand All @@ -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.
Expand Down Expand Up @@ -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,
Expand All @@ -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)
91 changes: 91 additions & 0 deletions behavior_metrics/brains/CARLA/utils/high_level_command.py
Original file line number Diff line number Diff line change
@@ -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

10 changes: 9 additions & 1 deletion behavior_metrics/brains/CARLA/utils/test_utils.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Original file line number Diff line number Diff line change
@@ -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>
Original file line number Diff line number Diff line change
Expand Up @@ -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>

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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'
Expand Down
Loading

0 comments on commit 59db63e

Please sign in to comment.