allow passing MoveReq options in motion/builtin.Do (viamrobotics#4632)

components/camera/collectors.go

@@ -1,7 +1,6 @@
 package camera
 
 import (
-	"bytes"
 	"context"
 	"time"
 
@@ -49,7 +48,7 @@ func newNextPointCloudCollector(resource interface{}, params data.CollectorParam
 
 		ctx = context.WithValue(ctx, data.FromDMContextKey{}, true)
 
-		v, err := camera.NextPointCloud(ctx)
+		pc, err := camera.NextPointCloud(ctx)
 		if err != nil {
 			// A modular filter component can be created to filter the readings from a component. The error ErrNoCaptureToStore
 			// is used in the datamanager to exclude readings from being captured and stored.
@@ -58,22 +57,16 @@ func newNextPointCloudCollector(resource interface{}, params data.CollectorParam
 			}
 			return res, data.FailedToReadErr(params.ComponentName, nextPointCloud.String(), err)
 		}
-
-		var buf bytes.Buffer
-		headerSize := 200
-		if v != nil {
-			buf.Grow(headerSize + v.Size()*4*4) // 4 numbers per point, each 4 bytes
-			err = pointcloud.ToPCD(v, &buf, pointcloud.PCDBinary)
-			if err != nil {
-				return res, errors.Errorf("failed to convert returned point cloud to PCD: %v", err)
-			}
+		bytes, err := pointcloud.ToBytes(pc)
+		if err != nil {
+			return res, errors.Errorf("failed to convert returned point cloud to PCD: %v", err)
 		}
 		ts := data.Timestamps{
 			TimeRequested: timeRequested,
 			TimeReceived:  time.Now(),
 		}
 		return data.NewBinaryCaptureResult(ts, []data.Binary{{
-			Payload:  buf.Bytes(),
+			Payload:  bytes,
 			MimeType: data.MimeTypeApplicationPcd,
 		}}), nil
 	})

components/camera/server.go

@@ -1,7 +1,6 @@
 package camera
 
 import (
-	"bytes"
 	"context"
 	"fmt"
 	"image"
@@ -201,18 +200,14 @@ func (s *serviceServer) GetPointCloud(
 		return nil, err
 	}
 
-	var buf bytes.Buffer
-	buf.Grow(200 + (pc.Size() * 4 * 4)) // 4 numbers per point, each 4 bytes
-	_, pcdSpan := trace.StartSpan(ctx, "camera::server::NextPointCloud::ToPCD")
-	err = pointcloud.ToPCD(pc, &buf, pointcloud.PCDBinary)
-	pcdSpan.End()
+	bytes, err := pointcloud.ToBytes(pc)
 	if err != nil {
 		return nil, err
 	}
 
 	return &pb.GetPointCloudResponse{
 		MimeType:   utils.MimeTypePCD,
-		PointCloud: buf.Bytes(),
+		PointCloud: bytes,
 	}, nil
 }
 

motionplan/errors.go

@@ -28,7 +28,12 @@ var (
 	errMixedFrameTypes = errors.New("unable to plan for PTG and non-PTG frames simultaneously")
 )
 
-func genIKConstraintErr(failures map[string]int, constraintFailCnt int) error {
+// NewAlgAndConstraintMismatchErr is returned when an incompatible planning_alg is specified and there are contraints.
+func NewAlgAndConstraintMismatchErr(planAlg string) error {
+	return fmt.Errorf("cannot specify a planning alg other than cbirrt with topo constraints. alg specified was %s", planAlg)
+}
+
+func newIKConstraintErr(failures map[string]int, constraintFailCnt int) error {
 	ikConstraintFailures := errIKConstraint
 	for failName, count := range failures {
 		ikConstraintFailures += fmt.Sprintf("{ %s: %.2f%% }, ", failName, 100*float64(count)/float64(constraintFailCnt))

motionplan/motionPlanner.go

@@ -469,7 +469,7 @@ IK:
 			return nil, errIKSolve
 		}
 
-		return nil, genIKConstraintErr(failures, constraintFailCnt)
+		return nil, newIKConstraintErr(failures, constraintFailCnt)
 	}
 
 	keys := make([]float64, 0, len(solutions))

pointcloud/pointcloud_utils.go

@@ -1,6 +1,7 @@
 package pointcloud
 
 import (
+	"bytes"
 	"math"
 
 	"github.com/golang/geo/r3"
@@ -117,3 +118,16 @@ func ToBasicOctree(cloud PointCloud) (*BasicOctree, error) {
 	}
 	return basicOctree, nil
 }
+
+// ToBytes takes a pointcloud object and converts it to bytes.
+func ToBytes(cloud PointCloud) ([]byte, error) {
+	if cloud == nil {
+		return nil, errors.New("pointcloud cannot be nil")
+	}
+	var buf bytes.Buffer
+	buf.Grow(200 + (cloud.Size() * 4 * 4)) // 4 numbers per point, each 4 bytes, 200 is header size
+	if err := ToPCD(cloud, &buf, PCDBinary); err != nil {
+		return nil, err
+	}
+	return buf.Bytes(), nil
+}

robot/server/server.go

@@ -276,13 +276,11 @@ func (s *Server) TransformPCD(ctx context.Context, req *pb.TransformPCDRequest)
 		return nil, err
 	}
 	// transform pointcloud back to PCD bytes
-	var buf bytes.Buffer
-	buf.Grow(200 + (final.Size() * 4 * 4)) // 4 numbers per point, each 4 bytes
-	err = pointcloud.ToPCD(final, &buf, pointcloud.PCDBinary)
+	bytes, err := pointcloud.ToBytes(final)
 	if err != nil {
 		return nil, err
 	}
-	return &pb.TransformPCDResponse{PointCloudPcd: buf.Bytes()}, err
+	return &pb.TransformPCDResponse{PointCloudPcd: bytes}, err
 }
 
 // StopAll will stop all current and outstanding operations for the robot and stops all actuators and movement.

services/motion/builtin/builtin.go

@@ -13,6 +13,7 @@ import (
 	"github.com/pkg/errors"
 	pb "go.viam.com/api/service/motion/v1"
 	"google.golang.org/protobuf/encoding/protojson"
+	"google.golang.org/protobuf/types/known/structpb"
 
 	"go.viam.com/rdk/components/movementsensor"
 	"go.viam.com/rdk/logging"
@@ -360,6 +361,14 @@ func (ms *builtIn) DoCommand(ctx context.Context, cmd map[string]interface{}) (m
 		if err != nil {
 			return nil, err
 		}
+		fields := moveReqProto.Extra.AsMap()
+		if extra, err := utils.AssertType[map[string]interface{}](fields["fields"]); err == nil {
+			v, err := structpb.NewStruct(extra)
+			if err != nil {
+				return nil, err
+			}
+			moveReqProto.Extra = v
+		}
 		moveReq, err := motion.MoveReqFromProto(&moveReqProto)
 		if err != nil {
 			return nil, err

services/motion/builtin/builtin_test.go

@@ -1256,20 +1256,23 @@ func TestDoCommand(t *testing.T) {
 		ComponentName: gripper.Named("pieceGripper"),
 		WorldState:    worldState,
 		Destination:   referenceframe.NewPoseInFrame("c", spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: -30, Z: -50})),
+		Extra:         nil,
 	}
 
 	// 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{} {
+	doOverWire := func(ms motion.Service, 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)
+		if err != nil {
+			return map[string]interface{}{}, err
+		}
 		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) {
+	testDoPlan := func(moveReq motion.MoveReq) (motionplan.Trajectory, error) {
 		ms, teardown := setupMotionServiceFromConfig(t, "../data/moving_arm.json")
 		defer teardown()
 
@@ -1281,15 +1284,25 @@ func TestDoCommand(t *testing.T) {
 		cmd := map[string]interface{}{DoPlan: string(bytes)}
 
 		// simulate going over the wire
-		resp, ok := doOverWire(ms, cmd)[DoPlan]
+		respMap, err := doOverWire(ms, cmd)
+		if err != nil {
+			return nil, err
+		}
+		resp, ok := respMap[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)
+		return trajectory, err
+	}
+
+	t.Run("DoPlan", func(t *testing.T) {
+		trajectory, err := testDoPlan(moveReq)
 		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()
@@ -1301,10 +1314,23 @@ func TestDoCommand(t *testing.T) {
 		cmd := map[string]interface{}{DoExecute: plan.Trajectory()}
 
 		// simulate going over the wire
-		resp, ok := doOverWire(ms, cmd)[DoExecute]
+		respMap, err := doOverWire(ms, cmd)
+		test.That(t, err, test.ShouldBeNil)
+		resp, ok := respMap[DoExecute]
 		test.That(t, ok, test.ShouldBeTrue)
 
 		// the client will need to decode the response still
 		test.That(t, resp, test.ShouldBeTrue)
 	})
+
+	t.Run("Extras transmitted correctly", func(t *testing.T) {
+		// test that DoPlan correctly breaks if bad inputs are provided, meaning it is being parsed correctly
+		moveReq.Extra = map[string]interface{}{
+			"motion_profile": motionplan.LinearMotionProfile,
+			"planning_alg":   "rrtstar",
+		}
+		_, err = testDoPlan(moveReq)
+		test.That(t, err, test.ShouldNotBeNil)
+		test.That(t, err, test.ShouldResemble, motionplan.NewAlgAndConstraintMismatchErr("rrtstar"))
+	})
 }