From 98df2d7f24c97124ad4f322ec7ec2e06af0bbfa4 Mon Sep 17 00:00:00 2001 From: Ray Bjorkman Date: Mon, 23 Sep 2024 16:11:21 -0400 Subject: [PATCH] add a wrapper around DoCommand utilities --- services/motion/builtin/builtin.go | 45 ++++++++++++++----- services/motion/builtin/builtin_test.go | 59 +++++++------------------ 2 files changed, 50 insertions(+), 54 deletions(-) diff --git a/services/motion/builtin/builtin.go b/services/motion/builtin/builtin.go index 1aca849e8b4..b4065d352d8 100644 --- a/services/motion/builtin/builtin.go +++ b/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,9 +364,9 @@ 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 @@ -378,7 +374,7 @@ func (ms *builtIn) DoCommand(ctx context.Context, cmd map[string]interface{}) (m 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 +} diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index 343d2277224..89dd9640a82 100644 --- a/services/motion/builtin/builtin_test.go +++ b/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) }