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 TextSearchFrame(QDockWidget):
'''
A frame to find text in the Editor.
'''
search_result_signal = Signal(str, bool, str, int)
''' @ivar: A signal emitted after search_threaded was started.
(search text, found or not, file, position in text)
for each result a signal will be emitted.
'''
replace_signal = Signal(str, str, int, str)
''' @ivar: A signal emitted to replace string at given position.
(search text, file, position in text, replaced by text)
'''
def __init__(self, tabwidget, parent=None):
QDockWidget.__init__(self, "Find", parent)
self.setObjectName('SearchFrame')
self.setFeatures(QDockWidget.DockWidgetMovable
| QDockWidget.DockWidgetFloatable)
self._dockwidget = QFrame(self)
self.vbox_layout = QVBoxLayout(self._dockwidget)
self.layout().setContentsMargins(0, 0, 0, 0)
self.layout().setSpacing(1)
# frame with two rows for find and replace
find_replace_frame = QFrame(self)
find_replace_vbox_layout = QVBoxLayout(find_replace_frame)
find_replace_vbox_layout.setContentsMargins(0, 0, 0, 0)
find_replace_vbox_layout.setSpacing(1)
find_replace_vbox_layout.addSpacerItem(
QSpacerItem(1, 1, QSizePolicy.Expanding, QSizePolicy.Expanding))
# create frame with find row
find_frame = self._create_find_frame()
find_replace_vbox_layout.addWidget(find_frame)
rplc_frame = self._create_replace_frame()
find_replace_vbox_layout.addWidget(rplc_frame)
# frame for find&replace and search results
self.vbox_layout.addWidget(find_replace_frame)
self.vbox_layout.addWidget(self._create_found_frame())
self.vbox_layout.addStretch(2024)
self.setWidget(self._dockwidget)
# intern search parameters
self._tabwidget = tabwidget
self.current_search_text = ''
self.search_results = []
self.search_results_fileset = set()
self._search_result_index = -1
self._search_recursive = False
self._search_thread = None
def _create_find_frame(self):
find_frame = QFrame(self)
find_hbox_layout = QHBoxLayout(find_frame)
find_hbox_layout.setContentsMargins(0, 0, 0, 0)
find_hbox_layout.setSpacing(1)
self.search_field = EnchancedLineEdit(find_frame)
self.search_field.setPlaceholderText('search text')
self.search_field.textChanged.connect(self.on_search_text_changed)
self.search_field.returnPressed.connect(self.on_search)
find_hbox_layout.addWidget(self.search_field)
self.search_result_label = QLabel(find_frame)
self.search_result_label.setText(' ')
find_hbox_layout.addWidget(self.search_result_label)
self.find_button_back = QPushButton("<")
self.find_button_back.setFixedWidth(44)
self.find_button_back.clicked.connect(self.on_search_back)
find_hbox_layout.addWidget(self.find_button_back)
self.find_button = QPushButton(">")
self.find_button.setDefault(True)
# self.find_button.setFlat(True)
self.find_button.setFixedWidth(44)
self.find_button.clicked.connect(self.on_search)
find_hbox_layout.addWidget(self.find_button)
return find_frame
def _create_replace_frame(self):
# create frame with replace row
self.rplc_frame = rplc_frame = QFrame(self)
rplc_hbox_layout = QHBoxLayout(rplc_frame)
rplc_hbox_layout.setContentsMargins(0, 0, 0, 0)
rplc_hbox_layout.setSpacing(1)
self.replace_field = EnchancedLineEdit(rplc_frame)
self.replace_field.setPlaceholderText('replace text')
self.replace_field.returnPressed.connect(self.on_replace)
rplc_hbox_layout.addWidget(self.replace_field)
self.replace_result_label = QLabel(rplc_frame)
self.replace_result_label.setText(' ')
rplc_hbox_layout.addWidget(self.replace_result_label)
self.replace_button = replace_button = QPushButton("> &Replace >")
replace_button.setFixedWidth(90)
replace_button.clicked.connect(self.on_replace_click)
rplc_hbox_layout.addWidget(replace_button)
rplc_frame.setVisible(False)
return rplc_frame
def _create_found_frame(self):
self.found_files_frame = ff_frame = QGroupBox("recursive search")
ff_frame.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
self.found_files_vbox_layout = QVBoxLayout(ff_frame)
self.found_files_vbox_layout.setSpacing(0)
self.found_files_vbox_layout.setContentsMargins(0, 0, 0, 0)
self.found_files_list = QListWidget(ff_frame)
self.found_files_list.setSizePolicy(QSizePolicy.Expanding,
QSizePolicy.Expanding)
self.found_files_list.setFrameStyle(QFrame.StyledPanel)
self.found_files_list.itemActivated.connect(self.on_itemActivated)
self.found_files_list.setStyleSheet("QListWidget {"
"background-color:transparent;"
"}"
"QListWidget::item {"
"background-color:transparent;"
"}"
"QListWidget::item:selected {"
"background-color: darkgray;"
"}")
self.found_files_vbox_layout.addWidget(self.found_files_list)
ff_frame.setCheckable(True)
ff_frame.setChecked(False)
ff_frame.setFlat(True)
self.found_files_list.setVisible(False)
return self.found_files_frame
def keyPressEvent(self, event):
'''
Enable the shortcats for search and replace
'''
self.parent().keyPressEvent(event)
def on_search(self):
'''
Initiate the new search or request a next search result.
'''
if self.current_search_text != self.search_field.text(
) or self._search_recursive != self.found_files_frame.isChecked():
# clear current search results
self._reset()
self.current_search_text = self.search_field.text()
if self.current_search_text:
path_text = {}
self._wait_for_result = True
for i in range(self._tabwidget.count()):
path_text[self._tabwidget.widget(
i).filename] = self._tabwidget.widget(
i).document().toPlainText()
self._search_recursive = self.found_files_frame.isChecked()
self._search_thread = TextSearchThread(
self.current_search_text,
self._tabwidget.currentWidget().filename,
path_text=path_text,
recursive=self._search_recursive)
self._search_thread.search_result_signal.connect(
self.on_search_result)
self._search_thread.warning_signal.connect(
self.on_warning_result)
self._search_thread.start()
elif self.search_results:
self._check_position()
if self.search_results:
if self._search_result_index + 1 >= len(self.search_results):
self._search_result_index = -1
self._search_result_index += 1
self.search_result_signal.emit(
*self.search_results[self._search_result_index])
self.replace_button.setEnabled(True)
self._update_label()
def on_search_back(self):
'''
Slot to handle the search back function.
'''
self._check_position(False)
if self.search_results:
self._search_result_index -= 1
if self._search_result_index < 0:
self._search_result_index = len(self.search_results) - 1
self._update_label()
self.search_result_signal.emit(
*self.search_results[self._search_result_index])
self.replace_button.setEnabled(True)
def _check_position(self, forward=True):
try:
# if the position of the textCursor was changed by the user, move the search index
cur_pos = self._tabwidget.currentWidget().textCursor().position()
st, _f, pa, idx = self.search_results[self._search_result_index]
sear_pos = idx + len(st)
if cur_pos != sear_pos:
first_idx = self._get_current_index_for_current_file()
if first_idx != -1:
st, _f, pa, idx = self.search_results[first_idx]
sear_pos = idx + len(st)
while cur_pos > sear_pos and self._tabwidget.currentWidget(
).filename == pa:
first_idx += 1
st, _f, pa, idx = self.search_results[first_idx]
sear_pos = idx + len(st)
self._search_result_index = first_idx
if forward:
self._search_result_index -= 1
else:
self._reset(True)
except:
pass
def _get_current_index_for_current_file(self):
for index in range(len(self.search_results)):
_st, _f, pa, _idx = self.search_results[index]
if self._tabwidget.currentWidget().filename == pa:
return index
return -1
def on_search_result(self, search_text, found, path, index):
'''
Slot to handle the signals for search result. This signals are forwarded used
search_result_signal.
'''
if found and search_text == self.current_search_text:
self.search_results_fileset.add(path)
item = (search_text, found, path, index)
if item not in self.search_results:
self.search_results.append((search_text, found, path, index))
if self._wait_for_result:
self._search_result_index += 1
if index >= self._tabwidget.currentWidget().textCursor(
).position() or self._tabwidget.currentWidget(
).filename != path:
self._wait_for_result = False
self.search_result_signal.emit(
*self.search_results[self._search_result_index])
self.replace_button.setEnabled(True)
if self.search_results:
if len(self.search_results_fileset) > 1:
for item in self.search_results_fileset:
pkg, path = package_name(os.path.dirname(item))
itemstr = '%s [%s]' % (os.path.basename(item), pkg)
if not self.found_files_list.findItems(
itemstr, Qt.MatchExactly):
list_item = QListWidgetItem(itemstr)
list_item.setToolTip(item)
self.found_files_list.addItem(list_item)
self.found_files_frame.setVisible(True)
self.found_files_list.setVisible(
len(self.search_results_fileset) > 0)
self._update_label()
def on_warning_result(self, text):
rospy.logwarn(text)
def on_replace_click(self):
self.on_replace()
self.on_search()
def on_replace(self):
'''
Emits the replace signal, but only if currently selected text is equal to the searched one.
'''
if self.search_results:
try:
search_text, _found, path, index = self.search_results[
self._search_result_index]
cursor = self._tabwidget.currentWidget().textCursor()
if cursor.selectedText() == search_text:
rptxt = self.replace_field.text()
for rindex in range(self._search_result_index + 1,
len(self.search_results)):
st, _f, pa, idx = self.search_results[rindex]
if path == pa:
self.search_results.pop(rindex)
self.search_results.insert(
rindex,
(st, _f, pa, idx + len(rptxt) - len(st)))
else:
break
self._remove_search_result(self._search_result_index)
self.replace_signal.emit(search_text, path, index, rptxt)
else:
self.replace_button.setEnabled(False)
except:
import traceback
print traceback.format_exc()
pass
def on_itemActivated(self, item):
'''
Go to the results for the selected file entry in the list.
'''
item_path = item.toolTip()
new_search_index = -1
tmp_index = -1
search_index = -1
tmp_search_text = ''
for search_text, found, path, index in self.search_results:
new_search_index += 1
if item_path == path:
if tmp_index == -1:
tmp_index = new_search_index
tmp_search_text = search_text
search_index = index
if new_search_index > self._search_result_index:
self.search_result_signal.emit(search_text, found, path,
index)
self._search_result_index = new_search_index
self._update_label()
return
if tmp_index != -1:
self.search_result_signal.emit(tmp_search_text, True, item_path,
search_index)
self._search_result_index = tmp_index
self._update_label()
def on_search_text_changed(self, _text):
'''
Clear search result if the text was changed.
'''
self._reset()
def _update_label(self, clear_label=False):
'''
Updates the status label for search results. The info is created from search result lists.
'''
msg = ' '
if self.search_results:
count_files = len(self.search_results_fileset)
msg = '%d/%d' % (self._search_result_index + 1,
len(self.search_results))
if count_files > 1:
msg = '%s(%d)' % (msg, count_files)
if self._search_thread is not None and self._search_thread.is_alive():
msg = 'searching..%s' % msg
elif not msg.strip() and self.current_search_text:
msg = '0 found'
self.current_search_text = ''
if clear_label:
msg = ' '
self.search_result_label.setText(msg)
self.find_button_back.setEnabled(len(self.search_results))
def file_changed(self, path):
'''
Clears search results if for changed file are some search results are available
:param path: changed file path
:type path: str
'''
if path in self.search_results_fileset:
self._reset()
def set_replace_visible(self, value):
self.rplc_frame.setVisible(value)
self.raise_()
self.activateWindow()
if value:
self.replace_field.setFocus()
self.replace_field.selectAll()
self.setWindowTitle("Find / Replace")
else:
self.setWindowTitle("Find")
self.search_field.setFocus()
def is_replace_visible(self):
return self.rplc_frame.isVisible()
def _reset(self, force_new_search=False):
# clear current search results
if self._search_thread is not None:
self._search_thread.search_result_signal.disconnect()
self._search_thread.stop()
self._search_thread = None
self.current_search_text = ''
self.search_results = []
self.search_results_fileset = set()
self.found_files_list.clear()
self.found_files_list.setVisible(False)
self._update_label(True)
self._search_result_index = -1
self.find_button_back.setEnabled(False)
if force_new_search:
self.on_search()
def enable(self):
self.setVisible(True)
# self.show()
self.raise_()
self.activateWindow()
self.search_field.setFocus()
self.search_field.selectAll()
def _remove_search_result(self, index):
try:
self.search_results.pop(index)
# create new set with files contain the search text
new_path_set = set(path
for _st, _fd, path, _idx in self.search_results)
# remove the file from the list widget
for pp in self.search_results_fileset - new_path_set:
for wi_idx in range(self.found_files_list.count()):
# we have to compare each tooltip of the item
if pp == self.found_files_list.item(wi_idx).toolTip():
self.found_files_list.takeItem(wi_idx)
break
self.search_results_fileset = new_path_set
self.found_files_list.setVisible(
len(self.search_results_fileset) > 0)
except:
import traceback
print traceback.format_exc()
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_()