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)