replicable_create_model.py

Go to the documentation of this file.

#!/usr/bin/env python

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

from estirabot_common import *

from smach import StateMachine, CBState, Iterator
from smach_ros import ServiceState
from actionlib import *
from actionlib.msg import *
from std_msgs.msg import String
from object_manipulation_msgs.msg import PickupAction,PickupGoal,PlaceAction,PlaceGoal
from sensor_msgs.msg import PointCloud2, JointState
from iri_deformable_analysis.srv import DoDeformableAnalysis, DoDeformableAnalysisRequest
from iri_dynamixel_gripper.msg import closeAction, openAction
from estirabot_msgs.msg import PomdpGraspConfig
from geometry_msgs.msg import Transform, PoseStamped, Pose, Quaternion
from pprint import pprint
from std_msgs.msg import Int8

class PerformAnalysis(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['success','fail','timeout'],
                                   input_keys=['pcl_RGB','descriptors','descriptors_index'],
                                   output_keys=['best_grasp_pose'])

    def execute(self, userdata):
        # Log the PCL
        pub = rospy.Publisher('/debug/pcl_table_scene', PointCloud2,  None, False, True)
        pub.publish(userdata.pcl_RGB)

        # Set up the desired descriptor (via param server)
        print "Descriptor:"
        pprint(userdata.descriptors[userdata.descriptors_index])
        client = dynamic_reconfigure.client.Client("/estirabot/skills/deformable_analysis/iri_feature_map")
        config = client.update_configuration({ 'selected_centroid' : userdata.descriptors[userdata.descriptors_index]})
        #rospy.set_param('/estirabot/skills/deformable_analysis/iri_feature_map/selected_centroid', userdata.descriptors[userdata.descriptors_index])

        pub = rospy.Publisher('/log/descriptor_used', Int8, None, False, True)
        pub.publish(userdata.descriptors[userdata.descriptors_index])


        # Config and launch the deformable analysis
        handler = rospy.ServiceProxy('/estirabot/skills/deformable_analysis/do_deformable_analysis',
                                     DoDeformableAnalysis)

        analysis_request = DoDeformableAnalysisRequest()
        analysis_request.pcl_to_analyze = userdata.pcl_RGB
        # TODO: need to fix a fusion criteria (always use 1 = FEATURE MAP)
        analysis_request.fusion_criteria_id = 1

        try:
            response = handler(analysis_request)

            if (not response):
                return 'fail'

        except rospy.ServiceException, e:
            return 'timeout'

        pose_st = response.deformable_analysis.graspability.best_grasp_pose
        userdata.best_grasp_pose = pose_st
        print("end analysis")

        return 'success'

class CalculateGrasp(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['success','fail','all_ik_fail'],
                                   input_keys=['best_grasp_pose','gripper_options','gripper_index'],
                                   output_keys=['grasp_goal'])
        self.tf_manager = TransformManager()

    def calculate_gripper_orientation(self, userdata):
        orientation = Quaternion()

        print "Gripper orientation:"
        pprint(userdata.gripper_options[userdata.gripper_index])
        pub = rospy.Publisher('/log/gripper_orientation_used', Int8, None, False, True)
        pub.publish(userdata.gripper_options[userdata.gripper_index])

        if (userdata.gripper_options[userdata.gripper_index] == 1):
            orientation.x = 1.0
            orientation.y = 0
            orientation.z = 0
            orientation.w = 0
        elif (userdata.gripper_options[userdata.gripper_index] == 0):
            orientation.x = 0.7071
            orientation.y = 0.7071
            orientation.z = 0
            orientation.w = 0
        else:
            raise ValueError("Gripper option not valid %i", userdata.gripper_options[gripper_index])

        return orientation


    def execute(self, userdata):
        # shorcuts
        grasp_pose_st = userdata.best_grasp_pose

        # grasp_pose_st comes referecence to camera frame_id
        # we need to transform the message to wam base frame wam_link0 
        grasp_pose_st_wam = self.tf_manager.transform_pose('wam_link0', grasp_pose_st)

        #security check
        if (grasp_pose_st_wam.pose.position.z < -0.282):
            grasp_pose_st_wam.pose.position.z = -0.282

        # Orientation is handling depending of the config
        try:
            grasp_pose_st_wam.pose.orientation = self.calculate_gripper_orientation(userdata)
        except ValueError as e:
            rospy.logerr('%s', e.value)
            return 'fail'

        #pprint("Grasp original")
        #pprint(grasp_pose_st_wam)
        pub = rospy.Publisher('/debug/grasp/original', PoseStamped, None, False, True)
        pub.publish(grasp_pose_st_wam)

        # For grasp we need the inverse kinematics and get joinst positions
        try:
            joints_grasp = get_inverse_kinematics_from_pose(grasp_pose_st_wam.pose)

        #except NoKinematicSolutionException, e:
        except rospy.ServiceException, e:
            pub = rospy.Publisher('/log/number_of_clothes', Int8, None, False, True)
            pub.publish(99)
            print "Inverse Kinematic failed. Please change the scene and press a key"
            raw_input()
            return 'all_ik_fail'

        userdata.grasp_goal = grasp_pose_st_wam.pose
        return 'success'

def get_sm_place_goal_callback(userdata, default_goal):
    pose_stamped                 = PoseStamped()
    pose_stamped.header.frame_id = 'wam_link0'
    pose_stamped.header.stamp    = rospy.Time.now()
    pose_stamped.pose            = userdata.sm_pomdp_config.place_point

    goal = PlaceGoal()
    goal.place_locations.append(pose_stamped)

    return goal

class LiftAction(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['success','aborted'])

    def execute(self, userdata):
        return 'success'

class InputResult(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['ok'])

    def execute(self, userdata):
        keyserver = GetKeyStroke()
        c = -1
        print "Press [0-5] for clothes grabbed:"
        while ((c < 0) or (c > 5)):
            c = keyserver()
            try:
                c = int(c)
            except ValueError:
                continue

        print "Your value was: " + str(c)
        pub = rospy.Publisher('/log/number_of_clothes', Int8, None, False, True)
        pub.publish(c)

        return 'ok'

# Iterator over rounds given a fixed config
def construct_round_iterator_sm():

    # 1. START_ROUND state machine
    sm_start_round = smach.StateMachine(outcomes=['success','aborted'])


    with sm_start_round:
        smach.StateMachine.add('MOVE_TO_HOME_POSITION', EstirabotGoHome(),
                          transitions={'success': 'WAIT_TO_STABLE_SCENARIO',
                                       'fail'   : 'aborted'})

        # Need to wait some seconds to be sure clothes are not in the air after
        # a lift action. Otherwise PCL can see them as part os scenario.
        smach.StateMachine.add('WAIT_TO_STABLE_SCENARIO', WaitSeconds(2),
                          transitions={'finish': 'success'})


    # 2. CALCULATE_GRASP_POINT state machine
    sm_calculate_grasp_point = smach.StateMachine(outcomes = ['success','aborted','all_ik_fail' ],
                         input_keys  = ['descriptors', 'descriptors_index', 'gripper_options', 'gripper_index'],
                         output_keys = ['sm_grasp_goal'])

    with sm_calculate_grasp_point:
        # Get kinect image and process and make analisys
        smach.StateMachine.add('GET_KINECT_IMAGE', EstirabotGetPCL("/estirabot/skills/pcl_distance_filtered"),
                transitions = {'success' : 'PERFORM_ANALISYS',
                               'fail'    : 'aborted'},
                remapping   = {'pcl_RGB'         : 'sm_pcl_output'})
        smach.StateMachine.add('PERFORM_ANALISYS', PerformAnalysis(),
                transitions = {'success'        : 'CALCULATE_GRASP',
                               'fail'           : 'aborted',
                               'timeout'        : 'aborted'},
                remapping   = {'pcl_RGB'         : 'sm_pcl_output',
                               'best_grasp_pose' : 'sm_best_grasp_pose'})
        smach.StateMachine.add('CALCULATE_GRASP', CalculateGrasp(),
                transitions = {'success'        : 'success',
                               'fail'           : 'aborted',
                               'all_ik_fail'    : 'all_ik_fail'},
                remapping   = {'pcl_RGB'         : 'sm_pcl_output',
                               'best_grasp_pose' : 'sm_best_grasp_pose',
                               'grasp_goal'      : 'sm_grasp_goal'})

    # 3. PICK & LIFT
    sm_pick_and_lift = smach.StateMachine(outcomes=['success','aborted'],
                                          input_keys=['sm_grasp_goal'])

    with sm_pick_and_lift:
        smach.StateMachine.add('DO_MOVE_TO_GRASP', EstirabotCartesianMove(),
                 transitions = {'success'        : 'DO_CLOSE_GRIPPER',
                                'fail'           : 'aborted'},
                 remapping   = {'goal'           : 'sm_grasp_goal'})

        smach.StateMachine.add('DO_CLOSE_GRIPPER',
                           smach_ros.SimpleActionState('/estirabot/gripper/close_action_',
                               closeAction),
                           {'succeeded' : 'DO_LIFT',
                            'preempted' : 'aborted',
                            'aborted'   : 'WAIT_TO_GRIPPER_FOR_CLOSE'})

        smach.StateMachine.add('WAIT_TO_GRIPPER_FOR_CLOSE', WaitSeconds(20),
                          transitions={'finish': 'DO_CLOSE_GRIPPER'})

        smach.StateMachine.add('DO_LIFT', EstirabotGoHome(),
                        transitions={'success' : 'success',
                                     'fail'    : 'aborted'})
    # 4. CHECK
    sm_check_result = smach.StateMachine(outcomes=['success'])

    with sm_check_result:
        smach.StateMachine.add('INPUT_RESULT', InputResult(),
                transitions = { 'ok' : 'success'})

    # 5. DROP
    sm_drop = smach.StateMachine(outcomes=['success','aborted'])

    with sm_drop:
        # TODO: move to place position need new state <------ 

        smach.StateMachine.add('DO_PLACE',
                       smach_ros.SimpleActionState('/estirabot/gripper/open_action_',
                                                  openAction),
                       {'succeeded': 'success',
                        'preempted': 'aborted',
                        'aborted'  : 'WAIT_TO_GRIPPER_FOR_OPEN'})

        smach.StateMachine.add('WAIT_TO_GRIPPER_FOR_OPEN', WaitSeconds(8),
                transitions={'finish': 'DO_PLACE'})

    # MAIN STATE MACHINE
    sm = StateMachine(outcomes = ['continue','aborted'],
                      input_keys  = ['descriptors','descriptors_index','gripper_options','gripper_index'],
                      output_keys = ['descriptors','descriptors_index','gripper_options','gripper_index'])
    sm.userdata.rounds = 10

    with sm:

        round_iterator = Iterator(outcomes = ['succeeded','aborted'],
                            input_keys  = ['descriptors','descriptors_index','gripper_options','gripper_index'],
                            it = range(0, sm.userdata.rounds),
                            output_keys = ['descriptors','descriptors_index','gripper_options','gripper_index'],
                            it_label = 'round_index',
                            exhausted_outcome = 'succeeded')

        with round_iterator:
            container_sm = StateMachine(outcomes = ['succeeded','aborted','continue'],
                input_keys = ['round_index','descriptors','descriptors_index','gripper_options','gripper_index'],
                output_keys = ['round_index','descriptors','descriptors_index','gripper_options','gripper_index'])

            with container_sm:
                # 1 START ROUND
                smach.StateMachine.add('START_ROUND', sm_start_round,
                        transitions={'success': 'CALCULATE_GRASP_POINT',
                                     'aborted': 'aborted'})

                # 2 CALCULATE GRASP POINT
                smach.StateMachine.add('CALCULATE_GRASP_POINT', sm_calculate_grasp_point,
                        transitions={'success'     : 'PICK_AND_LIFT',
                                     'aborted'     : 'aborted',
                                     'all_ik_fail' : 'continue'})

                # 3 PICK AND LIFT
                smach.StateMachine.add('PICK_AND_LIFT', sm_pick_and_lift,
                        transitions={'success': 'CHECK_RESULT',
                                     'aborted': 'aborted' })
                # 4. CHECK
                smach.StateMachine.add('CHECK_RESULT', sm_check_result,
                        transitions = {'success': 'DROP'})

                # 5. DROP
                smach.StateMachine.add('DROP', sm_drop,
                        transitions = {'success': 'continue',
                                       'aborted': 'aborted'})

            #close container_sm
            Iterator.set_contained_state('CONTAINER_STATE', 
                                         container_sm, 
                                         loop_outcomes=['continue'])
        #close the round_iterator
        StateMachine.add('ROUND_ITERATOR',round_iterator,
                     {'succeeded':'continue',
                      'aborted':'aborted'})
    return sm

# Iterator to choose between gripper configuration
def construct_gripper_orientation_iterator_sm():
    sm = StateMachine(outcomes    = ['continue','aborted'], 
                      input_keys  = ['descriptors','descriptors_index'],
                      output_keys = ['gripper_options','gripper_index','descriptors','descriptors_index'])

    #sm.userdata.gripper_options = [ 0, 1 ]
    sm.userdata.gripper_options = [ 0 ]

    sm.userdata.total_options = len(sm.userdata.gripper_options)

    sm_container = construct_round_iterator_sm()

    with sm:
        gripper_iterator = Iterator(outcomes = ['succeeded','aborted'],
                               input_keys = ['gripper_options','descriptors','descriptors_index'],
                               it = range(0, sm.userdata.total_options),
                               output_keys = ['gripper_options', 'gripper_index', 'descriptors','descriptors_index'],
                               it_label = 'gripper_index',
                               exhausted_outcome = 'succeeded')

        with gripper_iterator:
            Iterator.set_contained_state('GRIPPER_CONTAINER_STATE',
                                    sm_container,
                                    loop_outcomes=['continue'])

        smach.StateMachine.add('GRIPPER_ITERATOR', gripper_iterator,
                 {'succeeded':'continue',
                  'aborted':'aborted'})

    return sm

# Iterator to round over descriptor iterators
def construct_descriptors_iterator_sm():
    sm = StateMachine(outcomes  = ['succeeded','aborted'],
                                   output_keys = ['descriptors','descriptors_index'])
    # sm.userdata.descriptors = [ 11, 14, 15, 16, 22 ]
    sm.userdata.descriptors = [ 11 ]
    sm.userdata.total_descriptors = len(sm.userdata.descriptors)

    sm_container = construct_gripper_orientation_iterator_sm()

    with sm:
        descriptor_iterator = Iterator(outcomes = ['succeeded','aborted'],
                               input_keys = ['descriptors'],
                               it = range(0, sm.userdata.total_descriptors),
                               output_keys = ['descriptors','descriptors_index'],
                               it_label = 'descriptors_index',
                               exhausted_outcome = 'succeeded')

        with descriptor_iterator:
            Iterator.set_contained_state('DESCRIPTOR_CONTAINER_STATE',
                                    sm_container,
                                    loop_outcomes=['continue'])

        smach.StateMachine.add('DESCRIPTOR_ITERATOR', descriptor_iterator,
                 {'succeeded':'succeeded',
                  'aborted':'aborted'})

    return sm


def main():
    rospy.init_node("pomdp_sm")
    sm_iterator = construct_descriptors_iterator_sm()

    # Run state machine introspection server for smach viewer
    sis = smach_ros.IntrospectionServer('estirabot_smach_replicable', sm_iterator,
                                        '/estirabot_smach_replicable')

    sis.start()


    outcome = sm_iterator.execute()

    rospy.spin()
    sis.stop()

if __name__ == "__main__":
    main()