guiding.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 sys
import tf

from tibi_dabo_msgs.msg import sequenceAction, sequenceGoal
from geometry_msgs.msg import PoseStamped
from smach_ros import ServiceState,SimpleActionState
from tf import TransformListener
from iri_perception_msgs.msg import peopleTrackingArray
import time
import threading

exit = 0

class SimpleActionWithFeedbackState(smach_ros.SimpleActionState):
#class SimpleActionWithFeedbackState(smach.State):
    def __init__(self,
            # Action info
            action_name,
            action_spec,
            # Default goal 
            goal = None,
            goal_key = None,
            goal_slots = [],
            goal_cb = None,
            goal_cb_args = [],
            goal_cb_kwargs = {},
            # Result modes
            result_key = None,
            result_slots = [],
            result_cb = None,
            result_cb_args = [],
            result_cb_kwargs = {},
            # Keys
            input_keys = [],
            output_keys = [],
            outcomes = [],
            # Timeouts
            exec_timeout = None,
            preempt_timeout = rospy.Duration(60.0),
            server_wait_timeout = rospy.Duration(60.0)
):
        smach_ros.SimpleActionState.__init__(self,action_name,action_spec,goal,goal_key,goal_slots,goal_cb,goal_cb_args,goal_cb_kwargs,result_key,result_slots,result_cb,result_cb_args,result_cb_kwargs,input_keys,output_keys,outcomes,exec_timeout,preempt_timeout,server_wait_timeout)
        self.subscriber = rospy.Subscriber('peopleSet', peopleTrackingArray, self.callback)
        self.tf = TransformListener()
        self.person_id = -1

#    def _goal_feedback_cb(self, feedback):
#        sys.stdout.write('feedback callback called')
#        sys.stdout.flush()
   
    def callback(self,data):
        sys.stdout.write('people callback called!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n')
        sys.stdout.flush()
#        for i in range(0,len(peopleSet.peopleSet)):
#           if peopleSet.peopleSet[i].target_id == self.person_id:
#                target_frame = "/base_link"
#                source_fame  = peopleSet.header.frame_id
#                time         = peopleSet.header.stamp
#                if self.tf.canTransform(target_frame, source_frame, time):
#                    pose_msg = PoseStamped()
#                    pose_msg.header = peopleSet.header
#                    pose_msg.pose.position.x = peopleSet.peopleSet.x
#                    pose_msg.pose.position.y = peopleSet.peopleSet.y
#                    self.person_pose=self.tf.TransformPose(target_frame, pose_msg)
#                break

    def execute(self,userdata):
        self.person_id = userdata.person_id
        return smach_ros.SimpleActionState.execute(self,userdata)
        
        
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] = ""
  tts_goal.sequence_file[3] = userdata.left_in
  tts_goal.sequence_file[4] = userdata.right_in
  return tts_goal

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

    # Create a SMACH state machine
    sm = smach.StateMachine(['succeeded','aborted','preempted'])
    sm.userdata.valid = 0 
    sm.userdata.init_count = 0 
    sm.userdata.detect_count = 0
    sm.userdata.ask_count = 0
    # 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

    # Add states to the State Machine
    with sm:
        sm.userdata.sm_hri_tts_exit  = "El experimento ha acabado. Gracias por tu ayuda, ha sido un placer. Adios!"
        sm.userdata.sm_hri_left_exit = "left_arm_farewell.xml"
        sm.userdata.sm_hri_right_exit = "right_arm_farewell.xml"
        sm.userdata.sm_tracker_person_id=1;
        smach.StateMachine.add('HRI_EXIT',
                               SimpleActionWithFeedbackState('hri',
                                                             sequenceAction,
                                                             goal_cb=hri_goal_cb,
                                                             input_keys=['tts_in','left_in','right_in','person_id']),
                               transitions={'succeeded':'succeeded'},
                               remapping={'tts_in':'sm_hri_tts_exit',
                                          'left_in':'sm_hri_left_exit',
                                          'right_in':'sm_hri_right_exit',
                                          'person_id':'sm_tracker_person_id'})
                              
    # 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()