示例#1
0
class Application(object):
    def __init__(self):
        self.root = tk.Tk()
        self.root.geometry(WINDOW_SIZE_1)
        self.root.title("ArmMaster - OPENARM control console")
        self.root.resizable(0, 0)
        sysstr = platform.system()
        if sysstr == "Windows":
            self.root.iconbitmap(bitmap='res/logo.ico')
        elif sysstr == "Linux":
            self.root.iconbitmap(bitmap='@res/logo.xbm')

        self.init_gui()

    def init_gui(self):
        self.setting_frame = Frame(self.root)
        
        self.robot = Robot()
        Label(self.setting_frame, text="类型").grid(row=0, column=0, pady=20, sticky=W)
        self.robot_model = ttk.Combobox(self.setting_frame, values=MODEL_LIST)
        self.robot_model.current(0)
        self.robot_model.grid(row=0, column=1, pady=20)

        self.port = StringVar(value="/dev/tty")
        Label(self.setting_frame, text="端口").grid(row=1, column=0, sticky=W)
        self.port_entry = Entry(self.setting_frame, textvariable=self.port)
        self.port_entry.grid(row=1, column=1, sticky=W+E)

        self.baudrate = StringVar(value="9600")
        Label(self.setting_frame, text="波特率").grid(row=2, column=0, pady=5, sticky=W)
        self.baudrate_entry = Entry(self.setting_frame, textvariable=self.baudrate)
        self.baudrate_entry.grid(row=2, column=1, pady=5, sticky=W+E)

        Button(self.setting_frame, text="连接", width=10, command=self.start_main).grid(row=3, column=1, pady=10, sticky=E)
        
        self.setting_frame.pack()

    def start_main(self):
        if self.robot_model.current() == -1:
            return

        model = self.robot_model.get()
        port = self.port.get()
        baudrate = int(self.baudrate.get())

        print("Configuring robot ...")
        if not self.robot.configure(model, port, baudrate):
            return
        
        print("Initializing robot ...")
        if not self.robot.initialize():
            return
        
        sleep(2)
        print("Loading ...")
        self.robot.start()

        config_file = 'config/'+model+'.yaml'
        with open(config_file) as f:
            conf = yaml.safe_load(f)
            f.close()

        self.joints = conf['Joints']
        self.num_joints = len(self.joints)
        self.joint_scales = []
        self.joint_angles = [IntVar() for i in range(self.num_joints)]
        self.current_angles = [IntVar() for i in range(self.num_joints)]
        self.joint_progresses = []
        self.joint_feedback = []

        self.main_frame = Frame(self.root)

    # control container
        self.control_container = Frame(self.main_frame)
        # joint scales
        for i in range(self.num_joints):
            Label(self.control_container, text=self.joints[i]["name"]).grid(row=i, column=0, padx=10, pady=10)
            sc = Scale(self.control_container, orient=HORIZONTAL, sliderlength=10, resolution=1, \
                            from_=self.joints[i]['params']['min_angle'], \
                                to=self.joints[i]['params']['max_angle'], \
                                    variable=self.joint_angles[i])
            sc.grid(row=i, column=1, pady=10)
            sc.set(self.joints[i]['params']['norm'])
            self.joint_scales.append(sc)
        
        # set button
        Button(self.control_container, text="设置", activebackground='grey', command=self.set_joints).grid(row=6, column=0, pady=20, sticky=W+E+N+S)
        Button(self.control_container, text="重置", activebackground='grey', command=self.reset_joints).grid(row=6, column=1, pady=20, sticky=W+E+N+S)
        
        self.control_container.grid(row=0, column=0, rowspan=6, columnspan=2, padx=10, sticky=W+E+N+S)

    # monitor container
        self.monitor_contrainer = Frame(self.main_frame)
        # current angles info
        Label(self.monitor_contrainer, text='当前关节角度: ').grid(row=0, column=1)
        for i in range(self.num_joints):
            pb = ttk.Progressbar(self.monitor_contrainer, orient=HORIZONTAL, maximum=180, length=300, mode='determinate', variable=self.current_angles[i])
            pb.grid(row=i+1, column=1, pady=20)
            self.joint_progresses.append(pb)
            lb = Label(self.monitor_contrainer, width=10, textvariable=self.current_angles[i])
            lb.grid(row=i+1, column=2, pady=20)
            self.joint_feedback.append(lb)

        self.monitor_contrainer.grid(row=0, column=2, rowspan=6, columnspan=3, padx=30, sticky=W+E+N+S)

        self.setting_frame.pack_forget()
        self.root.geometry(WINDOW_SIZE_2)
        self.main_frame.pack()

    # monitor thread
        self.monit_loop = threading.Thread(name='monitor', target=self.monitor)
        self.monit_loop.setDaemon(True)
        self.monit_loop.start()

    def loop(self):
        self.root.mainloop()
    
    def init_robot(self):
        self.robot.configure()
        self.robot.initialize()
        sleep(2)
        self.robot.start()

    def set_joints(self):
        angles = []
        for i in range(self.num_joints):
            angles.append(self.joint_angles[i].get())
        self.robot.set_joint_angles(angles)

    def reset_joints(self):
        angles = []
        for i in range(self.num_joints):
            angle = self.joints[i]['params']['norm']
            self.joint_angles[i].set(angle)
            angles.append(angle)
        self.robot.set_joint_angles(angles) 

    def monitor(self):
        t = now()
        while True:
            if (now()- t)>UPDATE_INTERVAL:
                current_angles = self.robot.get_joint_angles()
                if current_angles:
                    for i in range(self.num_joints):
                        self.current_angles[i].set(current_angles[i])
                    t = now()
示例#2
0
class KeyboardController():
    def __init__(self):
        self.robot = Robot()
        self.current_angles = [0 for i in range(NUM_JOINTS)]
        self.target_angles = [0 for i in range(NUM_JOINTS)]
        self.target_end_effector_state = EndEffectorStateList.IDLE
        self.selected = UD2  # default select JOINT2
        self.shutdown = False

    def setup(self):
        self.__load_params()
        self.__setup_robot()

    def start(self):
        print(msg)

        feedback_thread = threading.Thread(name='feedback',
                                           target=self.__feedback)
        feedback_thread.setDaemon(True)
        feedback_thread.start()

        listen_key_thread = threading.Thread(name='listen_key',
                                             target=self.__listen_key)
        listen_key_thread.setDaemon(True)
        listen_key_thread.start()

        try:
            while not self.shutdown:
                if not operator.eq(self.current_angles, self.target_angles):
                    self.robot.set_joint_angles(self.target_angles)

                if self.target_end_effector_state != EndEffectorStateList.IDLE:
                    self.robot.set_end_effector_state(
                        self.target_end_effector_state)

                self.target_end_effector_state = EndEffectorStateList.IDLE
                self.cmd = ''  # clear command

        except Exception as e:
            print(e)

        finally:
            shutdown()

    def __load_params(self):
        # get parameters from server
        self.param_arm_model = rospy.get_param('~arm_model', 'unspecified')
        self.param_port = rospy.get_param('~port', '/dev/ttyUSB0')
        self.param_baudrate = rospy.get_param('~baudrate', 9600)

        if self.param_arm_model == 'unspecified':
            print('*** Error: Robot arm model not specified.')
            quit()

    def __setup_robot(self):
        # initialize robot
        if not self.robot.configure(name=self.param_arm_model,
                                    port=self.param_port,
                                    baudrate=self.param_baudrate):
            shutdown()

        if not self.robot.initialize():
            shutdown()

        if not self.robot.start():
            shutdown()

        print("Wait for robot to reset.")
        sleep(ROBOT_INIT_TIME)

    def __feedback(self):
        t = now()
        while (1):
            if (now() - t) > UPDATE_INTERVAL:
                current_angles = self.robot.get_joint_angles()
                if current_angles:
                    for i in range(NUM_JOINTS):
                        self.current_angles[i] = current_angles[i]
                t = now()

    def __listen_key(self):
        try:
            while (1):
                key = getKey()
                if key == '\x03':
                    break

                if key in 'wasd':
                    if key == 'a':  # move left
                        if self.target_angles[LR] > self.robot.robot_config(
                        )['Joints'][LR]['params']['min_angle']:
                            self.target_angles[LR] -= 1
                    elif key == 'd':  # move right
                        if self.target_angles[LR] < self.robot.robot_config(
                        )['Joints'][LR]['params']['max_angle']:
                            self.target_angles[LR] += 1
                    elif key == 'w':  # move up
                        if self.target_angles[
                                self.selected] > self.robot.robot_config(
                                )['Joints'][
                                    self.selected]['params']['min_angle']:
                            self.target_angles[self.selected] -= 1
                    elif key == 's':  # move down
                        if self.target_angles[
                                self.selected] < self.robot.robot_config(
                                )['Joints'][
                                    self.selected]['params']['max_angle']:
                            self.target_angles[self.selected] += 1
                elif key == 'u':
                    self.selected = UD1
                    print('**** JOINT 1 selected.')
                elif key == 'i':
                    self.selected = UD2
                    print('**** JOINT 2 selected.')
                elif key == 'j':
                    self.target_end_effector_state = EndEffectorStateList.GRIP
                elif key == 'k':
                    self.target_end_effector_state = EndEffectorStateList.RELEASE
                else:
                    continue

        except Exception as e:
            print(e)

        finally:
            self.shutdown = True