Skip to content

Commit b6e87ce

Browse files
erherh
committed
find cup automatically and crop automatically
1 parent c27090b commit b6e87ce

File tree

3 files changed

+103
-23
lines changed

3 files changed

+103
-23
lines changed

‎cmd/tools/cmd-tools.go‎

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,8 @@ import (
44
"context"
55
"flag"
66
"fmt"
7+
"image/png"
8+
"os"
79
"time"
810

911
"github.com/golang/geo/r3"
@@ -194,6 +196,22 @@ func realMain() error {
194196
logger.Infof("left : %v", left)
195197
logger.Infof("right: %v", right)
196198
return nil
199+
case "pour-glass-find-crop":
200+
box, err := vc.PourGlassFindCroppedRect(ctx)
201+
if err != nil {
202+
return err
203+
}
204+
logger.Infof("box: %v", box)
205+
img, err := vc.PourGlassFindCroppedImage(ctx, box)
206+
if err != nil {
207+
return err
208+
}
209+
file, err := os.Create("foo.png")
210+
if err != nil {
211+
return fmt.Errorf("couldn't create file ", err)
212+
}
213+
defer file.Close()
214+
return png.Encode(file, img)
197215
default:
198216
return fmt.Errorf("unknown command: %v", cmd)
199217
}

‎pour/config.go‎

Lines changed: 16 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -54,7 +54,6 @@ type Config struct {
5454
GlassPourMotionThreshold float64 `json:"glass_pour_motion_threshold"`
5555

5656
CupFinderService string `json:"cup_finder_service"` // find the cups on the table
57-
CupTopService string `json:"cup_top_service"` // to hone in on the cup
5857

5958
Positions map[string]ConfigStatePostions
6059

@@ -67,9 +66,10 @@ type Config struct {
6766
BottleHeight float64 `json:"bottle_height"`
6867
CupHeight float64 `json:"cup_height"`
6968

70-
BottleMotionService string `json:"bottle_motion_service"`
71-
CupMotionService string `json:"cup_motion_service"`
72-
PickQualityService string `json:"pick_quality_service"`
69+
BottleMotionService string `json:"bottle_motion_service"`
70+
CupMotionService string `json:"cup_motion_service"`
71+
PickQualityService string `json:"pick_quality_service"`
72+
PourGlassFindService string `json:"pour_glass_find_service"`
7373

7474
Loop bool `json:"loop"`
7575
}
@@ -89,6 +89,10 @@ func (cfg *Config) Validate(path string) ([]string, []string, error) {
8989
deps = append(deps, cfg.PickQualityService)
9090
}
9191

92+
if cfg.PourGlassFindService != "" {
93+
deps = append(deps, cfg.PourGlassFindService)
94+
}
95+
9296
if cfg.ArmName == "" {
9397
return nil, nil, fmt.Errorf("need an arm name")
9498
}
@@ -116,9 +120,6 @@ func (cfg *Config) Validate(path string) ([]string, []string, error) {
116120
if cfg.CupFinderService != "" {
117121
optionals = append(optionals, cfg.CupFinderService)
118122
}
119-
if cfg.CupTopService != "" {
120-
optionals = append(optionals, cfg.CupTopService)
121-
}
122123

123124
if cfg.BottleGripper != "" {
124125
deps = append(deps, cfg.BottleGripper)
@@ -156,16 +157,16 @@ type Pour1Components struct {
156157
CamVision vision.Service
157158

158159
CupFinder vision.Service
159-
CupTop vision.Service
160160

161161
Positions map[string]StagePositions
162162

163163
BottleGripper gripper.Gripper
164164
BottleArm arm.Arm
165165

166-
BottleMotionService motion.Service
167-
CupMotionService motion.Service
168-
PickQualityService vision.Service
166+
BottleMotionService motion.Service
167+
CupMotionService motion.Service
168+
PickQualityService vision.Service
169+
PourGlassFindService vision.Service
169170
}
170171

171172
func Pour1ComponentsFromDependencies(config *Config, deps resource.Dependencies) (*Pour1Components, error) {
@@ -220,15 +221,15 @@ func Pour1ComponentsFromDependencies(config *Config, deps resource.Dependencies)
220221
}
221222
}
222223

223-
if config.CupFinderService != "" {
224-
c.CupFinder, err = vision.FromDependencies(deps, config.CupFinderService)
224+
if config.PourGlassFindService != "" {
225+
c.PourGlassFindService, err = vision.FromDependencies(deps, config.PourGlassFindService)
225226
if err != nil {
226227
return nil, err
227228
}
228229
}
229230

230-
if config.CupTopService != "" {
231-
c.CupTop, err = vision.FromDependencies(deps, config.CupTopService)
231+
if config.CupFinderService != "" {
232+
c.CupFinder, err = vision.FromDependencies(deps, config.CupFinderService)
232233
if err != nil {
233234
return nil, err
234235
}

‎pour/vinocart.go‎

Lines changed: 69 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -21,11 +21,13 @@ import (
2121
"github.com/golang/geo/r3"
2222

2323
"go.viam.com/rdk/app"
24+
"go.viam.com/rdk/components/camera"
2425
"go.viam.com/rdk/components/switch"
2526
"go.viam.com/rdk/logging"
2627
"go.viam.com/rdk/pointcloud"
2728
"go.viam.com/rdk/referenceframe"
2829
"go.viam.com/rdk/resource"
30+
"go.viam.com/rdk/rimage"
2931
"go.viam.com/rdk/robot"
3032
"go.viam.com/rdk/services/generic"
3133
"go.viam.com/rdk/services/motion"
@@ -379,6 +381,14 @@ func (vc *VinoCart) checkPickQuality(ctx context.Context) error {
379381
return fmt.Errorf("bad pick %v", cs[0])
380382
}
381383

384+
func saveImageToDatasetFromCamera(ctx context.Context, cam camera.Camera, dataClient *app.DataClient, dataSetId string) error {
385+
imgs, _, err := cam.Images(ctx)
386+
if err != nil {
387+
return err
388+
}
389+
return saveImageToDataset(ctx, cam.Name(), imgs[0].Image, dataClient, dataSetId)
390+
}
391+
382392
func saveImageToDataset(ctx context.Context, component resource.Name, img image.Image, dataClient *app.DataClient, dataSetId string) error {
383393
pid := os.Getenv("VIAM_MACHINE_PART_ID")
384394
if pid == "" {
@@ -767,24 +777,58 @@ func (vc *VinoCart) goTo(ctx context.Context, poss ...toggleswitch.Switch) error
767777
return multierr.Combine(errors...)
768778
}
769779

770-
func (vc *VinoCart) DebugGetGlassPourCamImage(ctx context.Context, loopNumber int) (image.Image, string, error) {
771-
nimgs, _, err := vc.c.GlassPourCam.Images(ctx)
780+
func (vc *VinoCart) PourGlassFindCroppedRect(ctx context.Context) (*image.Rectangle, error) {
781+
detections, err := vc.c.PourGlassFindService.DetectionsFromCamera(ctx, "", nil)
772782
if err != nil {
773-
return nil, "", err
783+
return nil, err
784+
}
785+
786+
if len(detections) == 0 {
787+
return nil, fmt.Errorf("did not find glass to monitor pour")
788+
}
789+
790+
return detections[0].BoundingBox(), nil
791+
}
792+
793+
func (vc *VinoCart) PourGlassFindCroppedImage(ctx context.Context, r *image.Rectangle) (image.Image, error) {
794+
795+
imgs, _, err := vc.c.GlassPourCam.Images(ctx)
796+
if err != nil {
797+
return nil, err
798+
}
799+
800+
img := imgs[0].Image
801+
802+
lazy, ok := img.(*rimage.LazyEncodedImage)
803+
if ok {
804+
img, err = lazy.DecodedImage()
805+
if err != nil {
806+
return nil, err
807+
}
774808
}
775-
if len(nimgs) == 0 {
776-
return nil, "", fmt.Errorf("GlassPourCam returned no images")
809+
810+
return img.(subImager).SubImage(*r), nil
811+
}
812+
813+
type subImager interface {
814+
SubImage(r image.Rectangle) image.Image
815+
}
816+
817+
func (vc *VinoCart) DebugGetGlassPourCamImage(ctx context.Context, box *image.Rectangle, loopNumber int) (image.Image, string, error) {
818+
img, err := vc.PourGlassFindCroppedImage(ctx, box)
819+
if err != nil {
820+
return nil, "", err
777821
}
778822

779823
fn := ""
780824
if loopNumber >= 0 {
781-
fn, err = saveImage(nimgs[0].Image, loopNumber)
825+
fn, err = saveImage(img, loopNumber)
782826
if err != nil {
783827
return nil, "", err
784828
}
785829
}
786830

787-
return nimgs[0].Image, fn, nil
831+
return img, fn, nil
788832
}
789833

790834
func (vc *VinoCart) Pour(ctx context.Context) error {
@@ -817,6 +861,23 @@ func (vc *VinoCart) Pour(ctx context.Context) error {
817861
wg := sync.WaitGroup{}
818862
wg.Add(1)
819863

864+
if vc.dataClient != nil && vc.c.GlassPourCam != nil {
865+
vc.logger.Infof("uploading image to dataset for cup finding")
866+
wg.Add(1)
867+
go func() {
868+
defer wg.Done()
869+
err := saveImageToDatasetFromCamera(context.Background(), vc.c.GlassPourCam, vc.dataClient, "683d1210c83b3f3823ec70ff")
870+
if err != nil {
871+
vc.logger.Errorf("error saving cup cam to data set: %v", err)
872+
}
873+
}()
874+
}
875+
876+
box, err := vc.PourGlassFindCroppedRect(ctx)
877+
if err != nil {
878+
return err
879+
}
880+
820881
go func() {
821882
defer wg.Done()
822883
err := vc.doPourMotion(ctx, pourContext)
@@ -840,7 +901,7 @@ func (vc *VinoCart) Pour(ctx context.Context) error {
840901
for time.Since(start) < totalTime {
841902
loopStart := time.Now()
842903

843-
img, fn, err := vc.DebugGetGlassPourCamImage(ctx /* loopNumber */, -1)
904+
img, fn, err := vc.DebugGetGlassPourCamImage(ctx /* loopNumber */, box, -1)
844905
if err != nil {
845906
return err
846907
}

0 commit comments

Comments
 (0)