pomdp_execution_basket_3_piles.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 tf

#from pomdp_create_model 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 iri_common_smach.utils_msg import build_pose
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 estirabot_msgs.msg import PomdpGraspConfig
from estirabot_msgs.srv import obs2action, obs2actionRequest
from geometry_msgs.msg import Transform, PoseStamped, Pose
from pprint import pprint
from std_msgs.msg import Int8, Int32

from iri_common_smach.st_get_pcl import GetPCL
from iri_common_smach.transform_manager import TransformManager
from iri_common_smach.homogeneous_product import *
from iri_common_smach.utils_msg           import build_pose_stamped_msg, build_transform_msg
from iri_common_smach.get_keystroke       import GetKeyStroke

from iri_wam_smach.sm_wam_move_from_pose       import SM_WAM_MoveFromPose 

from estirabot_apps_base.utils_pomdp            import PomdpConfigFactory 
from estirabot_apps_base.sm_move                import SM_ESTIRABOT_GoHome
from estirabot_apps_base.sm_deformable_analysis import SM_ESTIRABOT_DeformableAnalysis
from estirabot_apps_base.sm_deformable_analysis import PerformAnalysis
from estirabot_apps_base.sm_generate_grasp      import *
from TranslatePomdp3piles import TranslatePomdp 


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

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

        print "Your value was: " + str(c)
        return 'success'

class PlaceHandler():
    def __init__(self):
        self.next_left_zone  = 1
        self.next_right_zone = 1

    def calculate_place_pose(self, pomdp_config):
        # If place_point is None it implies special basket
        if (pomdp_config.place_point == None):
            return self.get_basket_pose()

        if (pomdp_config.grabbing_zone == pomdp_config.RIGHT_ZONE):
            return self.get_right_place_pose()
        elif (pomdp_config.grabbing_zone == pomdp_config.LEFT_ZONE):
            return self.get_left_place_pose()
        else:
            rospy.logerr("Value for grasping zone is not LEFT or RIGHT")
            return build_home_pose()

    def get_basket_pose(self):
        return build_pose(-0.165, 0.514, -0.188, 1, 0, 0, 0)

    def get_right_place_pose(self):
        if (self.next_right_zone == 1):
            pose = build_pose(0.422, -0.390, -0.234, 1, 0, 0, 0)
        elif (self.next_right_zone == 2):
            pose = build_pose(0.584, -0.390, -0.234, 1, 0, 0, 0)
        elif (self.next_right_zone == 3):
            pose = build_pose(0.633, -0.260, -0.234, 1, 0, 0, 0)
        elif (self.next_right_zone == 4):
            pose = build_pose(0.476, -0.260, -0.234, 1, 0, 0, 0)
        else:
            rospy.logerr("Righ pose next zone is out of range: " + str(self.next_right_zone))

        self.next_right_zone = self.next_right_zone + 1

        # reset to 1 again
        if (self.next_right_zone > 4):
            self.next_right_zone = 1

        return pose

    def get_left_place_pose(self):
        if (self.next_left_zone == 1):
            pose = build_pose(0.422, 0.390, -0.234, 1, 0, 0, 0)
        elif (self.next_left_zone == 2):
            pose = build_pose(0.584, 0.390, -0.234, 1, 0, 0, 0)
        elif (self.next_left_zone == 3):
            pose = build_pose(0.633, 0.260, -0.234, 1, 0, 0, 0)
        elif (self.next_left_zone == 4):
            pose = build_pose(0.476, 0.260, -0.234, 1, 0, 0, 0)
        else:
            rospy.logerr("Left pose next zone is out of range: " + str(self.next_left_zone))

        self.next_left_zone = self.next_left_zone + 1

        # reset to 1 again
        if (self.next_left_zone > 4):
            self.next_left_zone = 1

        return pose

class BuildPickupGoalFromTransform(smach.State):
    fingers_grasp_configs = []
    def __init__(self):
        smach.State.__init__(self, outcomes=['success','fail','no_ik_solution'],
                                   input_keys=['grasp_pose','transform'],
                                   output_keys=['sm_grasp_goal'])
        self.fingers_grasp_configs.append("GSTO")
        self.fingers_grasp_configs.append("SM 600")
        self.fingers_grasp_configs.append("GTC")
        self.grasp_pose_st_wam                 = PoseStamped()
        self.grasp_pose_st_wam.header.frame_id = '/wam_link0'
        self.grasp_pose_st_wam.header.stamp    = rospy.Time.now()
        self.grasp_pose_st_wam.pose = build_pose(-0.08, -0.54, -0.35, 1, 0, 0, 0)

    def execute(self, userdata):
        # shortcuts
        rospy.set_param('/estirabot/skills/grasp/fingers_configuration', self.fingers_grasp_configs)

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

        # pre-grasp posture is relative to real_grasp pose
        pre_grasp_pose_st        = PoseStamped()
        pre_grasp_pose_st.header = self.grasp_pose_st_wam.header

        modified_grasp_pose_st.pose = homogeneous_product_pose_transform(self.grasp_pose_st_wam.pose,
                                                                         userdata.transform)

        pprint("PRE-Grasp modifier applied")
        pub = rospy.Publisher('/debug/grasp/pre_grasp', PoseStamped, None, False, True)
        pub.publish(pre_grasp_pose_st)

        # For both grasp and pre-grasp we need the inverse kinematics and get joinst positions
        try:
            joints_grasp     = get_inverse_kinematics_from_pose(modified_grasp_pose_st)

        #except NoKinematicSolutionException, e:
        except rospy.ServiceException, e:
            return 'no_ik_solution'

        userdata.sm_grasp_goal =  build_simple_pickup_goal(joints_grasp,
                                                        joints_pre_grasp,
                                                        self.fingers_grasp_configs)
        return 'success'

class TakeFromBasket(smach.State):
    fingers_grasp_configs = []
    def __init__(self):
        smach.State.__init__(self, outcomes=['success','fail','no_ik_solution'],
                                   output_keys=['sm_grasp_goal'])
        self.fingers_grasp_configs.append("GSTO")
        self.fingers_grasp_configs.append("SM 600")
        self.fingers_grasp_configs.append("GTC")
        self.grasp_pose_st_wam                 = PoseStamped()
        self.grasp_pose_st_wam.header.frame_id = '/wam_link0'
        self.grasp_pose_st_wam.header.stamp    = rospy.Time.now()
        self.grasp_pose_st_wam.pose = build_pose(-0.08, -0.54, -0.35, 1, 0, 0, 0)

    def execute(self, userdata):
        # shortcuts
        rospy.set_param('/estirabot/skills/grasp/fingers_configuration', self.fingers_grasp_configs)

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

        # pre-grasp posture is relative to real_grasp pose
        pre_grasp_pose_st        = PoseStamped()
        pre_grasp_pose_st.header = self.grasp_pose_st_wam.header

        pre_grasp_pose_st.pose = homogeneous_product_pose_transform(self.grasp_pose_st_wam.pose,
                                                          build_transform_msg(0, 0, -0.08, 0, 0, 0, 1))
        pprint("PRE-Grasp modifier applied")
        pub = rospy.Publisher('/debug/grasp/pre_grasp', PoseStamped, None, False, True)
        pub.publish(pre_grasp_pose_st)

        # For both grasp and pre-grasp we need the inverse kinematics and get joinst positions
        try:
            joints_grasp     = get_inverse_kinematics_from_pose(self.grasp_pose_st_wam)
            joints_pre_grasp = get_inverse_kinematics_from_pose(pre_grasp_pose_st)

        #except NoKinematicSolutionException, e:
        except rospy.ServiceException, e:
            return 'no_ik_solution'

        userdata.sm_grasp_goal =  build_simple_pickup_goal(joints_grasp,
                                                        joints_pre_grasp,
                                                        self.fingers_grasp_configs)
        return 'success'

class FirstAction(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes    = ['ok','fail'],
                                   output_keys = ['pomdp_action','place_handler'])

    def execute(self, userdata):
        # Config and launch the deformable analysis
        request = obs2actionRequest()
        request.first       = True
        handler = rospy.ServiceProxy('/estirabot/skills/planner/obs2action', obs2action)
        response = handler(request)

        pprint(response)
        userdata.pomdp_action = response.action
        pub = rospy.Publisher('/log/pomdp/action', Int8, None, False, True)
        pub.publish(response.action)

        if (not response):
            return 'fail'

        return 'ok'

class LogPCL(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['success','fail'],
                                   input_keys=['sm_pcl_to_log'],
                                   output_keys=[''])

    def execute(self, userdata):
        # Log the PCL
        pub = rospy.Publisher('/log/pcl_table', PointCloud2,  None, False, True)
        pub.publish(userdata.sm_pcl_to_log)
        return 'success'


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

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

        rospy.set_param('/estirabot/skills/grasp/fingers_configuration', pomdp_config.fingers_grasp_configs)

        # 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)

        # Orientation is fixed so ignoring previous
        grasp_pose_st_wam.pose.orientation.x = 0
        grasp_pose_st_wam.pose.orientation.y = 0
        grasp_pose_st_wam.pose.orientation.z = 0
        grasp_pose_st_wam.pose.orientation.w = 1.0

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

        # real grasp is build applying grasp modifier to grasp_pose
        current_modifier     = pomdp_config.approach_config.grasp_modifier_used
        pprint("Using grasp modifier number: " + str(current_modifier))
        real_grasp_st        = PoseStamped()
        real_grasp_st.header = grasp_pose_st_wam.header
        real_grasp_st.pose   = homogeneous_product_pose_transform(grasp_pose_st_wam.pose,
                                                          pomdp_config.approach_config.grasp_modifiers[current_modifier])
        if (real_grasp_st.pose.position.z < -0.282):
            real_grasp_st.pose.position.z = -0.282

        pprint("Grasp modifier applied")
        pub = rospy.Publisher('/debug/grasp/real', PoseStamped, None, False, True)
        pub.publish(real_grasp_st)

        # pre-grasp posture is relative to real_grasp pose
        pre_grasp_pose_st        = PoseStamped()
        pre_grasp_pose_st.header = grasp_pose_st_wam.header

        pre_grasp_pose_st.pose = homogeneous_product_pose_transform(real_grasp_st.pose,
                                                          pomdp_config.approach_config.pre_grasp_modifier)
        pprint("PRE-Grasp modifier applied")
        pub = rospy.Publisher('/debug/grasp/pre_grasp', PoseStamped, None, False, True)
        pub.publish(pre_grasp_pose_st)

        # GRASP is Kinematic compliant so save and log the final configuration
        pub_config = rospy.Publisher('/log/pomdp_config', PomdpGraspConfig, None, False, True)
        pub_config.publish(pomdp_config)

        # For both grasp and pre-grasp we need the inverse kinematics and get joinst positions
        try:
            joints_grasp     = get_inverse_kinematics_from_pose(real_grasp_st)
            joints_pre_grasp = get_inverse_kinematics_from_pose(pre_grasp_pose_st)

        #except NoKinematicSolutionException, e:
        except rospy.ServiceException, e:
            # Let's go to next grasp_modifier (if any)
            if (current_modifier < len(pomdp_config.approach_config.grasp_modifiers)-1):
                # Mark next configuration to use 
                pomdp_config.approach_config.grasp_modifier_used = pomdp_config.approach_config.grasp_modifier_used + 1
                pomdp_config.approach_config.pre_grasp_modifier
                return 'no_ik_solution'
            else:
                # all modifiers were tested and are not valid. Return error
                pub = rospy.Publisher('/log/number_of_clothes', Int8, None, False, True)
                pub.publish(0)
                rospy.logerr("Inverse Kinematics has no solution")
                return 'all_ik_fail'

        #     rospy.logerr("WamIK service failed")
        #           return 'fail'

        userdata.grasp_goal =  build_simple_pickup_goal(joints_grasp,
                                                        joints_pre_grasp,
                                                        pomdp_config.fingers_grasp_configs)
        return 'success'

class CountObjects(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes    = ['fail','success'],
                                   input_keys  = ['sm_pcl_table_left','sm_pcl_table_right'],
                                   output_keys = ['left_objects','right_objects','grasp_objects'])
        self.number = -1
        # Subscribe to count numbers
        sub = rospy.Subscriber("/estirabot/skills/iri_textile_count/num_objects_found",
                               Int32, self.recieve_number)

    def execute(self, userdata):
        # Left objects
        self.calculate_objects(userdata.sm_pcl_table_left)
        userdata.left_objects = self.number
        pprint ("Left objects: " + str(self.number))
        pub = rospy.Publisher('/debug/observation/left_objects', Int8, None, False, True)
        pub.publish(self.number)

        # sleep required for visualizing the histograms since they're published to the same topic
        # rospy.sleep(2)

        # Right objects
        self.calculate_objects(userdata.sm_pcl_table_right)
        userdata.right_objects = self.number
        pprint ("Right objects: " + str(self.number))
        pub = rospy.Publisher('/debug/observation/right_objects', Int8, None, False, True)
        pub.publish(self.number)

        userdata.grasp_objects = 0 #blind at grasp (but true since drop works 100%)

        return 'success'

    def recieve_number(self, data):
        self.number = data.data

    def calculate_objects(self, pcl):
        self.number = -1
        # Publish the pcl in /estirabot/skills/table/pcl
        pub = rospy.Publisher('/estirabot/skills/table/pcl', PointCloud2, None, False, True)
        pub.publish(pcl)
        # Wait to recieve the callback
        while (self.number == -1 and (not rospy.is_shutdown())):
            pprint("waiting")
            time.sleep(.1)

class WeightObjects(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes    = ['fail','success'],
                                   output_keys = ['left_objects','right_objects','grasp_objects'])
        self.number = -1
        # Subscribe to count numbers
        #sub = rospy.Subscriber("/estirabot/skills/iri_textile_count/num_objects_found",
        #                       Int32, self.recieve_number)

    def execute(self, userdata):
        keyserver = GetKeyStroke()
        self.number = -1
        print "Press [0-5] for clothes grabbed, 9 for error:"
        while (self.number < 0):
            self.number = keyserver()
            try:
                self.number = int(self.number)
            except ValueError:
                print "Error. Press [0-5] for clothes grabbed:"
                continue

        print "Your value was: " + str(self.number)
        pub = rospy.Publisher('/debug/observation/grasp_objects', Int8, None, False, True)
        userdata.grasp_objects = self.number
        pub.publish(self.number)

        pub = rospy.Publisher('/debug/observation/left_objects', Int8, None, False, True)
        userdata.left_objects = self.number
        pub.publish(0) #unreliable data at table

        pub = rospy.Publisher('/debug/observation/right_objects', Int8, None, False, True)
        userdata.right_objects = self.number
        pub.publish(0) #unreliable data at table

        return 'success'

    def recieve_number(self, data):
        self.number = data.data

    def calculate_objects(self, pcl):
        self.number = -1
        # Publish the pcl in /estirabot/skills/table/pcl
        pub = rospy.Publisher('/estirabot/skills/table/pcl', PointCloud2, None, False, True)
        pub.publish(pcl)
        # Wait to recieve the callback
        while (self.number == -1 and (not rospy.is_shutdown())):
            pprint("waiting")
            time.sleep(.1)

class PrePlace(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes     = ['fail','success'],
                                   input_keys   = ['sm_pomdp_config'],
                                   output_keys   = ['pose_st'])

    def execute(self, userdata):
        tf = Transform()
        tf.translation.z = -0.4

        pose_stamped                 = PoseStamped()
        pose_stamped.header.frame_id = '/wam_link0'
        pose_stamped.header.stamp    = rospy.Time.now()
        pose_stamped.pose = homogeneous_product_pose_transform(userdata.sm_pomdp_config.place_point, tf)

        userdata.pose_st = pose_stamped
        return 'success'


class UpdateBelieve(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes     = ['fail','finish','continue'],
                                   input_keys   = ['left_objects','right_objects','grasp_objects'],
                                   output_keys  = ['pomdp_action'])

    def execute(self, userdata):
        request = obs2actionRequest()
        # Build observation
        right_objs = userdata.right_objects
        left_objs  = userdata.left_objects
        grasp_objs  = userdata.grasp_objects

        if (right_objs > 4):
            right_objs = 4

        if (left_objs > 4):
            left_objs = 4
        
        if (grasp_objs > 4):
            grasp_objs = 4
        
        request.first       = False
        request.observation = grasp_objs * 5 * 5 + right_objs + left_objs * 5
        pub = rospy.Publisher('/log/pomdp/observation', Int8, None, False, True)
        pub.publish(request.observation)

        pprint(request)
        handler = rospy.ServiceProxy('/estirabot/skills/planner/obs2action', obs2action)
        response = handler(request)

        if (not response):
            return 'fail'

        pprint(response)
        if (response.goal_reached == True):
            return 'finish'
    
        userdata.pomdp_action = response.action
        pub2 = rospy.Publisher('/log/pomdp/action', Int8, None, False, True)
        pub2.publish(response.action)

        return 'continue'

def get_sm_place_right_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            = build_pose(0.4, -0.4, 0.3, 1, 0, 0, 0)

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

    return goal
def get_sm_place_left_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            = build_pose(0.4, 0.4, 0.3, 1, 0, 0, 0)

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

    return goal


def get_sm_place_init_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            = build_pose(0.4, -0.4, -0.23, 1, 0, 0, 0)

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

    return goal

def get_sm_grasp_goal_callback(userdata, default_goal):
    return userdata.sm_grasp_goal

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

def construct_main_sm():

    factory_home = SM_ESTIRABOT_GoHome()
    sm_home = factory_home.build_sm()
    factory_move_cartesian = SM_WAM_MoveFromPose('/estirabot/wam_wrapper/joints_move','/estirabot/wam_ik/wamik')
    sm_move_cartesian = factory_move_cartesian.build_sm()


    sm = StateMachine(outcomes = ['succeeded','aborted'])

    with sm:
        # Create the place handler
        sm.userdata.place_handler = PlaceHandler()

        #TODO machine-in-machine instead of duplicates 
        #it's better to build up a standalone statie similar to DO_GRASP
        smach.StateMachine.add('TAKE_FROM_BASKET', TakeFromBasket(),
                transitions = { 'success':'DO_GRASP_INIT',
                                'no_ik_solution':'aborted',
                                'fail':'aborted'})
        # 0.1 GRASP
        smach.StateMachine.add('DO_GRASP_INIT',
                               smach_ros.SimpleActionState('/estirabot/skills/grasp/pickup',
                                                          PickupAction,
                                                          goal_cb = get_sm_grasp_goal_callback,
                                                          input_keys=['sm_grasp_goal']),
                               {'succeeded': 'DO_LIFT_INIT',
                                'preempted': 'aborted',
                                'aborted'  : 'aborted'})
        # 0.2 LIFT
        smach.StateMachine.add('DO_LIFT_INIT', sm_home,
                        transitions={'success' : 'DO_PLACE_INIT',
                                     'aborted'    : 'aborted'})
        # 0.4 DO PLACE
        smach.StateMachine.add('DO_PLACE_INIT',
                       smach_ros.SimpleActionState('/estirabot/skills/iri_place/place',
                                                  PlaceAction,
                                                  goal_cb = get_sm_place_init_goal_callback),
                       {'succeeded': 'BACK_TO_HOME_INIT',
                        'preempted': 'aborted',
                        'aborted'  : 'aborted'})

        smach.StateMachine.add('BACK_TO_HOME_INIT', sm_home,
                          transitions={'success': 'GIVE_FIRST_ACTION',
                                       'aborted'   : 'aborted'})

        smach.StateMachine.add('GIVE_FIRST_ACTION', FirstAction(),
                transitions = { 'ok':'START_ROUND',
                                'fail':'aborted'},
                remapping   = { 'pomdp_action' : 'pomdp_action'})

        smach.StateMachine.add('START_ROUND', sm_home,
                          transitions={'success': 'LOG_PCL',
                                       'aborted'   : 'aborted'})
        smach.StateMachine.add('LOG_PCL', 
                                GetPCL("/estirabot/ceiling_kinect/camera/rgb/points"),
                transitions = {'success' : 'LOG_IMAGE',
                               'fail'    : 'aborted'},
                remapping   = {'pcl_RGB'  : 'sm_pcl_to_log'})
        smach.StateMachine.add('LOG_IMAGE', LogPCL(),
                          transitions={'success': 'TRANSLATE_POMDP_ACTION',
                                       'fail'   : 'aborted'})

        # 2. RUN POMDP ACTION
        smach.StateMachine.add('TRANSLATE_POMDP_ACTION', TranslatePomdp(),
            transitions = { 'image_from_left'  : 'DROP_LEFT',
                            'image_from_right' : 'DROP_RIGHT',
                            'drop':'DO_PLACE',
                            'fail':'aborted'},
            remapping   = { 'sm_pomdp_config' : 'sm_pomdp_config'})


        smach.StateMachine.add('DROP_LEFT',
                       smach_ros.SimpleActionState('/estirabot/skills/iri_place/place',
                                                  PlaceAction,
                                                  goal_cb = get_sm_place_left_goal_callback,
                                                  input_keys=['sm_pomdp_config']),
                       {'succeeded': 'GET_KINECT_IMAGE_FROM_LEFT',
                        'preempted': 'aborted',
                        'aborted'  : 'aborted'})

        smach.StateMachine.add('DROP_RIGHT',
                       smach_ros.SimpleActionState('/estirabot/skills/iri_place/place',
                                                  PlaceAction,
                                                  goal_cb = get_sm_place_right_goal_callback,
                                                  input_keys=['sm_pomdp_config']),
                       {'succeeded': 'GET_KINECT_IMAGE_FROM_RIGHT',
                        'preempted': 'aborted',
                        'aborted'  : 'aborted'})
        # 2. CALCULATE GRASP POINT
        # 2.1.a GET KINECT IMAGE FROM LEFT
        smach.StateMachine.add('GET_KINECT_IMAGE_FROM_LEFT', 
                                GetPCL("/estirabot/skills/table/left/pcl"),
                transitions = {'success' : 'PERFORM_ANALISYS',
                               'fail'    : 'aborted'},
                remapping   = {'pcl_RGB'  : 'sm_pcl_output'})

        # 2.1.b GET KINECT IMAGE FROM RIGHT
        smach.StateMachine.add('GET_KINECT_IMAGE_FROM_RIGHT', 
                              GetPCL("/estirabot/skills/table/right/pcl"),
                transitions = {'success' : 'PERFORM_ANALISYS',
                               'fail'    : 'aborted'},
                remapping   = {'pcl_RGB'  : 'sm_pcl_output'})

        # 2.2 ANALYSIS
        smach.StateMachine.add('PERFORM_ANALISYS', PerformAnalysis(),
                transitions = {'success'        : 'CALCULATE_GRASP',
                               'timeout'        : 'PERFORM_ANALISYS', # hack relaunch since descriptors died
                               'fail'           : 'aborted'},
                remapping   = {'pcl_RGB'         : 'sm_pcl_output',
                               'best_grasp_pose' : 'sm_best_grasp_pose',
                               'deformable_config' : 'sm_pomdp_config'}) # compatibles
        # 2.3 CALCULATE GRASP
        smach.StateMachine.add('CALCULATE_GRASP', CalculateGrasp(),
                transitions = {'success'        : 'DO_GRASP',
                               'no_ik_solution' : 'CALCULATE_GRASP',
                               'fail'           : 'aborted',
                               'all_ik_fail'    : 'GET_TABLE_IMAGE_FROM_LEFT'},
                remapping   = {'pcl_RGB'         : 'sm_pcl_output',
                               'best_grasp_pose' : 'sm_best_grasp_pose',
                               'grasp_goal'      : 'sm_grasp_goal'})
        # 3.1 GRASP
        smach.StateMachine.add('DO_GRASP',
                               smach_ros.SimpleActionState('/estirabot/skills/grasp/pickup',
                                                          PickupAction,
                                                          goal_cb = get_sm_grasp_goal_callback,
                                                          input_keys=['sm_grasp_goal']),
                               {'succeeded': 'DO_LIFT',
                                'preempted': 'aborted',
                                'aborted'  : 'aborted'})
        # 3.2 LIFT
        smach.StateMachine.add('DO_LIFT', sm_home,
                        transitions={'success' : 'WEIGHT_OBJECTS',
                                     'aborted'    : 'aborted'})
        # 4.3 GET NUMBER OF OBJECTS
        smach.StateMachine.add('WEIGHT_OBJECTS', WeightObjects(),
                        transitions={'success' : 'UPDATE_BELIEVE_STATE',
                                     'fail'    : 'aborted'})
        # 3.3.1 DO PRE PLACE
        smach.StateMachine.add('DO_PRE_PLACE', PrePlace(),
                        transitions={'success' : 'DO_MOVE',
                                     'fail'    : 'aborted'},
                        remapping = {'pose_st' : 'sm_pose_st'})

        smach.StateMachine.add('DO_MOVE', sm_move_cartesian,
                        transitions={'success' : 'DO_PLACE',
                                     'aborted'    : 'aborted',
                                     'no_kinematic_solution'    : 'DO_PLACE'}, #TODO recompute pre place point 
                        remapping = {'pose_st' : 'sm_pose_st'})
        #
        # 3.3.2 DO PLACE
        smach.StateMachine.add('DO_PLACE',
                       smach_ros.SimpleActionState('/estirabot/skills/iri_place/place',
                                                  PlaceAction,
                                                  goal_cb = get_sm_place_goal_callback,
                                                  input_keys=['sm_pomdp_config']),
                       {'succeeded': 'BACK_TO_HOME',
                        'preempted': 'aborted',
                        'aborted'  : 'aborted'})

        smach.StateMachine.add('BACK_TO_HOME', sm_home,
                          transitions={'success': 'GET_TABLE_IMAGE_FROM_LEFT',
                                       'aborted'   : 'aborted'})

        # 4. OBSERVATION
        # 4.1 GET PCL FROM LEFT
        smach.StateMachine.add('GET_TABLE_IMAGE_FROM_LEFT', 
                      GetPCL("/estirabot/skills/table/left/pcl"),
                transitions = {'success' : 'GET_TABLE_IMAGE_FROM_RIGHT',
                               'fail'    : 'aborted'},
                remapping   = {'pcl_RGB'  : 'sm_pcl_table_left'})
        # 4.2 GET PCL FROM RIGHT
        smach.StateMachine.add('GET_TABLE_IMAGE_FROM_RIGHT', 
                      GetPCL("/estirabot/skills/table/right/pcl"),
                transitions = {'success' : 'COUNT_OBJECTS',
                               'fail'    : 'aborted'},
                remapping   = {'pcl_RGB'  : 'sm_pcl_table_right'})
        # 4.3 GET NUMBER OF OBJECTS
        smach.StateMachine.add('COUNT_OBJECTS', CountObjects(),
                        transitions={'success' : 'UPDATE_BELIEVE_STATE',
                                     'fail'    : 'aborted'})
        # 5 UPDATE BELIEVE STATE
        smach.StateMachine.add('UPDATE_BELIEVE_STATE', UpdateBelieve(),
                        transitions={'continue' : 'START_ROUND',
                                     'finish'   : 'succeeded',
                                     'fail'     : 'aborted'})

    return sm

def main():
    rospy.init_node("pomdp_sm")
    sm_main = construct_main_sm()

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

    sis.start()

    outcome = sm_main.execute()

    rospy.spin()
    sis.stop()

if __name__ == "__main__":
    main()