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

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('move_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(userdata.goal_x_in, userdata.goal_y_in, 0)
  q = tf.transformations.quaternion_from_euler(0, 0, userdata.goal_theta_in)
  nav_goal.target_pose.pose = Pose(position=p, orientation=Quaternion(*q))
  return nav_goal

# people tracking result callback
def pt_result_cb(userdata, state, result):
  rospy.loginfo('result!! num_persons=%d',len(result.ps.peopleSet))
  if len(result.ps.peopleSet)<1:
    return 'aborted'
  else:
    userdata.frame_id_pt_out = result.ps.header.frame_id
    userdata.x_pt_out        = result.ps.peopleSet[0].x
    userdata.y_pt_out        = result.ps.peopleSet[0].y
    userdata.id_pt_out       = result.ps.peopleSet[0].targetId
    userdata.theta_pt_out    = 0
    return 'succeeded'

# Check Distance  between robot and person



# 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     = 2.0
    #sm.userdata.sm_goal_y     = 0.0
    #sm.userdata.sm_goal_theta = 0.0

    # gets called when ALL child states are terminated
    def hri_mb_out_cb(outcome_map):
      if outcome_map['CC_HRI'] == 'succeeded' and outcome_map['CC_MOVE_BASE'] == 'succeeded':
          return 'succeeded'
      else:
          return 'aborted'

    # Concurrence State: HRI + Move Base
    hri_mb_cc = smach.Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
                                  default_outcome='aborted',
                                  input_keys=['cc_hri_tts', 'cc_hri_head', '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_mb_cc:
        smach.Concurrence.add('CC_HRI',
                              smach_ros.SimpleActionState('hri',
                                                          sequenceAction,
                                                          goal_cb=hri_goal_cb,
                                                          input_keys=['tts_in','head_in']),
                              remapping={'tts_in':'cc_hri_tts',
                                         'head_in':'cc_hri_head'})

        smach.Concurrence.add('CC_MOVE_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  = "I am looking for a person in the room"
        sm.userdata.sm_hri_head_1 = "head_random_4.xml"
        smach.StateMachine.add('HRI_BEFORE_CC',
                               smach_ros.SimpleActionState('hri',
                                                           sequenceAction,
                                                           goal_cb=hri_goal_cb,
                                                           input_keys=['tts_in', 'head_in']),
                               transitions={'succeeded':'AC_PT'},
                               remapping={'tts_in':'sm_hri_tts_1',
                                          'head_in':'sm_hri_head_1'})
                               
        # People Tracking Action Client
        smach.StateMachine.add('AC_PT',
                               smach_ros.SimpleActionState('people_tracking',
                                                           peopleTrackAction,
                                                           result_cb=pt_result_cb,
                                                           output_keys=['frame_id_pt_out','x_pt_out',
                                                                        'y_pt_out','id_pt_out','theta_pt_out']),
                               transitions={'succeeded':'CC_HRI_MB',
                                            'aborted':'HRI_FAIL'},
                               remapping={'frame_id_pt_out':'sm_frame_id',
                                          'x_pt_out':'sm_goal_x',
                                          'y_pt_out':'sm_goal_y',
                                          'id_pt_out':'sm_target_id',
                                          'theta_pt_out':'sm_goal_theta'})

        # HRI Fail
        sm.userdata.sm_hri_tts_2  = "I could not find anyone in this room"
        sm.userdata.sm_hri_head_2 = "head_random_4.xml"
        smach.StateMachine.add('HRI_FAIL',
                               smach_ros.SimpleActionState('hri',
                                                           sequenceAction,
                                                           goal_cb=hri_goal_cb,
                                                           input_keys=['tts_in', 'head_in']),
                               transitions={'succeeded':'HRI_BEFORE_CC'},
                               remapping={'tts_in':'sm_hri_tts_2',
                                          'head_in':'sm_hri_head_2'})


        # First concurrent state
        sm.userdata.sm_hri_tts_3  =  "I found a person close to me. I am approaching the target"
        sm.userdata.sm_hri_head_3 = "head_random_4.xml"
        smach.StateMachine.add('CC_HRI_MB',
                               hri_mb_cc,
                               transitions={'succeeded':'HRI_AFTER_CC'},
                               remapping={'cc_hri_tts':'sm_hri_tts_3',
                                          'cc_hri_head':'sm_hri_head_3',
                                          'cc_frame_id':'sm_frame_id', 
                                          'cc_goal_x':'sm_goal_x',  #puc posar 0 pq aixi no es moura?
                                          'cc_goal_y':'sm_goal_y',  #puc posar 0 pq aixi no es moura?
                                          'cc_goal_theta':'sm_goal_theta'})

        ## Second concurrent state
        #sm.userdata.sm_hri_tts_3 = "More concurrent text"
        #sm.userdata.sm_goal_x_2  = 1.0
        #smach.StateMachine.add('CC_HRI_MB_2',
                               #hri_mb_cc,
                               #transitions={'succeeded':'HRI_AFTER_CC'},
                               #remapping={'cc_hri_tts':'sm_hri_tts_3',
                                          #'cc_frame_id':'sm_frame_id', 
                                          #'cc_goal_x':'sm_goal_x_2',
                                          #'cc_goal_y':'sm_goal_y',
                                          #'cc_goal_theta':'sm_goal_theta'})

        # HRI After Concurrence State
        sm.userdata.sm_hri_tts_4  = "I wish I have someone standing in front of me"
        sm.userdata.sm_hri_head_4 = "head_random_4.xml"
        smach.StateMachine.add('HRI_AFTER_CC',
                               smach_ros.SimpleActionState('hri',
                                                           sequenceAction,
                                                           goal_cb=hri_goal_cb,
                                                           input_keys=['tts_in', 'head_in']),
                               transitions={'succeeded':'succeeded'},
                               remapping={'tts_in':'sm_hri_tts_4',
                                          'head_in':'sm_hri_head_4'})

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