feat: ReceiveObject and HandoverObject are working.

tasks/qualification/CMakeLists.txt

@@ -62,7 +62,7 @@ catkin_generate_virtualenv(
 # Generate actions in the 'action' folder
 add_action_files(
   DIRECTORY action
-  FILES WaitGreet.action Identify.action Greet.action GetName.action LearnFace.action GetCommand.action Guide.action DetectPeople.action FindPerson.action ReceiveObject.action
+  FILES WaitGreet.action Identify.action Greet.action GetName.action LearnFace.action GetCommand.action Guide.action DetectPeople.action FindPerson.action ReceiveObject.action HandoverObject.action
 )
 
 # Generate added messages and services with any dependencies listed here

tasks/qualification/action/HandoverObject.action

@@ -0,0 +1,2 @@
+---
+---

tasks/qualification/config/motions.yaml

@@ -0,0 +1,17 @@
+play_motion:
+  motions:
+    reach_arm:
+      joints: [arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint, arm_7_joint]
+      points:
+      - positions: [1.61, -0.93, -3.14, 1.83, -1.58, -0.62, 0.0]
+        time_from_start: 0.0
+    open_gripper:
+      joints: [gripper_left_finger_joint, gripper_right_finger_joint]
+      points:
+      - positions: [0.04, 0.04]
+        time_from_start: 0.0
+    close_gripper:
+      joints: [gripper_left_finger_joint, gripper_right_finger_joint]
+      points:
+      - positions: [0.0, 0.0]
+        time_from_start: 0.0

tasks/qualification/launch/better_qualification.launch

@@ -19,7 +19,8 @@
     <node pkg="qualification" type="guide" name="guide_action" output="screen"/>
     <node pkg="qualification" type="identify" name="identify_action" output="screen"/>
     <node pkg="qualification" type="learn_face" name="learn_face_action" output="screen"/>
-
+    <node pkg="qualification" type="receive_object" name="receive_object_action" output="screen"/>
+    <node pkg="qualification" type="handover_object" name="handover_object_action" output="screen"/>
 
 
     <!-- <node pkg="qualification" type="better_qualification" name="better_qualification" output="screen"/> -->

tasks/qualification/nodes/actions/guide

@@ -54,7 +54,9 @@ class Guide:
         self.move_base.wait_for_server()
         self.tts = SimpleActionClient("/tts", TtsAction)
         self.tts.wait_for_server()
-        self.laser_dist = rospy.ServiceProxy("/unsafe_traversal/laser_dist_checker")
+        self.laser_dist = rospy.ServiceProxy(
+            "/unsafe_traversal/laser_dist_checker", LaserDist
+        )
         self.laser_dist.wait_for_service()
         self._action_server = actionlib.SimpleActionServer(
             "guide",

tasks/qualification/nodes/actions/handover_object

@@ -2,8 +2,10 @@
 import rospy
 from actionlib import SimpleActionServer, SimpleActionClient
 
-from lasr_speech_msgs.msg import TranscribeSpeechAction, TranscribeSpeechGoal
-
+from lasr_speech_recognition_msgs.msg import (
+    TranscribeSpeechAction,
+    TranscribeSpeechGoal,
+)
 from qualification.msg import HandoverObjectAction, HandoverObjectResult
 from play_motion_msgs.msg import PlayMotionAction, PlayMotionGoal
 from pal_interaction_msgs.msg import TtsGoal, TtsAction
@@ -30,6 +32,8 @@ class HandoverObject:
             auto_start=False,
         )
 
+        self._action_server.start()
+
     def execute_cb(self, goal):
         tts_goal = TtsGoal()
         tts_goal.rawtext.lang_id = "en_GB"
@@ -46,7 +50,7 @@ class HandoverObject:
                 break
         pm_goal = PlayMotionGoal(motion_name="open_gripper", skip_planning=False)
         self.play_motion.send_goal_and_wait(pm_goal)
-        pm_goal = PlayMotionGoal(motion_name="back_to_default", skip_planning=False)
+        pm_goal = PlayMotionGoal(motion_name="home", skip_planning=False)
         self.play_motion.send_goal_and_wait(pm_goal)
         self._action_server.set_succeeded(HandoverObjectResult())
 

tasks/qualification/nodes/actions/receive_object

@@ -2,8 +2,10 @@
 import rospy
 from actionlib import SimpleActionServer, SimpleActionClient
 
-from lasr_speech_msgs.msg import TranscribeSpeechAction, TranscribeSpeechGoal
-
+from lasr_speech_recognition_msgs.msg import (
+    TranscribeSpeechAction,
+    TranscribeSpeechGoal,
+)
 from qualification.msg import ReceiveObjectAction, ReceiveObjectResult
 from play_motion_msgs.msg import PlayMotionAction, PlayMotionGoal
 from pal_interaction_msgs.msg import TtsGoal, TtsAction
@@ -30,6 +32,8 @@ class ReceiveObject:
             auto_start=False,
         )
 
+        self._action_server.start()
+
     def execute_cb(self, goal):
         tts_goal = TtsGoal()
         tts_goal.rawtext.lang_id = "en_GB"
@@ -52,7 +56,7 @@ class ReceiveObject:
                 break
         pm_goal = PlayMotionGoal(motion_name="close_gripper", skip_planning=False)
         self.play_motion.send_goal_and_wait(pm_goal)
-        pm_goal = PlayMotionGoal(motion_name="back_to_default", skip_planning=False)
+        pm_goal = PlayMotionGoal(motion_name="home", skip_planning=False)
         self.play_motion.send_goal_and_wait(pm_goal)
         self._action_server.set_succeeded(ReceiveObjectResult())
 

tasks/qualification/nodes/better_qualification

@@ -20,6 +20,10 @@ from qualification.msg import (
     GuideGoal,
     FindPersonAction,
     FindPersonGoal,
+    ReceiveObjectAction,
+    ReceiveObjectGoal,
+    HandoverObjectAction,
+    HandoverObjectGoal,
 )
 
 from pal_interaction_msgs.msg import TtsGoal, TtsAction
@@ -69,6 +73,14 @@ torso_controller = SimpleActionClient(
     "torso_controller/follow_joint_trajectory", FollowJointTrajectoryAction
 )
 torso_controller.wait_for_server()
+print("receive_object")
+receive_object = SimpleActionClient("receive_object", ReceiveObjectAction)
+receive_object.wait_for_server()
+print("handover_object")
+handover_object = SimpleActionClient("handover_object", HandoverObjectAction)
+handover_object.wait_for_server()
+
+
 print("All action servers are ready")