From d939dd59176a0114e0d59e99f7a5eb70f03fd32e Mon Sep 17 00:00:00 2001 From: Vince-C156 Date: Mon, 5 Feb 2024 15:56:58 -0800 Subject: [PATCH 1/3] temp commit --- tests/imaging/image_processor_tests.py | 1 + uavf_2024/imaging/image_processor.py | 2 +- uavf_2024/imaging/imaging_types.py | 1 + .../shape_instance_segmenter.py | 27 ++++++++++++++++++- 4 files changed, 29 insertions(+), 2 deletions(-) diff --git a/tests/imaging/image_processor_tests.py b/tests/imaging/image_processor_tests.py index af8b4fd7..30b6cba3 100644 --- a/tests/imaging/image_processor_tests.py +++ b/tests/imaging/image_processor_tests.py @@ -203,6 +203,7 @@ def test_metrics(self, debug_letter_confusion = False): for img, ground_truth in zip(imgs, labels): predictions = image_processor.process_image(img) + if debug_letter_confusion: prediction_list.append(predictions) diff --git a/uavf_2024/imaging/image_processor.py b/uavf_2024/imaging/image_processor.py index ce166707..b6514cea 100644 --- a/uavf_2024/imaging/image_processor.py +++ b/uavf_2024/imaging/image_processor.py @@ -114,7 +114,7 @@ def process_image(self, img: Image) -> list[FullPrediction]: letter_imgs = [] for shape_res in results: # These are all linear operations so not parallelized (yet) - + print(f"{shape_res.confidences}, {shape_res.cnf_matrix_preds}") # Color segmentations shape_conf = shape_res.confidences letter_img = cv.resize(shape_res.img.get_array().astype(np.float32), (128,128)) diff --git a/uavf_2024/imaging/imaging_types.py b/uavf_2024/imaging/imaging_types.py index ab8656c7..77da8d5c 100644 --- a/uavf_2024/imaging/imaging_types.py +++ b/uavf_2024/imaging/imaging_types.py @@ -121,6 +121,7 @@ class InstanceSegmentationResult: mask: np.ndarray img: 'Image' id : int + cnf_matrix_preds: list[float] @dataclass class Target3D: diff --git a/uavf_2024/imaging/shape_detection/shape_instance_segmenter.py b/uavf_2024/imaging/shape_detection/shape_instance_segmenter.py index a29ef05c..0be8165b 100644 --- a/uavf_2024/imaging/shape_detection/shape_instance_segmenter.py +++ b/uavf_2024/imaging/shape_detection/shape_instance_segmenter.py @@ -9,12 +9,36 @@ CURRENT_FILE_PATH = os.path.dirname(os.path.realpath(__file__)) +SHAPES = [ + "circle", + "semicircle", + "quartercircle", + "triangle", + "rectangle", + "pentagon", + "star", + "cross", + "person", + "background" +] + class ShapeInstanceSegmenter: def __init__(self, img_size): self.shape_model = YOLO(f"{CURRENT_FILE_PATH}/weights/seg-v8n-best.pt") rand_input = np.random.rand(1, img_size, img_size, 3).astype(np.float32) self.shape_model.predict(list(rand_input), verbose=False) self.num_processed = 0 + self.cnf_matrix = {'circle' : [0.83, 0, 0, 0, 0, .01, 0, 0, 0, .04], + 'semicircle': [.01, .67, .28, .02, .05, .03, 0, 0, .01, .19], + 'quartercircle': [0, .18, .43, 0, .41, .17, 0, 0, 0, .29], + 'triangle': [0, .03, 0, .91, .01, 0, 0, 0, 0, .03], + 'rectangle': [.01, 0, .19, 0, .46, .08, 0, 0, 0, .24], + 'pentagon': [.10, .03, .08, 0, .01, .68, 0, 0, 0, .13], + 'star': [0, .01, 0, .04, 0, 0, .97, .02, 0, .02], + 'cross': [0, .04, 0, .01, 0, 0, 0, .96, .03, .02], + 'person': [0, .01, 0, .01, .01, 0, 0, 0, .91, .05], + 'background': [.05, .01, .02, .02, .03, .03, .03, .02, .04, 0] + } @profiler @@ -49,7 +73,8 @@ def predict(self, tiles: tuple[Tile]) -> list[InstanceSegmentationResult]: confidences = confidences, mask = mask[y:y+h, x:x+w].unsqueeze(2).cpu().numpy(), img = tiles[img_index].img.make_sub_image(x, y, w, h), - id = self.num_processed + id = self.num_processed, + cnf_matrix_preds = self.cnf_matrix[SHAPES[cls.int()]] ) ) self.num_processed += 1 From f60766d7f7c3644f3174061faf51f8498cea7528 Mon Sep 17 00:00:00 2001 From: Vince-C156 Date: Mon, 5 Feb 2024 17:29:06 -0800 Subject: [PATCH 2/3] "added autonomous land, takeoff, arm/disarm" -Nathan Yang --- uavf_2024/gnc/commander_node.py | 88 +++++++++++++++++++++++++++++++-- 1 file changed, 85 insertions(+), 3 deletions(-) diff --git a/uavf_2024/gnc/commander_node.py b/uavf_2024/gnc/commander_node.py index fe121430..8be79499 100644 --- a/uavf_2024/gnc/commander_node.py +++ b/uavf_2024/gnc/commander_node.py @@ -36,6 +36,7 @@ def __init__(self, args): self.waypoints_client = self.create_client(mavros_msgs.srv.WaypointPush, 'mavros/mission/push') self.clear_mission_client = self.create_client(mavros_msgs.srv.WaypointClear, 'mavros/mission/clear') + self.land_client = self.create_client(mavros_msgs.srv.CommandTOL, 'mavros/cmd/land') self.cur_state = None self.state_sub = self.create_subscription( @@ -214,16 +215,83 @@ def wait_for_takeoff(self): ''' self.log('Waiting for takeoff') + self.takeoff_client.wait_for_service() + + takeoff_request = mavros_msgs.srv.CommandTOL.Request(altitude=20.0, min_pitch=0.0, yaw=0.0, latitude=0.0, longitude=0.0) + takeoff_response = self.takeoff_client.call(takeoff_request) + + return takeoff_response.success + + def takeoff(self): + success = self.wait_for_takeoff() + + if success: + self.log('Takeoff successful') + else: + self.log('Takeoff failed') + + return success + + def wait_for_land(self): + self.land_client.wait_for_service() + + land_request = mavros_msgs.srv.CommandTOL.Request(altitude=0.0, min_pitch=0.0, yaw=0.0, latitude=0.0, longitude=0.0) + land_response = self.land_client.call(land_request) + + return land_response.success + + def land(self): + success = self.wait_for_land() + + if success: + self.log('Landing successful') + else: + self.log('Landing failed') + + return success + + def arm(self, b: bool): + ''' + True for arm + False for disarm + ''' + arm_response = self.arm_client.call(mavros_msgs.srv.CommandBool.Request(value = b)) + arm_disarm_string = 'Arming' if b else 'Disarming' + if arm_response.success: + self.log(arm_disarm_string + ' successful') + else: + self.log(arm_disarm_string + ' failed') + return arm_response.success + + def reload_payload(self): + ''' + this will do nothing for now + ''' + self.log('Nothing will happen for now') + def execute_mission_loop(self): while not self.got_global_pos: pass + arm_success = self.arm(True) + + if not arm_success: + self.log('Mission aborted due to failed arming') + return + for lap in range(len(self.payloads)): self.log('Lap', lap) - # Wait for takeoff - self.wait_for_takeoff() + # # Wait for takeoff + # self.wait_for_takeoff() + + + takeoff_success = self.takeoff() + if not takeoff_success: + self.log('Mission aborted due to failed takeoff') + return + # Fly waypoint lap self.execute_waypoints(self.mission_wps) @@ -234,4 +302,18 @@ def execute_mission_loop(self): self.dropzone_planner.conduct_air_drop() # Fly back to home position - self.execute_waypoints([(self.home_global_pos.latitude, self.home_global_pos.longitude)]) \ No newline at end of file + self.execute_waypoints([(self.home_global_pos.latitude, self.home_global_pos.longitude)]) + + self.reload_payload() + + land_success = self.land() + + if not land_success: + self.log('Mission aborted due to landing failure.') + return + + disarm_success = self.arm(False) + + if not disarm_success: + self.log('Mission aborted due to failed disarming') + return \ No newline at end of file From 14184627ddf2484dba503c7c85e251c1b1eff89b Mon Sep 17 00:00:00 2001 From: nathanyvng Date: Mon, 19 Feb 2024 17:53:18 -0800 Subject: [PATCH 3/3] reverted d939... --- tests/imaging/image_processor_tests.py | 1 - uavf_2024/imaging/image_processor.py | 2 +- uavf_2024/imaging/imaging_types.py | 1 - .../shape_instance_segmenter.py | 27 +------------------ 4 files changed, 2 insertions(+), 29 deletions(-) diff --git a/tests/imaging/image_processor_tests.py b/tests/imaging/image_processor_tests.py index 30b6cba3..af8b4fd7 100644 --- a/tests/imaging/image_processor_tests.py +++ b/tests/imaging/image_processor_tests.py @@ -203,7 +203,6 @@ def test_metrics(self, debug_letter_confusion = False): for img, ground_truth in zip(imgs, labels): predictions = image_processor.process_image(img) - if debug_letter_confusion: prediction_list.append(predictions) diff --git a/uavf_2024/imaging/image_processor.py b/uavf_2024/imaging/image_processor.py index b6514cea..ce166707 100644 --- a/uavf_2024/imaging/image_processor.py +++ b/uavf_2024/imaging/image_processor.py @@ -114,7 +114,7 @@ def process_image(self, img: Image) -> list[FullPrediction]: letter_imgs = [] for shape_res in results: # These are all linear operations so not parallelized (yet) - print(f"{shape_res.confidences}, {shape_res.cnf_matrix_preds}") + # Color segmentations shape_conf = shape_res.confidences letter_img = cv.resize(shape_res.img.get_array().astype(np.float32), (128,128)) diff --git a/uavf_2024/imaging/imaging_types.py b/uavf_2024/imaging/imaging_types.py index 77da8d5c..ab8656c7 100644 --- a/uavf_2024/imaging/imaging_types.py +++ b/uavf_2024/imaging/imaging_types.py @@ -121,7 +121,6 @@ class InstanceSegmentationResult: mask: np.ndarray img: 'Image' id : int - cnf_matrix_preds: list[float] @dataclass class Target3D: diff --git a/uavf_2024/imaging/shape_detection/shape_instance_segmenter.py b/uavf_2024/imaging/shape_detection/shape_instance_segmenter.py index 0be8165b..a29ef05c 100644 --- a/uavf_2024/imaging/shape_detection/shape_instance_segmenter.py +++ b/uavf_2024/imaging/shape_detection/shape_instance_segmenter.py @@ -9,36 +9,12 @@ CURRENT_FILE_PATH = os.path.dirname(os.path.realpath(__file__)) -SHAPES = [ - "circle", - "semicircle", - "quartercircle", - "triangle", - "rectangle", - "pentagon", - "star", - "cross", - "person", - "background" -] - class ShapeInstanceSegmenter: def __init__(self, img_size): self.shape_model = YOLO(f"{CURRENT_FILE_PATH}/weights/seg-v8n-best.pt") rand_input = np.random.rand(1, img_size, img_size, 3).astype(np.float32) self.shape_model.predict(list(rand_input), verbose=False) self.num_processed = 0 - self.cnf_matrix = {'circle' : [0.83, 0, 0, 0, 0, .01, 0, 0, 0, .04], - 'semicircle': [.01, .67, .28, .02, .05, .03, 0, 0, .01, .19], - 'quartercircle': [0, .18, .43, 0, .41, .17, 0, 0, 0, .29], - 'triangle': [0, .03, 0, .91, .01, 0, 0, 0, 0, .03], - 'rectangle': [.01, 0, .19, 0, .46, .08, 0, 0, 0, .24], - 'pentagon': [.10, .03, .08, 0, .01, .68, 0, 0, 0, .13], - 'star': [0, .01, 0, .04, 0, 0, .97, .02, 0, .02], - 'cross': [0, .04, 0, .01, 0, 0, 0, .96, .03, .02], - 'person': [0, .01, 0, .01, .01, 0, 0, 0, .91, .05], - 'background': [.05, .01, .02, .02, .03, .03, .03, .02, .04, 0] - } @profiler @@ -73,8 +49,7 @@ def predict(self, tiles: tuple[Tile]) -> list[InstanceSegmentationResult]: confidences = confidences, mask = mask[y:y+h, x:x+w].unsqueeze(2).cpu().numpy(), img = tiles[img_index].img.make_sub_image(x, y, w, h), - id = self.num_processed, - cnf_matrix_preds = self.cnf_matrix[SHAPES[cls.int()]] + id = self.num_processed ) ) self.num_processed += 1