diff --git a/go.mod b/go.mod index f46f627e626..4fb3f3f7595 100644 --- a/go.mod +++ b/go.mod @@ -81,7 +81,7 @@ require ( go.uber.org/atomic v1.11.0 go.uber.org/multierr v1.11.0 go.uber.org/zap v1.27.0 - go.viam.com/api v0.1.452 + go.viam.com/api v0.1.455 go.viam.com/test v1.2.4 go.viam.com/utils v0.1.150 goji.io v2.0.2+incompatible diff --git a/go.sum b/go.sum index e3791f4530b..db0d646cd4f 100644 --- a/go.sum +++ b/go.sum @@ -1513,8 +1513,8 @@ go.uber.org/zap v1.18.1/go.mod h1:xg/QME4nWcxGxrpdeYfq7UvYrLh66cuVKdrbD1XF/NI= go.uber.org/zap v1.23.0/go.mod h1:D+nX8jyLsMHMYrln8A0rJjFt/T/9/bGgIhAqxv5URuY= go.uber.org/zap v1.27.0 h1:aJMhYGrd5QSmlpLMr2MftRKl7t8J8PTZPA732ud/XR8= go.uber.org/zap v1.27.0/go.mod h1:GB2qFLM7cTU87MWRP2mPIjqfIDnGu+VIO4V/SdhGo2E= -go.viam.com/api v0.1.452 h1:/rexbvi6Bfva/iqNEu2ftS6NrrQICg5CPnj1PKgTAi4= -go.viam.com/api v0.1.452/go.mod h1:gwJriv6EVWe97uFzzzWjzP3NPfpCrKtRAdWtYglUpqs= +go.viam.com/api v0.1.455 h1:WY++sFAydRGcg/E1va85BFPi9U7IuEf2Dk0JrPZ1Wzs= +go.viam.com/api v0.1.455/go.mod h1:gwJriv6EVWe97uFzzzWjzP3NPfpCrKtRAdWtYglUpqs= go.viam.com/test v1.2.4 h1:JYgZhsuGAQ8sL9jWkziAXN9VJJiKbjoi9BsO33TW3ug= go.viam.com/test v1.2.4/go.mod h1:zI2xzosHdqXAJ/kFqcN+OIF78kQuTV2nIhGZ8EzvaJI= go.viam.com/utils v0.1.150 h1:45r/HlGxeAaOQgHakjUhOKFUitOiN0vRlTxkXmzA7iA= diff --git a/module/modmanager/module.go b/module/modmanager/module.go index c0458b7e8ab..027290969cd 100644 --- a/module/modmanager/module.go +++ b/module/modmanager/module.go @@ -67,7 +67,8 @@ func (m *module) dial() error { if !rutils.TCPRegex.MatchString(addrToDial) { addrToDial = "unix:" + addrToDial } - conn, err := grpc.Dial( //nolint:staticcheck + //nolint:staticcheck + conn, err := grpc.Dial( addrToDial, grpc.WithDefaultCallOptions(grpc.MaxCallRecvMsgSize(rpc.MaxMessageSize)), grpc.WithTransportCredentials(insecure.NewCredentials()), diff --git a/module/module_test.go b/module/module_test.go index 2faf2b447c8..1777f9e0218 100644 --- a/module/module_test.go +++ b/module/module_test.go @@ -186,7 +186,8 @@ func TestModuleFunctions(t *testing.T) { test.That(t, m.Start(ctx), test.ShouldBeNil) - conn, err := grpc.Dial( //nolint:staticcheck + //nolint:staticcheck + conn, err := grpc.Dial( "unix://"+addr, grpc.WithTransportCredentials(insecure.NewCredentials()), grpc.WithStreamInterceptor(grpc_retry.StreamClientInterceptor()), @@ -477,7 +478,8 @@ func TestAttributeConversion(t *testing.T) { test.That(t, m.AddModelFromRegistry(ctx, shell.API, modelWithReconfigure), test.ShouldBeNil) test.That(t, m.Start(ctx), test.ShouldBeNil) - conn, err := grpc.Dial( //nolint:staticcheck + //nolint:staticcheck + conn, err := grpc.Dial( "unix://"+addr, grpc.WithTransportCredentials(insecure.NewCredentials()), grpc.WithStreamInterceptor(grpc_retry.StreamClientInterceptor()), diff --git a/robot/client/client.go b/robot/client/client.go index 4f02725b15b..83eb18f2243 100644 --- a/robot/client/client.go +++ b/robot/client/client.go @@ -973,6 +973,33 @@ func (rc *RobotClient) FrameSystemConfig(ctx context.Context) (*framesystem.Conf return &framesystem.Config{Parts: result}, nil } +// GetPose returns the pose of the specified component in the given destination frame. +func (rc *RobotClient) GetPose( + ctx context.Context, + componentName, destinationFrame string, + supplementalTransforms []*referenceframe.LinkInFrame, + extra map[string]interface{}, +) (*referenceframe.PoseInFrame, error) { + ext, err := protoutils.StructToStructPb(extra) + if err != nil { + return nil, err + } + transforms, err := referenceframe.LinkInFramesToTransformsProtobuf(supplementalTransforms) + if err != nil { + return nil, err + } + resp, err := rc.client.GetPose(ctx, &pb.GetPoseRequest{ + ComponentName: componentName, + DestinationFrame: destinationFrame, + SupplementalTransforms: transforms, + Extra: ext, + }) + if err != nil { + return nil, err + } + return referenceframe.ProtobufToPoseInFrame(resp.Pose), nil +} + // TransformPose will transform the pose of the requested poseInFrame to the desired frame in the robot's frame system. // // import ( @@ -986,9 +1013,9 @@ func (rc *RobotClient) TransformPose( ctx context.Context, query *referenceframe.PoseInFrame, destination string, - additionalTransforms []*referenceframe.LinkInFrame, + supplementalTransforms []*referenceframe.LinkInFrame, ) (*referenceframe.PoseInFrame, error) { - transforms, err := referenceframe.LinkInFramesToTransformsProtobuf(additionalTransforms) + transforms, err := referenceframe.LinkInFramesToTransformsProtobuf(supplementalTransforms) if err != nil { return nil, err } @@ -1036,6 +1063,27 @@ func (rc *RobotClient) TransformPointCloud(ctx context.Context, srcpc pointcloud return output, nil } +// CurrentInputs returns a map of the current inputs for each component of a machine's frame system +// and a map of statuses indicating which of the machine's components may be actuated through input values. +func (rc *RobotClient) CurrentInputs(ctx context.Context) (referenceframe.FrameSystemInputs, error) { + input := make(referenceframe.FrameSystemInputs) + for _, name := range rc.ResourceNames() { + res, err := rc.ResourceByName(name) + if err != nil { + return nil, err + } + inputEnabled, ok := res.(framesystem.InputEnabled) + if ok { + pos, err := inputEnabled.CurrentInputs(ctx) + if err != nil { + return nil, err + } + input[name.ShortName()] = pos + } + } + return input, nil +} + // StopAll cancels all current and outstanding operations for the machine and stops all actuators and movement. // // err := machine.StopAll(ctx.Background()) diff --git a/robot/client/client_test.go b/robot/client/client_test.go index 97f79626e1d..89906426d5d 100644 --- a/robot/client/client_test.go +++ b/robot/client/client_test.go @@ -2008,6 +2008,67 @@ func TestShutDown(t *testing.T) { test.That(t, shutdownCalled, test.ShouldBeTrue) } +func TestCurrentInputs(t *testing.T) { + logger := logging.NewTestLogger(t) + listener, err := net.Listen("tcp", "localhost:0") + test.That(t, err, test.ShouldBeNil) + gServer := grpc.NewServer() + + testAPI := resource.APINamespaceRDK.WithComponentType(arm.SubtypeName) + testName := resource.NewName(testAPI, "arm1") + testName2 := resource.NewName(testAPI, "arm2") + + expectedInputs := referenceframe.FrameSystemInputs{ + testName.ShortName(): []referenceframe.Input{{0}, {math.Pi}, {-math.Pi}, {0}, {math.Pi}, {-math.Pi}}, + testName2.ShortName(): []referenceframe.Input{{math.Pi}, {-math.Pi}, {0}, {math.Pi}, {-math.Pi}, {0}}, + } + injectArm := &inject.Arm{ + JointPositionsFunc: func(ctx context.Context, extra map[string]any) ([]referenceframe.Input, error) { + return expectedInputs[testName.ShortName()], nil + }, + KinematicsFunc: func(ctx context.Context) (referenceframe.Model, error) { + return referenceframe.ParseModelJSONFile(rutils.ResolveFile("components/arm/example_kinematics/ur5e.json"), "") + }, + } + injectArm2 := &inject.Arm{ + JointPositionsFunc: func(ctx context.Context, extra map[string]any) ([]referenceframe.Input, error) { + return expectedInputs[testName2.ShortName()], nil + }, + KinematicsFunc: func(ctx context.Context) (referenceframe.Model, error) { + return referenceframe.ParseModelJSONFile(rutils.ResolveFile("components/arm/example_kinematics/xarm6_kinematics_test.json"), "") + }, + } + resourceNames := []resource.Name{testName, testName2} + resources := map[resource.Name]arm.Arm{testName: injectArm, testName2: injectArm2} + injectRobot := &inject.Robot{ + ResourceNamesFunc: func() []resource.Name { return resourceNames }, + ResourceByNameFunc: func(n resource.Name) (resource.Resource, error) { return resources[n], nil }, + MachineStatusFunc: func(ctx context.Context) (robot.MachineStatus, error) { + return robot.MachineStatus{State: robot.StateRunning}, nil + }, + ResourceRPCAPIsFunc: func() []resource.RPCAPI { return nil }, + } + + armSvc, err := resource.NewAPIResourceCollection(arm.API, resources) + test.That(t, err, test.ShouldBeNil) + gServer.RegisterService(&armpb.ArmService_ServiceDesc, arm.NewRPCServiceServer(armSvc)) + pb.RegisterRobotServiceServer(gServer, server.New(injectRobot)) + + go gServer.Serve(listener) + defer gServer.Stop() + + client, err := New(context.Background(), listener.Addr().String(), logger) + test.That(t, err, test.ShouldBeNil) + defer func() { + test.That(t, client.Close(context.Background()), test.ShouldBeNil) + }() + + inputs, err := client.CurrentInputs(context.Background()) + test.That(t, err, test.ShouldBeNil) + test.That(t, len(inputs), test.ShouldEqual, 2) + test.That(t, inputs, test.ShouldResemble, expectedInputs) +} + func TestUnregisteredResourceByName(t *testing.T) { logger := logging.NewTestLogger(t) listener, err := net.Listen("tcp", "localhost:0") diff --git a/robot/framesystem/framesystem.go b/robot/framesystem/framesystem.go index 1ff01cbccee..ddfa2f74d75 100644 --- a/robot/framesystem/framesystem.go +++ b/robot/framesystem/framesystem.go @@ -39,7 +39,18 @@ type InputEnabled interface { GoToInputs(context.Context, ...[]referenceframe.Input) error } -// A Service that returns the frame system for a robot. +// Service is an interface that wraps a RobotFrameSystem in a Resource. +type Service interface { + resource.Resource + RobotFrameSystem +} + +// RobotFrameSystem defines the API to interact with the FrameSystemService +// +// GetPose example: +// +// // Assume that a gripper is correctly configured with a frame on your machine +// myGripperPose, err := machine.GetPose(context.Background(), gripperName, referenceframe.World, nil, nil) // // TransformPose example: // @@ -65,8 +76,19 @@ type InputEnabled interface { // myCurrentInputs, err := fsService.CurrentInputs(context.Background()) // // frameSystem, err := fsService.FrameSystem(context.Background(), nil) -type Service interface { - resource.Resource +type RobotFrameSystem interface { + // FrameSystemConfig returns the individual parts that make up a robot's frame system + FrameSystemConfig(ctx context.Context) (*Config, error) + + // GetPose returns the pose a component within a frame system. + // It returns a `PoseInFrame` describing the pose of the specified component relative to the specified destination frame. + // The `supplemental_transforms` argument can be used to augment the machine's existing frame system with additional frames. + GetPose( + ctx context.Context, + componentName, destinationFrame string, + supplementalTransforms []*referenceframe.LinkInFrame, + extra map[string]interface{}, + ) (*referenceframe.PoseInFrame, error) // TransformPose returns a transformed pose in the destination reference frame. // This method converts a given source pose from one reference frame to a specified destination frame. @@ -74,7 +96,7 @@ type Service interface { ctx context.Context, pose *referenceframe.PoseInFrame, dst string, - additionalTransforms []*referenceframe.LinkInFrame, + supplementalTransforms []*referenceframe.LinkInFrame, ) (*referenceframe.PoseInFrame, error) // TransformPointCloud returns a new point cloud with points adjusted from one reference frame to a specified destination frame. @@ -82,10 +104,7 @@ type Service interface { // CurrentInputs returns a map of the current inputs for each component of a machine's frame system // and a map of statuses indicating which of the machine's components may be actuated through input values. - CurrentInputs(ctx context.Context) (referenceframe.FrameSystemInputs, map[string]InputEnabled, error) - - // FrameSystem returns the frame system of the machine and incorporates any specified additional transformations. - FrameSystem(ctx context.Context, additionalTransforms []*referenceframe.LinkInFrame) (referenceframe.FrameSystem, error) + CurrentInputs(ctx context.Context) (referenceframe.FrameSystemInputs, error) } // FromDependencies is a helper for getting the framesystem from a collection of dependencies. @@ -118,8 +137,7 @@ func (svc *frameSystemService) Name() resource.Name { // Config is a slice of *config.FrameSystemPart. type Config struct { resource.TriviallyValidateConfig - Parts []*referenceframe.FrameSystemPart - AdditionalTransforms []*referenceframe.LinkInFrame + Parts []*referenceframe.FrameSystemPart } // String prints out a table of each frame in the system, with columns of name, parent, translation and orientation. @@ -178,7 +196,6 @@ func (svc *frameSystemService) Reconfigure(ctx context.Context, deps resource.De components := make(map[string]resource.Resource) for name, r := range deps { short := name.ShortName() - // is this only for InputEnabled components or everything? if _, present := components[short]; present { return DuplicateResourceShortNameError(short) } @@ -200,6 +217,31 @@ func (svc *frameSystemService) Reconfigure(ctx context.Context, deps resource.De return nil } +func (svc *frameSystemService) FrameSystemConfig(ctx context.Context) (*Config, error) { + return &Config{Parts: svc.parts}, nil +} + +// GetPose returns the pose of the specified component in the given destination frame. +func (svc *frameSystemService) GetPose( + ctx context.Context, + componentName, destinationFrame string, + supplementalTransforms []*referenceframe.LinkInFrame, + extra map[string]interface{}, +) (*referenceframe.PoseInFrame, error) { + if destinationFrame == "" { + destinationFrame = referenceframe.World + } + if componentName == "" { + return nil, errors.New("must provide component name") + } + return svc.TransformPose( + ctx, + referenceframe.NewPoseInFrame(componentName, spatialmath.NewZeroPose()), + destinationFrame, + supplementalTransforms, + ) +} + // TransformPose will transform the pose of the requested poseInFrame to the desired frame in the robot's frame system. func (svc *frameSystemService) TransformPose( ctx context.Context, @@ -210,61 +252,63 @@ func (svc *frameSystemService) TransformPose( ctx, span := trace.StartSpan(ctx, "services::framesystem::TransformPose") defer span.End() - fs, err := svc.FrameSystem(ctx, additionalTransforms) + fs, err := referenceframe.NewFrameSystem(LocalFrameSystemName, svc.parts, additionalTransforms) if err != nil { return nil, err } - input := referenceframe.NewZeroInputs(fs) svc.partsMu.RLock() defer svc.partsMu.RUnlock() - // build maps of relevant components and inputs from initial inputs - for name, inputs := range input { - // skip frame if it does not have input - if len(inputs) == 0 { - continue - } - - // add component to map - component, ok := svc.components[name] - if !ok { - return nil, DependencyNotFoundError(name) - } - inputEnabled, ok := component.(InputEnabled) - if !ok { - return nil, NotInputEnabledError(component) - } - - // add input to map - pos, err := inputEnabled.CurrentInputs(ctx) - if err != nil { - return nil, err - } - input[name] = pos + input, err := svc.CurrentInputs(ctx) + if err != nil { + return nil, err } tf, err := fs.Transform(input, pose, dst) if err != nil { return nil, err } - pose, _ = tf.(*referenceframe.PoseInFrame) - return pose, nil + return tf.(*referenceframe.PoseInFrame), nil +} + +// TransformPointCloud applies the same pose offset to each point in a single pointcloud and returns the transformed point cloud. +// if destination string is empty, defaults to transforming to the world frame. +// Do not move the robot between the generation of the initial pointcloud and the receipt +// of the transformed pointcloud because that will make the transformations inaccurate. +func (svc *frameSystemService) TransformPointCloud(ctx context.Context, srcpc pointcloud.PointCloud, srcName, dstName string, +) (pointcloud.PointCloud, error) { + if dstName == "" { + dstName = referenceframe.World + } + if srcName == "" { + return nil, errors.New("srcName cannot be empty, must provide name of point cloud origin") + } + // get transform pose needed to get to destination frame + sourceFrameZero := referenceframe.NewPoseInFrame(srcName, spatialmath.NewZeroPose()) + theTransform, err := svc.TransformPose(ctx, sourceFrameZero, dstName, nil) + if err != nil { + return nil, err + } + // returned the transformed pointcloud where the transform was applied to each point + pc := srcpc.CreateNewRecentered(theTransform.Pose()) + err = pointcloud.ApplyOffset(srcpc, theTransform.Pose(), pc) + if err != nil { + return nil, err + } + return pc, nil } // CurrentInputs will get present inputs for a framesystem from a robot and return a map of those inputs, as well as a map of the // InputEnabled resources that those inputs came from. -func (svc *frameSystemService) CurrentInputs( - ctx context.Context, -) (referenceframe.FrameSystemInputs, map[string]InputEnabled, error) { - fs, err := svc.FrameSystem(ctx, []*referenceframe.LinkInFrame{}) +func (svc *frameSystemService) CurrentInputs(ctx context.Context) (referenceframe.FrameSystemInputs, error) { + fs, err := NewFromService(ctx, svc, nil) if err != nil { - return nil, nil, err + return nil, err } input := referenceframe.NewZeroInputs(fs) // build maps of relevant components and inputs from initial inputs - resources := map[string]InputEnabled{} for name, original := range input { // skip frames with no input if len(original) == 0 { @@ -274,60 +318,36 @@ func (svc *frameSystemService) CurrentInputs( // add component to map component, ok := svc.components[name] if !ok { - return nil, nil, DependencyNotFoundError(name) + return nil, DependencyNotFoundError(name) } inputEnabled, ok := component.(InputEnabled) if !ok { - return nil, nil, NotInputEnabledError(component) + return nil, NotInputEnabledError(component) } - resources[name] = inputEnabled // add input to map pos, err := inputEnabled.CurrentInputs(ctx) if err != nil { - return nil, nil, err + return nil, err } input[name] = pos } - return input, resources, nil + return input, nil } -// FrameSystem returns the frame system of the robot. -func (svc *frameSystemService) FrameSystem( +// NewFromService creates a referenceframe.FrameSystem from the given Service's FrameSystemConfig and returns it. +// Supplemental transforms can be provided to augment the FrameSystemConfig. +func NewFromService( ctx context.Context, - additionalTransforms []*referenceframe.LinkInFrame, + service Service, + supplementalTransforms []*referenceframe.LinkInFrame, ) (referenceframe.FrameSystem, error) { - _, span := trace.StartSpan(ctx, "services::framesystem::FrameSystem") - defer span.End() - return referenceframe.NewFrameSystem(LocalFrameSystemName, svc.parts, additionalTransforms) -} - -// TransformPointCloud applies the same pose offset to each point in a single pointcloud and returns the transformed point cloud. -// if destination string is empty, defaults to transforming to the world frame. -// Do not move the robot between the generation of the initial pointcloud and the receipt -// of the transformed pointcloud because that will make the transformations inaccurate. -func (svc *frameSystemService) TransformPointCloud(ctx context.Context, srcpc pointcloud.PointCloud, srcName, dstName string, -) (pointcloud.PointCloud, error) { - if dstName == "" { - dstName = referenceframe.World - } - if srcName == "" { - return nil, errors.New("srcName cannot be empty, must provide name of point cloud origin") - } - // get transform pose needed to get to destination frame - sourceFrameZero := referenceframe.NewPoseInFrame(srcName, spatialmath.NewZeroPose()) - theTransform, err := svc.TransformPose(ctx, sourceFrameZero, dstName, nil) - if err != nil { - return nil, err - } - // returned the transformed pointcloud where the transform was applied to each point - pc := srcpc.CreateNewRecentered(theTransform.Pose()) - err = pointcloud.ApplyOffset(srcpc, theTransform.Pose(), pc) + fsCfg, err := service.FrameSystemConfig(ctx) if err != nil { return nil, err } - return pc, nil + return referenceframe.NewFrameSystem(service.Name().ShortName(), fsCfg.Parts, supplementalTransforms) } // PrefixRemoteParts applies prefixes to a list of FrameSystemParts appropriate to the remote they originate from. diff --git a/robot/framesystem/framesystem_test.go b/robot/framesystem/framesystem_test.go index a95de996085..8058e4f1b7d 100644 --- a/robot/framesystem/framesystem_test.go +++ b/robot/framesystem/framesystem_test.go @@ -27,7 +27,7 @@ func TestEmptyConfigFrameService(t *testing.T) { fsCfg, err := r.FrameSystemConfig(ctx) test.That(t, err, test.ShouldBeNil) test.That(t, fsCfg.Parts, test.ShouldHaveLength, 0) - fs, err := referenceframe.NewFrameSystem("test", fsCfg.Parts, fsCfg.AdditionalTransforms) + fs, err := referenceframe.NewFrameSystem("test", fsCfg.Parts, nil) test.That(t, err, test.ShouldBeNil) test.That(t, fs.FrameNames(), test.ShouldHaveLength, 0) } @@ -108,7 +108,7 @@ func TestNewFrameSystemFromConfigWithTransforms(t *testing.T) { &spatialmath.R4AA{Theta: math.Pi / 2, RX: 0., RY: 1., RZ: 0.}, ) - fsCfg.AdditionalTransforms = []*referenceframe.LinkInFrame{ + additionalTransforms := []*referenceframe.LinkInFrame{ referenceframe.NewLinkInFrame("pieceArm", testPose, "frame1", nil), referenceframe.NewLinkInFrame("pieceGripper", testPose, "frame2", nil), referenceframe.NewLinkInFrame("frame2", testPose, "frame2a", nil), @@ -116,7 +116,7 @@ func TestNewFrameSystemFromConfigWithTransforms(t *testing.T) { referenceframe.NewLinkInFrame(referenceframe.World, testPose, "frame3", nil), } - fs, err := referenceframe.NewFrameSystem("test", fsCfg.Parts, fsCfg.AdditionalTransforms) + fs, err := referenceframe.NewFrameSystem("test", fsCfg.Parts, additionalTransforms) test.That(t, err, test.ShouldBeNil) // 4 frames defined + 5 from transforms, 18 frames when including the offset, test.That(t, len(fs.FrameNames()), test.ShouldEqual, 18) @@ -235,7 +235,7 @@ func TestNewFrameSystemFromBadConfig(t *testing.T) { test.That(t, err, test.ShouldBeError, tc.err) return } - _, err = referenceframe.NewFrameSystem(tc.num, fsCfg.Parts, fsCfg.AdditionalTransforms) + _, err = referenceframe.NewFrameSystem(tc.num, fsCfg.Parts, nil) test.That(t, err, test.ShouldBeError, tc.err) }) } @@ -255,8 +255,7 @@ func TestNewFrameSystemFromBadConfig(t *testing.T) { } fsCfg, err := r.FrameSystemConfig(ctx) test.That(t, err, test.ShouldBeNil) - fsCfg.AdditionalTransforms = transforms - fs, err := referenceframe.NewFrameSystem("", fsCfg.Parts, fsCfg.AdditionalTransforms) + fs, err := referenceframe.NewFrameSystem("", fsCfg.Parts, transforms) test.That(t, err, test.ShouldBeError, referenceframe.NewParentFrameMissingError("frame2", "noParent")) test.That(t, fs, test.ShouldBeNil) }) @@ -267,8 +266,7 @@ func TestNewFrameSystemFromBadConfig(t *testing.T) { } fsCfg, err := r.FrameSystemConfig(ctx) test.That(t, err, test.ShouldBeNil) - fsCfg.AdditionalTransforms = transforms - fs, err := referenceframe.NewFrameSystem("", fsCfg.Parts, fsCfg.AdditionalTransforms) + fs, err := referenceframe.NewFrameSystem("", fsCfg.Parts, transforms) test.That(t, err, test.ShouldBeError, referenceframe.ErrEmptyStringFrameName) test.That(t, fs, test.ShouldBeNil) }) diff --git a/robot/impl/local_robot.go b/robot/impl/local_robot.go index 6abeda8ff39..490f6483832 100644 --- a/robot/impl/local_robot.go +++ b/robot/impl/local_robot.go @@ -1139,14 +1139,24 @@ func (r *localRobot) extractModelFrameJSON(ctx context.Context, name resource.Na return nil, referenceframe.ErrNoModelInformation } +// GetPose returns the pose of the specified component in the given destination frame. +func (r *localRobot) GetPose( + ctx context.Context, + componentName, destinationFrame string, + supplementalTransforms []*referenceframe.LinkInFrame, + extra map[string]interface{}, +) (*referenceframe.PoseInFrame, error) { + return r.frameSvc.GetPose(ctx, componentName, destinationFrame, supplementalTransforms, extra) +} + // TransformPose will transform the pose of the requested poseInFrame to the desired frame in the robot's frame system. func (r *localRobot) TransformPose( ctx context.Context, pose *referenceframe.PoseInFrame, dst string, - additionalTransforms []*referenceframe.LinkInFrame, + supplementalTransforms []*referenceframe.LinkInFrame, ) (*referenceframe.PoseInFrame, error) { - return r.frameSvc.TransformPose(ctx, pose, dst, additionalTransforms) + return r.frameSvc.TransformPose(ctx, pose, dst, supplementalTransforms) } // TransformPointCloud will transform the pointcloud to the desired frame in the robot's frame system. @@ -1160,6 +1170,12 @@ func (r *localRobot) TransformPointCloud( return r.frameSvc.TransformPointCloud(ctx, srcpc, srcName, dstName) } +// CurrentInputs returns a map of the current inputs for each component of a machine's frame system +// and a map of statuses indicating which of the machine's components may be actuated through input values. +func (r *localRobot) CurrentInputs(ctx context.Context) (referenceframe.FrameSystemInputs, error) { + return r.frameSvc.CurrentInputs(ctx) +} + // RobotFromConfigPath is a helper to read and process a config given its path and then create a robot based on it. func RobotFromConfigPath( ctx context.Context, diff --git a/robot/impl/resource_manager_test.go b/robot/impl/resource_manager_test.go index 826a27b28fa..a876ca0c476 100644 --- a/robot/impl/resource_manager_test.go +++ b/robot/impl/resource_manager_test.go @@ -1868,6 +1868,15 @@ func (rr *dummyRobot) FrameSystemConfig(ctx context.Context) (*framesystem.Confi panic("change to return nil") } +func (rr *dummyRobot) GetPose( + ctx context.Context, + componentName, destinationFrame string, + supplementalTransforms []*referenceframe.LinkInFrame, + extra map[string]interface{}, +) (*referenceframe.PoseInFrame, error) { + panic("change to return nil") +} + func (rr *dummyRobot) TransformPose( ctx context.Context, pose *referenceframe.PoseInFrame, @@ -1882,6 +1891,10 @@ func (rr *dummyRobot) TransformPointCloud(ctx context.Context, srcpc pointcloud. panic("change to return nil") } +func (rr *dummyRobot) CurrentInputs(ctx context.Context) (referenceframe.FrameSystemInputs, error) { + panic("change to return nil") +} + func (rr *dummyRobot) ProcessManager() pexec.ProcessManager { panic("change to return nil") } diff --git a/robot/impl/robot_framesystem_test.go b/robot/impl/robot_framesystem_test.go index 7b4fe90e0ca..e94b2170022 100644 --- a/robot/impl/robot_framesystem_test.go +++ b/robot/impl/robot_framesystem_test.go @@ -108,8 +108,7 @@ func TestFrameSystemConfigWithRemote(t *testing.T) { test.That(t, err, test.ShouldBeNil) fsCfg, err := r2.FrameSystemConfig(context.Background()) test.That(t, err, test.ShouldBeNil) - fsCfg.AdditionalTransforms = transforms - fs, err := referenceframe.NewFrameSystem("test", fsCfg.Parts, fsCfg.AdditionalTransforms) + fs, err := referenceframe.NewFrameSystem("", fsCfg.Parts, transforms) test.That(t, err, test.ShouldBeNil) test.That(t, fs.FrameNames(), test.ShouldHaveLength, 34) diff --git a/robot/robot.go b/robot/robot.go index 867dd13c464..a302c489e03 100644 --- a/robot/robot.go +++ b/robot/robot.go @@ -16,8 +16,6 @@ import ( "go.viam.com/rdk/grpc" "go.viam.com/rdk/logging" "go.viam.com/rdk/operation" - "go.viam.com/rdk/pointcloud" - "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" "go.viam.com/rdk/robot/framesystem" "go.viam.com/rdk/robot/packages" @@ -80,6 +78,8 @@ const ( // // Shut down the robot. // err := machine.Shutdown(context.Background()) type Robot interface { + framesystem.RobotFrameSystem + // GetModelsFromModules returns a list of models supported by the configured modules, // and specifies whether the models are from a local or registry module. GetModelsFromModules(ctx context.Context) ([]resource.ModuleModel, error) @@ -111,22 +111,6 @@ type Robot interface { // Logger returns the logger the robot is using. Logger() logging.Logger - // FrameSystemConfig returns the individual parts that make up a robot's frame system - FrameSystemConfig(ctx context.Context) (*framesystem.Config, error) - - // TransformPose will transform the pose of the requested poseInFrame to the desired frame in the robot's frame system. - TransformPose( - ctx context.Context, - pose *referenceframe.PoseInFrame, - dst string, - additionalTransforms []*referenceframe.LinkInFrame, - ) (*referenceframe.PoseInFrame, error) - - // TransformPointCloud will transform the pointcloud to the desired frame in the robot's frame system. - // Do not move the robot between the generation of the initial pointcloud and the receipt - // of the transformed pointcloud because that will make the transformations inaccurate. - TransformPointCloud(ctx context.Context, srcpc pointcloud.PointCloud, srcName, dstName string) (pointcloud.PointCloud, error) - // CloudMetadata returns app-related information about the robot. CloudMetadata(ctx context.Context) (cloud.Metadata, error) diff --git a/robot/server/server.go b/robot/server/server.go index aac7f3a74da..d9e8b7ccfaa 100644 --- a/robot/server/server.go +++ b/robot/server/server.go @@ -296,6 +296,19 @@ func (s *Server) FrameSystemConfig(ctx context.Context, req *pb.FrameSystemConfi return &pb.FrameSystemConfigResponse{FrameSystemConfigs: configs}, nil } +// GetPose returns the pose of a specified component in the desired frame in the robot's frame system. +func (s *Server) GetPose(ctx context.Context, req *pb.GetPoseRequest) (*pb.GetPoseResponse, error) { + transforms, err := referenceframe.LinkInFramesFromTransformsProtobuf(req.GetSupplementalTransforms()) + if err != nil { + return nil, err + } + pose, err := s.robot.GetPose(ctx, req.ComponentName, req.DestinationFrame, transforms, req.Extra.AsMap()) + if err != nil { + return nil, err + } + return &pb.GetPoseResponse{Pose: referenceframe.PoseInFrameToProtobuf(pose)}, nil +} + // TransformPose will transform the pose of the requested poseInFrame to the desired frame in the robot's frame system. func (s *Server) TransformPose(ctx context.Context, req *pb.TransformPoseRequest) (*pb.TransformPoseResponse, error) { transforms, err := referenceframe.LinkInFramesFromTransformsProtobuf(req.GetSupplementalTransforms()) diff --git a/services/motion/builtin/builtin.go b/services/motion/builtin/builtin.go index f47397ed2f8..d5b386463d8 100644 --- a/services/motion/builtin/builtin.go +++ b/services/motion/builtin/builtin.go @@ -13,7 +13,6 @@ import ( "time" "github.com/go-viper/mapstructure/v2" - "github.com/golang/geo/r3" "github.com/google/uuid" "github.com/pkg/errors" "go.uber.org/zap" @@ -35,7 +34,6 @@ import ( "go.viam.com/rdk/services/motion/builtin/state" "go.viam.com/rdk/services/slam" "go.viam.com/rdk/services/vision" - "go.viam.com/rdk/spatialmath" "go.viam.com/rdk/utils" ) @@ -138,7 +136,7 @@ type builtIn struct { movementSensors map[resource.Name]movementsensor.MovementSensor slamServices map[resource.Name]slam.Service visionServices map[resource.Name]vision.Service - components map[resource.Name]resource.Resource + components map[string]resource.Resource logger logging.Logger state *state.State configuredDefaultExtras map[string]any @@ -186,7 +184,7 @@ func (ms *builtIn) Reconfigure( movementSensors := make(map[resource.Name]movementsensor.MovementSensor) slamServices := make(map[resource.Name]slam.Service) visionServices := make(map[resource.Name]vision.Service) - components := make(map[resource.Name]resource.Resource) + componentMap := make(map[string]resource.Resource) for name, dep := range deps { switch dep := dep.(type) { case framesystem.Service: @@ -198,13 +196,13 @@ func (ms *builtIn) Reconfigure( case vision.Service: visionServices[name] = dep default: - components[name] = dep + componentMap[name.ShortName()] = dep } } ms.movementSensors = movementSensors ms.slamServices = slamServices ms.visionServices = visionServices - ms.components = components + ms.components = componentMap if ms.state != nil { ms.state.Stop() } @@ -329,6 +327,7 @@ func (ms *builtIn) MoveOnGlobe(ctx context.Context, req motion.MoveOnGlobeReq) ( return id, nil } +// GetPose is deprecated. func (ms *builtIn) GetPose( ctx context.Context, componentName resource.Name, @@ -336,20 +335,10 @@ func (ms *builtIn) GetPose( supplementalTransforms []*referenceframe.LinkInFrame, extra map[string]interface{}, ) (*referenceframe.PoseInFrame, error) { + ms.logger.Warn("GetPose is deprecated. Please switch to using the GetPose method defined on the FrameSystem service") ms.mu.RLock() defer ms.mu.RUnlock() - if destinationFrame == "" { - destinationFrame = referenceframe.World - } - return ms.fsService.TransformPose( - ctx, - referenceframe.NewPoseInFrame( - componentName.ShortName(), - spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 0, Z: 0}), - ), - destinationFrame, - supplementalTransforms, - ) + return ms.fsService.GetPose(ctx, componentName.ShortName(), destinationFrame, supplementalTransforms, extra) } func (ms *builtIn) StopPlan( @@ -470,13 +459,13 @@ func (ms *builtIn) DoCommand(ctx context.Context, cmd map[string]interface{}) (m } func (ms *builtIn) plan(ctx context.Context, req motion.MoveReq, logger logging.Logger) (motionplan.Plan, error) { - frameSys, err := ms.fsService.FrameSystem(ctx, req.WorldState.Transforms()) + frameSys, err := framesystem.NewFromService(ctx, ms.fsService, req.WorldState.Transforms()) if err != nil { return nil, err } // build maps of relevant components and inputs from initial inputs - fsInputs, _, err := ms.fsService.CurrentInputs(ctx) + fsInputs, err := ms.fsService.CurrentInputs(ctx) if err != nil { return nil, err } @@ -580,12 +569,6 @@ func (ms *builtIn) plan(ctx context.Context, req motion.MoveReq, logger logging. } func (ms *builtIn) execute(ctx context.Context, trajectory motionplan.Trajectory) error { - // build maps of relevant components from initial inputs - _, resources, err := ms.fsService.CurrentInputs(ctx) - if err != nil { - return err - } - // Batch GoToInputs calls if possible; components may want to blend between inputs combinedSteps := []map[string][][]referenceframe.Input{} currStep := map[string][][]referenceframe.Input{} @@ -648,11 +631,15 @@ func (ms *builtIn) execute(ctx context.Context, trajectory motionplan.Trajectory if len(inputs) == 0 { continue } - r, ok := resources[name] + r, ok := ms.components[name] if !ok { - return fmt.Errorf("plan had step for resource %s but no resource with that name found in framesystem", name) + return fmt.Errorf("plan had step for resource %s but it was not found in the motion", name) + } + ie, err := utils.AssertType[framesystem.InputEnabled](r) + if err != nil { + return err } - if err := r.GoToInputs(ctx, inputs...); err != nil { + if err := ie.GoToInputs(ctx, inputs...); err != nil { // If there is an error on GoToInputs, stop the component if possible before returning the error if actuator, ok := r.(inputEnabledActuator); ok { if stopErr := actuator.Stop(ctx, nil); stopErr != nil { diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index b8d984c663b..9e7182f4ecb 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -30,6 +30,7 @@ import ( "go.viam.com/rdk/pointcloud" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" + "go.viam.com/rdk/robot/framesystem" robotimpl "go.viam.com/rdk/robot/impl" "go.viam.com/rdk/services/motion" "go.viam.com/rdk/services/motion/builtin/state" @@ -901,7 +902,7 @@ func TestGetTransientDetectionsMath(t *testing.T) { test.That(t, ok, test.ShouldBeTrue) getTransientDetectionMock := func(currentPose, obstaclePose spatialmath.Pose) []spatialmath.Geometry { - inputMap, _, err := mr.fsService.CurrentInputs(ctx) + inputMap, err := ms.(*builtIn).fsService.CurrentInputs(ctx) test.That(t, err, test.ShouldBeNil) k, err := mr.kinematicBase.Kinematics(ctx) test.That(t, err, test.ShouldBeNil) @@ -914,7 +915,7 @@ func TestGetTransientDetectionsMath(t *testing.T) { ) test.That(t, err, test.ShouldBeNil) - cam, ok := ms.(*builtIn).components[resource.NewName(camera.API, "injectedCamera")] + cam, ok := ms.(*builtIn).components[resource.NewName(camera.API, "injectedCamera").ShortName()] test.That(t, ok, test.ShouldBeTrue) tf, err := mr.localizingFS.Transform( @@ -1054,7 +1055,7 @@ func TestCheckPlan(t *testing.T) { movementSensor, ok := localizer.(movementsensor.MovementSensor) test.That(t, ok, test.ShouldBeTrue) - fakeBase, ok := ms.(*builtIn).components[baseResource] + fakeBase, ok := ms.(*builtIn).components[baseResource.ShortName()] test.That(t, ok, test.ShouldBeTrue) req := motion.MoveOnGlobeReq{ @@ -1399,12 +1400,12 @@ func TestMultiWaypointPlanning(t *testing.T) { test.That(t, len(plan), test.ShouldBeGreaterThan, 0) // Verify start configuration matches current robot state - fsInputs, _, err := ms.(*builtIn).fsService.CurrentInputs(ctx) + fsInputs, err := ms.(*builtIn).fsService.CurrentInputs(ctx) test.That(t, err, test.ShouldBeNil) test.That(t, plan[0], test.ShouldResemble, fsInputs) // Verify final pose - frameSys, err := ms.(*builtIn).fsService.FrameSystem(ctx, nil) + frameSys, err := framesystem.NewFromService(ctx, ms.(*builtIn).fsService, nil) test.That(t, err, test.ShouldBeNil) finalConfig := plan[len(plan)-1] @@ -1438,7 +1439,7 @@ func TestMultiWaypointPlanning(t *testing.T) { plan := getPlanFromMove(t, moveReq) test.That(t, len(plan), test.ShouldBeGreaterThan, 0) - frameSys, err := ms.(*builtIn).fsService.FrameSystem(ctx, nil) + frameSys, err := framesystem.NewFromService(ctx, ms.(*builtIn).fsService, nil) test.That(t, err, test.ShouldBeNil) // Verify start configuration matches start pose @@ -1509,7 +1510,7 @@ func TestMultiWaypointPlanning(t *testing.T) { test.That(t, foundMatchingConfig, test.ShouldBeTrue) // Verify final pose - frameSys, err := ms.(*builtIn).fsService.FrameSystem(ctx, nil) + frameSys, err := framesystem.NewFromService(ctx, ms.(*builtIn).fsService, nil) test.That(t, err, test.ShouldBeNil) finalConfig := plan[len(plan)-1] @@ -1546,7 +1547,7 @@ func TestMultiWaypointPlanning(t *testing.T) { test.That(t, startArmConfig, test.ShouldResemble, referenceframe.FloatsToInputs(startConfig)) // Verify final pose - frameSys, err := ms.(*builtIn).fsService.FrameSystem(ctx, nil) + frameSys, err := framesystem.NewFromService(ctx, ms.(*builtIn).fsService, nil) test.That(t, err, test.ShouldBeNil) finalConfig := plan[len(plan)-1] diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index a9232afeec1..f62fdf4226e 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -233,13 +233,15 @@ func (mr *moveRequest) getTransientDetections( if err != nil { return nil, err } + // the inputMap informs where we are in the world // the inputMap will be used downstream to transform the observed geometry from the camera frame // into the world frame - inputMap, _, err := mr.fsService.CurrentInputs(ctx) + inputMap, err := mr.fsService.CurrentInputs(ctx) if err != nil { return nil, err } + k, err := mr.kinematicBase.Kinematics(ctx) if err != nil { return nil, err @@ -642,7 +644,7 @@ func (ms *builtIn) newMoveOnGlobeRequest( localizer := motion.TwoDLocalizer(motion.NewMovementSensorLocalizer(movementSensor, origin, movementSensorToBase.Pose())) // create a KinematicBase from the componentName - baseComponent, ok := ms.components[req.ComponentName] + baseComponent, ok := ms.components[req.ComponentName.ShortName()] if !ok { return nil, resource.NewNotFoundError(req.ComponentName) } @@ -651,7 +653,7 @@ func (ms *builtIn) newMoveOnGlobeRequest( return nil, fmt.Errorf("cannot move component of type %T because it is not a Base", baseComponent) } - fs, err := ms.fsService.FrameSystem(ctx, nil) + fs, err := framesystem.NewFromService(ctx, ms.fsService, nil) if err != nil { return nil, err } @@ -758,7 +760,7 @@ func (ms *builtIn) newMoveOnMapRequest( limits = append(limits, referenceframe.Limit{Min: -2 * math.Pi, Max: 2 * math.Pi}) // create a KinematicBase from the componentName - component, ok := ms.components[req.ComponentName] + component, ok := ms.components[req.ComponentName.ShortName()] if !ok { return nil, resource.DependencyNotFoundError(req.ComponentName) } @@ -770,7 +772,7 @@ func (ms *builtIn) newMoveOnMapRequest( // build kinematic options kinematicsOptions := kbOptionsFromCfg(motionCfg, valExtra) - fs, err := ms.fsService.FrameSystem(ctx, nil) + fs, err := framesystem.NewFromService(ctx, ms.fsService, nil) if err != nil { return nil, err } @@ -919,7 +921,7 @@ func (ms *builtIn) createBaseMoveRequest( } } - currentInputs, _, err := ms.fsService.CurrentInputs(ctx) + currentInputs, err := ms.fsService.CurrentInputs(ctx) if err != nil { return nil, err } diff --git a/services/motion/client.go b/services/motion/client.go index 8a1bbcb02ca..0bdc2f0214c 100644 --- a/services/motion/client.go +++ b/services/motion/client.go @@ -110,6 +110,7 @@ func (c *client) GetPose( if err != nil { return nil, err } + //nolint:staticcheck resp, err := c.client.GetPose(ctx, &pb.GetPoseRequest{ Name: c.name, ComponentName: protoutils.ResourceNameToProto(componentName), diff --git a/services/motion/motion.go b/services/motion/motion.go index b42ac05fdbc..067a07448f1 100644 --- a/services/motion/motion.go +++ b/services/motion/motion.go @@ -306,29 +306,6 @@ type PlanWithStatus struct { // // For more information, see the [MoveOnGlobe method docs]. // -// GetPose example: -// -// // Insert code to connect to your machine. -// // (see CONNECT tab of your machine's page in the Viam app) -// -// // Assumes a gripper configured with name "my_gripper" on the machine -// gripperName := gripper.Named("my_gripper") -// -// // Access the motion service -// motionService, err := motion.FromRobot(machine, "builtin") -// if err != nil { -// logger.Fatal(err) -// } -// -// myGripperPose, err := motionService.GetPose(context.Background(), gripperName, referenceframe.World, nil, nil) -// if err != nil { -// logger.Fatal(err) -// } -// logger.Info("Position of my_gripper from the motion service:", myGripperPose.Pose().Point()) -// logger.Info("Orientation of my_gripper from the motion service:", myGripperPose.Pose().Orientation()) -// -// For more information, see the [GetPose method docs]. -// // StopPlan example: // // motionService, err := motion.FromRobot(machine, "builtin") @@ -370,10 +347,11 @@ type PlanWithStatus struct { // [Move method docs]: https://docs.viam.com/dev/reference/apis/services/motion/#move // [MoveOnMap method docs]: https://docs.viam.com/dev/reference/apis/services/motion/#moveonmap // [MoveOnGlobe method docs]: https://docs.viam.com/dev/reference/apis/services/motion/#moveonglobe -// [GetPose method docs]: https://docs.viam.com/dev/reference/apis/services/motion/#getpose // [StopPlan method docs]: https://docs.viam.com/dev/reference/apis/services/motion/#stopplan // [ListPlanStatuses method docs]: https://docs.viam.com/dev/reference/apis/services/motion/#listplanstatuses // [PlanHistory method docs]: https://docs.viam.com/dev/reference/apis/services/motion/#getplan +// +// [GetPose method docs]: https://docs.viam.com/dev/reference/apis/services/motion/#getpose type Service interface { resource.Resource @@ -398,6 +376,7 @@ type Service interface { // GetPose returns the location and orientation of a component within a frame system. // It returns a `PoseInFrame` describing the pose of the specified component relative to the specified destination frame. // The `supplemental_transforms` argument can be used to augment the machine's existing frame system with additional frames. + // deprecated, use framesystem.Servce.GetPose GetPose( ctx context.Context, componentName resource.Name, diff --git a/services/motion/server.go b/services/motion/server.go index e5f89664b04..dd636b5cbfa 100644 --- a/services/motion/server.go +++ b/services/motion/server.go @@ -73,6 +73,7 @@ func (server *serviceServer) MoveOnGlobe(ctx context.Context, req *pb.MoveOnGlob return &pb.MoveOnGlobeResponse{ExecutionId: id.String()}, nil } +//nolint:staticcheck func (server *serviceServer) GetPose(ctx context.Context, req *pb.GetPoseRequest) (*pb.GetPoseResponse, error) { svc, err := server.coll.Resource(req.Name) if err != nil { @@ -89,6 +90,7 @@ func (server *serviceServer) GetPose(ctx context.Context, req *pb.GetPoseRequest if err != nil { return nil, err } + //nolint:staticcheck return &pb.GetPoseResponse{Pose: referenceframe.PoseInFrameToProtobuf(pose)}, nil } diff --git a/testutils/inject/framesystem_service.go b/testutils/inject/framesystem_service.go index 650ec9ee4c5..2768cfbdfdb 100644 --- a/testutils/inject/framesystem_service.go +++ b/testutils/inject/framesystem_service.go @@ -14,7 +14,13 @@ import ( // If you use an injected frame system, do not also create the system's default frame system as well. type FrameSystemService struct { framesystem.Service - name resource.Name + name resource.Name + GetPoseFunc func( + ctx context.Context, + componentName, destinationFrame string, + supplementalTransforms []*referenceframe.LinkInFrame, + extra map[string]interface{}, + ) (*referenceframe.PoseInFrame, error) TransformPoseFunc func( ctx context.Context, pose *referenceframe.PoseInFrame, @@ -52,6 +58,19 @@ func (fs *FrameSystemService) Name() resource.Name { return fs.name } +// GetPose calls the injected GetPose or the real variant. +func (fs *FrameSystemService) GetPose( + ctx context.Context, + componentName, destinationFrame string, + supplementalTransforms []*referenceframe.LinkInFrame, + extra map[string]interface{}, +) (*referenceframe.PoseInFrame, error) { + if fs.GetPoseFunc == nil { + return fs.Service.GetPose(ctx, componentName, destinationFrame, supplementalTransforms, extra) + } + return fs.GetPoseFunc(ctx, componentName, destinationFrame, supplementalTransforms, extra) +} + // TransformPose calls the injected method or the real variant. func (fs *FrameSystemService) TransformPose( ctx context.Context, @@ -77,27 +96,6 @@ func (fs *FrameSystemService) TransformPointCloud( return fs.TransformPointCloudFunc(ctx, srcpc, srcName, dstName) } -// CurrentInputs calls the injected method or the real variant. -func (fs *FrameSystemService) CurrentInputs( - ctx context.Context, -) (referenceframe.FrameSystemInputs, map[string]framesystem.InputEnabled, error) { - if fs.CurrentInputsFunc == nil { - return fs.Service.CurrentInputs(ctx) - } - return fs.CurrentInputsFunc(ctx) -} - -// FrameSystem calls the injected method of the real variant. -func (fs *FrameSystemService) FrameSystem( - ctx context.Context, - additionalTransforms []*referenceframe.LinkInFrame, -) (referenceframe.FrameSystem, error) { - if fs.FrameSystemFunc == nil { - return fs.Service.FrameSystem(ctx, additionalTransforms) - } - return fs.FrameSystemFunc(ctx, additionalTransforms) -} - // DoCommand calls the injected DoCommand or the real variant. func (fs *FrameSystemService) DoCommand(ctx context.Context, cmd map[string]interface{}, diff --git a/testutils/inject/robot.go b/testutils/inject/robot.go index 64450cbc609..056dc9ef360 100644 --- a/testutils/inject/robot.go +++ b/testutils/inject/robot.go @@ -46,6 +46,7 @@ type Robot struct { additionalTransforms []*referenceframe.LinkInFrame, ) (*referenceframe.PoseInFrame, error) TransformPointCloudFunc func(ctx context.Context, srcpc pointcloud.PointCloud, srcName, dstName string) (pointcloud.PointCloud, error) + CurrentInputsFunc func(ctx context.Context) (referenceframe.FrameSystemInputs, error) ModuleAddressesFunc func() (config.ParentSockAddrs, error) CloudMetadataFunc func(ctx context.Context) (cloud.Metadata, error) MachineStatusFunc func(ctx context.Context) (robot.MachineStatus, error) @@ -256,6 +257,17 @@ func (r *Robot) TransformPointCloud(ctx context.Context, srcpc pointcloud.PointC return r.TransformPointCloudFunc(ctx, srcpc, srcName, dstName) } +// CurrentInputs returns a map of the current inputs for each component of a machine's frame system +// and a map of statuses indicating which of the machine's components may be actuated through input values. +func (r *Robot) CurrentInputs(ctx context.Context) (referenceframe.FrameSystemInputs, error) { + r.Mu.RLock() + defer r.Mu.RUnlock() + if r.CurrentInputsFunc == nil { + return r.LocalRobot.CurrentInputs(ctx) + } + return r.CurrentInputs(ctx) +} + // ModuleAddresses calls the injected ModuleAddresses or the real one. func (r *Robot) ModuleAddresses() (config.ParentSockAddrs, error) { r.Mu.RLock()