RSDK-2935 add slam services as weak dependencies to builtin motion service

internal/resource.go

@@ -17,3 +17,12 @@ var ComponentDependencyWildcardMatcher = ResourceMatcher(componentDependencyWild
 type componentDependencyWildcardMatcher string
 
 func (c componentDependencyWildcardMatcher) notActuallyImplementedYet() {}
+
+// SLAMDependencyWildcardMatcher is used internally right now for lack of a better way to
+// "select" slam services that another resource is dependency on. Usage of this is an
+// anti-pattern and a better matcher system should exist.
+var SLAMDependencyWildcardMatcher = ResourceMatcher(slamDependencyWildcardMatcher("rdk:service:slam/*:*"))
+
+type slamDependencyWildcardMatcher string
+
+func (s slamDependencyWildcardMatcher) notActuallyImplementedYet() {}

robot/impl/local_robot.go

@@ -35,6 +35,7 @@ import (
 	"go.viam.com/rdk/robot/packages"
 	"go.viam.com/rdk/robot/web"
 	weboptions "go.viam.com/rdk/robot/web/options"
+	"go.viam.com/rdk/services/slam"
 	"go.viam.com/rdk/session"
 	"go.viam.com/rdk/utils"
 )
@@ -545,7 +546,7 @@ func (r *localRobot) getDependencies(
 	return allDeps, nil
 }
 
-func (r *localRobot) getWeakDependencyNames(api resource.API, model resource.Model) []internal.ResourceMatcher {
+func (r *localRobot) getWeakDependencyMatchers(api resource.API, model resource.Model) []internal.ResourceMatcher {
 	reg, ok := resource.LookupRegistration(api, model)
 	if !ok {
 		return nil
@@ -554,11 +555,12 @@ func (r *localRobot) getWeakDependencyNames(api resource.API, model resource.Mod
 }
 
 func (r *localRobot) getWeakDependencies(resName resource.Name, api resource.API, model resource.Model) resource.Dependencies {
-	weakDepNames := r.getWeakDependencyNames(api, model)
+	weakDepMatchers := r.getWeakDependencyMatchers(api, model)
 
 	allResources := map[resource.Name]resource.Resource{}
 	internalResources := map[resource.Name]resource.Resource{}
 	components := map[resource.Name]resource.Resource{}
+	slamServices := map[resource.Name]resource.Resource{}
 	for _, n := range r.manager.resources.Names() {
 		if !(n.API.IsComponent() || n.API.IsService()) {
 			continue
@@ -570,28 +572,32 @@ func (r *localRobot) getWeakDependencies(resName resource.Name, api resource.API
 			}
 			continue
 		}
+		allResources[n] = res
 		switch {
 		case n.API.IsComponent():
-			allResources[n] = res
 			components[n] = res
-		default:
-			allResources[n] = res
-			if n.API.Type.Namespace == resource.APINamespaceRDKInternal {
-				internalResources[n] = res
-			}
+		case n.API.SubtypeName == slam.API.SubtypeName:
+			slamServices[n] = res
+		case n.API.Type.Namespace == resource.APINamespaceRDKInternal:
+			internalResources[n] = res
 		}
 	}
 
-	deps := make(resource.Dependencies, len(weakDepNames))
-	for _, dep := range weakDepNames {
-		switch dep {
-		case internal.ComponentDependencyWildcardMatcher:
-			for k, v := range components {
+	deps := make(resource.Dependencies, len(weakDepMatchers))
+	for _, matcher := range weakDepMatchers {
+		match := func(resouces map[resource.Name]resource.Resource) {
+			for k, v := range resouces {
 				if k == resName {
 					continue
 				}
 				deps[k] = v
 			}
+		}
+		switch matcher {
+		case internal.ComponentDependencyWildcardMatcher:
+			match(components)
+		case internal.SLAMDependencyWildcardMatcher:
+			match(slamServices)
 		default:
 			// no other matchers supported right now. you could imagine a LiteralMatcher in the future
 		}
@@ -660,15 +666,12 @@ func (r *localRobot) updateWeakDependents(ctx context.Context) {
 			}
 			continue
 		}
+		allResources[n] = res
 		switch {
 		case n.API.IsComponent():
-			allResources[n] = res
 			components[n] = res
-		default:
-			allResources[n] = res
-			if n.API.Type.Namespace == resource.APINamespaceRDKInternal {
-				internalResources[n] = res
-			}
+		case n.API.Type.Namespace == resource.APINamespaceRDKInternal:
+			internalResources[n] = res
 		}
 	}
 
@@ -700,7 +703,7 @@ func (r *localRobot) updateWeakDependents(ctx context.Context) {
 		if err != nil {
 			return
 		}
-		if len(r.getWeakDependencyNames(conf.API, conf.Model)) == 0 {
+		if len(r.getWeakDependencyMatchers(conf.API, conf.Model)) == 0 {
 			return
 		}
 		deps, err := r.getDependencies(ctx, resName, resNode)

services/motion/builtin/builtin.go

@@ -3,21 +3,24 @@ package builtin
 
 import (
 	"context"
-	"errors"
 	"fmt"
 	"sync"
 
+	"github.com/pkg/errors"
+
 	"github.com/edaniels/golog"
 	"github.com/golang/geo/r3"
 
 	servicepb "go.viam.com/api/service/motion/v1"
 	"go.viam.com/rdk/components/arm"
+	"go.viam.com/rdk/internal"
 	"go.viam.com/rdk/motionplan"
 	"go.viam.com/rdk/operation"
 	"go.viam.com/rdk/referenceframe"
 	"go.viam.com/rdk/resource"
 	"go.viam.com/rdk/robot/framesystem"
 	"go.viam.com/rdk/services/motion"
+	"go.viam.com/rdk/services/slam"
 	"go.viam.com/rdk/spatialmath"
 )
 
@@ -34,6 +37,7 @@ func init() {
 			) (motion.Service, error) {
 				return NewBuiltIn(ctx, deps, conf, logger)
 			},
+			WeakDependencies: []internal.ResourceMatcher{internal.SLAMDependencyWildcardMatcher},
 		})
 }
 
@@ -64,24 +68,30 @@ func (ms *builtIn) Reconfigure(
 	ctx context.Context,
 	deps resource.Dependencies,
 	conf resource.Config,
-) error {
+) (err error) {
 	ms.lock.Lock()
 	defer ms.lock.Unlock()
 
-	fsService, err := resource.FromDependencies[framesystem.Service](deps, framesystem.InternalServiceName)
-	if err != nil {
-		return err
+	slamServices := make(map[resource.Name]slam.Service)
+	for name, dep := range deps {
+		switch dep := dep.(type) {
+		case framesystem.Service:
+			ms.fsService = dep
+		case slam.Service:
+			slamServices[name] = dep
+		}
 	}
-	ms.fsService = fsService
+	ms.slamServices = slamServices
 	return nil
 }
 
 type builtIn struct {
 	resource.Named
 	resource.TriviallyCloseable
-	fsService framesystem.Service
-	logger    golog.Logger
-	lock      sync.Mutex
+	fsService    framesystem.Service
+	slamServices map[resource.Name]slam.Service
+	logger       golog.Logger
+	lock         sync.Mutex
 }
 
 // Move takes a goal location and will plan and execute a movement to move a component specified by its name to that destination.
@@ -165,7 +175,12 @@ func (ms *builtIn) MoveOnMap(
 	slamName resource.Name,
 	extra map[string]interface{},
 ) (bool, error) {
-	return false, errors.New("this is not implemented yet")
+	slamService, ok := ms.slamServices[slamName]
+	if !ok {
+		return false, errors.Wrap(resource.NewNotFoundError(slamName), "motion service missing weak dependency")
+	}
+	_ = slamService
+	return true, nil
 }
 
 // MoveSingleComponent will pass through a move command to a component with a MoveToPosition method that takes a pose. Arms are the only

services/motion/builtin/builtin_test.go

@@ -10,6 +10,7 @@ import (
 	"go.viam.com/test"
 
 	"go.viam.com/rdk/components/arm"
+	"go.viam.com/rdk/components/base"
 	"go.viam.com/rdk/components/camera"
 	"go.viam.com/rdk/components/gripper"
 
@@ -21,6 +22,8 @@ import (
 	framesystemparts "go.viam.com/rdk/robot/framesystem/parts"
 	robotimpl "go.viam.com/rdk/robot/impl"
 	"go.viam.com/rdk/services/motion"
+	_ "go.viam.com/rdk/services/register"
+	"go.viam.com/rdk/services/slam"
 	"go.viam.com/rdk/spatialmath"
 )
 
@@ -220,6 +223,20 @@ func TestMoveSingleComponent(t *testing.T) {
 	})
 }
 
+func TestMoveOnMap(t *testing.T) {
+	ms, closeFn := setupMotionServiceFromConfig(t, "../data/wheeled_base.json")
+	defer closeFn()
+	success, err := ms.MoveOnMap(
+		context.Background(),
+		base.Named("test_base"),
+		spatialmath.NewPoseFromPoint(r3.Vector{Y: 10}),
+		slam.Named("test_slam"),
+		nil,
+	)
+	test.That(t, err, test.ShouldBeNil)
+	test.That(t, success, test.ShouldBeTrue)
+}
+
 func TestMultiplePieces(t *testing.T) {
 	var err error
 	ms, teardown := setupMotionServiceFromConfig(t, "../data/fake_tomato.json")

services/motion/data/wheeled_base.json

@@ -0,0 +1,90 @@
+{
+  "components": [
+    {
+      "name": "test_base",
+      "type": "base",
+      "model": "fake",
+      "attributes": {
+        "wheel_circumference_mm": 217,
+        "left": [
+          "fake-left"
+        ],
+        "right": [
+          "fake-right"
+        ],
+        "width_mm": 260,
+        "spin_slip_factor": 1.76
+      },
+      "depends_on": [],
+      "frame": {
+        "parent": "world",
+        "translation": {
+          "x": 0,
+          "y": 0,
+          "z": 0
+        },
+        "orientation": {
+          "type": "ov_degrees",
+          "value": {
+            "x": 0,
+            "y": 0,
+            "z": 1,
+            "th": 0
+          }
+        },
+        "geometry": {
+          "r": 20,
+          "translation": {
+            "x": 0,
+            "y": 0,
+            "z": 0
+          }
+        }
+      }
+    },
+    {
+      "name": "fake-left",
+      "type": "motor",
+      "model": "fake",
+      "attributes": {
+        "pins": {
+          "dir": "",
+          "pwm": ""
+        },
+        "board": "",
+        "max_rpm": 1
+      },
+      "depends_on": []
+    },
+    {
+      "name": "fake-right",
+      "type": "motor",
+      "model": "fake",
+      "attributes": {
+        "pins": {
+          "dir": "",
+          "pwm": ""
+        },
+        "board": "",
+        "max_rpm": 1
+      },
+      "depends_on": []
+    },
+    {
+      "name": "fake-board",
+      "type": "board",
+      "model": "fake",
+      "attributes": {
+        "fail_new": false
+      },
+      "depends_on": []
+    }
+  ],
+  "services": [
+    {
+      "model": "fake",
+      "name": "test_slam",
+      "type": "slam"
+    }
+  ]
+}