num_frame += 1
ret, frame = cap.read()
if frame is None:
break
input_framesize = frame.shape[:2]
# 調整影像大小
frame = cv2.resize(frame, (width, height))
posed_frame, joints_info = process_frame(frame,
width,
height,
num_frame,
body=body_estimate,
hands=hand_estimate)
rec_list = util.get_bbox(joints_info, width, height)
id_list, new_id = track(pre_rec_list, pre_id_list, rec_list, new_id,
confidence)
if action_estimate:
action_list = action_estimation(joints_info)
angle_value = util.joint_angle(joints_info)
for p in range(len(action_list)):
id = id_list[p]
left_top = rec_list[p][0] # 矩形左上座標
right_buttom = rec_list[p][1] # 矩形右下座標
cv2.rectangle(posed_frame, left_top, right_buttom, (0, 255, 0),
2)
text = 'id={} {}'.format(id, action_list[p])
cv2.putText(posed_frame, text, left_top,
cv2.FONT_HERSHEY_COMPLEX, 0.5, (0, 0, 255), 1,
cv2.LINE_AA) # 輸出類別
cap.set(4, height)
new_id = 1
pre_rec_list = []
pre_id_list = []
num_frame = 0
while True:
num_frame += 1
ret, oriImg = cap.read()
candidate, subset = body_estimation(oriImg)
joints_norm = util.skeleton_info(candidate, subset, width, height,
num_frame)
# detect action
action_list, rec_list = action_estimation(joints_norm, width, height)
id_list, new_id = track(pre_rec_list, pre_id_list, rec_list, new_id)
for p in range(len(action_list)):
id = id_list[p]
left_top = rec_list[p][0]
right_buttom = rec_list[p][1]
cv2.rectangle(oriImg, left_top, right_buttom, (0, 255, 0), 2)
text = 'ID={} {}'.format(id, action_list[p])
cv2.putText(oriImg, text, left_top, cv2.FONT_HERSHEY_COMPLEX, 1,
(0, 0, 255), 1, cv2.LINE_AA) # 輸出類別
canvas = copy.deepcopy(oriImg)
canvas = util.draw_bodypose(canvas, candidate, subset)
if hand_estimation:
# detect hand
hands_list = util.handDetect(candidate, subset, oriImg)
def run(self):
# logger.setLevel(logging.DEBUG)
logger.info("Starting track process track {}".format(self.track_name))
self.track = track(self.track_name)
self.car_dict = dict(
) # maps from client_addr to car_model (or None if a spectator)
self.car_states_list = list(
) # list of all car states, to send to clients and put in each car's state
self.spectator_list = list(
) # maps from client_addr to car_model (or None if a spectator)
self.track_socket = socket.socket(
socket.AF_INET, socket.SOCK_DGRAM) # make a new datagram socket
self.track_socket.settimeout(
0
) # put track socket in nonblocking mode to just poll for client messages
# find range of ports we can try to open for client to connect to
try:
self.track_socket.bind(('0.0.0.0', self.local_port_number))
except Exception as e:
logger.error(
'track process aborting: could not bind to the local port {} that server told us to use: got {}'
.format(self.local_port_number, e))
raise e
self.track_socket_address = self.track_socket.getsockname(
) # get the port info for our local port
logger.info('for track {} bound free local UDP port address {}'.format(
self.track_name, self.local_port_number))
last_time = timer()
# Track process makes a single socket bound to a single port for all the clients (cars and spectators).
# To handle multiple clients, when it gets a message from a client, it responds to the client using the client address.
looper = loop_timer(MODEL_UPDATE_RATE_HZ)
looper.LOG_INTERVAL_SEC = 60
while not self.exit:
now = timer()
dt = now - last_time
last_time = now
if now - self.last_message_time > KILL_ZOMBIE_TRACK_TIMEOUT_S:
logger.warning(
'track process {} got no input for {}s, terminating'.
format(self.track_name, KILL_ZOMBIE_TRACK_TIMEOUT_S))
self.exit = True
self.cleanup()
continue
self.process_server_queue() # 'add_car' 'add_spectator'
# Here we make the constrained real time from real time
# If requested timestep bigger than maximal timestep, make the update for maximal allowed timestep
# We limit timestep to avoid instability
if dt > MAX_TIMESTEP:
s = 'bounded real dt_sec={:.1f}ms to {:.2f}ms'.format(
dt * 1000, MAX_TIMESTEP * 1000)
logger.info(s)
dt = MAX_TIMESTEP
# now we do main simulation/response
# update all the car models
for client, model in self.car_dict.items():
if isinstance(model, car_model):
model.update(dt) # car_state time updates already here
model.time += dt # car_model time updates here
# poll for UDP messages
# update the global list of car states that cars share
self.car_states_list.clear()
for model in self.car_dict.values():
# put copy of each state in list but strip off the contained list of other car states
model_copy: car_state = copy.copy(model.car_state)
self.car_states_list.append(model_copy)
# process incoming UDP messages from clients, e.g. to update command
while True:
try:
msg, payload, client = self.receive_msg()
self.handle_client_msg(msg, payload, client)
except socket.timeout:
break
except BlockingIOError:
break
except Exception as e:
logger.warning(
'caught Exception {} while processing UDP messages from client'
.format(e))
break
try:
looper.sleep_leftover_time()
except KeyboardInterrupt:
logger.info('KeyboardInterrupt, stopping server')
self.exit = True
continue
self.cleanup()
logger.info('ended track {}'.format(self.track_name))
def __init__(self,
track_name: str = 'track',
spectate: bool = False,
car_name: str = CAR_NAME,
controller: Optional[object] = None,
server_host: str = SERVER_HOST,
server_port: int = SERVER_PORT,
joystick_number: int = JOYSTICK_NUMBER,
fps: float = FPS,
widthPixels: int = SCREEN_WIDTH_PIXELS,
heightPixels: int = SCREEN_HEIGHT_PIXELS,
timeout_s: float = SERVER_TIMEOUT_SEC,
record: Optional[str] = None,
replay_file_list: Optional[List[str]] = None,
lidar: float = None
):
"""
Makes a new instance of client that users use to run a car on a track.
:param track_name: string name of track, without .png suffix
:param spectate: set True to just spectate
:param car_name: Your name for your car
:param controller: Optional autodrive controller that implements the read method to return (car_command, user_input)
:param server_host: hostname of server, e.g. 'localhost' or 'telluridevm.iniforum.ch'
:param server_port: port on server to initiate communication
:param joystick_number: joystick number if more than one
:param fps: desired frames per second of game loop
:param widthPixels: screen width in pixels (must match server settings)
:param heightPixels: height, must match server settings
:param timeout_s: socket read timeout for blocking reads (main loop uses nonblocking reads)
:param record: set it None to not record. Set it to a string to add note for this recording to file name to record data for all cars to CSV files
:param replay_file_list: None for normal live mode, or List[str] of filenames to play back a set of car recordings together
"""
pygame.init()
logger.info('using pygame version {}'.format(pygame.version.ver))
pygame.display.set_caption("l2race")
self.spectate = spectate
self.widthPixels = widthPixels
self.heightPixels = heightPixels
self.screen = pygame.display.set_mode(size=(self.widthPixels, self.heightPixels), flags=0)
pygame.freetype.init()
try:
self.game_font = pygame.freetype.SysFont(GAME_FONT_NAME, GAME_FONT_SIZE)
except:
logger.warning('cannot get specified globals.py font {}, using pygame default font'.format(GAME_FONT_NAME))
self.game_font = pygame.font.Font(pygame.font.get_default_font(), GAME_FONT_SIZE)
self.clock = pygame.time.Clock()
self.exit = False
self.input = None
self.fps = fps
self.sock: Optional[socket] = None # our socket used for communicating with server
self.server_host: str = server_host
self.server_port: int = server_port
self.serverStartAddr: Tuple[str, int] = (
self.server_host, self.server_port) # manager address, different port on server used during game
self.gameSockAddr: Optional[Tuple[str, int]] = None # address used during game
self.server_timeout_s: float = timeout_s
self.gotServer: bool = False
self.recording_enabled: bool = not record is None
self.record_note: Optional[str] = record if not record is None else None
self.data_recorders: Optional[List[data_recorder]] = None
self.replay_file_list: Optional[List[str]] = replay_file_list
self.track_name: str = track_name
self.car_name: str = car_name
self.car: Optional[car] = None # will make it later after we get info from server about car
self.input: keyboard_and_joystick_input = keyboard_and_joystick_input()
self.server_message = None # holds messsages sent from server to be displayed
self.last_server_message_time = time.time()
# if controller is None:
# self.controller = pid_next_waypoint_car_controller()
# else:
# self.controller = controller
# spectator data structures
self.track_instance: track = track(track_name=self.track_name)
self.spectate_cars: Dict[
str, car] = dict() # dict of other cars (NOT including ourselves) on the track, by name of the car. Each entry is a car() that we make here. For spectators, the list contains all cars. The cars contain the car_state. The complete list of all cars is this dict plus self.car
self.autodrive_controller = controller # automatic self driving controller specified in constructor
self.lidar = lidar # variable controlling if to show lidar mini and with what precission
self.t_max = 0.0
def replay(self) -> bool:
"""
Replays the self.replay_file_list recordings. It will immediately return False if it cannot find the file to play. Otherwise
it will start a loop that runs over the entire file to play it, and finally return True.
:returns: False if it cannot find the file to play, True at the end of playing the entire recording.
"""
# Load data
file_path = None
filename = None
# Find the right file
if (self.replay_file_list is not None) and (self.replay_file_list is not 'last'):
if isinstance(self.replay_file_list, List) and len(self.replay_file_list) > 1:
filename = self.replay_file_list[0]
raise NotImplemented('Replaying more than one recording is not yet implemented')
if isinstance(self.replay_file_list, str):
filename = self.replay_file_list
if not filename.endswith('.csv'):
filename = filename + '.csv'
# if filename is a path, then use the path, otherwise, look for file at starting folder and in DATA_FOLDER_NAME folder
if os.sep in filename:
# treat as local or DATA_FOLDER filename
file_path = filename
if not os.path.isfile(filename):
logger.error('Cannot replay: There is no race recording file with name {}'.format(filename))
return False
else:
# check if file found in DATA_FOLDER_NAME or at local starting point
if not os.path.isfile(filename):
file_path = os.path.join(DATA_FOLDER_NAME, filename)
if not os.path.isfile(file_path):
logger.error(
'Cannot replay: There is no race recording file with name {} at local folder or in {}'.format(
file_path, DATA_FOLDER_NAME))
return False
else:
file_path = filename
elif self.replay_file_list == 'last':
try:
import glob
list_of_files = glob.glob(DATA_FOLDER_NAME + '/*.csv')
file_path = max(list_of_files, key=os.path.getctime)
except FileNotFoundError:
logger.error('Cannot replay: No race recording found in data folder ' + DATA_FOLDER_NAME)
return False
else:
logger.error('Cannot replay: filename is None')
return False
# Get race recording
logger.info('Replaying file {}'.format(file_path))
try:
data: DataFrame = pd.read_csv(file_path, comment='#') # skip comment lines starting with #
except Exception as e:
logger.error('Cannot replay: Caught {} trying to read CSV file {}'.format(e, file_path))
return False
# Get used car name
s = str(pd.read_csv(file_path, skiprows=5, nrows=1))
self.track_name = re.search('"(.*)"', s).group(1)
logger.debug(self.track_name)
# Get used track
s = str(pd.read_csv(file_path, skiprows=6, nrows=1))
self.car_name = re.search('"(.*)"', s).group(1)
logger.debug(self.car_name)
# print(file_path)
# print(data.head())
# Define car and track
self.track_instance = track(self.track_name)
self.car = car(name=self.car_name, our_track=self.track_instance, screen=self.screen)
# decimate data to make it play faster
# data = data.iloc[::4, :]
# Run a loop to print data
n_rows = data.shape[0]
r = 0
step = 1
scale = 10
looper = loop_timer(rate_hz=self.fps)
while not self.exit:
try:
looper.sleep_leftover_time()
except KeyboardInterrupt:
logger.info('KeyboardInterrupt, stopping client')
self.exit = True
car_command, user_input = self.input.read() # gets input from keyboard or joystick
if self.input.exit:
self.exit = True
continue
playback_speed = car_command.steering
row = data.iloc[
r] # position based indexing of DataFrame https://pythonhow.com/accessing-dataframe-columns-rows-and-cells/
# for index, row in data.iterrows():
self.car.car_state.command.autodrive_enabled = row['cmd.auto']
self.car.car_state.command.steering = row['cmd.steering']
self.car.car_state.command.throttle = row['cmd.throttle']
self.car.car_state.command.brake = row['cmd.brake']
self.car.car_state.command.reverse = row['cmd.reverse']
self.car.car_state.time = row['time']
self.car.car_state.position_m = Vector2(row['pos.x'], row['pos.y'])
self.car.car_state.velocity_m_per_sec = Vector2(row['vel.x'], row['vel.y'])
self.car.car_state.speed_m_per_sec = row['speed']
self.car.car_state.accel_m_per_sec_2 = Vector2(row['accel.x'], row['accel.y'])
self.car.car_state.steering_angle_deg = row['steering_angle']
self.car.car_state.body_angle_deg = row['body_angle']
self.car.car_state.yaw_rate_deg_per_sec = row['yaw_rate']
self.car.car_state.drift_angle_deg = row['drift_angle']
# Drawing
self.draw()
frac = float(r) / n_rows
w = frac * (self.screen.get_width() - 10)
pygame.draw.rect(self.screen, [200, 200, 200], [10, self.screen.get_height() - 20, w, 10], True)
pygame.display.flip()
if user_input.quit:
self.cleanup()
break
if user_input.restart_car:
r = 0
if playback_speed >= -0.05: # offset from zero to handle joysticks that have negative offset
r = r + step if r < n_rows - step else n_rows
if r > n_rows - 1: r = n_rows - 1
else:
r = r - step if r > 0 else 0
step = int(abs(playback_speed) * scale)
step = 1 if step < 1 else step
# speedup is factor times normal speed, limited by rendering rate
looper.rate_hz = self.fps * (1 + abs(scale * playback_speed))
return True
def load_data(args, filepath=None, inputs_list=None, outputs_list=None):
'''
Loads dataset from CSV file
Args:
filepath: path to CSV file
seq_len: number of samples in time input to RNN
stride: step over data in samples between samples
medfilt: median filter window, 0 to disable
test_plot: test_plot.py file requires other way of loading data
Returns:
Unnormalized numpy arrays
input_data: indexed by [sample, sequence, # sensor inputs * cw_len]
input dict: dictionary of input data names with key index into sensor index value
targets: indexed by [sample, sequence, # output sensor values * pw_len]
target dict: dictionary of target data names with key index into sensor index value
actual: raw input data indexed by [sample,sensor]
actual dict: dictionary of actual inputs with key index into sensor index value
mean_features, std_features, mean_targets_data, std_targets_data: the means and stds of training and raw_targets data.
These are vectors with length # of sensorss
The last dimension of input_data and targets is organized by
all sensors for sample0, all sensors for sample1, .....all sensors for sampleN, where N is the prediction length
'''
if filepath is None:
filepath = args.val_file_name
if inputs_list is None:
inputs_list = args.inputs_list
if outputs_list is None:
outputs_list = args.outputs_list
if type(filepath) == list:
filepaths = filepath
else:
filepaths = [filepath]
all_features = []
all_targets = []
for one_filepath in filepaths:
# Load dataframe
print('loading data from ' + str(one_filepath))
print('')
df = pd.read_csv(one_filepath, comment='#')
print('processing data to generate sequences')
print('')
# Wrap body_angle to be in range +/- 180
df['body_angle_deg'] = df['body_angle_deg'].apply(wrap_angle_deg)
print('Max angle is {} and min angle is {}.'.format(
max(df['body_angle_deg']), min(df['body_angle_deg'])))
df['body_angle.cos'] = df['body_angle_deg'].apply(cosdg)
df['body_angle.sin'] = df['body_angle_deg'].apply(sindg)
if df['body_angle_deg'].equals(df.apply(SinCos2Angle_wrapper, axis=1)):
print('Conversion of angle ideal')
print('')
else:
print('Angle conversion error: {}'.format(
max(
abs(df['body_angle_deg'] -
df.apply(SinCos2Angle_wrapper, axis=1)))))
print('')
# Calculate time difference between time steps and at it to data frame
time = df['time'].to_numpy()
deltaTime = np.diff(time)
deltaTime = np.insert(deltaTime, 0, 0)
df['dt'] = deltaTime
if args.extend_df:
# Calculate distance to the track edge in front of the car and add it to the data frame
# Get used car name
import re
s = str(pd.read_csv(one_filepath, skiprows=5, nrows=1))
track_name = re.search('"(.*)"', s).group(1)
media_folder_path = '../../media/tracks/'
my_track = track(track_name=track_name,
media_folder_path=media_folder_path)
def calculate_hit_distance(row):
return my_track.get_hit_distance(angle=row['body_angle_deg'],
x_car=row['position_m.x'],
y_car=row['position_m.y'])
df['hit_distance'] = df.apply(calculate_hit_distance, axis=1)
def nearest_waypoint_idx(row):
return my_track.get_nearest_waypoint_idx(x=row['position_m.x'],
y=row['position_m.y'])
df['nearest_waypoint_idx'] = df.apply(nearest_waypoint_idx, axis=1)
max_idx = max(df['nearest_waypoint_idx'])
def get_nth_next_waypoint_x(row, n: int):
return pixels2meters(
my_track.waypoints_x[int(row['nearest_waypoint_idx'] + n) %
max_idx])
def get_nth_next_waypoint_y(row, n: int):
return pixels2meters(
my_track.waypoints_y[int(row['nearest_waypoint_idx'] + n) %
max_idx])
df['first_next_waypoint.x'] = df.apply(get_nth_next_waypoint_x,
axis=1,
args=(1, ))
df['first_next_waypoint.y'] = df.apply(get_nth_next_waypoint_y,
axis=1,
args=(1, ))
df['fifth_next_waypoint.x'] = df.apply(get_nth_next_waypoint_x,
axis=1,
args=(5, ))
df['fifth_next_waypoint.y'] = df.apply(get_nth_next_waypoint_y,
axis=1,
args=(5, ))
df['twentieth_next_waypoint.x'] = df.apply(get_nth_next_waypoint_x,
axis=1,
args=(20, ))
df['twentieth_next_waypoint.y'] = df.apply(get_nth_next_waypoint_y,
axis=1,
args=(20, ))
if not args.do_not_normalize:
df['position_m.x'] /= normalization_distance
df['position_m.y'] /= normalization_distance
df['velocity_m_per_sec.x'] /= normalization_velocity
df['velocity_m_per_sec.y'] /= normalization_velocity
df['accel_m_per_sec_2.x'] /= normalization_acceleration
df['accel_m_per_sec_2.y'] /= normalization_acceleration
# https://stackoverflow.com/questions/2320986/easy-way-to-keeping-angles-between-179-and-180-degrees
# The first line is not workign for e.g. -200
# df['body_angle_deg'] = (((df['body_angle_deg'] + 180) % 360) - 180) / normalization_angle # Wrapping AND normalizing angle
df['body_angle_deg'] /= normalization_angle # normalizing angle
# 1) You already wrapped it in the above line.
# 2) For cos and sin you do not need to wrap the angle - they do it for you (periodic function)
# 3) You surely shouldn't normalize angle you put into sine and cos
# 4) Keep going. ;-)
# I move the wrapping above and do it always. Here leve only normalization (which sin, cos do not need)
# Wrapping due to:
# https://stackoverflow.com/questions/2320986/easy-way-to-keeping-angles-between-179-and-180-degrees
# df['body_angle.sin'] = np.sin(((((df['body_angle_deg'] + 180) % 360) - 180)*normalization_angle + 180)*np.pi/180)
# df['body_angle.cos'] = np.cos(((((df['body_angle_deg'] + 180) % 360) - 180)*normalization_angle + 180)*np.pi/180)
if args.extend_df:
df['hit_distance'] /= normalization_distance
df['first_next_waypoint.x'] /= normalization_distance
df['first_next_waypoint.y'] /= normalization_distance
df['fifth_next_waypoint.x'] /= normalization_distance
df['fifth_next_waypoint.y'] /= normalization_distance
df['twentieth_next_waypoint.x'] /= normalization_distance
df['twentieth_next_waypoint.y'] /= normalization_distance
# Get Raw Data
inputs = copy.deepcopy(df)
outputs = copy.deepcopy(df)
inputs.drop(inputs.tail(1).index, inplace=True) # Drop last row
outputs.drop(outputs.head(1).index, inplace=True)
inputs.reset_index(inplace=True) # Reset index
outputs.reset_index(inplace=True)
if args.cheat_dt and ('dt' in inputs_list):
inputs['dt'] = outputs['dt']
print('dt cheating enabled!')
print('')
inputs = inputs[inputs_list]
outputs = outputs[outputs_list]
features = np.array(inputs)
targets = np.array(outputs)
all_features.append(features)
all_targets.append(targets)
if type(filepath) == list:
return all_features, all_targets
else:
return features, targets