Rdk webcam lag fix (#5053)

Author: AGanguly13

components/camera/videosource/webcam.go

@@ -4,6 +4,7 @@ package videosource
 import (
 	"context"
 	"fmt"
+	"image"
 	"path/filepath"
 	"strings"
 	"sync"
@@ -43,7 +44,10 @@ var (
 // as equally acceptable. See https://github.com/pion/mediadevices/blob/c10fb000dbbb28597e068468f3175dc68a281bfd/pkg/prop/int.go#L104
 // Setting it to 1 could theoretically allow 1x1 resolutions. 2 is small enough and even,
 // allowing all real camera resolutions while ensuring proper distance calculations.
-const minResolutionDimension = 2
+const (
+	minResolutionDimension = 2
+	defaultFrameRate       = float32(30.0)
+)
 
 func init() {
 	resource.RegisterComponent(
@@ -54,6 +58,15 @@ func init() {
 		})
 }
 
+// WebcamBuffer is a buffer for webcam frames.
+// WARNING: This struct is NOT thread safe. It must be protected by the mutex in the webcam struct.
+type WebcamBuffer struct {
+	frame   image.Image // Holds the frames and their release functions in the buffer
+	release func()
+	err     error
+	worker  *goutils.StoppableWorkers // A separate worker for the webcam buffer that allows stronger concurrency control.
+}
+
 // WebcamConfig is the native config attribute struct for webcams.
 type WebcamConfig struct {
 	CameraParameters     *transform.PinholeCameraIntrinsics `json:"intrinsic_parameters,omitempty"`
@@ -199,6 +212,8 @@ type webcam struct {
 	disconnected bool
 	logger       logging.Logger
 	workers      *goutils.StoppableWorkers
+
+	buffer *WebcamBuffer
 }
 
 // NewWebcam returns the webcam discovered based on the given config as the Camera interface type.
@@ -213,6 +228,8 @@ func NewWebcam(
 		logger:  logger.WithFields("camera_name", conf.ResourceName().ShortName()),
 		workers: goutils.NewBackgroundStoppableWorkers(),
 	}
+	cam.buffer = NewWebcamBuffer(cam.workers.Context())
+
 	if err := cam.Reconfigure(ctx, deps, conf); err != nil {
 		return nil, err
 	}
@@ -230,12 +247,12 @@ func (c *webcam) Reconfigure(
 	if err != nil {
 		return err
 	}
-
+	c.buffer.worker.Stop() // Calling this before locking shuts down the goroutines, and allows stopBuffer() to handle rest of the shutdown.
 	c.mu.Lock()
 	defer c.mu.Unlock()
+	c.buffer.stopBuffer()
 
 	c.cameraModel = camera.NewPinholeModelWithBrownConradyDistortion(newConf.CameraParameters, newConf.DistortionParameters)
-
 	driverReinitNotNeeded := c.conf.Format == newConf.Format &&
 		c.conf.Path == newConf.Path &&
 		c.conf.Width == newConf.Width &&
@@ -256,6 +273,13 @@ func (c *webcam) Reconfigure(
 
 	// only set once we're good
 	c.conf = *newConf
+
+	if c.conf.FrameRate == 0.0 {
+		c.conf.FrameRate = defaultFrameRate
+	}
+	c.buffer = NewWebcamBuffer(c.workers.Context())
+	c.startBuffer()
+
 	return nil
 }
 
@@ -371,15 +395,11 @@ func (c *webcam) Images(ctx context.Context) ([]camera.NamedImage, resource.Resp
 		return nil, resource.ResponseMetadata{}, err
 	}
 
-	img, release, err := c.reader.Read()
+	img, err := c.getLatestFrame()
 	if err != nil {
 		return nil, resource.ResponseMetadata{}, errors.Wrap(err, "monitoredWebcam: call to get Images failed")
 	}
-	defer func() {
-		if release != nil {
-			release()
-		}
-	}()
+
 	namedImg, err := camera.NamedImageFromImage(img, c.Name().Name, utils.MimeTypeJPEG)
 	if err != nil {
 		return nil, resource.ResponseMetadata{}, err
@@ -408,11 +428,10 @@ func (c *webcam) Image(ctx context.Context, mimeType string, extra map[string]in
 	if c.reader == nil {
 		return nil, camera.ImageMetadata{}, errors.New("underlying reader is nil")
 	}
-	img, release, err := c.reader.Read()
+	img, err := c.getLatestFrame()
 	if err != nil {
 		return nil, camera.ImageMetadata{}, err
 	}
-	defer release()
 
 	if mimeType == "" {
 		mimeType = utils.MimeTypeJPEG
@@ -476,15 +495,84 @@ func (c *webcam) Geometries(ctx context.Context, extra map[string]interface{}) (
 	return make([]spatialmath.Geometry, 0), nil
 }
 
+// NewWebcamBuffer creates a new WebcamBuffer struct.
+func NewWebcamBuffer(ctx context.Context) *WebcamBuffer {
+	return &WebcamBuffer{
+		worker: goutils.NewStoppableWorkers(ctx),
+	}
+}
+
+// Must lock the mutex before calling this function.
+func (c *webcam) getLatestFrame() (image.Image, error) {
+	if c.buffer.frame == nil {
+		if c.buffer.err != nil {
+			return nil, c.buffer.err
+		}
+		return nil, errors.New("no frames available to read")
+	}
+
+	return c.buffer.frame, nil
+}
+
+func (c *webcam) startBuffer() {
+	if c.buffer.frame != nil {
+		return // webcam buffer already started
+	}
+
+	interFrameDuration := time.Duration(float32(time.Second) / c.conf.FrameRate)
+	ticker := time.NewTicker(interFrameDuration)
+	c.buffer.worker.Add(func(closedCtx context.Context) {
+		defer ticker.Stop()
+		for {
+			select {
+			case <-closedCtx.Done():
+				return
+			case <-ticker.C:
+				// We must unlock the mutex even if the release() or read() functions panic.
+				func() {
+					c.mu.Lock()
+					defer c.mu.Unlock()
+					if c.buffer.release != nil {
+						c.buffer.release()
+						c.buffer.release = nil
+						c.buffer.frame = nil
+					}
+					img, release, err := c.reader.Read()
+					c.buffer.err = err
+					if err != nil {
+						c.logger.Errorf("error reading frame: %v", err)
+						return // next iteration of for loop
+					}
+					c.buffer.frame = img
+					c.buffer.release = release
+				}()
+			}
+		}
+	})
+}
+
+// Must lock the mutex before using this function.
+func (buffer *WebcamBuffer) stopBuffer() {
+	if buffer == nil {
+		return
+	}
+
+	// Release the remaining frame.
+	if buffer.release != nil {
+		buffer.release()
+		buffer.release = nil
+	}
+}
+
 func (c *webcam) Close(ctx context.Context) error {
+	c.workers.Stop()
+	c.buffer.worker.Stop()
 	c.mu.Lock()
+	defer c.mu.Unlock()
 	if c.closed {
-		c.mu.Unlock()
 		return errors.New("webcam already closed")
 	}
 	c.closed = true
-	c.mu.Unlock()
-	c.workers.Stop()
 
 	return c.driver.Close()
 }