diff --git a/tasks/qualification/CMakeLists.txt b/tasks/qualification/CMakeLists.txt
index 68162bb7b..bcc8c8d18 100644
--- a/tasks/qualification/CMakeLists.txt
+++ b/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
diff --git a/tasks/qualification/action/HandoverObject.action b/tasks/qualification/action/HandoverObject.action
new file mode 100644
index 000000000..a49ba4844
--- /dev/null
+++ b/tasks/qualification/action/HandoverObject.action
@@ -0,0 +1,2 @@
+---
+---
\ No newline at end of file
diff --git a/tasks/qualification/config/motions.yaml b/tasks/qualification/config/motions.yaml
index e69de29bb..f9de136e2 100644
--- a/tasks/qualification/config/motions.yaml
+++ b/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
\ No newline at end of file
diff --git a/tasks/qualification/launch/better_qualification.launch b/tasks/qualification/launch/better_qualification.launch
index bddecb380..c1def5502 100644
--- a/tasks/qualification/launch/better_qualification.launch
+++ b/tasks/qualification/launch/better_qualification.launch
@@ -19,7 +19,8 @@
-
+
+
diff --git a/tasks/qualification/nodes/actions/guide b/tasks/qualification/nodes/actions/guide
index 66f2d9468..44464c338 100644
--- a/tasks/qualification/nodes/actions/guide
+++ b/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",
diff --git a/tasks/qualification/nodes/actions/handover_object b/tasks/qualification/nodes/actions/handover_object
index 37d61d73d..e8ac22110 100644
--- a/tasks/qualification/nodes/actions/handover_object
+++ b/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())
diff --git a/tasks/qualification/nodes/actions/receive_object b/tasks/qualification/nodes/actions/receive_object
index a6eb23543..5ac0e968b 100644
--- a/tasks/qualification/nodes/actions/receive_object
+++ b/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())
diff --git a/tasks/qualification/nodes/better_qualification b/tasks/qualification/nodes/better_qualification
index 09cfb247d..1684d50b7 100644
--- a/tasks/qualification/nodes/better_qualification
+++ b/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")