def test_linearring_from_invalid(): coords = [(0.0, 0.0), (0.0, 0.0), (0.0, 0.0)] line = LineString(coords) assert not line.is_valid with pytest.raises(TopologicalError): LinearRing(line)
STEERING_FACTOR_WEIGHT = 0.0 STEERING_FACTOR_EASING = 'linear' PROGRESS_FACTOR_WEIGHT = 0.0 PROGRESS_FACTOR_EASING = 'linear' LANE_FACTOR_WEIGHT = 0.0 LANE_FACTOR_EASING = 'quintic' RACE_LINE_FACTOR_WEIGHT = 0.5 RACE_LINE_FACTOR_EASING = 'linear' # Globals g_last_progress_value = 0.0 g_last_progress_time = 0.0 g_last_speed_value = 0.0 g_last_steering_angle = 0.0 # for getting current race line waypoint distances g_race_line_dists = [LineString(RACE_LINE_WAYPOINTS).project(Point(p), normalized=True) for p in LineString(RACE_LINE_WAYPOINTS).coords[:-1]] + [1.0] #=============================================================================== # # REWARD # #=============================================================================== def reward_function(params): """Reward function is: f(s,w,h,t,p) = 1.0 * W(s,Ks) * W(w,Kw) * W(h,Kh) * W(t,Kt) * W(p,Kp) * W(l,Kl) s: speed factor, linear 0..1 for range of speed from 0 to MAX_SPEED w: wheel factor, non-linear 0..1 for wheels being off the track and vehicle in danger of going off the track. We want to use the full
def _build_spline(self): '''Build spline for lane change Returns: tuple: lane change lane, lane point distance, prepared lane change spline. ''' # cetner line center_line = self._track_data.center_line # start lane start_lane_line = self._start_lane.lane["track_line"] start_lane_dists = self._start_lane.lane["dists"] # end lane end_lane_line = self._end_lane.lane["track_line"] end_lane_dists = self._end_lane.lane["dists"] start_lane_point = Point( np.array(self._start_lane.eval_spline( self._lane_change_start_dist))[:, 0]) end_lane_point = Point( np.array(self._end_lane.eval_spline( self._lane_change_end_dist))[:, 0]) end_offset = 0.0 if ( self._lane_change_start_dist < self._lane_change_end_dist ) else center_line.length # Find prev/next points on each lane current_prev_index = bisect.bisect_left(start_lane_dists, self._current_dist) - 1 start_prev_index = bisect.bisect_left(start_lane_dists, self._lane_change_start_dist) - 1 end_next_index = bisect.bisect_right(end_lane_dists, self._lane_change_end_dist) # Define intervals on start/end lanes to build the spline from num_start_coords = len(start_lane_line.coords) num_end_coords = len(end_lane_line.coords) if self._track_data.is_loop: num_start_coords -= 1 num_end_coords -= 1 start_index_0 = (current_prev_index - 3) % num_start_coords start_index_1 = start_prev_index end_index_0 = end_next_index end_index_1 = (end_next_index + 3) % num_end_coords # Grab waypoint indices for these intervals (some corner cases here...) if start_index_0 < start_index_1: start_indices = list(range(start_index_0, start_index_1 + 1)) start_offsets = [0.0] * len(start_indices) else: start_indices_0 = list(range(start_index_0, num_start_coords)) start_indices_1 = list(range(start_index_1 + 1)) start_indices = start_indices_0 + start_indices_1 start_offsets = [-center_line.length] * len(start_indices_0) \ + [0.0] * len(start_indices_1) if end_index_0 < end_index_1: end_indices = list(range(end_index_0, end_index_1 + 1)) end_offsets = [end_offset] * len(end_indices) else: end_indices_0 = list(range(end_index_0, num_end_coords)) end_indices_1 = list(range(end_index_1 + 1)) end_indices = end_indices_0 + end_indices_1 end_offsets = [end_offset] * len(end_indices_0) \ + [end_offset + center_line.length] * len(end_indices_1) # Logic to avoid start and end point are too close to track waypoints before_start_lane_point = Point( np.array(start_lane_line.coords.xy)[:, start_indices[-1]]) after_end_lane_point = Point( np.array(end_lane_line.coords.xy)[:, end_indices[0]]) if before_start_lane_point.distance(start_lane_point) < DIST_THRESHOLD: # pop last index of start_indices start_indices.pop() start_offsets.pop() if after_end_lane_point.distance(end_lane_point) < DIST_THRESHOLD: # pop first index of end_indices end_indices.pop(0) end_offsets.pop(0) # Build the spline u = np.hstack( (np.array(start_lane_dists)[start_indices] + np.array(start_offsets), self._lane_change_start_dist, self._lane_change_end_dist + end_offset, np.array(end_lane_dists)[end_indices] + np.array(end_offsets))) x = np.hstack((np.array(start_lane_line.coords.xy)[:, start_indices], start_lane_point.xy, end_lane_point.xy, np.array(end_lane_line.coords.xy)[:, end_indices])) u, ui = np.unique(u, return_index=True) x = x[:, ui] bot_car_spline, _ = splprep(x, u=u, k=SPLINE_DEGREE, s=0) return TrackLine(LineString(np.array( np.transpose(x)))), u, bot_car_spline
[4.98041611, 0.6783534 ], [5.06645943, 0.67304432]]) # The TARGET_NUMBER_STEPS is a way to weight the progress over other factors. progress really should # be the most important feature, and we don't want race-line hunting to overshadow the progress by # them being orders of magnitude off. 150 steps is a fair pace on the Toronto track, so below that # the heading and race-line proximity are dominant factors. Above 150 progress pace, the car will # get progress as the dominant reward factor. TARGET_NUMBER_STEPS = 150 # LONDON_RACE_LINE, BOWTIE_RACE_LINE, NEW_YORK_RACE_LINE, OVAL_RACE_LINE, CANADA_RACE_LINE RACE_LINE_WAYPOINTS = BOWTIE_RACE_LINE # Globals g_last_progress_value = 0.0 g_start_offset_percent = 0.0 g_race_line_string = LineString(RACE_LINE_WAYPOINTS) # for getting current race line waypoint distances g_race_line_dists = [ LineString(RACE_LINE_WAYPOINTS).project(Point(p), normalized=True) for p in LineString(RACE_LINE_WAYPOINTS).coords[:-1] ] + [1.0] g_highest_speed = 0.0 g_moving_average_speed = 0.0 g_last_position = (0.0, 0.0) #=============================================================================== # # REWARD # #===============================================================================
def __init__(self): # Create the observation space self.observation_space = spaces.Box(low=0, high=255, shape=(TRAINING_IMAGE_SIZE[1], TRAINING_IMAGE_SIZE[0], 3), dtype=np.uint8) # Create the action space self.action_space = spaces.Box(low=np.array([-1, 0]), high=np.array([+1, +1]), dtype=np.float32) if node_type == SIMULATION_WORKER: # ROS initialization rospy.init_node('rl_coach', anonymous=True) # wait for required services rospy.wait_for_service('/deepracer_simulation/get_waypoints') rospy.wait_for_service('/deepracer_simulation/reset_car') rospy.wait_for_service('/gazebo/get_model_state') rospy.wait_for_service('/gazebo/get_link_state') rospy.wait_for_service('/gazebo/clear_joint_forces') self.get_model_state = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState) self.get_link_state = rospy.ServiceProxy('/gazebo/get_link_state', GetLinkState) self.clear_forces_client = rospy.ServiceProxy('/gazebo/clear_joint_forces', JointRequest) self.reset_car_client = rospy.ServiceProxy('/deepracer_simulation/reset_car', ResetCarSrv) get_waypoints_client = rospy.ServiceProxy('/deepracer_simulation/get_waypoints', GetWaypointSrv) # Create the publishers for sending speed and steering info to the car self.velocity_pub_dict = OrderedDict() self.steering_pub_dict = OrderedDict() for topic in VELOCITY_TOPICS: self.velocity_pub_dict[topic] = rospy.Publisher(topic, Float64, queue_size=1) for topic in STEERING_TOPICS: self.steering_pub_dict[topic] = rospy.Publisher(topic, Float64, queue_size=1) # send model outputs and reward to topic self.last_reward = rospy.Publisher("/deepracer/last_reward", Float64, queue_size=1) self.last_action= rospy.Publisher("/deepracer/last_action", Float64, queue_size=1) self.pub_current_progress = rospy.Publisher("/deepracer/current_progress", Float64, queue_size=1) # Read in parameters self.world_name = rospy.get_param('WORLD_NAME') self.job_type = rospy.get_param('JOB_TYPE') self.aws_region = rospy.get_param('AWS_REGION') self.metrics_s3_bucket = rospy.get_param('METRICS_S3_BUCKET') self.metrics_s3_object_key = rospy.get_param('METRICS_S3_OBJECT_KEY') self.metrics = [] self.simulation_job_arn = 'arn:aws:robomaker:' + self.aws_region + ':' + \ rospy.get_param('ROBOMAKER_SIMULATION_JOB_ACCOUNT_ID') + \ ':simulation-job/' + rospy.get_param('AWS_ROBOMAKER_SIMULATION_JOB_ID') if self.job_type == TRAINING_JOB: from custom_files.customer_reward_function import reward_function self.reward_function = reward_function self.metric_name = rospy.get_param('METRIC_NAME') self.metric_namespace = rospy.get_param('METRIC_NAMESPACE') self.training_job_arn = rospy.get_param('TRAINING_JOB_ARN') self.target_number_of_episodes = rospy.get_param('NUMBER_OF_EPISODES') self.target_reward_score = rospy.get_param('TARGET_REWARD_SCORE') else: from markov.defaults import reward_function self.reward_function = reward_function self.number_of_trials = 0 self.target_number_of_trials = rospy.get_param('NUMBER_OF_TRIALS') # Request the waypoints waypoints = None try: resp = get_waypoints_client() waypoints = np.array(resp.waypoints).reshape(resp.row, resp.col) except Exception as ex: utils.json_format_logger("Unable to retrieve waypoints: {}".format(ex), **utils.build_system_error_dict(utils.SIMAPP_ENVIRONMENT_EXCEPTION, utils.SIMAPP_EVENT_ERROR_CODE_500)) is_loop = np.all(waypoints[0,:] == waypoints[-1,:]) if is_loop: self.center_line = LinearRing(waypoints[:,0:2]) self.inner_border = LinearRing(waypoints[:,2:4]) self.outer_border = LinearRing(waypoints[:,4:6]) self.road_poly = Polygon(self.outer_border, [self.inner_border]) else: self.center_line = LineString(waypoints[:,0:2]) self.inner_border = LineString(waypoints[:,2:4]) self.outer_border = LineString(waypoints[:,4:6]) self.road_poly = Polygon(np.vstack((self.outer_border, np.flipud(self.inner_border)))) self.center_dists = [self.center_line.project(Point(p), normalized=True) for p in self.center_line.coords[:-1]] + [1.0] self.track_length = self.center_line.length # Queue used to maintain image consumption synchronicity self.image_queue = queue.Queue(IMG_QUEUE_BUF_SIZE) rospy.Subscriber('/camera/zed/rgb/image_rect_color', sensor_image, self.callback_image) # Initialize state data self.episodes = 0 self.start_ndist = 0.0 self.reverse_dir = False self.change_start = rospy.get_param('CHANGE_START_POSITION', (self.job_type == TRAINING_JOB)) self.alternate_dir = rospy.get_param('ALTERNATE_DRIVING_DIRECTION', False) self.is_simulation_done = False self.steering_angle = 0 self.speed = 0 self.action_taken = 0 self.prev_progress = 0 self.prev_point = Point(0, 0) self.prev_point_2 = Point(0, 0) self.next_state = None self.reward = None self.reward_in_episode = 0 self.done = False self.steps = 0 self.simulation_start_time = 0 self.allow_servo_step_signals = False
def test_race_line_heading(self): reward.RACE_LINE_WAYPOINTS = reward.OVAL_TRACK_RACE_LINE reward.g_race_line_dists = [ LineString(reward.RACE_LINE_WAYPOINTS).project(Point(p), normalized=True) for p in LineString(reward.RACE_LINE_WAYPOINTS).coords[:-1] ] + [1.0] params = self.default_params() # racing line point 10 is close to track centerline on the bottom params['x'] = reward.OVAL_TRACK_RACE_LINE[9][0] params['y'] = reward.OVAL_TRACK_RACE_LINE[9][1] params['heading'] = 0 self.assertTrue( math.isclose(reward.race_line_heading(params), 1.0, abs_tol=1e-2)) params['heading'] = 45 self.assertTrue( math.isclose(reward.race_line_heading(params), 0.5, abs_tol=1e-2)) params['heading'] = -45 self.assertTrue( math.isclose(reward.race_line_heading(params), 0.5, abs_tol=1e-2)) params['heading'] = 90 self.assertTrue( math.isclose(reward.race_line_heading(params), 0.0, abs_tol=1e-2)) params['heading'] = -90 self.assertTrue( math.isclose(reward.race_line_heading(params), 0.0, abs_tol=1e-2)) params['heading'] = 30 self.assertTrue( math.isclose(reward.race_line_heading(params), 0.66, abs_tol=1e-2)) # close to -170 params['x'] = reward.OVAL_TRACK_RACE_LINE[65][0] params['y'] = reward.OVAL_TRACK_RACE_LINE[65][1] params['heading'] = 170 self.assertTrue( math.isclose(reward.race_line_heading(params), 0.81, abs_tol=1e-2)) # close to 170 params['x'] = reward.OVAL_TRACK_RACE_LINE[55][0] params['y'] = reward.OVAL_TRACK_RACE_LINE[55][1] params['heading'] = -170 self.assertTrue( math.isclose(reward.race_line_heading(params), 0.84, abs_tol=1e-2)) # add steering angle params['x'] = reward.OVAL_TRACK_RACE_LINE[9][0] params['y'] = reward.OVAL_TRACK_RACE_LINE[9][1] params['heading'] = 0 params['steering_angle'] = 30 self.assertTrue( math.isclose(reward.race_line_heading(params), 0.66, abs_tol=1e-2)) # close to -170 params['x'] = reward.OVAL_TRACK_RACE_LINE[65][0] params['y'] = reward.OVAL_TRACK_RACE_LINE[65][1] params['heading'] = 170 params['steering_angle'] = 17 self.assertTrue( math.isclose(reward.race_line_heading(params), 1.0, abs_tol=1e-2)) # close to 170 params['x'] = reward.OVAL_TRACK_RACE_LINE[55][0] params['y'] = reward.OVAL_TRACK_RACE_LINE[55][1] params['heading'] = -170 params['steering_angle'] = -20 self.assertTrue( math.isclose(reward.race_line_heading(params), 0.94, abs_tol=1e-2))
def __init__(self): # Create the observation space img_width = TRAINING_IMAGE_SIZE[0] img_height = TRAINING_IMAGE_SIZE[1] self.observation_space = spaces.Box(low=0, high=255, shape=(img_height, img_width, 3), dtype=np.uint8) # Create the action space self.action_space = spaces.Box(low=np.array([-1, 0]), high=np.array([+1, +1]), dtype=np.float32) if node_type == SIMULATION_WORKER: # ROS initialization rospy.init_node('rl_coach', anonymous=True) rospy.Subscriber('/camera/zed/rgb/image_rect_color', sensor_image, self.callback_image) self.ack_publisher = rospy.Publisher('/vesc/low_level/ackermann_cmd_mux/output', AckermannDriveStamped, queue_size=100) self.set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) self.get_model_state = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState) self.get_link_state = rospy.ServiceProxy('/gazebo/get_link_state', GetLinkState) # Read in parameters self.world_name = rospy.get_param('WORLD_NAME') self.job_type = rospy.get_param('JOB_TYPE') self.aws_region = rospy.get_param('AWS_REGION') self.metrics_s3_bucket = rospy.get_param('METRICS_S3_BUCKET') self.metrics_s3_object_key = rospy.get_param('METRICS_S3_OBJECT_KEY') self.metrics = [] self.simulation_job_arn = 'arn:aws:robomaker:' + self.aws_region + ':' + \ rospy.get_param('ROBOMAKER_SIMULATION_JOB_ACCOUNT_ID') + \ ':simulation-job/' + rospy.get_param('AWS_ROBOMAKER_SIMULATION_JOB_ID') if self.job_type == TRAINING_JOB: from custom_files.customer_reward_function import reward_function self.reward_function = reward_function self.metric_name = rospy.get_param('METRIC_NAME') self.metric_namespace = rospy.get_param('METRIC_NAMESPACE') self.training_job_arn = rospy.get_param('TRAINING_JOB_ARN') self.target_number_of_episodes = rospy.get_param('NUMBER_OF_EPISODES') self.target_reward_score = rospy.get_param('TARGET_REWARD_SCORE') else: from markov.defaults import reward_function self.reward_function = reward_function self.number_of_trials = 0 self.target_number_of_trials = rospy.get_param('NUMBER_OF_TRIALS') # Read in the waypoints BUNDLE_CURRENT_PREFIX = os.environ.get("BUNDLE_CURRENT_PREFIX", None) if not BUNDLE_CURRENT_PREFIX: raise ValueError("Cannot get BUNDLE_CURRENT_PREFIX") route_file_name = os.path.join(BUNDLE_CURRENT_PREFIX, 'opt', 'install', 'deepracer_simulation', 'share', 'deepracer_simulation', 'routes', '{}.npy'.format(self.world_name)) waypoints = np.load(route_file_name) self.is_loop = np.all(waypoints[0,:] == waypoints[-1,:]) if self.is_loop: self.center_line = LinearRing(waypoints[:,0:2]) self.inner_border = LinearRing(waypoints[:,2:4]) self.outer_border = LinearRing(waypoints[:,4:6]) self.road_poly = Polygon(self.outer_border, [self.inner_border]) else: self.center_line = LineString(waypoints[:,0:2]) self.inner_border = LineString(waypoints[:,2:4]) self.outer_border = LineString(waypoints[:,4:6]) self.road_poly = Polygon(np.vstack((self.outer_border, np.flipud(self.inner_border)))) self.center_dists = [self.center_line.project(Point(p), normalized=True) for p in self.center_line.coords[:-1]] + [1.0] self.track_length = self.center_line.length # Initialize state data self.episodes = 0 self.start_dist = 0.0 self.round_robin = (self.job_type == TRAINING_JOB) self.is_simulation_done = False self.image = None self.steering_angle = 0 self.speed = 0 self.action_taken = 0 self.prev_progress = 0 self.prev_point = Point(0, 0) self.prev_point_2 = Point(0, 0) self.next_state = None self.reward = None self.reward_in_episode = 0 self.done = False self.steps = 0 self.simulation_start_time = 0 self.reverse_dir = False
def test_linearring_from_invalid(self): coords = [(0.0, 0.0), (0.0, 0.0), (0.0, 0.0)] line = LineString(coords) self.assertFalse(line.is_valid) with self.assertRaises(TopologicalError): ring = LinearRing(line)
for d in data: idx.append(idx[-1] + np.size(d,0)) # # Interface node positions along mesh edges # ifp = [] # interface node positions i_nodes = [] # interior node indices for which the distance is < 0 for i in range(0, np.size(idx)-1): print("* Shape %d/%d" %(i+1, np.size(idx)-1)) d = dscaled[idx[i]:idx[i+1]] if_nodes = [(d[j][0], d[j][1]) for j in range(0, len(d))] # Add the first node to the end to create a closed loop for intersecting: if_loop = if_nodes if_loop.append(if_nodes[0]) interface = LineString(if_loop) # Intersect edges_close with the interface; construct interface nodes. for j in range(0, len(edges_close)): n1 = edges[edges_close[j], 0] n2 = edges[edges_close[j], 1] o = LineString( [p[n1,:], p[n2,:]] ) x = interface.intersection(o) if (np.size(x) == 2): ifp.append( [x.x, x.y] ) if (np.size(x) > 2): # print("\nWarning: mesh edge crosses multiple interface edges! " # "Using the centroid for interface node position.") ifp.append( [x[:].centroid.x, x[:].centroid.y] ) if ((j+1) % 1000 == 0 or (j+1) == len(edges_close)): print("\r- Processing edge %d / %d" %(j+1,len(edges_close)), end='')
if d["geometry"]["type"] == "Point": mean = d["geometry"]["coordinates"] elif d["geometry"]["type"] == "LineString": mean = get_mean(d["geometry"]["coordinates"]) elif d["geometry"]["type"] == "Polygon": mean = get_mean(d["geometry"]["coordinates"][0]) d.pop("geometry") d["GeoCoordinate"] = {"longitude" : mean[0], "latitude" : mean[1]} listOfStations.append(d) for d_i, d in enumerate(data): if "railway" in d and not "station" in d["railway"]: if "geometry" in d: poly = np.array(d["geometry"]["coordinates"]) poly = LineString(poly if len(poly.shape) == 2 else poly[0]) geom = ops.transform(tr.transform, poly) for r in listOfStations: circle = Point([r["GeoCoordinate"]["longitude"],r["GeoCoordinate"]["latitude"]]) circle = ops.transform(tr.transform, circle) circle = Point(circle).buffer(100).boundary i = circle.intersection(geom) if not i.is_empty: if "stations" in d: d["stations"].append(r["@id"]) else: d["stations"] = [r["@id"]] if "stations" in d: print(len(d["stations"]))
def link(self): for osm_id,road in self.segment_map.items(): road_id = "r{0}".format(osm_id) segment_link_map = {} for segment in road: segment_links = self.link_segment(segment,osm_id) segment_link_map[segment] = segment_links for (segment1,segment2) in zip(road[:-1],road[1:]): assert(segment1[1] == segment2[0]) buildings1 = segment_link_map[segment1] buildings2 = segment_link_map[segment2] if segment1[1] in self.int_map: i_id = "i{0}".format(self.int_map[segment1[1]]) self.add_edge(i_id,road_id, reverse=False) minDistBuilding1 = self.findMinDistanceBuilding(self.int_map[segment1[1]],segment1[1][0],segment1[1][1],buildings1[0]) b1_id = "b{0}".format(minDistBuilding1) minDistBuilding1R = self.findMinDistanceBuilding(self.int_map[segment1[1]],segment1[1][0],segment1[1][1],buildings1[1]) b1r_id = "b{0}".format(minDistBuilding1R) minDistBuilding2 = self.findMinDistanceBuilding(self.int_map[segment1[1]],segment1[1][0],segment1[1][1],buildings2[0]) b2_id = "b{0}".format(minDistBuilding2) minDistBuilding2R = self.findMinDistanceBuilding(self.int_map[segment1[1]],segment1[1][0],segment1[1][1],buildings2[1]) b2r_id = "b{0}".format(minDistBuilding2R) #self.add_edge(b1_id,i_id) #self.add_edge(b2_id,i_id) #self.add_edge(b1r_id,i_id) #self.add_edge(b2r_id,i_id) self.add_edge(b1_id,b2_id, weight=0.5) self.add_edge(b1r_id,b2r_id, weight=0.5) nb = self.nodes[minDistBuilding1] cp = (nb['x'],nb['y']) #cpLatlon = to_latlon(cp) nbR = self.nodes[minDistBuilding1R] cpR = (nbR['x'],nbR['y']) #cpRLatlon = to_latlon(cpR) nb2 = self.nodes[minDistBuilding2] cp2 = (nb2['x'],nb2['y']) #cp2Latlon = to_latlon(cp2) nb2R = self.nodes[minDistBuilding2R] cp2R = (nb2R['x'],nb2R['y']) #cp2RLatlon = to_latlon(cp2R) edgeLine = LineString([cp, cp2]) edgeLine2 = LineString([cpR, cp2R]) roads = self.segment_lookup([cp[0],cp[1]],5) for rd in roads: rdLine = LineString([rd[0],rd[1]]) if edgeLine.intersects(rdLine): self.add_edge(b1_id,i_id) self.add_edge(b2_id,i_id) if edgeLine2.intersects(rdLine): self.add_edge(b1r_id,i_id) self.add_edge(b2r_id,i_id) else: minDist = 99999999.9 bldPair = (buildings1[0][0],buildings2[0][-1]) for building in buildings1[0]: bld = self.nodes[building] minDistBuilding = self.findMinDistanceBuilding(building,bld['x'],bld['y'],buildings2[0]) minBld = self.nodes[minDistBuilding] dist = self.getDistance(bld['x'],bld['y'],minBld['x'],minBld['y']) if dist < minDist: minDist = dist bldPair = (building,minDistBuilding) b1_id = "b{0}".format(bldPair[0]) b2_id = "b{0}".format(bldPair[1]) minDist = 99999999.9 bldPair = (buildings1[1][0],buildings2[1][-1]) for building in buildings1[1]: bld = self.nodes[building] minDistBuilding = self.findMinDistanceBuilding(building,bld['x'],bld['y'],buildings2[1]) minBld = self.nodes[minDistBuilding] dist = self.getDistance(bld['x'],bld['y'],minBld['x'],minBld['y']) if dist < minDist: minDist = dist bldPair = (building,minDistBuilding) b1r_id = "b{0}".format(bldPair[0]) b2r_id = "b{0}".format(bldPair[1]) self.add_edge(b1_id,b2_id) self.add_edge(b1r_id,b2r_id) start = road[0] end = road[-1] buildings1 = segment_link_map[start] buildings2 = segment_link_map[end] if start[0] in self.int_map: i_id = "i{0}".format(self.int_map[start[0]]) minDistBuilding = self.findMinDistanceBuilding(self.int_map[start[0]],start[0][0],start[0][1],buildings1[0]) b1_id = "b{0}".format(minDistBuilding) minDistBuildingR = self.findMinDistanceBuilding(self.int_map[start[0]],start[0][0],start[0][1],buildings1[1]) b1r_id = "b{0}".format(minDistBuildingR) self.add_edge(b1_id,i_id) self.add_edge(b1r_id,i_id) self.add_edge(b1_id,b1r_id, weight=0.5) if end[1] in self.int_map: i_id = "i{0}".format(self.int_map[end[1]]) minDistBuilding = self.findMinDistanceBuilding(self.int_map[end[1]],end[1][0],end[1][1],buildings2[0]) b2_id = "b{0}".format(minDistBuilding) minDistBuildingR = self.findMinDistanceBuilding(self.int_map[end[1]],end[1][0],end[1][1],buildings2[1]) b2r_id = "b{0}".format(minDistBuildingR) self.add_edge(b2_id,i_id) self.add_edge(b2r_id,i_id) self.add_edge(b2_id,b2r_id, weight=0.5)
def categorizeBuildings(self): def isLeft(a,b,c): return ((b[0] - a[0])*(c[1] - a[1]) - (b[1] - a[1])*(c[0] - a[0])) > 0; def getIntersectionArea(bldPoly,line,perpLine,bldID): if not line.intersects(bldPoly): return 0.0 pt1 = list(line.coords)[0] pt2 = list(line.coords)[1] perppt1 = list(perpLine.coords)[0] perppt2 = list(perpLine.coords)[1] dx = perppt2[0] - perppt1[0] dy = perppt2[1] - perppt1[1] pt3 = (pt1[0]-dx,pt1[1]-dy) pt4 = (pt2[0]-dx,pt2[1]-dy) linePoly = Polygon([pt1,pt3,pt4,pt2]) try: intersection_area = linePoly.intersection(bldPoly).area return intersection_area/bldPoly.area except: return -1 def overlapsNeighbor(bldPoly,neighborPoly): try: intersection_area = bldPoly.intersection(neighborPoly).area return intersection_area/bldPoly.area > 0.05 except: return False Rd2BldLeft = {} Rd2BldRight = {} for bld in self.buildingCenters.keys(): bldID = self.buildingCenters[bld][0] #if bldID < 3700 or bldID > 3800: #if bldID != 3751: #continue roads = self.segment_lookup([bld[0],bld[1]],10) p1 = (bld[0], bld[1]) p1LatLon = to_latlon(p1) res = self.buildingKDTree.query([bld[0],bld[1]], 20) neighbors = [] for item in res[1][1:]: #start from index 1 because index 0 is always the original building bld buildingID = self.buildingCenters[self.buildingCenters.keys()[item]][0] neighbor = self.buildings[buildingID] #this part removes a bug that some neighbor shapes have extra information, which will result in Polygon error later on. start = neighbor[0] for i in range(1,len(neighbor)): if neighbor[i] == start and i > 2: break neighbor = neighbor[:i+1] neighbors.append(neighbor) roadDict = {} bldVertices = self.buildingCenters[bld][1] bldPolygon = Polygon(bldVertices) for rd in roads: rdLine = LineString([rd[0],rd[1]]) intersectsSomething = False p3Intersect = self.perpIntersectPoint(p1,rd[0],rd[1]) if not p3Intersect: continue bldVertices.append(p1) for vertex in bldVertices: if intersectsSomething: break p3 = self.perpIntersectPoint(vertex,rd[0],rd[1]) vertexLatLon = to_latlon(vertex) if p3: p3LatLon = to_latlon(p3) perpLine = LineString([p1,p3]) for neighbor in neighbors: neighborPoly = Polygon(neighbor) if overlapsNeighbor(bldPolygon,neighborPoly): continue if perpLine.intersects(neighborPoly): intersectRd = False intersectionRdArea = 0 if neighborPoly.intersects(rdLine): intersectRd = True intersectionRdArea = getIntersectionArea(neighborPoly,rdLine,perpLine,bldID) if intersectionRdArea > 0.4 or not intersectRd: #if bldID == 4287 or bldID == 4288: #print intersectionRdArea #road_lines.record(0) #road_lines.poly(shapeType=shapefile.POLYLINE, parts=[[vertexLatLon,p3LatLon]]) intersectsSomething = True break for rd2 in roads: if rd2 == rd: continue roadLine2 = LineString([rd2[0],rd2[1]]) if perpLine.intersects(roadLine2): intersectsSomething = True break if not intersectsSomething: perpLine = LineString([p1,p3Intersect]) if perpLine.length > (3*bldPolygon.length)/4: continue #if rdLine.length < (bldPolygon.length/3): # continue p3IntersectLatLon = to_latlon(p3Intersect) road_lines.record(0) road_lines.poly(shapeType=shapefile.POLYLINE, parts=[[p3IntersectLatLon,p1LatLon]]) if isLeft(rd[0],rd[1],p1): if rd not in Rd2BldLeft: Rd2BldLeft[rd] = [bldID] else: Rd2BldLeft[rd].append(bldID) else: if rd not in Rd2BldRight: Rd2BldRight[rd] = [bldID] else: Rd2BldRight[rd].append(bldID) return (Rd2BldLeft, Rd2BldRight)