class WoZPlugin(Plugin):
def __init__(self, context):
super(WoZPlugin, self).__init__(context)
self._widget = QWidget()
self._widget.setFixedSize(600, 600)
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
large_box = QtGui.QVBoxLayout()
#Button for outreaching to recieve book from Human
button_box = QtGui.QVBoxLayout()
box_1 = QtGui.QHBoxLayout()
box_2 = QtGui.QHBoxLayout()
box_3 = QtGui.QHBoxLayout()
box_4 = QtGui.QHBoxLayout()
box_5 = QtGui.QHBoxLayout()
box_6 = QtGui.QHBoxLayout()
box_7 = QtGui.QHBoxLayout()
box_8 = QtGui.QHBoxLayout()
box_9 = QtGui.QHBoxLayout()
box_10 = QtGui.QHBoxLayout()
box_1.addWidget(self.create_button('print nice', self.printCallback))
self._widget.setObjectName('TrashbotGUI')
self._widget.setLayout(large_box)
context.add_widget(self._widget)
def create_button(self, name, callback):
btn = QtGui.QPushButton(name, self._widget)
btn.clicked.connect(callback)
btn.setAutoRepeat(True)
return btn
def printCallback(self):
print 'nice'
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 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 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):
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):
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 Milestone1GUI(Plugin):
RECEIVE_FROM_HUMAN_R_POS = [
0.00952670015493673, 0.3270780665526253, 0.03185028324084582,
-1.3968658009779173, -3.058799671876592, -1.1083678332942686,
-1.6314425515258866
]
READ_FIDUCIAL_R_POS = [
0.41004856860653505, 0.29772362823537935, -0.019944325676627628,
-1.8394298656773618, -0.44139252862458106, -0.09922194050427624,
-4.761735317011306
]
NAVIGATE_R_POS = [
-0.3594077470836499, 0.971353000916152, -1.9647276598906076,
-1.431900313132731, -1.1839177367219755, -0.09817772642188527,
-1.622044624784374
]
PLACE_ON_SHELF_R_POS = [
-0.15015144487461773, 0.4539704512093072, -0.10846018983280459,
-0.9819529421527269, -3.0207362886631235, -0.4990689162195487,
-1.6026396464199553
]
LOWER_ON_SHELF_R_POS = [
-0.2159792990560645, 0.6221451583409631, -0.1886376030177479,
-0.959223940465513, 9.690004126983537, -0.2866303982732683,
111.39703078836274
]
RELEASE_BOOK_R_POS = [
-0.15545746838546493, 0.44145040258984786, -0.13267376861465774,
-0.972108533778647, -3.028545645401449, -0.38572817936010495,
0.008017066746929924
]
TUCKED_UNDER_L_POS = [
0.05688828299939419, 1.2955758539090194, 1.7180547710154033,
-1.4511548177467404, -0.28718096455759035, -0.0938208188421713,
-10.980395466225648
]
sound_sig = Signal(SoundRequest)
def __init__(self, context):
super(Milestone1GUI, self).__init__(context)
self._sound_client = SoundClient()
rospy.Subscriber('robotsound', SoundRequest, self.sound_cb)
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'
]
# 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()
#init torso
self.torso = Torso()
#init gripper
self.l_gripper = Gripper('l')
self.r_gripper = Gripper('r')
#init navigation
self.navigation = Navigation()
self._widget = QWidget()
self._widget.setFixedSize(600, 600)
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
large_box = QtGui.QVBoxLayout()
#Button for outreaching to recieve book from Human
button_box = QtGui.QVBoxLayout()
box_1 = QtGui.QHBoxLayout()
box_2 = QtGui.QHBoxLayout()
box_3 = QtGui.QHBoxLayout()
box_4 = QtGui.QHBoxLayout()
box_5 = QtGui.QHBoxLayout()
box_6 = QtGui.QHBoxLayout()
box_7 = QtGui.QHBoxLayout()
box_8 = QtGui.QHBoxLayout()
box_9 = QtGui.QHBoxLayout()
box_10 = QtGui.QHBoxLayout()
box_1.addItem(QtGui.QSpacerItem(15, 2))
box_1.addWidget(
self.create_button('Prepare To Take', self.prepare_to_take))
box_1.addItem(QtGui.QSpacerItem(445, 2))
box_2.addItem(QtGui.QSpacerItem(15, 2))
box_2.addWidget(
self.create_button('Take From Human', self.take_from_human))
box_2.addItem(QtGui.QSpacerItem(445, 2))
box_3.addItem(QtGui.QSpacerItem(15, 2))
box_3.addWidget(
self.create_button('Prepare To Navigate',
self.prepare_to_navigate))
box_3.addItem(QtGui.QSpacerItem(445, 2))
# Button to move to shelf
box_5.addItem(QtGui.QSpacerItem(15, 2))
box_5.addWidget(
self.create_button('Navigate To Shelf', self.navigate_to_shelf))
box_5.addItem(QtGui.QSpacerItem(445, 2))
box_4.addItem(QtGui.QSpacerItem(15, 2))
box_4.addWidget(
self.create_button('Place On Shelf', self.place_on_shelf))
box_4.addItem(QtGui.QSpacerItem(445, 2))
box_6.addItem(QtGui.QSpacerItem(15, 2))
box_6.addWidget(
self.create_button('Give Information', self.give_information))
box_6.addItem(QtGui.QSpacerItem(445, 2))
box_7.addItem(QtGui.QSpacerItem(15, 2))
box_7.addWidget(
self.create_button('Navigate To Person', self.navigate_to_person))
box_7.addItem(QtGui.QSpacerItem(445, 2))
self.book_textbox = QtGui.QLineEdit()
self.book_textbox.setFixedWidth(100)
box_8.addItem(QtGui.QSpacerItem(15, 2))
box_8.addWidget(self.book_textbox)
box_8.addWidget(
self.create_button('Pick Up Book', self.pick_up_from_shelf_button))
box_8.addItem(QtGui.QSpacerItem(445, 2))
box_9.addItem(QtGui.QSpacerItem(15, 2))
box_9.addWidget(self.create_button('Localize', self.spin_base))
box_9.addItem(QtGui.QSpacerItem(445, 2))
box_10.addItem(QtGui.QSpacerItem(15, 2))
box_10.addWidget(
self.create_button('Non-nav Pick Up', self.pick_up_from_shelf))
box_10.addItem(QtGui.QSpacerItem(445, 2))
button_box.addItem(QtGui.QSpacerItem(20, 120))
button_box.addLayout(box_1)
button_box.addLayout(box_2)
button_box.addLayout(box_3)
button_box.addLayout(box_5)
button_box.addLayout(box_4)
button_box.addLayout(box_6)
button_box.addLayout(box_7)
button_box.addLayout(box_8)
button_box.addLayout(box_9)
button_box.addLayout(box_10)
button_box.addItem(QtGui.QSpacerItem(20, 240))
large_box.addLayout(button_box)
self.marker_perception = ReadMarkers()
self.speech_recognition = SpeechRecognition(self)
self.bookDB = BookDB()
self.book_map = self.bookDB.getAllBookCodes()
self._widget.setObjectName('Milestone1GUI')
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__))) +
"/../../librarian_gui_background.jpg"))
def get_function(self, text):
functions = {
"bring-me-a-book": self.pick_up_from_shelf_routine,
"put-this-book-back": self.prepare_to_take,
"give-me-information": self.give_information_routine,
"here-you-go": self.put_this_book_back_routine,
}
if text not in functions:
print("Could not find key %s" % text)
return None
return functions[text]
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 prepare_to_take(self):
# Open gripper and move arms to take book
self.torso.move(.15) # move torso .1 meter from base (MAX is .2)
self.saved_l_arm_pose = Milestone1GUI.TUCKED_UNDER_L_POS
self.saved_r_arm_pose = Milestone1GUI.RECEIVE_FROM_HUMAN_R_POS
self.move_arm('l', 5.0)
self.move_arm('r', 5.0) # Increase these numbers for slower movement
self.l_gripper.close_gripper()
self.r_gripper.open_gripper(True)
self._sound_client.say("Please give me a book")
def take_from_human(self):
self.marker_perception.is_listening = True
# Close gripper and move arms to see book
self.r_gripper.close_gripper(True)
self._sound_client.say("Thank you")
time.sleep(1)
self.saved_r_arm_pose = Milestone1GUI.READ_FIDUCIAL_R_POS
self.move_arm('r', 5.0,
True) # Increase these numbers for slower movement
self.look_at_r_gripper()
rospy.loginfo("marker id returned by get_marker_id is: " +
str(self.marker_perception.get_marker_id()))
def look_at_r_gripper(self):
name_space = '/head_traj_controller/point_head_action'
head_client = SimpleActionClient(name_space, PointHeadAction)
head_client.wait_for_server()
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = "r_gripper_tool_frame"
head_goal.min_duration = rospy.Duration(0.3)
head_goal.target.point = Point(0, 0, 0)
head_goal.max_velocity = 1.0
head_client.send_goal(head_goal)
head_client.wait_for_result(rospy.Duration(5.0))
if (head_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logwarn('Head action unsuccessful.')
def prepare_to_navigate(self):
self.marker_perception.is_listening = False
# Tuck arms
self.saved_r_arm_pose = Milestone1GUI.NAVIGATE_R_POS
self.move_arm('r', 5.0) # Increase these numbers for slower movement
#spin 360 * rotate_count degress clock wise
def spin_base(self, rotate_count):
# Adjust height and tuck arms before localization
self._sound_client.say("Please wait while I orient myself.")
self.torso.move(.15)
self.saved_l_arm_pose = Milestone1GUI.TUCKED_UNDER_L_POS
self.saved_r_arm_pose = Milestone1GUI.NAVIGATE_R_POS
self.move_arm('l', 5.0, True)
self.move_arm('r', 5.0, True)
self.l_gripper.close_gripper()
self.r_gripper.close_gripper()
if not rotate_count:
rotate_count = 2
topic_name = '/base_controller/command'
base_publisher = rospy.Publisher(topic_name, Twist)
twist_msg = Twist()
twist_msg.linear = Vector3(0.0, 0.0, 0.0)
twist_msg.angular = Vector3(0.0, 0.0, 0.5)
start_time = rospy.get_rostime()
while rospy.get_rostime() < start_time + rospy.Duration(
15.0 * rotate_count):
base_publisher.publish(twist_msg)
def navigate_to_shelf(self, marker_id=None):
# Move forward, place book on the shelf, and move back
if marker_id is None or marker_id is False:
marker_id = self.marker_perception.get_marker_id()
rospy.loginfo("marker id returned by get_marker_id is: " +
str(marker_id))
if marker_id is False:
rospy.loginfo("wuuuuuuuut")
if marker_id is None:
self._sound_client.say("I don't think I am holding a book "
"right now")
rospy.logwarn("Navigate to shelf called when marker id is None")
return
book = self.book_map.get(unicode(marker_id))
if book is None:
self._sound_client.say("The book that I am holding is unknown "
"to me")
rospy.logwarn(
"Navigate to shelf called when marker id is not in database")
return
x = book.getXCoordinate()
y = book.getYCoordinate()
self.navigation.move_to_shelf(x, y)
# Ye Olde Dropping way of putting a book on the shelf. Deprecating.
def drop_on_shelf(self):
self.saved_r_arm_pose = Milestone1GUI.PLACE_ON_SHELF_R_POS
self.move_arm('r', 4.0,
True) # Increase these numbers for slower movement
self.move_base(True)
self.r_gripper.open_gripper(True)
self.saved_r_arm_pose = Milestone1GUI.RELEASE_BOOK_R_POS
self.move_arm('r', 2.0,
True) # Increase these numbers for slower movement
self.move_base(False)
self.marker_perception.reset_marker_id()
def place_on_shelf(self):
self.saved_r_arm_pose = Milestone1GUI.PLACE_ON_SHELF_R_POS
self.move_arm('r', 4.0,
True) # Increase these numbers for slower movement
self.move_base(True)
self.saved_r_arm_pose = Milestone1GUI.LOWER_ON_SHELF_R_POS
self.move_arm('r', 2.0,
True) # Increase these numbers for slower movement
self.r_gripper.open_gripper(True)
self.move_base(False)
self.marker_perception.reset_marker_id()
def give_information(self):
marker_id = self.marker_perception.get_marker_id()
rospy.loginfo("marker id returned by get_marker_id is: " +
str(marker_id))
if marker_id is not None:
book = self.book_map.get(unicode(marker_id))
if book is None:
rospy.logwarn(
"Give information called when marker id is not in database"
)
self._sound_client.say(
"The book that I am holding is unknown to me")
else:
self._sound_client.say(book.getInformation())
else:
rospy.logwarn("Give information called when marker id is None")
self._sound_client.say(
"I don't think I am holding a book right now")
def pick_up_from_shelf_button(self):
self.pick_up_from_shelf_routine(self.book_textbox.text())
def pick_up_from_shelf_routine(self, book_title):
book_id = self.bookDB.getBookIdByTitle(book_title)
if book_id is None:
rospy.logwarn("Book asked for was not present in database")
self._sound_client.say(
"The book you requested is not present in the database.")
else:
self.torso.move(.15) # move torso .1 meter from base (MAX is .2)
self.saved_l_arm_pose = Milestone1GUI.TUCKED_UNDER_L_POS
self.move_arm('l', 5.0)
self.l_gripper.close_gripper()
# self.marker_perception.set_marker_id(book_id)
self.prepare_to_navigate()
time.sleep(5)
self.navigate_to_shelf(book_id) # Navigate to book location
self.pick_up_from_shelf() # Pick up from the shelf
self.prepare_to_navigate()
time.sleep(5)
self.navigate_to_person(
) # Navigate to designated help desk location
self.give_book() # Give Book to Human
def put_this_book_back_routine(self):
self.take_from_human()
time.sleep(5)
self.prepare_to_navigate()
time.sleep(5)
self.navigate_to_shelf()
self.place_on_shelf()
self.prepare_to_navigate()
# TODO: return to human?
def give_information_routine(self, book_title):
book_id = self.bookDB.getBookIdByTitle(book_title)
if book_id is None:
rospy.logwarn("Book asked for was not present in database")
self._sound_client.say(
"The book you requested is not present in the database.")
return
book = self.book_map.get(unicode(book_id))
self._sound_client.say(book.getInformation())
def pick_up_from_shelf(self):
self.saved_r_arm_pose = Milestone1GUI.LOWER_ON_SHELF_R_POS
self.move_arm('r', 4.0,
True) # Increase these numbers for slower movement
self.r_gripper.open_gripper(True)
self.move_base(True)
self.r_gripper.close_gripper(True)
self.move_base(False)
def navigate_to_person(self):
x = 2.82690143585
y = -0.416650295258
z = 0.252587109056
w = 0.967574158573
self.navigation.move_to_shelf(x, y, z, w)
def give_book(self):
self.saved_r_arm_pose = Milestone1GUI.RECEIVE_FROM_HUMAN_R_POS
self.move_arm('r', 4.0, True)
self.r_gripper.open_gripper(True)
# Moves forward to the bookshelf (or backward if isForward is false)
def move_base(self, isForward):
topic_name = '/base_controller/command'
base_publisher = rospy.Publisher(topic_name, Twist)
distance = 0.25
if not isForward:
distance *= -1
twist_msg = Twist()
twist_msg.linear = Vector3(distance, 0.0, 0.0)
twist_msg.angular = Vector3(0.0, 0.0, 0.0)
for x in range(0, 15):
rospy.loginfo("Moving the base")
base_publisher.publish(twist_msg)
time.sleep(0.15)
time.sleep(1.5)
# Moves arms using kinematics
def move_arm(self, side_prefix, time_to_joints=2.0, wait=False):
# 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)
self.move_to_joints(side_prefix, self.saved_r_arm_pose,
time_to_joints, wait)
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)
self.move_to_joints(side_prefix, self.saved_l_arm_pose,
time_to_joints, wait)
pass
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 move_to_joints(self,
side_prefix,
positions,
time_to_joint,
wait=False):
'''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)
if side_prefix == 'r':
traj_goal.trajectory.joint_names = self.r_joint_names
action_client = self.r_traj_action_client
else:
traj_goal.trajectory.joint_names = self.l_joint_names
action_client = self.l_traj_action_client
action_client.send_goal(traj_goal)
if wait:
time.sleep(time_to_joint)
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
class Milestone1GUI(Plugin):
RECEIVE_FROM_HUMAN_R_POS = [0.00952670015493673, 0.3270780665526253,
0.03185028324084582, -1.3968658009779173, -3.058799671876592,
-1.1083678332942686, -1.6314425515258866]
READ_FIDUCIAL_R_POS = [0.41004856860653505, 0.29772362823537935,
-0.019944325676627628, -1.8394298656773618, -0.44139252862458106,
-0.09922194050427624, -4.761735317011306]
NAVIGATE_R_POS = [-0.3594077470836499, 0.971353000916152, -1.9647276598906076,
-1.431900313132731, -1.1839177367219755, -0.09817772642188527, -1.622044624784374]
PLACE_ON_SHELF_R_POS = [-0.15015144487461773, 0.4539704512093072,
-0.10846018983280459, -0.9819529421527269, -3.0207362886631235, -0.4990689162195487,
-1.6026396464199553]
LOWER_ON_SHELF_R_POS = [-0.2159792990560645, 0.6221451583409631,
-0.1886376030177479, -0.959223940465513, 9.690004126983537, -0.2866303982732683,
111.39703078836274]
RELEASE_BOOK_R_POS = [-0.15545746838546493, 0.44145040258984786,
-0.13267376861465774, -0.972108533778647, -3.028545645401449, -0.38572817936010495,
0.008017066746929924]
TUCKED_UNDER_L_POS = [0.05688828299939419, 1.2955758539090194, 1.7180547710154033,
-1.4511548177467404, -0.28718096455759035, -0.0938208188421713, -10.980395466225648]
sound_sig = Signal(SoundRequest)
def __init__(self, context):
super(Milestone1GUI, self).__init__(context)
self._sound_client = SoundClient()
rospy.Subscriber('robotsound', SoundRequest, self.sound_cb)
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']
# 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()
#init torso
self.torso = Torso()
#init gripper
self.l_gripper = Gripper('l')
self.r_gripper = Gripper('r')
#init navigation
self.navigation = Navigation()
self._widget = QWidget()
self._widget.setFixedSize(600, 600)
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
large_box = QtGui.QVBoxLayout()
#Button for outreaching to recieve book from Human
button_box = QtGui.QVBoxLayout()
box_1 = QtGui.QHBoxLayout()
box_2 = QtGui.QHBoxLayout()
box_3 = QtGui.QHBoxLayout()
box_4 = QtGui.QHBoxLayout()
box_5 = QtGui.QHBoxLayout()
box_6 = QtGui.QHBoxLayout()
box_7 = QtGui.QHBoxLayout()
box_8 = QtGui.QHBoxLayout()
box_9 = QtGui.QHBoxLayout()
box_10 = QtGui.QHBoxLayout()
box_1.addItem(QtGui.QSpacerItem(15,2))
box_1.addWidget(self.create_button('Prepare To Take', self.prepare_to_take))
box_1.addItem(QtGui.QSpacerItem(445,2))
box_2.addItem(QtGui.QSpacerItem(15,2))
box_2.addWidget(self.create_button('Take From Human', self.take_from_human))
box_2.addItem(QtGui.QSpacerItem(445,2))
box_3.addItem(QtGui.QSpacerItem(15,2))
box_3.addWidget(self.create_button('Prepare To Navigate', self.prepare_to_navigate))
box_3.addItem(QtGui.QSpacerItem(445,2))
# Button to move to shelf
box_5.addItem(QtGui.QSpacerItem(15,2))
box_5.addWidget(self.create_button('Navigate To Shelf', self.navigate_to_shelf))
box_5.addItem(QtGui.QSpacerItem(445,2))
box_4.addItem(QtGui.QSpacerItem(15,2))
box_4.addWidget(self.create_button('Place On Shelf', self.place_on_shelf))
box_4.addItem(QtGui.QSpacerItem(445,2))
box_6.addItem(QtGui.QSpacerItem(15,2))
box_6.addWidget(self.create_button('Give Information', self.give_information))
box_6.addItem(QtGui.QSpacerItem(445,2))
box_7.addItem(QtGui.QSpacerItem(15,2))
box_7.addWidget(self.create_button('Navigate To Person', self.navigate_to_person))
box_7.addItem(QtGui.QSpacerItem(445,2))
self.book_textbox = QtGui.QLineEdit()
self.book_textbox.setFixedWidth(100)
box_8.addItem(QtGui.QSpacerItem(15,2))
box_8.addWidget(self.book_textbox)
box_8.addWidget(self.create_button('Pick Up Book', self.pick_up_from_shelf_button))
box_8.addItem(QtGui.QSpacerItem(445,2))
box_9.addItem(QtGui.QSpacerItem(15,2))
box_9.addWidget(self.create_button('Localize', self.spin_base))
box_9.addItem(QtGui.QSpacerItem(445,2))
box_10.addItem(QtGui.QSpacerItem(15,2))
box_10.addWidget(self.create_button('Non-nav Pick Up', self.pick_up_from_shelf))
box_10.addItem(QtGui.QSpacerItem(445,2))
button_box.addItem(QtGui.QSpacerItem(20,120))
button_box.addLayout(box_1)
button_box.addLayout(box_2)
button_box.addLayout(box_3)
button_box.addLayout(box_5)
button_box.addLayout(box_4)
button_box.addLayout(box_6)
button_box.addLayout(box_7)
button_box.addLayout(box_8)
button_box.addLayout(box_9)
button_box.addLayout(box_10)
button_box.addItem(QtGui.QSpacerItem(20,240))
large_box.addLayout(button_box)
self.marker_perception = ReadMarkers()
self.speech_recognition = SpeechRecognition(self)
self.bookDB = BookDB()
self.book_map = self.bookDB.getAllBookCodes()
self._widget.setObjectName('Milestone1GUI')
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__))) +
"/../../librarian_gui_background.jpg"))
def get_function(self, text):
functions = {
"bring-me-a-book":
self.pick_up_from_shelf_routine,
"put-this-book-back":
self.prepare_to_take,
"give-me-information":
self.give_information_routine,
"here-you-go":
self.put_this_book_back_routine,
}
if text not in functions:
print ("Could not find key %s" % text)
return None
return functions[text]
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 prepare_to_take(self):
# Open gripper and move arms to take book
self.torso.move(.15) # move torso .1 meter from base (MAX is .2)
self.saved_l_arm_pose = Milestone1GUI.TUCKED_UNDER_L_POS
self.saved_r_arm_pose = Milestone1GUI.RECEIVE_FROM_HUMAN_R_POS
self.move_arm('l', 5.0)
self.move_arm('r', 5.0) # Increase these numbers for slower movement
self.l_gripper.close_gripper()
self.r_gripper.open_gripper(True)
self._sound_client.say("Please give me a book")
def take_from_human(self):
self.marker_perception.is_listening = True
# Close gripper and move arms to see book
self.r_gripper.close_gripper(True)
self._sound_client.say("Thank you")
time.sleep(1)
self.saved_r_arm_pose = Milestone1GUI.READ_FIDUCIAL_R_POS
self.move_arm('r', 5.0, True) # Increase these numbers for slower movement
self.look_at_r_gripper()
rospy.loginfo("marker id returned by get_marker_id is: " +
str(self.marker_perception.get_marker_id()))
def look_at_r_gripper(self):
name_space = '/head_traj_controller/point_head_action'
head_client = SimpleActionClient(name_space, PointHeadAction)
head_client.wait_for_server()
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = "r_gripper_tool_frame"
head_goal.min_duration = rospy.Duration(0.3)
head_goal.target.point = Point(0, 0, 0)
head_goal.max_velocity = 1.0
head_client.send_goal(head_goal)
head_client.wait_for_result(rospy.Duration(5.0))
if (head_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logwarn('Head action unsuccessful.')
def prepare_to_navigate(self):
self.marker_perception.is_listening = False
# Tuck arms
self.saved_r_arm_pose = Milestone1GUI.NAVIGATE_R_POS
self.move_arm('r', 5.0) # Increase these numbers for slower movement
#spin 360 * rotate_count degress clock wise
def spin_base(self, rotate_count):
# Adjust height and tuck arms before localization
self._sound_client.say("Please wait while I orient myself.")
self.torso.move(.15)
self.saved_l_arm_pose = Milestone1GUI.TUCKED_UNDER_L_POS
self.saved_r_arm_pose = Milestone1GUI.NAVIGATE_R_POS
self.move_arm('l', 5.0, True)
self.move_arm('r', 5.0, True)
self.l_gripper.close_gripper()
self.r_gripper.close_gripper()
if not rotate_count:
rotate_count = 2
topic_name = '/base_controller/command'
base_publisher = rospy.Publisher(topic_name, Twist)
twist_msg = Twist()
twist_msg.linear = Vector3(0.0, 0.0, 0.0)
twist_msg.angular = Vector3(0.0, 0.0, 0.5)
start_time = rospy.get_rostime()
while rospy.get_rostime() < start_time + rospy.Duration(15.0 * rotate_count):
base_publisher.publish(twist_msg)
def navigate_to_shelf(self, marker_id = None):
# Move forward, place book on the shelf, and move back
if marker_id is None or marker_id is False:
marker_id = self.marker_perception.get_marker_id()
rospy.loginfo("marker id returned by get_marker_id is: " + str(marker_id))
if marker_id is False:
rospy.loginfo("wuuuuuuuut")
if marker_id is None:
self._sound_client.say("I don't think I am holding a book "
"right now")
rospy.logwarn("Navigate to shelf called when marker id is None")
return
book = self.book_map.get(unicode(marker_id))
if book is None:
self._sound_client.say("The book that I am holding is unknown "
"to me")
rospy.logwarn("Navigate to shelf called when marker id is not in database")
return
x = book.getXCoordinate()
y = book.getYCoordinate()
self.navigation.move_to_shelf(x, y)
# Ye Olde Dropping way of putting a book on the shelf. Deprecating.
def drop_on_shelf(self):
self.saved_r_arm_pose = Milestone1GUI.PLACE_ON_SHELF_R_POS
self.move_arm('r', 4.0, True) # Increase these numbers for slower movement
self.move_base(True)
self.r_gripper.open_gripper(True)
self.saved_r_arm_pose = Milestone1GUI.RELEASE_BOOK_R_POS
self.move_arm('r', 2.0, True) # Increase these numbers for slower movement
self.move_base(False)
self.marker_perception.reset_marker_id()
def place_on_shelf(self):
self.saved_r_arm_pose = Milestone1GUI.PLACE_ON_SHELF_R_POS
self.move_arm('r', 4.0, True) # Increase these numbers for slower movement
self.move_base(True)
self.saved_r_arm_pose = Milestone1GUI.LOWER_ON_SHELF_R_POS
self.move_arm('r', 2.0, True) # Increase these numbers for slower movement
self.r_gripper.open_gripper(True)
self.move_base(False)
self.marker_perception.reset_marker_id()
def give_information(self):
marker_id = self.marker_perception.get_marker_id()
rospy.loginfo("marker id returned by get_marker_id is: " + str(marker_id))
if marker_id is not None:
book = self.book_map.get(unicode(marker_id))
if book is None:
rospy.logwarn("Give information called when marker id is not in database")
self._sound_client.say("The book that I am holding is unknown to me")
else:
self._sound_client.say(book.getInformation())
else:
rospy.logwarn("Give information called when marker id is None")
self._sound_client.say("I don't think I am holding a book right now")
def pick_up_from_shelf_button(self):
self.pick_up_from_shelf_routine(self.book_textbox.text())
def pick_up_from_shelf_routine(self, book_title):
book_id = self.bookDB.getBookIdByTitle(book_title)
if book_id is None:
rospy.logwarn("Book asked for was not present in database")
self._sound_client.say("The book you requested is not present in the database.")
else:
self.torso.move(.15) # move torso .1 meter from base (MAX is .2)
self.saved_l_arm_pose = Milestone1GUI.TUCKED_UNDER_L_POS
self.move_arm('l', 5.0)
self.l_gripper.close_gripper()
# self.marker_perception.set_marker_id(book_id)
self.prepare_to_navigate()
time.sleep(5)
self.navigate_to_shelf(book_id) # Navigate to book location
self.pick_up_from_shelf() # Pick up from the shelf
self.prepare_to_navigate()
time.sleep(5)
self.navigate_to_person() # Navigate to designated help desk location
self.give_book() # Give Book to Human
def put_this_book_back_routine(self):
self.take_from_human()
time.sleep(5)
self.prepare_to_navigate()
time.sleep(5)
self.navigate_to_shelf()
self.place_on_shelf()
self.prepare_to_navigate()
# TODO: return to human?
def give_information_routine(self, book_title):
book_id = self.bookDB.getBookIdByTitle(book_title)
if book_id is None:
rospy.logwarn("Book asked for was not present in database")
self._sound_client.say("The book you requested is not present in the database.")
return
book = self.book_map.get(unicode(book_id))
self._sound_client.say(book.getInformation())
def pick_up_from_shelf(self):
self.saved_r_arm_pose = Milestone1GUI.LOWER_ON_SHELF_R_POS
self.move_arm('r', 4.0, True) # Increase these numbers for slower movement
self.r_gripper.open_gripper(True)
self.move_base(True)
self.r_gripper.close_gripper(True)
self.move_base(False)
def navigate_to_person(self):
x = 2.82690143585
y = -0.416650295258
z = 0.252587109056
w = 0.967574158573
self.navigation.move_to_shelf(x, y, z, w)
def give_book(self):
self.saved_r_arm_pose = Milestone1GUI.RECEIVE_FROM_HUMAN_R_POS
self.move_arm('r', 4.0, True)
self.r_gripper.open_gripper(True)
# Moves forward to the bookshelf (or backward if isForward is false)
def move_base(self, isForward):
topic_name = '/base_controller/command'
base_publisher = rospy.Publisher(topic_name, Twist)
distance = 0.25
if not isForward:
distance *= -1
twist_msg = Twist()
twist_msg.linear = Vector3(distance, 0.0, 0.0)
twist_msg.angular = Vector3(0.0, 0.0, 0.0)
for x in range(0, 15):
rospy.loginfo("Moving the base")
base_publisher.publish(twist_msg)
time.sleep(0.15)
time.sleep(1.5)
# Moves arms using kinematics
def move_arm(self, side_prefix, time_to_joints = 2.0, wait = False):
# 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)
self.move_to_joints(side_prefix, self.saved_r_arm_pose, time_to_joints, wait)
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)
self.move_to_joints(side_prefix, self.saved_l_arm_pose, time_to_joints, wait)
pass
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 move_to_joints(self, side_prefix, positions, time_to_joint, wait = False):
'''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)
if side_prefix == 'r':
traj_goal.trajectory.joint_names = self.r_joint_names
action_client = self.r_traj_action_client
else:
traj_goal.trajectory.joint_names = self.l_joint_names
action_client = self.l_traj_action_client
action_client.send_goal(traj_goal)
if wait:
time.sleep(time_to_joint)
