def construct_sm(): TFUtil() # Check to see if this is running in sim where the dynamic reconfigure doesn't exist sim = rospy.get_param('~sim', False) rospy.logdebug('sim is %s', sim) if not sim: #this ensures that the forearm camera triggers when the texture projector is off projector_client = dynamic_reconfigure.client.Client('camera_synchronizer_node') forearm_projector_off = {'forearm_r_trig_mode': 4} #off projector_client.update_configuration(forearm_projector_off) # Define fixed goals # Declare wall norm goal # This is the point at which we want the head to look rough_align_distance = 0.88 precise_align_distance = 0.74 look_point = PointStamped() look_point.header.frame_id = 'base_link' look_point.point.x = -0.14 look_point.point.y = -rough_align_distance # later over written in align_base look_point.point.z = 0.3 wall_norm_goal = DetectWallNormGoal() wall_norm_goal.look_point = look_point wall_norm_goal.look_point.point.y = -precise_align_distance align_base_goal = AlignBaseGoal() align_base_goal.look_point = look_point align_base_goal.desired_distance = rough_align_distance align_base_goal_rough = copy.deepcopy(align_base_goal) vision_detect_outlet_goal = VisionOutletDetectionGoal() vision_detect_outlet_goal.camera_name = "/r_forearm_cam" vision_detect_outlet_goal.prior.pose = toMsg(PyKDL.Frame(PyKDL.Rotation.RPY(0, 0, -pi/2), PyKDL.Vector(-0.14, -0.82, 0.29))) vision_detect_outlet_goal.prior.header.frame_id = "base_link" # Construct state machine sm = StateMachine( outcomes=['succeeded','aborted','preempted'], output_keys=['base_to_outlet']) # Add static goals to userdata sm.userdata.wall_norm_goal = wall_norm_goal sm.userdata.align_base_goal = align_base_goal sm.userdata.vision_detect_outlet_goal = vision_detect_outlet_goal # Define nominal sequence with sm: StateMachine.add('LOWER_SPINE', SimpleActionState('torso_controller/position_joint_action', SingleJointPositionAction, goal = SingleJointPositionGoal(position=0.01)), {'succeeded':'ROUGH_ALIGN_BASE'}) StateMachine.add('ROUGH_ALIGN_BASE', SimpleActionState('align_base', AlignBaseAction, goal = align_base_goal_rough), {'succeeded':'MOVE_ARM_DETECT_OUTLET'}) StateMachine.add('MOVE_ARM_DETECT_OUTLET', SimpleActionState('r_arm_controller/joint_trajectory_generator',JointTrajectoryAction, goal = get_generator_goal('pr2_plugs_configuration/detect_outlet')), {'succeeded':'OUTLET_LATERAL_SEARCH', 'aborted':'FAIL_MOVE_ARM_OUTLET_TO_FREE'}), StateMachine.add('OUTLET_LATERAL_SEARCH', OutletSearchState(offsets = (0.0, 0.15, -0.3, 0.45, -0.6), desired_distance = rough_align_distance), {'succeeded':'PRECISE_ALIGN_BASE', 'aborted':'FAIL_MOVE_ARM_OUTLET_TO_FREE'}) # Align the base precisely @smach.cb_interface(input_keys=['outlet_rough_pose']) def get_precise_align_goal(ud, goal): goal.offset = ud.outlet_rough_pose.pose.position.y return goal StateMachine.add('PRECISE_ALIGN_BASE', SimpleActionState('align_base', AlignBaseAction, goal = AlignBaseGoal(offset = 0, desired_distance = precise_align_distance, look_point=look_point), goal_cb = get_precise_align_goal), {'succeeded':'DETECT_WALL_NORM', 'aborted':'FAIL_MOVE_ARM_OUTLET_TO_FREE'}) # Get wall norm @smach.cb_interface(input_keys=['wall_norm_goal'], output_keys=['wall_norm_goal']) def get_wall_norm_goal(ud, goal): ud.wall_norm_goal.look_point.header.stamp = rospy.Time.now() return ud.wall_norm_goal @smach.cb_interface(input_keys=['vision_detect_outlet_goal'], output_keys=['vision_detect_outlet_goal']) def store_wall_norm_result(ud, result_state, result): ud.vision_detect_outlet_goal.wall_normal = result.wall_norm StateMachine.add('DETECT_WALL_NORM', SimpleActionState('detect_wall_norm', DetectWallNormAction, goal_cb = get_wall_norm_goal, result_cb = store_wall_norm_result), {'succeeded':'DETECT_OUTLET', 'aborted':'DETECT_WALL_NORM'}) # Precise detection @smach.cb_interface(input_keys=['vision_detect_outlet_goal'], output_keys=['vision_detect_outlet_goal']) def get_vision_detect_goal(ud, goal): ud.vision_detect_outlet_goal.wall_normal.header.stamp = rospy.Time.now() ud.vision_detect_outlet_goal.prior.header.stamp = rospy.Time.now() return ud.vision_detect_outlet_goal @smach.cb_interface(output_keys=['base_to_outlet']) def store_precise_outlet_result(ud, result_state, result): if result_state == GoalStatus.SUCCEEDED: y = rospy.get_param('plugs_calibration_offset/y') z = rospy.get_param('plugs_calibration_offset/z') outlet_pose_corrected = PoseStamped() outlet_pose_corrected.pose = toMsg(fromMsg(result.outlet_pose.pose) * PyKDL.Frame(PyKDL.Vector(0, y, z))) outlet_pose_corrected.header = result.outlet_pose.header rospy.loginfo("Using calibration offset y: %f and z: %f"%(y,z)) ud.base_to_outlet = TFUtil.wait_and_transform("base_link", outlet_pose_corrected).pose print 'outlet not corrected' print TFUtil.wait_and_transform("base_link", result.outlet_pose).pose print 'outlet corrected' print TFUtil.wait_and_transform("base_link", outlet_pose_corrected).pose StateMachine.add('DETECT_OUTLET', SimpleActionState('vision_outlet_detection', VisionOutletDetectionAction, goal_cb = get_vision_detect_goal, result_cb = store_precise_outlet_result), {'succeeded':'succeeded', 'aborted':'FAIL_MOVE_ARM_OUTLET_TO_FREE'}) # Define recovery states StateMachine.add('FAIL_MOVE_ARM_OUTLET_TO_FREE', SimpleActionState('r_arm_controller/joint_trajectory_generator',JointTrajectoryAction, goal = get_generator_goal('pr2_plugs_configuration/recover_outlet_to_free')), {'succeeded':'aborted'}) return sm
def construct_sm(): TFUtil() # Define fixed goals # Open gripper goal open_gripper_goal = Pr2GripperCommandGoal() open_gripper_goal.command.position = 0.07 open_gripper_goal.command.max_effort = 99999 # Close gripper goal close_gripper_goal = Pr2GripperCommandGoal() close_gripper_goal.command.position = 0.0 close_gripper_goal.command.max_effort = 99999 # Construct state machine sm = StateMachine( ['succeeded','aborted','preempted'], output_keys = ['plug_on_base_pose', 'gripper_plug_grasp_pose']) # Hardcoded poses for approach / grasping sm.userdata.gripper_plug_grasp_pose_approach = toMsg(PyKDL.Frame(PyKDL.Vector(0, 0.05, 0)).Inverse()) sm.userdata.gripper_plug_grasp_pose = toMsg(PyKDL.Frame(PyKDL.Rotation.RPY(pi/2, 0.0, -pi/9), PyKDL.Vector(-0.03, 0.0, 0.005)).Inverse()) # Define nominal sequence with sm: StateMachine.add('RAISE_SPINE', SimpleActionState('torso_controller/position_joint_action', SingleJointPositionAction, goal = SingleJointPositionGoal(position=0.16)), {'succeeded':'MOVE_ARM_BASE_DETECT_POSE'}) # Move arm to detect the plug on the base StateMachine.add('MOVE_ARM_BASE_DETECT_POSE', SimpleActionState('r_arm_controller/joint_trajectory_generator', JointTrajectoryAction, goal = get_generator_goal('pr2_plugs_configuration/detect_plug_on_base')), {'succeeded':'DETECT_PLUG_ON_BASE'}) # Detect the plug @smach.cb_interface(output_keys=['plug_on_base_pose']) def store_detect_plug_result(ud, result_state, result): if result_state == actionlib.GoalStatus.SUCCEEDED: ud.plug_on_base_pose = TFUtil.wait_and_transform('base_link', result.plug_pose).pose StateMachine.add('DETECT_PLUG_ON_BASE', SimpleActionState('detect_plug_on_base',DetectPlugOnBaseAction, goal = DetectPlugOnBaseGoal(), result_cb = store_detect_plug_result), {'succeeded':'MOVE_ARM_BASE_GRASP_POSE', 'aborted':'MOVE_ARM_BASE_DETECT_POSE'}) # Move arm to the grasp pose StateMachine.add('MOVE_ARM_BASE_GRASP_POSE', SimpleActionState('r_arm_controller/joint_trajectory_generator',JointTrajectoryAction, goal = get_generator_goal('pr2_plugs_configuration/grasp_plug')), {'succeeded':'OPEN_GRIPPER', 'aborted':'RECOVER_GRASP_TO_DETECT_POSE'}) StateMachine.add('OPEN_GRIPPER', SimpleActionState('r_gripper_controller/gripper_action', Pr2GripperCommandAction, goal = open_gripper_goal), {'succeeded':'APPROACH_PLUG'}) @smach.cb_interface(input_keys = ['plug_on_base_pose', 'gripper_plug_grasp_pose_approach', 'gripper_plug_grasp_pose']) def get_approach_plug_goal(ud, goal): """Get the ik goal for approaching the plug to grasp it """ pose_gripper_wrist = fromMsg(TFUtil.wait_and_lookup('r_gripper_tool_frame', 'r_wrist_roll_link').pose) goal = ArmMoveIKGoal() goal.pose.pose = toMsg(fromMsg(ud.plug_on_base_pose) * fromMsg(ud.gripper_plug_grasp_pose_approach).Inverse() * fromMsg(ud.gripper_plug_grasp_pose).Inverse() * pose_gripper_wrist) goal.pose.header.stamp = rospy.Time.now() goal.pose.header.frame_id = 'base_link' goal.ik_seed = get_action_seed('pr2_plugs_configuration/grasp_plug_seed') goal.move_duration = rospy.Duration(3.0) return goal StateMachine.add('APPROACH_PLUG', SimpleActionState('r_arm_ik', ArmMoveIKAction, goal_cb = get_approach_plug_goal), {'succeeded':'GRASP_PLUG', 'aborted':'DETECT_PLUG_ON_BASE'}) @smach.cb_interface(input_keys = ['plug_on_base_pose', 'gripper_plug_grasp_pose']) def get_grasp_plug_goal(ud, goal): """Get the ik goal for grasping the plug.""" pose_gripper_wrist = fromMsg(TFUtil.wait_and_lookup('r_gripper_tool_frame', 'r_wrist_roll_link').pose) goal = ArmMoveIKGoal() goal.pose.pose = toMsg(fromMsg(ud.plug_on_base_pose) * fromMsg(ud.gripper_plug_grasp_pose).Inverse() * pose_gripper_wrist) goal.pose.header.stamp = rospy.Time.now() goal.pose.header.frame_id = 'base_link' goal.ik_seed = get_action_seed('pr2_plugs_configuration/grasp_plug_seed') goal.move_duration = rospy.Duration(3.0) return goal StateMachine.add('GRASP_PLUG', SimpleActionState('r_arm_ik', ArmMoveIKAction, goal_cb = get_grasp_plug_goal), {'succeeded':'CLOSE_GRIPPER', 'aborted':'DETECT_PLUG_ON_BASE'}) StateMachine.add('CLOSE_GRIPPER', SimpleActionState('r_gripper_controller/gripper_action', Pr2GripperCommandAction, goal = close_gripper_goal), { 'succeeded':'DETECT_PLUG_ON_BASE', 'aborted':'REMOVE_PLUG'}) StateMachine.add('REMOVE_PLUG', SimpleActionState('r_arm_controller/joint_trajectory_generator',JointTrajectoryAction, goal = get_generator_goal('pr2_plugs_configuration/remove_plug')), {'succeeded':'LOWER_SPINE'}) StateMachine.add('LOWER_SPINE', SimpleActionState('torso_controller/position_joint_action', SingleJointPositionAction, goal = SingleJointPositionGoal(position=0.01)), {'succeeded':'succeeded'}) # Define recovery states StateMachine.add('RECOVER_GRASP_TO_DETECT_POSE', SimpleActionState('r_arm_controller/joint_trajectory_generator',JointTrajectoryAction, goal = get_generator_goal('pr2_plugs_configuration/recover_grasp_to_detect')), { 'succeeded':'DETECT_PLUG_ON_BASE', 'aborted':'RECOVER_GRASP_TO_DETECT_POSE'}) return sm