def main():
rospy.init_node('arm_calibration')
sm_arm_calibration = smach.StateMachine(
outcomes=['calibrated', 'calibration_failed'])
with sm_arm_calibration:
sm_arm_calibration.userdata.motors = [
"torso_turn", "torso_lift", "shoulder", "elbow", "wrist",
"gripper", "head_pan", "head_tilt"
]
smach.StateMachine.add('EnableMotors',
misc_tools.EnableMotors(),
remapping={'motors': 'motors'},
transitions={'success': 'ArmCalibration'})
sm_arm = arm_calibration_sm.createSM()
smach.StateMachine.add('ArmCalibration',
sm_arm,
transitions={
'calibrated': 'calibrated',
'calibration_failed': 'calibration_failed'
})
sis = smach_ros.IntrospectionServer('introspection_server',
sm_arm_calibration, '/SM_ROOT')
sis.start()
sm_arm_calibration.execute()
sis.stop()
def main():
rospy.init_node('head_calibration')
sm_head_calibration = smach.StateMachine(
outcomes=['calibrated', 'calibration_failed'])
with sm_head_calibration:
sm_head_calibration.userdata.motors = ["head_pan", "head_tilt"]
smach.StateMachine.add('EnableMotors',
misc_tools.EnableMotors(),
remapping={'motors': 'motors'},
transitions={'success': 'HeadCalibration'})
sm_head = head_calibration.createSM()
smach.StateMachine.add('HeadCalibration',
sm_head,
remapping={'motors': 'motors'},
transitions={
'head_calibrated': 'calibrated',
'head_calibration_failed':
'calibration_failed'
})
sis = smach_ros.IntrospectionServer('introspection_server',
sm_head_calibration, '/SM_ROOT')
sis.start()
sm_head_calibration.execute()
sis.stop()
def main():
rospy.init_node('move_arm_planner_as')
sm = smach.StateMachine(outcomes=['success', 'preempted', 'error'],
input_keys=['goal'],
output_keys=['feedback', 'result'])
sm.userdata.goal_link_name = "palm_link"
sm.userdata.motors = [
'torso_turn', 'torso_lift', 'shoulder', 'elbow', 'wrist'
]
sm.userdata.feedback = pick_and_place_msgs.MoveArmFeedback()
sm.userdata.result = pick_and_place_msgs.MoveArmResult()
sm.userdata.error_code = int()
sm.userdata.pose_goal = True
with sm:
smach.StateMachine.add('ParseGoal',
ParseGoal(),
remapping={
'incoming_goal': 'goal',
'feedback': 'feedback',
'goal_pose': 'goal_pose'
},
transitions={'success': 'EnableMotors'})
smach.StateMachine.add('EnableMotors',
misc_tools.EnableMotors(),
transitions={'success': 'MoveArm'},
remapping={'motors': 'motors'})
sm_move_arm = move_arm_sm.createSM()
smach.StateMachine.add('MoveArm',
sm_move_arm,
remapping={
'goal_link_name': 'goal_link_name',
'goal_pose': 'goal_pose',
'result': 'result'
},
transitions={
'success': 'success',
'preempted': 'preempted',
'error': 'error'
})
asw = ActionServerWrapper('move_arm_planner',
pick_and_place_msgs.MoveArmAction,
wrapped_container=sm,
goal_key='goal',
feedback_key='feedback',
result_key='result',
succeeded_outcomes=['success'],
aborted_outcomes=['error'],
preempted_outcomes=['preempted'])
asw.run_server()
rospy.spin()
def main():
rospy.init_node('move_gripper_as')
sm = smach.StateMachine(outcomes=['success', 'preempted', 'error'],
input_keys=['goal', 'feedback', 'result'],
output_keys=['feedback', 'result'])
sm.userdata.gripper_control_goal = control_msgs.msg.FollowJointTrajectoryGoal(
)
sm.userdata.motors = ['gripper']
with sm:
smach.StateMachine.add('ParseGoal',
ParseGoal(),
remapping={
'incoming_goal': 'goal',
'feedback': 'feedback',
'gripper_goal': 'gripper_control_goal'
},
transitions={'success': 'EnableMotors'})
smach.StateMachine.add('EnableMotors',
misc_tools.EnableMotors(),
transitions={'success': 'MoveGripper'},
remapping={'motors': 'motors'})
smach.StateMachine.add(
'MoveGripper',
SimpleActionState('gripper_controller',
control_msgs.msg.FollowJointTrajectoryAction,
goal_cb=trajectory_control.generalGoalCb,
result_cb=trajectory_control.generalResponseCb,
input_keys=['gripper_control_goal'],
output_keys=['error_code']),
remapping={'error_code': 'error_code'},
transitions={
'succeeded': 'ParseMoveGripperErrorCode',
'aborted': 'ParseMoveGripperErrorCode',
'preempted': 'preempted'
})
smach.StateMachine.add(
'ParseMoveGripperErrorCode',
trajectory_control.FollowJointTrajectoryErrorCodesParser(),
transitions={
'success': 'Finalise',
'parsed': 'Finalise'
},
remapping={
'error_code': 'error_code',
'error_message': 'error_message'
})
smach.StateMachine.add('Finalise',
Finalise(),
remapping={
'error_message': 'error_message',
'feedback': 'feedback',
'result': 'result'
},
transitions={
'success': 'success',
'error': 'error'
})
asw = ActionServerWrapper('move_gripper',
MoveGripperAction,
wrapped_container=sm,
goal_key='goal',
feedback_key='feedback',
result_key='result',
succeeded_outcomes=['success'],
aborted_outcomes=['error'],
preempted_outcomes=['preempted'])
asw.run_server()
rospy.spin()
def createSM():
sm_find_object = smach.StateMachine(
outcomes=['object_found', 'no_objects_found', 'preempted', 'aborted'],
input_keys=[
'table_pose', 'look_around', 'min_confidence', 'object_names',
'tf_listener'
],
output_keys=[
'recognised_objects', 'object_names', 'objects_info',
'tabletop_centre_pose', 'tabletop_front_place_pose',
'error_message', 'error_code', 'tf_listener'
])
with sm_find_object:
# move head
sm_find_object.userdata.motors = ['head_pan', 'head_tilt']
sm_find_object.userdata.tabletop_centre_pose = geometry_msgs.PoseStamped(
)
sm_find_object.userdata.tabletop_front_place_pose = geometry_msgs.PoseStamped(
)
sm_find_object.userdata.pose_bottom_left = geometry_msgs.PoseStamped()
sm_find_object.userdata.pose_bottom_left.header.stamp = rospy.Time.now(
)
sm_find_object.userdata.pose_bottom_left.header.frame_id = "base_footprint"
sm_find_object.userdata.pose_bottom_left.pose.position.x = 0.2
sm_find_object.userdata.pose_bottom_left.pose.position.y = -0.2
sm_find_object.userdata.pose_bottom_left.pose.position.z = 0.0
sm_find_object.userdata.pose_bottom_left.pose.orientation.w = 1.0
sm_find_object.userdata.pose_bottom_centre = geometry_msgs.PoseStamped(
)
sm_find_object.userdata.pose_bottom_centre.header = sm_find_object.userdata.pose_bottom_left.header
sm_find_object.userdata.pose_bottom_centre.pose.position.x = 0.2
sm_find_object.userdata.pose_bottom_centre.pose.position.y = 0.0
sm_find_object.userdata.pose_bottom_centre.pose.position.z = 0.0
sm_find_object.userdata.pose_bottom_centre.pose.orientation.w = 1.0
sm_find_object.userdata.pose_bottom_right = geometry_msgs.PoseStamped()
sm_find_object.userdata.pose_bottom_right.header = sm_find_object.userdata.pose_bottom_left.header
sm_find_object.userdata.pose_bottom_right.pose.position.x = 0.2
sm_find_object.userdata.pose_bottom_right.pose.position.y = 0.2
sm_find_object.userdata.pose_bottom_right.pose.position.z = 0.0
sm_find_object.userdata.pose_bottom_right.pose.orientation.w = 1.0
sm_find_object.userdata.pose_top_left = geometry_msgs.PoseStamped()
sm_find_object.userdata.pose_top_left.header = sm_find_object.userdata.pose_bottom_left.header
sm_find_object.userdata.pose_top_left.pose.position.x = 0.5
sm_find_object.userdata.pose_top_left.pose.position.y = -0.5
sm_find_object.userdata.pose_top_left.pose.position.z = 0.5
sm_find_object.userdata.pose_top_left.pose.orientation.w = 1.0
sm_find_object.userdata.pose_top_centre = geometry_msgs.PoseStamped()
sm_find_object.userdata.pose_top_centre.header = sm_find_object.userdata.pose_bottom_left.header
sm_find_object.userdata.pose_top_centre.pose.position.x = 0.5
sm_find_object.userdata.pose_top_centre.pose.position.y = 0.0
sm_find_object.userdata.pose_top_centre.pose.position.z = 0.5
sm_find_object.userdata.pose_top_centre.pose.orientation.w = 1.0
sm_find_object.userdata.pose_top_right = geometry_msgs.PoseStamped()
sm_find_object.userdata.pose_top_right.header = sm_find_object.userdata.pose_bottom_left.header
sm_find_object.userdata.pose_top_right.pose.position.x = 0.5
sm_find_object.userdata.pose_top_right.pose.position.y = 0.5
sm_find_object.userdata.pose_top_right.pose.position.z = 0.5
sm_find_object.userdata.pose_top_right.pose.orientation.w = 1.0
sm_find_object.userdata.pose_centre = geometry_msgs.PoseStamped()
sm_find_object.userdata.pose_centre.header = sm_find_object.userdata.pose_bottom_left.header
sm_find_object.userdata.pose_centre.pose.position.x = 0.5
sm_find_object.userdata.pose_centre.pose.position.y = 0.0
sm_find_object.userdata.pose_centre.pose.position.z = 0.0
sm_find_object.userdata.pose_centre.pose.orientation.w = 1.0
# object recognition
sm_find_object.userdata.error_message = str()
sm_find_object.userdata.error_code = int()
# wait
sm_find_object.userdata.wait_0sec = 0.0
sm_find_object.userdata.wait_1sec = 1.0
sm_find_object.userdata.wait_2sec = 2.0
sm_find_object.userdata.wait_3sec = 3.0
sm_find_object.userdata.wait_5sec = 5.0
sm_find_object.userdata.wait_10sec = 10.0
# reset TryAgain
sm_find_object.userdata.reset_true = True
sm_find_object.userdata.reset_false = False
# reset TryAgain
sm_find_object.userdata.reset_true = True
sm_find_object.userdata.reset_false = False
sm_find_object.userdata.objects_info = []
# 3D sensor modes
sm_find_object.userdata.color_mode_high_res = 5
sm_find_object.userdata.depth_mode_high_res = sm_find_object.userdata.color_mode_high_res
sm_find_object.userdata.ir_mode_high_res = sm_find_object.userdata.color_mode_high_res
sm_find_object.userdata.color_mode_low_res = 8
sm_find_object.userdata.depth_mode_low_res = sm_find_object.userdata.color_mode_low_res
sm_find_object.userdata.ir_mode_low_res = sm_find_object.userdata.color_mode_low_res
smach.StateMachine.add('EnableHighResPointCloud',
misc_tools.change3DSensorDriverMode(),
transitions={'done': 'EnableMotors'},
remapping={
'color_mode': 'color_mode_high_res',
'depth_mode': 'depth_mode_high_res',
'ir_mode': 'ir_mode_high_res'
})
smach.StateMachine.add('EnableMotors',
misc_tools.EnableMotors(),
transitions={'success': 'ResetTryAgain'},
remapping={'motors': 'motors'})
try_again_sm = TryAgain()
smach.StateMachine.add('ResetTryAgain',
try_again_sm,
remapping={
'reset': 'reset_true',
'look_around': 'look_around'
},
transitions={
'bottom_centre': 'aborted',
'bottom_right': 'aborted',
'top_right': 'aborted',
'top_centre': 'aborted',
'top_left': 'aborted',
'done': 'CheckTablePose'
})
smach.StateMachine.add('CheckTablePose',
CheckTablePose(),
transitions={
'look_around': 'MoveHeadBottomLeft',
'is_zero': 'RecognizeObjects',
'is_not_zero': 'MoveHead'
},
remapping={
'look_around': 'look_around',
'table_pose': 'table_pose'
})
smach.StateMachine.add(
'MoveHead',
SimpleActionState('head_controller',
control_msgs.FollowJointTrajectoryAction,
goal_cb=trajectory_control.headControlRequestCb,
result_cb=trajectory_control.generalResponseCb,
input_keys=['tf_listener', 'goal_pose'],
output_keys=['tf_listener', 'error_code']),
remapping={
'tf_listener': 'tf_listener',
'goal_pose': 'table_pose',
'error_code': 'error_code'
},
transitions={
'succeeded': 'ParseMoveHeadErrorCode',
'aborted': 'ParseMoveHeadErrorCode',
'preempted': 'preempted'
})
smach.StateMachine.add(
'MoveHeadBottomLeft',
SimpleActionState('head_controller',
control_msgs.FollowJointTrajectoryAction,
goal_cb=trajectory_control.headControlRequestCb,
result_cb=trajectory_control.generalResponseCb,
input_keys=['tf_listener', 'goal_pose'],
output_keys=['tf_listener', 'error_code']),
remapping={
'tf_listener': 'tf_listener',
'goal_pose': 'pose_bottom_left',
'error_code': 'error_code'
},
transitions={
'succeeded': 'ParseMoveHeadErrorCode',
'aborted': 'ParseMoveHeadErrorCode',
'preempted': 'preempted'
})
smach.StateMachine.add(
'MoveHeadBottomCentre',
SimpleActionState('head_controller',
control_msgs.FollowJointTrajectoryAction,
goal_cb=trajectory_control.headControlRequestCb,
result_cb=trajectory_control.generalResponseCb,
input_keys=['tf_listener', 'goal_pose'],
output_keys=['tf_listener', 'error_code']),
remapping={
'tf_listener': 'tf_listener',
'goal_pose': 'pose_bottom_centre',
'error_code': 'error_code'
},
transitions={
'succeeded': 'ParseMoveHeadErrorCode',
'aborted': 'ParseMoveHeadErrorCode',
'preempted': 'preempted'
})
smach.StateMachine.add(
'MoveHeadBottomRight',
SimpleActionState('head_controller',
control_msgs.FollowJointTrajectoryAction,
goal_cb=trajectory_control.headControlRequestCb,
result_cb=trajectory_control.generalResponseCb,
input_keys=['tf_listener', 'goal_pose'],
output_keys=['tf_listener', 'error_code']),
remapping={
'tf_listener': 'tf_listener',
'goal_pose': 'pose_bottom_right',
'error_code': 'error_code'
},
transitions={
'succeeded': 'ParseMoveHeadErrorCode',
'aborted': 'ParseMoveHeadErrorCode',
'preempted': 'preempted'
})
smach.StateMachine.add(
'MoveHeadTopRight',
SimpleActionState('head_controller',
control_msgs.FollowJointTrajectoryAction,
goal_cb=trajectory_control.headControlRequestCb,
result_cb=trajectory_control.generalResponseCb,
input_keys=['tf_listener', 'goal_pose'],
output_keys=['error_code']),
remapping={
'tf_listener': 'tf_listener',
'goal_pose': 'pose_top_right',
'error_code': 'error_code'
},
transitions={
'succeeded': 'ParseMoveHeadErrorCode',
'aborted': 'ParseMoveHeadErrorCode',
'preempted': 'preempted'
})
smach.StateMachine.add(
'MoveHeadTopCentre',
SimpleActionState('head_controller',
control_msgs.FollowJointTrajectoryAction,
goal_cb=trajectory_control.headControlRequestCb,
result_cb=trajectory_control.generalResponseCb,
input_keys=['tf_listener', 'goal_pose'],
output_keys=['error_code']),
remapping={
'tf_listener': 'tf_listener',
'goal_pose': 'pose_top_centre',
'error_code': 'error_code'
},
transitions={
'succeeded': 'ParseMoveHeadErrorCode',
'aborted': 'ParseMoveHeadErrorCode',
'preempted': 'preempted'
})
smach.StateMachine.add(
'MoveHeadTopLeft',
SimpleActionState('head_controller',
control_msgs.FollowJointTrajectoryAction,
goal_cb=trajectory_control.headControlRequestCb,
result_cb=trajectory_control.generalResponseCb,
input_keys=['tf_listener', 'goal_pose'],
output_keys=['error_code']),
remapping={
'tf_listener': 'tf_listener',
'goal_pose': 'pose_top_left',
'error_code': 'error_code'
},
transitions={
'succeeded': 'ParseMoveHeadErrorCode',
'aborted': 'ParseMoveHeadErrorCode',
'preempted': 'preempted'
})
smach.StateMachine.add(
'ParseMoveHeadErrorCode',
trajectory_control.FollowJointTrajectoryErrorCodesParser(),
transitions={
'success': 'WaitForPointcloudAlignment',
'parsed': 'aborted'
},
remapping={
'error_code': 'error_code',
'error_message': 'error_message'
})
smach.StateMachine.add('WaitForPointcloudAlignment',
misc_tools.Wait(),
remapping={'duration': 'wait_1sec'},
transitions={'done': 'RecognizeObjects'})
smach.StateMachine.add(
'RecognizeObjects',
SimpleActionState(
'recognize_objects',
object_recognition_msgs.ObjectRecognitionAction,
goal_cb=object_recognition.objectRecognitionGoalCb,
result_cb=object_recognition.objectRecognitionResultCb,
input_keys=[
'min_confidence', 'error_code', 'error_message',
'tf_listener'
],
output_keys=[
'recognised_objects', 'object_names', 'error_code',
'error_message', 'tf_listener'
]),
remapping={
'min_confidence': 'min_confidence',
'recognised_objects': 'recognised_objects',
'object_names': 'object_names',
'error_code': 'error_code',
'error_message': 'error_message',
'tf_listener': 'tf_listener'
},
transitions={
'succeeded': 'DisableHighResPointCloudSuccess',
'no_objects_found': 'TryAgain',
'preempted': 'preempted',
'aborted': 'aborted'
})
smach.StateMachine.add('TryAgain',
try_again_sm,
remapping={
'reset': 'reset_false',
'look_around': 'look_around'
},
transitions={
'bottom_centre': 'MoveHeadBottomCentre',
'bottom_right': 'MoveHeadBottomRight',
'top_right': 'MoveHeadTopRight',
'top_centre': 'MoveHeadTopCentre',
'top_left': 'MoveHeadTopLeft',
'done': 'DisableHighResPointCloudNoSuccess'
})
smach.StateMachine.add('DisableHighResPointCloudNoSuccess',
misc_tools.change3DSensorDriverMode(),
transitions={'done': 'no_objects_found'},
remapping={
'color_mode': 'color_mode_low_res',
'depth_mode': 'depth_mode_low_res',
'ir_mode': 'ir_mode_low_res'
})
smach.StateMachine.add('DisableHighResPointCloudSuccess',
misc_tools.change3DSensorDriverMode(),
transitions={'done': 'GetObjectMeshes'},
remapping={
'color_mode': 'color_mode_low_res',
'depth_mode': 'depth_mode_low_res',
'ir_mode': 'ir_mode_low_res'
})
smach.StateMachine.add(
'GetObjectMeshes',
object_recognition.GetObjectInformation(),
remapping={
'recognised_objects': 'recognised_objects',
'objects_info': 'objects_info',
'error_message': 'error_message',
'error_code': 'error_code'
},
transitions={'done': 'AddObjectsToPlanningScene'})
smach.StateMachine.add('AddObjectsToPlanningScene',
misc_tools.AddObjectsToPlanningScene(),
remapping={
'recognised_objects': 'recognised_objects',
'object_names': 'object_names',
'objects_info': 'objects_info',
'error_message': 'error_message',
'error_code': 'error_code'
},
transitions={'done': 'AddTableToPlanningScene'})
smach.StateMachine.add('AddTableToPlanningScene',
object_recognition.GetTable(),
remapping={
'tabletop_centre_pose':
'tabletop_centre_pose',
'tabletop_front_place_pose':
'tabletop_front_place_pose',
'tf_listener': 'tf_listener'
},
transitions={
'table_added': 'object_found',
'no_table_added': 'aborted'
})
return sm_find_object
def main():
rospy.init_node('move_arm_ik_as')
compute_ik_topic = "compute_ik"
sm = smach.StateMachine(outcomes=['success',
'preempted',
'error'],
input_keys=['goal'],
output_keys=['feedback',
'result'])
sm.userdata.robot_state = moveit_msgs.RobotState()
sm.userdata.goal_link_name = "palm_link"
sm.userdata.grasp_dist = 0.0
sm.userdata.grasp_height = 0.0
sm.userdata.result = pick_and_place_msgs.MoveArmResult()
sm.userdata.error_code = int()
sm.userdata.motors = ['torso_turn', 'torso_lift', 'shoulder', 'elbow', 'wrist']
sm.userdata.zero_true = True
sm.userdata.zero_false = False
sm.userdata.default_true = True
sm.userdata.default_false = False
sm.userdata.bottom_true = True
sm.userdata.bottom_false = False
sm.userdata.false = False
with sm:
smach.StateMachine.add('ParseGoal',
ParseGoal(),
remapping={'incoming_goal':'goal',
'feedback':'feedback',
'goal_pose':'goal_pose'},
transitions={'success':'GetRobotState'})
smach.StateMachine.add('GetRobotState',
misc_tools.GetRobotState(),
remapping={'robot_state':'robot_state'},
transitions={'success':'PrepareIKSeedcurrentState'})
smach.StateMachine.add('PrepareIKSeedcurrentState',
ik.PrepareIKSeed(),
remapping={'zero':'zero_false',
'default':'default_false',
'bottom':'bottom_false',
'robot_state':'robot_state'},
transitions={'success':'GetIKcurrentState',
'error':'error'})
smach.StateMachine.add('GetIKcurrentState',
ServiceState(compute_ik_topic,
moveit_srvs.GetPositionIK,
request_cb = ik.getPositionIKRequestCb,
response_cb = ik.getPositionIKResponseArmControllerGoalCb,
input_keys=['goal_pose',
'goal_link_name',
'robot_state',
'avoid_collisions'],
output_keys=['arm_control_goal',
'error_code']),
remapping={'goal_pose':'goal_pose',
'goal_link_name':'goal_link_name',
'robot_state':'robot_state',
'avoid_collisions':'false',
'arm_control_goal':'arm_control_goal',
'error_code':'error_code',
'error_message':'error_message'},
transitions={'succeeded':'ParseIKcurrentStateErrorCode',
'preempted':'preempted',
'aborted':'error',
'failed':'ParseIKcurrentStateErrorCode'})
smach.StateMachine.add('ParseIKcurrentStateErrorCode',
misc_tools.MoveItErrorCodesParser(),
transitions={'success':'EnableMotors',
'no_ik_solution':'error',
'planning_failed':'error',
'parsed':'error'},
remapping={'result':'result',
'error_code':'error_code'})
smach.StateMachine.add('PrepareIKSeedzeroState',
ik.PrepareIKSeed(),
remapping={'zero':'zero_true',
'default':'default_false',
'bottom':'bottom_false',
'robot_state':'robot_state'},
transitions={'success':'GetIKzeroState',
'error':'error'})
smach.StateMachine.add('GetIKzeroState',
ServiceState(compute_ik_topic,
moveit_srvs.GetPositionIK,
request_cb = ik.getPositionIKRequestCb,
response_cb = ik.getPositionIKResponseArmControllerGoalCb,
input_keys=['goal_pose',
'goal_link_name',
'robot_state',
'avoid_collisions'],
output_keys=['arm_control_goal',
'error_code']),
remapping={'goal_pose':'goal_pose',
'goal_link_name':'goal_link_name',
'grasp_dist':'grasp_dist',
'grasp_height':'grasp_height',
'robot_state':'robot_state',
'avoid_collisions':'false',
'arm_control_goal':'arm_control_goal',
'error_code':'error_code'},
transitions={'succeeded':'ParseIKzeroStateErrorCode',
'preempted':'preempted',
'aborted':'error',
'failed':'ParseIKzeroStateErrorCode'})
smach.StateMachine.add('ParseIKzeroStateErrorCode',
misc_tools.MoveItErrorCodesParser(),
transitions={'success':'EnableMotors',
'no_ik_solution':'PrepareIKSeedDefaultState',
'planning_failed':'error',
'parsed':'error'},
remapping={'result':'result',
'error_code':'error_code'})
smach.StateMachine.add('PrepareIKSeedDefaultState',
ik.PrepareIKSeed(),
remapping={'zero':'zero_false',
'default':'default_true',
'bottom':'bottom_false',
'robot_state':'robot_state'},
transitions={'success':'GetIKdefaultState',
'error':'error'})
smach.StateMachine.add('GetIKdefaultState',
ServiceState(compute_ik_topic,
moveit_srvs.GetPositionIK,
request_cb = ik.getPositionIKRequestCb,
response_cb = ik.getPositionIKResponseArmControllerGoalCb,
input_keys=['goal_pose',
'goal_link_name',
'robot_state',
'avoid_collisions'],
output_keys=['arm_control_goal',
'error_code']),
remapping={'goal_pose':'goal_pose',
'goal_link_name':'goal_link_name',
'grasp_dist':'grasp_dist',
'grasp_height':'grasp_height',
'robot_state':'robot_state',
'avoid_collisions':'false',
'arm_control_goal':'arm_control_goal',
'error_code':'error_code'},
transitions={'succeeded':'ParseIKdefaultStateErrorCode',
'preempted':'preempted',
'aborted':'error',
'failed':'ParseIKdefaultStateErrorCode'})
smach.StateMachine.add('ParseIKdefaultStateErrorCode',
misc_tools.MoveItErrorCodesParser(),
transitions={'success':'EnableMotors',
'no_ik_solution':'PrepareIKSeedBottomState',
'planning_failed':'error',
'parsed':'error'},
remapping={'result':'result',
'error_code':'error_code'})
smach.StateMachine.add('PrepareIKSeedBottomState',
ik.PrepareIKSeed(),
remapping={'zero':'zero_false',
'default':'default_false',
'bottom':'bottom_true',
'robot_state':'robot_state'},
transitions={'success':'PrepareIKSeedBottomState',
'error':'error'})
smach.StateMachine.add('GetIKBottomState',
ServiceState(compute_ik_topic,
moveit_srvs.GetPositionIK,
request_cb = ik.getPositionIKRequestCb,
response_cb = ik.getPositionIKResponseArmControllerGoalCb,
input_keys=['goal_pose',
'goal_link_name',
'robot_state',
'avoid_collisions'],
output_keys=['arm_control_goal',
'error_code']),
remapping={'goal_pose':'goal_pose',
'goal_link_name':'goal_link_name',
'grasp_dist':'grasp_dist',
'grasp_height':'grasp_height',
'robot_state':'robot_state',
'avoid_collisions':'false',
'arm_control_goal':'arm_control_goal',
'error_code':'error_code'},
transitions={'succeeded':'ParseIKBottomStateErrorCode',
'preempted':'preempted',
'aborted':'error',
'failed':'ParseIKBottomStateErrorCode'})
smach.StateMachine.add('ParseIKBottomStateErrorCode',
misc_tools.MoveItErrorCodesParser(),
transitions={'success':'EnableMotors',
'no_ik_solution':'error',
'planning_failed':'error',
'parsed':'error'},
remapping={'result':'result',
'error_code':'error_code'})
smach.StateMachine.add('EnableMotors',
misc_tools.EnableMotors(),
transitions={'success':'MoveArm'},
remapping={'motors':'motors'})
smach.StateMachine.add('MoveArm',
SimpleActionState('arm_controller',
control_msgs.FollowJointTrajectoryAction,
goal_cb=trajectory_control.generalGoalCb,
result_cb=trajectory_control.generalResponseCb,
output_keys=['error_code'],),
remapping={'trajectory_goal':'arm_control_goal',
'error_code':'error_code'},
transitions={'succeeded':'ParseMoveArmErrorCode',
'aborted':'ParseMoveArmErrorCode',
'preempted':'preempted'})
smach.StateMachine.add('ParseMoveArmErrorCode',
trajectory_control.FollowJointTrajectoryErrorCodesParser(),
transitions={'success':'success',
'parsed':'error'},
remapping={'result':'result',
'error_code':'error_code'})
asw = ActionServerWrapper('move_arm_ik',
pick_and_place_msgs.MoveArmAction,
wrapped_container = sm,
goal_key = 'goal',
feedback_key = 'feedback',
result_key = 'result',
succeeded_outcomes = ['success'],
aborted_outcomes = ['error'],
preempted_outcomes = ['preempted'])
asw.run_server()
rospy.spin()