def decode_control(self, cod):
control = VehicleControl()
control.steer = 0
control.throttle = 0
control.brake = 0
control.hand_brake = False
control.reverse = False
if cod > 9:
control.hand_brake = True
if cod == 10:
control.steer = -1
elif cod == 10:
control.steer = -1
return control
if cod == 9:
control.reverse = True
control.throttle = 1
return control
if cod % 3 == 1:
control.brake = 1
elif cod % 3 == 2:
control.throttle = 1
if cod // 3 == 1:
control.steer = 1
elif cod // 3 == 2:
control.steer = -1
return control
def action_joystick(self):
# joystick
steering_axis = self.joystick.get_axis(0)
acc_axis = self.joystick.get_axis(2)
brake_axis = self.joystick.get_axis(5)
# print("axis 0 %f, axis 2 %f, axis 3 %f" % (steering_axis, acc_axis, brake_axis))
if (self.joystick.get_button(3)):
self._rear = True
if (self.joystick.get_button(2)):
self._rear = False
control = VehicleControl()
control.steer = steering_axis
control.throttle = (acc_axis + 1) / 2.0
control.brake = (brake_axis + 1) / 2.0
if control.brake < 0.001:
control.brake = 0.0
control.hand_brake = 0
control.reverse = self._rear
control.steer -= 0.0822
#print("steer %f, throttle %f, brake %f" % (control.steer, control.throttle, control.brake))
pygame.event.pump()
return control
def _compute_action(self, rgb_image, speed, directions=None):
#shit for demo
img = cv2.cvtColor(rgb_image, cv2.COLOR_RGB2BGR)
cv2.imwrite(self.img_dir + "/image_" + str(self.nr_img) + ".jpg", img)
#shit for demo
rgb_image = rgb_image[self._image_cut[0]:self._image_cut[1], :]
steer, acc, brake = self._control_function(rgb_image, speed,
directions)
# This a bit biased, but is to avoid fake breaking
if brake < 0.1:
brake = 0.0
if acc > brake:
brake = 0.0
# We limit speed to 35 km/h to avoid
if speed > 10.0 and brake == 0:
acc = 0.0
control = VehicleControl()
control.steer = steer
control.throttle = acc
control.brake = brake
control.hand_brake = 0
control.reverse = 0
return control
def control(self):
vcontrol = VehicleControl()
# Press Y
if self.controller.get_button(3):
return None
# Left stick
steer = self.controller.get_axis(0)
if abs(steer) >= 0.1:
vcontrol.steer = steer / 2.
# Right stick
throttle = -self.controller.get_axis(4)
if throttle > 0:
vcontrol.throttle = abs(throttle) / 1.5
else:
vcontrol.brake = abs(throttle)
# Press right stick
if self.controller.get_button(10):
vcontrol.hand_brake = True
self.client.send_control(vcontrol)
self.info[
'Throttle'] = vcontrol.throttle if throttle > 0 else -vcontrol.brake
self.info['Steer'] = vcontrol.steer
def _control_using_agent(self, action):
'''
Here we pass the action output by our agent
and control the car
outputs :
As of now consider the agent also outputs the keys W, A, S, D, space, reverse
so according to the output vector "action"
For example :
action = [1, 1, 0, 0, 0, 0] => it uses throttle and steer left
action = [0, 0, 0, 0, 0, 1] => car will be on reverse gear
'''
'''To be completed'''
# if action[6]:
# # if r is pressed
# return None
control = VehicleControl()
if action[1]:
control.steer = -1.0
if action[3]:
control.steer = 1.0
if action[0]:
control.throttle = 1.0
if action[2]:
control.brake = 1.0
if action[4]:
control.hand_brake = True
if action[5]:
self._is_on_reverse = not self._is_on_reverse
control.reverse = self._is_on_reverse
return control
def send_control_command(client, throttle, steer, brake,
hand_brake=False, reverse=False):
"""Send control command to CARLA client.
Send control command to CARLA client.
Args:
client: The CARLA client object
throttle: Throttle command for the sim car [0, 1]
steer: Steer command for the sim car [-1, 1]
brake: Brake command for the sim car [0, 1]
hand_brake: Whether the hand brake is engaged
reverse: Whether the sim car is in the reverse gear
"""
control = VehicleControl()
# Clamp all values within their limits
steer = np.fmax(np.fmin(steer, 1.0), -1.0)
throttle = np.fmax(np.fmin(throttle, 1.0), 0)
brake = np.fmax(np.fmin(brake, 1.0), 0)
control.steer = steer
control.throttle = throttle
control.brake = brake
control.hand_brake = hand_brake
control.reverse = reverse
client.send_control(control)
def _get_keyboard_control(self, keys, args):
"""
Return a VehicleControl message based on the pressed keys. Return None
if a new episode was requested.
"""
global flg_warning
if keys[K_r]:
return None
control = VehicleControl()
if keys[K_LEFT] or keys[K_a]:
control.steer = -1.0
if keys[K_RIGHT] or keys[K_d]:
control.steer = 1.0
if keys[K_UP] or keys[K_w]:
control.throttle = 1.0
if keys[K_DOWN] or keys[K_s]:
control.brake = 1.0
if keys[K_SPACE]:
control.hand_brake = True
if keys[K_q]:
self._is_on_reverse = not self._is_on_reverse
if keys[K_p]:
self._enable_autopilot = not self._enable_autopilot
if args.app == 'Control':
if flg_warning == -1:
control.throttle = 0.0
control.reverse = self._is_on_reverse
return control
def _get_keyboard_control(self, keys):
"""
Return a VehicleControl message based on the pressed keys. Return None
if a new episode was requested.
"""
if keys[K_r]:
return None
control = VehicleControl()
if keys[K_LEFT] or keys[K_a]:
control.steer = -0.5
if keys[K_RIGHT] or keys[K_d]:
control.steer = 0.5
if keys[K_UP] or keys[K_w]:
control.throttle = 0.7
if keys[K_DOWN] or keys[K_s]:
control.brake = 1.0
if keys[K_SPACE]:
control.hand_brake = True
if keys[K_q]:
self._is_on_reverse = not self._is_on_reverse
if keys[K_p]:
self._enable_autopilot = not self._enable_autopilot
if keys[K_2]:
self._command = 2
if keys[K_3]:
self._command = 3
if keys[K_4]:
self._command = 4
if keys[K_5]:
self._command = 5
control.reverse = self._is_on_reverse
return control
def _get_keyboard_control(self, keys):
"""
Return a VehicleControl message based on the pressed keys. Return None
if a new episode was requested.
"""
if keys[K_r]:
return None
control = VehicleControl()
if keys[K_LEFT] or keys[K_a]:
control.steer = -0.5
if keys[K_RIGHT] or keys[K_d]:
control.steer = 0.5
if keys[K_UP] or keys[K_w]:
control.throttle = 0.7
if keys[K_DOWN] or keys[K_s]:
control.brake = 1.0
if keys[K_SPACE]:
control.hand_brake = True
if keys[K_q]:
self._is_on_reverse = not self._is_on_reverse
if keys[K_p]:
self._enable_autopilot = not self._enable_autopilot
if keys[K_2]:
self._command = 2
if keys[K_3]:
self._command = 3
if keys[K_4]:
self._command = 4
if keys[K_5]:
self._command = 5
control.reverse = self._is_on_reverse
return control
def _get_keyboard_control(self, keys, measurements):
"""
Return a VehicleControl message based on the pressed keys. Return None
if a new episode was requested.
"""
if keys[K_p]:
control = measurements.player_measurements.autopilot_control
control.steer += random.uniform(-0.1, 0.1)
return control
if keys[K_r]:
return None
control = VehicleControl()
if keys[K_LEFT] or keys[K_a]:
control.steer = -1.0
if keys[K_RIGHT] or keys[K_d]:
control.steer = 1.0
if keys[K_UP] or keys[K_w]:
control.throttle = 1.0
if keys[K_DOWN] or keys[K_s]:
control.brake = 1.0
if keys[K_SPACE]:
control.hand_brake = True
if keys[K_q]:
self._is_on_reverse = not self._is_on_reverse
control.reverse = self._is_on_reverse
return control
def _get_keyboard_control(self, keys):
"""
Return a VehicleControl message based on the pressed keys. Return None
if a new episode was requested.
"""
if keys[K_r]:
return None, None
if keys[K_t]:
self.should_display = not self.should_display
return 'done', None
if keys[K_m]:
self.manual = True
return 'done', None
if keys[K_n]:
self.manual = False
return 'done', None
if keys[K_v]:
self.endnow = True
return 'done', None
control = VehicleControl()
if keys[K_LEFT] or keys[K_a]:
control.steer = -1.0
if keys[K_RIGHT] or keys[K_d]:
control.steer = 1.0
if keys[K_UP] or keys[K_w]:
control.throttle = 1.0
if keys[K_DOWN] or keys[K_s]:
control.brake = 1.0
if keys[K_SPACE]:
control.hand_brake = True
if keys[K_q]:
self._is_on_reverse = not self._is_on_reverse
if keys[K_c]:
self.manual_control = not self.manual_control
if keys[K_p]:
self._enable_autopilot = not self._enable_autopilot
control.reverse = self._is_on_reverse
reward = None
if keys[K_1]:
reward = -1
if keys[K_2]:
reward = -0.5
if keys[K_3]:
reward = -0.25
if keys[K_4]:
reward = -0.1
if keys[K_5]:
reward = 0
if keys[K_6]:
reward = 0.1
if keys[K_7]:
reward = 0.25
if keys[K_8]:
reward = 0.5
if keys[K_9]:
reward = 1
return control, reward
def get_control_from_a(a):
control = VehicleControl()
control.steer = a[0]
control.throttle = a[1]
control.brake = a[2]
control.hand_brake = bool(a[3])
control.reverse = bool(a[4])
return control
def _get_keyboard_control(self, keys, measurements, sensor_data):
"""
Return a VehicleControl message based on the pressed keys. Return None
if a new episode was requested.
"""
self._at_intersection = False
if keys[K_r]:
return None
if self._NNagent_drives:
if keys[K_LEFT] or keys[K_a]:
control = self.NNagent.run_step(measurements, sensor_data, 3,
None)
elif keys[K_RIGHT] or keys[K_d]:
control = self.NNagent.run_step(measurements, sensor_data, 4,
None)
elif keys[K_UP] or keys[K_w]:
control = self.NNagent.run_step(measurements, sensor_data, 2,
None)
else:
control = self.NNagent.run_step(measurements, sensor_data, -1,
None)
if keys[K_q]:
self._is_on_reverse = not self._is_on_reverse
if keys[K_p]:
self._enable_autopilot = True
self._NNagent_drives = not self._NNagent_drives
if keys[K_n]:
print("Turning off")
self._NNagent_drives = not self._NNagent_drives
if keys[K_i]:
self._at_intersection = True
control.reverse = self._is_on_reverse
else:
control = VehicleControl()
if keys[K_LEFT] or keys[K_a]:
control.steer = -1.0
if keys[K_RIGHT] or keys[K_d]:
control.steer = 1.0
if keys[K_UP] or keys[K_w]:
control.throttle = 1.0
if keys[K_DOWN] or keys[K_s]:
control.brake = 1.0
if keys[K_SPACE]:
control.hand_brake = True
if keys[K_q]:
self._is_on_reverse = not self._is_on_reverse
if keys[K_p]:
self._enable_autopilot = not self._enable_autopilot
if keys[K_n]:
print("Turning on")
self._NNagent_drives = not self._NNagent_drives
self._enable_autopilot = False
if keys[K_i]:
self._at_intersection = True
control.reverse = self._is_on_reverse
return control
def _get_autopilot_control(self, measurements):
control = VehicleControl()
control.steer = measurements.player_measurements.autopilot_control.steer
control.throttle = measurements.player_measurements.autopilot_control.throttle
control.brake = measurements.player_measurements.autopilot_control.brake
control.hand_brake = measurements.player_measurements.autopilot_control.hand_brake
control.reverse = measurements.player_measurements.autopilot_control.reverse
return control
def _get_keyboard_control(self, keys):
"""
Return a VehicleControl message based on the pressed keys. Return None
if a new episode was requested.
"""
if keys[K_r]:
return None
control = VehicleControl()
self.resetVal()
################################################### YOU CAN EDIT IT ACCORDING YOU....
if keys[K_LEFT] or keys[K_a]:
self._val1 = -1
print("Left")
if (pid.prev > 0):
pid.prev = 0
elif keys[K_RIGHT] or keys[K_d]:
self._val1 = 1
print("Right")
if (pid.prev < 0):
pid.prev = 0
pid.prev += self._val1
output = pid.kp * self._val1 + pid.ki * pid.prev
print(output)
control.steer = output #Imp Line
if keys[K_UP] or keys[K_w]:
self._val2 = 1
pid.prev = 0
elif keys[K_DOWN] or keys[K_s]:
control.brake = 1
pid.prev = 0
###
if (self._velocity < 77):
control.throttle = self._val2 #Imp Line
else:
control.throttle = self._val2 * 0.8
if keys[K_SPACE]:
control.hand_brake = True
pid.prev = 0
if keys[K_f]:
self._val3 = 1 - self._val3
if keys[K_q]:
self._is_on_reverse = not self._is_on_reverse
pid.prev = 0
if keys[K_p]:
self._enable_autopilot = not self._enable_autopilot
control.reverse = self._is_on_reverse
################################################################################
#print(control)
return control
def _get_keyboard_control(self, keys):
control = VehicleControl()
if keys[pl.K_LEFT] or keys[pl.K_a]:
control.steer = -1.0
if keys[pl.K_RIGHT] or keys[pl.K_d]:
control.steer = 1.0
if keys[pl.K_UP] or keys[pl.K_w]:
control.throttle = 1.0
if keys[pl.K_DOWN] or keys[pl.K_s]:
control.brake = 1.0
if keys[pl.K_SPACE]:
control.hand_brake = True
control.reverse = self._vehicle_in_reverse
return control
def _get_steering_control(self):
numAxes = self.js.get_numaxes()
jsInputs = [float(self.js.get_axis(i)) for i in range(numAxes)]
# print('Js inputs [%s]' % ', '.join(map(str, jsInputs)))
control = VehicleControl()
control.steer = jsInputs[0]
if ((1 - jsInputs[1]) / 2) > 0.001:
control.brake = (1 - jsInputs[1]) / 2
else:
control.brake = 0
if ((1 - jsInputs[2]) / 2) > 0.001:
control.throttle = (1 - jsInputs[2]) / 2
else:
control.throttle = 0
control.hand_brake = 0.0
control.reverse = 0.0
# print(control)
return control
def action_steering_wheel(self, jsInputs, jsButtons):
control = VehicleControl()
# Custom function to map range of inputs [1, -1] to outputs [0, 1] i.e 1 from inputs means nothing is pressed
# For the steering, it seems fine as it is
K1 = 1.0 # 0.55
steerCmd = K1 * math.tan(1.1 * jsInputs[self._steer_idx])
K2 = 1.6 # 1.6
throttleCmd = K2 + (
2.05 * math.log10(-0.7 * jsInputs[self._throttle_idx] + 1.4) -
1.2) / 0.92
if throttleCmd <= 0:
throttleCmd = 0
elif throttleCmd > 1:
throttleCmd = 1
brakeCmd = 1.6 + (2.05 * math.log10(-0.7 * jsInputs[self._brake_idx] +
1.4) - 1.2) / 0.92
if brakeCmd <= 0:
brakeCmd = 0
elif brakeCmd > 1:
brakeCmd = 1
#print("Steer Cmd, ", steerCmd, "Brake Cmd", brakeCmd, "ThrottleCmd", throttleCmd)
control.steer = steerCmd
control.brake = brakeCmd
control.throttle = throttleCmd
toggle = jsButtons[self._reverse_idx]
if toggle == 1:
self._is_on_reverse += 1
if self._is_on_reverse % 2 == 0:
control.reverse = False
if self._is_on_reverse > 1:
self._is_on_reverse = True
if self._is_on_reverse:
control.reverse = True
control.hand_brake = False # jsButtons[self.handbrake_idx]
return control
def _on_loop(self):
self._timer.tick()
measurements, _ = self.client.read_data()
self.pubState(measurements.player_measurements)
# Print measurements every second.
if self._timer.elapsed_seconds_since_lap() > 1.0:
self._print_player_measurements(measurements.player_measurements)
# Plot position on the map as well.
self._timer.lap()
if self._vehicle_control.restart:
self._on_new_episode()
else:
control = VehicleControl()
control.throttle = self._vehicle_control.accel_cmd.accel
control.brake = self._vehicle_control.brake_cmd.brake
control.steer = self._vehicle_control.steer_cmd.steer
control.hand_brake = self._vehicle_control.hand_brake
control.reverse = self._vehicle_control.reverse
self.client.send_control(control)
def decode_control(self, cod):
#将数字的ｃｏｎｔｒｏｌ转换为VehicleControl()
control = VehicleControl()
control.steer = 0
control.throttle = 0
control.brake = 0
control.hand_brake = False
control.reverse = False
th, steer = cod
if th > 0:
control.throttle = min(th, 1.0)
if th < 0:
control.brake = min(-th, 1.0)
control.steer = max(min(steer, 1.0), -1.0)
# if cod > 9:
# control.hand_brake = True
# if cod == 10:
# control.steer = -1
# elif cod == 11:
# control.steer = 1
# return control
# if cod == 9:
# control.reverse = True
# control.throttle = 1
# return control
# if cod % 3 == 1:
# control.brake = 1
# elif cod % 3 == 2:
# control.throttle = 1
# if cod // 3 == 1:
# control.steer = 1
# elif cod // 3 == 2:
# control.steer = -1
return control
def get_keyboard_control(keys, is_on_reverse, enable_autopilot):
if keys[K_r]:
return None
control = VehicleControl()
if keys[K_LEFT] or keys[K_a]:
control.steer = -1.0
if keys[K_RIGHT] or keys[K_d]:
control.steer = 1.0
if keys[K_UP] or keys[K_w]:
control.throttle = 1.0
if keys[K_DOWN] or keys[K_s]:
control.brake = 1.0
if keys[K_SPACE]:
control.hand_brake = True
if keys[K_q]:
is_on_reverse = not is_on_reverse
if keys[K_p]:
enable_autopilot = not enable_autopilot
control.reverse = is_on_reverse
return control, is_on_reverse, enable_autopilot
def _get_keyboard_control(self, keys):
"""
Return a VehicleControl message based on the pressed keys. Return None
if a new episode was requested.
"""
if keys[K_r]:
return None
control = VehicleControl()
if keys[K_LEFT] or keys[K_a]:
control.steer = -1.0
if keys[K_RIGHT] or keys[K_d]:
control.steer = 1.0
if keys[K_UP] or keys[K_w]:
control.throttle = 1.0
if keys[K_DOWN] or keys[K_s]:
control.brake = 1.0
if keys[K_SPACE]:
control.hand_brake = True
if keys[K_q]:
self._is_on_reverse = not self._is_on_reverse
control.reverse = self._is_on_reverse
return control
def get_keyboard_control(self, char):
control = VehicleControl()
if char == 'a':
print 'got a'
control.steer = -1.0
if char == 'd':
print 'got d'
control.steer = 1.0
if char == 'w':
print 'got w'
control.throttle = 1.0
if char == 's':
print 'got s'
control.brake = 1.0
if char == ' ':
print 'got space'
control.hand_brake = True
if char == 'q':
print 'got q'
self._is_on_reverse = not self._is_on_reverse
control.reverse = self._is_on_reverse
return control
def _get_keyboard_control(self, keys):
"""
Return a VehicleControl message based on the pressed keys. Return None
if a new episode was requested.
"""
if keys[K_r]:
return None
control = VehicleControl()
if keys[K_LEFT] or keys[K_a]:
#control.steer = -1.0
if self.steer > -1 and self.steer < 0:
control.steer = self.steer - .1
else:
control.steer = -1
self.steer = control.steer
if keys[K_RIGHT] or keys[K_d]:
#control.steer = 1.0
if self.steer > 0 and self.steer < 1:
control.steer = self.steer + .1
else:
control.steer = 1
self.steer = control.steer
if keys[K_UP] or keys[K_w]:
control.throttle = 1.0
if keys[K_DOWN] or keys[K_s]:
control.brake = 1.0
if keys[K_SPACE]:
control.hand_brake = True
if keys[K_q]:
self._is_on_reverse = not self._is_on_reverse
if keys[K_p]:
self._enable_autopilot = not self._enable_autopilot
control.reverse = self._is_on_reverse
return control
def _get_keyboard_control(self, keys):
"""
Return a VehicleControl message based on the pressed keys. Return None
if a new episode was requested.
"""
#returns NONE to close simulator
if keys[K_r]:
return None
#control is set to its accurate type
control = VehicleControl()
#input set to manual
if self._input_control == "Manual":
if self._xbox_cont:
#updates and checks for input from controller
pygame.event.pump()
#get_axis values may differ depending on controller
#values weighted to be used on carla
control.steer = self._joystick.get_axis(3)
print(control.steer)
control.throttle = (self._joystick.get_axis(5) + 1) / 2
control.brake = (self._joystick.get_axis(2) + 1) / 2
self._Manual_steer = control.steer
else:
if keys[K_LEFT] or keys[K_a]:
control.steer = -1.0
if keys[K_RIGHT] or keys[K_d]:
control.steer = 1.0
if keys[K_UP] or keys[K_w]:
control.throttle = 1.0
if keys[K_DOWN] or keys[K_s]:
control.brake = 1.0
#replay of previously recorded data
elif self._input_control == "Replay":
#read replay.csv file, _replay_frame values gives us correct row
control.steer = replay.iloc[self._replay_frame, 4]
control.throttle = replay.iloc[self._replay_frame, 6]
#input set to autonomous, steer values from model
else:
control.steer = self._AI_steer
control.throttle = 0.5
print(control.steer)
if keys[K_l]:
self._data_collection = not self._data_collection
if self._data_collection:
print("Data Collection ON")
else:
print("Data Collection OFF")
time.sleep(0.05)
#Check for keyboard commands to change input control device
if keys[K_m]:
print("Manual Control")
self._input_control = "Manual"
if keys[K_k]:
print("Manual Control: Takeover Noted")
self._takeovers += 1
self._input_control = "Manual"
if keys[K_n]:
print("AI Control")
self._input_control = "AI"
if keys[K_b]:
print("Replay Control")
self._input_control = "Replay"
if keys[K_SPACE]:
control.hand_brake = True
if keys[K_q]:
self._is_on_reverse = not self._is_on_reverse
time.sleep(0.05)
if keys[K_p]:
self._enable_autopilot = not self._enable_autopilot
control.reverse = self._is_on_reverse
return control
def throw_brake(self): control = VehicleControl() control.hand_brake = True self.client.send_control(control)
def run_carla_client(host, port, far):
# Here we will run a single episode with 300 frames.
number_of_frames = 30
frame_step = 10 # Save one image every 100 frames
image_size = [800, 600]
camera_local_pos = [0.3, 0.0, 1.3] # [X, Y, Z]
camera_local_rotation = [0, 0, 0] # [pitch(Y), yaw(Z), roll(X)]
fov = 70
autopilot = False
control = VehicleControl()
for start_i in range(150):
output_folder = '/home2/mariap/Packages/CARLA_0.8.2/PythonClient/_out/pos_' + str(
start_i)
if not os.path.exists(output_folder):
os.mkdir(output_folder)
print("make " + str(output_folder))
# Connect with the server
with make_carla_client(host, port) as client:
print('CarlaClient connected')
for start_i in range(150):
output_folder = '/home2/mariap/Packages/CARLA_0.8.2/PythonClient/_out/pos_' + str(
start_i)
print(output_folder)
# Here we load the settings.
settings = CarlaSettings()
settings.set(SynchronousMode=True,
SendNonPlayerAgentsInfo=True,
NumberOfVehicles=100,
NumberOfPedestrians=500,
WeatherId=random.choice([1, 3, 7, 8, 14]))
settings.randomize_seeds()
camera1 = Camera('CameraDepth', PostProcessing='Depth', FOV=fov)
camera1.set_image_size(*image_size)
camera1.set_position(*camera_local_pos)
camera1.set_rotation(*camera_local_rotation)
settings.add_sensor(camera1)
camera2 = Camera('CameraRGB', PostProcessing='SceneFinal', FOV=fov)
camera2.set_image_size(*image_size)
camera2.set_position(*camera_local_pos)
camera2.set_rotation(*camera_local_rotation)
settings.add_sensor(camera2)
camera3 = Camera('CameraSeg',
PostProcessing='SemanticSegmentation')
camera3.set_image_size(*image_size)
camera3.set_position(*camera_local_pos)
camera3.set_rotation(*camera_local_rotation)
settings.add_sensor(camera3)
client.load_settings(settings)
# Start at location index id '0'
client.start_episode(start_i)
cameras_dict = {}
pedestrians_dict = {}
cars_dict = {}
# Compute the camera transform matrix
camera_to_car_transform = camera2.get_unreal_transform()
# (Intrinsic) (3, 3) K Matrix
K = np.identity(3)
K[0, 2] = image_size[0] / 2.0
K[1, 2] = image_size[1] / 2.0
K[0,
0] = K[1,
1] = image_size[0] / (2.0 * np.tan(fov * np.pi / 360.0))
with open(output_folder + '/camera_intrinsics.p', 'w') as camfile:
pickle.dump(K, camfile)
# Iterate every frame in the episode except for the first one.
for frame in range(1, number_of_frames):
# Read the data produced by the server this frame.
measurements, sensor_data = client.read_data()
# Save one image every 'frame_step' frames
if not frame % frame_step:
for name, measurement in sensor_data.items():
filename = '{:s}/{:0>6d}'.format(name, frame)
measurement.save_to_disk(
os.path.join(output_folder, filename))
# Start transformations time mesure.
timer = StopWatch()
# RGB image [[[r,g,b],..[r,g,b]],..[[r,g,b],..[r,g,b]]]
image_RGB = to_rgb_array(sensor_data['CameraRGB'])
image_seg = np.tile(
labels_to_array(sensor_data['CameraSeg']), (1, 1, 3))
# 2d to (camera) local 3d
# We use the image_RGB to colorize each 3D point, this is optional.
# "max_depth" is used to keep only the points that are near to the
# camera, meaning 1.0 the farest points (sky)
point_cloud = depth_to_local_point_cloud(
sensor_data['CameraDepth'], image_RGB, max_depth=far)
point_cloud_seg = depth_to_local_point_cloud(
sensor_data['CameraDepth'], image_seg, max_depth=far)
# (Camera) local 3d to world 3d.
# Get the transform from the player protobuf transformation.
world_transform = Transform(
measurements.player_measurements.transform)
# Compute the final transformation matrix.
car_to_world_transform = world_transform * camera_to_car_transform
# Car to World transformation given the 3D points and the
# transformation matrix.
point_cloud.apply_transform(car_to_world_transform)
point_cloud_seg.apply_transform(car_to_world_transform)
Rt = car_to_world_transform.matrix
Rt_inv = car_to_world_transform.inverse(
).matrix # Rt_inv is the world to camera matrix !!
#R_inv=world_transform.inverse().matrix
cameras_dict[frame] = {}
cameras_dict[frame]['inverse_rotation'] = Rt_inv[:]
cameras_dict[frame]['rotation'] = Rt[:]
cameras_dict[frame]['translation'] = Rt_inv[0:3, 3]
cameras_dict[frame][
'camera_to_car'] = camera_to_car_transform.matrix
# Get non-player info
vehicles = {}
pedestrians = {}
for agent in measurements.non_player_agents:
# check if the agent is a vehicle.
if agent.HasField('vehicle'):
pos = agent.vehicle.transform.location
pos_vector = np.array([[pos.x], [pos.y], [pos.z],
[1.0]])
trnasformed_3d_pos = np.dot(Rt_inv, pos_vector)
pos2d = np.dot(K, trnasformed_3d_pos[:3])
# Normalize the point
norm_pos2d = np.array([
pos2d[0] / pos2d[2], pos2d[1] / pos2d[2],
pos2d[2]
])
# Now, pos2d contains the [x, y, d] values of the image in pixels (where d is the depth)
# You can use the depth to know the points that are in front of the camera (positive depth).
x_2d = image_size[0] - norm_pos2d[0]
y_2d = image_size[1] - norm_pos2d[1]
vehicles[agent.id] = {}
vehicles[agent.id]['transform'] = [
agent.vehicle.transform.location.x,
agent.vehicle.transform.location.y,
agent.vehicle.transform.location.z
]
vehicles[agent.id][
'bounding_box.transform'] = agent.vehicle.bounding_box.transform.location.z
vehicles[agent.id][
'yaw'] = agent.vehicle.transform.rotation.yaw
vehicles[agent.id]['bounding_box'] = [
agent.vehicle.bounding_box.extent.x,
agent.vehicle.bounding_box.extent.y,
agent.vehicle.bounding_box.extent.z
]
vehicle_transform = Transform(
agent.vehicle.bounding_box.transform)
pos = agent.vehicle.transform.location
bbox3d = agent.vehicle.bounding_box.extent
# Compute the 3D bounding boxes
# f contains the 4 points that corresponds to the bottom
f = np.array([[
pos.x + bbox3d.x, pos.y - bbox3d.y,
pos.z - bbox3d.z +
agent.vehicle.bounding_box.transform.location.z
],
[
pos.x + bbox3d.x,
pos.y + bbox3d.y,
pos.z - bbox3d.z + agent.vehicle.
bounding_box.transform.location.z
],
[
pos.x - bbox3d.x,
pos.y + bbox3d.y,
pos.z - bbox3d.z + agent.vehicle.
bounding_box.transform.location.z
],
[
pos.x - bbox3d.x,
pos.y - bbox3d.y,
pos.z - bbox3d.z + agent.vehicle.
bounding_box.transform.location.z
],
[
pos.x + bbox3d.x,
pos.y - bbox3d.y,
pos.z + bbox3d.z + agent.vehicle.
bounding_box.transform.location.z
],
[
pos.x + bbox3d.x,
pos.y + bbox3d.y,
pos.z + bbox3d.z + agent.vehicle.
bounding_box.transform.location.z
],
[
pos.x - bbox3d.x,
pos.y + bbox3d.y,
pos.z + bbox3d.z + agent.vehicle.
bounding_box.transform.location.z
],
[
pos.x - bbox3d.x,
pos.y - bbox3d.y,
pos.z + bbox3d.z + agent.vehicle.
bounding_box.transform.location.z
]])
f_rotated = vehicle_transform.transform_points(f)
f_2D_rotated = []
vehicles[
agent.id]['bounding_box_coord'] = f_rotated
for i in range(f.shape[0]):
point = np.array([[f_rotated[i, 0]],
[f_rotated[i, 1]],
[f_rotated[i, 2]], [1]])
transformed_2d_pos = np.dot(
Rt_inv,
point) # 3d Position in camera space
pos2d = np.dot(
K, transformed_2d_pos[:3]
) # Conversion to camera frustum space
norm_pos2d = np.array([
pos2d[0] / pos2d[2], pos2d[1] / pos2d[2],
pos2d[2]
])
#print([image_size[0] - (pos2d[0] / pos2d[2]), image_size[1] - (pos2d[1] / pos2d[2])])
f_2D_rotated.append(
np.array([
image_size[0] - norm_pos2d[0],
image_size[1] - norm_pos2d[1]
]))
vehicles[
agent.id]['bounding_box_2D'] = f_2D_rotated
elif agent.HasField('pedestrian'):
pedestrians[agent.id] = {}
pedestrians[agent.id]['transform'] = [
agent.pedestrian.transform.location.x,
agent.pedestrian.transform.location.y,
agent.pedestrian.transform.location.z
]
pedestrians[agent.id][
'yaw'] = agent.pedestrian.transform.rotation.yaw
pedestrians[agent.id]['bounding_box'] = [
agent.pedestrian.bounding_box.extent.x,
agent.pedestrian.bounding_box.extent.y,
agent.pedestrian.bounding_box.extent.z
]
vehicle_transform = Transform(
agent.pedestrian.bounding_box.transform)
pos = agent.pedestrian.transform.location
bbox3d = agent.pedestrian.bounding_box.extent
# Compute the 3D bounding boxes
# f contains the 4 points that corresponds to the bottom
f = np.array(
[[pos.x + bbox3d.x, pos.y - bbox3d.y, pos.z],
[pos.x + bbox3d.x, pos.y + bbox3d.y, pos.z],
[pos.x - bbox3d.x, pos.y + bbox3d.y, pos.z],
[pos.x - bbox3d.x, pos.y - bbox3d.y, pos.z],
[
pos.x + bbox3d.x, pos.y - bbox3d.y,
pos.z + bbox3d.z
],
[
pos.x + bbox3d.x, pos.y + bbox3d.y,
pos.z + bbox3d.z
],
[
pos.x - bbox3d.x, pos.y + bbox3d.y,
pos.z + bbox3d.z
],
[
pos.x - bbox3d.x, pos.y - bbox3d.y,
pos.z + bbox3d.z
]])
f_rotated = vehicle_transform.transform_points(f)
pedestrians[
agent.id]['bounding_box_coord'] = f_rotated
f_2D_rotated = []
for i in range(f.shape[0]):
point = np.array([[f_rotated[i, 0]],
[f_rotated[i, 1]],
[f_rotated[i, 2]], [1]])
transformed_2d_pos = np.dot(
Rt_inv, point) # See above for cars
pos2d = np.dot(K, transformed_2d_pos[:3])
norm_pos2d = np.array([
pos2d[0] / pos2d[2], pos2d[1] / pos2d[2],
pos2d[2]
])
f_2D_rotated.append([
image_size[0] - norm_pos2d[0],
image_size[1] - norm_pos2d[1]
])
pedestrians[
agent.id]['bounding_box_2D'] = f_2D_rotated
cars_dict[frame] = vehicles
pedestrians_dict[frame] = pedestrians
# End transformations time mesure.
timer.stop()
# Save PLY to disk
# This generates the PLY string with the 3D points and the RGB colors
# for each row of the file.
point_cloud.save_to_disk(
os.path.join(output_folder,
'{:0>5}.ply'.format(frame)))
point_cloud_seg.save_to_disk(
os.path.join(output_folder,
'{:0>5}_seg.ply'.format(frame)))
print_message(timer.milliseconds(), len(point_cloud),
frame)
if autopilot:
client.send_control(
measurements.player_measurements.autopilot_control)
else:
control.hand_brake = True
client.send_control(control)
with open(output_folder + '/cameras.p', 'w') as camerafile:
pickle.dump(cameras_dict, camerafile)
print(output_folder + "cameras.p")
with open(output_folder + '/people.p', 'w') as peoplefile:
pickle.dump(pedestrians_dict, peoplefile)
with open(output_folder + '/cars.p', 'w') as carfile:
pickle.dump(cars_dict, carfile)
