detect_outlet.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import roslib
00004 roslib.load_manifest('pr2_plugs_actions')
00005 
00006 import rospy
00007 
00008 import os,sys,time
00009 from math import *
00010 import PyKDL
00011 
00012 from actionlib_msgs.msg import *
00013 from pr2_common_action_msgs.msg import *
00014 from pr2_plugs_msgs.msg import *
00015 from pr2_controllers_msgs.msg import *
00016 from geometry_msgs.msg import *
00017 from trajectory_msgs.msg import *
00018 from move_base_msgs.msg import *
00019 import copy
00020 
00021 from pr2_arm_move_ik.tools import *
00022 from tf_conversions.posemath import fromMsg, toMsg
00023 from joint_trajectory_action_tools.tools import get_action_goal as get_generator_goal
00024 
00025 # State machine classes
00026 import smach
00027 from smach import *
00028 from smach_ros import *
00029 from pr2_plugs_actions.tf_util import TFUtil
00030 
00031 import actionlib
00032 import dynamic_reconfigure.client
00033 
00034 __all__ = ['construct_sm']
00035 
00036 class OutletSearchState(State):
00037     def __init__(self, offsets, desired_distance):
00038         State.__init__(self,
00039                 outcomes=['succeeded','preempted','aborted'],
00040                 input_keys=['align_base_goal','vision_detect_outlet_goal'],
00041                 output_keys=['outlet_rough_pose'])
00042 
00043         # Store lateral offset goals
00044         self.offsets = offsets
00045         self.desired_distance = desired_distance
00046 
00047         # Create action clients
00048         self.align_base_client = actionlib.SimpleActionClient('align_base', AlignBaseAction)
00049         self.align_base_client.wait_for_server()
00050         self.vision_detect_outlet_client = actionlib.SimpleActionClient('vision_outlet_detection', VisionOutletDetectionAction)
00051         self.vision_detect_outlet_client.wait_for_server()
00052 
00053     def execute(self,ud):
00054         """Iterate across a set of offsets to find the outlet"""
00055         preempt_timeout = rospy.Duration(10.0)
00056 
00057         align_base_goal = ud.align_base_goal
00058         vision_detect_outlet_goal = ud.vision_detect_outlet_goal
00059 
00060         # Iterate across move_base offsets
00061         for offset in self.offsets:
00062             # align base
00063             rospy.loginfo("Search base alignment, offset %f..."%offset)
00064             align_base_goal.offset = offset
00065             align_base_goal.desired_distance = self.desired_distance
00066             if self.align_base_client.send_goal_and_wait(align_base_goal, rospy.Duration(40.0), preempt_timeout) != GoalStatus.SUCCEEDED:
00067                 rospy.logerr('Aligning base failed')
00068                 return 'aborted'
00069 
00070             # call vision outlet detection
00071             rospy.loginfo("Detecting outlet with the forearm camera...")
00072             vision_detect_outlet_goal.wall_normal = self.align_base_client.get_result().wall_norm
00073             vision_detect_outlet_goal.wall_normal.header.stamp = rospy.Time.now()
00074             vision_detect_outlet_goal.prior.header.stamp = rospy.Time.now()
00075             if self.vision_detect_outlet_client.send_goal_and_wait(vision_detect_outlet_goal, rospy.Duration(5.0), preempt_timeout) == GoalStatus.SUCCEEDED:
00076                 # Store the rough outlet position in the state machiine user data structure
00077                 ud.outlet_rough_pose = TFUtil.wait_and_transform('r_forearm_cam_optical_frame', self.vision_detect_outlet_client.get_result().outlet_pose)
00078                 # Set succeeded, and return
00079                 return 'succeeded'
00080 
00081         rospy.logerr("Could not find outlet in search.")
00082         return 'aborted'
00083 
00084 
00085 # Code block state for grasping the plug
00086 def construct_sm():
00087     TFUtil()
00088 
00089     # Check to see if this is running in sim where the dynamic reconfigure doesn't exist
00090     sim = rospy.get_param('~sim', False)
00091     rospy.logdebug('sim is %s', sim)
00092     if not sim:
00093         #this ensures that the forearm camera triggers when the texture projector is off
00094         projector_client = dynamic_reconfigure.client.Client('camera_synchronizer_node')
00095         forearm_projector_off = {'forearm_r_trig_mode': 4} #off
00096         projector_client.update_configuration(forearm_projector_off)
00097 
00098     # Define fixed goals
00099     # Declare wall norm goal
00100     # This is the point at which we want the head to look
00101     rough_align_distance = 0.88
00102     precise_align_distance = 0.74
00103     look_point = PointStamped()
00104     look_point.header.frame_id = 'base_link'
00105     look_point.point.x = -0.14
00106     look_point.point.y = -rough_align_distance # later over written in align_base
00107     look_point.point.z = 0.3
00108 
00109     wall_norm_goal = DetectWallNormGoal()
00110     wall_norm_goal.look_point = look_point
00111     wall_norm_goal.look_point.point.y = -precise_align_distance
00112 
00113     align_base_goal = AlignBaseGoal()
00114     align_base_goal.look_point = look_point
00115     align_base_goal.desired_distance = rough_align_distance
00116     align_base_goal_rough = copy.deepcopy(align_base_goal)
00117 
00118     vision_detect_outlet_goal = VisionOutletDetectionGoal()
00119     vision_detect_outlet_goal.camera_name = "/r_forearm_cam"
00120     vision_detect_outlet_goal.prior.pose = toMsg(PyKDL.Frame(PyKDL.Rotation.RPY(0, 0, -pi/2), PyKDL.Vector(-0.14, -0.82, 0.29)))
00121     vision_detect_outlet_goal.prior.header.frame_id = "base_link"
00122 
00123     # Construct state machine
00124     sm = StateMachine(
00125             outcomes=['succeeded','aborted','preempted'],
00126             output_keys=['base_to_outlet'])
00127 
00128     # Add static goals to userdata
00129     sm.userdata.wall_norm_goal = wall_norm_goal
00130     sm.userdata.align_base_goal = align_base_goal
00131     sm.userdata.vision_detect_outlet_goal = vision_detect_outlet_goal
00132 
00133     # Define nominal sequence
00134     with sm:
00135         StateMachine.add('LOWER_SPINE',
00136                 SimpleActionState('torso_controller/position_joint_action', SingleJointPositionAction,
00137                     goal = SingleJointPositionGoal(position=0.01)),
00138                 {'succeeded':'ROUGH_ALIGN_BASE'})
00139 
00140         StateMachine.add('ROUGH_ALIGN_BASE',
00141                 SimpleActionState('align_base', AlignBaseAction,
00142                     goal = align_base_goal_rough),
00143                 {'succeeded':'MOVE_ARM_DETECT_OUTLET'})
00144 
00145         StateMachine.add('MOVE_ARM_DETECT_OUTLET',
00146                 SimpleActionState('r_arm_controller/joint_trajectory_generator',JointTrajectoryAction, goal = get_generator_goal('pr2_plugs_configuration/detect_outlet')),
00147                 {'succeeded':'OUTLET_LATERAL_SEARCH',
00148                     'aborted':'FAIL_MOVE_ARM_OUTLET_TO_FREE'}),
00149 
00150         StateMachine.add('OUTLET_LATERAL_SEARCH',
00151                 OutletSearchState(offsets = (0.0, 0.15, -0.3, 0.45, -0.6), desired_distance = rough_align_distance),
00152                 {'succeeded':'PRECISE_ALIGN_BASE',
00153                     'aborted':'FAIL_MOVE_ARM_OUTLET_TO_FREE'})
00154 
00155         # Align the base precisely
00156         @smach.cb_interface(input_keys=['outlet_rough_pose'])
00157         def get_precise_align_goal(ud, goal):
00158             goal.offset = ud.outlet_rough_pose.pose.position.y
00159             return goal
00160         StateMachine.add('PRECISE_ALIGN_BASE',
00161                 SimpleActionState('align_base', AlignBaseAction,
00162                     goal = AlignBaseGoal(offset = 0, desired_distance = precise_align_distance, look_point=look_point),
00163                     goal_cb = get_precise_align_goal),
00164                 {'succeeded':'DETECT_WALL_NORM',
00165                     'aborted':'FAIL_MOVE_ARM_OUTLET_TO_FREE'})
00166 
00167         # Get wall norm
00168         @smach.cb_interface(input_keys=['wall_norm_goal'], output_keys=['wall_norm_goal'])
00169         def get_wall_norm_goal(ud, goal):
00170             ud.wall_norm_goal.look_point.header.stamp = rospy.Time.now()
00171             return ud.wall_norm_goal
00172 
00173         @smach.cb_interface(input_keys=['vision_detect_outlet_goal'], output_keys=['vision_detect_outlet_goal'])
00174         def store_wall_norm_result(ud, result_state, result):
00175             ud.vision_detect_outlet_goal.wall_normal = result.wall_norm
00176 
00177         StateMachine.add('DETECT_WALL_NORM',
00178                 SimpleActionState('detect_wall_norm', DetectWallNormAction,
00179                     goal_cb = get_wall_norm_goal,
00180                     result_cb = store_wall_norm_result),
00181                 {'succeeded':'DETECT_OUTLET',
00182                     'aborted':'DETECT_WALL_NORM'})
00183 
00184         # Precise detection
00185         @smach.cb_interface(input_keys=['vision_detect_outlet_goal'], output_keys=['vision_detect_outlet_goal'])
00186         def get_vision_detect_goal(ud, goal):
00187             ud.vision_detect_outlet_goal.wall_normal.header.stamp = rospy.Time.now()
00188             ud.vision_detect_outlet_goal.prior.header.stamp = rospy.Time.now()
00189             return ud.vision_detect_outlet_goal
00190 
00191         @smach.cb_interface(output_keys=['base_to_outlet'])
00192         def store_precise_outlet_result(ud, result_state, result):
00193             if result_state == GoalStatus.SUCCEEDED:
00194                 y = rospy.get_param('plugs_calibration_offset/y')
00195                 z = rospy.get_param('plugs_calibration_offset/z')
00196                 outlet_pose_corrected = PoseStamped()
00197                 outlet_pose_corrected.pose = toMsg(fromMsg(result.outlet_pose.pose) * PyKDL.Frame(PyKDL.Vector(0, y, z)))
00198                 outlet_pose_corrected.header = result.outlet_pose.header
00199                 rospy.loginfo("Using calibration offset y: %f and z: %f"%(y,z))
00200                 ud.base_to_outlet = TFUtil.wait_and_transform("base_link", outlet_pose_corrected).pose
00201                 print 'outlet not corrected'
00202                 print  TFUtil.wait_and_transform("base_link", result.outlet_pose).pose
00203                 print 'outlet corrected'
00204                 print  TFUtil.wait_and_transform("base_link", outlet_pose_corrected).pose
00205 
00206         StateMachine.add('DETECT_OUTLET', SimpleActionState('vision_outlet_detection', VisionOutletDetectionAction,
00207                                                             goal_cb = get_vision_detect_goal,
00208                                                             result_cb = store_precise_outlet_result),
00209                          {'succeeded':'succeeded',
00210                           'aborted':'FAIL_MOVE_ARM_OUTLET_TO_FREE'})
00211 
00212         # Define recovery states
00213         StateMachine.add('FAIL_MOVE_ARM_OUTLET_TO_FREE',
00214                 SimpleActionState('r_arm_controller/joint_trajectory_generator',JointTrajectoryAction, goal = get_generator_goal('pr2_plugs_configuration/recover_outlet_to_free')),
00215                 {'succeeded':'aborted'})
00216 
00217     return sm
00218 
00219 if __name__ == "__main__":
00220     rospy.init_node("detect_outlet")#,log_level=rospy.DEBUG)
00221     TFUtil()
00222 
00223     sm_detect_outlet = construct_sm()
00224 
00225     # Run state machine introspection server
00226     intro_server = IntrospectionServer('detect_outlet',sm_detect_outlet,'/RECHARGE/DETECT_OUTLET')
00227     intro_server.start()
00228 
00229     # Run state machine action server 
00230     asw = ActionServerWrapper(
00231             'detect_outlet', DetectOutletAction, sm_detect_outlet,
00232             succeeded_outcomes = ['succeeded'],
00233             aborted_outcomes = ['aborted'],
00234             preempted_outcomes = ['preempted'],
00235             result_slots_map = {'base_to_outlet_pose':'base_to_outlet'})
00236     asw.run_server()
00237 
00238     rospy.spin()
00239 
00240     intro_server.stop()


pr2_plugs_actions
Author(s): Jon Bohren, Patrick Mihelich, Wim Meeussen, and Melonee Wise
autogenerated on Thu Nov 28 2013 11:47:13