77This module provides a human agent to control the ego vehicle via keyboard
88"""
99
10- import time
11- from threading import Thread
12- import cv2
13- import numpy as np
10+ from __future__ import print_function
11+
12+ import json
1413
1514try :
1615 import pygame
2322 from pygame .locals import K_d
2423 from pygame .locals import K_s
2524 from pygame .locals import K_w
25+ from pygame .locals import K_q
2626except ImportError :
2727 raise RuntimeError ('cannot import pygame, make sure pygame package is installed' )
2828
@@ -37,13 +37,9 @@ class HumanInterface(object):
3737 Class to control a vehicle manually for debugging purposes
3838 """
3939
40- def __init__ (self , parent ):
41- self .quit = False
42- self ._parent = parent
40+ def __init__ (self ):
4341 self ._width = 800
4442 self ._height = 600
45- self ._throttle_delta = 0.05
46- self ._steering_delta = 0.01
4743 self ._surface = None
4844
4945 pygame .init ()
@@ -52,39 +48,23 @@ def __init__(self, parent):
5248 self ._display = pygame .display .set_mode ((self ._width , self ._height ), pygame .HWSURFACE | pygame .DOUBLEBUF )
5349 pygame .display .set_caption ("Human Agent" )
5450
55- def run (self ):
51+ def run_interface (self , input_data ):
5652 """
5753 Run the GUI
5854 """
59- while not self ._parent .agent_engaged and not self .quit :
60- time .sleep (0.5 )
61-
62- controller = KeyboardControl ()
63- while not self .quit :
64- self ._clock .tick_busy_loop (20 )
65- controller .parse_events (self ._parent .current_control , self ._clock )
66- # Process events
67- pygame .event .pump ()
68-
69- # process sensor data
70- input_data = self ._parent .sensor_interface .get_data ()
71- image_center = input_data ['Center' ][1 ][:, :, - 2 ::- 1 ]
72- image_left = input_data ['Left' ][1 ][:, :, - 2 ::- 1 ]
73- image_right = input_data ['Right' ][1 ][:, :, - 2 ::- 1 ]
74- image_rear = input_data ['Rear' ][1 ][:, :, - 2 ::- 1 ]
75-
76- top_row = np .hstack ((image_left , image_center , image_right ))
77- bottom_row = np .hstack ((0 * image_rear , image_rear , 0 * image_rear ))
78- comp_image = np .vstack ((top_row , bottom_row ))
79- # resize image
80- image_rescaled = cv2 .resize (comp_image , dsize = (self ._width , self ._height ), interpolation = cv2 .INTER_CUBIC )
81-
82- # display image
83- self ._surface = pygame .surfarray .make_surface (image_rescaled .swapaxes (0 , 1 ))
84- if self ._surface is not None :
85- self ._display .blit (self ._surface , (0 , 0 ))
86- pygame .display .flip ()
55+ # process sensor data
56+ image_center = input_data ['Center' ][1 ][:, :, - 2 ::- 1 ]
57+
58+ # display image
59+ self ._surface = pygame .surfarray .make_surface (image_center .swapaxes (0 , 1 ))
60+ if self ._surface is not None :
61+ self ._display .blit (self ._surface , (0 , 0 ))
62+ pygame .display .flip ()
8763
64+ def quit_interface (self ):
65+ """
66+ Stops the pygame window
67+ """
8868 pygame .quit ()
8969
9070
@@ -96,21 +76,17 @@ class HumanAgent(AutonomousAgent):
9676
9777 current_control = None
9878 agent_engaged = False
79+ prev_timestamp = 0
9980
10081 def setup (self , path_to_conf_file ):
10182 """
10283 Setup the agent parameters
10384 """
10485
10586 self .agent_engaged = False
106- self .current_control = carla .VehicleControl ()
107- self .current_control .steer = 0.0
108- self .current_control .throttle = 1.0
109- self .current_control .brake = 0.0
110- self .current_control .hand_brake = False
111- self ._hic = HumanInterface (self )
112- self ._thread = Thread (target = self ._hic .run )
113- self ._thread .start ()
87+ self .prev_timestamp = 0
88+ self ._hic = HumanInterface ()
89+ self ._controller = KeyboardControl (path_to_conf_file )
11490
11591 def sensors (self ):
11692 """
@@ -132,17 +108,7 @@ def sensors(self):
132108
133109 """
134110 sensors = [{'type' : 'sensor.camera.rgb' , 'x' : 0.7 , 'y' : 0.0 , 'z' : 1.60 , 'roll' : 0.0 , 'pitch' : 0.0 , 'yaw' : 0.0 ,
135- 'width' : 300 , 'height' : 200 , 'fov' : 100 , 'id' : 'Center' },
136-
137- {'type' : 'sensor.camera.rgb' , 'x' : 0.7 , 'y' : - 0.4 , 'z' : 1.60 , 'roll' : 0.0 , 'pitch' : 0.0 ,
138- 'yaw' : - 45.0 , 'width' : 300 , 'height' : 200 , 'fov' : 100 , 'id' : 'Left' },
139-
140- {'type' : 'sensor.camera.rgb' , 'x' : 0.7 , 'y' : 0.4 , 'z' : 1.60 , 'roll' : 0.0 , 'pitch' : 0.0 , 'yaw' : 45.0 ,
141- 'width' : 300 , 'height' : 200 , 'fov' : 100 , 'id' : 'Right' },
142-
143- {'type' : 'sensor.camera.rgb' , 'x' : - 1.8 , 'y' : 0 , 'z' : 1.60 , 'roll' : 0.0 , 'pitch' : 0.0 ,
144- 'yaw' : 180.0 , 'width' : 300 , 'height' : 200 , 'fov' : 130 , 'id' : 'Rear' },
145-
111+ 'width' : 800 , 'height' : 600 , 'fov' : 100 , 'id' : 'Center' },
146112 {'type' : 'sensor.other.gnss' , 'x' : 0.7 , 'y' : - 0.4 , 'z' : 1.60 , 'id' : 'GPS' }
147113 ]
148114
@@ -153,15 +119,18 @@ def run_step(self, input_data, timestamp):
153119 Execute one step of navigation.
154120 """
155121 self .agent_engaged = True
156- time .sleep (0.1 )
157- return self .current_control
122+ self ._hic .run_interface (input_data )
123+
124+ control = self ._controller .parse_events (timestamp - self .prev_timestamp )
125+ self .prev_timestamp = timestamp
126+
127+ return control
158128
159129 def destroy (self ):
160130 """
161131 Cleanup
162132 """
163- self ._hic .quit = True
164- self ._thread .join ()
133+ self ._hic .quit_interface = True
165134
166135
167136class KeyboardControl (object ):
@@ -170,33 +139,91 @@ class KeyboardControl(object):
170139 Keyboard control for the human agent
171140 """
172141
173- def __init__ (self ):
142+ def __init__ (self , path_to_conf_file ):
174143 """
175144 Init
176145 """
177146 self ._control = carla .VehicleControl ()
178147 self ._steer_cache = 0.0
148+ self ._clock = pygame .time .Clock ()
149+
150+ # Get the mode
151+ if path_to_conf_file :
152+
153+ with (open (path_to_conf_file , "r" )) as f :
154+ lines = f .read ().split ("\n " )
155+ self ._mode = lines [0 ].split (" " )[1 ]
156+ self ._endpoint = lines [1 ].split (" " )[1 ]
157+
158+ # Get the needed vars
159+ if self ._mode == "log" :
160+ self ._log_data = {'records' : []}
161+
162+ elif self ._mode == "playback" :
163+ self ._index = 0
164+ self ._control_list = []
165+
166+ with open (self ._endpoint ) as fd :
167+ try :
168+ self ._records = json .load (fd )
169+ self ._json_to_control ()
170+ except ValueError :
171+ # Moving to Python 3.5+ this can be replaced with json.JSONDecodeError
172+ pass
173+ else :
174+ self ._mode = "normal"
175+ self ._endpoint = None
179176
180- def parse_events (self , control , clock ):
177+ def _json_to_control (self ):
178+ """
179+ Parses the json file into a list of carla.VehicleControl
180+ """
181+
182+ # transform strs into VehicleControl commands
183+ for entry in self ._records ['records' ]:
184+ control = carla .VehicleControl (throttle = entry ['control' ]['throttle' ],
185+ steer = entry ['control' ]['steer' ],
186+ brake = entry ['control' ]['brake' ],
187+ hand_brake = entry ['control' ]['hand_brake' ],
188+ reverse = entry ['control' ]['reverse' ],
189+ manual_gear_shift = entry ['control' ]['manual_gear_shift' ],
190+ gear = entry ['control' ]['gear' ])
191+ self ._control_list .append (control )
192+
193+ def parse_events (self , timestamp ):
181194 """
182195 Parse the keyboard events and set the vehicle controls accordingly
183196 """
184- for event in pygame .event .get ():
185- if event .type == pygame .QUIT :
186- return
197+ # Move the vehicle
198+ if self ._mode == "playback" :
199+ self ._parse_json_control ()
200+ else :
201+ self ._parse_vehicle_keys (pygame .key .get_pressed (), timestamp * 1000 )
202+
203+ # Record the control
204+ if self ._mode == "log" :
205+ self ._record_control ()
187206
188- self ._parse_vehicle_keys (pygame .key .get_pressed (), clock .get_time ())
189- control .steer = self ._control .steer
190- control .throttle = self ._control .throttle
191- control .brake = self ._control .brake
192- control .hand_brake = self ._control .hand_brake
207+ return self ._control
193208
194209 def _parse_vehicle_keys (self , keys , milliseconds ):
195210 """
196211 Calculate new vehicle controls based on input keys
197212 """
198- self ._control .throttle = 0.6 if keys [K_UP ] or keys [K_w ] else 0.0
199- steer_increment = 15.0 * 5e-4 * milliseconds
213+ for event in pygame .event .get ():
214+ if event .type == pygame .QUIT :
215+ return
216+ elif event .type == pygame .KEYUP :
217+ if event .key == K_q :
218+ self ._control .gear = 1 if self ._control .reverse else - 1
219+ self ._control .reverse = self ._control .gear < 0
220+
221+ if keys [K_UP ] or keys [K_w ]:
222+ self ._control .throttle = 0.6
223+ else :
224+ self ._control .throttle = 0.0
225+
226+ steer_increment = 3e-4 * milliseconds
200227 if keys [K_LEFT ] or keys [K_a ]:
201228 self ._steer_cache -= steer_increment
202229 elif keys [K_RIGHT ] or keys [K_d ]:
@@ -208,3 +235,42 @@ def _parse_vehicle_keys(self, keys, milliseconds):
208235 self ._control .steer = round (self ._steer_cache , 1 )
209236 self ._control .brake = 1.0 if keys [K_DOWN ] or keys [K_s ] else 0.0
210237 self ._control .hand_brake = keys [K_SPACE ]
238+
239+ def _parse_json_control (self ):
240+ """
241+ Gets the control corresponding to the current frame
242+ """
243+
244+ if self ._index < len (self ._control_list ):
245+ self ._control = self ._control_list [self ._index ]
246+ self ._index += 1
247+ else :
248+ print ("JSON file has no more entries" )
249+
250+ def _record_control (self ):
251+ """
252+ Saves the list of control into a json file
253+ """
254+
255+ new_record = {
256+ 'control' : {
257+ 'throttle' : self ._control .throttle ,
258+ 'steer' : self ._control .steer ,
259+ 'brake' : self ._control .brake ,
260+ 'hand_brake' : self ._control .hand_brake ,
261+ 'reverse' : self ._control .reverse ,
262+ 'manual_gear_shift' : self ._control .manual_gear_shift ,
263+ 'gear' : self ._control .gear
264+ }
265+ }
266+
267+ self ._log_data ['records' ].append (new_record )
268+
269+ def __del__ (self ):
270+ """
271+ Delete method
272+ """
273+ # Get ready to log user commands
274+ if self ._mode == "log" and self ._log_data :
275+ with open (self ._endpoint , 'w' ) as fd :
276+ json .dump (self ._log_data , fd , indent = 4 , sort_keys = True )
0 commit comments