def recognizer():
    pub = rospy.Publisher('/recognizer/output',String, queue_size=10)
    rospy.init_node('HRI_Recognizer')
    msg = String()
    s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    s.bind(("127.0.0.1", 0))
    port = s.getsockname()[1]
    s.listen(1)


    p = multiprocessing.Process(target=callJulius, args=(port,))
    p.start()
    conn, addr = s.accept()

    while not rospy.is_shutdown():
        data = conn.recv(4096)
        if data.find("sentence1") >= 0:
            data=data[data.find("sentence1")+15:data.find(" </s>")]
            if data == "GO A HEAD":
                data = "GO AHEAD"
            elif data == "GOLEFT":
                data = "GO LEFT"
            elif data == "TAKEOFF":
                data = "TAKE OFF"
            msg.data = data
            pub.publish(msg)

    s.close()
    p.close()

    def execute(self, userdata):


        tosay = String()
        myint = Int32()
        tosay.data = "Bonne nuit."
        self.french_pub.publish(tosay)
        rospy.sleep(0.01)

        myint.data = 1
        self.brightness1_pub.publish(myint)
        rospy.sleep(0.01)
        myint.data = 6
        self.brightness2_pub.publish(myint)
        rospy.sleep(0.01)
        myint.data = 6
        self.brightness3_pub.publish(myint)
        rospy.sleep(0.3)

        # Shutdown everything
        self.OFF3_pub.publish(Empty())
        rospy.sleep(0.01)
        self.OFF2_pub.publish(Empty())
        rospy.sleep(0.01)
        self.OFF1_pub.publish(Empty())
        rospy.sleep(0.01)
        self.heatOFF_pub.publish(Empty())
        rospy.sleep(0.01)

        return 'succeeded'

def callback_thread(data,time):
   global res
   global test_var
   global thread1
   if checker == "true":
      if data.data != "SWITCH":
         result = data.data
         if result == "COMEBACK":
            result="COME BACK"
         elif result == "TAKEPICTURE": 
            result = "TAKE PICTURE"
         elif result == "SCANFOREST":
            result = "SCAN FOREST"
         elif result == "SCANAREA":
            result = "SCAN AREA"
         elif result == "TAKEOFF":
            result="TAKE OFF"
         elif result == "NEXTTO":
            result="NEXT"
         print "hello"
         window.insert(INSERT,'Genius:  ','hcolor')
         window.insert(END,result.upper()+'\n','hnbcolor')
         string = String()
         string.data = result.upper()
         result = result.lower()
         res = result
         thread1 = res
         thread.start_new_thread(sleeping_time, (res,5,))
         thread.start_new_thread(compare_thread, (res,1,))

    def test_unsubscribe_does_not_receive_further_msgs(self):
        topic = "/test_unsubscribe_does_not_receive_further_msgs"
        msg_type = "std_msgs/String"
        client = "client_test_unsubscribe_does_not_receive_further_msgs"

        msg = String()
        msg.data = "dsajfadsufasdjf"

        pub = rospy.Publisher(topic, String)
        multi = MultiSubscriber(topic, msg_type)

        received = {"count": 0}

        def cb(msg):
            received["count"] = received["count"] + 1

        multi.subscribe(client, cb)
        sleep(0.5)
        pub.publish(msg)
        sleep(0.5)
        self.assertEqual(received["count"], 1)
        multi.unsubscribe(client)
        sleep(0.5)
        pub.publish(msg)
        sleep(0.5)
        self.assertEqual(received["count"], 1)

 def final_result(self, hyp, uttid):
     """ Insert the final result. """
     rospy.loginfo("final_result publish!!!!")
     msg = String()
     msg.data = str(hyp.lower())
     rospy.loginfo(msg.data)
     self.pub.publish(msg)

    def test_immediate_publish(self):
        """ This test makes sure the PublisherConsistencyListener is working"""
        topic = "/test_immediate_publish"
        msg_class = String

        msg = String()
        string = "why halo thar"
        msg.data = string

        received = {"msg": None}
        def callback(msg):
            print "Received a msg! ", msg
            received["msg"] = msg

        rospy.Subscriber(topic, msg_class, callback)

        class temp_listener(rospy.SubscribeListener):
            def peer_subscribe(self, topic_name, topic_publish, peer_publish):
                print "peer subscribe in temp listener"

        listener = PublisherConsistencyListener()
        publisher = rospy.Publisher(topic, msg_class, temp_listener())
        listener.attach(publisher)
        publisher.publish(msg)
        sleep(0.5)

        self.assertEqual(received["msg"], msg)

 def test_cmd_handler_right(self):
     """Checks that person rotates clockwise when given right msg."""
     msg = String()
     msg.data = "right"
     self.person.cmd_handler(msg)
     self.assertTrue(self.person.rotation_executing)
     self.assertEqual(self.person.velocity.angular.z, -0.5)

    def _send_heat(self, heat):
	h = Int16()
        h.data = heat
	self._publisherth.publish(h)
	str = String()
        str.data = "setheat"
        self._publisherauto.publish(str)

    def execute(self, ud):

        initial_pose = PoseWithCovarianceStamped()
        initial_pose.header.frame_id = 'map'
        initial_pose.pose.pose.position.x = 0.0
        initial_pose.pose.pose.position.y = 0.0
        initial_pose.pose.pose.orientation.x = 0.0
        initial_pose.pose.pose.orientation.y = 0.0
        initial_pose.pose.pose.orientation.z = 0.0
        initial_pose.pose.pose.orientation.w = 1.0

        self.amcl_initial_pose_pub.publish(initial_pose)

        neck_cmd = Float64()
        neck_cmd.data = -2.0
        self.neck_pub.publish(neck_cmd)

        rospy.sleep(rospy.Duration(2))
        neck_cmd.data = -0.7
        self.neck_pub.publish(neck_cmd)

        rospy.sleep(rospy.Duration(4))

        tts_msg = String()
        tts_msg.data = "I am ready to begin the inspection."
        self.tts_pub.publish(tts_msg)

        return 'init_done'

    def auto_callback(self, data):
	#self._widget.textEdit.append('autocall')  
	str = String() 
        if data.data == "detectC":
	   self._widget.victim_label.setText('Found Hole')
           self._widget.current_z_angular_label.setEnabled(True)
	   self._widget.label.setEnabled(True)
	  # self._widget.groupBox.setEnabled(True)
	   self._widget.comfirm_button.setEnabled(True)
	   self._widget.approach_victim_button.setEnabled(True)
	   self._widget.ignor_button.setEnabled(True)
           str.data = "stop"
           self._publishersys.publish(str)
   	if data.data == "detectT":
	   self._widget.victim_label.setText('Found Temp')
           self._widget.current_z_angular_label.setEnabled(True)
	   self._widget.label.setEnabled(True)
	  # self._widget.groupBox.setEnabled(True)
	   self._widget.comfirm_button.setEnabled(True)
	   self._widget.approach_victim_button.setEnabled(True)
	   self._widget.ignor_button.setEnabled(True)
	if data.data == "detectQ":
	   self._widget.victim_label.setText('QR Found')
           self._widget.current_z_angular_label.setEnabled(True)
	   self._widget.label.setEnabled(True)

    def	_on_check_sensor_pressed(self):
	str = String()
	str.data = "check_sensor"
	self._publishersys.publish(str)
	self._widget.victim_label.setText('Check SS')
	self._widget.CO2.setText('0')
	self._widget.current_z_angular_label.setText('0')

def main(argv=sys.argv[1:]):
    parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
    parser.add_argument(
        '--reliable', dest='reliable', action='store_true',
        help='set qos profile to reliable')
    parser.set_defaults(reliable=False)
    parser.add_argument(
        '-n', '--number_of_cycles', type=int, default=20,
        help='number of sending attempts')
    args = parser.parse_args(argv)
    rclpy.init()

    if args.reliable:
        custom_qos_profile = qos_profile_default
        print('Reliable publisher')
    else:
        custom_qos_profile = qos_profile_sensor_data
        print('Best effort publisher')

    node = rclpy.create_node('talker_qos')

    chatter_pub = node.create_publisher(String, 'chatter_qos', custom_qos_profile)

    msg = String()

    cycle_count = 0
    while rclpy.ok() and cycle_count < args.number_of_cycles:
        msg.data = 'Hello World: {0}'.format(cycle_count)
        print('Publishing: "{0}"'.format(msg.data))
        chatter_pub.publish(msg)
        cycle_count += 1
        sleep(1)

    def createTopLevel(self, stimuliFile, numReps):
        """Creates the top level directory, randomizes input list"""
        self.sessionID = str(rospy.Time.now())
        self.topleveldir = os.environ["HOME"] + "/UltraspeechData/" + str(datetime.date.today()) + "_" + self.sessionID

        os.makedirs(self.topleveldir)
        monocamdir = self.topleveldir + "/monocam"
        os.makedirs(monocamdir)
        stereorightdir = self.topleveldir + "/stereoright"
        os.makedirs(stereorightdir)
        stereoleftdir = self.topleveldir + "/stereoleft"
        os.makedirs(stereoleftdir)
        ultrasounddir = self.topleveldir + "/ultrasound"
        os.makedirs(ultrasounddir)

        stimuli = open(stimuliFile, "r").readlines()
        random.shuffle(stimuli)

        self.stimuliList = []
        for i in range(numReps):
            for j in stimuli:
                if j != "\n":
                    self.stimuliList.append(j[:-1])

        self.numReps = numReps
        self.numBatches = int(math.ceil(float(len(self.stimuliList)) / 10.0))
        self.console.set_text("Press Start to begin")

        for i in range(2):
            # this tells the logger where to create the logfile
            msg = String()
            msg.data = str(self.topleveldir)
            self.startupTopic.publish(msg)
            time.sleep(1)  # for some reason the first one is not published

    def publish_haptic_control(self, ctrl):
        # publish haptic feedback
        haptic_msg = String()
        haptic_msg.data = "{:d},{:d}".format(int(ctrl[0]), int(ctrl[1]))
        print haptic_msg.data, "\r"

        self.haptic_msg_pub.publish(haptic_msg)

    def default(self, ci='unused'):
        """ Publish the data of the semantic camera as a ROS String message that contains a lisp-list.

        This function was designed for the use with CRAM and the Adapto group
        """
        string = String()
        string.data = "("
        for obj in self.data['visible_objects']:
            # if object has no description, set to '-'
            if obj['description'] == '':
                description = '-'

            # send tf-frame for every object
            self.sendTransform(self, obj['position'], obj['orientation'], \
                               rospy.Time.now(), str(obj['name']), "/map")

            # Build string from name, description, location and orientation in the global world frame
            string.data += "(" + str(obj['name']) + " " + description + " " + \
                           str(obj['position'].x) + " " + \
                           str(obj['position'].y) + " " + \
                           str(obj['position'].z) + " " + \
                           str(obj['orientation'].x) + " " + \
                           str(obj['orientation'].y) + " " + \
                           str(obj['orientation'].z) + " " + \
                           str(obj['orientation'].w) + ")"

        string.data += ")"
        self.publish(string)

    def run(self):
        while not rospy.is_shutdown():
            msg = String()
            msg.data = 'yeah'
            self.pub_test.publish(msg)

            # update states

            # option 1: direct
            
            # option 2: hybrid

            if self.enabled:
                gain_vel = 0.1
                gain_rot = 0.5
                self.cmdtwist.vel *= gain_vel
                self.cmdtwist.rot *= gain_rot
                # self.cmdfrm.Integrate(self.cmdtwist, self.freq)
                tmptwist = self.cmdfrm.M.Inverse(self.cmdtwist)
                self.cmdfrm.Integrate(tmptwist, self.freq)

                # hybrid
                self.ctrl_hybrid()
                
                cmdtrans = (self.cmdfrm.p.x(), self.cmdfrm.p.y(), self.cmdfrm.p.z())
                cmdrot = self.cmdfrm.M.GetQuaternion()
                self.ber.sendTransform(cmdtrans,
                                       cmdrot,
                                       rospy.Time.now(),
                                       "wam/cmd",
                                       "wam/base_link")
            
            # print cmdTrans
            self.rate.sleep()

 def final_result(self, hyp, uttid):
     """ Insert the final result. """
     msg = String()
     msg.data = str(hyp.lower())
     #print '****************************faceCommander node: %s'%msg.data
     rospy.loginfo("<Speech> faceCommander: %s", msg.data)
     self.pub.publish(msg)

def publish_users():
    # build list of current users online
    active_user, mod_time = get_active_user()
    online_users = []
    
    # if users == {} and active_user and time.time() - mod_time > INACTIVITY_LOGOUT:
    # we should remove the active user
    # remove_active_user()
    # active_user = ""
    
    for user in users.keys():
        last_ping = users[user]
        if user == active_user:
            # the active user file may have been updated before presence was published
            last_ping = max(last_ping, mod_time)
            rospy.logdebug("active user %s being considered; now: %f, last_ping: %f, mod_time: %f" % (user, time.time(), last_ping, mod_time))
        if time.time() - last_ping < INACTIVITY_LOGOUT:
            online_users.append(user)
        else:
            if user == active_user:
                # we should remove the active user
                remove_active_user()
                active_user = ""
            
    online_users.sort()
    out = String()
    out.data = "%s:%s" % (','.join(online_users), active_user)
    if active_user and online_users != []:
        publisher.publish(out)
        rospy.logdebug("publishing online users: %s" % out)

    def cbLabels(self, ros_data):
	labels = ros_data.labels
	if labels is None or self.filename is None:
	    return
	
	messages = []
	idx = 0
	for kType in self.knowledgeTypes:
	    #affList = affList + self.affDictionary[label.rsplit(None,1)[-1]] + ','
	    message = [] 
	    for label in labels:
		msg = String()
		if label.data in self.dictionaries[idx]:
		  msg.data = self.dictionaries[idx][label.data]
		  message.append(msg)
		else:
		  print "Object " + label.data + " not found in knowledge source"
	    idx += 1
	    arrMsg = ObjectKnowledgeArray()
	    arrMsg.data = message
	    messages.append(arrMsg)
        kmsg = ObjectKnowledge()
        kmsg.knowledge = messages
        kmsg.labels = labels
	self.knowPub.publish(kmsg)

    def execute(self, ud):

        tts_msg = String()
        tts_msg.data = 'Do you want me to continue following you?'
        self.tts_pub.publish(tts_msg)

        self.question_asked = True

        return_param = ''

        while True:
            self.mutex.acquire()
            if self.confirmation_received:
                if self.continue_following:
                    tts_msg.data = "I will continue following you."
                    self.tts_pub.publish(tts_msg)
                    try:
                        self.people_follower_srv.call(request=peopleFollowerRequest.START_FOLLOWING)
                    except rospy.ServiceException:
                        pass
                    return_param = 'continue_following'
                else:
                    tts_msg.data = "I will stop following you."
                    self.tts_pub.publish(tts_msg)
                    return_param = 'ask_guiding'

                self.mutex.release()
                break
            self.mutex.release()

        return return_param