def __init__(self, motion_publisher, robot_config):
super(MotionEditorWidget, self).__init__()
self.robot_config = robot_config
self._motion_publisher = motion_publisher
self._motion_data = MotionData(robot_config)
self._filter_pattern = ''
self._playback_marker = None
self._playback_timer = None
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'motion_editor.ui')
loadUi(ui_file, self)
self.list_widgets = {}
for group_type in robot_config.group_types():
list_widget = QListWidget()
list_widget.setSortingEnabled(True)
list_widget.setDragDropMode(QAbstractItemView.DragOnly)
list_widget.setContextMenuPolicy(Qt.CustomContextMenu)
list_widget.customContextMenuRequested.connect(
lambda pos, _group_type=group_type: self.
positions_list_context_menu(_group_type, pos))
list_widget.itemChanged.connect(self.on_list_item_changed)
self.position_lists_layout.addWidget(list_widget)
self.list_widgets[group_type] = list_widget
self._timeline_widget = TimelineWidget()
for track_name in self.robot_config.sorted_groups():
track_type = self.robot_config.groups[track_name].group_type
track = self._timeline_widget.add_track(track_name, track_type)
list_widget = self.list_widgets[track_type]
palette = list_widget.palette()
palette.setColor(QPalette.Base, track._colors['track'])
list_widget.setPalette(palette)
self.timeline_group.layout().addWidget(self._timeline_widget)
for group_type in robot_config.group_types():
label = QLabel(group_type)
label.setAlignment(Qt.AlignHCenter | Qt.AlignVCenter)
self.group_label_layout.addWidget(label)
self.update_motion_name_combo()
self.stop_motion_button.pressed.connect(self.on_motion_stop_pressed)
def __init__(self, motion_publisher, robot_config):
super(MotionEditorWidget, self).__init__()
self.robot_config = robot_config
self._motion_publisher = motion_publisher
self._motion_data = MotionData(robot_config)
self._filter_pattern = ''
self._playback_marker = None
self._playback_timer = None
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'motion_editor.ui')
loadUi(ui_file, self)
self.list_widgets = {}
for group_type in robot_config.group_types():
list_widget = QListWidget()
list_widget.setSortingEnabled(True)
list_widget.setDragDropMode(QAbstractItemView.DragOnly)
list_widget.setContextMenuPolicy(Qt.CustomContextMenu)
list_widget.customContextMenuRequested.connect(
lambda pos, _group_type=group_type: self.positions_list_context_menu(_group_type, pos)
)
list_widget.itemChanged.connect(self.on_list_item_changed)
self.position_lists_layout.addWidget(list_widget)
self.list_widgets[group_type] = list_widget
self._timeline_widget = TimelineWidget()
for track_name in self.robot_config.sorted_groups():
track_type = self.robot_config.groups[track_name].group_type
track = self._timeline_widget.add_track(track_name, track_type)
list_widget = self.list_widgets[track_type]
palette = list_widget.palette()
palette.setColor(QPalette.Base, track._colors['track'])
list_widget.setPalette(palette)
self.timeline_group.layout().addWidget(self._timeline_widget)
for group_type in robot_config.group_types():
label = QLabel(group_type)
label.setAlignment(Qt.AlignHCenter | Qt.AlignVCenter)
self.group_label_layout.addWidget(label)
self.update_motion_name_combo()
self.stop_motion_button.pressed.connect(self.on_motion_stop_pressed)
class SimpleGUI(Plugin):
# For sending speech
sound_sig = Signal(SoundRequest)
# Joints for arm poses
joint_sig = Signal(JointState)
def __init__(self, context):
self.prompt_width = 170
self.input_width = 250
super(SimpleGUI, self).__init__(context)
self.setObjectName('SimpleGUI')
self._widget = QWidget()
self._sound_client = SoundClient()
#find relative path for files to load
self.local_dir = os.path.dirname(__file__)
self.dir = os.path.join(self.local_dir, './lib/rqt_simplegui/')
if not os.path.isdir(self.dir):
os.makedirs(self.dir)
#need to add any additional subfolders as needed
if not os.path.isdir(self.dir + 'animations/'):
os.makedirs(self.dir + 'animations/')
# Creates a subscriber to the ROS topic, having msg type SoundRequest
rospy.Subscriber('robotsound', SoundRequest, self.sound_cb)
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
self.sound_sig.connect(self.sound_sig_cb)
# Code used for saving/ loading arm poses for the robot
switch_srv_name = 'pr2_controller_manager/switch_controller'
rospy.loginfo('Waiting for switch controller service...')
rospy.wait_for_service(switch_srv_name)
self.switch_service_client = rospy.ServiceProxy(switch_srv_name,
SwitchController)
self.r_joint_names = ['r_shoulder_pan_joint',
'r_shoulder_lift_joint',
'r_upper_arm_roll_joint',
'r_elbow_flex_joint',
'r_forearm_roll_joint',
'r_wrist_flex_joint',
'r_wrist_roll_joint']
self.l_joint_names = ['l_shoulder_pan_joint',
'l_shoulder_lift_joint',
'l_upper_arm_roll_joint',
'l_elbow_flex_joint',
'l_forearm_roll_joint',
'l_wrist_flex_joint',
'l_wrist_roll_joint']
self.all_joint_names = []
self.all_joint_poses = []
# Hash tables storing the name of the pose and the
# associated positions for that pose, initially empty
self.saved_l_poses = {}
self.saved_r_poses = {}
# Hash tables for storing name of animations and the associated pose list
self.saved_animations = {}
self.lock = threading.Lock()
rospy.Subscriber('joint_states', JointState, self.joint_states_cb)
#parsing for animations
dir = os.path.dirname(__file__)
qWarning(dir)
filename = os.path.join(self.dir, 'animations/')
ani_path = filename
ani_listing = os.listdir(ani_path)
for infile in ani_listing:
pose_left = []
pose_right = []
left_gripper_states = []
right_gripper_states = []
line_num = 1
for line in fileinput.input(ani_path + infile):
if (line_num % 2 == 1):
pose = [float(x) for x in line.split()]
pose_left.append(pose[:len(pose)/2])
pose_right.append(pose[len(pose)/2:])
else:
states = line.split()
left_gripper_states.append(states[0])
right_gripper_states.append(states[1])
line_num += 1
self.saved_animations[os.path.splitext(infile)[0]] = Quad(pose_left, pose_right, left_gripper_states, right_gripper_states)
# Create a trajectory action client
r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server for RIGHT arm...')
self.r_traj_action_client.wait_for_server()
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server for LEFT arm...')
self.l_traj_action_client.wait_for_server()
# Navigation functionality initialization
self.roomNav = RoomNavigator()
self._tf_listener = TransformListener()
self.animPlay = AnimationPlayer(None, None, None, None)
# Detection and pickup functionality
self.pap = PickAndPlaceManager(self._tf_listener, self.roomNav, self.animPlay)
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
self.joint_sig.connect(self.joint_sig_cb)
# Create a large vertical box that is top aligned
large_box = QtGui.QVBoxLayout()
large_box.setAlignment(QtCore.Qt.AlignTop)
large_box.setMargin(0)
large_box.addItem(QtGui.QSpacerItem(10,0))
# Buttons for controlling the head of the robot
head_box = QtGui.QHBoxLayout()
head_box.addItem(QtGui.QSpacerItem(230,0))
head_box.addWidget(self.create_pressed_button('Head Up'))
head_box.addStretch(1)
large_box.addLayout(head_box)
button_box = QtGui.QHBoxLayout()
button_box.addItem(QtGui.QSpacerItem(80,0))
button_box.addWidget(self.create_pressed_button('Head Turn Left'))
button_box.addWidget(self.create_pressed_button('Head Down'))
button_box.addWidget(self.create_pressed_button('Head Turn Right'))
button_box.addStretch(1)
button_box.setMargin(0)
button_box.setSpacing(0)
large_box.addLayout(button_box)
# Shows what the robot says
speech_box = QtGui.QHBoxLayout()
self.speech_label = QtGui.QLabel('Robot has not spoken yet')
palette = QtGui.QPalette()
palette.setColor(QtGui.QPalette.Foreground,QtCore.Qt.blue)
self.speech_label.setPalette(palette) #
speech_box.addItem(QtGui.QSpacerItem(100,0))
#speech_box.addWidget(self.speech_label) #
large_box.addLayout(speech_box)
# Speak button
speak_button_box = QtGui.QHBoxLayout();
speech_prompt = QtGui.QLabel('Enter Speech Text:')
speech_prompt.setFixedWidth(self.prompt_width)
speak_button_box.addWidget(speech_prompt)
robot_says = QtGui.QLineEdit(self._widget)
robot_says.setFixedWidth(self.input_width)
robot_says.textChanged[str].connect(self.onChanged) #
speak_button_box.addWidget(robot_says)
speak_button_box.addWidget(self.create_button('Speak'))
speak_button_box.addStretch(1)
large_box.addLayout(speak_button_box)
large_box.addItem(QtGui.QSpacerItem(0,50))
# Buttons for arm poses
pose_button_box1 = QtGui.QHBoxLayout()
pose_button_box1.addItem(QtGui.QSpacerItem(150,0))
pose_button_box1.addWidget(self.create_button('Relax Left Arm'))
pose_button_box1.addWidget(self.create_button('Relax Right Arm'))
pose_button_box1.addStretch(1)
large_box.addLayout(pose_button_box1)
# Buttons for grippers
gripper_button_box = QtGui.QHBoxLayout()
gripper_button_box.addItem(QtGui.QSpacerItem(150,0))
gripper_button_box.addWidget(self.create_button('Open Left Gripper'))
gripper_button_box.addWidget(self.create_button('Open Right Gripper'))
gripper_button_box.addStretch(1)
large_box.addLayout(gripper_button_box)
large_box.addItem(QtGui.QSpacerItem(0,25))
# Buttons for animation
animation_box = QtGui.QHBoxLayout()
play_anim_label = QtGui.QLabel('Select Animation:')
play_anim_label.setFixedWidth(self.prompt_width)
animation_box.addWidget(play_anim_label)
self.saved_animations_list = QtGui.QComboBox(self._widget)
animation_box.addWidget(self.saved_animations_list)
pose_time_label = QtGui.QLabel('Duration(sec):')
pose_time_label.setFixedWidth(100)
animation_box.addWidget(pose_time_label)
self.pose_time = QtGui.QLineEdit(self._widget)
self.pose_time.setFixedWidth(50)
self.pose_time.setText('2.0')
animation_box.addWidget(self.pose_time)
animation_box.addWidget(self.create_button('Play Animation'))
animation_box.addStretch(1)
large_box.addLayout(animation_box)
animation_box2 = QtGui.QHBoxLayout()
animation_name_label = QtGui.QLabel('Enter Animation Name:')
animation_name_label.setFixedWidth(self.prompt_width)
animation_box2.addWidget(animation_name_label)
self.animation_name = QtGui.QLineEdit(self._widget)
self.animation_name.setFixedWidth(self.input_width)
animation_box2.addWidget(self.animation_name)
animation_box2.addWidget(self.create_button('Save Animation'))
animation_box2.addStretch(1)
large_box.addLayout(animation_box2)
animation_box3 = QtGui.QHBoxLayout()
pose_name_label = QtGui.QLabel('Enter Pose Name:')
pose_name_label.setFixedWidth(self.prompt_width)
animation_box3.addWidget(pose_name_label)
self.pose_name_temp = QtGui.QLineEdit(self._widget)
self.pose_name_temp.setFixedWidth(self.input_width)
animation_box3.addWidget(self.pose_name_temp)
animation_box3.addWidget(self.create_button('Add Current Pose'))
animation_box3.addStretch(1)
large_box.addLayout(animation_box3)
# Playing around with UI stuff
play_box = QtGui.QHBoxLayout()
pose_sequence_label = QtGui.QLabel('Current Pose Sequence:')
pose_sequence_label.setFixedWidth(self.prompt_width)
pose_sequence_label.setAlignment(QtCore.Qt.AlignTop)
self.list_widget = QListWidget()
self.list_widget.setDragDropMode(QAbstractItemView.InternalMove)
self.list_widget.setMaximumSize(self.input_width, 200)
play_box.addWidget(pose_sequence_label)
play_box.addWidget(self.list_widget)
play_box.addStretch(1)
large_box.addLayout(play_box)
large_box.addItem(QtGui.QSpacerItem(0,50))
# Buttons for first row of base controls
first_base_button_box = QtGui.QHBoxLayout()
first_base_button_box.addItem(QtGui.QSpacerItem(70,0))
first_base_button_box.addWidget(self.create_pressed_button('Rotate Left'))
first_base_button_box.addWidget(self.create_pressed_button('^'))
first_base_button_box.addWidget(self.create_pressed_button('Rotate Right'))
first_base_button_box.addStretch(1)
large_box.addLayout(first_base_button_box)
# Buttons for second row of base controls
second_base_button_box = QtGui.QHBoxLayout()
second_base_button_box.addItem(QtGui.QSpacerItem(70,0))
second_base_button_box.addWidget(self.create_pressed_button('<'))
second_base_button_box.addWidget(self.create_pressed_button('v'))
second_base_button_box.addWidget(self.create_pressed_button('>'))
second_base_button_box.addWidget(self.create_button('Move to Bin'))
second_base_button_box.addWidget(self.create_button('Object Detect'))
second_base_button_box.addWidget(self.create_button('Clean Room'))
second_base_button_box.addStretch(1)
large_box.addLayout(second_base_button_box)
# Animation related items to store intermediate pose co-ordinates to save
self.animation_map = {}
self.create_state = False
self._widget.setObjectName('SimpleGUI')
self._widget.setLayout(large_box)
context.add_widget(self._widget)
# Look straight and down when launched
self.head_x = 1.0
self.head_y = 0.0
self.head_z = 0.5
self.head_action(self.head_x, self.head_y, self.head_z)
# Set grippers to closed on initialization
self.left_gripper_state = ''
self.right_gripper_state = ''
self.gripper_action('l', 0.0)
self.gripper_action('r', 0.0)
# Lab 6
self.marker_publisher = rospy.Publisher('visualization_marker', Marker)
# Saved states for poses
saved_pose_box = QtGui.QHBoxLayout()
self.saved_left_poses = QtGui.QLabel('')
self.saved_right_poses = QtGui.QLabel('')
saved_pose_box.addWidget(self.saved_left_poses)
saved_pose_box.addWidget(self.saved_right_poses)
large_box.addLayout(saved_pose_box)
# Preload the map of animations
self.saved_animations_list.addItems(self.saved_animations.keys())
# Move the torso all the way down
# self.torso_down(True)
# Autonomous navigation stuff
'''
self.locations = [Pose(Point(2.48293590546, 3.90075874329, 0.000), Quaternion(0.000, 0.000, -0.783917630973, 0.620864838632)),
Pose(Point(3.70106744766, 0.304672241211, 0.000), Quaternion(0.000, 0.000, 0.950186880196, -0.311680754463)),
Pose(Point(2.57326722145, 1.51304531097, 0.000), Quaternion(0.000, 0.000, 0.96127194482, -0.275601611212)),
Pose(Point(1.28060531616, 1.52380752563, 0.000), Quaternion(0.000, 0.000, 0.946345258806, -0.323157316388)),
Pose(Point(2.11048603058, 0.420155525208, 0.000), Quaternion(0.000, 0.000, 0.945222393391, -0.326427062346)),
Pose(Point(2.82733058929, -0.739856719971, 0.000), Quaternion(0.000, 0.000, 0.945473998362, -0.325697587373)),
Pose(Point(1.29184818268, -1.90485572815, 0.000), Quaternion(0.000, 0.000, 0.942275557345, -0.334838429739)),
Pose(Point(0.722846984863, -1.0921459198, 0.000), Quaternion(0.000, 0.000, 0.949330143731, -0.314280572424))]
'''
self.locations = [Pose(Point(2.04748392105, 2.04748010635, 0.000), Quaternion(0.000, 0.000, -0.776968030817, 0.629540053601)),
Pose(Point(2.34193611145, 1.43208932877, 0), Quaternion(0, 0, -0.841674385779, 0.539985396398)),
Pose(Point(3.43202018738, -0.258297920227, 0.000), Quaternion(0.000, 0.000, 0.996656413122, -0.0817067572629)),
Pose(Point(0.931655406952, -1.96435832977, 0.000), Quaternion(0.000, 0.000, 0.691187586713, 0.722675390458)),
Pose(Point(-0.369112968445, 0.0330476760864, 0.000), Quaternion(0.000, 0.000, 0.0275340398899, 0.999620866453))]
self.index = 1;
rospy.loginfo("Completed GUI initialization")
# Event for when text box is changed
def onChanged(self, text):
self.speech_label.setText(text)
self.speech_label.adjustSize()
def sound_cb(self, sound_request):
qWarning('Received sound.')
self.sound_sig.emit(sound_request)
def create_button(self, name):
btn = QtGui.QPushButton(name, self._widget)
btn.setFixedWidth(150)
btn.clicked.connect(self.command_cb)
return btn
def create_pressed_button(self, name):
btn = QtGui.QPushButton(name, self._widget)
btn.setFixedWidth(150)
btn.pressed.connect(self.command_cb)
btn.setAutoRepeat(True) # To make sure the movement repeats itself
return btn
def sound_sig_cb(self, sound_request):
qWarning('Received sound signal.')
qWarning('Robot said: ' + sound_request.arg)
self.speech_label.setText(sound_request.arg) #'Robot said: ' +
#a button was clicked
def command_cb(self):
button_name = self._widget.sender().text()
#robot talk button clicked
if (button_name == 'Speak'):
qWarning('Robot will say: ' + self.speech_label.text() )
self._sound_client.say(self.speech_label.text())
self.show_text_in_rviz("Robot is Speaking")
#gripper button selected
elif ('Gripper' in button_name):
self.gripper_click(button_name)
# Move forward
elif (button_name == '^'):
self.base_action(0.25, 0.0, 0.0, 0.0, 0.0, 0.0)
# Move left
elif (button_name == '<'):
self.base_action(0.0, 0.25, 0.0, 0.0, 0.0, 0.0)
# Move right
elif (button_name == '>'):
self.base_action(0.0, -0.25, 0.0, 0.0, 0.0, 0.0)
# Move back
elif (button_name == 'v'):
self.base_action(-0.25, 0.0, 0.0, 0.0, 0.0, 0.0)
#Rotate Left
elif (button_name == 'Rotate Left'):
self.base_action(0.0, 0.0, 0.0, 0.0, 0.0, 0.50)
# Rotate Right
elif (button_name == 'Rotate Right'):
self.base_action(0.0, 0.0, 0.0, 0.0, 0.0, -0.50)
# A head button selected
elif ('Head' in button_name):
self.rotate_head(button_name)
#An arm button selected
#third param unused in freeze/relax
#Second word in button should be side
elif ('Arm' in button_name):
arm_side = button_name.split()[1]
if ('Freeze' in button_name or 'Relax' in button_name):
new_arm_state = button_name.split()[0]
self.toggle_arm(arm_side[0].lower(), new_arm_state, True)
old_arm_state = ''
if (new_arm_state == 'Relax'):
old_arm_state = 'Freeze'
else:
old_arm_state = 'Relax'
self._widget.sender().setText('%s %s Arm' % (old_arm_state, arm_side))
elif('Play Animation' == button_name):
self.animPlay.left_poses = self.saved_animations[self.saved_animations_list.currentText()].left
self.animPlay.right_poses = self.saved_animations[self.saved_animations_list.currentText()].right
self.animPlay.left_gripper_states = self.saved_animations[self.saved_animations_list.currentText()].left_gripper
self.animPlay.right_gripper_states = self.saved_animations[self.saved_animations_list.currentText()].right_gripper
if self.pose_time.text() == '':
self.show_warning('Please enter a duration in seconds.')
else:
self.animPlay.play(self.pose_time.text())
elif('Animation' in button_name):
if ('Save' in button_name):
if self.animation_name.text() == '':
self.show_warning('Please enter name for animation')
else:
self.save_animation(self.animation_name.text())
self.list_widget.clear()
self.animation_name.setText('')
elif('Add Current Pose' == button_name):
if self.pose_name_temp.text() == '':
self.show_warning('Insert name for pose')
else:
self.animation_map[self.pose_name_temp.text()] = Quad(self.get_joint_state('l'), self.get_joint_state('r'), self.left_gripper_state, self.right_gripper_state)
list_item = QListWidgetItem()
list_item.setText(self.pose_name_temp.text())
self.list_widget.addItem(list_item)
self.pose_name_temp.setText('')
elif('Move to Bin' == button_name):
self.move_to_bin_action()
elif('Clean Room' == button_name):
rospy.loginfo("STARTING AUTONOMOUS MODE")
self.tuck_arms()
while(self.index < len(self.locations)):
self.roomNav.move_to_trash_location(self.locations[self.index])
# self.index += 1
self.head_action(1.0, 0, -0.50, True)
# Returns Nonce if nothing, and the point of the first object it sees otherwise
map_point = self.pap.detect_objects()
if(map_point == None):
self.index += 1
else:
self.pick_and_move_trash_action()
rospy.loginfo("FINISHED AUTONOMOUS MODE")
self.index = 1
elif('Object Detect' == button_name):
self.pick_and_move_trash_action()
def pick_and_move_trash_action(self, map_point = None):
if map_point == None:
self.head_action(1.0, 0, -0.50, True)
map_point = self.pap.detect_objects()
if map_point == None:
return
# Convert to base link and move towards the object 0.50m away
map_point = Transformer.transform(self._tf_listener, map_point.pose, map_point.header.frame_id, '/base_link')
rospy.loginfo("Object is " + str (map_point.pose.position.x) + " away")
'''
if(map_point.pose.position.x < 0.8):
self.roomNav.move_to_trash_location(self.locations[self.index - 1])
'''
move_back_first = False;
if(map_point.pose.position.x < 0.7):
move_back_first = True;
map_point.pose.position.x -= 0.450
map_point = Transformer.transform(self._tf_listener, map_point.pose, '/base_link', '/map')
if(move_back_first):
self.roomNav.move_to_trash_location(self.locations[self.index - 1])
success = self.roomNav.move_to_trash_location(map_point.pose)
'''This part of the code strafes the robot left to get closer to the object'''
'''
rate = rospy.Rate(10)
position = Point()
move_cmd = Twist()
move_cmd.linear.y = 0.25
odom_frame = '/odom_combined'
# Find out if the robot uses /base_link or /base_footprint
try:
self._tf_listener.waitForTransform(odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))
self.base_frame = '/base_footprint'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
try:
self._tf_listener.waitForTransform(odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0))
self.base_frame = '/base_link'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("Cannot find transform between " + odom_frame + " and /base_link or /base_footprint")
rospy.signal_shutdown("tf Exception")
# Get current position
position = self.get_odom()
x_start = position.x
y_start = position.y
# Distance travelled
distance = 0
goal_distance = 0.25
rospy.loginfo("Strafing left")
# Enter the loop to move along a side
while distance < goal_distance:
rospy.loginfo("Distance is at " + str(distance))
# Publish the Twist message and sleep 1 cycle
self.base_action(0, 0.25, 0, 0, 0, 0)
rate.sleep()
# Get the current position
position = self.get_odom()
# Compute the Euclidean distance from the start
distance = abs(position.y - y_start)
'''
if(success):
# Move head to look at the object, this will wait for a result
self.head_action(0, 0.4, 0.55, True)
# Move arms to ready pickup position, this will wait for a result before trying to detect and pick up object
self.animPlay.left_poses = self.saved_animations['ready_pickup'].left
self.animPlay.right_poses = self.saved_animations['ready_pickup'].right
self.animPlay.left_gripper_states = self.saved_animations['ready_pickup'].left_gripper
self.animPlay.right_gripper_states = self.saved_animations['ready_pickup'].right_gripper
self.animPlay.play('3.0')
for i in range (0,3):
success = self.pap.detect_and_pickup()
# Move head back to look forward
# Move head to look at the object, this will wait for a result
self.head_action(1.0, 0.0, 0.55, True)
if success:
break
# Move to bin
self.move_to_bin_action()
def get_odom(self):
# Get the current transform between the odom and base frames
try:
trans = self._tf_listener.lookupTransform('/odom_combined', self.base_frame, rospy.Time(0))
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("TF Exception")
return
return Point(trans[0][0],trans[0][1],trans[0][2])
# gripper_type is either 'l' for left or 'r' for right
# gripper position is the position as a parameter to the gripper goal
def gripper_action(self, gripper_type, gripper_position):
name_space = '/' + gripper_type + '_gripper_controller/gripper_action'
gripper_client = SimpleActionClient(name_space, GripperCommandAction)
gripper_client.wait_for_server()
gripper_goal = GripperCommandGoal()
gripper_goal.command.position = gripper_position
gripper_goal.command.max_effort = 30.0
gripper_client.send_goal(gripper_goal)
# update the state of the current gripper being modified
if (gripper_type == 'l'):
if (gripper_position == 0.0):
self.left_gripper_state = 'closed'
else:
self.left_gripper_state = 'open'
else:
if (gripper_position == 0.0):
self.right_gripper_state = 'closed'
else:
self.right_gripper_state = 'open'
def base_action(self, x, y, z, theta_x, theta_y, theta_z):
topic_name = '/base_controller/command'
base_publisher = rospy.Publisher(topic_name, Twist)
twist_msg = Twist()
twist_msg.linear = Vector3(x, y, z)
twist_msg.angular = Vector3(theta_x, theta_y, theta_z)
base_publisher.publish(twist_msg)
# Moves the torso of the robot down to its maximum
def torso_down(self, wait = False):
self.torso_client = SimpleActionClient('/torso_controller/position_joint_action', SingleJointPositionAction)
torso_goal = SingleJointPositionGoal()
torso_goal.position = 0.0
torso_goal.min_duration = rospy.Duration(5.0)
torso_goal.max_velocity = 1.0
self.torso_client.send_goal(torso_goal)
if wait:
# wait for the head movement to finish before we try to detect and pickup an object
finished_within_time = self.torso_client.wait_for_result(rospy.Duration(15))
# Check for success or failure
if not finished_within_time:
self.torso_client.cancel_goal()
rospy.loginfo("Timed out achieving torso movement goal")
else:
state = self.torso_client.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Torso movement goal succeeded!")
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo("Torso movement goal failed with error code: " + str(state))
def head_action(self, x, y, z, wait = False):
name_space = '/head_traj_controller/point_head_action'
head_client = SimpleActionClient(name_space, PointHeadAction)
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = 'base_link'
head_goal.min_duration = rospy.Duration(1.0)
head_goal.target.point = Point(x, y, z)
head_client.send_goal(head_goal)
if wait:
# wait for the head movement to finish before we try to detect and pickup an object
finished_within_time = head_client.wait_for_result(rospy.Duration(5))
# Check for success or failure
if not finished_within_time:
head_client.cancel_goal()
rospy.loginfo("Timed out achieving head movement goal")
else:
state = head_client.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Head movement goal succeeded!")
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo("Head movement goal failed with error code: " + str(self.goal_states[state]))
def tuck_arms(self):
rospy.loginfo('Left tuck arms')
self.animPlay.left_poses = self.saved_animations['left_tuck'].left
self.animPlay.right_poses = self.saved_animations['left_tuck'].right
self.animPlay.left_gripper_states = self.saved_animations['left_tuck'].left_gripper
self.animPlay.right_gripper_states = self.saved_animations['left_tuck'].right_gripper
self.animPlay.play('3.0')
def move_to_bin_action(self):
# First tuck arms
self.tuck_arms()
# Move to the bin
rospy.loginfo('Clicked the move to bin button')
self.roomNav.move_to_bin()
# Throw the trash away
rospy.loginfo('Throwing trash away with left arm')
self.animPlay.left_poses = self.saved_animations['l_dispose'].left
self.animPlay.right_poses = self.saved_animations['l_dispose'].right
self.animPlay.left_gripper_states = self.saved_animations['l_dispose'].left_gripper
self.animPlay.right_gripper_states = self.saved_animations['l_dispose'].right_gripper
self.animPlay.play('2.0')
# Tuck arms again
self.tuck_arms()
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
def show_text_in_rviz(self, text):
marker = Marker(type=Marker.TEXT_VIEW_FACING, id=0,
lifetime=rospy.Duration(1.5),
pose=Pose(Point(0.5, 0.5, 1.45), Quaternion(0, 0, 0, 1)),
scale=Vector3(0.06, 0.06, 0.06),
header=Header(frame_id='base_link'),
color=ColorRGBA(0.0, 1.0, 0.0, 0.8), text=text)
self.marker_publisher.publish(marker)
def show_arrow_in_rviz(self, arrow):
marker = Marker(type=Marker.ARROW, id=0,
lifetime=rospy.Duration(1.5),
pose=Pose(Point(0.5, 0.5, 1.45), Quaternion(0, 0, 0, 1)),
scale=Vector3(0.06, 0.06, 0.06),
header=Header(frame_id='base_link'),
color=ColorRGBA(0.0, 1.0, 0.0, 0.8), arrow=arrow)
self.marker_publisher.publish(marker)
def save_animation(self, animation_name):
if animation_name != '':
filename = os.path.join(self.dir, 'animations/')
anim_file = open(filename + animation_name + '.txt', 'w')
l_animation_queue = []
r_animation_queue = []
l_gripper_queue = []
r_gripper_queue = []
for i in range(self.list_widget.count()):
item = self.list_widget.item(i)
pose_name = item.text()
anim_file.write(re.sub(',' , '', str(self.animation_map[pose_name].left).strip('[]') +
' ' + str(self.animation_map[pose_name].right).strip('[]')))
anim_file.write('\n')
anim_file.write(str(self.animation_map[pose_name].left_gripper) + ' ' + str(self.animation_map[pose_name].right_gripper))
l_animation_queue.append(self.animation_map[pose_name].left)
r_animation_queue.append(self.animation_map[pose_name].right)
l_gripper_queue.append(self.animation_map[pose_name].left_gripper)
r_gripper_queue.append(self.animation_map[pose_name].right_gripper)
anim_file.write('\n')
anim_file.close()
self.saved_animations[animation_name] = Quad(l_animation_queue, r_animation_queue, l_gripper_queue, r_gripper_queue)
self.saved_animations_list.addItem(animation_name) # Bug? Multiple entries
# Reset the pending queue
self.l_animation_queue = []
self.r_animation_queue = []
else:
self.show_warning('Please insert name for animation')
def gripper_click(self, button_name):
grip_side = ''
grip_side_text = ''
if ('Left' in button_name):
grip_side = 'l'
grip_side_text = 'left'
else:
grip_side = 'r'
grip_side_text = 'right'
if ('Open' in button_name):
grip_action = 20.0
grip_action_text = 'close'
qWarning('Robot opened %s gripper' % (grip_side_text))
else:
grip_action = 0.0
grip_action_text = 'open'
qWarning('Robot closed %s gripper' % (grip_side_text))
self.show_text_in_rviz("%sing %s Gripper" % (grip_action_text.capitalize(), grip_side_text.capitalize()))
self.gripper_action(grip_side, grip_action)
self._widget.sender().setText('%s %s Gripper' % (grip_action_text.capitalize(), grip_side_text.capitalize()))
def rotate_head(self, button_name):
if('Left' in button_name):
#qWarning('x: %s, y: %s' % (self.head_x, self.head_y))
if (self.head_x < -0.8 and self.head_y > 0.0):
self.show_warning('Can\'t rotate anymore')
elif (self.head_y < 0.0):
self.head_x += 0.1
self.head_y = -((1.0 - self.head_x ** 2.0) ** 0.5)
self.show_text_in_rviz("Turning Head Left")
else:
self.head_x -= 0.1
self.head_y = (1.0 - self.head_x ** 2.0) ** 0.5
self.show_text_in_rviz("Turning Head Left")
qWarning('x: %s, y: %s' % (self.head_x, self.head_y))
self.head_action(self.head_x, self.head_y, self.head_z)
elif('Up' in button_name):
if (self.head_z <= 1.6):
self.head_z += 0.1
self.show_text_in_rviz("Moving Head Up")
self.head_action(self.head_x, self.head_y, self.head_z)
else:
self.show_warning('Can\'t look up anymore')
elif('Down' in button_name):
if (self.head_z >= -2.2):
self.head_z -= 0.1
self.show_text_in_rviz("Moving Head Down")
self.head_action(self.head_x, self.head_y, self.head_z)
else:
self.show_warning('Can\'t look down anymore')
else:
#qWarning('x: %s, y: %s' % (self.head_x, self.head_y))
if (self.head_x < -0.8 and self.head_y < 0.0):
self.show_warning('Can\'t rotate anymore')
elif (self.head_y > 0.0):
self.head_x += 0.1
self.head_y = (1.0 - self.head_x ** 2.0) ** 0.5
self.show_text_in_rviz("Turning Head Right")
else:
self.head_x -= 0.1
self.head_y = -((1.0 - self.head_x ** 2.0) ** 0.5)
self.show_text_in_rviz("Turning Head Right")
#qWarning('x: %s, y: %s' % (self.head_x, self.head_y))
self.head_action(self.head_x, self.head_y, self.head_z)
def toggle_arm(self, side, toggle, button):
controller_name = side + '_arm_controller'
start_controllers = []
stop_controllers = []
if (toggle == 'Relax'):
stop_controllers.append(controller_name)
else:
start_controllers.append(controller_name)
self.set_arm_mode(start_controllers, stop_controllers)
def set_arm_mode(self, start_controllers, stop_controllers):
try:
self.switch_service_client(start_controllers, stop_controllers, 1)
except rospy.ServiceException:
rospy.logerr('Could not change arm mode.')
def joint_states_cb(self, msg):
'''Callback function that saves the joint positions when a
joint_states message is received'''
self.lock.acquire()
self.all_joint_names = msg.name
self.all_joint_poses = msg.position
self.joint_sig.emit(msg)
self.lock.release()
def joint_sig_cb(self, msg):
pass
def get_joint_state(self, side_prefix):
'''Returns position for arm joints on the requested side (r/l)'''
if side_prefix == 'r':
joint_names = self.r_joint_names
else:
joint_names = self.l_joint_names
if self.all_joint_names == []:
rospy.logerr("No robot_state messages received yet!\n")
return None
positions = []
self.lock.acquire()
for joint_name in joint_names:
if joint_name in self.all_joint_names:
index = self.all_joint_names.index(joint_name)
position = self.all_joint_poses[index]
positions.append(position)
else:
rospy.logerr("Joint %s not found!", joint_name)
self.lock.release()
return None
self.lock.release()
return positions
def show_warning(self, text):
qWarning(text)
msgBox = QMessageBox()
msgBox.setText(text)
msgBox.exec_()
class SimpleGUI(Plugin):
# For sending speech
sound_sig = Signal(SoundRequest)
# Joints for arm poses
joint_sig = Signal(JointState)
def __init__(self, context):
self.prompt_width = 170
self.input_width = 250
super(SimpleGUI, self).__init__(context)
self.setObjectName('SimpleGUI')
self._widget = QWidget()
self._sound_client = SoundClient()
#find relative path for files to load
self.local_dir = os.path.dirname(__file__)
self.dir = os.path.join(self.local_dir, './lib/rqt_simplegui/')
if not os.path.isdir(self.dir):
os.makedirs(self.dir)
#need to add any additional subfolders as needed
if not os.path.isdir(self.dir + 'animations/'):
os.makedirs(self.dir + 'animations/')
# Creates a subscriber to the ROS topic, having msg type SoundRequest
rospy.Subscriber('robotsound', SoundRequest, self.sound_cb)
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
self.sound_sig.connect(self.sound_sig_cb)
# Code used for saving/ loading arm poses for the robot
switch_srv_name = 'pr2_controller_manager/switch_controller'
rospy.loginfo('Waiting for switch controller service...')
rospy.wait_for_service(switch_srv_name)
self.switch_service_client = rospy.ServiceProxy(
switch_srv_name, SwitchController)
self.r_joint_names = [
'r_shoulder_pan_joint', 'r_shoulder_lift_joint',
'r_upper_arm_roll_joint', 'r_elbow_flex_joint',
'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'
]
self.l_joint_names = [
'l_shoulder_pan_joint', 'l_shoulder_lift_joint',
'l_upper_arm_roll_joint', 'l_elbow_flex_joint',
'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'
]
self.all_joint_names = []
self.all_joint_poses = []
# Hash tables storing the name of the pose and the
# associated positions for that pose, initially empty
self.saved_l_poses = {}
self.saved_r_poses = {}
# Hash tables for storing name of animations and the associated pose list
self.saved_animations = {}
self.lock = threading.Lock()
rospy.Subscriber('joint_states', JointState, self.joint_states_cb)
#parsing for animations
dir = os.path.dirname(__file__)
qWarning(dir)
filename = os.path.join(self.dir, 'animations/')
ani_path = filename
ani_listing = os.listdir(ani_path)
for infile in ani_listing:
pose_left = []
pose_right = []
left_gripper_states = []
right_gripper_states = []
line_num = 1
for line in fileinput.input(ani_path + infile):
if (line_num % 2 == 1):
pose = [float(x) for x in line.split()]
pose_left.append(pose[:len(pose) / 2])
pose_right.append(pose[len(pose) / 2:])
else:
states = line.split()
left_gripper_states.append(states[0])
right_gripper_states.append(states[1])
line_num += 1
self.saved_animations[os.path.splitext(infile)[0]] = Quad(
pose_left, pose_right, left_gripper_states,
right_gripper_states)
# Create a trajectory action client
r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
self.r_traj_action_client = SimpleActionClient(r_traj_controller_name,
JointTrajectoryAction)
rospy.loginfo(
'Waiting for a response from the trajectory action server for RIGHT arm...'
)
self.r_traj_action_client.wait_for_server()
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(l_traj_controller_name,
JointTrajectoryAction)
rospy.loginfo(
'Waiting for a response from the trajectory action server for LEFT arm...'
)
self.l_traj_action_client.wait_for_server()
# Navigation functionality initialization
self.roomNav = RoomNavigator()
self._tf_listener = TransformListener()
self.animPlay = AnimationPlayer(None, None, None, None)
# Detection and pickup functionality
self.pap = PickAndPlaceManager(self._tf_listener, self.roomNav,
self.animPlay)
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
self.joint_sig.connect(self.joint_sig_cb)
# Create a large vertical box that is top aligned
large_box = QtGui.QVBoxLayout()
large_box.setAlignment(QtCore.Qt.AlignTop)
large_box.setMargin(0)
large_box.addItem(QtGui.QSpacerItem(10, 0))
# Buttons for controlling the head of the robot
head_box = QtGui.QHBoxLayout()
head_box.addItem(QtGui.QSpacerItem(230, 0))
head_box.addWidget(self.create_pressed_button('Head Up'))
head_box.addStretch(1)
large_box.addLayout(head_box)
button_box = QtGui.QHBoxLayout()
button_box.addItem(QtGui.QSpacerItem(80, 0))
button_box.addWidget(self.create_pressed_button('Head Turn Left'))
button_box.addWidget(self.create_pressed_button('Head Down'))
button_box.addWidget(self.create_pressed_button('Head Turn Right'))
button_box.addStretch(1)
button_box.setMargin(0)
button_box.setSpacing(0)
large_box.addLayout(button_box)
# Shows what the robot says
speech_box = QtGui.QHBoxLayout()
self.speech_label = QtGui.QLabel('Robot has not spoken yet')
palette = QtGui.QPalette()
palette.setColor(QtGui.QPalette.Foreground, QtCore.Qt.blue)
self.speech_label.setPalette(palette) #
speech_box.addItem(QtGui.QSpacerItem(100, 0))
#speech_box.addWidget(self.speech_label) #
large_box.addLayout(speech_box)
# Speak button
speak_button_box = QtGui.QHBoxLayout()
speech_prompt = QtGui.QLabel('Enter Speech Text:')
speech_prompt.setFixedWidth(self.prompt_width)
speak_button_box.addWidget(speech_prompt)
robot_says = QtGui.QLineEdit(self._widget)
robot_says.setFixedWidth(self.input_width)
robot_says.textChanged[str].connect(self.onChanged) #
speak_button_box.addWidget(robot_says)
speak_button_box.addWidget(self.create_button('Speak'))
speak_button_box.addStretch(1)
large_box.addLayout(speak_button_box)
large_box.addItem(QtGui.QSpacerItem(0, 50))
# Buttons for arm poses
pose_button_box1 = QtGui.QHBoxLayout()
pose_button_box1.addItem(QtGui.QSpacerItem(150, 0))
pose_button_box1.addWidget(self.create_button('Relax Left Arm'))
pose_button_box1.addWidget(self.create_button('Relax Right Arm'))
pose_button_box1.addStretch(1)
large_box.addLayout(pose_button_box1)
# Buttons for grippers
gripper_button_box = QtGui.QHBoxLayout()
gripper_button_box.addItem(QtGui.QSpacerItem(150, 0))
gripper_button_box.addWidget(self.create_button('Open Left Gripper'))
gripper_button_box.addWidget(self.create_button('Open Right Gripper'))
gripper_button_box.addStretch(1)
large_box.addLayout(gripper_button_box)
large_box.addItem(QtGui.QSpacerItem(0, 25))
# Buttons for animation
animation_box = QtGui.QHBoxLayout()
play_anim_label = QtGui.QLabel('Select Animation:')
play_anim_label.setFixedWidth(self.prompt_width)
animation_box.addWidget(play_anim_label)
self.saved_animations_list = QtGui.QComboBox(self._widget)
animation_box.addWidget(self.saved_animations_list)
pose_time_label = QtGui.QLabel('Duration(sec):')
pose_time_label.setFixedWidth(100)
animation_box.addWidget(pose_time_label)
self.pose_time = QtGui.QLineEdit(self._widget)
self.pose_time.setFixedWidth(50)
self.pose_time.setText('2.0')
animation_box.addWidget(self.pose_time)
animation_box.addWidget(self.create_button('Play Animation'))
animation_box.addStretch(1)
large_box.addLayout(animation_box)
animation_box2 = QtGui.QHBoxLayout()
animation_name_label = QtGui.QLabel('Enter Animation Name:')
animation_name_label.setFixedWidth(self.prompt_width)
animation_box2.addWidget(animation_name_label)
self.animation_name = QtGui.QLineEdit(self._widget)
self.animation_name.setFixedWidth(self.input_width)
animation_box2.addWidget(self.animation_name)
animation_box2.addWidget(self.create_button('Save Animation'))
animation_box2.addStretch(1)
large_box.addLayout(animation_box2)
animation_box3 = QtGui.QHBoxLayout()
pose_name_label = QtGui.QLabel('Enter Pose Name:')
pose_name_label.setFixedWidth(self.prompt_width)
animation_box3.addWidget(pose_name_label)
self.pose_name_temp = QtGui.QLineEdit(self._widget)
self.pose_name_temp.setFixedWidth(self.input_width)
animation_box3.addWidget(self.pose_name_temp)
animation_box3.addWidget(self.create_button('Add Current Pose'))
animation_box3.addStretch(1)
large_box.addLayout(animation_box3)
# Playing around with UI stuff
play_box = QtGui.QHBoxLayout()
pose_sequence_label = QtGui.QLabel('Current Pose Sequence:')
pose_sequence_label.setFixedWidth(self.prompt_width)
pose_sequence_label.setAlignment(QtCore.Qt.AlignTop)
self.list_widget = QListWidget()
self.list_widget.setDragDropMode(QAbstractItemView.InternalMove)
self.list_widget.setMaximumSize(self.input_width, 200)
play_box.addWidget(pose_sequence_label)
play_box.addWidget(self.list_widget)
play_box.addStretch(1)
large_box.addLayout(play_box)
large_box.addItem(QtGui.QSpacerItem(0, 50))
# Buttons for first row of base controls
first_base_button_box = QtGui.QHBoxLayout()
first_base_button_box.addItem(QtGui.QSpacerItem(70, 0))
first_base_button_box.addWidget(
self.create_pressed_button('Rotate Left'))
first_base_button_box.addWidget(self.create_pressed_button('^'))
first_base_button_box.addWidget(
self.create_pressed_button('Rotate Right'))
first_base_button_box.addStretch(1)
large_box.addLayout(first_base_button_box)
# Buttons for second row of base controls
second_base_button_box = QtGui.QHBoxLayout()
second_base_button_box.addItem(QtGui.QSpacerItem(70, 0))
second_base_button_box.addWidget(self.create_pressed_button('<'))
second_base_button_box.addWidget(self.create_pressed_button('v'))
second_base_button_box.addWidget(self.create_pressed_button('>'))
second_base_button_box.addWidget(self.create_button('Move to Bin'))
second_base_button_box.addWidget(self.create_button('Object Detect'))
second_base_button_box.addWidget(self.create_button('Clean Room'))
second_base_button_box.addStretch(1)
large_box.addLayout(second_base_button_box)
# Animation related items to store intermediate pose co-ordinates to save
self.animation_map = {}
self.create_state = False
self._widget.setObjectName('SimpleGUI')
self._widget.setLayout(large_box)
context.add_widget(self._widget)
# Look straight and down when launched
self.head_x = 1.0
self.head_y = 0.0
self.head_z = 0.5
self.head_action(self.head_x, self.head_y, self.head_z)
# Set grippers to closed on initialization
self.left_gripper_state = ''
self.right_gripper_state = ''
self.gripper_action('l', 0.0)
self.gripper_action('r', 0.0)
# Lab 6
self.marker_publisher = rospy.Publisher('visualization_marker', Marker)
# Saved states for poses
saved_pose_box = QtGui.QHBoxLayout()
self.saved_left_poses = QtGui.QLabel('')
self.saved_right_poses = QtGui.QLabel('')
saved_pose_box.addWidget(self.saved_left_poses)
saved_pose_box.addWidget(self.saved_right_poses)
large_box.addLayout(saved_pose_box)
# Preload the map of animations
self.saved_animations_list.addItems(self.saved_animations.keys())
# Move the torso all the way down
# self.torso_down(True)
# Autonomous navigation stuff
'''
self.locations = [Pose(Point(2.48293590546, 3.90075874329, 0.000), Quaternion(0.000, 0.000, -0.783917630973, 0.620864838632)),
Pose(Point(3.70106744766, 0.304672241211, 0.000), Quaternion(0.000, 0.000, 0.950186880196, -0.311680754463)),
Pose(Point(2.57326722145, 1.51304531097, 0.000), Quaternion(0.000, 0.000, 0.96127194482, -0.275601611212)),
Pose(Point(1.28060531616, 1.52380752563, 0.000), Quaternion(0.000, 0.000, 0.946345258806, -0.323157316388)),
Pose(Point(2.11048603058, 0.420155525208, 0.000), Quaternion(0.000, 0.000, 0.945222393391, -0.326427062346)),
Pose(Point(2.82733058929, -0.739856719971, 0.000), Quaternion(0.000, 0.000, 0.945473998362, -0.325697587373)),
Pose(Point(1.29184818268, -1.90485572815, 0.000), Quaternion(0.000, 0.000, 0.942275557345, -0.334838429739)),
Pose(Point(0.722846984863, -1.0921459198, 0.000), Quaternion(0.000, 0.000, 0.949330143731, -0.314280572424))]
'''
self.locations = [
Pose(Point(2.04748392105, 2.04748010635, 0.000),
Quaternion(0.000, 0.000, -0.776968030817, 0.629540053601)),
Pose(Point(2.34193611145, 1.43208932877, 0),
Quaternion(0, 0, -0.841674385779, 0.539985396398)),
Pose(Point(3.43202018738, -0.258297920227, 0.000),
Quaternion(0.000, 0.000, 0.996656413122, -0.0817067572629)),
Pose(Point(0.931655406952, -1.96435832977, 0.000),
Quaternion(0.000, 0.000, 0.691187586713, 0.722675390458)),
Pose(Point(-0.369112968445, 0.0330476760864, 0.000),
Quaternion(0.000, 0.000, 0.0275340398899, 0.999620866453))
]
self.index = 1
rospy.loginfo("Completed GUI initialization")
# Event for when text box is changed
def onChanged(self, text):
self.speech_label.setText(text)
self.speech_label.adjustSize()
def sound_cb(self, sound_request):
qWarning('Received sound.')
self.sound_sig.emit(sound_request)
def create_button(self, name):
btn = QtGui.QPushButton(name, self._widget)
btn.setFixedWidth(150)
btn.clicked.connect(self.command_cb)
return btn
def create_pressed_button(self, name):
btn = QtGui.QPushButton(name, self._widget)
btn.setFixedWidth(150)
btn.pressed.connect(self.command_cb)
btn.setAutoRepeat(True) # To make sure the movement repeats itself
return btn
def sound_sig_cb(self, sound_request):
qWarning('Received sound signal.')
qWarning('Robot said: ' + sound_request.arg)
self.speech_label.setText(sound_request.arg) #'Robot said: ' +
#a button was clicked
def command_cb(self):
button_name = self._widget.sender().text()
#robot talk button clicked
if (button_name == 'Speak'):
qWarning('Robot will say: ' + self.speech_label.text())
self._sound_client.say(self.speech_label.text())
self.show_text_in_rviz("Robot is Speaking")
#gripper button selected
elif ('Gripper' in button_name):
self.gripper_click(button_name)
# Move forward
elif (button_name == '^'):
self.base_action(0.25, 0.0, 0.0, 0.0, 0.0, 0.0)
# Move left
elif (button_name == '<'):
self.base_action(0.0, 0.25, 0.0, 0.0, 0.0, 0.0)
# Move right
elif (button_name == '>'):
self.base_action(0.0, -0.25, 0.0, 0.0, 0.0, 0.0)
# Move back
elif (button_name == 'v'):
self.base_action(-0.25, 0.0, 0.0, 0.0, 0.0, 0.0)
#Rotate Left
elif (button_name == 'Rotate Left'):
self.base_action(0.0, 0.0, 0.0, 0.0, 0.0, 0.50)
# Rotate Right
elif (button_name == 'Rotate Right'):
self.base_action(0.0, 0.0, 0.0, 0.0, 0.0, -0.50)
# A head button selected
elif ('Head' in button_name):
self.rotate_head(button_name)
#An arm button selected
#third param unused in freeze/relax
#Second word in button should be side
elif ('Arm' in button_name):
arm_side = button_name.split()[1]
if ('Freeze' in button_name or 'Relax' in button_name):
new_arm_state = button_name.split()[0]
self.toggle_arm(arm_side[0].lower(), new_arm_state, True)
old_arm_state = ''
if (new_arm_state == 'Relax'):
old_arm_state = 'Freeze'
else:
old_arm_state = 'Relax'
self._widget.sender().setText('%s %s Arm' %
(old_arm_state, arm_side))
elif ('Play Animation' == button_name):
self.animPlay.left_poses = self.saved_animations[
self.saved_animations_list.currentText()].left
self.animPlay.right_poses = self.saved_animations[
self.saved_animations_list.currentText()].right
self.animPlay.left_gripper_states = self.saved_animations[
self.saved_animations_list.currentText()].left_gripper
self.animPlay.right_gripper_states = self.saved_animations[
self.saved_animations_list.currentText()].right_gripper
if self.pose_time.text() == '':
self.show_warning('Please enter a duration in seconds.')
else:
self.animPlay.play(self.pose_time.text())
elif ('Animation' in button_name):
if ('Save' in button_name):
if self.animation_name.text() == '':
self.show_warning('Please enter name for animation')
else:
self.save_animation(self.animation_name.text())
self.list_widget.clear()
self.animation_name.setText('')
elif ('Add Current Pose' == button_name):
if self.pose_name_temp.text() == '':
self.show_warning('Insert name for pose')
else:
self.animation_map[self.pose_name_temp.text()] = Quad(
self.get_joint_state('l'), self.get_joint_state('r'),
self.left_gripper_state, self.right_gripper_state)
list_item = QListWidgetItem()
list_item.setText(self.pose_name_temp.text())
self.list_widget.addItem(list_item)
self.pose_name_temp.setText('')
elif ('Move to Bin' == button_name):
self.move_to_bin_action()
elif ('Clean Room' == button_name):
rospy.loginfo("STARTING AUTONOMOUS MODE")
self.tuck_arms()
while (self.index < len(self.locations)):
self.roomNav.move_to_trash_location(self.locations[self.index])
# self.index += 1
self.head_action(1.0, 0, -0.50, True)
# Returns Nonce if nothing, and the point of the first object it sees otherwise
map_point = self.pap.detect_objects()
if (map_point == None):
self.index += 1
else:
self.pick_and_move_trash_action()
rospy.loginfo("FINISHED AUTONOMOUS MODE")
self.index = 1
elif ('Object Detect' == button_name):
self.pick_and_move_trash_action()
def pick_and_move_trash_action(self, map_point=None):
if map_point == None:
self.head_action(1.0, 0, -0.50, True)
map_point = self.pap.detect_objects()
if map_point == None:
return
# Convert to base link and move towards the object 0.50m away
map_point = Transformer.transform(self._tf_listener, map_point.pose,
map_point.header.frame_id,
'/base_link')
rospy.loginfo("Object is " + str(map_point.pose.position.x) + " away")
'''
if(map_point.pose.position.x < 0.8):
self.roomNav.move_to_trash_location(self.locations[self.index - 1])
'''
move_back_first = False
if (map_point.pose.position.x < 0.7):
move_back_first = True
map_point.pose.position.x -= 0.450
map_point = Transformer.transform(self._tf_listener, map_point.pose,
'/base_link', '/map')
if (move_back_first):
self.roomNav.move_to_trash_location(self.locations[self.index - 1])
success = self.roomNav.move_to_trash_location(map_point.pose)
'''This part of the code strafes the robot left to get closer to the object'''
'''
rate = rospy.Rate(10)
position = Point()
move_cmd = Twist()
move_cmd.linear.y = 0.25
odom_frame = '/odom_combined'
# Find out if the robot uses /base_link or /base_footprint
try:
self._tf_listener.waitForTransform(odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))
self.base_frame = '/base_footprint'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
try:
self._tf_listener.waitForTransform(odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0))
self.base_frame = '/base_link'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("Cannot find transform between " + odom_frame + " and /base_link or /base_footprint")
rospy.signal_shutdown("tf Exception")
# Get current position
position = self.get_odom()
x_start = position.x
y_start = position.y
# Distance travelled
distance = 0
goal_distance = 0.25
rospy.loginfo("Strafing left")
# Enter the loop to move along a side
while distance < goal_distance:
rospy.loginfo("Distance is at " + str(distance))
# Publish the Twist message and sleep 1 cycle
self.base_action(0, 0.25, 0, 0, 0, 0)
rate.sleep()
# Get the current position
position = self.get_odom()
# Compute the Euclidean distance from the start
distance = abs(position.y - y_start)
'''
if (success):
# Move head to look at the object, this will wait for a result
self.head_action(0, 0.4, 0.55, True)
# Move arms to ready pickup position, this will wait for a result before trying to detect and pick up object
self.animPlay.left_poses = self.saved_animations[
'ready_pickup'].left
self.animPlay.right_poses = self.saved_animations[
'ready_pickup'].right
self.animPlay.left_gripper_states = self.saved_animations[
'ready_pickup'].left_gripper
self.animPlay.right_gripper_states = self.saved_animations[
'ready_pickup'].right_gripper
self.animPlay.play('3.0')
for i in range(0, 3):
success = self.pap.detect_and_pickup()
# Move head back to look forward
# Move head to look at the object, this will wait for a result
self.head_action(1.0, 0.0, 0.55, True)
if success:
break
# Move to bin
self.move_to_bin_action()
def get_odom(self):
# Get the current transform between the odom and base frames
try:
trans = self._tf_listener.lookupTransform('/odom_combined',
self.base_frame,
rospy.Time(0))
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("TF Exception")
return
return Point(trans[0][0], trans[0][1], trans[0][2])
# gripper_type is either 'l' for left or 'r' for right
# gripper position is the position as a parameter to the gripper goal
def gripper_action(self, gripper_type, gripper_position):
name_space = '/' + gripper_type + '_gripper_controller/gripper_action'
gripper_client = SimpleActionClient(name_space, GripperCommandAction)
gripper_client.wait_for_server()
gripper_goal = GripperCommandGoal()
gripper_goal.command.position = gripper_position
gripper_goal.command.max_effort = 30.0
gripper_client.send_goal(gripper_goal)
# update the state of the current gripper being modified
if (gripper_type == 'l'):
if (gripper_position == 0.0):
self.left_gripper_state = 'closed'
else:
self.left_gripper_state = 'open'
else:
if (gripper_position == 0.0):
self.right_gripper_state = 'closed'
else:
self.right_gripper_state = 'open'
def base_action(self, x, y, z, theta_x, theta_y, theta_z):
topic_name = '/base_controller/command'
base_publisher = rospy.Publisher(topic_name, Twist)
twist_msg = Twist()
twist_msg.linear = Vector3(x, y, z)
twist_msg.angular = Vector3(theta_x, theta_y, theta_z)
base_publisher.publish(twist_msg)
# Moves the torso of the robot down to its maximum
def torso_down(self, wait=False):
self.torso_client = SimpleActionClient(
'/torso_controller/position_joint_action',
SingleJointPositionAction)
torso_goal = SingleJointPositionGoal()
torso_goal.position = 0.0
torso_goal.min_duration = rospy.Duration(5.0)
torso_goal.max_velocity = 1.0
self.torso_client.send_goal(torso_goal)
if wait:
# wait for the head movement to finish before we try to detect and pickup an object
finished_within_time = self.torso_client.wait_for_result(
rospy.Duration(15))
# Check for success or failure
if not finished_within_time:
self.torso_client.cancel_goal()
rospy.loginfo("Timed out achieving torso movement goal")
else:
state = self.torso_client.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Torso movement goal succeeded!")
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo(
"Torso movement goal failed with error code: " +
str(state))
def head_action(self, x, y, z, wait=False):
name_space = '/head_traj_controller/point_head_action'
head_client = SimpleActionClient(name_space, PointHeadAction)
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = 'base_link'
head_goal.min_duration = rospy.Duration(1.0)
head_goal.target.point = Point(x, y, z)
head_client.send_goal(head_goal)
if wait:
# wait for the head movement to finish before we try to detect and pickup an object
finished_within_time = head_client.wait_for_result(
rospy.Duration(5))
# Check for success or failure
if not finished_within_time:
head_client.cancel_goal()
rospy.loginfo("Timed out achieving head movement goal")
else:
state = head_client.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Head movement goal succeeded!")
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo(
"Head movement goal failed with error code: " +
str(self.goal_states[state]))
def tuck_arms(self):
rospy.loginfo('Left tuck arms')
self.animPlay.left_poses = self.saved_animations['left_tuck'].left
self.animPlay.right_poses = self.saved_animations['left_tuck'].right
self.animPlay.left_gripper_states = self.saved_animations[
'left_tuck'].left_gripper
self.animPlay.right_gripper_states = self.saved_animations[
'left_tuck'].right_gripper
self.animPlay.play('3.0')
def move_to_bin_action(self):
# First tuck arms
self.tuck_arms()
# Move to the bin
rospy.loginfo('Clicked the move to bin button')
self.roomNav.move_to_bin()
# Throw the trash away
rospy.loginfo('Throwing trash away with left arm')
self.animPlay.left_poses = self.saved_animations['l_dispose'].left
self.animPlay.right_poses = self.saved_animations['l_dispose'].right
self.animPlay.left_gripper_states = self.saved_animations[
'l_dispose'].left_gripper
self.animPlay.right_gripper_states = self.saved_animations[
'l_dispose'].right_gripper
self.animPlay.play('2.0')
# Tuck arms again
self.tuck_arms()
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
def show_text_in_rviz(self, text):
marker = Marker(type=Marker.TEXT_VIEW_FACING,
id=0,
lifetime=rospy.Duration(1.5),
pose=Pose(Point(0.5, 0.5, 1.45),
Quaternion(0, 0, 0, 1)),
scale=Vector3(0.06, 0.06, 0.06),
header=Header(frame_id='base_link'),
color=ColorRGBA(0.0, 1.0, 0.0, 0.8),
text=text)
self.marker_publisher.publish(marker)
def show_arrow_in_rviz(self, arrow):
marker = Marker(type=Marker.ARROW,
id=0,
lifetime=rospy.Duration(1.5),
pose=Pose(Point(0.5, 0.5, 1.45),
Quaternion(0, 0, 0, 1)),
scale=Vector3(0.06, 0.06, 0.06),
header=Header(frame_id='base_link'),
color=ColorRGBA(0.0, 1.0, 0.0, 0.8),
arrow=arrow)
self.marker_publisher.publish(marker)
def save_animation(self, animation_name):
if animation_name != '':
filename = os.path.join(self.dir, 'animations/')
anim_file = open(filename + animation_name + '.txt', 'w')
l_animation_queue = []
r_animation_queue = []
l_gripper_queue = []
r_gripper_queue = []
for i in range(self.list_widget.count()):
item = self.list_widget.item(i)
pose_name = item.text()
anim_file.write(
re.sub(
',', '',
str(self.animation_map[pose_name].left).strip('[]') +
' ' +
str(self.animation_map[pose_name].right).strip('[]')))
anim_file.write('\n')
anim_file.write(
str(self.animation_map[pose_name].left_gripper) + ' ' +
str(self.animation_map[pose_name].right_gripper))
l_animation_queue.append(self.animation_map[pose_name].left)
r_animation_queue.append(self.animation_map[pose_name].right)
l_gripper_queue.append(
self.animation_map[pose_name].left_gripper)
r_gripper_queue.append(
self.animation_map[pose_name].right_gripper)
anim_file.write('\n')
anim_file.close()
self.saved_animations[animation_name] = Quad(
l_animation_queue, r_animation_queue, l_gripper_queue,
r_gripper_queue)
self.saved_animations_list.addItem(
animation_name) # Bug? Multiple entries
# Reset the pending queue
self.l_animation_queue = []
self.r_animation_queue = []
else:
self.show_warning('Please insert name for animation')
def gripper_click(self, button_name):
grip_side = ''
grip_side_text = ''
if ('Left' in button_name):
grip_side = 'l'
grip_side_text = 'left'
else:
grip_side = 'r'
grip_side_text = 'right'
if ('Open' in button_name):
grip_action = 20.0
grip_action_text = 'close'
qWarning('Robot opened %s gripper' % (grip_side_text))
else:
grip_action = 0.0
grip_action_text = 'open'
qWarning('Robot closed %s gripper' % (grip_side_text))
self.show_text_in_rviz(
"%sing %s Gripper" %
(grip_action_text.capitalize(), grip_side_text.capitalize()))
self.gripper_action(grip_side, grip_action)
self._widget.sender().setText(
'%s %s Gripper' %
(grip_action_text.capitalize(), grip_side_text.capitalize()))
def rotate_head(self, button_name):
if ('Left' in button_name):
#qWarning('x: %s, y: %s' % (self.head_x, self.head_y))
if (self.head_x < -0.8 and self.head_y > 0.0):
self.show_warning('Can\'t rotate anymore')
elif (self.head_y < 0.0):
self.head_x += 0.1
self.head_y = -((1.0 - self.head_x**2.0)**0.5)
self.show_text_in_rviz("Turning Head Left")
else:
self.head_x -= 0.1
self.head_y = (1.0 - self.head_x**2.0)**0.5
self.show_text_in_rviz("Turning Head Left")
qWarning('x: %s, y: %s' % (self.head_x, self.head_y))
self.head_action(self.head_x, self.head_y, self.head_z)
elif ('Up' in button_name):
if (self.head_z <= 1.6):
self.head_z += 0.1
self.show_text_in_rviz("Moving Head Up")
self.head_action(self.head_x, self.head_y, self.head_z)
else:
self.show_warning('Can\'t look up anymore')
elif ('Down' in button_name):
if (self.head_z >= -2.2):
self.head_z -= 0.1
self.show_text_in_rviz("Moving Head Down")
self.head_action(self.head_x, self.head_y, self.head_z)
else:
self.show_warning('Can\'t look down anymore')
else:
#qWarning('x: %s, y: %s' % (self.head_x, self.head_y))
if (self.head_x < -0.8 and self.head_y < 0.0):
self.show_warning('Can\'t rotate anymore')
elif (self.head_y > 0.0):
self.head_x += 0.1
self.head_y = (1.0 - self.head_x**2.0)**0.5
self.show_text_in_rviz("Turning Head Right")
else:
self.head_x -= 0.1
self.head_y = -((1.0 - self.head_x**2.0)**0.5)
self.show_text_in_rviz("Turning Head Right")
#qWarning('x: %s, y: %s' % (self.head_x, self.head_y))
self.head_action(self.head_x, self.head_y, self.head_z)
def toggle_arm(self, side, toggle, button):
controller_name = side + '_arm_controller'
start_controllers = []
stop_controllers = []
if (toggle == 'Relax'):
stop_controllers.append(controller_name)
else:
start_controllers.append(controller_name)
self.set_arm_mode(start_controllers, stop_controllers)
def set_arm_mode(self, start_controllers, stop_controllers):
try:
self.switch_service_client(start_controllers, stop_controllers, 1)
except rospy.ServiceException:
rospy.logerr('Could not change arm mode.')
def joint_states_cb(self, msg):
'''Callback function that saves the joint positions when a
joint_states message is received'''
self.lock.acquire()
self.all_joint_names = msg.name
self.all_joint_poses = msg.position
self.joint_sig.emit(msg)
self.lock.release()
def joint_sig_cb(self, msg):
pass
def get_joint_state(self, side_prefix):
'''Returns position for arm joints on the requested side (r/l)'''
if side_prefix == 'r':
joint_names = self.r_joint_names
else:
joint_names = self.l_joint_names
if self.all_joint_names == []:
rospy.logerr("No robot_state messages received yet!\n")
return None
positions = []
self.lock.acquire()
for joint_name in joint_names:
if joint_name in self.all_joint_names:
index = self.all_joint_names.index(joint_name)
position = self.all_joint_poses[index]
positions.append(position)
else:
rospy.logerr("Joint %s not found!", joint_name)
self.lock.release()
return None
self.lock.release()
return positions
def show_warning(self, text):
qWarning(text)
msgBox = QMessageBox()
msgBox.setText(text)
msgBox.exec_()