def __init__(self, context):
super(HalloweenGUI, self).__init__(context)
self.setObjectName('HalloweenGUI')
self._widget = QWidget()
self._widget.setFixedSize(525, 300)
self.arm_db = ArmDB()
self._tf_listener = TransformListener()
# Action/service/message clients or servers
switch_srv_name = 'pr2_controller_manager/switch_controller'
rospy.loginfo('Waiting for switch controller service...')
rospy.wait_for_service(switch_srv_name)
self.switch_service_client = rospy.ServiceProxy(
switch_srv_name, SwitchController)
self.r_joint_names = [
'r_shoulder_pan_joint', 'r_shoulder_lift_joint',
'r_upper_arm_roll_joint', 'r_elbow_flex_joint',
'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'
]
self.l_joint_names = [
'l_shoulder_pan_joint', 'l_shoulder_lift_joint',
'l_upper_arm_roll_joint', 'l_elbow_flex_joint',
'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'
]
self.all_joint_names = []
self.all_joint_poses = []
self.saved_r_arm_pose = None
self.saved_l_arm_pose = None
self.lock = threading.Lock()
rospy.Subscriber('joint_states', JointState, self.joint_states_cb)
# Create a trajectory action client
r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
self.r_traj_action_client = SimpleActionClient(r_traj_controller_name,
JointTrajectoryAction)
rospy.loginfo(
'Waiting for a response from the trajectory action server for RIGHT arm...'
)
self.r_traj_action_client.wait_for_server()
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(l_traj_controller_name,
JointTrajectoryAction)
rospy.loginfo(
'Waiting for a response from the trajectory action server for LEFT arm...'
)
self.l_traj_action_client.wait_for_server()
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
self.joint_sig.connect(self.joint_sig_cb)
large_box = QtGui.QVBoxLayout()
arm_box = QtGui.QHBoxLayout()
right_arm_box = QtGui.QVBoxLayout()
left_arm_box = QtGui.QVBoxLayout()
left_arm_box.addItem(QtGui.QSpacerItem(50, 50))
right_arm_box.addItem(QtGui.QSpacerItem(50, 50))
right_arm_box.addWidget(self.create_button('Relax right arm'))
right_arm_box.addWidget(self.create_button('Freeze right arm'))
left_arm_box.addWidget(self.create_button('Relax left arm'))
left_arm_box.addWidget(self.create_button('Freeze left arm'))
left_arm_box.addItem(QtGui.QSpacerItem(50, 20))
right_arm_box.addItem(QtGui.QSpacerItem(50, 20))
left_pose_saver = PoseSaver(PoseSaver.LEFT, self)
right_pose_saver = PoseSaver(PoseSaver.RIGHT, self)
left_arm_box.addWidget(
self.create_button("Create left arm pose",
left_pose_saver.create_closure()))
right_arm_box.addWidget(
self.create_button("Create right arm pose",
right_pose_saver.create_closure()))
left_arm_box.addItem(QtGui.QSpacerItem(50, 20))
right_arm_box.addItem(QtGui.QSpacerItem(50, 20))
# Dropdown boxes for saved poses
left_pose_loader = PoseLoader(PoseLoader.LEFT, self)
right_pose_loader = PoseLoader(PoseLoader.RIGHT, self)
self.combo_box_left = left_pose_loader.create_button()
self.combo_box_right = right_pose_loader.create_button()
left_arm_box.addWidget(self.combo_box_left)
right_arm_box.addWidget(self.combo_box_right)
left_pose_option_box = QtGui.QHBoxLayout()
right_pose_option_box = QtGui.QHBoxLayout()
right_pose_option_box.addWidget(self.create_button('Move to pose (R)'))
left_pose_option_box.addWidget(self.create_button('Move to pose (L)'))
# Buttons for deleting poses for left/right arms
left_pose_option_box.addWidget(self.create_button('Delete pose (L)'))
right_pose_option_box.addWidget(self.create_button('Delete pose (R)'))
left_arm_box.addLayout(left_pose_option_box)
right_arm_box.addLayout(right_pose_option_box)
left_arm_box.addItem(QtGui.QSpacerItem(50, 50))
right_arm_box.addItem(QtGui.QSpacerItem(50, 50))
arm_box.addLayout(left_arm_box)
arm_box.addItem(QtGui.QSpacerItem(20, 20))
arm_box.addLayout(right_arm_box)
large_box.addLayout(arm_box)
# Initialize state of saved arm poses for selected dropdowns
self.update_saved_l_arm_pose()
self.update_saved_r_arm_pose()
# Update saved arm pose data on the changing of selected pose
self.combo_box_left.connect(
self.combo_box_left, QtCore.SIGNAL("currentIndexChanged(QString)"),
self.update_saved_l_arm_pose)
self.combo_box_right.connect(
self.combo_box_right,
QtCore.SIGNAL("currentIndexChanged(QString)"),
self.update_saved_r_arm_pose)
self._widget.setObjectName('HalloweenGUI')
self._widget.setLayout(large_box)
context.add_widget(self._widget)
self._widget.setStyleSheet(
"QWidget { image: url(%s) }" %
(str(os.path.dirname(os.path.realpath(__file__))) +
"/../../arm_gui_bg_large.png"))
rospy.loginfo('GUI initialization complete.')
def __init__(self, side, gui):
assert (side == PoseLoader.LEFT or side == PoseLoader.RIGHT)
self.side = side
self.gui = gui
self.arm_db = ArmDB()
roslib.load_manifest('rospy')
roslib.load_manifest('trajectory_msgs')
roslib.load_manifest('control_msgs')
roslib.load_manifest('pr2_mechanism_msgs')
roslib.load_manifest('sensor_msgs')
roslib.load_manifest('actionlib')
from subprocess import call
import threading
import rospy
from qt_gui.plugin import Plugin
from python_qt_binding import QtGui,QtCore
from python_qt_binding.QtGui import QWidget, QFrame
from python_qt_binding.QtGui import QGroupBox
from python_qt_binding.QtCore import QSignalMapper, qWarning, Signal
from trajectory_msgs.msg import JointTrajectoryPoint
from control_msgs.msg import JointTrajectoryGoal
from control_msgs.msg import JointTrajectoryAction
from pr2_mechanism_msgs.srv import SwitchController
from sensor_msgs.msg import JointState
from actionlib import SimpleActionClient
import json
from arm_db import ArmDB
a = ArmDB()
print a.getAllLeftPos()
print a.getAllRightPos()
a.savePos('l', 'my pose', [1,2,3])
b = ArmDB()
print b.getAllLeftPos()
