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
