RSDK-4810-plan-executor (viamrobotics#3233)

stevebriskin · Nov 20, 2023 · 51fe753 · 51fe753
1 parent f2355d1
commit 51fe753
Show file tree

Hide file tree

Showing 7 changed files with 414 additions and 329 deletions.
diff --git a/services/motion/builtin/builtin.go b/services/motion/builtin/builtin.go
@@ -10,6 +10,7 @@ import (
 	geo "github.com/kellydunn/golang-geo"
 	"github.com/pkg/errors"
 	servicepb "go.viam.com/api/service/motion/v1"
+	"go.viam.com/utils"
 
 	"go.viam.com/rdk/components/movementsensor"
 	"go.viam.com/rdk/internal"
@@ -248,40 +249,42 @@ func (ms *builtIn) MoveOnMap(
 	ms.mu.RLock()
 	defer ms.mu.RUnlock()
 	operation.CancelOtherWithLabel(ctx, builtinOpLabel)
-	valExtra, err := newValidatedExtra(extra)
-	if err != nil {
-		return false, err
-	}
 	// make call to motionplan
-	mr, err := ms.newMoveOnMapRequest(ctx, componentName, destination, slamName, valExtra)
+	mr, err := ms.newMoveOnMapRequest(ctx, motion.MoveOnMapReq{
+		ComponentName: componentName,
+		Destination:   destination,
+		SlamName:      slamName,
+		Extra:         extra,
+	})
 	if err != nil {
 		return false, fmt.Errorf("error making plan for MoveOnMap: %w", err)
 	}
 
-	ma := newMoveAttempt(ctx, mr)
-	if err := ma.start(); err != nil {
+	cancelCtx, cancelFn := context.WithCancel(ctx)
+	defer cancelFn()
+	// If the context is cancelled early, cancel the planner executor
+	utils.PanicCapturingGo(func() {
+		<-cancelCtx.Done()
+		mr.logger.Debug("context done")
+		mr.Cancel()
+	})
+	planResp, err := mr.Plan()
+	if err != nil {
 		return false, err
 	}
-
-	// this ensures that if the context is cancelled we always return early
-	if err := ctx.Err(); err != nil {
-		ma.cancel()
+	resp, err := mr.Execute(planResp.Waypoints)
+	// Error
+	if err != nil {
 		return false, err
 	}
 
-	select {
-	// if context was cancelled by the calling function, error out
-	case <-ctx.Done():
-		ma.cancel()
-		return false, ctx.Err()
-
-	// once execution responds: return the result to the caller
-	case resp := <-ma.responseChan:
-		mr.planRequest.Logger.Info("got move on map response")
-		ms.logger.Debugf("execution completed: %s", resp)
-		ma.cancel()
-		return resp.success, resp.err
+	// Didn't reach goal
+	if resp.Replan {
+		ms.logger.Warnf("didn't reach the goal. Reason: %s\n", resp.ReplanReason)
+		return false, nil
 	}
+	// Reached goal
+	return true, nil
 }
 
 type validatedExtra struct {
@@ -350,74 +353,51 @@ func (ms *builtIn) MoveOnGlobe(
 		extra,
 	)
 	operation.CancelOtherWithLabel(ctx, builtinOpLabel)
-	valExtra, err := newValidatedExtra(extra)
-	if err != nil {
-		return false, err
+	req := motion.MoveOnGlobeReq{
+		ComponentName:      componentName,
+		Destination:        destination,
+		MovementSensorName: movementSensorName,
+		Obstacles:          obstacles,
+		MotionCfg:          motionCfg,
+		Extra:              extra,
 	}
 
-	mr, err := ms.newMoveOnGlobeRequest(ctx, componentName, destination, movementSensorName, obstacles, motionCfg, nil, valExtra)
+	mr, err := ms.newMoveOnGlobeRequest(ctx, req, nil, 0)
 	if err != nil {
 		return false, err
 	}
+	cancelCtx, cancelFn := context.WithCancel(ctx)
+	defer cancelFn()
+	// If the context is cancelled early, cancel the planner executor
+	utils.PanicCapturingGo(func() {
+		<-cancelCtx.Done()
+		mr.logger.Debug("context done")
+		mr.Cancel()
+	})
 
 	replanCount := 0
 	// start a loop that plans every iteration and exits when something is read from the success channel
 	for {
-		ma := newMoveAttempt(ctx, mr)
-		if err := ma.start(); err != nil {
+		planResp, err := mr.Plan()
+		if err != nil {
 			return false, err
 		}
 
-		// this ensures that if the context is cancelled we always return early at the top of the loop
-		if err := ctx.Err(); err != nil {
-			ma.cancel()
+		resp, err := mr.Execute(planResp.Waypoints)
+		// failure
+		if err != nil {
 			return false, err
 		}
-
-		select {
-		// if context was cancelled by the calling function, error out
-		case <-ctx.Done():
-			ma.cancel()
-			return false, ctx.Err()
-
-		// once execution responds: return the result to the caller
-		case resp := <-ma.responseChan:
-			ms.logger.Debugf("execution response: %s", resp)
-			ma.cancel()
-
-			// If we have a false `success` and nil error, that means replan
-			if resp.success || resp.err != nil {
-				return resp.success, resp.err
-			}
-			ms.logger.Info("reached end of plan, but not at goal; triggering a replan")
-
-		// if the position poller hit an error return it, otherwise replan
-		case resp := <-ma.position.responseChan:
-			ms.logger.Debugf("position response: %s", resp)
-			ma.cancel()
-			if resp.err != nil {
-				return false, resp.err
-			}
-			ms.logger.Info("position drift triggering a replan")
-
-		// if the obstacle poller hit an error return it, otherwise replan
-		case resp := <-ma.obstacle.responseChan:
-			ms.logger.Debugf("obstacle response: %s", resp)
-			ma.cancel()
-			if resp.err != nil {
-				return false, resp.err
-			}
-			ms.logger.Info("obstacle detection triggering a replan")
+		// success
+		if !resp.Replan {
+			return true, nil
 		}
 
-		if valExtra.maxReplans >= 0 {
-			replanCount++
-			if replanCount > valExtra.maxReplans {
-				return false, fmt.Errorf("exceeded maximum number of replans: %d", valExtra.maxReplans)
-			}
-		}
+		// replan
+		ms.logger.Warnf("Replanning triggered: Reason: %s\n", resp.ReplanReason)
+		replanCount++
 		// TODO: RSDK-4509 obstacles should include any transient obstacles which may have triggered a replan, if any.
-		mr, err = ms.newMoveOnGlobeRequest(ctx, componentName, destination, movementSensorName, obstacles, motionCfg, mr.seedPlan, valExtra)
+		mr, err = ms.newMoveOnGlobeRequest(ctx, req, planResp.Motionplan, replanCount)
 		if err != nil {
 			return false, err
 		}