From b88d2091f37d2eecab15f15c87ac9a198ab982e5 Mon Sep 17 00:00:00 2001 From: Jared Swift Date: Sat, 6 Jul 2024 14:11:41 +0100 Subject: [PATCH 01/20] refactor: black --- skills/src/lasr_skills/adjust_camera.py | 82 +++++++++---- .../src/receptionist/state_machine.py | 2 +- .../receptionist/states/get_name_and_drink.py | 41 ++++--- .../receptionist/states/get_name_or_drink.py | 37 +++--- .../states/local_speech_recognition.py | 88 ++++++++++---- .../receptionist/states/speech_recovery.py | 114 +++++++++++++----- 6 files changed, 248 insertions(+), 116 deletions(-) diff --git a/skills/src/lasr_skills/adjust_camera.py b/skills/src/lasr_skills/adjust_camera.py index 158ea68c0..c57fef38b 100644 --- a/skills/src/lasr_skills/adjust_camera.py +++ b/skills/src/lasr_skills/adjust_camera.py @@ -108,7 +108,7 @@ def __init__( with self: smach.StateMachine.add( - 'init', + "init", PlayMotion(motion_name=init_state), transitions={ "succeeded": "GET_IMAGE", @@ -158,7 +158,7 @@ def __init__( bodypix_model=bodypix_model, bodypix_confidence=bodypix_confidence, max_attempts=max_attempts, - init_state=init_state + init_state=init_state, ), transitions=_transitions, ) @@ -176,11 +176,11 @@ def __init__( class DecideAdjustCamera(smach.State): def __init__( - self, + self, bodypix_model: str = "resnet50", bodypix_confidence: float = 0.7, max_attempts=1000, - init_state="u1m" + init_state="u1m", ): smach.State.__init__( self, @@ -239,14 +239,18 @@ def execute(self, userdata): has_more_than_one_one_eye = len(missing_keypoints.intersection(HEAD)) <= 1 rospy.logwarn(f"missing keypoints: {missing_keypoints}") - rospy.logwarn(f"missing shoulders: {missing_keypoints.intersection(MIDDLE)}, missing eyes: {missing_keypoints.intersection(HEAD)}") - - if not has_more_than_one_shoulder and not has_more_than_one_one_eye: + rospy.logwarn( + f"missing shoulders: {missing_keypoints.intersection(MIDDLE)}, missing eyes: {missing_keypoints.intersection(HEAD)}" + ) + + if not has_more_than_one_shoulder and not has_more_than_one_one_eye: # This is the case that not any centre points can be used, # In this case most likely it is the guest standing either too close or not in the camera at all. # However we may still try to get this person back into the frame if some part of them are detected. # Otherwise we say something like "Please stand in front of me but keep a bit distance.". - rospy.logwarn("The person might not actually be in the frame, trying to recover.") + rospy.logwarn( + "The person might not actually be in the frame, trying to recover." + ) miss_head = len(missing_keypoints.intersection(HEAD)) >= 2 miss_middle = len(missing_keypoints.intersection(MIDDLE)) >= 2 miss_torso = len(missing_keypoints.intersection(TORSO)) >= 4 @@ -259,18 +263,48 @@ def execute(self, userdata): needs_to_move_down = not miss_head and miss_middle and miss_torso needs_to_move_left = miss_right needs_to_move_right = miss_left - rospy.logwarn(f"Needs to move up: {needs_to_move_up}, down: {needs_to_move_down}, left: {needs_to_move_left}, right: {needs_to_move_right}.") - + rospy.logwarn( + f"Needs to move up: {needs_to_move_up}, down: {needs_to_move_down}, left: {needs_to_move_left}, right: {needs_to_move_right}." + ) + if not (needs_to_move_left and needs_to_move_right): if needs_to_move_left: - self.position = (self.position[0], self.position[1] - 1 if self.position[1] > -1 else self.position[1]) + self.position = ( + self.position[0], + ( + self.position[1] - 1 + if self.position[1] > -1 + else self.position[1] + ), + ) elif needs_to_move_right: - self.position = (self.position[0], self.position[1] + 1 if self.position[1] < 1 else self.position[1]) + self.position = ( + self.position[0], + ( + self.position[1] + 1 + if self.position[1] < 1 + else self.position[1] + ), + ) if not needs_to_move_up and needs_to_move_down: if needs_to_move_up: - self.position = (self.position[0] + 1 if self.position[0] < 3 else self.position[0], self.position[1]) + self.position = ( + ( + self.position[0] + 1 + if self.position[0] < 3 + else self.position[0] + ), + self.position[1], + ) elif needs_to_move_down: - self.position = (self.position[0] - 1 if self.position[0] > 0 else self.position[0], self.position[1]) + self.position = ( + ( + self.position[0] - 1 + if self.position[0] > 0 + else self.position[0] + ), + self.position[1], + ) elif has_both_eyes and not has_both_shoulders: # in this case try to make eyes into the upper 1/3 of the frame, @@ -288,10 +322,10 @@ def execute(self, userdata): else: pass # if x at left 2/7 or left shoulder dissappear, move left 1 step - if eyes_middle[0] <= 2/7: + if eyes_middle[0] <= 2 / 7: self.position[1] -= 1 # if x at right 2/7 or right shoulder dissappear, move right 1 step - elif eyes_middle[0] >= 5/7: + elif eyes_middle[0] >= 5 / 7: self.position[1] += 1 pass @@ -311,10 +345,10 @@ def execute(self, userdata): elif shoulders_middle[1] <= 1 / 4: self.position[0] += 1 # if x at left 2/7, move left 1 step - if shoulders_middle[0] <= 2/7: + if shoulders_middle[0] <= 2 / 7: self.position[1] -= 1 # if x at right 2/7, move right 1 step - elif shoulders_middle[0] >= 5/7: + elif shoulders_middle[0] >= 5 / 7: self.position[1] += 1 pass @@ -354,15 +388,15 @@ def execute(self, userdata): # if y at upper 1/4: up move 1 step elif very_middle[1] <= 1 / 4: self.position[0] += 1 - print('if y at upper 1/3: up move 1 step.') + print("if y at upper 1/3: up move 1 step.") # if x at left 2/7, move left 1 step - if very_middle[0] <= 2/7: + if very_middle[0] <= 2 / 7: self.position[1] -= 1 - print('if x at left 2/7, move left 1 step.') + print("if x at left 2/7, move left 1 step.") # if x at right 2/7, move right 1 step - elif very_middle[0] >= 5/7: + elif very_middle[0] >= 5 / 7: self.position[1] += 1 - print('if x at right 2/7, move right 1 step.') + print("if x at right 2/7, move right 1 step.") pass # keep the position in the range. @@ -379,5 +413,5 @@ def execute(self, userdata): if self.counter > self.max_attempts: return "truncated" self.counter += 1 - + return position_dict[(self.position[0], self.position[1])] diff --git a/tasks/receptionist/src/receptionist/state_machine.py b/tasks/receptionist/src/receptionist/state_machine.py index 5ffc64b75..78ed090ba 100755 --- a/tasks/receptionist/src/receptionist/state_machine.py +++ b/tasks/receptionist/src/receptionist/state_machine.py @@ -279,4 +279,4 @@ def _guide_guest(self, guest_id: int) -> None: "preempted": "failed", "aborted": "failed", }, - ) \ No newline at end of file + ) diff --git a/tasks/receptionist/src/receptionist/states/get_name_and_drink.py b/tasks/receptionist/src/receptionist/states/get_name_and_drink.py index 11171f088..4afb63975 100644 --- a/tasks/receptionist/src/receptionist/states/get_name_and_drink.py +++ b/tasks/receptionist/src/receptionist/states/get_name_and_drink.py @@ -7,9 +7,8 @@ import smach from smach import UserData from typing import List, Dict, Any -from receptionist.states import ( - SpeechRecovery -) +from receptionist.states import SpeechRecovery + class GetNameAndDrink(smach.StateMachine): class ParseNameAndDrink(smach.State): @@ -78,25 +77,25 @@ def execute(self, userdata: UserData) -> str: outcome = "failed" return outcome - + class PostRecoveryDecision(smach.State): def __init__( self, guest_id: str, param_key: str = "/receptionist/priors", ): - + smach.State.__init__( self, outcomes=["succeeded", "failed", "failed_name", "failed_drink"], input_keys=["guest_transcription", "guest_data"], output_keys=["guest_data", "guest_transcription"], - ) + ) self._guest_id = guest_id prior_data: Dict[str, List[str]] = rospy.get_param(param_key) self._possible_names = [name.lower() for name in prior_data["names"]] self._possible_drinks = [drink.lower() for drink in prior_data["drinks"]] - + def execute(self, userdata: UserData) -> str: if not self._recovery_name_and_drink_required(userdata): if userdata.guest_data[self._guest_id]["name"] == "unknown": @@ -106,7 +105,7 @@ def execute(self, userdata: UserData) -> str: else: outcome = "failed" return outcome - + def _recovery_name_and_drink_required(self, userdata: UserData) -> bool: """Determine whether both the name and drink requires recovery. @@ -120,11 +119,11 @@ def _recovery_name_and_drink_required(self, userdata: UserData) -> bool: return False def __init__( - self, - guest_id: str, - last_resort: bool, - param_key: str = "/receptionist/priors", - ): + self, + guest_id: str, + last_resort: bool, + param_key: str = "/receptionist/priors", + ): self._guest_id = guest_id self._param_key = param_key @@ -135,26 +134,32 @@ def __init__( outcomes=["succeeded", "failed", "failed_name", "failed_drink"], input_keys=["guest_transcription", "guest_data"], output_keys=["guest_data", "guest_transcription"], - ) with self: smach.StateMachine.add( "PARSE_NAME_AND_DRINK", - self.ParseNameAndDrink(guest_id=self._guest_id, param_key=self._param_key), + self.ParseNameAndDrink( + guest_id=self._guest_id, param_key=self._param_key + ), transitions={"succeeded": "succeeded", "failed": "SPEECH_RECOVERY"}, ) smach.StateMachine.add( "SPEECH_RECOVERY", SpeechRecovery(self._guest_id, self._last_resort), - transitions={"succeeded": "succeeded", "failed": "POST_RECOVERY_DECISION"}, + transitions={ + "succeeded": "succeeded", + "failed": "POST_RECOVERY_DECISION", + }, ) smach.StateMachine.add( "POST_RECOVERY_DECISION", - self.PostRecoveryDecision(guest_id=self._guest_id, param_key=self._param_key), + self.PostRecoveryDecision( + guest_id=self._guest_id, param_key=self._param_key + ), transitions={ "failed": "failed", "failed_name": "failed_name", "failed_drink": "failed_drink", }, - ) \ No newline at end of file + ) diff --git a/tasks/receptionist/src/receptionist/states/get_name_or_drink.py b/tasks/receptionist/src/receptionist/states/get_name_or_drink.py index 0ca946bbe..67e0b7409 100644 --- a/tasks/receptionist/src/receptionist/states/get_name_or_drink.py +++ b/tasks/receptionist/src/receptionist/states/get_name_or_drink.py @@ -7,9 +7,8 @@ import smach from smach import UserData from typing import List, Dict, Any -from receptionist.states import ( - SpeechRecovery -) +from receptionist.states import SpeechRecovery + class GetNameOrDrink(smach.StateMachine): class ParseTranscribedInfo(smach.State): @@ -39,9 +38,10 @@ def __init__( prior_data: Dict[str, List[str]] = rospy.get_param(param_key) possible_drinks = [drink.lower() for drink in prior_data["drinks"]] possible_names = [name.lower() for name in prior_data["names"]] - self._possible_information = {"drink": possible_drinks, "name": possible_names}[ - self._type - ] + self._possible_information = { + "drink": possible_drinks, + "name": possible_names, + }[self._type] def execute(self, userdata: UserData) -> str: """Parse the guest's information. @@ -64,7 +64,9 @@ def execute(self, userdata: UserData) -> str: for key_phrase in self._possible_information: if key_phrase in transcription: userdata.guest_data[self._guest_id][self._type] = key_phrase - rospy.loginfo(f"Guest/Drink {self._type} identified as: {key_phrase}") + rospy.loginfo( + f"Guest/Drink {self._type} identified as: {key_phrase}" + ) information_found = True break if not information_found: @@ -73,14 +75,14 @@ def execute(self, userdata: UserData) -> str: outcome = "failed" return outcome - + def __init__( - self, - guest_id: str, - last_resort: bool, - info_type: str, - param_key: str = "/receptionist/priors", - ): + self, + guest_id: str, + last_resort: bool, + info_type: str, + param_key: str = "/receptionist/priors", + ): self._last_resort = last_resort self._guest_id = guest_id @@ -92,13 +94,16 @@ def __init__( outcomes=["succeeded", "failed"], input_keys=["guest_transcription", "guest_data"], output_keys=["guest_data", "guest_transcription"], - ) with self: smach.StateMachine.add( "PARSE_NAME_OR_DRINK", - self.ParseTranscribedInfo(guest_id=self._guest_id, info_type=self._info_type, param_key=self._param_key), + self.ParseTranscribedInfo( + guest_id=self._guest_id, + info_type=self._info_type, + param_key=self._param_key, + ), transitions={"succeeded": "succeeded", "failed": "SPEECH_RECOVERY"}, ) smach.StateMachine.add( diff --git a/tasks/receptionist/src/receptionist/states/local_speech_recognition.py b/tasks/receptionist/src/receptionist/states/local_speech_recognition.py index 565111d07..867dba845 100644 --- a/tasks/receptionist/src/receptionist/states/local_speech_recognition.py +++ b/tasks/receptionist/src/receptionist/states/local_speech_recognition.py @@ -1,21 +1,57 @@ import jellyfish as jf - -available_names = ["adel", "angel", "axel", "charlie", "jane", "jules", "morgan", "paris", "robin", "simone"] -available_single_drinks = ["cola", "milk",] -available_double_drinks = ["iced", "tea", "pack", "juice", "orange", "red", "wine", "tropical"] -double_drinks_dict = {"iced" : "iced tea", - "tea": "iced tea", - "pack" : "juice pack", - "orange" : "orange juice", - "red" : "red wine", - "wine" : "red wine", - "tropical" : "tropical juice", - # "juice": ["orange juice", "tropical juice", "juice pack"], - } -available_drinks = list(set(available_single_drinks).union(set(available_double_drinks))) -excluded_words = ["my", "name", "is", "and", "favourite","drink", "you", "can", "call", "me"] +available_names = [ + "adel", + "angel", + "axel", + "charlie", + "jane", + "jules", + "morgan", + "paris", + "robin", + "simone", +] +available_single_drinks = [ + "cola", + "milk", +] +available_double_drinks = [ + "iced", + "tea", + "pack", + "juice", + "orange", + "red", + "wine", + "tropical", +] +double_drinks_dict = { + "iced": "iced tea", + "tea": "iced tea", + "pack": "juice pack", + "orange": "orange juice", + "red": "red wine", + "wine": "red wine", + "tropical": "tropical juice", + # "juice": ["orange juice", "tropical juice", "juice pack"], +} +available_drinks = list( + set(available_single_drinks).union(set(available_double_drinks)) +) +excluded_words = [ + "my", + "name", + "is", + "and", + "favourite", + "drink", + "you", + "can", + "call", + "me", +] def speech_recovery(sentence): @@ -23,7 +59,7 @@ def speech_recovery(sentence): sentence_list = list(set(sentence_split) - set(excluded_words)) print(f"final name: {handle_name(sentence_list, True)}") print(f"final drink: {handle_drink(sentence_list, True)}") - + def handle_name(sentence_list, last_resort): result = handle_similar_spelt(sentence_list, available_names, 1) @@ -38,7 +74,7 @@ def handle_name(sentence_list, last_resort): else: print("Last resort name") return handle_closest_spelt(sentence_list, available_names) - + def handle_drink(sentence_list, last_resort): result = infer_second_drink(sentence_list) @@ -50,7 +86,7 @@ def handle_drink(sentence_list, last_resort): else: result = handle_similar_sound(sentence_list, available_drinks, 0) print(f"drink (sound): {result}") - + if result != "unknown": if result in available_single_drinks: print(f"final attempt drink: {result}") @@ -88,13 +124,14 @@ def handle_similar_sound(sentence_list, available_words, distance_threshold): if distance <= distance_threshold: print(input_word, available_word) return available_word - return "unknown" + return "unknown" + def infer_second_drink(sentence_list): for input_word in sentence_list: if input_word == "juice": choices = ["pack", "orange", "tropical"] - closest_word = handle_closest_spelt(sentence_list, choices) + closest_word = handle_closest_spelt(sentence_list, choices) if closest_word == "pack": return "juice pack" elif closest_word == "orange": @@ -106,8 +143,9 @@ def infer_second_drink(sentence_list): return double_drinks_dict[input_word] return "unknown" + def handle_closest_spelt(sentence_list, choices): - closest_distance = float('inf') + closest_distance = float("inf") closest_word = None for input_word in sentence_list: for available_word in choices: @@ -121,11 +159,13 @@ def handle_closest_spelt(sentence_list, choices): def get_damerau_levenshtein_distance(word_1, word_2): return jf.damerau_levenshtein_distance(word_1, word_2) + def get_levenshtein_soundex_distance(word_1, word_2): soundex_word_1 = jf.soundex(word_1) soundex_word_2 = jf.soundex(word_2) return jf.levenshtein_distance(soundex_word_1, soundex_word_2) + # print(get_damerau_levenshtein_distance("juice", "shoes")) # print(get_levenshtein_soundex_distance("juice", "shoes")) @@ -139,8 +179,8 @@ def get_levenshtein_soundex_distance(word_1, word_2): # available_names = ["adel", "angel", "axel", "charlie", "jane", "jules", "morgan", "paris", "robin", "simone"] # available_single_drinks = ["cola", "milk",] # available_double_drinks = ["iced", "tea", "juice", "pack", "orange", "red", "wine", "tropical"] -# double_drinks_dict = {"iced" : "iced tea", -# "tea": "iced tea", +# double_drinks_dict = {"iced" : "iced tea", +# "tea": "iced tea", # "pack" : "juice pack", # "orange" : "orange juice", # "red" : "red wine", @@ -174,4 +214,4 @@ def get_levenshtein_soundex_distance(word_1, word_2): print("======") sentence = "my name is morgen and my favourite drink is mll" speech_recovery(sentence) - print("======") \ No newline at end of file + print("======") diff --git a/tasks/receptionist/src/receptionist/states/speech_recovery.py b/tasks/receptionist/src/receptionist/states/speech_recovery.py index e2113bf8d..d44d45e69 100644 --- a/tasks/receptionist/src/receptionist/states/speech_recovery.py +++ b/tasks/receptionist/src/receptionist/states/speech_recovery.py @@ -5,6 +5,7 @@ from smach import UserData from typing import List, Dict, Any + class SpeechRecovery(smach.State): def __init__( self, @@ -20,25 +21,64 @@ def __init__( ) self._guest_id = guest_id - self._last_resort = last_resort + self._last_resort = last_resort self._input_type = input_type - self._available_names = ["adel", "angel", "axel", "charlie", "jane", "jules", "morgan", "paris", "robin", "simone"] - self._available_single_drinks = ["cola", "milk",] - self._available_double_drinks = ["iced", "tea", "pack", "juice", "orange", "red", "wine", "tropical"] - self._double_drinks_dict = {"iced" : "iced tea", - "tea": "iced tea", - "pack" : "juice pack", - "orange" : "orange juice", - "red" : "red wine", - "wine" : "red wine", - "tropical" : "tropical juice", - # "juice": ["orange juice", "tropical juice", "juice pack"], - } - self._available_drinks = list(set(self._available_single_drinks).union(set(self._available_double_drinks))) - self._excluded_words = ["my", "name", "is", "and", "favourite","drink", "you", "can", "call", "me"] + self._available_names = [ + "adel", + "angel", + "axel", + "charlie", + "jane", + "jules", + "morgan", + "paris", + "robin", + "simone", + ] + self._available_single_drinks = [ + "cola", + "milk", + ] + self._available_double_drinks = [ + "iced", + "tea", + "pack", + "juice", + "orange", + "red", + "wine", + "tropical", + ] + self._double_drinks_dict = { + "iced": "iced tea", + "tea": "iced tea", + "pack": "juice pack", + "orange": "orange juice", + "red": "red wine", + "wine": "red wine", + "tropical": "tropical juice", + # "juice": ["orange juice", "tropical juice", "juice pack"], + } + self._available_drinks = list( + set(self._available_single_drinks).union(set(self._available_double_drinks)) + ) + self._excluded_words = [ + "my", + "name", + "is", + "and", + "favourite", + "drink", + "you", + "can", + "call", + "me", + ] def execute(self, userdata: UserData) -> str: - filtered_sentence = userdata.guest_transcription.lower().translate(str.maketrans('', '', string.punctuation)) + filtered_sentence = userdata.guest_transcription.lower().translate( + str.maketrans("", "", string.punctuation) + ) sentence_split = filtered_sentence.split() if sentence_split is None: return "failed" @@ -65,15 +105,17 @@ def execute(self, userdata: UserData) -> str: final_name = self._handle_name(sentence_list, self._last_resort) userdata.guest_data[self._guest_id]["name"] = final_name print(f"Recovered name: {final_name} ") - if userdata.guest_data[self._guest_id]["drink"] == "unknown": + if userdata.guest_data[self._guest_id]["drink"] == "unknown": final_drink = self._handle_drink(sentence_list, self._last_resort) userdata.guest_data[self._guest_id]["drink"] = final_drink print(f"Recovered drink: {final_drink} ") - if userdata.guest_data[self._guest_id]["name"] == "unknown" or userdata.guest_data[self._guest_id]["drink"] == "unknown": + if ( + userdata.guest_data[self._guest_id]["name"] == "unknown" + or userdata.guest_data[self._guest_id]["drink"] == "unknown" + ): return "failed" else: return "succeeded" - def _handle_name(self, sentence_list, last_resort): result = self._handle_similar_spelt(sentence_list, self._available_names, 1) @@ -88,7 +130,6 @@ def _handle_name(self, sentence_list, last_resort): else: print("Last resort name") return self._handle_closest_spelt(sentence_list, self._available_names) - def _handle_drink(self, sentence_list, last_resort): result = self._infer_second_drink(sentence_list) @@ -98,9 +139,11 @@ def _handle_drink(self, sentence_list, last_resort): if result != "unknown": print(f"drink (spelt): {result}") else: - result = self._handle_similar_sound(sentence_list, self._available_drinks, 0) + result = self._handle_similar_sound( + sentence_list, self._available_drinks, 0 + ) print(f"drink (sound): {result}") - + if result != "unknown": if result in self._available_single_drinks: print(f"final attempt drink: {result}") @@ -113,7 +156,9 @@ def _handle_drink(self, sentence_list, last_resort): return "unknown" else: print("Last resort drink") - closest_spelt = self._handle_closest_spelt(sentence_list, self._available_drinks) + closest_spelt = self._handle_closest_spelt( + sentence_list, self._available_drinks + ) if closest_spelt in self._available_single_drinks: print(f"final attempt during last resort drink: {closest_spelt}") return closest_spelt @@ -121,30 +166,32 @@ def _handle_drink(self, sentence_list, last_resort): sentence_list.append(closest_spelt) return self._infer_second_drink(closest_spelt) - def _handle_similar_spelt(self, sentence_list, available_words, distance_threshold): for input_word in sentence_list: for available_word in available_words: - distance = self._get_damerau_levenshtein_distance(input_word, available_word) + distance = self._get_damerau_levenshtein_distance( + input_word, available_word + ) if distance <= distance_threshold: return available_word return "unknown" - def _handle_similar_sound(self, sentence_list, available_words, distance_threshold): for input_word in sentence_list: for available_word in available_words: - distance = self._get_levenshtein_soundex_distance(input_word, available_word) + distance = self._get_levenshtein_soundex_distance( + input_word, available_word + ) if distance <= distance_threshold: print(input_word, available_word) return available_word - return "unknown" + return "unknown" def _infer_second_drink(self, sentence_list): for input_word in sentence_list: if input_word == "juice": choices = ["pack", "orange", "tropical"] - closest_word = self._handle_closest_spelt(sentence_list, choices) + closest_word = self._handle_closest_spelt(sentence_list, choices) if closest_word == "pack": return "juice pack" elif closest_word == "orange": @@ -157,21 +204,22 @@ def _infer_second_drink(self, sentence_list): return "unknown" def _handle_closest_spelt(self, sentence_list, choices): - closest_distance = float('inf') + closest_distance = float("inf") closest_word = None for input_word in sentence_list: for available_word in choices: - distance = self._get_damerau_levenshtein_distance(input_word, available_word) + distance = self._get_damerau_levenshtein_distance( + input_word, available_word + ) if distance < closest_distance: closest_distance = distance closest_word = available_word return closest_word - def _get_damerau_levenshtein_distance(self, word_1, word_2): return jf.damerau_levenshtein_distance(word_1, word_2) def _get_levenshtein_soundex_distance(self, word_1, word_2): soundex_word_1 = jf.soundex(word_1) soundex_word_2 = jf.soundex(word_2) - return jf.levenshtein_distance(soundex_word_1, soundex_word_2) \ No newline at end of file + return jf.levenshtein_distance(soundex_word_1, soundex_word_2) From 89d1c009ac95831311ee206af14e9cc1d450ecd1 Mon Sep 17 00:00:00 2001 From: Benteng Ma Date: Sun, 7 Jul 2024 17:39:41 +0100 Subject: [PATCH 02/20] Have a quick fix of hat-hair issue, and a quick remove of the dress (but doesn't change any code structure). --- .../image_with_masks_and_attributes.py | 10 ++++++++++ tasks/receptionist/scripts/main.py | 3 --- 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py b/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py index 235ba7a4c..5bd26247b 100644 --- a/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py +++ b/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py @@ -98,6 +98,11 @@ def describe(self) -> dict: glasses = self.attributes["Eyeglasses"] - 0.5 hat = self.attributes["Wearing_Hat"] - 0.5 + if hat >= 0.25: # probably wearing hat + has_hair *= ( + 0.5 - hat + ) * has_hair # give a penalty to the hair confidence (hope this helps!) + result = { "has_hair": has_hair, "hair_colour": hair_colour, @@ -136,6 +141,8 @@ def from_parent_instance( ) def describe(self) -> dict: + # Maybe not keep ‘sleeveless’ anymore but might do this in the actual environment. + result = { "top": self.attributes["top"] / 2, "outwear": self.attributes["outwear"] / 2, @@ -178,4 +185,7 @@ def describe(self) -> dict: max_attribute = "sleeveless dress" result["max_dress"] = max_attribute + # QUICK FIX that won't break the code + result["max_dress"] = 0.0 + return result diff --git a/tasks/receptionist/scripts/main.py b/tasks/receptionist/scripts/main.py index 9011ceb2b..4f387420b 100644 --- a/tasks/receptionist/scripts/main.py +++ b/tasks/receptionist/scripts/main.py @@ -35,9 +35,6 @@ orientation=Quaternion(**seat_pose_param["orientation"]), ) - sweep_points_param = rospy.get_param("/receptionist/sweep_points") - sweep_points = [Point(**point) for point in sweep_points_param] - seat_area_param = rospy.get_param("/receptionist/seat_area") sofa_area_param = rospy.get_param("/receptionist/sofa_area") From 16ff8da214e1c24c566ebfeeb659ad221d134f7f Mon Sep 17 00:00:00 2001 From: Benteng Ma Date: Sun, 7 Jul 2024 17:40:41 +0100 Subject: [PATCH 03/20] a quick fix of hat-hair penalty. --- .../image_with_masks_and_attributes.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py b/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py index 5bd26247b..56a17cee3 100644 --- a/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py +++ b/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py @@ -100,8 +100,8 @@ def describe(self) -> dict: if hat >= 0.25: # probably wearing hat has_hair *= ( - 0.5 - hat - ) * has_hair # give a penalty to the hair confidence (hope this helps!) + (0.5 - hat) / 0.5 * has_hair + ) # give a penalty to the hair confidence (hope this helps!) result = { "has_hair": has_hair, From 3fca2cec17d4e9e8739413940222e8c9049c178b Mon Sep 17 00:00:00 2001 From: Benteng Ma Date: Sun, 7 Jul 2024 18:23:58 +0100 Subject: [PATCH 04/20] modified adjust_camera a little such as it generally is lowered down a bit. --- skills/src/lasr_skills/adjust_camera.py | 46 ++++++++++++++++--------- 1 file changed, 29 insertions(+), 17 deletions(-) diff --git a/skills/src/lasr_skills/adjust_camera.py b/skills/src/lasr_skills/adjust_camera.py index c57fef38b..7bfa1fd2e 100644 --- a/skills/src/lasr_skills/adjust_camera.py +++ b/skills/src/lasr_skills/adjust_camera.py @@ -9,6 +9,7 @@ import rospkg import rosparam import os +import math LEFT = { @@ -307,7 +308,7 @@ def execute(self, userdata): ) elif has_both_eyes and not has_both_shoulders: - # in this case try to make eyes into the upper 1/3 of the frame, + # in this case try to make eyes into the upper 1/5 of the frame, eyes_middle = ( (keypoint_info["leftEye"][0] + keypoint_info["rightEye"][0]) / 2, (keypoint_info["leftEye"][1] + keypoint_info["rightEye"][1]) / 2, @@ -370,25 +371,36 @@ def execute(self, userdata): (eyes_middle[1] + shoulders_middle[1]) / 2, ) rospy.logwarn(f"very middle {very_middle}") - # if y at upper 1/5 for eyes: move up 1 step - if eyes_middle[1] <= 1 / 5: - self.position[0] += 1 - print("if y at upper 1/5 for eyes: move up 1 step") + eyes_to_shoulders_distance = math.sqrt((eyes_middle[0] - shoulders_middle[0]) ** 2 + (eyes_middle[1] - shoulders_middle[1])) + if eyes_to_shoulders_distance > 1/3: + # person is kind of close to the camera, + # if y at upper 1/5 for eyes: move up 1 step + if eyes_middle[1] <= 1 / 5: + self.position[0] += 1 + print("if y at upper 1/5 for eyes: move up 1 step") + else: + if ( + 1 / 4 <= very_middle[1] <= 2 / 3 + and 1 / 3 <= very_middle[0] <= 2 / 3 + ): + print("finished.") + return "finished" + # if y at down 1/3: down move 1 step + if very_middle[1] >= 2 / 3: + self.position[0] -= 1 + print("if y at down 1/3: down move 1 step.") + # if y at upper 1/4: up move 1 step + elif very_middle[1] <= 1 / 4: + self.position[0] += 1 + print("if y at upper 1/3: up move 1 step.") else: - if ( - 1 / 4 <= very_middle[1] <= 2 / 3 - and 1 / 3 <= very_middle[0] <= 2 / 3 - ): - print("finished.") - return "finished" - # if y at down 1/3: down move 1 step - if very_middle[1] >= 2 / 3: + # person is kind of far from the camera. + # in this case simply try to the middle-point of the shoulder to the centre up + if shoulders_middle[1] >= 1 / 2: self.position[0] -= 1 - print("if y at down 1/3: down move 1 step.") - # if y at upper 1/4: up move 1 step - elif very_middle[1] <= 1 / 4: + elif shoulders_middle[1] <= 1 / 2 - 1 / 5: self.position[0] += 1 - print("if y at upper 1/3: up move 1 step.") + pass # if x at left 2/7, move left 1 step if very_middle[0] <= 2 / 7: self.position[1] -= 1 From 667f9cf30552a9b961d06f955bf0d2fb427d05f6 Mon Sep 17 00:00:00 2001 From: Benteng Ma Date: Sun, 7 Jul 2024 20:17:02 +0100 Subject: [PATCH 05/20] I feel that the 'closest' and 'futherest' of cropped_detection is kind of revered, so I corrected it. --- .../cropped_detection.py | 8 +-- skills/config/motions.yaml | 6 +- skills/src/lasr_skills/adjust_camera.py | 59 ++++++++++++------- 3 files changed, 46 insertions(+), 27 deletions(-) diff --git a/common/vision/lasr_vision_cropped_detection/src/lasr_vision_cropped_detection/cropped_detection.py b/common/vision/lasr_vision_cropped_detection/src/lasr_vision_cropped_detection/cropped_detection.py index e9360cc57..5453b3a41 100644 --- a/common/vision/lasr_vision_cropped_detection/src/lasr_vision_cropped_detection/cropped_detection.py +++ b/common/vision/lasr_vision_cropped_detection/src/lasr_vision_cropped_detection/cropped_detection.py @@ -227,13 +227,13 @@ def _3d_mask_crop( ] if crop_method == "closest": - detections = [det for _, det in sorted(zip(distances, detections))] - distances.sort() + detections = [det for _, det in sorted(zip(distances, detections), reverse=True)] + distances.sort(reverse=True) elif crop_method == "furthest": detections = [ - det for _, det in sorted(zip(distances, detections), reverse=True) + det for _, det in sorted(zip(distances, detections)) # , reverse=True) ] - distances.sort(reverse=True) + distances.sort() # reverse=True) else: raise ValueError(f"Invalid 3D crop_method: {crop_method}") diff --git a/skills/config/motions.yaml b/skills/config/motions.yaml index 97afe2c75..8af6ac21f 100644 --- a/skills/config/motions.yaml +++ b/skills/config/motions.yaml @@ -84,17 +84,17 @@ play_motion: u3l: joints: [torso_lift_joint, head_1_joint, head_2_joint] points: - - positions: [0.4, 0.35, 0.05] + - positions: [0.4, 0.35, 0.1] time_from_start: 0.0 u3m: joints: [torso_lift_joint, head_1_joint, head_2_joint] points: - - positions: [0.4, 0.0, 0.05] + - positions: [0.4, 0.0, 0.1] time_from_start: 0.0 u3r: joints: [torso_lift_joint, head_1_joint, head_2_joint] points: - - positions: [0.4, -0.35, 0.05] + - positions: [0.4, -0.35, 0.1] time_from_start: 0.0 u2l: diff --git a/skills/src/lasr_skills/adjust_camera.py b/skills/src/lasr_skills/adjust_camera.py index 7bfa1fd2e..850a039b6 100644 --- a/skills/src/lasr_skills/adjust_camera.py +++ b/skills/src/lasr_skills/adjust_camera.py @@ -372,35 +372,54 @@ def execute(self, userdata): ) rospy.logwarn(f"very middle {very_middle}") eyes_to_shoulders_distance = math.sqrt((eyes_middle[0] - shoulders_middle[0]) ** 2 + (eyes_middle[1] - shoulders_middle[1])) - if eyes_to_shoulders_distance > 1/3: + rospy.logwarn(f"eyes to shoulder distance {eyes_to_shoulders_distance}") + if eyes_to_shoulders_distance > 0.14: # person is kind of close to the camera, - # if y at upper 1/5 for eyes: move up 1 step - if eyes_middle[1] <= 1 / 5: - self.position[0] += 1 - print("if y at upper 1/5 for eyes: move up 1 step") + if eyes_to_shoulders_distance >= 0.24: + # person if very close to the camera. + # ACTUALLY, maybe should just tell person to stand back? + # if y at upper 1/5 for eyes: move up 1 step + if eyes_middle[1] <= 1 / 5: + self.position[0] += 1 + print("if y at upper 1/4 for eyes: move up 1 step") + else: + # if y at down 1/5: down move 1 step + if very_middle[1] >= 4 / 5: + self.position[0] -= 1 + print("if y at down 1/5: down move 1 step.") else: - if ( - 1 / 4 <= very_middle[1] <= 2 / 3 - and 1 / 3 <= very_middle[0] <= 2 / 3 - ): - print("finished.") - return "finished" - # if y at down 1/3: down move 1 step - if very_middle[1] >= 2 / 3: - self.position[0] -= 1 - print("if y at down 1/3: down move 1 step.") - # if y at upper 1/4: up move 1 step - elif very_middle[1] <= 1 / 4: + # if y at upper 1/7 for eyes: move up 1 step + if eyes_middle[1] <= 1 / 7: self.position[0] += 1 - print("if y at upper 1/3: up move 1 step.") + print("if y at upper 1/7 for eyes: move up 1 step") + elif eyes_middle[1] >= 2 / 9: + self.position[0] -= 1 + print("if y lower than upper 2/9 for eyes: move down 1 step") + else: + if ( + 1 / 4 <= very_middle[1] <= 1 / 2 + and 2 / 7 <= very_middle[0] <= 5 / 7 + ): + print("finished.") + return "finished" + # if y at down 3/7: down move 1 step + if very_middle[1] >= 4 / 7: + self.position[0] -= 1 + print("if y at down 3/7: down move 1 step.") + # if y at upper 1/4: up move 1 step + elif very_middle[1] <= 1 / 4: + self.position[0] += 1 + print("if y at upper 1/4: up move 1 step.") else: # person is kind of far from the camera. # in this case simply try to the middle-point of the shoulder to the centre up if shoulders_middle[1] >= 1 / 2: self.position[0] -= 1 - elif shoulders_middle[1] <= 1 / 2 - 1 / 5: + elif shoulders_middle[1] <= 1 / 2 - 2 / 7: self.position[0] += 1 - pass + elif 2 / 7 <= very_middle[0] <= 5 / 7: + print("finished.") + return "finished" # if x at left 2/7, move left 1 step if very_middle[0] <= 2 / 7: self.position[1] -= 1 From 314ba28464bdf169e580f3e5ae6e926932c0ce3c Mon Sep 17 00:00:00 2001 From: Benteng Ma Date: Sun, 7 Jul 2024 20:21:28 +0100 Subject: [PATCH 06/20] probably the most common usage of GetCroppedImage is 'closest' so make it the default. --- skills/src/lasr_skills/vision/get_cropped_image.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/skills/src/lasr_skills/vision/get_cropped_image.py b/skills/src/lasr_skills/vision/get_cropped_image.py index 459ee258d..a7108bb1e 100755 --- a/skills/src/lasr_skills/vision/get_cropped_image.py +++ b/skills/src/lasr_skills/vision/get_cropped_image.py @@ -20,7 +20,7 @@ class GetCroppedImage(smach.State): def __init__( self, object_name: str, - method: str = "centered", + method: str = "closest", use_mask: bool = True, yolo_model: str = "yolov8x-seg.pt", yolo_model_confidence: float = 0.5, From 0ae1d711f55e24c8c220484f9047060a915ff733 Mon Sep 17 00:00:00 2001 From: Benteng Ma Date: Sun, 7 Jul 2024 20:36:35 +0100 Subject: [PATCH 07/20] modified all the 'reverse' parts. --- .../lasr_vision_cropped_detection/cropped_detection.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/common/vision/lasr_vision_cropped_detection/src/lasr_vision_cropped_detection/cropped_detection.py b/common/vision/lasr_vision_cropped_detection/src/lasr_vision_cropped_detection/cropped_detection.py index 5453b3a41..209132c04 100644 --- a/common/vision/lasr_vision_cropped_detection/src/lasr_vision_cropped_detection/cropped_detection.py +++ b/common/vision/lasr_vision_cropped_detection/src/lasr_vision_cropped_detection/cropped_detection.py @@ -172,14 +172,14 @@ def _3d_bbox_crop( for det in detections ] if crop_method == "closest": - detections = [det for _, det in sorted(zip(distances, detections))] - distances.sort() + detections = [det for _, det in sorted(zip(distances, detections), reverse=True)] + distances.sort(reverse=True) elif crop_method == "furthest": detections = [ - det for _, det in sorted(zip(distances, detections), reverse=True) + det for _, det in sorted(zip(distances, detections)) # , reverse=True) ] - distances.sort(reverse=True) + distances.sort() # , reverse=True) else: raise ValueError(f"Invalid 3D crop_method: {crop_method}") From 7c44db5c047135865a4d2b6e23ff52a61505f67d Mon Sep 17 00:00:00 2001 From: Benteng Ma Date: Mon, 8 Jul 2024 17:59:11 +0100 Subject: [PATCH 08/20] corrected a small mistake of removing the 'dress' category. --- .../image_with_masks_and_attributes.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py b/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py index 56a17cee3..24db7b8cb 100644 --- a/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py +++ b/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py @@ -186,6 +186,6 @@ def describe(self) -> dict: result["max_dress"] = max_attribute # QUICK FIX that won't break the code - result["max_dress"] = 0.0 + result["dress"] = 0.0 return result From 30cc76e3d5c29f0a21eeea488a6350aa47d79e3d Mon Sep 17 00:00:00 2001 From: Benteng Ma Date: Tue, 9 Jul 2024 14:45:04 +0100 Subject: [PATCH 09/20] Changed matt's reverse list back. --- .../cropped_detection.py | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/common/vision/lasr_vision_cropped_detection/src/lasr_vision_cropped_detection/cropped_detection.py b/common/vision/lasr_vision_cropped_detection/src/lasr_vision_cropped_detection/cropped_detection.py index 209132c04..0cba74b78 100644 --- a/common/vision/lasr_vision_cropped_detection/src/lasr_vision_cropped_detection/cropped_detection.py +++ b/common/vision/lasr_vision_cropped_detection/src/lasr_vision_cropped_detection/cropped_detection.py @@ -171,15 +171,15 @@ def _3d_bbox_crop( ) for det in detections ] - if crop_method == "closest": - detections = [det for _, det in sorted(zip(distances, detections), reverse=True)] - distances.sort(reverse=True) + if crop_method == "closest": + detections = [det for _, det in sorted(zip(distances, detections))] + distances.sort() elif crop_method == "furthest": detections = [ - det for _, det in sorted(zip(distances, detections)) # , reverse=True) + det for _, det in sorted(zip(distances, detections), reverse=True) ] - distances.sort() # , reverse=True) + distances.sort(reverse=True) else: raise ValueError(f"Invalid 3D crop_method: {crop_method}") @@ -227,16 +227,17 @@ def _3d_mask_crop( ] if crop_method == "closest": - detections = [det for _, det in sorted(zip(distances, detections), reverse=True)] - distances.sort(reverse=True) + detections = [det for _, det in sorted(zip(distances, detections))] + distances.sort() elif crop_method == "furthest": detections = [ - det for _, det in sorted(zip(distances, detections)) # , reverse=True) + det for _, det in sorted(zip(distances, detections), reverse=True) ] - distances.sort() # reverse=True) + distances.sort(reverse=True) else: raise ValueError(f"Invalid 3D crop_method: {crop_method}") + masked_images = [] unified_mask = np.zeros(rgb_image.shape).astype(rgb_image.dtype) From 406d746bab556e1baa18ee4a3fd72dd19c748b0c Mon Sep 17 00:00:00 2001 From: Benteng Ma Date: Tue, 9 Jul 2024 15:03:59 +0100 Subject: [PATCH 10/20] Added new colour estimation of cloth. --- .../__init__.py | 176 +++++++++++++++++- .../image_with_masks_and_attributes.py | 30 +++ 2 files changed, 205 insertions(+), 1 deletion(-) diff --git a/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/__init__.py b/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/__init__.py index 8b1704c56..fc52c442e 100644 --- a/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/__init__.py +++ b/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/__init__.py @@ -1,6 +1,6 @@ import json from os import path - +import matplotlib.pyplot as plt import cv2 import numpy as np import rospkg @@ -20,6 +20,180 @@ ) +reference_colours = { + "blue_very_light": np.array([240, 248, 255]), # Alice blue + "blue_light": np.array([173, 216, 230]), # Light blue + "blue_sky": np.array([135, 206, 235]), # Sky blue + "blue_powder": np.array([176, 224, 230]), # Powder blue + "blue_celeste": np.array([178, 255, 255]), # Celeste, very pale blue shade + "blue_periwinkle": np.array([204, 204, 255]), # Periwinkle, a mix of light blue and lavender + "blue_cadet": np.array([95, 158, 160]), # Cadet blue, a muted blue-green + "blue": np.array([0, 0, 255]), # Standard blue + "blue_royal": np.array([65, 105, 225]), # Royal blue + "blue_deep": np.array([0, 0, 139]), # Deep blue + "blue_dark": np.array([0, 0, 128]), # Dark blue + # "blue_navy": np.array([0, 0, 80]), # Navy blue + + "yellow_very_light": np.array([255, 255, 204]), # Very light yellow + "yellow_light": np.array([255, 255, 224]), # Light yellow + "yellow": np.array([255, 255, 0]), # Standard yellow + "yellow_gold": np.array([255, 215, 0]), # Gold yellow + "yellow_dark": np.array([204, 204, 0]), # Dark yellow + "yellow_mustard": np.array([255, 219, 88]), # Mustard yellow + + "red_very_light": np.array([255, 204, 204]), # Very light red + "red_light": np.array([255, 102, 102]), # Light red + "red": np.array([255, 0, 0]), # Standard red + "red_dark": np.array([139, 0, 0]), # Dark red + "red_maroon": np.array([128, 0, 0]), # Maroon + + "orange_very_light": np.array([255, 229, 180]), # Very light orange + "orange_light": np.array([255, 179, 71]), # Light orange + "orange": np.array([255, 165, 0]), # Standard orange + "orange_dark": np.array([255, 140, 0]), # Dark orange + "orange_burnt": np.array([204, 85, 0]), # Burnt orange + + "black": np.array([30, 30, 30]), # Black + "white": np.array([255, 255, 255]), # White + "gray": np.array([160, 160, 160]) # Gray +} + +colour_group_map = { + "blue_very_light": "blue", + "blue_light": "blue", + "blue_sky": "blue", + "blue_powder": "blue", + "blue_celeste": "blue", + "blue_periwinkle": "blue", + "blue_cadet": "blue", + "blue": "blue", + "blue_royal": "blue", + "blue_deep": "blue", + "blue_dark": "blue", + + "yellow_very_light": "yellow", + "yellow_light": "yellow", + "yellow": "yellow", + "yellow_gold": "yellow", + "yellow_dark": "yellow", + "yellow_mustard": "yellow", + + "red_very_light": "red", + "red_light": "red", + "red": "red", + "red_dark": "red", + "red_maroon": "red", + + "orange_very_light": "orange", + "orange_light": "orange", + "orange": "orange", + "orange_dark": "orange", + "orange_burnt": "orange", + + "black": "black", + "white": "white", + "gray": "gray" +} + +possible_colours = ["blue", "yellow", "red", "orange", "black", "white", "gray"] + + +def estimate_colour(rgb_array): + # Calculate distances to each reference colour + # distances = {colour: cie76_distance(rgb_array, ref_rgb) for colour, ref_rgb in reference_colours.items()} + distances = {colour: np.linalg.norm(rgb_array - ref_rgb) for colour, ref_rgb in reference_colours.items()} + + # Find the colour with the smallest distance + estimated_colour = min(distances, key=distances.get) + + return estimated_colour + + +def split_and_sample_colours(image, mask, square_size): + height, width, _ = image.shape + squares_colours = {} + valid_squares = set() + + square_index = 0 + + for y in range(0, height, square_size): + for x in range(0, width, square_size): + square = image[y:y + square_size, x:x + square_size] + mask_square = mask[y:y + square_size, x:x + square_size] + + # Calculate the average colour + average_colour = square.mean(axis=(0, 1)) + + # Save the average colour in the dictionary + squares_colours[square_index] = estimate_colour(average_colour) + # squares_colours[square_index] = average_colour # estimate_colour(average_colour) + + # Check the mask condition + a = np.sum(mask_square) + if np.sum(mask_square) > 0.5 * square_size * square_size: + valid_squares.add(square_index) + + square_index += 1 + + return squares_colours, valid_squares + + +def gaussian_blur(image, kernel_size, rep=3): + """ + Apply Gaussian blur to an RGB image. + + Parameters: + image (numpy.ndarray): The input RGB image. + kernel_size (int): The size of the Gaussian kernel. If an even number is provided, it will be incremented to the next odd number. + + Returns: + numpy.ndarray: The blurred RGB image. + """ + if kernel_size % 2 == 0: + kernel_size += 1 # Increment kernel size to make it odd if it's even + + for _ in range(rep): + image = cv2.GaussianBlur(image, (kernel_size, kernel_size), 0) + return image + + +def visualize_grids(image, squares_colours, square_size): + """ + Display the image with coloured grids based on average colours. + + Parameters: + image (numpy.ndarray): The original RGB image. + squares_colours (dict): Dictionary with the average colours of each square. + square_size (int): The size of each square. + """ + height, width, _ = image.shape + grid_image = np.zeros((height, width, 3), dtype=np.uint8) + + square_index = 0 + for y in range(0, height, square_size): + for x in range(0, width, square_size): + # Get the colour from the dictionary + colour = np.array(reference_colours[squares_colours[square_index]]) + # Fill the square using numpy slicing + grid_image[y:y + square_size, x:x + square_size] = colour + + # Optionally draw a white border around the square + grid_image[y:y+square_size, x:x+1] = [255, 255, 255] # Left border + grid_image[y:y+square_size, x+square_size-1:x+square_size] = [255, 255, 255] # Right border + grid_image[y:y+1, x:x+square_size] = [255, 255, 255] # Top border + grid_image[y+square_size-1:y+square_size, x:x+square_size] = [255, 255, 255] # Bottom border + + square_index += 1 + + plt.figure(figsize=(10, 10)) + plt.imshow(grid_image) + plt.title('Image with Average colour Grids') + plt.axis('off') + plt.figure() + plt.imshow(image) + plt.show() + + def X2conv(in_channels, out_channels, inner_channels=None): inner_channels = out_channels // 2 if inner_channels is None else inner_channels down_conv = nn.Sequential( diff --git a/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py b/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py index 24db7b8cb..dc6f04349 100644 --- a/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py +++ b/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py @@ -1,4 +1,5 @@ import numpy as np +from lasr_vision_feature_extraction import split_and_sample_colours, colour_group_map, estimate_colour, possible_colours from lasr_vision_feature_extraction.categories_and_attributes import ( CategoriesAndAttributes, ) @@ -188,4 +189,33 @@ def describe(self) -> dict: # QUICK FIX that won't break the code result["dress"] = 0.0 + # ========= colour estimation below: ========= + # blurred_image = gaussian_blur(self.image, kernel_size=13, rep=3) + blurred_image = self.image + for cloth in ["top", "down", "outwear", "dress"]: + mask = self.masks[cloth] + # plt.imshow(mask) + # plt.show() + squares_colours, valid_squares = split_and_sample_colours(blurred_image, mask, 20) + # visualize_grids(blurred_image, squares_colours, square_size=20) + _squares_colours = {} + for k in squares_colours.keys(): + if k in valid_squares: + _squares_colours[k] = squares_colours[k] + squares_colours = {k: colour_group_map[colour] for k, colour in _squares_colours.items()} + squares_colours_count = {} + for k, colour in squares_colours.items(): + if colour not in squares_colours_count.keys(): + squares_colours_count[colour] = 1 + else: + squares_colours_count[colour] += 1 + print(squares_colours_count) + tag = cloth + "_colour" + result[tag] = {} + for k in possible_colours: + if k in squares_colours_count.keys(): + result[tag][k] = squares_colours_count[k] / len(squares_colours) + else: + result[tag][k] = 0.0 + return result From 6fd7234dec7504810db20874fc72f9ac69b67ab4 Mon Sep 17 00:00:00 2001 From: Benteng Ma Date: Tue, 9 Jul 2024 15:09:30 +0100 Subject: [PATCH 11/20] up to date to the main. --- common/third_party/leg_tracker | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/third_party/leg_tracker b/common/third_party/leg_tracker index 1f7c2a436..e6cbb2bba 160000 --- a/common/third_party/leg_tracker +++ b/common/third_party/leg_tracker @@ -1 +1 @@ -Subproject commit 1f7c2a43621e7cee319a2769537bca5d6b90f909 +Subproject commit e6cbb2bba218e0684714a08972a7bdfa99118e3c From fe353da637606e88f38908b3e79a79515fa07afb Mon Sep 17 00:00:00 2001 From: Benteng Ma Date: Tue, 9 Jul 2024 15:12:43 +0100 Subject: [PATCH 12/20] reformatted some files. --- .../cropped_detection.py | 1 - .../__init__.py | 113 +++++++++--------- .../image_with_masks_and_attributes.py | 15 ++- skills/src/lasr_skills/adjust_camera.py | 9 +- 4 files changed, 78 insertions(+), 60 deletions(-) diff --git a/common/vision/lasr_vision_cropped_detection/src/lasr_vision_cropped_detection/cropped_detection.py b/common/vision/lasr_vision_cropped_detection/src/lasr_vision_cropped_detection/cropped_detection.py index 0cba74b78..ad3c09f46 100644 --- a/common/vision/lasr_vision_cropped_detection/src/lasr_vision_cropped_detection/cropped_detection.py +++ b/common/vision/lasr_vision_cropped_detection/src/lasr_vision_cropped_detection/cropped_detection.py @@ -237,7 +237,6 @@ def _3d_mask_crop( else: raise ValueError(f"Invalid 3D crop_method: {crop_method}") - masked_images = [] unified_mask = np.zeros(rgb_image.shape).astype(rgb_image.dtype) diff --git a/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/__init__.py b/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/__init__.py index fc52c442e..a72e132ee 100644 --- a/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/__init__.py +++ b/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/__init__.py @@ -22,40 +22,38 @@ reference_colours = { "blue_very_light": np.array([240, 248, 255]), # Alice blue - "blue_light": np.array([173, 216, 230]), # Light blue - "blue_sky": np.array([135, 206, 235]), # Sky blue - "blue_powder": np.array([176, 224, 230]), # Powder blue - "blue_celeste": np.array([178, 255, 255]), # Celeste, very pale blue shade - "blue_periwinkle": np.array([204, 204, 255]), # Periwinkle, a mix of light blue and lavender - "blue_cadet": np.array([95, 158, 160]), # Cadet blue, a muted blue-green - "blue": np.array([0, 0, 255]), # Standard blue - "blue_royal": np.array([65, 105, 225]), # Royal blue - "blue_deep": np.array([0, 0, 139]), # Deep blue - "blue_dark": np.array([0, 0, 128]), # Dark blue + "blue_light": np.array([173, 216, 230]), # Light blue + "blue_sky": np.array([135, 206, 235]), # Sky blue + "blue_powder": np.array([176, 224, 230]), # Powder blue + "blue_celeste": np.array([178, 255, 255]), # Celeste, very pale blue shade + "blue_periwinkle": np.array( + [204, 204, 255] + ), # Periwinkle, a mix of light blue and lavender + "blue_cadet": np.array([95, 158, 160]), # Cadet blue, a muted blue-green + "blue": np.array([0, 0, 255]), # Standard blue + "blue_royal": np.array([65, 105, 225]), # Royal blue + "blue_deep": np.array([0, 0, 139]), # Deep blue + "blue_dark": np.array([0, 0, 128]), # Dark blue # "blue_navy": np.array([0, 0, 80]), # Navy blue - "yellow_very_light": np.array([255, 255, 204]), # Very light yellow - "yellow_light": np.array([255, 255, 224]), # Light yellow - "yellow": np.array([255, 255, 0]), # Standard yellow - "yellow_gold": np.array([255, 215, 0]), # Gold yellow - "yellow_dark": np.array([204, 204, 0]), # Dark yellow - "yellow_mustard": np.array([255, 219, 88]), # Mustard yellow - + "yellow_light": np.array([255, 255, 224]), # Light yellow + "yellow": np.array([255, 255, 0]), # Standard yellow + "yellow_gold": np.array([255, 215, 0]), # Gold yellow + "yellow_dark": np.array([204, 204, 0]), # Dark yellow + "yellow_mustard": np.array([255, 219, 88]), # Mustard yellow "red_very_light": np.array([255, 204, 204]), # Very light red - "red_light": np.array([255, 102, 102]), # Light red - "red": np.array([255, 0, 0]), # Standard red - "red_dark": np.array([139, 0, 0]), # Dark red - "red_maroon": np.array([128, 0, 0]), # Maroon - + "red_light": np.array([255, 102, 102]), # Light red + "red": np.array([255, 0, 0]), # Standard red + "red_dark": np.array([139, 0, 0]), # Dark red + "red_maroon": np.array([128, 0, 0]), # Maroon "orange_very_light": np.array([255, 229, 180]), # Very light orange - "orange_light": np.array([255, 179, 71]), # Light orange - "orange": np.array([255, 165, 0]), # Standard orange - "orange_dark": np.array([255, 140, 0]), # Dark orange - "orange_burnt": np.array([204, 85, 0]), # Burnt orange - + "orange_light": np.array([255, 179, 71]), # Light orange + "orange": np.array([255, 165, 0]), # Standard orange + "orange_dark": np.array([255, 140, 0]), # Dark orange + "orange_burnt": np.array([204, 85, 0]), # Burnt orange "black": np.array([30, 30, 30]), # Black "white": np.array([255, 255, 255]), # White - "gray": np.array([160, 160, 160]) # Gray + "gray": np.array([160, 160, 160]), # Gray } colour_group_map = { @@ -70,42 +68,41 @@ "blue_royal": "blue", "blue_deep": "blue", "blue_dark": "blue", - "yellow_very_light": "yellow", "yellow_light": "yellow", "yellow": "yellow", "yellow_gold": "yellow", "yellow_dark": "yellow", "yellow_mustard": "yellow", - "red_very_light": "red", "red_light": "red", "red": "red", "red_dark": "red", "red_maroon": "red", - "orange_very_light": "orange", "orange_light": "orange", "orange": "orange", "orange_dark": "orange", "orange_burnt": "orange", - "black": "black", "white": "white", - "gray": "gray" + "gray": "gray", } possible_colours = ["blue", "yellow", "red", "orange", "black", "white", "gray"] -def estimate_colour(rgb_array): +def estimate_colour(rgb_array): # Calculate distances to each reference colour # distances = {colour: cie76_distance(rgb_array, ref_rgb) for colour, ref_rgb in reference_colours.items()} - distances = {colour: np.linalg.norm(rgb_array - ref_rgb) for colour, ref_rgb in reference_colours.items()} - + distances = { + colour: np.linalg.norm(rgb_array - ref_rgb) + for colour, ref_rgb in reference_colours.items() + } + # Find the colour with the smallest distance estimated_colour = min(distances, key=distances.get) - + return estimated_colour @@ -113,28 +110,28 @@ def split_and_sample_colours(image, mask, square_size): height, width, _ = image.shape squares_colours = {} valid_squares = set() - + square_index = 0 - + for y in range(0, height, square_size): for x in range(0, width, square_size): - square = image[y:y + square_size, x:x + square_size] - mask_square = mask[y:y + square_size, x:x + square_size] - + square = image[y : y + square_size, x : x + square_size] + mask_square = mask[y : y + square_size, x : x + square_size] + # Calculate the average colour average_colour = square.mean(axis=(0, 1)) - + # Save the average colour in the dictionary squares_colours[square_index] = estimate_colour(average_colour) # squares_colours[square_index] = average_colour # estimate_colour(average_colour) - + # Check the mask condition a = np.sum(mask_square) if np.sum(mask_square) > 0.5 * square_size * square_size: valid_squares.add(square_index) - + square_index += 1 - + return squares_colours, valid_squares @@ -151,7 +148,7 @@ def gaussian_blur(image, kernel_size, rep=3): """ if kernel_size % 2 == 0: kernel_size += 1 # Increment kernel size to make it odd if it's even - + for _ in range(rep): image = cv2.GaussianBlur(image, (kernel_size, kernel_size), 0) return image @@ -175,20 +172,28 @@ def visualize_grids(image, squares_colours, square_size): # Get the colour from the dictionary colour = np.array(reference_colours[squares_colours[square_index]]) # Fill the square using numpy slicing - grid_image[y:y + square_size, x:x + square_size] = colour + grid_image[y : y + square_size, x : x + square_size] = colour # Optionally draw a white border around the square - grid_image[y:y+square_size, x:x+1] = [255, 255, 255] # Left border - grid_image[y:y+square_size, x+square_size-1:x+square_size] = [255, 255, 255] # Right border - grid_image[y:y+1, x:x+square_size] = [255, 255, 255] # Top border - grid_image[y+square_size-1:y+square_size, x:x+square_size] = [255, 255, 255] # Bottom border + grid_image[y : y + square_size, x : x + 1] = [255, 255, 255] # Left border + grid_image[y : y + square_size, x + square_size - 1 : x + square_size] = [ + 255, + 255, + 255, + ] # Right border + grid_image[y : y + 1, x : x + square_size] = [255, 255, 255] # Top border + grid_image[y + square_size - 1 : y + square_size, x : x + square_size] = [ + 255, + 255, + 255, + ] # Bottom border square_index += 1 plt.figure(figsize=(10, 10)) plt.imshow(grid_image) - plt.title('Image with Average colour Grids') - plt.axis('off') + plt.title("Image with Average colour Grids") + plt.axis("off") plt.figure() plt.imshow(image) plt.show() diff --git a/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py b/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py index dc6f04349..6ecf24065 100644 --- a/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py +++ b/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py @@ -1,5 +1,10 @@ import numpy as np -from lasr_vision_feature_extraction import split_and_sample_colours, colour_group_map, estimate_colour, possible_colours +from lasr_vision_feature_extraction import ( + split_and_sample_colours, + colour_group_map, + estimate_colour, + possible_colours, +) from lasr_vision_feature_extraction.categories_and_attributes import ( CategoriesAndAttributes, ) @@ -196,13 +201,17 @@ def describe(self) -> dict: mask = self.masks[cloth] # plt.imshow(mask) # plt.show() - squares_colours, valid_squares = split_and_sample_colours(blurred_image, mask, 20) + squares_colours, valid_squares = split_and_sample_colours( + blurred_image, mask, 20 + ) # visualize_grids(blurred_image, squares_colours, square_size=20) _squares_colours = {} for k in squares_colours.keys(): if k in valid_squares: _squares_colours[k] = squares_colours[k] - squares_colours = {k: colour_group_map[colour] for k, colour in _squares_colours.items()} + squares_colours = { + k: colour_group_map[colour] for k, colour in _squares_colours.items() + } squares_colours_count = {} for k, colour in squares_colours.items(): if colour not in squares_colours_count.keys(): diff --git a/skills/src/lasr_skills/adjust_camera.py b/skills/src/lasr_skills/adjust_camera.py index 850a039b6..755948841 100644 --- a/skills/src/lasr_skills/adjust_camera.py +++ b/skills/src/lasr_skills/adjust_camera.py @@ -371,7 +371,10 @@ def execute(self, userdata): (eyes_middle[1] + shoulders_middle[1]) / 2, ) rospy.logwarn(f"very middle {very_middle}") - eyes_to_shoulders_distance = math.sqrt((eyes_middle[0] - shoulders_middle[0]) ** 2 + (eyes_middle[1] - shoulders_middle[1])) + eyes_to_shoulders_distance = math.sqrt( + (eyes_middle[0] - shoulders_middle[0]) ** 2 + + (eyes_middle[1] - shoulders_middle[1]) + ) rospy.logwarn(f"eyes to shoulder distance {eyes_to_shoulders_distance}") if eyes_to_shoulders_distance > 0.14: # person is kind of close to the camera, @@ -394,7 +397,9 @@ def execute(self, userdata): print("if y at upper 1/7 for eyes: move up 1 step") elif eyes_middle[1] >= 2 / 9: self.position[0] -= 1 - print("if y lower than upper 2/9 for eyes: move down 1 step") + print( + "if y lower than upper 2/9 for eyes: move down 1 step" + ) else: if ( 1 / 4 <= very_middle[1] <= 1 / 2 From c46782321f46b1e36991d3510c8a9fc6aa5c5b03 Mon Sep 17 00:00:00 2001 From: Benteng Ma Date: Tue, 9 Jul 2024 18:36:44 +0100 Subject: [PATCH 13/20] correct the launch file, didn't find any issues that cause adjust camera not truncated? --- skills/launch/unit_test_describe_people.launch | 2 +- skills/src/lasr_skills/adjust_camera.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/skills/launch/unit_test_describe_people.launch b/skills/launch/unit_test_describe_people.launch index f1b76530e..417878ef5 100644 --- a/skills/launch/unit_test_describe_people.launch +++ b/skills/launch/unit_test_describe_people.launch @@ -3,7 +3,7 @@ - + diff --git a/skills/src/lasr_skills/adjust_camera.py b/skills/src/lasr_skills/adjust_camera.py index 755948841..4605c6513 100644 --- a/skills/src/lasr_skills/adjust_camera.py +++ b/skills/src/lasr_skills/adjust_camera.py @@ -180,7 +180,7 @@ def __init__( self, bodypix_model: str = "resnet50", bodypix_confidence: float = 0.7, - max_attempts=1000, + max_attempts=5, init_state="u1m", ): smach.State.__init__( From 0fd439d240114819caaa5cd81fc62c9eec681021 Mon Sep 17 00:00:00 2001 From: Benteng Ma Date: Tue, 9 Jul 2024 18:51:54 +0100 Subject: [PATCH 14/20] Added some test information outputs, seems everything is fine. Shouldn't be this slow though. --- skills/scripts/unit_test_adjust_camera.py | 4 ++-- skills/src/lasr_skills/adjust_camera.py | 1 + 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/skills/scripts/unit_test_adjust_camera.py b/skills/scripts/unit_test_adjust_camera.py index 8270b5d59..3fcd68d91 100644 --- a/skills/scripts/unit_test_adjust_camera.py +++ b/skills/scripts/unit_test_adjust_camera.py @@ -12,8 +12,8 @@ sm.add( "AdjustCamera", AdjustCamera( - max_attempts=1000, - debug=True, + max_attempts=3, + debug=False, init_state="u1m", ), transitions={"finished": "end", "failed": "end", "truncated": "end"}, diff --git a/skills/src/lasr_skills/adjust_camera.py b/skills/src/lasr_skills/adjust_camera.py index 4605c6513..46057e4ff 100644 --- a/skills/src/lasr_skills/adjust_camera.py +++ b/skills/src/lasr_skills/adjust_camera.py @@ -207,6 +207,7 @@ def __init__( self.counter = 0 def execute(self, userdata): + rospy.logwarn(f"Start attempt number {self.counter}.") req = BodyPixKeypointDetectionRequest() req.image_raw = userdata.img_msg req.dataset = self._bodypix_model From aab29061d58b5cafa322e9128a43a3dcab29070f Mon Sep 17 00:00:00 2001 From: Benteng Ma Date: Tue, 9 Jul 2024 19:37:38 +0100 Subject: [PATCH 15/20] fixed a few issue that makes the service die. --- .../__init__.py | 161 ------------------ .../image_with_masks_and_attributes.py | 128 ++++++++++++-- 2 files changed, 117 insertions(+), 172 deletions(-) diff --git a/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/__init__.py b/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/__init__.py index a72e132ee..ae9423e95 100644 --- a/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/__init__.py +++ b/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/__init__.py @@ -1,6 +1,5 @@ import json from os import path -import matplotlib.pyplot as plt import cv2 import numpy as np import rospkg @@ -20,121 +19,6 @@ ) -reference_colours = { - "blue_very_light": np.array([240, 248, 255]), # Alice blue - "blue_light": np.array([173, 216, 230]), # Light blue - "blue_sky": np.array([135, 206, 235]), # Sky blue - "blue_powder": np.array([176, 224, 230]), # Powder blue - "blue_celeste": np.array([178, 255, 255]), # Celeste, very pale blue shade - "blue_periwinkle": np.array( - [204, 204, 255] - ), # Periwinkle, a mix of light blue and lavender - "blue_cadet": np.array([95, 158, 160]), # Cadet blue, a muted blue-green - "blue": np.array([0, 0, 255]), # Standard blue - "blue_royal": np.array([65, 105, 225]), # Royal blue - "blue_deep": np.array([0, 0, 139]), # Deep blue - "blue_dark": np.array([0, 0, 128]), # Dark blue - # "blue_navy": np.array([0, 0, 80]), # Navy blue - "yellow_very_light": np.array([255, 255, 204]), # Very light yellow - "yellow_light": np.array([255, 255, 224]), # Light yellow - "yellow": np.array([255, 255, 0]), # Standard yellow - "yellow_gold": np.array([255, 215, 0]), # Gold yellow - "yellow_dark": np.array([204, 204, 0]), # Dark yellow - "yellow_mustard": np.array([255, 219, 88]), # Mustard yellow - "red_very_light": np.array([255, 204, 204]), # Very light red - "red_light": np.array([255, 102, 102]), # Light red - "red": np.array([255, 0, 0]), # Standard red - "red_dark": np.array([139, 0, 0]), # Dark red - "red_maroon": np.array([128, 0, 0]), # Maroon - "orange_very_light": np.array([255, 229, 180]), # Very light orange - "orange_light": np.array([255, 179, 71]), # Light orange - "orange": np.array([255, 165, 0]), # Standard orange - "orange_dark": np.array([255, 140, 0]), # Dark orange - "orange_burnt": np.array([204, 85, 0]), # Burnt orange - "black": np.array([30, 30, 30]), # Black - "white": np.array([255, 255, 255]), # White - "gray": np.array([160, 160, 160]), # Gray -} - -colour_group_map = { - "blue_very_light": "blue", - "blue_light": "blue", - "blue_sky": "blue", - "blue_powder": "blue", - "blue_celeste": "blue", - "blue_periwinkle": "blue", - "blue_cadet": "blue", - "blue": "blue", - "blue_royal": "blue", - "blue_deep": "blue", - "blue_dark": "blue", - "yellow_very_light": "yellow", - "yellow_light": "yellow", - "yellow": "yellow", - "yellow_gold": "yellow", - "yellow_dark": "yellow", - "yellow_mustard": "yellow", - "red_very_light": "red", - "red_light": "red", - "red": "red", - "red_dark": "red", - "red_maroon": "red", - "orange_very_light": "orange", - "orange_light": "orange", - "orange": "orange", - "orange_dark": "orange", - "orange_burnt": "orange", - "black": "black", - "white": "white", - "gray": "gray", -} - -possible_colours = ["blue", "yellow", "red", "orange", "black", "white", "gray"] - - -def estimate_colour(rgb_array): - # Calculate distances to each reference colour - # distances = {colour: cie76_distance(rgb_array, ref_rgb) for colour, ref_rgb in reference_colours.items()} - distances = { - colour: np.linalg.norm(rgb_array - ref_rgb) - for colour, ref_rgb in reference_colours.items() - } - - # Find the colour with the smallest distance - estimated_colour = min(distances, key=distances.get) - - return estimated_colour - - -def split_and_sample_colours(image, mask, square_size): - height, width, _ = image.shape - squares_colours = {} - valid_squares = set() - - square_index = 0 - - for y in range(0, height, square_size): - for x in range(0, width, square_size): - square = image[y : y + square_size, x : x + square_size] - mask_square = mask[y : y + square_size, x : x + square_size] - - # Calculate the average colour - average_colour = square.mean(axis=(0, 1)) - - # Save the average colour in the dictionary - squares_colours[square_index] = estimate_colour(average_colour) - # squares_colours[square_index] = average_colour # estimate_colour(average_colour) - - # Check the mask condition - a = np.sum(mask_square) - if np.sum(mask_square) > 0.5 * square_size * square_size: - valid_squares.add(square_index) - - square_index += 1 - - return squares_colours, valid_squares - - def gaussian_blur(image, kernel_size, rep=3): """ Apply Gaussian blur to an RGB image. @@ -154,51 +38,6 @@ def gaussian_blur(image, kernel_size, rep=3): return image -def visualize_grids(image, squares_colours, square_size): - """ - Display the image with coloured grids based on average colours. - - Parameters: - image (numpy.ndarray): The original RGB image. - squares_colours (dict): Dictionary with the average colours of each square. - square_size (int): The size of each square. - """ - height, width, _ = image.shape - grid_image = np.zeros((height, width, 3), dtype=np.uint8) - - square_index = 0 - for y in range(0, height, square_size): - for x in range(0, width, square_size): - # Get the colour from the dictionary - colour = np.array(reference_colours[squares_colours[square_index]]) - # Fill the square using numpy slicing - grid_image[y : y + square_size, x : x + square_size] = colour - - # Optionally draw a white border around the square - grid_image[y : y + square_size, x : x + 1] = [255, 255, 255] # Left border - grid_image[y : y + square_size, x + square_size - 1 : x + square_size] = [ - 255, - 255, - 255, - ] # Right border - grid_image[y : y + 1, x : x + square_size] = [255, 255, 255] # Top border - grid_image[y + square_size - 1 : y + square_size, x : x + square_size] = [ - 255, - 255, - 255, - ] # Bottom border - - square_index += 1 - - plt.figure(figsize=(10, 10)) - plt.imshow(grid_image) - plt.title("Image with Average colour Grids") - plt.axis("off") - plt.figure() - plt.imshow(image) - plt.show() - - def X2conv(in_channels, out_channels, inner_channels=None): inner_channels = out_channels // 2 if inner_channels is None else inner_channels down_conv = nn.Sequential( diff --git a/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py b/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py index 6ecf24065..014da92b2 100644 --- a/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py +++ b/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py @@ -1,14 +1,122 @@ import numpy as np -from lasr_vision_feature_extraction import ( - split_and_sample_colours, - colour_group_map, - estimate_colour, - possible_colours, -) from lasr_vision_feature_extraction.categories_and_attributes import ( CategoriesAndAttributes, ) +reference_colours = { + "blue_very_light": np.array([240, 248, 255]), # Alice blue + "blue_light": np.array([173, 216, 230]), # Light blue + "blue_sky": np.array([135, 206, 235]), # Sky blue + "blue_powder": np.array([176, 224, 230]), # Powder blue + "blue_celeste": np.array([178, 255, 255]), # Celeste, very pale blue shade + "blue_periwinkle": np.array( + [204, 204, 255] + ), # Periwinkle, a mix of light blue and lavender + "blue_cadet": np.array([95, 158, 160]), # Cadet blue, a muted blue-green + "blue": np.array([0, 0, 255]), # Standard blue + "blue_royal": np.array([65, 105, 225]), # Royal blue + "blue_deep": np.array([0, 0, 139]), # Deep blue + "blue_dark": np.array([0, 0, 128]), # Dark blue + # "blue_navy": np.array([0, 0, 80]), # Navy blue + "yellow_very_light": np.array([255, 255, 204]), # Very light yellow + "yellow_light": np.array([255, 255, 224]), # Light yellow + "yellow": np.array([255, 255, 0]), # Standard yellow + "yellow_gold": np.array([255, 215, 0]), # Gold yellow + "yellow_dark": np.array([204, 204, 0]), # Dark yellow + "yellow_mustard": np.array([255, 219, 88]), # Mustard yellow + "red_very_light": np.array([255, 204, 204]), # Very light red + "red_light": np.array([255, 102, 102]), # Light red + "red": np.array([255, 0, 0]), # Standard red + "red_dark": np.array([139, 0, 0]), # Dark red + "red_maroon": np.array([128, 0, 0]), # Maroon + "orange_very_light": np.array([255, 229, 180]), # Very light orange + "orange_light": np.array([255, 179, 71]), # Light orange + "orange": np.array([255, 165, 0]), # Standard orange + "orange_dark": np.array([255, 140, 0]), # Dark orange + "orange_burnt": np.array([204, 85, 0]), # Burnt orange + "black": np.array([30, 30, 30]), # Black + "white": np.array([255, 255, 255]), # White + "gray": np.array([160, 160, 160]), # Gray +} + +colour_group_map = { + "blue_very_light": "blue", + "blue_light": "blue", + "blue_sky": "blue", + "blue_powder": "blue", + "blue_celeste": "blue", + "blue_periwinkle": "blue", + "blue_cadet": "blue", + "blue": "blue", + "blue_royal": "blue", + "blue_deep": "blue", + "blue_dark": "blue", + "yellow_very_light": "yellow", + "yellow_light": "yellow", + "yellow": "yellow", + "yellow_gold": "yellow", + "yellow_dark": "yellow", + "yellow_mustard": "yellow", + "red_very_light": "red", + "red_light": "red", + "red": "red", + "red_dark": "red", + "red_maroon": "red", + "orange_very_light": "orange", + "orange_light": "orange", + "orange": "orange", + "orange_dark": "orange", + "orange_burnt": "orange", + "black": "black", + "white": "white", + "gray": "gray", +} + +possible_colours = ["blue", "yellow", "red", "orange", "black", "white", "gray"] + + +def estimate_colour(rgb_array): + # Calculate distances to each reference colour + # distances = {colour: cie76_distance(rgb_array, ref_rgb) for colour, ref_rgb in reference_colours.items()} + distances = { + colour: np.linalg.norm(rgb_array - ref_rgb) + for colour, ref_rgb in reference_colours.items() + } + + # Find the colour with the smallest distance + estimated_colour = min(distances, key=distances.get) + + return estimated_colour + + +def split_and_sample_colours(image, mask, square_size): + height, width, _ = image.shape + squares_colours = {} + valid_squares = set() + + square_index = 0 + + for y in range(0, height, square_size): + for x in range(0, width, square_size): + square = image[y : y + square_size, x : x + square_size] + mask_square = mask[y : y + square_size, x : x + square_size] + + # Calculate the average colour + average_colour = square.mean(axis=(0, 1)) + + # Save the average colour in the dictionary + squares_colours[square_index] = estimate_colour(average_colour) + # squares_colours[square_index] = average_colour # estimate_colour(average_colour) + + # Check the mask condition + a = np.sum(mask_square) + if np.sum(mask_square) > 0.5 * square_size * square_size: + valid_squares.add(square_index) + + square_index += 1 + + return squares_colours, valid_squares + def _softmax(x: list[float]) -> list[float]: """Compute softmax values for each set of scores in x without using NumPy.""" @@ -191,20 +299,18 @@ def describe(self) -> dict: max_attribute = "sleeveless dress" result["max_dress"] = max_attribute - # QUICK FIX that won't break the code + # QUICK FIX that won't break the code but not returning dress anymore. result["dress"] = 0.0 # ========= colour estimation below: ========= + # blurred_imagge kept here so that we can quickly make it work if we need. # blurred_image = gaussian_blur(self.image, kernel_size=13, rep=3) blurred_image = self.image - for cloth in ["top", "down", "outwear", "dress"]: + for cloth in ["top", "outwear", "dress"]: # "down", mask = self.masks[cloth] - # plt.imshow(mask) - # plt.show() squares_colours, valid_squares = split_and_sample_colours( blurred_image, mask, 20 ) - # visualize_grids(blurred_image, squares_colours, square_size=20) _squares_colours = {} for k in squares_colours.keys(): if k in valid_squares: From 471ee6018e633fb288331be37372d23058f3ca58 Mon Sep 17 00:00:00 2001 From: Benteng Ma Date: Wed, 10 Jul 2024 14:39:44 +0100 Subject: [PATCH 16/20] reformatted --- tasks/gpsr/src/gpsr/states/object_comparison.py | 8 ++++++-- .../src/receptionist/states/get_name_and_drink.py | 2 ++ .../src/receptionist/states/local_speech_recognition.py | 2 ++ 3 files changed, 10 insertions(+), 2 deletions(-) diff --git a/tasks/gpsr/src/gpsr/states/object_comparison.py b/tasks/gpsr/src/gpsr/states/object_comparison.py index 1ef947885..e169bfb6c 100755 --- a/tasks/gpsr/src/gpsr/states/object_comparison.py +++ b/tasks/gpsr/src/gpsr/states/object_comparison.py @@ -57,7 +57,9 @@ def count_category(self, object_dictionary, count_object): def execute(self, userdata): detected_objects = userdata.object_dict counts = self.count_category(self.object_weight, detected_objects) - category_counts = {key: value for key, value in counts.items() if value != 0} + category_counts = { + key: value for key, value in counts.items() if value != 0 + } userdata.category_dict = category_counts userdata.detections_categories = list(category_counts.keys()) return "succeeded" @@ -149,7 +151,7 @@ def __init__(self): "sorted_size", "sorted_weights", ], - output_keys=["say_text"] + output_keys=["say_text"], ) def execute(self, userdata): @@ -250,6 +252,8 @@ def __init__( transitions={"succeeded": "succeeded", "failed": "failed"}, remapping={"text": "say_text"}, ) + + # if __name__ == "__main__": # import rospy # from sensor_msgs.msg import PointCloud2 diff --git a/tasks/receptionist/src/receptionist/states/get_name_and_drink.py b/tasks/receptionist/src/receptionist/states/get_name_and_drink.py index a9c81430a..b20bd862e 100644 --- a/tasks/receptionist/src/receptionist/states/get_name_and_drink.py +++ b/tasks/receptionist/src/receptionist/states/get_name_and_drink.py @@ -77,6 +77,7 @@ def execute(self, userdata: UserData) -> str: outcome = "failed" return outcome + class PostRecoveryDecision(smach.State): def __init__( self, @@ -93,6 +94,7 @@ def __init__( prior_data: Dict[str, List[str]] = rospy.get_param(param_key) self._possible_names = [name.lower() for name in prior_data["names"]] self._possible_drinks = [drink.lower() for drink in prior_data["drinks"]] + def execute(self, userdata: UserData) -> str: if not self._recovery_name_and_drink_required(userdata): if userdata.guest_data[self._guest_id]["name"] == "unknown": diff --git a/tasks/receptionist/src/receptionist/states/local_speech_recognition.py b/tasks/receptionist/src/receptionist/states/local_speech_recognition.py index 6c270660b..5f79f4d83 100644 --- a/tasks/receptionist/src/receptionist/states/local_speech_recognition.py +++ b/tasks/receptionist/src/receptionist/states/local_speech_recognition.py @@ -60,6 +60,7 @@ def speech_recovery(sentence): print(f"final name: {handle_name(sentence_list, True)}") print(f"final drink: {handle_drink(sentence_list, True)}") + def handle_name(sentence_list, last_resort): result = handle_similar_spelt(sentence_list, available_names, 1) if result != "unknown": @@ -74,6 +75,7 @@ def handle_name(sentence_list, last_resort): print("Last resort name") return handle_closest_spelt(sentence_list, available_names) + def handle_drink(sentence_list, last_resort): result = infer_second_drink(sentence_list) if result != "unknown": From 0b2a69cac1bbe7780a57a2d1c9d8bdf37eac8809 Mon Sep 17 00:00:00 2001 From: Benteng Ma Date: Tue, 5 Dec 2023 18:44:38 +0000 Subject: [PATCH 17/20] Test --- common/vision/lasr_vision_torch/nodes/service | 70 +++++++++++++++++++ 1 file changed, 70 insertions(+) create mode 100644 common/vision/lasr_vision_torch/nodes/service diff --git a/common/vision/lasr_vision_torch/nodes/service b/common/vision/lasr_vision_torch/nodes/service new file mode 100644 index 000000000..ab8705380 --- /dev/null +++ b/common/vision/lasr_vision_torch/nodes/service @@ -0,0 +1,70 @@ +from lasr_vision_msgs.srv import TorchFaceFeatureDetection, TorchFaceFeatureDetectionRequest, TorchFaceFeatureDetectionResponse +from lasr_vision_msgs.msg import FeatureWithColour, ColourPrediction +from colour_estimation import closest_colours, RGB_COLOURS, RGB_HAIR_COLOURS +from cv2_img import msg_to_cv2_img +from torch_module.helpers import binary_erosion_dilation, median_color_float + +import numpy as np +import torch +import rospy +import lasr_vision_torch + +model = lasr_vision_torch.load_face_classifier_model() + + +def detect(request: TorchFaceFeatureDetectionRequest) -> TorchFaceFeatureDetectionResponse: + # decode the image + rospy.loginfo('Decoding') + frame = msg_to_cv2_img(request.image_raw) + + # 'hair', 'hat', 'glasses', 'face' + input_image = torch.from_numpy(frame).permute(2, 0, 1).unsqueeze(0).float() + input_image /= 255.0 + masks_batch_pred, pred_classes = model(input_image) + + thresholds_mask = [ + 0.5, 0.75, 0.25, 0.5, # 0.5, 0.5, 0.5, 0.5, + ] + thresholds_pred = [ + 0.6, 0.8, 0.1, 0.5, + ] + erosion_iterations = 1 + dilation_iterations = 1 + categories = ['hair', 'hat', 'glasses', 'face',] + + masks_batch_pred = binary_erosion_dilation( + masks_batch_pred, thresholds=thresholds_mask, + erosion_iterations=erosion_iterations, dilation_iterations=dilation_iterations + ) + + median_colours = (median_color_float( + input_image, masks_batch_pred).detach().squeeze(0)*255).numpy().astype(np.uint8) + + # discarded: masks = masks_batch_pred.detach().squeeze(0).numpy().astype(np.uint8) + # discarded: mask_list = [masks[i,:,:] for i in range(masks.shape[0])] + + pred_classes = pred_classes.detach().squeeze(0).numpy() + # discarded: class_list = [categories[i] for i in range( + # pred_classes.shape[0]) if pred_classes[i].item() > thresholds_pred[i]] + colour_list = [median_colours[i, :] + for i in range(median_colours.shape[0])] + + response = TorchFaceFeatureDetectionResponse() + response.detected_features = [ + FeatureWithColour(categories[i], [ + ColourPrediction(colour, distance) + for colour, distance + in closest_colours(colour_list[i], RGB_HAIR_COLOURS if categories[i] == 'hair' else RGB_COLOURS) + ]) + for i + in range(pred_classes.shape[0]) + if pred_classes[i].item() > thresholds_pred[i] + ] + + return response +#test test + +rospy.init_node('torch_service') +rospy.Service('/torch/detect/face_features', TorchFaceFeatureDetection, detect) +rospy.loginfo('Torch service started') +rospy.spin() From a1486f46f3398e7b4dd47e3151f5af5d82be8c20 Mon Sep 17 00:00:00 2001 From: Benteng Ma Date: Wed, 6 Dec 2023 17:02:13 +0000 Subject: [PATCH 18/20] Message Change --- common/helpers/numpy2message/src/__init__.py | 19 ++++++ common/vision/lasr_vision_torch/nodes/service | 62 ++++++++++++++++++- skills/scripts/unit_test_describe_people.py | 2 +- 3 files changed, 81 insertions(+), 2 deletions(-) create mode 100644 common/helpers/numpy2message/src/__init__.py diff --git a/common/helpers/numpy2message/src/__init__.py b/common/helpers/numpy2message/src/__init__.py new file mode 100644 index 000000000..3ddad23fe --- /dev/null +++ b/common/helpers/numpy2message/src/__init__.py @@ -0,0 +1,19 @@ +import numpy as np + + +def numpy2message(np_array: np.ndarray) -> list[bytes, list[int], str]: + data = np_array.tobytes() + shape = list(np_array.shape) + dtype = str(np_array.dtype) + return data, shape, dtype + + +def message2numpy(data:bytes, shape:list[int], dtype:str) -> np.ndarray: + array_shape = tuple(shape) + array_dtype = np.dtype(dtype) + + deserialized_array = np.frombuffer(data, dtype=array_dtype) + deserialized_array = deserialized_array.reshape(array_shape) + + return deserialized_array + diff --git a/common/vision/lasr_vision_torch/nodes/service b/common/vision/lasr_vision_torch/nodes/service index ab8705380..e09829dae 100644 --- a/common/vision/lasr_vision_torch/nodes/service +++ b/common/vision/lasr_vision_torch/nodes/service @@ -3,12 +3,14 @@ from lasr_vision_msgs.msg import FeatureWithColour, ColourPrediction from colour_estimation import closest_colours, RGB_COLOURS, RGB_HAIR_COLOURS from cv2_img import msg_to_cv2_img from torch_module.helpers import binary_erosion_dilation, median_color_float +from numpy2message import message2numpy import numpy as np import torch import rospy import lasr_vision_torch + model = lasr_vision_torch.load_face_classifier_model() @@ -16,6 +18,12 @@ def detect(request: TorchFaceFeatureDetectionRequest) -> TorchFaceFeatureDetecti # decode the image rospy.loginfo('Decoding') frame = msg_to_cv2_img(request.image_raw) + torso_mask_data, torso_mask_shape, torso_mask_dtype = request.torso_mask_data, request.torso_mask_shape, request.torso_mask_dtype + head_mask_data, head_mask_shape, head_mask_dtype = request.head_mask_data, request.head_mask_shape, request.head_mask_dtype + torsal_mask = message2numpy(torso_mask_data, torso_mask_shape, torso_mask_dtype) + head_mask = message2numpy(head_mask_data, head_mask_shape, head_mask_dtype) + print(torso_mask_shape) + print(head_mask_shape) # 'hair', 'hat', 'glasses', 'face' input_image = torch.from_numpy(frame).permute(2, 0, 1).unsqueeze(0).float() @@ -62,7 +70,59 @@ def detect(request: TorchFaceFeatureDetectionRequest) -> TorchFaceFeatureDetecti ] return response -#test test + + +# def detect(request: TorchFaceFeatureDetectionRequest) -> TorchFaceFeatureDetectionResponse: +# # decode the image +# rospy.loginfo('Decoding') +# frame = msg_to_cv2_img(request.image_raw) + +# # 'hair', 'hat', 'glasses', 'face' +# input_image = torch.from_numpy(frame).permute(2, 0, 1).unsqueeze(0).float() +# input_image /= 255.0 +# masks_batch_pred, pred_classes = model(input_image) + +# thresholds_mask = [ +# 0.5, 0.75, 0.25, 0.5, # 0.5, 0.5, 0.5, 0.5, +# ] +# thresholds_pred = [ +# 0.6, 0.8, 0.1, 0.5, +# ] +# erosion_iterations = 1 +# dilation_iterations = 1 +# categories = ['hair', 'hat', 'glasses', 'face',] + +# masks_batch_pred = binary_erosion_dilation( +# masks_batch_pred, thresholds=thresholds_mask, +# erosion_iterations=erosion_iterations, dilation_iterations=dilation_iterations +# ) + +# median_colours = (median_color_float( +# input_image, masks_batch_pred).detach().squeeze(0)*255).numpy().astype(np.uint8) + +# # discarded: masks = masks_batch_pred.detach().squeeze(0).numpy().astype(np.uint8) +# # discarded: mask_list = [masks[i,:,:] for i in range(masks.shape[0])] + +# pred_classes = pred_classes.detach().squeeze(0).numpy() +# # discarded: class_list = [categories[i] for i in range( +# # pred_classes.shape[0]) if pred_classes[i].item() > thresholds_pred[i]] +# colour_list = [median_colours[i, :] +# for i in range(median_colours.shape[0])] + +# response = TorchFaceFeatureDetectionResponse() +# response.detected_features = [ +# FeatureWithColour(categories[i], [ +# ColourPrediction(colour, distance) +# for colour, distance +# in closest_colours(colour_list[i], RGB_HAIR_COLOURS if categories[i] == 'hair' else RGB_COLOURS) +# ]) +# for i +# in range(pred_classes.shape[0]) +# if pred_classes[i].item() > thresholds_pred[i] +# ] + +# return response +# test test rospy.init_node('torch_service') rospy.Service('/torch/detect/face_features', TorchFaceFeatureDetection, detect) diff --git a/skills/scripts/unit_test_describe_people.py b/skills/scripts/unit_test_describe_people.py index f85220d52..85178d044 100644 --- a/skills/scripts/unit_test_describe_people.py +++ b/skills/scripts/unit_test_describe_people.py @@ -19,6 +19,6 @@ sm.execute() - print("\n\nDetected people:", sm.userdata["people"][0]["features"]) + # print('\n\nDetected people:', sm.userdata['people']) rospy.signal_shutdown("down") From 7c749c083be6638bd7efe3644c730fbdd6e8dec3 Mon Sep 17 00:00:00 2001 From: Benteng Ma Date: Mon, 15 Jul 2024 13:09:15 +0100 Subject: [PATCH 19/20] started colour estimation of cloth skill --- skills/src/lasr_skills/estimate_cloth_colour.py | 5 +++++ 1 file changed, 5 insertions(+) create mode 100644 skills/src/lasr_skills/estimate_cloth_colour.py diff --git a/skills/src/lasr_skills/estimate_cloth_colour.py b/skills/src/lasr_skills/estimate_cloth_colour.py new file mode 100644 index 000000000..f29c51ce6 --- /dev/null +++ b/skills/src/lasr_skills/estimate_cloth_colour.py @@ -0,0 +1,5 @@ +import smach + + +class EstimateClothColour(smach.StateMachine): + pass From 917e14fe1f4abc36ab219339814dbb26499190e4 Mon Sep 17 00:00:00 2001 From: Benteng Ma Date: Mon, 15 Jul 2024 15:11:54 +0100 Subject: [PATCH 20/20] Added detect clothing implementation. --- .../image_with_masks_and_attributes.py | 2 +- skills/src/lasr_skills/detect_clothing.py | 113 +++++++++++++++++- .../src/lasr_skills/estimate_cloth_colour.py | 5 - 3 files changed, 108 insertions(+), 12 deletions(-) delete mode 100644 skills/src/lasr_skills/estimate_cloth_colour.py diff --git a/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py b/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py index 014da92b2..4c37fccc5 100644 --- a/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py +++ b/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py @@ -325,7 +325,7 @@ def describe(self) -> dict: else: squares_colours_count[colour] += 1 print(squares_colours_count) - tag = cloth + "_colour" + tag = cloth + " colour" result[tag] = {} for k in possible_colours: if k in squares_colours_count.keys(): diff --git a/skills/src/lasr_skills/detect_clothing.py b/skills/src/lasr_skills/detect_clothing.py index 0c599905b..cc5a7385d 100644 --- a/skills/src/lasr_skills/detect_clothing.py +++ b/skills/src/lasr_skills/detect_clothing.py @@ -1,18 +1,119 @@ import smach +import rospy from typing import Optional +from lasr_vision_msgs.srv import ( + TorchFaceFeatureDetectionDescription, +) +from lasr_skills import DescribePeople -class DetectClothing(smach.State): +colour_list = ["blue", "yellow", "black", "white", "red", "orange", "gray"] +cloth_list = ["t shirt", "shirt", "blouse", "sweater", "coat", "jacket"] +cloth_type_map = { + "t shirt": "short sleeve top", + "shirt": "long sleeve top", + "blouse": "long sleeve top", + "sweater": "long sleeve top", + "coat": "long sleeve outwear", + "jacket": "long sleeve outwear", +} +cloth_type_rough_map = { + "short sleeve top": "top", + "long sleeve top": "top", + "long sleeve outwear": "outwear", +} + - _clothing_to_detect: Optional[str] +class DetectClothing(smach.StateMachine): def __init__(self, clothing_to_detect: Optional[str] = None): + """ + clothing_to_detect: "blue shirt" + """ smach.State.__init__( - self, outcomes=["succeeded", "failed"], input_keys=["img_msg"] + self, outcomes=["succeeded", "failed"], input_keys=["img_msg"], ) - self._clothing_to_detect = clothing_to_detect + # self._clothing_to_detect = clothing_to_detect - def execute(self, userdata): - return "succeeded" + colour = clothing_to_detect.split[0] + cloth = clothing_to_detect.split[1] + + with self: + smach.StateMachine.add( + "GET_ATTRIBUTES", + DescribePeople(), + transitions={ + "succeeded": "DECIDE", + "failed": "failed", + }, + ) + + smach.StateMachine.add( + "DECIDE", + self.Descriminator( + colour=colour, + cloth=cloth, + ), + transitions={ + "succeeded": "succeeded", + "failed": "failed", + }, + ) + + class Descriminator(smach.State): + def __init__( + self, + colour: str, + cloth: str, + ): + smach.State.__init__( + self, + outcomes=[ + "succeeded", + "fail", + ], + input_keys = [ + "img_msg", + ], + output_keys = [], + ) + self.colour = colour + self.cloth = cloth + + self.face_features = rospy.ServiceProxy( + "/torch/detect/face_features", TorchFaceFeatureDetectionDescription + ) + + def execute(self, userdata): + if self.colour not in colour_list or self.cloth not in cloth_list: + return "failed" + if len(userdata.people) == 0: + return "failed" + + feature_dict = userdata.people[0]["features"] + + inquired_cloth_type = cloth_type_map[self.cloth] + + inquired_rough_cloth_type = cloth_type_rough_map[inquired_cloth_type] + + confidence = feature_dict[inquired_rough_cloth_type] + + if confidence < 0.25: + return "failed" + + cloth_type = cloth_type_rough_map["max_" + inquired_rough_cloth_type] + + if inquired_cloth_type != cloth_type: + return "failed" + + colour_percentage = feature_dict[inquired_cloth_type + " colour"] + + largest_colour = max(colour_percentage, key=colour_percentage.get) + + if inquired_cloth_type == largest_colour: + return "succeeded" + + return "failed" + diff --git a/skills/src/lasr_skills/estimate_cloth_colour.py b/skills/src/lasr_skills/estimate_cloth_colour.py deleted file mode 100644 index f29c51ce6..000000000 --- a/skills/src/lasr_skills/estimate_cloth_colour.py +++ /dev/null @@ -1,5 +0,0 @@ -import smach - - -class EstimateClothColour(smach.StateMachine): - pass