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()