def execute(self, ud):
if ud['user_command'].command in self.valid_commands:
rospy.loginfo("Executing User Command '%s'", ud['user_command'].command)
ud['ucmd_progress'] = thorp_msgs.UserCommandFeedback('executing_command')
return ud['user_command'].command
else:
rospy.logwarn("Invalid User Command: '%s'", ud['user_command'].command)
ud['ucmd_outcome'] = thorp_msgs.UserCommandResult('invalid_command')
return 'invalid_command'
def execute(self, ud):
ud.base_cube_pose.pose.position.z += ud.cube_height
return 'succeeded'
rospy.init_node('stack_all_cubes_smach')
# Object manipulation top-level sm
sm = smach.StateMachine(outcomes=['stop', 'error', 'aborted', 'preempted'],
input_keys=['user_command'],
output_keys=['ucmd_progress', 'ucmd_outcome'])
with sm:
''' User data at startup '''
sm.userdata.user_command = thorp_msgs.UserCommandGoal()
sm.userdata.ucmd_progress = thorp_msgs.UserCommandFeedback()
sm.userdata.ucmd_outcome = thorp_msgs.UserCommandResult()
sm.userdata.od_attempt = 0
sm.userdata.arm_ref_frame = rospy.get_param('~arm_ctrl_ref_frame',
'arm_base_link')
sm.userdata.output_frame = rospy.get_param('~rec_objects_frame', 'map')
sm.userdata.cube_height = 0.025 # TODO get from Collision object!
# sm.userdata.named_pose_target_type = thorp_msgs.MoveToTargetGoal.NAMED_TARGET
# sm.userdata.arm_folded_named_pose = 'resting'
# sm.userdata.close_gripper = control_msgs.GripperCommand()
# sm.userdata.close_gripper.position = 0.0
# sm.userdata.open_gripper = control_msgs.GripperCommand()
# sm.userdata.open_gripper.position = 0.05
# Other fields created at runtime are objects, object_names, object_name, base_cube_name, base_cube_pose and other_cubes
smach.StateMachine.add('ExecuteUserCommand',