From bcd4c052bf19d1b09b53c257d172d7b3556d8f95 Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Thu, 24 Feb 2022 14:41:15 -0700 Subject: [PATCH 01/10] Revert "src/swarmie/launch/apriltags.xml: remap targets for cnn_classifier.py" This reverts commit 11b734af83b1da65340a4282753e783b76e8042f. --- src/swarmie/launch/apriltags.xml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/swarmie/launch/apriltags.xml b/src/swarmie/launch/apriltags.xml index 8c3c503a..b5c17d02 100644 --- a/src/swarmie/launch/apriltags.xml +++ b/src/swarmie/launch/apriltags.xml @@ -46,7 +46,8 @@ - + + From afa6b524cb81815e3654ee0bfd8d5424eeb67232 Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Fri, 29 Apr 2022 13:43:26 -0600 Subject: [PATCH 02/10] cnn_classifier.py: +classifier_int topic, -apriltag detection blinding --- .../src/cnn_classifier.py | 21 ++++--------------- 1 file changed, 4 insertions(+), 17 deletions(-) diff --git a/src/tensorflow_classifier/src/cnn_classifier.py b/src/tensorflow_classifier/src/cnn_classifier.py index b1b863b7..aab26090 100755 --- a/src/tensorflow_classifier/src/cnn_classifier.py +++ b/src/tensorflow_classifier/src/cnn_classifier.py @@ -9,7 +9,7 @@ from keras.models import load_model import rospy import sys -from std_msgs.msg import String +from std_msgs.msg import String, UInt8 from sensor_msgs.msg import Image from apriltags2to1.msg import AprilTagDetection, AprilTagDetectionArray import message_filters @@ -34,15 +34,14 @@ def __init__(self, robot_name): # setup required ROS publishers and subscribers self.image_data_subscriber = message_filters.Subscriber('camera/image', Image) - self.apriltag_subscriber = message_filters.Subscriber('targets_for_classifier', AprilTagDetectionArray) + self.apriltag_subscriber = message_filters.Subscriber('targets', AprilTagDetectionArray) # use an approximate time filter to keep the above two subscribers synchronized to a single callback self.synchronizer = message_filters.ApproximateTimeSynchronizer( [self.image_data_subscriber, self.apriltag_subscriber], 10, 0.1, allow_headerless=True) self.synchronizer.registerCallback(self.image_data_callback) - + self.int_classifier_publisher = rospy.Publisher('classifier_int', UInt8, queue_size=5) self.classifier_publisher = rospy.Publisher('classifier', String, queue_size=10) - self.apriltag_publisher = rospy.Publisher('targets', AprilTagDetectionArray, queue_size=10) def image_data_callback(self, image_msg, apriltag_msg): """ @@ -73,19 +72,7 @@ def image_data_callback(self, image_msg, apriltag_msg): # always print the classification of the image from the usb camera as we get it, for debugging purposes self.classifier_publisher.publish('classification = ' + msg_class) - - # purge 'bad' classification detections from the apriltag detection array, then publish what remains - new_detections = AprilTagDetectionArray() - CUBE = 0 - NEST = 256 - - for detection in apriltag_msg.detections: - if detection.id == NEST: - new_detections.detections.append(detection) - elif detection.id == CUBE and classification == 1: - new_detections.detections.append(detection) - - self.apriltag_publisher.publish(new_detections) + self.int_classifier_publisher.publish(classification) if __name__ == '__main__': From fdf75f401fd6a5e618df10f80e5787cda59c8de8 Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Fri, 29 Apr 2022 14:43:29 -0600 Subject: [PATCH 03/10] mobility/src/mobility/swarmie.py: created classification circular buffer --- src/mobility/src/mobility/swarmie.py | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/src/mobility/src/mobility/swarmie.py b/src/mobility/src/mobility/swarmie.py index 760ffbcf..60dedcfd 100644 --- a/src/mobility/src/mobility/swarmie.py +++ b/src/mobility/src/mobility/swarmie.py @@ -27,6 +27,7 @@ import threading swarmie_lock = threading.Lock() +classifier_lock = threading.Lock() from .utils import block_detection, block_pose, filter_detections, is_moving from mobility import sync @@ -190,6 +191,9 @@ def start(self, **kwargs): self.CIRCULAR_BUFFER_SIZE = 90 self.targets = [[]]*self.CIRCULAR_BUFFER_SIZE # The rolling buffer of targets msgs was AprilTagDetectionArray() self.targets_index = 0 # Used to keep track of the most recent targets index, holds the values 0-89 + self.CIRCULAR_CLASSIFIER_BUFFER_SIZE = 5 + self.classifier_queue = [[]]*self.CIRCULAR_CLASSIFIER_BUFFER_SIZE + self.classifier_queue_index = 0 # Intialize this ROS node. anon = False @@ -249,6 +253,7 @@ def start(self, **kwargs): rospy.Subscriber('home_point', PointStamped, self._home_point) rospy.Subscriber('home_point/approx', PointStamped, self._home_point, callback_args=True) + rospy.Subscriber('classifier_int', UInt8, self._classifier_int) # Wait for Odometry messages to come in. # Don't wait for messages on /obstacle because it's published infrequently @@ -281,7 +286,12 @@ def _update_params(self, config): @sync(swarmie_lock) def _odom(self, msg) : self.OdomLocation.Odometry = msg - + + @sync(classifier_lock) + def _classifier_int(self, msg) : + self.classifier_queue_index = (self.classifier_queue_index + 1) % self.CIRCULAR_CLASSIFIER_BUFFER_SIZE + self.classifier_queue[self.classifier_queue_index] = msg + @sync(swarmie_lock) def _obstacle(self, msg) : self.Obstacles &= ~msg.mask @@ -650,6 +660,11 @@ def simulator_running(self): return True return False + @sync(classifier_lock) + def good_classification(self): + return 1 in self.classifier_queue_index #classification == 1 => "good" + + def get_latest_targets(self,id=-1): """ Return the latest `apriltags2to1.msg.AprilTagDetectionArray`. (it might be out of date) and will be affected by twinkeling with an optional id flag""" From b076022b06dc150d2f205d85689c2303829ac1bc Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Fri, 29 Apr 2022 14:44:19 -0600 Subject: [PATCH 04/10] src/mobility/behavior/pickup|search.py: add good classification check --- src/mobility/src/mobility/behavior/pickup.py | 2 +- src/mobility/src/mobility/behavior/search.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/mobility/src/mobility/behavior/pickup.py b/src/mobility/src/mobility/behavior/pickup.py index 0cfe876a..e996dc64 100755 --- a/src/mobility/src/mobility/behavior/pickup.py +++ b/src/mobility/src/mobility/behavior/pickup.py @@ -37,7 +37,7 @@ def setup_approach(save_loc=False): block = swarmie.get_nearest_block_location(targets_buffer_age=5.0) - if block is not None: + if block is not None and swarmie.good_classification(): print("Making the setup approach.") cur_loc = swarmie.get_odom_location().get_pose() dist = math.hypot(cur_loc.x - block.x, cur_loc.y - block.y) diff --git a/src/mobility/src/mobility/behavior/search.py b/src/mobility/src/mobility/behavior/search.py index 909e3d8a..f27592c6 100755 --- a/src/mobility/src/mobility/behavior/search.py +++ b/src/mobility/src/mobility/behavior/search.py @@ -62,7 +62,7 @@ def random_walk(num_moves): print("I found a tag!") # Let's drive there to be helpful. rospy.sleep(0.3) - if not planner.sees_home_tag(): + if not planner.sees_home_tag() and swarmie.good_classification(): found_tag = True search_exit(0) From b3664a7989aebd15f0183bcba6bd59aa2653e493 Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Fri, 29 Apr 2022 15:17:22 -0600 Subject: [PATCH 05/10] src/mobility/src/mobility/swarmie.py: correct to use queue not its index --- src/mobility/src/mobility/swarmie.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mobility/src/mobility/swarmie.py b/src/mobility/src/mobility/swarmie.py index 60dedcfd..073f1b50 100644 --- a/src/mobility/src/mobility/swarmie.py +++ b/src/mobility/src/mobility/swarmie.py @@ -662,7 +662,7 @@ def simulator_running(self): @sync(classifier_lock) def good_classification(self): - return 1 in self.classifier_queue_index #classification == 1 => "good" + return 1 in self.classifier_queue #classification == 1 => "good" def get_latest_targets(self,id=-1): From a73ce22c9c8898fafa1fdb8ebc4c718d99ae7465 Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Fri, 29 Apr 2022 15:20:55 -0600 Subject: [PATCH 06/10] src/mobility/swarmie.py: flatten classifier_queue to be list of ints --- src/mobility/src/mobility/swarmie.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mobility/src/mobility/swarmie.py b/src/mobility/src/mobility/swarmie.py index 073f1b50..d6a30e82 100644 --- a/src/mobility/src/mobility/swarmie.py +++ b/src/mobility/src/mobility/swarmie.py @@ -290,7 +290,7 @@ def _odom(self, msg) : @sync(classifier_lock) def _classifier_int(self, msg) : self.classifier_queue_index = (self.classifier_queue_index + 1) % self.CIRCULAR_CLASSIFIER_BUFFER_SIZE - self.classifier_queue[self.classifier_queue_index] = msg + self.classifier_queue[self.classifier_queue_index] = msg.data @sync(swarmie_lock) def _obstacle(self, msg) : From ab50fc9223ff44c897972acbe73ac4136a85e670 Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Fri, 29 Apr 2022 15:31:36 -0600 Subject: [PATCH 07/10] src/mobility/src/mobility/behavior/pickup.py: exit if no good cubes --- src/mobility/src/mobility/behavior/pickup.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/mobility/src/mobility/behavior/pickup.py b/src/mobility/src/mobility/behavior/pickup.py index e996dc64..e599c3e3 100755 --- a/src/mobility/src/mobility/behavior/pickup.py +++ b/src/mobility/src/mobility/behavior/pickup.py @@ -36,8 +36,12 @@ def setup_approach(save_loc=False): rospy.sleep(3) block = swarmie.get_nearest_block_location(targets_buffer_age=5.0) - - if block is not None and swarmie.good_classification(): + if not swarmie.good_classification(): + print("No good classified blocks detected.") + swarmie.wrist_up() + sys.exit(1) + + if block is not None: print("Making the setup approach.") cur_loc = swarmie.get_odom_location().get_pose() dist = math.hypot(cur_loc.x - block.x, cur_loc.y - block.y) From e891685f2944bb400d15bddb596c90588a9356f9 Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Thu, 5 May 2022 15:18:33 -0600 Subject: [PATCH 08/10] pickup.py: revert claw_offset_distance, tune timings, fix spacing --- src/mobility/src/mobility/behavior/pickup.py | 255 +++++++++---------- 1 file changed, 125 insertions(+), 130 deletions(-) diff --git a/src/mobility/src/mobility/behavior/pickup.py b/src/mobility/src/mobility/behavior/pickup.py index e599c3e3..3fe408e7 100755 --- a/src/mobility/src/mobility/behavior/pickup.py +++ b/src/mobility/src/mobility/behavior/pickup.py @@ -3,14 +3,14 @@ from __future__ import print_function import sys -import rospy +import rospy import math -import random +import random import tf -import angles +import angles from std_msgs.msg import String -from geometry_msgs.msg import Point +from geometry_msgs.msg import Point from mobility.msg import MoveResult from swarmie_msgs.msg import Obstacle @@ -22,146 +22,141 @@ '''Pickup node.''' +claw_offset_distance = 0.23 + def setup_approach(save_loc=False): - """Drive a little closer to the nearest block if it's far enough away.""" - global claw_offset_distance + """Drive a little closer to the nearest block if it's far enough away.""" + global claw_offset_distance + if swarmie.simulator_running(): + extra_offset = 0.20 + else: + extra_offset = 0.15 + + swarmie.fingers_open() + swarmie.set_wrist_angle(1.15) + rospy.sleep(2) + + block = swarmie.get_nearest_block_location(targets_buffer_age=3.0) + if not swarmie.good_classification(): + print("No good classified blocks detected.") + swarmie.wrist_middle() + sys.exit(1) + + if block is not None: + print("Making the setup approach.") + cur_loc = swarmie.get_odom_location().get_pose() + dist = math.hypot(cur_loc.x - block.x, cur_loc.y - block.y) + + if dist > (claw_offset_distance + extra_offset): + swarmie.drive_to( + block, + claw_offset=claw_offset_distance + extra_offset, + ignore=Obstacle.VISION_SAFE | Obstacle.IS_SONAR, + timeout=20, + **swarmie.speed_slow + ) + + if save_loc: + swarmie.print_infoLog('Setting resource pile location.') + swarmie.add_resource_pile_location() + + +def approach(save_loc=False): + global claw_offset_distance + setup_approach(save_loc) + print("Attempting a pickup.") + rospy.sleep(1) + block = swarmie.get_nearest_block_location(targets_buffer_age=1.0) + print("claw_offset_distance:", claw_offset_distance) + if block is not None: + # claw_offset should be a positive distance of how short drive_to needs to be. + swarmie.drive_to( + block, + claw_offset=claw_offset_distance, + ignore=Obstacle.VISION_SAFE | Obstacle.IS_SONAR, + timeout=20, + **swarmie.speed_slow + ) + # Grab - minimal pickup with sim_check. if swarmie.simulator_running(): - extra_offset = 0.20 + finger_close_angle = 0 else: - extra_offset = 0.15 - - swarmie.fingers_open() - swarmie.set_wrist_angle(1.15) - rospy.sleep(3) - - block = swarmie.get_nearest_block_location(targets_buffer_age=5.0) - if not swarmie.good_classification(): - print("No good classified blocks detected.") - swarmie.wrist_up() - sys.exit(1) - - if block is not None: - print("Making the setup approach.") - cur_loc = swarmie.get_odom_location().get_pose() - dist = math.hypot(cur_loc.x - block.x, cur_loc.y - block.y) + finger_close_angle = 0.2 - if dist > (claw_offset_distance + extra_offset): - swarmie.drive_to( - block, - claw_offset=claw_offset_distance+extra_offset, - ignore=Obstacle.VISION_SAFE | Obstacle.IS_SONAR, - timeout=20, - **swarmie.speed_slow - ) + swarmie.set_finger_angle(finger_close_angle) # close + rospy.sleep(1) + swarmie.wrist_up() + rospy.sleep(.5) + # did we successfully grab a block? + if swarmie.has_block(): + swarmie.wrist_middle() + swarmie.drive(-0.3, ignore=Obstacle.VISION_SAFE | Obstacle.IS_SONAR, timeout=20) + return True + else: + swarmie.set_wrist_angle(0.55) + rospy.sleep(1) + swarmie.fingers_open() + # Wait a moment for a block to fall out of claw + rospy.sleep(0.25) + else: + print("No legal blocks detected.") + swarmie.wrist_middle() + sys.exit(1) - if save_loc: - swarmie.print_infoLog('Setting resource pile location.') - swarmie.add_resource_pile_location() + # otherwise reset claw and return False + swarmie.wrist_up() + return False -def approach(save_loc=False): - global claw_offset_distance - print ("Attempting a pickup.") +def recover(): + global claw_offset_distance - setup_approach(save_loc) - rospy.sleep(3) - block = swarmie.get_nearest_block_location(targets_buffer_age=0.5) + print("Missed, trying to recover.") + try: + swarmie.drive(-0.15, ignore=Obstacle.VISION_SAFE | Obstacle.IS_SONAR, timeout=10) + # Wait a moment to detect tags before possible backing up further + rospy.sleep(1.5) + block = swarmie.get_nearest_block_location(targets_buffer_age=1.5) if block is not None: - # claw_offset should be a positive distance of how short drive_to needs to be. - swarmie.drive_to( - block, - claw_offset=claw_offset_distance, - ignore=Obstacle.VISION_SAFE | Obstacle.IS_SONAR, - timeout=20, - **swarmie.speed_slow - ) - # Grab - minimal pickup with sim_check. - - if swarmie.simulator_running(): - finger_close_angle = 0 - else: - finger_close_angle = 0.2 - - swarmie.set_finger_angle(finger_close_angle) #close - rospy.sleep(1) - swarmie.wrist_up() - rospy.sleep(.5) - # did we succesuflly grab a block? - if swarmie.has_block(): - swarmie.wrist_middle() - swarmie.drive(-0.3, - ignore=Obstacle.VISION_SAFE | Obstacle.IS_SONAR, - timeout=20) - return True - else: - swarmie.set_wrist_angle(0.55) - rospy.sleep(1) - swarmie.fingers_open() - # Wait a moment for a block to fall out of claw - rospy.sleep(0.25) + pass else: - print("No legal blocks detected.") - swarmie.wrist_up() - sys.exit(1) + swarmie.drive(-0.15, ignore=Obstacle.VISION_SAFE | Obstacle.IS_SONAR, timeout=10) - # otherwise reset claw and return Falase - swarmie.wrist_up() - return False - - -def recover(): - global claw_offset_distance + except (AbortException, InsideHomeException): + raise + except DriveException as e: + print("Oh no, we have an exception!", e) + pass - print ("Missed, trying to recover.") - try: - swarmie.drive(-0.15, - ignore=Obstacle.VISION_SAFE | Obstacle.IS_SONAR, - timeout=20) - # Wait a moment to detect tags before possible backing up further - rospy.sleep(3) - - block = swarmie.get_nearest_block_location(targets_buffer_age=4.0) - - if block is not None: - pass - else: - swarmie.drive(-0.15, - ignore=Obstacle.VISION_SAFE | Obstacle.IS_SONAR, - timeout=20) - - except (AbortException, InsideHomeException): - raise - except DriveException as e: - print("Oh no, we have an exception!", e) - pass def main(**kwargs): - global claw_offset_distance + global claw_offset_distance - claw_offset_distance = 0.4 - if swarmie.simulator_running(): - claw_offset_distance = 0.16 - - for i in range(3): - try: - if approach(save_loc=bool(i == 0)): - print ("Got it!") - sys.exit(0) - recover() - except TimeoutException: - rospy.logwarn( - ('Timeout exception during pickup. This rover may be ' + - 'physically deadlocked with an obstacle or another rover.') - ) - swarmie.drive(random.uniform(-0.1, -0.2), - ignore=Obstacle.VISION_SAFE | Obstacle.IS_SONAR, - timeout=20) - - print ("Giving up after too many attempts.") - return 1 - -if __name__ == '__main__' : - swarmie.start(node_name='pickup') - sys.exit(main()) + claw_offset_distance = 0.23 + if swarmie.simulator_running(): + claw_offset_distance = 0.16 + + for i in range(3): + try: + if approach(save_loc=bool(i == 0)): + print("Got it!") + sys.exit(0) + recover() + except TimeoutException: + rospy.logwarn( + ('Timeout exception during pickup. This rover may be ' + + 'physically deadlocked with an obstacle or another rover.') + ) + swarmie.drive(random.uniform(-0.1, -0.2), + ignore=Obstacle.VISION_SAFE | Obstacle.IS_SONAR, + timeout=20) + + print("Giving up after too many attempts.") + return 1 + + +if __name__ == '__main__': + swarmie.start(node_name='pickup') + sys.exit(main()) From ecd535b6a83fa737580b8a55f06889bafee610c4 Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Thu, 5 May 2022 16:18:45 -0600 Subject: [PATCH 09/10] mobility/behavior/search.py: add extra check for good classification --- src/mobility/src/mobility/behavior/search.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mobility/src/mobility/behavior/search.py b/src/mobility/src/mobility/behavior/search.py index f27592c6..6abc325b 100755 --- a/src/mobility/src/mobility/behavior/search.py +++ b/src/mobility/src/mobility/behavior/search.py @@ -121,7 +121,7 @@ def return_to_last_exit_position(last_pose, skip_drive_to=False): except TagException: rospy.sleep(0.3) # build buffer a little # too risky to stop for targets if home is in view too - if not planner.sees_home_tag(): + if not planner.sees_home_tag() and swarmie.good_classification(): # success! found_tag = True search_exit(0) From 8de080c0ecbbb1b48ccb3d743a18c05f86a3f167 Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Mon, 9 May 2022 13:29:12 -0700 Subject: [PATCH 10/10] Delete CNN_9010_wAug_160.h5 --- classifiers/CNN_9010_wAug_160.h5 | 3 --- 1 file changed, 3 deletions(-) delete mode 100644 classifiers/CNN_9010_wAug_160.h5 diff --git a/classifiers/CNN_9010_wAug_160.h5 b/classifiers/CNN_9010_wAug_160.h5 deleted file mode 100644 index 47662b73..00000000 --- a/classifiers/CNN_9010_wAug_160.h5 +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:bcb2b50327a60c26a526e7132e1540703c7884602f5f9f9629f8008292c6375c -size 248723000