def interpolate_trajectory(world_map, waypoints_trajectory, hop_resolution=1.0): """ Given some raw keypoints interpolate a full dense trajectory to be used by the user. Args: world: an reference to the CARLA world so we can use the planner waypoints_trajectory: the current coarse trajectory hop_resolution: is the resolution, how dense is the provided trajectory going to be made Return: route: full interpolated route both in GPS coordinates and also in its original form. """ dao = GlobalRoutePlannerDAO(world_map, hop_resolution) grp = GlobalRoutePlanner(dao) grp.setup() # Obtain route plan route = [] for i in range(len(waypoints_trajectory) - 1): # Goes until the one before the last. waypoint = waypoints_trajectory[i] waypoint_next = waypoints_trajectory[i + 1] interpolated_trace = grp.trace_route(waypoint, waypoint_next) for wp_tuple in interpolated_trace: route.append((wp_tuple[0].transform, wp_tuple[1])) return route
def __init__(self, vehicle, resolution=15, threshold_before=2.5, threshold_after=5.0): from agents.navigation.global_route_planner import GlobalRoutePlanner from agents.navigation.global_route_planner_dao import GlobalRoutePlannerDAO # Max skip avoids misplanning when route includes both lanes. self._max_skip = 20 self._threshold_before = threshold_before self._threshold_after = threshold_after self._vehicle = vehicle self._map = vehicle.get_world().get_map() self._grp = GlobalRoutePlanner( GlobalRoutePlannerDAO(self._map, resolution)) self._grp.setup() self._route = None self._waypoints_queue = deque(maxlen=20000) self.target = (None, None) self.checkpoint = (None, None) self.distance_to_goal = float("inf") self.distances = deque(maxlen=20000)
def __init__(self, carla_world): """ Constructor """ self._world = carla_world self._map = carla_world.get_map() self._ego_vehicle = None self._ego_vehicle_location = None # self.current_route_list = RouteList() self._on_tick = None self._goal = None self._role_name = rospy.get_param("~role_name", 'ego_vehicle') self._dao = GlobalRoutePlannerDAO( self._map, sampling_resolution=WAYPOINT_DISTANCE) self._random_route_planner = RandomRoutePlanner(self._dao) self._random_route_planner.setup() self._global_planner = GlobalRoutePlanner(self._dao) self._global_planner.setup() self._getWaypointService = rospy.Service( '/carla_waypoint_publisher/{}/get_waypoint'.format( self._role_name), GetWaypoint, self._get_waypoint) self._getActorWaypointService = rospy.Service( '/carla_waypoint_publisher/{}/get_actor_waypoint'.format( self._role_name), GetActorWaypoint, self._get_actor_waypoint) self._getRouteService = rospy.Service( '/carla_client_interface/{}/get_route'.format(self._role_name), RoutePlanService, self._get_route) # self._getAgentPotentialRoutesService = rospy.Service( # '/carla_client_interface/{}/agent_potential_routes'.format(self._role_name), # RoutePlanService, self._get_agent_potential_route_service) self._update_lock = threading.Lock() rospy.loginfo("Waiting for ego vehicle .....")
def _set_route(self, hop_resolution=1.0): world = CarlaDataProvider.get_world() dao = GlobalRoutePlannerDAO(world.get_map(), hop_resolution) grp = GlobalRoutePlanner(dao) grp.setup() spawn_points = CarlaDataProvider._spawn_points # start, target = random.choice(spawn_points, size=2) start_idx, target_idx = random.choice(len(spawn_points), size=2) # DEBUG # start_idx, target_idx = 57, 87 # start_idx, target_idx = 2, 18 # print (start_idx, target_idx) start = spawn_points[start_idx] target = spawn_points[target_idx] route = grp.trace_route(start.location, target.location) self.route = [(w.transform, c) for w, c in route] CarlaDataProvider.set_ego_vehicle_route([(w.transform.location, c) for w, c in route]) gps_route = location_route_to_gps(self.route, *_get_latlon_ref(world)) self.config.agent.set_global_plan(gps_route, self.route) self.timeout = self._estimate_route_timeout()
def interpolate_trajectory(world, waypoints_trajectory, hop_resolution=1.0): """ This method is fixed based on original interpolate_trajectory Given some raw keypoints interpolate a full dense trajectory to be used by the user. :param world: an reference to the CARLA world so we can use the planner :param waypoints_trajectory: the current coarse trajectory :param hop_resolution: is the resolution, how dense is the provided trajectory going to be made :return: the full interpolated route both in waypoints. """ dao = GlobalRoutePlannerDAO(world.get_map(), hop_resolution) grp = GlobalRoutePlanner(dao) grp.setup() # Obtain route plan route = [] # waypoints for i in range(len(waypoints_trajectory) - 1): # Goes until the one before the last. waypoint = waypoints_trajectory[i] waypoint_next = waypoints_trajectory[i + 1] interpolated_trace = grp.trace_route(waypoint, waypoint_next) for wp_tuple in interpolated_trace: route.append((wp_tuple[0], wp_tuple[1])) return route
def __init__(self, vehicle, ignore_traffic_light=False): self.vehicle = vehicle self.ignore_traffic_light = ignore_traffic_light self.world = vehicle.get_world() self.map = self.world.get_map() self.control_vehicle = pid.VehiclePIDController(vehicle, max_throttle=1, args_lateral={ 'K_P': 0.58, 'K_D': 0.4, 'K_I': 0.5 }, args_longitudinal={ 'K_P': 0.15, 'K_D': 0.05, 'K_I': 0.07 }) # Relacionados à rota self.spawn_location = None self.destination_location = None self.dao = GlobalRoutePlannerDAO(self.map, 2.0) self.grp = GlobalRoutePlanner(self.dao) self.grp.setup() self.route = [] # Relacionados aos obstáculos e sensores self.obstacle_info = {'distance': None, 'actor': None} self.emergency_brake_distance = 3 self.dynamic_brake_distance = 0 self.previous_obstacle = None self.status = "" # Sensores # Sensor de colisão sendo usado para definir a posição da câmera do simulador # Não é usado de fato como sensor e seus dados não são tratados self.camera_bp = self.world.get_blueprint_library().find( 'sensor.other.collision') self.camera_transform = carla.Transform(carla.Location(x=-5.5, z=3.5), carla.Rotation(pitch=345)) self._camera = self.world.spawn_actor(self.camera_bp, self.camera_transform, attach_to=vehicle) # Sensor de obstáculos # É uma implementação de um sensor de radar que já identifica o obstáculo # e a distância até ele self.obstacle_sensor_bp = self.world.get_blueprint_library().find( 'sensor.other.obstacle') self.obstacle_sensor_bp.set_attribute('distance', '30.0') self.obstacle_sensor_bp.set_attribute('only_dynamics', 'True') self.obstacle_sensor_bp.set_attribute('sensor_tick', '0.1') self.obstacle_sensor_bp.set_attribute('hit_radius', '1.0') # Posicionado na frente do radiador do veículo self.obstacle_sensor_transform = carla.Transform( carla.Location(x=1.5, z=0.5), carla.Rotation()) self.obstacle_sensor = self.world.spawn_actor( self.obstacle_sensor_bp, self.obstacle_sensor_transform, attach_to=vehicle) self.obstacle_sensor.listen(lambda data: self.obstacle_detection(data))
def __init__(self, actor, osc_position, distance, along_route=False, comparison_operator=operator.lt, name="InTriggerDistanceToOSCPosition"): """ Setup parameters """ super(InTriggerDistanceToOSCPosition, self).__init__(name) self._actor = actor self._osc_position = osc_position self._distance = distance self._along_route = along_route self._comparison_operator = comparison_operator self._map = CarlaDataProvider.get_map() if self._along_route: # Get the global route planner, used to calculate the route dao = GlobalRoutePlannerDAO(self._map, 0.5) grp = GlobalRoutePlanner(dao) grp.setup() self._grp = grp else: self._grp = None
def __init__(self, actor, other_actor, time, condition_freespace=False, along_route=False, comparison_operator=operator.lt, name="TimeToArrival"): """ Setup parameters """ super(InTimeToArrivalToVehicle, self).__init__(name) self.logger.debug("%s.__init__()" % (self.__class__.__name__)) self._map = CarlaDataProvider.get_map() self._actor = actor self._other_actor = other_actor self._time = time self._condition_freespace = condition_freespace self._along_route = along_route self._comparison_operator = comparison_operator if self._along_route: # Get the global route planner, used to calculate the route dao = GlobalRoutePlannerDAO(self._map, 0.5) grp = GlobalRoutePlanner(dao) grp.setup() self._grp = grp else: self._grp = None
def interpolate_trajectory(world, waypoints_trajectory, hop_resolution=1.0): """ Given some raw keypoints interpolate a full dense trajectory to be used by the user. :param world: an reference to the CARLA world so we can use the planner :param waypoints_trajectory: the current coarse trajectory :param hop_resolution: is the resolution, how dense is the provided trajectory going to be made :return: the full interpolated route both in GPS coordinates and also in its original form. """ dao = GlobalRoutePlannerDAO(world.get_map(), hop_resolution) grp = GlobalRoutePlanner(dao) grp.setup() # Obtain route plan route = [] for i in range(len(waypoints_trajectory) - 1): # Goes until the one before the last. waypoint = waypoints_trajectory[i] waypoint_next = waypoints_trajectory[i + 1] interpolated_trace = grp.trace_route(waypoint, waypoint_next) for wp_tuple in interpolated_trace: route.append((wp_tuple[0].transform, wp_tuple[1])) # Increase the route position to avoid fails lat_ref, lon_ref = _get_latlon_ref(world) return location_route_to_gps(route, lat_ref, lon_ref), route
def main(): client = carla.Client('localhost', 2000) client.set_timeout(10) world = client.get_world() map = world.get_map() vehicle = world.get_actor(87) location1 = map.get_waypoint(vehicle.get_location()) print(location1) #waypoint1 = vehicle.get_location() #custom_controller = VehiclePIDController(vehicle, args_lateral = {'K_P': 1, 'K_D': 0.0, 'K_I': 0}, args_longitudinal = {'K_P': 1, 'K_D': 0.0, 'K_I': 0.0}) vehicle2 = world.get_actor(86) vehicle2.set_simulate_physics(True) location2 = map.get_waypoint(vehicle2.get_location()) print(location2) #waypoint2 = vehicle.get_location() #vehicle2.set_transform(waypoint.transform) #control_signal = custom_controller.run_step(1, waypoint) #vehicle.apply_control(control_signal) #Routeplanner amap = world.get_map() sampling_resolution = 2 dao = GlobalRoutePlannerDAO(amap, sampling_resolution) grp = GlobalRoutePlanner(dao) grp.setup() spawn_points = world.get_map().get_spawn_points() spawn_point = spawn_points[50] vehicle2.set_transform(spawn_point) spawn_point2 = spawn_points[100] vehicle.set_transform(spawn_point2) a = carla.Location(spawn_points[50].location) b = carla.Location(spawn_points[100].location) w1 = grp.trace_route(a, b) i = 0 for w in w1: if i % 10 == 0: world.debug.draw_string(w[0].transform.location, 'O', draw_shadow=False, color=carla.Color(r=255, g=0, b=0), life_time=120.0, persistent_lines=True) else: world.debug.draw_string(w[0].transform.location, 'O', draw_shadow=False, color=carla.Color(r=0, g=0, b=255), life_time=1000.0, persistent_lines=True) i += 1
def __init__(self, carla_map): self._map = carla_map # Setup global planner. self._grp = GlobalRoutePlanner( GlobalRoutePlannerDAO( self._map, 1.0 # Distance between waypoints )) self._grp.setup()
def get_planner_command(self, current_point, end_point): current_location = current_point.location end_location = end_point.location global_Dao = GlobalRoutePlannerDAO(self.map) global_planner = GlobalRoutePlanner(global_Dao) global_planner.setup() commands = global_planner.abstract_route_plan(current_location, end_location) direction = commands[0].value return direction
def __init__(self, simulator_map, log_file=None): self._logger = erdos.utils.setup_logging('hd_map', log_file) self._map = simulator_map # Setup global planner. self._grp = GlobalRoutePlanner( GlobalRoutePlannerDAO( self._map, 1.0 # Distance between waypoints )) self._grp.setup()
def __init__(self, world_map, sampling_resolution, waypoint_error_threshold): self.global_route_planner_dao = GlobalRoutePlannerDAO(world_map, 1) self.global_route_planner = GlobalRoutePlanner( self.global_route_planner_dao) self.global_route_planner.setup() self.endpoint = None self.waypoints = None self.target_waypoint_i = 0 self.waypoint_error_threshold = waypoint_error_threshold
def __init__(self, carla_map, log_file_name=None): self._map = carla_map # Setup global planner. self._grp = GlobalRoutePlanner( GlobalRoutePlannerDAO( self._map, 1.0 # Distance between waypoints )) self._grp.setup() self._logger = setup_logging('hd_map', log_file_name)
def get_global_planner( world: carla.libcarla.World, planner_resolution: float ) -> agents.navigation.global_route_planner.GlobalRoutePlanner: """ Get a global route planner object. The planner object can be used to retrieve point to point routes and route topologies. """ world_map = world.get_map() dao = GlobalRoutePlannerDAO(world_map, planner_resolution) grp = GlobalRoutePlanner(dao) grp.setup() return grp
def calculate_route(self, goal): """ Calculate a route from the current location to 'goal' """ rospy.loginfo("Calculating route to x={}, y={}, z={}".format( goal.location.x, goal.location.y, goal.location.z)) dao = GlobalRoutePlannerDAO(self.world.get_map()) grp = GlobalRoutePlanner(dao) grp.setup() route = grp.trace_route( self.ego_vehicle.get_location(), carla.Location(goal.location.x, goal.location.y, goal.location.z)) return route
def main(): #pygame.init() client = carla.Client('localhost', 2000) client.set_timeout(10) world = client.get_world() vehicle2 = world.get_actor(86) frame = None #custom_controller = VehiclePIDController(vehicle2, args_lateral = {'K_P': 1, 'K_D': 0.0, 'K_I': 0}, args_longitudinal = {'K_P': 1, 'K_D': 0.0, 'K_I': 0.0}) #print('Enabling synchronous mode') #settings = world.get_settings() #settings.synchronous_mode = True #world.apply_settings(settings) vehicle2.set_simulate_physics(True) amap = world.get_map() sampling_resolution = 2 dao = GlobalRoutePlannerDAO(amap, sampling_resolution) grp = GlobalRoutePlanner(dao) grp.setup() spawn_points = world.get_map().get_spawn_points() driving_car = BasicAgent(vehicle2, target_speed=15) destiny = spawn_points[100].location driving_car.set_destination((destiny.x, destiny.y, destiny.z)) vehicle2.set_autopilot(True) #clock = pygame.time.Clock() while True: #clock.tick() world.tick() ts = world.wait_for_tick() # Get control commands control_hero = driving_car.run_step() vehicle2.apply_control(control_hero) if frame is not None: if ts.frame_count != frame + 1: logging.warning('frame skip!') print("frame skip!") frame = ts.frame_count
def _trace_route(self, start_waypoint, end_waypoint): """ This method sets up a global router and returns the optimal route from start_waypoint to end_waypoint """ # Setting up global router if self._grp is None: dao = GlobalRoutePlannerDAO(self._vehicle.get_world().get_map(), self._hop_resolution) grp = GlobalRoutePlanner(dao) grp.setup() self._grp = grp # Obtain route plan route = self._grp.trace_route(start_waypoint.transform.location, end_waypoint.transform.location) return route
def __init__(self, world: carla.World, route_resolution=1.0): """ Args: world (carla.World): the carla world instance route_resolution (float): the resolution in meters for planned route """ self._world = world self._map = world.get_map() self._waypoints = self._map.generate_waypoints(2.0) self._actors = [] # including vehicles and walkers self._actor_dict = {} self._actor_locations = {} self._route_resolution = route_resolution dao = GlobalRoutePlannerDAO(world.get_map(), sampling_resolution=route_resolution) self._global_route_planner = GlobalRoutePlanner(dao) self._global_route_planner.setup()
def global_plan( world: carla.World, # pylint: disable=no-member origin: carla.Location, # pylint: disable=no-member destination: carla.Location, # pylint: disable=no-member ) -> Tuple[Sequence[carla.Waypoint], Sequence[Any], float]: # pylint: disable=no-member """Generates the optimal plan between two location, respecting the topology. Args: world: The `CARLA` world. origin: The starting location. destination: The final destination. Returns: waypoints: A sequence of waypoints. roadoptions: A sequence of commands to navigate at each waypoint. distances: The distance per pair of waypoints of the plan. """ try: from agents.navigation.global_route_planner import GlobalRoutePlanner # pylint: disable=import-error from agents.navigation.global_route_planner_dao import GlobalRoutePlannerDAO # pylint: disable=import-error except ImportError: raise ImportError( "Missing CARLA installation, " "make sure the environment variable CARLA_ROOT is provided " "and that the PythonAPI is `easy_install`ed") # Setup global planner. grp_dao = GlobalRoutePlannerDAO(wmap=world.get_map(), sampling_resolution=1) grp = GlobalRoutePlanner(grp_dao) grp.setup() # Generate plan. waypoints, roadoptions = zip(*grp.trace_route(origin, destination)) # Accummulate pairwise distance. distances = [0.0] for i in range(1, len(waypoints)): loc_tm1 = waypoints[i - 1].transform.location loc_tm1 = np.asarray([loc_tm1.x, loc_tm1.y, loc_tm1.z]) loc_t = waypoints[i].transform.location loc_t = np.asarray([loc_t.x, loc_t.y, loc_t.z]) distances.append(np.linalg.norm(loc_tm1 - loc_t)) return waypoints, roadoptions, distances
def main(): global actor_list client = carla.Client(host, port) client.set_timeout(10.0) try: world = client.get_world() except: print("ERROR: Cannot get world !") import sys sys.exit() #set_weather(world) try: blueprint_library = world.get_blueprint_library() # add vehicle vehicle = add_vehicle_component(world, blueprint_library) # put the vehicle to drive around. vehicle.set_autopilot(True) map = world.get_map() dao = GlobalRoutePlannerDAO(map) planner = GlobalRoutePlanner(dao) planner.setup() G = planner._graph plt.subplot(111) nx.draw_networkx_edges(G, pos=nx.spring_layout(G)) plt.savefig("path.pdf") #time.sleep(1.0) finally: print('destroying actors') for actor in actor_list: actor.destroy() actor_list = [] print('done.')
def __init__(self, vehicle, resolution=1.5): from agents.navigation.global_route_planner import GlobalRoutePlanner from agents.navigation.global_route_planner_dao import GlobalRoutePlannerDAO self._dt = 1.0 / 10.0 self._target_speed = 20.0 # Km/h self._sampling_radius = self._target_speed * 1 / 3.6 # 1 seconds horizon self._min_distance = self._sampling_radius * 0.9 self._vehicle = vehicle self._map = vehicle.get_world().get_map() self._grp = GlobalRoutePlanner(GlobalRoutePlannerDAO(self._map, resolution)) self._grp.setup() self._route = None self._waypoints_queue = deque(maxlen=20000) self.target = (None, None) self.checkpoint = (None, None) self.distance_to_goal = float('inf') self.distances = deque(maxlen=20000)
def _set_route(self, hop_resolution=1.0): world = CarlaDataProvider.get_world() dao = GlobalRoutePlannerDAO(world.get_map(), hop_resolution) grp = GlobalRoutePlanner(dao) grp.setup() spawn_points = CarlaDataProvider._spawn_points start = spawn_points[self.start_idx] target = spawn_points[self.target_idx] route = grp.trace_route(start.location, target.location) self.route = [(w.transform, c) for w, c in route] CarlaDataProvider.set_ego_vehicle_route([(w.transform.location, c) for w, c in route]) gps_route = location_route_to_gps(self.route, *_get_latlon_ref(world)) self.agent.set_global_plan(gps_route, self.route) self.timeout = self._estimate_route_timeout()
def _trace_route(self, start_waypoint, end_waypoint): """ This method sets up a global router and returns the optimal route from start_waypoint to end_waypoint. :param start_waypoint: initial position :param end_waypoint: final position """ # Setting up global router if self._grp is None: wld = self.vehicle.get_world() dao = GlobalRoutePlannerDAO( wld.get_map(), sampling_resolution=self._sampling_resolution) grp = GlobalRoutePlanner(dao) grp.setup() self._grp = grp # Obtain route plan route = self._grp.trace_route(start_waypoint.transform.location, end_waypoint.transform.location) return route
def main(): client = carla.Client('localhost', 2000) client.set_timeout(2.0) world = client.get_world() _map = world.get_map() dao = GlobalRoutePlannerDAO(_map) grp = GlobalRoutePlanner(dao) grp.setup() blueprint_library = world.get_blueprint_library() # vehicle blueprint = random.choice( world.get_blueprint_library().filter('vehicle.lin*')) spawn_points = world.get_map().get_spawn_points() spawn_point = random.choice( spawn_points) if spawn_points else carla.Transform() _vehicle = world.try_spawn_actor(blueprint, spawn_point) print(_vehicle) start_waypoint = _map.get_waypoint(_vehicle.get_location()) end_waypoint = random.choice( spawn_points) if spawn_points else carla.Transform() # Obtain route plan x1 = start_waypoint.transform.location.x y1 = start_waypoint.transform.location.y x2 = end_waypoint.location.x y2 = end_waypoint.location.y print(x1, x2, y1, y2) _graph, _id_map = grp.build_graph() print(_graph)
def __init__(self, args): self.render_display = False self.record_display = False self.record_vision = True self.record_dir = None #'/nfs/kun1/users/aviralkumar/carla_data/' self.vision_size = args.vision_size self.vision_fov = args.vision_fov self.changing_weather_speed = float(args.weather) self.frame_skip = args.frame_skip self.max_episode_steps = args.steps self.multiagent = args.multiagent self.start_lane = args.lane self.follow_traffic_lights = args.lights if self.record_display: assert self.render_display self.actor_list = [] if self.render_display: pygame.init() self.render_display = pygame.display.set_mode( (800, 600), pygame.HWSURFACE | pygame.DOUBLEBUF) self.font = get_font() self.clock = pygame.time.Clock() self.client = carla.Client('localhost', 2000) self.client.set_timeout(2.0) self.world = self.client.get_world() self.map = self.world.get_map() ## Define the route planner self.route_planner_dao = GlobalRoutePlannerDAO(self.map, sampling_resolution=0.1) self.route_planner = CustomGlobalRoutePlanner(self.route_planner_dao) # tests specific to map 4: if self.start_lane and self.map.name != "Town04": raise NotImplementedError # remove old vehicles and sensors (in case they survived) self.world.tick() actor_list = self.world.get_actors() for vehicle in actor_list.filter("*vehicle*"): print("Warning: removing old vehicle") vehicle.destroy() for sensor in actor_list.filter("*sensor*"): print("Warning: removing old sensor") sensor.destroy() self.vehicle = None self.vehicles_list = [] # their ids self.reset_vehicle() # creates self.vehicle self.actor_list.append(self.vehicle) blueprint_library = self.world.get_blueprint_library() if self.render_display: self.camera_display = self.world.spawn_actor( blueprint_library.find('sensor.camera.rgb'), carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), attach_to=self.vehicle) self.actor_list.append(self.camera_display) bp = blueprint_library.find('sensor.camera.rgb') bp.set_attribute('image_size_x', str(self.vision_size)) bp.set_attribute('image_size_y', str(self.vision_size)) bp.set_attribute('fov', str(self.vision_fov)) location = carla.Location(x=1.6, z=1.7) self.camera_vision = self.world.spawn_actor( bp, carla.Transform(location, carla.Rotation(yaw=0.0)), attach_to=self.vehicle) self.actor_list.append(self.camera_vision) if self.record_display or self.record_vision: if self.record_dir is None: self.record_dir = "carla-{}-{}x{}-fov{}".format( self.map.name.lower(), self.vision_size, self.vision_size, self.vision_fov) if self.frame_skip > 1: self.record_dir += '-{}'.format(self.frame_skip) if self.changing_weather_speed > 0.0: self.record_dir += '-weather' if self.multiagent: self.record_dir += '-mutiagent' if self.follow_traffic_lights: self.record_dir += '-lights' self.record_dir += '-{}k'.format(self.max_episode_steps // 1000) now = datetime.datetime.now() self.record_dir += now.strftime("-%Y-%m-%d-%H-%M-%S") if not os.path.exists(self.record_dir): os.mkdir(self.record_dir) if self.render_display: self.sync_mode = CarlaSyncMode(self.world, self.camera_display, self.camera_vision, fps=20) else: self.sync_mode = CarlaSyncMode(self.world, self.camera_vision, fps=20) # weather self.weather = Weather(self.world, self.changing_weather_speed) # dummy variables, to match deep mind control's APIs low = -1.0 high = 1.0 self.action_space = DotMap() self.action_space.low.min = lambda: low self.action_space.high.max = lambda: high self.action_space.shape = [2] self.observation_space = DotMap() self.observation_space.shape = (3, self.vision_size, self.vision_size) self.observation_space.dtype = np.dtype(np.uint8) self.reward_range = None self.metadata = None self.action_space.sample = lambda: np.random.uniform( low=low, high=high, size=self.action_space.shape[0]).astype( np.float32) # roaming carla agent self.agent = None self.world.tick() self.reset_init() # creates self.agent ## Initialize the route planner self.route_planner.setup() ## Collision detection self._proximity_threshold = 10.0 self._traffic_light_threshold = 5.0 self.actor_list = self.world.get_actors() for idx in range(len(self.actor_list)): print(idx, self.actor_list[idx]) # import ipdb; ipdb.set_trace() self.vehicle_list = self.actor_list.filter("*vehicle*") self.lights_list = self.actor_list.filter("*traffic_light*") self.object_list = self.actor_list.filter("*traffic.*") ## Initialize the route planner self.route_planner.setup() ## The map is deterministic so for reward relabelling, we can ## instantiate the environment object and then query the distance function ## in the env, which directly uses this map_graph, and we need not save it. self._map_graph = self.route_planner._graph ## This is a dummy for the target location, we can make this an input ## to the env in RL code. self.target_location = carla.Location(x=-13.473097, y=134.311234, z=-0.010433) ## Now reset the env once self.reset()
def main(): global actor_list, flag, frame client = carla.Client(host, port) client.set_timeout(5.0) try: world = client.get_world() except: print("ERROR: Cannot get world !") import sys sys.exit() set_weather(world) #carla.TrafficLight.set_green_time(float(999)) #carla.TrafficLight.set_red_time(0) try: blueprint_library = world.get_blueprint_library() # add vehicle vehicle = add_vehicle_component(world, blueprint_library) # put the vehicle to drive around. #vehicle.set_autopilot(True) map = world.get_map() spawn_points = map.get_spawn_points() destination = spawn_points[random.randint(0, len(spawn_points) - 1)].location agent = BasicAgent(vehicle, target_speed=20) agent.set_destination((destination.x, destination.y, destination.z)) dao = GlobalRoutePlannerDAO(map) planner = GlobalRoutePlanner(dao) planner.setup() vehicle.set_simulate_physics(False) my_location = vehicle.get_location() trace_list = planner.trace_route(my_location, destination) waypoints = [] for (waypoint, road_option) in trace_list: waypoints.append(waypoint) next_point = waypoints[0].transform camera = add_camera_component(world, blueprint_library, vehicle) camera.stop() while True: vehicle.set_transform(next_point) my_location = next_point.location #my_location = vehicle.get_location() me2destination = my_location.distance(destination) if me2destination < 50: destination = spawn_points[random.randint( 0, len(spawn_points) - 1)].location #agent.set_destination((destination.x,destination.y,destination.z)) print("destination change !!!") trace_list = planner.trace_route(my_location, destination) waypoints = [] for (waypoint, road_option) in trace_list: waypoints.append(waypoint) print(len(waypoints)) if len(waypoints) < 5: print('my_location:', my_location.x, my_location.y, my_location.z) print('destination:', destination.x, destination.y, destination.z) print('me2destination:', me2destination) next_point = waypoints[random.randint(0, min(len(waypoints) - 1, 50))].transform camera.listen(lambda image: deal_image(image)) while not flag: sleep(0.01) camera.stop() flag = False get_instruct(waypoints) for waypoint in waypoints[0:min(len(waypoints) - 1, 30)]: box_point = carla.Location(waypoint.transform.location.x, waypoint.transform.location.y, waypoint.transform.location.z - 0.4) box = carla.BoundingBox(box_point, carla.Vector3D(x=2, y=0.1, z=0.5)) rotation = carla.Rotation( pitch=waypoint.transform.rotation.pitch, yaw=waypoint.transform.rotation.yaw, roll=waypoint.transform.rotation.roll) world.debug.draw_box(box=box, rotation=rotation, thickness=1.2, life_time=0) sleep(0.3) camera.listen(lambda image: deal_image(image)) while not flag: sleep(0.01) camera.stop() flag = False sleep(1.0) finally: print('destroying actors') for actor in actor_list: actor.destroy() actor_list = [] print('done.')
def _initialize_global_route_planner(self): dao = GlobalRoutePlannerDAO(self._vehicle.get_world().get_map(), self._hop_resolution) grp = GlobalRoutePlanner(dao) grp.setup() return grp
def set_destination(self, location): start_waypoint = self._map.get_waypoint(self._vehicle.get_location()) end_waypoint = self._map.get_waypoint( carla.Location(location[0], location[1], location[2])) solution = [] # Setting up global router dao = GlobalRoutePlannerDAO(self._vehicle.get_world().get_map()) grp = GlobalRoutePlanner(dao) grp.setup() # Obtain route plan x1 = start_waypoint.transform.location.x y1 = start_waypoint.transform.location.y x2 = end_waypoint.transform.location.x y2 = end_waypoint.transform.location.y route = grp.plan_route((x1, y1), (x2, y2)) current_waypoint = start_waypoint route.append(RoadOption.VOID) for action in route: # Generate waypoints to next junction wp_choice = current_waypoint.next(self._hop_resolution) while len(wp_choice) == 1: current_waypoint = wp_choice[0] solution.append((current_waypoint, RoadOption.LANEFOLLOW)) wp_choice = current_waypoint.next(self._hop_resolution) # Stop at destination if current_waypoint.transform.location.distance( end_waypoint.transform.location ) < self._hop_resolution: break if action == RoadOption.VOID: break # Select appropriate path at the junction if len(wp_choice) > 1: # Current heading vector current_transform = current_waypoint.transform current_location = current_transform.location projected_location = current_location + \ carla.Location( x=math.cos(math.radians(current_transform.rotation.yaw)), y=math.sin(math.radians(current_transform.rotation.yaw))) v_current = vector(current_location, projected_location) direction = 0 if action == RoadOption.LEFT: direction = 1 elif action == RoadOption.RIGHT: direction = -1 elif action == RoadOption.STRAIGHT: direction = 0 select_criteria = float('inf') # Choose correct path for wp_select in wp_choice: v_select = vector(current_location, wp_select.transform.location) cross = float('inf') if direction == 0: cross = abs(np.cross(v_current, v_select)[-1]) else: cross = direction * np.cross(v_current, v_select)[-1] if cross < select_criteria: select_criteria = cross current_waypoint = wp_select # Generate all waypoints within the junction # along selected path solution.append((current_waypoint, action)) current_waypoint = current_waypoint.next( self._hop_resolution)[0] while current_waypoint.is_intersection: solution.append((current_waypoint, action)) current_waypoint = current_waypoint.next( self._hop_resolution)[0] assert solution self._current_plan = solution self._local_planner.set_global_plan(self._current_plan)