def _compute_action(self, carla_direction, direction):
start = time.time()
# Predict the intermediate representations
prediction = self._neural_net.predict(self._state.image_hist,
[direction])
logging.info("Time for prediction: {}".format(time.time() - start))
logging.info("CARLA Direction {}, Real Direction {}".format(
carla_direction, direction))
# update the speed limit if a speed limit sign is detected
if prediction['speed_sign'][0] != -1:
self._state.speed_limit = prediction['speed_sign']
# Calculate the control
control = carla.VehicleControl()
control.throttle, control.brake = self._longitudinal_control(
prediction, direction)
control.steer = self._lateral_control(prediction)
print(control)
return control
def applyAction(self, action):
speed_limit = 10
if self.world.velocAtual() >= speed_limit:
throttle=0.0
print("\nAcima do limite de velocidade de", speed_limit, "km/h...")
else:
throttle=0.5
actions = (
(0.0, 0.0, False), #sem acao
(throttle, 0.0, False), #frente
(throttle, -0.5, False), #frente-direita
(throttle, 0.5, False), #frente-esquerda
(throttle, 0.0, True), #ré/freio
(throttle, -0.5, True), #ré-direita
(throttle, 0.5, True) #ré-esquerda
) #7 acoes
print("\tAção: ", actions[action])
self.player.apply_control(carla.VehicleControl(actions[action][0], actions[action][1], reverse=actions[action][2]))
return None
def _get_keyboard_control(self, keys):
control = carla.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 time.time() - self._last_toggle > 1:
if keys[K_q]:
self._last_toggle = time.time()
self._is_on_reverse = not self._is_on_reverse
print("Reverse: ", self._is_on_reverse)
if keys[K_p]:
self._last_toggle = time.time()
self._autopilot_enabled = not self._autopilot_enabled
print("Autopilot: ", self._autopilot_enabled)
control.reverse = self._is_on_reverse
return control
def run_step(self, target_speed, relative_angle):
# TODO this run step needs to have the waypoint changed to relative angle.
"""
Execute one step of control invoking both lateral and longitudinal PID controllers to reach a target waypoint
at a given target_speed.
:param target_speed: desired vehicle speed
:param waypoint: target location encoded as a waypoint
:return: distance (in meters) to the waypoint
"""
throttle = self._lon_controller.run_step(target_speed)
steering = self._lat_controller.run_step(relative_angle)
control = carla.VehicleControl()
control.steer = steering
control.throttle = throttle
control.brake = 0.0
control.hand_brake = False
control.manual_gear_shift = False
return control
def run_step(
self, vehicle: carla.Vehicle, next_waypoint: carla.Waypoint, left_lane_x, right_lane_x
) -> carla.VehicleControl:
"""
run one step of Pure Pursuit Control
Args:
vehicle: current vehicle state
next_waypoint: Next waypoint, Waypoint
**kwargs:
Returns:
Vehicle Control
"""
control = carla.VehicleControl(
throttle=self.longitunal_controller.run_step(vehicle=vehicle),
steer=self.latitunal_controller.run_step(
vehicle=vehicle, next_waypoint=next_waypoint,
left_lane_x=left_lane_x, right_lane_x=right_lane_x
),
)
return control
def run_step(self, input_data, timestamp):
"""
Execute one step of navigation.
"""
print("=====================>")
for key, val in input_data.items():
if hasattr(val[1], 'shape'):
shape = val[1].shape
print("[{} -- {:06d}] with shape {}".format(key, val[0], shape))
else:
print("[{} -- {:06d}] ".format(key, val[0]))
print("<=====================")
# DO SOMETHING SMART
# RETURN CONTROL
control = carla.VehicleControl()
control.steer = 0.0
control.throttle = 0.0
control.brake = 0.0
control.hand_brake = False
return control
def on_control_msg(self, msg):
""" Invoked when a ControlMessage is received.
Args:
msg: A control.messages.ControlMessage message.
"""
# If auto pilot is enabled for the ego vehicle we do not apply the
# control, but we still want to tick in this method to ensure that
# all operators finished work before the world ticks.
if not self._auto_pilot:
# Transform the message to a carla control cmd.
vec_control = carla.VehicleControl(
throttle=msg.throttle,
steer=msg.steer,
brake=msg.brake,
hand_brake=msg.hand_brake,
reverse=msg.reverse)
self._driving_vehicle.apply_control(vec_control)
# Tick the world after the operator received a control command.
# This usually indicates that all the operators have completed
# processing the previous timestamp. However, this is not always
# true (e.g., logging operators that are not part of the main loop).
self._tick_simulator()
def run_step(self, input_data, timestamp):
# Get the current directions for following the route
directions = self._get_current_direction(input_data['GPS'][1])
logging.debug("Directions {}".format(directions))
# Take the forward speed and normalize it for it to go from 0-1
norm_speed = input_data['can_bus'][1]['speed'] / g_conf.SPEED_FACTOR
norm_speed = torch.cuda.FloatTensor([norm_speed]).unsqueeze(0)
directions_tensor = torch.cuda.LongTensor([directions])
# Compute the forward pass processing the sensors got from CARLA.
model_outputs = self._model.forward_branch(
self._process_sensors(input_data['rgb'][1]), norm_speed,
directions_tensor)
steer, throttle, brake = self._process_model_outputs(model_outputs[0])
control = carla.VehicleControl()
control.steer = float(steer)
control.throttle = float(throttle)
control.brake = float(brake)
logging.debug("Output ", control)
# There is the posibility to replace some of the predictions with oracle predictions.
self.first_iter = False
return control
def test_true(self):
print("TestStickyControl.test_true")
inp_control = carla.VehicleControl(throttle=1.0)
bp_vehicles = self.world.get_blueprint_library().filter("vehicle.*")
bp_veh = bp_vehicles[0]
d0, v0 = self.run_scenario(bp_veh, inp_control, sticky="True")
d1, v1 = self.run_scenario(bp_veh, inp_control, continous=True)
d2, v2 = self.run_scenario(bp_veh,
inp_control,
continous=True,
sticky="False")
if not equal_tol(d0, d1, 1e-3) or not equal_tol(v0, v1, 1e-3):
self.fail(
"%s: The input is not sticky: StickyTrue: [%f, %f] ContinousThrottle: [%f, %f]"
% (bp_veh.id, d0, v0, d1, v1))
if not equal_tol(d0, d2, 1e-3) or not equal_tol(v0, v2, 1e-3):
self.fail(
"%s: The input is not sticky: StickyTrue: [%f, %f] ContinousThrottle: [%f, %f]"
% (bp_veh.id, d0, v0, d2, v2))
def emergency_brake(self):
# Get length and speed
length = self.braking_distance()
speed = self.get_speed()
# It doesn't work very well in low and high speed
# It needs to have a high enough concentration to act
if speed > 2.77 and speed < 20:
if length + 1 > self.depth:
self.koncentration += 1
if self.koncentration == 50:
#activate emergency brake
self.emergency = True
print("BRAKE NOW")
else:
self.koncentration = 0
if self.emergency == True:
#Stop car immediatelt
self.player.apply_control(
carla.VehicleControl(throttle=0, steer=0, brake=1))
def run_step(self, target_speed, waypoint, real_dt):
"""
Execute one step of control invoking both lateral and longitudinal PID controllers to reach a target waypoint
at a given target_speed.
:param target_speed: desired vehicle speed
:param waypoint: target location encoded as a waypoint
:return: distance (in meters) to the waypoint
"""
self._lon_controller._dt = real_dt
self._lat_controller._dt = real_dt
throttle, brake = self._lon_controller.run_step(target_speed)
steering = self._lat_controller.run_step(waypoint)
control = carla.VehicleControl()
control.steer = steering
control.throttle = throttle
control.brake = brake
control.hand_brake = False
control.manual_gear_shift = False
return control
def reset_episode(self, initial_distance, initial_speed):
velocity = 0.0
while True:
action = self.pid_controller.update(initial_speed, velocity)
control = carla.VehicleControl(throttle=action + 0.4,
brake=0.0,
steer=0.0)
self.ego_vehicle.apply_control(control)
self.world.tick()
image = self.image_queue.get()
if not self.collision_queue.empty():
_ = self.collision_queue.get()
print("collision occurred during reset...")
distance = self.dist_calc.getTrueDistance()
velocity = self.ego_vehicle.get_velocity().y
if self.collect_path is not None and distance < 110.0:
if self.perception:
self.collect_data(image, distance, velocity, -1,
self.step_count, distance)
else:
self.collect_data(image, distance, velocity, -1,
self.step_count)
self.step_count += 1
weather_parameter = 0.0 # + float(self.step_count)/2.0
weather = carla.WeatherParameters(
cloudyness=80.0,
precipitation=weather_parameter, #0.0 + self.episode_count*1.0,
precipitation_deposits=
weather_parameter, # + self.episode_count*1.0,
sun_altitude_angle=70.0)
self.world.set_weather(weather)
if self.gui:
self.display.updateViewer(image)
if (distance <= initial_distance):
break
#print("The car reset to initial distance: {} and initial speed: {}".format(distance, velocity))
return [distance, velocity]
def run_iter(self, target_speed, waypoint, radius, max_iters):
"""
Execute max_iters step(s) of control invoking both lateral and longitudinal PID controllers to reach a
target waypoint within a distance specified by radius at a given target_speed.
:param target_speed: desired vehicle speed
:param waypoint: target location encoded as a waypoint
:param radius: termination criterion based on distance between the vehicle and the target waypoint
:param max_iters: termination criterion based on number of iterations (<0 --> no limit)
:return: distance (in meters) to the waypoint
"""
_buffer = []
iters = 0
if max_iters < 0:
max_iters = math.inf
control = carla.VehicleControl()
vehicle_transform = self._vehicle.get_transform()
while distance_vehicle(waypoint, vehicle_transform) > radius and iters < max_iters:
throttle = self._long_controller.run_step(target_speed)
steering = self._later_controller.run_step(waypoint)
control.steer = steering
control.throttle = throttle
control.brake = 0.0
control.hand_brake = False
self._vehicle.apply_control(control)
vehicle_transform = self._vehicle.get_transform()
loc = vehicle_transform.location
dx = waypoint.transform.location.x - loc.x
dy = waypoint.transform.location.y - loc.y
_error = math.sqrt(dx * dx + dy * dy)
_buffer.append(_error)
iters += 1
vehicle_transform = self._vehicle.get_transform()
return distance_vehicle(waypoint, vehicle_transform)
def run_step_using_learned_controller(self, input_data, timestamp):
if not self.initialized:
self._init()
tick_data = self.tick(input_data)
img = torchvision.transforms.functional.to_tensor(tick_data['image'])
img = img[None].cuda()
target = torch.from_numpy(tick_data['target'])
target = target[None].cuda()
points, (target_cam, _) = self.net.forward(img, target)
control = self.net.controller(points).cpu().squeeze()
steer = control[0].item()
desired_speed = control[1].item()
speed = tick_data['speed']
brake = desired_speed < 0.4 or (speed / desired_speed) > 1.1
delta = np.clip(desired_speed - speed, 0.0, 0.25)
throttle = self._speed_controller.step(delta)
throttle = np.clip(throttle, 0.0, 0.75)
throttle = throttle if not brake else 0.0
control = carla.VehicleControl()
control.steer = steer
control.throttle = throttle
control.brake = float(brake)
if DEBUG:
debug_display(tick_data, target_cam.squeeze(),
points.cpu().squeeze(), steer, throttle, brake,
desired_speed, self.step)
return control
def run_step(self, input_data, timestamp):
"""
Execute one step of navigation.
"""
control = carla.VehicleControl()
control.steer = 0.0
control.throttle = 0.0
control.brake = 0.0
control.hand_brake = False
if not self._agent:
hero_actor = None
for actor in CarlaDataProvider.get_world().get_actors():
if 'role_name' in actor.attributes and actor.attributes['role_name'] == 'hero':
hero_actor = actor
break
if hero_actor:
self._agent = BasicAgent(hero_actor)
return control
if not self._route_assigned:
if self._global_plan:
plan = []
for transform, road_option in self._global_plan_world_coord:
wp = CarlaDataProvider.get_map().get_waypoint(transform.location)
plan.append((wp, road_option))
self._agent._local_planner.set_global_plan(plan) # pylint: disable=protected-access
self._route_assigned = True
else:
print("[Timestamp: {}]".format(timestamp))
control = self._agent.run_step()
return control
def run_step(self, input_data, timestamp):
game_time = int(timestamp * 1000)
self._logger.debug("Current game time {}".format(game_time))
erdos_timestamp = erdos.Timestamp(coordinates=[game_time])
if not self._sent_open_drive:
# We do not have access to the open drive map. Send top watermark.
self._sent_open_drive = True
self._open_drive_stream.send(
erdos.WatermarkMessage(erdos.Timestamp(is_top=True)))
self.send_waypoints_msg(erdos_timestamp)
for key, val in input_data.items():
if key == 'ground_objects':
self.send_ground_objects(val[1], erdos_timestamp)
elif key == 'scene_layout':
self.send_scene_layout(val[1], erdos_timestamp)
elif key == 'can_bus':
self.send_pose_msg(val[1], erdos_timestamp)
elif key == 'gnss':
self.send_gnss_data(val[1], erdos_timestamp)
else:
self._logger.warning("Sensor {} not used".format(key))
# Wait until the control is set.
while True:
control_msg = self._control_stream.read()
if not isinstance(control_msg, erdos.WatermarkMessage):
output_control = carla.VehicleControl()
output_control.throttle = control_msg.throttle
output_control.brake = control_msg.brake
output_control.steer = control_msg.steer
output_control.reverse = control_msg.reverse
output_control.hand_brake = control_msg.hand_brake
output_control.manual_gear_shift = False
return output_control
def get_mpc_control(world, vehicle, m):
# TODO: add more params in the dict,
# which can be automatically converted (to replace args)
params_dict = {'target_speed': 30}
params = AttrDict(params_dict)
controller = MPCController(params.target_speed)
# get current "measurements"
t = vehicle.get_transform()
v = vehicle.get_velocity()
c = vehicle.get_control()
measurements_dict = {
"v": v,
"t": t
} # TODO: create a dict that return the world&vehicle data similar as 0.8 API
measurements = AttrDict(measurements_dict)
cur_wp = m.get_waypoint(t.location)
local_interval = 0.5
horizon = 5
# initiate a series of waypoints
future_wps = []
future_wps.append(cur_wp)
for i in range(horizon):
# TODO: check whether "next" works here
future_wps.append(random.choice(future_wps[-1].next(local_interval)))
one_log_dict = controller.control(future_wps, measurements)
# print("one_log_dict in run_carla_client")
# print(one_log_dict)
control = carla.VehicleControl()
control.throttle, control.steer = one_log_dict['throttle'], one_log_dict[
'steer']
return control
def __init__(self, world, start_in_autopilot):
self._autopilot_enabled = start_in_autopilot
self._world = world
self._control = carla.VehicleControl()
self._lights = carla.VehicleLightState.NONE
world.player.set_autopilot(self._autopilot_enabled)
self._restrictor = carla.RssRestrictor()
self._vehicle_physics = world.player.get_physics_control()
world.player.set_light_state(self._lights)
self._steer_cache = 0.0
self._mouse_steering_center = None
self._surface = pygame.Surface(
(self.MOUSE_STEERING_RANGE * 2, self.MOUSE_STEERING_RANGE * 2))
self._surface.set_colorkey(pygame.Color('black'))
self._surface.set_alpha(60)
line_width = 2
pygame.draw.polygon(self._surface, (0, 0, 255),
[(0, 0),
(0, self.MOUSE_STEERING_RANGE * 2 - line_width),
(self.MOUSE_STEERING_RANGE * 2 - line_width,
self.MOUSE_STEERING_RANGE * 2 - line_width),
(self.MOUSE_STEERING_RANGE * 2 - line_width, 0),
(0, 0)], line_width)
pygame.draw.polygon(
self._surface, (0, 0, 255),
[(0, self.MOUSE_STEERING_RANGE),
(self.MOUSE_STEERING_RANGE * 2, self.MOUSE_STEERING_RANGE)],
line_width)
pygame.draw.polygon(
self._surface, (0, 0, 255),
[(self.MOUSE_STEERING_RANGE, 0),
(self.MOUSE_STEERING_RANGE, self.MOUSE_STEERING_RANGE * 2)],
line_width)
world.hud.notification("Press 'H' or '?' for help.", seconds=4.0)
def reset(self):
# start_point = random.choice(self.spawn_points)
# yujiyu
start_point = self.spawn_points[2]
self.destination = random.choice(self.spawn_points)
self.vehicle.set_transform(start_point)
self.global_dict['plan_map'], self.destination = replan(
self.agent, self.destination, copy.deepcopy(self.origin_map),
self.spawn_points)
self.global_dict['collision'] = False
start_waypoint = self.agent._map.get_waypoint(
self.agent._vehicle.get_location())
end_waypoint = self.agent._map.get_waypoint(self.destination.location)
self.route_trace = self.agent._trace_route(start_waypoint,
end_waypoint)
start_point.rotation = self.route_trace[0][0].transform.rotation
self.vehicle.set_transform(start_point)
#yujiyu
self.vehicle.apply_control(
carla.VehicleControl(throttle=0.0, steer=0.0, brake=0.0))
#
"""
trans_costmap_stack = get_costmap_stack(self.global_dict)
trans_costmap_stack = torch.stack(trans_costmap_stack)
self.state = trans_costmap_stack[0]
"""
self.state = self.global_dict['trans_costmap']
self.done = False
self.reward = 0.0
# print('RESET !!!!!!!!')
return self.state
def main():
client = carla.Client('127.0.0.1', 2000)
client.set_timeout(10.0)
world = client.get_world()
keyboard = Keyboard(0.05)
control = np.array([0.0, 0.0, 0.0, 0.0])
ego_id = int(input('Enter Ego Vehicle ID:\n'))
actor_list = world.get_actors()
ego_vehicle = actor_list.find(ego_id)
while True:
key = keyboard.read()
if key in ACTION_KEYS:
ACTION_KEYS[key](control)
elif (key == 'h'):
control = np.array([0.0, 0.0, 0.0, 0.0])
elif (key == '\x03'):
print('Control C')
break
elif (key == 'l' or key == 'L'):
print(ego_vehicle.get_transform())
else:
# Steer and break reset
control[1] = 0
control[2] = 0
print(control)
ego_vehicle.apply_control(
carla.VehicleControl(throttle=control[0],
steer=control[1],
brake=control[2],
reverse=bool(control[3])))
def __init__(self, world, start_in_autopilot):
self._autopilot_enabled = start_in_autopilot
if isinstance(world.player, carla.Vehicle):
self._control = carla.VehicleControl()
world.player.set_autopilot(self._autopilot_enabled)
elif isinstance(world.player, carla.Walker):
self._control = carla.WalkerControl()
self._autopilot_enabled = False
self._rotation = world.player.get_transform().rotation
else:
raise NotImplementedError("Actor type not supported")
self._steer_cache = 0.0
world.hud.notification("Press 'H' or '?' for help.", seconds=4.0)
# initialize steering wheel
pygame.joystick.init()
joystick_count = pygame.joystick.get_count()
if joystick_count > 1:
raise ValueError("Please Connect Just One Joystick")
self._joystick = pygame.joystick.Joystick(0)
self._joystick.init()
self._parser = ConfigParser()
self._parser.read('wheel_config.ini')
self._steer_idx = int(
self._parser.get('Microsoft Sidewinder', 'steering_wheel'))
self._throttle_idx = int(
self._parser.get('Microsoft Sidewinder', 'throttle'))
self._brake_idx = int(self._parser.get('Microsoft Sidewinder',
'brake'))
self._reverse_idx = int(
self._parser.get('Microsoft Sidewinder', 'reverse'))
self._handbrake_idx = int(
self._parser.get('Microsoft Sidewinder', 'handbrake'))
def _reset_world(self, soft=False):
# init actor
if not soft:
spawn_point = env_utils.random_spawn_point(self.map)
else:
spawn_point = self.spawn_point
if self.vehicle is None:
blueprint = env_utils.random_blueprint(
self.world, actor_filter=self.vehicle_filter)
self.vehicle: carla.Vehicle = env_utils.spawn_actor(
self.world, blueprint, spawn_point)
self._create_sensors()
self.synchronous_context = CARLASyncContext(self.world,
self.sensors,
fps=self.fps)
else:
self.vehicle.apply_control(carla.VehicleControl())
self.vehicle.set_velocity(carla.Vector3D(x=0.0, y=0.0, z=0.0))
self.vehicle.set_transform(spawn_point)
# reset reward variables
self.collision_penalty = 0.0
def step(self, action: int):
done = False
kmh = self.get_speed()
if len(self.collision_hist) != 0:
done = True
reward = settings.COLLISION_REWARD
else:
reward = kmh * (settings.SPEED_MAX_REWARD - settings.
SPEED_MIN_REWARD) / 100 + settings.SPEED_MIN_REWARD
if self.episode_start + settings.SECONDS_PER_EXPISODE < time.time(
) and self.train:
done = True
if not self.check_rotation():
done = True
reward = settings.BAD_ROTATION_REWARD
if settings.TIME_WEIGHTED_REWARDS:
reward *= (time.time() -
self.episode_start) / settings.SECONDS_PER_EXPISODE
selected_action = settings.ACTIONS[action]
if selected_action:
self.agent_vehicle.apply_control(
carla.VehicleControl(
throttle=selected_action[0] * settings.THROT_AM,
steer=selected_action[1] * settings.STEER_AM,
brake=selected_action[2] * settings.BRAKE_AM))
return [
self.cameras["front"], self.cameras["left"], self.cameras["right"],
kmh, action
], reward, done
def createObjectInSimulator(self, obj):
# Extract blueprint
blueprint = self.blueprintLib.find(obj.blueprint)
if obj.rolename is not None:
blueprint.set_attribute('role_name', obj.rolename)
# set walker as not invincible
if blueprint.has_attribute('is_invincible'):
blueprint.set_attribute('is_invincible', 'False')
# Set up transform
loc = utils.scenicToCarlaLocation(obj.position, world=self.world, blueprint=obj.blueprint)
rot = utils.scenicToCarlaRotation(obj.heading)
transform = carla.Transform(loc, rot)
# Create Carla actor
carlaActor = self.world.try_spawn_actor(blueprint, transform)
if carlaActor is None:
self.destroy()
raise SimulationCreationError(f'Unable to spawn object {obj}')
obj.carlaActor = carlaActor
carlaActor.set_simulate_physics(obj.physics)
if isinstance(carlaActor, carla.Vehicle):
carlaActor.apply_control(carla.VehicleControl(manual_gear_shift=True, gear=1))
elif isinstance(carlaActor, carla.Walker):
carlaActor.apply_control(carla.WalkerControl())
# spawn walker controller
controller_bp = self.blueprintLib.find('controller.ai.walker')
controller = self.world.try_spawn_actor(controller_bp, carla.Transform(), carlaActor)
if controller is None:
self.destroy()
raise SimulationCreationError(f'Unable to spawn carla controller for object {obj}')
obj.carlaController = controller
return carlaActor
def run_step(self, input_data):
print("=====================>")
for key, val in input_data.items():
if hasattr(val[1], 'shape'):
shape = val[1].shape
print("[{} -- {:06d}] with shape {}".format(
key, val[0], shape))
if key == 'Center':
filepath = '/opt/Work/out/{}_{:06d}.png'.format(
key, val[0])
cv2.imwrite(filepath, val[1][:, :, ::-1])
print("<=====================")
# DO SOMETHING SMART
# RETURN CONTROL
control = carla.VehicleControl()
control.steer = 0.0
control.throttle = 1.0
control.brake = 0.0
control.hand_brake = False
return control
def connect(self, host_ip, port, time_out=4.0):
'''
连接在指定ip,端口启动了carlaUE或者carlaUE docker环境的server
'''
import carla
self.carla_imported = carla
self.client = carla.Client(host_ip, port)
self.client.set_timeout(time_out)
self.world = self.client.get_world()
self.map = self.world.get_map()
self.debug = self.world.debug
self.control = carla.VehicleControl(throttle=0,
steer=0,
brake=0.0,
hand_brake=False,
reverse=False,
manual_gear_shift=False,
gear=0)
# 用于销毁自己车辆的函数
def d():
self.client.apply_batch([carla.command.DestroyActor(self.vehicle)])
self.destroy_func = d
def step(self, action):
self.action = action
if action[0] < -0.5:
action[0] = -0.5
elif action[0] > 0.5:
action[0] = 0.5
control = carla.VehicleControl()
control.throttle = 0.5
control.steer = float(action[0])
control.brake = 0
#
self.actor_list[0].apply_control(
control
) # actor_list = [vehicle, camera, collision_sensor, lane_invasion_sensor]
observation = self._get_observation()
done = self._is_game_over(
collision=self.actor_list[2].num_collisions,
lane_intersection=self.actor_list[3].num_laneintersections)
reward = self._get_reward(done=done)
# print(reward, done, observation)
info = {}
# observation = np.random.random((1, 512))
# reward = np.random.random()
# done = False
# info = {}
# print(observation, reward, done, info)
im = self.vae.decode(observation)
im = im[0, :, :, :]
im = im.astype(np.uint8)
cv2.imshow('im2', im)
cv2.waitKey(2)
return observation, reward, done, info
def ego_driving(self, destination):
self._agent.set_destination((destination['x'],
destination['y'],
destination['z']))
print("Ego Vehicle Driving...")
while True:
if self._scenario_done:
break
input_data = self._sensor_list.get_data()
try:
control = self._agent.run_step(input_data)
self._physical_vehicle.apply_control(control)
except Exception as e:
self._scenario_done = True
self.subthread_err_msg = "The Agent's run_step function has problem! Please have a check."
try:
if self._agent.done():
self.dest_arrive = True
self._physical_vehicle.apply_control(carla.VehicleControl(throttle=0.0, steer=0.0, brake = 1.0))
break
except Exception as e:
self._scenario_done = True
self.subthread_err_msg = "The Agent's done function has problem! Please have a check."
self._level_done = True
def run_step(self, world, action):
# not enough waypoints in the horizon? => add more!
# print("player1")
# print(world.player1.get_location().x)
# print(world.player1.get_location().y)
# print(world.player1.get_location().z)
col_hist = world.collision_sensor.get_collision_history()
# print("collision_history:")
# print(col_hist)
# print("frame_number_new:")
col_fram = world.hud.frame
# print(col_fram)
if col_hist.get(col_fram) is not None:
print("new_collision_value and newly start:")
print(col_hist.get(col_fram))
# print(world.player1.get_velocity())
# print("vehicle2")
# print(world.vehicle2.get_location().x)
# print(world.vehicle2.get_location())
# print(world.vehicle2.get_velocity().x)
# print(world.vehicle2.get_velocity())
# RL method (input: front variables; output: variables below)
control = carla.VehicleControl()
control.steer = action[0]
control.throttle = action[1]
control.brake = action[2]
control.hand_brake = False
control.manual_gear_shift = False
control.collisionFlag = col_hist.get(col_fram)
if control.collisionFlag:
print('collisionFlag')
return control, control.collisionFlag
def respawn_actors(world, actors):
"""re-spawn all the actors in the carla world
Args:
world: client.get_world()
actors:world.get_actors()
Example:
client = carla.Client('127.0.0.1', 2000)
client.set_timeout(10.0) # seconds
actor_list = world.get_actors()
vehicles = list(actor_list.filter('vehicle*'))
if carla_actors_static(vehicles):
respawn_static_actors(vehicles)
"""
for vehicle in actors:
v = vehicle.get_velocity()
a_v = vehicle.get_angular_velocity()
v.x = 0.
v.y = 0.
v.z = 0.
a_v.x = 0.
a_v.y = 0.
a_v.z = 0.
vehicle.set_velocity(v)
vehicle.set_angular_velocity(a_v)
vehicle.apply_control(
carla.VehicleControl(throttle=0, steer=0, brake=0))
spawn_points = list(world.get_map().get_spawn_points())
index = random.randint(0, (len(spawn_points)) - 1)
# index = 45
# spawn_points[index].location.z = spawn_points[index].location.z
spawn_points[index].location.z = 0.001
vehicle.set_transform(spawn_points[index])
