Merge branch 'main' into haip/mask-tags

5 files changed, 65 insertions, 26 deletions

diff --git a/virtual-programs/apriltags.folk b/virtual-programs/apriltags.folk
new file mode 100644
index 00000000..01377b78
--- /dev/null
+++ b/virtual-programs/apriltags.folk
@@ -0,0 +1,37 @@
+if {$::isLaptop} return
+
+On process {
+    source pi/AprilTags.tcl
+    AprilTags::init
+
+    # TODO: Clean this up. We retract these so that we don't bounce
+    # statements back to the main Folk process that it sends us.
+    Retract /anyone/ wishes $::thisProcess shares all wishes
+    Retract /anyone/ wishes $::thisProcess shares all claims
+    Wish $::thisProcess receives statements like \
+        [list /someone/ claims the camera frame is /grayFrame/]
+    Wish $::thisProcess shares statements like \
+        [list /someone/ wishes /process/ receives statements like /pattern/]
+    Wish $::thisProcess shares statements like \
+        [list /someone/ claims tag /tag/ has center /center/ size /size/]
+    Wish $::thisProcess shares statements like \
+        [list /someone/ claims tag /tag/ has corners /corners/]
+    Wish $::thisProcess shares statements like \
+        [list /someone/ claims the AprilTag time is /aprilTime/]
+    Wish $::thisProcess shares statements like \
+        [list /someone/ claims $::thisProcess has pid /pid/]
+
+    When the camera frame is /grayFrame/ {
+        set aprilTime [time {
+            set tags [AprilTags::detect $grayFrame]
+        }]
+        Commit {
+            Claim the AprilTag time is $aprilTime
+
+            foreach tag $tags {
+                Claim tag [dict get $tag id] has center [dict get $tag center] size [dict get $tag size]
+                Claim tag [dict get $tag id] has corners [dict get $tag corners]
+            }
+        }
+    }
+}
diff --git a/virtual-programs/camera.folk b/virtual-programs/camera.folk
index b865392e..b73398f5 100644
--- a/virtual-programs/camera.folk
+++ b/virtual-programs/camera.folk
@@ -7,31 +7,17 @@ namespace eval ::Camera {
 
 On process {
     source pi/Camera.tcl
-    source pi/AprilTags.tcl
     Camera::init 1280 720
-    AprilTags::init
 
     puts "Camera tid: [getTid]"
     
-    while true {
+    forever {
         set cameraTime [time {
             set grayFrame [Camera::grayFrame]
         }]
-        set aprilTime [time {
-            set tags [AprilTags::detect $grayFrame]
-        }]
-        
         Commit {
-            Claim the camera frame is $grayFrame
-
             Claim the camera time is $cameraTime
-            Claim the AprilTag time is $aprilTime
-
-            foreach tag $tags {
-                Claim tag [dict get $tag id] has center [dict get $tag center] size [dict get $tag size]
-                Claim tag [dict get $tag id] has corners [dict get $tag corners]
-            }
+            Claim the camera frame is $grayFrame
         }
-        Step
     }
 }
diff --git a/virtual-programs/images.folk b/virtual-programs/images.folk
index 89dbcb23..6846975e 100644
--- a/virtual-programs/images.folk
+++ b/virtual-programs/images.folk
@@ -93,12 +93,10 @@ When /someone/ wishes /p/ has camera slice & \
      the camera frame is /f/ & \
      /p/ has region /r/ {
 
-    # okay, need to get top & bottom
-    set top [region top $r]
-    lassign [lindex $r 0] a b c d
-
-    lassign [projectorToCamera $top] px0 py0
-    lassign [projectorToCamera $a] px1 py1
+    # Convert region in projector coordinates to camera
+    lassign [regionToBbox $r] minX minY maxX maxY
+    lassign [projectorToCamera [list $minX $minY]] px0 py0
+    lassign [projectorToCamera [list $maxX $maxY]] px1 py1
 
     # Clamp to image bounds
     set x [expr {int(max(min($px0, $px1), 0))}]
@@ -119,6 +117,8 @@ When when /p/ has camera slice /slice/ /lambda/ with environment /e/ {
 # Display a camera slice
 When /someone/ wishes /p/ displays camera slice /slice/ & /p/ has region /r/ {
   set origin [lindex $r 0 0]
-  set scale [expr {$Display::WIDTH / $Camera::WIDTH}]
-  Wish display runs [list Display::image {*}$origin $slice $scale]
+  # set scale [expr {$Display::WIDTH / $Camera::WIDTH}]
+  # Use 1x scale instead of $scale so the projected tag doesn't redetect.
+  # TODO: Mask the tag out?
+  Wish display runs [list Display::image {*}$origin $slice 1]
 }
diff --git a/virtual-programs/label.folk b/virtual-programs/label.folk
index 1bf2fac1..ad5e406f 100644
--- a/virtual-programs/label.folk
+++ b/virtual-programs/label.folk
@@ -1,7 +1,7 @@
 When /thing/ has region /region/ {
     lassign [region centroid $region] x y
-    set width [region width $region]
-    set height [region height $region]
+    # set width [region width $region]
+    # set height [region height $region]
     set radians [region angle $region]
 
     When the collected matches for [list /someone/ wishes $thing is labelled /text/] are /matches/ {
diff --git a/virtual-programs/laser.folk b/virtual-programs/laser.folk
new file mode 100644
index 00000000..e71cccc8
--- /dev/null
+++ b/virtual-programs/laser.folk
@@ -0,0 +1,16 @@
+return
+
+source vendor/blobdetect/blobdetect.tcl
+
+When the camera frame is /grayFrame/ {
+    set blobTime [time {
+        set threshold 250
+        set blobs [::BlobDetect::detect $grayFrame $threshold]
+    }]
+    Commit {
+        Claim the blob detection time is $blobTime
+        foreach blob $blobs {
+            Claim laser blob [dict get $blob id] has center [dict get $blob center] size [dict get $blob size]
+        }
+    }
+}