TranslatePomdp2piles.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

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

    def execute(self, userdata):
        # Stupid trick to not declare sm_pomdp_config as input (SMACH goodies)
        config                    = 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.grabbing_zone == config.LEFT_ZONE):
            return 'image_from_left'
        else:
            return 'image_from_right'

    def get_config_from_pomdp_action(pomdp_action):
        """
        Translate a pomdp_action number into a PomdpConfig object
        """
    
        if (pomdp_action > 21):
            rospy.logfatal("Pomdp action id out of range: " + str(pomdp_action))
    
        server = PomdpConfigFactory()
    
        if  (pomdp_action == 20):
            magic_number = 114
        elif (pomdp_action == 21):
            magic_number = 114
        elif (pomdp_action % 10 == 0):
            magic_number = 1
        elif (pomdp_action % 10 == 1):
            magic_number = 3
        elif (pomdp_action % 10 == 2):
            magic_number = 5
        elif (pomdp_action % 10 == 3):
            magic_number = 11
        elif (pomdp_action % 10 == 4):
            magic_number = 14
        elif (pomdp_action % 10 == 5):
            magic_number = 101
        elif (pomdp_action % 10 == 6):
            magic_number = 103
        elif (pomdp_action % 10 == 7):
            magic_number = 105
        elif (pomdp_action % 10 == 8):
            magic_number = 111
        elif (pomdp_action % 10 == 9):
            magic_number = 114
    
        # From magic number get configuration
        config = server.get_instance(magic_number)
    
        config.actiontype = config.GRAB
        # Also set the grabbing zone
        if (pomdp_action == 20):
            config.grabbing_zone = config.RIGHT_ZONE
            config.actiontype = config.DROP
        elif (pomdp_action == 21):
            config.grabbing_zone = config.LEFT_ZONE
            config.actiontype = config.DROP
        elif (pomdp_action > 9):
            config.grabbing_zone = config.LEFT_ZONE
        else:
            config.grabbing_zone = config.RIGHT_ZONE
    
        # 20 and 21 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 == 20) or (pomdp_action == 21):
            config.place_point = None
    
        return config