diff --git a/CMakeLists.txt b/CMakeLists.txt index d3c2f33..7c86dcc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(catkin REQUIRED COMPONENTS tf aruco message_generation + actionlib_msgs ) add_service_files( @@ -19,9 +20,16 @@ add_service_files( InformController.srv ) -generate_messages(DEPENDENCIES std_msgs) +add_action_files( + DIRECTORY action + FILES + RequestSpeech.action + ) + +generate_messages(DEPENDENCIES actionlib_msgs std_msgs) + catkin_package( - CATKIN_DEPENDS message_runtime + CATKIN_DEPENDS message_runtime actionlib_msgs INCLUDE_DIRS include ) diff --git a/action/RequestSpeech.action b/action/RequestSpeech.action new file mode 100644 index 0000000..279b6a8 --- /dev/null +++ b/action/RequestSpeech.action @@ -0,0 +1,5 @@ +string[] vocabulary +---- +string result +---- +bool success diff --git a/package.xml b/package.xml index 5cd2dc2..b900115 100644 --- a/package.xml +++ b/package.xml @@ -48,6 +48,7 @@ std_msgs aruco message_generation + actionlib_msgs cv_bridge image_transport roscpp @@ -56,6 +57,7 @@ std_msgs aruco message_runtime + actionlib_msgs diff --git a/script/calibrator.py b/script/calibrator.py new file mode 100755 index 0000000..98fe0c6 --- /dev/null +++ b/script/calibrator.py @@ -0,0 +1,11 @@ +#! /usr/bin/env python +import rospy + +from controller import inform_controller_factory + + +_inform_controller = inform_controller_factory('calibrator') + + +if __name__ == '__main__': + rospy.init_node('teleoperation/calibrator') diff --git a/script/controller.py b/script/controller.py index c07074c..e4583d9 100755 --- a/script/controller.py +++ b/script/controller.py @@ -73,6 +73,6 @@ def handle_request(r): if __name__ == '__main__': - rospy.init_node('controller', log_level=rospy.INFO) + rospy.init_node('teleoperation/controller', log_level=rospy.INFO) ic = rospy.Service('inform_controller', InformController, handle_request) rospy.spin() diff --git a/script/speech.py b/script/speech.py new file mode 100755 index 0000000..6866cb1 --- /dev/null +++ b/script/speech.py @@ -0,0 +1,81 @@ +#! /usr/bin/env python +import os + +import rospy +import actionlib +from naoqi import ALProxy, ALModule, ALBroker + + +speech_broker = None +almem = None +sas = actionlib.SimpleActionServer() + + +def request_speech(goal): + recognized = '' + if not speech_detector.start_speech(goal): + sas.publish_feedback(False) + return + while not sas.is_preempt_requested() and not speech_detector.have_word(): + rospy.Rate(10).sleep() + speech_detector.stop_speech(goal) + if speech_detector.have_word(): + recognized = speech_detector.get_recognized_and_erase() + sas.publish_feedback(True) + sas.set_succeeded(recognized) + + +class SpeechDetectorModule(ALModule): + + def __init__(self, name): + ALModule.__init__(self, name) + self.recognized = None + self.subid = 'teleoperation_speech' + self.asr = ALProxy('ALSpeechRecognition') + self.asr.setLanguage('English') + self.stop_speech() + + def start_speech(self, voc): + if self.running: + return False + # self.stop_speech() + self.asr.setVocabulary(voc, False) + self.asr.subscribe(self.subid) + self.asr.pause(False) + self.running = True + return True + + def have_word(self): + return self.recognized is not None + + def get_recognized_and_erase(self): + result = self.recognized + self.recognized = None + return result + + def stop_speech(self): + self.asr.pause(True) + self.asr.unsubscribe(self.subid) + self.running = False + + def on_word_recognized(self, *_args): + rospy.loginfo(almem.getData('WordRecognized')) + + +if __name__ == '__main__': + rospy.init_node('speech') + speech_broker = ALBroker('speech_broker', '0.0.0.0', 0, + os.environ['NAO_IP'], 9559) + speech_detector = SpeechDetectorModule('speech_detector') + almem = ALProxy('ALMemory') + almem.subscribeToEvent("WordRecognized", + "speech_detector", + "on_word_recognized") + + speech_detector.start_speech(['start', 'stop', 'pause']) + + while not rospy.is_shutdown(): + rospy.Rate(4).sleep() + + speech_detector.stop_speech() + speech_broker.shutdown() diff --git a/script/walker.py b/script/walker.py index 5582cdf..60c6ab3 100755 --- a/script/walker.py +++ b/script/walker.py @@ -37,7 +37,7 @@ def _speed(pos, interval): if __name__ == '__main__': - rospy.init_node('walker') + rospy.init_node('teleoperation/walker') rospy.wait_for_service('inform_controller') ll = tf.TransformListener() mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)