def on_play_2(self):
change_to_controller('position')
pm = SimpleActionClient('/play_motion', PlayMotionAction)
pm.wait_for_server()
pmg = PlayMotionGoal()
pmg.motion_name = 'LBD_2X'
pm.send_goal(pmg)
pm.wait_for_result(rospy.Duration(0.1))
def __init__(self, parent=None):
super(LearningGUI, self).__init__(parent)
rospy.loginfo("Getting joint limits from URDF...")
self.joint_limits = get_joint_limits()
self.rospack = rospkg.RosPack()
self.continuous_recording_mode_active = False
path = self.rospack.get_path('learning_gui')
print "path is: " + str(path)
# Decide which GUI to use thanks to joint states...
# set subscriber
self.last_js = None
self.joint_states_sub = rospy.Subscriber('/joint_states',
JointState,
self.js_cb,
queue_size=1)
rospy.loginfo("Waiting for first joint states...")
while self.last_js is None:
rospy.sleep(0.2)
rospy.loginfo("Guessing end effector in use...")
if "hand_thumb_joint" in self.last_js.name:
uic.loadUi(path + '/resources/tiago_learn_hand.ui', self)
self.joint_names = [
'head_1_joint', 'head_2_joint', 'arm_1_joint', 'arm_2_joint',
'arm_3_joint', 'arm_4_joint', 'arm_5_joint', 'arm_6_joint',
'arm_7_joint', 'torso_lift_joint', 'hand_index_joint',
'hand_mrl_joint', 'hand_thumb_joint'
]
self.eef = 'hand'
elif "gripper_left_finger_joint" in self.last_js.name:
uic.loadUi(path + '/resources/tiago_learn_gripper.ui', self)
self.joint_names = [
'head_1_joint', 'head_2_joint', 'arm_1_joint', 'arm_2_joint',
'arm_3_joint', 'arm_4_joint', 'arm_5_joint', 'arm_6_joint',
'arm_7_joint', 'torso_lift_joint', 'gripper_left_finger_joint',
'gripper_right_finger_joint'
]
self.eef = 'gripper'
self.set_sliders_limits()
self.set_callbacks_sliders()
self.controllers_srv = rospy.ServiceProxy(
'/controller_manager/list_controllers', ListControllers)
change_to_controller('position')
self.controller_mode = 'position'
self.gui_disabled_status = {
'gravity_b': False,
'position_b': True,
'start_continuous': False,
'stop_recording': True,
'capture_waypoint': False,
'play_05_b': True,
'play_1_b': True,
'play_2_b': True,
'save_b': True,
'arm_1_slider': False, # For wrist controller
'arm_2_slider': False,
'arm_3_slider': False,
'arm_4_slider': False
}
self.set_buttons_disabled()
# check if we are in gravity to enable stuff related to it like
# commanding with the wrist
self.set_callbacks_buttons()
self.show()
self.update_gui_rate = 2.0 # Hz
self.arm_pub = rospy.Publisher('/arm_controller/command',
JointTrajectory,
queue_size=1)
self.wrist_pub = rospy.Publisher('/wrist_controller/command',
JointTrajectory,
queue_size=1)
self.head_pub = rospy.Publisher('/head_controller/command',
JointTrajectory,
queue_size=1)
self.torso_pub = rospy.Publisher('/torso_controller/command',
JointTrajectory,
queue_size=1)
self.hand_pub = rospy.Publisher('/hand_controller/command',
JointTrajectory,
queue_size=1)
self.gripper_pub = rospy.Publisher('/gripper_controller/command',
JointTrajectory,
queue_size=1)
# self.play_motion_pub = rospy.Publisher(
# '/play_motion/goal', PlayMotionActionGoal, queue_size=1)
self.update_timer = QTimer(self)
self.update_timer.setInterval(1000.0 / self.update_gui_rate)
self.update_timer.timeout.connect(self.update_gui)
self.update_timer.start()
def on_position(self):
change_to_controller('position')
self.gui_disabled_status['position_b'] = True
self.gui_disabled_status['gravity_b'] = False
def on_gravity(self):
change_to_controller('gravity', self.use_wrist_in_position)
self.gui_disabled_status['position_b'] = False
self.gui_disabled_status['gravity_b'] = True
