Example #1
0
    def __init__(self, parent=None):
        QtGui.QWidget.__init__(self, parent)
        loadUi(os.path.join(path, 'resources', 'path.ui'), self)

        try:
            rospy.wait_for_service('robot_send_command', timeout=5)
            self.send_command = rospy.ServiceProxy('robot_send_command',
                                                   SrvRobotCommand)
        except:
            rospy.loginfo('ERROR connecting to service robot_send_command.')
        #self.pub = rospy.Publisher(
        #    'robot_command_json', MsgRobotCommand, queue_size=10)

        self.btnLoadPath.clicked.connect(self.btnLoadPathClicked)
        icon = QtGui.QIcon.fromTheme('document-open')
        self.btnLoadPath.setIcon(icon)
        self.btnSavePath.clicked.connect(self.btnSavePathClicked)
        icon = QtGui.QIcon.fromTheme('document-save')
        self.btnSavePath.setIcon(icon)
        self.btnRunPath.clicked.connect(self.btnRunPathClicked)
        icon = QtGui.QIcon.fromTheme('media-playback-start')
        self.btnRunPath.setIcon(icon)

        self.btnDelete.clicked.connect(self.btnDeleteClicked)
        self.btnLoadPose.clicked.connect(self.btnLoadPoseClicked)
        self.btnStep.clicked.connect(self.btnStepClicked)
        self.btnCancel.clicked.connect(self.btnCancelClicked)

        self.listWidgetPoses.itemSelectionChanged.connect(self.lstPosesClicked)
        self.listWidgetPoses.itemDoubleClicked.connect(self.qlistDoubleClicked)

        self.jason = Jason()
        self.ok_command = "OK"

        self.offset_position = 100
        self.quat = [0, np.sin(np.deg2rad(45)), 0, np.cos(np.deg2rad(45))]
        self.quat_inv = [0, -np.sin(np.deg2rad(45)), 0, np.cos(np.deg2rad(45))]

        self.pub_marker_array = rospy.Publisher('visualization_marker_array',
                                                MarkerArray,
                                                queue_size=10)

        self.marker_array = MarkerArray()

        self.lines = LinesMarker()
        self.lines.set_size(0.005)
        self.lines.set_color((1, 0, 0, 1))
        self.lines.set_frame('/workobject')
        self.marker_array.markers.append(self.lines.marker)

        self.arrow = ArrowMarker(0.1)
        self.arrow.set_color((0, 0, 1, 1))
        self.arrow.set_frame('/workobject')
        # self.arrow.set_position((0.2, 0.2, 0.2))
        # self.arrow.set_orientation((0, 0, 0, 1))
        self.marker_array.markers.append(self.arrow.marker)

        for id, m in enumerate(self.marker_array.markers):
            m.id = id

        self.tmrRunPath = QtCore.QTimer(self)
        self.tmrRunPath.timeout.connect(self.timeRunPathEvent)
Example #2
0
class QtPath(QtGui.QWidget):
    def __init__(self, parent=None):
        QtGui.QWidget.__init__(self, parent)
        loadUi(os.path.join(path, 'resources', 'path.ui'), self)

        try:
            rospy.wait_for_service('robot_send_command', timeout=5)
            self.send_command = rospy.ServiceProxy('robot_send_command',
                                                   SrvRobotCommand)
        except:
            rospy.loginfo('ERROR connecting to service robot_send_command.')
        #self.pub = rospy.Publisher(
        #    'robot_command_json', MsgRobotCommand, queue_size=10)

        self.btnLoadPath.clicked.connect(self.btnLoadPathClicked)
        icon = QtGui.QIcon.fromTheme('document-open')
        self.btnLoadPath.setIcon(icon)
        self.btnSavePath.clicked.connect(self.btnSavePathClicked)
        icon = QtGui.QIcon.fromTheme('document-save')
        self.btnSavePath.setIcon(icon)
        self.btnRunPath.clicked.connect(self.btnRunPathClicked)
        icon = QtGui.QIcon.fromTheme('media-playback-start')
        self.btnRunPath.setIcon(icon)

        self.btnDelete.clicked.connect(self.btnDeleteClicked)
        self.btnLoadPose.clicked.connect(self.btnLoadPoseClicked)
        self.btnStep.clicked.connect(self.btnStepClicked)
        self.btnCancel.clicked.connect(self.btnCancelClicked)

        self.listWidgetPoses.itemSelectionChanged.connect(self.lstPosesClicked)
        self.listWidgetPoses.itemDoubleClicked.connect(self.qlistDoubleClicked)

        self.jason = Jason()
        self.ok_command = "OK"

        self.offset_position = 100
        self.quat = [0, np.sin(np.deg2rad(45)), 0, np.cos(np.deg2rad(45))]
        self.quat_inv = [0, -np.sin(np.deg2rad(45)), 0, np.cos(np.deg2rad(45))]

        self.pub_marker_array = rospy.Publisher('visualization_marker_array',
                                                MarkerArray,
                                                queue_size=10)

        self.marker_array = MarkerArray()

        self.lines = LinesMarker()
        self.lines.set_size(0.005)
        self.lines.set_color((1, 0, 0, 1))
        self.lines.set_frame('/workobject')
        self.marker_array.markers.append(self.lines.marker)

        self.arrow = ArrowMarker(0.1)
        self.arrow.set_color((0, 0, 1, 1))
        self.arrow.set_frame('/workobject')
        # self.arrow.set_position((0.2, 0.2, 0.2))
        # self.arrow.set_orientation((0, 0, 0, 1))
        self.marker_array.markers.append(self.arrow.marker)

        for id, m in enumerate(self.marker_array.markers):
            m.id = id

        self.tmrRunPath = QtCore.QTimer(self)
        self.tmrRunPath.timeout.connect(self.timeRunPathEvent)

    def insertPose(self, pose):
        (x, y, z), (qx, qy, qz, qw) = pose
        str_pose = '((%.3f, %.3f, %.3f), (%.4f, %.4f, %.4f, %.4f))' % (
            x, y, z, qx, qy, qz, qw)
        #item = QtGui.QListWidgetItem('item_text')
        #self.listWidgetPoses.addItem(item)
        self.listWidgetPoses.addItem(str_pose)
        #self.listWidgetPoses.insertItem(0, '0, 1, 2')

    def insertCommand(self, command, insert=False, position=0):
        if not insert:
            self.listWidgetPoses.addItem(command)
        else:
            self.listWidgetPoses.insertItem(position, command)

    def removeComamnd(self):
        item = self.listWidgetPoses.takeItem(0)
        if item:
            print item.text()
            return item.text()
        else:
            return None

    def btnLoadPathClicked(self):
        self.listWidgetPoses.clear()
        filename = QtGui.QFileDialog.getOpenFileName(
            self, 'Load Path Routine', os.path.join(path, 'routines'),
            'Jason Routine Files (*.jas)')[0]
        print 'Load routine:', filename
        cmds = self.jason.load_commands(filename)
        [self.insertCommand(cmd) for cmd in cmds]
        self.arr = []
        self.getMoveCommands()

    def btnSavePathClicked(self):
        filename = QtGui.QFileDialog.getSaveFileName(
            self, 'Load Path Routine', os.path.join(path, 'routines'),
            'Jason Routine Files (*.jas)')[0]
        n_row = self.listWidgetPoses.count()
        if n_row > 0:
            cmds = [
                str(self.listWidgetPoses.item(row).text())
                for row in range(n_row)
            ]
            self.jason.save_commands(filename, cmds)
        print 'Saved routine:', filename

    def btnRunPathClicked(self):
        """Start-Stop sending commands to robot from the list of commands."""
        if self.tmrRunPath.isActive():
            self.tmrRunPath.stop()
            self.btnRunPath.setText('Run')
        else:
            self.btnRunPath.setText('Stop')
            self.tmrRunPath.start(100)  # time in ms

    def btnDeleteClicked(self):
        row = self.listWidgetPoses.currentRow()
        self.listWidgetPoses.takeItem(row)
        #self.listWidgetPoses.clear()

    def btnLoadPoseClicked(self):
        rob_pose = self.send_command('{"get_pose":1}')
        default_command = '{"move":' + rob_pose.response + '}'
        str_command = QtGui.QInputDialog.getText(self,
                                                 "Load Jason Command",
                                                 "Comamnd:",
                                                 text=default_command)
        row = self.listWidgetPoses.currentRow()
        if len(str_command[0]) > 3:
            self.insertCommand(str_command[0], insert=True, position=row)
        print str_command

    def btnStepClicked(self):
        n_row = self.listWidgetPoses.count()
        if n_row > 0:
            row = self.listWidgetPoses.currentRow()
            if row == -1:
                row = 0
            item_text = self.listWidgetPoses.item(row)
            #self.pub.publish(item_text.text())
            self.sendCommand(item_text.text())
            row += 1
            if row == n_row:
                row = 0
            self.listWidgetPoses.setCurrentRow(row)

    def lstPosesClicked(self):
        row = self.listWidgetPoses.currentRow()
        item_text = self.listWidgetPoses.item(row)
        str_item = item_text.text()
        command = json.loads(str_item)
        if 'move' in command:
            orientation = np.array([
                command["move"][1][1], command["move"][1][2],
                command["move"][1][3], command["move"][1][0]
            ])
            position = np.array(command["move"][0]) * 0.001
            self.arrow.set_new_position(position)
            self.arrow.set_new_orientation(orientation)
            self.arrow.set_color((0, 0, 1, 1))
        else:
            self.arrow.set_color((0, 0, 0, 0))
        self.pub_marker_array.publish(self.marker_array)

    def qlistDoubleClicked(self):
        row = self.listWidgetPoses.currentRow()
        item_text = self.listWidgetPoses.item(row)
        str_command = QtGui.QInputDialog.getText(self,
                                                 "Load Jason Command",
                                                 "Comamnd:",
                                                 text=item_text.text())
        if len(str_command[0]) > 3:
            self.listWidgetPoses.takeItem(row)
            self.insertCommand(str_command[0], insert=True, position=row)

    def btnCancelClicked(self):
        self.sendCommand('{"cancel":1}')

    def getMoveCommands(self):
        n_row = self.listWidgetPoses.count()
        # row = self.listWidgetPoses.currentRow()
        points = []
        for row in range(n_row):
            item_text = self.listWidgetPoses.item(row)
            str_item = item_text.text()
            comando = json.loads(str_item)
            if 'move' in comando:
                point = comando["move"][0]
                points.append(point)
        points = np.array(points) * 0.001
        print points
        self.lines.set_points(points)
        self.pub_marker_array.publish(self.marker_array)

    def sendCommand(self, command):
        rob_response = self.send_command(command)
        print 'Sended command:', command
        print 'Received response:', rob_response
        self.ok_command = rob_response.response

    def timeRunPathEvent(self):
        """Sends a command each time event from the list of commands."""
        n_row = self.listWidgetPoses.count()
        if n_row > 0:
            row = self.listWidgetPoses.currentRow()
            if row == -1:
                row = 0
            item_text = self.listWidgetPoses.item(row)
            #self.pub.publish(item_text.text())
            self.sendCommand(item_text.text())
            if self.ok_command == "OK":
                row += 1
                if row == n_row:
                    row = 0
                    self.btnRunPathClicked()
                self.listWidgetPoses.setCurrentRow(row)
Example #3
0
    def __init__(self, parent=None):
        QtGui.QWidget.__init__(self, parent)
        loadUi(os.path.join(path, 'resources', 'path.ui'), self)

        try:
            rospy.wait_for_service('robot_send_command', timeout=5)
            self.send_command = rospy.ServiceProxy(
                'robot_send_command', SrvRobotCommand)
        except:
            rospy.loginfo('ERROR connecting to service robot_send_command.')
        #self.pub = rospy.Publisher(
        #    import tf'robot_command_json', MsgRobotCommand, queue_size=10)

        self.pub_marker_array = rospy.Publisher(
            'visualization_marker_array', MarkerArray, queue_size=10)

        self.btnLoadPath.clicked.connect(self.btnLoadPathClicked)
        icon = QtGui.QIcon.fromTheme('document-open')
        self.btnLoadPath.setIcon(icon)
        self.btnSavePath.clicked.connect(self.btnSavePathClicked)
        icon = QtGui.QIcon.fromTheme('document-save')
        self.btnSavePath.setIcon(icon)
        self.btnRunPath.clicked.connect(self.btnRunPathClicked)
        icon = QtGui.QIcon.fromTheme('media-playback-start')
        self.btnRunPath.setIcon(icon)
        self.btnRunTest.clicked.connect(self.btnRunTestClicked)
        self.btnRunTest.setIcon(icon)

        self.btnDelete.clicked.connect(self.btnDeleteClicked)
        self.btnLoadPose.clicked.connect(self.btnLoadPoseClicked)
        self.btnStep.clicked.connect(self.btnStepClicked)
        self.btnCancel.clicked.connect(self.btnCancelClicked)

        self.listWidgetPoses.itemSelectionChanged.connect(self.lstPosesClicked)
        self.listWidgetPoses.itemDoubleClicked.connect(self.qlistDoubleClicked)

        self.testing = False
        self.ok_command = "OK"
        self.path_markers = PathMarkers()

        # Parse robot description file
        robot = URDF.from_parameter_server()
        tcp = robot.joint_map['tcp0']
        workobject = robot.joint_map['workobject']

        tool = [tcp.origin.position,
                list(tf.quaternion_from_euler(*tcp.origin.rotation))]
        print 'Tool:', tool
        workobject = [workobject.origin.position,
                      list(tf.quaternion_from_euler(*workobject.origin.rotation))]
        print 'Workobject:', workobject
        if rospy.has_param('/powder'):
            powder = rospy.get_param('/powder')
        else:
            print '/powder param missing, loaded default'
            powder = {'carrier': 5.0, 'shield': 10.0, 'stirrer': 20.0, 'turntable': 4.0}
        print 'Powder:', powder
        if rospy.has_param('/process'):
            process = rospy.get_param('/process')
        else:
            print '/process param missing, loaded default'
            process = {'focus': 0, 'power': 1000, 'speed': 8}
        print 'Process:', process

        self.jason = Jason()
        self.jason.set_tool(tool)
        self.jason.set_workobject(workobject)
        self.jason.set_powder(
            powder['carrier'], powder['stirrer'], powder['turntable'])
        self.jason.set_process(process['speed'], process['power'])

        self.tmrRunPath = QtCore.QTimer(self)
        self.tmrRunPath.timeout.connect(self.timeRunPathEvent)
Example #4
0
class QtPath(QtGui.QWidget):
    def __init__(self, parent=None):
        QtGui.QWidget.__init__(self, parent)
        loadUi(os.path.join(path, 'resources', 'path.ui'), self)

        try:
            rospy.wait_for_service('robot_send_command', timeout=5)
            self.send_command = rospy.ServiceProxy(
                'robot_send_command', SrvRobotCommand)
        except:
            rospy.loginfo('ERROR connecting to service robot_send_command.')
        #self.pub = rospy.Publisher(
        #    import tf'robot_command_json', MsgRobotCommand, queue_size=10)

        self.pub_marker_array = rospy.Publisher(
            'visualization_marker_array', MarkerArray, queue_size=10)

        self.btnLoadPath.clicked.connect(self.btnLoadPathClicked)
        icon = QtGui.QIcon.fromTheme('document-open')
        self.btnLoadPath.setIcon(icon)
        self.btnSavePath.clicked.connect(self.btnSavePathClicked)
        icon = QtGui.QIcon.fromTheme('document-save')
        self.btnSavePath.setIcon(icon)
        self.btnRunPath.clicked.connect(self.btnRunPathClicked)
        icon = QtGui.QIcon.fromTheme('media-playback-start')
        self.btnRunPath.setIcon(icon)
        self.btnRunTest.clicked.connect(self.btnRunTestClicked)
        self.btnRunTest.setIcon(icon)

        self.btnDelete.clicked.connect(self.btnDeleteClicked)
        self.btnLoadPose.clicked.connect(self.btnLoadPoseClicked)
        self.btnStep.clicked.connect(self.btnStepClicked)
        self.btnCancel.clicked.connect(self.btnCancelClicked)

        self.listWidgetPoses.itemSelectionChanged.connect(self.lstPosesClicked)
        self.listWidgetPoses.itemDoubleClicked.connect(self.qlistDoubleClicked)

        self.testing = False
        self.ok_command = "OK"
        self.path_markers = PathMarkers()

        # Parse robot description file
        robot = URDF.from_parameter_server()
        tcp = robot.joint_map['tcp0']
        workobject = robot.joint_map['workobject']

        tool = [tcp.origin.position,
                list(tf.quaternion_from_euler(*tcp.origin.rotation))]
        print 'Tool:', tool
        workobject = [workobject.origin.position,
                      list(tf.quaternion_from_euler(*workobject.origin.rotation))]
        print 'Workobject:', workobject
        if rospy.has_param('/powder'):
            powder = rospy.get_param('/powder')
        else:
            print '/powder param missing, loaded default'
            powder = {'carrier': 5.0, 'shield': 10.0, 'stirrer': 20.0, 'turntable': 4.0}
        print 'Powder:', powder
        if rospy.has_param('/process'):
            process = rospy.get_param('/process')
        else:
            print '/process param missing, loaded default'
            process = {'focus': 0, 'power': 1000, 'speed': 8}
        print 'Process:', process

        self.jason = Jason()
        self.jason.set_tool(tool)
        self.jason.set_workobject(workobject)
        self.jason.set_powder(
            powder['carrier'], powder['stirrer'], powder['turntable'])
        self.jason.set_process(process['speed'], process['power'])

        self.tmrRunPath = QtCore.QTimer(self)
        self.tmrRunPath.timeout.connect(self.timeRunPathEvent)

    def insertPose(self, pose):
        (x, y, z), (qx, qy, qz, qw) = pose
        str_pose = '((%.3f, %.3f, %.3f), (%.4f, %.4f, %.4f, %.4f))' %(x, y, z, qx, qy, qz, qw)
        #item = QtGui.QListWidgetItem('item_text')
        #self.listWidgetPoses.addItem(item)
        self.listWidgetPoses.addItem(str_pose)
        #self.listWidgetPoses.insertItem(0, '0, 1, 2')

    def insertCommand(self, command, insert=False, position=0):
        if not insert:
            self.listWidgetPoses.addItem(command)
        else:
            self.listWidgetPoses.insertItem(position, command)

    def removeComamnd(self):
        item = self.listWidgetPoses.takeItem(0)
        if item:
            print item.text()
            return item.text()
        else:
            return None

    def loadCommands(self, commands):
        self.listWidgetPoses.clear()
        [self.insertCommand(cmd) for cmd in commands]
        self.arr = []
        self.getMoveCommands()

    def btnLoadPathClicked(self):
        filename = QtGui.QFileDialog.getOpenFileName(
            self, 'Load Path Routine', os.path.join(path, 'routines'),
            'Jason Routine Files (*.jas)')[0]
        print 'Load routine:', filename
        commands = self.jason.load_commands(filename)
        self.loadCommands(commands)

    def btnSavePathClicked(self):
        filename = QtGui.QFileDialog.getSaveFileName(
            self, 'Load Path Routine', os.path.join(path, 'routines'),
            'Jason Routine Files (*.jas)')[0]
        n_row = self.listWidgetPoses.count()
        if n_row > 0:
            cmds = [str(self.listWidgetPoses.item(row).text()) for row in range(n_row)]
            self.jason.save_commands(filename, cmds)
        print 'Saved routine:', filename

    def btnRunPathClicked(self):
        """Start-Stop sending commands to robot from the list of commands."""
        if self.tmrRunPath.isActive():
            if not self.testing:
                self.tmrRunPath.stop()
                self.btnRunPath.setText('Run')
                icon = QtGui.QIcon.fromTheme('media-playback-start')
                self.btnRunPath.setIcon(icon)
                self.btnRunTest.setEnabled(True)
        else:
            self.btnRunTest.setEnabled(False)
            self.sendCommand('{"reset_laser":1}')
            self.sendCommand('{"reset_powder":1}')
            self.sendCommand('{"reset_wire":1}')
            self.btnRunPath.setText('Stop')
            icon = QtGui.QIcon.fromTheme('media-playback-stop')
            self.btnRunPath.setIcon(icon)
            self.tmrRunPath.start(100)  # time in ms

    def btnRunTestClicked(self):
        """Start-Stop sending test commands to robot from the list of commands."""
        if self.tmrRunPath.isActive():
            if self.testing:
                self.tmrRunPath.stop()
                self.btnRunTest.setText('Test')
                icon = QtGui.QIcon.fromTheme('media-playback-start')
                self.btnRunTest.setIcon(icon)
                self.testing = False
                self.btnRunPath.setEnabled(True)
        else:
            self.testing = True
            self.btnRunPath.setEnabled(False)
            self.sendCommand('{"reset_laser":1}')
            self.sendCommand('{"reset_powder":1}')
            self.sendCommand('{"reset_wire":1}')
            self.btnRunTest.setText('Stop')
            icon = QtGui.QIcon.fromTheme('media-playback-stop')
            self.btnRunTest.setIcon(icon)
            self.tmrRunPath.start(100)  # time in ms

    def btnDeleteClicked(self):
        row = self.listWidgetPoses.currentRow()
        self.listWidgetPoses.takeItem(row)
        #self.listWidgetPoses.clear()

    def btnLoadPoseClicked(self):
        rob_pose = self.send_command('{"get_pose":1}')
        default_command = '{"move":' + rob_pose.response + '}'
        str_command = QtGui.QInputDialog.getText(
            self, "Load Jason Command", "Comamnd:", text=default_command)
        row = self.listWidgetPoses.currentRow()
        if len(str_command[0]) > 3:
            self.insertCommand(str_command[0], insert=True, position=row)
        print str_command

    def btnStepClicked(self):
        n_row = self.listWidgetPoses.count()
        if n_row > 0:
            row = self.listWidgetPoses.currentRow()
            if row == -1:
                row = 0
            item_text = self.listWidgetPoses.item(row)
            #self.pub.publish(item_text.text())
            self.sendCommand(item_text.text())
            if len(self.ok_command.split()) == 0:
                self.invalid_command("No response command")
                return
            if len(self.ok_command.split()) == 1:
                if self.ok_command.split()[0] == "ERR_COMMAND":
                    self.invalid_command("Check the command name")
                    return
                if self.ok_command.split()[0] == "PARAM_ERROR":
                    self.invalid_command("Check the command parameters")
                    return
                if self.ok_command.split()[0] == "NOK":
                    self.invalid_command("Not a Json comand")
                    return
            if len(self.ok_command.split()) == 3:
                if self.ok_command.split()[2] == "BUFFER_FULL":
                    return
            row += 1
            if row == n_row:
                row = 0
            self.listWidgetPoses.setCurrentRow(row)

    def lstPosesClicked(self):
        row = self.listWidgetPoses.currentRow()
        item_text = self.listWidgetPoses.item(row)
        command = json.loads(item_text.text())
        pose = None
        if 'move' in command:
            orientation = np.array([command["move"][1][1],
                                    command["move"][1][2],
                                    command["move"][1][3],
                                    command["move"][1][0]])
            position = np.array(command["move"][0]) * 0.001
            pose = (position, orientation)
        self.path_markers.set_pose(pose)
        self.pub_marker_array.publish(self.path_markers.marker_array)

    def qlistDoubleClicked(self):
        row = self.listWidgetPoses.currentRow()
        item_text = self.listWidgetPoses.item(row)
        str_command = QtGui.QInputDialog.getText(
            self, "Load Jason Command", "Comamnd:", text=item_text.text())
        if len(str_command[0]) > 3:
            self.listWidgetPoses.takeItem(row)
            self.insertCommand(str_command[0], insert=True, position=row)

    def btnCancelClicked(self):
        self.sendCommand('{"reset_laser":1}')
        self.sendCommand('{"reset_powder":1}')
        self.sendCommand('{"reset_wire":1}')
        self.sendCommand('{"cancel":1}')
        if self.tmrRunPath.isActive():
            self.tmrRunPath.stop()
            icon = QtGui.QIcon.fromTheme('media-playback-start')
            self.btnRunPath.setText('Run')
            self.btnRunTest.setText('Test')
            self.btnRunPath.setIcon(icon)
            self.btnRunTest.setIcon(icon)
            self.testing = False
            self.btnRunTest.setEnabled(True)
            self.btnRunPath.setEnabled(True)

    def getMoveCommands(self):
        n_row = self.listWidgetPoses.count()
        # row = self.listWidgetPoses.currentRow()
        path = []
        for row in range(n_row):
            item_text = self.listWidgetPoses.item(row)
            command = json.loads(item_text.text())
            if 'move' in command:
                path.append(command['move'])
        self.path_markers.set_path(path)
        self.pub_marker_array.publish(self.path_markers.marker_array)

    def sendCommand(self, command):
        if self.testing:
            command = command.lower()
            command = command.replace(' ','')
            if command.lower().find('laser') == 2:
                return
            if command.lower().find('powder') == 2:
                return
            if command.lower().find('move') == 2:
                if command.find(',true') > 0:
                    command = command.replace(',true','')
                if command.find(',false') > 0:
                    command = command.replace(',false','')
        rob_response = self.send_command(command)
        print 'Sended command:', command
        print 'Received response:', rob_response
        self.ok_command = rob_response.response

    def timeRunPathEvent(self):
        """Sends a command each time event from the list of commands."""
        n_row = self.listWidgetPoses.count()
        if n_row > 0:
            row = self.listWidgetPoses.currentRow()
            if row == -1:
                row = 0
            item_text = self.listWidgetPoses.item(row)
            self.sendCommand(item_text.text())
            if len(self.ok_command.split()) == 3:
                if self.ok_command.split()[2] == "BUFFER_FULL":
                    return
            if len(self.ok_command.split()) == 1:
                if self.ok_command.split()[0] == "ERR_COMMAND":
                    if self.testing:
                        self.btnRunTestClicked()
                    else:
                        self.btnRunPathClicked()
                    self.invalid_command("Check the command name")
                    return
                if self.ok_command.split()[0] == "PARAM_ERROR":
                    if self.testing:
                        self.btnRunTestClicked()
                    else:
                        self.btnRunPathClicked()
                    self.invalid_command("Check the command parameters")
                    return
                if self.ok_command.split()[0] == "NOK":
                    if self.testing:
                        self.btnRunTestClicked()
                    else:
                        self.btnRunPathClicked()
                    self.invalid_command("Not a Json comand")
                    return
            if len(self.ok_command.split()) == 0:
                if self.testing:
                    self.btnRunTestClicked()
                else:
                    self.btnRunPathClicked()
                self.invalid_command("No response command")
                return
            row += 1
            if row == n_row:
                row = 0
                if self.testing:
                    self.btnRunTestClicked()
                else:
                    self.btnRunPathClicked()
            self.listWidgetPoses.setCurrentRow(row)

    def invalid_command(self, informative):
        msg = QtGui.QMessageBox()
        msg.setIcon(QtGui.QMessageBox.Warning)
        msg.setText("Invalid command")
        msg.setInformativeText(informative)
        msg.setWindowTitle("Command error")
        #msg.setDetailedText("The details are as follows:")
        msg.setStandardButtons(QtGui.QMessageBox.Ok)
        #msg.buttonClicked.connect(msgbtn)
        retval = msg.exec_()
Example #5
0
    def __init__(self, parent=None):
        QtGui.QWidget.__init__(self, parent)
        loadUi(os.path.join(path, 'resources', 'path.ui'), self)

        try:
            rospy.wait_for_service('robot_send_command', timeout=5)
            self.send_command = rospy.ServiceProxy(
                'robot_send_command', SrvRobotCommand)
        except:
            rospy.loginfo('ERROR connecting to service robot_send_command.')
        #self.pub = rospy.Publisher(
        #    'robot_command_json', MsgRobotCommand, queue_size=10)

        self.btnLoadPath.clicked.connect(self.btnLoadPathClicked)
        icon = QtGui.QIcon.fromTheme('document-open')
        self.btnLoadPath.setIcon(icon)
        self.btnSavePath.clicked.connect(self.btnSavePathClicked)
        icon = QtGui.QIcon.fromTheme('document-save')
        self.btnSavePath.setIcon(icon)
        self.btnRunPath.clicked.connect(self.btnRunPathClicked)
        icon = QtGui.QIcon.fromTheme('media-playback-start')
        self.btnRunPath.setIcon(icon)

        self.btnDelete.clicked.connect(self.btnDeleteClicked)
        self.btnLoadPose.clicked.connect(self.btnLoadPoseClicked)
        self.btnStep.clicked.connect(self.btnStepClicked)
        self.btnCancel.clicked.connect(self.btnCancelClicked)

        self.listWidgetPoses.itemSelectionChanged.connect(self.lstPosesClicked)
        self.listWidgetPoses.itemDoubleClicked.connect(self.qlistDoubleClicked)

        self.jason = Jason()
        self.ok_command = "OK"

        self.offset_position = 100
        self.quat = [0, np.sin(np.deg2rad(45)), 0, np.cos(np.deg2rad(45))]
        self.quat_inv = [0, -np.sin(np.deg2rad(45)), 0, np.cos(np.deg2rad(45))]

        self.pub_marker_array = rospy.Publisher(
            'visualization_marker_array', MarkerArray, queue_size=10)

        self.marker_array = MarkerArray()

        self.lines = LinesMarker()
        self.lines.set_size(0.005)
        self.lines.set_color((1, 0, 0, 1))
        self.lines.set_frame('/workobject')
        self.marker_array.markers.append(self.lines.marker)

        self.arrow = ArrowMarker(0.1)
        self.arrow.set_color((0, 0, 1, 1))
        self.arrow.set_frame('/workobject')
        # self.arrow.set_position((0.2, 0.2, 0.2))
        # self.arrow.set_orientation((0, 0, 0, 1))
        self.marker_array.markers.append(self.arrow.marker)

        for id, m in enumerate(self.marker_array.markers):
            m.id = id

        self.tmrRunPath = QtCore.QTimer(self)
        self.tmrRunPath.timeout.connect(self.timeRunPathEvent)
Example #6
0
class QtPath(QtGui.QWidget):
    def __init__(self, parent=None):
        QtGui.QWidget.__init__(self, parent)
        loadUi(os.path.join(path, 'resources', 'path.ui'), self)

        try:
            rospy.wait_for_service('robot_send_command', timeout=5)
            self.send_command = rospy.ServiceProxy(
                'robot_send_command', SrvRobotCommand)
        except:
            rospy.loginfo('ERROR connecting to service robot_send_command.')
        #self.pub = rospy.Publisher(
        #    'robot_command_json', MsgRobotCommand, queue_size=10)

        self.btnLoadPath.clicked.connect(self.btnLoadPathClicked)
        icon = QtGui.QIcon.fromTheme('document-open')
        self.btnLoadPath.setIcon(icon)
        self.btnSavePath.clicked.connect(self.btnSavePathClicked)
        icon = QtGui.QIcon.fromTheme('document-save')
        self.btnSavePath.setIcon(icon)
        self.btnRunPath.clicked.connect(self.btnRunPathClicked)
        icon = QtGui.QIcon.fromTheme('media-playback-start')
        self.btnRunPath.setIcon(icon)

        self.btnDelete.clicked.connect(self.btnDeleteClicked)
        self.btnLoadPose.clicked.connect(self.btnLoadPoseClicked)
        self.btnStep.clicked.connect(self.btnStepClicked)
        self.btnCancel.clicked.connect(self.btnCancelClicked)

        self.listWidgetPoses.itemSelectionChanged.connect(self.lstPosesClicked)
        self.listWidgetPoses.itemDoubleClicked.connect(self.qlistDoubleClicked)

        self.jason = Jason()
        self.ok_command = "OK"

        self.offset_position = 100
        self.quat = [0, np.sin(np.deg2rad(45)), 0, np.cos(np.deg2rad(45))]
        self.quat_inv = [0, -np.sin(np.deg2rad(45)), 0, np.cos(np.deg2rad(45))]

        self.pub_marker_array = rospy.Publisher(
            'visualization_marker_array', MarkerArray, queue_size=10)

        self.marker_array = MarkerArray()

        self.lines = LinesMarker()
        self.lines.set_size(0.005)
        self.lines.set_color((1, 0, 0, 1))
        self.lines.set_frame('/workobject')
        self.marker_array.markers.append(self.lines.marker)

        self.arrow = ArrowMarker(0.1)
        self.arrow.set_color((0, 0, 1, 1))
        self.arrow.set_frame('/workobject')
        # self.arrow.set_position((0.2, 0.2, 0.2))
        # self.arrow.set_orientation((0, 0, 0, 1))
        self.marker_array.markers.append(self.arrow.marker)

        for id, m in enumerate(self.marker_array.markers):
            m.id = id

        self.tmrRunPath = QtCore.QTimer(self)
        self.tmrRunPath.timeout.connect(self.timeRunPathEvent)

    def insertPose(self, pose):
        (x, y, z), (qx, qy, qz, qw) = pose
        str_pose = '((%.3f, %.3f, %.3f), (%.4f, %.4f, %.4f, %.4f))' %(x, y, z, qx, qy, qz, qw)
        #item = QtGui.QListWidgetItem('item_text')
        #self.listWidgetPoses.addItem(item)
        self.listWidgetPoses.addItem(str_pose)
        #self.listWidgetPoses.insertItem(0, '0, 1, 2')

    def insertCommand(self, command, insert=False, position=0):
        if not insert:
            self.listWidgetPoses.addItem(command)
        else:
            self.listWidgetPoses.insertItem(position, command)

    def removeComamnd(self):
        item = self.listWidgetPoses.takeItem(0)
        if item:
            print item.text()
            return item.text()
        else:
            return None

    def btnLoadPathClicked(self):
        self.listWidgetPoses.clear()
        filename = QtGui.QFileDialog.getOpenFileName(
            self, 'Load Path Routine', os.path.join(path, 'routines'),
            'Jason Routine Files (*.jas)')[0]
        print 'Load routine:', filename
        cmds = self.jason.load_commands(filename)
        [self.insertCommand(cmd) for cmd in cmds]
        self.arr = []
        self.getMoveCommands()

    def btnSavePathClicked(self):
        filename = QtGui.QFileDialog.getSaveFileName(
            self, 'Load Path Routine', os.path.join(path, 'routines'),
            'Jason Routine Files (*.jas)')[0]
        n_row = self.listWidgetPoses.count()
        if n_row > 0:
            cmds = [str(self.listWidgetPoses.item(row).text()) for row in range(n_row)]
            self.jason.save_commands(filename, cmds)
        print 'Saved routine:', filename

    def btnRunPathClicked(self):
        """Start-Stop sending commands to robot from the list of commands."""
        if self.tmrRunPath.isActive():
            self.tmrRunPath.stop()
            self.btnRunPath.setText('Run')
        else:
            self.btnRunPath.setText('Stop')
            self.tmrRunPath.start(100)  # time in ms

    def btnDeleteClicked(self):
        row = self.listWidgetPoses.currentRow()
        self.listWidgetPoses.takeItem(row)
        #self.listWidgetPoses.clear()

    def btnLoadPoseClicked(self):
        rob_pose = self.send_command('{"get_pose":1}')
        default_command = '{"move":' + rob_pose.response + '}'
        str_command = QtGui.QInputDialog.getText(
            self, "Load Jason Command", "Comamnd:", text=default_command)
        row = self.listWidgetPoses.currentRow()
        if len(str_command[0]) > 3:
            self.insertCommand(str_command[0], insert=True, position=row)
        print str_command

    def btnStepClicked(self):
        n_row = self.listWidgetPoses.count()
        if n_row > 0:
            row = self.listWidgetPoses.currentRow()
            if row == -1:
                row = 0
            item_text = self.listWidgetPoses.item(row)
            #self.pub.publish(item_text.text())
            self.sendCommand(item_text.text())
            row += 1
            if row == n_row:
                row = 0
            self.listWidgetPoses.setCurrentRow(row)

    def lstPosesClicked(self):
        row = self.listWidgetPoses.currentRow()
        item_text = self.listWidgetPoses.item(row)
        str_item = item_text.text()
        command = json.loads(str_item)
        if 'move' in command:
            orientation = np.array([command["move"][1][1],
                                    command["move"][1][2],
                                    command["move"][1][3],
                                    command["move"][1][0]])
            position = np.array(command["move"][0]) * 0.001
            self.arrow.set_new_position(position)
            self.arrow.set_new_orientation(orientation)
            self.arrow.set_color((0, 0, 1, 1))
        else:
            self.arrow.set_color((0, 0, 0, 0))
        self.pub_marker_array.publish(self.marker_array)

    def qlistDoubleClicked(self):
        row = self.listWidgetPoses.currentRow()
        item_text = self.listWidgetPoses.item(row)
        str_command = QtGui.QInputDialog.getText(
            self, "Load Jason Command", "Comamnd:", text=item_text.text())
        if len(str_command[0]) > 3:
            self.listWidgetPoses.takeItem(row)
            self.insertCommand(str_command[0], insert=True, position=row)

    def btnCancelClicked(self):
        self.sendCommand('{"cancel":1}')

    def getMoveCommands(self):
        n_row = self.listWidgetPoses.count()
        # row = self.listWidgetPoses.currentRow()
        points = []
        for row in range(n_row):
            item_text = self.listWidgetPoses.item(row)
            str_item = item_text.text()
            comando = json.loads(str_item)
            if 'move' in comando:
                point = comando["move"][0]
                points.append(point)
        points = np.array(points) * 0.001
        print points
        self.lines.set_points(points)
        self.pub_marker_array.publish(self.marker_array)

    def sendCommand(self, command):
        rob_response = self.send_command(command)
        print 'Sended command:', command
        print 'Received response:', rob_response
        self.ok_command = rob_response.response

    def timeRunPathEvent(self):
        """Sends a command each time event from the list of commands."""
        n_row = self.listWidgetPoses.count()
        if n_row > 0:
            row = self.listWidgetPoses.currentRow()
            if row == -1:
                row = 0
            item_text = self.listWidgetPoses.item(row)
            #self.pub.publish(item_text.text())
            self.sendCommand(item_text.text())
            if self.ok_command == "OK":
                row += 1
                if row == n_row:
                    row = 0
                    self.btnRunPathClicked()
                self.listWidgetPoses.setCurrentRow(row)