add a wrapper around motion/builtin DoCommand utilities

services/motion/builtin/builtin.go

@@ -44,14 +44,10 @@ func init() {
 	)
 }
 
-// export keys to be used with DoCommand so they can be referenced by clients.
-const (
-	DoPlan    = "plan"
-	DoExecute = "execute"
-)
-
 const (
 	builtinOpLabel                     = "motion-service"
+	doPlan                             = "plan"
+	doExecute                          = "execute"
 	maxTravelDistanceMM                = 5e6 // this is equivalent to 5km
 	lookAheadDistanceMM        float64 = 5e6
 	defaultSmoothIter                  = 30
@@ -336,11 +332,11 @@ func (ms *builtIn) PlanHistory(
 }
 
 // DoCommand supports two commands which are specified through the command map
-//   - DoPlan generates and returns a Trajectory for a given motionpb.MoveRequest without executing it
+//   - doPlan generates and returns a Trajectory for a given motionpb.MoveRequest without executing it
 //     required key: DoPlan
 //     input value: a motionpb.MoveRequest which will be used to create a Trajectory
 //     output value: a motionplan.Trajectory specified as a map (the mapstructure.Decode function is useful for decoding this)
-//   - DoExecute takes a Trajectory and executes it
+//   - doExecute takes a Trajectory and executes it
 //     required key: DoExecute
 //     input value: a motionplan.Trajectory
 //     output value: a bool
@@ -350,7 +346,7 @@ func (ms *builtIn) DoCommand(ctx context.Context, cmd map[string]interface{}) (m
 	operation.CancelOtherWithLabel(ctx, builtinOpLabel)
 
 	resp := make(map[string]interface{}, 0)
-	if req, ok := cmd[DoPlan]; ok {
+	if req, ok := cmd[doPlan]; ok {
 		bytes, err := json.Marshal(req)
 		if err != nil {
 			return nil, err
@@ -368,17 +364,17 @@ func (ms *builtIn) DoCommand(ctx context.Context, cmd map[string]interface{}) (m
 		if err != nil {
 			return nil, err
 		}
-		resp[DoPlan] = plan.Trajectory()
+		resp[doPlan] = plan.Trajectory()
 	}
-	if req, ok := cmd[DoExecute]; ok {
+	if req, ok := cmd[doExecute]; ok {
 		var trajectory motionplan.Trajectory
 		if err := mapstructure.Decode(req, &trajectory); err != nil {
 			return nil, err
 		}
 		if err := ms.execute(ctx, trajectory); err != nil {
 			return nil, err
 		}
-		resp[DoExecute] = true
+		resp[doExecute] = true
 	}
 	return resp, nil
 }
@@ -512,3 +508,28 @@ func (ms *builtIn) execute(ctx context.Context, trajectory motionplan.Trajectory
 	}
 	return nil
 }
+
+// DoPlan is a helper function to wrap doPlan (a utility inside DoCommand) with types that are easier to work with.
+func DoPlan(ctx context.Context, ms motion.Service, req motion.MoveReq) (motionplan.Trajectory, error) {
+	proto, err := req.ToProto(ms.Name().Name)
+	if err != nil {
+		return nil, err
+	}
+	resp, err := ms.DoCommand(ctx, map[string]interface{}{doPlan: proto})
+	if err != nil {
+		return nil, err
+	}
+	respMap, ok := resp[doPlan]
+	if !ok {
+		return nil, errors.New("could not find Trajectory in DoCommand response")
+	}
+	var trajectory motionplan.Trajectory
+	err = mapstructure.Decode(respMap, &trajectory)
+	return trajectory, err
+}
+
+// DoExecute is a helper function to wrap doExecute (a utility inside DoCommand) with types that are easier to work with.
+func DoExecute(ctx context.Context, ms motion.Service, traj motionplan.Trajectory) error {
+	_, err := ms.DoCommand(ctx, map[string]interface{}{doExecute: traj})
+	return err
+}

services/motion/builtin/builtin_test.go

@@ -8,7 +8,6 @@ import (
 	"testing"
 	"time"
 
-	"github.com/go-viper/mapstructure/v2"
 	"github.com/golang/geo/r3"
 	geo "github.com/kellydunn/golang-geo"
 	"github.com/pkg/errors"
@@ -1247,56 +1246,32 @@ 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})),
-	}
+	ms, teardown := setupMotionServiceFromConfig(t, "../data/moving_arm.json")
+	defer teardown()
 
-	// need to simulate what happens when the DoCommand message is serialized/deserialized into proto
-	doOverWire := func(ms motion.Service, cmd map[string]interface{}) map[string]interface{} {
+	// need to simulate what happens when the DoCommand message is serialized/deserialized into proto so wrap service with inject
+	// service that mirrors the transformations to/from proto that will happen with a client/server
+	ims := inject.NewMotionService("x")
+	ims.DoCommandFunc = func(ctx context.Context, cmd map[string]interface{}) (map[string]interface{}, error) {
 		command, err := protoutils.StructToStructPb(cmd)
 		test.That(t, err, test.ShouldBeNil)
 		resp, err := ms.DoCommand(ctx, command.AsMap())
 		test.That(t, err, test.ShouldBeNil)
 		respProto, err := protoutils.StructToStructPb(resp)
 		test.That(t, err, test.ShouldBeNil)
-		return respProto.AsMap()
+		return respProto.AsMap(), nil
 	}
 
-	t.Run("DoPlan", func(t *testing.T) {
-		ms, teardown := setupMotionServiceFromConfig(t, "../data/moving_arm.json")
-		defer teardown()
-
-		// format the command to send DoCommand
-		proto, err := moveReq.ToProto(ms.Name().Name)
-		test.That(t, err, test.ShouldBeNil)
-		cmd := map[string]interface{}{DoPlan: proto}
-
-		// simulate going over the wire
-		resp, ok := doOverWire(ms, cmd)[DoPlan]
-		test.That(t, ok, test.ShouldBeTrue)
-
-		// the client will need to decode the response still
-		var trajectory motionplan.Trajectory
-		err = mapstructure.Decode(resp, &trajectory)
-		test.That(t, err, test.ShouldBeNil)
-		test.That(t, len(trajectory), test.ShouldEqual, 2)
-	})
-	t.Run("DoExectute", func(t *testing.T) {
-		ms, teardown := setupMotionServiceFromConfig(t, "../data/moving_arm.json")
-		defer teardown()
-
-		plan, err := ms.(*builtIn).plan(ctx, moveReq)
-		test.That(t, err, test.ShouldBeNil)
-
-		// format the command to sent DoCommand
-		cmd := map[string]interface{}{DoExecute: plan.Trajectory()}
+	moveReq := motion.MoveReq{
+		ComponentName: gripper.Named("pieceGripper"),
+		Destination:   referenceframe.NewPoseInFrame("c", spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: -30, Z: -50})),
+	}
 
-		// simulate going over the wire
-		resp, ok := doOverWire(ms, cmd)[DoExecute]
-		test.That(t, ok, test.ShouldBeTrue)
+	// test DoPlan
+	traj, err := DoPlan(ctx, ims, moveReq)
+	test.That(t, err, test.ShouldBeNil)
+	test.That(t, len(traj), test.ShouldEqual, 2)
 
-		// the client will need to decode the response still
-		test.That(t, resp, test.ShouldBeTrue)
-	})
+	// test DoExecute
+	test.That(t, DoExecute(ctx, ims, traj), test.ShouldBeNil)
 }