[RSDK-8574][RSDK-8184]: return an error if user tries to move a senso…

…r controlled base without copy/pasting tuned PID values to the config (#4291)

components/base/sensorcontrolled/movestraight.go

@@ -34,7 +34,7 @@ func (sb *sensorBase) MoveStraight(
 	// If a position movement sensor or controls are not configured, we cannot use this MoveStraight method.
 	// Instead we need to use the MoveStraight method of the base that the sensorcontrolled base wraps.
 	// If there is no valid velocity sensor, there won't be a controlLoopConfig.
-	if len(sb.controlLoopConfig.Blocks) == 0 {
+	if sb.controlLoopConfig == nil {
 		sb.logger.CWarnf(ctx,
 			"control loop not configured, using base %s's MoveStraight",
 			sb.controlledBase.Name().ShortName())
@@ -54,6 +54,11 @@ func (sb *sensorBase) MoveStraight(
 		}
 	}
 
+	// check tuning status
+	if err := sb.checkTuningStatus(); err != nil {
+		return err
+	}
+
 	// make sure the control loop is enabled
 	if sb.loop == nil {
 		if err := sb.startControlLoop(); err != nil {

components/base/sensorcontrolled/sensorcontrolled.go

@@ -3,6 +3,7 @@ package sensorcontrolled
 
 import (
 	"context"
+	"fmt"
 	"sync"
 	"time"
 
@@ -26,6 +27,7 @@ const (
 	typeLinVel         = "linear_velocity"
 	typeAngVel         = "angular_velocity"
 	defaultControlFreq = 10 // Hz
+	getPID             = "get_tuned_pid"
 )
 
 var (
@@ -48,13 +50,20 @@ func (cfg *Config) Validate(path string) ([]string, error) {
 	if len(cfg.MovementSensor) == 0 {
 		return nil, resource.NewConfigValidationError(path, errors.New("need at least one movement sensor for base"))
 	}
-
 	deps = append(deps, cfg.MovementSensor...)
+
 	if cfg.Base == "" {
 		return nil, resource.NewConfigValidationFieldRequiredError(path, "base")
 	}
-
 	deps = append(deps, cfg.Base)
+
+	for _, pidConf := range cfg.ControlParameters {
+		if pidConf.Type != typeLinVel && pidConf.Type != typeAngVel {
+			return nil, resource.NewConfigValidationError(path,
+				errors.New("control_parameters type must be 'linear_velocity' or 'angular_velocity'"))
+		}
+	}
+
 	return deps, nil
 }
 
@@ -75,9 +84,11 @@ type sensorBase struct {
 	// headingFunc returns the current angle between (-180,180) and whether Spin is supported
 	headingFunc func(ctx context.Context) (float64, bool, error)
 
-	controlLoopConfig control.Config
+	controlLoopConfig *control.Config
 	blockNames        map[string][]string
 	loop              *control.Loop
+	configPIDVals     []control.PIDConfig
+	tunedVals         *[]control.PIDConfig
 	controlFreq       float64
 }
 
@@ -95,9 +106,11 @@ func createSensorBase(
 	logger logging.Logger,
 ) (base.Base, error) {
 	sb := &sensorBase{
-		logger: logger,
-		Named:  conf.ResourceName().AsNamed(),
-		opMgr:  operation.NewSingleOperationManager(),
+		logger:        logger,
+		tunedVals:     &[]control.PIDConfig{{}, {}},
+		configPIDVals: []control.PIDConfig{{}, {}},
+		Named:         conf.ResourceName().AsNamed(),
+		opMgr:         operation.NewSingleOperationManager(),
 	}
 
 	if err := sb.Reconfigure(ctx, deps, conf); err != nil {
@@ -195,26 +208,28 @@ func (sb *sensorBase) Reconfigure(ctx context.Context, deps resource.Dependencie
 
 	if sb.velocities != nil && len(newConf.ControlParameters) != 0 {
 		// assign linear and angular PID correctly based on the given type
-		var linear, angular control.PIDConfig
-		for _, c := range newConf.ControlParameters {
-			switch c.Type {
+		for _, pidConf := range newConf.ControlParameters {
+			switch pidConf.Type {
 			case typeLinVel:
-				linear = c
+				// configPIDVals at index 0 is linear
+				sb.configPIDVals[0] = pidConf
 			case typeAngVel:
-				angular = c
+				// configPIDVals at index 1 is angular
+				sb.configPIDVals[1] = pidConf
 			default:
-				sb.logger.Warn("control_parameters type must be 'linear_velocity' or 'angular_velocity'")
+				return fmt.Errorf("control_parameters type '%v' not accepted, type must be 'linear_velocity' or 'angular_velocity'",
+					pidConf.Type)
 			}
 		}
 
 		// unlock the mutex before setting up the control loop so that the motors
 		// are not locked, and can run if any auto-tuning is necessary
 		sb.mu.Unlock()
-		if err := sb.setupControlLoop(linear, angular); err != nil {
+		if err := sb.setupControlLoop(sb.configPIDVals[0], sb.configPIDVals[1]); err != nil {
 			sb.mu.Lock()
 			return err
 		}
-		// relock the mutex after setting up the control loop since there is still a  defer unlock
+		// relock the mutex after setting up the control loop since there is still a defer unlock
 		sb.mu.Lock()
 	}
 
@@ -255,6 +270,25 @@ func (sb *sensorBase) Geometries(ctx context.Context, extra map[string]interface
 	return sb.controlledBase.Geometries(ctx, extra)
 }
 
+func (sb *sensorBase) DoCommand(ctx context.Context, req map[string]interface{}) (map[string]interface{}, error) {
+	resp := make(map[string]interface{})
+
+	sb.mu.Lock()
+	defer sb.mu.Unlock()
+	ok := req[getPID].(bool)
+	if ok {
+		var respStr string
+		for _, pidConf := range *sb.tunedVals {
+			if !pidConf.NeedsAutoTuning() {
+				respStr += fmt.Sprintf("{p: %v, i: %v, d: %v, type: %v} ", pidConf.P, pidConf.I, pidConf.D, pidConf.Type)
+			}
+		}
+		resp[getPID] = respStr
+	}
+
+	return resp, nil
+}
+
 func (sb *sensorBase) Close(ctx context.Context) error {
 	if err := sb.Stop(ctx, nil); err != nil {
 		return err
@@ -314,3 +348,15 @@ func (sb *sensorBase) determineHeadingFunc(ctx context.Context,
 		}
 	}
 }
+
+// if loop is tuning, return an error
+// if loop has been tuned but the values haven't been added to the config, error with tuned values.
+func (sb *sensorBase) checkTuningStatus() error {
+	if sb.loop != nil && sb.loop.GetTuning(context.Background()) {
+		return control.TuningInProgressErr(sb.Name().ShortName())
+	} else if (sb.configPIDVals[0].NeedsAutoTuning() && !(*sb.tunedVals)[0].NeedsAutoTuning()) ||
+		(sb.configPIDVals[1].NeedsAutoTuning() && !(*sb.tunedVals)[1].NeedsAutoTuning()) {
+		return control.TunedPIDErr(sb.Name().ShortName(), *sb.tunedVals)
+	}
+	return nil
+}

components/base/sensorcontrolled/sensorcontrolled_test.go

@@ -3,6 +3,7 @@ package sensorcontrolled
 import (
 	"context"
 	"errors"
+	"fmt"
 	"strings"
 	"sync"
 	"testing"
@@ -28,6 +29,8 @@ const (
 	// compassValue and orientationValue should be different for tests.
 	defaultCompassValue     = 45.
 	defaultOrientationValue = 40.
+	wrongTypeLinVel         = "linear"
+	wrongTypeAngVel         = "angulr_velocity"
 )
 
 var (
@@ -145,16 +148,16 @@ func TestSensorBase(t *testing.T) {
 	test.That(t, sb.Close(ctx), test.ShouldBeNil)
 }
 
-func sBaseTestConfig(msNames []string, freq float64) resource.Config {
+func sBaseTestConfig(msNames []string, freq float64, linType, angType string) resource.Config {
 	controlParams := make([]control.PIDConfig, 2)
 	controlParams[0] = control.PIDConfig{
-		Type: typeLinVel,
+		Type: linType,
 		P:    0.5,
 		I:    0.5,
 		D:    0.0,
 	}
 	controlParams[1] = control.PIDConfig{
-		Type: typeAngVel,
+		Type: angType,
 		P:    0.5,
 		I:    0.5,
 		D:    0.0,
@@ -177,7 +180,7 @@ func msDependencies(t *testing.T, msNames []string,
 ) (resource.Dependencies, resource.Config) {
 	t.Helper()
 
-	cfg := sBaseTestConfig(msNames, defaultControlFreq)
+	cfg := sBaseTestConfig(msNames, defaultControlFreq, typeLinVel, typeAngVel)
 
 	deps := make(resource.Dependencies)
 
@@ -279,7 +282,7 @@ func TestReconfig(t *testing.T) {
 
 	deps, _ = msDependencies(t, []string{"setvel2"})
 	// generate a config with a non default freq
-	cfg = sBaseTestConfig([]string{"setvel2"}, 100)
+	cfg = sBaseTestConfig([]string{"setvel2"}, 100, typeLinVel, typeAngVel)
 	err = b.Reconfigure(ctx, deps, cfg)
 	test.That(t, err, test.ShouldBeNil)
 	test.That(t, sb.velocities.Name().ShortName(), test.ShouldResemble, "setvel2")
@@ -336,14 +339,20 @@ func TestReconfig(t *testing.T) {
 	test.That(t, err, test.ShouldBeNil)
 	test.That(t, headingSupported, test.ShouldBeFalse)
 	test.That(t, headingBad, test.ShouldEqual, 0)
+
+	deps, _ = msDependencies(t, []string{"setvel2"})
+	// generate a config with invalid pid types
+	cfg = sBaseTestConfig([]string{"setvel2"}, 100, wrongTypeLinVel, wrongTypeAngVel)
+	err = b.Reconfigure(ctx, deps, cfg)
+	test.That(t, err.Error(), test.ShouldContainSubstring, "type must be 'linear_velocity' or 'angular_velocity'")
 }
 
 func TestSensorBaseWithVelocitiesSensor(t *testing.T) {
 	ctx := context.Background()
 	logger := logging.NewTestLogger(t)
 	deps, _ := msDependencies(t, []string{"setvel1"})
 	// generate a config with a non default freq
-	cfg := sBaseTestConfig([]string{"setvel1"}, 100)
+	cfg := sBaseTestConfig([]string{"setvel1"}, 100, typeLinVel, typeAngVel)
 
 	b, err := createSensorBase(ctx, deps, cfg, logger)
 	test.That(t, err, test.ShouldBeNil)
@@ -511,3 +520,32 @@ func TestSensorBaseMoveStraight(t *testing.T) {
 		orientationValue = defaultOrientationValue
 	})
 }
+
+func TestSensorBaseDoCommand(t *testing.T) {
+	ctx := context.Background()
+	logger := logging.NewTestLogger(t)
+	deps, cfg := msDependencies(t, []string{"setvel1", "position1", "orientation1"})
+	b, err := createSensorBase(ctx, deps, cfg, logger)
+	test.That(t, err, test.ShouldBeNil)
+
+	sb, ok := b.(*sensorBase)
+	test.That(t, ok, test.ShouldBeTrue)
+
+	expectedPID := control.PIDConfig{P: 0.1, I: 2.0, D: 0.0}
+	sb.tunedVals = &[]control.PIDConfig{expectedPID, {}}
+	expectedeMap := make(map[string]interface{})
+	expectedeMap["get_tuned_pid"] = (fmt.Sprintf("{p: %v, i: %v, d: %v, type: %v} ",
+		expectedPID.P, expectedPID.I, expectedPID.D, expectedPID.Type))
+
+	req := make(map[string]interface{})
+	req["get_tuned_pid"] = true
+	resp, err := b.DoCommand(ctx, req)
+	test.That(t, err, test.ShouldBeNil)
+	test.That(t, resp, test.ShouldResemble, expectedeMap)
+
+	emptyMap := make(map[string]interface{})
+	req["get_tuned_pid"] = false
+	resp, err = b.DoCommand(ctx, req)
+	test.That(t, err, test.ShouldBeNil)
+	test.That(t, resp, test.ShouldResemble, emptyMap)
+}

components/base/sensorcontrolled/spin.go

@@ -27,11 +27,16 @@ func (sb *sensorBase) Spin(ctx context.Context, angleDeg, degsPerSec float64, ex
 	// If an orientation movement sensor or controls are not configured, we cannot use this Spin method.
 	// Instead we need to use the Spin method of the base that the sensorBase wraps.
 	// If there is no valid velocity sensor, there won't be a controlLoopConfig.
-	if len(sb.controlLoopConfig.Blocks) == 0 {
+	if sb.controlLoopConfig == nil {
 		sb.logger.CWarnf(ctx, "control parameters not configured, using %v's Spin method", sb.controlledBase.Name().ShortName())
 		return sb.controlledBase.Spin(ctx, angleDeg, degsPerSec, extra)
 	}
 
+	// check tuning status
+	if err := sb.checkTuningStatus(); err != nil {
+		return err
+	}
+
 	prevAngle, hasOrientation, err := sb.headingFunc(ctx)
 	if err != nil {
 		return err

components/base/sensorcontrolled/velocities.go

@@ -19,11 +19,16 @@ func (sb *sensorBase) SetVelocity(
 	ctx, done := sb.opMgr.New(ctx)
 	defer done()
 
-	if len(sb.controlLoopConfig.Blocks) == 0 {
+	if sb.controlLoopConfig == nil {
 		sb.logger.CWarnf(ctx, "control parameters not configured, using %v's SetVelocity method", sb.controlledBase.Name().ShortName())
 		return sb.controlledBase.SetVelocity(ctx, linear, angular, extra)
 	}
 
+	// check tuning status
+	if err := sb.checkTuningStatus(); err != nil {
+		return err
+	}
+
 	// make sure the control loop is enabled
 	if sb.loop == nil {
 		if err := sb.startControlLoop(); err != nil {
@@ -43,7 +48,7 @@ func (sb *sensorBase) SetVelocity(
 // startControlLoop uses the control config to initialize a control loop and store it on the sensor controlled base struct.
 // The sensor base is the controllable interface that implements State and GetState called from the endpoint block of the control loop.
 func (sb *sensorBase) startControlLoop() error {
-	loop, err := control.NewLoop(sb.logger, sb.controlLoopConfig, sb)
+	loop, err := control.NewLoop(sb.logger, *sb.controlLoopConfig, sb)
 	if err != nil {
 		return err
 	}
@@ -80,6 +85,7 @@ func (sb *sensorBase) setupControlLoop(linear, angular control.PIDConfig) error
 	sb.controlLoopConfig = pl.ControlConf
 	sb.loop = pl.ControlLoop
 	sb.blockNames = pl.BlockNames
+	sb.tunedVals = pl.TunedVals
 
 	return nil
 }