TranslatePomdp3piles.py

Go to the documentation of this file.

#!/usr/bin/env python

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

from pprint import pprint
from iri_common_smach.utils_msg import build_pose_stamped_msg, build_transform_msg
from estirabot_msgs.msg         import PomdpGraspConfig
from estirabot_apps_base.utils_pomdp            import PomdpConfigFactory 

class TranslatePomdp(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes    = ['fail','image_from_left','image_from_right','drop'],
                                   input_keys  = ['pomdp_action','place_handler'],
                                   output_keys = ['sm_pomdp_config','place_handler'])

    def get_config_from_pomdp_action(self, pomdp_action):
        """
        Translate a pomdp_action number into a PomdpConfig object
        """
    
        if (pomdp_action > 22):
            rospy.logfatal("Pomdp action id out of range: " + str(pomdp_action))
    
        server = PomdpConfigFactory()
    
        if   (pomdp_action == 0 or pomdp_action == 10): #grabb
            magic_number = 1 
        elif (pomdp_action == 1 or pomdp_action == 11): #grabb
            magic_number = 3
        elif (pomdp_action == 2 or pomdp_action == 12): #grabb
            magic_number = 5
        elif (pomdp_action == 3 or pomdp_action == 13): #grabb
            magic_number = 11
        elif (pomdp_action == 4 or pomdp_action == 14): #grabb
            magic_number = 14
        elif (pomdp_action == 5 or pomdp_action == 15): #grabb
            magic_number = 101
        elif (pomdp_action == 6 or pomdp_action == 16): #grabb
            magic_number = 103
        elif (pomdp_action == 7 or pomdp_action == 17): #grabb
            magic_number = 105
        elif (pomdp_action == 8 or pomdp_action == 18): #grabb
            magic_number = 111
        elif (pomdp_action == 9 or pomdp_action == 19): #grabb
            magic_number = 114
        elif (pomdp_action == 20): #dropb
            magic_number = 114
        elif (pomdp_action == 21): #dropa
            magic_number = 114
        elif (pomdp_action == 22): #dropout
            magic_number = 114

        # From magic number get configuration
        config = server.get_instance(magic_number)
    
        if (pomdp_action >= 0 and pomdp_action <= 19):
            config.actiontype = config.GRAB
        elif (pomdp_action == 20 or pomdp_action == 21 or pomdp_action == 22):
            config.actiontype = config.DROP
        else:
            rospy.logfatal("Pomdp action id out of range: " + str(pomdp_action))
            
        # Also set the grabbing zone
        if (pomdp_action >= 0 and pomdp_action <= 9):
            config.grabbing_zone = config.RIGHT_ZONE
        elif (pomdp_action >= 10 and pomdp_action <= 19):
            config.grabbing_zone = config.LEFT_ZONE
        elif (pomdp_action == 20):
            config.grabbing_zone = config.RIGHT_ZONE
        elif (pomdp_action == 21):
            config.grabbing_zone = config.LEFT_ZONE
        else:
            #drop action does not have a grasping zone
            pass 
    
        # dropout should get an special value for place.
        # Hack for this is set place_point to None and check for it
        # in PlaceHandler. Dirty as hell.
        if (pomdp_action == 22):
            config.place_point = None
    
        return config

    def execute(self, userdata):
        # Stupid trick to not declare sm_pomdp_config as input (SMACH goodies)
        config                    = self.get_config_from_pomdp_action(userdata.pomdp_action)
        config.place_point        = userdata.place_handler.calculate_place_pose(config)

        userdata.sm_pomdp_config = config

        pub_config = rospy.Publisher('/debug/pomdp_config', PomdpGraspConfig, None, False, True)
        pub_config.publish(config)
        pprint(config)

        if (config.actiontype == config.DROP):
            return 'drop'

        if (config.grabbing_zone == config.LEFT_ZONE):
            return 'image_from_left'
        else:
            return 'image_from_right'