def plot_gaussians(persons,
group_data,
idx,
ellipse_param,
N=200,
show_group_space=True,
plot=True):
""" Plots surface and contour of 2D Gaussian function given ellipse parameters."""
group_radius = group_data['group_radius'][idx]
pspace_radius = group_data['pspace_radius'][idx]
ospace_radius = group_data['ospace_radius'][idx]
group_pos = group_data['group_pose'][idx]
# Initial Gaussians amplitude
A = 1
# Gets the values of x and y of all the persons
x = [item[0] for item in persons]
y = [item[1] for item in persons]
# Gets the coordinates of a windows around the group
xmin = min(x) - 150
xmax = max(x) + 150
ymin = min(y) - 150
ymax = max(y) + 150
X = np.linspace(xmin, xmax, N)
Y = np.linspace(ymin, ymax, N)
X, Y = np.meshgrid(X, Y)
# Pack X and Y into a single 3-dimensional array
pos = np.zeros(X.shape + (2, ))
pos[:, :, 0] = X
pos[:, :, 1] = Y
# plot using subplots
fig, axs = plt.subplots(1, 2, sharey=False, tight_layout=True)
plot_kwargs = {'color': 'g', 'linestyle': '-', 'linewidth': 0.8}
# Personal Space as gaussian for each person in the group
Z_F = np.zeros([N, N])
for person in persons:
sigma = None
sigma_back = None
Z1 = None
mu = np.array([person[0], person[1]])
sigma = params_conversion(ellipse_param[0], ellipse_param[1],
person[2])
sigma_back = params_conversion(ellipse_param[0] / BACK_FACTOR,
ellipse_param[1], person[2])
# The distribution on the variables X, Y packed into pos.
Z1 = asymmetric_gaussian(pos, mu, sigma, person[2],
(person[0], person[1]), N, sigma_back)
# Z matrix only updates the values where Z1 > Z
cond = None
cond = Z1 > Z_F
Z_F[cond] = Z1[cond]
plot_person(person[0], person[1], person[2], axs[0], plot_kwargs)
F_approaching_area = plot_group(group_pos, group_radius, pspace_radius,
ospace_radius, axs[0])
show_group_space = True
sigma = None
if show_group_space:
Z1 = None
mu = np.array([group_pos[0], group_pos[1]])
sigma = params_conversion(ospace_radius, ospace_radius, 0)
Z1 = A * multivariate_gaussian(pos, mu, sigma)
Z1 = multivariate_normal(mu, sigma).pdf(pos)
# Normalization
A1 = 1 / Z1.max()
Z1 = A1 * Z1
# Z matrix only updates the values where Z1 > Z
cond = None
cond = Z1 > Z_F
Z_F[cond] = Z1[cond]
cs1 = axs[0].contour(X, Y, Z_F, cmap="jet", linewidths=0.8, levels=10)
F_approaching_filter, F_approaching_zones = approaching_area_filtering(
F_approaching_area, cs1.allsegs[LEVEL][0])
F_approaching_filter, F_approaching_zones = approaching_heuristic(
group_radius, pspace_radius, group_pos, F_approaching_filter,
cs1.allsegs[LEVEL][0], F_approaching_zones)
F_x_approach = [j[0] for j in F_approaching_filter]
F_y_approach = [k[1] for k in F_approaching_filter]
F_approaching_perimeter = (len(F_x_approach) * 2 * math.pi *
group_radius) / len(F_approaching_area[0])
axs[0].plot(F_x_approach, F_y_approach, 'c.', markersize=5)
F_center_x, F_center_y, F_orientation = zones_center(
F_approaching_zones, group_pos, group_radius)
axs[0].plot(F_center_x, F_center_y, 'r.', markersize=5)
for i, angle in enumerate(F_orientation):
draw_arrow(F_center_x[i], F_center_y[i], angle, axs[0])
axs[0].set_xlabel(r'$x$ $[cm]$')
axs[0].set_ylabel(r'$y$ $[cm]$')
axs[0].set_title(r'Adaptive Parameters - Perimeter = %d $cm$' %
F_approaching_perimeter)
axs[0].set_aspect(aspect=1)
###########################################################################
Z_F = np.zeros([N, N])
for person in persons:
sigma = None
sigma_back = None
Z1 = None
mu = np.array([person[0], person[1]])
sigma = params_conversion(F_PSPACEX, F_PSPACEY, person[2])
sigma_back = params_conversion(F_PSPACEX / BACK_FACTOR, F_PSPACEY,
person[2])
# The distribution on the variables X, Y packed into pos.
Z1 = asymmetric_gaussian(pos, mu, sigma, person[2],
(person[0], person[1]), N, sigma_back)
# Z matrix only updates the values where Z1 > Z
cond = None
cond = Z1 > Z_F
Z_F[cond] = Z1[cond]
plot_person(person[0], person[1], person[2], axs[1], plot_kwargs)
H_approaching_area = plot_group(group_pos, group_radius, pspace_radius,
ospace_radius, axs[1])
show_group_space = True
sigma = None
if show_group_space:
Z1 = None
mu = np.array([group_pos[0], group_pos[1]])
sigma = params_conversion(ospace_radius, ospace_radius, 0)
Z1 = A * multivariate_gaussian(pos, mu, sigma)
Z1 = multivariate_normal(mu, sigma).pdf(pos)
# Normalization
A1 = 1 / Z1.max()
Z1 = A1 * Z1
# Z matrix only updates the values where Z1 > Z
cond = None
cond = Z1 > Z_F
Z_F[cond] = Z1[cond]
cs1 = axs[1].contour(X, Y, Z_F, cmap="jet", linewidths=0.8, levels=10)
H_approaching_filter, H_approaching_zones = approaching_area_filtering(
H_approaching_area, cs1.allsegs[LEVEL][0])
H_approaching_filter, H_approaching_zones = approaching_heuristic(
group_radius, pspace_radius, group_pos, H_approaching_filter,
cs1.allsegs[LEVEL][0], H_approaching_zones)
H_x_approach = [j[0] for j in H_approaching_filter]
H_y_approach = [k[1] for k in H_approaching_filter]
H_approaching_perimeter = (len(H_x_approach) * 2 * math.pi *
group_radius) / len(F_approaching_area[0])
axs[1].plot(H_x_approach, H_y_approach, 'c.', markersize=5)
H_center_x, H_center_y, H_orientation = zones_center(
H_approaching_zones, group_pos, group_radius)
axs[1].plot(H_center_x, H_center_y, 'r.', markersize=5)
for i, angle in enumerate(H_orientation):
draw_arrow(H_center_x[i], H_center_y[i], angle, axs[1])
axs[1].set_xlabel(r'$x$ $[cm]$')
axs[1].set_ylabel(r'$y$ $[cm]$')
axs[1].set_title(r'Fixed Parameters - Perimeter = %d $cm$' %
H_approaching_perimeter)
########################################################################
axs[1].set_aspect(aspect=1)
fig.tight_layout()
if plot:
plt.show(block=False)
print("==================================================")
input("Hit Enter To Close... ")
plt.cla()
plt.clf()
plt.close()
return F_approaching_perimeter, H_approaching_perimeter
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 plot_gaussians(persons,
group_data,
idx,
ellipse_param,
N=200,
show_group_space=True):
""" Plots surface and contour of 2D Gaussian function given ellipse parameters."""
group_radius = group_data['group_radius'][idx]
pspace_radius = group_data['pspace_radius'][idx]
ospace_radius = group_data['ospace_radius'][idx]
group_pos = group_data['group_pose'][idx]
# Initial Gaussians amplitude
A = 1
# Gets the values of x and y of all the persons
x = [item[0] for item in persons]
y = [item[1] for item in persons]
# Gets the coordinates of a windows around the group
xmin = min(x) - 150
xmax = max(x) + 150
ymin = min(y) - 150
ymax = max(y) + 150
X = np.linspace(xmin, xmax, N)
Y = np.linspace(ymin, ymax, N)
X, Y = np.meshgrid(X, Y)
# Pack X and Y into a single 3-dimensional array
pos = np.zeros(X.shape + (2, ))
pos[:, :, 0] = X
pos[:, :, 1] = Y
Z = np.zeros([N, N])
# plot using subplots
fig = plt.figure()
ax1 = fig.add_subplot(1, 2, 2, projection='3d')
ax2 = fig.add_subplot(1, 2, 1)
plot_kwargs = {'color': 'g', 'linestyle': '-', 'linewidth': 0.8}
# Personal Space as gaussian for each person in the group
for person in persons:
Z1 = None
mu = np.array([person[0], person[1]])
Sigma = params_conversion(ellipse_param[0], ellipse_param[1],
person[2])
Sigma_back = params_conversion(ellipse_param[0] / BACK_FACTOR,
ellipse_param[1], person[2])
# The distribution on the variables X, Y packed into pos.
Z1 = asymmetric_gaussian(pos, mu, Sigma, person[2],
(person[0], person[1]), N, Sigma_back)
# Z1 = multivariate_normal(mu, Sigma).pdf(pos)
# Z = Z1
# Z matrix only updates the values where Z1 > Z
cond = Z1 > Z
Z[cond] = Z1[cond]
plot_person(person[0], person[1], person[2], ax2, plot_kwargs)
approaching_area = plot_group(group_pos, group_radius, pspace_radius,
ospace_radius, ax2)
show_group_space = True
if show_group_space:
Z1 = None
mu = np.array([group_pos[0], group_pos[1]])
Sigma = params_conversion(ospace_radius, ospace_radius, 0)
Z1 = A * multivariate_gaussian(pos, mu, Sigma)
Z1 = multivariate_normal(mu, Sigma).pdf(pos)
# Normalization
A1 = 1 / Z1.max()
Z1 = A1 * Z1
# Z matrix only updates the values where Z1 > Z
cond = Z1 > Z
Z[cond] = Z1[cond]
surf = ax1.plot_surface(X,
Y,
Z,
rstride=2,
cstride=2,
linewidth=1,
antialiased=False,
cmap="jet")
# plt.rc('text', usetex=True)
# plt.rc('font', family='Arial')
ax1.set_xlabel(r'$x$ $[cm]$', labelpad=10)
ax1.set_ylabel(r'$y$ $[cm]$', labelpad=10)
ax1.set_zlabel(r'Cost', labelpad=10)
cs = ax2.contour(X, Y, Z, cmap="jet", linewidths=0.8, levels=10)
# cs = ax2.contour(X, Y, Z, cmap="jet", linewidths=0.8)
fig.colorbar(cs)
# Approaching Area filtering - remove points that are inside the personal space of a person
approaching_filter, approaching_zones = approaching_area_filtering(
approaching_area, cs.allsegs[LEVEL][0])
approaching_filter, approaching_zones = approaching_heuristic(
group_radius, pspace_radius, group_pos, approaching_filter,
cs.allsegs[LEVEL][0], approaching_zones)
x_approach = [j[0] for j in approaching_filter]
y_approach = [k[1] for k in approaching_filter]
ax2.plot(x_approach, y_approach, 'c.', markersize=5)
center_x, center_y, orientation = zones_center(approaching_zones,
group_pos, group_radius)
ax2.plot(center_x, center_y, 'r.', markersize=5)
for i, angle in enumerate(orientation):
draw_arrow(center_x[i], center_y[i], angle, ax2)
# robot_pose = [0, 0, 3.14]
# Plots robot from top view
# plot_robot(robot_pose, ax2)
# ax2.annotate("Robot_Initial", (robot_pose[0], robot_pose[1]))
# Estimates the goal pose for the robot to approach the group
# goal_pose = approaching_pose(robot_pose, approaching_filter, group_pos)
# print("Goal Pose (cm,cm,rad) = " + str(goal_pose))
# plot_robot(goal_pose, ax2)
# ax2.annotate("Robot_Goal", (goal_pose[0], goal_pose[1]))
ax2.set_xlabel(r'$x$ $[cm]$')
ax2.set_ylabel(r'$y$ $[cm]$')
ax2.set_aspect(aspect=1)
fig.tight_layout()
plt.show(block=False)
print("==================================================")
input("Hit Enter To Close... ")
surf.remove()
plt.clf()
plt.close()
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)