diff --git a/.gitignore b/.gitignore index e5c2e6d5..882e411f 100644 --- a/.gitignore +++ b/.gitignore @@ -122,3 +122,5 @@ harmoni_actuators/harmoni_tts/content/* doc/_build harmoni_actuators/harmoni_tts/scale_stats.npy harmoni_actuators/harmoni_tts/temp_data/tts.wav +harmoni_detectors/harmoni_imageai/src/*.h5 +harmoni_detectors/harmoni_imageai/src/*.json diff --git a/dockerfiles/harmoni/noetic/face_detect/dockerfile b/dockerfiles/harmoni/noetic/face_detect/dockerfile index 9de93f69..172caa7d 100644 --- a/dockerfiles/harmoni/noetic/face_detect/dockerfile +++ b/dockerfiles/harmoni/noetic/face_detect/dockerfile @@ -8,4 +8,4 @@ RUN \ facenet-pytorch \ Pillow -CMD ["/bin/bash"] +CMD ["/bin/bash"] \ No newline at end of file diff --git a/dockerfiles/harmoni/noetic/full/dockerfile b/dockerfiles/harmoni/noetic/full/dockerfile index 3633ebb6..f27ec802 100644 --- a/dockerfiles/harmoni/noetic/full/dockerfile +++ b/dockerfiles/harmoni/noetic/full/dockerfile @@ -29,10 +29,10 @@ RUN mkdir -p $ROS_WS/src WORKDIR $ROS_WS #caching protection for Harmoni, per https://stackoverflow.com/a/39278224/5715374 -ADD https://api.github.com/repos/interaction-lab/HARMONI/git/refs/heads/develop /root/version.json +#ADD https://api.github.com/repos/micolspitale93/git/refs/heads/develop /root/version.json RUN git -C src clone \ -b develop \ - https://github.com/interaction-lab/HARMONI.git + https://github.com/micolspitale93/HARMONI.git # ================================================================== # Build Harmoni from source diff --git a/harmoni_actuators/harmoni_face/setup.py b/harmoni_actuators/harmoni_face/setup.py index 9561d454..30e46f9e 100755 --- a/harmoni_actuators/harmoni_face/setup.py +++ b/harmoni_actuators/harmoni_face/setup.py @@ -5,7 +5,8 @@ # fetch values from package.xml setup_args = generate_distutils_setup( - packages=["harmoni_face"], package_dir={"": "src"}, + packages=["harmoni_face"], + package_dir={"": "src"}, ) setup(**setup_args) diff --git a/harmoni_actuators/harmoni_face/src/harmoni_face/face_client.py b/harmoni_actuators/harmoni_face/src/harmoni_face/face_client.py index d70e74d3..725dd3a0 100644 --- a/harmoni_actuators/harmoni_face/src/harmoni_face/face_client.py +++ b/harmoni_actuators/harmoni_face/src/harmoni_face/face_client.py @@ -44,7 +44,6 @@ def setup_ros(self): queue_size=1, ) rospy.loginfo("Checking that face is connected to ROS websocket") - print(self.name + "/is_connected") rospy.wait_for_service(self.name + "/is_connected") rospy.loginfo("Done, face is connected to ROS websocket") self.connected=True diff --git a/harmoni_actuators/harmoni_face/src/harmoni_face/face_service.py b/harmoni_actuators/harmoni_face/src/harmoni_face/face_service.py index 7177f026..77bac38d 100755 --- a/harmoni_actuators/harmoni_face/src/harmoni_face/face_service.py +++ b/harmoni_actuators/harmoni_face/src/harmoni_face/face_service.py @@ -274,12 +274,10 @@ def _get_viseme_data(self, data): rospy.loginfo("The validated visemes are %s" % viseme_set) return viseme_bool, viseme_set - - def main(): """Set names, collect params, and give service to server""" service_name = ActuatorNameSpace.face.name - instance_id = rospy.get_param("/instance_id") + instance_id = rospy.get_param("/instance_id") #default service_id_mouth = f"{service_name}_mouth_{instance_id}" service_id_eyes = f"{service_name}_eyes_{instance_id}" service_id_nose = f"{service_name}_nose_{instance_id}" @@ -296,13 +294,21 @@ def main(): service_server_eyes = HarmoniServiceServer(service_id_eyes, s_eyes) service_server_mouth = HarmoniServiceServer(service_id_mouth, s_mouth) service_server_nose = HarmoniServiceServer(service_id_nose, s_nose) + + print(service_name) + print("****************************************************************************") + print(service_id_eyes) + print(service_id_mouth) + print(service_id_nose) + service_server_eyes.start_sending_feedback() service_server_mouth.start_sending_feedback() service_server_nose.start_sending_feedback() + rospy.spin() except rospy.ROSInterruptException: pass if __name__ == "__main__": - main() + main() \ No newline at end of file diff --git a/harmoni_actuators/harmoni_face/test/face.test b/harmoni_actuators/harmoni_face/test/face.test index b2661863..ab1646c8 100644 --- a/harmoni_actuators/harmoni_face/test/face.test +++ b/harmoni_actuators/harmoni_face/test/face.test @@ -7,7 +7,7 @@ - eyes + diff --git a/harmoni_actuators/harmoni_face/test/rostest_face.py b/harmoni_actuators/harmoni_face/test/rostest_face.py index 7b2dbdcc..e1aad8be 100755 --- a/harmoni_actuators/harmoni_face/test/rostest_face.py +++ b/harmoni_actuators/harmoni_face/test/rostest_face.py @@ -140,4 +140,4 @@ def main(): if __name__ == "__main__": - main() + main() \ No newline at end of file diff --git a/harmoni_actuators/harmoni_gesture/launch/gesture_service.launch b/harmoni_actuators/harmoni_gesture/launch/gesture_service.launch index 47bd49e5..6313c861 100755 --- a/harmoni_actuators/harmoni_gesture/launch/gesture_service.launch +++ b/harmoni_actuators/harmoni_gesture/launch/gesture_service.launch @@ -1,6 +1,6 @@ - + diff --git a/harmoni_actuators/harmoni_gesture/nodes/gesture_service.py b/harmoni_actuators/harmoni_gesture/nodes/gesture_service.py index b959587e..b2d0dd87 100755 --- a/harmoni_actuators/harmoni_gesture/nodes/gesture_service.py +++ b/harmoni_actuators/harmoni_gesture/nodes/gesture_service.py @@ -46,6 +46,7 @@ def __init__(self, name, param): ) self.state = State.INIT self.setup_gesture() + return def _gesture_done_callback(self, data): @@ -219,7 +220,6 @@ def _get_gesture_data(self, data): self.gesture_pub.publish(str(data)) return True - def main(): service_name = ActuatorNameSpace.gesture.name instance_id = rospy.get_param("/instance_id") diff --git a/harmoni_actuators/harmoni_gesture/test/gesture.test b/harmoni_actuators/harmoni_gesture/test/gesture.test index cf7b543a..5ec313e3 100644 --- a/harmoni_actuators/harmoni_gesture/test/gesture.test +++ b/harmoni_actuators/harmoni_gesture/test/gesture.test @@ -17,4 +17,4 @@ - + \ No newline at end of file diff --git a/harmoni_actuators/harmoni_speaker/launch/speaker_service.launch b/harmoni_actuators/harmoni_speaker/launch/speaker_service.launch index c86631bd..43312f3a 100755 --- a/harmoni_actuators/harmoni_speaker/launch/speaker_service.launch +++ b/harmoni_actuators/harmoni_speaker/launch/speaker_service.launch @@ -5,9 +5,4 @@ - - - - - diff --git a/harmoni_actuators/harmoni_speaker/nodes/__init__.py b/harmoni_actuators/harmoni_speaker/nodes/__init__.py new file mode 100755 index 00000000..e69de29b diff --git a/harmoni_actuators/harmoni_speaker/nodes/speaker_service.py b/harmoni_actuators/harmoni_speaker/src/harmoni_speaker/speaker_service.py similarity index 93% rename from harmoni_actuators/harmoni_speaker/nodes/speaker_service.py rename to harmoni_actuators/harmoni_speaker/src/harmoni_speaker/speaker_service.py index 5ed81c24..b553587c 100755 --- a/harmoni_actuators/harmoni_speaker/nodes/speaker_service.py +++ b/harmoni_actuators/harmoni_speaker/src/harmoni_speaker/speaker_service.py @@ -19,6 +19,7 @@ import wave import os +AUDIO_DELAY = 0.5 # this constant is used to make shorter the duration in which the service is sleeping. class SpeakerService(HarmoniServiceManager): """Takes sound and publishes it to the default audio topic for the audio_play package @@ -57,7 +58,7 @@ def do(self, data): if type(data) == str: if ".wav" in data: data = self.file_path_to_audio_data(data) - duration = data["duration"] + duration = data["duration"] - AUDIO_DELAY else: data = ast.literal_eval(data) data = data["audio_data"] @@ -106,7 +107,6 @@ def file_path_to_audio_data(self, path): os.remove(file_handle) return {"audio_data": data, "duration": duration} - def main(): """Set names, collect params, and give service to server""" @@ -123,6 +123,10 @@ def main(): service_server = HarmoniServiceServer(service_id, s) + print(service_name) + print("****************************************************************************") + print(service_id) + service_server.start_sending_feedback() rospy.spin() except rospy.ROSInterruptException: diff --git a/harmoni_actuators/harmoni_speaker/test/speaker.test b/harmoni_actuators/harmoni_speaker/test/speaker.test old mode 100644 new mode 100755 diff --git a/harmoni_actuators/harmoni_speaker/test/speaker_qt.test b/harmoni_actuators/harmoni_speaker/test/speaker_qt.test new file mode 100755 index 00000000..4ad6a6ce --- /dev/null +++ b/harmoni_actuators/harmoni_speaker/test/speaker_qt.test @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/harmoni_actuators/harmoni_speaker/test/unittest_speaker.py b/harmoni_actuators/harmoni_speaker/test/unittest_speaker.py old mode 100644 new mode 100755 diff --git a/harmoni_actuators/harmoni_tts/CMakeLists.txt b/harmoni_actuators/harmoni_tts/CMakeLists.txt old mode 100644 new mode 100755 diff --git a/harmoni_actuators/harmoni_tts/README.md b/harmoni_actuators/harmoni_tts/README.md old mode 100644 new mode 100755 diff --git a/harmoni_actuators/harmoni_tts/config/configuration.yaml b/harmoni_actuators/harmoni_tts/config/configuration.yaml old mode 100644 new mode 100755 index d86c8964..3eda27f8 --- a/harmoni_actuators/harmoni_tts/config/configuration.yaml +++ b/harmoni_actuators/harmoni_tts/config/configuration.yaml @@ -2,7 +2,7 @@ tts: default_param: region_name: "us-west-2" - language: "en-US" + language: "it-IT" outdir: "$(find harmoni_tts)/temp_data" wav_header_length: 24 - voice: "Ivy" + voice: "Bianca" diff --git a/harmoni_actuators/harmoni_tts/launch/tts_polly_service.launch b/harmoni_actuators/harmoni_tts/launch/tts_polly_service.launch old mode 100644 new mode 100755 diff --git a/harmoni_actuators/harmoni_tts/launch/tts_service.launch b/harmoni_actuators/harmoni_tts/launch/tts_service.launch old mode 100644 new mode 100755 diff --git a/harmoni_actuators/harmoni_tts/package.xml b/harmoni_actuators/harmoni_tts/package.xml old mode 100644 new mode 100755 diff --git a/harmoni_actuators/harmoni_tts/setup.py b/harmoni_actuators/harmoni_tts/setup.py old mode 100644 new mode 100755 diff --git a/harmoni_actuators/harmoni_tts/src/harmoni_tts/__init__.py b/harmoni_actuators/harmoni_tts/src/harmoni_tts/__init__.py old mode 100644 new mode 100755 diff --git a/harmoni_actuators/harmoni_tts/nodes/aws_tts_service.py b/harmoni_actuators/harmoni_tts/src/harmoni_tts/aws_tts_service.py similarity index 97% rename from harmoni_actuators/harmoni_tts/nodes/aws_tts_service.py rename to harmoni_actuators/harmoni_tts/src/harmoni_tts/aws_tts_service.py index 4f3077fc..7422b89e 100755 --- a/harmoni_actuators/harmoni_tts/nodes/aws_tts_service.py +++ b/harmoni_actuators/harmoni_tts/src/harmoni_tts/aws_tts_service.py @@ -54,6 +54,8 @@ def _setup_aws_tts(self): "S": "POSTALVEOLAR", "r": "POSTALVEOLAR", "k": "VELAR_GLOTTAL", + "J": "PALATAL_NASAL", + "L": "PALATAL_LATERAL_APPROXIMANT", "i": "CLOSE_FRONT_VOWEL", "u": "CLOSE_BACK_VOWEL", "@": "MID_CENTRAL_VOWEL", @@ -298,7 +300,6 @@ def request(self, input_text): self.result_msg = "" return {"response": self.state, "message": self.result_msg} - def main(): """[summary] Main function for starting HarmoniPolly service @@ -315,6 +316,10 @@ def main(): service_server = HarmoniServiceServer(service_id, s) + print(service_name) + print("****************************************************************************") + print(service_id) + service_server.start_sending_feedback() rospy.spin() except rospy.ROSInterruptException: @@ -322,4 +327,4 @@ def main(): if __name__ == "__main__": - main() + main() \ No newline at end of file diff --git a/harmoni_actuators/harmoni_tts/temp_data/tts.wav b/harmoni_actuators/harmoni_tts/temp_data/tts.wav old mode 100644 new mode 100755 index ff722f9b..3fe6641e Binary files a/harmoni_actuators/harmoni_tts/temp_data/tts.wav and b/harmoni_actuators/harmoni_tts/temp_data/tts.wav differ diff --git a/harmoni_actuators/harmoni_web/setup.py b/harmoni_actuators/harmoni_web/setup.py index f8f00572..bb4dbd31 100644 --- a/harmoni_actuators/harmoni_web/setup.py +++ b/harmoni_actuators/harmoni_web/setup.py @@ -5,8 +5,7 @@ # fetch values from package.xml setup_args = generate_distutils_setup( - packages=['harmoni_web'], - package_dir={'': 'src'}, + packages=['harmoni_web'], package_dir={'': 'src'}, ) setup(**setup_args) diff --git a/harmoni_actuators/harmoni_web/nodes/web_service.py b/harmoni_actuators/harmoni_web/src/harmoni_web/web_service.py similarity index 97% rename from harmoni_actuators/harmoni_web/nodes/web_service.py rename to harmoni_actuators/harmoni_web/src/harmoni_web/web_service.py index 656b7051..2eedf44a 100755 --- a/harmoni_actuators/harmoni_web/nodes/web_service.py +++ b/harmoni_actuators/harmoni_web/src/harmoni_web/web_service.py @@ -195,7 +195,6 @@ def _event_click_callback(self, event): self.result_msg = event.data return - def main(): """Set names, collect params, and give service to server""" service_name = ActuatorNameSpace.web.name @@ -206,6 +205,11 @@ def main(): # params = rospy.get_param(service_name + "/" + instance_id + "_param/") s = WebService(service_id) service_server = HarmoniServiceServer(service_id, s) + + print(service_name) + print("**********************************************************************************************") + print(service_id) + service_server.start_sending_feedback() rospy.spin() except rospy.ROSInterruptException: @@ -213,4 +217,4 @@ def main(): if __name__ == "__main__": - main() + main() \ No newline at end of file diff --git a/harmoni_actuators/harmoni_web/web/index.html b/harmoni_actuators/harmoni_web/web/index.html index 7108a258..b9f5c6e1 100644 --- a/harmoni_actuators/harmoni_web/web/index.html +++ b/harmoni_actuators/harmoni_web/web/index.html @@ -12,7 +12,7 @@ - + Harmoni Web diff --git a/harmoni_actuators/harmoni_web/web/src/config/config.json b/harmoni_actuators/harmoni_web/web/src/config/config.json index a7f00eef..0bf66ce3 100644 --- a/harmoni_actuators/harmoni_web/web/src/config/config.json +++ b/harmoni_actuators/harmoni_web/web/src/config/config.json @@ -363,7 +363,24 @@ } ] } - ] - } + ] + }, + { + "component": "container", + "id": "raccolta_container", + "children": [ + { + "component": "row", + "id": "row_only", + "children": [ + { + "component": "img", + "children": "", + "id": "img_only" + } + ] + } + ] + } ] } \ No newline at end of file diff --git a/harmoni_actuators/harmoni_web/web/src/css/activity_style.css b/harmoni_actuators/harmoni_web/web/src/css/activity_style.css new file mode 100644 index 00000000..c5b55308 --- /dev/null +++ b/harmoni_actuators/harmoni_web/web/src/css/activity_style.css @@ -0,0 +1,56 @@ +html, +body { + height: 100%; + margin: 0; + overflow: hidden; + font-family: "Palatino Linotype", "Book Antiqua", Palatino, serif; + text-align: center; + font-size: 2vmin; + background-color: white; +} + +.container{ + height: 100%; + width: 100%; + text-align: center; + display: flex; +} + +button{ + margin: auto; + text-align: center; +} + +div{ + margin: auto; + text-align: center; + +} + +img{ + + background-position: center; + background-repeat: no-repeat; + background-size: cover; + width: 100%; + height: 100%; +} + + +img:hover { + opacity: 0.5; + } + +.row, .col{ + margin: auto; + text-align: center; + width: 100%; + height: 100%; + display: contents; +} + +.title{ + margin: auto; + font-size: 3vmin; + text-align: center; +} \ No newline at end of file diff --git a/harmoni_actuators/harmoni_web/web/src/css/style.css b/harmoni_actuators/harmoni_web/web/src/css/style.css index db0077a4..1cf46d7f 100755 --- a/harmoni_actuators/harmoni_web/web/src/css/style.css +++ b/harmoni_actuators/harmoni_web/web/src/css/style.css @@ -8,7 +8,7 @@ body { font-family: "Palatino Linotype", "Book Antiqua", Palatino, serif; text-align: center; font-size: 2vmin; - background-color: green; + background-color: white; } .container{ @@ -33,8 +33,10 @@ div{ img{ margin: auto; text-align: center; + background-size: cover; vertical-align: auto; width: 100%; + height: 100%; } diff --git a/harmoni_core/harmoni_common_lib/src/harmoni_common_lib/action_server.py b/harmoni_core/harmoni_common_lib/src/harmoni_common_lib/action_server.py index 5310929a..d5f13183 100755 --- a/harmoni_core/harmoni_common_lib/src/harmoni_common_lib/action_server.py +++ b/harmoni_core/harmoni_common_lib/src/harmoni_common_lib/action_server.py @@ -178,7 +178,6 @@ def set_aborted(self, result=None, text=""): def publish_feedback(self, state): self._feedback.state = state rospy.logdebug("(Server) The feedback is " + str(self._feedback.state)) - self.current_goal.publish_feedback(self._feedback) def get_default_result(self): @@ -191,6 +190,7 @@ def set_preempted(self, result=None, text=""): result = self.get_default_result() with self.action_server.lock, self.lock: rospy.logdebug("Setting the current goal as canceled") + rospy.logdebug(self.name) self.current_goal.set_canceled(result, text) # @brief Allows users to register a callback to be invoked when a new goal is available diff --git a/harmoni_core/harmoni_common_lib/src/harmoni_common_lib/constants.py b/harmoni_core/harmoni_common_lib/src/harmoni_common_lib/constants.py index c915fa7a..6e3f94d1 100755 --- a/harmoni_core/harmoni_common_lib/src/harmoni_common_lib/constants.py +++ b/harmoni_core/harmoni_common_lib/src/harmoni_common_lib/constants.py @@ -53,7 +53,44 @@ class DialogueNameSpace(Enum): class DetectorNameSpace(Enum): stt = "/harmoni/detecting/stt/" face_detect = "/harmoni/detecting/face_detect/" - + card_detect = "/harmoni/detecting/card_detect/" + imageai_yolo = "/harmoni/detecting/imageai/yolo" + imageai_custom_yolo = "/harmoni/detecting/imageai/custom_yolo" + +class DialogStateLex(Enum): + CONFIRM_INTENT = "ConfirmIntent" + ELICIT_INTENT = "ElicitIntent" + ELICIT_SLOT = "ElicitSlot" + FAILED = "Failed" + FULFILLED = "Fulfilled" + READY_FOR_FULFILLMENT = "ReadyForFulfillment" + UNKNOWN_TO_SDK_VERSION = "UnknownToSdkVersion" + +#these class is used to store the names of the intent of the bot in amazon lex +class IntentName(Enum): + INTERACTION = "Background_noInterazione" # no interaction background + VISUAL = "Background_visivo" # visual background + CARTA = "Carta" # paper + RACCOLTA = "ConfirmRaccolta" # confirm recycling + PLASTICA = "Plastica" # plastic + STOP = "Stop" # stop + VETRO = "Vetro" # glass + OMBRELLO = "Ombrello" # umbrella + NOCAPITO = "NonHoCapito" # I can't undestand class Resources(Enum): face = ["eyes", "mouth", "nose"] + + +class PyTreeNameSpace(Enum): + scene = "scene" + visual = "visual" + interaction = "interaction" + mainactivity = "mainactivity" + timer = "timer" + invalid_response = "invalid_response" + analyzer = "analyzer" + trigger = "trigger" + buttons = "buttons" + + diff --git a/harmoni_core/harmoni_common_lib/src/harmoni_common_lib/service_server.py b/harmoni_core/harmoni_common_lib/src/harmoni_common_lib/service_server.py index cd487de3..4cca7aed 100755 --- a/harmoni_core/harmoni_common_lib/src/harmoni_common_lib/service_server.py +++ b/harmoni_core/harmoni_common_lib/src/harmoni_common_lib/service_server.py @@ -86,7 +86,9 @@ def _preempt_callback(self): """Used to signal a cancel/pause to the currently running service so that a new goal can be received. """ - self.service_manager.pause() + self.service_manager.stop() + + def _execute_goal_received_callback(self, goal): """Turns action goals into calls to the service manager. Is passed to the diff --git a/harmoni_core/harmoni_decision/CMakeLists.txt b/harmoni_core/harmoni_decision/CMakeLists.txt old mode 100644 new mode 100755 diff --git a/harmoni_core/harmoni_decision/config/configuration.yaml b/harmoni_core/harmoni_decision/config/configuration.yaml old mode 100644 new mode 100755 index 069a083b..fbffe49f --- a/harmoni_core/harmoni_decision/config/configuration.yaml +++ b/harmoni_core/harmoni_decision/config/configuration.yaml @@ -7,7 +7,7 @@ harmoni: #face_detect: ["default"] hardware: microphone: ["default"] - #camera: ["default"] + camera: ["object","person"] face: ["default"] - speaker: ["default"] - # gesture: ["default"] + speaker: ["voice_robot","external_sound"] + #gesture: ["default"] diff --git a/harmoni_core/harmoni_decision/launch/hardware_service.launch b/harmoni_core/harmoni_decision/launch/hardware_service.launch old mode 100644 new mode 100755 index 8a5f8a74..bbf4b3c4 --- a/harmoni_core/harmoni_decision/launch/hardware_service.launch +++ b/harmoni_core/harmoni_decision/launch/hardware_service.launch @@ -1 +1 @@ - \ No newline at end of file + \ No newline at end of file diff --git a/harmoni_core/harmoni_decision/launch/harmoni_decision.launch b/harmoni_core/harmoni_decision/launch/harmoni_decision.launch old mode 100644 new mode 100755 diff --git a/harmoni_core/harmoni_decision/launch/harmoni_service.launch b/harmoni_core/harmoni_decision/launch/harmoni_service.launch old mode 100644 new mode 100755 index 590c7cf5..267b2691 --- a/harmoni_core/harmoni_decision/launch/harmoni_service.launch +++ b/harmoni_core/harmoni_decision/launch/harmoni_service.launch @@ -1 +1 @@ - + \ No newline at end of file diff --git a/harmoni_core/harmoni_decision/launch/launcher.launch b/harmoni_core/harmoni_decision/launch/launcher.launch old mode 100644 new mode 100755 index 3fc44c31..f806510b --- a/harmoni_core/harmoni_decision/launch/launcher.launch +++ b/harmoni_core/harmoni_decision/launch/launcher.launch @@ -3,7 +3,7 @@ - + diff --git a/harmoni_core/harmoni_decision/launch/pytree_service.launch b/harmoni_core/harmoni_decision/launch/pytree_service.launch new file mode 100644 index 00000000..bfc374f3 --- /dev/null +++ b/harmoni_core/harmoni_decision/launch/pytree_service.launch @@ -0,0 +1 @@ + \ No newline at end of file diff --git a/harmoni_core/harmoni_decision/nodes/behavior_controller.py b/harmoni_core/harmoni_decision/nodes/behavior_controller.py old mode 100644 new mode 100755 diff --git a/harmoni_core/harmoni_decision/package.xml b/harmoni_core/harmoni_decision/package.xml old mode 100644 new mode 100755 diff --git a/harmoni_core/harmoni_decision/setup.py b/harmoni_core/harmoni_decision/setup.py old mode 100644 new mode 100755 diff --git a/harmoni_core/harmoni_pattern/CMakeLists.txt b/harmoni_core/harmoni_pattern/CMakeLists.txt old mode 100644 new mode 100755 index 08779c06..059d6102 --- a/harmoni_core/harmoni_pattern/CMakeLists.txt +++ b/harmoni_core/harmoni_pattern/CMakeLists.txt @@ -19,7 +19,7 @@ find_package(catkin REQUIRED COMPONENTS ## Uncomment this if the package has a setup.py. This macro ensures ## modules and global scripts declared therein get installed ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -#catkin_python_setup() +catkin_python_setup() ################################################ ## Declare ROS messages, services and actions ## diff --git a/harmoni_core/harmoni_pattern/config/configuration.yaml b/harmoni_core/harmoni_pattern/config/configuration.yaml old mode 100644 new mode 100755 index 52b6069a..5e4d76a9 --- a/harmoni_core/harmoni_pattern/config/configuration.yaml +++ b/harmoni_core/harmoni_pattern/config/configuration.yaml @@ -28,3 +28,8 @@ speak_test: default_param: trigger_intent: "Hey" pattern_scripting: $(find harmoni_pattern)/pattern_scripting/speak_test.json + +prova_tts_speaker: + default_param: + trigger_intent: "Hey" + pattern_scripting: $(find harmoni_pattern)/pattern_scripting/prova_tts_speaker.json diff --git a/harmoni_core/harmoni_pattern/launch/sequence_pattern.launch b/harmoni_core/harmoni_pattern/launch/sequence_pattern.launch old mode 100644 new mode 100755 diff --git a/harmoni_core/harmoni_pattern/package.xml b/harmoni_core/harmoni_pattern/package.xml old mode 100644 new mode 100755 diff --git a/harmoni_core/harmoni_pattern/pattern_scripting/prova_tts_speaker.json b/harmoni_core/harmoni_pattern/pattern_scripting/prova_tts_speaker.json new file mode 100644 index 00000000..3a1010e7 --- /dev/null +++ b/harmoni_core/harmoni_pattern/pattern_scripting/prova_tts_speaker.json @@ -0,0 +1,24 @@ +[ + { + "set": "sequence", + "steps": [ + { + "tts_default": { + "action_goal": "REQUEST", + "resource_type": "service", + "wait_for": "new", + "trigger": "ciao corrado e tharu" + } + }, + { + "speaker_default": { + "action_goal": "DO", + "resource_type": "actuator", + "wait_for": "", + "trigger": "ciao" + } + } + + ] + } +] \ No newline at end of file diff --git a/harmoni_core/harmoni_pattern/setup.py b/harmoni_core/harmoni_pattern/setup.py old mode 100644 new mode 100755 index e1cd3c49..191e0c49 --- a/harmoni_core/harmoni_pattern/setup.py +++ b/harmoni_core/harmoni_pattern/setup.py @@ -5,9 +5,8 @@ # fetch values from package.xml setup_args = generate_distutils_setup( - # scripts=[''], - # packages=['harmoni_pattern'], - # package_dir={'': 'src'}, + packages=["harmoni_pattern"], + package_dir={"": "src"}, ) setup(**setup_args) diff --git a/harmoni_core/harmoni_pattern/test/sequential.test b/harmoni_core/harmoni_pattern/test/sequential.test old mode 100644 new mode 100755 diff --git a/harmoni_core/harmoni_pattern/test/sequential_speak.test b/harmoni_core/harmoni_pattern/test/sequential_speak.test old mode 100644 new mode 100755 diff --git a/harmoni_core/harmoni_pytree/CMakeLists.txt b/harmoni_core/harmoni_pytree/CMakeLists.txt new file mode 100755 index 00000000..39a353ba --- /dev/null +++ b/harmoni_core/harmoni_pytree/CMakeLists.txt @@ -0,0 +1,200 @@ +cmake_minimum_required(VERSION 2.8.3) +project(harmoni_pytree) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + audio_common_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES harmoni_pytree +# CATKIN_DEPENDS roscpp rospy +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/harmoni_pytree.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/harmoni_pytree_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_harmoni_pytree.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +#catkin_add_nosetests(test/unittest_pytree.py) +catkin_add_nosetests(test) diff --git a/harmoni_core/harmoni_pytree/README.md b/harmoni_core/harmoni_pytree/README.md new file mode 100755 index 00000000..588ded10 --- /dev/null +++ b/harmoni_core/harmoni_pytree/README.md @@ -0,0 +1,71 @@ +# HARMONI Pytree + +PyTree is one among the several ways to implement behaviour trees. +A tree in pytree is made of two elements: nodes and leaves. +Nodes can be of different nature but in general they belong to the family of composites (sequence, parallel, selector). The management of these elements it’s not something that users have to do, everything has already been done by the creators of pytree. +Leaves come from a single element called “behaviour” and these are the components that users can create. Even if pytree made some basic behaviours available for us (you can find some of them [here](https://py-trees.readthedocs.io/en/devel/modules.html#module-py_trees.behaviours)), developers are required to create their own behaviours ([skeleton of behaviour](https://py-trees.readthedocs.io/en/devel/behaviours.html#skeleton)). We provide some leaves that represent the pytree version of clients of HARMONI. + +## Setup + +In order to use PyTree import the python library by running the following command: + +```bash +$ pip install py_trees +``` +**(optional)** If you want to create and see dot_tree you must install pydot and graphviz. +For pydot run: + +```bash +$ pip install pydot +``` + For graphicviz run: + +```bash +$ sudo apt install graphviz +``` +## Usage + +The execution of a tree is done by ticking it.
+A tick starts from the root and then is propagated down in the tree up to leaves. Leaves will perform their tasks and in the end compute a state that will be returned and propagated back until the root.
+As mentioned before a tree is composed of nodes and leaves. Since all the leaves are behaviours that have their own class we suggest creating a different script for the body that will import all the behaviours that it needs (an example [here](https://py-trees.readthedocs.io/en/devel/trees.html#skeleton)) +You can notice that after creating the tree all you need to do is call the function `behaviour_tree.tick_tock()` that will take care of ticking the tree. Parameters `pre_tick_handler` and `post_tick_handler` are used to lick functions that will be executed respectively before and after the tickling of the tree. In general they are used to see the status of the tree and additional information like the content of the blackboards. +We suggest adding a special element of the family of visitors that is called *snapshot visitor*. This component is used combined with `display.unicode_tree` to also show the statue of the visited element in the tree.
+You can add it by adding the following lines in the declaration of the behaviour tree + +``` +snapshot_visitor = py_trees.visitors.SnapshotVisitor() +behaviour_tree.visitors.append(snapshot_visitor) +``` +and then adding in the parameter `visited` in `dispaly.unicode_tree` like that + +``` +py_trees.display.unicode_tree( + root=behaviour_tree.root, + visited=snapshot_visitor.visited) +``` +You can also decide to manually tick the tree by running `behaviour_tree.tick()` instead of `tick_tock()` function. +PyTree offers a lot of components and useful tools that developers can use in their trees but here we will mention **composites** and **blackboards**. We recommend reading the documentation for understand all the concepts: [https://py-trees.readthedocs.io/en/devel/behaviours.html](https://py-trees.readthedocs.io/en/devel/behaviours.html) + +#### composites + +Composites are responsible for directing the path traced through the tree on a given tick (execution). They are the factories (Sequences and Parallels) and decision makers (Selectors) of a behaviour tree. +Composite behaviours typically manage children and apply some logic to the way they execute and return a result, but generally don’t do anything themselves. Perform the checks or actions you need to do in the non-composite behaviours. +[https://py-trees.readthedocs.io/en/devel/composites.html#composites](https://py-trees.readthedocs.io/en/devel/composites.html#composites) + +#### Blackboards + +Blackboards are not a necessary component of behaviour tree implementations, but are nonetheless, a fairly common mechanism for sharing data between behaviours in the tree. +[https://py-trees.readthedocs.io/en/devel/blackboards.html#blackboards](https://py-trees.readthedocs.io/en/devel/blackboards.html#blackboards) + +## Testing + +Here are the steps that you can follow in order to run a demo of pytree in HARMONI. This demo show how work a small three with just two behaviours: microphone and stt. + +1. Create a valid google credential of using stt service: [https://cloud.google.com/speech-to-text/docs/before-you-begin#setting_up_your_google_cloud_platform_project](https://cloud.google.com/speech-to-text/docs/before-you-begin#setting_up_your_google_cloud_platform_project) +2. Run the following command in the container to start the demo: + + ``` bash + roslaunch harmoni_pytree mic_and_stt.launch + ``` + +You will see some information about the structure of the tree and the way in which pytree engines tick the tree. Some blackboards are used to show the results of the leaves and in particular in backboards stt/result shows the result of the speech to text services. \ No newline at end of file diff --git a/harmoni_core/harmoni_pytree/config/configuration.yaml b/harmoni_core/harmoni_pytree/config/configuration.yaml new file mode 100755 index 00000000..9b466473 --- /dev/null +++ b/harmoni_core/harmoni_pytree/config/configuration.yaml @@ -0,0 +1,3 @@ +pytree: + default_param: + prova: "prova" diff --git a/harmoni_core/harmoni_pytree/launch/leaves/aws_lex_service.launch b/harmoni_core/harmoni_pytree/launch/leaves/aws_lex_service.launch new file mode 100644 index 00000000..f33ed47e --- /dev/null +++ b/harmoni_core/harmoni_pytree/launch/leaves/aws_lex_service.launch @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/harmoni_core/harmoni_pytree/launch/leaves/aws_tts_service.launch b/harmoni_core/harmoni_pytree/launch/leaves/aws_tts_service.launch new file mode 100644 index 00000000..7149d7fc --- /dev/null +++ b/harmoni_core/harmoni_pytree/launch/leaves/aws_tts_service.launch @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/harmoni_core/harmoni_pytree/launch/leaves/buttons.launch b/harmoni_core/harmoni_pytree/launch/leaves/buttons.launch new file mode 100644 index 00000000..8fd64ecd --- /dev/null +++ b/harmoni_core/harmoni_pytree/launch/leaves/buttons.launch @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/harmoni_core/harmoni_pytree/launch/leaves/camera_service.launch b/harmoni_core/harmoni_pytree/launch/leaves/camera_service.launch new file mode 100644 index 00000000..aec40de1 --- /dev/null +++ b/harmoni_core/harmoni_pytree/launch/leaves/camera_service.launch @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/harmoni_core/harmoni_pytree/launch/leaves/facial_exp_service.launch b/harmoni_core/harmoni_pytree/launch/leaves/facial_exp_service.launch new file mode 100644 index 00000000..075c9aca --- /dev/null +++ b/harmoni_core/harmoni_pytree/launch/leaves/facial_exp_service.launch @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/harmoni_core/harmoni_pytree/launch/leaves/speaker_service.launch b/harmoni_core/harmoni_pytree/launch/leaves/speaker_service.launch new file mode 100644 index 00000000..e9b6dfad --- /dev/null +++ b/harmoni_core/harmoni_pytree/launch/leaves/speaker_service.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/harmoni_core/harmoni_pytree/launch/leaves/stt_google_service.launch b/harmoni_core/harmoni_pytree/launch/leaves/stt_google_service.launch new file mode 100644 index 00000000..8522cac8 --- /dev/null +++ b/harmoni_core/harmoni_pytree/launch/leaves/stt_google_service.launch @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/harmoni_core/harmoni_pytree/launch/leaves/web_service.launch b/harmoni_core/harmoni_pytree/launch/leaves/web_service.launch new file mode 100644 index 00000000..4106ddfa --- /dev/null +++ b/harmoni_core/harmoni_pytree/launch/leaves/web_service.launch @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/harmoni_core/harmoni_pytree/launch/leaves/yolo_custom_service.launch b/harmoni_core/harmoni_pytree/launch/leaves/yolo_custom_service.launch new file mode 100644 index 00000000..6c660e02 --- /dev/null +++ b/harmoni_core/harmoni_pytree/launch/leaves/yolo_custom_service.launch @@ -0,0 +1,10 @@ + + + + + + + + + + \ No newline at end of file diff --git a/harmoni_core/harmoni_pytree/launch/leaves/yolo_service.launch b/harmoni_core/harmoni_pytree/launch/leaves/yolo_service.launch new file mode 100644 index 00000000..56bee459 --- /dev/null +++ b/harmoni_core/harmoni_pytree/launch/leaves/yolo_service.launch @@ -0,0 +1,10 @@ + + + + + + + + + + \ No newline at end of file diff --git a/harmoni_core/harmoni_pytree/launch/subtrees/interactionbg.launch b/harmoni_core/harmoni_pytree/launch/subtrees/interactionbg.launch new file mode 100644 index 00000000..0d551db3 --- /dev/null +++ b/harmoni_core/harmoni_pytree/launch/subtrees/interactionbg.launch @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/harmoni_core/harmoni_pytree/launch/subtrees/mainactivity.launch b/harmoni_core/harmoni_pytree/launch/subtrees/mainactivity.launch new file mode 100644 index 00000000..51801d85 --- /dev/null +++ b/harmoni_core/harmoni_pytree/launch/subtrees/mainactivity.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/harmoni_core/harmoni_pytree/launch/subtrees/mic_and_stt.launch b/harmoni_core/harmoni_pytree/launch/subtrees/mic_and_stt.launch new file mode 100644 index 00000000..ae64f61b --- /dev/null +++ b/harmoni_core/harmoni_pytree/launch/subtrees/mic_and_stt.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/harmoni_core/harmoni_pytree/launch/subtrees/root.launch b/harmoni_core/harmoni_pytree/launch/subtrees/root.launch new file mode 100644 index 00000000..08b894ef --- /dev/null +++ b/harmoni_core/harmoni_pytree/launch/subtrees/root.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/harmoni_core/harmoni_pytree/launch/subtrees/visualbg.launch b/harmoni_core/harmoni_pytree/launch/subtrees/visualbg.launch new file mode 100644 index 00000000..56b7f7dc --- /dev/null +++ b/harmoni_core/harmoni_pytree/launch/subtrees/visualbg.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/harmoni_core/harmoni_pytree/package.xml b/harmoni_core/harmoni_pytree/package.xml new file mode 100755 index 00000000..93a75842 --- /dev/null +++ b/harmoni_core/harmoni_pytree/package.xml @@ -0,0 +1,66 @@ + + + harmoni_pytree + 0.0.0 + The harmoni_pytree package + + + + + chris + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + rospy + rospy + rospy + rostest + rosunit + + + + + + + + + + diff --git a/harmoni_core/harmoni_pytree/resources/demo.json b/harmoni_core/harmoni_pytree/resources/demo.json new file mode 100644 index 00000000..76544af5 --- /dev/null +++ b/harmoni_core/harmoni_pytree/resources/demo.json @@ -0,0 +1,161 @@ +{ + "scene" : [ + { + "utterance" : "domanda raccolta", + "face" : "null", + "gesture" : "{'gesture':'QT/bye', 'timing': 0.5}", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/land.jpeg?alt=media&token=79c113ec-4e61-49c1-bbdf-b865a946247e'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "True" + }, + { + "utterance" : "nella plastica vanno buttati…", + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/land2.jpeg?alt=media&token=a3437794-a763-4b47-9052-485fd5c61ae5'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + }, + { + "utterance" : "oggetto bicchiere", + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/bicchiere.jpeg?alt=media&token=38d8ce3a-9f76-452e-9cca-a54a042493a0'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "True" + }, + { + "utterance" : "oggetto lattina", + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/lattina.jpg?alt=media&token=8a7a7508-e777-40de-9dd3-71d7a830afb3'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "True" + }, + { + "utterance" : "oggetto tetrapak", + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/carta.jpeg?alt=media&token=4d55fbf5-0255-4d32-bf42-8b0178111ee8'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "True" + }, + { + "utterance" : "chiedi dell'ombrello", + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/land.jpeg?alt=media&token=79c113ec-4e61-49c1-bbdf-b865a946247e'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "True" + }, + { + "utterance" : "ombrello" , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/land4.jpg?alt=media&token=4315c235-87d6-400e-a3ee-6e9e044087dc'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + }, + { + "utterance" : "Ora abbiamo finito, ciao!" , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/land.jpeg?alt=media&token=79c113ec-4e61-49c1-bbdf-b865a946247e'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + } + + ], + "error_handling" : + { + "riprendiamo_interacion" : { + "utterance" : "Okay, riprendiamo da dove avevamo interrotto", + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/land2.jpeg?alt=media&token=a3437794-a763-4b47-9052-485fd5c61ae5'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + }, + "riprendiamo_visual" : { + "utterance" : "Ah eccoti! Riprendiamo da dove avevamo interrotto", + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/land2.jpeg?alt=media&token=a3437794-a763-4b47-9052-485fd5c61ae5'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + }, + "risposta_corretta" : { + "utterance" : "Risposta esatta, ma non usarmi", + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/land4.jpg?alt=media&token=4315c235-87d6-400e-a3ee-6e9e044087dc'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + }, + "sei_sicuro" : { + "utterance" : "Sei sicuro, dove lo butteresti?", + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/land4.jpg?alt=media&token=4315c235-87d6-400e-a3ee-6e9e044087dc'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + }, + "risposta_sbagliata_carta" : { + "utterance" : "La risposta esatta era carta", + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/land3.png?alt=media&token=94e0e89d-4e41-4b1f-961f-9bd8ad064c7c'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + }, + "risposta_sbagliata_vetro" : { + "utterance" : "La risposta esatta era vetro", + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/land3.png?alt=media&token=94e0e89d-4e41-4b1f-961f-9bd8ad064c7c'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + }, + "risposta_sbagliata_plastica" : { + "utterance" : "La risposta esatta era plastica", + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/land3.png?alt=media&token=94e0e89d-4e41-4b1f-961f-9bd8ad064c7c'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + }, + "risposta_sbagliata" : { + "utterance" : "Risposta sbagliata, ma non usarmi", + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/land3.png?alt=media&token=94e0e89d-4e41-4b1f-961f-9bd8ad064c7c'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + }, + "terapista" : { + "utterance" : "Okay non ti preoccupare, ora chiamiamo il terapista", + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/land2.jpeg?alt=media&token=a3437794-a763-4b47-9052-485fd5c61ae5'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + }, + "stop" : { + "utterance" : "Vabene facciamo una pausa e chiamiamo il terapista", + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/land.jpeg?alt=media&token=79c113ec-4e61-49c1-bbdf-b865a946247e'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + }, + "no_capito" : { + "utterance" : "RIpetiamo, ma non usarmi", + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/land2.jpeg?alt=media&token=a3437794-a763-4b47-9052-485fd5c61ae5'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + } + } + +} diff --git a/harmoni_core/harmoni_pytree/resources/interaction_bg.json b/harmoni_core/harmoni_pytree/resources/interaction_bg.json new file mode 100644 index 00000000..89bba697 --- /dev/null +++ b/harmoni_core/harmoni_pytree/resources/interaction_bg.json @@ -0,0 +1,31 @@ +{ + "scene" : [ + { + "utterance" : "il bimbo non parla", + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "True" + } + ], + "error_handling" : + { + "riprendiamo" : { + "utterance" : "Okay, riprendiamo da dove avevamo interrotto", + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "False" + }, + "terapista" : { + "utterance" : "Okay non ti preoccupare, ora chiamiamo il terapista", + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "False" + } + } +} \ No newline at end of file diff --git a/harmoni_core/harmoni_pytree/resources/mainactivity.json b/harmoni_core/harmoni_pytree/resources/mainactivity.json new file mode 100644 index 00000000..e03b83c7 --- /dev/null +++ b/harmoni_core/harmoni_pytree/resources/mainactivity.json @@ -0,0 +1,392 @@ +{ + "scene" : [ + { + "utterance" : "E' mattina, ci siamo appena svegliati. Andiamo a fare colazione insieme.", + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/cucina.jpg?alt=media&token=fa5bf3bd-c93a-42c4-b6b4-3abac95bcb50'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + }, + { + "utterance" : "E’ finito il latte, dobbiamo andare a comprarlo.", + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "False" + }, + { + "utterance" : "ma prima buttiamo via le varie cose", + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "False" + }, + { + "utterance" : "domanda raccolta", + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/rifiuti.jpg?alt=media&token=4234c6f6-91c1-415e-8ffd-1f8b776c778c'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "True" + }, + { + "utterance" : "La raccolta differenziata è la divisione dei rifiuti serve per tenere l'ambiente più pulito riciclando", + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "False" + }, + { + "utterance" : "se consideriamo plastica, carta e vetro", + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "False" + }, + { + "utterance" : "nel bidone della plastica vanno buttati le bottiglie di plastica, i vasetti dello yogurt, le confezioni di merendine e così via" , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/oggetti_plastica.jpeg?alt=media&token=83fcea48-38ea-44a8-9a95-f24ba6b6b39e'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + }, + { + "utterance" : "nel bidone della carta vanno buttati i giornali, i fogli, le confezioni del latte e dei succhi di frutta e così via." , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/oggetti_carta.jpeg?alt=media&token=75318a24-c9d1-42e5-b009-a84e4c681f2f'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + }, + { + "utterance" : "nel bidone del vetro vanno buttati le bottiglie, i bicchieri, i barattoli e i vasi di vetro. Prima di buttarli si dovrebbero pulire." , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/oggetti_vetro.jpeg?alt=media&token=8e5225ae-f12b-4c04-915c-4055db33cf63'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + }, + { + "utterance" : "Adesso ti mostro qualche oggetto e ti faccio delle domande per vedere se hai capito", + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/Domande.jpg?alt=media&token=7740ab70-e808-414a-8396-530f668ad02f'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + }, + { + "utterance" : "oggetto tetrapak" , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/carta2.jpg?alt=media&token=50b48b55-dce0-43fe-bc44-dee4a2a97099'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "True" + }, + { + "utterance" : "oggetto lattina" , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/plastica1.jpg?alt=media&token=aac16410-ffd6-4e59-a635-52e4e07f3ad9'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "True" + }, + { + "utterance" : "oggetto bicchiere" , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/sugo.jpeg?alt=media&token=e821001c-b988-43f3-832a-c0d0a6f82328'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "True" + }, + { + "utterance" : "Ora che abbiamo finito andiamo a comprare il latte per fare colazione" , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/cucina.jpg?alt=media&token=fa5bf3bd-c93a-42c4-b6b4-3abac95bcb50'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + }, + { + "utterance" : "Guarda che nuvole scure fuori" , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/cielo_nuvoloso.jpg?alt=media&token=bb832192-47cc-4131-af44-0a7f154bdcab'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + }, + { + "utterance" : "chiedi dell'ombrello" , + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "True" + }, + { + "utterance" : "" , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/cielo%20nuvoloso-2.jpg?alt=media&token=3916946c-1c73-4fd8-9246-af2ee80c82b6'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + }, + { + "utterance" : "Oh no, sta piovendo!" , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/pioggia1.jpg?alt=media&token=ba5a1a44-1c00-48ea-9195-86bd44322f0d'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + }, + { + "utterance" : "ombrello setta frase" , + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "False" + }, + { + "utterance" : "", + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/pioggia4.jpg?alt=media&token=208fdfd9-44bf-430d-af5a-6d303d3e4c6f'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + }, + { + "utterance" : "Andiamo subito dentro il supermercato" , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/supermercato.png?alt=media&token=d8636ff5-fde2-4dc7-a0e1-8b723138e9da'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + }, + { + "utterance" : "compriamo quello che serve e torniamo a casa " , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/carrello.jpg?alt=media&token=08cc85ac-f786-4aae-bdf0-d23b2a2a40cb'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + }, + { + "utterance" : "Finalmente possiamo andare a casa a mangiare!" , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/supermercato.png?alt=media&token=d8636ff5-fde2-4dc7-a0e1-8b723138e9da'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + }, + { + "utterance" : "" , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/pioggia5.jpg?alt=media&token=e92c1f08-6680-4e88-95d5-2288b9ce16c5'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + }, + { + "utterance" : "" , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/pioggia1.jpg?alt=media&token=ba5a1a44-1c00-48ea-9195-86bd44322f0d'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + }, + { + "utterance" : "Abbiamo fatto una vera e propria colazione da campione." , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/colazione2.jpeg?alt=media&token=28b0d33b-28a1-45d0-842a-67ab085e5457'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + }, + { + "utterance" : "ora dobbiamo solo sparecchiare" , + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "False" + }, + { + "utterance" : "oggetto lattina" , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/plastica2.jpg?alt=media&token=6b8bb006-428c-4490-9433-39b7040702b8'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "True" + }, + { + "utterance" : "oggetto tetrapak" , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/carta3.jpg?alt=media&token=155a0ff9-2f98-4a04-a61d-b8b2b2817f85'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "True" + }, + { + "utterance" : "oggetto bicchiere" , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/vetro2.jpg?alt=media&token=638cc00e-a80f-4bb4-9aa3-b314439dee27'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "True" + }, + { + "utterance" : "oggetto lattina" , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/plastica3.jpg?alt=media&token=8ef7e67c-2750-416c-9d64-456f391a87cb'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "True" + }, + { + "utterance" : "oggetto bicchiere" , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/bottiglietta-vetro.jpeg?alt=media&token=cb3447e0-086e-4c51-bdaf-cdb747aa96fe'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "True" + }, + { + "utterance" : "oggetto tetrapak" , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/patatine.jpeg?alt=media&token=b1951f54-25a4-423b-a460-6a4b46caa190'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "True" + }, + { + "utterance" : "Abbiamo finito e sei stato bravissimo!" , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/cucina.jpg?alt=media&token=fa5bf3bd-c93a-42c4-b6b4-3abac95bcb50'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + }, + { + "utterance" : "Oggi abbiamo imparato come separare vetro, carta e plastica" , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/rifiuti.jpg?alt=media&token=4234c6f6-91c1-415e-8ffd-1f8b776c778c'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + }, + { + "utterance" : "Abbiamo anche capito che quando il cielo è molto scuro si dovrebbe portare l’ombrello." , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/cielo_nuvoloso.jpg?alt=media&token=bb832192-47cc-4131-af44-0a7f154bdcab'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + }, + { + "utterance" : "Spero tu ti sia divertito, io mi sono divertito molto con te!" , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/bolledisapone.jpg?alt=media&token=72e6cdb0-df0e-4ca0-b393-1277133ae6b6'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + } + + ], + "error_handling" : + { + "riprendiamo_interacion" : { + "utterance" : "Okay, riprendiamo da dove avevamo interrotto", + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "False" + }, + "riprendiamo_visual" : { + "utterance" : "Ah eccoti! Riprendiamo da dove avevamo interrotto", + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "False" + }, + "risposta_corretta" : { + "utterance" : "Risposta esatta, ma non usarmi", + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "False" + }, + "sei_sicuro" : { + "utterance" : "Sei sicuro, dove lo butteresti?", + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "False" + }, + "risposta_sbagliata_carta" : { + "utterance" : "La risposta esatta era carta", + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "False" + }, + "risposta_sbagliata_vetro" : { + "utterance" : "La risposta esatta era vetro", + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "False" + }, + "risposta_sbagliata_plastica" : { + "utterance" : "La risposta esatta era plastica", + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "False" + }, + "risposta_sbagliata" : { + "utterance" : "Risposta sbagliata, ma non usarmi", + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "False" + }, + "terapista" : { + "utterance" : "Okay non ti preoccupare, ora chiamiamo il terapista", + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "False" + }, + "stop" : { + "utterance" : "Vabene facciamo una pausa e chiamiamo il terapista", + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "False" + }, + "no_capito" : { + "utterance" : "Ripetiamo, ma non usarmi", + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "False" + } + } +} diff --git a/harmoni_core/harmoni_pytree/resources/short_activity.json b/harmoni_core/harmoni_pytree/resources/short_activity.json new file mode 100644 index 00000000..d854619c --- /dev/null +++ b/harmoni_core/harmoni_pytree/resources/short_activity.json @@ -0,0 +1,234 @@ +{ + "scene" : [ + { + "utterance" : "Ciao, sono Kitty e oggi parliamo della raccolta differenziata!", + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/rifiuti.jpg?alt=media&token=4234c6f6-91c1-415e-8ffd-1f8b776c778c'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + }, + { + "utterance" : "domanda raccolta", + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "True" + }, + { + "utterance" : "La raccolta differenziata è la divisione dei rifiuti serve per tenere l'ambiente più pulito riciclando", + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "False" + }, + { + "utterance" : "se consideriamo plastica, carta e vetro", + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "False" + }, + { + "utterance" : "nel bidone della plastica vanno buttati le bottiglie di plastica, i vasetti dello yogurt,le confezioni di merendine e così via" , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/oggetti_plastica.jpeg?alt=media&token=83fcea48-38ea-44a8-9a95-f24ba6b6b39e'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + }, + { + "utterance" : "nel bidone della carta vanno buttati i giornali, i fogli, le confezioni del latte e dei succhi di frutta e così via." , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/oggetti_carta.jpeg?alt=media&token=75318a24-c9d1-42e5-b009-a84e4c681f2f'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + }, + { + "utterance" : "nel bidone del vetro vanno buttati le bottiglie, i bicchieri, i barattoli e i vasi di vetro. Prima di buttarli si dovrebbero pulire." , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/oggetti_vetro.jpeg?alt=media&token=8e5225ae-f12b-4c04-915c-4055db33cf63'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + }, + { + "utterance" : "Adesso ti mostro qualche oggetto e ti faccio delle domande per vedere se hai capito", + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/Domande.jpg?alt=media&token=7740ab70-e808-414a-8396-530f668ad02f'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + }, + { + "utterance" : "oggetto tetrapak" , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/carta2.jpg?alt=media&token=50b48b55-dce0-43fe-bc44-dee4a2a97099'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "True" + }, + { + "utterance" : "oggetto lattina" , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/plastica1.jpg?alt=media&token=aac16410-ffd6-4e59-a635-52e4e07f3ad9'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "True" + }, + { + "utterance" : "oggetto bicchiere" , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/sugo.jpeg?alt=media&token=e821001c-b988-43f3-832a-c0d0a6f82328'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "True" + }, + { + "utterance" : "oggetto lattina" , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/plastica2.jpg?alt=media&token=6b8bb006-428c-4490-9433-39b7040702b8'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "True" + }, + { + "utterance" : "oggetto tetrapak" , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/carta3.jpg?alt=media&token=155a0ff9-2f98-4a04-a61d-b8b2b2817f85'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "True" + }, + { + "utterance" : "oggetto bicchiere" , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/vetro2.jpg?alt=media&token=638cc00e-a80f-4bb4-9aa3-b314439dee27'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "True" + }, + { + "utterance" : "Abbiamo finito e sei stato bravissimo!" , + "face" : "null", + "gesture" : "null", + "image" : "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/bolledisapone.jpg?alt=media&token=72e6cdb0-df0e-4ca0-b393-1277133ae6b6'},{'component_id':'raccolta_container', 'set_content': ''}]", + "sound" : "null", + "do_trigger" : "False" + }, + { + "utterance" : "Oggi abbiamo imparato come separare vetro, carta e plastica" , + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "False" + }, + { + "utterance" : "Spero tu ti sia divertito, io mi sono divertito molto con te!" , + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "False" + } + + + + ], + "error_handling" : + { + "riprendiamo_interacion" : { + "utterance" : "Okay, riprendiamo da dove avevamo interrotto", + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "False" + }, + "riprendiamo_visual" : { + "utterance" : "Ah eccoti! Riprendiamo da dove avevamo interrotto", + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "False" + }, + "risposta_corretta" : { + "utterance" : "Risposta esatta, ma non usarmi", + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "False" + }, + "sei_sicuro" : { + "utterance" : "Sei sicuro, dove lo butteresti?", + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "False" + }, + "risposta_sbagliata_carta" : { + "utterance" : "La risposta esatta era carta", + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "False" + }, + "risposta_sbagliata_vetro" : { + "utterance" : "La risposta esatta era vetro", + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "False" + }, + "risposta_sbagliata_plastica" : { + "utterance" : "La risposta esatta era plastica", + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "False" + }, + "risposta_sbagliata" : { + "utterance" : "Risposta sbagliata, ma non usarmi", + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "False" + }, + "terapista" : { + "utterance" : "Okay non ti preoccupare, ora chiamiamo il terapista", + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "False" + }, + "stop" : { + "utterance" : "Vabene facciamo una pausa e chiamiamo il terapista", + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "False" + }, + "no_capito" : { + "utterance" : "Ripetiamo, ma non usarmi", + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "False" + } + } +} diff --git a/harmoni_core/harmoni_pytree/resources/therapist.json b/harmoni_core/harmoni_pytree/resources/therapist.json new file mode 100644 index 00000000..146d43ae --- /dev/null +++ b/harmoni_core/harmoni_pytree/resources/therapist.json @@ -0,0 +1,5 @@ +{ + "scene" : { + "utterance" : "scena 1" + } +} diff --git a/harmoni_core/harmoni_pytree/resources/visual_bg.json b/harmoni_core/harmoni_pytree/resources/visual_bg.json new file mode 100644 index 00000000..5b57d5d0 --- /dev/null +++ b/harmoni_core/harmoni_pytree/resources/visual_bg.json @@ -0,0 +1,22 @@ +{ + "scene" : [ + { + "utterance" : "Il bimbo non c'è", + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "False" + } + ], + "terapista": + { + "utterance" : "Okay non ti preoccupare, ora chiamiamo il terapista", + "face" : "null", + "gesture" : "null", + "image" : "null", + "sound" : "null", + "do_trigger" : "False" + } + +} \ No newline at end of file diff --git a/harmoni_core/harmoni_pytree/setup.py b/harmoni_core/harmoni_pytree/setup.py new file mode 100755 index 00000000..eaa46cca --- /dev/null +++ b/harmoni_core/harmoni_pytree/setup.py @@ -0,0 +1,11 @@ +# ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + packages=["harmoni_pytree"], package_dir={"": "src"}, +) + +setup(**setup_args) diff --git a/harmoni_core/harmoni_pytree/src/harmoni_pytree/__init__.py b/harmoni_core/harmoni_pytree/src/harmoni_pytree/__init__.py new file mode 100755 index 00000000..e69de29b diff --git a/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/aws_lex_analyzer_service.py b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/aws_lex_analyzer_service.py new file mode 100644 index 00000000..211c4c8d --- /dev/null +++ b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/aws_lex_analyzer_service.py @@ -0,0 +1,166 @@ +#!/usr/bin/env python3 + +# Common Imports +import rospy +from harmoni_common_lib.constants import * +from actionlib_msgs.msg import GoalStatus +from harmoni_common_lib.action_client import HarmoniActionClient +import harmoni_common_lib.helper_functions as hf +from harmoni_bot.aws_lex_service import AWSLexService + +# Specific Imports +from harmoni_common_lib.constants import ActuatorNameSpace, ActionType, DialogueNameSpace +from botocore.exceptions import BotoCoreError, ClientError +from contextlib import closing +from collections import deque +import soundfile as sf +import numpy as np +import re +import json +import ast +import sys + +#py_tree +import py_trees +import time + +import py_trees.console + +class AWSLexAnalyzerServicePytree(py_trees.behaviour.Behaviour): + def __init__(self, name): + self.name = name + self.server_state = None + self.service_client_lex = None + self.client_result = None + self.send_request = True + + self.blackboards = [] + + self.blackboard_stt = self.attach_blackboard_client(name=self.name, namespace=DetectorNameSpace.stt.name) + self.blackboard_stt.register_key("result", access=py_trees.common.Access.READ) + self.blackboard_card_detect = self.attach_blackboard_client(name=self.name, namespace=DetectorNameSpace.card_detect.name) + self.blackboard_card_detect.register_key("result", access=py_trees.common.Access.READ) + self.blackboard_buttons = self.attach_blackboard_client(name=self.name, namespace=PyTreeNameSpace.buttons.name) + self.blackboard_buttons.register_key("result", access=py_trees.common.Access.READ) + self.blackboard_bot = self.attach_blackboard_client(name=self.name, namespace=DialogueNameSpace.bot.name+"/"+ PyTreeNameSpace.analyzer.name) + self.blackboard_bot.register_key("result", access=py_trees.common.Access.WRITE) + self.blackboard_mainactivity = self.attach_blackboard_client(name=self.name, namespace=PyTreeNameSpace.mainactivity.name) + self.blackboard_mainactivity.register_key("counter_no_answer", access=py_trees.common.Access.WRITE) + + super(AWSLexAnalyzerServicePytree, self).__init__(name) + self.logger.debug("%s.__init__()" % (self.__class__.__name__)) + + def setup(self,**additional_parameters): + """ + for parameter in additional_parameters: + print(parameter, additional_parameters[parameter]) + if(parameter ==DialogueNameSpace.bot.name): + self.mode = additional_parameters[parameter] + """ + self.service_client_lex = HarmoniActionClient(self.name) + self.server_name = "bot_default" + self.service_client_lex.setup_client(self.server_name, + self._result_callback, + self._feedback_callback) + self.logger.debug("Behavior %s interface action clients have been set up!" % (self.server_name)) + + self.blackboard_bot.result = "null" + self.blackboard_mainactivity.counter_no_answer = 0 + + self.logger.debug("%s.setup()" % (self.__class__.__name__)) + + def initialise(self): + self.logger.debug("%s.initialise()" % (self.__class__.__name__)) + + def update(self): + if self.send_request: + self.send_request = False + if self.blackboard_card_detect.result != "null": + self.logger.debug(f"Sending again goal to {self.server_name}") + self.service_client_lex.send_goal( + action_goal = ActionType["REQUEST"].value, + optional_data=self.blackboard_card_detect.result, + wait=False, + ) + self.logger.debug(f"Goal sent to {self.server_name}") + new_status = py_trees.common.Status.RUNNING + elif self.blackboard_stt.result != "null": + self.logger.debug(f"Sending goal to {self.server_name}") + self.service_client_lex.send_goal( + action_goal = ActionType["REQUEST"].value, + optional_data=self.blackboard_stt.result, + wait=False, + ) + self.logger.debug(f"Goal sent to {self.server_name}") + new_status = py_trees.common.Status.RUNNING + elif self.blackboard_buttons.result != "null": + self.logger.debug(f"Sending goal to {self.server_name}") + self.service_client_lex.send_goal( + action_goal = ActionType["REQUEST"].value, + optional_data=self.blackboard_buttons.result, + wait=False, + ) + self.logger.debug(f"Goal sent to {self.server_name}") + new_status = py_trees.common.Status.RUNNING + else: + self.blackboard_mainactivity.counter_no_answer += 1 + self.blackboard_bot.result = "void_answer" + new_status = py_trees.common.Status.SUCCESS + else: + new_state = self.service_client_lex.get_state() + print("update : ",new_state) + if new_state == GoalStatus.ACTIVE: + new_status = py_trees.common.Status.RUNNING + elif new_state == GoalStatus.SUCCEEDED: + if self.client_result is not None: + self.blackboard_bot.result = eval(self.client_result) + self.client_result = None + new_status = py_trees.common.Status.SUCCESS + else: + self.logger.debug(f"Waiting fot the result ({self.server_name})") + new_status = py_trees.common.Status.RUNNING + elif new_state == GoalStatus.PENDING: + self.send_request = True + self.logger.debug(f"Cancelling goal to {self.server_name}") + self.service_client_lex.cancel_all_goals() + self.client_result = None + self.logger.debug(f"Goal cancelled to {self.server_name}") + #self.service_client_lex.stop_tracking_goal() + #self.logger.debug(f"Goal tracking stopped to {self.server_name}") + new_status = py_trees.common.Status.RUNNING + else: + new_status = py_trees.common.Status.FAILURE + raise + + self.logger.debug("%s.update()[%s]--->[%s]" % (self.__class__.__name__, self.status, new_status)) + return new_status + + def terminate(self, new_status): + new_state = self.service_client_lex.get_state() + print("terminate : ",new_state) + if new_state == GoalStatus.SUCCEEDED or new_state == GoalStatus.ABORTED or new_state == GoalStatus.LOST: + self.send_request = True + if new_state == GoalStatus.PENDING: + self.send_request = True + self.logger.debug(f"Cancelling goal to {self.server_name}") + self.service_client_lex.cancel_all_goals() + self.client_result = None + self.logger.debug(f"Goal cancelled to {self.server_name}") + #self.service_client_lex.stop_tracking_goal() + #self.logger.debug(f"Goal tracking stopped to {self.server_name}") + self.logger.debug("%s.terminate()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + + def _result_callback(self, result): + """ Recieve and store result with timestamp """ + self.logger.debug("The result of the request has been received") + self.logger.debug( + f"The result callback message from {result['service']} was {len(result['message'])} long" + ) + self.client_result = result["message"] + return + + def _feedback_callback(self, feedback): + """ Feedback is currently just logged """ + self.logger.debug("The feedback recieved is %s." % feedback) + self.server_state = feedback["state"] + return diff --git a/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/aws_lex_trigger_service.py b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/aws_lex_trigger_service.py new file mode 100755 index 00000000..0bd43658 --- /dev/null +++ b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/aws_lex_trigger_service.py @@ -0,0 +1,161 @@ +#!/usr/bin/env python3 + +# Common Imports +import rospy +from harmoni_common_lib.constants import * +from actionlib_msgs.msg import GoalStatus +from harmoni_common_lib.action_client import HarmoniActionClient +import harmoni_common_lib.helper_functions as hf +from harmoni_bot.aws_lex_service import AWSLexService + +# Specific Imports +from harmoni_common_lib.constants import ActuatorNameSpace, ActionType, DialogueNameSpace +from botocore.exceptions import BotoCoreError, ClientError +from contextlib import closing +from collections import deque +import soundfile as sf +import numpy as np +import re +import json +import ast +import sys + +#py_tree +import py_trees +import time + +import py_trees.console + +class AWSLexTriggerServicePytree(py_trees.behaviour.Behaviour): + def __init__(self, name): + self.name = name + self.server_state = None + self.service_client_lex = None + self.client_result = None + self.send_request = True + + self.blackboards = [] + self.blackboard_scene = self.attach_blackboard_client(name=self.name, namespace=PyTreeNameSpace.scene.name) + self.blackboard_scene.register_key("utterance", access=py_trees.common.Access.READ) + self.blackboard_bot = self.attach_blackboard_client(name=self.name, namespace=DialogueNameSpace.bot.name+"/"+PyTreeNameSpace.trigger.name) + self.blackboard_bot.register_key("result", access=py_trees.common.Access.WRITE) + + super(AWSLexTriggerServicePytree, self).__init__(name) + self.logger.debug("%s.__init__()" % (self.__class__.__name__)) + + def setup(self,**additional_parameters): + self.service_client_lex = HarmoniActionClient(self.name) + self.server_name = "bot_default" + self.service_client_lex.setup_client(self.server_name, + self._result_callback, + self._feedback_callback) + self.logger.debug("Behavior %s interface action clients have been set up!" % (self.server_name)) + + self.blackboard_bot.result = "null" + + self.logger.debug("%s.setup()" % (self.__class__.__name__)) + + def initialise(self): + self.logger.debug("%s.initialise()" % (self.__class__.__name__)) + + def update(self): + if self.send_request: + self.send_request = False + self.logger.debug(f"Sending goal to {self.server_name}") + self.service_client_lex.send_goal( + action_goal = ActionType["REQUEST"].value, + optional_data=self.blackboard_scene.utterance, + wait=False, + ) + self.logger.debug(f"Goal sent to {self.server_name}") + new_status = py_trees.common.Status.RUNNING + else: + new_state = self.service_client_lex.get_state() + print("update : ",new_state) + if new_state == GoalStatus.ACTIVE: + new_status = py_trees.common.Status.RUNNING + elif new_state == GoalStatus.SUCCEEDED: + if self.client_result is not None: + self.blackboard_bot.result = eval(self.client_result) + self.client_result = None + new_status = py_trees.common.Status.SUCCESS + else: + self.logger.debug(f"Waiting fot the result ({self.server_name})") + new_status = py_trees.common.Status.RUNNING + elif new_state == GoalStatus.PENDING: + self.send_request = True + self.logger.debug(f"Cancelling goal to {self.server_name}") + self.service_client_lex.cancel_all_goals() + self.client_result = None + self.logger.debug(f"Goal cancelled to {self.server_name}") + #self.service_client_lex.stop_tracking_goal() + #self.logger.debug(f"Goal tracking stopped to {self.server_name}") + new_status = py_trees.common.Status.RUNNING + else: + new_status = py_trees.common.Status.FAILURE + raise + + self.logger.debug("%s.update()[%s]--->[%s]" % (self.__class__.__name__, self.status, new_status)) + return new_status + + + + def terminate(self, new_status): + new_state = self.service_client_lex.get_state() + print("terminate : ",new_state) + if new_state == GoalStatus.SUCCEEDED or new_state == GoalStatus.ABORTED or new_state == GoalStatus.LOST: + self.send_request = True + if new_state == GoalStatus.PENDING: + self.send_request = True + self.logger.debug(f"Cancelling goal to {self.server_name}") + self.service_client_lex.cancel_all_goals() + self.client_result = None + self.logger.debug(f"Goal cancelled to {self.server_name}") + #self.service_client_lex.stop_tracking_goal() + #self.logger.debug(f"Goal tracking stopped to {self.server_name}") + self.logger.debug("%s.terminate()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + + def _result_callback(self, result): + """ Recieve and store result with timestamp """ + self.logger.debug("The result of the request has been received") + self.logger.debug( + f"The result callback message from {result['service']} was {len(result['message'])} long" + ) + self.client_result = result['message'] + return + + def _feedback_callback(self, feedback): + """ Feedback is currently just logged """ + self.logger.debug("The feedback recieved is %s." % feedback) + self.server_state = feedback["state"] + return + +def main(): + #command_line_argument_parser().parse_args() + + py_trees.logging.level = py_trees.logging.Level.DEBUG + blackboardProva = py_trees.blackboard.Client(name="blackboardProva", namespace=PyTreeNameSpace.scene.name) + blackboardProva.register_key("utterance", access=py_trees.common.Access.WRITE) + blackboardProva.utterance = "domanda raccolta" + blackboardProva2 = py_trees.blackboard.Client(name="blackboardProva2", namespace=DialogueNameSpace.bot.name+"/"+PyTreeNameSpace.trigger.name) + blackboardProva2.register_key("result", access=py_trees.common.Access.READ) + print(blackboardProva) + print(blackboardProva2) + + rospy.init_node("bot_default", log_level=rospy.INFO) + + awslexPyTree = AWSLexTriggerServicePytree("AWSLexTriggerServicePytreeTest") + awslexPyTree.setup() + try: + for unused_i in range(0, 10): + awslexPyTree.tick_once() + time.sleep(0.5) + print(blackboardProva) + print(blackboardProva2) + print("\n") + except KeyboardInterrupt: + print("Exception occurred") + pass + +if __name__ == "__main__": + main() diff --git a/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/aws_tts_service.py b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/aws_tts_service.py new file mode 100755 index 00000000..b826e556 --- /dev/null +++ b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/aws_tts_service.py @@ -0,0 +1,168 @@ +#!/usr/bin/env python3 + +# Common Imports +import rospy +import roslib + +from harmoni_common_lib.constants import * +from actionlib_msgs.msg import GoalStatus +from harmoni_common_lib.service_server import HarmoniServiceServer +from harmoni_common_lib.service_manager import HarmoniServiceManager +from harmoni_common_lib.action_client import HarmoniActionClient +import harmoni_common_lib.helper_functions as hf +from harmoni_tts.aws_tts_service import AWSTtsService +# Specific Imports +from harmoni_common_lib.constants import ActuatorNameSpace, ActionType, State, DialogueNameSpace +from botocore.exceptions import BotoCoreError, ClientError +from contextlib import closing +from collections import deque +import soundfile as sf +import numpy as np +import boto3 +import re +import json +import ast +import sys + +#py_tree +import py_trees +import time + +import py_trees.console + +class AWSTtsServicePytree(py_trees.behaviour.Behaviour): + def __init__(self, name): + self.name = name + self.server_state = None + self.service_client_tts = None + self.client_result = None + self.send_request = True + + # here there is the inizialization of the blackboards + self.blackboards = [] + self.blackboard_tts = self.attach_blackboard_client(name=self.name, namespace=ActuatorNameSpace.tts.name) + self.blackboard_tts.register_key("result", access=py_trees.common.Access.WRITE) + self.blackboard_bot = self.attach_blackboard_client(name=self.name, namespace=DialogueNameSpace.bot.name +"/"+PyTreeNameSpace.trigger.name) + self.blackboard_bot.register_key("result", access=py_trees.common.Access.READ) + + super(AWSTtsServicePytree, self).__init__(name) + self.logger.debug("%s.__init__()" % (self.__class__.__name__)) + + def setup(self,**additional_parameters): + self.service_client_tts = HarmoniActionClient(self.name) + self.server_name = "tts_default" + self.service_client_tts.setup_client(self.server_name, + self._result_callback, + self._feedback_callback) + self.logger.debug("Behavior %s interface action clients have been set up!" % (self.server_name)) + + self.blackboard_tts.result = "null" + + self.logger.debug("%s.setup()" % (self.__class__.__name__)) + + def initialise(self): + self.logger.debug("%s.initialise()" % (self.__class__.__name__)) + + def update(self): + if self.send_request: + self.send_request = False + self.logger.debug(f"Sending goal to {self.server_name}") + self.service_client_tts.send_goal( + action_goal = ActionType["REQUEST"].value, + optional_data = self.blackboard_bot.result["message"], + wait=False, + ) + self.logger.debug(f"Goal sent to {self.server_name}") + new_status = py_trees.common.Status.RUNNING + else: + new_state = self.service_client_tts.get_state() + print("update : ",new_state) + if new_state == GoalStatus.ACTIVE: + new_status = py_trees.common.Status.RUNNING + elif new_state == GoalStatus.SUCCEEDED: + if self.client_result is not None: + self.blackboard_tts.result = self.client_result + self.client_result = None + new_status = py_trees.common.Status.SUCCESS + else: + self.logger.debug(f"Waiting fot the result ({self.server_name})") + new_status = py_trees.common.Status.RUNNING + elif new_state == GoalStatus.PENDING: + self.send_request = True + self.logger.debug(f"Cancelling goal to {self.server_name}") + self.service_client_tts.cancel_all_goals() + self.client_result = None + self.logger.debug(f"Goal cancelled to {self.server_name}") + #self.service_client_tts.stop_tracking_goal() + #self.logger.debug(f"Goal tracking stopped to {self.server_name}") + new_status = py_trees.common.Status.RUNNING + else: + new_status = py_trees.common.Status.FAILURE + raise + + self.logger.debug("%s.update()[%s]--->[%s]" % (self.__class__.__name__, self.status, new_status)) + return new_status + + + def terminate(self, new_status): + new_state = self.service_client_tts.get_state() + print("terminate : ",new_state) + if new_state == GoalStatus.SUCCEEDED or new_state == GoalStatus.ABORTED or new_state == GoalStatus.LOST: + self.send_request = True + if new_state == GoalStatus.PENDING: + self.send_request = True + self.logger.debug(f"Cancelling goal to {self.server_name}") + self.service_client_tts.cancel_all_goals() + self.client_result = None + self.logger.debug(f"Goal cancelled to {self.server_name}") + #self.service_client_tts.stop_tracking_goal() + #self.logger.debug(f"Goal tracking stopped to {self.server_name}") + self.logger.debug("%s.terminate()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + + def _result_callback(self, result): + """ Recieve and store result with timestamp """ + self.logger.debug("The result of the request has been received") + self.logger.debug( + f"The result callback message from {result['service']} was {len(result['message'])} long" + ) + self.client_result = result["message"] + return + + def _feedback_callback(self, feedback): + """ Feedback is currently just logged """ + self.logger.debug("The feedback recieved is %s." % feedback) + self.server_state = feedback["state"] + return + +def main(): + #command_line_argument_parser().parse_args() + + py_trees.logging.level = py_trees.logging.Level.DEBUG + + blackboardProva = py_trees.blackboard.Client(name="blackboardProva", namespace=DialogueNameSpace.bot.name +"/"+PyTreeNameSpace.trigger.name) + blackboardProva.register_key("result", access=py_trees.common.Access.WRITE) + blackboardProva.result = { + "message": "Ciao sono Kitty" + } + blackboardProva2 = py_trees.blackboard.Client(name="blackboardProva2", namespace=ActuatorNameSpace.tts.name) + blackboardProva2.register_key("result", access=py_trees.common.Access.READ) + print(blackboardProva) + print(blackboardProva2) + + rospy.init_node("tts_default", log_level=rospy.INFO) + + ttsPyTree = AWSTtsServicePytree("AWSTtsServicePytreeTest") + ttsPyTree.setup() + try: + for unused_i in range(0, 10): + ttsPyTree.tick_once() + time.sleep(0.5) + print(blackboardProva) + print(blackboardProva2) + print("\n") + except KeyboardInterrupt: + print("Exception occurred") + pass + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/buttons.py b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/buttons.py new file mode 100755 index 00000000..33b1bae9 --- /dev/null +++ b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/buttons.py @@ -0,0 +1,101 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +from harmoni_common_lib.constants import * +import rospy +import py_trees +import random +import serial +import time + +class Buttons(py_trees.behaviour.Behaviour): + def __init__(self, name): + self.name = name + self.blackboards = [] + self.blackboard_buttons = self.attach_blackboard_client(name=self.name, namespace=PyTreeNameSpace.buttons.name) + self.blackboard_buttons.register_key("result", access=py_trees.common.Access.WRITE) + + super(Buttons, self).__init__(name) + self.logger.debug("%s.__init__()" % (self.__class__.__name__)) + + def setup(self): + self.wemos = serial.Serial("/dev/ttyUSB0",timeout=1) + self.blackboard_buttons.result = "null" + self.read_serial = [] + self.start_time = None + self.max_duration = 4 + self.logger.debug("%s.setup()" % (self.__class__.__name__)) + + def initialise(self): + self.blackboard_buttons.result = "null" + self.logger.debug("%s.initialise()" % (self.__class__.__name__)) + + def update(self): + if self.start_time == None: + self.start_time = time.time() #time.time() is the current time + print("Timer started at: ", self.start_time) + new_status = py_trees.common.Status.RUNNING + else: + self.elapsed_time = time.time() - self.start_time + print("elapsed buttons: ",self.elapsed_time) + if self.max_duration < self.elapsed_time: + print("Timeout buttons!") + self.start_time = None + self.blackboard_buttons.result = "null" + new_status = py_trees.common.Status.SUCCESS + else: + self.read_serial = [] + self.read_serial = self.wemos.readlines() + print("@@@@@@@@@@@@@@@@@@") + print(self.read_serial) + if len(self.read_serial) != 0: + self.read_serial = str(self.read_serial[0]) + print(self.read_serial) + if self.read_serial == "b'pressb2\\r\\n'": + print("si") + self.blackboard_buttons.result = "si" + new_status = py_trees.common.Status.SUCCESS + elif self.read_serial == "b'pressb1\\r\\n'": + print("no") + self.blackboard_buttons.result = "no" + new_status = py_trees.common.Status.SUCCESS + else: + print("errore") + self.blackboard_buttons.result = "null" + new_status = py_trees.common.Status.SUCCESS + else: + new_status = py_trees.common.Status.RUNNING + self.logger.debug("%s.update()[%s]--->[%s]" % (self.__class__.__name__, self.status, new_status)) + return new_status + + + def terminate(self, new_status): + self.start_time = None + self.logger.debug(" %s [Foo::terminate().terminate()][%s->%s]" % (self.name, self.status, new_status)) + +def main(): + #command_line_argument_parser().parse_args() + + py_trees.logging.level = py_trees.logging.Level.DEBUG + + rospy.init_node("buttons_default", log_level=rospy.INFO) + + blackboardProva = py_trees.blackboard.Client(name="blackboardProva", namespace=PyTreeNameSpace.buttons.name) + blackboardProva.register_key("result", access=py_trees.common.Access.READ) + print(blackboardProva) + + buttons = Buttons("ButtonsPytreeTest") + + buttons.setup() + try: + for unused_i in range(0, 50): + buttons.tick_once() + time.sleep(1) + print(blackboardProva) + print("\n") + except KeyboardInterrupt: + print("Exception occurred") + pass + +if __name__ == "__main__": + main() diff --git a/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/camera_service.py b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/camera_service.py new file mode 100755 index 00000000..6c1f0717 --- /dev/null +++ b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/camera_service.py @@ -0,0 +1,152 @@ +#!/usr/bin/env python3 + +# Common Imports +import rospy +import roslib + +from harmoni_common_lib.constants import * +from actionlib_msgs.msg import GoalStatus +from harmoni_common_lib.action_client import HarmoniActionClient +import harmoni_common_lib.helper_functions as hf +# Other Imports +from harmoni_common_lib.constants import SensorNameSpace + +# Specific Imports +from harmoni_common_lib.constants import ActuatorNameSpace, ActionType, DialogueNameSpace +from botocore.exceptions import BotoCoreError, ClientError +from contextlib import closing +from collections import deque +import numpy as np +import boto3 +import re +import json +import ast +import sys + +#py_tree +import py_trees +import time + +import py_trees.console + +class CameraServicePytree(py_trees.behaviour.Behaviour): + + def __init__(self, name = "CameraServicePytree"): + + self.name = name + self.server_state = None + self.service_client_camera = None + self.client_result = None + self.send_request = True + + # here there is the inizialization of the blackboards + self.blackboards = [] + self.blackboard_camera = self.attach_blackboard_client(name=self.name, namespace=SensorNameSpace.camera.name) + #self.blackboard_camera.register_key("state", access=py_trees.common.Access.WRITE) + + super(CameraServicePytree, self).__init__(name) + self.logger.debug("%s.__init__()" % (self.__class__.__name__)) + + def setup(self,**additional_parameters): + self.service_client_camera = HarmoniActionClient(self.name) + self.server_name = "camera_default" + self.service_client_camera.setup_client(self.server_name, + self._result_callback, + self._feedback_callback) + self.logger.debug("Behavior %s interface action clients have been set up!" % (self.server_name)) + + self.logger.debug("%s.setup()" % (self.__class__.__name__)) + + def initialise(self): + self.logger.debug("%s.initialise()" % (self.__class__.__name__)) + + def update(self): + if self.send_request: + self.send_request = False + self.logger.debug(f"Sending goal to {self.server_name}") + # Send request for each sensor service to set themselves up + self.service_client_camera.send_goal( + action_goal=ActionType["ON"].value, + optional_data="Setup", + wait="", + ) + self.logger.debug(f"Goal sent to {self.server_name}") + new_status = py_trees.common.Status.RUNNING + else: + new_state = self.service_client_camera.get_state() + print(new_state) + if new_state == GoalStatus.ABORTED: + #FIXME dovrebbe essere .FAILURE + new_status = py_trees.common.Status.SUCCESS + elif new_state == GoalStatus.SUCCEEDED: + new_status = py_trees.common.Status.SUCCESS + else: + new_status = py_trees.common.Status.FAILURE + raise + + + self.logger.debug("%s.update()[%s]--->[%s]" % (self.__class__.__name__, self.status, new_status)) + return new_status + + def terminate(self, new_status): + """ + new_state = self.service_client_camera.get_state() + print("terminate : ",new_state) + if new_state == GoalStatus.SUCCEEDED or new_state == GoalStatus.ABORTED or new_state == GoalStatus.LOST: + self.send_request = True + if new_state == GoalStatus.PENDING: + self.send_request = True + self.logger.debug(f"Cancelling goal to {self.server_name}") + self.service_client_camera.cancel_all_goals() + self.client_result = None + self.logger.debug(f"Goal cancelled to {self.server_name}") + self.service_client_camera.stop_tracking_goal() + self.logger.debug(f"Goal tracking stopped to {self.server_name}") + """ + self.logger.debug("%s.terminate()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + + def _result_callback(self, result): + """ Recieve and store result with timestamp """ + self.logger.debug("The result of the request has been received") + self.logger.debug( + f"The result callback message from {result['service']} was {len(result['message'])} long" + ) + self.client_result = result["message"] + return + + def _feedback_callback(self, feedback): + """ Feedback is currently just logged """ + self.logger.debug("The feedback recieved is %s." % feedback) + self.server_state = feedback["state"] + return + +def main(): + #command_line_argument_parser().parse_args() + + py_trees.logging.level = py_trees.logging.Level.DEBUG + + blackboardProva = py_trees.blackboard.Client(name="blackboardProva", namespace="harmoni_camera") + blackboardProva.register_key("result_message", access=py_trees.common.Access.READ) + + rospy.init_node("camera_default", log_level=rospy.INFO) + + print(blackboardProva) + + cameraPyTree = CameraServicePytree("CameraServicePytreeTest") + + additional_parameters = dict([ + ("CameraServicePytree_mode",False)]) + + cameraPyTree.setup(**additional_parameters) + try: + for unused_i in range(0, 10): + cameraPyTree.tick_once() + time.sleep(0.5) + print(blackboardProva) + print("\n") + except KeyboardInterrupt: + print("Exception occurred") + pass + +if __name__ == "__main__": + main() diff --git a/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/custom_yolo_service.py b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/custom_yolo_service.py new file mode 100755 index 00000000..2d3b6740 --- /dev/null +++ b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/custom_yolo_service.py @@ -0,0 +1,172 @@ +#!/usr/bin/env python3 + +# Common Imports +import rospy +import roslib + +from harmoni_common_lib.constants import * +from actionlib_msgs.msg import GoalStatus +from harmoni_common_lib.service_server import HarmoniServiceServer +from harmoni_common_lib.service_manager import HarmoniServiceManager +from harmoni_common_lib.action_client import HarmoniActionClient +from harmoni_imageai.custom_service import ImageAICustomService +import harmoni_common_lib.helper_functions as hf + +# Specific Imports +from harmoni_common_lib.constants import DetectorNameSpace, ActionType +from sensor_msgs.msg import Image +from imageai.Detection.Custom import CustomVideoObjectDetection +from botocore.exceptions import BotoCoreError, ClientError +from contextlib import closing +from collections import deque +import numpy as np +import boto3 +import re +import json +import ast +import sys + +#py_tree +import py_trees +import time + +import py_trees.console + +class ImageAICustomServicePytree(py_trees.behaviour.Behaviour): + + def __init__(self, name = "ImageAICustomServicePytree"): + self.name = name + self.server_state = None + self.service_client_custom = None + self.client_result = None + self.server_name = None + self.send_request = True + + self.blackboards = [] + self.blackboard_card_detection = self.attach_blackboard_client(name=self.name, namespace=DetectorNameSpace.card_detect.name) + #self.blackboard_card_detection.register_key("state", access=py_trees.common.Access.WRITE) + self.blackboard_card_detection.register_key("result", access=py_trees.common.Access.WRITE) + + + super(ImageAICustomServicePytree, self).__init__(name) + self.logger.debug("%s.__init__()" % (self.__class__.__name__)) + + def setup(self,**additional_parameters): + self.service_client_custom = HarmoniActionClient(self.name) + self.server_name = "imageai_custom_yolo_default" + self.service_client_custom.setup_client(self.server_name, + self._result_callback, + self._feedback_callback) + self.logger.debug("Behavior %s interface action clients have been set up!" % (self.server_name)) + + self.blackboard_card_detection.result = "null" + + self.logger.debug("%s.setup()" % (self.__class__.__name__)) + + def initialise(self): + self.logger.debug("%s.initialise()" % (self.__class__.__name__)) + + def update(self): + if self.send_request: + self.send_request = False + self.logger.debug(f"Sending goal to {self.server_name}") + self.service_client_custom.send_goal( + action_goal = ActionType["REQUEST"].value, + optional_data="", + wait=False, + ) + self.logger.debug(f"Goal sent to {self.server_name}") + new_status = py_trees.common.Status.RUNNING + else: + new_state = self.service_client_custom.get_state() + print(new_state) + if new_state == GoalStatus.ACTIVE: + new_status = py_trees.common.Status.RUNNING + elif new_state == GoalStatus.SUCCEEDED: + if self.client_result is not None: + self.blackboard_card_detection.result = self.client_result + self.client_result = None + new_status = py_trees.common.Status.SUCCESS + else: + self.logger.debug(f"Waiting fot the result ({self.server_name})") + new_status = py_trees.common.Status.RUNNING + elif new_state == GoalStatus.PENDING: + self.send_request = True + self.logger.debug(f"Cancelling goal to {self.server_name}") + self.service_client_custom.cancel_all_goals() + self.client_result = None + self.logger.debug(f"Goal cancelled to {self.server_name}") + #self.service_client_custom.stop_tracking_goal() + #self.logger.debug(f"Goal tracking stopped to {self.server_name}") + new_status = py_trees.common.Status.RUNNING + else: + new_status = py_trees.common.Status.FAILURE + raise + + + self.logger.debug("%s.update()[%s]--->[%s]" % (self.__class__.__name__, self.status, new_status)) + return new_status + + + def terminate(self, new_status): + new_state = self.service_client_custom.get_state() + print("terminate : ",new_state) + if new_status == py_trees.common.Status.INVALID: + if new_state == GoalStatus.ACTIVE: + self.send_request = True + if new_state == GoalStatus.SUCCEEDED or new_state == GoalStatus.ABORTED or new_state == GoalStatus.LOST: + self.send_request = True + if new_state == GoalStatus.PENDING: + self.send_request = True + self.logger.debug(f"Cancelling goal to {self.server_name}") + self.service_client_custom.cancel_all_goals() + self.client_result = None + self.logger.debug(f"Goal cancelled to {self.server_name}") + #self.service_client_custom.stop_tracking_goal() + #self.logger.debug(f"Goal tracking stopped to {self.server_name}") + self.logger.debug("%s.terminate()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + + def _result_callback(self, result): + """ Recieve and store result with timestamp """ + self.logger.debug("The result of the request has been received") + self.logger.debug( + f"The result callback message from {result['service']} was {len(result['message'])} long" + ) + self.client_result = result["message"] + return + + def _feedback_callback(self, feedback): + """ Feedback is currently just logged """ + self.logger.debug("The feedback recieved is %s." % feedback) + self.server_state = feedback["state"] + return +def main(): + #command_line_argument_parser().parse_args() + + py_trees.logging.level = py_trees.logging.Level.DEBUG + + blackboardProva = py_trees.blackboard.Client(name="blackboardProva", namespace=DetectorNameSpace.card_detect.name) + blackboardProva.register_key("result", access=py_trees.common.Access.READ) + print(blackboardProva) + + rospy.init_node("imageai_default", log_level=rospy.INFO) + + customPyTree = ImageAICustomServicePytree("ImageAICustomServicePytreeTest") + + additional_parameters = dict([ + ("ImageAICustomServicePytree_mode",False)]) + + customPyTree.setup(**additional_parameters) + try: + + for unused_i in range(0, 12): + customPyTree.tick_once() + time.sleep(1) + print(blackboardProva) + print("\n") + except KeyboardInterrupt: + print("Exception occurred") + pass + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/external_speaker_service.py b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/external_speaker_service.py new file mode 100644 index 00000000..9f9022ab --- /dev/null +++ b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/external_speaker_service.py @@ -0,0 +1,129 @@ +#!/usr/bin/env python3 + +# Common Imports +import rospy, rospkg, roslib + +from harmoni_common_lib.constants import * +from harmoni_common_lib.service_server import HarmoniServiceServer +from harmoni_common_lib.service_manager import HarmoniServiceManager +from harmoni_common_lib.action_client import HarmoniActionClient +from actionlib_msgs.msg import GoalStatus +import harmoni_common_lib.helper_functions as hf +from harmoni_speaker.speaker_service import SpeakerService + +# Specific Imports +from audio_common_msgs.msg import AudioData +from harmoni_common_lib.constants import ActuatorNameSpace, ActionType, State +from botocore.exceptions import BotoCoreError, ClientError +from contextlib import closing +from collections import deque +import soundfile as sf +import numpy as np +import boto3 +import re +import json +import ast +import sys +import time + +# import wget +import contextlib +import ast +import wave +import os + +#py_tree +import py_trees + +class ExternalSpeakerServicePytree(py_trees.behaviour.Behaviour): + + def __init__(self, name = "ExternalSpeakerServicePytree"): + self.name = name + self.service_client_ext_speaker = None + self.client_result = None + self.server_state = None + self.server_name = None + self.send_request = True + + self.blackboards = [] + self.blackboard_ext_speaker = self.attach_blackboard_client(name=self.name, namespace=PyTreeNameSpace.scene.name) + self.blackboard_ext_speaker.register_key("sound", access=py_trees.common.Access.READ) + + super(ExternalSpeakerServicePytree, self).__init__(name) + self.logger.debug("%s.__init__()" % (self.__class__.__name__)) + + def setup(self,**additional_parameters): + """ + for parameter in additional_parameters: + print(parameter, additional_parameters[parameter]) + if(parameter ==ActuatorNameSpace.speaker.name): + self.mode = additional_parameters[parameter] + """ + self.service_client_ext_speaker = HarmoniActionClient(self.name) + self.server_name = "speaker_default" + self.service_client_ext_speaker.setup_client(self.server_name, + self._result_callback, + self._feedback_callback) + self.logger.debug("Behavior %s interface action clients have been set up!" % (self.server_name)) + + self.logger.debug("%s.setup()" % (self.__class__.__name__)) + + def initialise(self): + self.logger.debug("%s.initialise()" % (self.__class__.__name__)) + + + def update(self): + if self.send_request: + self.logger.debug(f"Sending goal to {self.server_name}") + self.service_client_ext_speaker.send_goal( + action_goal = ActionType["DO"].value, + optional_data="", + wait=False, + ) + self.logger.debug(f"Goal sent to {self.server_name}") + new_status = py_trees.common.Status.RUNNING + else: + new_state = self.service_client_ext_speaker.get_state() + print(new_state) + if new_state == GoalStatus.ACTIVE: + new_status = py_trees.common.Status.RUNNING + elif new_state == GoalStatus.SUCCEEDED: + new_status = py_trees.common.Status.SUCCESS + else: + new_status = py_trees.common.Status.FAILURE + + self.logger.debug("%s.update()[%s]--->[%s]" % (self.__class__.__name__, self.status, new_status)) + return new_status + + def terminate(self, new_status): + new_state = self.service_client_ext_speaker.get_state() + print("terminate : ",new_state) + if new_state == GoalStatus.SUCCEEDED or new_state == GoalStatus.ABORTED or new_state == GoalStatus.LOST: + self.send_request = True + if new_state == GoalStatus.PENDING: + self.send_request = True + self.logger.debug(f"Cancelling goal to {self.server_name}") + self.service_client_ext_speaker.cancel_all_goals() + self.client_result = None + self.logger.debug(f"Goal cancelled to {self.server_name}") + #self.service_client_ext_speaker.stop_tracking_goal() + #self.logger.debug(f"Goal tracking stopped to {self.server_name}") + self.logger.debug("%s.terminate()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + + def _result_callback(self, result): + """ Recieve and store result with timestamp """ + self.logger.debug("The result of the request has been received") + self.logger.debug( + f"The result callback message from {result['service']} was {len(result['message'])} long" + ) + self.client_result = result["response"] + return + + def _feedback_callback(self, feedback): + """ Feedback is currently just logged """ + self.logger.debug("The feedback recieved is %s." % feedback) + self.server_state = feedback["state"] + return + +def main(): + rospy.init_node("speaker_default", log_level=rospy.INFO) \ No newline at end of file diff --git a/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/facial_exp_service.py b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/facial_exp_service.py new file mode 100755 index 00000000..c5691582 --- /dev/null +++ b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/facial_exp_service.py @@ -0,0 +1,166 @@ +#!/usr/bin/env python3 + +# Common Imports +import rospy, rospkg, roslib + +from harmoni_common_lib.constants import * +from harmoni_common_lib.service_server import HarmoniServiceServer +from harmoni_common_lib.service_manager import HarmoniServiceManager +from harmoni_common_lib.action_client import HarmoniActionClient +from actionlib_msgs.msg import GoalStatus +import harmoni_common_lib.helper_functions as hf +from harmoni_face.face_service import EyesService, MouthService, NoseService +from harmoni_face.face_client import Face + +# Specific Imports +from audio_common_msgs.msg import AudioData +from harmoni_common_lib.constants import ActuatorNameSpace, ActionType, State +from botocore.exceptions import BotoCoreError, ClientError +from contextlib import closing +from collections import deque +import soundfile as sf +import numpy as np +import boto3 +import re +import json +import ast +import sys +import time + +# import wget +import contextlib +import ast +import wave +import os + +#py_tree +import py_trees + +class FacialExpServicePytree(py_trees.behaviour.Behaviour): + def __init__(self, name): + + self.name = name + self.service_client_mouth = None + self.service_client_eyes = None + self.service_client_nose = None + self.client_result = None + self.send_request = True + + self.blackboards = [] + + self.blackboard_scene = self.attach_blackboard_client(name=self.name, namespace=PyTreeNameSpace.scene.name) + self.blackboard_scene.register_key("face_exp", access=py_trees.common.Access.READ) + + super(FacialExpServicePytree, self).__init__(name) + self.logger.debug("%s.__init__()" % (self.__class__.__name__)) + + def setup(self,**additional_parameters): + self.server_name = "face" + self.instance_id = "default" + self.name_mouth = ActuatorNameSpace.face.name + "_mouth_" + self.instance_id + self.service_client_mouth = HarmoniActionClient(self.name_mouth) + self.name_nose = ActuatorNameSpace.face.name + "_nose_" + self.instance_id + self.service_client_nose = HarmoniActionClient(self.name_nose) + self.name_eyes = ActuatorNameSpace.face.name + "_eyes_" + self.instance_id + self.service_client_eyes = HarmoniActionClient(self.name_eyes) + self.service_client_mouth.setup_client(self.name_mouth, self._result_callback, self._feedback_callback) + self.service_client_eyes.setup_client(self.name_eyes, self._result_callback, self._feedback_callback) + self.service_client_nose.setup_client(self.name_nose, self._result_callback, self._feedback_callback) + self.logger.debug("Behavior %s interface action clients have been set up!" % (self.server_name)) + + self.logger.debug("%s.setup()" % (self.__class__.__name__)) + + def initialise(self): + self.logger.debug("%s.initialise()" % (self.__class__.__name__)) + + def update(self): + + if self.send_request: + self.send_request = False + self.data = self.blackboard_scene.face_exp + self.logger.debug(f"Sending goal to {self.server_name}") + self.service_client_mouth.send_goal( + action_goal=ActionType.DO.value, + optional_data=self.data, + wait=True, + ) + self.logger.debug(f"Goal sent to {self.server_name}") + new_status = py_trees.common.Status.RUNNING + else: + new_state = self.service_client_mouth.get_state() + print(new_state) + if new_state == GoalStatus.ACTIVE: + new_status = py_trees.common.Status.RUNNING + elif new_state == GoalStatus.SUCCEEDED: + new_status = py_trees.common.Status.SUCCESS + else: + new_status = py_trees.common.Status.FAILURE + + self.logger.debug("%s.update()[%s]--->[%s]" % (self.__class__.__name__, self.status, new_status)) + return new_status + + def terminate(self, new_status): + new_state = self.service_client_mouth.get_state() + print("terminate : ",new_state) + if new_state == GoalStatus.SUCCEEDED or new_state == GoalStatus.ABORTED or new_state == GoalStatus.LOST: + self.send_request = True + if new_state == GoalStatus.PENDING: + self.send_request = True + self.logger.debug(f"Cancelling goal to {self.server_name}") + self.service_client_mouth.cancel_all_goals() + self.client_result = None + self.logger.debug(f"Goal cancelled to {self.server_name}") + #self.service_client_mouth.stop_tracking_goal() + #self.logger.debug(f"Goal tracking stopped to {self.server_name}") + self.logger.debug("%s.terminate()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + + def _result_callback(self, result): + """ Recieve and store result with timestamp """ + self.logger.debug("The result of the request has been received") + self.logger.debug( + f"The result callback message from {result['service']} was {len(result['message'])} long" + ) + self.client_result = result["message"] + return + + def _feedback_callback(self, feedback): + """ Feedback is currently just logged """ + self.logger.debug("The feedback recieved is %s." % feedback) + self.server_state = feedback["state"] + return + +def main(): + #command_line_argument_parser().parse_args() + py_trees.logging.level = py_trees.logging.Level.DEBUG + + blackboardProva = py_trees.blackboard.Client(name="blackboardProva", namespace=PyTreeNameSpace.scene.name) + blackboardProva.register_key("face_exp", access=py_trees.common.Access.WRITE) + + blackboardProva.face_exp = "[{'start': 1, 'type': 'viseme', 'id': 'POSTALVEOLAR'}]" + """ + [{'start':10, 'type': 'gaze', 'id':'target', 'point': [1,5,10]}] + [{'start': 1, 'type': 'au', 'id': 'au13', 'pose': 1}] + [{'start': 2, 'type': 'action', 'id': 'breath_face'}] + [{'start': 5, 'type': 'action', 'id': 'saucy_face'}] + [{'start': 8, 'type': 'viseme', 'id': 'POSTALVEOLAR'}] + """ + + print(blackboardProva) + + rospy.init_node("face_default", log_level=rospy.INFO) + + facePyTree = FacialExpServicePytree("FaceServiceTest") + + facePyTree.setup() + try: + for unused_i in range(0, 12): + facePyTree.tick_once() + time.sleep(0.5) + print(blackboardProva) + print("\n") + except KeyboardInterrupt: + print("Exception occurred") + pass + +if __name__ == "__main__": + main() diff --git a/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/gesture_service.py b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/gesture_service.py new file mode 100755 index 00000000..d211693a --- /dev/null +++ b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/gesture_service.py @@ -0,0 +1,154 @@ +#!/usr/bin/env python3 + +# Common Imports +import rospy +import roslib + +from harmoni_common_lib.constants import State +from actionlib_msgs.msg import GoalStatus +from harmoni_common_lib.service_server import HarmoniServiceServer +from harmoni_common_lib.service_manager import HarmoniServiceManager +from harmoni_common_lib.action_client import HarmoniActionClient +import harmoni_common_lib.helper_functions as hf +from harmoni_gesture.gesture_service import GestureService +# Specific Imports +from harmoni_common_lib.constants import ActuatorNameSpace, ActionType, State +from harmoni_gesture.qt_gesture_interface import GestureInterface +from botocore.exceptions import BotoCoreError, ClientError +from contextlib import closing +from collections import deque +import soundfile as sf +import numpy as np +import boto3 +import re +import json +import ast +import sys + +#py_tree +import py_trees +import time + +import py_trees.console + +class GestureServicePytree(py_trees.behaviour.Behaviour): + + def __init__(self, name): + self.name = name + self.server_state = None + self.service_client_gesture = None + self.client_result = None + self.send_request = True + + # here there is the inizialization of the blackboards + self.blackboards = [] + self.blackboard_scene = self.attach_blackboard_client(name=self.name, namespace=PyTreeNameSpace.scene.name) + self.blackboard_scene.register_key("gesture", access=py_trees.common.Access.READ) + + super(GestureServicePytree, self).__init__(name) + self.logger.debug("%s.__init__()" % (self.__class__.__name__)) + + def setup(self,**additional_parameters): + + #self.qt_gesture_service = GestureInterface(service_name, params) + #self.gesture_service = GestureService(service_name,params) + #TODO the first parameter in setup_client must be "equals" in all the leaves + self.service_client_gesture = HarmoniActionClient(self.name) + self.client_result = deque() + self.server_name = service_name + "_" + instance_id + self.service_client_gesture.setup_client(self.server_name, + self._result_callback, + self._feedback_callback) + self.logger.debug("Behavior %s interface action clients have been set up!" % (self.server_name)) + self.logger.debug("%s.setup()" % (self.__class__.__name__)) + + def initialise(self): + self.logger.debug("%s.initialise()" % (self.__class__.__name__)) + + def update(self): + new_state = self.service_client_gesture.get_state() + print(new_state) + if self.send_request: + self.send_request = False + self.logger.debug(f"Sending goal to {self.server_name}") + self.service_client_gesture.send_goal( + action_goal = ActionType["DO"].value, + optional_data = self.blackboard_scene.gesture, + wait=False, + ) + self.logger.debug(f"Goal sent to {self.server_name}") + new_status = py_trees.common.Status.RUNNING + else: + if new_state == GoalStatus.ACTIVE: + new_status = py_trees.common.Status.RUNNING + elif new_state == GoalStatus.SUCCEEDED: + new_status = py_trees.common.Status.SUCCESS + else: + new_status = py_trees.common.Status.FAILURE + + self.logger.debug("%s.update()[%s]--->[%s]" % (self.__class__.__name__, self.status, new_status)) + return new_status + + def terminate(self, new_status): + """ + new_state = self.service_client_gesture.get_state() + print("terminate : ",new_state) + if new_state == GoalStatus.SUCCEEDED or new_state == GoalStatus.ABORTED or new_state == GoalStatus.LOST: + self.send_request = True + if new_state == GoalStatus.PENDING: + self.send_request = True + self.logger.debug(f"Cancelling goal to {self.server_name}") + self.service_client_gesture.cancel_all_goals() + self.client_result = None + self.logger.debug(f"Goal cancelled to {self.server_name}") + self.service_client_gesture.stop_tracking_goal() + self.logger.debug(f"Goal tracking stopped to {self.server_name}") + """ + self.logger.debug("%s.terminate()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + + def _result_callback(self, result): + """ Recieve and store result with timestamp """ + self.logger.debug("The result of the request has been received") + self.logger.debug( + f"The result callback message from {result['service']} was {len(result['message'])} long" + ) + self.client_result = result["message"] + return + + def _feedback_callback(self, feedback): + """ Feedback is currently just logged """ + self.logger.debug("The feedback recieved is %s." % feedback) + self.server_state = feedback["state"] + return + +def main(): + #command_line_argument_parser().parse_args() + + py_trees.logging.level = py_trees.logging.Level.DEBUG + + blackboardProva = py_trees.blackboard.Client(name="blackboardProva", namespace="harmoni_gesture") + blackboardProva.register_key("result_data", access=py_trees.common.Access.WRITE) + blackboardProva.register_key("result_message", access=py_trees.common.Access.WRITE) + + blackboardProva.result_message = "SUCCESS" + blackboardProva.result_data = "{'gesture':'QT/sad', 'timing': 2}" + + print(blackboardProva) + + rospy.init_node("gesture_default", log_level=rospy.INFO) + + gesturePyTree = GestureServicePytree("GestureServiceTest") + + additional_parameters = dict([ + ("GestureServicePytree_mode",False)]) + + gesturePyTree.setup(**additional_parameters) + try: + for unused_i in range(0, 7): + gesturePyTree.tick_once() + time.sleep(0.5) + print(blackboardProva) + print("\n") + except KeyboardInterrupt: + print("Exception occurred") + pass \ No newline at end of file diff --git a/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/google_service.py b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/google_service.py new file mode 100755 index 00000000..e18da8a2 --- /dev/null +++ b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/google_service.py @@ -0,0 +1,176 @@ +#!/usr/bin/env python3 + +# Common Imports +import rospy +import roslib + +from harmoni_common_lib.constants import * +from actionlib_msgs.msg import GoalStatus +from harmoni_common_lib.service_server import HarmoniServiceServer +from harmoni_common_lib.service_manager import HarmoniServiceManager +from harmoni_common_lib.action_client import HarmoniActionClient +from harmoni_imageai.custom_service import ImageAICustomService +import harmoni_common_lib.helper_functions as hf + +# Specific Imports +from harmoni_common_lib.constants import DetectorNameSpace, ActionType +from sensor_msgs.msg import Image +from imageai.Detection.Custom import CustomVideoObjectDetection +from botocore.exceptions import BotoCoreError, ClientError +from contextlib import closing +from collections import deque +import numpy as np +import boto3 +import re +import json +import ast +import sys + +#py_tree +import py_trees +import time + +import py_trees.console + +class SpeechToTextServicePytree(py_trees.behaviour.Behaviour): + + def __init__(self, name = "SpeechToTextServicePytree"): + + self.name = name + self.service_client_stt = None + self.client_result = None + self.server_state = None + self.server_name = None + self.send_request = True + + self.blackboards = [] + self.blackboard_microphone = self.attach_blackboard_client(name=self.name, namespace=SensorNameSpace.microphone.name) + self.blackboard_microphone.register_key("state", access=py_trees.common.Access.READ) + self.blackboard_stt = self.attach_blackboard_client(name=self.name, namespace=DetectorNameSpace.stt.name) + self.blackboard_stt.register_key("result", access=py_trees.common.Access.WRITE) + + super(SpeechToTextServicePytree, self).__init__(name) + self.logger.debug("%s.__init__()" % (self.__class__.__name__)) + + def setup(self,**additional_parameters): + """ + for parameter in additional_parameters: + print(parameter, additional_parameters[parameter]) + if(parameter =="SpeechToTextServicePytree_mode"): + self.mode = additional_parameters[parameter] + """ + self.service_client_stt = HarmoniActionClient(self.name) + self.server_name = "stt_default" + self.service_client_stt.setup_client(self.server_name, + self._result_callback, + self._feedback_callback) + self.logger.debug("Behavior %s interface action clients have been set up!" % (self.server_name)) + + self.blackboard_stt.result = "null" + + self.logger.debug("%s.setup()" % (self.__class__.__name__)) + + def initialise(self): + self.logger.debug("%s.initialise()" % (self.__class__.__name__)) + + def update(self): + + if self.send_request: + self.send_request = False + self.logger.debug(f"Sending goal to {self.server_name}") + self.service_client_stt.send_goal( + action_goal = ActionType["REQUEST"].value, + optional_data="", + wait=False, + ) + self.logger.debug(f"Goal sent to {self.server_name}") + new_status = py_trees.common.Status.RUNNING + else: + new_state = self.service_client_stt.get_state() + print(new_state) + if new_state == GoalStatus.ACTIVE: + new_status = py_trees.common.Status.RUNNING + elif new_state == GoalStatus.SUCCEEDED: + if self.client_result is not None: + self.blackboard_stt.result = self.client_result + self.client_result = None + new_status = py_trees.common.Status.SUCCESS + else: + self.logger.debug(f"Waiting fot the result ({self.server_name})") + new_status = py_trees.common.Status.RUNNING + elif new_state == GoalStatus.PENDING: + self.send_request = True + self.logger.debug(f"Cancelling goal to {self.server_name}") + self.service_client_stt.cancel_all_goals() + self.client_result = None + self.logger.debug(f"Goal cancelled to {self.server_name}") + #self.service_client_stt.stop_tracking_goal() + #self.logger.debug(f"Goal tracking stopped to {self.server_name}") + new_status = py_trees.common.Status.RUNNING + else: + new_status = py_trees.common.Status.FAILURE + raise + + self.logger.debug("%s.update()[%s]--->[%s]" % (self.__class__.__name__, self.status, new_status)) + return new_status + + def terminate(self, new_status): + new_state = self.service_client_stt.get_state() + print("terminate : ",new_state) + if new_status == py_trees.common.Status.INVALID: + if new_state == GoalStatus.ACTIVE: + self.send_request = True + if new_state == GoalStatus.SUCCEEDED or new_state == GoalStatus.ABORTED or new_state == GoalStatus.LOST: + self.send_request = True + if new_state == GoalStatus.PENDING: + self.send_request = True + self.logger.debug(f"Cancelling goal to {self.server_name}") + self.service_client_stt.cancel_all_goals() + self.client_result = None + self.logger.debug(f"Goal cancelled to {self.server_name}") + #self.service_client_stt.stop_tracking_goal() + #self.logger.debug(f"Goal tracking stopped to {self.server_name}") + + self.logger.debug("%s.terminate()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + + def _result_callback(self, result): + """ Recieve and store result with timestamp """ + self.logger.debug("The result of the request has been received") + self.logger.debug( + f"The result callback message from {result['service']} was {len(result['message'])} long" + ) + self.client_result = result["message"] + return + + def _feedback_callback(self, feedback): + """ Feedback is currently just logged """ + self.logger.debug("The feedback recieved is %s." % feedback) + self.server_state = feedback["state"] + return + +def main(): + #command_line_argument_parser().parse_args() + + py_trees.logging.level = py_trees.logging.Level.DEBUG + + blackboardProva = py_trees.blackboard.Client(name="blackboardProva", namespace=DetectorNameSpace.stt.name) + blackboardProva.register_key("result", access=py_trees.common.Access.READ) + print(blackboardProva) + + rospy.init_node("stt_default", log_level=rospy.INFO) + + sttPyTree = SpeechToTextServicePytree("GoogleSSTPytreeTest") + + sttPyTree.setup() + try: + for unused_i in range(0, 20): + sttPyTree.tick_once() + time.sleep(2) + print(blackboardProva) + print("\n") + except KeyboardInterrupt: + print("Exception occurred") + pass + +if __name__ == "__main__": + main() diff --git a/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/lip_sync_service.py b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/lip_sync_service.py new file mode 100755 index 00000000..6b0b9f63 --- /dev/null +++ b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/lip_sync_service.py @@ -0,0 +1,176 @@ +#!/usr/bin/env python3 + +# Common Imports +import rospy, rospkg, roslib + +from harmoni_common_lib.constants import * +from harmoni_common_lib.service_server import HarmoniServiceServer +from harmoni_common_lib.service_manager import HarmoniServiceManager +from harmoni_common_lib.action_client import HarmoniActionClient +from actionlib_msgs.msg import GoalStatus +import harmoni_common_lib.helper_functions as hf +from harmoni_face.face_service import EyesService, MouthService, NoseService +from harmoni_face.face_client import Face + +# Specific Imports +from audio_common_msgs.msg import AudioData +from harmoni_common_lib.constants import ActuatorNameSpace, ActionType, State +from botocore.exceptions import BotoCoreError, ClientError +from contextlib import closing +from collections import deque +import soundfile as sf +import numpy as np +import boto3 +import re +import json +import ast +import sys +import time + +# import wget +import contextlib +import ast +import wave +import os + +#py_tree +import py_trees + +class LipSyncServicePytree(py_trees.behaviour.Behaviour): + def __init__(self, name): + + self.name = name + self.service_client_mouth = None + self.service_client_eyes = None + self.service_client_nose = None + self.client_result = None + self.send_request = True + + self.blackboards = [] + self.blackboard_tts = self.attach_blackboard_client(name=self.name, namespace=ActuatorNameSpace.tts.name) + self.blackboard_tts.register_key("result", access=py_trees.common.Access.READ) + self.blackboard_lips = self.attach_blackboard_client(name=self.name, namespace=Resources.face.value[1]) + #self.blackboard_lips.register_key("state", access=py_trees.common.Access.WRITE) + + super(LipSyncServicePytree, self).__init__(name) + self.logger.debug("%s.__init__()" % (self.__class__.__name__)) + + def setup(self,**additional_parameters): + self.logger.debug("Begin %s.setup()" % (self.__class__.__name__)) + """ + for parameter in additional_parameters: + print(parameter, additional_parameters[parameter]) + if(parameter == ActuatorNameSpace.face.name): + self.mode = additional_parameters[parameter] + """ + self.service_client_face = HarmoniActionClient(self.name) + self.server_name = "face" + + #self.service_client_face.setup_client(self.server_name, self._result_callback, self._feedback_callback) + + self.instance_id = "default" + + self.name_mouth = ActuatorNameSpace.face.name + "_mouth_" + self.instance_id + self.service_client_mouth = HarmoniActionClient(self.name_mouth) + self.name_nose = ActuatorNameSpace.face.name + "_nose_" + self.instance_id + self.service_client_nose = HarmoniActionClient(self.name_nose) + self.name_eyes = ActuatorNameSpace.face.name + "_eyes_" + self.instance_id + self.service_client_eyes = HarmoniActionClient(self.name_eyes) + self.service_client_mouth.setup_client(self.name_mouth, self._result_callback, self._feedback_callback) + self.service_client_eyes.setup_client(self.name_eyes, self._result_callback, self._feedback_callback) + self.service_client_nose.setup_client(self.name_nose, self._result_callback, self._feedback_callback) + + self.logger.debug("Behavior %s interface action clients have been set up!" % (self.server_name)) + + self.logger.debug("End %s.setup()" % (self.__class__.__name__)) + + def initialise(self): + self.logger.debug("%s.initialise()" % (self.__class__.__name__)) + + def update(self): + new_state = self.service_client_mouth.get_state() + print(new_state) + if self.send_request: + self.send_request = False + self.logger.debug(f"Sending goal to {self.server_name}") + self.service_client_mouth.send_goal( + action_goal = ActionType["DO"].value, + optional_data = self.blackboard_tts.result, + wait=False, + ) + self.logger.debug(f"Goal sent to {self.server_name}") + new_status = py_trees.common.Status.RUNNING + else: + if new_state == GoalStatus.ACTIVE: + new_status = py_trees.common.Status.RUNNING + elif new_state == GoalStatus.SUCCEEDED: + new_status = py_trees.common.Status.SUCCESS + else: + new_status = py_trees.common.Status.FAILURE + + self.logger.debug("%s.update()[%s]--->[%s]" % (self.__class__.__name__, self.status, new_status)) + return new_status + + def terminate(self, new_status): + new_state = self.service_client_mouth.get_state() + print("terminate : ",new_state) + if new_state == GoalStatus.SUCCEEDED or new_state == GoalStatus.ABORTED or new_state == GoalStatus.LOST: + self.send_request = True + if new_state == GoalStatus.PENDING: + self.send_request = True + self.logger.debug(f"Cancelling goal to {self.server_name}") + self.service_client_mouth.cancel_all_goals() + self.client_result = None + self.logger.debug(f"Goal cancelled to {self.server_name}") + #self.service_client_mouth.stop_tracking_goal() + #self.logger.debug(f"Goal tracking stopped to {self.server_name}") + self.logger.debug("%s.terminate()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + + def _result_callback(self, result): + """ Recieve and store result with timestamp """ + self.logger.debug("The result of the request has been received") + self.logger.debug( + f"The result callback message from {result['service']} was {len(result['message'])} long" + ) + self.client_result = result["message"] + return + + def _feedback_callback(self, feedback): + """ Feedback is currently just logged """ + self.logger.debug("The feedback recieved is %s." % feedback) + self.server_state = feedback["state"] + return + +def main(): + #command_line_argument_parser().parse_args() + pass + """ + py_trees.logging.level = py_trees.logging.Level.DEBUG + + blackboardProva = py_trees.blackboard.Client(name="blackboardProva", namespace="harmoni_gesture") + blackboardProva.register_key("result_data", access=py_trees.common.Access.WRITE) + blackboardProva.register_key("result_message", access=py_trees.common.Access.WRITE) + + blackboardProva.result_message = "SUCCESS" + blackboardProva.result_data = "{'gesture':'QT/sad', 'timing': 2}" + + rospy.init_node + + print(blackboardProva) + + gesturePyTree = GestureServicePytree("GestureServiceTest") + + additional_parameters = dict([ + ("GestureServicePytree_mode",False)]) + + gesturePyTree.setup(**additional_parameters) + try: + for unused_i in range(0, 7): + gesturePyTree.tick_once() + time.sleep(0.5) + print(blackboardProva) + print("\n") + except KeyboardInterrupt: + print("Exception occurred") + pass + """ \ No newline at end of file diff --git a/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/microphone_service.py b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/microphone_service.py new file mode 100755 index 00000000..9d61748d --- /dev/null +++ b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/microphone_service.py @@ -0,0 +1,162 @@ +#!/usr/bin/env python3 + +# Common Imports +import rospy +import roslib + +from harmoni_common_lib.constants import State +from actionlib_msgs.msg import GoalStatus +from harmoni_common_lib.action_client import HarmoniActionClient +import harmoni_common_lib.helper_functions as hf +#from harmoni_microphone.microphone_service import MicrophoneService +# Other Imports +from harmoni_common_lib.constants import SensorNameSpace +from audio_common_msgs.msg import AudioData +import pyaudio +import wave +import numpy as np +# Specific Imports +from harmoni_common_lib.constants import ActuatorNameSpace, ActionType, DialogueNameSpace +from botocore.exceptions import BotoCoreError, ClientError +from contextlib import closing +from collections import deque +import soundfile as sf +import numpy as np +import boto3 +import re +import json +import ast +import sys + +#py_tree +import py_trees +import time + +import py_trees.console + +class MicrophoneServicePytree(py_trees.behaviour.Behaviour): + + def __init__(self, name = "MicrophoneServicePytree"): + + self.name = name + self.server_state = None + self.service_client_microphone = None + self.client_result = None + self.send_request = True + + self.blackboards = [] + self.blackboard_microphone = self.attach_blackboard_client(name=self.name, namespace=SensorNameSpace.microphone.name) + #self.blackboard_microphone.register_key("state", access=py_trees.common.Access.WRITE) + + super(MicrophoneServicePytree, self).__init__(name) + self.logger.debug("%s.__init__()" % (self.__class__.__name__)) + + def setup(self,**additional_parameters): + """ + for parameter in additional_parameters: + print(parameter, additional_parameters[parameter]) + if(parameter =="MicrophoneServicePytree_mode"): + self.mode = additional_parameters[parameter] + """ + + self.service_client_microphone = HarmoniActionClient(self.name) + self.server_name = "microphone_default" + self.service_client_microphone.setup_client(self.server_name, + self._result_callback, + self._feedback_callback) + self.logger.debug("Behavior %s interface action clients have been set up!" % (self.server_name)) + + self.logger.debug("%s.setup()" % (self.__class__.__name__)) + + def initialise(self): + self.logger.debug("%s.initialise()" % (self.__class__.__name__)) + + def update(self): + if self.send_request: + self.send_request = False + self.logger.debug(f"Sending goal to {self.server_name}") + # Send request for each sensor service to set themselves up + self.service_client_microphone.send_goal( + action_goal=ActionType["ON"].value, + optional_data="Setup", + wait="", + ) + self.logger.debug(f"Goal sent to {self.server_name}") + new_status = py_trees.common.Status.RUNNING + else: + new_state = self.service_client_microphone.get_state() + print(new_state) + if new_state == GoalStatus.LOST: + new_status = py_trees.common.Status.FAILURE + raise + elif new_state == GoalStatus.ABORTED: + new_status = py_trees.common.Status.SUCCESS + elif new_state == GoalStatus.SUCCEEDED: + new_status = py_trees.common.Status.SUCCESS + else: + new_status = py_trees.common.Status.FAILURE + raise + + self.logger.debug("%s.update()[%s]--->[%s]" % (self.__class__.__name__, self.status, new_status)) + return new_status + + def terminate(self, new_status): + """ + new_state = self.service_client_microphone.get_state() + print("terminate : ",new_state) + if new_state == GoalStatus.SUCCEEDED or new_state == GoalStatus.ABORTED or new_state == GoalStatus.LOST: + self.send_request = True + if new_state == GoalStatus.PENDING: + self.send_request = True + self.logger.debug(f"Cancelling goal to {self.server_name}") + self.service_client_microphone.cancel_all_goals() + self.client_result = None + self.logger.debug(f"Goal cancelled to {self.server_name}") + self.service_client_microphone.stop_tracking_goal() + self.logger.debug(f"Goal tracking stopped to {self.server_name}") + """ + self.logger.debug("%s.terminate()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + + def _result_callback(self, result): + """ Recieve and store result with timestamp """ + self.logger.debug("The result of the request has been received") + self.logger.debug( + f"The result callback message from {result['service']} was {len(result['message'])} long" + ) + self.client_result = result["message"] + return + + def _feedback_callback(self, feedback): + """ Feedback is currently just logged """ + self.logger.debug("The feedback recieved is %s." % feedback) + self.server_state = feedback["state"] + return + + +def main(): + #command_line_argument_parser().parse_args() + + py_trees.logging.level = py_trees.logging.Level.DEBUG + + blackboardProva = py_trees.blackboard.Client(name="blackboardProva", namespace="harmoni_microphone") + blackboardProva.register_key("result_message", access=py_trees.common.Access.READ) + + rospy.init_node("microphone_default", log_level=rospy.INFO) + + print(blackboardProva) + + microphonePyTree = MicrophoneServicePytree("MicrophoneServicePytreeTest") + + additional_parameters = dict([ + ("MicrophoneServicePytree_mode",False)]) + + microphonePyTree.setup(**additional_parameters) + try: + for unused_i in range(0, 3): + microphonePyTree.tick_once() + time.sleep(0.5) + print(blackboardProva) + print("\n") + except KeyboardInterrupt: + print("Exception occurred") + pass \ No newline at end of file diff --git a/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/scene_manager_interactionbg.py b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/scene_manager_interactionbg.py new file mode 100644 index 00000000..fbca183c --- /dev/null +++ b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/scene_manager_interactionbg.py @@ -0,0 +1,132 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +from harmoni_common_lib.constants import * +import py_trees +import random +import rospkg +import json +import os + + +class SceneManagerInteractionBg(py_trees.behaviour.Behaviour): + def __init__(self, name): + self.name = name + + self.blackboards = [] + self.blackboard_scene = self.attach_blackboard_client(name=self.name, namespace=PyTreeNameSpace.scene.name) + #self.blackboard_scene.register_key(key=PyTreeNameSpace.interaction.name+"/state", access=py_trees.common.Access.WRITE) + self.blackboard_scene.register_key(key=PyTreeNameSpace.interaction.name+"/scene_counter", access=py_trees.common.Access.WRITE) + self.blackboard_scene.register_key(key=PyTreeNameSpace.interaction.name+"/max_num_scene", access=py_trees.common.Access.WRITE) #NEW + self.blackboard_scene.register_key(key=PyTreeNameSpace.interaction.name+"/do_trigger", access=py_trees.common.Access.WRITE) #NEW + self.blackboard_scene.register_key(key=PyTreeNameSpace.interaction.name+"/do_kid", access=py_trees.common.Access.WRITE) #NEW + self.blackboard_scene.register_key("utterance", access=py_trees.common.Access.WRITE) + self.blackboard_scene.register_key("face_exp", access=py_trees.common.Access.WRITE) + self.blackboard_interaction = self.attach_blackboard_client(name=self.name, namespace=PyTreeNameSpace.interaction.name) + self.blackboard_interaction.register_key("inside", access=py_trees.common.Access.WRITE) + self.blackboard_interaction.register_key("finished", access=py_trees.common.Access.WRITE) #NEW + self.blackboard_interaction.register_key("call_therapist", access=py_trees.common.Access.WRITE) + self.blackboard_bot = self.attach_blackboard_client(name=self.name, namespace=DialogueNameSpace.bot.name) + self.blackboard_bot.register_key(key=PyTreeNameSpace.analyzer.name +"/"+"result", access=py_trees.common.Access.WRITE) + self.blackboard_bot.register_key(key=PyTreeNameSpace.trigger.name +"/"+"result", access=py_trees.common.Access.WRITE) + + super(SceneManagerInteractionBg, self).__init__(name) + self.logger.debug("%s.__init__()" % (self.__class__.__name__)) + + + def setup(self): + self.logger.debug(" %s [SceneManagerInteractionBg::setup()]" % self.name) + #this is the name of the json without the extension + json_name = "interaction_bg" + rospack = rospkg.RosPack() + pck_path = rospack.get_path("harmoni_pytree") + pattern_script_path = pck_path + f"/resources/{json_name}.json" + with open(pattern_script_path, "r") as read_file: + self.context = json.load(read_file) + + self.blackboard_scene.interaction.max_num_scene = len(self.context["scene"])+1 + self.blackboard_scene.interaction.scene_counter = 0 + self.blackboard_scene.utterance = "null" + self.blackboard_scene.face_exp = "null" + self.blackboard_interaction.call_therapist = False + self.blackboard_interaction.inside = False + self.blackboard_scene.interaction.do_trigger = "null" + self.blackboard_scene.interaction.do_kid = True + + def initialise(self): + self.logger.debug(" %s [SceneManagerInteractionBg::initialise()]" % self.name) + + def update(self): + self.logger.debug(" %s [SceneManagerInteractionBg::update()]" % self.name) + + self.blackboard_scene.interaction.do_trigger = False + self.blackboard_interaction.call_therapist = False + self.blackboard_interaction.finished = False + self.blackboard_scene.interaction.do_kid = True + self.blackboard_interaction.inside = True + self.blackboard_scene.face_exp = "null" + + print("STATE OF SCENE MANAGER INTERACTION") + + if self.blackboard_scene.interaction.scene_counter == 0: + print("self.blackboard_scene.interaction.scene_counter == 0") + self.blackboard_scene.interaction.do_trigger = True + self.blackboard_scene.utterance = self.context["scene"][self.blackboard_scene.interaction.scene_counter]["utterance"] + self.blackboard_scene.face_exp = self.context["scene"][self.blackboard_scene.interaction.scene_counter]["face"] + self.blackboard_scene.interaction.scene_counter += 1 + else: + if self.blackboard_bot.analyzer.result == "void_answer": + print("self.blackboard_bot.analyzer.result == void_answer") + self.blackboard_interaction.call_therapist = True + self.blackboard_scene.utterance = self.context["error_handling"]["terapista"]["utterance"] + self.blackboard_scene.face_exp = self.context["error_handling"]["terapista"]["face"] + else: + intentName = self.blackboard_bot.analyzer.result["intentName"] + dialogState = self.blackboard_bot.analyzer.result["dialogState"] + message = self.blackboard_bot.analyzer.result["message"] + print("Intent name: ", intentName) + print("Dialog state: ", dialogState) + if intentName == "Stop": + print("intentName == Stop") + self.blackboard_interaction.call_therapist = True + self.blackboard_scene.utterance = self.context["error_handling"]["terapista"]["utterance"] + self.blackboard_scene.face_exp = self.context["error_handling"]["terapista"]["face"] + elif intentName == "NonHoCapito": + print("intentName == NonHoCapito") + self.blackboard_interaction.call_therapist = True + self.blackboard_scene.utterance = self.context["error_handling"]["terapista"]["utterance"] + self.blackboard_scene.face_exp = self.context["error_handling"]["terapista"]["face"] + elif dialogState == DialogStateLex.FULFILLED.value or dialogState == DialogStateLex.READY_FOR_FULFILLMENT.value: + print("dialogState == FULFILLED") + self.blackboard_scene.utterance = message + self.blackboard_scene.interaction.do_kid = False + self.blackboard_scene.interaction.scene_counter += 1 + elif dialogState == DialogStateLex.CONFIRM_INTENT.value: + print("dialogState == CONFIRM_INTENT") + self.blackboard_scene.utterance = message + elif dialogState == DialogStateLex.FAILED.value: + print("dialogState == FAILED") + self.blackboard_interaction.call_therapist = True + self.blackboard_scene.utterance = message + + self.blackboard_scene.interaction.do_kid = False + self.blackboard_scene.interaction.scene_counter += 1 + elif dialogState == DialogStateLex.ELICIT_SLOT.value: + print("dialogState == ELICIT_SLOT") + self.blackboard_scene.utterance = message + elif dialogState == DialogStateLex.ELICIT_INTENT.value: + print("dialogState == ELICIT_INTENT") + self.blackboard_scene.utterance = self.context["scene"][0]["utterance"] + self.blackboard_scene.interaction.do_trigger = True + else: + raise + self.blackboard_bot.analyzer.result = "null" + + self.blackboard_bot.trigger.result = { + "message": self.blackboard_scene.utterance + } + return py_trees.common.Status.SUCCESS + + def terminate(self, new_status): + self.logger.debug(" %s [SceneManagerInteractionBg::terminate()][%s->%s]" % (self.name, self.status, new_status)) + diff --git a/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/scene_manager_main.py b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/scene_manager_main.py new file mode 100644 index 00000000..04746bfb --- /dev/null +++ b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/scene_manager_main.py @@ -0,0 +1,251 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +from harmoni_common_lib.constants import * +import py_trees +import random +import os +import json +import time +import rospkg + +class SceneManagerMain(py_trees.behaviour.Behaviour): + def __init__(self, name): + super(SceneManagerMain, self).__init__(name) + + self.blackboards = [] + self.blackboard_scene = self.attach_blackboard_client(name=self.name, namespace=PyTreeNameSpace.scene.name) + #self.blackboard_scene.register_key(PyTreeNameSpace.mainactivity.name+"/state", access=py_trees.common.Access.WRITE) + self.blackboard_scene.register_key(key=PyTreeNameSpace.mainactivity.name+"/scene_counter", access=py_trees.common.Access.WRITE) + self.blackboard_scene.register_key(key=PyTreeNameSpace.mainactivity.name+"/max_num_scene", access=py_trees.common.Access.WRITE) #NEW + self.blackboard_scene.register_key(key=PyTreeNameSpace.mainactivity.name+"/do_kid", access=py_trees.common.Access.WRITE) #NEW + self.blackboard_scene.register_key(key=PyTreeNameSpace.mainactivity.name+"/do_trigger", access=py_trees.common.Access.WRITE) #NEW + self.blackboard_scene.register_key(key="utterance", access=py_trees.common.Access.WRITE) + self.blackboard_scene.register_key(key="face_exp", access=py_trees.common.Access.WRITE) + self.blackboard_scene.register_key(key="gesture", access=py_trees.common.Access.WRITE) + self.blackboard_scene.register_key(key="image", access=py_trees.common.Access.WRITE) + self.blackboard_scene.register_key(key="sound", access=py_trees.common.Access.WRITE) + self.blackboard_bot = self.attach_blackboard_client(name=self.name, namespace=DialogueNameSpace.bot.name) + self.blackboard_bot.register_key(key=PyTreeNameSpace.analyzer.name +"/"+"result", access=py_trees.common.Access.WRITE) + self.blackboard_bot.register_key(key=PyTreeNameSpace.trigger.name +"/"+"result", access=py_trees.common.Access.WRITE) + self.blackboard_mainactivity = self.attach_blackboard_client(name=self.name, namespace = PyTreeNameSpace.mainactivity.name) + self.blackboard_mainactivity.register_key(key="counter_no_answer", access=py_trees.common.Access.WRITE) + self.blackboard_mainactivity.register_key(key="call_therapist", access=py_trees.common.Access.WRITE) + self.blackboard_visual= self.attach_blackboard_client(name=self.name, namespace=PyTreeNameSpace.visual.name) + self.blackboard_visual.register_key("inside", access=py_trees.common.Access.WRITE) + self.blackboard_interaction= self.attach_blackboard_client(name=self.name, namespace=PyTreeNameSpace.interaction.name) + self.blackboard_interaction.register_key("inside", access=py_trees.common.Access.WRITE) + + self.logger.debug("%s.__init__()" % (self.__class__.__name__)) + + def setup(self): + pattern_name = "mainactivity" + rospack = rospkg.RosPack() + pck_path = rospack.get_path("harmoni_pytree") + pattern_script_path = pck_path + f"/resources/{pattern_name}.json" + with open(pattern_script_path, "r") as read_file: + self.context = json.load(read_file) + + self.counter_non_ho_capito = 0 + self.blackboard_scene.mainactivity.max_num_scene = len(self.context["scene"]) + self.blackboard_mainactivity.counter_no_answer = 0 + self.blackboard_scene.mainactivity.scene_counter = 0 + self.blackboard_scene.utterance = "null" + self.blackboard_scene.face_exp = "null" + self.blackboard_scene.gesture = "null" + self.blackboard_scene.image = "null" + self.blackboard_scene.sound = "null" + self.blackboard_mainactivity.call_therapist = False + self.blackboard_scene.mainactivity.do_kid = False + self.blackboard_scene.mainactivity.do_trigger = False + self.error_message = "Mi dispiace, proviamo a chiedere aiuto al terapista" + self.blackboard_visual.inside = False + self.blackboard_interaction.inside = False + + self.logger.debug(" %s [SceneManagerMain::setup()]" % self.name) + + def initialise(self): + self.logger.debug(" %s [SceneManagerMain::initialise()]" % self.name) + + def update(self): + self.logger.debug(" %s [SceneManagerMain::update()]" % self.name) + + self.blackboard_mainactivity.call_therapist = False + print("STATE OF SCENE MANAGER MAIN") + + if self.blackboard_visual.inside == True: + print("visual/inside: true") + self.blackboard_visual.inside = False + if not self.blackboard_scene.mainactivity.do_kid: + self.blackboard_scene.mainactivity.scene_counter -= 1 + self.blackboard_scene.utterance = self.context["error_handling"]["riprendiamo_visual"]["utterance"] + self.blackboard_scene.face_exp = self.context["error_handling"]["riprendiamo_visual"]["face"] + self.blackboard_scene.gesture = self.context["error_handling"]["riprendiamo_visual"]["gesture"] + self.blackboard_scene.image = self.context["error_handling"]["riprendiamo_visual"]["image"] + self.blackboard_scene.sound = self.context["error_handling"]["riprendiamo_visual"]["sound"] + self.blackboard_scene.mainactivity.do_trigger = self.context["error_handling"]["riprendiamo_visual"]["do_trigger"]=="True" + self.blackboard_scene.mainactivity.do_kid = self.blackboard_scene.mainactivity.do_trigger + elif self.blackboard_interaction.inside == True: + print("interaction/inside: true") + self.blackboard_interaction.inside = False + self.blackboard_mainactivity.counter_no_answer = 0 + self.blackboard_scene.utterance = self.context["scene"][self.blackboard_scene.mainactivity.scene_counter]["utterance"] + self.blackboard_scene.face_exp = self.context["scene"][self.blackboard_scene.mainactivity.scene_counter]["face"] + self.blackboard_scene.gesture = self.context["scene"][self.blackboard_scene.mainactivity.scene_counter]["gesture"] + self.blackboard_scene.image = self.context["scene"][self.blackboard_scene.mainactivity.scene_counter]["image"] + self.blackboard_scene.sound = self.context["scene"][self.blackboard_scene.mainactivity.scene_counter]["sound"] + self.blackboard_scene.mainactivity.do_trigger = self.context["scene"][self.blackboard_scene.mainactivity.scene_counter]["do_trigger"] == "True" + else: + self.blackboard_scene.mainactivity.do_kid = False + self.blackboard_scene.mainactivity.do_trigger = self.context["scene"][self.blackboard_scene.mainactivity.scene_counter]["do_trigger"]=="True" + if self.blackboard_scene.mainactivity.do_trigger == True: + print("mainactivity/do_trigger = true") + if self.blackboard_bot.analyzer.result == "null": + print("analyzer/result == null") + self.blackboard_scene.utterance = self.context["scene"][self.blackboard_scene.mainactivity.scene_counter]["utterance"] + self.blackboard_scene.face_exp = self.context["scene"][self.blackboard_scene.mainactivity.scene_counter]["face"] + self.blackboard_scene.gesture = self.context["scene"][self.blackboard_scene.mainactivity.scene_counter]["gesture"] + self.blackboard_scene.image = self.context["scene"][self.blackboard_scene.mainactivity.scene_counter]["image"] + self.blackboard_scene.sound = self.context["scene"][self.blackboard_scene.mainactivity.scene_counter]["sound"] + self.blackboard_scene.mainactivity.do_kid = True + self.counter_non_ho_capito = 0 + else: + if self.blackboard_bot.analyzer.result == "void_answer": + print("analyzer/result == void_answer") + self.blackboard_scene.utterance = self.context["scene"][self.blackboard_scene.mainactivity.scene_counter]["utterance"] + self.blackboard_scene.face_exp = self.context["scene"][self.blackboard_scene.mainactivity.scene_counter]["face"] + self.blackboard_scene.gesture = self.context["scene"][self.blackboard_scene.mainactivity.scene_counter]["gesture"] + self.blackboard_scene.image = self.context["scene"][self.blackboard_scene.mainactivity.scene_counter]["image"] + self.blackboard_scene.sound = self.context["scene"][self.blackboard_scene.mainactivity.scene_counter]["sound"] + self.blackboard_scene.mainactivity.do_kid = True + else: + dialogState = self.blackboard_bot.analyzer.result["dialogState"] + print("dialogState = ", dialogState) + if dialogState == DialogStateLex.FULFILLED.value or dialogState == DialogStateLex.READY_FOR_FULFILLMENT.value: + intentName = self.blackboard_bot.analyzer.result["intentName"] + message = self.blackboard_bot.analyzer.result["message"] + print("intentName = ", intentName) + print("message = ", message) + self.blackboard_scene.utterance = message + if intentName == IntentName.STOP.value: + self.blackboard_mainactivity.call_therapist = True + self.blackboard_scene.face_exp = self.context["error_handling"]["stop"]["face"] + self.blackboard_scene.gesture = self.context["error_handling"]["stop"]["gesture"] + self.blackboard_scene.image = self.context["error_handling"]["stop"]["image"] + self.blackboard_scene.sound = self.context["error_handling"]["stop"]["sound"] + self.blackboard_scene.sound = self.context["error_handling"]["stop"]["sound"] + self.blackboard_scene.mainactivity.do_trigger = self.context["error_handling"]["stop"]["do_trigger"]=="True" + self.blackboard_scene.mainactivity.do_kid = self.blackboard_scene.mainactivity.do_trigger + elif intentName == IntentName.NOCAPITO.value: + self.blackboard_scene.face_exp = self.context["error_handling"]["no_capito"]["face"] + self.blackboard_scene.gesture = self.context["error_handling"]["no_capito"]["gesture"] + self.blackboard_scene.image = self.context["error_handling"]["no_capito"]["image"] + self.blackboard_scene.sound = self.context["error_handling"]["no_capito"]["sound"] + self.blackboard_scene.mainactivity.do_trigger = False + self.blackboard_scene.mainactivity.do_kid = False + else: + if intentName == IntentName.OMBRELLO.value: + self.context["scene"][19]["utterance"] = "Fortunatamente abbiamo portato l'ombrello" + #self.context["scene"][19]["face"] = "[{'start':10, 'type': 'gaze', 'id':'target', 'point': [1,5,10]}]" + self.blackboard_scene.mainactivity.scene_counter += 1 + self.blackboard_scene.face_exp = self.context["error_handling"]["risposta_corretta"]["face"] + self.blackboard_scene.gesture = self.context["error_handling"]["risposta_corretta"]["gesture"] + self.blackboard_scene.image = self.context["error_handling"]["risposta_corretta"]["image"] + self.blackboard_scene.sound = self.context["error_handling"]["risposta_corretta"]["sound"] + self.blackboard_scene.mainactivity.do_trigger = False + self.blackboard_scene.mainactivity.do_kid = False + elif dialogState == DialogStateLex.FAILED.value: + intentName = self.blackboard_bot.analyzer.result["intentName"] + message = self.blackboard_bot.analyzer.result["message"] + print("intentName = ", intentName) + print("message = ", message) + if intentName == IntentName.OMBRELLO.value: + self.context["scene"][18]["utterance"] = "Peccato che non abbiamo portato l'ombrello" + #self.context["scene"][18]["face"] = "[{'start': 5, 'type': 'action', 'id': 'happy_face'}]" + self.blackboard_scene.mainactivity.scene_counter += 1 + self.blackboard_scene.utterance = message + self.blackboard_scene.face_exp = self.context["error_handling"]["risposta_sbagliata"]["face"] + self.blackboard_scene.gesture = self.context["error_handling"]["risposta_sbagliata"]["gesture"] + self.blackboard_scene.image = self.context["error_handling"]["risposta_sbagliata"]["image"] + self.blackboard_scene.sound = self.context["error_handling"]["risposta_sbagliata"]["sound"] + self.blackboard_scene.mainactivity.do_trigger = False + self.blackboard_scene.mainactivity.do_kid = False + elif dialogState == DialogStateLex.ELICIT_SLOT.value: + intentName = self.blackboard_bot.analyzer.result["intentName"] + message = self.blackboard_bot.analyzer.result["message"] + print("intentName = ", intentName) + print("message = ", message) + if intentName == IntentName.CARTA.value or intentName == IntentName.PLASTICA.value or intentName == IntentName.VETRO.value: + if intentName == IntentName.CARTA.value: + oggetto = "tetrapak" + elif intentName == IntentName.PLASTICA.value: + oggetto = "lattina" + elif intentName == IntentName.VETRO.value: + oggetto = "bicchiere" + self.blackboard_scene.utterance = "confirm " + oggetto + self.blackboard_scene.face_exp = self.context["error_handling"]["sei_sicuro"]["face"] + self.blackboard_scene.gesture = self.context["error_handling"]["sei_sicuro"]["gesture"] + self.blackboard_scene.image = self.context["error_handling"]["sei_sicuro"]["image"] + self.blackboard_scene.sound = self.context["error_handling"]["sei_sicuro"]["sound"] + self.blackboard_scene.mainactivity.do_trigger = True + self.blackboard_scene.mainactivity.do_kid = True + else: + self.blackboard_scene.utterance = self.context["scene"][self.blackboard_scene.mainactivity.scene_counter]["utterance"] + self.blackboard_scene.face_exp = self.context["scene"][self.blackboard_scene.mainactivity.scene_counter]["face"] + self.blackboard_scene.gesture = self.context["scene"][self.blackboard_scene.mainactivity.scene_counter]["gesture"] + self.blackboard_scene.image = self.context["scene"][self.blackboard_scene.mainactivity.scene_counter]["image"] + self.blackboard_scene.sound = self.context["scene"][self.blackboard_scene.mainactivity.scene_counter]["sound"] + self.blackboard_scene.mainactivity.do_kid = self.blackboard_scene.mainactivity.do_trigger + elif dialogState == DialogStateLex.ELICIT_INTENT.value: + print("-scene_counter_mainactivity- qui non dovremmo esserci") + self.blackboard_scene.utterance = self.context["scene"][self.blackboard_scene.mainactivity.scene_counter]["utterance"] + self.blackboard_scene.face_exp = self.context["scene"][self.blackboard_scene.mainactivity.scene_counter]["face"] + self.blackboard_scene.gesture = self.context["scene"][self.blackboard_scene.mainactivity.scene_counter]["gesture"] + self.blackboard_scene.image = self.context["scene"][self.blackboard_scene.mainactivity.scene_counter]["image"] + self.blackboard_scene.sound = self.context["scene"][self.blackboard_scene.mainactivity.scene_counter]["sound"] + self.blackboard_scene.mainactivity.do_kid = self.blackboard_scene.mainactivity.do_trigger + elif dialogState == DialogStateLex.CONFIRM_INTENT.value: + print("-scene_counter_mainactivity- qui non dovremmo esserci") + self.blackboard_scene.utterance = self.context["scene"][self.blackboard_scene.mainactivity.scene_counter]["utterance"] + self.blackboard_scene.face_exp = self.context["scene"][self.blackboard_scene.mainactivity.scene_counter]["face"] + self.blackboard_scene.gesture = self.context["scene"][self.blackboard_scene.mainactivity.scene_counter]["gesture"] + self.blackboard_scene.image = self.context["scene"][self.blackboard_scene.mainactivity.scene_counter]["image"] + self.blackboard_scene.sound = self.context["scene"][self.blackboard_scene.mainactivity.scene_counter]["sound"] + self.blackboard_scene.mainactivity.do_kid = self.blackboard_scene.mainactivity.do_trigger + else: + print("mainactivity/do_trigger = false") + self.blackboard_scene.utterance = self.context["scene"][self.blackboard_scene.mainactivity.scene_counter]["utterance"] + self.blackboard_scene.face_exp = self.context["scene"][self.blackboard_scene.mainactivity.scene_counter]["face"] + self.blackboard_scene.gesture = self.context["scene"][self.blackboard_scene.mainactivity.scene_counter]["gesture"] + self.blackboard_scene.image = self.context["scene"][self.blackboard_scene.mainactivity.scene_counter]["image"] + self.blackboard_scene.sound = self.context["scene"][self.blackboard_scene.mainactivity.scene_counter]["sound"] + self.blackboard_mainactivity.counter_no_answer = 0 + self.blackboard_scene.mainactivity.scene_counter += 1 + + self.blackboard_bot.trigger.result = { + "message": self.blackboard_scene.utterance + } + self.blackboard_bot.analyzer.result = "null" + + return py_trees.common.Status.SUCCESS + + def terminate(self, new_status): + self.logger.debug(" %s [SceneManagerMain::terminate().terminate()][%s->%s]" % (self.name, self.status, new_status)) + +def main(): + + py_trees.logging.level = py_trees.logging.Level.DEBUG + + action = SceneManagerMain("ppp") + action.setup() + try: + for unused_i in range(0, 12): + action.tick_once() + time.sleep(0.5) + print("\n") + except KeyboardInterrupt: + pass + + +if __name__ == "__main__": + main() diff --git a/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/scene_manager_visualbg.py b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/scene_manager_visualbg.py new file mode 100644 index 00000000..06743bd5 --- /dev/null +++ b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/scene_manager_visualbg.py @@ -0,0 +1,123 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +import json +import os +import py_trees +import rospkg +import random + +from harmoni_common_lib.constants import * + +class SceneManagerVisualBg(py_trees.behaviour.Behaviour): + def __init__(self, name): + + self.name = name + + self.blackboards = [] + self.blackboard_scene = self.attach_blackboard_client(name=self.name, namespace=PyTreeNameSpace.scene.name) + #self.blackboard_scene.register_key(PyTreeNameSpace.visual.name+"/state", access=py_trees.common.Access.WRITE) + self.blackboard_scene.register_key(key=PyTreeNameSpace.visual.name+"/scene_counter", access=py_trees.common.Access.WRITE) + self.blackboard_scene.register_key(key=PyTreeNameSpace.visual.name+"/do_trigger", access=py_trees.common.Access.WRITE) #NEW + self.blackboard_scene.register_key(key="utterance", access=py_trees.common.Access.WRITE) + self.blackboard_scene.register_key(key="face_exp", access=py_trees.common.Access.WRITE) + self.blackboard_visual = self.attach_blackboard_client(name=self.name, namespace=PyTreeNameSpace.visual.name) + self.blackboard_visual.register_key(key="inside", access=py_trees.common.Access.WRITE) + self.blackboard_visual.register_key(key="finished", access=py_trees.common.Access.WRITE) + self.blackboard_visual.register_key(key="call_therapist", access=py_trees.common.Access.WRITE) + self.blackboard_bot = self.attach_blackboard_client(name=self.name, namespace=DialogueNameSpace.bot.name) + self.blackboard_bot.register_key(key=PyTreeNameSpace.analyzer.name +"/"+"result", access=py_trees.common.Access.WRITE) + self.blackboard_bot.register_key(key=PyTreeNameSpace.trigger.name +"/"+"result", access=py_trees.common.Access.WRITE) + self.blackboard_mainactivity = self.attach_blackboard_client(name=self.name, namespace=PyTreeNameSpace.mainactivity.name) + self.blackboard_mainactivity.register_key(key="counter_no_answer", access=py_trees.common.Access.WRITE) + self.blackboard_interaction = self.attach_blackboard_client(name=self.name, namespace=PyTreeNameSpace.interaction.name) + self.blackboard_interaction.register_key("finished", access=py_trees.common.Access.WRITE) #NEW + self.blackboard_interaction.register_key("inside", access=py_trees.common.Access.WRITE) + + super(SceneManagerVisualBg, self).__init__(name) + self.logger.debug("%s.__init__()" % (self.__class__.__name__)) + + def setup(self): + + #this is the name of the json without the extension + json_name = "visual_bg" + rospack = rospkg.RosPack() + pck_path = rospack.get_path("harmoni_pytree") + pattern_script_path = pck_path + f"/resources/{json_name}.json" + with open(pattern_script_path, "r") as read_file: + self.context = json.load(read_file) + + #self.blackboard_scene.visual.max_num_scene = len(self.context["scene"]) #da cambiare + self.blackboard_mainactivity.counter_no_answer = 0 + self.blackboard_scene.visual.scene_counter = 0 + self.blackboard_scene.utterance = "null" + self.blackboard_scene.face_exp = "null" + self.blackboard_visual.call_therapist = False + self.blackboard_visual.inside = False + self.blackboard_scene.visual.do_trigger = "null" + + self.logger.debug(" %s [SceneManagerVisualBg::setup()]" % self.name) + + def initialise(self): + self.logger.debug(" %s [SceneManagerVisualBg::initialise()]" % self.name) + + def update(self): + + self.logger.debug(" %s [SceneManagerVisualBg::update()]" % self.name) + #reset of interaction + self.blackboard_mainactivity.counter_no_answer = 0 + self.blackboard_interaction.finished = True + self.blackboard_interaction.inside = False + self.blackboard_visual.finished = False + self.blackboard_visual.inside = True + + print("STATE OF SCENE MANAGER VISAUL") + + if self.blackboard_scene.visual.scene_counter == 0: + print("self.blackboard_scene.visual.scene_counter == 0") + self.blackboard_scene.visual.do_trigger = True + self.blackboard_scene.utterance = self.context["scene"][self.blackboard_scene.visual.scene_counter]["utterance"] + self.blackboard_scene.face_exp = self.context["scene"][self.blackboard_scene.visual.scene_counter]["face"] + self.blackboard_scene.visual.scene_counter += 1 + else: + self.blackboard_scene.visual.do_trigger = False + if self.blackboard_bot.analyzer.result == "void_answer": + print("self.blackboard_bot.analyzer.result == void_answer") + self.blackboard_visual.call_therapist = True + self.blackboard_bot.trigger.result = { + "message": self.context["terapista"]["utterance"] + } + self.blackboard_scene.face_exp = self.context["terapista"]["face"] + else: + dialogState = self.blackboard_bot.analyzer.result["dialogState"] + message = self.blackboard_bot.analyzer.result["message"] + if dialogState == DialogStateLex.FAILED.value: + print("dialogState == FAILED") + self.blackboard_bot.trigger.result = { + "message": message + } + + self.blackboard_visual.call_therapist = True + else: + if self.blackboard_scene.visual.scene_counter <= 2: + print("self.blackboard_scene.visual.scene_counter <= 2") + self.blackboard_bot.trigger.result = { + "message": message + } + self.blackboard_scene.visual.scene_counter += 1 + else: + print("self.blackboard_scene.visual.scene_counter > 2") + self.blackboard_bot.trigger.result = { + "message": self.context["terapista"]["utterance"] + } + self.blackboard_scene.face_exp = self.context["terapista"]["face"] + self.blackboard_visual.call_therapist = True + + return py_trees.common.Status.SUCCESS + + def terminate(self, new_status): + """ + if new_status == py_trees.common.Status.INVALID: + self.scene_counter = 0 + """ + self.logger.debug(" %s [SceneManagerVisualBg::terminate().terminate()][%s->%s]" % (self.name, self.status, new_status)) diff --git a/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/speaker_service.py b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/speaker_service.py new file mode 100755 index 00000000..a4b20c88 --- /dev/null +++ b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/speaker_service.py @@ -0,0 +1,148 @@ +#!/usr/bin/env python3 + +# Common Imports +import rospy, rospkg, roslib + +from harmoni_common_lib.constants import * +from harmoni_common_lib.service_server import HarmoniServiceServer +from harmoni_common_lib.service_manager import HarmoniServiceManager +from harmoni_common_lib.action_client import HarmoniActionClient +from actionlib_msgs.msg import GoalStatus +import harmoni_common_lib.helper_functions as hf +from harmoni_speaker.speaker_service import SpeakerService + +# Specific Imports +from audio_common_msgs.msg import AudioData +from harmoni_common_lib.constants import ActuatorNameSpace, ActionType, State +from botocore.exceptions import BotoCoreError, ClientError +from contextlib import closing +from collections import deque +import soundfile as sf +import numpy as np +import boto3 +import re +import json +import ast +import sys +import time + +# import wget +import contextlib +import ast +import wave +import os + +#py_tree +import py_trees + +class SpeakerServicePytree(py_trees.behaviour.Behaviour): + + def __init__(self, name = "SpeakerServicePytree"): + self.name = name + self.service_client_speaker = None + self.client_result = None + self.server_state = None + self.server_name = None + self.send_request = True + + self.blackboards = [] + self.blackboard_tts = self.attach_blackboard_client(name=self.name, namespace=ActuatorNameSpace.tts.name) + self.blackboard_tts.register_key("result", access=py_trees.common.Access.WRITE) + self.blackboard_speaker = self.attach_blackboard_client(name=self.name, namespace=ActuatorNameSpace.speaker.name) + #self.blackboard_speaker.register_key("state", access=py_trees.common.Access.WRITE) + + super(SpeakerServicePytree, self).__init__(name) + self.logger.debug("%s.__init__()" % (self.__class__.__name__)) + + def setup(self,**additional_parameters): + self.service_client_speaker = HarmoniActionClient(self.name) + self.server_name = "speaker_default" + self.service_client_speaker.setup_client(self.server_name, + self._result_callback, + self._feedback_callback) + self.logger.debug("Behavior %s interface action clients have been set up!" % (self.server_name)) + + self.logger.debug("%s.setup()" % (self.__class__.__name__)) + + def initialise(self): + self.logger.debug("%s.initialise()" % (self.__class__.__name__)) + + def update(self): + if self.send_request: + self.send_request = False + self.logger.debug(f"Sending goal to {self.server_name}") + self.service_client_speaker.send_goal( + action_goal = ActionType["DO"].value, + optional_data=self.blackboard_tts.result, + wait=False, + ) + self.logger.debug(f"Goal sent to {self.server_name}") + new_status = py_trees.common.Status.RUNNING + else: + new_state = self.service_client_speaker.get_state() + print(new_state) + if new_state == GoalStatus.ACTIVE: + new_status = py_trees.common.Status.RUNNING + elif new_state == GoalStatus.SUCCEEDED: + new_status = py_trees.common.Status.SUCCESS + else: + new_status = py_trees.common.Status.FAILURE + raise + + self.logger.debug("%s.update()[%s]--->[%s]" % (self.__class__.__name__, self.status, new_status)) + return new_status + + + def terminate(self, new_status): + new_state = self.service_client_speaker.get_state() + print("terminate : ",new_state) + if new_state == GoalStatus.SUCCEEDED or new_state == GoalStatus.ABORTED or new_state == GoalStatus.LOST: + self.send_request = True + if new_state == GoalStatus.PENDING: + self.send_request = True + self.logger.debug(f"Cancelling goal to {self.server_name}") + self.service_client_speaker.cancel_all_goals() + self.client_result = None + self.logger.debug(f"Goal cancelled to {self.server_name}") + #self.service_client_speaker.stop_tracking_goal() + #self.logger.debug(f"Goal tracking stopped to {self.server_name}") + self.logger.debug("%s.terminate()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + + def _result_callback(self, result): + """ Recieve and store result with timestamp """ + self.logger.debug("The result of the request has been received") + self.client_result = result + return + + def _feedback_callback(self, feedback): + """ Feedback is currently just logged """ + self.logger.debug("The feedback recieved is %s." % feedback) + self.server_state = feedback["state"] + return + +def main(): + #command_line_argument_parser().parse_args() + + py_trees.logging.level = py_trees.logging.Level.DEBUG + + blackboardProva = py_trees.blackboard.Client(name="blackboardProva", namespace=ActuatorNameSpace.tts.name) + blackboardProva.register_key("result", access=py_trees.common.Access.WRITE) + blackboardProva.result = "/root/harmoni_catkin_ws/src/HARMONI/harmoni_actuators/harmoni_tts/temp_data/tts.wav" + print(blackboardProva) + + rospy.init_node("speaker_default", log_level=rospy.INFO) + + speakerPyTree = SpeakerServicePytree("SpeakerServicePytreeTest") + speakerPyTree.setup() + try: + for unused_i in range(0, 10): + speakerPyTree.tick_once() + time.sleep(0.5) + print(blackboardProva) + print("\n") + except KeyboardInterrupt: + print("Exception occurred") + pass + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/subtree_result_interaction.py b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/subtree_result_interaction.py new file mode 100644 index 00000000..1df21b83 --- /dev/null +++ b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/subtree_result_interaction.py @@ -0,0 +1,52 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +from harmoni_common_lib.constants import * +import py_trees +import random + + +class SubTreeResultInteractionBg(py_trees.behaviour.Behaviour): + def __init__(self, name): + super(SubTreeResultInteractionBg, self).__init__(name) + self.logger.debug("%s.__init__()" % (self.__class__.__name__)) + + self.name = name + self.blackboards = [] + + self.blackboard_scene_interaction = self.attach_blackboard_client(name=self.name, namespace=PyTreeNameSpace.scene.name +"/"+ PyTreeNameSpace.interaction.name) + self.blackboard_scene_interaction.register_key("scene_counter", access=py_trees.common.Access.WRITE) + self.blackboard_scene_interaction.register_key("max_num_scene", access=py_trees.common.Access.READ) #NEW + self.blackboard_mainactivity = self.attach_blackboard_client(name=self.name, namespace= PyTreeNameSpace.mainactivity.name) + self.blackboard_mainactivity.register_key("counter_no_answer", access=py_trees.common.Access.WRITE) + self.blackboard_interaction = self.attach_blackboard_client(name=self.name, namespace=PyTreeNameSpace.interaction.name) + self.blackboard_interaction.register_key("finished", access=py_trees.common.Access.WRITE) #NEW + self.blackboard_interaction.register_key(key="call_therapist", access=py_trees.common.Access.READ) + + self.blackboard_scene = self.attach_blackboard_client(name=self.name, namespace=PyTreeNameSpace.scene.name) + self.blackboard_scene.register_key(key="therapist_needed", access=py_trees.common.Access.WRITE) + + + def setup(self): + self.blackboard_scene.therapist_needed = False + self.blackboard_mainactivity.counter_no_answer = 0 + self.blackboard_scene_interaction.scene_counter = 0 + self.blackboard_interaction.finished = True + + self.logger.debug(" %s [SubTreeResultInteractionBg::setup()]" % self.name) + + def initialise(self): + self.logger.debug(" %s [SubTreeResultInteractionBg::initialise()]" % self.name) + + def update(self): + + if self.blackboard_scene_interaction.scene_counter == self.blackboard_scene_interaction.max_num_scene: + self.blackboard_scene_interaction.scene_counter = 0 + self.blackboard_interaction.finished = True + if self.blackboard_interaction.call_therapist: + self.blackboard_scene.therapist_needed = True + self.logger.debug(" %s [SubTreeResultInteractionBg::update()]" % self.name) + return py_trees.common.Status.SUCCESS + + def terminate(self, new_status): + self.logger.debug(" %s [SubTreeResultInteractionBg::terminate().terminate()][%s->%s]" % (self.name, self.status, new_status)) diff --git a/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/subtree_result_main.py b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/subtree_result_main.py new file mode 100644 index 00000000..e2c78669 --- /dev/null +++ b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/subtree_result_main.py @@ -0,0 +1,40 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +from harmoni_common_lib.constants import * +import py_trees +import random + + +class SubTreeResultMain(py_trees.behaviour.Behaviour): + def __init__(self, name): + super(SubTreeResultMain, self).__init__(name) + self.logger.debug("%s.__init__()" % (self.__class__.__name__)) + self.blackboard_scene_mainactivity = self.attach_blackboard_client(name=self.name, namespace=PyTreeNameSpace.scene.name +"/"+ PyTreeNameSpace.mainactivity.name) + self.blackboard_scene_mainactivity.register_key("max_num_scene", access=py_trees.common.Access.READ) #NEW + self.blackboard_scene_mainactivity.register_key("scene_counter", access=py_trees.common.Access.READ) + self.blackboard_mainactivity = self.attach_blackboard_client(name=self.name, namespace=PyTreeNameSpace.mainactivity.name) + self.blackboard_mainactivity.register_key(key="finished", access=py_trees.common.Access.WRITE) #NEW + self.blackboard_mainactivity.register_key(key="call_therapist", access=py_trees.common.Access.READ) + self.blackboard_scene = self.attach_blackboard_client(name=self.name, namespace=PyTreeNameSpace.scene.name) + self.blackboard_scene.register_key(key="therapist_needed", access=py_trees.common.Access.WRITE) + + + def setup(self): + self.blackboard_scene.therapist_needed = False + self.blackboard_mainactivity.finished = False + self.logger.debug(" %s [SubTreeResultMain::setup()]" % self.name) + + def initialise(self): + self.logger.debug(" %s [SubTreeResultMain::initialise()]" % self.name) + + def update(self): + self.logger.debug(" %s [SubTreeResultMain::update()]" % self.name) + if self.blackboard_scene_mainactivity.max_num_scene == self.blackboard_scene_mainactivity.scene_counter: + self.blackboard_mainactivity.finished = True + if self.blackboard_mainactivity.call_therapist: + self.blackboard_scene.therapist_needed = True + return py_trees.common.Status.SUCCESS + + def terminate(self, new_status): + self.logger.debug(" %s [SubTreeResultMain::terminate().terminate()][%s->%s]" % (self.name, self.status, new_status)) diff --git a/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/subtree_result_visualbg.py b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/subtree_result_visualbg.py new file mode 100644 index 00000000..1b01419f --- /dev/null +++ b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/subtree_result_visualbg.py @@ -0,0 +1,52 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +from harmoni_common_lib.constants import * +import py_trees +import random + + +class SubTreeResultVisualBg(py_trees.behaviour.Behaviour): + def __init__(self, name): + super(SubTreeResultVisualBg, self).__init__(name) + self.logger.debug("%s.__init__()" % (self.__class__.__name__)) + + self.name = name + self.blackboards = [] + self.blackboard_scene_visual = self.attach_blackboard_client(name=self.name, namespace=PyTreeNameSpace.scene.name + "/" + PyTreeNameSpace.visual.name) + self.blackboard_scene_visual.register_key("scene_counter", access=py_trees.common.Access.WRITE) + #self.blackboard_scene_visual.register_key("max_num_scene", access=py_trees.common.Access.READ) #NEW + self.blackboard_visual = self.attach_blackboard_client(name=self.name, namespace=PyTreeNameSpace.visual.name) + self.blackboard_visual.register_key(key="finished", access=py_trees.common.Access.WRITE) #NEW + self.blackboard_visual.register_key(key="call_therapist", access=py_trees.common.Access.READ) + self.blackboard_face_detect = self.attach_blackboard_client(name=self.name, namespace=DetectorNameSpace.face_detect.name) + self.blackboard_face_detect.register_key("result", access=py_trees.common.Access.READ) + + self.blackboard_scene = self.attach_blackboard_client(name=self.name, namespace=PyTreeNameSpace.scene.name) + self.blackboard_scene.register_key(key="therapist_needed", access=py_trees.common.Access.WRITE) + + + def setup(self): + self.blackboard_scene_visual.scene_counter = 0 + self.blackboard_scene.therapist_needed = False + self.blackboard_visual.finished = True + self.logger.debug(" %s [SubTreeResultVisualBg::setup()]" % self.name) + + def initialise(self): + self.logger.debug(" %s [SubTreeResultVisualBg::initialise()]" % self.name) + + def update(self): + if self.blackboard_face_detect.result == "null": + self.blackboard_visual.finished = False + else: + self.blackboard_scene_visual.scene_counter = 0 + self.blackboard_visual.finished = True + + if self.blackboard_visual.call_therapist: + self.blackboard_scene.therapist_needed = True + + self.logger.debug(" %s [SubTreeResultVisualBg::update()]" % self.name) + return py_trees.common.Status.SUCCESS + + def terminate(self, new_status): + self.logger.debug(" %s [SubTreeResultVisualBg::terminate().terminate()][%s->%s]" % (self.name, self.status, new_status)) diff --git a/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/web_service.py b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/web_service.py new file mode 100755 index 00000000..31aeb341 --- /dev/null +++ b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/web_service.py @@ -0,0 +1,161 @@ +#!/usr/bin/env python3 + +# Common Imports +import rospy, rospkg, roslib + +from harmoni_common_lib.action_client import HarmoniActionClient +from actionlib_msgs.msg import GoalStatus +import harmoni_common_lib.helper_functions as hf + +# Specific Imports +from harmoni_web.web_service import WebService +from harmoni_common_lib.constants import * +from contextlib import closing +from collections import deque +import soundfile as sf +import numpy as np +import re +import json +import ast +import sys +import time + +# import wget +import ast + +#py_tree +import py_trees + +class WebServicePytree(py_trees.behaviour.Behaviour): + def __init__(self, name = "WebServicePytree"): + + self.name = name + self.service_client_web = None + self.server_state = None + self.server_name = None + self.client_result = None + self.old_image = None + + self.blackboards = [] + self.blackboard_scene = self.attach_blackboard_client(name=self.name, namespace=PyTreeNameSpace.scene.name) + self.blackboard_scene.register_key("image", access=py_trees.common.Access.READ) + + super(WebServicePytree, self).__init__(name) + self.logger.debug("%s.__init__()" % (self.__class__.__name__)) + + def setup(self,**additional_parameters): + + self.service_client_web = HarmoniActionClient(self.name) + self.server_name = "web_default" + self.service_client_web.setup_client(self.server_name, + self._result_callback, + self._feedback_callback) + self.logger.debug("Behavior %s interface action clients have been set up!" % (self.server_name)) + + self.logger.debug("%s.setup()" % (self.__class__.__name__)) + + def initialise(self): + self.logger.debug("%s.initialise()" % (self.__class__.__name__)) + + def update(self): + if self.old_image != self.blackboard_scene.image: + self.logger.debug(f"Sending goal to {self.server_name}") + self.service_client_web.send_goal( + action_goal = ActionType["DO"].value, + optional_data = self.blackboard_scene.image, + wait=False, + ) + self.logger.debug(f"Goal sent to {self.server_name}") + self.old_image = self.blackboard_scene.image + new_status = py_trees.common.Status.RUNNING + else: + new_state = self.service_client_web.get_state() + print(new_state) + if new_state == GoalStatus.ACTIVE: + new_status = py_trees.common.Status.SUCCESS + elif new_state == GoalStatus.SUCCEEDED: + new_status = py_trees.common.Status.SUCCESS + elif new_state == GoalStatus.PENDING: + self.send_request = True + self.logger.debug(f"Cancelling goal to {self.server_name}") + self.service_client_yolo.cancel_all_goals() + self.client_result = None + self.logger.debug(f"Goal cancelled to {self.server_name}") + #self.service_client_yolo.stop_tracking_goal() + #self.logger.debug(f"Goal tracking stopped to {self.server_name}") + new_status = py_trees.common.Status.RUNNING + else: + new_status = py_trees.common.Status.FAILURE + raise + + self.logger.debug("%s.update()[%s]--->[%s]" % (self.__class__.__name__, self.status, new_status)) + return new_status + + def terminate(self, new_status): + """ + new_state = self.service_client_web.get_state() + print("terminate : ",new_state) + if new_state == GoalStatus.SUCCEEDED or new_state == GoalStatus.ABORTED or new_state == GoalStatus.LOST: + self.send_request = True + if new_state == GoalStatus.PENDING: + self.send_request = True + self.logger.debug(f"Cancelling goal to {self.server_name}") + self.service_client_web.cancel_all_goals() + self.client_result = None + self.logger.debug(f"Goal cancelled to {self.server_name}") + self.service_client_web.stop_tracking_goal() + self.logger.debug(f"Goal tracking stopped to {self.server_name}") + """ + self.logger.debug("%s.terminate()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + + def _result_callback(self, result): + """ Recieve and store result with timestamp """ + self.logger.debug("The result of the request has been received") + self.logger.debug( + f"The result callback message from {result['service']} was {len(result['message'])} long" + ) + self.client_result = result["message"] + return + + def _feedback_callback(self, feedback): + """ Feedback is currently just logged """ + self.logger.debug("The feedback recieved is %s." % feedback) + self.server_state = feedback["state"] + return + + +def main(): + #command_line_argument_parser().parse_args() + + py_trees.logging.level = py_trees.logging.Level.DEBUG + + rospy.init_node("web_default" , log_level=rospy.INFO) + + blackboardProva = py_trees.blackboard.Client(name="blackboardProva", namespace=PyTreeNameSpace.scene.name) + blackboardProva.register_key("image", access=py_trees.common.Access.WRITE) + print(blackboardProva) + + blackboardProva.image = "[{'component_id':'img_only', 'set_content':'https://www.google.it/images/branding/googlelogo/2x/googlelogo_color_160x56dp.png'},{'component_id':'raccolta_container', 'set_content': ''}]" + + yoloPyTree = WebServicePytree("WebServicePytreeTest") + + additional_parameters = dict([ + ("WebServicePytree_mode",False)]) + + yoloPyTree.setup(**additional_parameters) + try: + for unused_i in range(0, 16): + yoloPyTree.tick_once() + if unused_i%2: + blackboardProva.image = "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/land.jpeg?alt=media&token=79c113ec-4e61-49c1-bbdf-b865a946247e'},{'component_id':'raccolta_container', 'set_content': ''}]" + else: + blackboardProva.image = "[{'component_id':'img_only', 'set_content':'https://firebasestorage.googleapis.com/v0/b/harmonithesis.appspot.com/o/land2.jpeg?alt=media&token=a3437794-a763-4b47-9052-485fd5c61ae5'},{'component_id':'raccolta_container', 'set_content': ''}]" + print(blackboardProva) + time.sleep(2) + print("\n") + except KeyboardInterrupt: + print("Exception occurred") + pass + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/yolo_service.py b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/yolo_service.py new file mode 100755 index 00000000..fa46a36c --- /dev/null +++ b/harmoni_core/harmoni_pytree/src/harmoni_pytree/leaves/yolo_service.py @@ -0,0 +1,165 @@ +#!/usr/bin/env python3 + +# Common Imports +import rospy +import roslib + +from harmoni_common_lib.constants import * +from actionlib_msgs.msg import GoalStatus +from harmoni_common_lib.service_server import HarmoniServiceServer +from harmoni_common_lib.service_manager import HarmoniServiceManager +from harmoni_common_lib.action_client import HarmoniActionClient +#from harmoni_imageai.yolo_service import ImageAIYoloService +import harmoni_common_lib.helper_functions as hf + +# Specific Imports +from harmoni_common_lib.constants import DetectorNameSpace, ActionType +from sensor_msgs.msg import Image +from imageai.Detection import VideoObjectDetection +from botocore.exceptions import BotoCoreError, ClientError +from contextlib import closing +from collections import deque +import numpy as np +import boto3 +import re +import json +import ast +import sys + +#py_tree +import py_trees +import time + +import py_trees.console + +class ImageAIYoloServicePytree(py_trees.behaviour.Behaviour): + + def __init__(self, name = "ImageAIYoloServicePytree"): + self.name = name + self.server_state = None + self.service_client_yolo = None + self.client_result = None + self.server_name = None + self.send_request = True + + self.blackboards = [] + self.blackboard_face_detection = self.attach_blackboard_client(name=self.name, namespace=DetectorNameSpace.face_detect.name) + #self.blackboard_face_detection.register_key("state", access=py_trees.common.Access.WRITE) + self.blackboard_face_detection.register_key("result", access=py_trees.common.Access.WRITE) + + super(ImageAIYoloServicePytree, self).__init__(name) + self.logger.debug("%s.__init__()" % (self.__class__.__name__)) + + def setup(self,**additional_parameters): + self.service_client_yolo = HarmoniActionClient(self.name) + self.server_name = "imageai_yolo_default" + self.service_client_yolo.setup_client(self.server_name, + self._result_callback, + self._feedback_callback) + self.logger.debug("Behavior %s interface action clients have been set up!" % (self.server_name)) + self.blackboard_face_detection.result = "person" + self.logger.debug("%s.setup()" % (self.__class__.__name__)) + + def initialise(self): + self.logger.debug("%s.initialise()" % (self.__class__.__name__)) + + def update(self): + if self.send_request: + self.send_request = False + self.logger.debug(f"Sending goal to {self.server_name}") + self.service_client_yolo.send_goal( + action_goal = ActionType["REQUEST"].value, + optional_data="", + wait=False, + ) + self.logger.debug(f"Goal sent to {self.server_name}") + new_status = py_trees.common.Status.RUNNING + else: + new_state = self.service_client_yolo.get_state() + print(new_state) + if new_state == GoalStatus.ACTIVE: + new_status = py_trees.common.Status.RUNNING + elif new_state == GoalStatus.SUCCEEDED: + if self.client_result is not None: + self.blackboard_face_detection.result = self.client_result + self.client_result = None + new_status = py_trees.common.Status.SUCCESS + else: + self.logger.debug(f"Waiting fot the result ({self.server_name})") + new_status = py_trees.common.Status.RUNNING + elif new_state == GoalStatus.PENDING: + self.send_request = True + self.logger.debug(f"Cancelling goal to {self.server_name}") + self.service_client_yolo.cancel_all_goals() + self.client_result = None + self.logger.debug(f"Goal cancelled to {self.server_name}") + #self.service_client_yolo.stop_tracking_goal() + #self.logger.debug(f"Goal tracking stopped to {self.server_name}") + new_status = py_trees.common.Status.RUNNING + else: + new_status = py_trees.common.Status.FAILURE + raise + + self.logger.debug("%s.update()[%s]--->[%s]" % (self.__class__.__name__, self.status, new_status)) + return new_status + + def terminate(self, new_status): + new_state = self.service_client_yolo.get_state() + print("terminate : ",new_state) + if new_state == GoalStatus.SUCCEEDED or new_state == GoalStatus.ABORTED or new_state == GoalStatus.LOST: + self.send_request = True + if new_state == GoalStatus.PENDING: + self.send_request = True + self.logger.debug(f"Cancelling goal to {self.server_name}") + self.service_client_service_client_yolotts.cancel_all_goals() + self.client_result = None + self.logger.debug(f"Goal cancelled to {self.server_name}") + #self.service_client_yolo.stop_tracking_goal() + #self.logger.debug(f"Goal tracking stopped to {self.server_name}") + self.logger.debug("%s.terminate()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + + def _result_callback(self, result): + """ Recieve and store result with timestamp """ + self.logger.debug("The result of the request has been received") + self.logger.debug( + f"The result callback message from {result['service']} was {len(result['message'])} long" + ) + self.client_result = result["message"] + return + + def _feedback_callback(self, feedback): + """ Feedback is currently just logged """ + self.logger.debug("The feedback recieved is %s." % feedback) + self.server_state = feedback + return + +def main(): + #command_line_argument_parser().parse_args() + + py_trees.logging.level = py_trees.logging.Level.DEBUG + + #rospy init node mi fa diventare un nodo ros + rospy.init_node("imageai_default", log_level=rospy.INFO) + + blackboardProva = py_trees.blackboard.Client(name="blackboardProva", namespace=DetectorNameSpace.face_detect.name) + blackboardProva.register_key("result", access=py_trees.common.Access.READ) + print(blackboardProva) + + yoloPyTree = ImageAIYoloServicePytree("ImageAIYoloServicePytreeTest") + + additional_parameters = dict([ + ("ImageAIYoloServicePytree_mode",False)]) + + yoloPyTree.setup(**additional_parameters) + try: + for unused_i in range(0, 10): + yoloPyTree.tick_once() + time.sleep(2) + print(blackboardProva) + print("\n") + except KeyboardInterrupt: + print("Exception occurred") + pass + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/harmoni_core/harmoni_pytree/src/harmoni_pytree/pytree_root.py b/harmoni_core/harmoni_pytree/src/harmoni_pytree/pytree_root.py new file mode 100644 index 00000000..66e43223 --- /dev/null +++ b/harmoni_core/harmoni_pytree/src/harmoni_pytree/pytree_root.py @@ -0,0 +1,74 @@ +#!/usr/bin/env python3 + +############################################################################## +# Imports +############################################################################## +import rospy +from harmoni_pytree.aws_tts_service_pytree import AWSTtsServicePytree +from harmoni_pytree.aws_lex_service_pytree import AWSLexServicePytree +from harmoni_pytree.speaker_service_pytree import SpeakerServicePytree +from harmoni_pytree.face_service_pytree import FaceServicePytree +from harmoni_common_lib.constants import ActuatorNameSpace, DialogueNameSpace, State +import argparse +import py_trees +import sys +import time + +import py_trees.console as console + +############################################################################## +# Classes +############################################################################## + +class PyTreeRoot: + def description(self): + content = "Demonstrates sequences in action.\n\n" + content += "A sequence is populated with 2-tick jobs that are allowed to run through to\n" + content += "completion.\n" + + if py_trees.console.has_colours: + banner_line = console.green + "*" * 79 + "\n" + console.reset + s = "\n" + s += banner_line + s += console.bold_white + "Sequences".center(79) + "\n" + console.reset + s += banner_line + s += "\n" + s += content + s += "\n" + s += banner_line + else: + s = content + return s + + + def epilog(self): + if py_trees.console.has_colours: + return console.cyan + "And his noodly appendage reached forth to tickle the blessed...\n" + console.reset + else: + return None + + + def command_line_argument_parser(self): + parser = argparse.ArgumentParser(description=self.description, + epilog=self.epilog, + formatter_class=argparse.RawDescriptionHelpFormatter, + ) + parser.add_argument('-r', '--render', action='store_true', help='render dot tree to file') + return parser + + + def create_root(self): + root = py_trees.composites.Sequence("Sequence") + tts = AWSTtsServicePytree("AwsTtsPyTreeTest") + chatbot = AWSLexServicePytree("AwsLexPyTreeTest") + speaker = SpeakerServicePytree("SpeakerPyTreeTest") + face = FaceServicePytree("FacePyTreeTest") + parall_speaker_face = py_trees.composites.Parallel("Parallel") + root.add_child(chatbot) + root.add_child(tts) + root.add_child(parall_speaker_face) + parall_speaker_face.add_child(speaker) + parall_speaker_face.add_child(face) + return root + + diff --git a/harmoni_core/harmoni_pytree/src/harmoni_pytree/subtrees/interaction_bg.py b/harmoni_core/harmoni_pytree/src/harmoni_pytree/subtrees/interaction_bg.py new file mode 100755 index 00000000..db21ae76 --- /dev/null +++ b/harmoni_core/harmoni_pytree/src/harmoni_pytree/subtrees/interaction_bg.py @@ -0,0 +1,266 @@ +#!/usr/bin/env python3 +############################################################################## +# Imports +############################################################################## + +import argparse +import functools +from py_trees.behaviours import dummy +from py_trees.idioms import either_or +import py_trees +import rospy +import time +import subprocess +import operator +import py_trees.console as console + +from harmoni_common_lib.constants import * + +from harmoni_pytree.leaves.buttons import Buttons +from harmoni_pytree.leaves.aws_lex_trigger_service import AWSLexTriggerServicePytree +from harmoni_pytree.leaves.aws_lex_analyzer_service import AWSLexAnalyzerServicePytree +from harmoni_pytree.leaves.aws_tts_service import AWSTtsServicePytree +from harmoni_pytree.leaves.facial_exp_service import FacialExpServicePytree +from harmoni_pytree.leaves.lip_sync_service import LipSyncServicePytree +from harmoni_pytree.leaves.google_service import SpeechToTextServicePytree +from harmoni_pytree.leaves.speaker_service import SpeakerServicePytree +from harmoni_pytree.leaves.custom_yolo_service import ImageAICustomServicePytree +from harmoni_pytree.leaves.scene_manager_interactionbg import SceneManagerInteractionBg +from harmoni_pytree.leaves.subtree_result_interaction import SubTreeResultInteractionBg + +############################################################################## +# Classes +############################################################################## +""" +Interaction Bg +""" + +def description(root): + content = "\n\n" + content += "\n" + content += "EVENTS\n" + if py_trees.console.has_colours: + banner_line = console.green + "*" * 79 + "\n" + console.reset + s = "\n" + s += banner_line + s += console.bold_white + "Interaction_Bg".center(79) + "\n" + console.reset + s += banner_line + s += "\n" + s += content + s += "\n" + s += banner_line + else: + s = content + return s + + +def epilog(): + if py_trees.console.has_colours: + return console.cyan + "And his noodly appendage reached forth to tickle the blessed...\n" + console.reset + else: + return None + +def command_line_argument_parser(): + parser = argparse.ArgumentParser(description=description(create_root()), + epilog=epilog(), + formatter_class=argparse.RawDescriptionHelpFormatter, + ) + group = parser.add_mutually_exclusive_group() + group.add_argument('-r', '--render', action='store_true', help='render dot tree to file') + group.add_argument('-i', '--interactive', action='store_true', help='pause and wait for keypress at each tick') + return parser + +def pre_tick_handler(behaviour_tree): + print("\n--------- Run %s ---------\n" % behaviour_tree.count) + +def post_tick_handler(snapshot_visitor, behaviour_tree): + print( + "\n" + py_trees.display.unicode_tree( + root=behaviour_tree.root, + visited=snapshot_visitor.visited, + previously_visited=snapshot_visitor.previously_visited + ) + ) + print(py_trees.display.unicode_blackboard()) + +def create_root(name = "Interaction_Bg"): + root = py_trees.composites.Sequence(name=name) + + Success1 = py_trees.behaviours.Success(name="Success") + Success2 = py_trees.behaviours.Success(name="Success") + Success3 = py_trees.behaviours.Success(name="Success") + Success4 = py_trees.behaviours.Success(name="Success") + Success5 = py_trees.behaviours.Success(name="Success") + Success6 = py_trees.behaviours.Success(name="Success") + Success7 = py_trees.behaviours.Success(name="Success") + + + scene_manager = SceneManagerInteractionBg("SceneManagerInteractionBg") + + bot_trigger=AWSLexTriggerServicePytree("AwsLexTriggerInteractionActivity") + + bot_analyzer=AWSLexAnalyzerServicePytree("AwsLexAnalyzerInteractionActivity") + + tts = AWSTtsServicePytree("AwsTtsInteractionBg") + + stt = SpeechToTextServicePytree("SpeechToTextInteractionBg") + + buttons = Buttons(name="ButtonsInteraction") + + custom_yolo = ImageAICustomServicePytree("DetectionCardInteraction") + + face_exp=FacialExpServicePytree("FacialExpInteractionBg") + + speaker=SpeakerServicePytree("SpeakerInteractionBg") + + lip_sync=LipSyncServicePytree("LipSyncInteractionBg") + + subtree_result=SubTreeResultInteractionBg("SubTreeInteraction") + + parall_speaker = py_trees.composites.Parallel(name="ParallelSpeaker") + parall_speaker.add_children([speaker,lip_sync]) + + parall_detect_kid = py_trees.composites.Parallel(name="ParallelDetectKid") + parall_detect_kid.add_children([stt, custom_yolo, buttons]) + + inverter_parall_detect_kid = py_trees.decorators.Inverter(name="ParallelDetectKidInverter",child=parall_detect_kid) + + periodic_success = py_trees.decorators.FailureIsRunning(child=py_trees.behaviours.SuccessEveryN(name="SuccessEveryN",n=3),name="FIsRSEN") + + eor_null_button = py_trees.idioms.either_or( + name="EitherOrNullButton", + conditions=[ + py_trees.common.ComparisonExpression(PyTreeNameSpace.buttons.name+ "/result", "null", operator.eq), + py_trees.common.ComparisonExpression(PyTreeNameSpace.buttons.name+ "/result", "null", operator.ne), + ], + subtrees=[periodic_success,Success7], + namespace="eor_null_button", + ) + eor_null_speech = py_trees.idioms.either_or( + name="EitherOrNullSpeech", + conditions=[ + py_trees.common.ComparisonExpression(DetectorNameSpace.stt.name+ "/result", "null", operator.eq), + py_trees.common.ComparisonExpression(DetectorNameSpace.stt.name+ "/result", "null", operator.ne), + ], + subtrees=[eor_null_button,Success6], + namespace="eor_null_speech", + ) + eor_null_card = py_trees.idioms.either_or( + name="EitherOrNullCard", + conditions=[ + py_trees.common.ComparisonExpression(DetectorNameSpace.card_detect.name+ "/result", "null", operator.eq), + py_trees.common.ComparisonExpression(DetectorNameSpace.card_detect.name+ "/result", "null", operator.ne), + ], + subtrees=[eor_null_speech,Success5], + namespace="eor_null_card", + ) + + selector_detect_kid = py_trees.composites.Selector(name="SelectorDetectKid") + selector_detect_kid.add_children([inverter_parall_detect_kid, eor_null_card]) + + sequen_detect_kid = py_trees.composites.Sequence(name="SequenceDetectKid") + sequen_detect_kid.add_children([selector_detect_kid, bot_analyzer]) + + eor_face = py_trees.idioms.either_or( + name="EitherOrFace", + conditions=[ + py_trees.common.ComparisonExpression(PyTreeNameSpace.scene.name + "/face_exp", "null", operator.ne), + py_trees.common.ComparisonExpression(PyTreeNameSpace.scene.name + "/face_exp", "null", operator.eq), + ], + subtrees=[face_exp, Success3], + namespace="eor_face", + ) + + eor_kid = py_trees.idioms.either_or( + name="EitherOrKid", + conditions=[ + py_trees.common.ComparisonExpression(PyTreeNameSpace.scene.name + "/"+ PyTreeNameSpace.interaction.name+ "/do_kid", True, operator.eq), + py_trees.common.ComparisonExpression(PyTreeNameSpace.scene.name + "/"+ PyTreeNameSpace.interaction.name+ "/do_kid", True, operator.ne), + ], + subtrees=[sequen_detect_kid, Success4], + namespace="eor_kid", + ) + + parall_detect_and_face = py_trees.composites.Parallel(name="ParallelDetectAndFace") + parall_detect_and_face.add_children([eor_kid, eor_face]) + + sequen_interaction_bg = py_trees.composites.Sequence(name="SequenceInteractionBg") + + eor_bot_trigger = py_trees.idioms.either_or( + name="EitherOrBotTrigger", + conditions=[ + py_trees.common.ComparisonExpression(PyTreeNameSpace.scene.name + "/" + PyTreeNameSpace.interaction.name + "/do_trigger", True, operator.ne), + py_trees.common.ComparisonExpression(PyTreeNameSpace.scene.name + "/" + PyTreeNameSpace.interaction.name + "/do_trigger", True, operator.eq), + ], + subtrees=[Success2, bot_trigger], + namespace="eor_bot_trigger", + ) + + sequen_interaction_bg.add_children([scene_manager, + eor_bot_trigger, + tts, + parall_speaker, + parall_detect_and_face]) + + eor_interaction_bg = either_or( + name="Either_Or_Interaction_Bg", + conditions=[ + py_trees.common.ComparisonExpression(PyTreeNameSpace.mainactivity.name+"/counter_no_answer", 2, operator.lt), + py_trees.common.ComparisonExpression(PyTreeNameSpace.mainactivity.name+"/counter_no_answer", 2, operator.ge), + ], + subtrees=[Success1,sequen_interaction_bg], + namespace="eor_interaction_bg", + ) + + root.add_children([eor_interaction_bg, subtree_result]) + + return root + +############################################################################## +# Main +############################################################################## + +def main(): + """ + Entry point for the demo script. + """ + + py_trees.logging.level = py_trees.logging.Level.DEBUG + root = create_root() + #print(description(root)) + #uncomment the following line if you want to render the dot_tree + #render_with_args() + + rospy.init_node("interactionbg_default", log_level=rospy.INFO) + + #################### + # Tree Stewardship + #################### + behaviour_tree = py_trees.trees.BehaviourTree(root) + behaviour_tree.add_pre_tick_handler(pre_tick_handler) + behaviour_tree.visitors.append(py_trees.visitors.DebugVisitor()) + snapshot_visitor = py_trees.visitors.SnapshotVisitor() + behaviour_tree.add_post_tick_handler(functools.partial(post_tick_handler, snapshot_visitor)) + behaviour_tree.visitors.append(snapshot_visitor) + behaviour_tree.setup(timeout=15) + + #################### + # Tick Tock + #################### + + #if args.interactive: + # py_trees.console.read_single_keypress() + for unused_i in range(1, 30): + try: + behaviour_tree.tick() + #if args.interactive: + # py_trees.console.read_single_keypress() + #else: + time.sleep(0.4) + except KeyboardInterrupt: + break + print("\n") + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/harmoni_core/harmoni_pytree/src/harmoni_pytree/subtrees/mainactivity.py b/harmoni_core/harmoni_pytree/src/harmoni_pytree/subtrees/mainactivity.py new file mode 100755 index 00000000..7ca96ff1 --- /dev/null +++ b/harmoni_core/harmoni_pytree/src/harmoni_pytree/subtrees/mainactivity.py @@ -0,0 +1,328 @@ +#!/usr/bin/env python3 +############################################################################## +# Imports +############################################################################## + +import argparse +import functools +from py_trees.behaviours import dummy +from py_trees.idioms import either_or +import py_trees +import time +import rospy +from random import randint +import subprocess +import operator +import py_trees.console as console + +from harmoni_common_lib.constants import * + +from harmoni_pytree.leaves.buttons import Buttons +from harmoni_pytree.leaves.aws_lex_trigger_service import AWSLexTriggerServicePytree +from harmoni_pytree.leaves.aws_lex_analyzer_service import AWSLexAnalyzerServicePytree +from harmoni_pytree.leaves.aws_tts_service import AWSTtsServicePytree +from harmoni_pytree.leaves.web_service import WebServicePytree +from harmoni_pytree.leaves.facial_exp_service import FacialExpServicePytree +from harmoni_pytree.leaves.lip_sync_service import LipSyncServicePytree +from harmoni_pytree.leaves.google_service import SpeechToTextServicePytree +from harmoni_pytree.leaves.speaker_service import SpeakerServicePytree +#from harmoni_pytree.leaves.gesture_service import GestureServicePytree +from harmoni_pytree.leaves.subtree_result_main import SubTreeResultMain +from harmoni_pytree.leaves.scene_manager_main import SceneManagerMain +from harmoni_pytree.leaves.custom_yolo_service import ImageAICustomServicePytree + +############################################################################## +# Classes +############################################################################## +""" +Main Activity +""" +def description(root): + content = "\n\n" + content += "\n" + content += "EVENTS\n" + if py_trees.console.has_colours: + banner_line = console.green + "*" * 79 + "\n" + console.reset + s = "\n" + s += banner_line + s += console.bold_white + "Main Activity".center(79) + "\n" + console.reset + s += banner_line + s += "\n" + s += content + s += "\n" + s += banner_line + else: + s = content + return s + +def epilog(): + if py_trees.console.has_colours: + return console.cyan + "And his noodly appendage reached forth to tickle the blessed...\n" + console.reset + else: + return None + +def command_line_argument_parser(): + parser = argparse.ArgumentParser(description=description(create_root()), + epilog=epilog(), + formatter_class=argparse.RawDescriptionHelpFormatter, + ) + group = parser.add_mutually_exclusive_group() + group.add_argument('-r', '--render', action='store_true', help='render dot tree to file') + group.add_argument('-i', '--interactive', action='store_true', help='pause and wait for keypress at each tick') + return parser + +def pre_tick_handler(behaviour_tree): + print("\n--------- Run %s ---------\n" % behaviour_tree.count) + +def post_tick_handler(snapshot_visitor, behaviour_tree): + print( + "\n" + py_trees.display.unicode_tree( + root=behaviour_tree.root, + visited=snapshot_visitor.visited, + previously_visited=snapshot_visitor.previously_visited + ) + ) + print(py_trees.display.unicode_blackboard()) + +def create_root(): + root = py_trees.composites.Sequence(name="mainactivity",memory=True) + + Success1 = py_trees.behaviours.Success(name="Success") + Success2 = py_trees.behaviours.Success(name="Success") + Success3 = py_trees.behaviours.Success(name="Success") + Success4 = py_trees.behaviours.Success(name="Success") + Success5 = py_trees.behaviours.Success(name="Success") + Success6 = py_trees.behaviours.Success(name="Success") + Success7 = py_trees.behaviours.Success(name="Success") + Success8 = py_trees.behaviours.Success(name="Success") + Success9 = py_trees.behaviours.Success(name="Success") + Success10 = py_trees.behaviours.Success(name="Success") + Success11 = py_trees.behaviours.Success(name="Success") + + scene_manager = SceneManagerMain("SceneManagerMain") + + #gesture=GestureServicePytree("GestureMainActivity") + gesture = py_trees.behaviours.Success(name="GestureMainActivity") + + projector = WebServicePytree(name="ProjectorMainActivity") + + ext_speaker=SpeakerServicePytree("ExternalSpeakerMainActivity") + + bot_trigger=AWSLexTriggerServicePytree("AwsLexTriggerMainActivity") + + bot_analyzer=AWSLexAnalyzerServicePytree("AwsLexAnalyzerMainActivity") + + tts = AWSTtsServicePytree("AwsTtsMainActivity") + + buttons = Buttons(name="ButtonsMainActivity") + + stt=SpeechToTextServicePytree("SpeechToTextMainActivity") + + face_exp=FacialExpServicePytree("FacialExpMainActivity") + + speaker=SpeakerServicePytree("SpeakerMainActivity") + + lip_sync=LipSyncServicePytree("LipsSyncMainActivity") + + custom_yolo = ImageAICustomServicePytree("DetectionCardMain") + + subtree_result = SubTreeResultMain("SubTreeResultMain") + + parall_speaker = py_trees.composites.Parallel(name="ParallelSpeaker") + parall_speaker.add_children([speaker,lip_sync]) + + eor_projector = py_trees.idioms.either_or( + name="EitherOrProjector", + conditions=[ + py_trees.common.ComparisonExpression(PyTreeNameSpace.scene.name + "/image", "null", operator.ne), + py_trees.common.ComparisonExpression(PyTreeNameSpace.scene.name + "/image", "null", operator.eq), + ], + subtrees=[projector, Success8], + namespace="eor_projector", + ) + + eor_trigger = py_trees.idioms.either_or( + name="EitherOrTrigger", + conditions=[ + py_trees.common.ComparisonExpression(PyTreeNameSpace.scene.name + "/" + PyTreeNameSpace.mainactivity.name + "/do_trigger", True, operator.eq), + py_trees.common.ComparisonExpression(PyTreeNameSpace.scene.name + "/" + PyTreeNameSpace.mainactivity.name + "/do_trigger", True, operator.ne), + ], + subtrees=[bot_trigger, Success7], + namespace="eor_trigger", + ) + + sequen_speaker = py_trees.composites.Sequence(name="SequenceSpeaker") + sequen_speaker.add_children([eor_trigger,tts,parall_speaker]) + + eor_speaker = py_trees.idioms.either_or( + name="EitherOrSpeaker", + conditions=[ + py_trees.common.ComparisonExpression(PyTreeNameSpace.scene.name + "/utterance", "null", operator.ne), + py_trees.common.ComparisonExpression(PyTreeNameSpace.scene.name + "/utterance", "null", operator.eq), + ], + subtrees=[sequen_speaker, py_trees.timers.Timer(name="SuccessAfter2", duration= 2.0)], + namespace="eor_speaker", + ) + + eor_external_speaker = py_trees.idioms.either_or( + name="EitherOrExternalSpeaker", + conditions=[ + py_trees.common.ComparisonExpression(PyTreeNameSpace.scene.name + "/sound", "null", operator.ne), + py_trees.common.ComparisonExpression(PyTreeNameSpace.scene.name + "/sound", "null", operator.eq), + ], + subtrees=[ext_speaker, Success4], + namespace="eor_external_speaker", + ) + + parall_robot = py_trees.composites.Parallel(name="ParallelRobot") + parall_robot.add_children([eor_external_speaker, eor_speaker]) + + sequen_robot = py_trees.composites.Sequence(name="SequenceRobot") + sequen_robot.add_children([scene_manager, eor_projector, parall_robot]) + + parall_detect_kid = py_trees.composites.Parallel(name="ParallelDetectKid") + parall_detect_kid.add_children([stt ,custom_yolo, buttons]) + + inverter_parall_detect_kid = py_trees.decorators.Inverter(name="ParallelDetectKidInverter",child=parall_detect_kid) + + periodic_success = py_trees.decorators.FailureIsRunning(child=py_trees.behaviours.SuccessEveryN(name="SuccessEveryN",n=3),name="FIsRSEN") + + eor_null_button = py_trees.idioms.either_or( + name="EitherOrNullButton", + conditions=[ + py_trees.common.ComparisonExpression(PyTreeNameSpace.buttons.name+ "/result", "null", operator.eq), + py_trees.common.ComparisonExpression(PyTreeNameSpace.buttons.name+ "/result", "null", operator.ne), + ], + subtrees=[periodic_success,Success11], + namespace="eor_null_button", + ) + eor_null_speech = py_trees.idioms.either_or( + name="EitherOrNullSpeech", + conditions=[ + py_trees.common.ComparisonExpression(DetectorNameSpace.stt.name+ "/result", "null", operator.eq), + py_trees.common.ComparisonExpression(DetectorNameSpace.stt.name+ "/result", "null", operator.ne), + ], + subtrees=[eor_null_button,Success10], + namespace="eor_null_speech", + ) + eor_null_card = py_trees.idioms.either_or( + name="EitherOrNullCard", + conditions=[ + py_trees.common.ComparisonExpression(DetectorNameSpace.card_detect.name+ "/result", "null", operator.eq), + py_trees.common.ComparisonExpression(DetectorNameSpace.card_detect.name+ "/result", "null", operator.ne), + ], + subtrees=[eor_null_speech,Success9], + namespace="eor_null_card", + ) + + selector_detect_kid = py_trees.composites.Selector(name="SelectorDetectKid") + selector_detect_kid.add_children([inverter_parall_detect_kid, eor_null_card]) + + sequen_detect_kid = py_trees.composites.Sequence(name="SequenceDetectKid") + sequen_detect_kid.add_children([selector_detect_kid, bot_analyzer]) + + eor_kid = py_trees.idioms.either_or( + name="EitherOrKid", + conditions=[ + py_trees.common.ComparisonExpression(PyTreeNameSpace.scene.name + "/"+ PyTreeNameSpace.mainactivity.name+ "/do_kid", True, operator.eq), + py_trees.common.ComparisonExpression(PyTreeNameSpace.scene.name + "/"+ PyTreeNameSpace.mainactivity.name+ "/do_kid", True, operator.ne), + ], + subtrees=[sequen_detect_kid, Success2], + namespace="eor_kid", + ) + eor_face = py_trees.idioms.either_or( + name="EitherOrFace", + conditions=[ + py_trees.common.ComparisonExpression(PyTreeNameSpace.scene.name + "/face_exp", "null", operator.ne), + py_trees.common.ComparisonExpression(PyTreeNameSpace.scene.name + "/face_exp", "null", operator.eq), + ], + subtrees=[face_exp, Success5], + namespace="eor_face", + ) + eor_gesture = py_trees.idioms.either_or( + name="EitherOrGesture", + conditions=[ + py_trees.common.ComparisonExpression(PyTreeNameSpace.scene.name + "/gesture", "null", operator.ne), + py_trees.common.ComparisonExpression(PyTreeNameSpace.scene.name + "/gesture", "null", operator.eq), + ], + subtrees=[gesture, Success3], + namespace="eor_gesture", + ) + + parall_face_gesture_kid = py_trees.composites.Parallel(name="ParallelFaceGestureKid") + parall_face_gesture_kid.add_children([eor_kid, eor_face, eor_gesture]) + + root.add_children([sequen_robot, parall_face_gesture_kid, subtree_result]) + + return root + +############################################################################## +# Main +############################################################################## + +def render_with_args(root): + + args = command_line_argument_parser().parse_args() + #################### + # Rendering + #################### + if args.render: + print("**************START RENDERING**************") + py_trees.display.render_dot_tree(root) + if py_trees.utilities.which("xdot"): + try: + subprocess.call(["xdot", "%s.dot" % root.name]) + except KeyboardInterrupt: + pass + else: + print("") + console.logerror("No xdot viewer found, skipping display [hint: sudo apt install xdot]") + print("") + print("**************END RENDERING**************") + +def main(): + """ + Entry point for the demo script. + """ + py_trees.logging.level = py_trees.logging.Level.DEBUG + root = create_root() + print(description(root)) + + #uncomment the following line if you want to render the dot_tree + #render_with_args(root) + + #################### + # Tree Stewardship + #################### + + rospy.init_node("mainactivity_default", log_level=rospy.INFO) + + behaviour_tree = py_trees.trees.BehaviourTree(root) + behaviour_tree.add_pre_tick_handler(pre_tick_handler) + behaviour_tree.visitors.append(py_trees.visitors.DebugVisitor()) + snapshot_visitor = py_trees.visitors.SnapshotVisitor() + behaviour_tree.add_post_tick_handler(functools.partial(post_tick_handler, snapshot_visitor)) + behaviour_tree.visitors.append(snapshot_visitor) + behaviour_tree.setup(timeout=15) + + #################### + # Tick Tock + #################### + + #if args.interactive: + # py_trees.console.read_single_keypress() + #while True: + for unused_i in range(1, 50): + try: + behaviour_tree.tick() + #if args.interactive: + # py_trees.console.read_single_keypress() + #else: + time.sleep(2) + except KeyboardInterrupt: + break + print("\n") + + +if __name__ == "__main__": + main() diff --git a/harmoni_core/harmoni_pytree/src/harmoni_pytree/subtrees/mic_and_stt.py b/harmoni_core/harmoni_pytree/src/harmoni_pytree/subtrees/mic_and_stt.py new file mode 100755 index 00000000..c05114f0 --- /dev/null +++ b/harmoni_core/harmoni_pytree/src/harmoni_pytree/subtrees/mic_and_stt.py @@ -0,0 +1,146 @@ +#!/usr/bin/env python3 +############################################################################## +# Imports +############################################################################## + +import argparse +import functools +from py_trees.behaviours import dummy +from py_trees.idioms import either_or +import py_trees +import time +import rospy +from random import randint +import subprocess +import operator +import py_trees.console as console +import running_or_failure as rf + +from harmoni_common_lib.constants import * + +from harmoni_pytree.leaves.aws_lex_trigger_service import AWSLexTriggerServicePytree +from harmoni_pytree.leaves.aws_lex_analyzer_service import AWSLexAnalyzerServicePytree +from harmoni_pytree.leaves.aws_tts_service import AWSTtsServicePytree +from harmoni_pytree.leaves.facial_exp_service import FacialExpServicePytree +from harmoni_pytree.leaves.lip_sync_service import LipSyncServicePytree +from harmoni_pytree.leaves.google_service import SpeechToTextServicePytree +from harmoni_pytree.leaves.microphone_service import MicrophoneServicePytree +from harmoni_pytree.leaves.speaker_service import SpeakerServicePytree +#from harmoni_pytree.leaves.gesture_service import GestureServicePytree +from harmoni_pytree.leaves.subtree_result_main import SubTreeResultMain +from harmoni_pytree.leaves.scene_manager_main import SceneManagerMain +from harmoni_pytree.leaves.custom_yolo_service import ImageAICustomServicePytree + +############################################################################## +# Classes +############################################################################## + + +def description(root): + content = "\n\n" + content += "\n" + content += "EVENTS\n" + if py_trees.console.has_colours: + banner_line = console.green + "*" * 79 + "\n" + console.reset + s = "\n" + s += banner_line + s += console.bold_white + "Test".center(79) + "\n" + console.reset + s += banner_line + s += "\n" + s += content + s += "\n" + s += banner_line + else: + s = content + return s + + +def epilog(): + if py_trees.console.has_colours: + return console.cyan + "And his noodly appendage reached forth to tickle the blessed...\n" + console.reset + else: + return None + + +def command_line_argument_parser(): + parser = argparse.ArgumentParser(description=description(create_root()), + epilog=epilog(), + formatter_class=argparse.RawDescriptionHelpFormatter, + ) + group = parser.add_mutually_exclusive_group() + group.add_argument('-b', '--with-blackboard-variables', default=False, action='store_true', help='add nodes for the blackboard variables') + group.add_argument('-r', '--render', action='store_true', help='render dot tree to file') + group.add_argument('-i', '--interactive', action='store_true', help='pause and wait for keypress at each tick') + return parser + + +def pre_tick_handler(behaviour_tree): + print("\n--------- Run %s ---------\n" % behaviour_tree.count) + + +def post_tick_handler(snapshot_visitor, behaviour_tree): + print( + "\n" + py_trees.display.unicode_tree( + root=behaviour_tree.root, + visited=snapshot_visitor.visited, + previously_visited=snapshot_visitor.previously_visited + ) + ) + print(py_trees.display.unicode_blackboard()) + + +def create_root(name= "MicAndSTT"): + + microphone=MicrophoneServicePytree("MicrophoneMainActivity") + stt=SpeechToTextServicePytree("SpeechToTextMainActivity") + + root = py_trees.composites.Sequence(name="MicAndSTT") + root.add_children([microphone, stt]) + + return root + +############################################################################## +# Main +############################################################################## + +def main(): + """ + Entry point for the demo script. + """ + py_trees.logging.level = py_trees.logging.Level.DEBUG + root = create_root() + print(description(root)) + + blackboardProva = py_trees.blackboard.Client(name="blackboardProva", namespace=DetectorNameSpace.stt.name) + blackboardProva.register_key("result", access=py_trees.common.Access.READ) + print(blackboardProva) + + #################### + # Tree Stewardship + #################### + + rospy.init_node("test_default", log_level=rospy.INFO) + + behaviour_tree = py_trees.trees.BehaviourTree(root) + behaviour_tree.add_pre_tick_handler(pre_tick_handler) + behaviour_tree.visitors.append(py_trees.visitors.DebugVisitor()) + snapshot_visitor = py_trees.visitors.SnapshotVisitor() + behaviour_tree.add_post_tick_handler(functools.partial(post_tick_handler, snapshot_visitor)) + behaviour_tree.visitors.append(snapshot_visitor) + behaviour_tree.setup(timeout=15) + + #################### + # Tick Tock + #################### + + for unused_i in range(1, 30): + try: + behaviour_tree.tick() + time.sleep(3) + except KeyboardInterrupt: + break + print("\n") + + +if __name__ == "__main__": + main() diff --git a/harmoni_core/harmoni_pytree/src/harmoni_pytree/subtrees/on_modules.py b/harmoni_core/harmoni_pytree/src/harmoni_pytree/subtrees/on_modules.py new file mode 100644 index 00000000..9ac5ae2f --- /dev/null +++ b/harmoni_core/harmoni_pytree/src/harmoni_pytree/subtrees/on_modules.py @@ -0,0 +1,15 @@ +#!/usr/bin/env python3 +import py_trees + +from harmoni_pytree.leaves.camera_service import CameraServicePytree +from harmoni_pytree.leaves.microphone_service import MicrophoneServicePytree + +def create_root(): + #camera = CameraServicePytree(name="Camera") + microphone=MicrophoneServicePytree("MicrophoneMainActivity") + #external_camera = CameraServicePytree(name="ExternalCamera") + root = py_trees.composites.Parallel("Parallel_On_Modules") + #root.add_children([camera, microphone]) + root.add_child(microphone) + + return root \ No newline at end of file diff --git a/harmoni_core/harmoni_pytree/src/harmoni_pytree/subtrees/reset.py b/harmoni_core/harmoni_pytree/src/harmoni_pytree/subtrees/reset.py new file mode 100644 index 00000000..7dce885c --- /dev/null +++ b/harmoni_core/harmoni_pytree/src/harmoni_pytree/subtrees/reset.py @@ -0,0 +1,143 @@ +#!/usr/bin/env python3 +############################################################################## +# Imports +############################################################################## + +import argparse +import functools +from os import name +from py_trees.behaviours import dummy +from py_trees.idioms import either_or +import py_trees +import time +from random import randint +import subprocess +import operator +import py_trees.console as console + +############################################################################## +# Classes +############################################################################## + + +def description(root): + content = "\n\n" + content += "\n" + content += "EVENTS\n" + if py_trees.console.has_colours: + banner_line = console.green + "*" * 79 + "\n" + console.reset + s = "\n" + s += banner_line + s += console.bold_white + "Reset".center(79) + "\n" + console.reset + s += banner_line + s += "\n" + s += content + s += "\n" + s += banner_line + else: + s = content + return s + + +def epilog(): + if py_trees.console.has_colours: + return console.cyan + "And his noodly appendage reached forth to tickle the blessed...\n" + console.reset + else: + return None + + +def command_line_argument_parser(): + parser = argparse.ArgumentParser(description=description(create_root()), + epilog=epilog(), + formatter_class=argparse.RawDescriptionHelpFormatter, + ) + group = parser.add_mutually_exclusive_group() + group.add_argument('-b', '--with-blackboard-variables', default=False, action='store_true', help='add nodes for the blackboard variables') + group.add_argument('-r', '--render', action='store_true', help='render dot tree to file') + group.add_argument('-i', '--interactive', action='store_true', help='pause and wait for keypress at each tick') + return parser + + +def pre_tick_handler(behaviour_tree): + print("\n--------- Run %s ---------\n" % behaviour_tree.count) + + +def post_tick_handler(snapshot_visitor, behaviour_tree): + print( + "\n" + py_trees.display.unicode_tree( + root=behaviour_tree.root, + visited=snapshot_visitor.visited, + previously_visited=snapshot_visitor.previously_visited + ) + ) + print(py_trees.display.unicode_blackboard()) + + +def create_root(name= "Reset"): + + root = py_trees.behaviours.Count(name='Count', fail_until=0, running_until=2, success_until=0, reset=True) + + return root + +############################################################################## +# Main +############################################################################## + +def main(): + """ + Entry point for the demo script. + """ + args = command_line_argument_parser().parse_args() + py_trees.logging.level = py_trees.logging.Level.DEBUG + root = create_root() + print(description(root)) + + #################### + # Rendering + #################### + """ + if args.render: + print("**************START RENDERING**************") + py_trees.display.render_dot_tree(root) + if py_trees.utilities.which("xdot"): + try: + subprocess.call(["xdot", "%s.dot" % root.name]) + except KeyboardInterrupt: + pass + else: + print("") + console.logerror("No xdot viewer found, skipping display [hint: sudo apt install xdot]") + print("") + print("**************END RENDERING**************") + """ + #################### + # Tree Stewardship + #################### + behaviour_tree = py_trees.trees.BehaviourTree(root) + behaviour_tree.add_pre_tick_handler(pre_tick_handler) + behaviour_tree.visitors.append(py_trees.visitors.DebugVisitor()) + snapshot_visitor = py_trees.visitors.SnapshotVisitor() + behaviour_tree.add_post_tick_handler(functools.partial(post_tick_handler, snapshot_visitor)) + behaviour_tree.visitors.append(snapshot_visitor) + behaviour_tree.setup(timeout=15) + + #################### + # Tick Tock + #################### + + #if args.interactive: + # py_trees.console.read_single_keypress() + for unused_i in range(1, 10): + try: + behaviour_tree.tick() + #if args.interactive: + # py_trees.console.read_single_keypress() + #else: + time.sleep(0.5) + except KeyboardInterrupt: + break + print("\n") + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/harmoni_core/harmoni_pytree/src/harmoni_pytree/subtrees/root.py b/harmoni_core/harmoni_pytree/src/harmoni_pytree/subtrees/root.py new file mode 100755 index 00000000..d9c828b5 --- /dev/null +++ b/harmoni_core/harmoni_pytree/src/harmoni_pytree/subtrees/root.py @@ -0,0 +1,174 @@ +#!/usr/bin/env python3 +############################################################################## +# Imports +############################################################################## + +import argparse +import functools +import py_trees +import rospy +import time +import subprocess +import py_trees.console as console +import os +from harmoni_common_lib.constants import * +from harmoni_pytree.leaves.yolo_service import ImageAIYoloServicePytree +from harmoni_pytree.subtrees import session as s +from harmoni_pytree.subtrees import reset as r +from harmoni_pytree.subtrees import on_modules as o + +############################################################################## +# Classes +############################################################################## +""" +Root +""" +def description(root): + content = "\n\n" + content += "\n" + content += "EVENTS\n" + if py_trees.console.has_colours: + banner_line = console.green + "*" * 79 + "\n" + console.reset + s = "\n" + s += banner_line + s += console.bold_white + "Root".center(79) + "\n" + console.reset + s += banner_line + s += "\n" + s += content + s += "\n" + s += banner_line + else: + s = content + return s + +def epilog(): + if py_trees.console.has_colours: + return console.cyan + "And his noodly appendage reached forth to tickle the blessed...\n" + console.reset + else: + return None + +def command_line_argument_parser(): + parser = argparse.ArgumentParser(description=description(create_root()), + epilog=epilog(), + formatter_class=argparse.RawDescriptionHelpFormatter, + ) + group = parser.add_mutually_exclusive_group() + group.add_argument('-r', '--render', action='store_true', help='render dot tree to file') + group.add_argument('-i', '--interactive', action='store_true', help='pause and wait for keypress at each tick') + return parser + +def pre_tick_handler(behaviour_tree): + print("\n--------- Run %s ---------\n" % behaviour_tree.count) + +def post_tick_handler(snapshot_visitor, behaviour_tree): + print( + "\n" + py_trees.display.unicode_tree( + root=behaviour_tree.root, + visited=snapshot_visitor.visited, + previously_visited=snapshot_visitor.previously_visited + ) + ) + print(py_trees.display.unicode_blackboard()) + + +def create_root(): + + root = py_trees.composites.Selector(name="root", memory=True) + + on_module = o.create_root() + + on_module_inverter = py_trees.decorators.Inverter(name="OnModuleInverter",child=on_module) + + yolo_service = ImageAIYoloServicePytree("FaceDetection") + + yolo_service_inverter = py_trees.decorators.Inverter(name="YoloInverter",child=yolo_service) + + session = s.create_root() + + ad_libitum = py_trees.composites.Selector(name="AdLibitum") + + ad_libitum.add_children([on_module_inverter, yolo_service_inverter, py_trees.behaviours.Running("Idle")]) + + parallel_onModule_detectFace_session = py_trees.composites.Parallel(name="ParallelOnModuleDetectFaceSession") + + parallel_onModule_detectFace_session.add_children([ad_libitum, session]) + + reset = r.create_root() + + root.add_children([parallel_onModule_detectFace_session, reset, py_trees.behaviours.Running("Idle")]) + + return root + +############################################################################## +# Main +############################################################################## + +def render_with_args(root): + + args = command_line_argument_parser().parse_args() + #################### + # Rendering + #################### + if args.render: + print("**************START RENDERING**************") + py_trees.display.render_dot_tree(root) + if py_trees.utilities.which("xdot"): + try: + subprocess.call(["xdot", "%s.dot" % root.name]) + except KeyboardInterrupt: + pass + else: + print("") + console.logerror("No xdot viewer found, skipping display [hint: sudo apt install xdot]") + print("") + print("**************END RENDERING**************") + +def main(): + """ + Entry point for the demo script. + """ + py_trees.logging.level = py_trees.logging.Level.DEBUG + root = create_root() + print(description(root)) + + #uncomment the following line if you want to render the dot_tree + #render_with_args(root) + + #################### + # Tree Stewardship + #################### + + rospy.init_node("root_default", log_level=rospy.INFO) + + behaviour_tree = py_trees.trees.BehaviourTree(root) + behaviour_tree.add_pre_tick_handler(pre_tick_handler) + #behaviour_tree.visitors.append(py_trees.visitors.DebugVisitor()) + snapshot_visitor = py_trees.visitors.SnapshotVisitor() + behaviour_tree.add_post_tick_handler(functools.partial(post_tick_handler, snapshot_visitor)) + behaviour_tree.visitors.append(snapshot_visitor) + behaviour_tree.setup(timeout=15) + + #################### + # Tick Tock + #################### + + try: + behaviour_tree.tick_tock( + period_ms=500, + number_of_iterations=py_trees.trees.CONTINUOUS_TICK_TOCK, + ) + except KeyboardInterrupt: + behaviour_tree.interrupt() + """ + for unused_i in range(1, 10): + try: + behaviour_tree.tick() + time.sleep(0.6) + except KeyboardInterrupt: + behaviour_tree.interrupt() + print("\n") + """ + + +if __name__ == "__main__": + main() diff --git a/harmoni_core/harmoni_pytree/src/harmoni_pytree/subtrees/running_or_failure.py b/harmoni_core/harmoni_pytree/src/harmoni_pytree/subtrees/running_or_failure.py new file mode 100644 index 00000000..2172f253 --- /dev/null +++ b/harmoni_core/harmoni_pytree/src/harmoni_pytree/subtrees/running_or_failure.py @@ -0,0 +1,149 @@ +#!/usr/bin/env python3 +############################################################################## +# Imports +############################################################################## + +import argparse +import functools +from os import name +from py_trees.behaviours import Success, dummy +import py_trees +import typing +import time +from random import randint +import subprocess +import operator +import py_trees.console as console +from py_trees import common + +############################################################################## +# Classes +############################################################################## + + +def description(root): + content = "\n\n" + content += "\n" + content += "EVENTS\n" + if py_trees.console.has_colours: + banner_line = console.green + "*" * 79 + "\n" + console.reset + s = "\n" + s += banner_line + s += console.bold_white + "Running or failure".center(79) + "\n" + console.reset + s += banner_line + s += "\n" + s += content + s += "\n" + s += banner_line + else: + s = content + return s + + +def epilog(): + if py_trees.console.has_colours: + return console.cyan + "And his noodly appendage reached forth to tickle the blessed...\n" + console.reset + else: + return None + + +def command_line_argument_parser(): + parser = argparse.ArgumentParser(description=description(create_root()), + epilog=epilog(), + formatter_class=argparse.RawDescriptionHelpFormatter, + ) + group = parser.add_mutually_exclusive_group() + group.add_argument('-b', '--with-blackboard-variables', default=False, action='store_true', help='add nodes for the blackboard variables') + group.add_argument('-r', '--render', action='store_true', help='render dot tree to file') + group.add_argument('-i', '--interactive', action='store_true', help='pause and wait for keypress at each tick') + return parser + + +def pre_tick_handler(behaviour_tree): + print("\n--------- Run %s ---------\n" % behaviour_tree.count) + + +def post_tick_handler(snapshot_visitor, behaviour_tree): + print( + "\n" + py_trees.display.unicode_tree( + root=behaviour_tree.root, + visited=snapshot_visitor.visited, + previously_visited=snapshot_visitor.previously_visited + ) + ) + print(py_trees.display.unicode_blackboard()) + + +def create_root(name= "RunningOrFailure", condition = typing.List[common.ComparisonExpression]): + + root = py_trees.idioms.either_or( + name=name+"(either_or)", + conditions=condition, + subtrees=[py_trees.behaviours.Running(), py_trees.behaviours.Failure()], + namespace="running_or_failure_"+name, + ) + + return root + +############################################################################## +# Main +############################################################################## + +def main(): + """ + Entry point for the demo script. + """ + args = command_line_argument_parser().parse_args() + py_trees.logging.level = py_trees.logging.Level.DEBUG + root = create_root() + print(description(root)) + + #################### + # Rendering + #################### + """ + if args.render: + print("**************START RENDERING**************") + py_trees.display.render_dot_tree(root) + if py_trees.utilities.which("xdot"): + try: + subprocess.call(["xdot", "%s.dot" % root.name]) + except KeyboardInterrupt: + pass + else: + print("") + console.logerror("No xdot viewer found, skipping display [hint: sudo apt install xdot]") + print("") + print("**************END RENDERING**************") + """ + #################### + # Tree Stewardship + #################### + behaviour_tree = py_trees.trees.BehaviourTree(root) + behaviour_tree.add_pre_tick_handler(pre_tick_handler) + behaviour_tree.visitors.append(py_trees.visitors.DebugVisitor()) + snapshot_visitor = py_trees.visitors.SnapshotVisitor() + behaviour_tree.add_post_tick_handler(functools.partial(post_tick_handler, snapshot_visitor)) + behaviour_tree.visitors.append(snapshot_visitor) + behaviour_tree.setup(timeout=15) + + #################### + # Tick Tock + #################### + + #if args.interactive: + # py_trees.console.read_single_keypress() + for unused_i in range(1, 10): + try: + behaviour_tree.tick() + #if args.interactive: + # py_trees.console.read_single_keypress() + #else: + time.sleep(0.5) + except KeyboardInterrupt: + break + print("\n") + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/harmoni_core/harmoni_pytree/src/harmoni_pytree/subtrees/session.py b/harmoni_core/harmoni_pytree/src/harmoni_pytree/subtrees/session.py new file mode 100644 index 00000000..879b6b5c --- /dev/null +++ b/harmoni_core/harmoni_pytree/src/harmoni_pytree/subtrees/session.py @@ -0,0 +1,175 @@ +#!/usr/bin/env python3 +############################################################################## +# Imports +############################################################################## + +import argparse +import functools +from py_trees.blackboard import Blackboard +from py_trees.idioms import either_or +import py_trees +import time +import subprocess +import py_trees.console as console +import os +import operator +from harmoni_common_lib.constants import * +import running_or_failure as rf +from harmoni_pytree.subtrees import visual_bg as v +from harmoni_pytree.subtrees import interaction_bg as i +from harmoni_pytree.subtrees import mainactivity as m +from harmoni_pytree.subtrees import therapist as t + +############################################################################## +# Classes +############################################################################## + + +def description(root): + content = "\n\n" + content += "\n" + content += "EVENTS\n" + if py_trees.console.has_colours: + banner_line = console.green + "*" * 79 + "\n" + console.reset + s = "\n" + s += banner_line + s += console.bold_white + "Session".center(79) + "\n" + console.reset + s += banner_line + s += "\n" + s += content + s += "\n" + s += banner_line + else: + s = content + return s + + +def epilog(): + if py_trees.console.has_colours: + return console.cyan + "And his noodly appendage reached forth to tickle the blessed...\n" + console.reset + else: + return None + + +def command_line_argument_parser(): + parser = argparse.ArgumentParser(description=description(create_root()), + epilog=epilog(), + formatter_class=argparse.RawDescriptionHelpFormatter, + ) + group = parser.add_mutually_exclusive_group() + group.add_argument('-r', '--render', action='store_true', help='render dot tree to file') + group.add_argument('-i', '--interactive', action='store_true', help='pause and wait for keypress at each tick') + return parser + + +def pre_tick_handler(behaviour_tree): + print("\n--------- Run %s ---------\n" % behaviour_tree.count) + + +def post_tick_handler(snapshot_visitor, behaviour_tree): + print( + "\n" + py_trees.display.unicode_tree( + root=behaviour_tree.root, + visited=snapshot_visitor.visited, + previously_visited=snapshot_visitor.previously_visited + ) + ) + print(py_trees.display.unicode_blackboard()) + + +def create_root(name = "Session"): + root = py_trees.composites.Selector(name = name) + + therapist = t.create_root() + + mainactivity = m.create_root() + inverter_mainactivity = py_trees.decorators.Inverter(name="InverterMainActivity",child=mainactivity) + + rf_mainactivity = rf.create_root(name="RFMainactivity", condition=[ + py_trees.common.ComparisonExpression(PyTreeNameSpace.mainactivity.name+"/finished", True, operator.ne), + py_trees.common.ComparisonExpression(PyTreeNameSpace.mainactivity.name+"/finished", True, operator.eq), + ]) + + visual_bg = v.create_root() + visual_bg_inverter = py_trees.decorators.Inverter(name="VisualBgInverter",child=visual_bg) + + rf_visual = rf.create_root(name="RFVisual", condition=[ + py_trees.common.ComparisonExpression(PyTreeNameSpace.visual.name+"/finished", True, operator.ne), + py_trees.common.ComparisonExpression(PyTreeNameSpace.visual.name+"/finished", True, operator.eq), + ]) + + interaction_bg = i.create_root() + interaction_bg_inverter = py_trees.decorators.Inverter(name="InteractionBgInverter",child=interaction_bg) + + rf_interaction = rf.create_root(name="RFInteracion", condition=[ + py_trees.common.ComparisonExpression(PyTreeNameSpace.interaction.name+"/finished", True, operator.ne), + py_trees.common.ComparisonExpression(PyTreeNameSpace.interaction.name+"/finished", True, operator.eq), + ]) + + root.add_children([therapist, visual_bg_inverter, rf_visual, interaction_bg_inverter, rf_interaction, inverter_mainactivity, rf_mainactivity]) + + return root + +############################################################################## +# Main +############################################################################## + +def main(): + """ + Entry point for the demo script. + """ + args = command_line_argument_parser().parse_args() + py_trees.logging.level = py_trees.logging.Level.DEBUG + root = create_root() + print(description(root)) + + #################### + # Rendering + #################### + """ + if args.render: + print("**************START RENDERING**************") + target_directory = os.path.join(os.getcwd(), "dot_folder/") + py_trees.display.render_dot_tree(root = root,target_directory = target_directory) + if py_trees.utilities.which("xdot"): + try: + subprocess.call(["xdot", target_directory+"%s.dot" % root.name]) + except KeyboardInterrupt: + pass + else: + print("") + console.logerror("No xdot viewer found, skipping display [hint: sudo apt install xdot]") + print("") + print("**************END RENDERING**************") + """ + #################### + # Tree Stewardship + #################### + behaviour_tree = py_trees.trees.BehaviourTree(root) + behaviour_tree.add_pre_tick_handler(pre_tick_handler) + behaviour_tree.visitors.append(py_trees.visitors.DebugVisitor()) + snapshot_visitor = py_trees.visitors.SnapshotVisitor() + behaviour_tree.add_post_tick_handler(functools.partial(post_tick_handler, snapshot_visitor)) + behaviour_tree.visitors.append(snapshot_visitor) + behaviour_tree.setup(timeout=15) + + #################### + # Tick Tock + #################### + + #if args.interactive: + # py_trees.console.read_single_keypress() + for unused_i in range(1, 12): + try: + behaviour_tree.tick() + #if args.interactive: + # py_trees.console.read_single_keypress() + #else: + time.sleep(0.5) + except KeyboardInterrupt: + break + print("\n") + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/harmoni_core/harmoni_pytree/src/harmoni_pytree/subtrees/therapist.py b/harmoni_core/harmoni_pytree/src/harmoni_pytree/subtrees/therapist.py new file mode 100644 index 00000000..6f18056d --- /dev/null +++ b/harmoni_core/harmoni_pytree/src/harmoni_pytree/subtrees/therapist.py @@ -0,0 +1,151 @@ +#!/usr/bin/env python3 +############################################################################## +# Imports +############################################################################## + +import argparse +import functools +from os import name +from py_trees.behaviours import dummy +from py_trees.idioms import either_or +import py_trees +import time +from random import randint +import subprocess +import operator +import py_trees.console as console + +############################################################################## +# Classes +############################################################################## + + +def description(root): + content = "\n\n" + content += "\n" + content += "EVENTS\n" + if py_trees.console.has_colours: + banner_line = console.green + "*" * 79 + "\n" + console.reset + s = "\n" + s += banner_line + s += console.bold_white + "Therapist".center(79) + "\n" + console.reset + s += banner_line + s += "\n" + s += content + s += "\n" + s += banner_line + else: + s = content + return s + + +def epilog(): + if py_trees.console.has_colours: + return console.cyan + "And his noodly appendage reached forth to tickle the blessed...\n" + console.reset + else: + return None + + +def command_line_argument_parser(): + parser = argparse.ArgumentParser(description=description(create_root()), + epilog=epilog(), + formatter_class=argparse.RawDescriptionHelpFormatter, + ) + group = parser.add_mutually_exclusive_group() + group.add_argument('-b', '--with-blackboard-variables', default=False, action='store_true', help='add nodes for the blackboard variables') + group.add_argument('-r', '--render', action='store_true', help='render dot tree to file') + group.add_argument('-i', '--interactive', action='store_true', help='pause and wait for keypress at each tick') + return parser + + +def pre_tick_handler(behaviour_tree): + print("\n--------- Run %s ---------\n" % behaviour_tree.count) + + +def post_tick_handler(snapshot_visitor, behaviour_tree): + print( + "\n" + py_trees.display.unicode_tree( + root=behaviour_tree.root, + visited=snapshot_visitor.visited, + previously_visited=snapshot_visitor.previously_visited + ) + ) + print(py_trees.display.unicode_blackboard()) + + +def create_root(name= "Therapist"): + + root = either_or( + name=name, + conditions=[ + py_trees.common.ComparisonExpression("scene/therapist_needed", True, operator.ne), + py_trees.common.ComparisonExpression("scene/therapist_needed", True, operator.eq), + ], + subtrees=[py_trees.behaviours.Failure(), py_trees.behaviours.Running()], + namespace= "therapist", + ) + + return root + +############################################################################## +# Main +############################################################################## + +def main(): + """ + Entry point for the demo script. + """ + args = command_line_argument_parser().parse_args() + py_trees.logging.level = py_trees.logging.Level.DEBUG + root = create_root() + print(description(root)) + + #################### + # Rendering + #################### + """ + if args.render: + print("**************START RENDERING**************") + py_trees.display.render_dot_tree(root) + if py_trees.utilities.which("xdot"): + try: + subprocess.call(["xdot", "%s.dot" % root.name]) + except KeyboardInterrupt: + pass + else: + print("") + console.logerror("No xdot viewer found, skipping display [hint: sudo apt install xdot]") + print("") + print("**************END RENDERING**************") + """ + #################### + # Tree Stewardship + #################### + behaviour_tree = py_trees.trees.BehaviourTree(root) + behaviour_tree.add_pre_tick_handler(pre_tick_handler) + behaviour_tree.visitors.append(py_trees.visitors.DebugVisitor()) + snapshot_visitor = py_trees.visitors.SnapshotVisitor() + behaviour_tree.add_post_tick_handler(functools.partial(post_tick_handler, snapshot_visitor)) + behaviour_tree.visitors.append(snapshot_visitor) + behaviour_tree.setup(timeout=15) + + #################### + # Tick Tock + #################### + + #if args.interactive: + # py_trees.console.read_single_keypress() + for unused_i in range(1, 10): + try: + behaviour_tree.tick() + #if args.interactive: + # py_trees.console.read_single_keypress() + #else: + time.sleep(0.5) + except KeyboardInterrupt: + break + print("\n") + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/harmoni_core/harmoni_pytree/src/harmoni_pytree/subtrees/visual_bg.py b/harmoni_core/harmoni_pytree/src/harmoni_pytree/subtrees/visual_bg.py new file mode 100755 index 00000000..e90e1cf0 --- /dev/null +++ b/harmoni_core/harmoni_pytree/src/harmoni_pytree/subtrees/visual_bg.py @@ -0,0 +1,243 @@ +#!/usr/bin/env python3 +############################################################################## +# Imports +############################################################################## + +import argparse +import functools +from os import name +from py_trees.behaviours import dummy +import py_trees +import time +from random import randint +import subprocess +import operator +import py_trees.console as console + +import rospy +from harmoni_common_lib.constants import * + +from harmoni_pytree.leaves.scene_manager_visualbg import SceneManagerVisualBg +from harmoni_pytree.leaves.subtree_result_visualbg import SubTreeResultVisualBg +from harmoni_pytree.leaves.aws_lex_trigger_service import AWSLexTriggerServicePytree +from harmoni_pytree.leaves.aws_lex_analyzer_service import AWSLexAnalyzerServicePytree +from harmoni_pytree.leaves.aws_tts_service import AWSTtsServicePytree +from harmoni_pytree.leaves.facial_exp_service import FacialExpServicePytree +from harmoni_pytree.leaves.lip_sync_service import LipSyncServicePytree +from harmoni_pytree.leaves.google_service import SpeechToTextServicePytree +from harmoni_pytree.leaves.speaker_service import SpeakerServicePytree + +############################################################################## +# Classes +############################################################################## + +""" +Visual Bg +""" +def description(root): + content = "\n\n" + content += "\n" + content += "EVENTS\n" + if py_trees.console.has_colours: + banner_line = console.green + "*" * 79 + "\n" + console.reset + s = "\n" + s += banner_line + s += console.bold_white + "Visual Bg".center(79) + "\n" + console.reset + s += banner_line + s += "\n" + s += content + s += "\n" + s += banner_line + else: + s = content + return s + + +def epilog(): + if py_trees.console.has_colours: + return console.cyan + "And his noodly appendage reached forth to tickle the blessed...\n" + console.reset + else: + return None + +def command_line_argument_parser(): + parser = argparse.ArgumentParser(description=description(create_root()), + epilog=epilog(), + formatter_class=argparse.RawDescriptionHelpFormatter, + ) + group = parser.add_mutually_exclusive_group() + group.add_argument('-b', '--with-blackboard-variables', default=False, action='store_true', help='add nodes for the blackboard variables') + group.add_argument('-r', '--render', action='store_true', help='render dot tree to file') + group.add_argument('-i', '--interactive', action='store_true', help='pause and wait for keypress at each tick') + return parser + +def pre_tick_handler(behaviour_tree): + print("\n--------- Run %s ---------\n" % behaviour_tree.count) + +def post_tick_handler(snapshot_visitor, behaviour_tree): + print( + "\n" + py_trees.display.unicode_tree( + root=behaviour_tree.root, + visited=snapshot_visitor.visited, + previously_visited=snapshot_visitor.previously_visited + ) + ) + print(py_trees.display.unicode_blackboard()) + +def create_root(name = "Visual_Bg"): + root = py_trees.composites.Sequence(name = name) + + Success1 = py_trees.behaviours.Success(name="Success") + Success2 = py_trees.behaviours.Success(name="Success") + Success3 = py_trees.behaviours.Success(name="Success") + Success4 = py_trees.behaviours.Success(name="Success") + + scene_manager = SceneManagerVisualBg("SceneManagerVisual") + + bot_trigger=AWSLexTriggerServicePytree("AwsLexTriggerVisualActivity") + + bot_analyzer=AWSLexAnalyzerServicePytree("AwsLexAnalyzerVisualActivity") + + tts = AWSTtsServicePytree("AwsTtsVisualBg") + + stt=SpeechToTextServicePytree("SpeechToTextPytreeVisualBg") + + face_exp=FacialExpServicePytree("FacialExpVisualBg") + + speaker=SpeakerServicePytree("SpeakerVisualBg") + + lip_sync=LipSyncServicePytree("LipSyncVisualBg") + + subtree_result = SubTreeResultVisualBg("SubTreeVisual") + + parall_speaker = py_trees.composites.Parallel(name="ParallelSpeaker") + parall_speaker.add_children([speaker,lip_sync]) + + inverter_stt = py_trees.decorators.Inverter(name="SttInverter",child=stt) + + periodic_success = py_trees.decorators.FailureIsRunning(child=py_trees.behaviours.SuccessEveryN(name="SuccessEveryN",n=3),name="FIsRSEN") + + eor_null_speech = py_trees.idioms.either_or( + name="EitherOrNullSpeech", + conditions=[ + py_trees.common.ComparisonExpression(DetectorNameSpace.stt.name+ "/result", "null", operator.eq), + py_trees.common.ComparisonExpression(DetectorNameSpace.stt.name+ "/result", "null", operator.ne), + ], + subtrees=[periodic_success,Success4], + namespace="eor_null_speech", + ) + + selector_stt = py_trees.composites.Selector(name="SelectorDetectKid") + selector_stt.add_children([inverter_stt, eor_null_speech]) + + sequen_detect_kid = py_trees.composites.Sequence(name="SequenceDetectKid") + sequen_detect_kid.add_children([selector_stt, bot_analyzer]) + + eor_face = py_trees.idioms.either_or( + name="EitherOrFace", + conditions=[ + py_trees.common.ComparisonExpression(PyTreeNameSpace.scene.name + "/face_exp", "null", operator.ne), + py_trees.common.ComparisonExpression(PyTreeNameSpace.scene.name + "/face_exp", "null", operator.eq), + ], + subtrees=[face_exp, Success3], + namespace="eor_face", + ) + + parall_detect_and_face = py_trees.composites.Parallel(name="ParallelDetectAndFace") + parall_detect_and_face.add_children([sequen_detect_kid, eor_face]) + + eor_trigger = py_trees.idioms.either_or( + name="EitherOrTrigger", + conditions=[ + py_trees.common.ComparisonExpression(PyTreeNameSpace.scene.name + "/" + PyTreeNameSpace.visual.name + "/do_trigger", True, operator.eq), + py_trees.common.ComparisonExpression(PyTreeNameSpace.scene.name + "/" + PyTreeNameSpace.visual.name + "/do_trigger", True, operator.ne), + ], + subtrees=[bot_trigger, Success2], + namespace="eor_trigger", + ) + sequen_visual = py_trees.composites.Sequence(name="SequenceVisual") + sequen_visual.add_children([scene_manager, eor_trigger, tts, parall_speaker, parall_detect_and_face]) + + eor_visual = py_trees.idioms.either_or( + name="EitherOrVisual", + conditions=[ + py_trees.common.ComparisonExpression(DetectorNameSpace.face_detect.name + "/result", "null", operator.eq), + py_trees.common.ComparisonExpression(DetectorNameSpace.face_detect.name + "/result", "null", operator.ne), + ], + subtrees=[sequen_visual, Success1], + namespace="eor_visual", + ) + + root.add_children([eor_visual, subtree_result]) + + return root + +############################################################################## +# Main +############################################################################## + +def render_with_args(root): + + args = command_line_argument_parser().parse_args() + #################### + # Rendering + #################### + if args.render: + print("**************START RENDERING**************") + py_trees.display.render_dot_tree(root) + if py_trees.utilities.which("xdot"): + try: + subprocess.call(["xdot", "%s.dot" % root.name]) + except KeyboardInterrupt: + pass + else: + print("") + console.logerror("No xdot viewer found, skipping display [hint: sudo apt install xdot]") + print("") + print("**************END RENDERING**************") + + +def main(): + """ + Entry point for the demo script. + """ + + py_trees.logging.level = py_trees.logging.Level.DEBUG + root = create_root() + print(description(root)) + + #uncomment the following line if you want to render the dot_tree + #render_with_args(root) + + #################### + # Tree Stewardship + #################### + rospy.init_node("visualbg_default", log_level=rospy.INFO) + + behaviour_tree = py_trees.trees.BehaviourTree(root) + behaviour_tree.add_pre_tick_handler(pre_tick_handler) + behaviour_tree.visitors.append(py_trees.visitors.DebugVisitor()) + snapshot_visitor = py_trees.visitors.SnapshotVisitor() + behaviour_tree.add_post_tick_handler(functools.partial(post_tick_handler, snapshot_visitor)) + behaviour_tree.visitors.append(snapshot_visitor) + additional_parameters = dict([]) + behaviour_tree.setup(timeout=15,**additional_parameters) + + #################### + # Tick Tock + #################### + + #if args.interactive: + # py_trees.console.read_single_keypress() + for unused_i in range(1, 50): + try: + behaviour_tree.tick() + #if args.interactive: + # py_trees.console.read_single_keypress() + #else: + time.sleep(1) + except KeyboardInterrupt: + break + print("\n") + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/harmoni_core/harmoni_pytree/test/bot_test/lex_pytree.test b/harmoni_core/harmoni_pytree/test/bot_test/lex_pytree.test new file mode 100644 index 00000000..6a68cd69 --- /dev/null +++ b/harmoni_core/harmoni_pytree/test/bot_test/lex_pytree.test @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/harmoni_core/harmoni_pytree/test/bot_test/rostest_lex_pytree.py b/harmoni_core/harmoni_pytree/test/bot_test/rostest_lex_pytree.py new file mode 100755 index 00000000..6e406e63 --- /dev/null +++ b/harmoni_core/harmoni_pytree/test/bot_test/rostest_lex_pytree.py @@ -0,0 +1,77 @@ +#!/usr/bin/env python3 + + +PKG = "test_lex" + +# Common Imports +import unittest, rospy, roslib, sys + +# Specific Imports +from actionlib_msgs.msg import GoalStatus +from harmoni_common_msgs.msg import harmoniAction, harmoniFeedback, harmoniResult +from std_msgs.msg import String +from harmoni_common_lib.action_client import HarmoniActionClient +from std_msgs.msg import String +from harmoni_common_lib.constants import ActuatorNameSpace, ActionType, State, DialogueNameSpace +from collections import deque +import os, io +import ast +import time +#py_tree +import py_trees +from harmoni_pytree.aws_lex_service_pytree import AWSLexServicePytree + +class TestLexPyTree(unittest.TestCase): + + def setUp(self): + """ + Set up the client for requesting to harmoni_bot + """ + rospy.init_node("test_lex", log_level=rospy.INFO) + self.data = rospy.get_param( + "test_bot_input" + ) + self.instance_id = rospy.get_param("instance_id") + # NOTE currently no feedback, status, or result is received. + py_trees.logging.level = py_trees.logging.Level.DEBUG + + self.blackboardProvaIn = py_trees.blackboard.Client(name="blackboardProva", namespace=DialogueNameSpace.bot.name) + self.blackboardProvaIn.register_key("result_data", access=py_trees.common.Access.WRITE) + self.blackboardProvaIn.register_key("result_message", access=py_trees.common.Access.WRITE) + self.blackboardProvaOut = py_trees.blackboard.Client(name="blackboardProva", namespace=DialogueNameSpace.bot.name + "output") + self.blackboardProvaOut.register_key("result_data", access=py_trees.common.Access.READ) + self.blackboardProvaOut.register_key("result_message", access=py_trees.common.Access.READ) + + self.blackboardProvaIn.result_message = State.SUCCESS + self.blackboardProvaIn.result_data = self.data + print(self.blackboardProvaIn) + print(self.blackboardProvaOut) + additional_parameters = dict([ + (DialogueNameSpace.bot.name,False)]) + rospy.loginfo("--------------------"+str(additional_parameters)) + self.botPyTree = AWSLexServicePytree("botPyTreeTest") + self.botPyTree.setup(**additional_parameters) + rospy.loginfo("Testbot: Started up. waiting for bot startup") + rospy.loginfo("Testbot: Started") + + + + def test_leaf_pytree_bot(self): + rospy.loginfo(f"The input data is {self.data}") + for unused_i in range(0, 3): + self.botPyTree.tick_once() + time.sleep(0.5) + print(self.blackboardProvaIn) + print(self.blackboardProvaOut) + print("\n") + return + + +def main(): + import rostest + rospy.loginfo("test_lex started") + rospy.loginfo("TestLex: sys.argv: %s" % str(sys.argv)) + rostest.rosrun(PKG, "test_lex_pytree", TestLexPyTree, sys.argv) + +if __name__ == "__main__": + main() diff --git a/harmoni_core/harmoni_pytree/test/face_test/face_pytree.test b/harmoni_core/harmoni_pytree/test/face_test/face_pytree.test new file mode 100644 index 00000000..cb9aeb9b --- /dev/null +++ b/harmoni_core/harmoni_pytree/test/face_test/face_pytree.test @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/harmoni_core/harmoni_pytree/test/face_test/rostest_face_pytree.py b/harmoni_core/harmoni_pytree/test/face_test/rostest_face_pytree.py new file mode 100755 index 00000000..b68e9ced --- /dev/null +++ b/harmoni_core/harmoni_pytree/test/face_test/rostest_face_pytree.py @@ -0,0 +1,75 @@ +#!/usr/bin/env python3 + + +PKG = "test_face_pytree" +# Common Imports +import unittest, rospy, roslib, sys + +# Specific Imports +from actionlib_msgs.msg import GoalStatus +from harmoni_common_msgs.msg import harmoniAction, harmoniFeedback, harmoniResult +from std_msgs.msg import String +from harmoni_common_lib.action_client import HarmoniActionClient +from std_msgs.msg import String +from harmoni_common_lib.constants import ActuatorNameSpace, ActionType, State +from collections import deque +import os, io +import ast +import time +#py_tree +import py_trees +from harmoni_pytree.face_service_pytree import FaceServicePytree + + +class TestFacePyTree(unittest.TestCase): + + def setUp(self): + """ + Set up the client for requesting to harmoni_face + """ + rospy.init_node("test_face_pytree", log_level=rospy.INFO) + self.data = rospy.get_param( + "test_face_input" + ) + self.instance_id = rospy.get_param("instance_id") + # NOTE currently no feedback, status, or result is received. + py_trees.logging.level = py_trees.logging.Level.DEBUG + + # ex namespace: harmoni_tts + self.blackboardProva = py_trees.blackboard.Client(name="blackboardProva", namespace=ActuatorNameSpace.tts.name) + self.blackboardProva.register_key("result_data", access=py_trees.common.Access.WRITE) + self.blackboardProva.register_key("result_message", access=py_trees.common.Access.WRITE) + + self.blackboardProva.result_message = State.SUCCESS + self.blackboardProva.result_data = self.data + + additional_parameters = dict([ + (ActuatorNameSpace.face.name,True)]) + rospy.loginfo("--------------------"+str(additional_parameters)) + self.facePyTree = FaceServicePytree("facePyTreeTest") + self.facePyTree.setup(**additional_parameters) + + rospy.loginfo("TestFace: Started up. waiting for face startup") + rospy.loginfo("TestFace: Started") + + + + def test_leaf_pytree_mouth(self): + rospy.loginfo(f"The input data is {self.data}") + for unused_i in range(0, 4): + self.facePyTree.tick_once() + time.sleep(0.5) + print(self.blackboardProva) + print("\n") + assert self.result_mouth == True + + +def main(): + import rostest + rospy.loginfo("test_face started") + rospy.loginfo("TestFace: sys.argv: %s" % str(sys.argv)) + rostest.rosrun(PKG, "test_face_pytree", TestFacePyTree, sys.argv) + + +if __name__ == "__main__": + main() diff --git a/harmoni_core/harmoni_pytree/test/gesture_test/gesture_pytree.test b/harmoni_core/harmoni_pytree/test/gesture_test/gesture_pytree.test new file mode 100644 index 00000000..0238d0d1 --- /dev/null +++ b/harmoni_core/harmoni_pytree/test/gesture_test/gesture_pytree.test @@ -0,0 +1,6 @@ + + + + + + diff --git a/harmoni_core/harmoni_pytree/test/gesture_test/rostest_gesture_pytree.py b/harmoni_core/harmoni_pytree/test/gesture_test/rostest_gesture_pytree.py new file mode 100755 index 00000000..179718d2 --- /dev/null +++ b/harmoni_core/harmoni_pytree/test/gesture_test/rostest_gesture_pytree.py @@ -0,0 +1,74 @@ +#!/usr/bin/env python3 + + +PKG = "test_harmoni_gesture" +# Common Imports +import unittest, rospy, roslib, sys + +# Specific Imports +from actionlib_msgs.msg import GoalStatus +from harmoni_common_msgs.msg import harmoniAction, harmoniFeedback, harmoniResult +from std_msgs.msg import String +from harmoni_common_lib.action_client import HarmoniActionClient +from std_msgs.msg import String +from harmoni_common_lib.constants import ActuatorNameSpace, ActionType, State +from collections import deque +import os, io +import ast +import time +#py_tree +import py_trees +from harmoni_pytree.gesture_service_pytree import GestureServicePytree + + +class TestGesturePyTree(unittest.TestCase): + + def setUp(self): + """ + Set up the client for requesting to harmoni_gesture + """ + rospy.init_node("test_gesture_pytree", log_level=rospy.INFO) + self.data = rospy.get_param( + "test_gesture_input" + ) + self.instance_id = rospy.get_param("instance_id") + # NOTE currently no feedback, status, or result is received. + py_trees.logging.level = py_trees.logging.Level.DEBUG + + self.blackboardProva = py_trees.blackboard.Client(name="blackboardProva", namespace=ActuatorNameSpace.gesture.name) + self.blackboardProva.register_key("result_data", access=py_trees.common.Access.WRITE) + self.blackboardProva.register_key("result_message", access=py_trees.common.Access.WRITE) + + self.blackboardProva.result_message = State.SUCCESS + self.blackboardProva.result_data = self.data + + additional_parameters = dict([ + (ActuatorNameSpace.gesture.name,True)]) + rospy.loginfo("--------------------"+str(additional_parameters)) + self.gesturePyTree = GestureServicePytree("gesturePyTreeTest") + self.gesturePyTree.setup(**additional_parameters) + + rospy.loginfo("TestGesture: Started up. waiting for gesture startup") + rospy.loginfo("TestGesture: Started") + + + + def test_leaf_pytree_gesture(self): + rospy.loginfo(f"The input data is {self.data}") + for unused_i in range(0, 4): + self.gesturePyTree.tick_once() + time.sleep(0.5) + print(self.blackboardProva) + print("\n") + return + + +def main(): + import rostest + rospy.loginfo("test_gesture started") + rospy.loginfo("TestGesture: sys.argv: %s" % str(sys.argv)) + rostest.rosrun(PKG, "test_gesture_pytree", TestGesture, sys.argv) + + +if __name__ == "__main__": + main() diff --git a/harmoni_core/harmoni_pytree/test/pytree.test b/harmoni_core/harmoni_pytree/test/pytree.test new file mode 100644 index 00000000..b40e8a41 --- /dev/null +++ b/harmoni_core/harmoni_pytree/test/pytree.test @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/harmoni_core/harmoni_pytree/test/speaker_test/rostest_speaker_pytree.py b/harmoni_core/harmoni_pytree/test/speaker_test/rostest_speaker_pytree.py new file mode 100755 index 00000000..7606ac59 --- /dev/null +++ b/harmoni_core/harmoni_pytree/test/speaker_test/rostest_speaker_pytree.py @@ -0,0 +1,75 @@ +#!/usr/bin/env python3 + + +PKG = "test_harmoni_speaker" +# Common Imports +import unittest, rospy, roslib, sys + +# Specific Imports +from actionlib_msgs.msg import GoalStatus +from harmoni_common_msgs.msg import harmoniAction, harmoniFeedback, harmoniResult +from std_msgs.msg import String +from harmoni_common_lib.action_client import HarmoniActionClient +from std_msgs.msg import String +from harmoni_common_lib.constants import ActuatorNameSpace, ActionType, State +from collections import deque +import os, io +import ast +import time +#py_tree +import py_trees +from harmoni_pytree.speaker_service_pytree import SpeakerServicePytree + + +class TestSpeakerPyTree(unittest.TestCase): + + def setUp(self): + """ + Set up the client for requesting to harmoni_speaker + """ + rospy.init_node("test_speaker_pytree", log_level=rospy.INFO) + self.data = rospy.get_param( + "test_speaker_input" + ) + self.instance_id = rospy.get_param("instance_id") + # NOTE currently no feedback, status, or result is received. + py_trees.logging.level = py_trees.logging.Level.DEBUG + + # ex namespace: harmoni_tts + self.blackboardProva = py_trees.blackboard.Client(name="blackboardProva", namespace=ActuatorNameSpace.tts.name) + self.blackboardProva.register_key("result_data", access=py_trees.common.Access.WRITE) + self.blackboardProva.register_key("result_message", access=py_trees.common.Access.WRITE) + + self.blackboardProva.result_message = State.SUCCESS + self.blackboardProva.result_data = self.data + + additional_parameters = dict([ + (ActuatorNameSpace.speaker.name,False)]) + rospy.loginfo("--------------------"+str(additional_parameters)) + self.speakerPyTree = SpeakerServicePytree("speakerPyTreeTest") + self.speakerPyTree.setup(**additional_parameters) + + rospy.loginfo("Testspeaker: Started up. waiting for speaker startup") + rospy.loginfo("Testspeaker: Started") + + + + def test_leaf_pytree_speaker(self): + rospy.loginfo(f"The input data is {self.data}") + for unused_i in range(0, 4): + self.speakerPyTree.tick_once() + time.sleep(0.5) + print(self.blackboardProva) + print("\n") + return + + +def main(): + import rostest + rospy.loginfo("test_speaker started") + rospy.loginfo("Testspeaker: sys.argv: %s" % str(sys.argv)) + rostest.rosrun(PKG, "test_speaker_pytree", TestSpeakerPyTree, sys.argv) + + +if __name__ == "__main__": + main() diff --git a/harmoni_core/harmoni_pytree/test/speaker_test/speaker_pytree.test b/harmoni_core/harmoni_pytree/test/speaker_test/speaker_pytree.test new file mode 100755 index 00000000..d9269ac5 --- /dev/null +++ b/harmoni_core/harmoni_pytree/test/speaker_test/speaker_pytree.test @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/harmoni_core/harmoni_pytree/test/test_pytree.py b/harmoni_core/harmoni_pytree/test/test_pytree.py new file mode 100755 index 00000000..e16af01a --- /dev/null +++ b/harmoni_core/harmoni_pytree/test/test_pytree.py @@ -0,0 +1,105 @@ +#!/usr/bin/env python3 + + +PKG = "test_harmoni_pattern" +# Common Imports +import unittest, rospy, roslib, sys + +# Specific Imports +from actionlib_msgs.msg import GoalStatus +from harmoni_common_lib.action_client import HarmoniActionClient +from harmoni_common_lib.constants import ( + DetectorNameSpace, + ActuatorNameSpace, + DialogueNameSpace, + SensorNameSpace, + ActionType, + State, +) +from harmoni_pytree.pytree_root import PyTreeRoot +from harmoni_common_msgs.msg import harmoniAction, harmoniFeedback, harmoniResult +from audio_common_msgs.msg import AudioData +from std_msgs.msg import String +import time +import py_trees +import py_trees.console as console + + +class TestPyTree(unittest.TestCase): + """This test of the sequential pattern player uses a simple mic test script. + + If the mic is launched and starts successfully (as verified by the subscriber) + then the test is passed. + """ + def setUp(self): + self.feedback = State.INIT + self.result = False + rospy.init_node("test_pytree", log_level=rospy.INFO) + """ + Entry point for the demo script. + """ + self.pytree_root = PyTreeRoot() + #args = self.pytree_root.command_line_argument_parser().parse_args() + print(self.pytree_root.description()) + py_trees.logging.level = py_trees.logging.Level.DEBUG + self.root = self.pytree_root.create_root() + #################### + # Tree Stewardship + #################### + print("Tree Stewardship") + self.behaviour_tree = py_trees.trees.BehaviourTree(self.root) + self.behaviour_tree.visitors.append(py_trees.visitors.DebugVisitor()) + self.behaviour_tree.visitors.append(py_trees.visitors.SnapshotVisitor()) + additional_parameters = dict([ + (ActuatorNameSpace.tts.name,False), + (ActuatorNameSpace.speaker.name,False), + (ActuatorNameSpace.face.name,True), + (DialogueNameSpace.bot.name,True)]) + self.behaviour_tree.setup(timeout=15,**additional_parameters) + + print(py_trees.display.unicode_tree(root=self.root)) + + #################### + # Execute + #################### + + blackboardProvaIn = py_trees.blackboard.Client(name="blackboardProva", namespace=DialogueNameSpace.bot.name) + blackboardProvaIn.register_key("result_data", access=py_trees.common.Access.WRITE) + blackboardProvaIn.register_key("result_message", access=py_trees.common.Access.WRITE) + blackboardProvaIn.result_message = State.SUCCESS + blackboardProvaIn.result_data = "Vorrei ordinare dei fiori" + + def print_tree(self, tree): + print(py_trees.display.unicode_tree(root=tree.root, show_status=True)) + + + def test_pytree(self): + #input = self.data + + + for i in range(1, 12): + print("\n--------- Tick {0} ---------\n".format(i)) + self.behaviour_tree.tick( + pre_tick_handler=None, + post_tick_handler=self.print_tree + ) + time.sleep(1.0) + if self.behaviour_tree.root.status == py_trees.common.Status.SUCCESS: + break + + rospy.loginfo("TestPyTree: Started up. waiting for pytree startup") + return + + + +def main(): + # TODO combine validity tests into test suite so that setup doesn't have to run over and over. + import rostest + + rospy.loginfo("test_pytree started") + rospy.loginfo("TestPyTree: sys.argv: %s" % str(sys.argv)) + rostest.rosrun(PKG, "test_pytree", TestPyTree, sys.argv) + + +if __name__ == "__main__": + main() diff --git a/harmoni_core/harmoni_pytree/test/tts_test/polly_pytree.test b/harmoni_core/harmoni_pytree/test/tts_test/polly_pytree.test new file mode 100644 index 00000000..ac35c415 --- /dev/null +++ b/harmoni_core/harmoni_pytree/test/tts_test/polly_pytree.test @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/harmoni_core/harmoni_pytree/test/tts_test/rostest_polly_pytree.py b/harmoni_core/harmoni_pytree/test/tts_test/rostest_polly_pytree.py new file mode 100755 index 00000000..98fb09fe --- /dev/null +++ b/harmoni_core/harmoni_pytree/test/tts_test/rostest_polly_pytree.py @@ -0,0 +1,83 @@ +#!/usr/bin/env python3 + + +PKG = "test_harmoni_polly" +# Common Imports +import unittest, rospy, roslib, sys + +# Specific Imports +from actionlib_msgs.msg import GoalStatus +from harmoni_common_msgs.msg import harmoniAction, harmoniFeedback, harmoniResult +from std_msgs.msg import String +from harmoni_common_lib.action_client import HarmoniActionClient +from std_msgs.msg import String +from harmoni_common_lib.constants import ActuatorNameSpace, ActionType, State, DialogueNameSpace +from collections import deque +import os, io +import ast +import time +#py_tree +import py_trees +from harmoni_pytree.aws_tts_service_pytree import AWSTtsServicePytree + + +class TestPollyPyTree(unittest.TestCase): + + def setUp(self): + """ + Set up the client for requesting to harmoni_tts + """ + rospy.init_node("test_polly_pytree", log_level=rospy.INFO) + self.data = rospy.get_param( + "test_polly_input" + ) + self.data = ast.literal_eval(self.data) + self.instance_id = rospy.get_param("instance_id") + # NOTE currently no feedback, status, or result is received. + py_trees.logging.level = py_trees.logging.Level.DEBUG + + # ex namespace: harmoni_tts + py_trees.logging.level = py_trees.logging.Level.DEBUG + + self.blackboardProva = py_trees.blackboard.Client(name="blackboardProva", namespace=ActuatorNameSpace.tts.name) + self.blackboardProva.register_key("result_data", access=py_trees.common.Access.READ) + self.blackboardProva.register_key("result_message", access=py_trees.common.Access.READ) + self.blackboard_output_bot = py_trees.blackboard.Client(name="blackboardProva", namespace=DialogueNameSpace.bot.name+"output") + self.blackboard_output_bot.register_key("result_data", access=py_trees.common.Access.WRITE) + self.blackboard_output_bot.register_key("result_message", access=py_trees.common.Access.WRITE) + + self.blackboard_output_bot.result_message = State.SUCCESS + self.blackboard_output_bot.result_data = self.data + print(self.blackboardProva) + print(self.blackboard_output_bot) + + additional_parameters = dict([ + (ActuatorNameSpace.tts.name,False)]) + rospy.loginfo("--------------------"+str(additional_parameters)) + self.ttsPyTree = AWSTtsServicePytree("ttsPyTreeTest") + self.ttsPyTree.setup(**additional_parameters) + + rospy.loginfo("Testtts: Started up. waiting for tts startup") + rospy.loginfo("Testtts: Started") + + + + def test_leaf_pytree_tts(self): + rospy.loginfo(f"The input data is {self.data}") + for unused_i in range(0, 4): + self.ttsPyTree.tick_once() + time.sleep(0.5) + print(self.blackboardProva) + print("\n") + return + + +def main(): + import rostest + rospy.loginfo("test_polly started") + rospy.loginfo("TestPolly: sys.argv: %s" % str(sys.argv)) + rostest.rosrun(PKG, "test_polly_pytree", TestPollyPyTree, sys.argv) + + +if __name__ == "__main__": + main() diff --git a/harmoni_core/harmoni_pytree/test/web_test/rostest_web_pytree.py b/harmoni_core/harmoni_pytree/test/web_test/rostest_web_pytree.py new file mode 100755 index 00000000..afaf9e5d --- /dev/null +++ b/harmoni_core/harmoni_pytree/test/web_test/rostest_web_pytree.py @@ -0,0 +1,82 @@ +#!/usr/bin/env python3 + + +PKG = "test_harmoni_web" +# Common Imports +import unittest, rospy, roslib, sys + +# Specific Imports +from actionlib_msgs.msg import GoalStatus +from harmoni_common_msgs.msg import harmoniAction, harmoniFeedback, harmoniResult +from std_msgs.msg import String +from harmoni_common_lib.action_client import HarmoniActionClient +from std_msgs.msg import String +from harmoni_common_lib.constants import ActuatorNameSpace, ActionType, State, DialogueNameSpace +from collections import deque +import os, io +import ast +import time +#py_tree +import py_trees +from harmoni_pytree.web_service_pytree import WebServicePytree + + +class TestWebPyTree(unittest.TestCase): + + def setUp(self): + """ + Set up the client for requesting to harmoni_web + """ + rospy.init_node("test_web_pytree", log_level=rospy.INFO) + self.data = rospy.get_param( + "test_web_input" + ) + self.instance_id = rospy.get_param("instance_id") + # NOTE currently no feedback, status, or result is received. + py_trees.logging.level = py_trees.logging.Level.DEBUG + + # ex namespace: harmoni_web + py_trees.logging.level = py_trees.logging.Level.DEBUG + + self.blackboardProva = py_trees.blackboard.Client(name="blackboardProva", namespace=ActuatorNameSpace.web.name) + self.blackboardProva.register_key("result_message", access=py_trees.common.Access.READ) + self.blackboardProva.register_key("result_data", access=py_trees.common.Access.READ) + + self.blackboard_input_web = py_trees.blackboard.Client(name="blackboardProva", namespace="input_web") + self.blackboard_input_web.register_key("result_data", access=py_trees.common.Access.WRITE) + self.blackboard_input_web.register_key("result_message", access=py_trees.common.Access.WRITE) + + self.blackboard_input_web.result_message = State.SUCCESS + self.blackboard_input_web.result_data = self.data + + additional_parameters = dict([ + (ActuatorNameSpace.web.name,False)]) + rospy.loginfo("--------------------"+str(additional_parameters)) + self.webPyTree = WebServicePytree("webPyTreeTest") + self.webPyTree.setup(**additional_parameters) + + rospy.loginfo("Testweb: Started up. waiting for web startup") + rospy.loginfo("Testweb: Started") + + + + def test_leaf_pytree_web(self): + rospy.loginfo(f"The input data is {self.data}") + for unused_i in range(0, 4): + self.webPyTree.tick_once() + time.sleep(2) + print(self.blackboardProva) + print(self.blackboard_input_web) + print("\n") + return + + +def main(): + import rostest + rospy.loginfo("test_web started") + rospy.loginfo("TestWeb: sys.argv: %s" % str(sys.argv)) + rostest.rosrun(PKG, "test_web_pytree", TestWebPyTree, sys.argv) + + +if __name__ == "__main__": + main() diff --git a/harmoni_core/harmoni_pytree/test/web_test/web_pytree.test b/harmoni_core/harmoni_pytree/test/web_test/web_pytree.test new file mode 100644 index 00000000..84b0bff1 --- /dev/null +++ b/harmoni_core/harmoni_pytree/test/web_test/web_pytree.test @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/harmoni_detectors/harmoni_imageai/CMakeLists.txt b/harmoni_detectors/harmoni_imageai/CMakeLists.txt new file mode 100755 index 00000000..2f63a7f2 --- /dev/null +++ b/harmoni_detectors/harmoni_imageai/CMakeLists.txt @@ -0,0 +1,200 @@ +cmake_minimum_required(VERSION 2.8.3) +project(harmoni_imageai) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + audio_common_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES harmoni_imageai +# CATKIN_DEPENDS roscpp rospy +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/harmoni_imageai.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/harmoni_imageai_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_harmoni_imageai.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +#catkin_add_nosetests(test/unittest_imageai.py) +catkin_add_nosetests(test) diff --git a/harmoni_detectors/harmoni_imageai/README.md b/harmoni_detectors/harmoni_imageai/README.md new file mode 100755 index 00000000..9ad887ab --- /dev/null +++ b/harmoni_detectors/harmoni_imageai/README.md @@ -0,0 +1,58 @@ +# HARMONI Imageai + + +ImageAI is a python library built to empower developers to build applications and systems with self-contained Deep Learning and Computer Vision capabilities. +Now in HARMONI you can find one among the several services that ImageAI provides: stream detection. + +In particular we created two different services + +- the first one use the state of art model for object detection yoloV3 +- the other one use a custom model trained on the top of yoloV3 + +## Setup + +In order to use ImageAI import the python library by running the following commands: + +```bash +$ pip install imageai --upgrade +$ pip install tensorflow==2.4.0 +``` + +**(recommended)** For more information follow the official documentation: [https://github.com/OlafenwaMoses/ImageAI#dependencies](https://github.com/OlafenwaMoses/ImageAI#dependencies) + +## Usage + +So as to use plain yoloV3 model you have to do nothing but download the **yoloV3** model [here](https://github.com/OlafenwaMoses/ImageAI/releases/download/1.0/yolo.h5) and then add it in the container in following path: `HARMONI/harmoni_detectors/harmoni_imageai/src/[PUT HERE]`. + +As far custom models are concerned, there are some steps that have to be followed: + +1. Create your own model following the steps in the documentation of ImageAI here: [https://github.com/OlafenwaMoses/ImageAI/blob/master/imageai/Detection/Custom/CUSTOMDETECTIONTRAINING.md](https://github.com/OlafenwaMoses/ImageAI/blob/master/imageai/Detection/Custom/CUSTOMDETECTIONTRAINING.md). +At the end of this process you will obtain 2 files: a model.h5 and a config.json. +2. Put these two files in the following path `HARMONI/harmoni_detectors/harmoni_imageai/src/[PUT HERE]` in the container. Be sure that names coincide with the ones in `custom_configuration.yaml` file under `harmoni_imageai` folder. + +Full documentation of ImageAI: [https://github.com/OlafenwaMoses/ImageAI#readme](https://github.com/OlafenwaMoses/ImageAI#readme) + + +## Testing + +Here are the steps that you can follow in order to run the ImageAI service in HARMONI. + +**YoloV3** +Run the following commands in order to run camera service and yolo service in two different terminals: + +``` bash +roslaunch harmoni_sensors camera_service.launch +roslaunch harmoni_imageai yolo_service.launch +``` + +Camera will open and one frame will be captured and put as input of the yolov3 model. Once the output of the imageai service arrives another frame will be captured and again used as input for yolov3 model and so on. + +**Custom yolo** +Run the following command in order to run camera service and custom yolo service in two different terminals: + +``` bash +roslaunch harmoni_sensors camera_service.launch +roslaunch harmoni_imageai custom_service.launch +``` + +Camera will open and one frame will be captured and put as input of the custom model. Once the output of the imageai service arrives another frame will be captured and again used as input for custom model and so on. diff --git a/harmoni_detectors/harmoni_imageai/config/custom_configuration.yaml b/harmoni_detectors/harmoni_imageai/config/custom_configuration.yaml new file mode 100755 index 00000000..19df356c --- /dev/null +++ b/harmoni_detectors/harmoni_imageai/config/custom_configuration.yaml @@ -0,0 +1,11 @@ +imageai_custom_yolo: + default_param: #param for detection with custom model + model_name: "yolo_custom_card.h5" #model has to be pretrained with "pretrained_YOLOv3" + json_name: "detection_config_card.json" + subscriber_id: "default" + frame_per_second: 30 + frame_detection_interval: 5 #the object detection will be updated after 5 frames (the number has to be between 5-20) + output_file_name: "temp_custom_output" + log_progress: True + minimum_percentage_probability: 95 #minimum percentage for recognizing objects + return_detected_frame: False #if true, allows you to obtain the detected video frame as a Numpy array diff --git a/harmoni_detectors/harmoni_imageai/config/yolo_configuration.yaml b/harmoni_detectors/harmoni_imageai/config/yolo_configuration.yaml new file mode 100755 index 00000000..f9454e7d --- /dev/null +++ b/harmoni_detectors/harmoni_imageai/config/yolo_configuration.yaml @@ -0,0 +1,8 @@ +imageai_yolo: + default_param: #param for detection with plane YOLOv3 + subscriber_id: "default" + frame_per_second: 30 + output_file_name: "temp_output" + log_progress: True + minimum_percentage_probability: 50 #minimum percentage for recognizing objects + return_detected_frame: False #if true, allows you to obtain the detected video frame as a Numpy array diff --git a/harmoni_detectors/harmoni_imageai/launch/custom_service.launch b/harmoni_detectors/harmoni_imageai/launch/custom_service.launch new file mode 100644 index 00000000..65fbfcc7 --- /dev/null +++ b/harmoni_detectors/harmoni_imageai/launch/custom_service.launch @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/harmoni_detectors/harmoni_imageai/launch/yolo_service.launch b/harmoni_detectors/harmoni_imageai/launch/yolo_service.launch new file mode 100755 index 00000000..251508a9 --- /dev/null +++ b/harmoni_detectors/harmoni_imageai/launch/yolo_service.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/harmoni_detectors/harmoni_imageai/package.xml b/harmoni_detectors/harmoni_imageai/package.xml new file mode 100755 index 00000000..a3cc6e25 --- /dev/null +++ b/harmoni_detectors/harmoni_imageai/package.xml @@ -0,0 +1,66 @@ + + + harmoni_imageai + 0.0.0 + The harmoni_imageai package + + + + + chris + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + rospy + rospy + rospy + rostest + rosunit + + + + + + + + + + diff --git a/harmoni_detectors/harmoni_imageai/setup.py b/harmoni_detectors/harmoni_imageai/setup.py new file mode 100755 index 00000000..dfda27ce --- /dev/null +++ b/harmoni_detectors/harmoni_imageai/setup.py @@ -0,0 +1,11 @@ +# ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + packages=["harmoni_imageai"], package_dir={"": "src"}, +) + +setup(**setup_args) diff --git a/harmoni_detectors/harmoni_imageai/src/harmoni_imageai/__init__.py b/harmoni_detectors/harmoni_imageai/src/harmoni_imageai/__init__.py new file mode 100755 index 00000000..e69de29b diff --git a/harmoni_detectors/harmoni_imageai/src/harmoni_imageai/custom_service.py b/harmoni_detectors/harmoni_imageai/src/harmoni_imageai/custom_service.py new file mode 100755 index 00000000..f2b86996 --- /dev/null +++ b/harmoni_detectors/harmoni_imageai/src/harmoni_imageai/custom_service.py @@ -0,0 +1,229 @@ +#!/usr/bin/env python3 + +# Common Imports +import rospy +import roslib + +from harmoni_common_lib.constants import State +from harmoni_common_lib.service_server import HarmoniServiceServer +from harmoni_common_lib.service_manager import HarmoniServiceManager +import harmoni_common_lib.helper_functions as hf + +# Specific Imports +from harmoni_common_lib.constants import State, DetectorNameSpace, SensorNameSpace +from sensor_msgs.msg import Image +from cv_bridge import CvBridge +from imageai.Detection.Custom import CustomObjectDetection +from std_msgs.msg import String +import numpy as np +import os +import io +import cv2 +from six.moves import queue + +class ImageAICustomService(HarmoniServiceManager): + """ + ImageAICustom service + """ + + def __init__(self, name, param): + super().__init__(name) + """ Initialization of variables and imageai parameters """ + self.subscriber_id = param["subscriber_id"] + self.model_name = param["model_name"] + self.json_name = param["json_name"] + self.frame_per_second = param["frame_per_second"] + self.frame_detection_interval = param["frame_detection_interval"] + self.output_file_name = param["output_file_name"] + self.minimum_percentage_probability = param["minimum_percentage_probability"] + self.return_detected_frame = param["return_detected_frame"] + + self.model_path = "/root/harmoni_catkin_ws/src/HARMONI/harmoni_detectors/harmoni_imageai/src/" + self.service_id = hf.get_child_id(self.name) + self.result_msg = "" + self.state = None + + self.detector = CustomObjectDetection() + self.detector.setModelTypeAsYOLOv3() + self.detector.setJsonPath("/root/harmoni_catkin_ws/src/HARMONI/harmoni_detectors/harmoni_imageai/src/detection_config_card.json") + self.detector.setModelPath(os.path.join(self.model_path, self.model_name)) + self.detector.loadModel() + self.capture_frame = False + self.custom_objects_probability = { + "carta" : 0, + "no" : 0, + "nocapito" : 0, + "ombrello" : 0, + "plastica" : 0, + "si" : 0, + "stop" : 0, + "vetro" : 0 + } + + self.cv_bridge = CvBridge() + self._buff = queue.Queue() + self.closed = False + + self.data = b"" + + """Setup publishers and subscribers""" + rospy.Subscriber( + SensorNameSpace.camera.value + self.subscriber_id, + Image, + self.callback, + ) + + self.channel_qt = rospy.Subscriber( + "/camera/color/image_raw", + Image, + self.callback, + ) + + self.text_pub = rospy.Publisher( + DetectorNameSpace.imageai_custom_yolo.value + self.service_id, String, queue_size=10 + ) + + rospy.Subscriber( + DetectorNameSpace.imageai_custom_yolo.value + self.service_id, + String, + self.imageai_callback, + ) + + """Setup the imageai service as server """ + self.state = State.INIT + return + + def pause_back(self, data): + rospy.loginfo(f"pausing for data: {len(data.data)}") + self.pause() + rospy.sleep(int(len(data.data) / 30000)) # TODO calibrate this guess + self.state = State.START + return + + def callback(self, data): + """ Callback function subscribing to the camera topic""" + if self.state == State.REQUEST: + if self.capture_frame: + self._buff.put(data) + self.capture_frame = False + + + def imageai_callback(self, data): + """ Callback function subscribing to the camera topic""" + self.response_received = True + + + def request(self, data): + rospy.loginfo("Start the %s request" % self.name) + self.state = State.REQUEST + self.capture_frame = True + try: + for element in self.custom_objects_probability: + self.custom_objects_probability[element] = 0 + + for i in range(1, 6): + data_tmp = self.cv_bridge.imgmsg_to_cv2(self._buff.get(), desired_encoding='passthrough') + self.detections = self.detector.detectObjectsFromImage(input_type="array", + output_type="array", + input_image=data_tmp, + minimum_percentage_probability=self.minimum_percentage_probability, + extract_detected_objects=True) + self.capture_frame = True + print("RISULTATO ANAISI FOTO NUMERO: ", i) + #self.result_msg = str(self.detections[1]) + if len(self.detections[1]) != 0: + for eachObject in self.detections[1]: + self.custom_objects_probability[eachObject["name"]] = eachObject["percentage_probability"] + print(eachObject["name"] , " : " , eachObject["percentage_probability"], " : ", eachObject["box_points"] ) + print("--------------------------------") + + self.result_msg = max(self.custom_objects_probability, key=self.custom_objects_probability.get) + else: + print("YoloCustom detection --> null") + self.result_msg = "null" + + if self.result_msg != "null": + break + + print("The winner is: ") + if self.result_msg == "null": + print("No one, the child didn't show anything =(") + else: + print(self.result_msg) + self.response_received = True + self.state = State.SUCCESS + + except rospy.ServiceException: + self.state = State.FAILED + self.response_received = True + rospy.loginfo("Service call failed") + self.result_msg = "" + finally: + self._buff.queue.clear() + return {"response": self.state, "message": self.result_msg} + + def start(self, rate=""): + + #try: + rospy.loginfo("Start the %s service" % self.name) + if self.state == State.INIT: + self.state = State.START + + + else: + self.state = State.START + + #except Exception: + # rospy.loginfo("Killed the %s service" % self.name) + return + + #TODO + def stop(self): + rospy.loginfo("Stop the %s service" % self.name) + try: + # Signal the STT input data generator to terminate so that the client's + # streaming_recognize method will not block the process termination. + self.closed = True + self._buff.put(None) + + self.state = State.SUCCESS + except Exception: + self.state = State.FAILED + return + + def pause(self): + rospy.loginfo("Pause the %s service" % self.name) + self.state = State.SUCCESS + return + +def main(): + """Set names, collect params, and give service to server""" + + service_name = DetectorNameSpace.imageai_custom_yolo.name # "imageai" + instance_id = rospy.get_param("instance_id") # "default" + service_id = f"{service_name}_{instance_id}" + try: + rospy.init_node(service_name, log_level=rospy.DEBUG) + + # stt/default_param/[all your params] + params = rospy.get_param(service_name + "/" + instance_id + "_param/") + + s = ImageAICustomService(service_id, params) + + service_server = HarmoniServiceServer(name=service_id, service_manager=s) + + print(service_name) + print("**********************************************************************************************") + print(service_id) + + #s.start() + + # Streaming audio from mic + service_server.start_sending_feedback() + rospy.spin() + + except rospy.ROSInterruptException: + pass + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/harmoni_detectors/harmoni_imageai/src/harmoni_imageai/yolo_service.py b/harmoni_detectors/harmoni_imageai/src/harmoni_imageai/yolo_service.py new file mode 100755 index 00000000..a81c06f3 --- /dev/null +++ b/harmoni_detectors/harmoni_imageai/src/harmoni_imageai/yolo_service.py @@ -0,0 +1,201 @@ +#!/usr/bin/env python3 + +# Common Imports +import rospy +import roslib + +from harmoni_common_lib.constants import State +from harmoni_common_lib.service_server import HarmoniServiceServer +from harmoni_common_lib.service_manager import HarmoniServiceManager +import harmoni_common_lib.helper_functions as hf + +# Specific Imports +from harmoni_common_lib.constants import State, DetectorNameSpace, SensorNameSpace +from cv_bridge import CvBridge +from sensor_msgs.msg import Image +from imageai.Detection import ObjectDetection +from std_msgs.msg import String +import numpy as np +import os +import io +import cv2 +from six.moves import queue + +class ImageAIYoloService(HarmoniServiceManager): + """ + ImageAIYolo service + """ + + def __init__(self, name, param): + super().__init__(name) + """ Initialization of variables and imageai parameters """ + self.subscriber_id = param["subscriber_id"] + self.frame_per_second = param["frame_per_second"] + self.output_file_name = param["output_file_name"] + self.minimum_percentage_probability = param["minimum_percentage_probability"] + self.return_detected_frame = param["return_detected_frame"] + self.log_progress = param["log_progress"] + + self.model_path = "/root/harmoni_catkin_ws/src/HARMONI/harmoni_detectors/harmoni_imageai/src/" + self.temp_path = "/root/harmoni_catkin_ws/src/HARMONI/harmoni_detectors/harmoni_imageai/temp_data/" + self.service_id = hf.get_child_id(self.name) + self.result_msg = "" + self.state = None + + self.detector = ObjectDetection() + self.detector.setModelTypeAsYOLOv3() + self.detector.setModelPath(os.path.join(self.model_path, "yolo.h5")) + self.detector.loadModel() + #custom_objects refers to the objects we want to detect + self.custom_objects = self.detector.CustomObjects(person=True) + self.capture_frame = False + + self.cv_bridge = CvBridge() + + self._buff = queue.Queue() + self.closed = False + + """Setup publishers and subscribers""" + self.channel = rospy.Subscriber( + SensorNameSpace.camera.value + self.subscriber_id, + Image, + self.callback, + ) + + self.channel_qt = rospy.Subscriber( + "/camera/color/image_raw", + Image, + self.callback, + ) + + self.text_pub = rospy.Publisher( + DetectorNameSpace.imageai_yolo.value + self.service_id, String, queue_size=10 + ) + + rospy.Subscriber( + DetectorNameSpace.imageai_yolo.value + self.service_id, + String, + self.imageai_callback, + ) + + """Setup the imageai service as server """ + self.state = State.INIT + return + + def pause_back(self, data): + rospy.loginfo(f"pausing for data: {len(data.data)}") + self.pause() + rospy.sleep(int(len(data.data) / 30000)) # TODO calibrate this guess + self.state = State.START + return + + def callback(self, data): + """ Callback function subscribing to the camera topic""" + if self.state == State.REQUEST: + if self.capture_frame: + self._buff.put(data) + self.capture_frame = False + + + def imageai_callback(self, data): + """ Callback function subscribing to the camera topic""" + self.response_received = True + + + def request(self, data): + rospy.loginfo("Start the %s request" % self.name) + self.state = State.REQUEST + self.capture_frame = True + try: + #detect objects coming from camera stream + data_tmp = self.cv_bridge.imgmsg_to_cv2(self._buff.get(), desired_encoding='passthrough') + self.detections = self.detector.detectObjectsFromImage(custom_objects=self.custom_objects, + input_type="stream", + output_type="array", + input_image=data_tmp, + minimum_percentage_probability=self.minimum_percentage_probability, + extract_detected_objects=True) + self.capture_frame = True + #self.result_msg = str(self.detections[1]) + self.result_msg = "" + if len(self.detections[1]) != 0: + for eachObject in self.detections[1]: + self.result_msg += str(eachObject["name"]) + " with: " +str(eachObject["percentage_probability"]) + " - " + print(eachObject["name"] , " : " , eachObject["percentage_probability"], " : ", eachObject["box_points"] ) + print("--------------------------------") + else: + print("Yolo detection --> null") + self.result_msg = "null" + + self.response_received = True + self.state = State.SUCCESS + + except rospy.ServiceException: + self.state = State.FAILED + self.response_received = True + rospy.loginfo("Service call failed") + self.result_msg = "" + return {"response": self.state, "message": self.result_msg} + + def start(self, rate=""): + + #try: + rospy.loginfo("Start the %s service" % self.name) + if self.state == State.INIT: + self.state = State.START + + else: + self.state = State.START + + #except Exception: + # rospy.loginfo("Killed the %s service" % self.name) + return + + #TODO + def stop(self): + rospy.loginfo("Stop the %s service" % self.name) + try: + self.closed = True + self._buff.put(None) + + self.state = State.SUCCESS + except Exception: + self.state = State.FAILED + return + + def pause(self): + rospy.loginfo("Pause the %s service" % self.name) + self.state = State.SUCCESS + return + +def main(): + """Set names, collect params, and give service to server""" + + service_name = DetectorNameSpace.imageai_yolo.name # "imageai" + instance_id = rospy.get_param("instance_id") # "default" + service_id = f"{service_name}_{instance_id}" + try: + rospy.init_node(service_name, log_level=rospy.DEBUG) + + # imageai/default_param/[all your params] + params = rospy.get_param(service_name + "/" + instance_id + "_param/") + + s = ImageAIYoloService(service_id, params) + + service_server = HarmoniServiceServer(name=service_id, service_manager=s) + + print(service_name) + print("**********************************************************************************************") + print(service_id) + + #s.start() + + # Streaming audio from mic + service_server.start_sending_feedback() + rospy.spin() + + except rospy.ROSInterruptException: + pass + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/harmoni_detectors/harmoni_imageai/test/__init__.py b/harmoni_detectors/harmoni_imageai/test/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/harmoni_detectors/harmoni_stt/config/google_configuration.yaml b/harmoni_detectors/harmoni_stt/config/google_configuration.yaml index 453daae5..fbb7ce24 100644 --- a/harmoni_detectors/harmoni_stt/config/google_configuration.yaml +++ b/harmoni_detectors/harmoni_stt/config/google_configuration.yaml @@ -1,13 +1,15 @@ stt: default_param: - language_id: "en-US" - sample_rate: 44100 #44100 for wav audio file, 16000 for wav file from mic + language_id: "it-IT" #"en-US" + sample_rate: 16000 #44100 for wav audio file, 16000 for wav file from mic audio_channel: 1 subscriber_id: "default" credential_path: "$(env HOME)/.gcp/private-keys.json" #path where private keys are mounted + max_duration: 4 ita_param: language_id: "it-IT" sample_rate: 16000 #44100 for wav audio file audio_channel: 1 subscriber_id: "default" credential_path: "$(env HOME)/.gcp/private-keys.json" #path where private keys are mounted + max_duration: 10 diff --git a/harmoni_detectors/harmoni_stt/launch/stt_google_service.launch b/harmoni_detectors/harmoni_stt/launch/stt_google_service.launch index 0ff5922b..dc12d951 100644 --- a/harmoni_detectors/harmoni_stt/launch/stt_google_service.launch +++ b/harmoni_detectors/harmoni_stt/launch/stt_google_service.launch @@ -1,6 +1,6 @@ - + diff --git a/harmoni_detectors/harmoni_stt/nodes/google_service.py b/harmoni_detectors/harmoni_stt/nodes/google_service.py deleted file mode 100755 index 5a02ee21..00000000 --- a/harmoni_detectors/harmoni_stt/nodes/google_service.py +++ /dev/null @@ -1,217 +0,0 @@ -#!/usr/bin/env python3 - -# Common Imports -import rospy -import roslib - -from harmoni_common_lib.constants import State -from harmoni_common_lib.service_server import HarmoniServiceServer -from harmoni_common_lib.service_manager import HarmoniServiceManager -import harmoni_common_lib.helper_functions as hf - -# Specific Imports -from harmoni_common_lib.constants import State, DetectorNameSpace, SensorNameSpace -from audio_common_msgs.msg import AudioData -from google.cloud import speech -from std_msgs.msg import String -import numpy as np -import os -import io - - -class SpeechToTextService(HarmoniServiceManager): - """ - Speech to text service using Google service - """ - - def __init__(self, name, param): - """ Initialization of variables and google parameters """ - super().__init__(name) - self.sample_rate = param["sample_rate"] - self.language = param["language_id"] - self.audio_channel = param["audio_channel"] - self.credential_path = param["credential_path"] - self.subscriber_id = param["subscriber_id"] - - self.service_id = hf.get_child_id(self.name) - - """ Setup the google request """ - self.setup_google() - - """Setup the google service as server """ - self.response_text = "" - self.data = b"" - - """Setup publishers and subscribers""" - rospy.Subscriber("/audio/audio", AudioData, self.playing_sound_pause_callback) - rospy.Subscriber( - SensorNameSpace.microphone.value + self.subscriber_id, - AudioData, - self.sound_data_callback, - ) - self.text_pub = rospy.Publisher( - DetectorNameSpace.stt.value + self.service_id, String, queue_size=10 - ) - - self.state = State.INIT - return - - def start(self, rate=""): - rospy.loginfo("Start the %s service" % self.name) - if self.state == State.INIT: - self.state = State.START - # self.transcribe_stream() # Start the microphone service at the INIT - else: - self.state = State.START - return - - def stop(self): - rospy.loginfo("Stop the %s service" % self.name) - try: - # self.close_stream() - self.state = State.SUCCESS - except Exception: - self.state = State.FAILED - return - - def pause(self): - rospy.loginfo("Pause the %s service" % self.name) - self.state = State.SUCCESS - return - - def setup_google(self): - """Setup google client for speech recognition""" - os.environ["GOOGLE_APPLICATION_CREDENTIALS"] = self.credential_path - self.client = speech.SpeechClient() - encoding = speech.RecognitionConfig.AudioEncoding.LINEAR16 - self.config = speech.RecognitionConfig( - encoding=speech.RecognitionConfig.AudioEncoding.LINEAR16, - sample_rate_hertz=self.sample_rate, - language_code=self.language, - audio_channel_count=self.audio_channel, - ) - config = speech.RecognitionConfig( - encoding=speech.RecognitionConfig.AudioEncoding.LINEAR16, - sample_rate_hertz=self.sample_rate, - language_code=self.language, - ) - self.streaming_config = speech.StreamingRecognitionConfig( - config=config, interim_results=True - ) - return - - def sound_data_callback(self, data): - """ Callback function subscribing to the microphone topic""" - data = np.fromstring(data.data, np.uint8) - # self.data = self.data.join(data) - # self.data = data.data - if self.state == State.START: - self.transcribe_stream_request(self.data) - else: - rospy.loginfo(f"Not Transcribing data because state is {self.state}") - - def transcribe_stream_request(self, data): - # TODO: streaming transcription https://github.com/googleapis/python-speech/blob/master/samples/microphone/transcribe_streaming_infinite.py - stream = data - rospy.loginfo("Transcribing Stream") - requests = ( - speech.StreamingRecognizeRequest(audio_content=chunk) for chunk in stream - ) - responses = self.client.streaming_recognize( - config=self.streaming_config, requests=requests - ) - rospy.loginfo(f"Responses: {responses}") - for response in responses: - rospy.loginfo(f"Response items: {response}") - # Once the transcription has settled, the first result will contain the - # is_final result. The other results will be for subsequent portions of - # the audio. - for result in response.results: - print("Finished: {}".format(result.is_final)) - print("Stability: {}".format(result.stability)) - alternatives = result.alternatives - # The alternatives are ordered from most likely to least. - for alternative in alternatives: - print("Confidence: {}".format(alternative.confidence)) - print("Transcript: {}".format(alternative.transcript)) - return - - def transcribe_file_request(self, data): - rate = "" # TODO: TBD - audio = {"content": data} - try: - rospy.loginfo("Request to google") - operation = self.client.long_running_recognize( - config=self.config, audio=audio - ) - rospy.loginfo("Waiting for the operation to complete.") - self.state = State.PAUSE - response = operation.result() - for result in response.results: - self.data = b"" - alternative = result.alternatives[0] - text = alternative.transcript - rospy.loginfo("The response is %s" % (text)) - print(self.response_text) - if text: - self.response_text = self.response_text + " " + text - else: - if self.response_text: - rospy.loginfo("Heard:" + self.response_text) - self.text_pub.publish(self.response_text[1:]) - self.response_text = "" - self.state = State.START - except rospy.ServiceException: - self.start = State.FAILED - rospy.loginfo("Service call failed") - self.response_received = True - self.result_msg = "" - return - - def request(self, input_data): - self.data = self.data.join(input_data) - rospy.loginfo("Start the %s request" % self.name) - self.state = State.START - self.transcribe_stream_request(self.data) - return - - def playing_sound_pause_callback(self, data): - """Sleeps when data is being published to the speaker""" - rospy.loginfo(f"pausing for data: {len(data.data)}") - self.pause() - rospy.sleep(int(len(data.data) / 22040)) - self.start() - return - - -def wav_to_data(path): - with io.open(path, "rb") as f: - content = f.read() - return content - - -def main(): - """Set names, collect params, and give service to server""" - - service_name = DetectorNameSpace.stt.name # "stt" - instance_id = rospy.get_param("instance_id") # "default" - service_id = f"{service_name}_{instance_id}" - - try: - rospy.init_node(service_name, log_level=rospy.DEBUG) - - # stt/default_param/[all your params] - params = rospy.get_param(service_name + "/" + instance_id + "_param/") - - s = SpeechToTextService(service_id, params) - - service_server = HarmoniServiceServer(service_id, s) - - service_server.start_sending_feedback() - rospy.spin() - except rospy.ROSInterruptException: - pass - - -if __name__ == "__main__": - main() diff --git a/harmoni_detectors/harmoni_stt/src/harmoni_stt/google_service.py b/harmoni_detectors/harmoni_stt/src/harmoni_stt/google_service.py new file mode 100755 index 00000000..201c36ce --- /dev/null +++ b/harmoni_detectors/harmoni_stt/src/harmoni_stt/google_service.py @@ -0,0 +1,361 @@ +#!/usr/bin/env python3 + +# Common Imports +import rospy +import roslib + +from harmoni_common_lib.constants import State +from harmoni_common_lib.service_server import HarmoniServiceServer +from harmoni_common_lib.service_manager import HarmoniServiceManager +import harmoni_common_lib.helper_functions as hf + +# Specific Imports +from harmoni_common_lib.constants import State, DetectorNameSpace, SensorNameSpace +from audio_common_msgs.msg import AudioData +from google.cloud import speech +from std_msgs.msg import String +import numpy as np +import os +import io +import time +from six.moves import queue + +class STTGoogleService(HarmoniServiceManager): + """ + Google service + """ + + def __init__(self, name, param): + super().__init__(name) + """ Initialization of variables and google parameters """ + self.sample_rate = param["sample_rate"] + self.language = param["language_id"] + self.audio_channel = param["audio_channel"] + self.credential_path = param["credential_path"] + self.subscriber_id = param["subscriber_id"] + self.max_duration = param["max_duration"] + self.start_time = None + self.elapsed_time = None + self.service_id = hf.get_child_id(self.name) + self.result_msg = "" + self.stt_response = "" + + self._buff = queue.Queue() + self.closed = False + + """ Setup the google request """ + self.setup_google() + + """Setup the google service as server """ + self.response_text = "" + self.data = b"" + + """Setup publishers and subscribers""" + rospy.Subscriber( + SensorNameSpace.microphone.value + self.subscriber_id, + AudioData, + self.callback, + ) + rospy.Subscriber("/audio/audio", AudioData, None) + + self.text_pub = rospy.Publisher( + DetectorNameSpace.stt.value + self.service_id, String, queue_size=10 + ) + + rospy.Subscriber( + DetectorNameSpace.stt.value + self.service_id, + String, + self.stt_callback, + ) + + """Setup the stt service as server """ + self.state = State.INIT + return + + def pause_back(self, data): + rospy.loginfo(f"pausing for data: {len(data.data)}") + self.pause() + rospy.sleep(int(len(data.data) / 30000)) # TODO calibrate this guess + self.state = State.START + return + + def setup_google(self): + os.environ["GOOGLE_APPLICATION_CREDENTIALS"] = self.credential_path + self.client = speech.SpeechClient() + self.config = speech.RecognitionConfig( + encoding=speech.RecognitionConfig.AudioEncoding.LINEAR16, + sample_rate_hertz=self.sample_rate, + language_code=self.language, + max_alternatives=1, + ) + self.streaming_config = speech.StreamingRecognitionConfig( + config=self.config, interim_results=True + ) + rospy.loginfo("SETUP FINISHED") + return + + def callback(self, data): + """ Callback function subscribing to the microphone topic""" + #rospy.loginfo("Add data to buffer") + if self.state == State.REQUEST: + self._buff.put(data.data) + # rospy.loginfo("Items in buffer: "+ str(self._buff.qsize())) + + # else: + # rospy.loginfo("Not Transcribing data") + + def listen_print_loop(self,responses): + """ Prints responses coming from Google STT """ + self.stt_response = "" + + for response in responses: + if not response.results: + continue + + rospy.loginfo(f"Response: {response}") + + # The `results` list is consecutive. For streaming, we only care about + # the first result being considered, since once it's `is_final`, it + # moves on to considering the next utterance. + result = response.results[0] + if not result.alternatives: + continue + + # Display the transcription of the top alternative. + transcript = result.alternatives[0].transcript + # self.text_pub.publish(transcript) + # self.response_received = True + + for result in response.results: + if result.is_final: + # rospy.loginfo(result.alternatives[0].transcript) + rospy.loginfo("STT response text: "+ result.alternatives[0].transcript) + self.stt_response = result.alternatives[0].transcript + self.text_pub.publish(self.stt_response) + self.response_received = True + + def listen_print_untill_result_is_final(self,responses): + """ Prints responses coming from Google STT """ + self.stt_response = "" + + self.start_time = time.time() + + for response in responses: + if not response.results: + continue + #rospy.loginfo(f"Response: {response}") + self.start_time = time.time() + + result = response.results[0] + if not result.alternatives: + continue + + #transcript = result.alternatives[0].transcript + for result in response.results: + if result.is_final: + # rospy.loginfo(result.alternatives[0].transcript) + rospy.loginfo("STT response text: "+ result.alternatives[0].transcript) + self.stt_response = result.alternatives[0].transcript + self.text_pub.publish(self.stt_response) + self.response_received = True + return + + def transcribe_file_request(self, data): + """ Transcribes a single audio file """ + + rate = "" # TODO: TBD + audio = {"content": data} + try: + rospy.loginfo("Request to google") + operation = self.client.long_running_recognize( + config=self.config, audio=audio + ) + rospy.loginfo("Waiting for the operation to complete.") + self.state = State.PAUSE + response = operation.result() + for result in response.results: + self.data = b"" + alternative = result.alternatives[0] + text = alternative.transcript + rospy.loginfo("The response is %s" % (text)) + print(self.response_text) + if text: + self.response_text = self.response_text + " " + text + else: + if self.response_text: + rospy.loginfo("Heard:" + self.response_text) + self.text_pub.publish(self.response_text[1:]) + self.response_text = "" + self.state = State.START + except rospy.ServiceException: + self.start = State.FAILED + rospy.loginfo("Service call failed") + self.response_received = True + self.result_msg = "" + return + + def stt_callback(self, data): + """ Callback function subscribing to the microphone topic""" + self.response_received = True + + + def request(self, data): + rospy.loginfo("Start the %s request" % self.name) + self.state = State.REQUEST + self.stt_response = "null" + self.response_received = False + try: + # Transcribes data coming from microphone + + audio_generator = self.generator() + self.requests = ( + speech.StreamingRecognizeRequest(audio_content=content) + for content in audio_generator + ) + + self.start_time = time.time() #time.time() is the current time + print("Timer started at: ", self.start_time) + + responses = self.client.streaming_recognize(self.streaming_config, self.requests) + + if self.max_duration < self.elapsed_time: + print("Timeout!") + print("STT response text: "+ self.stt_response) + self.response_received = True + self.state = State.SUCCESS + else: + self.listen_print_untill_result_is_final(responses) + + r = rospy.Rate(1) + while not self.response_received: + r.sleep() + + self.state = State.SUCCESS + self.result_msg = self.stt_response + self.response_received = True + + except rospy.ServiceException: + self.start = State.FAILED + rospy.loginfo("Service call failed") + self.response_received = True + self.result_msg = "" + finally: + self._buff.queue.clear() + return {"response": self.state, "message": self.result_msg} + + def wav_to_data(self, path): + with io.open(path, "rb") as f: + content = f.read() + return content + + def generator(self): + """ Generator of data for Google STT """ + # From https://cloud.google.com/speech-to-text/docs/streaming-recognize + + while not self.closed: + self.elapsed_time = time.time() - self.start_time + print("elapsed: ",self.elapsed_time) + + # Use a blocking get() to ensure there's at least one chunk of + # data, and stop iteration if the chunk is None, indicating the + # end of the audio stream. + chunk = self._buff.get() + + if chunk is None: + print("chunk is None") + return + data = [chunk] + + if self.max_duration < self.elapsed_time: + print("end") + return + + #TODO prova senza questo while + # Now consume whatever other data's still buffered. + + while True: + try: + chunk = self._buff.get(block=False) + if chunk is None: + print("chunk is None") + return + data.append(chunk) + except queue.Empty: + break + + yield b"".join(data) + + + def start(self, rate=""): + try: + rospy.loginfo("Start the %s service" % self.name) + if self.state == State.INIT: + self.state = State.START + + # Transcribes data coming from microphone + """ + audio_generator = self.generator() + self.requests = ( + speech.StreamingRecognizeRequest(audio_content=content) + for content in audio_generator + ) + responses = self.client.streaming_recognize(self.streaming_config, self.requests) + self.listen_print_loop(responses) + """ + + else: + self.state = State.START + + except Exception: + rospy.loginfo("Killed the %s service" % self.name) + return + + def stop(self): + rospy.loginfo("Stop the %s service" % self.name) + try: + # Signal the STT input data generator to terminate so that the client's + # streaming_recognize method will not block the process termination. + self.closed = True + self._buff.put(None) + + self.state = State.SUCCESS + except Exception: + self.state = State.FAILED + return + + def pause(self): + rospy.loginfo("Pause the %s service" % self.name) + self.state = State.SUCCESS + return + +def main(): + """Set names, collect params, and give service to server""" + + service_name = DetectorNameSpace.stt.name # "stt" + instance_id = rospy.get_param("instance_id") # "default" + service_id = f"{service_name}_{instance_id}" + try: + rospy.init_node(service_name, log_level=rospy.DEBUG) + + # stt/default_param/[all your params] + params = rospy.get_param(service_name + "/" + instance_id + "_param/") + + s = STTGoogleService(service_id, params) + + service_server = HarmoniServiceServer(name=service_id, service_manager=s) + + print(service_name) + print("**********************************************************************************************") + print(service_id) + + #s.start() + + # Streaming audio from mic + service_server.start_sending_feedback() + rospy.spin() + + except rospy.ROSInterruptException: + pass + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/harmoni_dialogues/harmoni_bot/CMakeLists.txt b/harmoni_dialogues/harmoni_bot/CMakeLists.txt old mode 100644 new mode 100755 diff --git a/harmoni_dialogues/harmoni_bot/README.md b/harmoni_dialogues/harmoni_bot/README.md old mode 100644 new mode 100755 diff --git a/harmoni_dialogues/harmoni_bot/config/configuration.yaml b/harmoni_dialogues/harmoni_bot/config/configuration.yaml old mode 100644 new mode 100755 index 2c31bd18..c02a554d --- a/harmoni_dialogues/harmoni_bot/config/configuration.yaml +++ b/harmoni_dialogues/harmoni_bot/config/configuration.yaml @@ -6,10 +6,10 @@ bot: bot_alias: "qt_robot_test" region_name: "eu-west-2" default_param: - user_id: "chris_testing" - bot_name: "GreeterPrototype" - bot_alias: "test_bot" - region_name: "us-west-2" + user_id: "corradopacelli" + bot_name: "ActivityBot" + bot_alias: "bot" + region_name: "eu-west-2" google: default_param: diff --git a/harmoni_dialogues/harmoni_bot/launch/bot_service.launch b/harmoni_dialogues/harmoni_bot/launch/bot_service.launch old mode 100644 new mode 100755 index 5ea5c4de..ff3d2599 --- a/harmoni_dialogues/harmoni_bot/launch/bot_service.launch +++ b/harmoni_dialogues/harmoni_bot/launch/bot_service.launch @@ -1,13 +1,12 @@ - - - - + + + diff --git a/harmoni_dialogues/harmoni_bot/package.xml b/harmoni_dialogues/harmoni_bot/package.xml old mode 100644 new mode 100755 diff --git a/harmoni_dialogues/harmoni_bot/rosdoc.yaml b/harmoni_dialogues/harmoni_bot/rosdoc.yaml old mode 100644 new mode 100755 diff --git a/harmoni_dialogues/harmoni_bot/setup.py b/harmoni_dialogues/harmoni_bot/setup.py old mode 100644 new mode 100755 diff --git a/harmoni_dialogues/harmoni_bot/src/harmoni_bot/__init__.py b/harmoni_dialogues/harmoni_bot/src/harmoni_bot/__init__.py old mode 100644 new mode 100755 diff --git a/harmoni_dialogues/harmoni_bot/nodes/aws_lex_service.py b/harmoni_dialogues/harmoni_bot/src/harmoni_bot/aws_lex_service.py similarity index 70% rename from harmoni_dialogues/harmoni_bot/nodes/aws_lex_service.py rename to harmoni_dialogues/harmoni_bot/src/harmoni_bot/aws_lex_service.py index 345778e7..e372774e 100755 --- a/harmoni_dialogues/harmoni_bot/nodes/aws_lex_service.py +++ b/harmoni_dialogues/harmoni_bot/src/harmoni_bot/aws_lex_service.py @@ -12,7 +12,7 @@ # Specific Imports from harmoni_common_lib.constants import DialogueNameSpace import boto3 - +import json class AWSLexService(HarmoniServiceManager): """This is a class representation of a harmoni_dialogue service @@ -33,13 +33,15 @@ def __init__(self, name, param): self.bot_name = param["bot_name"] self.bot_alias = param["bot_alias"] self.region_name = param["region_name"] - self.setup_aws_lex() + self.intentName = None self.state = State.INIT return def setup_aws_lex(self): """[summary] Setup the lex request, connecting to AWS services""" + rospy.loginfo("Connecting to Lex") self.lex_client = boto3.client("lex-runtime", region_name=self.region_name) + rospy.loginfo("Connected") return def request(self, input_text): @@ -67,29 +69,37 @@ def request(self, input_text): inputStream=textdata, ) rospy.loginfo(f"The lex response is {lex_response}") - if lex_response["ResponseMetadata"]["HTTPStatusCode"] == 200: - self.state = State.SUCCESS - if "intentName" in lex_response: - if lex_response["dialogState"] == "Fulfilled": - rospy.loginfo( - "The dialogue is fulfilled, end the conversation." - ) - rospy.loginfo("The response is %s" % (lex_response["message"])) - self.response_received = True - self.result_msg = lex_response["message"] + #if lex_response["ResponseMetadata"]["HTTPStatusCode"] == 200: + + if "intentName" in lex_response: + if lex_response["dialogState"] == "Fulfilled": + rospy.loginfo( + "The dialogue is fulfilled, end the conversation." + ) + self.response_received = True + #FIXME + #vorrei scrivere questo + lex_response.pop("audioStream") + if lex_response.get("intentName") is not None: + self.intentName = lex_response["intentName"] else: - self.start = State.FAILED - rospy.loginfo("Service call failed") - self.response_received = True - self.result_msg = "" + lex_response["intentName"] = self.intentName + self.result_msg = str(lex_response) + #ma scrivo questo + #self.result_msg = str(lex_response["message"]+"-"+lex_response["dialogState"]+"-"+lex_response["intentName"]) + self.state = State.SUCCESS + #else: + # self.start = State.FAILED + # rospy.loginfo("Service call failed") + # self.response_received = True + # self.result_msg = "" except rospy.ServiceException: - self.start = State.FAILED + self.state = State.FAILED rospy.loginfo("Service call failed") self.response_received = True self.result_msg = "" return {"response": self.state, "message": self.result_msg} - def main(): """[summary] Main function for starting HarmoniLex service @@ -101,7 +111,13 @@ def main(): rospy.init_node(service_name, log_level=rospy.DEBUG) params = rospy.get_param(service_name + "/" + instance_id + "_param/") s = AWSLexService(service_id, params) + s.setup_aws_lex() service_server = HarmoniServiceServer(service_id, s) + + print(service_name) + print("**********************************************************************************************") + print(service_id) + service_server.start_sending_feedback() rospy.spin() except rospy.ROSInterruptException: @@ -109,4 +125,4 @@ def main(): if __name__ == "__main__": - main() + main() \ No newline at end of file diff --git a/harmoni_sensors/harmoni_camera/config/configuration.yaml b/harmoni_sensors/harmoni_camera/config/configuration.yaml index dd616043..230740b1 100755 --- a/harmoni_sensors/harmoni_camera/config/configuration.yaml +++ b/harmoni_sensors/harmoni_camera/config/configuration.yaml @@ -6,3 +6,9 @@ camera: video_format: "bgr8" fps: 30 test_outdir: "$(find harmoni_camera)/temp_data/test_example.png" + person_param: + input_device_index: 0 + show: True + video_format: "bgr8" + fps: 30 + test_outdir: "$(find harmoni_camera)/temp_data/test_example.png" \ No newline at end of file diff --git a/harmoni_sensors/harmoni_camera/nodes/camera_service.py b/harmoni_sensors/harmoni_camera/nodes/camera_service.py index 0df0272b..e0f1a04b 100755 --- a/harmoni_sensors/harmoni_camera/nodes/camera_service.py +++ b/harmoni_sensors/harmoni_camera/nodes/camera_service.py @@ -150,6 +150,13 @@ def main(): service_server = HarmoniServiceServer(service_id, s) + print(service_name) + print("**********************************************************************************************") + print(service_id) + + #TODO: comment it out and create a test for ImageAI + s.start() + service_server.start_sending_feedback() rospy.spin() except rospy.ROSInterruptException: diff --git a/harmoni_sensors/harmoni_camera/temp_data/__init__.py b/harmoni_sensors/harmoni_camera/temp_data/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/harmoni_sensors/harmoni_camera/temp_data/test_example.png b/harmoni_sensors/harmoni_camera/temp_data/test_example.png deleted file mode 100644 index 78c5edc9..00000000 Binary files a/harmoni_sensors/harmoni_camera/temp_data/test_example.png and /dev/null differ diff --git a/harmoni_sensors/harmoni_microphone/CMakeLists.txt b/harmoni_sensors/harmoni_microphone/CMakeLists.txt index 102f9241..0c57d0cd 100755 --- a/harmoni_sensors/harmoni_microphone/CMakeLists.txt +++ b/harmoni_sensors/harmoni_microphone/CMakeLists.txt @@ -20,7 +20,7 @@ find_package(catkin REQUIRED COMPONENTS ## Uncomment this if the package has a setup.py. This macro ensures ## modules and global scripts declared therein get installed ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -#catkin_python_setup() +catkin_python_setup() ################################################ ## Declare ROS messages, services and actions ## diff --git a/harmoni_sensors/harmoni_microphone/config/configuration.yaml b/harmoni_sensors/harmoni_microphone/config/configuration.yaml index f09160d7..5e9116fe 100755 --- a/harmoni_sensors/harmoni_microphone/config/configuration.yaml +++ b/harmoni_sensors/harmoni_microphone/config/configuration.yaml @@ -2,7 +2,7 @@ microphone: default_param: audio_format_width: 2 - chunk_size: 1024 + chunk_size: 1600 total_channels: 1 audio_rate: 16000 device_name: default diff --git a/harmoni_sensors/harmoni_microphone/setup.py b/harmoni_sensors/harmoni_microphone/setup.py index 636136d8..2be993f2 100644 --- a/harmoni_sensors/harmoni_microphone/setup.py +++ b/harmoni_sensors/harmoni_microphone/setup.py @@ -6,8 +6,8 @@ # fetch values from package.xml setup_args = generate_distutils_setup( # scripts=[''], - # packages=['harmoni_microphone'], - # package_dir={'': 'src'}, + packages=['harmoni_microphone'], + package_dir={'': 'src'}, ) setup(**setup_args) diff --git a/harmoni_sensors/harmoni_microphone/src/harmoni_microphone/__init__.py b/harmoni_sensors/harmoni_microphone/src/harmoni_microphone/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/harmoni_sensors/harmoni_microphone/nodes/microphone_service.py b/harmoni_sensors/harmoni_microphone/src/harmoni_microphone/microphone_service.py similarity index 94% rename from harmoni_sensors/harmoni_microphone/nodes/microphone_service.py rename to harmoni_sensors/harmoni_microphone/src/harmoni_microphone/microphone_service.py index f26c1093..dfd5b0cc 100755 --- a/harmoni_sensors/harmoni_microphone/nodes/microphone_service.py +++ b/harmoni_sensors/harmoni_microphone/src/harmoni_microphone/microphone_service.py @@ -72,9 +72,12 @@ def start(self): self.state = State.START self._open_stream() # TODO handle possible exception from this self._read_stream_and_publish() + self.results_msg = "" + self.state = State.SUCCESS elif self.state == State.PAUSE: self.state = State.START self._read_stream_and_publish() + self.state = State.SUCCESS else: rospy.loginfo("Trying to start stream when already started") self.state = State.START @@ -136,7 +139,7 @@ def _read_stream_and_publish(self): latest_audio_data = self.stream.read( self.chunk_size, exception_on_overflow=False ) - raw_audio_bitstream = np.fromstring(latest_audio_data, np.uint8) + raw_audio_bitstream = np.frombuffer(latest_audio_data, np.uint8) raw_audio = raw_audio_bitstream.tolist() self.raw_mic_pub.publish(raw_audio) # Publishing raw AudioData elif ( @@ -155,6 +158,7 @@ def _get_device_index(self): Find the input audio devices configured in ~/.asoundrc. If the device is not found, pyaudio will use your machine default device """ + self.input_device_index = 0 for i in range(self.p.get_device_count()): device = self.p.get_device_info_by_index(i) # rospy.loginfo(device) @@ -184,6 +188,7 @@ def _record_audio_data_callback(self, data): """Callback function to write data""" data = np.fromstring(data.data, np.uint8) if self.first_audio_frame: + rospy.loginfo("Start recording") self.wf = wave.open(self.file_path, "wb") self.wf.setnchannels(self.total_channels) self.wf.setsampwidth(self.p.get_sample_size(self.audio_format)) @@ -213,6 +218,12 @@ def main(): service_server = HarmoniServiceServer(service_id, s) + print(service_name) + print("**********************************************************************************************") + print(service_id) + + s.start() + service_server.start_sending_feedback() rospy.spin() except rospy.ROSInterruptException: diff --git a/harmoni_sensors/harmoni_microphone/temp_data/__init__.py b/harmoni_sensors/harmoni_microphone/temp_data/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/harmoni_sensors/harmoni_microphone/test/rostest_microphone.py b/harmoni_sensors/harmoni_microphone/test/rostest_microphone.py index ea622767..cf1d3edd 100755 --- a/harmoni_sensors/harmoni_microphone/test/rostest_microphone.py +++ b/harmoni_sensors/harmoni_microphone/test/rostest_microphone.py @@ -96,7 +96,8 @@ def test_recording(self): # TODO use microphone service recording functionality # either through an import ant test or may add optional data # which specifies recording - pass + raise NotImplementedError() + def main(): diff --git a/requirements.txt b/requirements.txt index 26b5ae0b..3c89bd60 100644 --- a/requirements.txt +++ b/requirements.txt @@ -11,7 +11,7 @@ catkin_pkg catkin_tools boto3 awscli -google-cloud-speech==1.3.2 +google-cloud-speech dialogflow google-api-python-client pandas @@ -21,3 +21,5 @@ schedule func_timeout datetimerange wget +pytrees +tensorflow==2.4.0 \ No newline at end of file diff --git a/run_qt_nuc.sh b/run_qt_nuc.sh new file mode 100644 index 00000000..fee72883 --- /dev/null +++ b/run_qt_nuc.sh @@ -0,0 +1 @@ +#PROVA