pick_up_cloth.py

Go to the documentation of this file.

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import roslib; roslib.load_manifest('estirabot_apps')
import rospy
import smach
import smach_ros

from pprint import pprint

from smach_ros import SimpleActionState
from smach_ros import ServiceState
from smach import CBState

from geometry_msgs.msg import PoseStamped

from iri_common_smach.st_get_pcl import GetPCL
from iri_common_smach.utils_msg  import build_pose
from iri_common_smach.utils_msg  import build_transform_msg
#from iri_common_smach.transform_manager import TransformManager
#from iri_common_smach.st_transform_pose   import TransformPoseStamped
from iri_common_smach.homogeneous_product import homogeneous_product_pose_transform
from iri_wam_smach.sm_wam_move_from_pose  import SM_WAM_MoveFromPose
from estirabot_apps_base.sm_move             import SM_ESTIRABOT_GoHome

from estirabot_msgs.srv import TransformPose

from iri_common_drivers_msgs.msg import tool_closeAction, tool_openAction

from iri_common_smach.utils_msg import build_pose_stamped_msg

from iri_bow_object_detector.msg import *
from iri_wam_common_msgs.msg import *

from actionlib import *
from actionlib.msg import *

import ipdb

@smach.cb_interface(input_keys=['grasp_point'],
                                        output_keys=['grasp_pose_st', 'target_frame'],
                                        outcomes=['done'])
def build_grasp_pose_from_point_cb(ud):
        p                 = PoseStamped()
        p.pose            = build_pose(ud.grasp_point.x, ud.grasp_point.y, ud.grasp_point.z, 0, 1, 0, 0)
        p.header.frame_id = '/camera_rgb_optical_frame'
        p.header.stamp = rospy.Time.now()
        pub = rospy.Publisher('/debug/grasp_point', PoseStamped, None, False, True)
        rospy.sleep(1) # wait for subscribers
        pub.publish(p)
        ud.grasp_pose_st = p
        
        # transform pose
        #tf_manager = TransformManager()
        #p_transformed = tf_manager.transform_pose('wam_link0', p)
        #ud.grasp_pose_st = p_transformed
        
        ud.target_frame = '/estirabot_link_base'
        
        return 'done'

def grasping_point_goal_cb(userdata, goal):
        goal = GetGraspingPointGoal()
        goal.pointcloud = userdata.pcl_RGB
        return goal


def build_pick_up_goal_from_grasping_pose(pose_st):
        goal = SimpleBhandPickUpGoal()
        
        #goal.fingers_position_for_pre_grasp=["GM 5000"]
        #goal.fingers_position_for_grasp=["GTC"]
        goal.fingers_position_for_pre_grasp=["open"]
        goal.fingers_position_for_grasp=["close"]
        
        # Grasp pose is pose_st
        goal.grasp_pose = pose_st
        
        # Pre-grasp pose -0.15m above grasp
        goal.pre_grasp_modifier.position.x = 0
        goal.pre_grasp_modifier.position.y = 0
        goal.pre_grasp_modifier.position.z = -0.15
        goal.pre_grasp_modifier.orientation.x = 0
        goal.pre_grasp_modifier.orientation.y = 0
        goal.pre_grasp_modifier.orientation.z = 0
        goal.pre_grasp_modifier.orientation.w = 1
        
        # Lift modifier -0.20 on Z
        goal.lift.direction.header.frame_id = "/estirabot_link_base"
        goal.lift.direction.header.stamp = pose_st.header.stamp
        goal.lift.direction.vector.x = 0
        goal.lift.direction.vector.y = 0.441176471
        goal.lift.direction.vector.z = 0.735294118
        goal.lift.desired_distance = 0.60
        
        return goal


def move_pose_away(pose, z_offset):
        transform = build_transform_msg(0, 0, z_offset, 0, 0, 0, 1)
        return homogeneous_product_pose_transform(pose, transform)


def grasp_cloth_goal_cb(userdata, goal):
        #ipdb.set_trace()
        pose_st = userdata.sm_transformed_pose_st
        
        # Set orientation pointing to the table
        pose_st.pose.orientation.x = 0
        pose_st.pose.orientation.y = 1
        pose_st.pose.orientation.z = 0
        pose_st.pose.orientation.w = 0
        pose_st.pose = move_pose_away(pose_st.pose, 0.02)
        
        pub = rospy.Publisher('/debug/grasp_point2', PoseStamped, None, False, True)
        rospy.sleep(1) # wait for subscribers
        pub.publish(pose_st)
        
        goal = build_pick_up_goal_from_grasping_pose(pose_st)
        return goal

def lwpr_goal_cb(userdata, goal):
        #ipdb.set_trace()
        goal = LWPRTrajectoryReturningForceEstimationGoal()
        goal.model_filename = "/home/robot/code/iri-libbarrett/build/apps/iros13_2-LWPR"
        goal.points_filename = "/home/robot/code/iri-libbarrett/build/apps/tmp/iros13_2.txt"
        return goal

class TransformPoseStamped(smach.State):
        def __init__(self):
                smach.State.__init__(self, outcomes=['succeeded','preempted','aborted'],
                                     input_keys=['sm_grasp_pose','sm_target_frame'],
                                     output_keys=['target_pose'])
                self.service_topic = '/estirabot/skills/bow_detector/iri_transform_pose/transform_pose'
        
        def execute(self, userdata):
                rospy.logdebug('Executing state TransformPoseStamped')
                rospy.wait_for_service(self.service_topic)
                try:
                        get_target_pose = rospy.ServiceProxy(self.service_topic, TransformPose)
                        resp = get_target_pose(userdata.sm_grasp_pose, userdata.sm_target_frame)
                        
                        userdata.target_pose = resp.target_pose_st
                        return 'succeeded'
                
                except rospy.ServiceException, e:
                        print "Service call failed: %s"%e
                        return 'aborted'


def main():
        rospy.init_node('sm_pick_up_cloths')
        
        sm_move_factory = SM_WAM_MoveFromPose('/estirabot/wam_wrapper/joints_move',
                                              '/estirabot/wam_ik/pose_move')
        sm_move         = sm_move_factory.build_sm()
        factory    = SM_ESTIRABOT_GoHome()
        sm_goHome  = factory.build_sm()
        
        # Create a SMACH state machine
        sm = smach.StateMachine(outcomes=['finishOk','failedVision','failedMoveArm'])

        # Open the container
        with sm:
                # Add states to the container
                smach.StateMachine.add('GetPCL', GetPCL('/camera/depth_registered/points'),
                #smach.StateMachine.add('GetPCL', GetPCL('/estirabot/ceiling_kinect/camera/rgb/points'),
                                transitions={'success':'GetGraspingPointAction', 
                                                'fail':'failedVision'},
                                remapping={'pcl_RGB':'pcl_RGB'})
                
                smach.StateMachine.add('GetGraspingPointAction', 
                                SimpleActionState('/estirabot/skills/bow_detector/bow_object_detector/get_grasping_point',
                                                  GetGraspingPointAction,
                                                  #goal_slots=['pcl_RGB'],
                                                  goal_cb=grasping_point_goal_cb,
                                                  input_keys=['pcl_RGB'],
                                                  result_slots=['grasping_point']),
                                                  #server_wait_timeout=rospy.Duration(10.0)),
                                transitions={'succeeded':'ConvertGraspPoint',
                                                'preempted':'failedVision',
                                                'aborted':'failedVision'},
                                remapping={'grasping_point':'sm_grasp_point'})
                
                smach.StateMachine.add('ConvertGraspPoint', 
                                                          CBState(build_grasp_pose_from_point_cb),
                                transitions={'done':'TransformPoseStamped'},
                                remapping={'grasp_point' : 'sm_grasp_point',
                           'grasp_pose_st' : 'sm_grasp_pose',
                           'target_frame' : 'sm_target_frame'})

                
                #smach.StateMachine.add('TransformPoseStamped', TransformPoseStamped(),
                                #transitions={'success':'SimpleBhandPickUpAction', 
                                                #'fail':'failedVision'},
                                #remapping={'pose_st':'sm_grasp_pose',
                                                #'target_frame' : 'sm_target_frame',
                                                #'transformed_pose_st' : 'sm_transformed_pose_st'})
                
                
                smach.StateMachine.add('TransformPoseStamped', 
                                        TransformPoseStamped(),
                                transitions={'succeeded':'SimpleBhandPickUpAction', 
                                                'preempted':'failedVision',
                                                'aborted':'failedVision'},
                                remapping={'target_pose':'sm_transformed_pose_st'})
                
                smach.StateMachine.add('SimpleBhandPickUpAction', 
                                SimpleActionState('/estirabot/skills/grasp/pickup',
                                                  SimpleBhandPickUpAction,
                                                  #goal_slots=['pcl_RGB'],
                                                  goal_cb=grasp_cloth_goal_cb,
                                                  input_keys=['sm_transformed_pose_st']),
                                                  #server_wait_timeout=rospy.Duration(10.0)),
                                #transitions={'succeeded':'LWPR_action',
                                transitions={'succeeded':'OpenGripper',
                                                'preempted':'failedMoveArm',
                                                'aborted':'failedMoveArm'})
                
                # move to home
                smach.StateMachine.add('GoHome', 
                                        sm_goHome,
                                transitions={'success':'LWPR_action',
                                        'aborted':'failedMoveArm'})
                
                #smach.StateMachine.add('MoveHome', sm_move,
                     #transitions = {'success' : 'OpenGripper',
                                    #'no_kinematic_solution' : 'failedMoveArm',
                                    #'aborted': 'failedMoveArm'},
                     #remapping   = {'pose_st': 'sm_grasp_pose'})
                
                # lwpr
                smach.StateMachine.add('LWPR_action', 
                                SimpleActionState('/estirabot/wam_wrapper/lwpr_trajectory',
                                                  LWPRTrajectoryReturningForceEstimationAction,
                                                  #goal_slots=['pcl_RGB'],
                                                  goal_cb=lwpr_goal_cb),
                                                  #input_keys=['sm_transformed_pose_st']),
                                                  #server_wait_timeout=rospy.Duration(10.0)),
                                transitions={'succeeded':'finishOk',
                                                'preempted':'failedMoveArm',
                                                'aborted':'failedMoveArm'})
                
                # release
                smach.StateMachine.add('OpenGripper',
                       smach_ros.SimpleActionState('/estirabot/gripper/tool_open_action',
                                                  tool_openAction),
                       {'succeeded': 'CloseGripper',
                        'preempted': 'failedMoveArm',
                        'aborted'  : 'failedMoveArm'})
                        
                #move home
                smach.StateMachine.add('GoHome2', 
                                        sm_goHome,
                                        transitions={'success':'finishOk',
                                        'aborted':'failedMoveArm'})
                
                
                smach.StateMachine.add('CloseGripper',
                       smach_ros.SimpleActionState('/estirabot/gripper/tool_close_action',
                                                  tool_closeAction),
                       {'succeeded': 'finishOk',
                        'preempted': 'failedMoveArm',
                        'aborted'  : 'failedMoveArm'})
                
                # initial
                

        # Execute SMACH plan
        outcome = sm.execute()



if __name__ == '__main__':
        main()