diff options
Diffstat (limited to 'virtual-programs')
| -rw-r--r-- | virtual-programs/apriltags.folk | 156 | ||||
| -rw-r--r-- | virtual-programs/camera.folk | 3 | ||||
| -rw-r--r-- | virtual-programs/display.folk | 13 | ||||
| -rw-r--r-- | virtual-programs/tags-and-calibration.folk | 3 | ||||
| -rw-r--r-- | virtual-programs/time.folk | 2 |
5 files changed, 125 insertions, 52 deletions
diff --git a/virtual-programs/apriltags.folk b/virtual-programs/apriltags.folk index 3e95ee64..ac2c4f8b 100644 --- a/virtual-programs/apriltags.folk +++ b/virtual-programs/apriltags.folk @@ -1,6 +1,7 @@ if {$::isLaptop} return -On process { +# Plain detector. Runs on entire camera frame. +set mainDetectorProcess [On process { source pi/AprilTags.tcl AprilTags::init @@ -21,52 +22,119 @@ On process { set aprilTime [time { set tags [AprilTags::detect $grayFrame] }] - Commit { - Claim $::thisProcess detects tags $tags at $timestamp in time $aprilTime + Claim $::thisProcess detects tags $tags at $timestamp in time $aprilTime + } +}] + +# Incremental detector. Looks at regions where there were tags in the +# old camera frame. +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/ at /timestamp/] + Wish $::thisProcess receives statements like \ + [list /someone/ claims $mainDetectorProcess detects tags /tags/ at /timestamp/ in time /aprilTime/] + Wish $::thisProcess shares statements like \ + [list /someone/ wishes /process/ receives statements like /pattern/] + Wish $::thisProcess shares statements like \ + [list /someone/ claims $::thisProcess has pid /pid/] + Wish $::thisProcess shares statements like \ + [list /someone/ claims $::thisProcess detects tags /tags/ at /timestamp/ in time /aprilTime/] + Wish $::thisProcess shares statements like \ + [list /someone/ wishes /something/ is labelled /text/] + Wish $::thisProcess shares statements like \ + [list /someone/ wishes /something/ displays camera slice /slice/] + + proc subimage {im x y subwidth subheight} { + dict with im { + set x [expr {int($x)}] + set y [expr {int($y)}] + set subdata [expr {$data + ($y*$width + $x) * $components}] + dict create \ + width [int $subwidth] \ + height [int $subheight] \ + components $components \ + bytesPerRow $bytesPerRow \ + data [format 0x%x $subdata] + } + } + + When the camera frame is /grayFrame/ at /timestamp/ & \ + /process/ detects tags /prevTags/ at /something/ in time /something/ { + + if {$process eq $::thisProcess} { return } + + set tags [list] + set frameWidth [dict get $grayFrame width] + set frameHeight [dict get $grayFrame height] + set aprilTime 0 + foreach prevTag $prevTags { + set size [dict get $prevTag size] + set corners [dict get $prevTag corners] + set x [min {*}[lmap corner $corners {lindex $corner 0}]] + set y [min {*}[lmap corner $corners {lindex $corner 1}]] + set x1 [max {*}[lmap corner $corners {lindex $corner 0}]] + set y1 [max {*}[lmap corner $corners {lindex $corner 1}]] + + set x [max [- $x $size] 0] + set y [max [- $y $size] 0] + set x1 [min [+ $x1 $size] $frameWidth] + set y1 [min [+ $y1 $size] $frameHeight] + + set subimage [subimage $grayFrame $x $y [- $x1 $x] [- $y1 $y]] + set aprilTime [+ $aprilTime [baretime { + foreach tag [AprilTags::detect $subimage] { + dict with tag { + set center [vec2 add $center [list $x $y]] + set corners [lmap corner $corners {vec2 add $corner [list $x $y]}] + } + lappend tags $tag + } + }]] } + + # Wish 6 is labelled "[llength $prevTags] -> [llength $tags] ($aprilTime us)" + + Claim $::thisProcess detects tags $tags at $timestamp in time $aprilTime + # Wish 6 is labelled "\n\nStep time $::stepTime\n[join [lmap s [dict values [Statements::all]] {statement short $s}] "\n"]" } } -When /someone/ detects tags /tags/ at /timestamp/ in time /aprilTime/ { - 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] +# This cache is used to remember the last seen position of each tag, +# so that if the incremental detector blinks out, we still use the +# tag's last-found position from it, instead of the older position +# from the full detector, so as you move a tag its position doesn't +# glitch backward. +set ::tagsCache [dict create] +# TODO: Garbage-collect this cache. + +When the collected matches for [list /someone/ detects tags /tags/ at /timestamp/ in time /aprilTime/] are /matches/ { + set tagsSeen [dict create] + foreach match $matches { + set timestamp [dict get $match timestamp] + foreach tag [dict get $match tags] { + set id [dict get $tag id] + dict set tag timestamp $timestamp + + if {[dict exists $::tagsCache $id] && + [dict get $::tagsCache $id timestamp] > $timestamp} { + set tag [dict get $::tagsCache $id] + } else { + dict set ::tagsCache $id $tag + } + dict set tagsSeen $id $tag + } } -} -# 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] -# } -# } -# } -# } + dict for {id tag} $tagsSeen { + Claim tag $id has center [dict get $tag center] size [dict get $tag size] + Claim tag $id has corners [dict get $tag corners] + } + Claim the AprilTag time is [lmap m $matches {dict get $m aprilTime}] +} diff --git a/virtual-programs/camera.folk b/virtual-programs/camera.folk index 4f59a576..225a5880 100644 --- a/virtual-programs/camera.folk +++ b/virtual-programs/camera.folk @@ -20,7 +20,7 @@ On process { puts "Camera tid: [getTid] booting at [clock milliseconds]" - forever { + while true { set cameraTime [time { set grayFrame [Camera::grayFrame] }] @@ -28,6 +28,7 @@ On process { Claim the camera time is $cameraTime Claim the camera frame is $grayFrame at [clock milliseconds] } + Step } } diff --git a/virtual-programs/display.folk b/virtual-programs/display.folk index d5623d10..b0b878ca 100644 --- a/virtual-programs/display.folk +++ b/virtual-programs/display.folk @@ -60,7 +60,7 @@ On process { [list /someone/ claims the display time is /displayTime/] set ::frames [list] - forever { + while true { set displayList [list] foreach match [Statements::findMatches {/someone/ wishes display runs /command/ on layer /layer/}] { lappend displayList [list [dict get $match layer] [dict get $match command]] @@ -80,19 +80,22 @@ On process { } set displayCommands [lmap sublist [lsort -command lcomp $displayList] {lindex $sublist 1}] + set renderTime [baretime [list foreach command $displayCommands { {*}$command }]] set commitTime [baretime commitThenClearStaging] - set inLastSecond 0 + set framesInLastSecond 0 set now [clock milliseconds] lappend frames $now foreach frame $frames { if {$frame > $now - 1000} { - incr inLastSecond + incr framesInLastSecond } } - set frames [lreplace $frames 0 end-$inLastSecond] - Commit { Claim the display time is "render $renderTime us + commit $commitTime us ($inLastSecond fps)" } + set frames [lreplace $frames 0 end-$framesInLastSecond] + + Commit { Claim the display time is "render $renderTime us + commit $commitTime us ($::stepTime) ($framesInLastSecond fps)" } + Step } } # TODO: remove this compatibility hack diff --git a/virtual-programs/tags-and-calibration.folk b/virtual-programs/tags-and-calibration.folk index afb4aef4..b219b541 100644 --- a/virtual-programs/tags-and-calibration.folk +++ b/virtual-programs/tags-and-calibration.folk @@ -119,7 +119,8 @@ When (non-capturing) tag /tag/ has center /c/ size /size/ { } When (non-capturing) tag /tag/ is a tag { - puts "Added tag $tag" + puts "Added tag $tag" + On unmatch { puts "Removed tag $tag" } set tempPath "$::env(HOME)/folk-printed-programs/$tag.folk.temp" diff --git a/virtual-programs/time.folk b/virtual-programs/time.folk index 961b1863..1dff734d 100644 --- a/virtual-programs/time.folk +++ b/virtual-programs/time.folk @@ -1,3 +1,3 @@ When $::thisProcess has step count /t/ { - Claim the clock time is [/ [clock milliseconds] 1000.0] + Claim the clock time is [/ [clock milliseconds] 1000.0] } |