improve DoCommand in motion/builtin to properly support WorldStates (#…

…4629)
viamrobotics · Dec 16, 2024 · cbe96a4 · cbe96a4
1 parent 2c89ae3
commit cbe96a4
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 4 deletions.
diff --git a/services/motion/builtin/builtin.go b/services/motion/builtin/builtin.go
@@ -3,7 +3,6 @@ package builtin
 
 import (
 	"context"
-	"encoding/json"
 	"fmt"
 	"sync"
 	"time"
@@ -13,6 +12,7 @@ import (
 	"github.com/google/uuid"
 	"github.com/pkg/errors"
 	pb "go.viam.com/api/service/motion/v1"
+	"google.golang.org/protobuf/encoding/protojson"
 
 	"go.viam.com/rdk/components/movementsensor"
 	"go.viam.com/rdk/logging"
@@ -351,12 +351,12 @@ func (ms *builtIn) DoCommand(ctx context.Context, cmd map[string]interface{}) (m
 
 	resp := make(map[string]interface{}, 0)
 	if req, ok := cmd[DoPlan]; ok {
-		bytes, err := json.Marshal(req)
+		s, err := utils.AssertType[string](req)
 		if err != nil {
 			return nil, err
 		}
 		var moveReqProto pb.MoveRequest
-		err = json.Unmarshal(bytes, &moveReqProto)
+		err = protojson.Unmarshal([]byte(s), &moveReqProto)
 		if err != nil {
 			return nil, err
 		}

diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go
@@ -15,6 +15,7 @@ import (
 	commonpb "go.viam.com/api/common/v1"
 	"go.viam.com/test"
 	"go.viam.com/utils/protoutils"
+	"google.golang.org/protobuf/encoding/protojson"
 
 	"go.viam.com/rdk/components/arm"
 	armFake "go.viam.com/rdk/components/arm/fake"
@@ -1246,8 +1247,14 @@ func TestCheckPlan(t *testing.T) {
 
 func TestDoCommand(t *testing.T) {
 	ctx := context.Background()
+	box, err := spatialmath.NewBox(spatialmath.NewPoseFromPoint(r3.Vector{1000, 1000, 1000}), r3.Vector{1, 1, 1}, "box")
+	test.That(t, err, test.ShouldBeNil)
+	geometries := []*referenceframe.GeometriesInFrame{referenceframe.NewGeometriesInFrame("world", []spatialmath.Geometry{box})}
+	worldState, err := referenceframe.NewWorldState(geometries, nil)
+	test.That(t, err, test.ShouldBeNil)
 	moveReq := motion.MoveReq{
 		ComponentName: gripper.Named("pieceGripper"),
+		WorldState:    worldState,
 		Destination:   referenceframe.NewPoseInFrame("c", spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: -30, Z: -50})),
 	}
 
@@ -1269,7 +1276,9 @@ func TestDoCommand(t *testing.T) {
 		// 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}
+		bytes, err := protojson.Marshal(proto)
+		test.That(t, err, test.ShouldBeNil)
+		cmd := map[string]interface{}{DoPlan: string(bytes)}
 
 		// simulate going over the wire
 		resp, ok := doOverWire(ms, cmd)[DoPlan]