def edge_updater(self):
# publishes to edge_state
rate = rospy.Rate(10) # Hz
while not rospy.is_shutdown():
# check that costmap subscriber has started receiving messages
if self.costmap == None or self.robot_pose == None:
continue
rospy.loginfo("-----")
rospy.loginfo("Calculating visibility polygon...")
self.set_visibility_polygon()
rospy.loginfo("Checking edges...")
for (u, v) in self.base_graph.edges():
dist_to_u = util.euclidean_distance(self.base_graph.pos(u),
self.robot_pose)
dist_to_v = util.euclidean_distance(self.base_graph.pos(v),
self.robot_pose)
edge_in_range = ((dist_to_u <= self.robot_range)
or (dist_to_v <= self.robot_range))
edge_in_submap = (
self.curr_submap == self.base_graph.get_polygon(u, v))
if edge_in_range or edge_in_submap:
edge_state, edge_weight = self.check_edge(u, v)
# prepare message
msg = EdgeUpdate(str(u), str(v), edge_state, edge_weight)
self.edge_state_pub.publish(msg)
rate.sleep(
) # not sure if this is the right place to put this
def feedback_cb(self, feedback):
# print current pose at each feedback
# feedback is MoveBaseFeedback type msg
pose = feedback.base_position.pose
rospy.logdebug("Feedback for goal " + str(self.goal_cnt) + ":\n" +
self.print_pose_in_euler(pose))
rospy.loginfo("Feedback for goal {}: vprev = {}, vnext = {}".format(
self.goal_cnt, self.vprev, self.vnext))
self.v_publisher.publish(str(self.vprev), str(self.vnext))
# check if robot is close enough to send next goal
position = feedback.base_position.pose.position
(x, y) = (position.x, position.y)
# update distance travelled and current position
self.update_travel_dist(util.euclidean_distance((x, y), self.pos))
self.pos = (x, y)
curr_goal = self.pose_seq[self.goal_cnt].position
(gx, gy) = (curr_goal.x, curr_goal.y)
dist_to_curr_goal = util.euclidean_distance((x, y), (gx, gy))
if dist_to_curr_goal < TOL:
if self.vprev != self.vnext and self.mode != "naive":
self.curr_graph.update_edge(self.vprev, self.vnext,
self.base_map.G.UNBLOCKED)
if self.goal_cnt < len(self.pose_seq) - 1:
rospy.loginfo("Goal pose " + str(self.goal_cnt) + " reached!")
self.goal_cnt += 1
self.set_and_send_next_goal()
elif (self.goal_cnt == len(self.pose_seq) - 1):
rospy.loginfo("Reached end of path")
if self.completed_observation():
# if node under observation is no longer unknown, move to next node
# selecting next node in tree and setting path
if self.mode == "policy":
rospy.loginfo("SUSPENDING CALLBACKS...")
self.suspend = True
(u, v) = self.observation.E
state = self.curr_graph.edge_state(u, v)
# move to next node
self.node = self.node.next_node(state)
self.observation = self.node.opair
self.set_new_path(self.node.path) # update pose seq
self.set_and_send_next_goal()
def dist(self, cell1, cell2):
# heuristic for A*, cell1 and cell2 don't have to be neighbours
if not (self.passable(cell1) and self.passable(cell2)):
return float('inf')
return util.euclidean_distance(cell1, cell2)
def plan_callback(self, data):
# constantly checking returned rospath. If it exits the reg mark path as blocked
# extracting needed data
vnext = self.vnext
vcurr = self.vprev
if len(data.poses) > 0 and not self.suspend:
plan_dest = (data.poses[-1].pose.position.x,
data.poses[-1].pose.position.y)
if not (vnext == vcurr
or self.path_blocked or util.euclidean_distance(
plan_dest, self.curr_graph.pos(vnext)) > 0.25):
# Conditions explained:
# 1. if vnext == vcurr, robot is going back to where it came from, and I assume
# it can take the way it came to get back
# 2. if path_blocked = true, this means it's in the middle of replanning so
# vnext is being changed
# 3. if the final destination of given plan is not vnext, don't check
# 4. if path is empty don't check
rospath = to_2d_path(data)
rospy.loginfo("Checking edge ({},{})".format(vcurr, vnext))
rospy.logdebug("Path: {}".format(
util.print_coord_list(rospath)))
curr_edge_state = self.curr_graph.check_edge_state(
vcurr, vnext, rospath, PADDING, False)
rospy.loginfo(
" result of check_edge_state: {}".format(curr_edge_state))
if self.belief != []:
result = self.update_belief(vcurr, vnext, curr_edge_state)
if result == True:
self.client.cancel_goals_at_and_before_time(
rospy.get_rostime())
self.replan()
return
if (self.mode == "policy" or self.entered_openloop == True):
if (curr_edge_state == self.base_map.G.BLOCKED
and not self.edge_under_observation(vnext, vcurr)
and not self.suspend):
# recalculate path on curr_graph
self.path_blocked = True
if self.path_blocked:
rospy.loginfo("plan_cb: Path is blocked!")
self.client.cancel_goals_at_and_before_time(
rospy.get_rostime())
self.replan()
return
self.posearray_publisher.publish(self.conv_to_PoseArray(self.pose_seq))
def costfn3(known_cost_to_v, v, u, goal, outcomes, supermaps, p_Xy,
robot_range):
'''
Takes into account that submaps are convex, uses that to give better cost estimate.
Supermaps[i].G must be TGraph class object!
'''
tg = supermaps[
0].G # use this to get polygon, all of the supermaps should be identical
edge_est = tg.min_mid_dist(v, u)
unblocked_est = max(0, edge_est -
(robot_range / 2.0)) # Exp travel along edge before
# obsving edge is unblocked
line = LineString([tg.pos(v), tg.pos(u)])
midpt = line.centroid
o_pt = line.interpolate(unblocked_est)
vlist = tg.get_vertices_in_polygon(tg.get_polygon(v, u))
# Currently it's only possible to have 2 outcomes
# All the variables should return something, because if all other vertices are
# blocked, it should return v as ub and uu
for outcome in outcomes:
if outcome.state == tg.BLOCKED:
blocked_p = outcome.p
blocked_exp_cost, ub = min_exp_cost(midpt, v, goal, vlist, outcome,
supermaps, p_Xy)
blocked_exp_cost += util.euclidean_distance((midpt.x, midpt.y),
tg.pos(v))
elif outcome.state == tg.UNBLOCKED:
unblocked_p = outcome.p
# calc expected cost from end_u
unblocked_exp_cost, uu = min_exp_cost(o_pt, v, goal, vlist,
outcome, supermaps, p_Xy)
unblocked_exp_cost += util.euclidean_distance((o_pt.x, o_pt.y),
tg.pos(v))
exp_cost = (known_cost_to_v + blocked_p * blocked_exp_cost +
unblocked_p * unblocked_exp_cost)
return (uu, ub, exp_cost)
def k_means(data, k):
print("K-means clustering...")
print("Data shape for K-means:", data.shape)
if data.ndim == 1:
raise Exception(
"Reshape your data either using array.reshape(-1, 1) if your data has a single feature "
"or array.reshape(1, -1) if it contains a single sample.")
num_samples = data.shape[0]
# First column stores which cluster this sample belongs to,
# Second column stores the error between this sample and its centroid
cluster_assignment = np.zeros((num_samples, 2))
cluster_changed = True
# Step 1: init centroids
centroids = init_centroids(data, k)
# show_cluster(data, k, cluster_assignment[:, 0], centroids, title="K-means, initial centroids")
num_iterations = 0
while cluster_changed:
cluster_changed = False
# for each sample
for j in range(num_samples):
min_distance = 100000.0
min_index = 0
# for each centroid
# Step 2: find the centroid who is closest
for i in range(k):
distance = euclidean_distance(data[j], centroids[i])
if distance < min_distance:
min_distance = distance
min_index = i
# Step 3: update its cluster
if cluster_assignment[j, 0] != min_index:
cluster_changed = True
cluster_assignment[j] = min_index, np.power(min_distance, 2)
# Step 4: update centroids
for i in range(k):
points_in_cluster = data[np.nonzero(cluster_assignment[:,
0] == i)[0]]
centroids[i] = np.mean(points_in_cluster, axis=0)
# title = "K-means, #iter:" + num_iterations.__str__()
# show_cluster(data, k, cluster_assignment[:, 0], centroids, title=title)
num_iterations += 1
# show_cluster(data, k, cluster_assignment[:, 0], title="eigen_space")
return centroids, cluster_assignment[:, 0]
def calculate_cost(self, X, clusters, medoids):
"""
Total distance between samples and their medoid
:param clusters: Three medoids with samples assigned to each of them
:return: Total distance as mentioned above
"""
cost = 0
for i, cluster in enumerate(clusters):
medoid = medoids[i]
for sample_i in cluster:
cost += euclidean_distance(X[sample_i], medoid)
return cost
def add_connected_vertex(self,
label,
location,
last_vertex,
add_blocked=False):
# automatically adds edges to portals in the same polygon
# label - string
# location - (x,y), in reference to map
# last_vertex - last vertex that robot passed
# add vertex
self.graph.add_node(label, defn=Point(location))
# find polygon (search all edges that are adjacent to originating portal)
curr_poly = None
searched_polygons = [] # there's only two associated with any portal
for neighbor in self.graph.neighbors_iter(last_vertex):
logger.info("Checking polygon of ({},{})".format(
last_vertex, neighbor))
poly = self.get_polygon(last_vertex, neighbor)
if poly not in searched_polygons:
searched_polygons.append(poly)
if self.in_polygon(location, poly):
curr_poly = poly
break
# add edges to all portals that are unblocked/unknown to originating portal
for portal in self.get_vertices_in_polygon(poly):
if last_vertex != portal:
state = self.edge_state(last_vertex, portal)
else:
state = self.UNBLOCKED
if state != self.BLOCKED:
weight = util.euclidean_distance(self.pos(label),
self.pos(portal))
self.graph.add_edge(label,
portal,
weight=weight,
state=state,
polygon=curr_poly)
elif add_blocked == True and state == self.BLOCKED:
self.graph.add_edge(label,
portal,
weight=float('inf'),
state=self.BLOCKED,
polygon=curr_poly)
logger.info("Added new vertex {}".format(label))
logger.info(self)
def closest_medoid(self, sample, medoids):
"""
Calculate distance between each sample and every medoids
:param sample: Data point
:param medoids: Similar to centroid in KMeans.
:return: Assigining medoid to each sample
"""
closest_i = None
closest_distance = float('inf')
for i, medoid in enumerate(medoids):
distance = euclidean_distance(sample, medoid)
if distance < closest_distance:
closest_i = i
closest_distance = distance
return closest_i
def get_neighbors(self, test_row):
distances = list()
zippedTrainingData = zip(self.trainX, self.trainY)
for train_row in zippedTrainingData:
features = train_row[0]
label = train_row[1]
dist = euclidean_distance(test_row, features)
distances.append((features, label, dist))
distances.sort(key=lambda tup: tup[2]) # Sort according to the dist
neighbors = list()
for i in range(self.num_neighbors):
neighbors.append(distances[i][1])
return neighbors
def min_exp_cost(est_loc, vs, goal, ulist, outcome, supermaps, p_Xy):
'''
est_loc - shapely Point class object
vs - vertex that robot started observation from
goal - vertex
ulist - list of vertices
outcome - Outcome class object
supermaps - list of Map class objects
p_Xy - p(X = i|Y), probability of being in map i given the belief Y
Return min_exp_cost, min_u
'''
tg = supermaps[0].G
min_u = None
min_expcost = float('inf')
logger.info("Calculating min exp cost for {}".format(list(est_loc.coords)))
for u in ulist:
dist = util.euclidean_distance((est_loc.x, est_loc.y), tg.pos(u))
expcost = dist + expected_cost_outcome(u, goal, outcome, supermaps,
p_Xy)
logger.info(" ({},{}): dist={:.2f}, expcost={:.2f}".format(
vs, u, dist, expcost))
# check that edge is not blocked in any i of the outcome's belief
if vs == u:
if min_u == None or expcost < min_expcost:
logger.info(" Set as min")
min_u = u
min_expcost = expcost
else:
for i in outcome.Yo:
if supermaps[i].G.edge_state(vs, u) == tg.BLOCKED:
break
else:
if min_u == None or expcost < min_expcost:
logger.info(" Set as min")
min_u = u
min_expcost = expcost
assert (min_u != None), ("min_u = None! This is not right! Check logs.")
return min_expcost, min_u
def min_mid_dist(self, u, v):
a = self.pos(u)
b = self.pos(v)
return util.euclidean_distance(a, b)
def spawn_obstacles():
# obstacle probabilities
bgi_prob = 0.5
a_prob = 0.1
ce_prob = 0.5
dfh_prob = 0.4
i_prob = 0
ii_prob = 0
iii_prob = 0.9
vi_prob = 0
vii_prob = 0
viii_prob = 0
random.seed()
cmd = ''
del_cmd = ''
obstacles = []
v_spawn = False #only spawn dumpster_v if dumpster_vi or dumpster_vii present
avoid_set = [(18.25,17.521),(16.681,3.661),(16.681,6.725),(14.182,8.204),(14.294,17.532),(10.169,8.204),(10.168,17.558),(5.787,3.507),(2.841,6.814)] #list of doorway locations to prevent spawning debris in doorways
suff = random.randint(0,100) # random suffix to all model names
barriers = []
if random.random() < bgi_prob:
barriers += ['B', 'G', 'I']
if random.random() < a_prob:
barriers += ['A']
if random.random() < ce_prob:
barriers += ['C', 'E']
# ii_prob = 1 # correlate w/ dumpsters b/w (2,7)
if random.random() < dfh_prob:
barriers += ['D', 'F', 'H']
# add barriers to cmd
for letter in barriers:
temp_cmd, temp_del, model_name = barrier(letter, suff)
cmd += temp_cmd
del_cmd += temp_del
obstacles += [model_name]
if random.random() < i_prob:
x_pos1 = 18.424
x_pos2 = 17.198
y_pos = random.random()*6.0 + 9.0
model_name1 = 'block_i_1_{}'.format(suff)
model_name2 = 'block_i_2_{}'.format(suff)
cmd += ("rosrun gazebo_ros spawn_model " +
"-database drc_practice_orange_jersey_barrier " +
"-gazebo -model " + model_name1 + " -x " + str(x_pos1) +
" -y " + str(y_pos) + " -Y 1.571\n")
cmd += ("rosrun gazebo_ros spawn_model " +
"-database drc_practice_blue_cylinder " +
"-gazebo -model " + model_name2 + " -x " + str(x_pos2) +
" -y " + str(y_pos) + "\n")
del_cmd += ('rosservice call gazebo/delete_model block_i_1\n' +
'rosservice call gazebo/delete_model block_i_2\n')
obstacles += [model_name1, model_name2]
avoid_set.extend([(x_pos1,y_pos), (x_pos2,y_pos)])
if random.random() < ii_prob:
x_pos1 = 15.067
x_pos2 = 12.918
y_pos = random.random()*5.0 + 10.0
model_name1 = "dumpster_ii_1_{}".format(suff)
model_name2 = "dumpster_ii_2_{}".format(suff)
cmd += ("rosrun gazebo_ros spawn_model -database dumpster" +
" -gazebo -model " + model_name1 + " -x " + str(x_pos1) +
' -y ' + str(y_pos) + " -Y 0\n" +
"rosrun gazebo_ros spawn_model -database dumpster" +
" -gazebo -model " + model_name2 + " -x " + str(x_pos2) +
' -y ' + str(y_pos) + ' -Y 1.570796\n')
del_cmd += 'rosservice call gazebo/delete_model dumpster_ii_1\n\
rosservice call gazebo/delete_model dumpster_ii_2\n'
obstacles += [model_name1, model_name2]
avoid_set.append((x_pos1, y_pos))
avoid_set.append((x_pos2, y_pos))
if random.random() < iii_prob:
x_pos = random.random()*4.0 + 6.6
y_pos1 = 11.42
y_pos2 = 9.5
model_name1 = "dumpster_iii_1_{}".format(suff)
model_name2 = "dumpster_iii_2_{}".format(suff)
cmd += ('rosrun gazebo_ros spawn_model -database dumpster\
-gazebo -model ' + model_name1 +
' -x ' + str(x_pos) + ' -y ' + str(y_pos1) + ' -Y 0.0\n\
rosrun gazebo_ros spawn_model -database dumpster\
-gazebo -model ' + model_name2 +
' -x ' + str(x_pos) + ' -y ' + str(y_pos2) + ' -Y 0.0\n')
del_cmd = del_cmd + 'rosservice call gazebo/delete_model dumpster_iii_1\n\
rosservice call gazebo/delete_model dumpster_iii_2\n'
obstacles += [model_name1, model_name2]
avoid_set.append((x_pos,y_pos1))
avoid_set.append((x_pos,y_pos2))
if random.random() < vi_prob:
x_pos1 = 12.774
y_pos1 = 6.486
v_spawn = True
model_name = "dumpster_vi_{}".format(suff)
cmd += ('rosrun gazebo_ros spawn_model -database dumpster\
-gazebo -model ' + model_name +
' -x ' + str(x_pos1) + ' -y ' + str(y_pos1) + ' -Y -1.160\n')
del_cmd += 'rosservice call gazebo/delete_model dumpster_vi\n'
obstacles += [model_name]
avoid_set.append((x_pos1,y_pos1))
if random.random() < vii_prob:
x_pos = 13.127
y_pos = 3.735
v_spawn = True
model_name = "dumpster_vii_{}".format(suff)
cmd += ('rosrun gazebo_ros spawn_model -database dumpster\
-gazebo -model ' + model_name +
' -x ' + str(x_pos) + ' -y ' + str(y_pos) + ' -Y -1.567\n')
del_cmd += 'rosservice call gazebo/delete_model dumpster_vii\n'
obstacles += [model_name]
avoid_set.append((x_pos,y_pos))
if v_spawn:
x_pos = 15.198
y_pos = 4.968
model_name = "dumpster_v_{}".format(suff)
cmd += ('rosrun gazebo_ros spawn_model -database dumpster -gazebo -model ' +
model_name + ' -x ' + str(x_pos) + ' -y ' + str(y_pos) + ' -Y 0.0\n')
del_cmd += 'rosservice call gazebo/delete_model dumpster_v\n'
obstacles += [model_name]
avoid_set.append((x_pos,y_pos))
if random.random() < viii_prob:
x_pos = 4.3266
y_pos = 5.1462
model_name = "dumpster_viii_{}".format(suff)
cmd += ('rosrun gazebo_ros spawn_model -database dumpster\
-gazebo -model ' + model_name +
' -x ' + str(x_pos) + ' -y ' + str(y_pos) + ' -Y 1.092\n')
del_cmd += 'rosservice call gazebo/delete_model dumpster_viii\n'
obstacles += [model_name]
avoid_set.append((x_pos,y_pos))
num_debris = random.randint(0,11)
num_spawned = 0
nloops = 0
while num_spawned < num_debris:
# prevent infinite loops
nloops += 1
if nloops > 1000: break
loc = random.random()
if loc < 0.1: #a
x_pos = 18.8
ymin = 6
ymax = 16.75
y_pos = (ymax-ymin)*random.random()+ymin
elif loc < 0.2: #b
x_pos = 17.4
ymin = 6.0
ymax = 16.75
y_pos = (ymax-ymin)*random.random()+ymin
elif loc < 0.4: #c
xmin = 12.585
xmax = 15.95
x_pos = (xmax-xmin)*random.random()+xmin
ymin = 8.91
ymax = 16.75
y_pos = (ymax-ymin)*random.random()+ymin
elif loc < 0.6: #d
xmin = 6.55
xmax = 15.95
x_pos = (xmax-xmin)*random.random()+xmin
ymin = 3.0
ymax = 7.45
y_pos = (ymax-ymin)*random.random()+ymin
elif loc < 0.7: #e
xmin = 3.58
xmax = 5.0
x_pos = (xmax-xmin)*random.random()+xmin
ymin = 3.0
ymax = 7.48
y_pos = (ymax-ymin)*random.random()+ymin
elif loc < 0.85: #f1
xmin = 3.58
xmax = 11.11
x_pos = (xmax-xmin)*random.random()+xmin
ymin = 8.93
ymax = 11.94
y_pos = (ymax-ymin)*random.random()+ymin
else: #f2
xmin = 3.58
xmax = 11.11
x_pos = (xmax-xmin)*random.random()+xmin
ymin = 13.4
ymax = 16.8
y_pos = (ymax-ymin)*random.random()+ymin
if all(euclidean_distance(coord,(x_pos,y_pos))>3.0 for coord in avoid_set):
model_name = "debris{}_{}".format(num_spawned, suff)
cmd += ('rosrun gazebo_ros spawn_model -database drc_practice_blue_cylinder\
-gazebo -model ' + model_name + ' -x ' + str(x_pos) + ' -y ' + str(y_pos) + ' -Y 0.0\n')
del_cmd = del_cmd + 'rosservice call gazebo/delete_model debris' + str(num_spawned) + '\n'
obstacles += [model_name]
avoid_set.append((x_pos, y_pos))
num_spawned = num_spawned + 1
os.system(cmd)
return obstacles
def visible_set(gridGraph, observationPoint, obstacles):
# obstacles - list of visilibity polygons
# setup environment and observer location
ox = int((observationPoint[0] - gridGraph.origin[0]) / gridGraph.img_res)
oy = int((-observationPoint[1] + gridGraph.origin[1]) /
gridGraph.img_res) + gridGraph.imgheight
# print(ox)
# print(oy)
isovist = visibility_polygon(ox, oy, env)
isocnt = save_print_contour(isovist)
# prep visualization
# image = gridGraph.occ_grid.copy()
# image[np.where(image == 0)] = 255 # easier to see
# cv2.drawContours(image, [isocnt],-1, 0, 2)
# find visible edges
visible_set = []
if len(isocnt) == 0:
logger.warn('Simplified visibility polygon is empty for {}!'.format(
observationPoint))
return visible_set
for edge in list(gridGraph.graph.edges()):
(node1, node2) = edge
(xloc1, yloc1) = gridGraph.graph.node[node1]['pos']
(xloc2, yloc2) = gridGraph.graph.node[node2]['pos']
x1 = int((xloc1 - gridGraph.origin[0]) / gridGraph.img_res)
y1 = int(((-yloc1 + gridGraph.origin[1]) / gridGraph.img_res) +
gridGraph.imgheight)
x2 = int((xloc2 - gridGraph.origin[0]) / gridGraph.img_res)
y2 = int(((-yloc2 + gridGraph.origin[1]) / gridGraph.img_res) +
gridGraph.imgheight)
pt1_visible = point_in_polygon(isocnt, (x1, y1))
pt2_visible = point_in_polygon(isocnt, (x2, y2))
visible = pt1_visible and pt2_visible
at_least_one_endpt_visible = pt1_visible or pt2_visible
if at_least_one_endpt_visible:
for i in range(1, len(isocnt)):
if line_segments_intersect((isocnt[i - 1][0], isocnt[i][0]),
((x1, y1), (x2, y2))):
visible = False
visIntersect = intersection_point(
line_eqn(isocnt[i - 1][0], isocnt[i][0]),
line_eqn((x1, y1), (x2, y2)))
# if intersection_point returns False here, it means
# the lines are parallel and overlap each other
for obstacle in obstacles:
for j in range(1, obstacle.n()):
if line_segments_intersect(
((obstacle[j - 1].x(), obstacle[j - 1].y()),
(obstacle[j].x(), obstacle[j].y())),
((x1, y1), (x2, y2))):
obsIntersect = intersection_point(
line_eqn(
(obstacle[j - 1].x(),
obstacle[j - 1].y()),
(obstacle[j].x(), obstacle[j].y())),
line_eqn((x1, y1), (x2, y2)))
if (visIntersect == False) or (
obsIntersect
== False) or util.euclidean_distance(
visIntersect, obsIntersect
) < 0.1 / gridGraph.img_res:
visible = True
if visible:
visible_set.append(edge)
# cv2.line(image,(x1,y1),(x2,y2),0)
# cv2.circle(image, (ox, oy), 5, (0,0,255), thickness=-1)
# cv2.imshow("Image", image)
# cv2.waitKey(0)
return visible_set