def point_head_to(self, position, frame, pointing_frame = None,
pointing_axis = None):
"""Point the head to the (x,y,z) tuple specified by position in the
passed frame. Default pointing frame is head_tilt_link with axis
[1,0,0].
"""
goal = PointHeadGoal()
if pointing_frame is None:
pointing_frame = "head_tilt_link"
if pointing_axis is None:
pointing_axis = [1,0,0]
goal.target.header.frame_id = frame
goal.target.point.x = position[0]
goal.target.point.y = position[1]
goal.target.point.z = position[2]
goal.pointing_frame = pointing_frame
goal.pointing_axis.x = pointing_axis[0]
goal.pointing_axis.y = pointing_axis[1]
goal.pointing_axis.z = pointing_axis[2]
goal.min_duration = rospy.Duration(self.time_to_reach)
self.head_pointer_client.send_goal_and_wait(goal,
rospy.Duration(self.time_to_reach))
def point_head_to(self,
position,
frame,
pointing_frame=None,
pointing_axis=None):
"""Point the head to the (x,y,z) tuple specified by position in the
passed frame. Default pointing frame is head_tilt_link with axis
[1,0,0].
"""
goal = PointHeadGoal()
if pointing_frame is None:
pointing_frame = "head_tilt_link"
if pointing_axis is None:
pointing_axis = [1, 0, 0]
goal.target.header.frame_id = frame
goal.target.point.x = position[0]
goal.target.point.y = position[1]
goal.target.point.z = position[2]
goal.pointing_frame = pointing_frame
goal.pointing_axis.x = pointing_axis[0]
goal.pointing_axis.y = pointing_axis[1]
goal.pointing_axis.z = pointing_axis[2]
goal.min_duration = rospy.Duration(self.time_to_reach)
self.head_pointer_client.send_goal_and_wait(
goal, rospy.Duration(self.time_to_reach))
def move_head(self,
x,
y,
z,
min_dur=0.0,
max_velocity=1.0,
frame_id='base_link',
timeout=5.0):
point = PointStamped()
point.header.frame_id = frame_id
point.header.stamp = rospy.Time.now()
point.point.x, point.point.y, point.point.z = x, y, z
goal = PointHeadGoal()
goal.pointing_frame = 'head_plate_frame'
goal.max_velocity = max_velocity
goal.min_duration = rospy.Duration.from_sec(min_dur)
goal.target = point
self.head_client.send_goal(goal)
self.head_client.wait_for_result(
timeout=rospy.Duration.from_sec(timeout))
if not self.head_client.get_state() == GoalStatus.SUCCEEDED:
rospy.logerr('can not move head to:\n %s' % (goal))
return False
return True
def look_at_cb(self, where):
""" where: PoseStamped."""
goal = PointHeadGoal()
goal.target = where
goal.pointing_frame = "high_def_frame"
goal.min_duration = rospy.Duration(0.5)
goal.max_velocity = 0.5
self.head_action_client.send_goal_and_wait(goal, rospy.Duration(5))
def look_at_cb(self, where):
""" where: PoseStamped."""
goal = PointHeadGoal()
goal.target = where
goal.pointing_frame = "high_def_frame"
goal.min_duration = rospy.Duration(0.5)
goal.max_velocity = 0.5
self.head_action_client.send_goal_and_wait(goal, rospy.Duration(5))
def goal( frame, pos, d ):
g = PointHeadGoal()
g.target.header.frame_id = frame
g.target.point.x = pos[0]
g.target.point.y = pos[1]
g.target.point.z = pos[2]
g.min_duration = rospy.Duration( d )
# Note, looking @ head action source, it seems pointing_frame unimplemented...
# Just account with a static offset (for now) where desired.
return g
def point_head(self):
print "Pointing head"
head_goal = PointHeadGoal()
head_goal.target = PoseConverter.to_point_stamped_msg('/torso_lift_link',
np.mat([1., 0.4, 0.]).T,
np.mat(np.eye(3)))
head_goal.target.header.stamp = rospy.Time()
head_goal.min_duration = rospy.Duration(3.)
head_goal.max_velocity = 0.2
self.head_point_sac.send_goal_and_wait(head_goal)
def move_head_to_point_and_wait(point):
rospy.loginfo("Moving head to point and waiting for action to finish")
headGoal = PointHeadGoal()
headGoal.target.header.frame_id = 'base_link'
headGoal.min_duration = rospy.Duration(1.0)
headGoal.target.point = Point(1,0,1)
headGoal.target.point = point
headActionClient.send_goal(headGoal)
headActionClient.wait_for_result(rospy.Duration(10))
rospy.loginfo("Head action client finished")
def PointAdd( x, y, z, dur, state, res ):
pgoal = PointHeadGoal()
pgoal.target.header.frame_id = '/torso_lift_link'
pgoal.target.point.x = x
pgoal.target.point.y = y
pgoal.target.point.z = z
pgoal.min_duration = rospy.Duration( dur )
pgoal.max_velocity = 1.0
smach.StateMachine.add(
state,
SimpleActionState( '/head_traj_controller/point_head_action',
PointHeadAction,
goal = pgoal ),
transitions = { 'succeeded' : res })
return
def PointAdd(x, y, z, dur, state, res):
pgoal = PointHeadGoal()
pgoal.target.header.frame_id = '/torso_lift_link'
pgoal.target.point.x = x
pgoal.target.point.y = y
pgoal.target.point.z = z
pgoal.min_duration = rospy.Duration(dur)
pgoal.max_velocity = 1.0
smach.StateMachine.add(
state,
SimpleActionState('/head_traj_controller/point_head_action',
PointHeadAction,
goal=pgoal),
transitions={'succeeded': res})
return
def lookAt (self, pose):
goal = PointHeadGoal()
point = PointStamped()
point.header.frame_id = 'torso_lift_link'
point.point.x = pose.x
point.point.y = pose.y
point.point.z = pose.z
goal.target = point
goal.pointing_frame = "head_mount_kinect_rgb_link"
goal.pointing_frame = "head_mount_link"
goal.min_duration = rospy.Duration(0.5)
goal.max_velocity = 1.0
self.client.send_goal(goal)
def repoint_goal_cb(userdata, goal):
# Convert PoseStamped in userdata to PointHeadGoal
# mobj = userdata.look_points.poses[0] # We'll have head point to first object.
# pgoal = PointHeadGoal()
# pgoal.target.header.frame_id = userdata.look_points.header.frame_id
# pgoal.target.point.x = mobj.pose.position.x
# pgoal.target.point.y = mobj.pose.position.y
# pgoal.target.point.z = mobj.pose.position.z
# pgoal.min_duration = rospy.Duration(0.6)
# pgoal.max_velocity = 1.0
pgoal = PointHeadGoal()
pgoal.target.header.frame_id = '/torso_lift_link'
pgoal.target.point.x = 0.50
pgoal.target.point.y = 0.0
pgoal.target.point.z = -0.35
pgoal.min_duration = rospy.Duration(0.6)
pgoal.max_velocity = 1.0
return pgoal
def repoint_goal_cb(userdata, goal):
# Convert PoseStamped in userdata to PointHeadGoal
# mobj = userdata.look_points.poses[0] # We'll have head point to first object.
# pgoal = PointHeadGoal()
# pgoal.target.header.frame_id = userdata.look_points.header.frame_id
# pgoal.target.point.x = mobj.pose.position.x
# pgoal.target.point.y = mobj.pose.position.y
# pgoal.target.point.z = mobj.pose.position.z
# pgoal.min_duration = rospy.Duration(0.6)
# pgoal.max_velocity = 1.0
pgoal = PointHeadGoal()
pgoal.target.header.frame_id = '/torso_lift_link'
pgoal.target.point.x = 0.50
pgoal.target.point.y = 0.0
pgoal.target.point.z = -0.35
pgoal.min_duration = rospy.Duration(0.6)
pgoal.max_velocity = 1.0
return pgoal
def lookAt(self, pos, sim=False):
goal = PointHeadGoal()
point = PointStamped()
point.header.frame_id = self.frame
point.point.x = pos[0]
point.point.y = pos[1]
point.point.z = pos[2]
goal.target = point
# Point using kinect frame
goal.pointing_frame = 'head_mount_kinect_rgb_link'
if sim:
goal.pointing_frame = 'high_def_frame'
goal.pointing_axis.x = 1
goal.pointing_axis.y = 0
goal.pointing_axis.z = 0
goal.min_duration = rospy.Duration(1.0)
goal.max_velocity = 1.0
self.pointHeadClient.send_goal_and_wait(goal)
def lookAt(self, frame_id, x, y, z):
self._action_client.wait_for_server()
goal = PointHeadGoal()
point = PointStamped()
point.header.frame_id = frame_id
point.point.x = x
point.point.y = y
point.point.z = z
goal.target = point
#pointing high-def camera frame
goal.pointing_frame = "high_def_frame"
goal.pointing_axis.x = 1
goal.pointing_axis.y = 0
goal.pointing_axis.z = 0
goal.min_duration = rospy.Duration(1.0)
goal.max_velocity = 1.0
self._action_client.send_goal(goal)
self._action_client.wait_for_result()
return self._action_client.get_result()
def move_head(self):
if self.head_goal.point.x == 0.0:
return None
goal = PointHeadGoal()
goal.pointing_frame = POINTING_FRAME_RH2
goal.pointing_axis.x = 1.0
goal.pointing_axis.y = 0.0
goal.pointing_axis.z = 0.0
print "MOVEMENT TIME" + str(self.period)
goal.min_duration = rospy.Duration(self.period)
#goal.min_duration = rospy.Duration(2.0)
goal.max_velocity = MAX_HEAD_VEL
goal.target = self.head_goal
rospy.loginfo('Goal frame id [%s]', goal.target.header.frame_id)
goal.target.header.stamp = rospy.Time().now()
if check_correct_goal(goal):
#rospy.loginfo('GOOOOOAAAAL SEEENT [%s]', str(goal))
self.client.send_goal(goal)
else:
rospy.loginfo("Not sending goal because it's malformed: \n" + str(goal))
return None
def move_head(self, x, y, z,
min_dur = 0.0,
max_velocity = 1.0,
frame_id = 'base_link',
timeout = 5.0):
point = PointStamped()
point.header.frame_id = frame_id
point.header.stamp = rospy.Time.now()
point.point.x, point.point.y, point.point.z = x, y, z
goal = PointHeadGoal()
goal.pointing_frame = 'head_plate_frame'
goal.max_velocity = max_velocity
goal.min_duration = rospy.Duration.from_sec( min_dur )
goal.target = point
self.head_client.send_goal( goal )
self.head_client.wait_for_result( timeout =
rospy.Duration.from_sec( timeout ) )
if not self.head_client.get_state() == GoalStatus.SUCCEEDED:
rospy.logerr( 'can not move head to:\n %s'%( goal ) )
return False
return True
def sm_fetch_object():
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['succeeded','aborted','preempted'],
input_keys = [ 'tagid' ])
with sm:
# Servo in towards table
approach = sm_rfid_servo_approach.sm_rfid_servo_approach()
smach.StateMachine.add(
'SERVO_APPROACH', # outcomes: succeeded, aborted, preempted
approach,
remapping = { 'tagid' : 'tagid'}, #input
transitions = { 'succeeded': 'POINT_HEAD' })
# Point Head Down (eventaully roll this and perceive table into own sm?)
pgoal = PointHeadGoal()
pgoal.target.header.frame_id = '/torso_lift_link'
pgoal.target.point.x = 0.50
pgoal.min_duration = rospy.Duration(0.6)
pgoal.max_velocity = 1.0
smach.StateMachine.add(
'POINT_HEAD',
SimpleActionState( '/head_traj_controller/point_head_action',
PointHeadAction,
goal = pgoal ),
transitions = { 'succeeded' : 'PERCEIVE_TABLE' })
# Detect table and determine possible approach directions!
# requires:
# roslaunch hrl_pr2_lib openni_kinect.launch
# roslaunch hrl_object_fetching tabletop_detect.launch
smach.StateMachine.add(
'PERCEIVE_TABLE',
ServiceState( '/table_detect_inst',
DetectTableInst,
request = DetectTableInstRequest( 1.0 ),
response_slots = ['grasp_points']), # PoseArray
transitions = {'succeeded':'PERCEIVE_OBJECT'},
remapping = {'grasp_points':'approach_poses'})
# Setment objects
smach.StateMachine.add(
'PERCEIVE_OBJECT',
ServiceState( '/obj_segment_inst',
DetectTableInst,
request = DetectTableInstRequest( 1.0 ),
response_slots = ['grasp_points']), # PoseArray
transitions = {'succeeded':'APPROACH_TABLE'},
remapping = {'grasp_points':'object_poses'}) #output
# Move to the desired approach vector
sm_table = sm_approach.sm_approach_table()
smach.StateMachine.add(
'APPROACH_TABLE',
sm_table,
remapping = {'table_edge_poses':'approach_poses', # input (PoseArray)
'movebase_pose_global':'approach_movebase_pose', # output (PoseStamped)
'table_edge_global':'table_edge_global'}, # output (PoseStamped)
transitions = {'succeeded':'REPOINT_HEAD'})
# Re-Point Head to table's edge.
def repoint_goal_cb(userdata, goal):
# Convert PoseStamped in userdata to PointHeadGoal
# mobj = userdata.look_points.poses[0] # We'll have head point to first object.
# pgoal = PointHeadGoal()
# pgoal.target.header.frame_id = userdata.look_points.header.frame_id
# pgoal.target.point.x = mobj.pose.position.x
# pgoal.target.point.y = mobj.pose.position.y
# pgoal.target.point.z = mobj.pose.position.z
# pgoal.min_duration = rospy.Duration(0.6)
# pgoal.max_velocity = 1.0
pgoal = PointHeadGoal()
pgoal.target.header.frame_id = '/torso_lift_link'
pgoal.target.point.x = 0.50
pgoal.target.point.y = 0.0
pgoal.target.point.z = -0.35
pgoal.min_duration = rospy.Duration(0.6)
pgoal.max_velocity = 1.0
return pgoal
smach.StateMachine.add(
'REPOINT_HEAD',
SimpleActionState( '/head_traj_controller/point_head_action',
PointHeadAction,
goal_cb = repoint_goal_cb,
input_keys = ['look_points']),
remapping = {'look_points':'object_poses'}, # input (PoseStamped)
transitions = { 'succeeded' : 'PREP_UNFOLD' })
# Prep unfold
smach.StateMachine.add(
'PREP_UNFOLD',
ServiceState( 'rfid_handoff/stow',
HandoffSrv,
request = HandoffSrvRequest()),
transitions = { 'succeeded':'UNFOLD' })
# Unfold
smach.StateMachine.add(
'UNFOLD',
ServiceState( 'traj_playback/unfold',
TrajPlaybackSrv,
request = TrajPlaybackSrvRequest( 0 )),
transitions = { 'succeeded':'MANIPULATE' })
# Manipulate
sm_grasp = sm_overhead_grasp.sm_grasp()
smach.StateMachine.add(
'MANIPULATE',
sm_grasp,
transitions = {'succeeded':'BACKUP'})
# Backup (60cm)
smach.StateMachine.add(
'BACKUP',
ServiceState( '/rotate_backup',
RotateBackupSrv,
request = RotateBackupSrvRequest(0.0, -0.50)),
transitions = { 'succeeded':'PRE_STOW' })
smach.StateMachine.add(
'PRE_STOW',
ServiceState( 'rfid_handoff/stow_grasp',
HandoffSrv,
request = HandoffSrvRequest()),
transitions = { 'succeeded':'succeeded' })
# # Setup robot for further navigation:
# # fold arms, position head, position ear antennas.
# smach.StateMachine.add(
# 'PRE_STOW',
# ServiceState( 'rfid_handoff/pre_stow',
# HandoffSrv,
# request = HandoffSrvRequest()),
# transitions = { 'succeeded':'STOW' })
# smach.StateMachine.add(
# 'STOW',
# ServiceState( 'rfid_handoff/stow',
# HandoffSrv,
# request = HandoffSrvRequest()),
# transitions = { 'succeeded':'succeeded' })
return sm
def cousins_demo():
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['succeeded','aborted','preempted'],
input_keys = [ 'explore_id', # should be ''
'obj_id',
'person_id',
'explore_radius' ])
with sm:
sm_search = sm_rfid_explore.sm_search()
smach.StateMachine.add(
'RFID_SEARCH', # outcomes: succeded, aborted, preempted
sm_search,
remapping = { 'tagid' : 'explore_id', # input
'explore_radius' : 'explore_radius' },
transitions={'succeeded':'BEST_VANTAGE_OBJ'})
# Get best vantage for obj.
# The NextBestVantage was initialized in the search.
smach.StateMachine.add(
'BEST_VANTAGE_OBJ',
ServiceState( '/rfid_recorder/best_vantage',
NextBestVantage,
request_slots = ['tagid'],
response_slots = ['best_vantage']),
remapping = {'best_vantage':'best_vantage_obj', # output
'tagid':'obj_id'}, # input
transitions = {'succeeded':'MOVE_VANTAGE_OBJ'})
smach.StateMachine.add(
'MOVE_VANTAGE_OBJ',
SimpleActionState( '/move_base',
MoveBaseAction,
goal_slots = [ 'target_pose' ]),
remapping = { 'target_pose' : 'best_vantage_obj' }, # input
transitions = {'aborted':'BEST_VANTAGE_OBJ',
'preempted':'aborted',
'succeeded':'FETCH'})
# # FETCH
# smach.StateMachine.add('FETCH',PrintStr( 'Fetching object' ),
# transitions = { 'succeeded' : 'BEST_VANTAGE_PERSON' })
# Fetch
smach.StateMachine.add(
'FETCH',
sm_fetch.sm_fetch_object(),
remapping = {'tagid':'obj_id'},
transitions = {'succeeded':'POINT_HEAD',
'aborted':'BACKUP'})
# Backup (60cm)
smach.StateMachine.add(
'BACKUP',
ServiceState( '/rotate_backup',
RotateBackupSrv,
request = RotateBackupSrvRequest(0.0, -0.50)),
transitions = { 'succeeded':'PRE_STOW' })
smach.StateMachine.add(
'PRE_STOW',
ServiceState( 'rfid_handoff/stow_grasp',
HandoffSrv,
request = HandoffSrvRequest()),
transitions = { 'succeeded':'POINT_HEAD' })
# Point Head Down (eventaully roll this and perceive table into own sm?)
pgoal = PointHeadGoal()
pgoal.target.header.frame_id = '/torso_lift_link'
pgoal.target.point.x = 0.50
pgoal.target.point.z = 0.30
pgoal.min_duration = rospy.Duration(0.6)
pgoal.max_velocity = 1.0
smach.StateMachine.add(
'POINT_HEAD',
SimpleActionState( '/head_traj_controller/point_head_action',
PointHeadAction,
goal = pgoal ),
transitions = { 'succeeded' : 'BEST_VANTAGE_PERSON' })
# Get best vantage for obj.
# The NextBestVantage was initialized in the search.
smach.StateMachine.add(
'BEST_VANTAGE_PERSON',
ServiceState( '/rfid_recorder/best_vantage',
NextBestVantage,
request_slots = ['tagid'],
response_slots = ['best_vantage']),
remapping = {'best_vantage':'best_vantage_person', # output
'tagid':'person_id'}, # input
transitions = {'succeeded':'MOVE_VANTAGE_PERSON'})
smach.StateMachine.add(
'MOVE_VANTAGE_PERSON',
SimpleActionState( '/move_base',
MoveBaseAction,
goal_slots = [ 'target_pose' ]),
remapping = { 'target_pose' : 'best_vantage_person' }, # input
transitions = {'aborted':'BEST_VANTAGE_PERSON',
'preempted':'aborted',
'succeeded':'DELIVERY'})
sm_delivery = sm_rfid_delivery.sm_delivery()
smach.StateMachine.add(
'DELIVERY', # outcomes: succeeded, aborted, preempted
sm_delivery,
remapping = { 'tagid' : 'person_id'}, #input
transitions = { 'succeeded': 'succeeded' })
return sm
def sm_fetch_object():
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['tagid'])
with sm:
# Servo in towards table
approach = sm_rfid_servo_approach.sm_rfid_servo_approach()
smach.StateMachine.add(
'SERVO_APPROACH', # outcomes: succeeded, aborted, preempted
approach,
remapping={'tagid': 'tagid'}, #input
transitions={'succeeded': 'POINT_HEAD'})
# Point Head Down (eventaully roll this and perceive table into own sm?)
pgoal = PointHeadGoal()
pgoal.target.header.frame_id = '/torso_lift_link'
pgoal.target.point.x = 0.50
pgoal.min_duration = rospy.Duration(0.6)
pgoal.max_velocity = 1.0
smach.StateMachine.add('POINT_HEAD',
SimpleActionState(
'/head_traj_controller/point_head_action',
PointHeadAction,
goal=pgoal),
transitions={'succeeded': 'PERCEIVE_TABLE'})
# Detect table and determine possible approach directions!
# requires:
# roslaunch hrl_pr2_lib openni_kinect.launch
# roslaunch hrl_object_fetching tabletop_detect.launch
smach.StateMachine.add(
'PERCEIVE_TABLE',
ServiceState('/table_detect_inst',
DetectTableInst,
request=DetectTableInstRequest(1.0),
response_slots=['grasp_points']), # PoseArray
transitions={'succeeded': 'PERCEIVE_OBJECT'},
remapping={'grasp_points': 'approach_poses'})
# Setment objects
smach.StateMachine.add(
'PERCEIVE_OBJECT',
ServiceState('/obj_segment_inst',
DetectTableInst,
request=DetectTableInstRequest(1.0),
response_slots=['grasp_points']), # PoseArray
transitions={'succeeded': 'APPROACH_TABLE'},
remapping={'grasp_points': 'object_poses'}) #output
# Move to the desired approach vector
sm_table = sm_approach.sm_approach_table()
smach.StateMachine.add(
'APPROACH_TABLE',
sm_table,
remapping={
'table_edge_poses': 'approach_poses', # input (PoseArray)
'movebase_pose_global':
'approach_movebase_pose', # output (PoseStamped)
'table_edge_global': 'table_edge_global'
}, # output (PoseStamped)
transitions={'succeeded': 'REPOINT_HEAD'})
# Re-Point Head to table's edge.
def repoint_goal_cb(userdata, goal):
# Convert PoseStamped in userdata to PointHeadGoal
# mobj = userdata.look_points.poses[0] # We'll have head point to first object.
# pgoal = PointHeadGoal()
# pgoal.target.header.frame_id = userdata.look_points.header.frame_id
# pgoal.target.point.x = mobj.pose.position.x
# pgoal.target.point.y = mobj.pose.position.y
# pgoal.target.point.z = mobj.pose.position.z
# pgoal.min_duration = rospy.Duration(0.6)
# pgoal.max_velocity = 1.0
pgoal = PointHeadGoal()
pgoal.target.header.frame_id = '/torso_lift_link'
pgoal.target.point.x = 0.50
pgoal.target.point.y = 0.0
pgoal.target.point.z = -0.35
pgoal.min_duration = rospy.Duration(0.6)
pgoal.max_velocity = 1.0
return pgoal
smach.StateMachine.add(
'REPOINT_HEAD',
SimpleActionState('/head_traj_controller/point_head_action',
PointHeadAction,
goal_cb=repoint_goal_cb,
input_keys=['look_points']),
remapping={'look_points': 'object_poses'}, # input (PoseStamped)
transitions={'succeeded': 'PREP_UNFOLD'})
# Prep unfold
smach.StateMachine.add('PREP_UNFOLD',
ServiceState('rfid_handoff/stow',
HandoffSrv,
request=HandoffSrvRequest()),
transitions={'succeeded': 'UNFOLD'})
# Unfold
smach.StateMachine.add('UNFOLD',
ServiceState('traj_playback/unfold',
TrajPlaybackSrv,
request=TrajPlaybackSrvRequest(0)),
transitions={'succeeded': 'MANIPULATE'})
# Manipulate
sm_grasp = sm_overhead_grasp.sm_grasp()
smach.StateMachine.add('MANIPULATE',
sm_grasp,
transitions={'succeeded': 'BACKUP'})
# Backup (60cm)
smach.StateMachine.add('BACKUP',
ServiceState('/rotate_backup',
RotateBackupSrv,
request=RotateBackupSrvRequest(
0.0, -0.50)),
transitions={'succeeded': 'PRE_STOW'})
smach.StateMachine.add('PRE_STOW',
ServiceState('rfid_handoff/stow_grasp',
HandoffSrv,
request=HandoffSrvRequest()),
transitions={'succeeded': 'succeeded'})
# # Setup robot for further navigation:
# # fold arms, position head, position ear antennas.
# smach.StateMachine.add(
# 'PRE_STOW',
# ServiceState( 'rfid_handoff/pre_stow',
# HandoffSrv,
# request = HandoffSrvRequest()),
# transitions = { 'succeeded':'STOW' })
# smach.StateMachine.add(
# 'STOW',
# ServiceState( 'rfid_handoff/stow',
# HandoffSrv,
# request = HandoffSrvRequest()),
# transitions = { 'succeeded':'succeeded' })
return sm
offset_zeros = Vector3Stamped()
offset_zeros.header.frame_id = '/base_link'
request.goal_offset = offset_zeros # no offset
response = move_gripper(request)
# Move head to hard-coded position
client = actionlib.SimpleActionClient(
'/head_traj_controller/point_head_action', PointHeadAction)
client.wait_for_server()
g = PointHeadGoal()
g.target.header.frame_id = 'base_link'
g.target.point.x = 1.0
g.target.point.y = -0.45
g.target.point.z = 0.6
g.min_duration = rospy.Duration(1.0)
client.send_goal(g)
client.wait_for_result()
if client.get_state() == GoalStatus.SUCCEEDED:
print "Succeeded"
else:
print "Failed"
######### Find Stuff #################
# Find a device
rospy.wait_for_service('find_device')
