Esempio n. 1
0
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
Esempio n. 2
0
    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.")
Esempio n. 3
0
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()
Esempio n. 4
0
    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)