Go to the documentation of this file.00001
00002
00003 import sys
00004 from PyQt4 import QtCore, QtGui, uic
00005
00006 import roslib
00007 roslib.load_manifest("rospy")
00008 roslib.load_manifest("std_msgs")
00009 roslib.load_manifest("rosparam")
00010 import rospy
00011 import rosparam
00012 from std_msgs.msg import Bool
00013
00014 from arm_pose_move_gui import Ui_Frame as QTArmPoseMoveGUIFrame
00015 from msg import ArmPoseMoveCmd
00016 from arm_pose_move_backend import HEARTBEAT_TOPIC, COMMAND_TOPIC, MONITOR_RATE
00017
00018 class ArmPoseMoveGUIFrame(QtGui.QFrame):
00019 def __init__(self):
00020 super(ArmPoseMoveGUIFrame, self).__init__()
00021 params = rosparam.get_param("/arm_pose_move")
00022 self.pose_dict = params['poses']
00023 self.traj_dict = params['trajectories']
00024
00025 self.heartbeat_pub = rospy.Publisher(HEARTBEAT_TOPIC, Bool)
00026 self.cmd_pub = rospy.Publisher(COMMAND_TOPIC, ArmPoseMoveCmd)
00027
00028 self.init_ui()
00029
00030 def init_ui(self):
00031 self.ui = QTArmPoseMoveGUIFrame()
00032 self.ui.setupUi(self)
00033
00034 self.ui.start_button.clicked.connect(self.start_clk)
00035 self.ui.stop_button.clicked.connect(self.stop_clk)
00036 self.ui.reset_button.clicked.connect(self.reset_clk)
00037 self.traj_name_list, self.pose_name_list = [], []
00038 for traj in self.traj_dict:
00039 self.ui.traj_combo.addItem(self.traj_dict[traj]['text'])
00040 self.traj_name_list.append(traj)
00041 for pose in self.pose_dict:
00042 self.ui.joint_combo.addItem(self.pose_dict[pose]['text'])
00043 self.pose_name_list.append(pose)
00044
00045 self.monitor_timer = QtCore.QTimer(self)
00046 QtCore.QObject.connect(self.monitor_timer, QtCore.SIGNAL("timeout()"), self.monitor_cb)
00047 self.monitor_timer.start(MONITOR_RATE)
00048
00049 def start_clk(self):
00050 msg = ArmPoseMoveCmd()
00051 msg.type = msg.START
00052 if self.ui.traj_button.isChecked():
00053 msg.traj_name = self.traj_name_list[self.ui.traj_combo.currentIndex()]
00054 msg.is_trajectory = True
00055 msg.is_forward = self.ui.forward_button.isChecked()
00056 msg.is_setup = self.ui.setup_box.isChecked()
00057 else:
00058 msg.traj_name = self.pose_name_list[self.ui.joint_combo.currentIndex()]
00059 msg.is_trajectory = False
00060 self.cmd_pub.publish(msg)
00061
00062 def stop_clk(self):
00063 msg = ArmPoseMoveCmd()
00064 msg.type = msg.STOP
00065 self.cmd_pub.publish(msg)
00066
00067 def reset_clk(self):
00068 msg = ArmPoseMoveCmd()
00069 msg.type = msg.RESET
00070 self.cmd_pub.publish(msg)
00071
00072 def monitor_cb(self):
00073 self.heartbeat_pub.publish(Bool())
00074
00075 def main():
00076 rospy.init_node("arm_pose_move_interface")
00077 app = QtGui.QApplication(sys.argv)
00078 frame = ArmPoseMoveGUIFrame()
00079 frame.show()
00080 sys.exit(app.exec_())
00081
00082 if __name__ == "__main__":
00083 main()