diff --git a/components/camera/videosource/query.go b/components/camera/videosource/query.go new file mode 100644 index 00000000000..4fd9361af85 --- /dev/null +++ b/components/camera/videosource/query.go @@ -0,0 +1,225 @@ +package videosource + +import ( + "math" + "strings" + "time" + + "github.com/pion/mediadevices" + "github.com/pion/mediadevices/pkg/driver" + "github.com/pion/mediadevices/pkg/driver/availability" + "github.com/pion/mediadevices/pkg/driver/camera" + "github.com/pion/mediadevices/pkg/io/video" + "github.com/pion/mediadevices/pkg/prop" + "github.com/pkg/errors" + + "go.viam.com/rdk/logging" +) + +// Below is adapted from github.com/pion/mediadevices. +// It is further adapted from gostream's query.go +// However, this is the minimum code needed for webcam to work, placed in this directory. +// This vastly improves the debugging and feature development experience, by not over-DRY-ing. + +// GetNamedVideoSource attempts to find a device (not a screen) by the given name. +// If name is empty, it finds any device. +func getReaderAndDriver( + name string, + constraints mediadevices.MediaStreamConstraints, + logger logging.Logger, +) (video.Reader, driver.Driver, error) { + var ptr *string + if name == "" { + ptr = nil + } else { + ptr = &name + } + d, selectedMedia, err := getUserVideoDriver(constraints, ptr, logger) + if err != nil { + return nil, nil, err + } + reader, err := newReaderFromDriver(d, selectedMedia) + if err != nil { + return nil, nil, err + } + return reader, d, nil +} + +func getUserVideoDriver( + constraints mediadevices.MediaStreamConstraints, + label *string, + logger logging.Logger, +) (driver.Driver, prop.Media, error) { + var videoConstraints mediadevices.MediaTrackConstraints + if constraints.Video != nil { + constraints.Video(&videoConstraints) + } + return selectVideo(videoConstraints, label, logger) +} + +func newReaderFromDriver( + videoDriver driver.Driver, + mediaProp prop.Media, +) (video.Reader, error) { + recorder, ok := videoDriver.(driver.VideoRecorder) + if !ok { + return nil, errors.New("driver not a driver.VideoRecorder") + } + + if ok, err := driver.IsAvailable(videoDriver); !errors.Is(err, availability.ErrUnimplemented) && !ok { + return nil, errors.Wrap(err, "video driver not available") + } else if driverStatus := videoDriver.Status(); driverStatus != driver.StateClosed { + return nil, errors.New("video driver in use") + } else if err := videoDriver.Open(); err != nil { + return nil, errors.Wrap(err, "cannot open video driver") + } + + mediaProp.DiscardFramesOlderThan = time.Second + reader, err := recorder.VideoRecord(mediaProp) + if err != nil { + return nil, err + } + return reader, nil +} + +func labelFilter(target string, useSep bool) driver.FilterFn { + return driver.FilterFn(func(d driver.Driver) bool { + if !useSep { + return d.Info().Label == target + } + labels := strings.Split(d.Info().Label, camera.LabelSeparator) + for _, label := range labels { + if label == target { + return true + } + } + return false + }) +} + +func selectVideo( + constraints mediadevices.MediaTrackConstraints, + label *string, + logger logging.Logger, +) (driver.Driver, prop.Media, error) { + return selectBestDriver(getVideoFilterBase(), getVideoFilter(label), constraints, logger) +} + +func getVideoFilterBase() driver.FilterFn { + typeFilter := driver.FilterVideoRecorder() + notScreenFilter := driver.FilterNot(driver.FilterDeviceType(driver.Screen)) + return driver.FilterAnd(typeFilter, notScreenFilter) +} + +func getVideoFilter(label *string) driver.FilterFn { + filter := getVideoFilterBase() + if label != nil { + filter = driver.FilterAnd(filter, labelFilter(*label, true)) + } + return filter +} + +// select implements SelectSettings algorithm. +// Reference: https://w3c.github.io/mediacapture-main/#dfn-selectsettings +func selectBestDriver( + baseFilter driver.FilterFn, + filter driver.FilterFn, + constraints mediadevices.MediaTrackConstraints, + logger logging.Logger, +) (driver.Driver, prop.Media, error) { + var bestDriver driver.Driver + var bestProp prop.Media + minFitnessDist := math.Inf(1) + + baseDrivers := driver.GetManager().Query(baseFilter) + logger.Debugw("before specific filter, we found the following drivers", "count", len(baseDrivers)) + for _, d := range baseDrivers { + logger.Debugw(d.Info().Label, "priority", float32(d.Info().Priority), "type", d.Info().DeviceType) + } + + driverProperties := queryDriverProperties(filter, logger) + if len(driverProperties) == 0 { + logger.Debugw("found no drivers matching filter") + } else { + logger.Debugw("found drivers matching specific filter", "count", len(driverProperties)) + } + for d, props := range driverProperties { + priority := float64(d.Info().Priority) + logger.Debugw( + "considering driver", + "label", d.Info().Label, + "priority", priority) + for _, p := range props { + fitnessDist, ok := constraints.MediaConstraints.FitnessDistance(p) + if !ok { + logger.Debugw("driver does not satisfy any constraints", "label", d.Info().Label) + continue + } + fitnessDistWithPriority := fitnessDist - priority + logger.Debugw( + "driver properties satisfy some constraints", + "label", d.Info().Label, + "props", p, + "distance", fitnessDist, + "distance_with_priority", fitnessDistWithPriority) + if fitnessDistWithPriority < minFitnessDist { + minFitnessDist = fitnessDistWithPriority + bestDriver = d + bestProp = p + } + } + } + + if bestDriver == nil { + return nil, prop.Media{}, errors.New("failed to find the best driver that fits the constraints") + } + + logger.Debugw("winning driver", "label", bestDriver.Info().Label, "props", bestProp) + selectedMedia := prop.Media{} + selectedMedia.MergeConstraints(constraints.MediaConstraints) + selectedMedia.Merge(bestProp) + return bestDriver, selectedMedia, nil +} + +func queryDriverProperties( + filter driver.FilterFn, + logger logging.Logger, +) map[driver.Driver][]prop.Media { + var needToClose []driver.Driver + drivers := driver.GetManager().Query(filter) + m := make(map[driver.Driver][]prop.Media) + + for _, d := range drivers { + var status string + isAvailable, err := driver.IsAvailable(d) + if errors.Is(err, availability.ErrUnimplemented) { + s := d.Status() + status = string(s) + isAvailable = s == driver.StateClosed + } else if err != nil { + status = err.Error() + } + + if isAvailable { + err := d.Open() + if err != nil { + logger.Debugw("error opening driver for querying", "error", err) + // Skip this driver if we failed to open because we can't get the properties + continue + } + needToClose = append(needToClose, d) + m[d] = d.Properties() + } else { + logger.Debugw("driver not available", "name", d.Info().Name, "label", d.Info().Label, "status", status) + } + } + + for _, d := range needToClose { + // Since it was closed, we should close it to avoid a leak + if err := d.Close(); err != nil { + logger.Errorw("error closing driver", "error", err) + } + } + + return m +} diff --git a/components/camera/videosource/webcam.go b/components/camera/videosource/webcam.go index e71adaa288b..4e3093f4c13 100644 --- a/components/camera/videosource/webcam.go +++ b/components/camera/videosource/webcam.go @@ -1,4 +1,4 @@ -// Package videosource implements various camera models including webcam +// Package videosource implements webcam. It should be renamed webcam. package videosource import ( @@ -6,25 +6,22 @@ import ( _ "embed" "encoding/json" "fmt" - "image" "path/filepath" "strings" "sync" "time" "github.com/pion/mediadevices" - "github.com/pion/mediadevices/pkg/driver" + driverutils "github.com/pion/mediadevices/pkg/driver" "github.com/pion/mediadevices/pkg/driver/availability" mediadevicescamera "github.com/pion/mediadevices/pkg/driver/camera" "github.com/pion/mediadevices/pkg/frame" + "github.com/pion/mediadevices/pkg/io/video" "github.com/pion/mediadevices/pkg/prop" "github.com/pkg/errors" - "go.uber.org/multierr" - pb "go.viam.com/api/component/camera/v1" goutils "go.viam.com/utils" "go.viam.com/rdk/components/camera" - "go.viam.com/rdk/gostream" "go.viam.com/rdk/logging" "go.viam.com/rdk/pointcloud" "go.viam.com/rdk/resource" @@ -39,12 +36,11 @@ var ModelWebcam = resource.DefaultModelFamily.WithModel("webcam") //go:embed data/intrinsics.json var intrinsics []byte -var data map[string]transform.PinholeCameraIntrinsics - -// getVideoDrivers is a helper callback passed to the registered Discover func to get all video drivers. -func getVideoDrivers() []driver.Driver { - return driver.GetManager().Query(driver.FilterVideoRecorder()) -} +var ( + errClosed = errors.New("camera has been closed") + errDisconnected = errors.New("camera is disconnected; please try again in a few moments") + data map[string]transform.PinholeCameraIntrinsics +) func init() { resource.RegisterComponent( @@ -52,90 +48,12 @@ func init() { ModelWebcam, resource.Registration[camera.Camera, *WebcamConfig]{ Constructor: NewWebcam, - Discover: func(ctx context.Context, logger logging.Logger, extra map[string]interface{}) (interface{}, error) { - return Discover(ctx, getVideoDrivers, logger) - }, }) if err := json.Unmarshal(intrinsics, &data); err != nil { logging.Global().Errorw("cannot parse intrinsics json", "error", err) } } -// getProperties is a helper func for webcam discovery that returns the Media properties of a specific driver. -// It is NOT related to the GetProperties camera proto API. -func getProperties(d driver.Driver) (_ []prop.Media, err error) { - // Need to open driver to get properties - if d.Status() == driver.StateClosed { - errOpen := d.Open() - if errOpen != nil { - return nil, errOpen - } - defer func() { - if errClose := d.Close(); errClose != nil { - err = errClose - } - }() - } - return d.Properties(), err -} - -// Discover webcam attributes. -func Discover(ctx context.Context, getDrivers func() []driver.Driver, logger logging.Logger) (*pb.Webcams, error) { - mediadevicescamera.Initialize() - var webcams []*pb.Webcam - drivers := getDrivers() - for _, d := range drivers { - driverInfo := d.Info() - - props, err := getProperties(d) - if len(props) == 0 { - logger.CDebugw(ctx, "no properties detected for driver, skipping discovery...", "driver", driverInfo.Label) - continue - } else if err != nil { - logger.CDebugw(ctx, "cannot access driver properties, skipping discovery...", "driver", driverInfo.Label, "error", err) - continue - } - - if d.Status() == driver.StateRunning { - logger.CDebugw(ctx, "driver is in use, skipping discovery...", "driver", driverInfo.Label) - continue - } - - labelParts := strings.Split(driverInfo.Label, mediadevicescamera.LabelSeparator) - label := labelParts[0] - - name, id := func() (string, string) { - nameParts := strings.Split(driverInfo.Name, mediadevicescamera.LabelSeparator) - if len(nameParts) > 1 { - return nameParts[0], nameParts[1] - } - // fallback to the label if the name does not have an any additional parts to use. - return nameParts[0], label - }() - - wc := &pb.Webcam{ - Name: name, - Id: id, - Label: label, - Status: string(d.Status()), - Properties: make([]*pb.Property, 0, len(d.Properties())), - } - - for _, prop := range props { - pbProp := &pb.Property{ - WidthPx: int32(prop.Video.Width), - HeightPx: int32(prop.Video.Height), - FrameRate: prop.Video.FrameRate, - FrameFormat: string(prop.Video.FrameFormat), - } - wc.Properties = append(wc.Properties, pbProp) - } - webcams = append(webcams, wc) - } - - return &pb.Webcams{Webcams: webcams}, nil -} - // WebcamConfig is the native config attribute struct for webcams. type WebcamConfig struct { CameraParameters *transform.PinholeCameraIntrinsics `json:"intrinsic_parameters,omitempty"` @@ -163,14 +81,6 @@ func (c WebcamConfig) Validate(path string) ([]string, error) { return []string{}, nil } -// needsDriverReinit is a helper to check for config diffs and returns true iff the driver needs to be reinitialized. -func (c WebcamConfig) needsDriverReinit(other WebcamConfig) bool { - return !(c.Format == other.Format && - c.Path == other.Path && - c.Width == other.Width && - c.Height == other.Height) -} - // makeConstraints is a helper that returns constraints to mediadevices in order to find and make a video source. // Constraints are specifications for the video stream such as frame format, resolution etc. func makeConstraints(conf *WebcamConfig, logger logging.Logger) mediadevices.MediaStreamConstraints { @@ -215,108 +125,70 @@ func makeConstraints(conf *WebcamConfig, logger logging.Logger) mediadevices.Med } } -// getNamedVideoSource is a helper function for trying to open a webcam that attempts to -// find a video device (not a screen) by the given name. -// -// First it will try to use the path name after evaluating any symbolic links. If -// evaluation fails, it will try to use the path name as provided. -func getNamedVideoSource( +// findReaderAndDriver finds a video device and returns an image reader and the driver instance, +// as well as the path to the driver. +func findReaderAndDriver( + conf *WebcamConfig, path string, - fromLabel bool, - constraints mediadevices.MediaStreamConstraints, logger logging.Logger, -) (gostream.MediaSource[image.Image], error) { - if !fromLabel { +) (video.Reader, driverutils.Driver, string, error) { + mediadevicescamera.Initialize() + constraints := makeConstraints(conf, logger) + + // Handle specific path + if path != "" { resolvedPath, err := filepath.EvalSymlinks(path) if err == nil { path = resolvedPath } - } - return gostream.GetNamedVideoSource(filepath.Base(path), constraints, logger) -} - -// tryWebcamOpen is a helper for finding and making the video source that uses getNamedVideoSource to try and find -// a video device (gostream.MediaSource). If successful, it will wrap that MediaSource in a VideoSource. -func tryWebcamOpen( - ctx context.Context, - conf *WebcamConfig, - path string, - fromLabel bool, - constraints mediadevices.MediaStreamConstraints, - logger logging.Logger, -) (gostream.VideoSource, error) { - source, err := getNamedVideoSource(path, fromLabel, constraints, logger) - if err != nil { - return nil, err - } + reader, driver, err := getReaderAndDriver(filepath.Base(path), constraints, logger) + if err != nil { + return nil, nil, "", err + } - if conf.Width != 0 && conf.Height != 0 { - img, release, err := gostream.ReadMedia(ctx, source) + img, release, err := reader.Read() if release != nil { defer release() } if err != nil { - return nil, err - } - if img.Bounds().Dx() != conf.Width || img.Bounds().Dy() != conf.Height { - return nil, errors.Errorf("requested width and height (%dx%d) are not available for this webcam"+ - " (closest driver found by gostream supports resolution %dx%d)", - conf.Width, conf.Height, img.Bounds().Dx(), img.Bounds().Dy()) + return nil, nil, "", err } - } - return source, nil -} - -// getPathFromVideoSource is a helper function for finding and making the video source that -// returns the path derived from the underlying driver via MediaSource or an empty string if a path is not found. -func getPathFromVideoSource(src gostream.VideoSource, logger logging.Logger) string { - labels, err := gostream.LabelsFromMediaSource[image.Image, prop.Video](src) - if err != nil || len(labels) == 0 { - logger.Errorw("could not get labels from media source", "error", err) - return "" - } - return labels[0] // path is always the first element -} - -// findAndMakeVideoSource finds a video device and returns a video source with that video device as the source. -func findAndMakeVideoSource( - ctx context.Context, - conf *WebcamConfig, - path string, - logger logging.Logger, -) (gostream.VideoSource, string, error) { - mediadevicescamera.Initialize() - constraints := makeConstraints(conf, logger) - if path != "" { - cam, err := tryWebcamOpen(ctx, conf, path, false, constraints, logger) - if err != nil { - return nil, "", errors.Wrap(err, "cannot open webcam") + if conf.Width != 0 && conf.Height != 0 { + if img.Bounds().Dx() != conf.Width || img.Bounds().Dy() != conf.Height { + return nil, nil, "", errors.Errorf("requested width and height (%dx%d) are not available for this webcam"+ + " (closest driver found supports resolution %dx%d)", + conf.Width, conf.Height, img.Bounds().Dx(), img.Bounds().Dy()) + } } - return cam, path, nil + return reader, driver, path, nil } - source, err := gostream.GetAnyVideoSource(constraints, logger) + // Handle "any" path + reader, driver, err := getReaderAndDriver("", constraints, logger) if err != nil { - return nil, "", errors.Wrap(err, "found no webcams") + return nil, nil, "", errors.Wrap(err, "found no webcams") } - - if path == "" { - path = getPathFromVideoSource(source, logger) + labels := strings.Split(driver.Info().Label, mediadevicescamera.LabelSeparator) + if len(labels) == 0 { + logger.Error("no labels parsed from driver") + return nil, nil, "", nil } + path = labels[0] // path is always the first element - return source, path, nil + return reader, driver, path, nil } -// webcam is a video driver wrapper camera that ensures its underlying source stays connected. +// webcam is a video driver wrapper camera that ensures its underlying driver stays connected. type webcam struct { resource.Named mu sync.RWMutex hasLoggedIntrinsicsInfo bool - underlyingSource gostream.VideoSource - exposedSwapper gostream.HotSwappableVideoSource - exposedProjector camera.Camera + cameraModel transform.PinholeCameraModel + + reader video.Reader + driver driverutils.Driver // this is returned to us as a label in mediadevices but our config // treats it as a video path. @@ -354,16 +226,6 @@ func NewWebcam( return cam, nil } -// noopCloser overwrites the actual close method so that the real close method isn't called on Reconfigure. -// TODO(hexbabe): https://viam.atlassian.net/browse/RSDK-9264 -type noopCloser struct { - gostream.VideoSource -} - -func (n *noopCloser) Close(ctx context.Context) error { - return nil -} - func (c *webcam) Reconfigure( ctx context.Context, _ resource.Dependencies, @@ -377,24 +239,14 @@ func (c *webcam) Reconfigure( c.mu.Lock() defer c.mu.Unlock() - cameraModel := camera.NewPinholeModelWithBrownConradyDistortion(newConf.CameraParameters, newConf.DistortionParameters) - projector, err := camera.WrapVideoSourceWithProjector( - ctx, - &noopCloser{c}, - &cameraModel, - camera.ColorStream, - ) - if err != nil { - return err - } + c.cameraModel = camera.NewPinholeModelWithBrownConradyDistortion(newConf.CameraParameters, newConf.DistortionParameters) - needDriverReinit := c.conf.needsDriverReinit(*newConf) - if c.exposedProjector != nil { - goutils.UncheckedError(c.exposedProjector.Close(ctx)) - } - c.exposedProjector = projector + driverReinitNotNeeded := c.conf.Format == newConf.Format && + c.conf.Path == newConf.Path && + c.conf.Width == newConf.Width && + c.conf.Height == newConf.Height - if c.underlyingSource != nil && !needDriverReinit { + if c.driver != nil && c.reader != nil && driverReinitNotNeeded { c.conf = *newConf return nil } @@ -412,59 +264,38 @@ func (c *webcam) Reconfigure( return nil } -// MediaProperties returns the mediadevices Video properties of the underlying camera. -// It fulfills the MediaPropertyProvider interface. -func (c *webcam) MediaProperties(ctx context.Context) (prop.Video, error) { - c.mu.RLock() - defer c.mu.RUnlock() - if c.underlyingSource == nil { - return prop.Video{}, errors.New("no configured camera") - } - if provider, ok := c.underlyingSource.(gostream.VideoPropertyProvider); ok { - return provider.MediaProperties(ctx) - } - return prop.Video{}, nil -} - -// isCameraConnected is a helper for the alive-ness monitoring. +// isCameraConnected is a helper for monitoring connectivity to the driver. func (c *webcam) isCameraConnected() (bool, error) { c.mu.RLock() defer c.mu.RUnlock() - if c.underlyingSource == nil { + if c.driver == nil { return true, errors.New("no configured camera") } - d, err := gostream.DriverFromMediaSource[image.Image, prop.Video](c.underlyingSource) - if err != nil { - return true, errors.Wrap(err, "cannot get driver from media source") - } // TODO(RSDK-1959): this only works for linux - _, err = driver.IsAvailable(d) + _, err := driverutils.IsAvailable(c.driver) return !errors.Is(err, availability.ErrNoDevice), nil } // reconnectCamera tries to reconnect the camera to a driver that matches the config. // Assumes a write lock is held. func (c *webcam) reconnectCamera(conf *WebcamConfig) error { - if c.underlyingSource != nil { + if c.driver != nil { c.logger.Debug("closing current camera") - if err := c.underlyingSource.Close(c.cancelCtx); err != nil { - c.logger.Errorw("failed to close currents camera", "error", err) + if err := c.driver.Close(); err != nil { + c.logger.Errorw("failed to close current camera", "error", err) } - c.underlyingSource = nil + c.driver = nil + c.reader = nil } - newSrc, foundLabel, err := findAndMakeVideoSource(c.cancelCtx, conf, c.targetPath, c.logger) + reader, driver, foundLabel, err := findReaderAndDriver(conf, c.targetPath, c.logger) if err != nil { return errors.Wrap(err, "failed to find camera") } - if c.exposedSwapper == nil { - c.exposedSwapper = gostream.NewHotSwappableVideoSource(newSrc) - } else { - c.exposedSwapper.Swap(newSrc) - } - c.underlyingSource = newSrc + c.reader = reader + c.driver = driver c.disconnected = false c.closed = false if c.targetPath == "" { @@ -535,10 +366,7 @@ func (c *webcam) Monitor() { } func (c *webcam) Images(ctx context.Context) ([]camera.NamedImage, resource.ResponseMetadata, error) { - if c, ok := c.underlyingSource.(camera.ImagesSource); ok { - return c.Images(ctx) - } - img, release, err := camera.ReadImage(ctx, c.underlyingSource) + img, release, err := c.reader.Read() if err != nil { return nil, resource.ResponseMetadata{}, errors.Wrap(err, "monitoredWebcam: call to get Images failed") } @@ -561,17 +389,14 @@ func (c *webcam) ensureActive() error { return nil } -func (c *webcam) Stream(ctx context.Context, errHandlers ...gostream.ErrorHandler) (gostream.VideoStream, error) { - c.mu.RLock() - defer c.mu.RUnlock() - if err := c.ensureActive(); err != nil { - return nil, err - } - return c.exposedSwapper.Stream(ctx, errHandlers...) -} - func (c *webcam) Image(ctx context.Context, mimeType string, extra map[string]interface{}) ([]byte, camera.ImageMetadata, error) { - img, release, err := camera.ReadImage(ctx, c.underlyingSource) + if c.closed { + return nil, camera.ImageMetadata{}, errClosed + } + if c.reader == nil { + return nil, camera.ImageMetadata{}, errors.New("underlying reader is nil") + } + img, release, err := c.reader.Read() if err != nil { return nil, camera.ImageMetadata{}, err } @@ -588,99 +413,31 @@ func (c *webcam) Image(ctx context.Context, mimeType string, extra map[string]in } func (c *webcam) NextPointCloud(ctx context.Context) (pointcloud.PointCloud, error) { - c.mu.RLock() - defer c.mu.RUnlock() - if err := c.ensureActive(); err != nil { - return nil, err - } - return c.exposedProjector.NextPointCloud(ctx) -} - -// driverInfo gets the mediadevices Info struct containing info such as name and device type of the given driver. -// It is a helper func for serving Properties. -func (c *webcam) driverInfo() (driver.Info, error) { - c.mu.RLock() - defer c.mu.RUnlock() - if c.underlyingSource == nil { - return driver.Info{}, errors.New("no underlying source found in camera") - } - d, err := gostream.DriverFromMediaSource[image.Image, prop.Video](c.underlyingSource) - if err != nil { - return driver.Info{}, errors.Wrap(err, "cannot get driver from media source") - } - return d.Info(), nil + return nil, errors.New("NextPointCloud unimplemented") } func (c *webcam) Properties(ctx context.Context) (camera.Properties, error) { c.mu.RLock() defer c.mu.RUnlock() - if err := c.ensureActive(); err != nil { - return camera.Properties{}, err - } - props, err := c.exposedProjector.Properties(ctx) - if err != nil { + if err := c.ensureActive(); err != nil { return camera.Properties{}, err } - // Looking for intrinsics in map built using viam camera - // calibration here https://github.com/viam-labs/camera-calibration/tree/main - if props.IntrinsicParams == nil { - dInfo, err := c.driverInfo() - if err != nil { - if !c.hasLoggedIntrinsicsInfo { - c.logger.CErrorw(ctx, "can't find driver info for camera") - c.hasLoggedIntrinsicsInfo = true - } - } - - cameraIntrinsics, exists := data[dInfo.Name] - if !exists { - if !c.hasLoggedIntrinsicsInfo { - c.logger.CInfo(ctx, "camera model not found in known camera models for: ", dInfo.Name, ". returning "+ - "properties without intrinsics") - c.hasLoggedIntrinsicsInfo = true - } - return props, nil - } - if c.conf.Width != 0 { - if c.conf.Width != cameraIntrinsics.Width { - if !c.hasLoggedIntrinsicsInfo { - c.logger.CInfo(ctx, "camera model found in known camera models for: ", dInfo.Name, " but "+ - "intrinsics width doesn't match configured image width") - c.hasLoggedIntrinsicsInfo = true - } - return props, nil - } - } - if c.conf.Height != 0 { - if c.conf.Height != cameraIntrinsics.Height { - if !c.hasLoggedIntrinsicsInfo { - c.logger.CInfo(ctx, "camera model found in known camera models for: ", dInfo.Name, " but "+ - "intrinsics height doesn't match configured image height") - c.hasLoggedIntrinsicsInfo = true - } - return props, nil - } - } - if !c.hasLoggedIntrinsicsInfo { - c.logger.CInfo(ctx, "Intrinsics are known for camera model: ", dInfo.Name, ". adding intrinsics "+ - "to camera properties") - c.hasLoggedIntrinsicsInfo = true - } - props.IntrinsicParams = &cameraIntrinsics - if c.conf.FrameRate > 0 { - props.FrameRate = c.conf.FrameRate - } + var frameRate float32 + if c.conf.FrameRate > 0 { + frameRate = c.conf.FrameRate } - return props, nil + return camera.Properties{ + SupportsPCD: false, + ImageType: camera.ColorStream, + IntrinsicParams: c.cameraModel.PinholeCameraIntrinsics, + DistortionParams: c.cameraModel.Distortion, + MimeTypes: []string{utils.MimeTypeJPEG, utils.MimeTypePNG, utils.MimeTypeRawRGBA}, + FrameRate: frameRate, + }, nil } -var ( - errClosed = errors.New("camera has been closed") - errDisconnected = errors.New("camera is disconnected; please try again in a few moments") -) - func (c *webcam) Close(ctx context.Context) error { c.mu.Lock() if c.closed { @@ -692,15 +449,5 @@ func (c *webcam) Close(ctx context.Context) error { c.cancel() c.activeBackgroundWorkers.Wait() - var err error - if c.exposedSwapper != nil { - err = multierr.Combine(err, c.exposedSwapper.Close(ctx)) - } - if c.exposedProjector != nil { - err = multierr.Combine(err, c.exposedProjector.Close(ctx)) - } - if c.underlyingSource != nil { - err = multierr.Combine(err, c.underlyingSource.Close(ctx)) - } - return err + return c.driver.Close() } diff --git a/components/camera/videosource/webcam_e2e_test.go b/components/camera/videosource/webcam_e2e_test.go index 7e5537a1c11..dd373edcded 100644 --- a/components/camera/videosource/webcam_e2e_test.go +++ b/components/camera/videosource/webcam_e2e_test.go @@ -27,47 +27,6 @@ func findWebcam(t *testing.T, webcams []*pb.Webcam, name string) *pb.Webcam { return nil } -func TestWebcamDiscovery(t *testing.T) { - logger := logging.NewTestLogger(t) - - reg, ok := resource.LookupRegistration(camera.API, videosource.ModelWebcam) - test.That(t, ok, test.ShouldBeTrue) - - ctx := context.Background() - discoveries, err := reg.Discover(ctx, logger) - test.That(t, err, test.ShouldBeNil) - - webcams, ok := discoveries.(*pb.Webcams) - test.That(t, ok, test.ShouldBeTrue) - webcamsLen := len(webcams.Webcams) - - // Video capture and overlay minor numbers range == [0, 63] - // Start from the end of the range to avoid conflicts with other devices - // Source: https://www.kernel.org/doc/html/v4.9/media/uapi/v4l/diff-v4l.html - config, err := vcamera.Builder(logger). - NewCamera(62, "Lo Res Webcam", vcamera.Resolution{Width: 640, Height: 480}). - NewCamera(63, "Hi Res Webcam", vcamera.Resolution{Width: 1280, Height: 720}). - Stream() - - test.That(t, err, test.ShouldBeNil) - defer config.Shutdown() - - discoveries, err = reg.Discover(ctx, logger) - test.That(t, err, test.ShouldBeNil) - - webcams, ok = discoveries.(*pb.Webcams) - test.That(t, ok, test.ShouldBeTrue) - test.That(t, len(webcams.Webcams), test.ShouldEqual, webcamsLen+2) - - webcam := findWebcam(t, webcams.Webcams, "Hi Res Webcam") - test.That(t, webcam.Properties[0].WidthPx, test.ShouldEqual, 1280) - test.That(t, webcam.Properties[0].HeightPx, test.ShouldEqual, 720) - - webcam = findWebcam(t, webcams.Webcams, "Lo Res Webcam") - test.That(t, webcam.Properties[0].WidthPx, test.ShouldEqual, 640) - test.That(t, webcam.Properties[0].HeightPx, test.ShouldEqual, 480) -} - func newWebcamConfig(name, path string) resource.Config { conf := resource.NewEmptyConfig( resource.NewName(camera.API, name), diff --git a/components/camera/videosource/webcam_test.go b/components/camera/videosource/webcam_test.go index e72a28f0625..db782c0475c 100644 --- a/components/camera/videosource/webcam_test.go +++ b/components/camera/videosource/webcam_test.go @@ -1,60 +1,13 @@ package videosource_test import ( - "context" "testing" - "github.com/pion/mediadevices/pkg/driver" - "github.com/pion/mediadevices/pkg/prop" "go.viam.com/test" "go.viam.com/rdk/components/camera/videosource" - "go.viam.com/rdk/logging" ) -// fakeDriver is a driver has a label and media properties. -type fakeDriver struct { - label string - props []prop.Media -} - -func (d *fakeDriver) Open() error { return nil } -func (d *fakeDriver) Properties() []prop.Media { return d.props } -func (d *fakeDriver) ID() string { return d.label } -func (d *fakeDriver) Info() driver.Info { return driver.Info{Label: d.label} } -func (d *fakeDriver) Status() driver.State { return "some state" } -func (d *fakeDriver) Close() error { return nil } - -func newFakeDriver(label string, props []prop.Media) driver.Driver { - return &fakeDriver{label: label, props: props} -} - -func testGetDrivers() []driver.Driver { - props := prop.Media{ - Video: prop.Video{Width: 320, Height: 240, FrameFormat: "some format", FrameRate: 30.0}, - } - withProps := newFakeDriver("some label", []prop.Media{props}) - withoutProps := newFakeDriver("another label", []prop.Media{}) - return []driver.Driver{withProps, withoutProps} -} - -func TestDiscoveryWebcam(t *testing.T) { - logger := logging.NewTestLogger(t) - resp, err := videosource.Discover(context.Background(), testGetDrivers, logger) - - test.That(t, err, test.ShouldBeNil) - test.That(t, resp.Webcams, test.ShouldHaveLength, 1) - test.That(t, resp.Webcams[0].Label, test.ShouldResemble, "some label") - test.That(t, resp.Webcams[0].Status, test.ShouldResemble, "some state") - - respProps := resp.Webcams[0].Properties - test.That(t, respProps, test.ShouldHaveLength, 1) - test.That(t, respProps[0].WidthPx, test.ShouldResemble, int32(320)) - test.That(t, respProps[0].HeightPx, test.ShouldResemble, int32(240)) - test.That(t, respProps[0].FrameFormat, test.ShouldResemble, "some format") - test.That(t, respProps[0].FrameRate, test.ShouldResemble, float32(30)) -} - func TestWebcamValidation(t *testing.T) { webCfg := &videosource.WebcamConfig{ Width: 1280,