Anti blind

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())

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)

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"""

src/swarmie/launch/apriltags.xml

@@ -46,7 +46,8 @@
   <node name="apriltag2to1_converter" pkg="apriltags2to1" type="apriltag_converter_node.py"
         respawn="true">
     <param name="camera_frame_id" value="$(arg name)/camera_link" />
-    <remap from="targets/out" to="targets_for_classifier" />
+
+    <remap from="targets/out" to="targets" />
   </node>
 
 </launch>