class TransformPoseStamped(smach.State):
    """
    Use the tf to convert from input_key 'pose_st' (PoseStamped) to 'target_frame'
    """
    def __init__(self):
        smach.State.__init__(self, outcomes    = ['success','fail'],
                                   input_keys  = ['pose_st','target_frame'],
                                   output_keys = ['transformed_pose_st'])
        self.tf_manager = TransformManager()

    def execute(self, userdata):
        tf_pose_st = self.tf_manager.transform_pose(userdata.target_frame, userdata.pose_st)
        userdata.transformed_pose_st = tf_pose_st
        return 'success'
 def __init__(self):
     smach.State.__init__(self, outcomes    = ['success','fail'],
                                input_keys  = ['pose_st','target_frame'],
                                output_keys = ['transformed_pose_st'])
     self.tf_manager = TransformManager()