presentation_tibi.py

Go to the documentation of this file.

#!/usr/bin/env python

import roslib; roslib.load_manifest('tibi_dabo_smachs')
import rospy
import smach
import smach_ros
import tf
import math

from tf.transformations import euler_from_quaternion
from geometry_msgs.msg import Point, Pose, Quaternion
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from tibi_dabo_msgs.msg import sequenceAction, sequenceGoal
from iri_perception_msgs.msg import peopleTrackAction
from actionlib import *
from actionlib.msg import *
from smach_ros import SimpleActionState, ServiceState

# hri goal callback
def hri_goal_cb(userdata, goal):
  rospy.loginfo('hri_goal_cb')
  tts_goal = sequenceGoal()
  tts_goal.sequence_file = [None]*5
  tts_goal.sequence_file[0] = userdata.tts_in
  tts_goal.sequence_file[1] = ""
  tts_goal.sequence_file[2] = userdata.head_in
  tts_goal.sequence_file[3] = ""
  tts_goal.sequence_file[4] = ""
  return tts_goal

# move base goal callback
#def move_base_goal_cb(userdata, goal):
#   rospy.loginfo('rotate_base_goal_cb')
#  nav_goal = MoveBaseGoal()
  #nav_goal.target_pose.header.stamp    = rospy.Time.now()
  #nav_goal.target_pose.header.frame_id = userdata.frame_id_in
  #p = Point(0,1, 0)
  #q = tf.transformations.quaternion_from_euler(0, 0, 2)
  #nav_goal.target_pose.pose = Pose(position=p, orientation=Quaternion(*q))
  #return nav_goal

# main
def main():
    rospy.init_node('concurrent_hri_move_base')

    # Create a SMACH state machine
    sm = smach.StateMachine(['succeeded','aborted','preempted'])
    
    # Define userdata
    sm.userdata.sm_frame_id   = "/tibi/base_link"
    sm.userdata.sm_goal_x     = 0.0
    sm.userdata.sm_goal_y     = 0.0
    sm.userdata.sm_goal_theta = 0.0

  # Concurrence State: HRI + Rotate Base
   # hri_rb_cc = smach.Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
    #                              default_outcome='aborted',
     #                             input_keys=['cc_hri_tts', 'cc_hri_head', 'cc_hri_left', 'cc_frame_id', 
      #                                        'cc_goal_x', 'cc_goal_y', 'cc_goal_theta'],
       #                           child_termination_cb = lambda so: False,
        #                          outcome_cb = hri_mb_out_cb)
   # with hri_rb_cc:
    #    smach.Concurrence.add('CC_HRI',
     #                         smach_ros.SimpleActionState('hri',
      #                                                    sequenceAction,
       #                                                   goal_cb=hri_goal_cb,
        #                                                  input_keys=['tts_in','head_in','left_in']),
         #                     remapping={'tts_in':'cc_hri_tts',
          #                               'head_in':'cc_hri_head',
           #                              'left_in':'cc_hri_left'})

        #smach.Concurrence.add('CC_ROTATE_BASE',
         #                     smach_ros.SimpleActionState('MoveBase',
          #                                                MoveBaseAction,
           #                                               goal_cb=move_base_goal_cb,
            #                                              input_keys=['frame_id_in', 'goal_x_in', 
             #                                                         'goal_y_in', 'goal_theta_in']),
              #                remapping={'frame_id_in':'cc_frame_id',
               #                          'goal_x_in':'cc_goal_x',
                #                         'goal_y_in':'cc_goal_y',
                 #                        'goal_theta_in':'cc_goal_theta'})



    # Add states to the State Machine
    with sm:
        # HRI Presentation
        sm.userdata.sm_hri_tts_1  = "Hola a tothom. Benvinguts a l'edicio numero trenta del salo del comic de Barcelona. Jo soc la Tibi, un robot social. Estic aqui per explicar-vos  curiositats sobre el salo."
        sm.userdata.sm_hri_head_1 = "head_random_1.xml"
        sm.userdata.sm_hri_left_1 = "left_arm_greeting.xml"
        smach.StateMachine.add('HRI_1',
                               smach_ros.SimpleActionState('hri',
                                                           sequenceAction,
                                                           goal_cb=hri_goal_cb,
                                                           input_keys=['tts_in', 'head_in','left_in']),
                               transitions={'succeeded':'CC_HRI_RB_1'},
                               remapping={'tts_in':'sm_hri_tts_1',
                                          'head_in':'sm_hri_head_1',
                                          'left_in':'sm_hri_left_1'})

        # First concurrent state , robot approaches person
        sm.userdata.sm_hri_tts_2  = "Si necessiteu qualsevol cosa no dubteu en preguntar-me, tot i que ara que hi penso millor parleu amb en Dabo, es el meu germa, ens assemblem molt, pero a ell li agrada el color blau, no el taronja"
        sm.userdata.sm_hri_head_2 = "head_random_2.xml"
        sm.userdata.sm_hri_left_2=  "left_arm_random_1.xml"
        smach.StateMachine.add('CC_HRI_RB_1',
                               hri_rb_cc,
                               transitions={'succeeded':'HRI_2',
                                            'aborted':'HRI_BLOCKED'},
                               remapping={'cc_hri_tts':'sm_hri_tts_2',
                                          'cc_hri_head':'sm_hri_head_2',
                                          'left_in':'sm_hri_left_2',
                                          'cc_frame_id':'sm_frame_id', 
                                          'cc_goal_x':'sm_goal_x',  
                                          'cc_goal_y':'sm_goal_y',  
                                          'cc_goal_theta':'sm_goal_theta'})

        # HRI Presentation
        sm.userdata.sm_hri_tts_3  = "Per aqui tambe esta el meu cosi, es diu Teo, es aquest trasto de quatre rodes que nomes sap fer mapes. Semple parla de mapes, es un aborrit, despres podeu demanar-li que us fassi un mapa del recinte."
        sm.userdata.sm_hri_head_3 = "head_random_3.xml"
        sm.userdata.sm_hri_left_3 = "left_arm_random_2.xml"
        smach.StateMachine.add('HRI_2',
                               smach_ros.SimpleActionState('hri',
                                                           sequenceAction,
                                                           goal_cb=hri_goal_cb,
                                                           input_keys=['tts_in', 'head_in','left_in']),
                               transitions={'succeeded':'CC_HRI_RB_2'},
                               remapping={'tts_in':'sm_hri_tts_3',
                                          'head_in':'sm_hri_head_3',
                                          'left_in':'sm_hri_left_3'})

        # Second concurrent state , robot approaches person
        sm.userdata.sm_hri_tts_4  = " Ah! me n'oblidava tambe ha vingut l'auron, es aquell robot aranya amb tantes potes, ara esta hivernant, aixi que no el desperteu, que s'enfada i es desperta de mal humor"
        sm.userdata.sm_hri_head_4 = "head_random_3.xml"
        sm.userdata.sm_hri_left_4 = "left_arm_random_3.xml"
        smach.StateMachine.add('CC_HRI_RB_2',
                               hri_rb_cc,
                               transitions={'succeeded':'HRI_3',
                                            'aborted':'HRI_BLOCKED'},
                               remapping={'cc_hri_tts':'sm_hri_tts_4',
                                          'cc_hri_head':'sm_hri_head_4',
                                          'left_in':'sm_hri_left_4',
                                          'cc_frame_id':'sm_frame_id', 
                                          'cc_goal_x':'sm_goal_x',  
                                          'cc_goal_y':'sm_goal_y',  
                                          'cc_goal_theta':'sm_goal_theta'})

        # HRI Presentation
        sm.userdata.sm_hri_tts_5  = "I despres esta la uait mun , es la nostra mascota, es la bola blanca. I per ultim el Darwin, el mimat, tothmo diu que es el millor de tots, pero  com un nen petit. I alla al davant tenim el Rim, es de la competencia, pero a vegades ens trobem per pendre xocolata."
        sm.userdata.sm_hri_head_5 = "head_random_3.xml"
        sm.userdata.sm_hri_left_5= "left_arm_random_2.xml"
        smach.StateMachine.add('HRI_3',
                               smach_ros.SimpleActionState('hri',
                                                           sequenceAction,
                                                           goal_cb=hri_goal_cb,
                                                           input_keys=['tts_in', 'head_in','left_in']),
                               transitions={'succeeded':'FAREWELL'},
                               remapping={'tts_in':'sm_hri_tts_5',
                                          'head_in':'sm_hri_head_5',
                                          'left_in':'sm_hri_left_5'})




        # HRI 2, FAREWELL 3
        sm.userdata.sm_hri_tts_farewell  = "Si algu vol entrar per saber mes coses es benvingut. Espero veure-us aviat. Adeu a tots"
        sm.userdata.sm_hri_head_farewell = "head_farewell.xml"
        sm.userdata.sm_hri_left_farewell= "left_arm_farewell.xml"
        smach.StateMachine.add('FAREWELL',
                               smach_ros.SimpleActionState('hri',
                                                           sequenceAction,
                                                           goal_cb=hri_goal_cb,
                                                           input_keys=['tts_in', 'head_in','left_in']),
                               transitions={'succeeded':'succeeded'}, 
                               remapping={'tts_in':'sm_hri_tts_farewell',
                                          'head_in':'sm_hri_head_farewell',
                                          'left_in':'sm_hri_left_farewell'})


        # HRI , Robot is blocked, 
        sm.userdata.sm_hri_tts_blocked  = "Sergi, Fernando, Anais, no em puc moure, ajudeu-me"
        sm.userdata.sm_hri_head_blocked = "head_farewell.xml"
        sm.userdata.sm_hri_left_blocked= "left_arm_random_4.xml"
        smach.StateMachine.add('HRI_BLOCKED',
                               smach_ros.SimpleActionState('hri',
                                                           sequenceAction,
                                                           goal_cb=hri_goal_cb,
                                                           input_keys=['tts_in', 'head_in','left_in']),
                               transitions={'succeeded':'succeeded'}, 
                               remapping={'tts_in':'sm_hri_tts_blocked',
                                          'head_in':'sm_hri_head_blocked',
                                          'left_in':'sm_hri_left_blocked'})

    # Create and start the introspection server
    sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
    sis.start()

    # Execute the state machine
    outcome = sm.execute()

    # Wait for ctrl-c to stop the application
    rospy.spin()
    sis.stop()


if __name__ == '__main__':
    main()