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():
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_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_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()
precipitation=0,
sun_altitude_angle=random.randint(
40, 90))
set_weather(_world, weather)
controller = DualControl(world)
vehicle = world.player
agent = BasicAgent(vehicle, target_speed=31)
spawn_points = world_map.get_spawn_points()
waypoint_tuple_list = world_map.get_topology()
origin_map = get_map(waypoint_tuple_list)
destination = get_random_destination(spawn_points)
plan_map = replan(agent, destination, copy.deepcopy(origin_map))
#plan_map.resize((1000, 1000)).show()
clock = pygame.time.Clock()
while True:
if close2dest(vehicle, destination):
destination = get_random_destination(spawn_points)
plan_map = replan(agent, destination,
copy.deepcopy(origin_map))
clock.tick_busy_loop(60)
if controller.parse_events(world, clock):
break
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)
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()
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_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():
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()