From fbe248612ce9b96eb04a344a05cf10dad2993ace Mon Sep 17 00:00:00 2001 From: m-barker Date: Mon, 19 Feb 2024 22:03:59 +0000 Subject: [PATCH] chore: remove more un-usable code --- tasks/qualification/CMakeLists.txt | 50 ++-- tasks/qualification/config/motions.yaml | 17 -- .../launch/better_qualification.launch | 25 -- tasks/qualification/launch/setup.launch | 8 - tasks/qualification/launch/slow.launch | 7 - .../qualification/nodes/actions/detect_people | 64 ----- tasks/qualification/nodes/actions/find_person | 136 ----------- tasks/qualification/nodes/actions/get_command | 77 ------ .../qualification/nodes/better_qualification | 222 ------------------ 9 files changed, 25 insertions(+), 581 deletions(-) delete mode 100644 tasks/qualification/config/motions.yaml delete mode 100644 tasks/qualification/launch/better_qualification.launch delete mode 100644 tasks/qualification/launch/setup.launch delete mode 100644 tasks/qualification/launch/slow.launch delete mode 100644 tasks/qualification/nodes/actions/detect_people delete mode 100644 tasks/qualification/nodes/actions/find_person delete mode 100644 tasks/qualification/nodes/actions/get_command delete mode 100644 tasks/qualification/nodes/better_qualification diff --git a/tasks/qualification/CMakeLists.txt b/tasks/qualification/CMakeLists.txt index bcc8c8d18..f4af21f47 100644 --- a/tasks/qualification/CMakeLists.txt +++ b/tasks/qualification/CMakeLists.txt @@ -60,17 +60,17 @@ 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 HandoverObject.action -) +# 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 HandoverObject.action +# ) # Generate added messages and services with any dependencies listed here -generate_messages( - DEPENDENCIES - actionlib_msgs - geometry_msgs -) +# generate_messages( +# DEPENDENCIES +# actionlib_msgs +# geometry_msgs +# ) ################################################ ## Declare ROS dynamic reconfigure parameters ## @@ -158,22 +158,22 @@ include_directories( ## Mark executable scripts (Python etc.) for installation ## in contrast to setup.py, you can choose the destination -catkin_install_python(PROGRAMS - nodes/qualification - nodes/actions/wait_greet - nodes/actions/identify - nodes/actions/greet - nodes/actions/get_name - nodes/actions/learn_face - nodes/actions/get_command - nodes/actions/guide - nodes/actions/find_person - nodes/actions/detect_people - nodes/actions/receive_object - nodes/actions/handover_object - nodes/better_qualification - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) +# catkin_install_python(PROGRAMS +# nodes/qualification +# nodes/actions/wait_greet +# nodes/actions/identify +# nodes/actions/greet +# nodes/actions/get_name +# nodes/actions/learn_face +# nodes/actions/get_command +# nodes/actions/guide +# nodes/actions/find_person +# nodes/actions/detect_people +# nodes/actions/receive_object +# nodes/actions/handover_object +# nodes/better_qualification +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) ## Mark executables for installation ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html diff --git a/tasks/qualification/config/motions.yaml b/tasks/qualification/config/motions.yaml deleted file mode 100644 index f9de136e2..000000000 --- a/tasks/qualification/config/motions.yaml +++ /dev/null @@ -1,17 +0,0 @@ -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 deleted file mode 100644 index 1280b663a..000000000 --- a/tasks/qualification/launch/better_qualification.launch +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/tasks/qualification/launch/setup.launch b/tasks/qualification/launch/setup.launch deleted file mode 100644 index 7aa209158..000000000 --- a/tasks/qualification/launch/setup.launch +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/tasks/qualification/launch/slow.launch b/tasks/qualification/launch/slow.launch deleted file mode 100644 index 90f6946bc..000000000 --- a/tasks/qualification/launch/slow.launch +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - \ No newline at end of file diff --git a/tasks/qualification/nodes/actions/detect_people b/tasks/qualification/nodes/actions/detect_people deleted file mode 100644 index c3ae77c0a..000000000 --- a/tasks/qualification/nodes/actions/detect_people +++ /dev/null @@ -1,64 +0,0 @@ -#!/usr/bin/env python3 - -import rospy - -from qualification.msg import DetectPeopleAction, DetectPeopleResult -from actionlib import SimpleActionServer -from tf_module.srv import TfTransform, TfTransformRequest -from common_math import pcl_msg_to_cv2, seg_to_centroid -from cv_bridge3 import CvBridge -from sensor_msgs.msg import PointCloud2 -from geometry_msgs.msg import PointStamped, Point -from std_msgs.msg import String - -from lasr_vision_msgs.srv import YoloDetection -import numpy as np - - -class DetectPeople: - - def __init__(self): - - self.yolo = rospy.ServiceProxy("/yolov8/detect", YoloDetection) - self.tf = rospy.ServiceProxy("/tf_transform", TfTransform) - self.bridge = CvBridge() - - self._action_server = SimpleActionServer( - "detect_people", - DetectPeopleAction, - execute_cb=self.execute_cb, - auto_start=False, - ) - - self._action_server.start() - - def estimate_pose(self, pcl_msg, detection): - centroid_xyz = seg_to_centroid(pcl_msg, np.array(detection.xyseg)) - centroid = PointStamped() - centroid.point = Point(*centroid_xyz) - centroid.header = pcl_msg.header - tf_req = TfTransformRequest() - tf_req.target_frame = String("map") - tf_req.point = centroid - response = self.tf(tf_req) - print(response.target_point.point) - return response.target_point.point - - def execute_cb(self, _): - pcl_msg = rospy.wait_for_message("/xtion/depth_registered/points", PointCloud2) - cv_im = pcl_msg_to_cv2(pcl_msg) - img_msg = self.bridge.cv2_to_imgmsg(cv_im) - yolo_result = self.yolo(img_msg, "yolov8n-seg.pt", 0.5, 0.3) - result = DetectPeopleResult() - result.points = [ - self.estimate_pose(pcl_msg, detection) - for detection in yolo_result.detected_objects - ] - - self._action_server.set_succeeded(result) - - -if __name__ == "__main__": - rospy.init_node("detect_people") - DetectPeople() - rospy.spin() diff --git a/tasks/qualification/nodes/actions/find_person b/tasks/qualification/nodes/actions/find_person deleted file mode 100644 index 65b0427bf..000000000 --- a/tasks/qualification/nodes/actions/find_person +++ /dev/null @@ -1,136 +0,0 @@ -#!/usr/bin/env python3 - -import rospy -import actionlib - -import numpy as np -import math -from scipy.spatial.transform import Rotation as R - -from qualification.msg import ( - FindPersonAction, - FindPersonResult, - IdentifyAction, - IdentifyGoal, - DetectPeopleAction, - DetectPeopleGoal, -) -from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal -from actionlib import SimpleActionClient, SimpleActionServer -from pal_interaction_msgs.msg import TtsGoal, TtsAction -from geometry_msgs.msg import Pose, Point, Quaternion, PoseWithCovarianceStamped -from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal -from trajectory_msgs.msg import JointTrajectoryPoint - - -class FindPerson: - - KB = { - "lab": { - "pose": Pose( - position=Point(1.7, -0.72, 0.0), - orientation=Quaternion(0.0, 0.0, -0.45, 0.89), - ), - } - } - - def __init__(self): - rospy.loginfo("Waiting for action servers...") - rospy.loginfo("move_base") - self.move_base = SimpleActionClient("move_base", MoveBaseAction) - self.move_base.wait_for_server() - rospy.loginfo("head_controller") - self.head_controller = actionlib.SimpleActionClient( - "/head_controller/follow_joint_trajectory", FollowJointTrajectoryAction - ) - self.head_controller.wait_for_server() - rospy.loginfo("tts") - self.tts = SimpleActionClient("/tts", TtsAction) - self.tts.wait_for_server() - rospy.loginfo("identify") - self.identify = SimpleActionClient( - "identify", - IdentifyAction, - ) - self.identify.wait_for_server() - rospy.loginfo("detect_people") - self.detect_people = SimpleActionClient( - "detect_people", - DetectPeopleAction, - ) - self.detect_people.wait_for_server() - rospy.loginfo("find_person") - self._action_server = SimpleActionServer( - "find_person", - FindPersonAction, - execute_cb=self.execute_cb, - auto_start=False, - ) - - self._action_server.start() - rospy.loginfo("Ready") - - def execute_cb(self, goal): - tts_goal = TtsGoal() - tts_goal.rawtext.lang_id = "en_GB" - move_base_goal = MoveBaseGoal() - move_base_goal.target_pose.header.frame_id = "map" - head_goal = FollowJointTrajectoryGoal() - head_goal.trajectory.joint_names = ["head_1_joint", "head_2_joint"] - - tts_goal.rawtext.text = f"Finding {goal.name} in the {goal.location}..." - self.tts.send_goal_and_wait(tts_goal) - - move_base_goal.target_pose.header.stamp = rospy.Time.now() - move_base_goal.target_pose.pose = self.KB[goal.location]["pose"] - self.move_base.send_goal_and_wait(move_base_goal) - - joint_positions = [[0.5, 0.0], [0.0, 0.0], [-0.5, 0.00]] - - for joint_position in joint_positions: - point = JointTrajectoryPoint() - point.positions = joint_position - point.time_from_start = rospy.Duration(1) - head_goal.trajectory.points = [(point)] - self.head_controller.send_goal_and_wait(head_goal) - rospy.sleep(rospy.Duration(1.0)) - self.identify.send_goal_and_wait(IdentifyGoal()) - identify_result = self.identify.get_result() - if identify_result.name == goal.name: - self.detect_people.send_goal_and_wait(DetectPeopleGoal()) - detect_people_result = self.detect_people.get_result() - # Assume there is a single person - person_point = detect_people_result.points[0] - - point = JointTrajectoryPoint() - point.positions = [0.0, 0.0] - point.time_from_start = rospy.Duration(1) - head_goal.trajectory.points = [(point)] - self.head_controller.send_goal_and_wait(head_goal) - - orientation = Quaternion(0.0, 0.0, 0.0, 1.0) - robot_pose = rospy.wait_for_message( - "/amcl_pose", PoseWithCovarianceStamped - ).pose.pose - dist_x = person_point.x - robot_pose.position.x - dist_y = person_point.y - robot_pose.position.y - theta_deg = np.degrees(math.atan2(dist_y, dist_x)) - orientation = Quaternion( - *R.from_euler("z", theta_deg, degrees=True).as_quat() - ) - - move_base_goal.target_pose.header.stamp = rospy.Time.now() - move_base_goal.target_pose.pose = Pose( - position=Point(person_point.x - 0.3, person_point.y + 1, 0.0), - orientation=orientation, - ) - self.move_base.send_goal_and_wait(move_base_goal) - break - - self._action_server.set_succeeded(FindPersonResult()) - - -if __name__ == "__main__": - rospy.init_node("find_person") - FindPerson() - rospy.spin() diff --git a/tasks/qualification/nodes/actions/get_command b/tasks/qualification/nodes/actions/get_command deleted file mode 100644 index caa7264d1..000000000 --- a/tasks/qualification/nodes/actions/get_command +++ /dev/null @@ -1,77 +0,0 @@ -#!/usr/bin/env python3 - -import rospy -import actionlib - -from lasr_speech_recognition_msgs.msg import ( - TranscribeSpeechAction, - TranscribeSpeechGoal, -) -from qualification.msg import GetCommandAction, GetCommandResult -from pal_interaction_msgs.msg import TtsGoal, TtsAction -from actionlib import SimpleActionClient -from collections import defaultdict -import qualification.command_similarity as command_similarity -import rospkg -import os - -COMMAND_ROOT = os.path.join(rospkg.RosPack().get_path("qualification"), "data") - - -class GetCommand: - - def __init__(self): - - self.transcribe_speech = actionlib.SimpleActionClient( - "transcribe_speech", - TranscribeSpeechAction, - ) - self.transcribe_speech.wait_for_server() - rospy.loginfo("got transcribe_speech") - - self.tts = SimpleActionClient("/tts", TtsAction) - self.tts.wait_for_server() - - self._action_server = actionlib.SimpleActionServer( - "get_command", - GetCommandAction, - execute_cb=self.execute_cb, - auto_start=False, - ) - - self.issued_commands = defaultdict(list) - - self._action_server.start() - - def execute_cb(self, goal): - tts_goal = TtsGoal() - tts_goal.rawtext.lang_id = "en_GB" - - if goal.issuer in self.issued_commands.keys(): - tts_goal.rawtext.text = f"Can I do anything else for you, {goal.issuer}?" - else: - tts_goal.rawtext.text = f"{goal.issuer}, what can I do for you?" - - self.tts.send_goal_and_wait(tts_goal) - - self.transcribe_speech.send_goal(TranscribeSpeechGoal()) - self.transcribe_speech.wait_for_result() - transcribe_result = self.transcribe_speech.get_result().sequence.lower() - - result = GetCommandResult() - - if "no" not in transcribe_result.split(" "): - nearest_commands, distances = command_similarity.get_similar_commands( - transcribe_result, - os.path.join(COMMAND_ROOT, "command_index"), - os.path.join(COMMAND_ROOT, "command_list.txt"), - ) - result.command = nearest_commands[0] - self.issued_commands[goal.issuer].append(result.command) - self._action_server.set_succeeded(result) - - -if __name__ == "__main__": - rospy.init_node("get_command") - GetCommand() - rospy.spin() diff --git a/tasks/qualification/nodes/better_qualification b/tasks/qualification/nodes/better_qualification deleted file mode 100644 index 36df971bb..000000000 --- a/tasks/qualification/nodes/better_qualification +++ /dev/null @@ -1,222 +0,0 @@ -#!/usr/bin/env python3 -import rospy - -rospy.init_node("better_qualification") - -from qualification.msg import ( - WaitGreetAction, - WaitGreetGoal, - IdentifyAction, - IdentifyGoal, - GreetAction, - GreetGoal, - GetNameAction, - GetNameGoal, - LearnFaceAction, - LearnFaceGoal, - GetCommandAction, - GetCommandGoal, - GuideAction, - GuideGoal, - FindPersonAction, - FindPersonGoal, - ReceiveObjectAction, - ReceiveObjectGoal, - HandoverObjectAction, - HandoverObjectGoal, -) - -from pal_interaction_msgs.msg import TtsGoal, TtsAction -from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal -from trajectory_msgs.msg import JointTrajectoryPoint -from geometry_msgs.msg import ( - PoseStamped, - Pose, - Point, - Quaternion, - PoseWithCovarianceStamped, -) -from actionlib import SimpleActionClient -from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal -from std_msgs.msg import String - -# supress warnings -import warnings - -warnings.filterwarnings("ignore") - -location = "corridor" -name = "unknown" -person_pose = None - -print("Waiting for action servers...") -print("tts") -tts = SimpleActionClient("/tts", TtsAction) -tts.wait_for_server() -print("wait_greet") -wait_greet = SimpleActionClient("wait_greet", WaitGreetAction) -wait_greet.wait_for_server() -print("identify") -identify = SimpleActionClient("identify", IdentifyAction) -identify.wait_for_server() -print("greet") -greet = SimpleActionClient("greet", GreetAction) -greet.wait_for_server() -print("get_name") -get_name = SimpleActionClient("get_name", GetNameAction) -get_name.wait_for_server() -print("learn_face") -learn_face = SimpleActionClient("learn_face", LearnFaceAction) -learn_face.wait_for_server() -print("get_command") -get_command = SimpleActionClient("get_command", GetCommandAction) -get_command.wait_for_server() -print("guide") -guide = SimpleActionClient("guide", GuideAction) -guide.wait_for_server() -print("find_person") -find_person = SimpleActionClient("find_person", FindPersonAction) -find_person.wait_for_server() -print("torso_controller") -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") -from std_srvs.srv import Empty - -handover_object = rospy.ServiceProxy("handover_object", Empty) -# handover_object = SimpleActionClient("handover_object", HandoverObjectAction) -# handover_object.wait_for_server() -move_base = SimpleActionClient("move_base", MoveBaseAction) -move_base.wait_for_server() - -print("All action servers are ready") - -torso_raise_goal = FollowJointTrajectoryGoal() -torso_raise_goal.trajectory.joint_names = ["torso_lift_joint"] -point = JointTrajectoryPoint() -point.positions = [0.35] -point.time_from_start = rospy.Duration(1) -torso_raise_goal.trajectory.points.append(point) - - -torso_lower_goal = FollowJointTrajectoryGoal() -torso_lower_goal.trajectory.joint_names = ["torso_lift_joint"] -point = JointTrajectoryPoint() -point.positions = [0.15] -point.time_from_start = rospy.Duration(1) -torso_lower_goal.trajectory.points.append(point) - -tts_goal = TtsGoal() -tts_goal.rawtext.lang_id = "en_GB" -tts_goal.rawtext.text = "Begin" -tts.send_goal_and_wait(tts_goal) - -command_publisher = rospy.Publisher("/goal_string", String, queue_size=1) - - -def exec_command(command): - torso_controller.send_goal_and_wait(torso_lower_goal) - global name - global location - global person_pose - if ( - command == "Please guide me to the lab." - or command == "Navigate to the couch, meet Mary, and guide her" - ): - goal = GuideGoal(name, location, "lab") - guide.send_goal_and_wait(goal) - location = "lab" - elif command == "Please guide me to the kitchen.": - goal = GuideGoal(name, location, "kitchen") - guide.send_goal_and_wait(goal) - location = "kitchen" - elif command == "Go help Jared.": - name = "jared" - goal = FindPersonGoal("jared", location) - find_person.send_goal_and_wait(goal) - elif command == "Find the black tea at the desk and take it to Matt.": - if True: - command_publisher.publish(String("black-tea person matt")) - else: - goal = ReceiveObjectGoal("black-tea") - receive_object.send_goal_and_wait(goal) - - if person_pose is not None: - move_base_goal = MoveBaseGoal() - move_base_goal.target_pose.header.stamp = rospy.Time.now() - move_base_goal.target_pose.header.frame_id = "map" - move_base_goal.target_pose.pose = person_pose - move_base.send_goal_and_wait(move_base_goal) - handover_object() - - -# Wait to be greeted -wait_greet.send_goal_and_wait(WaitGreetGoal()) - -# Raise the torso -torso_controller.send_goal_and_wait(torso_raise_goal) - -# Try and identify the person -identify.send_goal_and_wait(IdentifyGoal()) -identify_result = identify.get_result() - -if identify_result.name != name or not identify_result.name: - # Greet the person - greet.send_goal_and_wait(GreetGoal(name=identify_result.name)) - -if not identify_result.success: - get_name.send_goal_and_wait(GetNameGoal()) - get_name_result = get_name.get_result() - name = get_name_result.name - learn_face.send_goal_and_wait(LearnFaceGoal(name)) -else: - name = identify_result.name - -# Get a command from the person, "guide me to the lab" -get_command.send_goal_and_wait(GetCommandGoal(name)) -command_result = get_command.get_result() -command = command_result.command -print(command) -exec_command(command) -# name = "nicole" -# Raise the torso -torso_controller.send_goal_and_wait(torso_raise_goal) -person_pose = rospy.wait_for_message("/amcl_pose", PoseWithCovarianceStamped).pose.pose -# Get a command from the person, "find Jared" -get_command.send_goal_and_wait(GetCommandGoal(name)) -command_result = get_command.get_result() -command = command_result.command -print(command) -exec_command(command) - -# Raise the torso -torso_controller.send_goal_and_wait(torso_raise_goal) - -# Get a command from the person, "take the pringles to {nicole}" -get_command.send_goal_and_wait(GetCommandGoal(name)) -command_result = get_command.get_result() -command = command_result.command -print(command) -exec_command(command) - -rospy.spin() - -# # go home, the end -# tts_goal = TtsGoal() -# tts_goal.rawtext.lang_id = "en_GB" -# tts_goal.rawtext.text = ( -# "I'll go home now, if you need any more assistance, just come let me know!" -# ) -# tts.send_goal_and_wait(tts_goal) - -# move_base_goal = MoveBaseGoal() -# move_base_goal.target_pose.header.stamp = rospy.Time.now() -# move_base_goal.target_pose.header.frame_id = "map" -# move_base_goal.target_pose.pose.position = Point(0, 0, 0) -# move_base_goal.target_pose.pose.orientation = Quaternion(0, 0, 0, 1) -# move_base.send_goal_and_wait(move_base_goal)