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 diff --git a/src/mobility/src/mobility/behavior/pickup.py b/src/mobility/src/mobility/behavior/pickup.py index 0cfe876a..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,142 +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 - if swarmie.simulator_running(): - extra_offset = 0.20 - else: - extra_offset = 0.15 - swarmie.fingers_open() - swarmie.set_wrist_angle(1.15) - rospy.sleep(3) +def setup_approach(save_loc=False): + """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() - block = swarmie.get_nearest_block_location(targets_buffer_age=5.0) - 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) +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(): + finger_close_angle = 0 + else: + 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) - - # otherwise reset claw and return Falase - swarmie.wrist_up() - return False - + swarmie.drive(-0.15, ignore=Obstacle.VISION_SAFE | Obstacle.IS_SONAR, timeout=10) -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()) diff --git a/src/mobility/src/mobility/behavior/search.py b/src/mobility/src/mobility/behavior/search.py index 909e3d8a..6abc325b 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) @@ -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) diff --git a/src/mobility/src/mobility/swarmie.py b/src/mobility/src/mobility/swarmie.py index 760ffbcf..d6a30e82 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.data + @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 #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""" 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 @@ - + + 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__':