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")