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)