class ArmGUI(Plugin):
joint_sig = Signal(JointState)
def __init__(self, context):
super(ArmGUI, self).__init__(context)
self.setObjectName('ArmGUI')
self._widget = QWidget()
self._widget.setFixedSize(525, 300)
self.arm_db = ArmDB()
# 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('ArmGUI')
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 create_button(self, name, method=None):
if method == None:
method = self.command_cb
btn = QtGui.QPushButton(name, self._widget)
btn.clicked.connect(method)
return btn
def command_cb(self):
button_name = self._widget.sender().text()
if (button_name == 'Relax right arm'):
self.relax_arm('r')
elif (button_name == 'Freeze right arm'):
self.freeze_arm('r')
elif (button_name == 'Relax left arm'):
self.relax_arm('l')
elif (button_name == 'Freeze left arm'):
self.freeze_arm('l')
elif (button_name == 'Move to pose (R)'):
self.move_arm('r')
elif (button_name == 'Move to pose (L)'):
self.move_arm('l')
elif (button_name == 'Delete pose (L)'):
self.delete_pose_left()
elif (button_name == 'Delete pose (R)'):
self.delete_pose_right()
def update_saved_l_arm_pose(self):
selected_index = self.combo_box_left.currentIndex()
if(selected_index == -1):
self.saved_l_arm_pose = None
else:
self.saved_l_arm_pose = self.combo_box_left.itemData(selected_index)
def update_saved_r_arm_pose(self):
selected_index = self.combo_box_right.currentIndex()
if(selected_index == -1):
self.saved_r_arm_pose = None
else:
self.saved_r_arm_pose = self.combo_box_right.itemData(selected_index)
def delete_pose_left(self):
selected_index = self.combo_box_left.currentIndex()
if (selected_index != -1):
self.arm_db.rmPos('l', self.combo_box_left.itemText(selected_index))
self.combo_box_left.removeItem(selected_index)
def delete_pose_right(self):
selected_index = self.combo_box_right.currentIndex()
if (selected_index != -1):
self.arm_db.rmPos('r', self.combo_box_right.itemText(selected_index))
self.combo_box_right.removeItem(selected_index)
def move_arm(self, side_prefix):
if (side_prefix == 'r'):
if self.saved_r_arm_pose is None:
rospy.logerr('Target pose for right arm is None, cannot move.')
else:
self.freeze_arm('r')
self.move_to_joints('r', self.saved_r_arm_pose, 2.0)
else:
if self.saved_l_arm_pose is None:
rospy.logerr('Target pose for left arm is None, cannot move.')
else:
self.freeze_arm('l')
self.move_to_joints('l', self.saved_l_arm_pose, 2.0)
pass
def move_to_joints(self, side_prefix, positions, time_to_joint):
'''Moves the arm to the desired joints'''
velocities = [0] * len(positions)
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1))
traj_goal.trajectory.points.append(JointTrajectoryPoint(positions=positions,
velocities=velocities, time_from_start=rospy.Duration(time_to_joint)))
if (side_prefix == 'r'):
traj_goal.trajectory.joint_names = self.r_joint_names
self.r_traj_action_client.send_goal(traj_goal)
else:
traj_goal.trajectory.joint_names = self.l_joint_names
self.l_traj_action_client.send_goal(traj_goal)
def relax_arm(self, side_prefix):
controller_name = side_prefix + '_arm_controller'
start_controllers = []
stop_controllers = [controller_name]
self.set_arm_mode(start_controllers, stop_controllers)
def freeze_arm(self, side_prefix):
controller_name = side_prefix + '_arm_controller'
start_controllers = [controller_name]
stop_controllers = []
self.set_arm_mode(start_controllers, stop_controllers)
def set_arm_mode(self, start_controllers, stop_controllers):
try:
self.switch_service_client(start_controllers, stop_controllers, 1)
except rospy.ServiceException:
rospy.logerr('Could not change arm mode.')
def joint_states_cb(self, msg):
'''Callback function that saves the joint positions when a
joint_states message is received'''
self.lock.acquire()
self.all_joint_names = msg.name
self.all_joint_poses = msg.position
self.joint_sig.emit(msg)
self.lock.release()
def joint_sig_cb(self, msg):
pass
def get_joint_state(self, side_prefix):
'''Returns position for arm joints on the requested side (r/l)'''
if side_prefix == 'r':
joint_names = self.r_joint_names
else:
joint_names = self.l_joint_names
if self.all_joint_names == []:
rospy.logerr("No robot_state messages received yet!\n")
return None
positions = []
self.lock.acquire()
for joint_name in joint_names:
if joint_name in self.all_joint_names:
index = self.all_joint_names.index(joint_name)
position = self.all_joint_poses[index]
positions.append(position)
else:
rospy.logerr("Joint %s not found!", joint_name)
self.lock.release()
return None
self.lock.release()
return positions
def shutdown_plugin(self):
# TODO unregister all publishers here
# Leave both arm controllers on
start_controllers = ['r_arm_controller', 'l_arm_controller']
stop_controllers = []
self.set_arm_mode(start_controllers, stop_controllers)
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
class PbDGUI(Plugin):
exp_state_sig = Signal(ExperimentState)
def __init__(self, context):
super(PbDGUI, self).__init__(context)
self.setObjectName('PbDGUI')
self._widget = QWidget()
self.speech_cmd_publisher = rospy.Publisher('recognized_command',
Command)
self.gui_cmd_publisher = rospy.Publisher('gui_command', GuiCommand)
rospy.Subscriber('experiment_state', ExperimentState,
self.exp_state_cb)
rospy.Subscriber('robotsound', SoundRequest, self.robotSoundReceived)
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
self.exp_state_sig.connect(self.update_state)
self.commands = dict()
self.commands[Command.TEST_MICROPHONE] = 'Test microphone'
self.commands[Command.NEW_DEMONSTRATION] = 'New demonstration'
self.commands[Command.START_RECORDING] = 'Start recording'
self.commands[Command.STOP_RECORDING] = 'Stop recording'
self.commands[Command.REPLAY_DEMONSTRATION] = 'Replay demonstration'
self.commands[Command.DETECT_SURFACE] = 'Detect surface'
self.commands[Command.TAKE_TOOL] = 'Take tool'
self.commands[Command.RELEASE_TOOL] = 'Release tool'
self.commands[Command.SAVE_ARM_POSE] = 'Save arm pose'
self.currentAction = -1
self.currentStep = -1
allWidgetsBox = QtGui.QVBoxLayout()
actionBox = QGroupBox('Demonstrations', self._widget)
self.actionGrid = QtGui.QGridLayout()
self.actionGrid.setHorizontalSpacing(0)
for i in range(6):
self.actionGrid.addItem(QtGui.QSpacerItem(90, 90), 0, i,
QtCore.Qt.AlignCenter)
self.actionGrid.setColumnStretch(i, 0)
self.actionIcons = dict()
actionBoxLayout = QtGui.QHBoxLayout()
actionBoxLayout.addLayout(self.actionGrid)
actionBox.setLayout(actionBoxLayout)
actionButtonGrid = QtGui.QHBoxLayout()
actionButtonGrid.addWidget(
self.create_button(Command.NEW_DEMONSTRATION))
self.stepsBox = QGroupBox('No demonstrations', self._widget)
self.stepsGrid = QtGui.QGridLayout()
self.l_model = QtGui.QStandardItemModel(self)
self.l_view = self._create_table_view(self.l_model,
self.l_row_clicked_cb)
self.r_model = QtGui.QStandardItemModel(self)
self.r_view = self._create_table_view(self.r_model,
self.r_row_clicked_cb)
self.stepsGrid.addItem(QtGui.QSpacerItem(280, 10), 0, 0, 2, 3)
self.stepsGrid.addItem(QtGui.QSpacerItem(10, 10), 0, 1, 2, 3)
self.stepsGrid.addItem(QtGui.QSpacerItem(280, 10), 0, 2, 2, 3)
self.stepsGrid.addWidget(QtGui.QLabel('Left Arm'), 0, 0)
self.stepsGrid.addWidget(QtGui.QLabel('Right Arm'), 0, 2)
self.stepsGrid.addWidget(self.l_view, 1, 0)
self.stepsGrid.addWidget(self.r_view, 1, 2)
stepsBoxLayout = QtGui.QHBoxLayout()
stepsBoxLayout.addLayout(self.stepsGrid)
self.stepsBox.setLayout(stepsBoxLayout)
stepsButtonGrid = QtGui.QHBoxLayout()
stepsButtonGrid.addWidget(
self.create_button(Command.REPLAY_DEMONSTRATION))
motionButtonGrid = QtGui.QHBoxLayout()
motionButtonGrid.addWidget(self.create_button(Command.START_RECORDING))
motionButtonGrid.addWidget(self.create_button(Command.STOP_RECORDING))
misc_grid = QtGui.QHBoxLayout()
misc_grid.addWidget(self.create_button(Command.TEST_MICROPHONE))
misc_grid.addWidget(self.create_button(Command.DETECT_SURFACE))
misc_grid.addStretch(1)
misc_grid2 = QtGui.QHBoxLayout()
misc_grid2.addWidget(self.create_button(Command.TAKE_TOOL))
misc_grid2.addWidget(self.create_button(Command.RELEASE_TOOL))
misc_grid2.addStretch(1)
misc_grid3 = QtGui.QHBoxLayout()
misc_grid3.addWidget(self.create_button(Command.SAVE_ARM_POSE))
misc_grid3.addStretch(1)
speechGroupBox = QGroupBox('Robot Speech', self._widget)
speechGroupBox.setObjectName('RobotSpeechGroup')
speechBox = QtGui.QHBoxLayout()
self.speechLabel = QtGui.QLabel('Robot has not spoken yet')
palette = QtGui.QPalette()
palette.setColor(QtGui.QPalette.Foreground, QtCore.Qt.blue)
self.speechLabel.setPalette(palette)
speechBox.addWidget(self.speechLabel)
speechGroupBox.setLayout(speechBox)
allWidgetsBox.addWidget(actionBox)
allWidgetsBox.addLayout(actionButtonGrid)
allWidgetsBox.addWidget(self.stepsBox)
allWidgetsBox.addLayout(motionButtonGrid)
allWidgetsBox.addLayout(stepsButtonGrid)
allWidgetsBox.addItem(QtGui.QSpacerItem(100, 20))
allWidgetsBox.addLayout(misc_grid)
allWidgetsBox.addItem(QtGui.QSpacerItem(100, 20))
allWidgetsBox.addLayout(misc_grid2)
allWidgetsBox.addItem(QtGui.QSpacerItem(100, 40))
allWidgetsBox.addLayout(misc_grid3)
allWidgetsBox.addItem(QtGui.QSpacerItem(100, 20))
allWidgetsBox.addWidget(speechGroupBox)
allWidgetsBox.addStretch(1)
# Fix layout and add main widget to the user interface
QtGui.QApplication.setStyle(QtGui.QStyleFactory.create('plastique'))
vAllBox = QtGui.QVBoxLayout()
vAllBox.addLayout(allWidgetsBox)
vAllBox.addStretch(1)
hAllBox = QtGui.QHBoxLayout()
hAllBox.addLayout(vAllBox)
hAllBox.addStretch(1)
self._widget.setObjectName('PbDGUI')
self._widget.setLayout(hAllBox)
context.add_widget(self._widget)
rospy.loginfo('Will wait for the experiment state service...')
rospy.wait_for_service('get_experiment_state')
exp_state_srv = rospy.ServiceProxy('get_experiment_state',
GetExperimentState)
rospy.loginfo('Got response from the experiment state service...')
response = exp_state_srv()
self.update_state(response.state)
def _create_table_view(self, model, row_click_cb):
proxy = QtGui.QSortFilterProxyModel(self)
proxy.setSourceModel(model)
view = QtGui.QTableView(self._widget)
verticalHeader = view.verticalHeader()
verticalHeader.sectionClicked.connect(row_click_cb)
view.setModel(proxy)
view.setMaximumWidth(250)
view.setSortingEnabled(False)
view.setCornerButtonEnabled(False)
return view
def get_uid(self, arm_index, index):
'''Returns a unique id of the marker'''
return (2 * (index + 1) + arm_index)
def get_arm_and_index(self, uid):
'''Returns a unique id of the marker'''
arm_index = uid % 2
index = (uid - arm_index) / 2
return (arm_index, (index - 1))
def r_row_clicked_cb(self, logicalIndex):
self.step_pressed(self.get_uid(0, logicalIndex))
def l_row_clicked_cb(self, logicalIndex):
self.step_pressed(self.get_uid(1, logicalIndex))
def create_button(self, command):
btn = QtGui.QPushButton(self.commands[command], self._widget)
btn.clicked.connect(self.command_cb)
return btn
def update_state(self, state):
qWarning('Received new state')
n_actions = len(self.actionIcons.keys())
if n_actions < state.n_actions:
for i in range(n_actions, state.n_actions):
self.new_action()
if (self.currentAction != (state.i_current_action - 1)):
self.delete_all_steps()
self.action_pressed(state.i_current_action - 1, False)
n_steps = self.n_steps()
if (n_steps < state.n_steps):
for i in range(n_steps, state.n_steps):
self.save_pose(frameType=ord(state.frame_types[i]))
elif (n_steps > state.n_steps):
n_to_remove = n_steps - state.n_steps
self.r_model.invisibleRootItem().removeRows(
state.n_steps, n_to_remove)
self.l_model.invisibleRootItem().removeRows(
state.n_steps, n_to_remove)
## TODO: DEAL with the following arrays!!!
state.r_gripper_states
state.l_gripper_states
state.r_ref_frames
state.l_ref_frames
state.objects
if (self.currentStep != state.i_current_step):
if (self.n_steps() > 0):
self.currentStep = state.i_current_step
arm_index, index = self.get_arm_and_index(self.currentStep)
if (arm_index == 0):
self.r_view.selectRow(index)
else:
self.l_view.selectRow(index)
def get_frame_type(self, fr_type):
if (fr_type > 1):
rospy.logwarn(
"Invalid frame type @ save_pose -> get_frame_type: " +
str(fr_type))
return ["Go to pose", "Maneuver"][fr_type]
def save_pose(self, actionIndex=None, frameType=0):
nColumns = 9
if actionIndex is None:
actionIndex = self.currentAction
stepIndex = self.n_steps(actionIndex)
r_step = [
QtGui.QStandardItem('Step' + str(stepIndex + 1)),
QtGui.QStandardItem(self.get_frame_type(frameType)),
QtGui.QStandardItem('Absolute')
]
l_step = [
QtGui.QStandardItem('Step' + str(stepIndex + 1)),
QtGui.QStandardItem(self.get_frame_type(frameType)),
QtGui.QStandardItem('Absolute')
]
self.r_model.invisibleRootItem().appendRow(r_step)
self.l_model.invisibleRootItem().appendRow(l_step)
self.update_table_view()
self.currentStep = stepIndex
def update_table_view(self):
self.l_view.setColumnWidth(0, 50)
self.l_view.setColumnWidth(1, 100)
self.l_view.setColumnWidth(2, 70)
self.r_view.setColumnWidth(0, 50)
self.r_view.setColumnWidth(1, 100)
self.r_view.setColumnWidth(2, 70)
def n_steps(self, actionIndex=None):
return self.l_model.invisibleRootItem().rowCount()
def delete_all_steps(self, actionIndex=None):
if actionIndex is None:
actionIndex = self.currentAction
n_steps = self.n_steps()
if (n_steps > 0):
self.l_model.invisibleRootItem().removeRows(0, n_steps)
self.r_model.invisibleRootItem().removeRows(0, n_steps)
def n_actions(self):
return len(self.actionIcons.keys())
def new_action(self):
nColumns = 6
actionIndex = self.n_actions()
for key in self.actionIcons.keys():
self.actionIcons[key].selected = False
self.actionIcons[key].updateView()
actIcon = ActionIcon(self._widget, actionIndex, self.action_pressed)
self.actionGrid.addLayout(actIcon, int(actionIndex / nColumns),
actionIndex % nColumns)
self.actionIcons[actionIndex] = actIcon
def step_pressed(self, step_index):
gui_cmd = GuiCommand(GuiCommand.SELECT_ACTION_STEP, step_index)
self.gui_cmd_publisher.publish(gui_cmd)
def action_pressed(self, actionIndex, isPublish=True):
for i in range(len(self.actionIcons.keys())):
key = self.actionIcons.keys()[i]
if key == actionIndex:
self.actionIcons[key].selected = True
else:
self.actionIcons[key].selected = False
self.actionIcons[key].updateView()
self.currentAction = actionIndex
self.stepsBox.setTitle('Steps for Action ' +
str(self.currentAction + 1))
if isPublish:
gui_cmd = GuiCommand(GuiCommand.SWITCH_TO_ACTION,
(actionIndex + 1))
self.gui_cmd_publisher.publish(gui_cmd)
def command_cb(self):
clickedButtonName = self._widget.sender().text()
for key in self.commands.keys():
if (self.commands[key] == clickedButtonName):
qWarning('Sending speech command: ' + key)
command = Command()
command.command = key
self.speech_cmd_publisher.publish(command)
def robotSoundReceived(self, soundReq):
if (soundReq.command == SoundRequest.SAY):
qWarning('Robot said: ' + soundReq.arg)
self.speechLabel.setText('Robot sound: ' + soundReq.arg)
def exp_state_cb(self, state):
qWarning('Received new experiment state.')
self.exp_state_sig.emit(state)
def shutdown_plugin(self):
# TODO unregister all publishers here
self.speech_cmd_publisher.unregister()
self.gui_cmd_publisher.unregister()
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
class HalloweenGUI(Plugin):
joint_sig = Signal(JointState)
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 create_button(self, name, method=None):
if method == None:
method = self.command_cb
btn = QtGui.QPushButton(name, self._widget)
btn.clicked.connect(method)
return btn
def command_cb(self):
button_name = self._widget.sender().text()
if (button_name == 'Relax right arm'):
self.relax_arm('r')
elif (button_name == 'Freeze right arm'):
self.freeze_arm('r')
elif (button_name == 'Relax left arm'):
self.relax_arm('l')
elif (button_name == 'Freeze left arm'):
self.freeze_arm('l')
elif (button_name == 'Move to pose (R)'):
self.move_arm('r')
elif (button_name == 'Move to pose (L)'):
self.move_arm('l')
elif (button_name == 'Delete pose (L)'):
self.delete_pose_left()
elif (button_name == 'Delete pose (R)'):
self.delete_pose_right()
def update_saved_l_arm_pose(self):
selected_index = self.combo_box_left.currentIndex()
if (selected_index == -1):
self.saved_l_arm_pose = None
else:
self.saved_l_arm_pose = self.combo_box_left.itemData(
selected_index)
def update_saved_r_arm_pose(self):
selected_index = self.combo_box_right.currentIndex()
if (selected_index == -1):
self.saved_r_arm_pose = None
else:
self.saved_r_arm_pose = self.combo_box_right.itemData(
selected_index)
def delete_pose_left(self):
selected_index = self.combo_box_left.currentIndex()
if (selected_index != -1):
self.arm_db.rmPos('l',
self.combo_box_left.itemText(selected_index))
self.combo_box_left.removeItem(selected_index)
def delete_pose_right(self):
selected_index = self.combo_box_right.currentIndex()
if (selected_index != -1):
self.arm_db.rmPos('r',
self.combo_box_right.itemText(selected_index))
self.combo_box_right.removeItem(selected_index)
def pose_distance(self, source, dest):
dist = 0
dist += (source.position.x - dest.position.x) ^ 2
dist += (source.position.y - dest.position.y) ^ 2
dist += (source.position.z - dest.position.z) ^ 2
return dist
def move_arm(self, side_prefix):
# forward kinematics
if (side_prefix == 'r'):
if self.saved_r_arm_pose is None:
rospy.logerr('Target pose for right arm is None, cannot move.')
else:
self.freeze_arm(side_prefix)
time_to_joints = 2.0
self.move_to_joints(side_prefix, self.saved_r_arm_pose,
time_to_joints)
else: # side_prefix == 'l'
if self.saved_l_arm_pose is None:
rospy.logerr('Target pose for left arm is None, cannot move.')
else:
self.freeze_arm(side_prefix)
time_to_joints = 2.0
self.move_to_joints(side_prefix, self.saved_l_arm_pose,
time_to_joints)
pass
def move_to_joints(self, side_prefix, positions, time_to_joint):
'''Moves the arm to the desired joints'''
velocities = [0] * len(positions)
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory.header.stamp = (rospy.Time.now() +
rospy.Duration(0.1))
traj_goal.trajectory.points.append(
JointTrajectoryPoint(
positions=positions,
velocities=velocities,
time_from_start=rospy.Duration(time_to_joint)))
if (side_prefix == 'r'):
traj_goal.trajectory.joint_names = self.r_joint_names
self.r_traj_action_client.send_goal(traj_goal)
else:
traj_goal.trajectory.joint_names = self.l_joint_names
self.l_traj_action_client.send_goal(traj_goal)
def relax_arm(self, side_prefix):
controller_name = side_prefix + '_arm_controller'
start_controllers = []
stop_controllers = [controller_name]
self.set_arm_mode(start_controllers, stop_controllers)
def freeze_arm(self, side_prefix):
controller_name = side_prefix + '_arm_controller'
start_controllers = [controller_name]
stop_controllers = []
self.set_arm_mode(start_controllers, stop_controllers)
def set_arm_mode(self, start_controllers, stop_controllers):
try:
self.switch_service_client(start_controllers, stop_controllers, 1)
except rospy.ServiceException:
rospy.logerr('Could not change arm mode.')
def joint_states_cb(self, msg):
'''Callback function that saves the joint positions when a
joint_states message is received'''
self.lock.acquire()
self.all_joint_names = msg.name
self.all_joint_poses = msg.position
self.joint_sig.emit(msg)
self.lock.release()
def joint_sig_cb(self, msg):
pass
def get_joint_state(self, side_prefix):
'''Returns position for arm joints on the requested side (r/l)'''
if side_prefix == 'r':
joint_names = self.r_joint_names
else:
joint_names = self.l_joint_names
if self.all_joint_names == []:
rospy.logerr("No robot_state messages received yet!\n")
return None
positions = []
self.lock.acquire()
for joint_name in joint_names:
if joint_name in self.all_joint_names:
index = self.all_joint_names.index(joint_name)
position = self.all_joint_poses[index]
positions.append(position)
else:
rospy.logerr("Joint %s not found!", joint_name)
self.lock.release()
return None
self.lock.release()
return positions
def shutdown_plugin(self):
# TODO unregister all publishers here
# Leave both arm controllers on
start_controllers = ['r_arm_controller', 'l_arm_controller']
stop_controllers = []
self.set_arm_mode(start_controllers, stop_controllers)
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
class SimpleGUI(Plugin):
sound_sig = Signal(SoundRequest)
def __init__(self, context):
super(SimpleGUI, self).__init__(context)
self.setObjectName('SimpleGUI')
self._widget = QWidget()
self._widget.setFixedSize(600, 600)
self._sound_client = SoundClient()
rospy.Subscriber('robotsound', SoundRequest, self.sound_cb)
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
self.sound_sig.connect(self.sound_sig_cb)
large_box = QtGui.QVBoxLayout()
# Textbox to enter words for PR2 to say
sound_textbox = QtGui.QLineEdit("Squirtle Squirtle") # Default Text
sound_textbox.setFixedWidth(450)
self.marker_publisher = rospy.Publisher('visualization_marker', Marker)
# Set a handler on the textbox to retrieve the text when button clicked
self.sound_textbox = sound_textbox
button_box = QtGui.QHBoxLayout()
button_box.addItem(QtGui.QSpacerItem(15,20))
button_box.addWidget(self.create_button('Speak', self.command_cb))
button_box.addWidget(sound_textbox)
button_box.addStretch(1)
large_box.addLayout(button_box)
speech_box = QtGui.QHBoxLayout()
speech_box.addItem(QtGui.QSpacerItem(15, 20))
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.addWidget(self.speech_label)
large_box.addLayout(speech_box)
large_box.addStretch(1)
#large_box.addItem(QtGui.QSpacerItem(50,20))
# Buttons to move the PR2's head
up_head = Head(Head.UP, self)
head_box = QtGui.QVBoxLayout()
up_head_box = QtGui.QHBoxLayout()
up_head_button = self.create_button('^', up_head.create_closure())
down_head = Head(Head.DOWN, self)
down_head_box = QtGui.QHBoxLayout()
down_head_button = self.create_button('v', down_head.create_closure())
right_head = Head(Head.RIGHT, self)
right_head_button = self.create_button('>', right_head.create_closure())
left_head = Head(Head.LEFT, self)
left_head_button = self.create_button('<', left_head.create_closure())
left_right_head_box = QtGui.QHBoxLayout()
up_head_box.addItem(QtGui.QSpacerItem(235,20))
up_head_box.addWidget(up_head_button)
up_head_box.addItem(QtGui.QSpacerItem(275,20))
left_right_head_box.addItem(QtGui.QSpacerItem(160,20))
left_right_head_box.addWidget(left_head_button)
left_right_head_box.addItem(QtGui.QSpacerItem(60,20))
left_right_head_box.addWidget(right_head_button)
left_right_head_box.addItem(QtGui.QSpacerItem(225,20))
down_head_box.addItem(QtGui.QSpacerItem(235,20))
down_head_box.addWidget(down_head_button)
down_head_box.addItem(QtGui.QSpacerItem(275,20))
head_box.addLayout(up_head_box)
head_box.addLayout(left_right_head_box)
head_box.addLayout(down_head_box)
large_box.addLayout(head_box)
# Buttons to move the grippers
gripper = Gripper(Gripper.RIGHT, Gripper.OPEN, self)
right_gripper = self.create_button('Right gripper',
gripper.create_closure())
gripper = Gripper(Gripper.LEFT, Gripper.OPEN, self)
left_gripper = self.create_button('Left gripper', gripper.create_closure())
large_box.addItem(QtGui.QSpacerItem(100,250))
gripper_box = QtGui.QHBoxLayout()
gripper_box.addItem(QtGui.QSpacerItem(75,20))
gripper_box.addWidget(left_gripper)
gripper_box.addItem(QtGui.QSpacerItem(450,20))
gripper_box.addWidget(right_gripper)
gripper_box.addItem(QtGui.QSpacerItem(75,20))
large_box.addLayout(gripper_box)
# Buttons to move the base
base_box = QtGui.QVBoxLayout()
large_box.addItem(QtGui.QSpacerItem(100,100))
forward_base_box = QtGui.QHBoxLayout()
forward_base = Base(Base.FORWARD, self)
forward_base_button = self.create_button('move forward', forward_base.create_closure())
forward_base_box.addItem(QtGui.QSpacerItem(400,20))
forward_base_box.addWidget(forward_base_button)
forward_base_box.addItem(QtGui.QSpacerItem(400,20))
base_box.addLayout(forward_base_box)
left_right_base_box = QtGui.QHBoxLayout()
left_base= Base(Base.LEFT, self)
left_base_button = self.create_button('move left',
left_base.create_closure())
right_base= Base(Base.RIGHT, self)
right_base_button= self.create_button('move right',
right_base.create_closure())
left_right_base_box.addItem(QtGui.QSpacerItem(300,20))
left_right_base_box.addWidget(left_base_button)
left_right_base_box.addItem(QtGui.QSpacerItem(50,20))
left_right_base_box.addWidget(right_base_button)
left_right_base_box.addItem(QtGui.QSpacerItem(300,20))
base_box.addLayout(left_right_base_box)
backward_base_box = QtGui.QHBoxLayout()
backward_base= Base(Base.BACKWARD, self)
backward_base_button = self.create_button('move backward',
backward_base.create_closure())
backward_base_box.addItem(QtGui.QSpacerItem(400,20))
backward_base_box.addWidget(backward_base_button)
backward_base_box.addItem(QtGui.QSpacerItem(400,20))
base_box.addLayout(backward_base_box)
large_box.addLayout(base_box)
turn_base_box = QtGui.QHBoxLayout()
counter_base= Base(Base.COUNTER, self)
counter_base_button = self.create_button('\\\n -->',
counter_base.create_closure())
clockwise_base= Base(Base.CLOCKWISE, self)
clockwise_base_button = self.create_button(' /\n<--',
clockwise_base.create_closure())
turn_base_box.addItem(QtGui.QSpacerItem(75,20))
turn_base_box.addWidget(counter_base_button)
turn_base_box.addItem(QtGui.QSpacerItem(225,20))
turn_base_box.addWidget(clockwise_base_button)
turn_base_box.addItem(QtGui.QSpacerItem(100,20))
large_box.addLayout(turn_base_box)
self._widget.setObjectName('SimpleGUI')
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__))) +
"/../../rosie_background.jpg"))
def show_text_in_rviz(self, text):
marker = Marker(type=Marker.TEXT_VIEW_FACING, id=0,
lifetime=rospy.Duration(1.5),
pose=Pose(Point(0.5, 0.5, 1.45), Quaternion(0, 0, 0, 1)),
scale=Vector3(0.06, 0.06, 0.06),
header=Header(frame_id='base_link'),
color=ColorRGBA(0.0, 1.0, 0.0, 0.8), text=text)
self.marker_publisher.publish(marker)
def sound_cb(self, sound_request):
qWarning('Received sound.')
self.sound_sig.emit(sound_request)
def create_button(self, name, method):
btn = QtGui.QPushButton(name, self._widget)
btn.clicked.connect(method)
btn.setAutoRepeat(True)
return btn
def sound_sig_cb(self, sound_request):
qWarning('Received sound signal.')
#if (sound_request.command == SoundRequest.SAY):
qWarning('Robot said: ' + sound_request.arg)
self.speech_label.setText('Robot said: ' + sound_request.arg)
def command_cb(self):
button_name = self._widget.sender().text()
if (button_name == 'Speak'):
qWarning('Robot will say: ' + self.sound_textbox.text())
self._sound_client.say(self.sound_textbox.text())
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
class PbDGUI(Plugin):
exp_state_sig = Signal(ExperimentState)
def __init__(self, context):
super(PbDGUI, self).__init__(context)
self.setObjectName('PbDGUI')
self._widget = QWidget()
self.speech_cmd_publisher = rospy.Publisher('recognized_command',
Command)
self.gui_cmd_publisher = rospy.Publisher('gui_command', GuiCommand)
rospy.Subscriber('experiment_state', ExperimentState,
self.exp_state_cb)
rospy.Subscriber('robotsound', SoundRequest, self.robotSoundReceived)
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
self.exp_state_sig.connect(self.update_state)
self.commands = dict()
self.commands[Command.CREATE_NEW_ACTION] = 'New action'
self.commands[Command.TEST_MICROPHONE] = 'Test microphone'
self.commands[Command.NEXT_ACTION] = 'Next action'
self.commands[Command.PREV_ACTION] = 'Previous action'
self.commands[Command.SAVE_POSE] = 'Save pose'
self.commands[Command.RELAX_RIGHT_ARM] = 'Relax right arm'
self.commands[Command.RELAX_LEFT_ARM] = 'Relax left arm'
self.commands[Command.FREEZE_RIGHT_ARM] = 'Freeze right arm'
self.commands[Command.FREEZE_LEFT_ARM] = 'Freeze left arm'
self.commands[Command.OPEN_RIGHT_HAND] = 'Open right hand'
self.commands[Command.OPEN_LEFT_HAND] = 'Open left hand'
self.commands[Command.CLOSE_RIGHT_HAND] = 'Close right hand'
self.commands[Command.CLOSE_LEFT_HAND] = 'Close left hand'
self.commands[Command.CLOSE_LEFT_HAND] = 'Close left hand'
self.commands[Command.EXECUTE_ACTION] = 'Execute action'
self.commands[Command.STOP_EXECUTION] = 'Stop execution'
self.commands[Command.DELETE_ALL_STEPS] = 'Delete all'
self.commands[Command.DELETE_LAST_STEP] = 'Delete last'
self.commands[Command.RECORD_OBJECT_POSE] = 'Record object poses'
self.currentAction = -1
self.currentStep = -1
allWidgetsBox = QtGui.QVBoxLayout()
actionBox = QGroupBox('Actions', self._widget)
self.actionGrid = QtGui.QGridLayout()
self.actionGrid.setHorizontalSpacing(0)
for i in range(6):
self.actionGrid.addItem(QtGui.QSpacerItem(90, 90), 0, i,
QtCore.Qt.AlignCenter)
self.actionGrid.setColumnStretch(i, 0)
self.actionIcons = dict()
actionBoxLayout = QtGui.QHBoxLayout()
actionBoxLayout.addLayout(self.actionGrid)
actionBox.setLayout(actionBoxLayout)
actionButtonGrid = QtGui.QHBoxLayout()
actionButtonGrid.addWidget(
self.create_button(Command.CREATE_NEW_ACTION))
self.stepsBox = QGroupBox('No actions created yet', self._widget)
self.stepsGrid = QtGui.QGridLayout()
self.l_model = QtGui.QStandardItemModel(self)
self.l_view = self._create_table_view(self.l_model,
self.l_row_clicked_cb)
self.r_model = QtGui.QStandardItemModel(self)
self.r_view = self._create_table_view(self.r_model,
self.r_row_clicked_cb)
self.stepsGrid.addItem(QtGui.QSpacerItem(280, 10), 0, 0, 2, 3)
self.stepsGrid.addItem(QtGui.QSpacerItem(10, 10), 0, 1, 2, 3)
self.stepsGrid.addItem(QtGui.QSpacerItem(280, 10), 0, 2, 2, 3)
self.stepsGrid.addWidget(QtGui.QLabel('Left Arm'), 0, 0)
self.stepsGrid.addWidget(QtGui.QLabel('Right Arm'), 0, 2)
self.stepsGrid.addWidget(self.l_view, 1, 0)
self.stepsGrid.addWidget(self.r_view, 1, 2)
stepsBoxLayout = QtGui.QHBoxLayout()
stepsBoxLayout.addLayout(self.stepsGrid)
self.stepsBox.setLayout(stepsBoxLayout)
stepsButtonGrid = QtGui.QHBoxLayout()
stepsButtonGrid.addWidget(self.create_button(Command.SAVE_POSE))
stepsButtonGrid.addWidget(self.create_button(Command.EXECUTE_ACTION))
stepsButtonGrid.addWidget(self.create_button(Command.STOP_EXECUTION))
stepsButtonGrid.addWidget(self.create_button(Command.DELETE_ALL_STEPS))
stepsButtonGrid.addWidget(self.create_button(Command.DELETE_LAST_STEP))
misc_grid = QtGui.QHBoxLayout()
misc_grid.addWidget(self.create_button(Command.TEST_MICROPHONE))
misc_grid.addWidget(self.create_button(Command.RECORD_OBJECT_POSE))
misc_grid.addStretch(1)
misc_grid2 = QtGui.QHBoxLayout()
misc_grid2.addWidget(self.create_button(Command.RELAX_RIGHT_ARM))
misc_grid2.addWidget(self.create_button(Command.RELAX_LEFT_ARM))
misc_grid2.addWidget(self.create_button(Command.FREEZE_RIGHT_ARM))
misc_grid2.addWidget(self.create_button(Command.FREEZE_LEFT_ARM))
misc_grid2.addStretch(1)
misc_grid3 = QtGui.QHBoxLayout()
misc_grid3.addWidget(self.create_button(Command.OPEN_RIGHT_HAND))
misc_grid3.addWidget(self.create_button(Command.OPEN_LEFT_HAND))
misc_grid3.addWidget(self.create_button(Command.CLOSE_RIGHT_HAND))
misc_grid3.addWidget(self.create_button(Command.CLOSE_LEFT_HAND))
misc_grid3.addStretch(1)
misc_grid4 = QtGui.QHBoxLayout()
misc_grid4.addWidget(self.create_button(Command.PREV_ACTION))
misc_grid4.addWidget(self.create_button(Command.NEXT_ACTION))
misc_grid4.addStretch(1)
speechGroupBox = QGroupBox('Robot Speech', self._widget)
speechGroupBox.setObjectName('RobotSpeechGroup')
speechBox = QtGui.QHBoxLayout()
self.speechLabel = QtGui.QLabel('Robot has not spoken yet')
palette = QtGui.QPalette()
palette.setColor(QtGui.QPalette.Foreground, QtCore.Qt.blue)
self.speechLabel.setPalette(palette)
speechBox.addWidget(self.speechLabel)
speechGroupBox.setLayout(speechBox)
allWidgetsBox.addWidget(actionBox)
allWidgetsBox.addLayout(actionButtonGrid)
allWidgetsBox.addWidget(self.stepsBox)
allWidgetsBox.addLayout(stepsButtonGrid)
allWidgetsBox.addItem(QtGui.QSpacerItem(100, 20))
allWidgetsBox.addLayout(misc_grid)
allWidgetsBox.addItem(QtGui.QSpacerItem(100, 20))
allWidgetsBox.addLayout(misc_grid2)
allWidgetsBox.addLayout(misc_grid3)
allWidgetsBox.addItem(QtGui.QSpacerItem(100, 20))
allWidgetsBox.addLayout(misc_grid4)
allWidgetsBox.addItem(QtGui.QSpacerItem(100, 20))
allWidgetsBox.addWidget(speechGroupBox)
allWidgetsBox.addStretch(1)
# Fix layout and add main widget to the user interface
QtGui.QApplication.setStyle(QtGui.QStyleFactory.create('plastique'))
vAllBox = QtGui.QVBoxLayout()
vAllBox.addLayout(allWidgetsBox)
vAllBox.addStretch(1)
hAllBox = QtGui.QHBoxLayout()
hAllBox.addLayout(vAllBox)
hAllBox.addStretch(1)
self._widget.setObjectName('PbDGUI')
self._widget.setLayout(hAllBox)
context.add_widget(self._widget)
rospy.loginfo('Will wait for the experiment state service...')
rospy.wait_for_service('get_experiment_state')
exp_state_srv = rospy.ServiceProxy('get_experiment_state',
GetExperimentState)
rospy.loginfo('Got response from the experiment state service...')
response = exp_state_srv()
self.update_state(response.state)
@staticmethod
def loginfo(message):
'''Because all other ROS logging has some kind of information
about what's being emitted, and qWarning doesn't, we're going to
wrap the function to provide a little bit of additional info for
newbies.
Args:
message (str): The message to log
'''
qWarning('[INFO] [pbd_gui.py] ' + message)
def _create_table_view(self, model, row_click_cb):
proxy = QtGui.QSortFilterProxyModel(self)
proxy.setSourceModel(model)
view = QtGui.QTableView(self._widget)
verticalHeader = view.verticalHeader()
verticalHeader.sectionClicked.connect(row_click_cb)
view.setModel(proxy)
view.setMaximumWidth(250)
view.setSortingEnabled(False)
view.setCornerButtonEnabled(False)
return view
def get_uid(self, arm_index, index):
'''Returns a unique id of the marker'''
return (2 * (index + 1) + arm_index)
def get_arm_and_index(self, uid):
'''Returns a unique id of the marker'''
arm_index = uid % 2
index = (uid - arm_index) / 2
return (arm_index, (index - 1))
def r_row_clicked_cb(self, logicalIndex):
self.step_pressed(self.get_uid(0, logicalIndex))
def l_row_clicked_cb(self, logicalIndex):
self.step_pressed(self.get_uid(1, logicalIndex))
def create_button(self, command):
btn = QtGui.QPushButton(self.commands[command], self._widget)
btn.clicked.connect(self.command_cb)
return btn
def update_state(self, state):
PbDGUI.loginfo('Received new state')
n_actions = len(self.actionIcons.keys())
if n_actions < state.n_actions:
for i in range(n_actions, state.n_actions):
self.new_action()
if (self.currentAction != (state.i_current_action - 1)):
self.delete_all_steps()
self.action_pressed(state.i_current_action - 1, False)
n_steps = self.n_steps()
if (n_steps < state.n_steps):
for i in range(n_steps, state.n_steps):
self.save_pose()
elif (n_steps > state.n_steps):
n_to_remove = n_steps - state.n_steps
self.r_model.invisibleRootItem().removeRows(
state.n_steps, n_to_remove)
self.l_model.invisibleRootItem().removeRows(
state.n_steps, n_to_remove)
## TODO: DEAL with the following arrays!!!
state.r_gripper_states
state.l_gripper_states
state.r_ref_frames
state.l_ref_frames
state.objects
if (self.currentStep != state.i_current_step):
if (self.n_steps() > 0):
self.currentStep = state.i_current_step
arm_index, index = self.get_arm_and_index(self.currentStep)
if (arm_index == 0):
self.r_view.selectRow(index)
else:
self.l_view.selectRow(index)
def save_pose(self, actionIndex=None):
nColumns = 9
if actionIndex is None:
actionIndex = self.currentAction
stepIndex = self.n_steps(actionIndex)
r_step = [
QtGui.QStandardItem('Step' + str(stepIndex + 1)),
QtGui.QStandardItem('Go to pose'),
QtGui.QStandardItem('Absolute')
]
l_step = [
QtGui.QStandardItem('Step' + str(stepIndex + 1)),
QtGui.QStandardItem('Go to pose'),
QtGui.QStandardItem('Absolute')
]
self.r_model.invisibleRootItem().appendRow(r_step)
self.l_model.invisibleRootItem().appendRow(l_step)
self.update_table_view()
self.currentStep = stepIndex
def update_table_view(self):
self.l_view.setColumnWidth(0, 50)
self.l_view.setColumnWidth(1, 100)
self.l_view.setColumnWidth(2, 70)
self.r_view.setColumnWidth(0, 50)
self.r_view.setColumnWidth(1, 100)
self.r_view.setColumnWidth(2, 70)
def n_steps(self, actionIndex=None):
return self.l_model.invisibleRootItem().rowCount()
def delete_all_steps(self, actionIndex=None):
if actionIndex is None:
actionIndex = self.currentAction
n_steps = self.n_steps()
if (n_steps > 0):
self.l_model.invisibleRootItem().removeRows(0, n_steps)
self.r_model.invisibleRootItem().removeRows(0, n_steps)
def n_actions(self):
return len(self.actionIcons.keys())
def new_action(self):
nColumns = 6
actionIndex = self.n_actions()
for key in self.actionIcons.keys():
self.actionIcons[key].selected = False
self.actionIcons[key].updateView()
actIcon = ActionIcon(self._widget, actionIndex, self.action_pressed)
self.actionGrid.addLayout(actIcon, int(actionIndex / nColumns),
actionIndex % nColumns)
self.actionIcons[actionIndex] = actIcon
def step_pressed(self, step_index):
gui_cmd = GuiCommand(GuiCommand.SELECT_ACTION_STEP, step_index)
self.gui_cmd_publisher.publish(gui_cmd)
def action_pressed(self, actionIndex, isPublish=True):
for i in range(len(self.actionIcons.keys())):
key = self.actionIcons.keys()[i]
if key == actionIndex:
self.actionIcons[key].selected = True
else:
self.actionIcons[key].selected = False
self.actionIcons[key].updateView()
self.currentAction = actionIndex
self.stepsBox.setTitle('Steps for Action ' +
str(self.currentAction + 1))
if isPublish:
gui_cmd = GuiCommand(GuiCommand.SWITCH_TO_ACTION,
(actionIndex + 1))
self.gui_cmd_publisher.publish(gui_cmd)
def command_cb(self):
clickedButtonName = self._widget.sender().text()
for key in self.commands.keys():
if (self.commands[key] == clickedButtonName):
PbDGUI.loginfo('Sending speech command: ' + key)
command = Command()
command.command = key
self.speech_cmd_publisher.publish(command)
def robotSoundReceived(self, soundReq):
if (soundReq.command == SoundRequest.SAY):
PbDGUI.loginfo('Robot said: ' + soundReq.arg)
self.speechLabel.setText('Robot sound: ' + soundReq.arg)
def exp_state_cb(self, state):
self.exp_state_sig.emit(state)
def shutdown_plugin(self):
# TODO unregister all publishers here
self.speech_cmd_publisher.unregister()
self.gui_cmd_publisher.unregister()
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
class ArmGUI(Plugin):
joint_sig = Signal(JointState)
def __init__(self, context):
super(ArmGUI, self).__init__(context)
self.setObjectName('ArmGUI')
self._widget = QWidget()
print "init"
# 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 = []
# saving our poses
self.saved_r_arm_poses = defaultdict(partial(list))
self.saved_l_arm_poses = defaultdict(partial(list))
self.saved_pose_sets = set()
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)
# controls time between poses
self.time_between_poses = 2.0
large_box = QtGui.QVBoxLayout()
button_box1 = QtGui.QHBoxLayout()
button_box1.addWidget(self.create_button('Relax arms'))
button_box1.addWidget(self.create_button('Freeze arms'))
button_box1.addStretch(1)
large_box.addLayout(button_box1)
large_box.addItem(QtGui.QSpacerItem(100,20))
button_box2 = QtGui.QHBoxLayout()
self.pose_set_text = QtGui.QLineEdit(self._widget)
button_box2.addWidget(self.pose_set_text)
button_box2.addWidget(self.create_button('Add to Pose Set'))
button_box2.addStretch(1)
large_box.addLayout(button_box2)
large_box.addItem(QtGui.QSpacerItem(100,20))
# box with a slider controlling speed robot moves to pose with
speed_box = QtGui.QHBoxLayout()
speed_label = QtGui.QLabel('Pose speed: ')
speed_box.addWidget(speed_label)
speed_box.addWidget(QtGui.QLabel('Slow'))
sldr = QtGui.QSlider(QtCore.Qt.Horizontal)
sldr.setSliderPosition(50)
speed_box.addWidget(sldr)
#sldr.setGeometry(50, 50, 130, 30);
sldr.valueChanged[int].connect(self.set_pose_speed)
speed_box.addWidget(QtGui.QLabel('Fast'))
speed_box.addStretch(1)
large_box.addLayout(speed_box)
button_box3 = QtGui.QHBoxLayout()
self.pose_selector = QtGui.QComboBox()
self.pose_selector.setSizeAdjustPolicy(QtGui.QComboBox.AdjustToContents)
self.pose_selector.currentIndexChanged[str].connect(self.update_pose_set_length)
button_box3.addWidget(self.pose_selector)
self.pose_set_length_label = QtGui.QLabel()
button_box3.addWidget(self.pose_set_length_label)
self.update_pose_set_length()
button_box3.addWidget(self.create_button('Do Pose Set'))
button_box3.addItem(QtGui.QSpacerItem(50,20))
button_box3.addWidget(self.create_button('Delete Pose Set'))
button_box3.addStretch(1)
large_box.addLayout(button_box3)
large_box.addItem(QtGui.QSpacerItem(100,20))
poses_box = QtGui.QVBoxLayout()
self.status_message_label = QtGui.QLabel('No Saved Poses')
poses_box.addWidget(self.status_message_label)
large_box.addLayout(poses_box)
large_box.addStretch(1)
self._widget.setObjectName('ArmGUI')
self._widget.setLayout(large_box)
context.add_widget(self._widget)
rospy.loginfo('GUI initialization complete.')
# currently maps from a quarter second between poses to ~3.5 seconds;
# pos comes in as the slider value which ranges from 0 to 100. it
# seems like even with the delay set to 0, there are automatic
# constraints, but to be safe the quarter second is added. there also
# seem to be poses that are intrinsically hard for the robot to move
# between, causing slow movement even with a low delay set.
def set_pose_speed(self, pos):
self.time_between_poses = (100 - pos) / 30 + .25
def create_button(self, name):
btn = QtGui.QPushButton(name, self._widget)
btn.clicked.connect(self.command_cb)
return btn
# This is a really janky way of doing this, you should check
# self._widget.sender() to figure out where the event originated.
def command_cb(self):
button_name = self._widget.sender().text()
if (button_name == 'Relax arms'):
self.relax_arm('r')
self.relax_arm('l')
elif (button_name == 'Freeze arms'):
self.freeze_arm('r')
self.freeze_arm('l')
elif (button_name == 'Add to Pose Set'):
self.save_pose()
elif (button_name == 'Do Pose Set'):
self.move_arm()
elif (button_name == 'Delete Pose Set'):
self.delete_pose()
self.update_pose_set_length()
def save_pose(self):
pose_set = self.pose_set_text.text()
if len(pose_set) is 0 or pose_set is None:
self.status_message_label.setText("Invalid pose name")
return
# if not already saved, make a new pose set
if pose_set not in self.saved_pose_sets:
self.pose_selector.addItem(pose_set)
self.saved_pose_sets.add(pose_set)
# auto select the saved pose
index = self.pose_selector.findText(pose_set)
self.pose_selector.setCurrentIndex(index)
r_joint_state = self.get_joint_state('r')
l_joint_state = self.get_joint_state('l')
self.saved_r_arm_poses[pose_set].append(r_joint_state)
self.saved_l_arm_poses[pose_set].append(l_joint_state)
self.saved_pose_sets.add(pose_set)
self.status_message_label.setText('Pose saved!')
def update_pose_set_length(self):
pose_set = self.pose_selector.currentText()
text = "(%d)" % len(self.saved_r_arm_poses[pose_set])
self.pose_set_length_label.setText(text)
def move_arm(self):
pose_set = self.pose_selector.currentText()
if not pose_set in self.saved_pose_sets:
rospy.logerr("Target pose set does not exist, I cannot move my little arms!")
self.status_message_label.setText("No pose set")
return
if pose_set in self.saved_r_arm_poses:
self.freeze_arm('r')
self.move_to_joints('r', self.saved_r_arm_poses[pose_set], self.time_between_poses)
if pose_set in self.saved_l_arm_poses:
self.freeze_arm('l')
self.move_to_joints('l', self.saved_l_arm_poses[pose_set], self.time_between_poses)
self.status_message_label.setText('Pose executing!')
def delete_pose(self):
pose_set = self.pose_selector.currentText()
rospy.loginfo('Clearing pose set %s' % pose_set)
# removing from the select box
pose_set_len = len(self.saved_r_arm_poses[pose_set])
index = self.pose_selector.findText(pose_set)
self.pose_selector.removeItem(index)
# removing from the pose set maps
self.saved_r_arm_poses.pop(pose_set, None)
self.saved_l_arm_poses.pop(pose_set, None)
self.saved_pose_sets.remove(pose_set)
self.status_message_label.setText('Pose deleted!')
# unsure if the time_to_joint input is still necessary
# with self.time_between_poses
def move_to_joints(self, side_prefix, positions, time_to_joint):
'''Moves the arm to the desired joints'''
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1))
time_move = time_to_joint + 3
print "using following positions %s" % positions
for pose in positions:
velocities = [0] * len(pose)
traj_goal.trajectory.points.append(JointTrajectoryPoint(positions=pose,
velocities=velocities, time_from_start=rospy.Duration(time_move)))
time_move = time_move + time_to_joint
if (side_prefix == 'r'):
traj_goal.trajectory.joint_names = self.r_joint_names
self.r_traj_action_client.send_goal(traj_goal)
else:
traj_goal.trajectory.joint_names = self.l_joint_names
self.l_traj_action_client.send_goal(traj_goal)
def relax_arm(self, side_prefix):
controller_name = side_prefix + '_arm_controller'
start_controllers = []
stop_controllers = [controller_name]
self.set_arm_mode(start_controllers, stop_controllers)
def freeze_arm(self, side_prefix):
controller_name = side_prefix + '_arm_controller'
start_controllers = [controller_name]
stop_controllers = []
self.set_arm_mode(start_controllers, stop_controllers)
def set_arm_mode(self, start_controllers, stop_controllers):
try:
self.switch_service_client(start_controllers, stop_controllers, 1)
except rospy.ServiceException:
rospy.logerr('Could not change arm mode.')
def joint_states_cb(self, msg):
'''Callback function that saves the joint positions when a
joint_states message is received'''
self.lock.acquire()
self.all_joint_names = msg.name
self.all_joint_poses = msg.position
self.joint_sig.emit(msg)
self.lock.release()
def joint_sig_cb(self, msg):
pass
def get_joint_state(self, side_prefix):
'''Returns position for arm joints on the requested side (r/l)'''
if side_prefix == 'r':
joint_names = self.r_joint_names
else:
joint_names = self.l_joint_names
if self.all_joint_names == []:
rospy.logerr("No robot_state messages received yet!\n")
return None
positions = []
self.lock.acquire()
for joint_name in joint_names:
if joint_name in self.all_joint_names:
index = self.all_joint_names.index(joint_name)
position = self.all_joint_poses[index]
positions.append(position)
else:
rospy.logerr("Joint %s not found!", joint_name)
self.lock.release()
return None
self.lock.release()
return positions
def shutdown_plugin(self):
# TODO unregister all publishers here
# Leave both arm controllers on
start_controllers = ['r_arm_controller', 'l_arm_controller']
stop_controllers = []
self.set_arm_mode(start_controllers, stop_controllers)
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
instance_settings.set_value("r_arm_poses", self.saved_r_arm_poses)
instance_settings.set_value("l_arm_poses", self.saved_l_arm_poses)
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
r_arm_poses = instance_settings.value("r_arm_poses")
if not r_arm_poses is None:
self.saved_r_arm_poses = r_arm_poses
for pose in self.saved_r_arm_poses.keys():
if not pose is None and pose != "":
self.pose_selector.addItem(pose)
self.saved_pose_sets.add(pose)
l_arm_poses = instance_settings.value("l_arm_poses")
if not l_arm_poses is None:
self.saved_l_arm_poses = l_arm_poses
class SimpleGUI(Plugin):
sound_sig = Signal(SoundRequest)
def __init__(self, context):
super(SimpleGUI, self).__init__(context)
self.setObjectName('SimpleGUI')
self._widget = QWidget()
self._widget.setFixedSize(600, 600)
self._sound_client = SoundClient()
rospy.Subscriber('robotsound', SoundRequest, self.sound_cb)
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
self.sound_sig.connect(self.sound_sig_cb)
large_box = QtGui.QVBoxLayout()
#Sound textbox
sound_textbox = QtGui.QLineEdit("Squirtle Squirtle") #Default Text
sound_textbox.setFixedWidth(450)
#Set a handle on the textbox to retrieve the text when button clicked
self.sound_textbox = sound_textbox
button_box = QtGui.QHBoxLayout()
button_box.addItem(QtGui.QSpacerItem(15, 20))
button_box.addWidget(self.create_button('Speak', self.command_cb))
button_box.addWidget(sound_textbox)
button_box.addStretch(1)
large_box.addLayout(button_box)
speech_box = QtGui.QHBoxLayout()
speech_box.addItem(QtGui.QSpacerItem(15, 20))
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.addWidget(self.speech_label)
large_box.addLayout(speech_box)
large_box.addStretch(1)
#large_box.addItem(QtGui.QSpacerItem(50,20))
up_head = Head(Head.UP)
head_box = QtGui.QVBoxLayout()
up_head_box = QtGui.QHBoxLayout()
up_head_button = self.create_button('^', up_head.create_closure())
#large_box.addWidget(up_head_button)
down_head = Head(Head.DOWN)
down_head_box = QtGui.QHBoxLayout()
down_head_button = self.create_button('v', down_head.create_closure())
#large_box.addWidget(down_head_button)
right_head = Head(Head.RIGHT)
right_head_button = self.create_button('>',
right_head.create_closure())
#large_box.addWidget(right_head_button)
left_head = Head(Head.LEFT)
left_head_button = self.create_button('<', left_head.create_closure())
left_right_head_box = QtGui.QHBoxLayout()
up_head_box.addItem(QtGui.QSpacerItem(235, 20))
up_head_box.addWidget(up_head_button)
up_head_box.addItem(QtGui.QSpacerItem(275, 20))
left_right_head_box.addItem(QtGui.QSpacerItem(160, 20))
left_right_head_box.addWidget(left_head_button)
left_right_head_box.addItem(QtGui.QSpacerItem(60, 20))
left_right_head_box.addWidget(right_head_button)
left_right_head_box.addItem(QtGui.QSpacerItem(225, 20))
down_head_box.addItem(QtGui.QSpacerItem(235, 20))
down_head_box.addWidget(down_head_button)
down_head_box.addItem(QtGui.QSpacerItem(275, 20))
head_box.addLayout(up_head_box)
head_box.addLayout(left_right_head_box)
head_box.addLayout(down_head_box)
large_box.addLayout(head_box)
#large_box.addItem(QtGui.QSpacerItem(500,20))
#large_box.addWidget(left_head_button)
gripper = Gripper(Gripper.RIGHT, Gripper.OPEN)
right_gripper = self.create_button('Right gripper!',
gripper.create_closure())
gripper = Gripper(Gripper.LEFT, Gripper.OPEN)
left_gripper = self.create_button('Left gripper!',
gripper.create_closure())
large_box.addItem(QtGui.QSpacerItem(100, 250))
gripper_box = QtGui.QHBoxLayout()
gripper_box.addItem(QtGui.QSpacerItem(75, 20))
gripper_box.addWidget(left_gripper)
gripper_box.addItem(QtGui.QSpacerItem(450, 20))
gripper_box.addWidget(right_gripper)
gripper_box.addItem(QtGui.QSpacerItem(75, 20))
large_box.addLayout(gripper_box)
base_box = QtGui.QVBoxLayout()
large_box.addItem(QtGui.QSpacerItem(100, 100))
#forward
forward_base_box = QtGui.QHBoxLayout()
forward_base = Base(Base.FORWARD)
forward_base_button = self.create_button('move forward',
forward_base.create_closure())
forward_base_box.addItem(QtGui.QSpacerItem(400, 20))
forward_base_box.addWidget(forward_base_button)
forward_base_box.addItem(QtGui.QSpacerItem(400, 20))
base_box.addLayout(forward_base_box)
#large_box.addWidget(forward_base_button)
#left
left_right_base_box = QtGui.QHBoxLayout()
left_base = Base(Base.LEFT)
left_base_button = self.create_button('move left',
left_base.create_closure())
#large_box.addWidget(left_base_button)
#right
right_base = Base(Base.RIGHT)
right_base_button = self.create_button('move right',
right_base.create_closure())
left_right_base_box.addItem(QtGui.QSpacerItem(300, 20))
left_right_base_box.addWidget(left_base_button)
left_right_base_box.addItem(QtGui.QSpacerItem(50, 20))
left_right_base_box.addWidget(right_base_button)
left_right_base_box.addItem(QtGui.QSpacerItem(300, 20))
base_box.addLayout(left_right_base_box)
#large_box.addWidget(right_base_button)
#backward
backward_base_box = QtGui.QHBoxLayout()
backward_base = Base(Base.BACKWARD)
backward_base_button = self.create_button(
'move backward', backward_base.create_closure())
backward_base_box.addItem(QtGui.QSpacerItem(400, 20))
backward_base_box.addWidget(backward_base_button)
backward_base_box.addItem(QtGui.QSpacerItem(400, 20))
base_box.addLayout(backward_base_box)
#large_box.addWidget(backward_base_button)
large_box.addLayout(base_box)
turn_base_box = QtGui.QHBoxLayout()
#turn left
turnleft_base = Base(Base.TURNLEFT)
turnleft_base_button = self.create_button(
' /\n<--', turnleft_base.create_closure())
#large_box.addWidget(turnleft_base_button)
#turn right
turnright_base = Base(Base.TURNRIGHT)
turnright_base_button = self.create_button(
'\\\n -->', turnright_base.create_closure())
turn_base_box.addItem(QtGui.QSpacerItem(75, 20))
turn_base_box.addWidget(turnright_base_button)
turn_base_box.addItem(QtGui.QSpacerItem(225, 20))
turn_base_box.addWidget(turnleft_base_button)
turn_base_box.addItem(QtGui.QSpacerItem(100, 20))
large_box.addLayout(turn_base_box)
#large_box.addWidget(turnright_base_button)
self._widget.setObjectName('SimpleGUI')
self._widget.setLayout(large_box)
context.add_widget(self._widget)
self._widget.setStyleSheet(
"QWidget { image: url(%s) }" %
"/home/vjampala/catkin_ws/src/cse481/hw1/rqt_simplegui/rosie_background.jpg"
)
def sound_cb(self, sound_request):
qWarning('Received sound.')
self.sound_sig.emit(sound_request)
def create_button(self, name, method):
btn = QtGui.QPushButton(name, self._widget)
btn.clicked.connect(method)
return btn
def sound_sig_cb(self, sound_request):
qWarning('Received sound signal.')
#if (sound_request.command == SoundRequest.SAY):
qWarning('Robot said: ' + sound_request.arg)
self.speech_label.setText('Robot said: ' + sound_request.arg)
def command_cb(self):
button_name = self._widget.sender().text()
if (button_name == 'Speak'):
qWarning('Robot will say: ' + self.sound_textbox.text())
self._sound_client.say(self.sound_textbox.text())
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
class SimpleGUI(Plugin):
# For sending speech
sound_sig = Signal(SoundRequest)
# Joints for arm poses
joint_sig = Signal(JointState)
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")
# Event for when text box is changed
def onChanged(self, text):
self.speech_label.setText(text)
self.speech_label.adjustSize()
def sound_cb(self, sound_request):
qWarning('Received sound.')
self.sound_sig.emit(sound_request)
def create_button(self, name):
btn = QtGui.QPushButton(name, self._widget)
btn.setFixedWidth(150)
btn.clicked.connect(self.command_cb)
return btn
def create_pressed_button(self, name):
btn = QtGui.QPushButton(name, self._widget)
btn.setFixedWidth(150)
btn.pressed.connect(self.command_cb)
btn.setAutoRepeat(True) # To make sure the movement repeats itself
return btn
def sound_sig_cb(self, sound_request):
qWarning('Received sound signal.')
qWarning('Robot said: ' + sound_request.arg)
self.speech_label.setText(sound_request.arg) #'Robot said: ' +
#a button was clicked
def command_cb(self):
button_name = self._widget.sender().text()
#robot talk button clicked
if (button_name == 'Speak'):
qWarning('Robot will say: ' + self.speech_label.text() )
self._sound_client.say(self.speech_label.text())
self.show_text_in_rviz("Robot is Speaking")
#gripper button selected
elif ('Gripper' in button_name):
self.gripper_click(button_name)
# Move forward
elif (button_name == '^'):
self.base_action(0.25, 0.0, 0.0, 0.0, 0.0, 0.0)
# Move left
elif (button_name == '<'):
self.base_action(0.0, 0.25, 0.0, 0.0, 0.0, 0.0)
# Move right
elif (button_name == '>'):
self.base_action(0.0, -0.25, 0.0, 0.0, 0.0, 0.0)
# Move back
elif (button_name == 'v'):
self.base_action(-0.25, 0.0, 0.0, 0.0, 0.0, 0.0)
#Rotate Left
elif (button_name == 'Rotate Left'):
self.base_action(0.0, 0.0, 0.0, 0.0, 0.0, 0.50)
# Rotate Right
elif (button_name == 'Rotate Right'):
self.base_action(0.0, 0.0, 0.0, 0.0, 0.0, -0.50)
# A head button selected
elif ('Head' in button_name):
self.rotate_head(button_name)
#An arm button selected
#third param unused in freeze/relax
#Second word in button should be side
elif ('Arm' in button_name):
arm_side = button_name.split()[1]
if ('Freeze' in button_name or 'Relax' in button_name):
new_arm_state = button_name.split()[0]
self.toggle_arm(arm_side[0].lower(), new_arm_state, True)
old_arm_state = ''
if (new_arm_state == 'Relax'):
old_arm_state = 'Freeze'
else:
old_arm_state = 'Relax'
self._widget.sender().setText('%s %s Arm' % (old_arm_state, arm_side))
elif('Play Animation' == button_name):
self.animPlay.left_poses = self.saved_animations[self.saved_animations_list.currentText()].left
self.animPlay.right_poses = self.saved_animations[self.saved_animations_list.currentText()].right
self.animPlay.left_gripper_states = self.saved_animations[self.saved_animations_list.currentText()].left_gripper
self.animPlay.right_gripper_states = self.saved_animations[self.saved_animations_list.currentText()].right_gripper
if self.pose_time.text() == '':
self.show_warning('Please enter a duration in seconds.')
else:
self.animPlay.play(self.pose_time.text())
elif('Animation' in button_name):
if ('Save' in button_name):
if self.animation_name.text() == '':
self.show_warning('Please enter name for animation')
else:
self.save_animation(self.animation_name.text())
self.list_widget.clear()
self.animation_name.setText('')
elif('Add Current Pose' == button_name):
if self.pose_name_temp.text() == '':
self.show_warning('Insert name for pose')
else:
self.animation_map[self.pose_name_temp.text()] = Quad(self.get_joint_state('l'), self.get_joint_state('r'), self.left_gripper_state, self.right_gripper_state)
list_item = QListWidgetItem()
list_item.setText(self.pose_name_temp.text())
self.list_widget.addItem(list_item)
self.pose_name_temp.setText('')
elif('Move to Bin' == button_name):
self.move_to_bin_action()
elif('Clean Room' == button_name):
rospy.loginfo("STARTING AUTONOMOUS MODE")
self.tuck_arms()
while(self.index < len(self.locations)):
self.roomNav.move_to_trash_location(self.locations[self.index])
# self.index += 1
self.head_action(1.0, 0, -0.50, True)
# Returns Nonce if nothing, and the point of the first object it sees otherwise
map_point = self.pap.detect_objects()
if(map_point == None):
self.index += 1
else:
self.pick_and_move_trash_action()
rospy.loginfo("FINISHED AUTONOMOUS MODE")
self.index = 1
elif('Object Detect' == button_name):
self.pick_and_move_trash_action()
def pick_and_move_trash_action(self, map_point = None):
if map_point == None:
self.head_action(1.0, 0, -0.50, True)
map_point = self.pap.detect_objects()
if map_point == None:
return
# Convert to base link and move towards the object 0.50m away
map_point = Transformer.transform(self._tf_listener, map_point.pose, map_point.header.frame_id, '/base_link')
rospy.loginfo("Object is " + str (map_point.pose.position.x) + " away")
'''
if(map_point.pose.position.x < 0.8):
self.roomNav.move_to_trash_location(self.locations[self.index - 1])
'''
move_back_first = False;
if(map_point.pose.position.x < 0.7):
move_back_first = True;
map_point.pose.position.x -= 0.450
map_point = Transformer.transform(self._tf_listener, map_point.pose, '/base_link', '/map')
if(move_back_first):
self.roomNav.move_to_trash_location(self.locations[self.index - 1])
success = self.roomNav.move_to_trash_location(map_point.pose)
'''This part of the code strafes the robot left to get closer to the object'''
'''
rate = rospy.Rate(10)
position = Point()
move_cmd = Twist()
move_cmd.linear.y = 0.25
odom_frame = '/odom_combined'
# Find out if the robot uses /base_link or /base_footprint
try:
self._tf_listener.waitForTransform(odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))
self.base_frame = '/base_footprint'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
try:
self._tf_listener.waitForTransform(odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0))
self.base_frame = '/base_link'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("Cannot find transform between " + odom_frame + " and /base_link or /base_footprint")
rospy.signal_shutdown("tf Exception")
# Get current position
position = self.get_odom()
x_start = position.x
y_start = position.y
# Distance travelled
distance = 0
goal_distance = 0.25
rospy.loginfo("Strafing left")
# Enter the loop to move along a side
while distance < goal_distance:
rospy.loginfo("Distance is at " + str(distance))
# Publish the Twist message and sleep 1 cycle
self.base_action(0, 0.25, 0, 0, 0, 0)
rate.sleep()
# Get the current position
position = self.get_odom()
# Compute the Euclidean distance from the start
distance = abs(position.y - y_start)
'''
if(success):
# Move head to look at the object, this will wait for a result
self.head_action(0, 0.4, 0.55, True)
# Move arms to ready pickup position, this will wait for a result before trying to detect and pick up object
self.animPlay.left_poses = self.saved_animations['ready_pickup'].left
self.animPlay.right_poses = self.saved_animations['ready_pickup'].right
self.animPlay.left_gripper_states = self.saved_animations['ready_pickup'].left_gripper
self.animPlay.right_gripper_states = self.saved_animations['ready_pickup'].right_gripper
self.animPlay.play('3.0')
for i in range (0,3):
success = self.pap.detect_and_pickup()
# Move head back to look forward
# Move head to look at the object, this will wait for a result
self.head_action(1.0, 0.0, 0.55, True)
if success:
break
# Move to bin
self.move_to_bin_action()
def get_odom(self):
# Get the current transform between the odom and base frames
try:
trans = self._tf_listener.lookupTransform('/odom_combined', self.base_frame, rospy.Time(0))
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("TF Exception")
return
return Point(trans[0][0],trans[0][1],trans[0][2])
# gripper_type is either 'l' for left or 'r' for right
# gripper position is the position as a parameter to the gripper goal
def gripper_action(self, gripper_type, gripper_position):
name_space = '/' + gripper_type + '_gripper_controller/gripper_action'
gripper_client = SimpleActionClient(name_space, GripperCommandAction)
gripper_client.wait_for_server()
gripper_goal = GripperCommandGoal()
gripper_goal.command.position = gripper_position
gripper_goal.command.max_effort = 30.0
gripper_client.send_goal(gripper_goal)
# update the state of the current gripper being modified
if (gripper_type == 'l'):
if (gripper_position == 0.0):
self.left_gripper_state = 'closed'
else:
self.left_gripper_state = 'open'
else:
if (gripper_position == 0.0):
self.right_gripper_state = 'closed'
else:
self.right_gripper_state = 'open'
def base_action(self, x, y, z, theta_x, theta_y, theta_z):
topic_name = '/base_controller/command'
base_publisher = rospy.Publisher(topic_name, Twist)
twist_msg = Twist()
twist_msg.linear = Vector3(x, y, z)
twist_msg.angular = Vector3(theta_x, theta_y, theta_z)
base_publisher.publish(twist_msg)
# Moves the torso of the robot down to its maximum
def torso_down(self, wait = False):
self.torso_client = SimpleActionClient('/torso_controller/position_joint_action', SingleJointPositionAction)
torso_goal = SingleJointPositionGoal()
torso_goal.position = 0.0
torso_goal.min_duration = rospy.Duration(5.0)
torso_goal.max_velocity = 1.0
self.torso_client.send_goal(torso_goal)
if wait:
# wait for the head movement to finish before we try to detect and pickup an object
finished_within_time = self.torso_client.wait_for_result(rospy.Duration(15))
# Check for success or failure
if not finished_within_time:
self.torso_client.cancel_goal()
rospy.loginfo("Timed out achieving torso movement goal")
else:
state = self.torso_client.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Torso movement goal succeeded!")
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo("Torso movement goal failed with error code: " + str(state))
def head_action(self, x, y, z, wait = False):
name_space = '/head_traj_controller/point_head_action'
head_client = SimpleActionClient(name_space, PointHeadAction)
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = 'base_link'
head_goal.min_duration = rospy.Duration(1.0)
head_goal.target.point = Point(x, y, z)
head_client.send_goal(head_goal)
if wait:
# wait for the head movement to finish before we try to detect and pickup an object
finished_within_time = head_client.wait_for_result(rospy.Duration(5))
# Check for success or failure
if not finished_within_time:
head_client.cancel_goal()
rospy.loginfo("Timed out achieving head movement goal")
else:
state = head_client.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Head movement goal succeeded!")
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo("Head movement goal failed with error code: " + str(self.goal_states[state]))
def tuck_arms(self):
rospy.loginfo('Left tuck arms')
self.animPlay.left_poses = self.saved_animations['left_tuck'].left
self.animPlay.right_poses = self.saved_animations['left_tuck'].right
self.animPlay.left_gripper_states = self.saved_animations['left_tuck'].left_gripper
self.animPlay.right_gripper_states = self.saved_animations['left_tuck'].right_gripper
self.animPlay.play('3.0')
def move_to_bin_action(self):
# First tuck arms
self.tuck_arms()
# Move to the bin
rospy.loginfo('Clicked the move to bin button')
self.roomNav.move_to_bin()
# Throw the trash away
rospy.loginfo('Throwing trash away with left arm')
self.animPlay.left_poses = self.saved_animations['l_dispose'].left
self.animPlay.right_poses = self.saved_animations['l_dispose'].right
self.animPlay.left_gripper_states = self.saved_animations['l_dispose'].left_gripper
self.animPlay.right_gripper_states = self.saved_animations['l_dispose'].right_gripper
self.animPlay.play('2.0')
# Tuck arms again
self.tuck_arms()
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
def show_text_in_rviz(self, text):
marker = Marker(type=Marker.TEXT_VIEW_FACING, id=0,
lifetime=rospy.Duration(1.5),
pose=Pose(Point(0.5, 0.5, 1.45), Quaternion(0, 0, 0, 1)),
scale=Vector3(0.06, 0.06, 0.06),
header=Header(frame_id='base_link'),
color=ColorRGBA(0.0, 1.0, 0.0, 0.8), text=text)
self.marker_publisher.publish(marker)
def show_arrow_in_rviz(self, arrow):
marker = Marker(type=Marker.ARROW, id=0,
lifetime=rospy.Duration(1.5),
pose=Pose(Point(0.5, 0.5, 1.45), Quaternion(0, 0, 0, 1)),
scale=Vector3(0.06, 0.06, 0.06),
header=Header(frame_id='base_link'),
color=ColorRGBA(0.0, 1.0, 0.0, 0.8), arrow=arrow)
self.marker_publisher.publish(marker)
def save_animation(self, animation_name):
if animation_name != '':
filename = os.path.join(self.dir, 'animations/')
anim_file = open(filename + animation_name + '.txt', 'w')
l_animation_queue = []
r_animation_queue = []
l_gripper_queue = []
r_gripper_queue = []
for i in range(self.list_widget.count()):
item = self.list_widget.item(i)
pose_name = item.text()
anim_file.write(re.sub(',' , '', str(self.animation_map[pose_name].left).strip('[]') +
' ' + str(self.animation_map[pose_name].right).strip('[]')))
anim_file.write('\n')
anim_file.write(str(self.animation_map[pose_name].left_gripper) + ' ' + str(self.animation_map[pose_name].right_gripper))
l_animation_queue.append(self.animation_map[pose_name].left)
r_animation_queue.append(self.animation_map[pose_name].right)
l_gripper_queue.append(self.animation_map[pose_name].left_gripper)
r_gripper_queue.append(self.animation_map[pose_name].right_gripper)
anim_file.write('\n')
anim_file.close()
self.saved_animations[animation_name] = Quad(l_animation_queue, r_animation_queue, l_gripper_queue, r_gripper_queue)
self.saved_animations_list.addItem(animation_name) # Bug? Multiple entries
# Reset the pending queue
self.l_animation_queue = []
self.r_animation_queue = []
else:
self.show_warning('Please insert name for animation')
def gripper_click(self, button_name):
grip_side = ''
grip_side_text = ''
if ('Left' in button_name):
grip_side = 'l'
grip_side_text = 'left'
else:
grip_side = 'r'
grip_side_text = 'right'
if ('Open' in button_name):
grip_action = 20.0
grip_action_text = 'close'
qWarning('Robot opened %s gripper' % (grip_side_text))
else:
grip_action = 0.0
grip_action_text = 'open'
qWarning('Robot closed %s gripper' % (grip_side_text))
self.show_text_in_rviz("%sing %s Gripper" % (grip_action_text.capitalize(), grip_side_text.capitalize()))
self.gripper_action(grip_side, grip_action)
self._widget.sender().setText('%s %s Gripper' % (grip_action_text.capitalize(), grip_side_text.capitalize()))
def rotate_head(self, button_name):
if('Left' in button_name):
#qWarning('x: %s, y: %s' % (self.head_x, self.head_y))
if (self.head_x < -0.8 and self.head_y > 0.0):
self.show_warning('Can\'t rotate anymore')
elif (self.head_y < 0.0):
self.head_x += 0.1
self.head_y = -((1.0 - self.head_x ** 2.0) ** 0.5)
self.show_text_in_rviz("Turning Head Left")
else:
self.head_x -= 0.1
self.head_y = (1.0 - self.head_x ** 2.0) ** 0.5
self.show_text_in_rviz("Turning Head Left")
qWarning('x: %s, y: %s' % (self.head_x, self.head_y))
self.head_action(self.head_x, self.head_y, self.head_z)
elif('Up' in button_name):
if (self.head_z <= 1.6):
self.head_z += 0.1
self.show_text_in_rviz("Moving Head Up")
self.head_action(self.head_x, self.head_y, self.head_z)
else:
self.show_warning('Can\'t look up anymore')
elif('Down' in button_name):
if (self.head_z >= -2.2):
self.head_z -= 0.1
self.show_text_in_rviz("Moving Head Down")
self.head_action(self.head_x, self.head_y, self.head_z)
else:
self.show_warning('Can\'t look down anymore')
else:
#qWarning('x: %s, y: %s' % (self.head_x, self.head_y))
if (self.head_x < -0.8 and self.head_y < 0.0):
self.show_warning('Can\'t rotate anymore')
elif (self.head_y > 0.0):
self.head_x += 0.1
self.head_y = (1.0 - self.head_x ** 2.0) ** 0.5
self.show_text_in_rviz("Turning Head Right")
else:
self.head_x -= 0.1
self.head_y = -((1.0 - self.head_x ** 2.0) ** 0.5)
self.show_text_in_rviz("Turning Head Right")
#qWarning('x: %s, y: %s' % (self.head_x, self.head_y))
self.head_action(self.head_x, self.head_y, self.head_z)
def toggle_arm(self, side, toggle, button):
controller_name = side + '_arm_controller'
start_controllers = []
stop_controllers = []
if (toggle == 'Relax'):
stop_controllers.append(controller_name)
else:
start_controllers.append(controller_name)
self.set_arm_mode(start_controllers, stop_controllers)
def set_arm_mode(self, start_controllers, stop_controllers):
try:
self.switch_service_client(start_controllers, stop_controllers, 1)
except rospy.ServiceException:
rospy.logerr('Could not change arm mode.')
def joint_states_cb(self, msg):
'''Callback function that saves the joint positions when a
joint_states message is received'''
self.lock.acquire()
self.all_joint_names = msg.name
self.all_joint_poses = msg.position
self.joint_sig.emit(msg)
self.lock.release()
def joint_sig_cb(self, msg):
pass
def get_joint_state(self, side_prefix):
'''Returns position for arm joints on the requested side (r/l)'''
if side_prefix == 'r':
joint_names = self.r_joint_names
else:
joint_names = self.l_joint_names
if self.all_joint_names == []:
rospy.logerr("No robot_state messages received yet!\n")
return None
positions = []
self.lock.acquire()
for joint_name in joint_names:
if joint_name in self.all_joint_names:
index = self.all_joint_names.index(joint_name)
position = self.all_joint_poses[index]
positions.append(position)
else:
rospy.logerr("Joint %s not found!", joint_name)
self.lock.release()
return None
self.lock.release()
return positions
def show_warning(self, text):
qWarning(text)
msgBox = QMessageBox()
msgBox.setText(text)
msgBox.exec_()
class SimpleGUI(Plugin):
sound_sig = Signal(SoundRequest)
def __init__(self, context):
super(SimpleGUI, self).__init__(context)
self.setObjectName('SimpleGUI')
self._widget = QWidget()
self._widget.setFixedSize(600, 600)
self._sound_client = SoundClient()
rospy.Subscriber('robotsound', SoundRequest, self.sound_cb)
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
self.sound_sig.connect(self.sound_sig_cb)
large_box = QtGui.QVBoxLayout()
#Sound textbox
sound_textbox = QtGui.QLineEdit("Squirtle Squirtle") #Default Text
sound_textbox.setFixedWidth(450)
#Set a handle on the textbox to retrieve the text when button clicked
self.sound_textbox = sound_textbox
button_box = QtGui.QHBoxLayout()
button_box.addItem(QtGui.QSpacerItem(15,20))
button_box.addWidget(self.create_button('Speak', self.command_cb))
button_box.addWidget(sound_textbox)
button_box.addStretch(1)
large_box.addLayout(button_box)
speech_box = QtGui.QHBoxLayout()
speech_box.addItem(QtGui.QSpacerItem(15, 20))
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.addWidget(self.speech_label)
large_box.addLayout(speech_box)
large_box.addStretch(1)
#large_box.addItem(QtGui.QSpacerItem(50,20))
up_head = Head(Head.UP)
head_box = QtGui.QVBoxLayout()
up_head_box = QtGui.QHBoxLayout()
up_head_button = self.create_button('^', up_head.create_closure())
#large_box.addWidget(up_head_button)
down_head = Head(Head.DOWN)
down_head_box = QtGui.QHBoxLayout()
down_head_button = self.create_button('v', down_head.create_closure())
#large_box.addWidget(down_head_button)
right_head = Head(Head.RIGHT)
right_head_button = self.create_button('>', right_head.create_closure())
#large_box.addWidget(right_head_button)
left_head = Head(Head.LEFT)
left_head_button = self.create_button('<', left_head.create_closure())
left_right_head_box = QtGui.QHBoxLayout()
up_head_box.addItem(QtGui.QSpacerItem(235,20))
up_head_box.addWidget(up_head_button)
up_head_box.addItem(QtGui.QSpacerItem(275,20))
left_right_head_box.addItem(QtGui.QSpacerItem(160,20))
left_right_head_box.addWidget(left_head_button)
left_right_head_box.addItem(QtGui.QSpacerItem(60,20))
left_right_head_box.addWidget(right_head_button)
left_right_head_box.addItem(QtGui.QSpacerItem(225,20))
down_head_box.addItem(QtGui.QSpacerItem(235,20))
down_head_box.addWidget(down_head_button)
down_head_box.addItem(QtGui.QSpacerItem(275,20))
head_box.addLayout(up_head_box)
head_box.addLayout(left_right_head_box)
head_box.addLayout(down_head_box)
large_box.addLayout(head_box)
#large_box.addItem(QtGui.QSpacerItem(500,20))
#large_box.addWidget(left_head_button)
gripper = Gripper(Gripper.RIGHT, Gripper.OPEN)
right_gripper = self.create_button('Right gripper!', gripper.create_closure())
gripper = Gripper(Gripper.LEFT, Gripper.OPEN)
left_gripper = self.create_button('Left gripper!', gripper.create_closure())
large_box.addItem(QtGui.QSpacerItem(100,250))
gripper_box = QtGui.QHBoxLayout()
gripper_box.addItem(QtGui.QSpacerItem(75,20))
gripper_box.addWidget(left_gripper)
gripper_box.addItem(QtGui.QSpacerItem(450,20))
gripper_box.addWidget(right_gripper)
gripper_box.addItem(QtGui.QSpacerItem(75,20))
large_box.addLayout(gripper_box)
base_box = QtGui.QVBoxLayout()
large_box.addItem(QtGui.QSpacerItem(100,100))
#forward
forward_base_box = QtGui.QHBoxLayout()
forward_base = Base(Base.FORWARD)
forward_base_button = self.create_button('move forward', forward_base.create_closure())
forward_base_box.addItem(QtGui.QSpacerItem(400,20))
forward_base_box.addWidget(forward_base_button)
forward_base_box.addItem(QtGui.QSpacerItem(400,20))
base_box.addLayout(forward_base_box)
#large_box.addWidget(forward_base_button)
#left
left_right_base_box = QtGui.QHBoxLayout()
left_base= Base(Base.LEFT)
left_base_button = self.create_button('move left', left_base.create_closure())
#large_box.addWidget(left_base_button)
#right
right_base= Base(Base.RIGHT)
right_base_button= self.create_button('move right', right_base.create_closure())
left_right_base_box.addItem(QtGui.QSpacerItem(300,20))
left_right_base_box.addWidget(left_base_button)
left_right_base_box.addItem(QtGui.QSpacerItem(50,20))
left_right_base_box.addWidget(right_base_button)
left_right_base_box.addItem(QtGui.QSpacerItem(300,20))
base_box.addLayout(left_right_base_box)
#large_box.addWidget(right_base_button)
#backward
backward_base_box = QtGui.QHBoxLayout()
backward_base= Base(Base.BACKWARD)
backward_base_button = self.create_button('move backward', backward_base.create_closure())
backward_base_box.addItem(QtGui.QSpacerItem(400,20))
backward_base_box.addWidget(backward_base_button)
backward_base_box.addItem(QtGui.QSpacerItem(400,20))
base_box.addLayout(backward_base_box)
#large_box.addWidget(backward_base_button)
large_box.addLayout(base_box)
turn_base_box = QtGui.QHBoxLayout()
#turn left
turnleft_base= Base(Base.TURNLEFT)
turnleft_base_button = self.create_button(' /\n<--', turnleft_base.create_closure())
#large_box.addWidget(turnleft_base_button)
#turn right
turnright_base= Base(Base.TURNRIGHT)
turnright_base_button = self.create_button('\\\n -->', turnright_base.create_closure())
turn_base_box.addItem(QtGui.QSpacerItem(75,20))
turn_base_box.addWidget(turnright_base_button)
turn_base_box.addItem(QtGui.QSpacerItem(225,20))
turn_base_box.addWidget(turnleft_base_button)
turn_base_box.addItem(QtGui.QSpacerItem(100,20))
large_box.addLayout(turn_base_box)
#large_box.addWidget(turnright_base_button)
self._widget.setObjectName('SimpleGUI')
self._widget.setLayout(large_box)
context.add_widget(self._widget)
self._widget.setStyleSheet("QWidget { image: url(%s) }" % "/home/vjampala/catkin_ws/src/cse481/hw1/rqt_simplegui/rosie_background.jpg")
def sound_cb(self, sound_request):
qWarning('Received sound.')
self.sound_sig.emit(sound_request)
def create_button(self, name, method):
btn = QtGui.QPushButton(name, self._widget)
btn.clicked.connect(method)
return btn
def sound_sig_cb(self, sound_request):
qWarning('Received sound signal.')
#if (sound_request.command == SoundRequest.SAY):
qWarning('Robot said: ' + sound_request.arg)
self.speech_label.setText('Robot said: ' + sound_request.arg)
def command_cb(self):
button_name = self._widget.sender().text()
if (button_name == 'Speak'):
qWarning('Robot will say: ' + self.sound_textbox.text())
self._sound_client.say(self.sound_textbox.text())
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
class SimpleGUI(Plugin):
# For sending speech
sound_sig = Signal(SoundRequest)
# Joints for arm poses
joint_sig = Signal(JointState)
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")
# Event for when text box is changed
def onChanged(self, text):
self.speech_label.setText(text)
self.speech_label.adjustSize()
def sound_cb(self, sound_request):
qWarning('Received sound.')
self.sound_sig.emit(sound_request)
def create_button(self, name):
btn = QtGui.QPushButton(name, self._widget)
btn.setFixedWidth(150)
btn.clicked.connect(self.command_cb)
return btn
def create_pressed_button(self, name):
btn = QtGui.QPushButton(name, self._widget)
btn.setFixedWidth(150)
btn.pressed.connect(self.command_cb)
btn.setAutoRepeat(True) # To make sure the movement repeats itself
return btn
def sound_sig_cb(self, sound_request):
qWarning('Received sound signal.')
qWarning('Robot said: ' + sound_request.arg)
self.speech_label.setText(sound_request.arg) #'Robot said: ' +
#a button was clicked
def command_cb(self):
button_name = self._widget.sender().text()
#robot talk button clicked
if (button_name == 'Speak'):
qWarning('Robot will say: ' + self.speech_label.text())
self._sound_client.say(self.speech_label.text())
self.show_text_in_rviz("Robot is Speaking")
#gripper button selected
elif ('Gripper' in button_name):
self.gripper_click(button_name)
# Move forward
elif (button_name == '^'):
self.base_action(0.25, 0.0, 0.0, 0.0, 0.0, 0.0)
# Move left
elif (button_name == '<'):
self.base_action(0.0, 0.25, 0.0, 0.0, 0.0, 0.0)
# Move right
elif (button_name == '>'):
self.base_action(0.0, -0.25, 0.0, 0.0, 0.0, 0.0)
# Move back
elif (button_name == 'v'):
self.base_action(-0.25, 0.0, 0.0, 0.0, 0.0, 0.0)
#Rotate Left
elif (button_name == 'Rotate Left'):
self.base_action(0.0, 0.0, 0.0, 0.0, 0.0, 0.50)
# Rotate Right
elif (button_name == 'Rotate Right'):
self.base_action(0.0, 0.0, 0.0, 0.0, 0.0, -0.50)
# A head button selected
elif ('Head' in button_name):
self.rotate_head(button_name)
#An arm button selected
#third param unused in freeze/relax
#Second word in button should be side
elif ('Arm' in button_name):
arm_side = button_name.split()[1]
if ('Freeze' in button_name or 'Relax' in button_name):
new_arm_state = button_name.split()[0]
self.toggle_arm(arm_side[0].lower(), new_arm_state, True)
old_arm_state = ''
if (new_arm_state == 'Relax'):
old_arm_state = 'Freeze'
else:
old_arm_state = 'Relax'
self._widget.sender().setText('%s %s Arm' %
(old_arm_state, arm_side))
elif ('Play Animation' == button_name):
self.animPlay.left_poses = self.saved_animations[
self.saved_animations_list.currentText()].left
self.animPlay.right_poses = self.saved_animations[
self.saved_animations_list.currentText()].right
self.animPlay.left_gripper_states = self.saved_animations[
self.saved_animations_list.currentText()].left_gripper
self.animPlay.right_gripper_states = self.saved_animations[
self.saved_animations_list.currentText()].right_gripper
if self.pose_time.text() == '':
self.show_warning('Please enter a duration in seconds.')
else:
self.animPlay.play(self.pose_time.text())
elif ('Animation' in button_name):
if ('Save' in button_name):
if self.animation_name.text() == '':
self.show_warning('Please enter name for animation')
else:
self.save_animation(self.animation_name.text())
self.list_widget.clear()
self.animation_name.setText('')
elif ('Add Current Pose' == button_name):
if self.pose_name_temp.text() == '':
self.show_warning('Insert name for pose')
else:
self.animation_map[self.pose_name_temp.text()] = Quad(
self.get_joint_state('l'), self.get_joint_state('r'),
self.left_gripper_state, self.right_gripper_state)
list_item = QListWidgetItem()
list_item.setText(self.pose_name_temp.text())
self.list_widget.addItem(list_item)
self.pose_name_temp.setText('')
elif ('Move to Bin' == button_name):
self.move_to_bin_action()
elif ('Clean Room' == button_name):
rospy.loginfo("STARTING AUTONOMOUS MODE")
self.tuck_arms()
while (self.index < len(self.locations)):
self.roomNav.move_to_trash_location(self.locations[self.index])
# self.index += 1
self.head_action(1.0, 0, -0.50, True)
# Returns Nonce if nothing, and the point of the first object it sees otherwise
map_point = self.pap.detect_objects()
if (map_point == None):
self.index += 1
else:
self.pick_and_move_trash_action()
rospy.loginfo("FINISHED AUTONOMOUS MODE")
self.index = 1
elif ('Object Detect' == button_name):
self.pick_and_move_trash_action()
def pick_and_move_trash_action(self, map_point=None):
if map_point == None:
self.head_action(1.0, 0, -0.50, True)
map_point = self.pap.detect_objects()
if map_point == None:
return
# Convert to base link and move towards the object 0.50m away
map_point = Transformer.transform(self._tf_listener, map_point.pose,
map_point.header.frame_id,
'/base_link')
rospy.loginfo("Object is " + str(map_point.pose.position.x) + " away")
'''
if(map_point.pose.position.x < 0.8):
self.roomNav.move_to_trash_location(self.locations[self.index - 1])
'''
move_back_first = False
if (map_point.pose.position.x < 0.7):
move_back_first = True
map_point.pose.position.x -= 0.450
map_point = Transformer.transform(self._tf_listener, map_point.pose,
'/base_link', '/map')
if (move_back_first):
self.roomNav.move_to_trash_location(self.locations[self.index - 1])
success = self.roomNav.move_to_trash_location(map_point.pose)
'''This part of the code strafes the robot left to get closer to the object'''
'''
rate = rospy.Rate(10)
position = Point()
move_cmd = Twist()
move_cmd.linear.y = 0.25
odom_frame = '/odom_combined'
# Find out if the robot uses /base_link or /base_footprint
try:
self._tf_listener.waitForTransform(odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))
self.base_frame = '/base_footprint'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
try:
self._tf_listener.waitForTransform(odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0))
self.base_frame = '/base_link'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("Cannot find transform between " + odom_frame + " and /base_link or /base_footprint")
rospy.signal_shutdown("tf Exception")
# Get current position
position = self.get_odom()
x_start = position.x
y_start = position.y
# Distance travelled
distance = 0
goal_distance = 0.25
rospy.loginfo("Strafing left")
# Enter the loop to move along a side
while distance < goal_distance:
rospy.loginfo("Distance is at " + str(distance))
# Publish the Twist message and sleep 1 cycle
self.base_action(0, 0.25, 0, 0, 0, 0)
rate.sleep()
# Get the current position
position = self.get_odom()
# Compute the Euclidean distance from the start
distance = abs(position.y - y_start)
'''
if (success):
# Move head to look at the object, this will wait for a result
self.head_action(0, 0.4, 0.55, True)
# Move arms to ready pickup position, this will wait for a result before trying to detect and pick up object
self.animPlay.left_poses = self.saved_animations[
'ready_pickup'].left
self.animPlay.right_poses = self.saved_animations[
'ready_pickup'].right
self.animPlay.left_gripper_states = self.saved_animations[
'ready_pickup'].left_gripper
self.animPlay.right_gripper_states = self.saved_animations[
'ready_pickup'].right_gripper
self.animPlay.play('3.0')
for i in range(0, 3):
success = self.pap.detect_and_pickup()
# Move head back to look forward
# Move head to look at the object, this will wait for a result
self.head_action(1.0, 0.0, 0.55, True)
if success:
break
# Move to bin
self.move_to_bin_action()
def get_odom(self):
# Get the current transform between the odom and base frames
try:
trans = self._tf_listener.lookupTransform('/odom_combined',
self.base_frame,
rospy.Time(0))
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("TF Exception")
return
return Point(trans[0][0], trans[0][1], trans[0][2])
# gripper_type is either 'l' for left or 'r' for right
# gripper position is the position as a parameter to the gripper goal
def gripper_action(self, gripper_type, gripper_position):
name_space = '/' + gripper_type + '_gripper_controller/gripper_action'
gripper_client = SimpleActionClient(name_space, GripperCommandAction)
gripper_client.wait_for_server()
gripper_goal = GripperCommandGoal()
gripper_goal.command.position = gripper_position
gripper_goal.command.max_effort = 30.0
gripper_client.send_goal(gripper_goal)
# update the state of the current gripper being modified
if (gripper_type == 'l'):
if (gripper_position == 0.0):
self.left_gripper_state = 'closed'
else:
self.left_gripper_state = 'open'
else:
if (gripper_position == 0.0):
self.right_gripper_state = 'closed'
else:
self.right_gripper_state = 'open'
def base_action(self, x, y, z, theta_x, theta_y, theta_z):
topic_name = '/base_controller/command'
base_publisher = rospy.Publisher(topic_name, Twist)
twist_msg = Twist()
twist_msg.linear = Vector3(x, y, z)
twist_msg.angular = Vector3(theta_x, theta_y, theta_z)
base_publisher.publish(twist_msg)
# Moves the torso of the robot down to its maximum
def torso_down(self, wait=False):
self.torso_client = SimpleActionClient(
'/torso_controller/position_joint_action',
SingleJointPositionAction)
torso_goal = SingleJointPositionGoal()
torso_goal.position = 0.0
torso_goal.min_duration = rospy.Duration(5.0)
torso_goal.max_velocity = 1.0
self.torso_client.send_goal(torso_goal)
if wait:
# wait for the head movement to finish before we try to detect and pickup an object
finished_within_time = self.torso_client.wait_for_result(
rospy.Duration(15))
# Check for success or failure
if not finished_within_time:
self.torso_client.cancel_goal()
rospy.loginfo("Timed out achieving torso movement goal")
else:
state = self.torso_client.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Torso movement goal succeeded!")
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo(
"Torso movement goal failed with error code: " +
str(state))
def head_action(self, x, y, z, wait=False):
name_space = '/head_traj_controller/point_head_action'
head_client = SimpleActionClient(name_space, PointHeadAction)
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = 'base_link'
head_goal.min_duration = rospy.Duration(1.0)
head_goal.target.point = Point(x, y, z)
head_client.send_goal(head_goal)
if wait:
# wait for the head movement to finish before we try to detect and pickup an object
finished_within_time = head_client.wait_for_result(
rospy.Duration(5))
# Check for success or failure
if not finished_within_time:
head_client.cancel_goal()
rospy.loginfo("Timed out achieving head movement goal")
else:
state = head_client.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Head movement goal succeeded!")
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo(
"Head movement goal failed with error code: " +
str(self.goal_states[state]))
def tuck_arms(self):
rospy.loginfo('Left tuck arms')
self.animPlay.left_poses = self.saved_animations['left_tuck'].left
self.animPlay.right_poses = self.saved_animations['left_tuck'].right
self.animPlay.left_gripper_states = self.saved_animations[
'left_tuck'].left_gripper
self.animPlay.right_gripper_states = self.saved_animations[
'left_tuck'].right_gripper
self.animPlay.play('3.0')
def move_to_bin_action(self):
# First tuck arms
self.tuck_arms()
# Move to the bin
rospy.loginfo('Clicked the move to bin button')
self.roomNav.move_to_bin()
# Throw the trash away
rospy.loginfo('Throwing trash away with left arm')
self.animPlay.left_poses = self.saved_animations['l_dispose'].left
self.animPlay.right_poses = self.saved_animations['l_dispose'].right
self.animPlay.left_gripper_states = self.saved_animations[
'l_dispose'].left_gripper
self.animPlay.right_gripper_states = self.saved_animations[
'l_dispose'].right_gripper
self.animPlay.play('2.0')
# Tuck arms again
self.tuck_arms()
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
def show_text_in_rviz(self, text):
marker = Marker(type=Marker.TEXT_VIEW_FACING,
id=0,
lifetime=rospy.Duration(1.5),
pose=Pose(Point(0.5, 0.5, 1.45),
Quaternion(0, 0, 0, 1)),
scale=Vector3(0.06, 0.06, 0.06),
header=Header(frame_id='base_link'),
color=ColorRGBA(0.0, 1.0, 0.0, 0.8),
text=text)
self.marker_publisher.publish(marker)
def show_arrow_in_rviz(self, arrow):
marker = Marker(type=Marker.ARROW,
id=0,
lifetime=rospy.Duration(1.5),
pose=Pose(Point(0.5, 0.5, 1.45),
Quaternion(0, 0, 0, 1)),
scale=Vector3(0.06, 0.06, 0.06),
header=Header(frame_id='base_link'),
color=ColorRGBA(0.0, 1.0, 0.0, 0.8),
arrow=arrow)
self.marker_publisher.publish(marker)
def save_animation(self, animation_name):
if animation_name != '':
filename = os.path.join(self.dir, 'animations/')
anim_file = open(filename + animation_name + '.txt', 'w')
l_animation_queue = []
r_animation_queue = []
l_gripper_queue = []
r_gripper_queue = []
for i in range(self.list_widget.count()):
item = self.list_widget.item(i)
pose_name = item.text()
anim_file.write(
re.sub(
',', '',
str(self.animation_map[pose_name].left).strip('[]') +
' ' +
str(self.animation_map[pose_name].right).strip('[]')))
anim_file.write('\n')
anim_file.write(
str(self.animation_map[pose_name].left_gripper) + ' ' +
str(self.animation_map[pose_name].right_gripper))
l_animation_queue.append(self.animation_map[pose_name].left)
r_animation_queue.append(self.animation_map[pose_name].right)
l_gripper_queue.append(
self.animation_map[pose_name].left_gripper)
r_gripper_queue.append(
self.animation_map[pose_name].right_gripper)
anim_file.write('\n')
anim_file.close()
self.saved_animations[animation_name] = Quad(
l_animation_queue, r_animation_queue, l_gripper_queue,
r_gripper_queue)
self.saved_animations_list.addItem(
animation_name) # Bug? Multiple entries
# Reset the pending queue
self.l_animation_queue = []
self.r_animation_queue = []
else:
self.show_warning('Please insert name for animation')
def gripper_click(self, button_name):
grip_side = ''
grip_side_text = ''
if ('Left' in button_name):
grip_side = 'l'
grip_side_text = 'left'
else:
grip_side = 'r'
grip_side_text = 'right'
if ('Open' in button_name):
grip_action = 20.0
grip_action_text = 'close'
qWarning('Robot opened %s gripper' % (grip_side_text))
else:
grip_action = 0.0
grip_action_text = 'open'
qWarning('Robot closed %s gripper' % (grip_side_text))
self.show_text_in_rviz(
"%sing %s Gripper" %
(grip_action_text.capitalize(), grip_side_text.capitalize()))
self.gripper_action(grip_side, grip_action)
self._widget.sender().setText(
'%s %s Gripper' %
(grip_action_text.capitalize(), grip_side_text.capitalize()))
def rotate_head(self, button_name):
if ('Left' in button_name):
#qWarning('x: %s, y: %s' % (self.head_x, self.head_y))
if (self.head_x < -0.8 and self.head_y > 0.0):
self.show_warning('Can\'t rotate anymore')
elif (self.head_y < 0.0):
self.head_x += 0.1
self.head_y = -((1.0 - self.head_x**2.0)**0.5)
self.show_text_in_rviz("Turning Head Left")
else:
self.head_x -= 0.1
self.head_y = (1.0 - self.head_x**2.0)**0.5
self.show_text_in_rviz("Turning Head Left")
qWarning('x: %s, y: %s' % (self.head_x, self.head_y))
self.head_action(self.head_x, self.head_y, self.head_z)
elif ('Up' in button_name):
if (self.head_z <= 1.6):
self.head_z += 0.1
self.show_text_in_rviz("Moving Head Up")
self.head_action(self.head_x, self.head_y, self.head_z)
else:
self.show_warning('Can\'t look up anymore')
elif ('Down' in button_name):
if (self.head_z >= -2.2):
self.head_z -= 0.1
self.show_text_in_rviz("Moving Head Down")
self.head_action(self.head_x, self.head_y, self.head_z)
else:
self.show_warning('Can\'t look down anymore')
else:
#qWarning('x: %s, y: %s' % (self.head_x, self.head_y))
if (self.head_x < -0.8 and self.head_y < 0.0):
self.show_warning('Can\'t rotate anymore')
elif (self.head_y > 0.0):
self.head_x += 0.1
self.head_y = (1.0 - self.head_x**2.0)**0.5
self.show_text_in_rviz("Turning Head Right")
else:
self.head_x -= 0.1
self.head_y = -((1.0 - self.head_x**2.0)**0.5)
self.show_text_in_rviz("Turning Head Right")
#qWarning('x: %s, y: %s' % (self.head_x, self.head_y))
self.head_action(self.head_x, self.head_y, self.head_z)
def toggle_arm(self, side, toggle, button):
controller_name = side + '_arm_controller'
start_controllers = []
stop_controllers = []
if (toggle == 'Relax'):
stop_controllers.append(controller_name)
else:
start_controllers.append(controller_name)
self.set_arm_mode(start_controllers, stop_controllers)
def set_arm_mode(self, start_controllers, stop_controllers):
try:
self.switch_service_client(start_controllers, stop_controllers, 1)
except rospy.ServiceException:
rospy.logerr('Could not change arm mode.')
def joint_states_cb(self, msg):
'''Callback function that saves the joint positions when a
joint_states message is received'''
self.lock.acquire()
self.all_joint_names = msg.name
self.all_joint_poses = msg.position
self.joint_sig.emit(msg)
self.lock.release()
def joint_sig_cb(self, msg):
pass
def get_joint_state(self, side_prefix):
'''Returns position for arm joints on the requested side (r/l)'''
if side_prefix == 'r':
joint_names = self.r_joint_names
else:
joint_names = self.l_joint_names
if self.all_joint_names == []:
rospy.logerr("No robot_state messages received yet!\n")
return None
positions = []
self.lock.acquire()
for joint_name in joint_names:
if joint_name in self.all_joint_names:
index = self.all_joint_names.index(joint_name)
position = self.all_joint_poses[index]
positions.append(position)
else:
rospy.logerr("Joint %s not found!", joint_name)
self.lock.release()
return None
self.lock.release()
return positions
def show_warning(self, text):
qWarning(text)
msgBox = QMessageBox()
msgBox.setText(text)
msgBox.exec_()
class PbDGUI(Plugin):
exp_state_sig = Signal(ExperimentState)
def __init__(self, context):
super(PbDGUI, self).__init__(context)
self.setObjectName('PbDGUI')
self._widget = QWidget()
self.speech_cmd_publisher = rospy.Publisher('recognized_command', Command)
self.gui_cmd_publisher = rospy.Publisher('gui_command', GuiCommand)
rospy.Subscriber('experiment_state', ExperimentState, self.exp_state_cb)
rospy.Subscriber('robotsound', SoundRequest, self.robotSoundReceived)
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
self.exp_state_sig.connect(self.update_state)
self.commands = dict()
self.commands[Command.TEST_MICROPHONE] = 'Test microphone'
self.commands[Command.START_RECORDING] = 'Start recording'
self.commands[Command.STOP_RECORDING] = 'Stop recording'
self.commands[Command.REPLAY_DEMONSTRATION] = 'Replay demonstration'
self.commands[Command.TAKE_TOOL] = 'Take tool'
self.commands[Command.RELEASE_TOOL] = 'Release tool'
self.currentAction = -1
self.currentStep = -1
allWidgetsBox = QtGui.QVBoxLayout()
actionBox = QGroupBox('Demonstrations', self._widget)
self.actionGrid = QtGui.QGridLayout()
self.actionGrid.setHorizontalSpacing(0)
for i in range(6):
self.actionGrid.addItem(QtGui.QSpacerItem(90, 90), 0, i, QtCore.Qt.AlignCenter)
self.actionGrid.setColumnStretch(i, 0)
self.actionIcons = dict()
actionBoxLayout = QtGui.QHBoxLayout()
actionBoxLayout.addLayout(self.actionGrid)
actionBox.setLayout(actionBoxLayout)
actionButtonGrid = QtGui.QHBoxLayout()
self.stepsBox = QGroupBox('No demonstrations', self._widget)
self.stepsGrid = QtGui.QGridLayout()
self.l_model = QtGui.QStandardItemModel(self)
self.l_view = self._create_table_view(self.l_model,
self.l_row_clicked_cb)
self.r_model = QtGui.QStandardItemModel(self)
self.r_view = self._create_table_view(self.r_model,
self.r_row_clicked_cb)
self.stepsGrid.addItem(QtGui.QSpacerItem(280, 10), 0, 0, 2, 3)
self.stepsGrid.addItem(QtGui.QSpacerItem(10, 10), 0, 1, 2, 3)
self.stepsGrid.addItem(QtGui.QSpacerItem(280, 10), 0, 2, 2, 3)
self.stepsGrid.addWidget(QtGui.QLabel('Left Arm'), 0, 0)
self.stepsGrid.addWidget(QtGui.QLabel('Right Arm'), 0, 2)
self.stepsGrid.addWidget(self.l_view, 1, 0)
self.stepsGrid.addWidget(self.r_view, 1, 2)
stepsBoxLayout = QtGui.QHBoxLayout()
stepsBoxLayout.addLayout(self.stepsGrid)
self.stepsBox.setLayout(stepsBoxLayout)
stepsButtonGrid = QtGui.QHBoxLayout()
stepsButtonGrid.addWidget(self.create_button(Command.REPLAY_DEMONSTRATION))
motionButtonGrid = QtGui.QHBoxLayout()
motionButtonGrid.addWidget(self.create_button(Command.START_RECORDING))
motionButtonGrid.addWidget(self.create_button(Command.STOP_RECORDING))
misc_grid = QtGui.QHBoxLayout()
misc_grid.addWidget(self.create_button(Command.TEST_MICROPHONE))
misc_grid.addStretch(1)
misc_grid2 = QtGui.QHBoxLayout()
misc_grid2.addWidget(self.create_button(Command.TAKE_TOOL))
misc_grid2.addWidget(self.create_button(Command.RELEASE_TOOL))
misc_grid2.addStretch(1)
speechGroupBox = QGroupBox('Robot Speech', self._widget)
speechGroupBox.setObjectName('RobotSpeechGroup')
speechBox = QtGui.QHBoxLayout()
self.speechLabel = QtGui.QLabel('Robot has not spoken yet')
palette = QtGui.QPalette()
palette.setColor(QtGui.QPalette.Foreground,QtCore.Qt.blue)
self.speechLabel.setPalette(palette)
speechBox.addWidget(self.speechLabel)
speechGroupBox.setLayout(speechBox)
allWidgetsBox.addWidget(actionBox)
allWidgetsBox.addLayout(actionButtonGrid)
allWidgetsBox.addWidget(self.stepsBox)
allWidgetsBox.addLayout(motionButtonGrid)
allWidgetsBox.addLayout(stepsButtonGrid)
allWidgetsBox.addItem(QtGui.QSpacerItem(100, 20))
allWidgetsBox.addLayout(misc_grid)
allWidgetsBox.addItem(QtGui.QSpacerItem(100, 20))
allWidgetsBox.addLayout(misc_grid2)
allWidgetsBox.addItem(QtGui.QSpacerItem(100, 40))
allWidgetsBox.addWidget(speechGroupBox)
allWidgetsBox.addStretch(1)
# Fix layout and add main widget to the user interface
QtGui.QApplication.setStyle(QtGui.QStyleFactory.create('plastique'))
vAllBox = QtGui.QVBoxLayout()
vAllBox.addLayout(allWidgetsBox)
vAllBox.addStretch(1)
hAllBox = QtGui.QHBoxLayout()
hAllBox.addLayout(vAllBox)
hAllBox.addStretch(1)
self._widget.setObjectName('PbDGUI')
self._widget.setLayout(hAllBox)
context.add_widget(self._widget)
rospy.loginfo('Will wait for the experiment state service...')
rospy.wait_for_service('get_experiment_state')
exp_state_srv = rospy.ServiceProxy('get_experiment_state',
GetExperimentState)
rospy.loginfo('Got response from the experiment state service...')
response = exp_state_srv()
self.update_state(response.state)
def _create_table_view(self, model, row_click_cb):
proxy = QtGui.QSortFilterProxyModel(self)
proxy.setSourceModel(model)
view = QtGui.QTableView(self._widget)
verticalHeader = view.verticalHeader()
verticalHeader.sectionClicked.connect(row_click_cb)
view.setModel(proxy)
view.setMaximumWidth(250)
view.setSortingEnabled(False)
view.setCornerButtonEnabled(False)
return view
def get_uid(self, arm_index, index):
'''Returns a unique id of the marker'''
return (2 * (index + 1) + arm_index)
def get_arm_and_index(self, uid):
'''Returns a unique id of the marker'''
arm_index = uid % 2
index = (uid - arm_index) / 2
return (arm_index, (index - 1))
def r_row_clicked_cb(self, logicalIndex):
self.step_pressed(self.get_uid(0, logicalIndex))
def l_row_clicked_cb(self, logicalIndex):
self.step_pressed(self.get_uid(1, logicalIndex))
def create_button(self, command):
btn = QtGui.QPushButton(self.commands[command], self._widget)
btn.clicked.connect(self.command_cb)
return btn
def update_state(self, state):
qWarning('Received new state')
n_actions = len(self.actionIcons.keys())
if n_actions < state.n_actions:
for i in range(n_actions, state.n_actions):
self.new_action()
if (self.currentAction != (state.i_current_action - 1)):
self.delete_all_steps()
self.action_pressed(state.i_current_action - 1, False)
n_steps = self.n_steps()
if (n_steps < state.n_steps):
for i in range(n_steps, state.n_steps):
self.save_pose(frameType=ord(state.frame_types[i]))
elif (n_steps > state.n_steps):
n_to_remove = n_steps - state.n_steps
self.r_model.invisibleRootItem().removeRows(state.n_steps,
n_to_remove)
self.l_model.invisibleRootItem().removeRows(state.n_steps,
n_to_remove)
## TODO: DEAL with the following arrays!!!
state.r_gripper_states
state.l_gripper_states
state.r_ref_frames
state.l_ref_frames
state.objects
if (self.currentStep != state.i_current_step):
if (self.n_steps() > 0):
self.currentStep = state.i_current_step
arm_index, index = self.get_arm_and_index(self.currentStep)
if (arm_index == 0):
self.r_view.selectRow(index)
else:
self.l_view.selectRow(index)
def get_frame_type(self, fr_type):
if (fr_type > 1):
rospy.logwarn("Invalid frame type @ save_pose -> get_frame_type: " + str(fr_type))
return ["Go to pose", "Maneuver"][fr_type]
def save_pose(self, actionIndex=None, frameType=0):
nColumns = 9
if actionIndex is None:
actionIndex = self.currentAction
stepIndex = self.n_steps(actionIndex)
r_step = [QtGui.QStandardItem('Step' + str(stepIndex + 1)),
QtGui.QStandardItem(self.get_frame_type(frameType)),
QtGui.QStandardItem('Absolute')]
l_step = [QtGui.QStandardItem('Step' + str(stepIndex + 1)),
QtGui.QStandardItem(self.get_frame_type(frameType)),
QtGui.QStandardItem('Absolute')]
self.r_model.invisibleRootItem().appendRow(r_step)
self.l_model.invisibleRootItem().appendRow(l_step)
self.update_table_view()
self.currentStep = stepIndex
def update_table_view(self):
self.l_view.setColumnWidth(0, 50)
self.l_view.setColumnWidth(1, 100)
self.l_view.setColumnWidth(2, 70)
self.r_view.setColumnWidth(0, 50)
self.r_view.setColumnWidth(1, 100)
self.r_view.setColumnWidth(2, 70)
def n_steps(self, actionIndex=None):
return self.l_model.invisibleRootItem().rowCount()
def delete_all_steps(self, actionIndex=None):
if actionIndex is None:
actionIndex = self.currentAction
n_steps = self.n_steps()
if (n_steps > 0):
self.l_model.invisibleRootItem().removeRows(0, n_steps)
self.r_model.invisibleRootItem().removeRows(0, n_steps)
def n_actions(self):
return len(self.actionIcons.keys())
def new_action(self):
nColumns = 6
actionIndex = self.n_actions()
for key in self.actionIcons.keys():
self.actionIcons[key].selected = False
self.actionIcons[key].updateView()
actIcon = ActionIcon(self._widget, actionIndex, self.action_pressed)
self.actionGrid.addLayout(actIcon, int(actionIndex/nColumns),
actionIndex%nColumns)
self.actionIcons[actionIndex] = actIcon
def step_pressed(self, step_index):
gui_cmd = GuiCommand(GuiCommand.SELECT_ACTION_STEP, step_index)
self.gui_cmd_publisher.publish(gui_cmd)
def action_pressed(self, actionIndex, isPublish=True):
for i in range(len(self.actionIcons.keys())):
key = self.actionIcons.keys()[i]
if key == actionIndex:
self.actionIcons[key].selected = True
else:
self.actionIcons[key].selected = False
self.actionIcons[key].updateView()
self.currentAction = actionIndex
self.stepsBox.setTitle('Steps for Action ' + str(self.currentAction+1))
if isPublish:
gui_cmd = GuiCommand(GuiCommand.SWITCH_TO_ACTION, (actionIndex+1))
self.gui_cmd_publisher.publish(gui_cmd)
def command_cb(self):
clickedButtonName = self._widget.sender().text()
for key in self.commands.keys():
if (self.commands[key] == clickedButtonName):
qWarning('Sending speech command: '+ key)
command = Command()
command.command = key
self.speech_cmd_publisher.publish(command)
def robotSoundReceived(self, soundReq):
if (soundReq.command == SoundRequest.SAY):
qWarning('Robot said: ' + soundReq.arg)
self.speechLabel.setText('Robot sound: ' + soundReq.arg)
def exp_state_cb(self, state):
qWarning('Received new experiment state.')
self.exp_state_sig.emit(state)
def shutdown_plugin(self):
# TODO unregister all publishers here
self.speech_cmd_publisher.unregister()
self.gui_cmd_publisher.unregister()
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
class OculusTeleopGUI(Plugin):
joint_sig = Signal(JointState)
sound_sig = Signal(SoundRequest)
# Constants to help us move the robot
HEAD_THETA = 0.0
HEAD_Z = 1.0
LEFT_GRIPPER_POSITION = 0.0
RIGHT_GRIPPER_POSITION = 0.0
GRIPPER_INTENSITY = 0.0
MOTION_INTENSITY = 0.0
ARM_INTENSITY = 0.0
r_arm_pose_box = QtGui.QComboBox()
l_arm_pose_box = QtGui.QComboBox()
def __init__(self, context):
super(OculusTeleopGUI, self).__init__(context)
self.setObjectName('OculusTeleopGUI')
self._widget = QWidget()
self.saved_r_arm_pose = dict()
self.saved_l_arm_pose = dict()
self.switchnode = SwitchNode()
self.armnode = ArmNode()
self.grippernode = GripperNode()
self.speaknode = SpeakNode()
# Logging variables
self.fileName = ""
self.fileNum = 1
self.isLogging = False
self.logRate = 10.0 # Hz
# Add Clients
self.marker_publisher = rospy.Publisher('visualization_marker', Marker)
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
self.sound_sig.connect(self.sound_sig_cb)
# Main layout element for GUI
large_box = QtGui.QVBoxLayout()
palette = QtGui.QPalette()
palette.setColor(QtGui.QPalette.Foreground,QtCore.Qt.blue)
# Create Button box for GUI
button_box = QtGui.QGridLayout()
# Add The Buttons to button box here
button_box.addWidget(self.create_button('Stack Init'), 1, 1)
button_box.addWidget(self.create_button('Drive Init'), 1, 3)
button_box.addWidget(self.create_button('Set Rate'), 3, 1)
button_box.addWidget(self.create_button('Get Rate'), 3, 3)
button_box.addWidget(self.create_button('Start Log'), 5, 1)
button_box.addWidget(self.create_button('Stop Log'), 5, 3)
# Text box for logging rate
self.rate_label = QtGui.QLabel('Enter Logging Rate (Hz): ')
self.rate_label.setPalette(palette)
button_box.addWidget(self.rate_label, 2, 1)
self.rate_text_in = QtGui.QLineEdit('10')
button_box.addWidget(self.rate_text_in, 2, 3)
# Text box for file names
self.file_label = QtGui.QLabel('Enter Log File Name: ')
self.file_label.setPalette(palette)
button_box.addWidget(self.file_label, 4, 1)
self.file_text_in = QtGui.QLineEdit('Default')
button_box.addWidget(self.file_text_in, 4, 3)
# Add Button Box to main GUI layout element
large_box.addLayout(button_box)
large_box.addItem(QtGui.QSpacerItem(100,20))
# Speech box for speech display created
speech_box = QtGui.QVBoxLayout()
self.speech_label = QtGui.QLabel('Robot is ready to run, press button to start')
self.speech_label.setPalette(palette)
speech_box.addWidget(self.speech_label)
# Speech box added to main layout
large_box.addLayout(speech_box)
# Puts layout in widget and adds widget name for RosGUI to identify
self._widget.setObjectName('OculusTeleopGUI')
self._widget.setLayout(large_box)
context.add_widget(self._widget)
rospy.loginfo('GUI initialization complete.')
# Will create a button, with all buttons using same event trigger
def create_button(self, name):
btn = QtGui.QPushButton(name, self._widget)
if (name == 'Stack Init'):
btn.clicked.connect(self.command_cb)
elif (name == 'Drive Init'):
btn.clicked.connect(self.command_cb)
elif (name == 'Set Rate'):
btn.clicked.connect(self.command_cb)
elif (name == 'Get Rate'):
btn.clicked.connect(self.command_cb)
elif (name == 'Start Log'):
btn.clicked.connect(self.command_cb)
elif (name == 'Stop Log'):
btn.clicked.connect(self.command_cb)
return btn
# Triggered when request to sound_play is made
def sound_sig_cb(self, sound_request):
qWarning('Received sound signal.')
#if (sound_request.command == SoundRequest.SAY):
qWarning('Robot said: ' + sound_request.arg)
# self.speech_label.setText('Robot said: ' + sound_request.arg)
# Triggered When Subscribber to robot sound detects a message
def sound_cb(self, sound_request):
qWarning('Received sound.')
self.sound_sig.emit(sound_request)
return
# Triggered when button is pressed: Place response actions here
def command_cb(self):
button_name = self._widget.sender().text()
speechLabel = '';
if (button_name == 'Stack Init'):
self.armnode.stack_init_motion()
speechLabel = 'is initializing arms for stacking';
elif (button_name == 'Drive Init'):
self.armnode.fold_motion()
speechLabel = 'is initializing arms for driving';
elif (button_name == 'Set Rate'):
self.setRate()
speechLabel = 'set logging rate to ' + str(self.logRate) + 'Hz.'
elif (button_name == 'Get Rate'):
qWarning('Logging Rate set to ' + str(self.logRate) + 'Hz')
speechLabel = 'is ready to log at ' + str(self.logRate) + 'Hz.'
elif (button_name == 'Start Log'):
self.start_logger()
speechLabel = 'has started logging.'
elif (button_name == 'Stop Log'):
self.logger.closeAllBags()
self.isLogging = False
speechLabel = 'has stopped logging.'
self.speech_label.setText('Robot ' + speechLabel)
def start_logger(self):
if (not self.isLogging):
name = self.file_text_in.text()
if (self.fileName == name):
self.fileNum += 1
else:
self.fileNum = 1
self.fileName = name
self.logger = Logger(self.fileName, str(self.fileNum), 1.0/self.logRate)
rospy.on_shutdown(self.logger.closeAllBags)
self.isLogging = True
def setRate(self):
if (not self.isLogging):
rate = self.rate_text_in.text()
self.logRate = float(rate)
def shutdown_plugin(self):
# TODO unregister all publishers here
# Leave both arm controllers on
start_controllers = ['r_arm_controller', 'l_arm_controller']
stop_controllers = []
self.armnode.set_arm_mode(start_controllers, stop_controllers, self.switchnode)
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
class SimpleGUI(Plugin):
sound_sig = Signal(SoundRequest)
def __init__(self, context):
super(SimpleGUI, self).__init__(context)
self.setObjectName('SimpleGUI')
self._widget = QWidget()
self._widget.setFixedSize(600, 600)
self._sound_client = SoundClient()
rospy.Subscriber('robotsound', SoundRequest, self.sound_cb)
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
self.sound_sig.connect(self.sound_sig_cb)
large_box = QtGui.QVBoxLayout()
# Textbox to enter words for PR2 to say
sound_textbox = QtGui.QLineEdit("Squirtle Squirtle") # Default Text
sound_textbox.setFixedWidth(450)
self.marker_publisher = rospy.Publisher('visualization_marker', Marker)
# Set a handler on the textbox to retrieve the text when button clicked
self.sound_textbox = sound_textbox
button_box = QtGui.QHBoxLayout()
button_box.addItem(QtGui.QSpacerItem(15, 20))
button_box.addWidget(self.create_button('Speak', self.command_cb))
button_box.addWidget(sound_textbox)
button_box.addStretch(1)
large_box.addLayout(button_box)
speech_box = QtGui.QHBoxLayout()
speech_box.addItem(QtGui.QSpacerItem(15, 20))
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.addWidget(self.speech_label)
large_box.addLayout(speech_box)
large_box.addStretch(1)
#large_box.addItem(QtGui.QSpacerItem(50,20))
# Buttons to move the PR2's head
up_head = Head(Head.UP, self)
head_box = QtGui.QVBoxLayout()
up_head_box = QtGui.QHBoxLayout()
up_head_button = self.create_button('^', up_head.create_closure())
down_head = Head(Head.DOWN, self)
down_head_box = QtGui.QHBoxLayout()
down_head_button = self.create_button('v', down_head.create_closure())
right_head = Head(Head.RIGHT, self)
right_head_button = self.create_button('>',
right_head.create_closure())
left_head = Head(Head.LEFT, self)
left_head_button = self.create_button('<', left_head.create_closure())
left_right_head_box = QtGui.QHBoxLayout()
up_head_box.addItem(QtGui.QSpacerItem(235, 20))
up_head_box.addWidget(up_head_button)
up_head_box.addItem(QtGui.QSpacerItem(275, 20))
left_right_head_box.addItem(QtGui.QSpacerItem(160, 20))
left_right_head_box.addWidget(left_head_button)
left_right_head_box.addItem(QtGui.QSpacerItem(60, 20))
left_right_head_box.addWidget(right_head_button)
left_right_head_box.addItem(QtGui.QSpacerItem(225, 20))
down_head_box.addItem(QtGui.QSpacerItem(235, 20))
down_head_box.addWidget(down_head_button)
down_head_box.addItem(QtGui.QSpacerItem(275, 20))
head_box.addLayout(up_head_box)
head_box.addLayout(left_right_head_box)
head_box.addLayout(down_head_box)
large_box.addLayout(head_box)
# Buttons to move the grippers
gripper = Gripper(Gripper.RIGHT, Gripper.OPEN, self)
right_gripper = self.create_button('Right gripper',
gripper.create_closure())
gripper = Gripper(Gripper.LEFT, Gripper.OPEN, self)
left_gripper = self.create_button('Left gripper',
gripper.create_closure())
large_box.addItem(QtGui.QSpacerItem(100, 250))
gripper_box = QtGui.QHBoxLayout()
gripper_box.addItem(QtGui.QSpacerItem(75, 20))
gripper_box.addWidget(left_gripper)
gripper_box.addItem(QtGui.QSpacerItem(450, 20))
gripper_box.addWidget(right_gripper)
gripper_box.addItem(QtGui.QSpacerItem(75, 20))
large_box.addLayout(gripper_box)
# Buttons to move the base
base_box = QtGui.QVBoxLayout()
large_box.addItem(QtGui.QSpacerItem(100, 100))
forward_base_box = QtGui.QHBoxLayout()
forward_base = Base(Base.FORWARD, self)
forward_base_button = self.create_button('move forward',
forward_base.create_closure())
forward_base_box.addItem(QtGui.QSpacerItem(400, 20))
forward_base_box.addWidget(forward_base_button)
forward_base_box.addItem(QtGui.QSpacerItem(400, 20))
base_box.addLayout(forward_base_box)
left_right_base_box = QtGui.QHBoxLayout()
left_base = Base(Base.LEFT, self)
left_base_button = self.create_button('move left',
left_base.create_closure())
right_base = Base(Base.RIGHT, self)
right_base_button = self.create_button('move right',
right_base.create_closure())
left_right_base_box.addItem(QtGui.QSpacerItem(300, 20))
left_right_base_box.addWidget(left_base_button)
left_right_base_box.addItem(QtGui.QSpacerItem(50, 20))
left_right_base_box.addWidget(right_base_button)
left_right_base_box.addItem(QtGui.QSpacerItem(300, 20))
base_box.addLayout(left_right_base_box)
backward_base_box = QtGui.QHBoxLayout()
backward_base = Base(Base.BACKWARD, self)
backward_base_button = self.create_button(
'move backward', backward_base.create_closure())
backward_base_box.addItem(QtGui.QSpacerItem(400, 20))
backward_base_box.addWidget(backward_base_button)
backward_base_box.addItem(QtGui.QSpacerItem(400, 20))
base_box.addLayout(backward_base_box)
large_box.addLayout(base_box)
turn_base_box = QtGui.QHBoxLayout()
counter_base = Base(Base.COUNTER, self)
counter_base_button = self.create_button('\\\n -->',
counter_base.create_closure())
clockwise_base = Base(Base.CLOCKWISE, self)
clockwise_base_button = self.create_button(
' /\n<--', clockwise_base.create_closure())
turn_base_box.addItem(QtGui.QSpacerItem(75, 20))
turn_base_box.addWidget(counter_base_button)
turn_base_box.addItem(QtGui.QSpacerItem(225, 20))
turn_base_box.addWidget(clockwise_base_button)
turn_base_box.addItem(QtGui.QSpacerItem(100, 20))
large_box.addLayout(turn_base_box)
self._widget.setObjectName('SimpleGUI')
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__))) +
"/../../rosie_background.jpg"))
def show_text_in_rviz(self, text):
marker = Marker(type=Marker.TEXT_VIEW_FACING,
id=0,
lifetime=rospy.Duration(1.5),
pose=Pose(Point(0.5, 0.5, 1.45),
Quaternion(0, 0, 0, 1)),
scale=Vector3(0.06, 0.06, 0.06),
header=Header(frame_id='base_link'),
color=ColorRGBA(0.0, 1.0, 0.0, 0.8),
text=text)
self.marker_publisher.publish(marker)
def sound_cb(self, sound_request):
qWarning('Received sound.')
self.sound_sig.emit(sound_request)
def create_button(self, name, method):
btn = QtGui.QPushButton(name, self._widget)
btn.clicked.connect(method)
btn.setAutoRepeat(True)
return btn
def sound_sig_cb(self, sound_request):
qWarning('Received sound signal.')
#if (sound_request.command == SoundRequest.SAY):
qWarning('Robot said: ' + sound_request.arg)
self.speech_label.setText('Robot said: ' + sound_request.arg)
def command_cb(self):
button_name = self._widget.sender().text()
if (button_name == 'Speak'):
qWarning('Robot will say: ' + self.sound_textbox.text())
self._sound_client.say(self.sound_textbox.text())
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
class SimpleGUI(Plugin):
sound_sig = Signal(SoundRequest)
def __init__(self, context):
super(SimpleGUI, self).__init__(context)
self.setObjectName('SimpleTeleopGUI')
self._widget = QWidget()
# INIT CLIENTS
self._gripper_client = GripperClient()
self._base_publisher = BasePublisher()
self._sound_client = SoundClient()
self._head_client = HeadClient()
rospy.Subscriber('robotsound', SoundRequest, self.sound_cb)
self.sound_sig.connect(self.sound_sig_cb)
self._base_speed = 0.5
# OVERALL LAYOUT
large_box = QtGui.QVBoxLayout()
upper_box = QtGui.QVBoxLayout()
large_box.addLayout(upper_box)
lower_box = QtGui.QHBoxLayout()
large_box.addLayout(lower_box)
gripper_box = QtGui.QHBoxLayout()
lower_box.addLayout(gripper_box)
# GRIPPER CONTROLS
gripper_box_label = QtGui.QLabel('Grippers: ')
gripper_box.addWidget(gripper_box_label)
gripper_left_btns = QtGui.QVBoxLayout()
g_left_open_btn = QtGui.QPushButton('Open L', self._widget)
g_left_open_btn.clicked.connect(self.gripper_cb)
gripper_left_btns.addWidget(g_left_open_btn)
g_left_close_btn = QtGui.QPushButton('Close L', self._widget)
g_left_close_btn.clicked.connect(self.gripper_cb)
gripper_left_btns.addWidget(g_left_close_btn)
gripper_box.addLayout(gripper_left_btns)
gripper_right_btns = QtGui.QVBoxLayout()
g_right_open_btn = QtGui.QPushButton('Open R', self._widget)
g_right_open_btn.clicked.connect(self.gripper_cb)
gripper_right_btns.addWidget(g_right_open_btn)
g_right_close_btn = QtGui.QPushButton('Close R', self._widget)
g_right_close_btn.clicked.connect(self.gripper_cb)
gripper_right_btns.addWidget(g_right_close_btn)
gripper_box.addLayout(gripper_right_btns)
lower_box.addItem(QtGui.QSpacerItem(100,20))
# SPEECH CONTROLS
speech_box = QtGui.QHBoxLayout()
speech_label = QtGui.QLabel('Speech: ')
speech_box.addWidget(speech_label)
self.speech_text = QtGui.QLineEdit(self._widget);
speech_box.addWidget(self.speech_text);
speech_box.addWidget(self.create_btn('Speak'))
speech_box.addStretch(1)
lower_box.addLayout(speech_box)
large_box.addItem(QtGui.QSpacerItem(100,20))
# 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.addWidget(self.speech_label)
# large_box.addLayout(speech_box)
# BASE MOVEMENT CONTROLS
base_controls = QtGui.QHBoxLayout()
base_label = QtGui.QLabel('Base Movement: ')
base_label.setGeometry(100,100,100,100)
move_speed_controls = QtGui.QVBoxLayout()
move_speed_controls.addWidget(base_label)
move_speed_label = QtGui.QLabel('Move Speed')
move_speed_controls.addWidget(move_speed_label)
move_speed_sldr = self.create_vert_sldr()
move_speed_sldr.valueChanged[int].connect(self._change_move_speed)
move_speed_controls.addWidget(move_speed_sldr)
move_base_controls = QtGui.QVBoxLayout()
move_fwd_controls = QtGui.QHBoxLayout()
move_fwd_right_btn = self.create_toggle_btn("Forward Left")
move_fwd_right_btn.clicked.connect(self.make_move_base_cb(0.3))
move_fwd_controls.addWidget(move_fwd_right_btn)
move_fwd_btn = self.create_toggle_btn("Forward")
move_fwd_btn.clicked.connect(self.make_move_base_cb(0.3))
move_fwd_controls.addWidget(move_fwd_btn)
move_fwd_left_btn = self.create_toggle_btn("Forward Right")
move_fwd_left_btn.clicked.connect(self.make_move_base_cb(0.3))
move_fwd_controls.addWidget(move_fwd_left_btn)
turn_controls = QtGui.QHBoxLayout()
turn_left_btn = self.create_toggle_btn("Turn Left")
turn_left_btn.clicked.connect(self.make_move_base_cb(0.3))
turn_controls.addWidget(turn_left_btn)
stop_btn = self.create_toggle_btn("STOP")
turn_controls.addWidget(stop_btn)
turn_right_btn = self.create_toggle_btn("Turn Right")
turn_right_btn.clicked.connect(self.make_move_base_cb(0.3))
turn_controls.addWidget(turn_right_btn)
move_bkwd_controls = QtGui.QHBoxLayout()
move_bkwd_right_btn = self.create_toggle_btn("Backward Left")
move_bkwd_right_btn.clicked.connect(self.make_move_base_cb(0.3))
move_bkwd_controls.addWidget(move_bkwd_right_btn)
move_bkwd_btn = self.create_toggle_btn("Backward")
move_bkwd_btn.clicked.connect(self.make_move_base_cb(0.3))
move_bkwd_controls.addWidget(move_bkwd_btn)
move_bkwd_left_btn = self.create_toggle_btn("Backward Right")
move_bkwd_left_btn.clicked.connect(self.make_move_base_cb(0.3))
move_bkwd_controls.addWidget(move_bkwd_left_btn)
move_base_controls.addLayout(move_fwd_controls)
move_base_controls.addLayout(turn_controls)
move_base_controls.addLayout(move_bkwd_controls)
base_controls.addLayout(move_speed_controls)
base_controls.addLayout(move_base_controls)
upper_box.addLayout(base_controls)
# HEAD CONTROLS
move_head_controls = QtGui.QHBoxLayout()
head_label = QtGui.QLabel('Head Position: ')
move_head_controls.addWidget(head_label)
move_vert_controls = QtGui.QHBoxLayout()
move_vert_sldr = self.create_vert_sldr()
move_vert_sldr.valueChanged[int].connect(self.move_head_vert)
move_hor_controls = QtGui.QHBoxLayout()
move_hor_sldr = self.create_hor_sldr()
move_hor_sldr.valueChanged[int].connect(self.move_head_hor)
move_vert_controls.addWidget(move_vert_sldr)
move_hor_controls.addWidget(move_hor_sldr)
move_head_controls.addLayout(move_vert_controls)
move_head_controls.addLayout(move_hor_controls)
upper_box.addLayout(move_head_controls)
# ARM CONTROLS
arm_controls = QtGui.QVBoxLayout()
arm_r_controls = QtGui.QVBoxLayout()
arm_l_controls = QtGui.QVBoxLayout()
arm_r_controls.addWidget(QtGui.QLabel('Right Arm: '))
arm_l_controls.addWidget(QtGui.QLabel('Left Arm: '))
arm_l_control_grid = QtGui.QGridLayout()
arm_r_control_grid = QtGui.QGridLayout()
#for i in range(1, 4):
# slider = QtGui.QSlider(QtCore.Qt.Vertical, self._widget)
# slider.setFocusPolicy(QtCore.Qt.NoFocus)
# slider.setGeometry(30, 40, 30, 100)
#arm_l_control_grid.addWidget(slider, 0, i-1)
#arm_l_control_grid.addWidget(QtGui.QLabel(str(i)), 1, i-1)
#for i in range(1, 4):
# slider = QtGui.QSlider(QtCore.Qt.Vertical, self._widget)
# slider.setFocusPolicy(QtCore.Qt.NoFocus)
# slider.setGeometry(30, 40, 30, 100)
#arm_r_control_grid.addWidget(slider, 0, i-1)
#arm_r_control_grid.addWidget(QtGui.QLabel(str(i)), 1, i-1)
arm_l_controls.addLayout(arm_l_control_grid)
arm_r_controls.addLayout(arm_r_control_grid)
arm_controls.addLayout(arm_l_controls)
arm_controls.addLayout(arm_r_controls)
# large_box.addLayout(arm_controls)
# SET EVERYTHING
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
self._widget.setObjectName('SimpleGUI')
self._widget.setLayout(large_box)
context.add_widget(self._widget)
def sound_cb(self, sound_request):
qWarning('Received sound.')
self.sound_sig.emit(sound_request)
def create_btn(self, name):
btn = QtGui.QPushButton(name, self._widget)
btn.clicked.connect(self.command_cb)
return btn
def create_toggle_btn(self, name):
btn = QtGui.QPushButton(name, self._widget)
btn.setAutoRepeat(True)
btn.setAutoRepeatInterval(10)
return btn
def create_vert_sldr(self):
sldr = QtGui.QSlider()
sldr.setSliderPosition(50)
return sldr
def create_hor_sldr(self):
sldr = QtGui.QSlider(QtCore.Qt.Horizontal)
sldr.setSliderPosition(50)
return sldr
def sound_sig_cb(self, sound_request):
qWarning('Received sound signal.')
#if (sound_request.command == SoundRequest.SAY):
qWarning('Robot said: ' + sound_request.arg)
#self.speech_label.setText('Robot said: ' + sound_request.arg)
def command_cb(self):
btn_name = self._widget.sender().text()
if (btn_name == 'Speak'):
text = self.speech_text.text()
qWarning('Robot will say: ' + text)
self._sound_client.say(text)
def gripper_cb(self):
btn_name = self._widget.sender().text()
if btn_name == 'Open L':
self._gripper_client.command(True, False)
elif btn_name == 'Open R':
self._gripper_client.command(False, False)
elif btn_name == 'Close L':
self._gripper_client.command(True, True)
elif btn_name == 'Close R':
self._gripper_client.command(False, True)
def make_move_base_cb(self, speed):
def callback():
self.move_base_cb(self._base_speed)
return callback
def move_base_cb(self, speed):
qWarning('Received movement command.')
btn_name = self._widget.sender().text()
if btn_name == 'Forward':
self._base_publisher.move_fwd_straight(speed)
elif btn_name == 'Forward Right':
self._base_publisher.move_fwd_right(speed)
elif btn_name == 'Forward Left':
self._base_publisher.move_fwd_left(speed)
elif btn_name == 'Turn Left':
self._base_publisher.turn_left(speed)
elif btn_name == 'Turn Right':
self._base_publisher.turn_right(speed)
elif btn_name == 'Backward':
self._base_publisher.move_bkwd_straight(speed)
elif btn_name == 'Backward Right':
self._base_publisher.move_bkwd_right(speed)
elif btn_name == 'Backward Left':
self._base_publisher.move_bkwd_left(speed)
def _change_move_speed(self, speed_percent):
self._base_speed = speed_percent / 100.0
def move_head_vert(self, pos):
self._head_client.move_head_vert(pos)
def move_head_hor(self, pos):
self._head_client.move_head_hor(pos)
def shutdown_plugin(self):
# nothing to cleanup
pass
def save_settings(self, plugin_settings, instance_settings):
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# v = instance_settings.value(k)
pass
class WaterPulse(Plugin):
sound_sig = Signal(SoundRequest)
positions = [[
-0.013001995432544766, -0.24895825213211362, -0.0055992576345036404,
-1.7647281786034612, -496.3549908032631, -0.09960750521541717,
25.125128627428623
],
[
0.0029160750999965845, -0.29700816845544387,
0.010917289481594317, -0.8034506550168371,
-496.35799885178454, -0.09965101413551591,
25.12517213634872
]]
def __init__(self, context):
super(WaterPulse, self).__init__(context)
self.setObjectName('WaterPulse')
self._widget = QWidget()
self._widget.setFixedSize(600, 600)
self._sound_client = SoundClient()
rospy.Subscriber('robotsound', SoundRequest, self.sound_cb)
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'
]
l_traj_contoller_name = None
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.sound_sig.connect(self.sound_sig_cb)
large_box = QtGui.QVBoxLayout()
# Textbox to enter words for PR2 to say
sound_textbox = QtGui.QLineEdit("Squirtle Squirtle") # Default Text
sound_textbox.setFixedWidth(450)
self.marker_publisher = rospy.Publisher('visualization_marker', Marker)
# Set a handler on the textbox to retrieve the text when button clicked
self.sound_textbox = sound_textbox
button_box = QtGui.QHBoxLayout()
button_box.addItem(QtGui.QSpacerItem(15, 20))
button_box.addWidget(self.create_button('Speak', self.command_cb))
button_box.addWidget(sound_textbox)
button_box.addStretch(1)
large_box.addLayout(button_box)
# 'Trick or Treat' & 'Thank You' Buttons
halloween_box = QtGui.QHBoxLayout()
halloween_box.addItem(QtGui.QSpacerItem(15, 20))
halloween_box.addWidget(
self.create_button('Trick or Treat', self.command_cb))
halloween_box.addWidget(
self.create_button('Thank You', self.command_cb))
halloween_box.addStretch(1)
large_box.addLayout(halloween_box)
# Buttons to move the PR2's head
up_head = Head(Head.UP, self)
head_box = QtGui.QVBoxLayout()
up_head_box = QtGui.QHBoxLayout()
up_head_button = self.create_button('^', up_head.create_closure())
down_head = Head(Head.DOWN, self)
down_head_box = QtGui.QHBoxLayout()
down_head_button = self.create_button('v', down_head.create_closure())
right_head = Head(Head.RIGHT, self)
right_head_button = self.create_button('>',
right_head.create_closure())
left_head = Head(Head.LEFT, self)
left_head_button = self.create_button('<', left_head.create_closure())
left_right_head_box = QtGui.QHBoxLayout()
head_speed_sld = QtGui.QSlider(QtCore.Qt.Horizontal)
head_speed_sld.setFocusPolicy(QtCore.Qt.NoFocus)
head_speed_sld.setMinimum(1)
head_speed_sld.setMaximum(5)
head_speed_sld.valueChanged[int].connect(Head.set_speed)
up_head_box.addItem(QtGui.QSpacerItem(235, 20))
up_head_box.addWidget(up_head_button)
up_head_box.addItem(QtGui.QSpacerItem(275, 20))
left_right_head_box.addItem(QtGui.QSpacerItem(160, 20))
left_right_head_box.addWidget(left_head_button)
left_right_head_box.addItem(QtGui.QSpacerItem(60, 20))
left_right_head_box.addWidget(right_head_button)
left_right_head_box.addItem(QtGui.QSpacerItem(225, 20))
down_head_box.addItem(QtGui.QSpacerItem(235, 20))
down_head_box.addWidget(down_head_button)
down_head_box.addItem(QtGui.QSpacerItem(275, 20))
head_sld_box = QtGui.QHBoxLayout()
head_sld_box.addItem(QtGui.QSpacerItem(225, 20))
head_sld_box.addWidget(head_speed_sld)
head_sld_box.addItem(QtGui.QSpacerItem(225, 20))
head_box.addLayout(up_head_box)
head_box.addLayout(left_right_head_box)
head_box.addLayout(down_head_box)
head_box.addLayout(head_sld_box)
large_box.addLayout(head_box)
# Buttons to move the grippers
gripper = Gripper(Gripper.RIGHT, Gripper.OPEN, self)
right_gripper = self.create_button('Right gripper',
gripper.create_closure())
gripper = Gripper(Gripper.LEFT, Gripper.OPEN, self)
left_gripper = self.create_button('Left gripper',
gripper.create_closure())
knock_button = self.create_button('Knock', self.knock)
large_box.addItem(QtGui.QSpacerItem(100, 250))
gripper_box = QtGui.QHBoxLayout()
gripper_box.addItem(QtGui.QSpacerItem(75, 20))
gripper_box.addWidget(left_gripper)
gripper_box.addWidget(knock_button)
gripper_box.addItem(QtGui.QSpacerItem(450, 20))
gripper_box.addWidget(right_gripper)
gripper_box.addItem(QtGui.QSpacerItem(75, 20))
large_box.addLayout(gripper_box)
# Buttons to move the base
base_box = QtGui.QVBoxLayout()
large_box.addItem(QtGui.QSpacerItem(100, 100))
forward_base_box = QtGui.QHBoxLayout()
forward_base = Base(Base.FORWARD, self)
forward_base_button = self.create_button('move forward',
forward_base.create_closure())
forward_base_box.addItem(QtGui.QSpacerItem(400, 20))
forward_base_box.addWidget(forward_base_button)
forward_base_box.addItem(QtGui.QSpacerItem(400, 20))
base_box.addLayout(forward_base_box)
left_right_base_box = QtGui.QHBoxLayout()
left_base = Base(Base.LEFT, self)
left_base_button = self.create_button('move left',
left_base.create_closure())
right_base = Base(Base.RIGHT, self)
right_base_button = self.create_button('move right',
right_base.create_closure())
left_right_base_box.addItem(QtGui.QSpacerItem(300, 20))
left_right_base_box.addWidget(left_base_button)
left_right_base_box.addItem(QtGui.QSpacerItem(50, 20))
left_right_base_box.addWidget(right_base_button)
left_right_base_box.addItem(QtGui.QSpacerItem(300, 20))
base_box.addLayout(left_right_base_box)
backward_base_box = QtGui.QHBoxLayout()
backward_base = Base(Base.BACKWARD, self)
backward_base_button = self.create_button(
'move backward', backward_base.create_closure())
backward_base_box.addItem(QtGui.QSpacerItem(400, 20))
backward_base_box.addWidget(backward_base_button)
backward_base_box.addItem(QtGui.QSpacerItem(400, 20))
base_box.addLayout(backward_base_box)
large_box.addLayout(base_box)
turn_base_box = QtGui.QHBoxLayout()
counter_base = Base(Base.COUNTER, self)
counter_base_button = self.create_button('\\\n -->',
counter_base.create_closure())
clockwise_base = Base(Base.CLOCKWISE, self)
clockwise_base_button = self.create_button(
' /\n<--', clockwise_base.create_closure())
turn_base_box.addItem(QtGui.QSpacerItem(75, 20))
turn_base_box.addWidget(counter_base_button)
turn_base_box.addItem(QtGui.QSpacerItem(225, 20))
turn_base_box.addWidget(clockwise_base_button)
turn_base_box.addItem(QtGui.QSpacerItem(100, 20))
large_box.addLayout(turn_base_box)
self._widget.setObjectName('WaterPulse')
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__))) +
"/../../rosie_background.jpg"))
def show_text_in_rviz(self, text):
marker = Marker(type=Marker.TEXT_VIEW_FACING,
id=0,
lifetime=rospy.Duration(1.5),
pose=Pose(Point(0.5, 0.5, 1.45),
Quaternion(0, 0, 0, 1)),
scale=Vector3(0.06, 0.06, 0.06),
header=Header(frame_id='base_link'),
color=ColorRGBA(0.0, 1.0, 0.0, 0.8),
text=text)
self.marker_publisher.publish(marker)
def sound_cb(self, sound_request):
qWarning('Received sound.')
self.sound_sig.emit(sound_request)
def create_button(self, name, method):
btn = QtGui.QPushButton(name, self._widget)
btn.clicked.connect(method)
btn.setAutoRepeat(True)
return btn
def sound_sig_cb(self, sound_request):
qWarning('Received sound signal.')
#if (sound_request.command == SoundRequest.SAY):
qWarning('Robot said: ' + sound_request.arg)
def command_cb(self):
button_name = self._widget.sender().text()
if (button_name == 'Speak'):
qWarning('Robot will say: ' + self.sound_textbox.text())
self._sound_client.say(self.sound_textbox.text())
elif (button_name == 'Trick or Treat'):
qWarning('Robot will say: Trick or Treat')
self._sound_client.say('Trick or Treat')
elif (button_name == 'Thank You'):
qWarning('Robot will say: Thank You')
self._sound_client.say('Thank You')
def knock(self):
for position in WaterPulse.positions:
velocities = [0] * len(position)
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory.header.stamp = (rospy.Time.now() +
rospy.Duration(0.1))
time_to_joint = 2.0
traj_goal.trajectory.points.append(
JointTrajectoryPoint(
positions=position,
velocities=velocities,
time_from_start=rospy.Duration(time_to_joint)))
traj_goal.trajectory.joint_names = self.l_joint_names
self.l_traj_action_client.send_goal(traj_goal)
result = 0
while (result < 2): # ACTIVE or PENDING
self.l_traj_action_client.wait_for_result()
result = self.l_traj_action_client.get_result()
#self.l_traj_action_client.wait_for_result()
#time.sleep(1)
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
class WaterPulse(Plugin):
sound_sig = Signal(SoundRequest)
positions = [[-0.013001995432544766, -0.24895825213211362, -0.0055992576345036404, -1.7647281786034612, -496.3549908032631, -0.09960750521541717, 25.125128627428623], [0.0029160750999965845, -0.29700816845544387, 0.010917289481594317, -0.8034506550168371, -496.35799885178454, -0.09965101413551591, 25.12517213634872]]
def __init__(self, context):
super(WaterPulse, self).__init__(context)
self.setObjectName('WaterPulse')
self._widget = QWidget()
self._widget.setFixedSize(600, 600)
self._sound_client = SoundClient()
rospy.Subscriber('robotsound', SoundRequest, self.sound_cb)
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']
l_traj_contoller_name = None
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.sound_sig.connect(self.sound_sig_cb)
large_box = QtGui.QVBoxLayout()
# Textbox to enter words for PR2 to say
sound_textbox = QtGui.QLineEdit("Squirtle Squirtle") # Default Text
sound_textbox.setFixedWidth(450)
self.marker_publisher = rospy.Publisher('visualization_marker', Marker)
# Set a handler on the textbox to retrieve the text when button clicked
self.sound_textbox = sound_textbox
button_box = QtGui.QHBoxLayout()
button_box.addItem(QtGui.QSpacerItem(15,20))
button_box.addWidget(self.create_button('Speak', self.command_cb))
button_box.addWidget(sound_textbox)
button_box.addStretch(1)
large_box.addLayout(button_box)
# 'Trick or Treat' & 'Thank You' Buttons
halloween_box = QtGui.QHBoxLayout()
halloween_box.addItem(QtGui.QSpacerItem(15,20))
halloween_box.addWidget(self.create_button('Trick or Treat', self.command_cb))
halloween_box.addWidget(self.create_button('Thank You', self.command_cb))
halloween_box.addStretch(1)
large_box.addLayout(halloween_box)
# Buttons to move the PR2's head
up_head = Head(Head.UP, self)
head_box = QtGui.QVBoxLayout()
up_head_box = QtGui.QHBoxLayout()
up_head_button = self.create_button('^', up_head.create_closure())
down_head = Head(Head.DOWN, self)
down_head_box = QtGui.QHBoxLayout()
down_head_button = self.create_button('v', down_head.create_closure())
right_head = Head(Head.RIGHT, self)
right_head_button = self.create_button('>', right_head.create_closure())
left_head = Head(Head.LEFT, self)
left_head_button = self.create_button('<', left_head.create_closure())
left_right_head_box = QtGui.QHBoxLayout()
head_speed_sld = QtGui.QSlider(QtCore.Qt.Horizontal)
head_speed_sld.setFocusPolicy(QtCore.Qt.NoFocus)
head_speed_sld.setMinimum(1)
head_speed_sld.setMaximum(5)
head_speed_sld.valueChanged[int].connect(Head.set_speed)
up_head_box.addItem(QtGui.QSpacerItem(235,20))
up_head_box.addWidget(up_head_button)
up_head_box.addItem(QtGui.QSpacerItem(275,20))
left_right_head_box.addItem(QtGui.QSpacerItem(160,20))
left_right_head_box.addWidget(left_head_button)
left_right_head_box.addItem(QtGui.QSpacerItem(60,20))
left_right_head_box.addWidget(right_head_button)
left_right_head_box.addItem(QtGui.QSpacerItem(225,20))
down_head_box.addItem(QtGui.QSpacerItem(235,20))
down_head_box.addWidget(down_head_button)
down_head_box.addItem(QtGui.QSpacerItem(275,20))
head_sld_box = QtGui.QHBoxLayout()
head_sld_box.addItem(QtGui.QSpacerItem(225,20))
head_sld_box.addWidget(head_speed_sld)
head_sld_box.addItem(QtGui.QSpacerItem(225,20))
head_box.addLayout(up_head_box)
head_box.addLayout(left_right_head_box)
head_box.addLayout(down_head_box)
head_box.addLayout(head_sld_box)
large_box.addLayout(head_box)
# Buttons to move the grippers
gripper = Gripper(Gripper.RIGHT, Gripper.OPEN, self)
right_gripper = self.create_button('Right gripper',
gripper.create_closure())
gripper = Gripper(Gripper.LEFT, Gripper.OPEN, self)
left_gripper = self.create_button('Left gripper', gripper.create_closure())
knock_button = self.create_button('Knock', self.knock)
large_box.addItem(QtGui.QSpacerItem(100,250))
gripper_box = QtGui.QHBoxLayout()
gripper_box.addItem(QtGui.QSpacerItem(75,20))
gripper_box.addWidget(left_gripper)
gripper_box.addWidget(knock_button)
gripper_box.addItem(QtGui.QSpacerItem(450,20))
gripper_box.addWidget(right_gripper)
gripper_box.addItem(QtGui.QSpacerItem(75,20))
large_box.addLayout(gripper_box)
# Buttons to move the base
base_box = QtGui.QVBoxLayout()
large_box.addItem(QtGui.QSpacerItem(100,100))
forward_base_box = QtGui.QHBoxLayout()
forward_base = Base(Base.FORWARD, self)
forward_base_button = self.create_button('move forward', forward_base.create_closure())
forward_base_box.addItem(QtGui.QSpacerItem(400,20))
forward_base_box.addWidget(forward_base_button)
forward_base_box.addItem(QtGui.QSpacerItem(400,20))
base_box.addLayout(forward_base_box)
left_right_base_box = QtGui.QHBoxLayout()
left_base= Base(Base.LEFT, self)
left_base_button = self.create_button('move left',
left_base.create_closure())
right_base= Base(Base.RIGHT, self)
right_base_button= self.create_button('move right',
right_base.create_closure())
left_right_base_box.addItem(QtGui.QSpacerItem(300,20))
left_right_base_box.addWidget(left_base_button)
left_right_base_box.addItem(QtGui.QSpacerItem(50,20))
left_right_base_box.addWidget(right_base_button)
left_right_base_box.addItem(QtGui.QSpacerItem(300,20))
base_box.addLayout(left_right_base_box)
backward_base_box = QtGui.QHBoxLayout()
backward_base= Base(Base.BACKWARD, self)
backward_base_button = self.create_button('move backward',
backward_base.create_closure())
backward_base_box.addItem(QtGui.QSpacerItem(400,20))
backward_base_box.addWidget(backward_base_button)
backward_base_box.addItem(QtGui.QSpacerItem(400,20))
base_box.addLayout(backward_base_box)
large_box.addLayout(base_box)
turn_base_box = QtGui.QHBoxLayout()
counter_base= Base(Base.COUNTER, self)
counter_base_button = self.create_button('\\\n -->',
counter_base.create_closure())
clockwise_base= Base(Base.CLOCKWISE, self)
clockwise_base_button = self.create_button(' /\n<--',
clockwise_base.create_closure())
turn_base_box.addItem(QtGui.QSpacerItem(75,20))
turn_base_box.addWidget(counter_base_button)
turn_base_box.addItem(QtGui.QSpacerItem(225,20))
turn_base_box.addWidget(clockwise_base_button)
turn_base_box.addItem(QtGui.QSpacerItem(100,20))
large_box.addLayout(turn_base_box)
self._widget.setObjectName('WaterPulse')
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__))) +
"/../../rosie_background.jpg"))
def show_text_in_rviz(self, text):
marker = Marker(type=Marker.TEXT_VIEW_FACING, id=0,
lifetime=rospy.Duration(1.5),
pose=Pose(Point(0.5, 0.5, 1.45), Quaternion(0, 0, 0, 1)),
scale=Vector3(0.06, 0.06, 0.06),
header=Header(frame_id='base_link'),
color=ColorRGBA(0.0, 1.0, 0.0, 0.8), text=text)
self.marker_publisher.publish(marker)
def sound_cb(self, sound_request):
qWarning('Received sound.')
self.sound_sig.emit(sound_request)
def create_button(self, name, method):
btn = QtGui.QPushButton(name, self._widget)
btn.clicked.connect(method)
btn.setAutoRepeat(True)
return btn
def sound_sig_cb(self, sound_request):
qWarning('Received sound signal.')
#if (sound_request.command == SoundRequest.SAY):
qWarning('Robot said: ' + sound_request.arg)
def command_cb(self):
button_name = self._widget.sender().text()
if (button_name == 'Speak'):
qWarning('Robot will say: ' + self.sound_textbox.text())
self._sound_client.say(self.sound_textbox.text())
elif (button_name == 'Trick or Treat'):
qWarning('Robot will say: Trick or Treat')
self._sound_client.say('Trick or Treat')
elif (button_name == 'Thank You'):
qWarning('Robot will say: Thank You')
self._sound_client.say('Thank You')
def knock(self):
for position in WaterPulse.positions:
velocities = [0] * len(position)
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory.header.stamp = (rospy.Time.now() +
rospy.Duration(0.1))
time_to_joint = 2.0
traj_goal.trajectory.points.append(JointTrajectoryPoint(
positions=position,
velocities=velocities,
time_from_start=rospy.Duration(time_to_joint)))
traj_goal.trajectory.joint_names = self.l_joint_names
self.l_traj_action_client.send_goal(traj_goal)
result = 0
while(result < 2): # ACTIVE or PENDING
self.l_traj_action_client.wait_for_result()
result = self.l_traj_action_client.get_result()
#self.l_traj_action_client.wait_for_result()
#time.sleep(1)
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
