Merge branch 'main' into arm32-build

viamrobotics · Sep 6, 2023 · 6115e3f · 6115e3f
2 parents 8aad23c + 473673b
commit 6115e3f
Show file tree

Hide file tree

Showing 8 changed files with 195 additions and 17 deletions.
diff --git a/components/camera/transformpipeline/mods.go b/components/camera/transformpipeline/mods.go
@@ -38,6 +38,10 @@ func newRotateTransform(ctx context.Context, source gostream.VideoSource, stream
 		return nil, camera.UnspecifiedStream, errors.Wrap(err, "cannot parse rotate attribute map")
 	}
 
+	if !am.Has("angle_degs") {
+		conf.Angle = 180 // Default to 180 for backwards-compatibility
+	}
+
 	props, err := propsFromVideoSource(ctx, source)
 	if err != nil {
 		return nil, camera.UnspecifiedStream, err

diff --git a/components/camera/transformpipeline/mods_test.go b/components/camera/transformpipeline/mods_test.go
@@ -171,15 +171,37 @@ func TestRotateColorSource(t *testing.T) {
 
 	img2 := rimage.ConvertImage(rawImage)
 
+	am = utils.AttributeMap{
+		// defaults to 180
+	}
+
+	rsDefault, stream, err := newRotateTransform(context.Background(), source, camera.ColorStream, am)
+	test.That(t, err, test.ShouldBeNil)
+	test.That(t, stream, test.ShouldEqual, camera.ColorStream)
+
+	rawImageDefault, _, err := camera.ReadImage(context.Background(), rsDefault)
+	test.That(t, err, test.ShouldBeNil)
+	test.That(t, rs.Close(context.Background()), test.ShouldBeNil)
+
+	err = rimage.WriteImageToFile(t.TempDir()+"/test_rotate_color_source.png", rawImageDefault)
+	test.That(t, err, test.ShouldBeNil)
+
+	img3 := rimage.ConvertImage(rawImageDefault)
+
 	for x := 0; x < img.Width(); x++ {
 		p1 := image.Point{x, 0}
 		p2 := image.Point{img.Width() - x - 1, img.Height() - 1}
+		p3 := image.Point{img.Width() - x - 1, img.Height() - 1}
 
 		a := img.Get(p1)
 		b := img2.Get(p2)
+		c := img3.Get(p3)
 
 		d := a.Distance(b)
 		test.That(t, d, test.ShouldEqual, 0)
+
+		d = a.Distance(c)
+		test.That(t, d, test.ShouldEqual, 0)
 	}
 
 	test.That(t, rs.Close(context.Background()), test.ShouldBeNil)
@@ -277,6 +299,36 @@ func TestRotateColorSource(t *testing.T) {
 
 	test.That(t, rs.Close(context.Background()), test.ShouldBeNil)
 	test.That(t, source.Close(context.Background()), test.ShouldBeNil)
+
+	source = gostream.NewVideoSource(&videosource.StaticSource{ColorImg: img}, prop.Video{})
+	am = utils.AttributeMap{
+		"angle_degs": 0, // no-op
+	}
+	rs, stream, err = newRotateTransform(context.Background(), source, camera.ColorStream, am)
+	test.That(t, err, test.ShouldBeNil)
+	test.That(t, stream, test.ShouldEqual, camera.ColorStream)
+
+	rawImage, _, err = camera.ReadImage(context.Background(), rs)
+	test.That(t, err, test.ShouldBeNil)
+	test.That(t, rs.Close(context.Background()), test.ShouldBeNil)
+
+	err = rimage.WriteImageToFile(t.TempDir()+"/test_rotate_color_source.png", rawImage)
+	test.That(t, err, test.ShouldBeNil)
+
+	img2 = rimage.ConvertImage(rawImage)
+
+	for x := 0; x < img.Width(); x++ {
+		p := image.Point{X: x}
+
+		a := img.Get(p)
+		b := img2.Get(p)
+
+		d := a.Distance(b)
+		test.That(t, d, test.ShouldEqual, 0)
+	}
+
+	test.That(t, rs.Close(context.Background()), test.ShouldBeNil)
+	test.That(t, source.Close(context.Background()), test.ShouldBeNil)
 }
 
 func TestRotateDepthSource(t *testing.T) {
@@ -302,14 +354,35 @@ func TestRotateDepthSource(t *testing.T) {
 	dm, err := rimage.ConvertImageToDepthMap(context.Background(), rawImage)
 	test.That(t, err, test.ShouldBeNil)
 
+	am = utils.AttributeMap{
+		// defaults to 180
+	}
+
+	rsDefault, stream, err := newRotateTransform(context.Background(), source, camera.DepthStream, am)
+	test.That(t, err, test.ShouldBeNil)
+	test.That(t, stream, test.ShouldEqual, camera.DepthStream)
+
+	rawImageDefault, _, err := camera.ReadImage(context.Background(), rsDefault)
+	test.That(t, err, test.ShouldBeNil)
+	test.That(t, rs.Close(context.Background()), test.ShouldBeNil)
+
+	err = rimage.WriteImageToFile(t.TempDir()+"/test_rotate_depth_source.png", rawImageDefault)
+	test.That(t, err, test.ShouldBeNil)
+
+	dmDefault, err := rimage.ConvertImageToDepthMap(context.Background(), rawImageDefault)
+	test.That(t, err, test.ShouldBeNil)
+
 	for x := 0; x < pc.Width(); x++ {
 		p1 := image.Point{x, 0}
 		p2 := image.Point{pc.Width() - x - 1, pc.Height() - 1}
+		p3 := image.Point{pc.Width() - x - 1, pc.Height() - 1}
 
 		d1 := pc.Get(p1)
 		d2 := dm.Get(p2)
+		d3 := dmDefault.Get(p3)
 
 		test.That(t, d1, test.ShouldEqual, d2)
+		test.That(t, d1, test.ShouldEqual, d3)
 	}
 
 	test.That(t, rs.Close(context.Background()), test.ShouldBeNil)
@@ -407,6 +480,36 @@ func TestRotateDepthSource(t *testing.T) {
 
 	test.That(t, rs.Close(context.Background()), test.ShouldBeNil)
 	test.That(t, source.Close(context.Background()), test.ShouldBeNil)
+
+	source = gostream.NewVideoSource(&videosource.StaticSource{DepthImg: pc}, prop.Video{})
+	am = utils.AttributeMap{
+		"angle_degs": 0, // no-op
+	}
+	rs, stream, err = newRotateTransform(context.Background(), source, camera.DepthStream, am)
+	test.That(t, err, test.ShouldBeNil)
+	test.That(t, stream, test.ShouldEqual, camera.DepthStream)
+
+	rawImage, _, err = camera.ReadImage(context.Background(), rs)
+	test.That(t, err, test.ShouldBeNil)
+	test.That(t, rs.Close(context.Background()), test.ShouldBeNil)
+
+	err = rimage.WriteImageToFile(t.TempDir()+"/test_rotate_depth_source.png", rawImage)
+	test.That(t, err, test.ShouldBeNil)
+
+	dm, err = rimage.ConvertImageToDepthMap(context.Background(), rawImage)
+	test.That(t, err, test.ShouldBeNil)
+
+	for x := 0; x < pc.Width(); x++ {
+		p := image.Point{X: x}
+
+		d1 := pc.Get(p)
+		d2 := dm.Get(p)
+
+		test.That(t, d1, test.ShouldEqual, d2)
+	}
+
+	test.That(t, rs.Close(context.Background()), test.ShouldBeNil)
+	test.That(t, source.Close(context.Background()), test.ShouldBeNil)
 }
 
 func BenchmarkColorRotate(b *testing.B) {

diff --git a/components/motor/gpiostepper/gpiostepper.go b/components/motor/gpiostepper/gpiostepper.go
@@ -462,13 +462,14 @@ func (m *gpioStepper) Close(ctx context.Context) error {
 	err := m.Stop(ctx, nil)
 
 	m.lock.Lock()
-	defer m.lock.Unlock()
 	if m.cancel != nil {
 		m.logger.Debugf("stopping control thread for motor (%s)", m.Name().Name)
 		m.cancel()
 		m.cancel = nil
-		m.waitGroup.Wait()
+		m.threadStarted = false
 	}
+	m.lock.Unlock()
+	m.waitGroup.Wait()
 
 	return err
 }
diff --git a/components/motor/gpiostepper/gpiostepper_test.go b/components/motor/gpiostepper/gpiostepper_test.go
@@ -102,6 +102,8 @@ func TestConfigs(t *testing.T) {
 		s := m.(*gpioStepper)
 
 		test.That(t, err, test.ShouldBeNil)
+		defer m.Close(ctx)
+
 		test.That(t, s.minDelay, test.ShouldEqual, 30*time.Microsecond)
 		test.That(t, s.stepsPerRotation, test.ShouldEqual, 200)
 		test.That(t, s.dirPin, test.ShouldEqual, pinB)
@@ -119,6 +121,8 @@ func TestConfigs(t *testing.T) {
 		s := m.(*gpioStepper)
 
 		test.That(t, err, test.ShouldBeNil)
+		defer m.Close(ctx)
+
 		test.That(t, s.dirPin, test.ShouldEqual, pinB)
 		test.That(t, s.stepPin, test.ShouldEqual, pinC)
 
@@ -150,12 +154,14 @@ func TestConfigs(t *testing.T) {
 		s := m.(*gpioStepper)
 
 		test.That(t, err, test.ShouldBeNil)
+		defer m.Close(ctx)
 		test.That(t, s.minDelay, test.ShouldEqual, 0*time.Microsecond)
 	})
 
 	t.Run("motor supports position reporting", func(t *testing.T) {
 		m, err := newGPIOStepper(ctx, &b, goodConfig, c.ResourceName(), logger)
 		test.That(t, err, test.ShouldBeNil)
+		defer m.Close(ctx)
 
 		properties, err := m.Properties(ctx, nil)
 		test.That(t, err, test.ShouldBeNil)
@@ -195,6 +201,7 @@ func TestRunning(t *testing.T) {
 	t.Run("isPowered false after init", func(t *testing.T) {
 		m, err := newGPIOStepper(ctx, &b, goodConfig, c.ResourceName(), logger)
 		test.That(t, err, test.ShouldBeNil)
+		defer m.Close(ctx)
 
 		on, powerPct, err := m.IsPowered(ctx, nil)
 		test.That(t, err, test.ShouldBeNil)
@@ -214,6 +221,7 @@ func TestRunning(t *testing.T) {
 		m, err := newGPIOStepper(ctx, &b, goodConfig, c.ResourceName(), logger)
 		s := m.(*gpioStepper)
 		test.That(t, err, test.ShouldBeNil)
+		defer m.Close(ctx)
 
 		// long running goFor
 		err = s.goForInternal(ctx, 100, 3)
@@ -249,6 +257,7 @@ func TestRunning(t *testing.T) {
 		m, err := newGPIOStepper(ctx, &b, goodConfig, c.ResourceName(), logger)
 		s := m.(*gpioStepper)
 		test.That(t, err, test.ShouldBeNil)
+		defer m.Close(ctx)
 
 		err = s.enable(ctx, true)
 		test.That(t, err, test.ShouldBeNil)
@@ -277,6 +286,7 @@ func TestRunning(t *testing.T) {
 		m, err := newGPIOStepper(ctx, &b, goodConfig, c.ResourceName(), logger)
 		s := m.(*gpioStepper)
 		test.That(t, err, test.ShouldBeNil)
+		defer m.Close(ctx)
 
 		err = s.GoFor(ctx, 10000, 1, nil)
 		test.That(t, err, test.ShouldBeNil)
@@ -296,6 +306,7 @@ func TestRunning(t *testing.T) {
 		m, err := newGPIOStepper(ctx, &b, goodConfig, c.ResourceName(), logger)
 		s := m.(*gpioStepper)
 		test.That(t, err, test.ShouldBeNil)
+		defer m.Close(ctx)
 
 		err = m.GoFor(ctx, -10000, 1, nil)
 		test.That(t, err, test.ShouldBeNil)
@@ -315,6 +326,7 @@ func TestRunning(t *testing.T) {
 		m, err := newGPIOStepper(ctx, &b, goodConfig, c.ResourceName(), logger)
 		s := m.(*gpioStepper)
 		test.That(t, err, test.ShouldBeNil)
+		defer m.Close(ctx)
 
 		err = m.GoFor(ctx, 10000, -1, nil)
 		test.That(t, err, test.ShouldBeNil)
@@ -334,6 +346,7 @@ func TestRunning(t *testing.T) {
 		m, err := newGPIOStepper(ctx, &b, goodConfig, c.ResourceName(), logger)
 		s := m.(*gpioStepper)
 		test.That(t, err, test.ShouldBeNil)
+		defer m.Close(ctx)
 
 		err = m.GoFor(ctx, -10000, -1, nil)
 		test.That(t, err, test.ShouldBeNil)
@@ -353,6 +366,7 @@ func TestRunning(t *testing.T) {
 		m, err := newGPIOStepper(ctx, &b, goodConfig, c.ResourceName(), logger)
 		s := m.(*gpioStepper)
 		test.That(t, err, test.ShouldBeNil)
+		defer m.Close(ctx)
 
 		ctx := context.Background()
 		var wg sync.WaitGroup
@@ -399,6 +413,7 @@ func TestRunning(t *testing.T) {
 	t.Run("enable pins handled properly during GoFor", func(t *testing.T) {
 		m, err := newGPIOStepper(ctx, &b, goodConfig, c.ResourceName(), logger)
 		test.That(t, err, test.ShouldBeNil)
+		defer m.Close(ctx)
 
 		ctx := context.Background()
 		var wg sync.WaitGroup
@@ -450,6 +465,7 @@ func TestRunning(t *testing.T) {
 		m, err := newGPIOStepper(ctx, &b, goodConfig, c.ResourceName(), logger)
 		s := m.(*gpioStepper)
 		test.That(t, err, test.ShouldBeNil)
+		defer m.Close(ctx)
 
 		err = s.goForInternal(ctx, 1000, 200)
 		test.That(t, err, test.ShouldBeNil)
@@ -482,6 +498,7 @@ func TestRunning(t *testing.T) {
 	t.Run("motor testing with 0 rpm", func(t *testing.T) {
 		m, err := newGPIOStepper(ctx, &b, goodConfig, c.ResourceName(), logger)
 		test.That(t, err, test.ShouldBeNil)
+		defer m.Close(ctx)
 
 		err = m.GoFor(ctx, 0, 1, nil)
 		test.That(t, err, test.ShouldBeNil)

diff --git a/components/powersensor/ina/ina.go b/components/powersensor/ina/ina.go
@@ -150,11 +150,6 @@ func newINA(
 		return nil, err
 	}
 
-	err = s.calibrate()
-	if err != nil {
-		return nil, err
-	}
-
 	return s, nil
 }
 
@@ -215,8 +210,8 @@ func (d *ina) calibrate() error {
 		return err
 	}
 
-	// setting config to 111 sets to normal operating mode
-	err = handle.WriteRegU16BE(configRegister, uint16(0x6F))
+	// set the config register to all default values.
+	err = handle.WriteRegU16BE(configRegister, uint16(0x399F))
 	if err != nil {
 		return err
 	}
@@ -231,7 +226,7 @@ func (d *ina) Voltage(ctx context.Context, extra map[string]interface{}) (float6
 	}
 	defer utils.UncheckedErrorFunc(handle.Close)
 
-	bus, err := handle.ReadRegU16BE(busVoltageRegister)
+	bus, err := handle.ReadRegS16BE(busVoltageRegister)
 	if err != nil {
 		return 0, false, err
 	}
@@ -259,7 +254,14 @@ func (d *ina) Current(ctx context.Context, extra map[string]interface{}) (float6
 	}
 	defer utils.UncheckedErrorFunc(handle.Close)
 
-	rawCur, err := handle.ReadRegU16BE(currentRegister)
+	// Calibrate each time the current value is read, so if anything else is also writing to these registers
+	// we have the correct value.
+	err = d.calibrate()
+	if err != nil {
+		return 0, false, err
+	}
+
+	rawCur, err := handle.ReadRegS16BE(currentRegister)
 	if err != nil {
 		return 0, false, err
 	}
@@ -277,7 +279,14 @@ func (d *ina) Power(ctx context.Context, extra map[string]interface{}) (float64,
 	}
 	defer utils.UncheckedErrorFunc(handle.Close)
 
-	pow, err := handle.ReadRegU16BE(powerRegister)
+	// Calibrate each time the power value is read, so if anything else is also writing to these registers
+	// we have the correct value.
+	err = d.calibrate()
+	if err != nil {
+		return 0, err
+	}
+
+	pow, err := handle.ReadRegS16BE(powerRegister)
 	if err != nil {
 		return 0, err
 	}

diff --git a/services/navigation/builtin/builtin.go b/services/navigation/builtin/builtin.go
@@ -230,6 +230,7 @@ func (svc *builtIn) Reconfigure(ctx context.Context, deps resource.Dependencies,
 		return err
 	}
 
+	svc.mode = navigation.ModeManual
 	svc.store = newStore
 	svc.storeType = string(svcConfig.Store.Type)
 	svc.base = base1
@@ -385,15 +386,15 @@ func (svc *builtIn) startWaypoint(ctx context.Context, extra map[string]interfac
 			return svc.waypointReached(ctx)
 		}
 
-		// loop until no waypoints remaining
+		// do not exit loop - even if there are no waypoints remaining
 		for {
 			if ctx.Err() != nil {
 				return
 			}
 
 			wp, err := svc.store.NextWaypoint(ctx)
 			if err != nil {
-				return
+				continue
 			}
 			svc.mu.Lock()
 			svc.waypointInProgress = &wp