1 files changed, 43 insertions, 0 deletions
diff --git a/controllers/final/detector.py b/controllers/final/detector.py
new file mode 100644
index 0000000..d874e73
--- /dev/null
+++ b/controllers/final/detector.py
@@ -0,0 +1,43 @@
+from py_trees.behaviour import Behaviour
+from py_trees.common import Status
+from constants import TIMEOUT, JAR_OFFSETS
+from setup import blackboard, robot, gps, compass, camera
+from utilities import robot_to_world_coords, grasp_point
+from numpy import arctan2
+
+# Detect the jar using the camera and store its position in the blackboard.
+class JarFinder(Behaviour):
+    def __init__(self, name, target_jar, jar_offset):
+        super().__init__(name)
+        self.target_jar = target_jar
+        self.offset = jar_offset
+
+    def initialise(self):
+        self.initial_time = robot.getTime()
+        print("%s.initialise(), targeting %s" % (self.name, self.target_jar))
+
+    def update(self):
+        if robot.getTime() - self.initial_time > TIMEOUT:
+            print(
+                "%s.update()[%s->%s], Timed out"
+                % (self.name, self.status, Status.FAILURE)
+            )
+            return Status.FAILURE
+
+        gps_pos = gps.getValues()
+        theta = arctan2(*compass.getValues()[:2])
+        for jar in camera.getRecognitionObjects():
+            print("%s.update(), %s detected." % (self.name, jar.getModel()))
+            if jar.getModel() == self.target_jar:
+                jar_pos = robot_to_world_coords(jar.getPosition(), gps_pos, theta)
+                blackboard["target_pos"] = grasp_point(jar_pos, gps_pos, self.offset)
+                print(
+                    "%s.update()[%s->%s], jar at [%.3f, %.3f]"
+                    % (self.name, self.status, Status.SUCCESS, *jar_pos)
+                )
+                return Status.SUCCESS
+        print(
+            "%s.update()[%s->%s], %s not found yet."
+            % (self.name, self.status, self.target_jar, Status.RUNNING)
+        )
+        return Status.RUNNING