def main():
global global_nav, global_vel, start_control, global_plan_map, global_vehicle, global_transform, max_steer_angle, global_a, state0, global_collision
client = carla.Client(config['host'], config['port'])
client.set_timeout(config['timeout'])
world = client.load_world('Town01')
weather = carla.WeatherParameters(cloudiness=random.randint(0, 10),
precipitation=0,
sun_altitude_angle=random.randint(
50, 90))
set_weather(world, weather)
blueprint = world.get_blueprint_library()
world_map = world.get_map()
vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.audi.a2')
#vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.yamaha.yzf')
#vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.*.*')
global_vehicle = vehicle
# Enables or disables the simulation of physics on this actor.
vehicle.set_simulate_physics(True)
physics_control = vehicle.get_physics_control()
max_steer_angle = np.deg2rad(physics_control.wheels[0].max_steer_angle)
spawn_points = world_map.get_spawn_points()
waypoint_tuple_list = world_map.get_topology()
origin_map = get_map(waypoint_tuple_list)
agent = BasicAgent(vehicle, target_speed=MAX_SPEED)
# prepare map
destination = carla.Transform()
destination.location = world.get_random_location_from_navigation()
global_plan_map, destination = replan(agent, destination,
copy.deepcopy(origin_map),
spawn_points)
sensor_dict = {
'camera': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': image_callback,
},
'camera:view': {
#'transform':carla.Transform(carla.Location(x=0.0, y=4.0, z=4.0), carla.Rotation(pitch=-30, yaw=-60)),
'transform':
carla.Transform(carla.Location(x=-3.0, y=0.0, z=6.0),
carla.Rotation(pitch=-45)),
#'transform':carla.Transform(carla.Location(x=0.0, y=0.0, z=6.0), carla.Rotation(pitch=-90)),
'callback':
view_image_callback,
},
'collision': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': collision_callback,
},
}
sm = SensorManager(world, blueprint, vehicle, sensor_dict)
sm.init_all()
time.sleep(0.5)
# start to plan
plan_thread = threading.Thread(target=make_plan, args=())
# visualization_thread = threading.Thread(target = show_traj, args=())
while True:
if (global_img is not None) and (global_nav is not None):
plan_thread.start()
break
else:
time.sleep(0.001)
# wait for the first plan result
while not start_control:
time.sleep(0.001)
# visualization_thread.start()
# start to control
print('Start to control')
ctrller = CapacController(world, vehicle, 30)
while True:
# change destination
if close2dest(vehicle, destination):
#destination = get_random_destination(spawn_points)
print('get destination !', time.time())
destination = carla.Transform()
destination.location = world.get_random_location_from_navigation()
# global_plan_map = replan(agent, destination, copy.deepcopy(origin_map))
global_plan_map, destination = replan(agent, destination,
copy.deepcopy(origin_map),
spawn_points)
if global_collision:
global_collision = False
start_point = random.choice(spawn_points)
vehicle.set_transform(start_point)
global_plan_map, destination = replan(agent, destination,
copy.deepcopy(origin_map),
spawn_points)
start_waypoint = agent._map.get_waypoint(
agent._vehicle.get_location())
end_waypoint = agent._map.get_waypoint(destination.location)
route_trace = agent._trace_route(start_waypoint, end_waypoint)
start_point.rotation = route_trace[0][0].transform.rotation
vehicle.set_transform(start_point)
v = global_vehicle.get_velocity()
a = global_vehicle.get_acceleration()
global_vel = np.sqrt(v.x**2 + v.y**2 + v.z**2)
global_a = np.sqrt(a.x**2 + a.y**2 + a.z**2)
#steer_angle = global_vehicle.get_control().steer*max_steer_angle
#w = global_vel*np.tan(steer_angle)/2.405
control_time = time.time()
dt = control_time - global_trajectory['time']
index = int((dt / args.max_t) // args.dt) + 2
if index > 0.99 / args.dt:
continue
"""
transform = vehicle.get_transform()
dx, dy, dyaw = get_transform(transform, global_transform)
dyaw = -dyaw
_x = global_trajectory['x'][index] - dx
_y = global_trajectory['y'][index] - dy
x = _x*np.cos(dyaw) + _y*np.sin(dyaw)
y = _y*np.cos(dyaw) - _x*np.sin(dyaw)
_vx = global_trajectory['vx'][index]
_vy = global_trajectory['vy'][index]
vx = _vx*np.cos(dyaw) + _vy*np.sin(dyaw)
vy = _vy*np.cos(dyaw) - _vx*np.sin(dyaw)
_ax = global_trajectory['ax'][index]
_ay = global_trajectory['ay'][index]
ax = _ax*np.cos(dyaw) + _ay*np.sin(dyaw)
ay = _ay*np.cos(dyaw) - _ax*np.sin(dyaw)
"""
#control = get_control(x, y, vx, vy, ax, ay)
control = ctrller.run_step(global_trajectory, index, state0)
vehicle.apply_control(control)
# x = global_trajectory['x']
# y = global_trajectory['y']
# plt.cla()
# plt.plot(x, y)
# plt.xlim(-1, 29)
# plt.ylim(-15, 15)
# plt.show()
"""
dyaw = np.deg2rad(global_transform.rotation.yaw)
x = global_trajectory['x'][index]*np.cos(dyaw) + global_trajectory['y'][index]*np.sin(dyaw)
y = global_trajectory['y'][index]*np.cos(dyaw) - global_trajectory['x'][index]*np.sin(dyaw)
"""
#localtion = carla.Location(x = global_transform.location.x+x, y=global_transform.location.y+y, z=2.0)
#world.debug.draw_point(localtion, size=0.3, color=carla.Color(255,0,0), life_time=10.0)
#print(global_vel*np.tan(control.steer)/w)
curve = None #show_traj(True)
visualize(global_view_img, global_nav, curve)
#time.sleep(1/60.)
cv2.destroyAllWindows()
sm.close_all()
vehicle.destroy()
def main():
global global_nav, global_vel, start_control, global_plan_map, global_vehicle, global_cost_map, global_transform, max_steer_angle, global_a, draw_cost_map, state0
client = carla.Client(config['host'], config['port'])
client.set_timeout(config['timeout'])
world = client.load_world('Town01')
weather = carla.WeatherParameters(
cloudiness=random.randint(0,10),
precipitation=0,
sun_altitude_angle=random.randint(50,90)
)
set_weather(world, weather)
blueprint = world.get_blueprint_library()
world_map = world.get_map()
vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.audi.a2')
#vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.yamaha.yzf')
#vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.*.*')
global_vehicle = vehicle
# Enables or disables the simulation of physics on this actor.
vehicle.set_simulate_physics(True)
physics_control = vehicle.get_physics_control()
max_steer_angle = np.deg2rad(physics_control.wheels[0].max_steer_angle)
sensor_dict = {
'camera':{
'transform':carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback':image_callback,
},
'camera:view':{
#'transform':carla.Transform(carla.Location(x=0.0, y=4.0, z=4.0), carla.Rotation(pitch=-30, yaw=-60)),
'transform':carla.Transform(carla.Location(x=-3.0, y=0.0, z=6.0), carla.Rotation(pitch=-45)),
#'transform':carla.Transform(carla.Location(x=0.0, y=0.0, z=6.0), carla.Rotation(pitch=-90)),
'callback':view_image_callback,
},
'lidar':{
'transform':carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback':lidar_callback,
},
}
sm = SensorManager(world, blueprint, vehicle, sensor_dict)
sm.init_all()
#spawn_points = world_map.get_spawn_points()
waypoint_tuple_list = world_map.get_topology()
origin_map = get_map(waypoint_tuple_list)
agent = BasicAgent(vehicle, target_speed=MAX_SPEED)
# prepare map
destination = carla.Transform()
destination.location = world.get_random_location_from_navigation()
#destination = get_random_destination(spawn_points)
global_plan_map = replan(agent, destination, copy.deepcopy(origin_map))
# start to plan
plan_thread = threading.Thread(target = make_plan, args=())
visualization_thread = threading.Thread(target = show_traj, args=())
while True:
if (global_img is not None) and (global_nav is not None) and (global_pcd is not None):
plan_thread.start()
break
else:
time.sleep(0.001)
# wait for the first plan result
while not start_control:
time.sleep(0.001)
#visualization_thread.start()
# start to control
print('Start to control')
ctrller = CapacController(world, vehicle, 30)
while True:
# change destination
if close2dest(vehicle, destination):
#destination = get_random_destination(spawn_points)
print('get destination !', time.time())
destination = carla.Transform()
destination.location = world.get_random_location_from_navigation()
global_plan_map = replan(agent, destination, copy.deepcopy(origin_map))
v = global_vehicle.get_velocity()
a = global_vehicle.get_acceleration()
global_vel = np.sqrt(v.x**2+v.y**2+v.z**2)
global_a = np.sqrt(a.x**2+a.y**2+a.z**2)
#steer_angle = global_vehicle.get_control().steer*max_steer_angle
#w = global_vel*np.tan(steer_angle)/2.405
control_time = time.time()
dt = control_time - global_trajectory['time']
index = int((dt/args.max_t)//args.dt)
if index > 0.99/args.dt:
continue
control = ctrller.run_step(global_trajectory, index, state0)
vehicle.apply_control(control)
curve = None#show_traj(True)
visualize(global_view_img, draw_cost_map, global_nav, curve)
#time.sleep(1/60.)
cv2.destroyAllWindows()
sm.close_all()
vehicle.destroy()
def main():
client = carla.Client(config['host'], config['port'])
client.set_timeout(config['timeout'])
world = client.load_world('Town01')
weather = carla.WeatherParameters(cloudiness=random.randint(0, 10),
precipitation=0,
sun_altitude_angle=random.randint(
50, 90))
set_weather(world, weather)
blueprint = world.get_blueprint_library()
world_map = world.get_map()
vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.audi.a2')
#vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.yamaha.yzf')
#vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.*.*')
global_dict['vehicle'] = vehicle
# Enables or disables the simulation of physics on this actor.
vehicle.set_simulate_physics(True)
physics_control = vehicle.get_physics_control()
global_dict['max_steer_angle'] = np.deg2rad(
physics_control.wheels[0].max_steer_angle)
sensor_dict = {
'camera': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': image_callback,
},
'camera:view': {
#'transform':carla.Transform(carla.Location(x=0.0, y=4.0, z=4.0), carla.Rotation(pitch=-30, yaw=-60)),
'transform':
carla.Transform(carla.Location(x=-3.0, y=0.0, z=9.0),
carla.Rotation(pitch=-45)),
#'transform':carla.Transform(carla.Location(x=0.0, y=0.0, z=6.0), carla.Rotation(pitch=-90)),
'callback':
view_image_callback,
},
'lidar': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': lidar_callback,
},
'collision': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': collision_callback,
},
}
sm = SensorManager(world, blueprint, vehicle, sensor_dict)
sm.init_all()
env = CARLAEnv(world, vehicle, global_dict, args, trajectory_model)
# start to control
print('Start to control')
state = env.reset()
while True:
state = env.reset()
sm.close_all()
vehicle.destroy()
def main():
global global_nav, global_vel
client = carla.Client(config['host'], config['port'])
client.set_timeout(config['timeout'])
world = client.load_world('Town01')
weather = carla.WeatherParameters(
cloudiness=random.randint(0,10),
precipitation=0,
sun_altitude_angle=random.randint(50,90)
)
set_weather(world, weather)
blueprint = world.get_blueprint_library()
world_map = world.get_map()
vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.audi.a2')
# Enables or disables the simulation of physics on this actor.
vehicle.set_simulate_physics(True)
sensor_dict = {
'camera':{
'transform':carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback':image_callback,
},
'lidar':{
'transform':carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback':lidar_callback,
},
}
sm = SensorManager(world, blueprint, vehicle, sensor_dict)
sm.init_all()
time.sleep(0.3)
spawn_points = world_map.get_spawn_points()
waypoint_tuple_list = world_map.get_topology()
origin_map = get_map(waypoint_tuple_list)
agent = BasicAgent(vehicle, target_speed=MAX_SPEED)
destination = get_random_destination(spawn_points)
plan_map = replan(agent, destination, copy.deepcopy(origin_map))
inverse_perspective_mapping = InversePerspectiveMapping(param, sensor_master)
time.sleep(0.3)
while True:
if close2dest(vehicle, destination):
destination = get_random_destination(spawn_points)
plan_map = replan(agent, destination, copy.deepcopy(origin_map))
v = vehicle.get_velocity()
global_vel = np.sqrt(v.x**2+v.y**2+v.z**2)
global_nav = get_nav(vehicle, plan_map)
result = get_cGAN_result()
img = copy.deepcopy(global_img)
mask = np.where(result > 200)
img[mask[0],mask[1]] = (255, 0, 0, 255)
ipm_image = inverse_perspective_mapping.getIPM(result)
cost_map = get_cost_map(ipm_image, global_pcd)
#plan_cost_map = draw_traj(cost_map)
plan_cost_map, trajectory = get_traj(cost_map, 0.7, show=True)
control = get_control(trajectory)
vehicle.apply_control(control)
visualize(img, plan_cost_map, global_nav)
cv2.destroyAllWindows()
sm.close_all()
vehicle.destroy()
def main():
global global_nav, global_control, global_pos, global_vel
client = carla.Client(config['host'], config['port'])
client.set_timeout(config['timeout'])
#world = client.get_world()
world = client.load_world('Town02')
weather = carla.WeatherParameters(cloudiness=random.randint(0, 10),
precipitation=0,
sun_altitude_angle=random.randint(
70, 90))
#world.set_weather(carla.WeatherParameters.ClearNoon)
set_weather(world, weather)
blueprint = world.get_blueprint_library()
world_map = world.get_map()
vehicle = add_vehicle(world,
blueprint,
vehicle_type='vehicle.bmw.grandtourer')
# Enables or disables the simulation of physics on this actor.
vehicle.set_simulate_physics(True)
sensor_dict = {
'camera': {
'transform': carla.Transform(carla.Location(x=1.5, y=0.0, z=2.0)),
'callback': image_callback,
},
'lidar': {
'transform': carla.Transform(carla.Location(x=1.5, y=0.0, z=2.0)),
'callback': lidar_callback,
},
}
sm = SensorManager(world, blueprint, vehicle, sensor_dict)
sm.init_all()
time.sleep(0.3)
spawn_points = world_map.get_spawn_points()
waypoint_tuple_list = world_map.get_topology()
origin_map = get_map(waypoint_tuple_list)
agent = BasicAgent(vehicle, target_speed=MAX_SPEED)
destination = get_random_destination(spawn_points)
plan_map = replan(agent, destination, copy.deepcopy(origin_map))
FPS = [str(time.time())]
for cnt in range(args.num):
if close2dest(vehicle, destination):
destination = get_random_destination(spawn_points)
plan_map = replan(agent, destination, copy.deepcopy(origin_map))
if vehicle.is_at_traffic_light():
traffic_light = vehicle.get_traffic_light()
if traffic_light.get_state() == carla.TrafficLightState.Red:
traffic_light.set_state(carla.TrafficLightState.Green)
#control = agent.run_step()
#control.manual_gear_shift = False
#control = None
#vehicle.apply_control(control)
#nav = get_nav(vehicle, plan_map)
global_pos = vehicle.get_transform()
global_vel = vehicle.get_velocity()
start_waypoint = agent._map.get_waypoint(agent._vehicle.get_location())
end_waypoint = agent._map.get_waypoint(destination.location)
route_trace = agent._trace_route(start_waypoint, end_waypoint)
x = []
y = []
traj_pose_list = []
for i in range(len(route_trace) - 1):
waypoint1 = route_trace[i][0].transform.location
waypoint2 = route_trace[i + 1][0].transform.location
dist = waypoint1.distance(waypoint2)
x.append(waypoint1.x)
y.append(waypoint1.y)
traj_pose_list.append((0, route_trace[i][0].transform))
plt.plot([global_pos.location.x, x[0]], [global_pos.location.y, y[0]],
color='r')
plt.plot(x[:10], y[:10], color='b')
plt.show()
empty_image = collect_perspective.getPM(traj_pose_list, global_pos,
global_img)
break
#cv2.imshow('Nav', nav)
#cv2.imshow('Vision', global_img)
#cv2.waitKey(10)
index = str(time.time())
if cnt % 100 == 0:
FPS.append(index)
print(round(1 / ((float(FPS[-1]) - float(FPS[-2])) / 100), 1))
cv2.destroyAllWindows()
vehicle.destroy()
sm.close_all()
class World(object):
def __init__(self, carla_world, hud):
self.world = carla_world
self.hud = hud
self.player = None
self.camera_manager = None
self._weather_presets = find_weather_presets()
self._weather_index = 0
self.restart()
self.world.on_tick(hud.on_world_tick)
def restart(self):
# Keep same camera config if the camera manager exists.
cam_index = self.camera_manager.index if self.camera_manager is not None else 0
cam_pos_index = self.camera_manager.transform_index if self.camera_manager is not None else 0
# Get a blueprint.
blueprint = random.choice(
self.world.get_blueprint_library().filter('vehicle.audi.a2'))
blueprint.set_attribute('role_name', 'hero')
if blueprint.has_attribute('color'):
color = random.choice(
blueprint.get_attribute('color').recommended_values)
blueprint.set_attribute('color', color)
# Spawn the player.
if self.player is not None:
spawn_point = self.player.get_transform()
spawn_point.location.z += 2.0
spawn_point.rotation.roll = 0.0
spawn_point.rotation.pitch = 0.0
self.destroy()
self.player = self.world.try_spawn_actor(blueprint, spawn_point)
while self.player is None:
spawn_points = self.world.get_map().get_spawn_points()
spawn_point = random.choice(
spawn_points) if spawn_points else carla.Transform()
self.player = self.world.try_spawn_actor(blueprint, spawn_point)
# Set up the sensors.
self.camera_manager = CameraManager(self.player, self.hud)
self.camera_manager.transform_index = cam_pos_index
self.camera_manager.set_sensor(cam_index, notify=False)
sensor_dict = {
'camera': {
'transform':
carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': image_callback,
},
'lidar': {
'transform':
carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': lidar_callback,
},
}
self.sm = SensorManager(self.world, self.world.get_blueprint_library(),
self.player, sensor_dict)
self.sm.init_all()
actor_type = get_actor_display_name(self.player)
self.hud.notification(actor_type)
def next_weather(self, reverse=False):
self._weather_index += -1 if reverse else 1
self._weather_index %= len(self._weather_presets)
preset = self._weather_presets[self._weather_index]
self.hud.notification('Weather: %s' % preset[1])
self.player.get_world().set_weather(preset[0])
def tick(self, clock):
self.hud.tick(self, clock)
def render(self, display):
self.camera_manager.render(display)
self.hud.render(display)
def destroy(self):
actors = [self.camera_manager.sensor, self.player]
self.sm.close_all()
for actor in actors:
if actor is not None:
actor.destroy()
def main():
global global_nav, global_vel, start_control, global_plan_map, global_vehicle, global_cost_map, global_transform, max_steer_angle, global_a, draw_cost_map
client = carla.Client(config['host'], config['port'])
client.set_timeout(config['timeout'])
world = client.load_world('Town01')
weather = carla.WeatherParameters(cloudiness=random.randint(0, 10),
precipitation=0,
sun_altitude_angle=random.randint(
50, 90))
set_weather(world, weather)
blueprint = world.get_blueprint_library()
world_map = world.get_map()
vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.audi.a2')
#vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.yamaha.yzf')
#vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.*.*')
global_vehicle = vehicle
# Enables or disables the simulation of physics on this actor.
vehicle.set_simulate_physics(True)
physics_control = vehicle.get_physics_control()
max_steer_angle = np.deg2rad(physics_control.wheels[0].max_steer_angle)
sensor_dict = {
'camera': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': image_callback,
},
'lidar': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': lidar_callback,
},
}
sm = SensorManager(world, blueprint, vehicle, sensor_dict)
sm.init_all()
#spawn_points = world_map.get_spawn_points()
waypoint_tuple_list = world_map.get_topology()
origin_map = get_map(waypoint_tuple_list)
agent = BasicAgent(vehicle, target_speed=MAX_SPEED)
# prepare map
destination = carla.Transform()
destination.location = world.get_random_location_from_navigation()
#destination = get_random_destination(spawn_points)
global_plan_map = replan(agent, destination, copy.deepcopy(origin_map))
# start to plan
plan_thread = threading.Thread(target=make_plan, args=())
while True:
if (global_img is not None) and (global_nav
is not None) and (global_pcd
is not None):
plan_thread.start()
break
else:
time.sleep(0.001)
# wait for the first plan result
while not start_control:
time.sleep(0.001)
# start to control
print('Start to control')
avg_dt = 1.0
distance = 0.
last_location = vehicle.get_location()
while True:
# change destination
if close2dest(vehicle, destination):
#destination = get_random_destination(spawn_points)
destination = carla.Transform()
destination.location = world.get_random_location_from_navigation()
global_plan_map = replan(agent, destination,
copy.deepcopy(origin_map))
v = global_vehicle.get_velocity()
a = global_vehicle.get_acceleration()
global_vel = np.sqrt(v.x**2 + v.y**2 + v.z**2)
global_a = np.sqrt(a.x**2 + a.y**2 + a.z**2)
#steer_angle = global_vehicle.get_control().steer*max_steer_angle
#w = global_vel*np.tan(steer_angle)/2.405
control_time = time.time()
dt = control_time - global_trajectory['time']
avg_dt = 0.99 * avg_dt + 0.01 * dt
#print(round(avg_dt, 3))
index = int((dt / args.max_t) // args.dt)
if index > 0.9 / args.dt:
continue
location = vehicle.get_location()
distance += location.distance(last_location)
last_location = location
#print(round(distance, 1))
transform = vehicle.get_transform()
dx, dy, dyaw = get_transform(transform, global_transform)
dyaw = -dyaw
_x = global_trajectory['x'][index] - dx
_y = global_trajectory['y'][index] - dy
x = _x * np.cos(dyaw) + _y * np.sin(dyaw)
y = _y * np.cos(dyaw) - _x * np.sin(dyaw)
_vx = global_trajectory['vx'][index]
_vy = global_trajectory['vy'][index]
vx = _vx * np.cos(dyaw) + _vy * np.sin(dyaw)
vy = _vy * np.cos(dyaw) - _vx * np.sin(dyaw)
_ax = global_trajectory['ax'][index]
_ay = global_trajectory['ay'][index]
ax = _ax * np.cos(dyaw) + _ay * np.sin(dyaw)
ay = _ay * np.cos(dyaw) - _ax * np.sin(dyaw)
control = get_new_control(x, y, vx, vy, ax, ay)
vehicle.apply_control(control)
#print(global_vel*np.tan(control.steer)/w)
#visualize(global_img, draw_cost_map, global_nav)
visualize(global_img)
#time.sleep(1/60.)
cv2.destroyAllWindows()
sm.close_all()
vehicle.destroy()
def main():
global global_img, global_view_img, global_pcd
client = carla.Client(config['host'], config['port'])
client.set_timeout(config['timeout'])
world = client.load_world('Town01')
weather = carla.WeatherParameters(cloudiness=random.randint(0, 10),
precipitation=0,
sun_altitude_angle=random.randint(
50, 90))
set_weather(world, weather)
blueprint = world.get_blueprint_library()
world_map = world.get_map()
vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.audi.a2')
vehicle.set_simulate_physics(True)
physics_control = vehicle.get_physics_control()
#max_steer_angle = np.deg2rad(physics_control.wheels[0].max_steer_angle)
sensor_dict = {
'camera': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': image_callback,
},
'camera:view': {
'transform':
carla.Transform(carla.Location(x=-3.0, y=0.0, z=8.0),
carla.Rotation(pitch=-45)),
'callback':
view_image_callback,
},
'lidar': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': lidar_callback,
},
}
sm = SensorManager(world, blueprint, vehicle, sensor_dict)
sm.init_all()
#agent = BasicAgent(vehicle, target_speed=MAX_SPEED)
destination = carla.Transform()
destination.location = world.get_random_location_from_navigation()
env = CARLAEnv()
env.reset()
env.step()
# start to control
print('Start to control')
vehicle.set_autopilot(True)
while True:
# change destination
if close2dest(vehicle, destination):
destination = carla.Transform()
destination.location = world.get_random_location_from_navigation()
#control = None
#vehicle.apply_control(control)
#localtion = carla.Location(x = global_transform.location.x+x, y=global_transform.location.y+y, z=2.0)
#world.debug.draw_point(localtion, size=0.3, color=carla.Color(255,0,0), life_time=10.0)
forward_vector = vehicle.get_transform().get_forward_vector()
begin = vehicle.get_location()
end = vehicle.get_location()
end.x = end.x + forward_vector.x * 8
end.y = end.y + forward_vector.y * 8
end.z = end.z + forward_vector.z * 8
world.debug.draw_line(begin,
end,
thickness=0.2,
color=carla.Color(r=255, g=0, b=0, a=128),
life_time=0.1)
cv2.imshow('Visualization', global_view_img)
cv2.waitKey(5)
#time.sleep(1/60.)
cv2.destroyAllWindows()
sm.close_all()
vehicle.destroy()
def main():
client = carla.Client(config['host'], config['port'])
client.set_timeout(config['timeout'])
world = client.load_world('Town01')
weather = carla.WeatherParameters(cloudiness=random.randint(0, 10),
precipitation=0,
sun_altitude_angle=random.randint(
50, 90))
set_weather(world, weather)
blueprint = world.get_blueprint_library()
world_map = world.get_map()
vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.audi.a2')
#vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.yamaha.yzf')
#vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.*.*')
global_dict['vehicle'] = vehicle
# Enables or disables the simulation of physics on this actor.
vehicle.set_simulate_physics(True)
physics_control = vehicle.get_physics_control()
global_dict['max_steer_angle'] = np.deg2rad(
physics_control.wheels[0].max_steer_angle)
sensor_dict = {
'camera': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': image_callback,
},
'camera:view': {
#'transform':carla.Transform(carla.Location(x=0.0, y=4.0, z=4.0), carla.Rotation(pitch=-30, yaw=-60)),
'transform':
carla.Transform(carla.Location(x=-3.0, y=0.0, z=9.0),
carla.Rotation(pitch=-45)),
#'transform':carla.Transform(carla.Location(x=0.0, y=0.0, z=6.0), carla.Rotation(pitch=-90)),
'callback':
view_image_callback,
},
'lidar': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': lidar_callback,
},
'collision': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': collision_callback,
},
}
sm = SensorManager(world, blueprint, vehicle, sensor_dict)
sm.init_all()
model = TD3(buffer_size=3e3, noise_decay_steps=5e3, batch_size=10)
env = CARLAEnv(world, vehicle, global_dict, args, trajectory_model)
state = env.reset()
plan_thread = threading.Thread(target=make_plan, args=())
while True:
if (global_dict['img']
is not None) and (global_dict['nav']
is not None) and (global_dict['pcd']
is not None):
plan_thread.start()
break
else:
time.sleep(0.001)
# start to control
print('Start to control')
episode_timesteps = 0
episode_reward = 0
max_steps = 1e9
total_steps = 0
max_episode_steps = 1000
learning_starts = 50
episode_num = 0
while total_steps < max_steps:
total_steps += 1
episode_timesteps = 0
episode_reward = 0
state_queue = []
state = env.reset()
for _ in range(10):
state_queue.append(state)
state = torch.stack(state_queue)
plan_time = global_dict['plan_time']
for step in range(max_episode_steps):
global_dict['state0'] = cu.getActorState('odom', plan_time,
global_dict['vehicle'])
global_dict['state0'].x = global_dict['transform'].location.x
global_dict['state0'].y = global_dict['transform'].location.y
global_dict['state0'].z = global_dict['transform'].location.z
global_dict['state0'].theta = np.deg2rad(
global_dict['transform'].rotation.yaw)
action = model.policy_net.get_action(state)
print('org:', action)
action = model.noise.get_action(action, total_steps)
print('after', action)
next_state, reward, done, _ = env.step(action, plan_time)
plan_time = global_dict['plan_time']
model.replay_buffer.push(copy.deepcopy(state), action, reward,
copy.deepcopy(next_state), done)
state = next_state
episode_reward += reward
total_steps += 1
episode_timesteps += 1
if done or episode_timesteps == max_episode_steps:
if len(model.replay_buffer) > learning_starts:
for i in range(episode_timesteps):
model.train_step(i)
episode_num += 1
if episode_num > 0 and episode_num % model.save_eps_num == 0:
model.save(directory=model.load_dir,
filename=str(episode_num))
model.writer.add_scalar('episode_reward', episode_reward,
episode_num)
break
sm.close_all()
vehicle.destroy()
def main():
client = carla.Client(config['host'], config['port'])
client.set_timeout(config['timeout'])
world = client.get_world()
weather = carla.WeatherParameters(cloudiness=30.0,
precipitation=30.0,
sun_altitude_angle=50.0)
set_weather(world, weather)
blueprint = world.get_blueprint_library()
world_map = world.get_map()
vehicle = add_vehicle(world,
blueprint,
vehicle_type='vehicle.bmw.grandtourer')
# Enables or disables the simulation of physics on this actor.
vehicle.set_simulate_physics(True)
sensor_dict = {
'camera': {
'transform': carla.Transform(carla.Location(x=1.5, y=0.0, z=2.0)),
'callback': image_callback,
},
'lidar': {
'transform': carla.Transform(carla.Location(x=1.5, y=0.0, z=2.0)),
'callback': lidar_callback,
},
'gnss': {
'transform': carla.Transform(carla.Location(x=0.0, y=0.0, z=0.0)),
'callback': gnss_callback,
},
'imu': {
'transform': carla.Transform(carla.Location(x=0.0, y=0.0, z=0.0)),
'callback': imu_callback,
},
}
sm = SensorManager(world, blueprint, vehicle, sensor_dict)
sm.init_all()
time.sleep(0.3)
vehicle.set_autopilot(True)
try:
while True:
if vehicle.is_at_traffic_light():
traffic_light = vehicle.get_traffic_light()
if traffic_light.get_state() == carla.TrafficLightState.Red:
traffic_light.set_state(carla.TrafficLightState.Green)
cv2.imshow('Raw image', global_img)
"""
vehicle.apply_control(carla.VehicleControl(
throttle=0.3,
steer = 0.0,
reverse=False,
manual_gear_shift=True,
gear=1,
brake=0.0,
hand_brake=False
))
"""
#vehicle.get_angular_velocity().z
#vehicle.get_velocity().x
cv2.waitKey(16)
#break
cv2.destroyAllWindows()
finally:
sm.close_all()
vehicle.destroy()
def main():
client = carla.Client(config['host'], config['port'])
client.set_timeout(config['timeout'])
world = client.load_world('Town01')
weather = carla.WeatherParameters(cloudiness=random.randint(0, 10),
precipitation=0,
sun_altitude_angle=random.randint(
50, 90))
set_weather(world, weather)
blueprint = world.get_blueprint_library()
# world_map = world.get_map()
vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.audi.a2')
#vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.yamaha.yzf')
#vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.*.*')
global_dict['vehicle'] = vehicle
# Enables or disables the simulation of physics on this actor.
# vehicle.set_simulate_physics(True)
physics_control = vehicle.get_physics_control()
global_dict['max_steer_angle'] = np.deg2rad(
physics_control.wheels[0].max_steer_angle)
sensor_dict = {
'camera': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': image_callback,
},
'camera:view': {
#'transform':carla.Transform(carla.Location(x=0.0, y=4.0, z=4.0), carla.Rotation(pitch=-30, yaw=-60)),
'transform':
carla.Transform(carla.Location(x=-3.0, y=0.0, z=9.0),
carla.Rotation(pitch=-45)),
#'transform':carla.Transform(carla.Location(x=0.0, y=0.0, z=6.0), carla.Rotation(pitch=-90)),
'callback':
view_image_callback,
},
'lidar': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': lidar_callback,
},
'collision': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': collision_callback,
},
}
sm = SensorManager(world, blueprint, vehicle, sensor_dict)
sm.init_all()
env = CARLAEnv(world, vehicle, global_dict, args, trajectory_model)
plan_thread = threading.Thread(target=make_plan, args=())
while True:
if (global_dict['img']
is not None) and (global_dict['nav'] is not None) and (
global_dict['trans_costmap'] is not None):
plan_thread.start()
break
else:
time.sleep(0.001)
# start to control
print('Start to control')
state = env.reset()
episode_timesteps = 0
episode_reward = 0
max_steps = 1e9
total_steps = 0
max_episode_steps = 1000
learning_starts = 10 #2000
episode_num = 0
last_episode_reward = 0
# log
total_driving_metre = 0
while total_steps < max_steps:
print("total_episode:", episode_num)
episode_num += 1
total_steps += 1
episode_timesteps = 0
episode_reward = 0
total_driving_metre = 0
#state_queue = []
state = env.reset()
#for _ in range(10): state_queue.append(state)
#state = torch.stack(state_queue)
plan_time = time.time()
global_dict['state0'] = cu.getActorState('odom', plan_time,
global_dict['vehicle'])
global_dict['state0'].x = global_dict['transform'].location.x
global_dict['state0'].y = global_dict['transform'].location.y
global_dict['state0'].z = global_dict['transform'].location.z
global_dict['state0'].theta = np.deg2rad(
global_dict['transform'].rotation.yaw)
add_noise = True if random.random() < 0.5 else False
for step in range(max_episode_steps):
action = model.policy_net.get_action(state)
#print('org:', action)
if add_noise:
if last_episode_reward < 50:
action = model.noise.get_action(action, 66)
else:
action = model.noise.get_action(action, total_steps // 2)
# print('action:', action)
x_last = global_dict['transform'].location.x
y_last = global_dict['transform'].location.y
next_state, reward, done, ts = env.step(action, plan_time)
plan_time = ts
global_dict['state0'] = cu.getActorState('odom', plan_time,
global_dict['vehicle'])
global_dict['state0'].x = global_dict['transform'].location.x
global_dict['state0'].y = global_dict['transform'].location.y
global_dict['state0'].z = global_dict['transform'].location.z
global_dict['state0'].theta = np.deg2rad(
global_dict['transform'].rotation.yaw)
x_now = global_dict['transform'].location.x
y_now = global_dict['transform'].location.y
driving_metre_in_step = np.sqrt((x_now - x_last)**2 +
(y_now - y_last)**2)
total_driving_metre += driving_metre_in_step
model.replay_buffer.push(state, action, reward, next_state, done)
# print("next state :", type(next_state)) Tensor
# print("next state shape:", next_state.shape)
# print("state shape:", state.shape) next state shape: torch.Size([1, 200, 400])
if len(model.replay_buffer) > max(learning_starts,
model.batch_size):
print("Start Train")
model.train_step(total_steps, noise_std=0.2,
noise_clip=0.5) #noise_std = 0.2
state = next_state
episode_reward += reward
total_steps += 1
episode_timesteps += 1
if done or episode_timesteps == max_episode_steps:
logger.add_scalar('episode_reward', episode_reward,
episode_num)
logger.add_scalar('total_driving_metre', total_driving_metre,
episode_num)
#if len(model.replay_buffer) > max(learning_starts, model.batch_size):
# for i in range(episode_timesteps):
# model.train_step(total_steps, noise_std = 0.1, noise_clip=0.25)
# print(len(model.replay_buffer))
if episode_reward > 50:
print('Success')
else:
print('Fail')
last_episode_reward = episode_reward
if episode_num % 50 == 0:
model.save(directory=ckpt_path, filename=str(episode_num))
break
sm.close_all()
vehicle.destroy()
def main():
global global_transform, max_steer_angle
client = carla.Client(config['host'], config['port'])
client.set_timeout(config['timeout'])
world = client.load_world('Town01')
weather = carla.WeatherParameters(cloudiness=random.randint(0, 10),
precipitation=0,
sun_altitude_angle=random.randint(
50, 90))
set_weather(world, weather)
settings = world.get_settings()
settings.synchronous_mode = True
settings.fixed_delta_seconds = 0.01
world.apply_settings(settings)
blueprint = world.get_blueprint_library()
# world_map = world.get_map()
vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.audi.a2')
global_dict['vehicle'] = vehicle
# Enables or disables the simulation of physics on this actor.
vehicle.set_simulate_physics(True)
physics_control = vehicle.get_physics_control()
env = CARLAEnv(world, vehicle, global_dict, args)
# state = env.reset()
max_steer_angle = np.deg2rad(physics_control.wheels[0].max_steer_angle)
sensor_dict = {
'camera': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': image_callback,
},
'camera:view': {
#'transform':carla.Transform(carla.Location(x=0.0, y=4.0, z=4.0), carla.Rotation(pitch=-30, yaw=-60)),
'transform':
carla.Transform(carla.Location(x=-3.0, y=0.0, z=6.0),
carla.Rotation(pitch=-45)),
#'transform':carla.Transform(carla.Location(x=0.0, y=0.0, z=6.0), carla.Rotation(pitch=-90)),
'callback':
view_image_callback,
},
'collision': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': collision_callback,
},
}
sm = SensorManager(world, blueprint, vehicle, sensor_dict)
sm.init_all()
time.sleep(0.5)
while True:
if (global_dict['img']
is not None) and (global_dict['nav']
is not None) and (global_dict['img_nav']
is not None):
break
else:
world.tick()
# time.sleep(0.01)
print('Start to control')
episode_timesteps = 0
episode_reward = 0
max_steps = 1e9
total_steps = 0
max_episode_steps = 1000
learning_starts = 2000 #2000
episode_num = 0
while total_steps < max_steps:
print("total_episode:", episode_num)
episode_num += 1
total_steps += 1
episode_timesteps = 0
episode_reward = 0
total_driving_metre = 0
state = env.reset()
plan_time = time.time()
global_dict['state0'] = cu.getActorState('odom', global_plan_time,
global_dict['vehicle'])
global_dict['state0'].x = global_transform.location.x
global_dict['state0'].y = global_transform.location.y
global_dict['state0'].z = global_transform.location.z
global_dict['state0'].theta = np.deg2rad(global_transform.rotation.yaw)
add_noise = True if random.random() < 0.5 else False
for step in range(max_episode_steps):
state_tensor = state.unsqueeze(0).to(device)
action = model.policy_net(state_tensor)
action = action.detach().cpu().numpy()[0]
if add_noise:
action = model.noise.get_action(action)
x_last = global_transform.location.x
y_last = global_transform.location.y
next_state, reward, done, ts = env.step(action)
plan_time = ts
global_dict['state0'] = cu.getActorState('odom', global_plan_time,
global_dict['vehicle'])
global_dict['state0'].x = global_transform.location.x
global_dict['state0'].y = global_transform.location.y
global_dict['state0'].z = global_transform.location.z
global_dict['state0'].theta = np.deg2rad(
global_transform.rotation.yaw)
x_now = global_transform.location.x
y_now = global_transform.location.y
driving_metre_in_step = np.sqrt((x_now - x_last)**2 +
(y_now - y_last)**2)
total_driving_metre += driving_metre_in_step
model.replay_buffer.push(state, action, reward, next_state, done)
if len(model.replay_buffer) > max(learning_starts,
model.batch_size):
print("Start Train")
# time_s = time.time()
model.train_step(
total_steps, noise_std=0.2,
noise_clip=0.3) #noise_std = 0.2 noise_clip = 0.5
# time_e = time.time()
# print('time:', time_e - time_s)
state = next_state
episode_reward += reward
total_steps += 1
episode_timesteps += 1
if done or episode_timesteps == max_episode_steps:
logger.add_scalar('episode_reward', episode_reward,
episode_num)
logger.add_scalar('total_driving_metre', total_driving_metre,
episode_num)
#if len(model.replay_buffer) > max(learning_starts, model.batch_size):
# for i in range(episode_timesteps):
# model.train_step(total_steps, noise_std = 0.1, noise_clip=0.25)
# print(len(model.replay_buffer))
if episode_reward > 50:
print('Success')
else:
print('Fail')
# last_episode_reward = episode_reward
if episode_num % 5 == 0:
model.save(directory=ckpt_path, filename=str(episode_num))
break
cv2.destroyAllWindows()
sm.close_all()
vehicle.destroy()
def main():
client = carla.Client(config['host'], config['port'])
client.set_timeout(config['timeout'])
world = client.load_world('Town01')
weather = carla.WeatherParameters(cloudiness=random.randint(0, 10),
precipitation=0,
sun_altitude_angle=random.randint(
50, 90))
set_weather(world, weather)
blueprint = world.get_blueprint_library()
world_map = world.get_map()
vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.audi.a2')
#vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.yamaha.yzf')
#vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.*.*')
global_dict['vehicle'] = vehicle
# Enables or disables the simulation of physics on this actor.
vehicle.set_simulate_physics(True)
physics_control = vehicle.get_physics_control()
global_dict['max_steer_angle'] = np.deg2rad(
physics_control.wheels[0].max_steer_angle)
sensor_dict = {
'camera': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': image_callback,
},
'camera:view': {
#'transform':carla.Transform(carla.Location(x=0.0, y=4.0, z=4.0), carla.Rotation(pitch=-30, yaw=-60)),
'transform':
carla.Transform(carla.Location(x=-3.0, y=0.0, z=6.0),
carla.Rotation(pitch=-45)),
#'transform':carla.Transform(carla.Location(x=0.0, y=0.0, z=6.0), carla.Rotation(pitch=-90)),
'callback':
view_image_callback,
},
'lidar': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': lidar_callback,
},
}
sm = SensorManager(world, blueprint, vehicle, sensor_dict)
sm.init_all()
#spawn_points = world_map.get_spawn_points()
waypoint_tuple_list = world_map.get_topology()
origin_map = get_map(waypoint_tuple_list)
agent = BasicAgent(vehicle, target_speed=args.max_speed)
# prepare map
destination = carla.Transform()
destination.location = world.get_random_location_from_navigation()
#destination = get_random_destination(spawn_points)
global_dict['plan_map'] = replan(agent, destination,
copy.deepcopy(origin_map))
# start to plan
plan_thread = threading.Thread(target=make_plan, args=())
#visualization_thread = threading.Thread(target = show_traj, args=())
while True:
if (global_dict['img']
is not None) and (global_dict['nav']
is not None) and (global_dict['pcd']
is not None):
plan_thread.start()
break
else:
time.sleep(0.001)
# wait for the first plan result
while not global_dict['start_control']:
time.sleep(0.001)
#visualization_thread.start()
# start to control
print('Start to control')
ctrller = CapacController(world, vehicle, 30)
env = CARLAEnv(world, vehicle)
env.reset()
#env.step()
while True:
# change destination
if close2dest(vehicle, destination):
#destination = get_random_destination(spawn_points)
print('get destination !', time.time())
destination = carla.Transform()
destination.location = world.get_random_location_from_navigation()
global_dict['plan_map'] = replan(agent, destination,
copy.deepcopy(origin_map))
v = global_dict['vehicle'].get_velocity()
a = global_dict['vehicle'].get_acceleration()
global_dict['vel'] = np.sqrt(v.x**2 + v.y**2 + v.z**2)
global_dict['a'] = np.sqrt(a.x**2 + a.y**2 + a.z**2)
#steer_angle = global_dict['vehicle'].get_control().steer*global_dict['max_steer_angle']
#w = global_vel*np.tan(steer_angle)/2.405
control_time = time.time()
dt = control_time - global_dict['trajectory']['time']
index = int((dt / args.max_t) // args.dt) + 5
if index > 0.99 / args.dt:
continue
"""
transform = vehicle.get_transform()
dx, dy, dyaw = get_diff_tf(transform, global_dict['transform'])
dyaw = -dyaw
_x = global_dict['trajectory']['x'][index] - dx
_y = global_dict['trajectory']['y'][index] - dy
x = _x*np.cos(dyaw) + _y*np.sin(dyaw)
y = _y*np.cos(dyaw) - _x*np.sin(dyaw)
_vx = global_dict['trajectory']['vx'][index]
_vy = global_dict['trajectory']['vy'][index]
vx = _vx*np.cos(dyaw) + _vy*np.sin(dyaw)
vy = _vy*np.cos(dyaw) - _vx*np.sin(dyaw)
_ax = global_dict['trajectory']['ax'][index]
_ay = global_dict['trajectory']['ay'][index]
ax = _ax*np.cos(dyaw) + _ay*np.sin(dyaw)
ay = _ay*np.cos(dyaw) - _ax*np.sin(dyaw)
"""
control = ctrller.run_step(global_dict['trajectory'], index,
global_dict['state0'])
env.step(control)
#vehicle.apply_control(control)
"""
dyaw = np.deg2rad(global_dict['transform'].rotation.yaw)
x = global_dict['trajectory']['x'][index]*np.cos(dyaw) + global_dict['trajectory']['y'][index]*np.sin(dyaw)
y = global_dict['trajectory']['y'][index]*np.cos(dyaw) - global_dict['trajectory']['x'][index]*np.sin(dyaw)
"""
curve = None #show_traj(True)
#visualize(global_dict['view_img'], global_dict['draw_cost_map'], global_dict['nav'], args, curve)
visualize(global_dict, global_dict['draw_cost_map'], args, curve)
cv2.destroyAllWindows()
sm.close_all()
vehicle.destroy()
def main():
global global_transform, max_steer_angle
client = carla.Client(config['host'], config['port'])
client.set_timeout(config['timeout'])
world = client.load_world('Town01')
weather = carla.WeatherParameters(cloudiness=random.randint(0, 10),
precipitation=0,
sun_altitude_angle=random.randint(
50, 90))
set_weather(world, weather)
settings = world.get_settings()
settings.synchronous_mode = True
settings.fixed_delta_seconds = 0.05
world.apply_settings(settings)
blueprint = world.get_blueprint_library()
# world_map = world.get_map()
vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.audi.a2')
global_dict['vehicle'] = vehicle
# Enables or disables the simulation of physics on this actor.
vehicle.set_simulate_physics(True)
physics_control = vehicle.get_physics_control()
env = CARLAEnv(world, vehicle, global_dict, args, generator)
# state = env.reset()
max_steer_angle = np.deg2rad(physics_control.wheels[0].max_steer_angle)
sensor_dict = {
'camera': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': image_callback,
},
'camera:view': {
#'transform':carla.Transform(carla.Location(x=0.0, y=4.0, z=4.0), carla.Rotation(pitch=-30, yaw=-60)),
'transform':
carla.Transform(carla.Location(x=-3.0, y=0.0, z=6.0),
carla.Rotation(pitch=-45)),
#'transform':carla.Transform(carla.Location(x=0.0, y=0.0, z=6.0), carla.Rotation(pitch=-90)),
'callback':
view_image_callback,
},
'collision': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': collision_callback,
},
}
sm = SensorManager(world, blueprint, vehicle, sensor_dict)
sm.init_all()
time.sleep(0.5)
while True:
if (global_dict['img']
is not None) and (global_dict['nav']
is not None) and (global_dict['img_nav']
is not None):
break
else:
world.tick()
# time.sleep(0.01)
# wait for the first plan result
# while not start_control:
# time.sleep(0.001)
print('Start to control')
# ctrller = CapacController(world, vehicle, 30)
episode_timesteps = 0
episode_reward = 0
max_steps = 1e9
total_steps = 0
max_episode_steps = 1000
learning_starts = 1000 #2000
episode_num = 0
start_train = False
while total_steps < max_steps:
print("total_episode:", episode_num)
episode_num += 1
total_steps += 1
episode_timesteps = 0
episode_reward = 0
total_driving_metre = 0
state = env.reset()
plan_time = time.time()
global_dict['state0'] = cu.getActorState('odom', global_plan_time,
global_dict['vehicle'])
global_dict['state0'].x = global_transform.location.x
global_dict['state0'].y = global_transform.location.y
global_dict['state0'].z = global_transform.location.z
global_dict['state0'].theta = np.deg2rad(global_transform.rotation.yaw)
add_noise = True if random.random() < 0.0 else False
print("add noise:", add_noise)
for step in range(max_episode_steps):
# t = torch.arange(0, 0.99, args.dt).unsqueeze(1).to(device)
# t.requires_grad = True
t = np.arange(0, 0.99, args.dt)
points_num = len(t)
# v = global_dict['v0'] if global_dict['v0'] > 4 else 4
# v_0 = torch.FloatTensor([v/args.max_speed]).unsqueeze(1)
# v_0 = v_0.to(device)
if state['v0'] < 4:
state['v0'] == 4
condition = torch.FloatTensor([state['v0'] / args.max_speed] *
points_num).view(-1, 1)
condition = condition.to(device)
input_img = state['img_nav'].unsqueeze(0).to(device)
v_0 = torch.FloatTensor([state['v0'] / args.max_speed
]).unsqueeze(1).to(device)
action = model.policy_net(input_img, v_0)
action = action.detach().cpu().numpy()[0]
if add_noise:
action = model.noise.get_action(action)
# print(action)
x_last = global_transform.location.x
y_last = global_transform.location.y
next_state, reward, done, ts = env.step(action, plan_time,
condition)
plan_time = ts
global_dict['state0'] = cu.getActorState('odom', global_plan_time,
global_dict['vehicle'])
global_dict['state0'].x = global_transform.location.x
global_dict['state0'].y = global_transform.location.y
global_dict['state0'].z = global_transform.location.z
global_dict['state0'].theta = np.deg2rad(
global_transform.rotation.yaw)
x_now = global_transform.location.x
y_now = global_transform.location.y
driving_metre_in_step = np.sqrt((x_now - x_last)**2 +
(y_now - y_last)**2)
total_driving_metre += driving_metre_in_step
model.replay_buffer.push(state, action, reward, next_state, done)
# print(sys.getsizeof(model.replay_buffer.buffer))
# print(u'当前进程的内存使用：%.4f GB' % (psutil.Process(os.getpid()).memory_info().rss / 1024 / 1024 / 1024) )
if len(model.replay_buffer) > max(learning_starts,
model.batch_size):
start_train = True
print("Start Train:")
# time_s = time.time()
model.train_step(
total_steps, noise_std=0.5,
noise_clip=2) #noise_std = 0.2 noise_clip = 0.5
# time_e = time.time()
# print('time:', time_e - time_s)
state = next_state
episode_reward += reward
total_steps += 1
episode_timesteps += 1
if done or episode_timesteps == max_episode_steps:
logger.add_scalar('episode_reward', episode_reward,
episode_num)
logger.add_scalar('total_driving_metre', total_driving_metre,
episode_num)
#if len(model.replay_buffer) > max(learning_starts, model.batch_size):
# for i in range(episode_timesteps):
# model.train_step(total_steps, noise_std = 0.1, noise_clip=0.25)
# print(len(model.replay_buffer))
print("episode_reward:%.2f" % (episode_reward))
# if episode_reward > 50:
# print('Success')
# else:
# print('Fail')
# last_episode_reward = episode_reward
if episode_num % 20 == 0 and start_train == True:
model.save(directory=ckpt_path, filename=str(episode_num))
break
cv2.destroyAllWindows()
sm.close_all()
vehicle.destroy()
def main():
client = carla.Client(config['host'], config['port'])
client.set_timeout(config['timeout'])
world = client.load_world('Town01')
weather = carla.WeatherParameters(cloudiness=random.randint(0, 10),
precipitation=0,
sun_altitude_angle=random.randint(
50, 90))
set_weather(world, weather)
blueprint = world.get_blueprint_library()
world_map = world.get_map()
vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.audi.a2')
#vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.yamaha.yzf')
#vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.*.*')
global_dict['vehicle'] = vehicle
# Enables or disables the simulation of physics on this actor.
vehicle.set_simulate_physics(True)
physics_control = vehicle.get_physics_control()
global_dict['max_steer_angle'] = np.deg2rad(
physics_control.wheels[0].max_steer_angle)
sensor_dict = {
'camera': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': image_callback,
},
'camera:view': {
#'transform':carla.Transform(carla.Location(x=0.0, y=4.0, z=4.0), carla.Rotation(pitch=-30, yaw=-60)),
'transform':
carla.Transform(carla.Location(x=-3.0, y=0.0, z=9.0),
carla.Rotation(pitch=-45)),
#'transform':carla.Transform(carla.Location(x=0.0, y=0.0, z=6.0), carla.Rotation(pitch=-90)),
'callback':
view_image_callback,
},
'lidar': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': lidar_callback,
},
'collision': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': collision_callback,
},
}
sm = SensorManager(world, blueprint, vehicle, sensor_dict)
sm.init_all()
ctrller = CapacController(world, vehicle, 30)
agent = BasicAgent(vehicle, target_speed=30)
world_map = world.get_map()
waypoint_tuple_list = world_map.get_topology()
origin_map = get_map(waypoint_tuple_list)
spawn_points = world_map.get_spawn_points()
route_trace = None
start_point = random.choice(spawn_points)
destination = random.choice(spawn_points)
vehicle.set_transform(start_point)
global_dict['plan_map'] = replan(agent, destination,
copy.deepcopy(origin_map))
global_dict['collision'] = False
start_waypoint = agent._map.get_waypoint(agent._vehicle.get_location())
end_waypoint = agent._map.get_waypoint(destination.location)
route_trace = agent._trace_route(start_waypoint, end_waypoint)
start_point.rotation = route_trace[0][0].transform.rotation
vehicle.set_transform(start_point)
plan_thread = threading.Thread(target=make_plan, args=())
while True:
if (global_dict['img'] is not None) and (global_dict['nav']
is not None):
plan_thread.start()
break
else:
time.sleep(0.001)
while global_dict['trajectory'] is None:
time.sleep(0.001)
while True:
done = False
trajectory = global_dict['trajectory']
for i in range(20):
if global_dict['collision']:
done = True
print('Done !')
break
if close2dest(vehicle, destination):
done = True
print('Success !')
break
control_time = time.time()
dt = control_time - trajectory['time']
index = int((dt / args.max_t) // args.dt) + 5
if index > 0.99 // args.dt - 10:
continue
control = ctrller.run_step(trajectory, index,
global_dict['state0'])
vehicle.apply_control(control)
cv2.imshow('Visualization', global_dict['view_img'])
cv2.imshow('Costmap', global_dict['draw_cost_map'])
cv2.waitKey(1)
if done or global_dict['collision']:
start_point = random.choice(spawn_points)
destination = random.choice(spawn_points)
vehicle.set_transform(start_point)
global_dict['plan_map'] = replan(agent, destination,
copy.deepcopy(origin_map))
global_dict['collision'] = False
start_waypoint = agent._map.get_waypoint(
agent._vehicle.get_location())
end_waypoint = agent._map.get_waypoint(destination.location)
route_trace = agent._trace_route(start_waypoint, end_waypoint)
start_point.rotation = route_trace[0][0].transform.rotation
vehicle.set_transform(start_point)
done = False
sm.close_all()
vehicle.destroy()
def main():
global global_nav, global_control, global_pos, global_vel
client = carla.Client(config['host'], config['port'])
client.set_timeout(config['timeout'])
#world = client.get_world()
world = client.load_world('Town10')
weather = carla.WeatherParameters(cloudiness=random.randint(0, 10),
precipitation=0,
sun_altitude_angle=random.randint(
50, 90))
#world.set_weather(carla.WeatherParameters.ClearNoon)
set_weather(world, weather)
blueprint = world.get_blueprint_library()
world_map = world.get_map()
vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.audi.a2')
# Enables or disables the simulation of physics on this actor.
vehicle.set_simulate_physics(True)
sensor_dict = {
'camera': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': image_callback,
},
'camera:view': {
'transform':
carla.Transform(carla.Location(x=-3.0, y=0.0, z=3.5),
carla.Rotation(pitch=-30)),
'callback':
view_image_callback,
},
'lidar': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': lidar_callback,
},
}
sm = SensorManager(world, blueprint, vehicle, sensor_dict)
sm.init_all()
time.sleep(0.3)
spawn_points = world_map.get_spawn_points()
waypoint_tuple_list = world_map.get_topology()
origin_map = get_map(waypoint_tuple_list)
agent = BasicAgent(vehicle, target_speed=MAX_SPEED)
#port = 8000
#tm = client.get_trafficmanager(port)
#vehicle.set_autopilot(True,port)
#tm.ignore_lights_percentage(vehicle,100)
destination = get_random_destination(spawn_points)
plan_map = replan(agent, destination, copy.deepcopy(origin_map))
joystick = JoyStick(verbose=False)
joystick.start()
for cnt in tqdm(range(args.num)):
if close2dest(vehicle, destination):
destination = get_random_destination(spawn_points)
plan_map = replan(agent, destination, copy.deepcopy(origin_map))
"""
if vehicle.is_at_traffic_light():
traffic_light = vehicle.get_traffic_light()
if traffic_light.get_state() == carla.TrafficLightState.Red:
traffic_light.set_state(carla.TrafficLightState.Green)
"""
#control = agent.run_step()
control = joystick.get()
velocity = vehicle.get_velocity()
if np.sqrt(velocity.x**2 + velocity.y**2) > 8:
control.throttle = 0
#control.manual_gear_shift = False
global_control = control
vehicle.apply_control(control)
nav = get_nav(vehicle, plan_map)
global_nav = nav
global_pos = vehicle.get_transform()
global_vel = vehicle.get_velocity()
#cv2.imshow('Nav', nav)
cv2.imshow('Vision', global_view_img)
cv2.waitKey(10)
index = str(time.time())
#save_data(index)
cmd_file.close()
pos_file.close()
vel_file.close()
cv2.destroyAllWindows()
sm.close_all()
vehicle.destroy()
def main():
global global_nav, global_control, global_pos, global_vel, global_acceleration, global_angular_velocity
client = carla.Client(config['host'], config['port'])
client.set_timeout(config['timeout'])
#world = client.get_world()
world = client.load_world('Town01')
"""
weather = carla.WeatherParameters(
cloudiness=random.randint(0,80),
precipitation=0,
sun_altitude_angle=random.randint(40,90)
)
set_weather(world, weather)
"""
world.set_weather(carla.WeatherParameters.MidRainSunset)
blueprint = world.get_blueprint_library()
world_map = world.get_map()
vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.audi.a2')
# Enables or disables the simulation of physics on this actor.
vehicle.set_simulate_physics(True)
_world = World(vehicle, world)
sensor_dict = {
'camera': {
'transform': carla.Transform(
carla.Location(x=0.5, y=0.0, z=2.5)
), #'transform':carla.Transform(carla.Location(x=-3.0, y=0.0, z=2.5), carla.Rotation(pitch=-20)),
'callback': image_callback,
},
'lidar': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': lidar_callback,
},
}
sm = SensorManager(world, blueprint, vehicle, sensor_dict)
sm.init_all()
time.sleep(0.3)
spawn_points = world_map.get_spawn_points()
waypoint_tuple_list = world_map.get_topology()
origin_map = get_map(waypoint_tuple_list)
#agent = BasicAgent(vehicle, target_speed=MAX_SPEED)
agent = BehaviorAgent(vehicle,
ignore_traffic_light=True,
behavior='normal')
_destination = carla.Transform()
destination = world.get_random_location_from_navigation()
_destination.location = destination
#port = 8000
#tm = client.get_trafficmanager(port)
#tm.ignore_lights_percentage(vehicle,100)
#destination = get_random_destination(spawn_points)
plan_map = replan2(agent, _destination, copy.deepcopy(origin_map))
#agent.set_destination(agent.vehicle.get_location(), destination, clean=True)
#for cnt in tqdm(range(args.num)):
for cnt in range(args.num):
if close2dest(vehicle, _destination):
_destination = carla.Transform()
destination = world.get_random_location_from_navigation()
_destination.location = destination
plan_map = replan2(agent, _destination, copy.deepcopy(origin_map))
#agent.set_destination(agent.vehicle.get_location(), destination, clean=True)
if vehicle.is_at_traffic_light():
traffic_light = vehicle.get_traffic_light()
if traffic_light.get_state() == carla.TrafficLightState.Red:
traffic_light.set_state(carla.TrafficLightState.Green)
agent.update_information(_world)
speed_limit = vehicle.get_speed_limit()
agent.get_local_planner().set_speed(speed_limit)
control = agent.run_step()
control.manual_gear_shift = False
global_control = control
vehicle.apply_control(control)
nav = get_nav(vehicle, plan_map)
global_nav = nav
global_pos = vehicle.get_transform()
global_vel = vehicle.get_velocity()
global_acceleration = vehicle.get_acceleration()
global_angular_velocity = vehicle.get_angular_velocity()
#yaw = global_pos.rotation.yaw
#_ax = global_acceleration.x
#_ay = global_acceleration.y
#ax = _ax*np.cos(yaw) + _ay*np.sin(yaw)
#ay = _ay*np.cos(yaw) - _ax*np.sin(yaw)
#cv2.imshow('Nav', nav)
cv2.imshow('Vision', global_img)
cv2.waitKey(10)
index = str(time.time())
#save_data(index)
cmd_file.close()
pos_file.close()
vel_file.close()
acc_file.close()
angular_vel_file.close()
cv2.destroyAllWindows()
sm.close_all()
vehicle.destroy()