human_assisted_object_detection.py

Go to the documentation of this file.

#!/usr/bin/env python

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

from iri_human_assisted_object_detection.srv import haod_init,haod_detect,haod_learn
from sensor_msgs.msg import Joy
from wiimote import *
from smach_ros import ServiceState
import time
import threading

def init_cb(userdata, response):
    if response.success == True:
      return 'succeeded'
    else:
      time.sleep(1);
      return 'aborted'

def assistance_needed_cb(userdata,response):
    if response.success == False:
      return 'succeeded'
    else:
      return 'aborted'

@smach.cb_interface(input_keys=['valid_det'])
def learn_cb(userdata,request):
    request.valid = userdata.valid_det

class WaitUserInput(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['valid_detection', 'invalid_detection', 'waiting', 'no_user_input'],output_keys=['valid_det'])

        self.mutex = threading.Lock()
        self.user_input = False
        self.user_answer = -1
        self.waiting_user_input = False
        self.count = 0

        self.subscriber = rospy.Subscriber('joy', Joy, self.callback)

    def callback(self, data):
        self.mutex.acquire()
        if self.waiting_user_input == True:
          if data.buttons[0] == 1:# the 1 button is pressed
            self.user_input = True 
            self.user_answer = 1
          else:
            if data.buttons[1] == 1: #the 2 button is pressed
              self.user_input = True 
              self.user_answer = 0
        self.mutex.release()

    def execute(self,userdata):
        #wait for a maximum of 5 seconds 
        self.mutex.acquire()
        self.waiting_user_input = True
        if self.user_input == True:
            if self.user_answer == 1:
                self.waiting_user_input = False 
                userdata.valid_det = True
                self.user_input = False;
                self.user_answer = -1;
                self.count = 0
                self.mutex.release()
                return 'valid_detection'
            else:
                self.waiting_user_input = False 
                userdata.valid_det = False
                self.user_input = False;
                self.user_answer = -1;
                self.count = 0
                self.mutex.release()
                return 'invalid_detection'
        self.count = self.count + 1
        self.mutex.release()
    
        time.sleep(.1)
        if self.count == 50:
            self.waiting_user_input = False 
            self.user_input = False;
            self.user_answer = -1;
            self.count = 0
            return 'no_user_input'
        else:
            return 'waiting'

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

    # Create a SMACH state machine
    sm = smach.StateMachine(['succeeded','aborted','preempted'])
    sm.userdata.valid = 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:
        smach.StateMachine.add('HAOD_INIT',
                               smach_ros.ServiceState('init',
                                                      haod_init,
                                                      response_cb=init_cb),
                               transitions={'succeeded':'HAOD_DETECT',
                                            'aborted':'HAOD_INIT'})
                               
        smach.StateMachine.add('HAOD_DETECT',
                               smach_ros.ServiceState('detect',
                                                      haod_detect,
                                                      response_cb=assistance_needed_cb),
                               transitions={'succeeded':'HAOD_DETECT',
                                            'aborted':'HAOD_USER_INPUT'})

        smach.StateMachine.add('HAOD_USER_INPUT',WaitUserInput(),
                               transitions={'valid_detection':'HAOD_LEARN',
                                            'invalid_detection':'HAOD_LEARN',
                                            'waiting':'HAOD_USER_INPUT',
                                            'no_user_input':'HAOD_DETECT'},
                               remapping={'valid_det':'valid'})

        smach.StateMachine.add('HAOD_LEARN',
                               smach_ros.ServiceState('learn',
                                                      haod_learn,
                                                      request_cb=learn_cb),
                               transitions={'succeeded':'HAOD_DETECT',
                                            'aborted':'aborted'},
                               remapping={'valid_det':'valid'})
    # 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()