def draw_person_top(x, y, angle, ax):
"""Draws a persons from a top view."""
top_y = HUMAN_Y / 2
top_x = HUMAN_X / 2
plot_ellipse(semimaj=top_x,
semimin=top_y,
phi=angle,
x_cent=x,
y_cent=y,
ax=ax)
def plot_person(x, y, angle, ax, plot_kwargs):
""" Plots a person from a top view."""
r = 10 # or whatever fits you
ax.arrow(x, y, r * math.cos(angle), r * math.sin(angle),
head_length=1, head_width=1, shape='full', color='blue')
ax.plot(x, y, 'bo', markersize=8)
top_y = HUMAN_Y / 2
top_x = HUMAN_X / 2
plot_ellipse(semimaj=top_x, semimin=top_y,
phi=angle, x_cent=x, y_cent=y, ax=ax)
def draw_personalspace(x, y, angle, ax, sx, sy, plot_kwargs, idx):
"""Draws personal space of an inidivdual and it."""
draw_arrow(x, y, angle, ax) # orientation arrow angle in radians
ax.plot(x, y, 'bo', markersize=8)
draw_person_top(x, y, angle, ax)
ax.text(x + 10, y + 10, "$P_" + str(idx) + "$", fontsize=12)
plot_ellipse(semimaj=sx, semimin=sy, phi=angle, x_cent=x, y_cent=y, ax=ax,
plot_kwargs=plot_kwargs)
# Multiply sx and sy by 2 to get the diameter
return patches.Ellipse((x, y), sx * 2, sy * 2, math.degrees(angle))
def plot_person(x, y, angle, ax, plot_kwargs):
""" Plots a person from a top view."""
draw_arrow(x, y, angle, ax)
ax.plot(x, y, 'bo', markersize=8)
top_y = HUMAN_Y / 2
top_x = HUMAN_X / 2
plot_ellipse(semimaj=top_x,
semimin=top_y,
phi=angle,
x_cent=x,
y_cent=y,
ax=ax)
def plot_robot(pose, ax):
"""Draws a robot from a top view."""
x = pose[0]
y = pose[1]
angle = pose[2]
top_y = HUMAN_Y / 2
top_x = HUMAN_Y / 2
plot_kwargs = {'color': 'black', 'linestyle': '-', 'linewidth': 1}
plot_ellipse(semimaj=top_x, semimin=top_y,
phi=angle, x_cent=x, y_cent=y, ax=ax, plot_kwargs=plot_kwargs)
draw_arrow(x, y, angle, ax) # orientation arrow angle in radians
ax.plot(x, y, 'o', color='black', markersize=5)
def getMatrixDifferenceEllipsePoints(cell,targetface, diffmatrix,primordialfaceid = 135,zoomfactor=1.): #getting the Target Form Matrix faces = qd.CellFaceIterator(cell) face = faces.next() while True: if face.getID() == targetface: break face = faces.next() #print "Face Id : ", face.getID() targetformmatrix = diffmatrix unitx = face.getUnitx() unity = face.getUnity() unitz = face.getUnitz() unit_mat = np.matrix([[qd.doublearray_getitem(unitx,0),qd.doublearray_getitem(unitx,1),qd.doublearray_getitem(unitx,2)], [qd.doublearray_getitem(unity,0),qd.doublearray_getitem(unity,1),qd.doublearray_getitem(unity,2)], [qd.doublearray_getitem(unitz,0),qd.doublearray_getitem(unitz,1),qd.doublearray_getitem(unitz,2)]]) #transposing unitmatrix transpose_unitmat = np.matrix(np.transpose(unit_mat)) #Getting Centroid of face xcent = face.getXCentralised() ycent = face.getYCentralised() zcent = face.getZCentralised() ##### getting data from ellipse & getting transformed coordinate to 3d Cartesian data = ep.plot_ellipse(cov=targetformmatrix, data_out=True,norm = True) points = zoomfactor*np.matrix(np.vstack((data,np.zeros(len(data[0]))))) transformedpoints = transpose_unitmat*points transformedpoints[0]+= xcent transformedpoints[1]+= ycent transformedpoints[2]+= zcent return transformedpoints
def approaching_heuristic(group_radius, pspace_radius, group_pos,
approaching_filter, contour_points,
approaching_zones):
""" """
approaching_radius = group_radius
approaching_radius += R_STEP
if not approaching_filter:
while not approaching_filter and approaching_radius <= pspace_radius:
approaching_area = None
approaching_filter = None
approaching_zones = None
approaching_area = plot_ellipse(semimaj=approaching_radius,
semimin=approaching_radius,
x_cent=group_pos[0],
y_cent=group_pos[1],
data_out=True)
approaching_filter, approaching_zones = approaching_area_filtering(
approaching_area, contour_points)
approaching_radius += R_STEP
return approaching_filter, approaching_zones
def plot_group(group_pose, group_radius, pspace_radius, ospace_radius, ax,
persons, sx, sy):
"""Plots o-space, p-space, group center and approaching area."""
# O Space Modeling
ax.plot(group_pose[0], group_pose[1], 'rx', markersize=8)
plot_kwargs = {'color': 'r', 'linestyle': '-', 'linewidth': 1}
plot_ellipse(semimaj=ospace_radius,
semimin=ospace_radius,
x_cent=group_pose[0],
y_cent=group_pose[1],
ax=ax,
plot_kwargs=plot_kwargs)
# P Space Modeling
plot_ellipse(semimaj=pspace_radius,
semimin=pspace_radius,
x_cent=group_pose[0],
y_cent=group_pose[1],
ax=ax,
plot_kwargs=plot_kwargs)
# approaching circle area
plot_kwargs = {'color': 'c', 'linestyle': ':', 'linewidth': 2}
plot_ellipse(semimaj=group_radius,
semimin=group_radius,
x_cent=group_pose[0],
y_cent=group_pose[1],
ax=ax,
plot_kwargs=plot_kwargs)
approaching_area = plot_ellipse(semimaj=group_radius,
semimin=group_radius,
x_cent=group_pose[0],
y_cent=group_pose[1],
data_out=True)
# Possible approaching area computation and personal space ploting
plot_kwargs = {'color': 'g', 'linestyle': '-', 'linewidth': 0.8}
approaching_filter = approaching_area
for idx, person in enumerate(persons, start=1):
shapely_diff_sy = 0.5 # Error between python modules used
shapely_diff_sx = 1
personal_space = draw_personalspace(
person[0], person[1], person[2], ax, sx - shapely_diff_sx,
sy - shapely_diff_sy, plot_kwargs,
idx) # plot using ellipse.py functions
# Approaching Area filtering - remove points that are inside the personal space of a person
approaching_filter = approachingfiltering_ellipses(
personal_space, approaching_filter, idx)
# possible approaching positions
approaching_x = [j[0] for j in approaching_filter]
approaching_y = [k[1] for k in approaching_filter]
ax.plot(approaching_x, approaching_y, 'c.', markersize=5)
def plot_group(group_pose, group_radius, pspace_radius, ospace_radius, ax):
"""Plots the group o space, p space and approaching circle area. """
# O Space Modeling
ax.plot(group_pose[0], group_pose[1], 'rx', markersize=8)
plot_kwargs = {'color': 'r', 'linestyle': '-', 'linewidth': 1}
plot_ellipse(semimaj=ospace_radius, semimin=ospace_radius, x_cent=group_pose[0],
y_cent=group_pose[1], ax=ax, plot_kwargs=plot_kwargs)
# P Space Modeling
plot_ellipse(semimaj=pspace_radius, semimin=pspace_radius, x_cent=group_pose[0],
y_cent=group_pose[1], ax=ax, plot_kwargs=plot_kwargs)
# approaching circle area
plot_kwargs = {'color': 'c', 'linestyle': ':', 'linewidth': 2}
plot_ellipse(semimaj=group_radius, semimin=group_radius, x_cent=group_pose[0],
y_cent=group_pose[1], ax=ax, plot_kwargs=plot_kwargs)
approaching_area = plot_ellipse(semimaj=group_radius, semimin=group_radius, x_cent=group_pose[0],
y_cent=group_pose[1], data_out=True)
return approaching_area
blobs_log = blob_log(data, max_sigma=3, num_sigma=3, threshold=.1)
y, x, sigma = blobs_log.T
plt.show()
pts = np.vstack((x, y)).transpose()
# cluster points
radius = 100
import time
t0 = time.time()
labels = clusterPts(pts, radius, num_clusters=10)
t1 = time.time() - t0
for lbl in labels:
ell = pts[lbl, :].astype(float)
shift = ell.min(axis=0)
scale = ell.max()
pp = (ell - shift) / scale
if len(pp) > 3:
p, _ = ellipse.fit_ellipse(pp[:, 0], pp[:, 1])
xx, yy = ellipse.plot_ellipse(p)
xx = xx * scale + shift[0]
yy = yy * scale + shift[1]
plt.imshow(data)
plt.scatter(ell[:, 0], ell[:, 1])
plt.plot(xx, yy)
plt.show()
print('num of clusters = %d' % len(labels))
print('time taken = %f' % t1)
ax.scatter(data_all[pid][0:20, 0],
data_all[pid][0:20, 1],
s=20,
color='blue',
alpha=alpha,
marker='o')
# Plot generated exemplars
ax.scatter(data_all[pid][20:, 0],
data_all[pid][20:, 1],
s=20,
color='red',
alpha=alpha,
marker='x')
# Plot the ellipses
plot_ellipse(ax, ms_post, ss_post)
# Standardize axes
lim = 2.5
ax.get_xaxis().set_visible(False)
ax.get_yaxis().set_visible(False)
ax.set_ylim(-lim, lim)
ax.set_xlim(-lim, lim)
ax.set_title(pid)
# Save the figure for easy future access
plt.savefig('real_fit%d.pdf' % ki)
## Model comparison
#Convert model and trace into dictionary pairs
dict_pairs = dict(zip(gmm_all, traces))
#Perform WAIC comparison
def run_behavior(self):
"""
"""
while not rospy.is_shutdown():
if self.people_received and self.groups:
self.people_received = False
if self.costmap_received and self.groups:
self.costmap_received = False
# # Choose the nearest pose
dis = 0
for idx, group in enumerate(self.groups):
# Choose the nearest group
dis_aux = euclidean_distance(group["pose"][0],
group["pose"][1],
self.pose[0],
self.pose[1])
if idx == 0:
goal_group = group
dis = dis_aux
elif dis > dis_aux:
goal_group = group
dis = dis_aux
# Meter algures uma condicap que verifica que se nao for possivel aproximar o grupo escolher outro
#Tentar plotar costmap
# Choose the nearest group
group = goal_group
g_radius = group["g_radius"] # Margin for safer results
pspace_radius = group["pspace_radius"]
ospace_radius = group["ospace_radius"]
approaching_area = plot_ellipse(semimaj=g_radius,
semimin=g_radius,
x_cent=group["pose"][0],
y_cent=group["pose"][1],
data_out=True)
approaching_filter, approaching_zones = approaching_area_filtering(
approaching_area, self.costmap)
approaching_filter, approaching_zones = approaching_heuristic(
g_radius, pspace_radius, group["pose"],
approaching_filter, self.costmap, approaching_zones)
center_x, center_y, orientation = zones_center(
approaching_zones, group["pose"], g_radius)
approaching_poses = []
for l, _ in enumerate(center_x):
approaching_poses.append(
(center_x[l], center_y[l], orientation[l]))
fig = plt.figure()
ax = fig.add_subplot(1, 1, 1)
plot_kwargs = {
'color': 'g',
'linestyle': '-',
'linewidth': 0.8
}
for person in group["members"]:
plot_person(person[0], person[1], person[2], ax,
plot_kwargs)
_ = plot_group(group["pose"], g_radius, pspace_radius,
ospace_radius, ax)
for i, angle in enumerate(orientation):
draw_arrow(center_x[i], center_y[i], angle, ax)
x_approach = [j[0] for j in approaching_filter]
y_approach = [k[1] for k in approaching_filter]
ax.plot(x_approach, y_approach, 'c.', markersize=5)
ax.plot(center_x, center_y, 'r.', markersize=5)
ax.set_aspect(aspect=1)
#fig.tight_layout()
plt.show()
# Verificar se zonas de aproximacao estao vazias, se estiverm escolher outro grupo ou fazer break
if approaching_zones:
len_areas = []
for zone in approaching_zones:
len_areas.append(len(zone))
# Choose the pose in the biggest approaching area
idx = len_areas.index(max(len_areas))
goal_pose = approaching_poses[idx][0:2]
goal_quaternion = tf.transformations.quaternion_from_euler(
0, 0, approaching_poses[idx][2])
try:
rospy.loginfo("Approaching group!")
result = movebase_client(goal_pose,
goal_quaternion)
if result:
rospy.loginfo("Goal execution done!")
break
except rospy.ROSInterruptException:
rospy.loginfo("Navigation test finished.")
else:
rospy.loginfo("Impossible to approach group.")
def run_behavior(self):
"""
"""
while not rospy.is_shutdown():
if self.people_received and self.groups:
self.people_received = False
if self.costmap_up_received and self.groups:
self.costmap_up_received = False
# # Choose the nearest pose
dis = 0
for idx, group in enumerate(self.groups):
# Choose the nearest group
dis_aux = euclidean_distance(group["pose"][0],
group["pose"][1],
self.pose[0],
self.pose[1])
if idx == 0:
goal_group = group
dis = dis_aux
elif dis > dis_aux:
goal_group = group
dis = dis_aux
# Meter algures uma condicap que verifica que se nao for possivel aproximar o grupo escolher outro
#Tentar plotar costmap
# Choose the nearest group
group = goal_group
g_radius = group["g_radius"] #Margin to for safer results
pspace_radius = group["pspace_radius"]
ospace_radius = group["ospace_radius"]
approaching_area = plot_ellipse(semimaj=g_radius,
semimin=g_radius,
x_cent=group["pose"][0],
y_cent=group["pose"][1],
data_out=True)
approaching_filter, approaching_zones = approaching_area_filtering(
approaching_area, self.costmap)
approaching_filter, approaching_zones = approaching_heuristic(
g_radius, pspace_radius, group["pose"],
approaching_filter, self.costmap, approaching_zones)
#################################################################################################################################
#Approaching area marker
marker = Marker()
marker.header.frame_id = "/map"
marker.type = marker.SPHERE_LIST
marker.action = marker.ADD
marker.pose.orientation.w = 1
for point_f in approaching_filter:
m_point = Point()
m_point.x = point_f[0]
m_point.y = point_f[1]
m_point.z = 0.1
marker.points.append(m_point)
t = rospy.Duration()
marker.lifetime = t
marker.scale.x = 0.03
marker.scale.y = 0.03
marker.scale.z = 0.03
marker.color.a = 1.0
marker.color.r = 1.0
self.pubm.publish(marker)
#######################################################################################
# Approaching pose Pose Array
ap_points = PoseArray()
ap_points.header.frame_id = "/map"
ap_points.header.stamp = rospy.Time.now()
center_x, center_y, orientation = zones_center(
approaching_zones, group["pose"], g_radius)
approaching_poses = []
for l, _ in enumerate(center_x):
approaching_poses.append(
(center_x[l], center_y[l], orientation[l]))
ap_pose = Pose()
ap_pose.position.x = center_x[l]
ap_pose.position.y = center_y[l]
ap_pose.position.z = 0.1
quaternion = tf.transformations.quaternion_from_euler(
0, 0, orientation[l])
ap_pose.orientation.x = quaternion[0]
ap_pose.orientation.y = quaternion[1]
ap_pose.orientation.z = quaternion[2]
ap_pose.orientation.w = quaternion[3]
ap_points.poses.append(ap_pose)
self.pubg.publish(ap_points)
z[:, np.newaxis] * np.random.multivariate_normal(m, s, size=N)
for z, m, s in zip(zs, mus, sigmas)
]
# Stack data into single array
data = np.sum(np.dstack(xs), axis=2)
## Plot them nicely
# Prepare subplots
fig, ax = plt.subplots(figsize=(8, 6))
# First, scatter
plt.scatter(data[:, 0], data[:, 1], c='g', alpha=0.5)
plt.scatter(mus[0, 0], mus[0, 1], c='r', s=100)
plt.scatter(mus[1, 0], mus[1, 1], c='b', s=100)
plt.scatter(mus[2, 0], mus[2, 1], c='y', s=100)
# Then, ellipses
plot_ellipse(ax, mus, sigmas)
ax.axis('equal')
plt.show()
## Build model and sample
# Number of iterations for sampler
draws = 2000
# Prepare lists of starting points for mu to prevent label-switching problem
testvals = [[-2, -2], [0, 0], [2, 2]]
# Model structure
with pm.Model() as mvgmm:
# Prior over component weights
p = pm.Dirichlet('p', a=np.array([1.] * K))
# Prior over component means
