Exemple #1
0
class NaoController:
    def __init__(self, classifier_path, preprocessor_path, response_mapping):
        self.response_mapping = response_mapping
        self.robot = Nao()
        self.camera_controller = CameraController()

        self.frame_preprocessor = pickle.load(open(preprocessor_path, 'rb'))
        self.classifier = pickle.load(open(classifier_path, 'rb'))

        self._flusher = Thread(target=self._flush, daemon=True)
        self._flusher.start()

    def _flush(self):
        while True:
            self.camera_controller.flush()

    def __del__(self):
        del self.camera_controller

    def step(self):
        return self.robot.step()

    def on_frame(self):
        frame = self.camera_controller.get_frame()
        if self.robot.is_moving():
            return
        processed = self.frame_preprocessor.preprocess(frame)
        label = self.classifier.predict(np.array(
            [processed]))[0] if processed is not None else None
        self.robot.play_motion(self.response_mapping[label])
Exemple #2
0
    def __init__(self, classifier_path, preprocessor_path, response_mapping):
        self.response_mapping = response_mapping
        self.robot = Nao()
        self.camera_controller = CameraController()

        self.frame_preprocessor = pickle.load(open(preprocessor_path, 'rb'))
        self.classifier = pickle.load(open(classifier_path, 'rb'))

        self._flusher = Thread(target=self._flush, daemon=True)
        self._flusher.start()
Exemple #3
0
    def __init__(self, img_size=(256, 160), img_observation=False):
        """Initialise environment.

        Args:
            img_size (int tuple): (width, height) of image to return.
            img_observation (bool): If true returns observation as dictionary
                                    containing img and state(euler coordinates).
                                    Otherwise returns observation as n array.
        """
        # Enable baxter and initialise environment
        baxter_interface.RobotEnable().enable()
        self._left_cam = CameraController("/cameras/left_hand_camera/image",
                                          img_size)
        self._left_arm = baxter_interface.Limb('left')
        self._left_arm.set_joint_position_speed(1.0)
        self._left_gripper = baxter_interface.Gripper('left')
        ns = "ExternalTools/left/PositionKinematicsNode/IKService"
        self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
        self._move_to_start_position()
        self._load_gazebo_models()
        rospy.wait_for_service(ns, 5.0)

        # Observation information
        self.img_observation = img_observation
        self.image = None
        self.img_size = img_size
        self.x_pos = None
        self.y_pos = None
        self.z_pos = None
        self.horiz_dist = None
        self.vert_dist = None
        self.depth_dist = None

        # Accuracy tollerance
        self._horiz_threshold = 0.05
        self._vert_threshold = 0.05
        self._min_depth_threshold = 1
        self._max_depth_threshold = 2

        # Object position
        self._obj_x_pos = None
        self._obj_y_pos = None
        self._obj_z_pos = None

        # Limit on endpoint movements
        self._x_move_lim = 0.28
        self._y_move_lim = 0.40

        # Limits on endpoint position relative to object
        self._x_dist_lim = 0.28
        self._y_dist_lim = 0.40

        # Limit on endpoint position relative to body
        self._x_pos_min = 0.36
Exemple #4
0
    def __init__(self, droneList):
        ShowBase.__init__(self)

        # set resolution
        wp = WindowProperties()
        wp.setSize(2000, 1500)
        # wp.setSize(1200, 900)
        # wp.setSize(800, 600)
        self.win.requestProperties(wp)

        self.setFrameRateMeter(True)
        self.render.setAntialias(AntialiasAttrib.MAuto)
        CameraController(self)

        # setup model directory
        self.modelDir = os.path.abspath(
            sys.path[0])  # Get the location of the 'py' file I'm running:
        self.modelDir = Filename.from_os_specific(self.modelDir).getFullpath(
        ) + "/models"  # Convert that to panda's unix-style notation.

        self.initScene()
        self.initBullet()

        self.droneManager = DroneManager(self, droneList)
        DroneRecorder(self.droneManager)

        self.stopwatchOn = False
        self.now = 0
        self.accept('t', self.toggleStopwatch)
Exemple #5
0
    def process_frame(self, frame_info, params):
        camera_ctrl = CameraController()
        camera_ctrl.get_presets()

        video_w = frame_info['video_w']
        video_h = frame_info['video_h']
        start_h = frame_info['start_h']
        end_h = frame_info['end_h']
        start_w = frame_info['start_w']
        end_w = frame_info['end_w']
        fps = frame_info['fps']
        f_resize_ratio = frame_info['resize_ratio']
        f_send_server = params['f_send_server']
        f_show = params['f_show']
        f_save = params['f_save']

        resize_w = int(f_resize_ratio * video_w)
        resize_h = int(f_resize_ratio * video_h)

        # out = cv2.VideoWriter('output.avi', cv2.VideoWriter_fourcc(*'MPEG'), fps, (video_w, video_h))
        frame = None
        self.last_time = time.time()
        while True:
            if self.quit_thread:
                break
            try:
                current_preset, moving = camera_ctrl.get_current_preset()
                print("current preset {} => ({}, {}, {})  {}".format(
                    current_preset.Name,
                    current_preset['PTZPosition'].PanTilt.x,
                    current_preset['PTZPosition'].PanTilt.y,
                    current_preset['PTZPosition'].Zoom.x, moving))
            except:
                time.sleep(0.5)
                continue
            if moving == "MOVING":
                time.sleep(0.5)
                continue
            print("frame _lock===")
            self._frame_lock = True
            if self.current_frame is not None:
                frame = self.current_frame.copy()

            if frame is None:
                time.sleep(0.01)
                self._frame_lock = False
                continue
            if f_resize_ratio <= 1:
                dim = (resize_w, resize_h)
                resize_frame = cv2.resize(frame, dim, cv2.INTER_AREA)
                img_draw, valid_rects = self.process_image(
                    resize_frame, DETECT_THRESHOLD)
                if f_send_server:
                    temp_name = str(time.time()) + '.jpg'

                    # cv2.imwrite(temp_name, img_draw)
                    json_req = make_request_json(ip_addr=CAMERA_IP,
                                                 img_file=temp_name,
                                                 count=len(valid_rects),
                                                 cam_name=current_preset.Name)
                    response_code = send_request(server=SERVER_URL,
                                                 cam_name=current_preset.Name,
                                                 req_json=json_req)
                    if response_code == 200:
                        self.last_time = time.time()
            else:
                print("frame ==copied ===")
                frame1 = frame[:end_h, :end_w].copy()
                frame2 = frame[:end_h, start_w:].copy()
                frame3 = frame[start_h:, :end_w].copy()
                frame4 = frame[start_h:, start_w:].copy()

                _, valid_rects1 = self.process_image(frame1, DETECT_THRESHOLD)
                _, valid_rects2 = self.process_image(frame2, DETECT_THRESHOLD)
                _, valid_rects3 = self.process_image(frame3, DETECT_THRESHOLD)
                _, valid_rects4 = self.process_image(frame4, DETECT_THRESHOLD)

                # -------------------------- combine the detection result ----------------------
                valid_rects = []
                for i in range(len(valid_rects1)):
                    rect = valid_rects1[i]
                    if rect[2] < end_w - 10 and rect[3] < end_h - 10:
                        valid_rects.append(rect)

                new_rect_list = []
                for i in range(len(valid_rects2)):
                    rect = valid_rects2[i]
                    if rect[0] > 10 and rect[3] < end_h - 10:
                        new_rect = [
                            rect[0] + start_w, rect[1], rect[2] + start_w,
                            rect[3]
                        ]
                        if not func.check_contain(valid_rects, new_rect):
                            new_rect_list.append(new_rect)

                valid_rects += new_rect_list
                new_rect_list = []
                for i in range(len(valid_rects3)):
                    rect = valid_rects3[i]
                    if rect[2] < end_w - 10 and rect[1] > 10:
                        new_rect = [
                            rect[0], rect[1] + start_h, rect[2],
                            rect[3] + start_h
                        ]
                        if not func.check_contain(valid_rects, new_rect):
                            new_rect_list.append(new_rect)

                valid_rects += new_rect_list
                new_rect_list = []
                for i in range(len(valid_rects4)):
                    rect = valid_rects4[i]
                    if rect[0] > 10 and rect[1] > 10:
                        new_rect = [
                            rect[0] + start_w, rect[1] + start_h,
                            rect[2] + start_w, rect[3] + start_h
                        ]
                        if not func.check_contain(valid_rects, new_rect):
                            new_rect_list.append(new_rect)
                valid_rects += new_rect_list

                # ----------------------- Send the result to server --------------------------
                if f_send_server:
                    temp_name = str(time.time()) + '.jpg'

                    # cv2.imwrite(temp_name, frame)
                    json_req = make_request_json(ip_addr=CAMERA_IP,
                                                 img_file=temp_name,
                                                 count=len(valid_rects),
                                                 cam_name=current_preset.Name)
                    send_request(server=SERVER_URL,
                                 cam_name=current_preset.Name,
                                 req_json=json_req)
                    # func.rm_file(temp_name)

                # ---------------------------- draw the result -------------------------------
                print('The are {} peoples.'.format(len(valid_rects)))

            self._frame_lock = False
    def __init__(self):
        """
        Summary => initialize the main window view.

        Description =>

        args => None

        return => None
        """
        super(MainWindow, self).__init__()
        self.ui = Ui_mainWindow()
        self.ui.setupUi(self)
        self.move(100, 100)  # move gui to the 100,100 on the screen
        self.setWindowIcon(QtGui.QIcon('Images/Icon.png'))  # sets icon for gui
        self.camera = CameraController(self.ui)
        # --------------------------------------------------------
        # set menuBar actions
        self.ui.actionExit_Application.setShortcut('Ctrl+Q')
        self.ui.actionExit_Application.setStatusTip('Leaving Application')
        self.ui.actionExit_Application.triggered.connect(
            self.close_application)
        restart_application = QtGui.QAction("&Restart Application", self)
        restart_application.setShortcut("R")
        restart_application.setStatusTip('Leave The App')
        restart_application.triggered.connect(self.restart_application)
        self.ui.menuFile.addAction(restart_application)

        # set unworking menuBar items, to display error message
        self.ui.actionLogin.triggered.connect(self.trigger_error)
        self.ui.actionToggle_Admin_View.triggered.connect(self.trigger_error)
        self.ui.actionTutorial.triggered.connect(self.trigger_error)

        # --------------------------------------------------------
        # set the stackedWidget view to be the first page
        self.display_stack(0)
        # --------------------------------------------------------
        # set up buttons
        self.ui.capture_picture.clicked.connect(self.capture_image)
        btn = self.ui.yes_no_button.button(self.ui.yes_no_button.Yes)
        btn.clicked.connect(self.accept_picture)
        btn = self.ui.yes_no_button.button(self.ui.yes_no_button.No)
        btn.clicked.connect(self.reject_picture)
        # self.ui.yes_no_button.clicked.connect(self.accept_picture)
        btn = self.ui.yes_no_button2.button(self.ui.yes_no_button.Yes)
        btn.clicked.connect(self.start_plot)
        self.ui.btn_continue1.clicked.connect(self.process_image)
        btn = self.ui.yes_no_button2.button(self.ui.yes_no_button.No)
        btn.clicked.connect(self.reject_style)
        # self.ui.video_capture.show()

        # style check boxes
        self.style_check_boxes = [
            self.ui.check_style1, self.ui.check_style2, self.ui.check_style3,
            self.ui.check_style4, self.ui.check_style5
        ]
        # set styles labels
        self.style_check_boxes[0].setText("Dithering")
        self.style_check_boxes[1].setText("edges")
        self.style_check_boxes[2].setText("No Style")
        self.style_check_boxes[3].setText("No Style")
        self.style_check_boxes[4].setText("No Style")
        # set style images
        pixmap = QtGui.QPixmap('Images/dithering_example.jpg')
        self.ui.image_style1.setPixmap(pixmap)

        pixmap = QtGui.QPixmap('Images/edges_style_example.jpg')
        pixmap = pixmap.scaled(100, 100, QtCore.Qt.KeepAspectRatio)
        self.ui.image_style2.setPixmap(pixmap)

        pixmap = QtGui.QPixmap("Images/blank_image.jpg")
        self.ui.image_style3.setPixmap(pixmap)

        pixmap = QtGui.QPixmap("Images/blank_image.jpg")
        self.ui.image_style4.setPixmap(pixmap)

        pixmap = QtGui.QPixmap("Images/blank_image.jpg")
        self.ui.image_style5.setPixmap(pixmap)

        # import text for description
        # got technique from
        # https://stackoverflow.com/questions/8369219/
        # how-do-i-read-a-text-file-into-a-string-variable-in-python
        text = ""
        with open('description.txt', 'r') as myfile:
            text = myfile.read()
        # set discription text
        self.ui.description_text.setText(text)
class MainWindow(QtGui.QMainWindow):
    """
    Summary => maintain the GUI used to control the robotic artist.

    Description => This class will be used to generate and control the GUI used
    for the system. You will be able to control the camera and see the feed
    comming from it.

    args => None

    return => None
    """
    def __init__(self):
        """
        Summary => initialize the main window view.

        Description =>

        args => None

        return => None
        """
        super(MainWindow, self).__init__()
        self.ui = Ui_mainWindow()
        self.ui.setupUi(self)
        self.move(100, 100)  # move gui to the 100,100 on the screen
        self.setWindowIcon(QtGui.QIcon('Images/Icon.png'))  # sets icon for gui
        self.camera = CameraController(self.ui)
        # --------------------------------------------------------
        # set menuBar actions
        self.ui.actionExit_Application.setShortcut('Ctrl+Q')
        self.ui.actionExit_Application.setStatusTip('Leaving Application')
        self.ui.actionExit_Application.triggered.connect(
            self.close_application)
        restart_application = QtGui.QAction("&Restart Application", self)
        restart_application.setShortcut("R")
        restart_application.setStatusTip('Leave The App')
        restart_application.triggered.connect(self.restart_application)
        self.ui.menuFile.addAction(restart_application)

        # set unworking menuBar items, to display error message
        self.ui.actionLogin.triggered.connect(self.trigger_error)
        self.ui.actionToggle_Admin_View.triggered.connect(self.trigger_error)
        self.ui.actionTutorial.triggered.connect(self.trigger_error)

        # --------------------------------------------------------
        # set the stackedWidget view to be the first page
        self.display_stack(0)
        # --------------------------------------------------------
        # set up buttons
        self.ui.capture_picture.clicked.connect(self.capture_image)
        btn = self.ui.yes_no_button.button(self.ui.yes_no_button.Yes)
        btn.clicked.connect(self.accept_picture)
        btn = self.ui.yes_no_button.button(self.ui.yes_no_button.No)
        btn.clicked.connect(self.reject_picture)
        # self.ui.yes_no_button.clicked.connect(self.accept_picture)
        btn = self.ui.yes_no_button2.button(self.ui.yes_no_button.Yes)
        btn.clicked.connect(self.start_plot)
        self.ui.btn_continue1.clicked.connect(self.process_image)
        btn = self.ui.yes_no_button2.button(self.ui.yes_no_button.No)
        btn.clicked.connect(self.reject_style)
        # self.ui.video_capture.show()

        # style check boxes
        self.style_check_boxes = [
            self.ui.check_style1, self.ui.check_style2, self.ui.check_style3,
            self.ui.check_style4, self.ui.check_style5
        ]
        # set styles labels
        self.style_check_boxes[0].setText("Dithering")
        self.style_check_boxes[1].setText("edges")
        self.style_check_boxes[2].setText("No Style")
        self.style_check_boxes[3].setText("No Style")
        self.style_check_boxes[4].setText("No Style")
        # set style images
        pixmap = QtGui.QPixmap('Images/dithering_example.jpg')
        self.ui.image_style1.setPixmap(pixmap)

        pixmap = QtGui.QPixmap('Images/edges_style_example.jpg')
        pixmap = pixmap.scaled(100, 100, QtCore.Qt.KeepAspectRatio)
        self.ui.image_style2.setPixmap(pixmap)

        pixmap = QtGui.QPixmap("Images/blank_image.jpg")
        self.ui.image_style3.setPixmap(pixmap)

        pixmap = QtGui.QPixmap("Images/blank_image.jpg")
        self.ui.image_style4.setPixmap(pixmap)

        pixmap = QtGui.QPixmap("Images/blank_image.jpg")
        self.ui.image_style5.setPixmap(pixmap)

        # import text for description
        # got technique from
        # https://stackoverflow.com/questions/8369219/
        # how-do-i-read-a-text-file-into-a-string-variable-in-python
        text = ""
        with open('description.txt', 'r') as myfile:
            text = myfile.read()
        # set discription text
        self.ui.description_text.setText(text)
# ------------------------------------------------------------
# methods
# ------------------------------------------------------------

    def display_stack(self, i):
        """
        Summary => changes the widget stack page.

        Description => will change the stacked widget view to the int given
        through i.

        This code was found on:
        https://www.tutorialspoint.com/pyqt/pyqt_qstackedwidget.htm

        If the specified page has any objects that needs to be initialized
        then the process will be ran through here.

        args => i => the int for the page wanted. Ranging from 0-3

        return => None
        """
        self.ui.page_layer.setCurrentIndex(i)

        if (i == 0):
            print("starting VideoCapture")
            # set checked_boxes to false
            self.ui.check_process.setChecked(False)
            # creating a thread to update images when complete.
            # update images in gui
            update = UpdateImages(self.ui)
            # connect signal
            self.connect(update, QtCore.SIGNAL("update_images(PyQt_PyObject)"),
                         self.update_images)
            # intialize the video_capture object
            self.video_capture = VideoCapture(self.ui, update)
            # connect video_capture to main gui thread
            self.connect(self.video_capture,
                         QtCore.SIGNAL("update_capture(QImage)"),
                         self.update_capture)
            self.camera.start_video_capture(self.video_capture)
        elif (i == 2):
            # set check boxes to false
            for check in self.style_check_boxes:
                check.setChecked(False)

    def update_images(self, images):
        """
        Summary => update all images in the ui.

        Description => This method will be called, when any of the non-static
                images in the ui has been updated.

                Works by when a signal is called from another thread it will
                update the given images. That are sent through a list.

        args => images - list - this is a list of string for the images

        return => None
        """
        pixmap = QtGui.QPixmap(images[0])
        # update images throughout gui that are not static.
        pixmap = pixmap.scaled(498, 300, QtCore.Qt.KeepAspectRatio)
        self.ui.captured_image.setPixmap(pixmap)
        self.ui.captured_image.setAlignment(QtCore.Qt.AlignCenter)

        pixmap = pixmap.scaled(100, 100, QtCore.Qt.KeepAspectRatio)
        self.ui.image_your_picture.setPixmap(pixmap)

        pixmap = QtGui.QPixmap(images[1])
        pixmap = pixmap.scaled(498, 300, QtCore.Qt.KeepAspectRatio)
        self.ui.styled_image.setPixmap(pixmap)
        self.ui.styled_image.setAlignment(QtCore.Qt.AlignCenter)

    def update_capture(self, img):
        """
        Summary => update the capture image.

        Description => This method will be called, when the video_capture is
                    running.

                    Will update the video_capture img/Qlabel every time
                    a new image comes through.

        args => None

        return => None
        """
        pixmap = QtGui.QPixmap(img)
        pixmap = pixmap.scaled(498, 300, QtCore.Qt.KeepAspectRatio)
        self.ui.video_capture.setPixmap(pixmap)
        self.ui.video_capture.setAlignment(QtCore.Qt.AlignCenter)

# ------------------------------------------------------------
# Actions
# ------------------------------------------------------------

    def process_image(self):
        """
        Summary => starts the process for processing.

        Description => start the process for processing the image that
                was captured ealier on in the gui. This method will be
                a action used for the specified button.

        args => None

        return => None
        """
        print("Image processing")
        self.processors = [Dithering(), EdgesStyle()]
        no_check = True
        checked = 0
        # loop through every processor
        for i, processor in enumerate(self.processors):
            # check if check_box style has been ticked
            if self.style_check_boxes[i].isChecked():
                no_check = False
                checked = checked + 1
        # check if no check_boxs have been ticked
        if no_check:
            error = QMessageBox()
            error.setIcon(QMessageBox.Information)
            error.setText("No style selected \n Please select the syle wanted")
            error.setWindowTitle("Error: No style")
            error.setStandardButtons(QMessageBox.Ok)
            error.exec_()
        else:
            if checked == 1:
                for i, processor in enumerate(self.processors):
                    # check if check_box style has been ticked
                    if self.style_check_boxes[i].isChecked():
                        # run the processor
                        processor.run()
                # update images in gui
                update = UpdateImages(self.ui)
                # connect signal to update thread
                self.connect(update,
                             QtCore.SIGNAL("update_images(PyQt_PyObject)"),
                             self.update_images)
                # run the update thread
                update.start()

                # display the next page
                self.display_stack(3)
            else:
                error = QMessageBox()
                error.setIcon(QMessageBox.Information)
                error.setWindowTitle("Error: Too many styles")
                error.setText("Too many style selected \n Please pick one")
                error.setStandardButtons(QMessageBox.Ok)
                error.exec_()

    def capture_image(self):
        """
        Summary => starts the process to capture the picture.

        Description => starts the process to capture the picture from the
                video feed. This image will be saved to 'Image/takenPicture'.

        args => None

        return => None
        """
        print("taken picture!!!!!")
        if self.ui.check_process.isChecked():
            # sleep to let the program to update, before changing screen
            time.sleep(0.1)
            self.camera.stop_video_capture()
            # changed to the acceptance page, after picture has been taken
            self.display_stack(1)
        else:
            error = QMessageBox()
            error.setIcon(QMessageBox.Information)
            error.setText("Please confirm that we can process your picture")
            error.setWindowTitle("Confirmation")
            error.setStandardButtons(QMessageBox.Ok)
            error.exec_()

    def accept_picture(self):
        """
        Summary => action to take the user to the next page.

        Description => when this action is given, it will take the user to
                the style picking page.

        args => None

        return => None
        """
        self.display_stack(2)

    def start_plot(self):
        """
        Summary => Will initialize the plotting process.

        Description => will initialize the plotting process from the
                coordinates given, when the image has been changed in the
                styling stage.

        args => None

        return => None
        """
        print("plotting")
        # create the plotter conroller (coordinates , scale)
        # plot coords for dithering style
        self.plotters = [
            DitheringPlotter(self.processors[0].get_coordinates(), 1),
            EdgeStylePlotter(self.processors[1].get_coordinates(), 1)
        ]
        # call run, when ready to throw through the process.
        for i, plotter in enumerate(self.plotters):
            # check if specified style is ticked
            if self.style_check_boxes[i].isChecked():
                # need to check if thread is running, so doesn't start more
                # than one thread from pressing button muiltiple times.
                if not plotter.is_alive():
                    plotter.start()

    def reject_style(self):
        """
        Summary => will reject the current style and ask if they are sure.

        Description => will reject the current stlye and before returning
                to the previous stage. The program will ask if they are sure
                by using a message box.

        args => None

        return => None

        """
        # if no, display message box to make sure that they don't want the
        # the image
        choice = QMessageBox.question(
            self, 'Are you sure', "Are you sure that you do not want" +
            " this image, if not you will be" + " returned to styling page?",
            QMessageBox.Yes | QMessageBox.No)
        if choice == QMessageBox.Yes:
            self.display_stack(2)
        else:
            pass

    def reject_picture(self):
        """
        Summary => will reject the current picture and ask if they are sure.

        Description => will reject the current picture and before returning
                to the previous stage. The program will ask if they are sure
                by using a message box.

        args => None

        return => None
        """
        # if no, display message box to make sure that they don't want the
        # the image
        choice = QMessageBox.question(
            self, 'Are you sure', "Are you sure that you do not want" +
            " this image, if not you will be" + " returned to home page?",
            QMessageBox.Yes | QMessageBox.No)
        if choice == QMessageBox.Yes:
            self.display_stack(0)
        else:
            pass

    def close_application(self):
        """
        Summary => will close the Gui.

        Description => will create a message box that will ask if you
        want to leave the Application. If yes, Application is closed and if
        no - return to Application

        args => None

        return => None
        """
        # creating exit pop-up message
        choice = QMessageBox.question(self, 'Leaving Application',
                                      "Leave Application?",
                                      QMessageBox.Yes | QMessageBox.No)
        # do actions depending on response
        if choice == QMessageBox.Yes:
            # close the camera feed, if it is still running
            self.camera.stop_video_capture()
            # reset each of the images to blank, when closed
            cv2.imwrite("Images/takenPicture.jpg",
                        cv2.imread("Images/blank.jpg"))
            cv2.imwrite("Images/processedImage.jpg",
                        cv2.imread("Images/blank.jpg"))
            print("Leaving Application")
            sys.exit()
        else:
            pass

    def restart_application(self):
        """
        Summary => will restart the Gui.

        Description => will create a message box that will ask if you
        want to restart the Application. If yes, Application will restart and
        set back to the first page and if no - return to Application.

        args => None

        return => None
        """
        if self.ui.page_layer.currentIndex() == 0:
            time.sleep(1)
            error = QMessageBox()
            error.setIcon(QMessageBox.Information)
            error.setText("Cannot restart this page")
            error.setWindowTitle("Error: cannot restart")
            error.setStandardButtons(QMessageBox.Ok)
            error.exec_()
        else:
            # creating exit pop-up message
            choice = QMessageBox.question(self, 'Restarting Application',
                                          "Restart Application?",
                                          QMessageBox.Yes | QMessageBox.No)
            # do actions depending on response
            if choice == QMessageBox.Yes:
                # close the camera feed, if it is still running
                self.camera.stop_video_capture()
                # reset each of the images to blank, when closed
                cv2.imwrite("Images/takenPicture.jpg",
                            cv2.imread("Images/blank.jpg"))
                cv2.imwrite("Images/processedImage.jpg",
                            cv2.imread("Images/blank.jpg"))
                print("Restarting Application")
                # change bk to front page
                self.display_stack(0)
            else:
                pass

    def trigger_error(self):
        """
        Summary => will send out of order error.

        Description => will send out of order error message.
                This message is to say that this part of the program is
                either broken or not complete.

        args => None

        return => None
        """
        error = QMessageBox()
        error.setIcon(QMessageBox.Information)
        error.setText("Error: this function is currently out of use")
        error.setWindowTitle("Error: Out of order")
        error.setStandardButtons(QMessageBox.Ok)
        error.exec_()
Exemple #8
0
class BaxterEnvironment:
    """Class representing the learning environment."""
    def __init__(self, img_size=(256, 160), img_observation=False):
        """Initialise environment.

        Args:
            img_size (int tuple): (width, height) of image to return.
            img_observation (bool): If true returns observation as dictionary
                                    containing img and state(euler coordinates).
                                    Otherwise returns observation as n array.
        """
        # Enable baxter and initialise environment
        baxter_interface.RobotEnable().enable()
        self._left_cam = CameraController("/cameras/left_hand_camera/image",
                                          img_size)
        self._left_arm = baxter_interface.Limb('left')
        self._left_arm.set_joint_position_speed(1.0)
        self._left_gripper = baxter_interface.Gripper('left')
        ns = "ExternalTools/left/PositionKinematicsNode/IKService"
        self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
        self._move_to_start_position()
        self._load_gazebo_models()
        rospy.wait_for_service(ns, 5.0)

        # Observation information
        self.img_observation = img_observation
        self.image = None
        self.img_size = img_size
        self.x_pos = None
        self.y_pos = None
        self.z_pos = None
        self.horiz_dist = None
        self.vert_dist = None
        self.depth_dist = None

        # Accuracy tollerance
        self._horiz_threshold = 0.05
        self._vert_threshold = 0.05
        self._min_depth_threshold = 1
        self._max_depth_threshold = 2

        # Object position
        self._obj_x_pos = None
        self._obj_y_pos = None
        self._obj_z_pos = None

        # Limit on endpoint movements
        self._x_move_lim = 0.28
        self._y_move_lim = 0.40

        # Limits on endpoint position relative to object
        self._x_dist_lim = 0.28
        self._y_dist_lim = 0.40

        # Limit on endpoint position relative to body
        self._x_pos_min = 0.36

    def reset(self):
        """Reset arm position and move object to random location.

        Returns:
            observation and low state desciptor

        """
        self._move_to_start_position()
        # Loop until gripper not obscuring object
        visible = False
        while not visible:
            self._move_block()
            self._update_state()
            visible = self.horiz_dist is not None
        return self._get_state()

    def step(self, action):
        """Attempt to execute the action.

        Returns:
            state, reward, done

        """
        # Make move if possible and update observation/state
        joint_angles = self._get_joint_angles(action)
        if joint_angles:
            self._move_arm(joint_angles)
        self._update_state()

        # Calculate reward and if new episode should be started
        reward = 0.
        done = False
        if self.horiz_dist is None:
            # Lost sight of object, end episode and return larger penalty
            reward = -3
            done = True
        else:
            # Return negative reward based on distances
            reward -= abs(self.horiz_dist)
            reward -= abs(self.vert_dist)

        return self._get_state(), reward, done

    def close(self):
        """Delete loaded models. To be called when node is shutting down."""
        self._delete_gazebo_models()

    def _get_state(self):
        if self.img_observation:
            return {'img': self.image, 'state': [self.x_pos, self.y_pos]}
        return [self.horiz_dist, self.vert_dist, self.x_pos, self.y_pos]

    def _update_state(self):
        # Update image
        self.image = self._left_cam.get_image()

        # Update cartesian coordinates
        pose = self._left_arm.endpoint_pose()

        self.z_pos = pose['position'].z
        self.x_pos = pose['position'].x
        self.y_pos = pose['position'].y

        # Calculate distance to object and distance from centre of camera
        self.horiz_dist, self.vert_dist = self._calc_dist()

    def _calc_dist(self):
        # Define lower and upper colour bounds
        BLUE_MIN = np.array([100, 150, 0], np.uint8)
        BLUE_MAX = np.array([140, 255, 255], np.uint8)

        # Threshold the image in the HSV colour space
        hsv_image = cv2.cvtColor(self.image, cv2.COLOR_RGB2HSV)
        mask = cv2.inRange(hsv_image, BLUE_MIN, BLUE_MAX)

        # If no object in image return none
        moments = cv2.moments(mask)
        if (moments["m00"] == 0):
            return None, None

        # Calculate normalised horizontal and vertical distances
        obj_x = int(moments["m10"] / moments["m00"])
        obj_y = int(moments["m01"] / moments["m00"])
        x_dist = obj_x - self.img_size[0] / 2
        y_dist = obj_y - self.img_size[1] / 2
        x_dist = float(x_dist) / (self.img_size[0] / 2)
        y_dist = float(y_dist) / (self.img_size[1] / 2)
        return x_dist, y_dist

    def _get_joint_angles(self, action):
        def clamp(n, minim, maxim):
            return max(minim, min(n, maxim))

        # Calculate desired endpoint position of arm
        pose = self._left_arm.endpoint_pose()
        x = pose['position'].x + action[0] * self._x_move_lim
        y = pose['position'].y + action[1] * self._y_move_lim

        # Clamp values to prevent losing sight of object
        x = clamp(x, self._obj_x_pos - self._x_dist_lim,
                  self._obj_x_pos + self._x_dist_lim)
        y = clamp(y, self._obj_y_pos - self._y_dist_lim,
                  self._obj_y_pos + self._y_dist_lim)

        # Clamp to prevent gripper being too close to body
        x = max(x, self._x_pos_min)

        # Create pose of desired position
        pose = Pose()
        pose.position.x = x
        pose.position.y = y
        pose.position.z = 0.3
        pose.orientation.x = 0.0
        pose.orientation.y = 1.0
        pose.orientation.z = 0.0
        pose.orientation.w = 0.0

        # Return result from inverse kinematics or none if move invalid
        i = 0
        joint_angles = None
        while joint_angles is None and i < 5:
            joint_angles = self._ik_request(pose)
            i = i + 1
        return joint_angles

    def _move_to_start_position(self):
        # Move arm to the starting position
        starting_joint_angles = {
            'left_w0': 0.003,
            'left_w1': 1.379,
            'left_w2': -0.121,
            'left_e0': 0.020,
            'left_e1': 1.809,
            'left_s0': -0.935,
            'left_s1': -1.618
        }
        self._move_arm(starting_joint_angles)
        self._left_gripper.open()

    def _ik_request(self, pose):
        # Send request
        hdr = Header(stamp=rospy.Time.now(), frame_id='base')
        ikreq = SolvePositionIKRequest()
        ikreq.pose_stamp.append(PoseStamped(header=hdr, pose=pose))
        try:
            resp = self._iksvc(ikreq)
        except (rospy.ServiceException, rospy.ROSException), e:
            rospy.logerr("Service call failed: %s" % (e, ))
            return None

        # If successful then return joint angles other return none
        resp_seeds = struct.unpack('<%dB' % len(resp.result_type),
                                   resp.result_type)
        limb_joints = {}
        if (resp_seeds[0] != resp.RESULT_INVALID):
            limb_joints = dict(
                zip(resp.joints[0].name, resp.joints[0].position))
        else:
            return None
        return limb_joints
Exemple #9
0
 def start(self):
     server_stream_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
     server_stream_socket.bind(("127.0.0.1", 12345))
     server_stream_socket.listen()
     camera = CameraController()
     state = 'end'
     while True:
         logging.warning("[Main]: Waiting for connection ...")
         (connection, address) = server_stream_socket.accept()
         logging.warning("[Main]: Connected.")
         stop = False
         while True:
             try:
                 logging.warning("[Main]: Waiting for data ...")
                 msg = connection.recv(1024)
                 msg = msg.decode('UTF-8')
                 if msg != '':
                     logging.warning("[Main]: Data received...")
                     logging.warning("[Main]: msg: {}".format(msg))
                 if 'start' in msg:
                     if state == 'end':
                         path = msg[6:]
                         Path(path).mkdir(parents=True, exist_ok=True)
                         state = 'start'
                         camera.set_video_path(path)
                         camera.start_camera()
                         connection.sendall(b"StreamPort:9090")
                 elif 'end' in msg:
                     if state == 'start':
                         state = 'end'
                         camera.stop_camera()
                         while True:
                             if camera.finished is True:
                                 camera.finished = False
                                 break
                         periods = []
                         for i in range(0, 8):
                             sp = []
                             for period in camera.session_info.periods[i]:
                                 sp.append(period.__dict__)
                             periods.append(sp)
                         result = Result(camera.result.__dict__, periods)
                         json_string = json.dumps(result.__dict__)
                         connection.sendall(f"SessionResult:{json_string}".encode('UTF-8'))
                         logging.warning("[Main]: msg sent")
                 elif 'exit' in msg:
                     logging.warning("[Main]: state='{}'".format(state))
                     if state == 'start':
                         state = 'end'
                         camera.stop_camera()
                         while True:
                             if camera.finished is True:
                                 camera.finished = False
                                 break
                         periods = []
                         for i in range(0, 8):
                             sp = []
                             for period in camera.session_info.periods[i]:
                                 sp.append(period.__dict__)
                             periods.append(sp)
                         result = Result(camera.result.__dict__, periods)
                         json_string = json.dumps(result.__dict__)
                         connection.sendall(f"SessionResult:{json_string}".encode('UTF-8'))
                         logging.warning("[Main]: msg sent")
                     else:
                         connection.sendall(f"EXITING".encode('UTF-8'))
                         logging.warning("[Main]: exiting in progress")
                     connection.close()
                     stop = True
                     break
                 else:
                     logging.warning("[Main]: Client disconnected")
                     connection.close()
                     break
             except:
                 logging.warning("[Main]: Socket disconnected")
                 connection.close()
                 break
         if stop:
             break
Exemple #10
0
    if camera_target is not None:
        ps = PoseStamped()
        ps.header.stamp = image.header.stamp
        ps.header.frame_id = 'base_link'
        ps.pose = ray_to_pose(p, camera_target)
        pose2_pub.publish(ps)

        cam.look_at(np.array(p) + camera_target, source_frame='base_link')


rospy.init_node('person_tracker')

bridge = CvBridge()
tr = TreeReader()
cam = CameraController()
track = None
camera_target = None
image_to_show = None

detect_objects = rospy.ServiceProxy('detect_objects', DetectObjects)
image_sub = rospy.Subscriber('image', Image, callback)
pose_pub = rospy.Publisher('detected_{}'.format(CLASS),
                           PoseStamped,
                           queue_size=1)
pose2_pub = rospy.Publisher('camera_target'.format(CLASS),
                            PoseStamped,
                            queue_size=1)

rospy.spin()
                v = detection[0] - mean[:2]
                v = np.outer(v, v)
                track.update(t, detection[0], detection[1] + v * 10)

        tracks = []
        for track in self._tracks:
            _, cov = track.get_prediction(t)
            if np.trace(cov[:2, :2]) / 2. < 6.**2:
                tracks.append(track)
        self._tracks = tracks


if __name__ == '__main__':
    rospy.init_node('openpose_tracker')

    cam = CameraController()
    tracker = Tracker()
    camera_target = None
    move_camera = False
    people_msg = None

    pose_pub = rospy.Publisher('person',
                               PoseWithCovarianceStamped,
                               queue_size=1)
    pose2_pub = rospy.Publisher('camera_target', PoseStamped, queue_size=1)

    def _cb(msg):
        global people_msg
        people_msg = msg

    rospy.Subscriber('people_tx2',
Exemple #12
0
    def import_sceneArcPkg(self, sceneArcPkg):

        actorArcPkg = None

        for arcPkg in sceneArcPkg:

            if arcPkg.get_ArchivePackageName() == "actor":

                actorArcPkg = arcPkg

        actorMgr = ActorManager()

        self.__actorMgr = actorMgr

        #actorArcPkg.print_metaData()

        for actorItem in actorArcPkg.get_itemsData():

            actor = actorMgr.load_res(_resId = actorItem[0],
                                      resPath = actorItem[1],
                                      extraResPath = actorItem[2])

            #print "the sceneMgr get_res : ", self.get_res(actorItem[0])

            actor.setPos(actorItem[3])
            actor.setHpr(actorItem[4])
            actor.setScale(actorItem[5])

            parentNode = self.get_res(actorItem[6])
            actor.reparentTo(parentNode)

        eventActionRecord = actorArcPkg.get_metaData("eventActionRecord")
        eventEffertRecord = actorArcPkg.get_metaData("eventEffertRecord")

        #print "in import : eventActionRecord ", len(eventActionRecord)

        for actorId, record in eventActionRecord.iteritems():

            for toggleEvent, actionName in record.iteritems():

                actorMgr.add_toggle_to_actor(toggleEvent, actorId, actionName)

        for actorId, record in eventEffertRecord.iteritems():

            for toggleEvent, effertList in record.iteritems():

                for effert in effertList:

                    print "import eventEffertRecord : ", toggleEvent, ", ", actorId, ", ", effert

                    actorMgr.add_effert_to_actor(toggleEvent, actorId, effert[0])
        #actorMgr.set_clock(globalClock)

        actorMgr.print_eventActionRecord()
        actorMgr.print_eventEffertRecord()

        #self.__actorMgr = actorMgr

        # Model数据读档
        modelArcPkg = None

        for arcPkg in sceneArcPkg:

            if arcPkg.get_ArchivePackageName() == "model":

                modelArcPkg = arcPkg

        modelMgr = ModelManager()
        self.__modelMgr = modelMgr

        for modelItem in modelArcPkg.get_itemsData():

            model = modelMgr.load_res(_resId = modelItem[0],
                                      resPath = modelItem[1])

            model.setPos(modelItem[2])
            model.setHpr(modelItem[3])
            model.setScale(modelItem[4])

            parentNode = self.get_res(resId = modelItem[5])
            model.reparentTo(parentNode)

        # Terrain数据读档
        terraArcPkg = None
        terraMgr = TerrainManager()

        for arcPkg in sceneArcPkg:

            if arcPkg.get_ArchivePackageName() == "terrain":

                terraArcPkg = arcPkg

        for terraItem in terraArcPkg.get_itemsData():

            terrain = terraMgr.load_res(_resId = terraItem[0],
                                        resPath = terraItem[1],
                                        extraResPath = terraItem[2])

            terrain.getRoot().setPos(terraItem[3])
            terrain.getRoot().setHpr(terraItem[4])
            terrain.getRoot().setScale(terraItem[5])

            parentNode = self.get_res(resId = terraItem[6])
            terrain.getRoot().reparentTo(parentNode)

        terraMgr.set_currTerrain(terraArcPkg.get_metaData("currTerraId"))

        self.__terraMgr = terraMgr

        # Camera数据读档
        cameraArcPkg = None

        for arcPkg in sceneArcPkg:

            if arcPkg.get_ArchivePackageName() == "camera":
                cameraArcPkg = arcPkg

        camCtrlr = CameraController()
        self.__camCtrlr = camCtrlr
        camCtrlr.bind_camera(self.__showbase.cam)
        camCtrlr.bind_ToggleHost(self.__showbase)

        cameraArcPkg = cameraArcPkg.get_itemsData()[0]
        #print "in import : ", cameraArcPkg[0]
        camCtrlr.get_camToCtrl().setPos(cameraArcPkg[0])
        camCtrlr.get_camToCtrl().setHpr(cameraArcPkg[1])
        camCtrlr.set_moveSpeed(cameraArcPkg[2])
        camCtrlr.set_rotateSpeed(cameraArcPkg[3])

        objToFocus = self.get_res(cameraArcPkg[4])
        camCtrlr.focus_on(objToFocus, cameraArcPkg[5])

        camCtrlr.set_optsSwitch(cameraArcPkg[6])
        #camCtrlr.set_toggleEventToOpts(cameraArcPkg[7])

        self.__actorMgr.bind_CameraController(camCtrlr)

        for toggleEvent, opt in cameraArcPkg[7].iteritems():

            camCtrlr.add_toggle_to_opt(toggleEvent, opt)

        # print "Camera Pos : ", camCtrlr.get_camToCtrl().getPos()
        # print "Camera Hpr : ", camCtrlr.get_camToCtrl().getHpr()

        # Light数据读档
        lightArcPkg = None

        for arcPkg in sceneArcPkg:

            if arcPkg.get_ArchivePackageName() == "light":

                lightArcPkg = arcPkg

        lightCtrlr = LightController()
        self.__lightCtrlr = lightCtrlr
        lightCtrlr.bind_SceneManager(self)

        for lightItem in lightArcPkg.get_itemsData():

            light = lightCtrlr.create_light(_lightId = lightItem[0],
                                            lightType = SeriousTools.extract_name_from_Id(lightItem[0]),
                                            lightColor = lightItem[1],
                                            lightPos = lightItem[2],
                                            lightHpr = lightItem[3],
                                            targetId = lightItem[4],
                                            parentId = lightItem[6])
            #print "in import : ", light
            for setorId in lightItem[5]:

               lightCtrlr.set_light_to(lightItem[0], setorId)