def image_callback(data):
global global_plan_time, global_transform
global_plan_time = time.time()
global_transform = global_dict['vehicle'].get_transform()
# 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)
array = np.frombuffer(data.raw_data, dtype=np.dtype("uint8"))
array = np.reshape(array, (data.height, data.width, 4)) # RGBA format
global_dict['img'] = array
# try:
global_dict['nav'] = get_nav(global_dict['vehicle'],
global_dict['plan_map'])
img = Image.fromarray(cv2.cvtColor(global_dict['img'], cv2.COLOR_BGR2RGB))
nav = Image.fromarray(cv2.cvtColor(global_dict['nav'], cv2.COLOR_BGR2RGB))
# nav = Image.fromarray(cv2.flip(cv2.cvtColor(global_dict['nav'],cv2.COLOR_BGR2RGB),1))
img = img_trans(img)
nav = img_trans(nav)
# global_dict['img_nav'] = torch.cat((img, nav), 0).unsqueeze(0).to(device)
global_dict['img_nav'] = torch.cat((img, nav), 0)
v = global_dict['vehicle'].get_velocity()
global_dict['v0'] = np.sqrt(v.x**2 + v.y**2)
global_dict['ts'] = time.time()
def image_callback(data):
global global_img, global_plan_time, global_vehicle, global_plan_map,global_nav, global_transform, global_v0
global_plan_time = time.time()
array = np.frombuffer(data.raw_data, dtype=np.dtype("uint8"))
array = np.reshape(array, (data.height, data.width, 4)) # RGBA format
global_img = array
global_transform = global_vehicle.get_transform()
try:
global_nav = get_nav(global_vehicle, global_plan_map)
v = global_vehicle.get_velocity()
global_v0 = np.sqrt(v.x**2+v.y**2+v.z**2)
except:
pass
def image_callback(data):
#global_dict['plan_time'] = time.time()
array = np.frombuffer(data.raw_data, dtype=np.dtype("uint8"))
array = np.reshape(array, (data.height, data.width, 4)) # RGBA format
global_dict['img'] = array
global_dict['transform'] = global_dict['vehicle'].get_transform()
try:
global_dict['nav'] = get_nav(global_dict['vehicle'],
global_dict['plan_map'])
v = global_dict['vehicle'].get_velocity()
global_dict['v0'] = np.sqrt(v.x**2 + v.y**2 + v.z**2)
except:
pass
def image_callback(data):
global state0, global_img, global_plan_time, global_vehicle, global_plan_map, global_nav, global_transform, global_v0
global_plan_time = time.time()
global_transform = global_vehicle.get_transform()
state0 = cu.getActorState('odom', global_plan_time, global_vehicle)
state0.x = global_transform.location.x
state0.y = global_transform.location.y
state0.z = global_transform.location.z
state0.theta = np.deg2rad(global_transform.rotation.yaw)
array = np.frombuffer(data.raw_data, dtype=np.dtype("uint8"))
array = np.reshape(array, (data.height, data.width, 4)) # RGBA format
global_img = array
# try:
global_nav = get_nav(global_vehicle, global_plan_map)
v = global_vehicle.get_velocity()
global_v0 = np.sqrt(v.x**2 + v.y**2)
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()
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)
nav = get_nav(vehicle, plan_map)
big_nav = get_big_nav(vehicle, plan_map)
global_nav = nav
global_pos = vehicle.get_transform()
global_vel = vehicle.get_velocity()
global_control = controller._control
global_acceleration = vehicle.get_acceleration()
global_angular_velocity = vehicle.get_angular_velocity()
cv2.imshow('Vision', big_nav)
cv2.waitKey(10)
index = str(time.time())
save_data(index)
world.tick(clock)
world.render(display)
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()
