예제 #1
0
        def redisplay(event):
            figure.clf()
            axes = figure.add_subplot(1, 1, 1)

            if (event is not None) or (gridding[0] is None):
                do_gridding(first_x.Value, first_y.Value, second_x.Value, second_y.Value)
            self.display_grid(background_image, gridding[0], image_set_number, axes)
            canvas.draw()
예제 #2
0
        def redisplay(event):
            figure.clf()
            axes = figure.add_subplot(1, 1, 1)

            if (event is not None) or (gridding[0] is None):
                do_gridding(first_x.Value, first_y.Value, second_x.Value,
                            second_y.Value)
            self.display_grid(background_image, gridding[0], image_set_number,
                              axes)
            canvas.draw()
예제 #3
0
 def display(self, workspace, figure):
     if self.show_window:
         figure.set_subplots((1, 1))
         figure.clf()
         ax = figure.subplot(0, 0)
         gridding = cpg.CPGridInfo()
         gridding.deserialize(workspace.display_data.gridding)
         self.display_grid(workspace.display_data.background_image,
                           gridding,
                           workspace.display_data.image_set_number, ax)
예제 #4
0
 def display(self, workspace, figure):
     if self.show_window:
         figure.set_subplots((1, 1))
         figure.clf()
         ax = figure.subplot(0, 0)
         gridding = cpg.CPGridInfo()
         gridding.deserialize(workspace.display_data.gridding)
         self.display_grid(
             workspace.display_data.background_image, gridding, workspace.display_data.image_set_number, ax
         )
예제 #5
0
    def update(self, robots, motions, short_range_measurements,
               long_range_measurements):
        """Perform visualization updates here."""

        self.logger.debug("Updating visualization.")

        # TODO convert to numpy arrays for scalability
        x_robot_gt = []
        y_robot_gt = []
        x_goal = []
        y_goal = []
        headings = []

        if (self.view_mode == "past_trajectories"):
            max_num_poses = 0
            for i in range(0, len(robots)):
                for j in range(0, len(robots)):
                    if (j in robots[i].n_poses):
                        max_num_poses = max(max_num_poses,
                                            robots[i].n_poses[j])

            # create square matrix showing all the robots' beliefs about each other
            robot_beliefs = np.zeros(
                [len(robots), len(robots), max_num_poses,
                 2])  # 2 bc of x and y

            for i, motion in enumerate(motions):
                self.logger.debug("Robot {} has pos {}, vel {}".format(
                    i, motion.pos, motion.vel))

                pos = motion.pos
                x_robot_gt.append(pos[0])
                y_robot_gt.append(pos[1])

                robot = robots[i]

                for goal_index in range(0, robot.goal_index + 1):
                    goal = robot.my_goals[goal_index]
                    x_goal.append(goal[0])
                    y_goal.append(goal[1])

                for other_robot_id in range(0, len(robots)):
                    if other_robot_id in robot.n_poses:
                        for pose_index in range(0,
                                                robot.n_poses[other_robot_id]):

                            start = sum([
                                robot.n_poses[id] for id in robot.n_poses
                                if id < other_robot_id
                            ])

                            j0 = robot.pose_dim * (start + pose_index)

                            robot_beliefs[i, other_robot_id, pose_index,
                                          0] = robot.x[j0 + 1]
                            robot_beliefs[i, other_robot_id, pose_index,
                                          1] = robot.x[j0 + 2]

                self.robot_errors[i].append(
                    self.get_euclidean_error(
                        pos, robot_beliefs[i, i, robot.n_poses[i] - 1, :]))

                vel = motion.vel
                headings.append((vel[0], vel[1]))

            # set the viewport bounds according to all the beliefs and goals
            x_min_coord_temp = min(np.min(robot_beliefs[:, :, :, 0]),
                                   min(x_goal))
            x_max_coord_temp = max(np.max(robot_beliefs[:, :, :, 0]),
                                   max(x_goal))
            y_min_coord_temp = min(np.min(robot_beliefs[:, :, :, 1]),
                                   min(y_goal))
            y_max_coord_temp = max(np.max(robot_beliefs[:, :, :, 1]),
                                   max(y_goal))

        else:

            robot_beliefs = np.zeros([len(robots), len(robots),
                                      2])  # 2 bc of x and y

            for i, motion in enumerate(motions):
                self.logger.debug("Robot {} has pos {}, vel {}".format(
                    i, motion.pos, motion.vel))

                pos = motion.pos
                x_robot_gt.append(pos[0])
                y_robot_gt.append(pos[1])

                robot = robots[i]

                for goal_index in range(0, robot.goal_index + 1):
                    goal = robot.my_goals[goal_index]
                    x_goal.append(goal[0])
                    y_goal.append(goal[1])

                for other_robot_id in range(0, len(robots)):
                    (other_bel_x,
                     other_bel_y) = robot.robot_pos(other_robot_id)
                    if other_bel_x is not None:
                        robot_beliefs[i, other_robot_id, 0] = other_bel_x
                        robot_beliefs[i, other_robot_id, 1] = other_bel_y

                self.robot_errors[i].append(
                    self.get_euclidean_error(pos, robot_beliefs[i, i, :]))

                vel = motion.vel
                headings.append((vel[0], vel[1]))

            # set the viewport bounds according to all the beliefs and goals
            x_min_coord_temp = min(np.min(robot_beliefs[:, :, 0]), min(x_goal))
            x_max_coord_temp = max(np.max(robot_beliefs[:, :, 0]), max(x_goal))
            y_min_coord_temp = min(np.min(robot_beliefs[:, :, 1]), min(y_goal))
            y_max_coord_temp = max(np.max(robot_beliefs[:, :, 1]), max(y_goal))

        # get the main update figure, clear it out for our update
        # fig = plt.figure(self.main_figure_number)
        # fig.clf()

        # draw the error graphs
        # error_fig = plt.figure(self.error_figure_number)
        # error_fig.clf()
        # num_subplots = len(robots)
        # for i in range(1, num_subplots + 1):
        #     plt.subplot(num_subplots, 1, i)
        #     plt.plot(self.robot_errors[i-1])

        # get the main update figure, clear it out for our update
        fig = plt.figure(self.main_figure_number)
        fig.clf()

        # calculate viewport bounds

        t1 = 2
        t2 = 2

        self.x_min_coord = min(self.x_min_coord,
                               (np.floor(x_min_coord_temp / t1) * t1) - t2)
        self.x_max_coord = max(self.x_max_coord,
                               (np.ceil(x_max_coord_temp / t1) * t1) + t2)
        self.y_min_coord = min(self.y_min_coord,
                               (np.floor(y_min_coord_temp / t1) * t1) - t2)
        self.y_max_coord = max(self.y_max_coord,
                               (np.ceil(y_max_coord_temp / t1) * t1) + t2)

        x_scale = self.x_max_coord - self.x_min_coord
        y_scale = self.y_max_coord - self.y_min_coord

        #make the scale the same on each axis
        if (x_scale > y_scale):
            self.y_max_coord = (
                (self.y_max_coord + self.y_min_coord) / 2) + (x_scale / 2)
            self.y_min_coord = self.y_max_coord - x_scale
        else:
            self.x_max_coord = (
                (self.x_max_coord + self.x_min_coord) / 2) + (y_scale / 2)
            self.x_min_coord = self.x_max_coord - y_scale

        plt.xlim([self.x_min_coord, self.x_max_coord])
        plt.ylim([self.y_min_coord, self.y_max_coord])

        # plt.xlim([self.x_min_coord - margin, self.x_max_coord + margin])
        # plt.ylim([self.y_min_coord - margin, self.y_max_coord + margin])

        #draw force fields

        X, Y = np.meshgrid(np.arange(self.x_min_coord, self.x_max_coord, 1),
                           np.arange(self.y_min_coord, self.y_max_coord, 1))
        U = self.disturbance(0 * X, X, Y)[1]
        V = self.disturbance(0 * X, X, Y)[2]
        Q = plt.quiver(X, Y, U, V, units='width', color=(0.0, 0.0, 0.0, 0.3))

        # draw robots ground truth
        plt.scatter(x_robot_gt, y_robot_gt, c='k', marker='o')

        if (self.view_mode == "past_trajectories"):
            # draw the robot belief as their own individual colors
            for i in range(0, len(robots)):
                belief_x = np.ndarray([])
                belief_y = np.ndarray([])

                i_x = 0
                i_y = 0

                for other_robot_id in range(0, len(robots)):
                    if (other_robot_id in robots[i].n_poses):
                        belief_x = np.append(
                            belief_x,
                            robot_beliefs[i, other_robot_id,
                                          0:robots[i].n_poses[other_robot_id],
                                          0])
                        belief_y = np.append(
                            belief_y,
                            robot_beliefs[i, other_robot_id,
                                          0:robots[i].n_poses[other_robot_id],
                                          1])

                        if (other_robot_id == i):
                            i_x = len(belief_x) - 1
                            i_y = len(belief_y) - 1

                plt.scatter(belief_x, belief_y, marker='o')

                plt.annotate(xy=(belief_x[i_x], belief_y[i_y]),
                             s="{}".format(i))

        else:
            for i in range(0, len(robots)):
                robot_i_beliefs_x = robot_beliefs[i, :, 0]
                robot_i_beliefs_y = robot_beliefs[i, :, 1]
                plt.scatter(robot_i_beliefs_x, robot_i_beliefs_y, marker='o')
                for j in range(len(robot_i_beliefs_x)):
                    plt.annotate(xy=(robot_i_beliefs_x[j],
                                     robot_i_beliefs_y[j]),
                                 s="{}".format(j))

        #draw legends
        robot_legend_labels = [
            "Robot {} beliefs".format(i) for i in range(0, len(robots))
        ]
        robot_legend_labels = ["Disturbance Field", "Ground Truths"
                               ] + robot_legend_labels
        plt.legend(robot_legend_labels,
                   loc='upper center',
                   bbox_to_anchor=(0.5, 1.15),
                   fancybox=True,
                   ncol=2)

        # draw "headings" aka the velocity vectors
        ax = plt.axes()
        for i, heading in enumerate(headings):
            ax.arrow(x_robot_gt[i], y_robot_gt[i], heading[0], heading[1])

        # draw goals as green x's
        plt.scatter(x_goal, y_goal, c='g', marker='x')

        # draw messages being sent between robots
        self.draw_messages(ax, motions, short_range_measurements, 'r')
        self.draw_messages(ax, motions, long_range_measurements, 'y')

        self.logger.debug("Writing frame to file!")
        for i in range(self.fps):  # Actually just output one frame per second
            self.writer.grab_frame()

        if not self.headless:
            plt.pause(0.01)
            plt.show()
예제 #6
0
    def _plot_airmass(self, figure, site, tgt_data, tz):
        """
        Plot into `figure` an airmass chart using target data from `info`
        with time plotted in timezone `tz` (a tzinfo instance).
        """
        # Urk! This seems to be necessary even though we are plotting
        # python datetime objects with timezone attached and setting
        # date formatters with the timezone
        tz_str = tz.tzname(None)
        mpl.rcParams['timezone'] = tz_str

        # set major ticks to hours
        majorTick = mpl_dt.HourLocator(tz=tz)
        majorFmt = mpl_dt.DateFormatter('%Hh')
        # set minor ticks to 15 min intervals
        minorTick = mpl_dt.MinuteLocator(range(0,59,15), tz=tz)

        figure.clf()
        ax1 = figure.add_subplot(111)

        #lstyle = 'o'
        lstyle = '-'
        lt_data = map(lambda info: info.ut.astimezone(tz),
                      tgt_data[0].history)
        # sanity check on dates in preferred timezone
        ## for dt in lt_data[:10]:
        ##     print(dt.strftime("%Y-%m-%d %H:%M:%S"))

        # plot targets airmass vs. time
        for i, info in enumerate(tgt_data):
            am_data = numpy.array(map(lambda info: info.airmass, info.history))
            am_min = numpy.argmin(am_data)
            am_data_dots = am_data
            color = self.colors[i % len(self.colors)]
            lc = color + lstyle
            # ax1.plot_date(lt_data, am_data, lc, linewidth=1.0, alpha=0.3, aa=True, tz=tz)
            ax1.plot_date(lt_data, am_data_dots, lc, linewidth=2.0,
                          aa=True, tz=tz)
            #xs, ys = mpl.mlab.poly_between(lt_data, 2.02, am_data)
            #ax1.fill(xs, ys, facecolor=self.colors[i], alpha=0.2)

            # plot object label
            targname = info.target.name
            ax1.text(mpl_dt.date2num(lt_data[am_data.argmin()]),
                     am_data.min() + 0.08, targname.upper(), color=color,
                     ha='center', va='center')

        ax1.set_ylim(2.02, 0.98)
        ax1.set_xlim(lt_data[0], lt_data[-1])
        ax1.xaxis.set_major_locator(majorTick)
        ax1.xaxis.set_minor_locator(minorTick)
        ax1.xaxis.set_major_formatter(majorFmt)
        labels = ax1.get_xticklabels()
        ax1.grid(True, color='#999999')

        # plot current hour
        lo = datetime.now(tz)
        #lo = datetime.now(tz=tz)
        hi = lo + timedelta(0, 3600.0)
        if lt_data[0] < lo < lt_data[-1]:
            ax1.axvspan(lo, hi, facecolor='#7FFFD4', alpha=0.25)

        # label axes
        localdate = lt_data[0].astimezone(tz).strftime("%Y-%m-%d")
        title = 'Airmass for the night of %s' % (localdate)
        ax1.set_title(title)
        ax1.set_xlabel(tz.tzname(None))
        ax1.set_ylabel('Airmass')

        # Plot moon altitude and degree scale
        ax2 = ax1.twinx()
        moon_data = numpy.array(map(lambda info: numpy.degrees(info.moon_alt),
                                    tgt_data[0].history))
        #moon_illum = site.moon_phase()
        ax2.plot_date(lt_data, moon_data, '#666666', linewidth=2.0,
                      alpha=0.5, aa=True, tz=tz)
        mxs, mys = mpl.mlab.poly_between(lt_data, 0, moon_data)
        # ax2.fill(mxs, mys, facecolor='#666666', alpha=moon_illum)
        ax2.set_ylabel('Moon Altitude (deg)', color='#666666')
        ax2.set_ylim(0, 90)
        ax2.set_xlim(lt_data[0], lt_data[-1])
        ax2.xaxis.set_major_locator(majorTick)
        ax2.xaxis.set_minor_locator(minorTick)
        ax2.xaxis.set_major_formatter(majorFmt)
        ax2.set_xlabel('')
        ax2.yaxis.tick_right()

        canvas = self.fig.canvas
        if canvas is not None:
            canvas.draw()