From b69664ed8e640da6765ffc8e2ccd5922de8d5fbd Mon Sep 17 00:00:00 2001 From: martha-johnston <106617924+martha-johnston@users.noreply.github.com> Date: Thu, 21 Sep 2023 12:11:15 -0400 Subject: [PATCH] RSDK-3130: simultaneous motor calls in parallel in wheeled base (#2945) --- components/base/wheeled/wheeled_base.go | 65 +++++++++++--------- components/base/wheeled/wheeled_base_test.go | 37 +++++++++++ 2 files changed, 74 insertions(+), 28 deletions(-) diff --git a/components/base/wheeled/wheeled_base.go b/components/base/wheeled/wheeled_base.go index cbb288cd307..8db94acb418 100644 --- a/components/base/wheeled/wheeled_base.go +++ b/components/base/wheeled/wheeled_base.go @@ -249,7 +249,7 @@ func (wb *wheeledBase) Spin(ctx context.Context, angleDeg, degsPerSec float64, e // Spin math rpm, revolutions := wb.spinMath(angleDeg, degsPerSec) - return wb.runAll(ctx, -rpm, revolutions, rpm, revolutions) + return wb.runAllGoFor(ctx, -rpm, revolutions, rpm, revolutions) } // MoveStraight commands a base to drive forward or backwards at a linear speed and for a specific distance. @@ -270,14 +270,14 @@ func (wb *wheeledBase) MoveStraight(ctx context.Context, distanceMm int, mmPerSe // Straight math rpm, rotations := wb.straightDistanceToMotorInputs(distanceMm, mmPerSec) - return wb.runAll(ctx, rpm, rotations, rpm, rotations) + return wb.runAllGoFor(ctx, rpm, rotations, rpm, rotations) } -// runAll executes `motor.GoFor` commands in parallel for left and right motors, +// runAllGoFor executes `motor.GoFor` commands in parallel for left and right motors, // with specified speeds and rotations and stops the base if an error occurs. // All callers must register an operation via `wb.opMgr.New` to ensure the left and right motors // receive consistent instructions. -func (wb *wheeledBase) runAll(ctx context.Context, leftRPM, leftRotations, rightRPM, rightRotations float64) error { +func (wb *wheeledBase) runAllGoFor(ctx context.Context, leftRPM, leftRotations, rightRPM, rightRotations float64) error { goForFuncs := func() []rdkutils.SimpleFunc { ret := []rdkutils.SimpleFunc{} @@ -359,7 +359,6 @@ func (wb *wheeledBase) SetVelocity(ctx context.Context, linear, angular r3.Vecto // Passing zero revolutions to `motor.GoFor` will have the motor run until // interrupted. Moreover, `motor.GoFor` will return immediately when given zero revolutions. const numRevolutions = 0 - errs := make([]error, 0) wb.mu.Lock() // Because `SetVelocity` does not create a new operation, canceling must be done atomically with @@ -372,16 +371,11 @@ func (wb *wheeledBase) SetVelocity(ctx context.Context, linear, angular r3.Vecto // finish "spinning" have undefined behavior due to the motors actually running at a // speed/direction that was not intended. wb.opMgr.CancelRunning(ctx) - defer wb.mu.Unlock() - for _, m := range wb.left { - errs = append(errs, m.GoFor(ctx, leftRPM, numRevolutions, nil)) - } - - for _, m := range wb.right { - errs = append(errs, m.GoFor(ctx, rightRPM, numRevolutions, nil)) - } + wb.mu.Unlock() - return multierr.Combine(errs...) + ctx, done := wb.opMgr.New(ctx) + defer done() + return wb.runAllGoFor(ctx, leftRPM, numRevolutions, rightRPM, numRevolutions) } // SetPower commands the base motors to run at powers corresponding to input linear and angular powers. @@ -403,27 +397,26 @@ func (wb *wheeledBase) SetPower(ctx context.Context, linear, angular r3.Vector, lPower, rPower := wb.differentialDrive(linear.Y, angular.Z) // Send motor commands - err := func() error { - var err error + setPowerFuncs := func() []rdkutils.SimpleFunc { + ret := []rdkutils.SimpleFunc{} - // `wheeledBase.SetPower` does not create a new operation via the `opMgr`. Set the - // underlying motor powers atomically. wb.mu.Lock() defer wb.mu.Unlock() for _, m := range wb.left { - err = multierr.Combine(err, m.SetPower(ctx, lPower, extra)) + motor := m + ret = append(ret, func(ctx context.Context) error { return motor.SetPower(ctx, lPower, extra) }) } for _, m := range wb.right { - err = multierr.Combine(err, m.SetPower(ctx, rPower, extra)) + motor := m + ret = append(ret, func(ctx context.Context) error { return motor.SetPower(ctx, rPower, extra) }) } - - return err + return ret }() - if err != nil { + + if _, err := rdkutils.RunInParallel(ctx, setPowerFuncs); err != nil { return multierr.Combine(err, wb.Stop(ctx, nil)) } - return nil } @@ -471,11 +464,27 @@ func (wb *wheeledBase) straightDistanceToMotorInputs(distanceMm int, mmPerSec fl // Stop commands the base to stop moving. func (wb *wheeledBase) Stop(ctx context.Context, extra map[string]interface{}) error { - var err error - for _, m := range wb.allMotors { - err = multierr.Combine(err, m.Stop(ctx, extra)) + stopFuncs := func() []rdkutils.SimpleFunc { + ret := []rdkutils.SimpleFunc{} + + wb.mu.Lock() + defer wb.mu.Unlock() + for _, m := range wb.left { + motor := m + ret = append(ret, func(ctx context.Context) error { return motor.Stop(ctx, extra) }) + } + + for _, m := range wb.right { + motor := m + ret = append(ret, func(ctx context.Context) error { return motor.Stop(ctx, extra) }) + } + return ret + }() + + if _, err := rdkutils.RunInParallel(ctx, stopFuncs); err != nil { + return multierr.Combine(err) } - return err + return nil } func (wb *wheeledBase) IsMoving(ctx context.Context) (bool, error) { diff --git a/components/base/wheeled/wheeled_base_test.go b/components/base/wheeled/wheeled_base_test.go index c4c609c16b5..afc75058bdd 100644 --- a/components/base/wheeled/wheeled_base_test.go +++ b/components/base/wheeled/wheeled_base_test.go @@ -2,6 +2,7 @@ package wheeled import ( "context" + "errors" "math" "testing" "time" @@ -16,8 +17,15 @@ import ( "go.viam.com/rdk/components/motor/fake" "go.viam.com/rdk/operation" "go.viam.com/rdk/resource" + "go.viam.com/rdk/testutils/inject" ) +func createFakeMotor() motor.Motor { + return &inject.Motor{ + StopFunc: func(ctx context.Context, extra map[string]interface{}) error { return errors.New("stop error") }, + } +} + func newTestCfg() resource.Config { return resource.Config{ Name: "test", @@ -32,6 +40,14 @@ func newTestCfg() resource.Config { } } +func createMockDeps(t *testing.T) resource.Dependencies { + t.Helper() + deps := make(resource.Dependencies) + deps[motor.Named("right")] = createFakeMotor() + deps[motor.Named("left")] = createFakeMotor() + return deps +} + func fakeMotorDependencies(t *testing.T, deps []string) resource.Dependencies { t.Helper() logger := golog.NewTestLogger(t) @@ -304,6 +320,27 @@ func TestWheelBaseMath(t *testing.T) { }) } +func TestStopError(t *testing.T) { + ctx := context.Background() + logger := golog.NewTestLogger(t) + deps := createMockDeps(t) + + fakecfg := resource.Config{ + Name: "base", + ConvertedAttributes: &Config{ + WidthMM: 100, + WheelCircumferenceMM: 100, + Left: []string{"left"}, + Right: []string{"right"}, + }, + } + base, err := createWheeledBase(ctx, deps, fakecfg, logger) + test.That(t, err, test.ShouldBeNil) + + err = base.Stop(ctx, nil) + test.That(t, err.Error(), test.ShouldContainSubstring, "stop error") +} + func TestWheeledBaseConstructor(t *testing.T) { ctx := context.Background() logger := golog.NewTestLogger(t)