add explore back in

services/motion/builtin/builtin.go

@@ -182,13 +182,14 @@ func (ms *builtIn) Close(ctx context.Context) error {
 func (ms *builtIn) Move(ctx context.Context, req motion.MoveReq) (bool, error) {
 	ms.mu.RLock()
 	defer ms.mu.RUnlock()
+	operation.CancelOtherWithLabel(ctx, builtinOpLabel)
 
 	plan, err := ms.plan(ctx, req)
 	if err != nil {
 		return false, err
 	}
 	err = ms.execute(ctx, plan.Trajectory())
-	return err != nil, err
+	return err == nil, err
 }
 
 func (ms *builtIn) MoveOnMap(ctx context.Context, req motion.MoveOnMapReq) (motion.ExecutionID, error) {
@@ -337,6 +338,7 @@ func (ms *builtIn) PlanHistory(
 func (ms *builtIn) DoCommand(ctx context.Context, cmd map[string]interface{}) (map[string]interface{}, error) {
 	ms.mu.RLock()
 	defer ms.mu.RUnlock()
+	operation.CancelOtherWithLabel(ctx, builtinOpLabel)
 
 	resp := make(map[string]interface{}, 0)
 	if req, ok := cmd[DoPlan]; ok {
@@ -373,8 +375,6 @@ func (ms *builtIn) DoCommand(ctx context.Context, cmd map[string]interface{}) (m
 }
 
 func (ms *builtIn) plan(ctx context.Context, req motion.MoveReq) (motionplan.Plan, error) {
-	operation.CancelOtherWithLabel(ctx, builtinOpLabel)
-
 	frameSys, err := ms.fsService.FrameSystem(ctx, req.WorldState.Transforms())
 	if err != nil {
 		return nil, err

services/motion/builtin/builtin_test.go

@@ -1242,7 +1242,6 @@ func TestCheckPlan(t *testing.T) {
 
 func TestDoCommand(t *testing.T) {
 	ctx := context.Background()
-
 	moveReq := motion.MoveReq{
 		ComponentName: gripper.Named("pieceGripper"),
 		Destination:   referenceframe.NewPoseInFrame("c", spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: -30, Z: -50})),

services/motion/client_test.go

@@ -11,8 +11,6 @@ import (
 	"github.com/google/uuid"
 	geo "github.com/kellydunn/golang-geo"
 	"github.com/pkg/errors"
-	commonpb "go.viam.com/api/common/v1"
-	pb "go.viam.com/api/service/motion/v1"
 	"go.viam.com/test"
 	"go.viam.com/utils/rpc"
 
@@ -26,7 +24,6 @@ import (
 	"go.viam.com/rdk/referenceframe"
 	"go.viam.com/rdk/resource"
 	"go.viam.com/rdk/services/motion"
-	"go.viam.com/rdk/services/motion/builtin"
 	"go.viam.com/rdk/services/slam"
 	"go.viam.com/rdk/spatialmath"
 	"go.viam.com/rdk/testutils"
@@ -38,54 +35,6 @@ var (
 	testMotionServiceName2 = motion.Named("motion2")
 )
 
-func TestDoCommandClient(t *testing.T) {
-	ctx := context.Background()
-	logger := logging.NewTestLogger(t)
-	listener1, err := net.Listen("tcp", "localhost:0")
-	test.That(t, err, test.ShouldBeNil)
-	rpcServer, err := rpc.NewServer(logger, rpc.WithUnauthenticated())
-	test.That(t, err, test.ShouldBeNil)
-
-	ms, err := builtin.NewBuiltIn(ctx, resource.Dependencies{}, resource.Config{ConvertedAttributes: &builtin.Config{}}, logging.NewLogger(""))
-	test.That(t, err, test.ShouldBeNil)
-	resources := map[resource.Name]motion.Service{
-		testMotionServiceName: ms,
-	}
-
-	svc, err := resource.NewAPIResourceCollection(motion.API, resources)
-	test.That(t, err, test.ShouldBeNil)
-	resourceAPI, ok, err := resource.LookupAPIRegistration[motion.Service](motion.API)
-	test.That(t, err, test.ShouldBeNil)
-	test.That(t, ok, test.ShouldBeTrue)
-	test.That(t, resourceAPI.RegisterRPCService(context.Background(), rpcServer, svc), test.ShouldBeNil)
-
-	go func() {
-		test.That(t, rpcServer.Serve(listener1), test.ShouldBeNil)
-	}()
-
-	defer func() {
-		test.That(t, rpcServer.Stop(), test.ShouldBeNil)
-	}()
-
-	conn, err := viamgrpc.Dial(context.Background(), listener1.Addr().String(), logger)
-
-	test.That(t, err, test.ShouldBeNil)
-
-	client, err := motion.NewClientFromConn(context.Background(), conn, "", testMotionServiceName, logger)
-	test.That(t, err, test.ShouldBeNil)
-
-	cmd := make(map[string]interface{})
-	cmd[builtin.DoPlan] = pb.MoveRequest{
-		Name: "builtin",
-		Destination: &commonpb.PoseInFrame{
-			ReferenceFrame: "world",
-			Pose:           spatialmath.PoseToProtobuf(spatialmath.NewZeroPose()),
-		},
-		ComponentName: &commonpb.ResourceName{},
-	}
-	client.DoCommand(ctx, cmd)
-}
-
 func TestClient(t *testing.T) {
 	ctx := context.Background()
 	logger := logging.NewTestLogger(t)

services/navigation/builtin/builtin.go

@@ -25,6 +25,7 @@ import (
 	"go.viam.com/rdk/resource"
 	"go.viam.com/rdk/robot/framesystem"
 	"go.viam.com/rdk/services/motion"
+	"go.viam.com/rdk/services/motion/explore"
 	"go.viam.com/rdk/services/navigation"
 	"go.viam.com/rdk/services/vision"
 	"go.viam.com/rdk/spatialmath"
@@ -391,11 +392,11 @@ func (svc *builtIn) Reconfigure(ctx context.Context, deps resource.Dependencies,
 
 	// Create explore motion service
 	// Note: this service will disappear after the explore motion model is integrated into builtIn
-	// exploreMotionConf := resource.Config{ConvertedAttributes: &explore.Config{}}
-	// svc.exploreMotionService, err = explore.NewExplore(ctx, deps, exploreMotionConf, svc.logger)
-	// if err != nil {
-	// 	return err
-	// }
+	exploreMotionConf := resource.Config{ConvertedAttributes: &explore.Config{}}
+	svc.exploreMotionService, err = explore.NewExplore(ctx, deps, exploreMotionConf, svc.logger)
+	if err != nil {
+		return err
+	}
 
 	svc.mode = navigation.ModeManual
 	svc.base = baseComponent
@@ -458,8 +459,7 @@ func (svc *builtIn) SetMode(ctx context.Context, mode navigation.Mode, extra map
 		if len(svc.motionCfg.ObstacleDetectors) == 0 {
 			return errors.New("explore mode requires at least one vision service")
 		}
-		panic("no way to explore")
-		// svc.startExploreMode(cancelCtx)
+		svc.startExploreMode(cancelCtx)
 	}
 
 	return nil

services/navigation/builtin/explore.go

@@ -0,0 +1,46 @@
+package builtin
+
+import (
+	"context"
+	"math"
+	"math/rand"
+
+	"github.com/golang/geo/r3"
+	"go.viam.com/utils"
+
+	"go.viam.com/rdk/referenceframe"
+	"go.viam.com/rdk/spatialmath"
+)
+
+const defaultDistanceMM = 50 * 1000
+
+func (svc *builtIn) startExploreMode(ctx context.Context) {
+	svc.logger.CDebug(ctx, "startExploreMode called")
+
+	svc.activeBackgroundWorkers.Add(1)
+
+	utils.ManagedGo(func() {
+		// Send motionCfg parameters through extra until motionCfg can be added to Move()
+		extra := map[string]interface{}{"motionCfg": *svc.motionCfg}
+
+		for {
+			if ctx.Err() != nil {
+				return
+			}
+
+			// Choose a new random point using a normal distribution centered on the position directly the robot
+			randAngle := rand.NormFloat64() + math.Pi
+			destination := referenceframe.NewPoseInFrame(svc.base.Name().Name, spatialmath.NewPose(
+				r3.Vector{
+					X: math.Sin(randAngle),
+					Y: math.Cos(randAngle),
+					Z: 0.,
+				}.Normalize().Mul(defaultDistanceMM), spatialmath.NewOrientationVector()))
+
+			_, err := svc.exploreMotionService.Move(ctx, svc.base.Name(), destination, nil, nil, extra)
+			if err != nil {
+				svc.logger.CDebugf(ctx, "error occurred when moving to point %v: %v", destination, err)
+			}
+		}
+	}, svc.activeBackgroundWorkers.Done)
+}

services/navigation/builtin/explore_test.go

@@ -0,0 +1,42 @@
+package builtin
+
+import (
+	"context"
+	"testing"
+	"time"
+
+	"github.com/golang/geo/r3"
+	"github.com/pkg/errors"
+	"go.viam.com/test"
+
+	"go.viam.com/rdk/motionplan"
+	frame "go.viam.com/rdk/referenceframe"
+	"go.viam.com/rdk/resource"
+	"go.viam.com/rdk/testutils/inject"
+)
+
+func TestExploreMode(t *testing.T) {
+	cancelCtx, cancel := context.WithCancel(context.Background())
+	defer cancel()
+	ns, teardown := setupNavigationServiceFromConfig(t, "../data/nav_no_map_cfg.json")
+
+	var points []r3.Vector
+	mockExploreMotionService := &inject.MotionService{}
+	mockExploreMotionService.MoveFunc = func(ctx context.Context, componentName resource.Name,
+		destination *frame.PoseInFrame, worldState *frame.WorldState, constraints *motionplan.Constraints,
+		extra map[string]interface{},
+	) (bool, error) {
+		points = append(points, destination.Pose().Point())
+		return false, errors.New("expected error")
+	}
+
+	nsStruct := ns.(*builtIn)
+	nsStruct.exploreMotionService = mockExploreMotionService
+
+	ctxTimeout, cancelFunc := context.WithTimeout(cancelCtx, 50*time.Millisecond)
+	defer cancelFunc()
+	nsStruct.startExploreMode(ctxTimeout)
+	<-ctxTimeout.Done()
+	teardown()
+	test.That(t, len(points), test.ShouldBeGreaterThan, 2)
+}