Merging changes receptionist to breakfast (#30)

Merge of changes from Receptionist for Serve Breakfast

ws/src/frida_hri_interfaces/CMakeLists.txt

@@ -20,12 +20,14 @@ add_message_files(
 add_service_files(
   FILES
   Speak.srv
+  GuestInfo.srv
 )
 
 ## Generate actions in the 'action' folder
 add_action_files(
   FILES
   Conversate.action
+  GuestAnalysis.action
 )
 
 ## Generate added messages and services with any dependencies listed here

ws/src/frida_hri_interfaces/action/GuestAnalysis.action

@@ -0,0 +1,9 @@
+#goal definition
+int32 guest_id 
+---
+#result definition
+int32 guest_id
+string description
+---
+#feedback
+int32 feedback

ws/src/frida_hri_interfaces/srv/GuestInfo.srv

@@ -0,0 +1,5 @@
+int32 guest_id 
+---
+bool success
+string name
+string favorite_drink

ws/src/frida_manipulation_interfaces/CMakeLists.txt

@@ -14,10 +14,11 @@ find_package(catkin REQUIRED COMPONENTS
   message_generation
 )
 
-# add_service_files(
-#   FILES
-#   EnableOctomap.srv
-# )
+add_service_files(
+  FILES
+  Gripper.srv
+#  EnableOctomap.srv
+)
 
 # add_message_files(
 #   FILES
@@ -29,12 +30,14 @@ find_package(catkin REQUIRED COMPONENTS
 add_action_files(
   FILES
   manipulationPickAndPlace.action
+  MoveJoint.action
 )
 
 generate_messages(
   DEPENDENCIES
   actionlib_msgs
   std_msgs
+  geometry_msgs
 )
 
 catkin_package(

ws/src/frida_manipulation_interfaces/action/MoveJoint.action

@@ -0,0 +1,20 @@
+#goal definition
+string predefined_position 
+int32 target_delta_x
+int32 target_delta_y
+float32[] joints_target
+geometry_msgs/Pose pose_target
+float32 speed
+float32 acceleration
+float32 position_tolerance
+float32 orientation_tolerance
+int32 planning_time
+int32 num_planning_attempts
+---
+#result definition
+int32 success
+float32 execution_time
+---
+#feedback
+string execution_state
+float32 completion_percentage

ws/src/frida_manipulation_interfaces/srv/Gripper.srv

@@ -0,0 +1,3 @@
+string state
+---
+bool success

ws/src/frida_navigation_interfaces/CMakeLists.txt

@@ -10,6 +10,7 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   rospy
   std_msgs
+  std_srvs
 )
 
 
@@ -19,6 +20,11 @@ add_action_files(
   navServ.action
 )
 
+add_service_files(
+  FILES
+  ViewAngle.srv
+)
+
 ## Generate added messages and services with any dependencies listed here
 generate_messages(
   DEPENDENCIES

ws/src/frida_navigation_interfaces/srv/ViewAngle.srv

@@ -0,0 +1,3 @@
+string text
+---
+int16 angle

ws/src/frida_task_manager/scripts/hri_tasks.py

@@ -12,9 +12,14 @@
 from std_msgs.msg import String
 from frida_hri_interfaces.msg import ConversateAction, ConversateFeedback, ConversateGoal, ConversateResult
 from frida_hri_interfaces.srv import Speak
+from frida_hri_interfaces.msg import GuestAnalysisAction, GuestAnalysisFeedback, GuestAnalysisGoal, GuestAnalysisResult
+from frida_hri_interfaces.srv import GuestInfo, GuestInfoResponse
 
 SPEAK_TOPIC = "/speech/speak"
+SPEAK_NOW_TOPIC = "/speech/speak_now"
 CONVERSATION_SERVER = "/conversation_as"
+GUEST_INFO_SERVICE = "/guest_info"
+GUEST_ANALYSIS_SERVER = "/guest_analysis_as"
 
 class TasksHRI:
     STATE_ENUM = {
@@ -30,13 +35,16 @@ class TasksHRI:
 
     def __init__(self) -> None:
         self.conversation_client = actionlib.SimpleActionClient(CONVERSATION_SERVER, ConversateAction)
-        #self.pub_speak = rospy.Publisher(SPEAK_TOPIC, String, queue_size=10)
+        self.pub_speak = rospy.Publisher(SPEAK_NOW_TOPIC, String, queue_size=10)
+
         try:
             rospy.wait_for_service(SPEAK_TOPIC, timeout=5.0)
         except rospy.ROSException:
-            rospy.logerr("Conversation service not available")
+            rospy.logerr("Speaker service not available")
         self.speak_client = rospy.ServiceProxy(SPEAK_TOPIC, Speak)
 
+        self.guest_description = ["", "", ""]
+
         rospy.loginfo("HRI Task Manager initialized")
 
     def execute_command(self, command: str, complement: str, perceived_information: str) -> int:
@@ -55,16 +63,54 @@ def execute_command(self, command: str, complement: str, perceived_information:
             #rospy.loginfo(f"Result: {result.success}")
             return result.success
         return 1
-        
+
     def cancel_command(self) -> None:
         """Method to cancel the current command"""
         self.conversation_client.cancel_all_goals()
         rospy.loginfo("Command canceled HRI")
 
-    def speak(self, text: str) -> None:
+    def get_guest_info(self, guest_id: int):
+        """Method to get the guest information
+        Returns the name and favorite drink of the guest"""
+        rospy.wait_for_service("/guest_info")
+        try:
+            guest_info = rospy.ServiceProxy(GUEST_INFO_SERVICE, GuestInfo)
+            response = guest_info(guest_id)
+            if response.success:
+                return response.name, response.favorite_drink
+            return "error", "error"
+        except rospy.ServiceException:
+            rospy.logerr("Service call failed")
+            return "error", "error"
+
+    def analyze_guest(self, guest_id: int) -> str:
+        """Method to analyze the guest
+        Returns the guest name and favorite drink"""
+        client = actionlib.SimpleActionClient(GUEST_ANALYSIS_SERVER, GuestAnalysisAction)
+        client.wait_for_server()
+
+        goal = GuestAnalysisGoal()
+        goal.guest_id = guest_id
+        client.send_goal(
+            goal,
+            done_cb=self.guest_analysis_done
+        )
+
+    def guest_analysis_done(self, status, result) -> None:
+        """Callback for the guest analysis"""
+        rospy.loginfo(f"Guest analysis result: {result.description}")
+        self.guest_description[result.guest_id] = result.description
+
+    def get_guest_description(self, guest_id: int) -> str:
+        """Method to get the guest description stored"""
+        return self.guest_description[guest_id]
+
+    def speak(self, text: str, now: bool = False) -> None:
         """Method to publish directly text to the speech node"""
-        #self.pub_speak.publish(text)
-        self.speak_client(text)
+        if now:
+            self.pub_speak.publish(text)
+        else:
+            self.speak_client(text)
 
 if __name__ == "__main__":
     try:

ws/src/frida_task_manager/scripts/manipulation_tasks.py

@@ -12,9 +12,12 @@
 from std_msgs.msg import String, Int32
 from frida_hri_interfaces.msg import Command, CommandList
 from frida_manipulation_interfaces.msg import manipulationPickAndPlaceAction, manipulationPickAndPlaceGoal, manipulationPickAndPlaceResult, manipulationPickAndPlaceFeedback
+from frida_manipulation_interfaces.msg import MoveJointAction, MoveJointFeedback, MoveJointGoal, MoveJointResult
 
 MANIPULATION_SERVER = "/manipulationServer"
+ARM_JOINTS_SERVER = "/arm_joints_as"
 PLACE_TARGET = -5
+POUR_TARGET = -10
 
 class TasksManipulation:
     """Manager for the manipulation area tasks"""
@@ -36,8 +39,12 @@ class TasksManipulation:
 
     def __init__(self) -> None:
         self.manipulation_client = actionlib.SimpleActionClient(MANIPULATION_SERVER, manipulationPickAndPlaceAction)
+        self.arm_joints_client = actionlib.SimpleActionClient(ARM_JOINTS_SERVER, MoveJointAction)
+
         if not self.manipulation_client.wait_for_server(timeout=rospy.Duration(10.0)):
             rospy.logerr("Manipulation server not initialized")
+        if not self.arm_joints_client.wait_for_server(timeout=rospy.Duration(10.0)):
+            rospy.logerr("Arm joints server not initialized")
 
         rospy.loginfo("Manipulation Task Manager initialized")
 
@@ -73,6 +80,15 @@ def manipulation_goal_feedback(feedback_msg):
         self.manipulation_client.wait_for_result()
         result = self.manipulation_client.get_result()
         return TasksManipulation.STATE["EXECUTION_SUCCESS"] if result.result else TasksManipulation.STATE["EXECUTION_ERROR"]
+
+    def move_arm_joints(self, target_x: int, target_y: int) -> int:
+        """Method to move the arm joints"""
+        self.arm_joints_client.send_goal(
+            MoveJointGoal(target_delta_x = target_x, target_delta_y = target_y)
+        )
+        self.arm_joints_client.wait_for_result()
+        result = self.arm_joints_client.get_result()
+        return TasksManipulation.STATE["EXECUTION_SUCCESS"] if result.success else TasksManipulation.STATE["EXECUTION_ERROR"]
 
     def cancel_command(self) -> None:
         """Method to cancel the current command"""