def _create_found_frame(self): self.found_files_frame = ff_frame = QGroupBox("recursive search") ff_frame.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) self.found_files_vbox_layout = QVBoxLayout(ff_frame) self.found_files_vbox_layout.setSpacing(0) self.found_files_vbox_layout.setContentsMargins(0, 0, 0, 0) self.found_files_list = QListWidget(ff_frame) self.found_files_list.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) self.found_files_list.setFrameStyle(QFrame.StyledPanel) self.found_files_list.itemActivated.connect(self.on_itemActivated) self.found_files_list.setStyleSheet("QListWidget {" "background-color:transparent;" "}" "QListWidget::item {" "background-color:transparent;" "}" "QListWidget::item:selected {" "background-color: darkgray;" "}") self.found_files_vbox_layout.addWidget(self.found_files_list) ff_frame.setCheckable(True) ff_frame.setChecked(False) ff_frame.setFlat(True) self.found_files_list.setVisible(False) return self.found_files_frame
def __init__(self, motion_publisher, robot_config): super(MotionEditorWidget, self).__init__() self.robot_config = robot_config self._motion_publisher = motion_publisher self._motion_data = MotionData(robot_config) self._filter_pattern = '' self._playback_marker = None self._playback_timer = None ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'motion_editor.ui') loadUi(ui_file, self) self.list_widgets = {} for group_type in robot_config.group_types(): list_widget = QListWidget() list_widget.setSortingEnabled(True) list_widget.setDragDropMode(QAbstractItemView.DragOnly) list_widget.setContextMenuPolicy(Qt.CustomContextMenu) list_widget.customContextMenuRequested.connect( lambda pos, _group_type=group_type: self. positions_list_context_menu(_group_type, pos)) list_widget.itemChanged.connect(self.on_list_item_changed) self.position_lists_layout.addWidget(list_widget) self.list_widgets[group_type] = list_widget self._timeline_widget = TimelineWidget() for track_name in self.robot_config.sorted_groups(): track_type = self.robot_config.groups[track_name].group_type track = self._timeline_widget.add_track(track_name, track_type) list_widget = self.list_widgets[track_type] palette = list_widget.palette() palette.setColor(QPalette.Base, track._colors['track']) list_widget.setPalette(palette) self.timeline_group.layout().addWidget(self._timeline_widget) for group_type in robot_config.group_types(): label = QLabel(group_type) label.setAlignment(Qt.AlignHCenter | Qt.AlignVCenter) self.group_label_layout.addWidget(label) self.update_motion_name_combo() self.stop_motion_button.pressed.connect(self.on_motion_stop_pressed)
def __init__(self, context): self.prompt_width = 170 self.input_width = 250 super(SimpleGUI, self).__init__(context) self.setObjectName('SimpleGUI') self._widget = QWidget() self._sound_client = SoundClient() #find relative path for files to load self.local_dir = os.path.dirname(__file__) self.dir = os.path.join(self.local_dir, './lib/rqt_simplegui/') if not os.path.isdir(self.dir): os.makedirs(self.dir) #need to add any additional subfolders as needed if not os.path.isdir(self.dir + 'animations/'): os.makedirs(self.dir + 'animations/') # Creates a subscriber to the ROS topic, having msg type SoundRequest rospy.Subscriber('robotsound', SoundRequest, self.sound_cb) QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10)) self.sound_sig.connect(self.sound_sig_cb) # Code used for saving/ loading arm poses for the robot 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 = [] # Hash tables storing the name of the pose and the # associated positions for that pose, initially empty self.saved_l_poses = {} self.saved_r_poses = {} # Hash tables for storing name of animations and the associated pose list self.saved_animations = {} self.lock = threading.Lock() rospy.Subscriber('joint_states', JointState, self.joint_states_cb) #parsing for animations dir = os.path.dirname(__file__) qWarning(dir) filename = os.path.join(self.dir, 'animations/') ani_path = filename ani_listing = os.listdir(ani_path) for infile in ani_listing: pose_left = [] pose_right = [] left_gripper_states = [] right_gripper_states = [] line_num = 1 for line in fileinput.input(ani_path + infile): if (line_num % 2 == 1): pose = [float(x) for x in line.split()] pose_left.append(pose[:len(pose) / 2]) pose_right.append(pose[len(pose) / 2:]) else: states = line.split() left_gripper_states.append(states[0]) right_gripper_states.append(states[1]) line_num += 1 self.saved_animations[os.path.splitext(infile)[0]] = Quad( pose_left, pose_right, left_gripper_states, right_gripper_states) # 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() # Navigation functionality initialization self.roomNav = RoomNavigator() self._tf_listener = TransformListener() self.animPlay = AnimationPlayer(None, None, None, None) # Detection and pickup functionality self.pap = PickAndPlaceManager(self._tf_listener, self.roomNav, self.animPlay) QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10)) self.joint_sig.connect(self.joint_sig_cb) # Create a large vertical box that is top aligned large_box = QtGui.QVBoxLayout() large_box.setAlignment(QtCore.Qt.AlignTop) large_box.setMargin(0) large_box.addItem(QtGui.QSpacerItem(10, 0)) # Buttons for controlling the head of the robot head_box = QtGui.QHBoxLayout() head_box.addItem(QtGui.QSpacerItem(230, 0)) head_box.addWidget(self.create_pressed_button('Head Up')) head_box.addStretch(1) large_box.addLayout(head_box) button_box = QtGui.QHBoxLayout() button_box.addItem(QtGui.QSpacerItem(80, 0)) button_box.addWidget(self.create_pressed_button('Head Turn Left')) button_box.addWidget(self.create_pressed_button('Head Down')) button_box.addWidget(self.create_pressed_button('Head Turn Right')) button_box.addStretch(1) button_box.setMargin(0) button_box.setSpacing(0) large_box.addLayout(button_box) # Shows what the robot says speech_box = QtGui.QHBoxLayout() self.speech_label = QtGui.QLabel('Robot has not spoken yet') palette = QtGui.QPalette() palette.setColor(QtGui.QPalette.Foreground, QtCore.Qt.blue) self.speech_label.setPalette(palette) # speech_box.addItem(QtGui.QSpacerItem(100, 0)) #speech_box.addWidget(self.speech_label) # large_box.addLayout(speech_box) # Speak button speak_button_box = QtGui.QHBoxLayout() speech_prompt = QtGui.QLabel('Enter Speech Text:') speech_prompt.setFixedWidth(self.prompt_width) speak_button_box.addWidget(speech_prompt) robot_says = QtGui.QLineEdit(self._widget) robot_says.setFixedWidth(self.input_width) robot_says.textChanged[str].connect(self.onChanged) # speak_button_box.addWidget(robot_says) speak_button_box.addWidget(self.create_button('Speak')) speak_button_box.addStretch(1) large_box.addLayout(speak_button_box) large_box.addItem(QtGui.QSpacerItem(0, 50)) # Buttons for arm poses pose_button_box1 = QtGui.QHBoxLayout() pose_button_box1.addItem(QtGui.QSpacerItem(150, 0)) pose_button_box1.addWidget(self.create_button('Relax Left Arm')) pose_button_box1.addWidget(self.create_button('Relax Right Arm')) pose_button_box1.addStretch(1) large_box.addLayout(pose_button_box1) # Buttons for grippers gripper_button_box = QtGui.QHBoxLayout() gripper_button_box.addItem(QtGui.QSpacerItem(150, 0)) gripper_button_box.addWidget(self.create_button('Open Left Gripper')) gripper_button_box.addWidget(self.create_button('Open Right Gripper')) gripper_button_box.addStretch(1) large_box.addLayout(gripper_button_box) large_box.addItem(QtGui.QSpacerItem(0, 25)) # Buttons for animation animation_box = QtGui.QHBoxLayout() play_anim_label = QtGui.QLabel('Select Animation:') play_anim_label.setFixedWidth(self.prompt_width) animation_box.addWidget(play_anim_label) self.saved_animations_list = QtGui.QComboBox(self._widget) animation_box.addWidget(self.saved_animations_list) pose_time_label = QtGui.QLabel('Duration(sec):') pose_time_label.setFixedWidth(100) animation_box.addWidget(pose_time_label) self.pose_time = QtGui.QLineEdit(self._widget) self.pose_time.setFixedWidth(50) self.pose_time.setText('2.0') animation_box.addWidget(self.pose_time) animation_box.addWidget(self.create_button('Play Animation')) animation_box.addStretch(1) large_box.addLayout(animation_box) animation_box2 = QtGui.QHBoxLayout() animation_name_label = QtGui.QLabel('Enter Animation Name:') animation_name_label.setFixedWidth(self.prompt_width) animation_box2.addWidget(animation_name_label) self.animation_name = QtGui.QLineEdit(self._widget) self.animation_name.setFixedWidth(self.input_width) animation_box2.addWidget(self.animation_name) animation_box2.addWidget(self.create_button('Save Animation')) animation_box2.addStretch(1) large_box.addLayout(animation_box2) animation_box3 = QtGui.QHBoxLayout() pose_name_label = QtGui.QLabel('Enter Pose Name:') pose_name_label.setFixedWidth(self.prompt_width) animation_box3.addWidget(pose_name_label) self.pose_name_temp = QtGui.QLineEdit(self._widget) self.pose_name_temp.setFixedWidth(self.input_width) animation_box3.addWidget(self.pose_name_temp) animation_box3.addWidget(self.create_button('Add Current Pose')) animation_box3.addStretch(1) large_box.addLayout(animation_box3) # Playing around with UI stuff play_box = QtGui.QHBoxLayout() pose_sequence_label = QtGui.QLabel('Current Pose Sequence:') pose_sequence_label.setFixedWidth(self.prompt_width) pose_sequence_label.setAlignment(QtCore.Qt.AlignTop) self.list_widget = QListWidget() self.list_widget.setDragDropMode(QAbstractItemView.InternalMove) self.list_widget.setMaximumSize(self.input_width, 200) play_box.addWidget(pose_sequence_label) play_box.addWidget(self.list_widget) play_box.addStretch(1) large_box.addLayout(play_box) large_box.addItem(QtGui.QSpacerItem(0, 50)) # Buttons for first row of base controls first_base_button_box = QtGui.QHBoxLayout() first_base_button_box.addItem(QtGui.QSpacerItem(70, 0)) first_base_button_box.addWidget( self.create_pressed_button('Rotate Left')) first_base_button_box.addWidget(self.create_pressed_button('^')) first_base_button_box.addWidget( self.create_pressed_button('Rotate Right')) first_base_button_box.addStretch(1) large_box.addLayout(first_base_button_box) # Buttons for second row of base controls second_base_button_box = QtGui.QHBoxLayout() second_base_button_box.addItem(QtGui.QSpacerItem(70, 0)) second_base_button_box.addWidget(self.create_pressed_button('<')) second_base_button_box.addWidget(self.create_pressed_button('v')) second_base_button_box.addWidget(self.create_pressed_button('>')) second_base_button_box.addWidget(self.create_button('Move to Bin')) second_base_button_box.addWidget(self.create_button('Object Detect')) second_base_button_box.addWidget(self.create_button('Clean Room')) second_base_button_box.addStretch(1) large_box.addLayout(second_base_button_box) # Animation related items to store intermediate pose co-ordinates to save self.animation_map = {} self.create_state = False self._widget.setObjectName('SimpleGUI') self._widget.setLayout(large_box) context.add_widget(self._widget) # Look straight and down when launched self.head_x = 1.0 self.head_y = 0.0 self.head_z = 0.5 self.head_action(self.head_x, self.head_y, self.head_z) # Set grippers to closed on initialization self.left_gripper_state = '' self.right_gripper_state = '' self.gripper_action('l', 0.0) self.gripper_action('r', 0.0) # Lab 6 self.marker_publisher = rospy.Publisher('visualization_marker', Marker) # Saved states for poses saved_pose_box = QtGui.QHBoxLayout() self.saved_left_poses = QtGui.QLabel('') self.saved_right_poses = QtGui.QLabel('') saved_pose_box.addWidget(self.saved_left_poses) saved_pose_box.addWidget(self.saved_right_poses) large_box.addLayout(saved_pose_box) # Preload the map of animations self.saved_animations_list.addItems(self.saved_animations.keys()) # Move the torso all the way down # self.torso_down(True) # Autonomous navigation stuff ''' self.locations = [Pose(Point(2.48293590546, 3.90075874329, 0.000), Quaternion(0.000, 0.000, -0.783917630973, 0.620864838632)), Pose(Point(3.70106744766, 0.304672241211, 0.000), Quaternion(0.000, 0.000, 0.950186880196, -0.311680754463)), Pose(Point(2.57326722145, 1.51304531097, 0.000), Quaternion(0.000, 0.000, 0.96127194482, -0.275601611212)), Pose(Point(1.28060531616, 1.52380752563, 0.000), Quaternion(0.000, 0.000, 0.946345258806, -0.323157316388)), Pose(Point(2.11048603058, 0.420155525208, 0.000), Quaternion(0.000, 0.000, 0.945222393391, -0.326427062346)), Pose(Point(2.82733058929, -0.739856719971, 0.000), Quaternion(0.000, 0.000, 0.945473998362, -0.325697587373)), Pose(Point(1.29184818268, -1.90485572815, 0.000), Quaternion(0.000, 0.000, 0.942275557345, -0.334838429739)), Pose(Point(0.722846984863, -1.0921459198, 0.000), Quaternion(0.000, 0.000, 0.949330143731, -0.314280572424))] ''' self.locations = [ Pose(Point(2.04748392105, 2.04748010635, 0.000), Quaternion(0.000, 0.000, -0.776968030817, 0.629540053601)), Pose(Point(2.34193611145, 1.43208932877, 0), Quaternion(0, 0, -0.841674385779, 0.539985396398)), Pose(Point(3.43202018738, -0.258297920227, 0.000), Quaternion(0.000, 0.000, 0.996656413122, -0.0817067572629)), Pose(Point(0.931655406952, -1.96435832977, 0.000), Quaternion(0.000, 0.000, 0.691187586713, 0.722675390458)), Pose(Point(-0.369112968445, 0.0330476760864, 0.000), Quaternion(0.000, 0.000, 0.0275340398899, 0.999620866453)) ] self.index = 1 rospy.loginfo("Completed GUI initialization")