diff --git a/common/vision/lasr_vision_deepface/CMakeLists.txt b/common/vision/lasr_vision_deepface/CMakeLists.txt index b22bda630..b63bca044 100644 --- a/common/vision/lasr_vision_deepface/CMakeLists.txt +++ b/common/vision/lasr_vision_deepface/CMakeLists.txt @@ -163,6 +163,7 @@ catkin_install_python(PROGRAMS scripts/create_dataset examples/relay examples/greet + examples/learn_face nodes/service DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/common/vision/lasr_vision_deepface/examples/learn_face b/common/vision/lasr_vision_deepface/examples/learn_face new file mode 100644 index 000000000..8bf90dd1a --- /dev/null +++ b/common/vision/lasr_vision_deepface/examples/learn_face @@ -0,0 +1,60 @@ +#!/usr/bin/env python3 + +import rospy +import actionlib +import cv2_img +import torch + +from lasr_vision_msgs.msg import ( + LearnFaceAction, + LearnFaceResult, +) +from lasr_vision_msgs.srv import ( + LearnFace as LF, + LearnFaceRequest as LFRequest, +) +from pal_interaction_msgs.msg import TtsGoal, TtsAction +from sensor_msgs.msg import Image +from actionlib import SimpleActionClient + + +class LearnFace: + + def __init__(self): + + self.learn_face = rospy.ServiceProxy("/learn_face", LF) + self.learn_face.wait_for_service() + rospy.loginfo("got learn_face") + + self.tts = SimpleActionClient("/tts", TtsAction) + self.tts.wait_for_server() + + self._action_server = actionlib.SimpleActionServer( + "learn_face", + LearnFaceAction, + execute_cb=self.execute_cb, + auto_start=False, + ) + + self._action_server.start() + + def execute_cb(self, goal): + rospy.loginfo("Learning face!") + + tts_goal = TtsGoal() + tts_goal.rawtext.lang_id = "en_GB" + tts_goal.rawtext.text = f"{goal.name}, give me a moment to learn your face" + self.tts.send_goal_and_wait(tts_goal) + req = LFRequest() + req.dataset = "learn_face_example_dataset" + req.name = goal.name + req.n_images = 10 + self.learn_face(req) + + self._action_server.set_succeeded(LearnFaceResult()) + + +if __name__ == "__main__": + rospy.init_node("learn_face") + LearnFace() + rospy.spin() diff --git a/common/vision/lasr_vision_msgs/CMakeLists.txt b/common/vision/lasr_vision_msgs/CMakeLists.txt index f5a0101e8..b46944abd 100644 --- a/common/vision/lasr_vision_msgs/CMakeLists.txt +++ b/common/vision/lasr_vision_msgs/CMakeLists.txt @@ -7,7 +7,7 @@ project(lasr_vision_msgs) ## 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 message_generation message_runtime sensor_msgs) +find_package(catkin REQUIRED COMPONENTS message_generation actionlib_msgs actionlib message_runtime sensor_msgs) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) @@ -63,16 +63,17 @@ add_service_files( LearnFace.srv ) -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) +# Generate actions in the 'action' folder +add_action_files( + DIRECTORY action + FILES + LearnFace.action +) ## Generate added messages and services with any dependencies listed here generate_messages( DEPENDENCIES + actionlib_msgs sensor_msgs ) diff --git a/common/vision/lasr_vision_msgs/action/LearnFace.action b/common/vision/lasr_vision_msgs/action/LearnFace.action new file mode 100644 index 000000000..7555f8c76 --- /dev/null +++ b/common/vision/lasr_vision_msgs/action/LearnFace.action @@ -0,0 +1,4 @@ + +string name +--- +--- \ No newline at end of file