diff --git a/cli/motion.go b/cli/motion.go index 6a62e131d76..c28e77af461 100644 --- a/cli/motion.go +++ b/cli/motion.go @@ -7,7 +7,6 @@ import ( "go.viam.com/utils" "go.viam.com/rdk/referenceframe" - "go.viam.com/rdk/robot" "go.viam.com/rdk/services/motion" "go.viam.com/rdk/spatialmath" ) @@ -103,18 +102,12 @@ func motionPrintStatusAction(c *cli.Context, args motionPrintArgs) error { for _, p := range frameSystem.Parts { n := p.FrameConfig.Name() - theComponent, err := robot.ResourceByName(robotClient, n) - if err != nil { - logger.Debugf("no component for %v", n) - continue - } - - pif, err := myMotion.GetPose(ctx, theComponent.Name(), "world", nil, nil) + pif, err := myMotion.GetPose(ctx, n, "world", nil, nil) if err != nil { return err } - printf(c.App.Writer, "%20s : %v", theComponent.Name().ShortName(), prettyString(pif.Pose())) + printf(c.App.Writer, "%20s : %v", n, prettyString(pif.Pose())) } return nil @@ -155,17 +148,12 @@ func motionGetPoseAction(c *cli.Context, args motionGetPoseArgs) error { utils.UncheckedError(robotClient.Close(ctx)) }() - theComponent, err := robot.ResourceByName(robotClient, args.Component) - if err != nil { - return err - } - myMotion, err := motion.FromRobot(robotClient, "builtin") if err != nil || myMotion == nil { return fmt.Errorf("no motion: %w", err) } - pif, err := myMotion.GetPose(ctx, theComponent.Name(), "world", nil, nil) + pif, err := myMotion.GetPose(ctx, args.Component, "world", nil, nil) if err != nil { return err } @@ -212,17 +200,12 @@ func motionSetPoseAction(c *cli.Context, args motionSetPoseArgs) error { utils.UncheckedError(robotClient.Close(ctx)) }() - theComponent, err := robot.ResourceByName(robotClient, args.Component) - if err != nil { - return err - } - myMotion, err := motion.FromRobot(robotClient, "builtin") if err != nil || myMotion == nil { return fmt.Errorf("no motion: %w", err) } - pose, err := myMotion.GetPose(ctx, theComponent.Name(), "world", nil, nil) + pose, err := myMotion.GetPose(ctx, args.Component, "world", nil, nil) if err != nil { return err } @@ -260,7 +243,7 @@ func motionSetPoseAction(c *cli.Context, args motionSetPoseArgs) error { printf(c.App.Writer, "going to pose %v", pose) req := motion.MoveReq{ - ComponentName: theComponent.Name(), + ComponentName: args.Component, Destination: pose, } _, err = myMotion.Move(ctx, req) diff --git a/services/motion/builtin/builtin.go b/services/motion/builtin/builtin.go index 1928a3010ec..9c58b548b0c 100644 --- a/services/motion/builtin/builtin.go +++ b/services/motion/builtin/builtin.go @@ -137,9 +137,9 @@ type builtIn struct { conf *Config mu sync.RWMutex fsService framesystem.Service - movementSensors map[resource.Name]movementsensor.MovementSensor - slamServices map[resource.Name]slam.Service - visionServices map[resource.Name]vision.Service + movementSensors map[string]movementsensor.MovementSensor + slamServices map[string]slam.Service + visionServices map[string]vision.Service components map[string]resource.Resource logger logging.Logger state *state.State @@ -185,22 +185,22 @@ func (ms *builtIn) Reconfigure( ms.configuredDefaultExtras["num_threads"] = config.NumThreads } - movementSensors := make(map[resource.Name]movementsensor.MovementSensor) - slamServices := make(map[resource.Name]slam.Service) - visionServices := make(map[resource.Name]vision.Service) + movementSensors := make(map[string]movementsensor.MovementSensor) + slamServices := make(map[string]slam.Service) + visionServices := make(map[string]vision.Service) componentMap := make(map[string]resource.Resource) for name, dep := range deps { switch dep := dep.(type) { case framesystem.Service: ms.fsService = dep case movementsensor.MovementSensor: - movementSensors[name] = dep + movementSensors[name.Name] = dep case slam.Service: - slamServices[name] = dep + slamServices[name.Name] = dep case vision.Service: - visionServices[name] = dep + visionServices[name.Name] = dep default: - componentMap[name.ShortName()] = dep + componentMap[name.Name] = dep } } ms.movementSensors = movementSensors @@ -326,7 +326,7 @@ func (ms *builtIn) MoveOnGlobe(ctx context.Context, req motion.MoveOnGlobeReq) ( // GetPose is deprecated. func (ms *builtIn) GetPose( ctx context.Context, - componentName resource.Name, + componentName string, destinationFrame string, supplementalTransforms []*referenceframe.LinkInFrame, extra map[string]interface{}, @@ -334,7 +334,7 @@ func (ms *builtIn) GetPose( 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() - return ms.fsService.GetPose(ctx, componentName.ShortName(), destinationFrame, supplementalTransforms, extra) + return ms.fsService.GetPose(ctx, componentName, destinationFrame, supplementalTransforms, extra) } func (ms *builtIn) StopPlan( @@ -480,9 +480,9 @@ func (ms *builtIn) plan(ctx context.Context, req motion.MoveReq, logger logging. } logger.CDebugf(ctx, "frame system inputs: %v", fsInputs) - movingFrame := frameSys.Frame(req.ComponentName.ShortName()) + movingFrame := frameSys.Frame(req.ComponentName) if movingFrame == nil { - return nil, fmt.Errorf("component named %s not found in robot frame system", req.ComponentName.ShortName()) + return nil, fmt.Errorf("component named %s not found in robot frame system", req.ComponentName) } startState, waypoints, err := waypointsFromRequest(req, fsInputs) @@ -720,7 +720,7 @@ func waypointsFromRequest( return nil, nil, errors.New("extras goal_state could not be interpreted as map[string]interface{}") } } else if req.Destination != nil { - goalState := armplanning.NewPlanState(referenceframe.FrameSystemPoses{req.ComponentName.ShortName(): req.Destination}, nil) + goalState := armplanning.NewPlanState(referenceframe.FrameSystemPoses{req.ComponentName: req.Destination}, nil) waypoints = append(waypoints, goalState) } return startState, waypoints, nil diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index 641c44d56ae..2d451021b38 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -21,7 +21,6 @@ import ( armFake "go.viam.com/rdk/components/arm/fake" "go.viam.com/rdk/components/base" "go.viam.com/rdk/components/camera" - "go.viam.com/rdk/components/gripper" "go.viam.com/rdk/components/movementsensor" _ "go.viam.com/rdk/components/register" "go.viam.com/rdk/config" @@ -36,8 +35,6 @@ import ( robotimpl "go.viam.com/rdk/robot/impl" "go.viam.com/rdk/services/motion" "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/testutils/inject" "go.viam.com/rdk/utils" @@ -136,12 +133,12 @@ func TestMoveFailures(t *testing.T) { ctx := context.Background() t.Run("fail on not finding gripper", func(t *testing.T) { grabPose := referenceframe.NewPoseInFrame("fakeGripper", spatialmath.NewPoseFromPoint(r3.Vector{X: 10.0, Y: 10.0, Z: 10.0})) - _, err = ms.Move(ctx, motion.MoveReq{ComponentName: gripper.Named("fake"), Destination: grabPose}) + _, err = ms.Move(ctx, motion.MoveReq{ComponentName: "fake", Destination: grabPose}) test.That(t, err, test.ShouldNotBeNil) }) t.Run("fail on nil destination", func(t *testing.T) { - _, err = ms.Move(ctx, motion.MoveReq{ComponentName: arm.Named("arm1")}) + _, err = ms.Move(ctx, motion.MoveReq{ComponentName: "arm1"}) test.That(t, err, test.ShouldNotBeNil) }) @@ -156,7 +153,7 @@ func TestMoveFailures(t *testing.T) { worldState, err := referenceframe.NewWorldState(nil, transforms) test.That(t, err, test.ShouldBeNil) poseInFrame := referenceframe.NewPoseInFrame("frame2", spatialmath.NewZeroPose()) - _, err = ms.Move(ctx, motion.MoveReq{ComponentName: arm.Named("arm1"), Destination: poseInFrame, WorldState: worldState}) + _, err = ms.Move(ctx, motion.MoveReq{ComponentName: "arm1", Destination: poseInFrame, WorldState: worldState}) test.That(t, err, test.ShouldBeError, referenceframe.NewParentFrameMissingError("frame2", "noParent")) }) } @@ -169,7 +166,7 @@ func TestArmMove(t *testing.T) { ms, teardown := setupMotionServiceFromConfig(t, "../data/moving_arm.json") defer teardown() grabPose := referenceframe.NewPoseInFrame("c", spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: -30, Z: -50})) - _, err = ms.Move(ctx, motion.MoveReq{ComponentName: gripper.Named("pieceGripper"), Destination: grabPose}) + _, err = ms.Move(ctx, motion.MoveReq{ComponentName: "pieceGripper", Destination: grabPose}) test.That(t, err, test.ShouldBeNil) }) @@ -177,7 +174,7 @@ func TestArmMove(t *testing.T) { ms, teardown := setupMotionServiceFromConfig(t, "../data/moving_arm.json") defer teardown() grabPose := referenceframe.NewPoseInFrame("pieceArm", spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: -30, Z: -50})) - _, err = ms.Move(ctx, motion.MoveReq{ComponentName: arm.Named("pieceArm"), Destination: grabPose}) + _, err = ms.Move(ctx, motion.MoveReq{ComponentName: "pieceArm", Destination: grabPose}) test.That(t, err, test.ShouldBeNil) }) @@ -185,7 +182,7 @@ func TestArmMove(t *testing.T) { ms, teardown := setupMotionServiceFromConfig(t, "../data/moving_arm.json") defer teardown() grabPose := referenceframe.NewPoseInFrame("pieceGripper", spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: -30, Z: -50})) - _, err = ms.Move(ctx, motion.MoveReq{ComponentName: gripper.Named("pieceGripper"), Destination: grabPose}) + _, err = ms.Move(ctx, motion.MoveReq{ComponentName: "pieceGripper", Destination: grabPose}) test.That(t, err, test.ShouldBeNil) }) @@ -205,7 +202,7 @@ func TestArmMove(t *testing.T) { worldState, err := referenceframe.NewWorldState(nil, transforms) test.That(t, err, test.ShouldBeNil) grabPose := referenceframe.NewPoseInFrame("testFrame2", spatialmath.NewPoseFromPoint(r3.Vector{X: -20, Y: -130, Z: -40})) - moveReq := motion.MoveReq{ComponentName: gripper.Named("pieceGripper"), Destination: grabPose, WorldState: worldState} + moveReq := motion.MoveReq{ComponentName: "pieceGripper", Destination: grabPose, WorldState: worldState} _, err = ms.Move(context.Background(), moveReq) test.That(t, err, test.ShouldBeNil) }) @@ -255,7 +252,7 @@ func TestArmMoveWithObstacles(t *testing.T) { test.That(t, err, test.ShouldBeNil) _, err = ms.Move( context.Background(), - motion.MoveReq{ComponentName: gripper.Named("pieceArm"), Destination: grabPose, WorldState: worldState}, + motion.MoveReq{ComponentName: "pieceArm", Destination: grabPose, WorldState: worldState}, ) // This fails due to a large obstacle being in the way test.That(t, err, test.ShouldNotBeNil) @@ -318,9 +315,9 @@ func TestPositionalReplanning(t *testing.T) { defer closeFunc(ctx) req := motion.MoveOnGlobeReq{ - ComponentName: resource.NewName(base.API, baseName), + ComponentName: baseName, Destination: dst, - MovementSensorName: resource.NewName(movementsensor.API, moveSensorName), + MovementSensorName: moveSensorName, MotionCfg: motionCfg, Extra: tc.extra, } @@ -377,7 +374,7 @@ func TestObstacleReplanningSlam(t *testing.T) { // This vision service should return nothing the first time it is called, and should return an obstacle all other times. // In this way we generate a valid plan, and then can create a transient obstacle which we must route around. - visSrvc, ok := ms.(*builtIn).visionServices[vision.Named("test-vision")].(*inject.VisionService) + visSrvc, ok := ms.(*builtIn).visionServices["test-vision"].(*inject.VisionService) test.That(t, ok, test.ShouldBeTrue) i := 0 visSrvc.GetObjectPointCloudsFunc = func(ctx context.Context, cameraName string, extra map[string]interface{}) ([]*viz.Object, error) { @@ -395,14 +392,14 @@ func TestObstacleReplanningSlam(t *testing.T) { } obstacleDetectorSlice := []motion.ObstacleDetectorName{ - {VisionServiceName: vision.Named("test-vision"), CameraName: camera.Named("test-camera")}, + {VisionServiceName: "test-vision", CameraName: "test-camera"}, } positionPollingFreq := 0. obstaclePollingFreq := 5. req := motion.MoveOnMapReq{ - ComponentName: base.Named("test-base"), + ComponentName: "test-base", Destination: spatialmath.NewPoseFromPoint(r3.Vector{X: 800, Y: 0, Z: 0}), - SlamName: slam.Named("test_slam"), + SlamName: "test_slam", MotionCfg: &motion.MotionConfiguration{ PositionPollingFreqHz: &positionPollingFreq, ObstaclePollingFreqHz: &obstaclePollingFreq, @@ -430,7 +427,7 @@ func TestMultiplePieces(t *testing.T) { ms, teardown := setupMotionServiceFromConfig(t, "../data/fake_tomato.json") defer teardown() grabPose := referenceframe.NewPoseInFrame("c", spatialmath.NewPoseFromPoint(r3.Vector{X: -0, Y: -30, Z: -50})) - _, err = ms.Move(context.Background(), motion.MoveReq{ComponentName: gripper.Named("gr"), Destination: grabPose}) + _, err = ms.Move(context.Background(), motion.MoveReq{ComponentName: "gr", Destination: grabPose}) test.That(t, err, test.ShouldBeNil) } @@ -439,35 +436,35 @@ func TestGetPose(t *testing.T) { ms, teardown := setupMotionServiceFromConfig(t, "../data/arm_gantry.json") defer teardown() - pose, err := ms.GetPose(context.Background(), arm.Named("gantry1"), "", nil, map[string]interface{}{}) + pose, err := ms.GetPose(context.Background(), "gantry1", "", nil, map[string]interface{}{}) test.That(t, err, test.ShouldBeNil) test.That(t, pose.Parent(), test.ShouldEqual, referenceframe.World) test.That(t, pose.Pose().Point().X, test.ShouldAlmostEqual, 1.2) test.That(t, pose.Pose().Point().Y, test.ShouldAlmostEqual, 0) test.That(t, pose.Pose().Point().Z, test.ShouldAlmostEqual, 0) - pose, err = ms.GetPose(context.Background(), arm.Named("arm1"), "", nil, map[string]interface{}{}) + pose, err = ms.GetPose(context.Background(), "arm1", "", nil, map[string]interface{}{}) test.That(t, err, test.ShouldBeNil) test.That(t, pose.Parent(), test.ShouldEqual, referenceframe.World) test.That(t, pose.Pose().Point().X, test.ShouldAlmostEqual, 501.2) test.That(t, pose.Pose().Point().Y, test.ShouldAlmostEqual, 0) test.That(t, pose.Pose().Point().Z, test.ShouldAlmostEqual, 300) - pose, err = ms.GetPose(context.Background(), arm.Named("arm1"), "gantry1", nil, map[string]interface{}{}) + pose, err = ms.GetPose(context.Background(), "arm1", "gantry1", nil, map[string]interface{}{}) test.That(t, err, test.ShouldBeNil) test.That(t, pose.Parent(), test.ShouldEqual, "gantry1") test.That(t, pose.Pose().Point().X, test.ShouldAlmostEqual, 500) test.That(t, pose.Pose().Point().Y, test.ShouldAlmostEqual, 0) test.That(t, pose.Pose().Point().Z, test.ShouldAlmostEqual, 300) - pose, err = ms.GetPose(context.Background(), arm.Named("gantry1"), "gantry1", nil, map[string]interface{}{}) + pose, err = ms.GetPose(context.Background(), "gantry1", "gantry1", nil, map[string]interface{}{}) test.That(t, err, test.ShouldBeNil) test.That(t, pose.Parent(), test.ShouldEqual, "gantry1") test.That(t, pose.Pose().Point().X, test.ShouldAlmostEqual, 0) test.That(t, pose.Pose().Point().Y, test.ShouldAlmostEqual, 0) test.That(t, pose.Pose().Point().Z, test.ShouldAlmostEqual, 0) - pose, err = ms.GetPose(context.Background(), arm.Named("arm1"), "arm1", nil, map[string]interface{}{}) + pose, err = ms.GetPose(context.Background(), "arm1", "arm1", nil, map[string]interface{}{}) test.That(t, err, test.ShouldBeNil) test.That(t, pose.Parent(), test.ShouldEqual, "arm1") test.That(t, pose.Pose().Point().X, test.ShouldAlmostEqual, 0) @@ -480,7 +477,7 @@ func TestGetPose(t *testing.T) { referenceframe.NewLinkInFrame("testFrame", testPose, "testFrame2", nil), } - pose, err = ms.GetPose(context.Background(), arm.Named("arm1"), "testFrame2", transforms, map[string]interface{}{}) + pose, err = ms.GetPose(context.Background(), "arm1", "testFrame2", transforms, map[string]interface{}{}) test.That(t, err, test.ShouldBeNil) test.That(t, pose.Pose().Point().X, test.ShouldAlmostEqual, -501.2) test.That(t, pose.Pose().Point().Y, test.ShouldAlmostEqual, 0) @@ -493,7 +490,7 @@ func TestGetPose(t *testing.T) { transforms = []*referenceframe.LinkInFrame{ referenceframe.NewLinkInFrame("noParent", testPose, "testFrame", nil), } - pose, err = ms.GetPose(context.Background(), arm.Named("arm1"), "testFrame", transforms, map[string]interface{}{}) + pose, err = ms.GetPose(context.Background(), "arm1", "testFrame", transforms, map[string]interface{}{}) test.That(t, err, test.ShouldBeError, referenceframe.NewParentFrameMissingError("testFrame", "noParent")) test.That(t, pose, test.ShouldBeNil) } @@ -513,7 +510,7 @@ func TestStoppableMoveFunctions(t *testing.T) { t.Run("successfully stop arms", func(t *testing.T) { armName := "test-arm" - injectArmName := arm.Named(armName) + injectArmName := armName goal := referenceframe.NewPoseInFrame( armName, spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: -10, Z: -10}), @@ -569,7 +566,7 @@ func TestStoppableMoveFunctions(t *testing.T) { }, } deps := resource.Dependencies{ - injectArmName: injectArm, + resource.Name{Name: injectArmName}: injectArm, } _, err = createFrameSystemService(ctx, deps, fsParts, logger) @@ -664,9 +661,9 @@ func TestStoppableMoveFunctions(t *testing.T) { } req := motion.MoveOnGlobeReq{ - ComponentName: injectBase.Name(), + ComponentName: injectBase.Name().Name, Destination: goal, - MovementSensorName: injectMovementSensor.Name(), + MovementSensorName: injectMovementSensor.Name().Name, MotionCfg: &motionCfg, Extra: extra, } @@ -716,9 +713,9 @@ func TestStoppableMoveFunctions(t *testing.T) { goal := spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 500}) req := motion.MoveOnMapReq{ - ComponentName: injectBase.Name(), + ComponentName: injectBase.Name().Name, Destination: goal, - SlamName: injectSlam.Name(), + SlamName: injectSlam.Name().Name, MotionCfg: &motion.MotionConfiguration{ PlanDeviationMM: 0.2, }, @@ -770,9 +767,9 @@ func TestStoppableMoveFunctions(t *testing.T) { ms.(*builtIn).fsService = fsSvc req := motion.MoveOnMapReq{ - ComponentName: injectBase.Name(), + ComponentName: injectBase.Name().Name, Destination: spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 500}), - SlamName: injectSlam.Name(), + SlamName: injectSlam.Name().Name, MotionCfg: &motion.MotionConfiguration{ PlanDeviationMM: 1, }, @@ -808,13 +805,13 @@ func TestGetTransientDetectionsSlam(t *testing.T) { // construct move request moveReq := motion.MoveOnMapReq{ - ComponentName: base.Named("test-base"), + ComponentName: "test-base", Destination: spatialmath.NewPoseFromPoint(r3.Vector{X: 10, Y: 0, Z: 0}), - SlamName: slam.Named("test_slam"), + SlamName: "test_slam", MotionCfg: &motion.MotionConfiguration{ PlanDeviationMM: 1, ObstacleDetectors: []motion.ObstacleDetectorName{ - {VisionServiceName: vision.Named("test-vision"), CameraName: camera.Named("test-camera")}, + {VisionServiceName: "test-vision", CameraName: "test-camera"}, }, }, } @@ -825,7 +822,7 @@ func TestGetTransientDetectionsSlam(t *testing.T) { mr, ok := planExecutor.(*moveRequest) test.That(t, ok, test.ShouldBeTrue) - injectedVis, ok := ms.(*builtIn).visionServices[vision.Named("test-vision")].(*inject.VisionService) + injectedVis, ok := ms.(*builtIn).visionServices["test-vision"].(*inject.VisionService) test.That(t, ok, test.ShouldBeTrue) // define injected method on vision service @@ -854,7 +851,7 @@ func TestGetTransientDetectionsSlam(t *testing.T) { testFn := func(t *testing.T, tc testCase) { t.Helper() - transformedGeoms, err := mr.getTransientDetections(ctx, injectedVis, camera.Named("test-camera")) + transformedGeoms, err := mr.getTransientDetections(ctx, injectedVis, "test-camera") test.That(t, err, test.ShouldBeNil) test.That(t, transformedGeoms.Parent(), test.ShouldEqual, referenceframe.World) test.That(t, len(transformedGeoms.Geometries()), test.ShouldEqual, 1) @@ -881,18 +878,18 @@ func TestGetTransientDetectionsMath(t *testing.T) { geoPoseOrigin := spatialmath.NewGeoPose(geo.NewPoint(0, 0), 0) destinationGeoPose := spatialmath.PoseToGeoPose(geoPoseOrigin, destinationPose) - moveSensor, ok := ms.(*builtIn).movementSensors[resource.NewName(movementsensor.API, moveSensorName)] + moveSensor, ok := ms.(*builtIn).movementSensors[moveSensorName] test.That(t, ok, test.ShouldBeTrue) // construct move request moveReq := motion.MoveOnGlobeReq{ - ComponentName: base.Named("test-base"), + ComponentName: "test-base", Destination: destinationGeoPose.Location(), - MovementSensorName: moveSensor.Name(), + MovementSensorName: moveSensor.Name().Name, MotionCfg: &motion.MotionConfiguration{ PlanDeviationMM: 1, ObstacleDetectors: []motion.ObstacleDetectorName{ - {VisionServiceName: vision.Named("injectedVisionSvc"), CameraName: camera.Named("test-camera")}, + {VisionServiceName: "injectedVisionSvc", CameraName: "test-camera"}, }, }, } @@ -997,7 +994,7 @@ func TestStopPlan(t *testing.T) { req := motion.StopPlanReq{} err := ms.StopPlan(ctx, req) - test.That(t, err, test.ShouldBeError, resource.NewNotFoundError(req.ComponentName)) + test.That(t, err, test.ShouldBeError, resource.NewNotFoundError(resource.Name{Name: req.ComponentName})) } func TestListPlanStatuses(t *testing.T) { @@ -1026,7 +1023,7 @@ func TestPlanHistory(t *testing.T) { defer closeFunc(ctx) req := motion.PlanHistoryReq{} history, err := ms.PlanHistory(ctx, req) - test.That(t, err, test.ShouldResemble, resource.NewNotFoundError(req.ComponentName)) + test.That(t, err, test.ShouldResemble, resource.NewNotFoundError(resource.Name{Name: req.ComponentName})) test.That(t, history, test.ShouldBeNil) } @@ -1056,13 +1053,13 @@ func TestCheckPlan(t *testing.T) { movementSensor, ok := localizer.(movementsensor.MovementSensor) test.That(t, ok, test.ShouldBeTrue) - fakeBase, ok := ms.(*builtIn).components[baseResource.ShortName()] + fakeBase, ok := ms.(*builtIn).components[baseName] test.That(t, ok, test.ShouldBeTrue) req := motion.MoveOnGlobeReq{ - ComponentName: fakeBase.Name(), + ComponentName: fakeBase.Name().Name, Destination: dst, - MovementSensorName: movementSensor.Name(), + MovementSensorName: movementSensor.Name().Name, } // construct move request @@ -1259,7 +1256,7 @@ func TestDoCommand(t *testing.T) { worldState, err := referenceframe.NewWorldState(geometries, nil) test.That(t, err, test.ShouldBeNil) moveReq := motion.MoveReq{ - ComponentName: gripper.Named("pieceGripper"), + ComponentName: "pieceGripper", WorldState: worldState, Destination: referenceframe.NewPoseInFrame("c", spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: -30, Z: -50})), Extra: nil, @@ -1396,7 +1393,7 @@ func TestMultiWaypointPlanning(t *testing.T) { wp2State := armplanning.NewPlanState(referenceframe.FrameSystemPoses{"pieceGripper": waypoint2}, nil) moveReq := motion.MoveReq{ - ComponentName: gripper.Named("pieceGripper"), + ComponentName: "pieceGripper", Destination: finalPose, Extra: map[string]interface{}{ "waypoints": []interface{}{wp1State.Serialize(), wp2State.Serialize()}, @@ -1442,7 +1439,7 @@ func TestMultiWaypointPlanning(t *testing.T) { finalPose := referenceframe.NewPoseInFrame("world", spatialmath.NewPoseFromPoint(r3.Vector{X: -800, Y: -180, Z: 34})) moveReq := motion.MoveReq{ - ComponentName: gripper.Named("pieceGripper"), + ComponentName: "pieceGripper", Destination: finalPose, Extra: map[string]interface{}{ "waypoints": []interface{}{wp1State.Serialize(), wp2State.Serialize()}, @@ -1495,7 +1492,7 @@ func TestMultiWaypointPlanning(t *testing.T) { finalPose := referenceframe.NewPoseInFrame("world", spatialmath.NewPoseFromPoint(r3.Vector{X: -800, Y: -180, Z: 34})) moveReq := motion.MoveReq{ - ComponentName: gripper.Named("pieceGripper"), + ComponentName: "pieceGripper", Destination: finalPose, Extra: map[string]interface{}{ "start_state": startState.Serialize(), @@ -1529,7 +1526,7 @@ func TestMultiWaypointPlanning(t *testing.T) { goalState := armplanning.NewPlanState(nil, referenceframe.FrameSystemInputs{"pieceArm": referenceframe.FloatsToInputs(goalConfig)}) moveReq := motion.MoveReq{ - ComponentName: gripper.Named("pieceGripper"), + ComponentName: "pieceGripper", Extra: map[string]interface{}{ "goal_state": goalState.Serialize(), "smooth_iter": 5, diff --git a/services/motion/builtin/move_on_globe_test.go b/services/motion/builtin/move_on_globe_test.go index 5b7f775e9be..7864994d428 100644 --- a/services/motion/builtin/move_on_globe_test.go +++ b/services/motion/builtin/move_on_globe_test.go @@ -12,14 +12,9 @@ import ( geo "github.com/kellydunn/golang-geo" "go.viam.com/test" - "go.viam.com/rdk/components/base" - "go.viam.com/rdk/components/camera" - "go.viam.com/rdk/components/movementsensor" _ "go.viam.com/rdk/components/register" "go.viam.com/rdk/pointcloud" - "go.viam.com/rdk/resource" "go.viam.com/rdk/services/motion" - "go.viam.com/rdk/services/vision" "go.viam.com/rdk/spatialmath" "go.viam.com/rdk/testutils/inject" viz "go.viam.com/rdk/vision" @@ -43,8 +38,8 @@ func TestMoveOnGlobe(t *testing.T) { defer closeFunc(ctx) req := motion.MoveOnGlobeReq{ - ComponentName: baseResource, - MovementSensorName: moveSensorResource, + ComponentName: baseName, + MovementSensorName: moveSensorName, Destination: dst, } executionID, err := ms.MoveOnGlobe(ctx, req) @@ -60,7 +55,7 @@ func TestMoveOnGlobe(t *testing.T) { test.That(t, ph[0].StatusHistory[0].State, test.ShouldEqual, motion.PlanStateInProgress) test.That(t, len(ph[0].Plan.Path()), test.ShouldNotEqual, 0) - err = ms.StopPlan(ctx, motion.StopPlanReq{ComponentName: baseResource}) + err = ms.StopPlan(ctx, motion.StopPlanReq{ComponentName: baseName}) test.That(t, err, test.ShouldBeNil) ph2, err := ms.PlanHistory(ctx, motion.PlanHistoryReq{ComponentName: req.ComponentName}) @@ -73,7 +68,7 @@ func TestMoveOnGlobe(t *testing.T) { test.That(t, len(ph2[0].Plan.Path()), test.ShouldNotEqual, 0) // Proves that calling StopPlan after the plan has reached a terminal state is idempotent - err = ms.StopPlan(ctx, motion.StopPlanReq{ComponentName: baseResource}) + err = ms.StopPlan(ctx, motion.StopPlanReq{ComponentName: baseName}) test.That(t, err, test.ShouldBeNil) ph3, err := ms.PlanHistory(ctx, motion.PlanHistoryReq{ComponentName: req.ComponentName}) test.That(t, err, test.ShouldBeNil) @@ -104,9 +99,9 @@ func TestMoveOnGlobe(t *testing.T) { geoGeometry := spatialmath.NewGeoGeometry(gpsPoint, []spatialmath.Geometry{geometry1, geometry2, geometry3, geometry4}) req := motion.MoveOnGlobeReq{ - ComponentName: baseResource, + ComponentName: baseName, Destination: dst, - MovementSensorName: moveSensorResource, + MovementSensorName: moveSensorName, Obstacles: []*spatialmath.GeoGeometry{geoGeometry}, MotionCfg: &motion.MotionConfiguration{}, Extra: extra, @@ -121,7 +116,7 @@ func TestMoveOnGlobe(t *testing.T) { t.Run("check offset constructed correctly", func(t *testing.T) { _, ms, closeFunc := CreateMoveOnGlobeTestEnvironment(ctx, t, gpsPoint, 80, nil) defer closeFunc(ctx) - movementSensorInBase, err := ms.GetPose(ctx, resource.NewName(movementsensor.API, "test-gps"), "test-base", nil, nil) + movementSensorInBase, err := ms.GetPose(ctx, "test-gps", "test-base", nil, nil) test.That(t, err, test.ShouldBeNil) test.That(t, movementSensorInBase.Pose().Point(), test.ShouldResemble, movementSensorInBasePoint) }) @@ -131,9 +126,9 @@ func TestMoveOnGlobe(t *testing.T) { defer closeFunc(ctx) req := motion.MoveOnGlobeReq{ - ComponentName: baseResource, + ComponentName: baseName, Destination: gpsPoint, - MovementSensorName: moveSensorResource, + MovementSensorName: moveSensorName, MotionCfg: &motion.MotionConfiguration{}, Extra: extra, } @@ -181,8 +176,8 @@ func TestBoundingRegionsConstraint(t *testing.T) { test.That(t, err, test.ShouldBeNil) req := motion.MoveOnGlobeReq{ - ComponentName: baseResource, - MovementSensorName: moveSensorResource, + ComponentName: baseName, + MovementSensorName: moveSensorName, Destination: dst, BoundingRegions: []*spatialmath.GeoGeometry{ spatialmath.NewGeoGeometry(origin, []spatialmath.Geometry{box}), @@ -202,8 +197,8 @@ func TestBoundingRegionsConstraint(t *testing.T) { test.That(t, err, test.ShouldBeNil) req := motion.MoveOnGlobeReq{ - ComponentName: baseResource, - MovementSensorName: moveSensorResource, + ComponentName: baseName, + MovementSensorName: moveSensorName, Destination: dst, BoundingRegions: []*spatialmath.GeoGeometry{ spatialmath.NewGeoGeometry(geo.NewPoint(20, 20), []spatialmath.Geometry{box}), @@ -222,8 +217,8 @@ func TestBoundingRegionsConstraint(t *testing.T) { defer closeFunc(ctx) req := motion.MoveOnGlobeReq{ - ComponentName: baseResource, - MovementSensorName: moveSensorResource, + ComponentName: baseName, + MovementSensorName: moveSensorName, Destination: dst, MotionCfg: motionCfg, Extra: extra, @@ -240,8 +235,8 @@ func TestBoundingRegionsConstraint(t *testing.T) { test.That(t, err, test.ShouldBeNil) req := motion.MoveOnGlobeReq{ - ComponentName: baseResource, - MovementSensorName: moveSensorResource, + ComponentName: baseName, + MovementSensorName: moveSensorName, Destination: dst, MotionCfg: motionCfg, BoundingRegions: []*spatialmath.GeoGeometry{ @@ -269,8 +264,8 @@ func TestBoundingRegionsConstraint(t *testing.T) { test.That(t, err, test.ShouldBeNil) req := motion.MoveOnGlobeReq{ - ComponentName: baseResource, - MovementSensorName: moveSensorResource, + ComponentName: baseName, + MovementSensorName: moveSensorName, Destination: dst, MotionCfg: motionCfg, BoundingRegions: []*spatialmath.GeoGeometry{ @@ -302,7 +297,7 @@ func TestObstacleReplanningGlobe(t *testing.T) { } obstacleDetectorSlice := []motion.ObstacleDetectorName{ - {VisionServiceName: vision.Named("injectedVisionSvc"), CameraName: camera.Named("injectedCamera")}, + {VisionServiceName: "injectedVisionSvc", CameraName: "injectedCamera"}, } obstaclePollingFreq := 5. @@ -409,9 +404,9 @@ func TestObstacleReplanningGlobe(t *testing.T) { srvc.GetObjectPointCloudsFunc = pcFunc req := motion.MoveOnGlobeReq{ - ComponentName: resource.NewName(base.API, baseName), + ComponentName: baseName, Destination: dst, - MovementSensorName: resource.NewName(movementsensor.API, moveSensorName), + MovementSensorName: moveSensorName, MotionCfg: cfg, Extra: tc.extra, } diff --git a/services/motion/builtin/move_on_map_test.go b/services/motion/builtin/move_on_map_test.go index dc787ecff78..1b0c9a3d711 100644 --- a/services/motion/builtin/move_on_map_test.go +++ b/services/motion/builtin/move_on_map_test.go @@ -20,7 +20,6 @@ import ( "go.viam.com/rdk/resource" robotimpl "go.viam.com/rdk/robot/impl" "go.viam.com/rdk/services/motion" - "go.viam.com/rdk/services/slam" "go.viam.com/rdk/spatialmath" "go.viam.com/rdk/testutils/inject" ) @@ -70,9 +69,9 @@ func TestMoveOnMap(t *testing.T) { ms.(*builtIn).fsService = fsSvc req := motion.MoveOnMapReq{ - ComponentName: base.Named("test-base"), + ComponentName: "test-base", Destination: spatialmath.NewPoseFromPoint(r3.Vector{X: 1001, Y: 1001}), - SlamName: slam.Named("test_slam"), + SlamName: "test_slam", Extra: map[string]interface{}{"timeout": 0.01}, } @@ -90,10 +89,10 @@ func TestMoveOnMap(t *testing.T) { easyGoalInSLAMFrame := spatialmath.PoseBetweenInverse(motion.SLAMOrientationAdjustment, easyGoalInBaseFrame) req := motion.MoveOnMapReq{ - ComponentName: base.Named("test-base"), + ComponentName: "test-base", Destination: easyGoalInSLAMFrame, MotionCfg: motionCfg, - SlamName: slam.Named("test_slam"), + SlamName: "test_slam", Extra: map[string]interface{}{"smooth_iter": 0}, } @@ -112,7 +111,7 @@ func TestMoveOnMap(t *testing.T) { test.That(t, ph[0].StatusHistory[0].State, test.ShouldEqual, motion.PlanStateInProgress) test.That(t, len(ph[0].Plan.Path()), test.ShouldNotEqual, 0) - err = ms.StopPlan(ctx, motion.StopPlanReq{ComponentName: baseResource}) + err = ms.StopPlan(ctx, motion.StopPlanReq{ComponentName: baseName}) test.That(t, err, test.ShouldBeNil) ph2, err := ms.PlanHistory(ctx, motion.PlanHistoryReq{ComponentName: req.ComponentName}) @@ -125,7 +124,7 @@ func TestMoveOnMap(t *testing.T) { test.That(t, len(ph2[0].Plan.Path()), test.ShouldNotEqual, 0) // Proves that calling StopPlan after the plan has reached a terminal state is idempotent - err = ms.StopPlan(ctx, motion.StopPlanReq{ComponentName: baseResource}) + err = ms.StopPlan(ctx, motion.StopPlanReq{ComponentName: baseName}) test.That(t, err, test.ShouldBeNil) ph3, err := ms.PlanHistory(ctx, motion.PlanHistoryReq{ComponentName: req.ComponentName}) test.That(t, err, test.ShouldBeNil) @@ -137,10 +136,10 @@ func TestMoveOnMap(t *testing.T) { defer closeFunc(ctx) req := motion.MoveOnMapReq{ - ComponentName: base.Named("test-base"), + ComponentName: "test-base", MotionCfg: motionCfg, Destination: spatialmath.NewPoseFromOrientation(&spatialmath.EulerAngles{Yaw: 3}), - SlamName: slam.Named("test_slam"), + SlamName: "test_slam", } timeoutCtx, timeoutFn := context.WithTimeout(ctx, time.Second*5) @@ -160,7 +159,7 @@ func TestMoveOnMapStaticObs(t *testing.T) { } baseName := "test-base" - slamName := "test-slam" + slamName := "test_slam" // Create an injected Base geometry, err := (&spatialmath.GeometryConfig{R: 30}).ParseConfig() @@ -201,9 +200,9 @@ func TestMoveOnMapStaticObs(t *testing.T) { goal := spatialmath.NewPoseFromPoint(r3.Vector{X: 0.6556e3, Y: 0.64152e3}) req := motion.MoveOnMapReq{ - ComponentName: injectBase.Name(), + ComponentName: injectBase.Name().Name, Destination: goal, - SlamName: injectSlam.Name(), + SlamName: injectSlam.Name().Name, MotionCfg: &motion.MotionConfiguration{PlanDeviationMM: 0.01}, Extra: extra, } diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index 11dbd4a5664..4bbbdebb383 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -14,6 +14,7 @@ import ( "go.viam.com/rdk/components/base" "go.viam.com/rdk/components/base/kinematicbase" + "go.viam.com/rdk/components/movementsensor" "go.viam.com/rdk/logging" "go.viam.com/rdk/motionplan" "go.viam.com/rdk/motionplan/baseplanning" @@ -66,7 +67,7 @@ type moveRequest struct { planRequest *baseplanning.PlanRequest seedPlan motionplan.Plan kinematicBase kinematicbase.KinematicBase - obstacleDetectors map[vision.Service][]resource.Name + obstacleDetectors map[vision.Service][]string replanCostFactor float64 // TODO(RSDK-8683): remove atGoalCheck and put it in the motionplan package // atGoalCheck func(basePose spatialmath.Pose) *state.ExecuteResponse @@ -222,12 +223,12 @@ func (mr *moveRequest) deviatedFromPlan(ctx context.Context, plan motionplan.Pla func (mr *moveRequest) getTransientDetections( ctx context.Context, visSrvc vision.Service, - camName resource.Name, + camName string, ) (*referenceframe.GeometriesInFrame, error) { mr.logger.CDebugf(ctx, "proceeding to get detections from vision service: %s with camera: %s", visSrvc.Name().ShortName(), - camName.ShortName(), + camName, ) baseExecutionState, err := mr.kinematicBase.ExecutionState(ctx) @@ -253,7 +254,7 @@ func (mr *moveRequest) getTransientDetections( )...) inputMap[mr.kinematicBase.Name().ShortName()] = kbInputs - detections, err := visSrvc.GetObjectPointClouds(ctx, camName.Name, nil) + detections, err := visSrvc.GetObjectPointClouds(ctx, camName, nil) if err != nil { return nil, err } @@ -263,7 +264,7 @@ func (mr *moveRequest) getTransientDetections( for i, detection := range detections { geometry := detection.Geometry // update the label of the geometry so we know it is transient - label := camName.ShortName() + "_transientObstacle_" + strconv.Itoa(i) + label := camName + "_transientObstacle_" + strconv.Itoa(i) if geometry.Label() != "" { label += "_" + geometry.Label() } @@ -274,7 +275,7 @@ func (mr *moveRequest) getTransientDetections( // in the world frame tf, err := mr.localizingFS.Transform( inputMap, - referenceframe.NewGeometriesInFrame(camName.ShortName(), []spatialmath.Geometry{geometry}), + referenceframe.NewGeometriesInFrame(camName, []spatialmath.Geometry{geometry}), referenceframe.World, ) if err != nil { @@ -616,7 +617,7 @@ func (ms *builtIn) newMoveOnGlobeRequest( // build the localizer from the movement sensor movementSensor, ok := ms.movementSensors[req.MovementSensorName] if !ok { - return nil, resource.DependencyNotFoundError(req.MovementSensorName) + return nil, resource.DependencyNotFoundError(movementsensor.Named(req.MovementSensorName)) } origin, _, err := movementSensor.Position(ctx, nil) @@ -630,7 +631,7 @@ func (ms *builtIn) newMoveOnGlobeRequest( } // add an offset between the movement sensor and the base if it is applicable - baseOrigin := referenceframe.NewPoseInFrame(req.ComponentName.ShortName(), spatialmath.NewZeroPose()) + baseOrigin := referenceframe.NewPoseInFrame(req.ComponentName, spatialmath.NewZeroPose()) movementSensorToBase, err := ms.fsService.TransformPose(ctx, baseOrigin, movementSensor.Name().ShortName(), nil) if err != nil { // here we make the assumption the movement sensor is coincident with the base @@ -640,9 +641,9 @@ 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.ShortName()] + baseComponent, ok := ms.components[req.ComponentName] if !ok { - return nil, resource.NewNotFoundError(req.ComponentName) + return nil, resource.NewNotFoundError(base.Named(req.ComponentName)) } b, ok := baseComponent.(base.Base) if !ok { @@ -736,7 +737,7 @@ func (ms *builtIn) newMoveOnMapRequest( // get the SLAM Service from the slamName slamSvc, ok := ms.slamServices[req.SlamName] if !ok { - return nil, resource.DependencyNotFoundError(req.SlamName) + return nil, resource.DependencyNotFoundError(slam.Named(req.SlamName)) } // verify slam is in localization mode @@ -756,9 +757,9 @@ 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.ShortName()] + component, ok := ms.components[req.ComponentName] if !ok { - return nil, resource.DependencyNotFoundError(req.ComponentName) + return nil, resource.DependencyNotFoundError(base.Named(req.ComponentName)) } b, ok := component.(base.Base) if !ok { @@ -898,19 +899,19 @@ func (ms *builtIn) createBaseMoveRequest( return nil, err } - obstacleDetectors := make(map[vision.Service][]resource.Name) + obstacleDetectors := make(map[vision.Service][]string) for _, obstacleDetectorNamePair := range motionCfg.obstacleDetectors { // get vision service visionServiceName := obstacleDetectorNamePair.VisionServiceName visionSvc, ok := ms.visionServices[visionServiceName] if !ok { - return nil, resource.DependencyNotFoundError(visionServiceName) + return nil, resource.DependencyNotFoundError(resource.Name{Name: visionServiceName}) } // add camera to vision service map camList, ok := obstacleDetectors[visionSvc] if !ok { - obstacleDetectors[visionSvc] = []resource.Name{obstacleDetectorNamePair.CameraName} + obstacleDetectors[visionSvc] = []string{obstacleDetectorNamePair.CameraName} } else { camList = append(camList, obstacleDetectorNamePair.CameraName) obstacleDetectors[visionSvc] = camList diff --git a/services/motion/builtin/state/state.go b/services/motion/builtin/state/state.go index 1ae14d7aead..e994b243c89 100644 --- a/services/motion/builtin/state/state.go +++ b/services/motion/builtin/state/state.go @@ -3,7 +3,6 @@ package state import ( - "cmp" "context" "fmt" "slices" @@ -64,7 +63,7 @@ type planMsg struct { } type stateUpdateMsg struct { - componentName resource.Name + componentName string executionID motion.ExecutionID planID motion.PlanID planStatus motion.PlanStatus @@ -76,7 +75,7 @@ type stateUpdateMsg struct { // required to shut down an execution's goroutine. type stateExecution struct { id motion.ExecutionID - componentName resource.Name + componentName string waitGroup *sync.WaitGroup cancelFunc context.CancelFunc history []motion.PlanWithStatus @@ -104,7 +103,7 @@ type execution[R any] struct { cancelCtx context.Context cancelFunc context.CancelFunc logger logging.Logger - componentName resource.Name + componentName string req R plannerExecutorConstructor PlannerExecutorConstructor[R] } @@ -291,7 +290,7 @@ type State struct { ttl time.Duration // mu protects the componentStateByComponent mu sync.RWMutex - componentStateByComponent map[resource.Name]componentState + componentStateByComponent map[string]componentState } // NewState creates a new state. @@ -324,7 +323,7 @@ func NewState( cancelCtx: cancelCtx, cancelFunc: cancelFunc, waitGroup: &sync.WaitGroup{}, - componentStateByComponent: make(map[resource.Name]componentState), + componentStateByComponent: make(map[string]componentState), ttl: ttl, logger: logger, } @@ -355,7 +354,7 @@ func NewState( func StartExecution[R any]( ctx context.Context, s *State, - componentName resource.Name, + componentName string, req R, plannerExecutorConstructor PlannerExecutorConstructor[R], ) (motion.ExecutionID, error) { @@ -395,7 +394,7 @@ func (s *State) Stop() { } // StopExecutionByResource stops the active execution with a given resource name in the State. -func (s *State) StopExecutionByResource(componentName resource.Name) error { +func (s *State) StopExecutionByResource(componentName string) error { // Read lock held to get the execution s.mu.RLock() componentExectionState, exists := s.componentStateByComponent[componentName] @@ -403,13 +402,13 @@ func (s *State) StopExecutionByResource(componentName resource.Name) error { // return error if component name is not in StateMap if !exists { s.mu.RUnlock() - return resource.NewNotFoundError(componentName) + return resource.NewNotFoundError(resource.Name{Name: componentName}) } e, exists := componentExectionState.executionsByID[componentExectionState.lastExecutionID()] if !exists { s.mu.RUnlock() - return resource.NewNotFoundError(componentName) + return resource.NewNotFoundError(resource.Name{Name: componentName}) } s.mu.RUnlock() @@ -431,7 +430,7 @@ func (s *State) PlanHistory(req motion.PlanHistoryReq) ([]motion.PlanWithStatus, defer s.mu.RUnlock() cs, exists := s.componentStateByComponent[req.ComponentName] if !exists { - return nil, resource.NewNotFoundError(req.ComponentName) + return nil, resource.NewNotFoundError(resource.Name{Name: req.ComponentName}) } executionID := req.ExecutionID @@ -446,7 +445,7 @@ func (s *State) PlanHistory(req motion.PlanHistoryReq) ([]motion.PlanWithStatus, if ex, exists := cs.executionsByID[executionID]; exists { return renderableHistory(ex.history[:1]), nil } - return nil, resource.NewNotFoundError(req.ComponentName) + return nil, resource.NewNotFoundError(resource.Name{Name: req.ComponentName}) } // specific execution id when lastPlanOnly is NOT enabled @@ -454,7 +453,7 @@ func (s *State) PlanHistory(req motion.PlanHistoryReq) ([]motion.PlanWithStatus, if ex, exists := cs.executionsByID[executionID]; exists { return renderableHistory(ex.history), nil } - return nil, resource.NewNotFoundError(req.ComponentName) + return nil, resource.NewNotFoundError(resource.Name{Name: req.ComponentName}) } return renderableHistory(cs.lastExecution().history), nil @@ -480,9 +479,7 @@ func (s *State) ListPlanStatuses(req motion.ListPlanStatusesReq) ([]motion.PlanS statuses := []motion.PlanStatusWithID{} componentNames := maps.Keys(s.componentStateByComponent) - slices.SortFunc(componentNames, func(a, b resource.Name) int { - return cmp.Compare(a.String(), b.String()) - }) + slices.Sort(componentNames) if req.OnlyActivePlans { for _, name := range componentNames { @@ -524,7 +521,7 @@ func (s *State) ListPlanStatuses(req motion.ListPlanStatusesReq) ([]motion.PlanS // ValidateNoActiveExecutionID returns an error if there is already an active // Execution for the resource name within the State. -func (s *State) ValidateNoActiveExecutionID(name resource.Name) error { +func (s *State) ValidateNoActiveExecutionID(name string) error { if es, err := s.activeExecution(name); err == nil { return fmt.Errorf("there is already an active executionID: %s", es.id) } @@ -606,7 +603,7 @@ func (s *State) updateStateStatusUpdate(update stateUpdateMsg) { s.componentStateByComponent[update.componentName] = componentExecutions } -func (s *State) activeExecution(name resource.Name) (stateExecution, error) { +func (s *State) activeExecution(name string) (stateExecution, error) { s.mu.RLock() defer s.mu.RUnlock() @@ -614,11 +611,11 @@ func (s *State) activeExecution(name resource.Name) (stateExecution, error) { es := cs.lastExecution() if _, exists := motion.TerminalStateSet[es.history[0].StatusHistory[0].State]; exists { - return stateExecution{}, resource.NewNotFoundError(name) + return stateExecution{}, resource.NewNotFoundError(resource.Name{Name: name}) } return es, nil } - return stateExecution{}, resource.NewNotFoundError(name) + return stateExecution{}, resource.NewNotFoundError(resource.Name{Name: name}) } func (s *State) purgeOlderThanTTL() error { diff --git a/services/motion/builtin/state/state_test.go b/services/motion/builtin/state/state_test.go index 1475df37269..7aa47382504 100644 --- a/services/motion/builtin/state/state_test.go +++ b/services/motion/builtin/state/state_test.go @@ -10,7 +10,6 @@ import ( "github.com/google/uuid" "go.viam.com/test" - "go.viam.com/rdk/components/base" "go.viam.com/rdk/logging" "go.viam.com/rdk/motionplan" "go.viam.com/rdk/referenceframe" @@ -58,7 +57,7 @@ func (tpe *testPlannerExecutor) AnchorGeoPose() *spatialmath.GeoPose { func TestState(t *testing.T) { logger := logging.NewTestLogger(t) - myBase := base.Named("mybase") + myBase := "mybase" t.Parallel() executionWaitingForCtxCancelledPlanConstructor := func( @@ -315,7 +314,7 @@ func TestState(t *testing.T) { test.That(t, err, test.ShouldBeNil) req2 := motion.PlanHistoryReq{} _, err = s.PlanHistory(req2) - test.That(t, err, test.ShouldBeError, resource.NewNotFoundError(req2.ComponentName)) + test.That(t, err, test.ShouldBeError, resource.NewNotFoundError(resource.Name{Name: req2.ComponentName})) }) t.Run("end to end test", func(t *testing.T) { @@ -585,14 +584,14 @@ func TestState(t *testing.T) { // first plan succeeds if replanCount == 0 { pbc := referenceframe.FrameSystemPoses{ - req.ComponentName.ShortName(): referenceframe.NewPoseInFrame(referenceframe.World, spatialmath.NewZeroPose()), + req.ComponentName: referenceframe.NewPoseInFrame(referenceframe.World, spatialmath.NewZeroPose()), } return motionplan.NewSimplePlan([]referenceframe.FrameSystemPoses{pbc}, nil), nil } // first replan succeeds if replanCount == 1 { pbc := referenceframe.FrameSystemPoses{ - req.ComponentName.ShortName(): referenceframe.NewPoseInFrame(referenceframe.World, spatialmath.NewZeroPose()), + req.ComponentName: referenceframe.NewPoseInFrame(referenceframe.World, spatialmath.NewZeroPose()), } return motionplan.NewSimplePlan([]referenceframe.FrameSystemPoses{pbc, pbc}, nil), nil } @@ -720,7 +719,7 @@ func TestState(t *testing.T) { // providing an executionID which is not known to the state returns an error pws14, err := s.PlanHistory(motion.PlanHistoryReq{ComponentName: myBase, ExecutionID: uuid.New()}) - test.That(t, err, test.ShouldBeError, resource.NewNotFoundError(myBase)) + test.That(t, err, test.ShouldBeError, resource.NewNotFoundError(resource.Name{Name: myBase})) test.That(t, len(pws14), test.ShouldEqual, 0) // Returns the last status of all plans that have executed @@ -862,14 +861,14 @@ func TestState(t *testing.T) { // the first execution (stopped) has been forgotten ph4, err := s.PlanHistory(motion.PlanHistoryReq{ComponentName: req.ComponentName, ExecutionID: executionID1}) - test.That(t, err, test.ShouldBeError, resource.NewNotFoundError(req.ComponentName)) + test.That(t, err, test.ShouldBeError, resource.NewNotFoundError(resource.Name{Name: req.ComponentName})) test.That(t, len(ph4), test.ShouldEqual, 0) - req2 := motion.MoveOnGlobeReq{ComponentName: base.Named("mybase2")} + req2 := motion.MoveOnGlobeReq{ComponentName: "mybase2"} executionID4, err := state.StartExecution(ctx, s, req2.ComponentName, req2, executionWaitingForCtxCancelledPlanConstructor) test.That(t, err, test.ShouldBeNil) - req3 := motion.MoveOnGlobeReq{ComponentName: base.Named("mybase3")} + req3 := motion.MoveOnGlobeReq{ComponentName: "mybase3"} _, err = state.StartExecution(ctx, s, req3.ComponentName, req3, executionWaitingForCtxCancelledPlanConstructor) test.That(t, err, test.ShouldBeNil) @@ -945,14 +944,14 @@ func TestState(t *testing.T) { time.Sleep(sleepTTLDuration) _, err = s.PlanHistory(motion.PlanHistoryReq{ComponentName: req.ComponentName}) - test.That(t, err, test.ShouldBeError, resource.NewNotFoundError(req.ComponentName)) + test.That(t, err, test.ShouldBeError, resource.NewNotFoundError(resource.Name{Name: req.ComponentName})) ps6, err := s.ListPlanStatuses(motion.ListPlanStatusesReq{}) test.That(t, err, test.ShouldBeNil) test.That(t, len(ps6), test.ShouldEqual, 0) pbc := referenceframe.FrameSystemPoses{ - req.ComponentName.ShortName(): referenceframe.NewPoseInFrame(referenceframe.World, spatialmath.NewZeroPose()), + req.ComponentName: referenceframe.NewPoseInFrame(referenceframe.World, spatialmath.NewZeroPose()), } // Failed after replanning @@ -1008,7 +1007,7 @@ func TestState(t *testing.T) { time.Sleep(sleepTTLDuration) _, err = s.PlanHistory(motion.PlanHistoryReq{ComponentName: req.ComponentName}) - test.That(t, err, test.ShouldBeError, resource.NewNotFoundError(req.ComponentName)) + test.That(t, err, test.ShouldBeError, resource.NewNotFoundError(resource.Name{Name: req.ComponentName})) ps8, err := s.ListPlanStatuses(motion.ListPlanStatusesReq{}) test.That(t, err, test.ShouldBeNil) diff --git a/services/motion/builtin/testutils.go b/services/motion/builtin/testutils.go index 1207acba3d6..23fe26b7232 100644 --- a/services/motion/builtin/testutils.go +++ b/services/motion/builtin/testutils.go @@ -41,8 +41,6 @@ var ( baseName = "test-base" moveSensorName = "test-movement-sensor" - moveSensorResource = resource.NewName(movementsensor.API, moveSensorName) - baseResource = resource.NewName(base.API, baseName) movementSensorInBasePoint = r3.Vector{X: -10, Y: 0, Z: 0} updateRate = 33 ) diff --git a/services/motion/builtin/validation_test.go b/services/motion/builtin/validation_test.go index cffe96ab8fa..61cb1421d5c 100644 --- a/services/motion/builtin/validation_test.go +++ b/services/motion/builtin/validation_test.go @@ -13,13 +13,8 @@ import ( geo "github.com/kellydunn/golang-geo" "go.viam.com/test" - "go.viam.com/rdk/components/base" - "go.viam.com/rdk/components/camera" - "go.viam.com/rdk/components/movementsensor" _ "go.viam.com/rdk/components/register" "go.viam.com/rdk/services/motion" - "go.viam.com/rdk/services/slam" - "go.viam.com/rdk/services/vision" "go.viam.com/rdk/spatialmath" ) @@ -36,9 +31,9 @@ func TestMoveCallInputs(t *testing.T) { defer closeFunc(ctx) req := motion.MoveOnMapReq{ - ComponentName: base.Named("non existent base"), + ComponentName: "non existent base", Destination: goalPose, - SlamName: slam.Named("test_slam"), + SlamName: "test_slam", MotionCfg: &motion.MotionConfiguration{PlanDeviationMM: 10}, } @@ -53,8 +48,8 @@ func TestMoveCallInputs(t *testing.T) { defer closeFunc(ctx) req := motion.MoveOnMapReq{ - ComponentName: base.Named("test-base"), - SlamName: slam.Named("test_slam"), + ComponentName: "test-base", + SlamName: "test_slam", MotionCfg: &motion.MotionConfiguration{PlanDeviationMM: 10}, } @@ -63,32 +58,15 @@ func TestMoveCallInputs(t *testing.T) { test.That(t, executionID, test.ShouldResemble, uuid.Nil) }) - t.Run("Returns an error if the base provided is not a base", func(t *testing.T) { - t.Parallel() - _, ms, closeFunc := CreateMoveOnMapTestEnvironment(ctx, t, "pointcloud/octagonspace.pcd", 40, nil) - defer closeFunc(ctx) - - req := motion.MoveOnMapReq{ - ComponentName: slam.Named("test_slam"), - Destination: goalPose, - SlamName: slam.Named("test_slam"), - MotionCfg: &motion.MotionConfiguration{PlanDeviationMM: 10}, - } - - executionID, err := ms.(*builtIn).MoveOnMap(context.Background(), req) - test.That(t, err, test.ShouldBeError, errors.New("Resource missing from dependencies. Resource: rdk:service:slam/test_slam")) - test.That(t, executionID, test.ShouldResemble, uuid.Nil) - }) - t.Run("Returns an error if the slamName provided is not SLAM", func(t *testing.T) { t.Parallel() _, ms, closeFunc := CreateMoveOnMapTestEnvironment(ctx, t, "pointcloud/octagonspace.pcd", 40, nil) defer closeFunc(ctx) req := motion.MoveOnMapReq{ - ComponentName: slam.Named("test-base"), + ComponentName: "test-base", Destination: goalPose, - SlamName: slam.Named("test-base"), + SlamName: "test-base", MotionCfg: &motion.MotionConfiguration{PlanDeviationMM: 10}, } @@ -103,9 +81,9 @@ func TestMoveCallInputs(t *testing.T) { defer closeFunc(ctx) req := motion.MoveOnMapReq{ - ComponentName: base.Named("test-base"), + ComponentName: "test-base", Destination: goalPose, - SlamName: slam.Named("test_slam"), + SlamName: "test_slam", MotionCfg: &motion.MotionConfiguration{PlanDeviationMM: -1}, } @@ -120,9 +98,9 @@ func TestMoveCallInputs(t *testing.T) { defer closeFunc(ctx) req := motion.MoveOnMapReq{ - ComponentName: base.Named("test-base"), + ComponentName: "test-base", Destination: goalPose, - SlamName: slam.Named("test_slam"), + SlamName: "test_slam", MotionCfg: &motion.MotionConfiguration{PlanDeviationMM: math.NaN()}, } @@ -137,9 +115,9 @@ func TestMoveCallInputs(t *testing.T) { defer closeFunc(ctx) pollingFreq := -1. req := motion.MoveOnMapReq{ - ComponentName: base.Named("test-base"), + ComponentName: "test-base", Destination: goalPose, - SlamName: slam.Named("test_slam"), + SlamName: "test_slam", MotionCfg: &motion.MotionConfiguration{ObstaclePollingFreqHz: &pollingFreq, PlanDeviationMM: 10}, } @@ -154,9 +132,9 @@ func TestMoveCallInputs(t *testing.T) { defer closeFunc(ctx) pollingFreq := math.NaN() req := motion.MoveOnMapReq{ - ComponentName: base.Named("test-base"), + ComponentName: "test-base", Destination: goalPose, - SlamName: slam.Named("test_slam"), + SlamName: "test_slam", MotionCfg: &motion.MotionConfiguration{ObstaclePollingFreqHz: &pollingFreq, PlanDeviationMM: 10}, } @@ -171,9 +149,9 @@ func TestMoveCallInputs(t *testing.T) { defer closeFunc(ctx) pollingFreq := -1. req := motion.MoveOnMapReq{ - ComponentName: base.Named("test-base"), + ComponentName: "test-base", Destination: goalPose, - SlamName: slam.Named("test_slam"), + SlamName: "test_slam", MotionCfg: &motion.MotionConfiguration{PositionPollingFreqHz: &pollingFreq, PlanDeviationMM: 10}, } @@ -188,9 +166,9 @@ func TestMoveCallInputs(t *testing.T) { defer closeFunc(ctx) pollingFreq := math.NaN() req := motion.MoveOnMapReq{ - ComponentName: base.Named("test-base"), + ComponentName: "test-base", Destination: goalPose, - SlamName: slam.Named("test_slam"), + SlamName: "test_slam", MotionCfg: &motion.MotionConfiguration{PositionPollingFreqHz: &pollingFreq, PlanDeviationMM: 10}, } @@ -205,9 +183,9 @@ func TestMoveCallInputs(t *testing.T) { defer closeFunc(ctx) req := motion.MoveOnMapReq{ - ComponentName: base.Named("test-base"), + ComponentName: "test-base", Destination: goalPose, - SlamName: slam.Named("test_slam"), + SlamName: "test_slam", MotionCfg: &motion.MotionConfiguration{AngularDegsPerSec: -1, PlanDeviationMM: 10}, } @@ -222,9 +200,9 @@ func TestMoveCallInputs(t *testing.T) { defer closeFunc(ctx) req := motion.MoveOnMapReq{ - ComponentName: base.Named("test-base"), + ComponentName: "test-base", Destination: goalPose, - SlamName: slam.Named("test_slam"), + SlamName: "test_slam", MotionCfg: &motion.MotionConfiguration{AngularDegsPerSec: math.NaN(), PlanDeviationMM: 10}, } @@ -239,9 +217,9 @@ func TestMoveCallInputs(t *testing.T) { defer closeFunc(ctx) req := motion.MoveOnMapReq{ - ComponentName: base.Named("test-base"), + ComponentName: "test-base", Destination: goalPose, - SlamName: slam.Named("test_slam"), + SlamName: "test_slam", MotionCfg: &motion.MotionConfiguration{LinearMPerSec: -1, PlanDeviationMM: 10}, } @@ -256,9 +234,9 @@ func TestMoveCallInputs(t *testing.T) { defer closeFunc(ctx) req := motion.MoveOnMapReq{ - ComponentName: base.Named("test-base"), + ComponentName: "test-base", Destination: goalPose, - SlamName: slam.Named("test_slam"), + SlamName: "test_slam", MotionCfg: &motion.MotionConfiguration{LinearMPerSec: math.NaN(), PlanDeviationMM: 10}, } @@ -273,9 +251,9 @@ func TestMoveCallInputs(t *testing.T) { defer closeFunc(ctx) req := motion.MoveOnMapReq{ - ComponentName: base.Named("test-base"), + ComponentName: "test-base", Destination: goalPose, - SlamName: slam.Named("test_slam"), + SlamName: "test_slam", MotionCfg: &motion.MotionConfiguration{PlanDeviationMM: 10}, Extra: map[string]interface{}{"collision_buffer_mm": "not a float"}, } @@ -293,9 +271,9 @@ func TestMoveCallInputs(t *testing.T) { defer closeFunc(ctx) req := motion.MoveOnMapReq{ - ComponentName: base.Named("test-base"), + ComponentName: "test-base", Destination: goalPose, - SlamName: slam.Named("test_slam"), + SlamName: "test_slam", MotionCfg: &motion.MotionConfiguration{PlanDeviationMM: 10}, Extra: map[string]interface{}{"collision_buffer_mm": -1.}, } @@ -312,9 +290,9 @@ func TestMoveCallInputs(t *testing.T) { defer closeFunc(ctx) req := motion.MoveOnMapReq{ - ComponentName: base.Named("test-base"), + ComponentName: "test-base", Destination: goalPose, - SlamName: slam.Named("test_slam"), + SlamName: "test_slam", MotionCfg: &motion.MotionConfiguration{PlanDeviationMM: 10}, Extra: map[string]interface{}{"collision_buffer_mm": 2000.}, } @@ -332,9 +310,9 @@ func TestMoveCallInputs(t *testing.T) { defer closeFunc(ctx) req := motion.MoveOnMapReq{ - ComponentName: base.Named("test-base"), + ComponentName: "test-base", Destination: goalPose, - SlamName: slam.Named("test_slam"), + SlamName: "test_slam", MotionCfg: &motion.MotionConfiguration{PlanDeviationMM: 10}, Extra: map[string]interface{}{"collision_buffer_mm": 1e-5}, } @@ -351,9 +329,9 @@ func TestMoveCallInputs(t *testing.T) { defer closeFunc(ctx) req := motion.MoveOnMapReq{ - ComponentName: base.Named("test-base"), + ComponentName: "test-base", Destination: goalPose, - SlamName: slam.Named("test_slam"), + SlamName: "test_slam", MotionCfg: &motion.MotionConfiguration{PlanDeviationMM: 10}, Extra: map[string]interface{}{"collision_buffer_mm": 0.1}, } @@ -370,9 +348,9 @@ func TestMoveCallInputs(t *testing.T) { defer closeFunc(ctx) req := motion.MoveOnMapReq{ - ComponentName: base.Named("test-base"), + ComponentName: "test-base", Destination: goalPose, - SlamName: slam.Named("test_slam"), + SlamName: "test_slam", MotionCfg: &motion.MotionConfiguration{PlanDeviationMM: 10}, Extra: map[string]interface{}{}, } @@ -389,9 +367,9 @@ func TestMoveCallInputs(t *testing.T) { defer closeFunc(ctx) req := motion.MoveOnMapReq{ - ComponentName: base.Named("test-base"), + ComponentName: "test-base", Destination: goalPose, - SlamName: slam.Named("test_slam"), + SlamName: "test_slam", MotionCfg: &motion.MotionConfiguration{PlanDeviationMM: 10}, } @@ -414,8 +392,8 @@ func TestMoveCallInputs(t *testing.T) { _, ms, closeFunc := CreateMoveOnGlobeTestEnvironment(ctx, t, gpsPoint, 80, nil) defer closeFunc(ctx) req := motion.MoveOnGlobeReq{ - ComponentName: base.Named("non existent base"), - MovementSensorName: moveSensorResource, + ComponentName: "non existent base", + MovementSensorName: moveSensorName, Destination: geo.NewPoint(0, 0), } executionID, err := ms.MoveOnGlobe(ctx, req) @@ -428,8 +406,8 @@ func TestMoveCallInputs(t *testing.T) { _, ms, closeFunc := CreateMoveOnGlobeTestEnvironment(ctx, t, gpsPoint, 80, nil) defer closeFunc(ctx) req := motion.MoveOnGlobeReq{ - ComponentName: baseResource, - MovementSensorName: movementsensor.Named("non existent movement sensor"), + ComponentName: baseName, + MovementSensorName: "non existent movement sensor", Destination: geo.NewPoint(0, 0), } executionID, err := ms.MoveOnGlobe(ctx, req) @@ -443,8 +421,8 @@ func TestMoveCallInputs(t *testing.T) { _, ms, closeFunc := CreateMoveOnGlobeTestEnvironment(ctx, t, gpsPoint, 80, nil) defer closeFunc(ctx) req := motion.MoveOnGlobeReq{ - ComponentName: baseResource, - MovementSensorName: moveSensorResource, + ComponentName: baseName, + MovementSensorName: moveSensorName, Destination: geo.NewPoint(0, 0), } executionID, err := ms.MoveOnGlobe(ctx, req) @@ -457,8 +435,8 @@ func TestMoveCallInputs(t *testing.T) { _, ms, closeFunc := CreateMoveOnGlobeTestEnvironment(ctx, t, gpsPoint, 80, nil) defer closeFunc(ctx) req := motion.MoveOnGlobeReq{ - ComponentName: baseResource, - MovementSensorName: moveSensorResource, + ComponentName: baseName, + MovementSensorName: moveSensorName, } executionID, err := ms.MoveOnGlobe(ctx, req) test.That(t, err, test.ShouldBeError, errors.New("destination cannot be nil")) @@ -471,8 +449,8 @@ func TestMoveCallInputs(t *testing.T) { defer closeFunc(ctx) req := motion.MoveOnGlobeReq{ - ComponentName: baseResource, - MovementSensorName: moveSensorResource, + ComponentName: baseName, + MovementSensorName: moveSensorName, } dests := []*geo.Point{ @@ -489,51 +467,13 @@ func TestMoveCallInputs(t *testing.T) { } }) - t.Run("returns an error if the base provided is not a base", func(t *testing.T) { - t.Parallel() - _, ms, closeFunc := CreateMoveOnGlobeTestEnvironment(ctx, t, gpsPoint, 80, nil) - defer closeFunc(ctx) - req := motion.MoveOnGlobeReq{ - ComponentName: moveSensorResource, - MovementSensorName: moveSensorResource, - Heading: 90, - Destination: dst, - Extra: map[string]interface{}{ - "timeout": 5., - "smooth_iter": 5., - }, - } - executionID, err := ms.MoveOnGlobe(ctx, req) - test.That(t, err, test.ShouldBeError, errors.New("resource \"rdk:component:movement_sensor/test-movement-sensor\" not found")) - test.That(t, executionID, test.ShouldResemble, uuid.Nil) - }) - - t.Run("returns an error if the movement_sensor provided is not a movement_sensor", func(t *testing.T) { - t.Parallel() - _, ms, closeFunc := CreateMoveOnGlobeTestEnvironment(ctx, t, gpsPoint, 80, nil) - defer closeFunc(ctx) - req := motion.MoveOnGlobeReq{ - ComponentName: baseResource, - MovementSensorName: baseResource, - Heading: 90, - Destination: dst, - Extra: map[string]interface{}{ - "timeout": 5., - "smooth_iter": 5., - }, - } - executionID, err := ms.MoveOnGlobe(ctx, req) - test.That(t, err, test.ShouldBeError, errors.New("Resource missing from dependencies. Resource: rdk:component:base/test-base")) - test.That(t, executionID, test.ShouldResemble, uuid.Nil) - }) - t.Run("errors when motion configuration has a negative PlanDeviationMM", func(t *testing.T) { t.Parallel() _, ms, closeFunc := CreateMoveOnGlobeTestEnvironment(ctx, t, gpsPoint, 80, nil) defer closeFunc(ctx) req := motion.MoveOnGlobeReq{ - ComponentName: baseResource, - MovementSensorName: moveSensorResource, + ComponentName: baseName, + MovementSensorName: moveSensorName, Heading: 90, Destination: dst, MotionCfg: &motion.MotionConfiguration{PlanDeviationMM: -1}, @@ -548,8 +488,8 @@ func TestMoveCallInputs(t *testing.T) { _, ms, closeFunc := CreateMoveOnGlobeTestEnvironment(ctx, t, gpsPoint, 80, nil) defer closeFunc(ctx) req := motion.MoveOnGlobeReq{ - ComponentName: baseResource, - MovementSensorName: moveSensorResource, + ComponentName: baseName, + MovementSensorName: moveSensorName, Heading: 90, Destination: dst, MotionCfg: &motion.MotionConfiguration{PlanDeviationMM: math.NaN()}, @@ -565,8 +505,8 @@ func TestMoveCallInputs(t *testing.T) { defer closeFunc(ctx) pollingFreq := -1. req := motion.MoveOnGlobeReq{ - ComponentName: baseResource, - MovementSensorName: moveSensorResource, + ComponentName: baseName, + MovementSensorName: moveSensorName, Heading: 90, Destination: dst, MotionCfg: &motion.MotionConfiguration{ObstaclePollingFreqHz: &pollingFreq}, @@ -582,8 +522,8 @@ func TestMoveCallInputs(t *testing.T) { defer closeFunc(ctx) pollingFreq := math.NaN() req := motion.MoveOnGlobeReq{ - ComponentName: baseResource, - MovementSensorName: moveSensorResource, + ComponentName: baseName, + MovementSensorName: moveSensorName, Heading: 90, Destination: dst, MotionCfg: &motion.MotionConfiguration{ObstaclePollingFreqHz: &pollingFreq}, @@ -599,8 +539,8 @@ func TestMoveCallInputs(t *testing.T) { defer closeFunc(ctx) pollingFreq := -1. req := motion.MoveOnGlobeReq{ - ComponentName: baseResource, - MovementSensorName: moveSensorResource, + ComponentName: baseName, + MovementSensorName: moveSensorName, Heading: 90, Destination: dst, MotionCfg: &motion.MotionConfiguration{PositionPollingFreqHz: &pollingFreq}, @@ -616,8 +556,8 @@ func TestMoveCallInputs(t *testing.T) { defer closeFunc(ctx) pollingFreq := math.NaN() req := motion.MoveOnGlobeReq{ - ComponentName: baseResource, - MovementSensorName: moveSensorResource, + ComponentName: baseName, + MovementSensorName: moveSensorName, Heading: 90, Destination: dst, MotionCfg: &motion.MotionConfiguration{PositionPollingFreqHz: &pollingFreq}, @@ -632,8 +572,8 @@ func TestMoveCallInputs(t *testing.T) { _, ms, closeFunc := CreateMoveOnGlobeTestEnvironment(ctx, t, gpsPoint, 80, nil) defer closeFunc(ctx) req := motion.MoveOnGlobeReq{ - ComponentName: baseResource, - MovementSensorName: moveSensorResource, + ComponentName: baseName, + MovementSensorName: moveSensorName, Heading: 90, Destination: dst, MotionCfg: &motion.MotionConfiguration{AngularDegsPerSec: -1}, @@ -648,8 +588,8 @@ func TestMoveCallInputs(t *testing.T) { _, ms, closeFunc := CreateMoveOnGlobeTestEnvironment(ctx, t, gpsPoint, 80, nil) defer closeFunc(ctx) req := motion.MoveOnGlobeReq{ - ComponentName: baseResource, - MovementSensorName: moveSensorResource, + ComponentName: baseName, + MovementSensorName: moveSensorName, Heading: 90, Destination: dst, MotionCfg: &motion.MotionConfiguration{AngularDegsPerSec: math.NaN()}, @@ -664,8 +604,8 @@ func TestMoveCallInputs(t *testing.T) { _, ms, closeFunc := CreateMoveOnGlobeTestEnvironment(ctx, t, gpsPoint, 80, nil) defer closeFunc(ctx) req := motion.MoveOnGlobeReq{ - ComponentName: baseResource, - MovementSensorName: moveSensorResource, + ComponentName: baseName, + MovementSensorName: moveSensorName, Heading: 90, Destination: dst, MotionCfg: &motion.MotionConfiguration{LinearMPerSec: -1}, @@ -680,8 +620,8 @@ func TestMoveCallInputs(t *testing.T) { _, ms, closeFunc := CreateMoveOnGlobeTestEnvironment(ctx, t, gpsPoint, 80, nil) defer closeFunc(ctx) req := motion.MoveOnGlobeReq{ - ComponentName: baseResource, - MovementSensorName: moveSensorResource, + ComponentName: baseName, + MovementSensorName: moveSensorName, Heading: 90, Destination: dst, MotionCfg: &motion.MotionConfiguration{LinearMPerSec: math.NaN()}, @@ -696,8 +636,8 @@ func TestMoveCallInputs(t *testing.T) { _, ms, closeFunc := CreateMoveOnGlobeTestEnvironment(ctx, t, gpsPoint, 80, nil) defer closeFunc(ctx) req := motion.MoveOnGlobeReq{ - ComponentName: baseResource, - MovementSensorName: moveSensorResource, + ComponentName: baseName, + MovementSensorName: moveSensorName, Heading: 90, Destination: dst, MotionCfg: &motion.MotionConfiguration{}, @@ -713,8 +653,8 @@ func TestMoveCallInputs(t *testing.T) { _, ms, closeFunc := CreateMoveOnGlobeTestEnvironment(ctx, t, gpsPoint, 80, nil) defer closeFunc(ctx) req := motion.MoveOnGlobeReq{ - ComponentName: baseResource, - MovementSensorName: moveSensorResource, + ComponentName: baseName, + MovementSensorName: moveSensorName, Heading: 90, Destination: dst, MotionCfg: &motion.MotionConfiguration{}, @@ -729,8 +669,8 @@ func TestMoveCallInputs(t *testing.T) { _, ms, closeFunc := CreateMoveOnGlobeTestEnvironment(ctx, t, gpsPoint, 80, nil) defer closeFunc(ctx) req := motion.MoveOnGlobeReq{ - ComponentName: baseResource, - MovementSensorName: moveSensorResource, + ComponentName: baseName, + MovementSensorName: moveSensorName, Heading: 90, Destination: dst, MotionCfg: &motion.MotionConfiguration{}, @@ -745,8 +685,8 @@ func TestMoveCallInputs(t *testing.T) { _, ms, closeFunc := CreateMoveOnGlobeTestEnvironment(ctx, t, gpsPoint, 80, nil) defer closeFunc(ctx) req := motion.MoveOnGlobeReq{ - ComponentName: baseResource, - MovementSensorName: moveSensorResource, + ComponentName: baseName, + MovementSensorName: moveSensorName, Heading: 90, Destination: dst, MotionCfg: &motion.MotionConfiguration{}, @@ -761,8 +701,8 @@ func TestMoveCallInputs(t *testing.T) { _, ms, closeFunc := CreateMoveOnGlobeTestEnvironment(ctx, t, gpsPoint, 80, nil) defer closeFunc(ctx) req := motion.MoveOnGlobeReq{ - ComponentName: baseResource, - MovementSensorName: moveSensorResource, + ComponentName: baseName, + MovementSensorName: moveSensorName, Heading: 90, Destination: dst, MotionCfg: &motion.MotionConfiguration{}, @@ -777,8 +717,8 @@ func TestMoveCallInputs(t *testing.T) { _, ms, closeFunc := CreateMoveOnGlobeTestEnvironment(ctx, t, gpsPoint, 80, nil) defer closeFunc(ctx) req := motion.MoveOnGlobeReq{ - ComponentName: baseResource, - MovementSensorName: moveSensorResource, + ComponentName: baseName, + MovementSensorName: moveSensorName, Heading: 90, Destination: dst, MotionCfg: &motion.MotionConfiguration{}, @@ -841,8 +781,8 @@ func TestNewValidatedMotionCfg(t *testing.T) { ObstaclePollingFreqHz: &pollingFreq, ObstacleDetectors: []motion.ObstacleDetectorName{ { - VisionServiceName: vision.Named("fakeVision"), - CameraName: camera.Named("fakeCamera"), + VisionServiceName: "fakeVision", + CameraName: "fakeCamera", }, }, }, requestTypeMoveOnMap) @@ -855,8 +795,8 @@ func TestNewValidatedMotionCfg(t *testing.T) { obstaclePollingFreqHz: pollingFreq, obstacleDetectors: []motion.ObstacleDetectorName{ { - VisionServiceName: vision.Named("fakeVision"), - CameraName: camera.Named("fakeCamera"), + VisionServiceName: "fakeVision", + CameraName: "fakeCamera", }, }, }) diff --git a/services/motion/client.go b/services/motion/client.go index 88e13f3bea6..80963226556 100644 --- a/services/motion/client.go +++ b/services/motion/client.go @@ -97,7 +97,7 @@ func (c *client) MoveOnGlobe( func (c *client) GetPose( ctx context.Context, - componentName resource.Name, + componentName string, destinationFrame string, supplementalTransforms []*referenceframe.LinkInFrame, extra map[string]interface{}, @@ -113,7 +113,7 @@ func (c *client) GetPose( //nolint:staticcheck resp, err := c.client.GetPose(ctx, &pb.GetPoseRequest{ Name: c.name, - ComponentName: protoutils.ResourceNameToProto(componentName), + ComponentName: componentName, DestinationFrame: destinationFrame, SupplementalTransforms: transforms, Extra: ext, @@ -131,7 +131,7 @@ func (c *client) StopPlan(ctx context.Context, req StopPlanReq) error { } _, err = c.client.StopPlan(ctx, &pb.StopPlanRequest{ Name: c.name, - ComponentName: protoutils.ResourceNameToProto(req.ComponentName), + ComponentName: req.ComponentName, Extra: ext, }) return err diff --git a/services/motion/client_test.go b/services/motion/client_test.go index 6d7dfd5cd65..6870e41a9ba 100644 --- a/services/motion/client_test.go +++ b/services/motion/client_test.go @@ -14,17 +14,12 @@ import ( "go.viam.com/test" "go.viam.com/utils/rpc" - "go.viam.com/rdk/components/arm" - "go.viam.com/rdk/components/base" - "go.viam.com/rdk/components/gripper" - "go.viam.com/rdk/components/movementsensor" viamgrpc "go.viam.com/rdk/grpc" "go.viam.com/rdk/logging" "go.viam.com/rdk/motionplan" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" "go.viam.com/rdk/services/motion" - "go.viam.com/rdk/services/slam" "go.viam.com/rdk/spatialmath" "go.viam.com/rdk/testutils" injectmotion "go.viam.com/rdk/testutils/inject/motion" @@ -64,10 +59,10 @@ func TestClient(t *testing.T) { zeroPoseInFrame := referenceframe.NewPoseInFrame(referenceframe.World, spatialmath.NewZeroPose()) globeDest := geo.NewPoint(0.0, 0.0) - gripperName := gripper.Named("fake") - baseName := base.Named("test-base") - gpsName := movementsensor.Named("test-gps") - slamName := slam.Named("test-slam") + gripperName := "fake" + baseName := "test-base" + gpsName := "test-gps" + slamName := "test-slam" // failing t.Run("Failing client", func(t *testing.T) { @@ -99,7 +94,7 @@ func TestClient(t *testing.T) { } injectMS.GetPoseFunc = func( ctx context.Context, - componentName resource.Name, + componentName string, destinationFrame string, supplementalTransforms []*referenceframe.LinkInFrame, extra map[string]interface{}, @@ -108,7 +103,7 @@ func TestClient(t *testing.T) { receivedTransforms[tf.Name()] = tf } return referenceframe.NewPoseInFrame( - destinationFrame+componentName.Name, spatialmath.NewPoseFromPoint(r3.Vector{X: 1, Y: 2, Z: 3})), nil + destinationFrame+componentName, spatialmath.NewPoseFromPoint(r3.Vector{X: 1, Y: 2, Z: 3})), nil } // Move @@ -126,7 +121,7 @@ func TestClient(t *testing.T) { for _, tf := range transforms { tfMap[tf.Name()] = tf } - poseResult, err := client.GetPose(context.Background(), arm.Named("arm1"), "foo", transforms, map[string]interface{}{}) + poseResult, err := client.GetPose(context.Background(), "arm1", "foo", transforms, map[string]interface{}{}) test.That(t, err, test.ShouldBeNil) test.That(t, poseResult.Parent(), test.ShouldEqual, "fooarm1") test.That(t, poseResult.Pose().Point().X, test.ShouldEqual, 1) @@ -165,7 +160,7 @@ func TestClient(t *testing.T) { passedErr = errors.New("fake GetPose error") injectMS.GetPoseFunc = func( ctx context.Context, - componentName resource.Name, + componentName string, destinationFrame string, supplementalTransform []*referenceframe.LinkInFrame, extra map[string]interface{}, @@ -179,7 +174,7 @@ func TestClient(t *testing.T) { test.That(t, resp, test.ShouldEqual, false) // GetPose - _, err = client2.GetPose(context.Background(), arm.Named("arm1"), "foo", nil, map[string]interface{}{}) + _, err = client2.GetPose(context.Background(), "arm1", "foo", nil, map[string]interface{}{}) test.That(t, err.Error(), test.ShouldContainSubstring, passedErr.Error()) test.That(t, client2.Close(context.Background()), test.ShouldBeNil) test.That(t, conn.Close(), test.ShouldBeNil) @@ -361,7 +356,7 @@ func TestClient(t *testing.T) { return errExpected } - req := motion.StopPlanReq{ComponentName: base.Named("mybase")} + req := motion.StopPlanReq{ComponentName: "mybase"} err := client.StopPlan(ctx, req) test.That(t, err, test.ShouldNotBeNil) test.That(t, err.Error(), test.ShouldContainSubstring, errExpected.Error()) @@ -375,7 +370,7 @@ func TestClient(t *testing.T) { return nil } - req := motion.StopPlanReq{ComponentName: base.Named("mybase")} + req := motion.StopPlanReq{ComponentName: "mybase"} err := client.StopPlan(ctx, req) test.That(t, err, test.ShouldBeNil) }) @@ -414,7 +409,7 @@ func TestClient(t *testing.T) { status := motion.PlanStatus{State: motion.PlanStateInProgress, Timestamp: time.Now().UTC(), Reason: nil} expectedResp := []motion.PlanStatusWithID{ - {PlanID: planID, ComponentName: base.Named("mybase"), ExecutionID: executionID, Status: status}, + {PlanID: planID, ComponentName: "mybase", ExecutionID: executionID, Status: status}, } injectMS.ListPlanStatusesFunc = func( @@ -446,8 +441,8 @@ func TestClient(t *testing.T) { statusB := motion.PlanStatus{State: motion.PlanStateInProgress, Timestamp: time.Now().UTC(), Reason: &reason} expectedResp := []motion.PlanStatusWithID{ - {PlanID: planIDA, ComponentName: base.Named("mybase"), ExecutionID: executionIDA, Status: statusA}, - {PlanID: planIDB, ComponentName: base.Named("mybase"), ExecutionID: executionIDB, Status: statusB}, + {PlanID: planIDA, ComponentName: "mybase", ExecutionID: executionIDA, Status: statusA}, + {PlanID: planIDB, ComponentName: "mybase", ExecutionID: executionIDB, Status: statusB}, } injectMS.ListPlanStatusesFunc = func( @@ -480,7 +475,7 @@ func TestClient(t *testing.T) { return nil, errExpected } - req := motion.PlanHistoryReq{ComponentName: base.Named("mybase")} + req := motion.PlanHistoryReq{ComponentName: "mybase"} resp, err := client.PlanHistory(ctx, req) test.That(t, err, test.ShouldNotBeNil) test.That(t, err.Error(), test.ShouldContainSubstring, errExpected.Error()) @@ -498,7 +493,7 @@ func TestClient(t *testing.T) { plan := motion.PlanWithMetadata{ ID: id, - ComponentName: base.Named("mybase"), + ComponentName: "mybase", ExecutionID: executionID, Plan: motionplan.NewSimplePlan(steps, nil), } @@ -511,7 +506,7 @@ func TestClient(t *testing.T) { return expectedResp, nil } - req := motion.PlanHistoryReq{ComponentName: base.Named("mybase")} + req := motion.PlanHistoryReq{ComponentName: "mybase"} resp, err := client.PlanHistory(ctx, req) test.That(t, err, test.ShouldBeNil) planHistoriesEqual(t, resp, expectedResp) @@ -532,7 +527,7 @@ func TestClient(t *testing.T) { planA := motion.PlanWithMetadata{ ID: idA, - ComponentName: base.Named("mybase"), + ComponentName: "mybase", ExecutionID: executionID, Plan: motionplan.NewSimplePlan(steps, nil), } @@ -546,7 +541,7 @@ func TestClient(t *testing.T) { timeBA := time.Now().UTC() planB := motion.PlanWithMetadata{ ID: idB, - ComponentName: base.Named("mybase"), + ComponentName: "mybase", ExecutionID: executionID, Plan: motionplan.NewSimplePlan(steps, nil), } @@ -564,7 +559,7 @@ func TestClient(t *testing.T) { return expectedResp, nil } - req := motion.PlanHistoryReq{ComponentName: base.Named("mybase")} + req := motion.PlanHistoryReq{ComponentName: "mybase"} resp, err := client.PlanHistory(ctx, req) test.That(t, err, test.ShouldBeNil) planHistoriesEqual(t, resp, expectedResp) diff --git a/services/motion/motion.go b/services/motion/motion.go index 067a07448f1..af4dcb7de37 100644 --- a/services/motion/motion.go +++ b/services/motion/motion.go @@ -16,7 +16,6 @@ import ( "go.viam.com/rdk/data" "go.viam.com/rdk/motionplan" - rprotoutils "go.viam.com/rdk/protoutils" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" "go.viam.com/rdk/robot" @@ -39,7 +38,7 @@ func init() { // PlanHistoryReq describes the request to PlanHistory(). type PlanHistoryReq struct { // ComponentName the returned plans should be associated with. - ComponentName resource.Name + ComponentName string // When true, only the most recent plan will be returned which matches the ComponentName & ExecutionID if one was provided. LastPlanOnly bool // Optional, when not uuid.Nil it specifies the ExecutionID of the plans that should be returned. @@ -51,7 +50,7 @@ type PlanHistoryReq struct { // MoveReq describes the request to the Move interface method. type MoveReq struct { // ComponentName of the component to move - ComponentName resource.Name + ComponentName string // Goal destination the component should be moved to Destination *referenceframe.PoseInFrame // The external environment to be considered for the duration of the move @@ -64,14 +63,14 @@ type MoveReq struct { // MoveOnGlobeReq describes the request to the MoveOnGlobe interface method. type MoveOnGlobeReq struct { // ComponentName of the component to move - ComponentName resource.Name + ComponentName string // Goal destination the component should be moved to Destination *geo.Point // Heading the component should have a when it reaches the goal. // Range [0-360] Left Hand Rule (N: 0, E: 90, S: 180, W: 270) Heading float64 // Name of the momement sensor which can be used to derive Position & Heading - MovementSensorName resource.Name + MovementSensorName string // Static obstacles that should be navigated around Obstacles []*spatialmath.GeoGeometry // Set of bounds which the robot must remain within while navigating @@ -98,9 +97,9 @@ func (r MoveOnGlobeReq) String() string { // MoveOnMapReq describes a request to MoveOnMap. type MoveOnMapReq struct { - ComponentName resource.Name + ComponentName string Destination spatialmath.Pose - SlamName resource.Name + SlamName string MotionCfg *MotionConfiguration Obstacles []spatialmath.Geometry Extra map[string]interface{} @@ -121,7 +120,7 @@ func (r MoveOnMapReq) String() string { // StopPlanReq describes the request to StopPlan(). type StopPlanReq struct { // ComponentName of the plan which should be stopped - ComponentName resource.Name + ComponentName string Extra map[string]interface{} } @@ -137,7 +136,7 @@ type PlanWithMetadata struct { // Unique ID of the plan ID PlanID // Name of the component the plan is planning for - ComponentName resource.Name + ComponentName string // Unique ID of the execution ExecutionID ExecutionID // The motionplan itself @@ -185,7 +184,7 @@ type ExecutionID = uuid.UUID // the status is associated with. type PlanStatusWithID struct { PlanID PlanID - ComponentName resource.Name + ComponentName string ExecutionID ExecutionID Status PlanStatus } @@ -379,7 +378,7 @@ type Service interface { // deprecated, use framesystem.Servce.GetPose GetPose( ctx context.Context, - componentName resource.Name, + componentName string, destinationFrame string, supplementalTransforms []*referenceframe.LinkInFrame, extra map[string]interface{}, @@ -402,8 +401,8 @@ type Service interface { // ObstacleDetectorName pairs a vision service name with a camera name. type ObstacleDetectorName struct { - VisionServiceName resource.Name - CameraName resource.Name + VisionServiceName string + CameraName string } // MotionConfiguration specifies how to configure a call. @@ -463,7 +462,7 @@ func (pws PlanWithStatus) ToProto() *pb.PlanWithStatus { func (ps PlanStatusWithID) ToProto() *pb.PlanStatusWithID { return &pb.PlanStatusWithID{ PlanId: ps.PlanID.String(), - ComponentName: rprotoutils.ResourceNameToProto(ps.ComponentName), + ComponentName: ps.ComponentName, ExecutionId: ps.ExecutionID.String(), Status: ps.Status.ToProto(), } @@ -489,7 +488,7 @@ func (p PlanWithMetadata) ToProto() *pb.Plan { return &pb.Plan{ Id: p.ID.String(), - ComponentName: rprotoutils.ResourceNameToProto(p.ComponentName), + ComponentName: p.ComponentName, ExecutionId: p.ExecutionID.String(), Steps: steps, } diff --git a/services/motion/motion_configuration.go b/services/motion/motion_configuration.go index aad40f2545f..55c4c6d68a3 100644 --- a/services/motion/motion_configuration.go +++ b/services/motion/motion_configuration.go @@ -4,8 +4,6 @@ import ( "math" pb "go.viam.com/api/service/motion/v1" - - "go.viam.com/rdk/protoutils" ) func configurationFromProto(motionCfg *pb.MotionConfiguration) *MotionConfiguration { @@ -19,8 +17,8 @@ func configurationFromProto(motionCfg *pb.MotionConfiguration) *MotionConfigurat if motionCfg.ObstacleDetectors != nil { for _, obstacleDetectorPair := range motionCfg.GetObstacleDetectors() { obstacleDetectors = append(obstacleDetectors, ObstacleDetectorName{ - VisionServiceName: protoutils.ResourceNameFromProto(obstacleDetectorPair.VisionService), - CameraName: protoutils.ResourceNameFromProto(obstacleDetectorPair.Camera), + VisionServiceName: obstacleDetectorPair.VisionService, + CameraName: obstacleDetectorPair.Camera, }) } } @@ -71,8 +69,8 @@ func (motionCfg MotionConfiguration) toProto() *pb.MotionConfiguration { pbObstacleDetector := []*pb.ObstacleDetector{} for _, obstacleDetectorPair := range motionCfg.ObstacleDetectors { pbObstacleDetector = append(pbObstacleDetector, &pb.ObstacleDetector{ - VisionService: protoutils.ResourceNameToProto(obstacleDetectorPair.VisionServiceName), - Camera: protoutils.ResourceNameToProto(obstacleDetectorPair.CameraName), + VisionService: obstacleDetectorPair.VisionServiceName, + Camera: obstacleDetectorPair.CameraName, }) } proto.ObstacleDetectors = pbObstacleDetector diff --git a/services/motion/motion_test.go b/services/motion/motion_test.go index cb8320d0c0a..9bb324a7806 100644 --- a/services/motion/motion_test.go +++ b/services/motion/motion_test.go @@ -16,15 +16,8 @@ import ( "google.golang.org/protobuf/types/known/structpb" "google.golang.org/protobuf/types/known/timestamppb" - "go.viam.com/rdk/components/base" - "go.viam.com/rdk/components/camera" - "go.viam.com/rdk/components/movementsensor" "go.viam.com/rdk/motionplan" - rprotoutils "go.viam.com/rdk/protoutils" "go.viam.com/rdk/referenceframe" - "go.viam.com/rdk/resource" - "go.viam.com/rdk/services/slam" - "go.viam.com/rdk/services/vision" "go.viam.com/rdk/spatialmath" ) @@ -36,7 +29,7 @@ func TestPlanWithStatus(t *testing.T) { planID := uuid.New() executionID := uuid.New() - baseName := base.Named("my-base1") + baseName := "my-base1" poseA := spatialmath.NewZeroPose() poseB := spatialmath.NewPose(r3.Vector{X: 100}, spatialmath.NewOrientationVector()) @@ -50,8 +43,8 @@ func TestPlanWithStatus(t *testing.T) { ComponentName: baseName, Plan: motionplan.NewSimplePlan( []referenceframe.FrameSystemPoses{ - {baseName.ShortName(): referenceframe.NewPoseInFrame(referenceframe.World, poseA)}, - {baseName.ShortName(): referenceframe.NewPoseInFrame(referenceframe.World, poseB)}, + {baseName: referenceframe.NewPoseInFrame(referenceframe.World, poseA)}, + {baseName: referenceframe.NewPoseInFrame(referenceframe.World, poseB)}, }, nil, ), @@ -60,16 +53,16 @@ func TestPlanWithStatus(t *testing.T) { protoPlan := &pb.Plan{ Id: planID.String(), ExecutionId: executionID.String(), - ComponentName: rprotoutils.ResourceNameToProto(baseName), + ComponentName: baseName, Steps: []*pb.PlanStep{ { Step: map[string]*pb.ComponentState{ - baseName.ShortName(): {Pose: spatialmath.PoseToProtobuf(poseA)}, + baseName: {Pose: spatialmath.PoseToProtobuf(poseA)}, }, }, { Step: map[string]*pb.ComponentState{ - baseName.ShortName(): {Pose: spatialmath.PoseToProtobuf(poseB)}, + baseName: {Pose: spatialmath.PoseToProtobuf(poseB)}, }, }, }, @@ -98,28 +91,36 @@ func TestPlanWithStatus(t *testing.T) { }, { description: "empty status returns an error", - input: &pb.PlanWithStatus{Plan: PlanWithMetadata{}.ToProto()}, - result: PlanWithStatus{}, - err: errors.New("received nil *pb.PlanStatus"), + input : &pb.PlanWithStatus{ + Plan: PlanWithMetadata{ + ComponentName: "test-component", + }.ToProto(), + }, + result: PlanWithStatus{}, + err: errors.New("received nil *pb.PlanStatus"), }, { description: "nil pointers in the status history returns an error", - input: &pb.PlanWithStatus{ - Plan: PlanWithMetadata{}.ToProto(), - Status: PlanStatus{}.ToProto(), - StatusHistory: []*pb.PlanStatus{nil}, + input : &pb.PlanWithStatus{ + Plan: PlanWithMetadata{ + ComponentName: "test-component", + }.ToProto(), }, result: PlanWithStatus{}, err: errors.New("received nil *pb.PlanStatus"), }, { description: "empty *pb.PlanWithStatus status returns an empty PlanWithStatus", - input: &pb.PlanWithStatus{ - Plan: PlanWithMetadata{}.ToProto(), + input : &pb.PlanWithStatus{ + Plan: PlanWithMetadata{ + ComponentName: "test-component", + }.ToProto(), Status: PlanStatus{}.ToProto(), }, result: PlanWithStatus{ - Plan: PlanWithMetadata{}, + Plan: PlanWithMetadata{ + ComponentName: "test-component", + }, StatusHistory: []PlanStatus{{}}, }, }, @@ -148,8 +149,8 @@ func TestPlanWithStatus(t *testing.T) { }, }, } - for _, tc := range testCases { - t.Run(tc.description, func(t *testing.T) { + for i, tc := range testCases { + t.Run(fmt.Sprintf("TestCase #%d: %s", i+1, tc.description), func(t *testing.T) { res, err := planWithStatusFromProto(tc.input) if tc.err != nil { test.That(t, err, test.ShouldBeError, tc.err) @@ -297,7 +298,7 @@ func TestPlanStatusWithID(t *testing.T) { id := uuid.New() - mybase := base.Named("mybase") + mybase := "mybase" timestamp := time.Now().UTC() timestampb := timestamppb.New(timestamp) reason := "some reason" @@ -331,12 +332,12 @@ func TestPlanStatusWithID(t *testing.T) { description: "no component name returns error", input: &pb.PlanStatusWithID{PlanId: id.String(), ExecutionId: id.String(), Status: &pb.PlanStatus{}}, result: PlanStatusWithID{}, - err: errors.New("received nil *commonpb.ResourceName"), + err: ErrEmptyComponentName, }, { description: "success case with a failed plan status & reason", input: &pb.PlanStatusWithID{ - ComponentName: rprotoutils.ResourceNameToProto(mybase), + ComponentName: mybase, ExecutionId: id.String(), PlanId: id.String(), Status: &pb.PlanStatus{State: pb.PlanState_PLAN_STATE_FAILED, Timestamp: timestampb, Reason: &reason}, @@ -351,7 +352,7 @@ func TestPlanStatusWithID(t *testing.T) { { description: "success case with a in progress plan status", input: &pb.PlanStatusWithID{ - ComponentName: rprotoutils.ResourceNameToProto(mybase), + ComponentName: mybase, ExecutionId: id.String(), PlanId: id.String(), Status: &pb.PlanStatus{State: pb.PlanState_PLAN_STATE_IN_PROGRESS, Timestamp: timestampb}, @@ -386,7 +387,7 @@ func TestPlanStatusWithID(t *testing.T) { id := uuid.New() - mybase := base.Named("mybase") + mybase := "mybase" timestamp := time.Now().UTC() timestampb := timestamppb.New(timestamp) reason := "some reason" @@ -398,7 +399,7 @@ func TestPlanStatusWithID(t *testing.T) { result: &pb.PlanStatusWithID{ PlanId: uuid.Nil.String(), ExecutionId: uuid.Nil.String(), - ComponentName: rprotoutils.ResourceNameToProto(resource.Name{}), + ComponentName: "", Status: PlanStatus{}.ToProto(), }, }, @@ -411,7 +412,7 @@ func TestPlanStatusWithID(t *testing.T) { Status: PlanStatus{State: PlanStateFailed, Timestamp: timestamp, Reason: &reason}, }, result: &pb.PlanStatusWithID{ - ComponentName: rprotoutils.ResourceNameToProto(mybase), + ComponentName: mybase, ExecutionId: id.String(), PlanId: id.String(), Status: &pb.PlanStatus{State: pb.PlanState_PLAN_STATE_FAILED, Timestamp: timestampb, Reason: &reason}, @@ -426,7 +427,7 @@ func TestPlanStatusWithID(t *testing.T) { Status: PlanStatus{State: PlanStateInProgress, Timestamp: timestamp}, }, result: &pb.PlanStatusWithID{ - ComponentName: rprotoutils.ResourceNameToProto(mybase), + ComponentName: mybase, ExecutionId: id.String(), PlanId: id.String(), Status: &pb.PlanStatus{State: pb.PlanState_PLAN_STATE_IN_PROGRESS, Timestamp: timestampb}, @@ -559,23 +560,23 @@ func TestPlan(t *testing.T) { planID := uuid.New() executionID := uuid.New() - baseName := base.Named("my-base1") + baseName := "my-base1" poseA := spatialmath.NewZeroPose() poseB := spatialmath.NewPose(r3.Vector{X: 100}, spatialmath.NewOrientationVector()) protoAB := &pb.Plan{ Id: planID.String(), ExecutionId: executionID.String(), - ComponentName: rprotoutils.ResourceNameToProto(baseName), + ComponentName: baseName, Steps: []*pb.PlanStep{ { Step: map[string]*pb.ComponentState{ - baseName.ShortName(): {Pose: spatialmath.PoseToProtobuf(poseA)}, + baseName: {Pose: spatialmath.PoseToProtobuf(poseA)}, }, }, { Step: map[string]*pb.ComponentState{ - baseName.ShortName(): {Pose: spatialmath.PoseToProtobuf(poseB)}, + baseName: {Pose: spatialmath.PoseToProtobuf(poseB)}, }, }, }, @@ -586,8 +587,8 @@ func TestPlan(t *testing.T) { ComponentName: baseName, Plan: motionplan.NewSimplePlan( []referenceframe.FrameSystemPoses{ - {baseName.ShortName(): referenceframe.NewPoseInFrame(referenceframe.World, poseA)}, - {baseName.ShortName(): referenceframe.NewPoseInFrame(referenceframe.World, poseB)}, + {baseName: referenceframe.NewPoseInFrame(referenceframe.World, poseA)}, + {baseName: referenceframe.NewPoseInFrame(referenceframe.World, poseB)}, }, nil, ), @@ -624,14 +625,14 @@ func TestPlan(t *testing.T) { description: "empty ComponentName in *pb.Plan{} returns an error", input: &pb.Plan{Id: planID.String(), ExecutionId: executionID.String()}, result: PlanWithMetadata{}, - err: errors.New("received nil *pb.ResourceName"), + err: ErrEmptyComponentName, }, { description: "a nil *pb.PlanStep{} returns an error", input: &pb.Plan{ Id: planID.String(), ExecutionId: executionID.String(), - ComponentName: rprotoutils.ResourceNameToProto(resource.Name{}), + ComponentName: "component_name", Steps: []*pb.PlanStep{nil}, }, result: PlanWithMetadata{}, @@ -642,12 +643,12 @@ func TestPlan(t *testing.T) { input: &pb.Plan{ Id: planID.String(), ExecutionId: executionID.String(), - ComponentName: rprotoutils.ResourceNameToProto(resource.Name{}), + ComponentName: "component_name", }, result: PlanWithMetadata{ ID: planID, ExecutionID: executionID, - ComponentName: resource.Name{}, + ComponentName: "component_name", }, }, { @@ -682,7 +683,7 @@ func TestPlan(t *testing.T) { input: PlanWithMetadata{}, result: &pb.Plan{ Id: uuid.Nil.String(), - ComponentName: rprotoutils.ResourceNameToProto(resource.Name{}), + ComponentName: "", ExecutionId: uuid.Nil.String(), }, }, @@ -703,9 +704,9 @@ func TestPlan(t *testing.T) { } func TestConfiguration(t *testing.T) { - visionCameraPairs := [][]resource.Name{ - {vision.Named("vision service 1"), camera.Named("camera 1")}, - {vision.Named("vision service 2"), camera.Named("camera 2")}, + visionCameraPairs := [][]string{ + {"vision service 1", "camera 1"}, + {"vision service 2", "camera 2"}, } obstacleDetectorsPB := []*pb.ObstacleDetector{} obstacleDetectors := []ObstacleDetectorName{} @@ -715,8 +716,8 @@ func TestConfiguration(t *testing.T) { CameraName: pair[1], }) obstacleDetectorsPB = append(obstacleDetectorsPB, &pb.ObstacleDetector{ - VisionService: rprotoutils.ResourceNameToProto(pair[0]), - Camera: rprotoutils.ResourceNameToProto(pair[1]), + VisionService: pair[0], + Camera: pair[1], }) } @@ -850,12 +851,12 @@ func TestMoveOnGlobeReq(t *testing.T) { test.That(t, err, test.ShouldBeNil) test.That(t, req.Name, test.ShouldResemble, "somename") - test.That(t, req.ComponentName.Name, test.ShouldResemble, "my-base") + test.That(t, req.ComponentName, test.ShouldResemble, "my-base") test.That(t, req.Destination.Latitude, test.ShouldAlmostEqual, dst.Lat()) test.That(t, req.Destination.Longitude, test.ShouldAlmostEqual, dst.Lng()) test.That(t, req.Heading, test.ShouldNotBeNil) test.That(t, *req.Heading, test.ShouldAlmostEqual, 0.5) - test.That(t, req.MovementSensorName.Name, test.ShouldResemble, "my-movementsensor") + test.That(t, req.MovementSensorName, test.ShouldResemble, "my-movementsensor") test.That(t, req.Obstacles, test.ShouldBeEmpty) test.That(t, req.MotionConfiguration, test.ShouldResemble, mogReq.MotionCfg.toProto()) test.That(t, req.Extra.AsMap(), test.ShouldBeEmpty) @@ -867,21 +868,21 @@ func TestMoveOnGlobeReq(t *testing.T) { req, err := mogReq.toProto(name) test.That(t, err, test.ShouldBeNil) test.That(t, req.Name, test.ShouldResemble, "somename") - test.That(t, req.ComponentName.Name, test.ShouldResemble, "my-base") + test.That(t, req.ComponentName, test.ShouldResemble, "my-base") test.That(t, req.Destination.Latitude, test.ShouldAlmostEqual, dst.Lat()) test.That(t, req.Destination.Longitude, test.ShouldAlmostEqual, dst.Lng()) test.That(t, req.Heading, test.ShouldNotBeNil) test.That(t, *req.Heading, test.ShouldAlmostEqual, 0.5) - test.That(t, req.MovementSensorName.Name, test.ShouldResemble, "my-movementsensor") + test.That(t, req.MovementSensorName, test.ShouldResemble, "my-movementsensor") test.That(t, req.Obstacles, test.ShouldBeEmpty) test.That(t, req.MotionConfiguration, test.ShouldBeNil) test.That(t, req.Extra.AsMap(), test.ShouldBeEmpty) }) }) - visionCameraPairs := [][]resource.Name{ - {vision.Named("vision service 1"), camera.Named("camera 1")}, - {vision.Named("vision service 2"), camera.Named("camera 2")}, + visionCameraPairs := [][]string{ + {"vision service 1", "camera 1"}, + {"vision service 2", "camera 2"}, } obstacleDetectorsPB := []*pb.ObstacleDetector{} obstacleDetectors := []ObstacleDetectorName{} @@ -891,8 +892,8 @@ func TestMoveOnGlobeReq(t *testing.T) { CameraName: pair[1], }) obstacleDetectorsPB = append(obstacleDetectorsPB, &pb.ObstacleDetector{ - VisionService: rprotoutils.ResourceNameToProto(pair[0]), - Camera: rprotoutils.ResourceNameToProto(pair[1]), + VisionService: pair[0], + Camera: pair[1], }) } @@ -914,7 +915,7 @@ func TestMoveOnGlobeReq(t *testing.T) { sphere, err := spatialmath.NewSphere(spatialmath.NewZeroPose(), 1, "sphere") test.That(t, err, test.ShouldBeNil) - mybase := base.Named("my-base") + mybase := "my-base" testCases := []testCase{ { @@ -935,30 +936,30 @@ func TestMoveOnGlobeReq(t *testing.T) { Destination: &commonpb.GeoPoint{Latitude: 1, Longitude: 2}, }, result: MoveOnGlobeReq{}, - err: errors.New("received nil *commonpb.ResourceName"), + err: ErrEmptyComponentName, }, { description: "an empty movement sensor name returns an error", input: &pb.MoveOnGlobeRequest{ Destination: &commonpb.GeoPoint{Latitude: 1, Longitude: 2}, - ComponentName: rprotoutils.ResourceNameToProto(mybase), + ComponentName: mybase, }, result: MoveOnGlobeReq{}, - err: errors.New("received nil *commonpb.ResourceName"), + err: ErrEmptyComponentName, }, { description: "an empty *pb.MoveOnGlobeRequest returns an empty MoveOnGlobeReq", input: &pb.MoveOnGlobeRequest{ Heading: &heading, Destination: &commonpb.GeoPoint{Latitude: 1, Longitude: 2}, - ComponentName: rprotoutils.ResourceNameToProto(mybase), - MovementSensorName: rprotoutils.ResourceNameToProto(movementsensor.Named("my-movementsensor")), + ComponentName: mybase, + MovementSensorName: "my-movementsensor", }, result: MoveOnGlobeReq{ Heading: heading, Destination: geo.NewPoint(1, 2), ComponentName: mybase, - MovementSensorName: movementsensor.Named("my-movementsensor"), + MovementSensorName: "my-movementsensor", MotionCfg: &defaultMotionCfg, Obstacles: []*spatialmath.GeoGeometry{}, BoundingRegions: []*spatialmath.GeoGeometry{}, @@ -970,8 +971,8 @@ func TestMoveOnGlobeReq(t *testing.T) { input: &pb.MoveOnGlobeRequest{ Heading: &heading, Destination: &commonpb.GeoPoint{Latitude: 1, Longitude: 2}, - ComponentName: rprotoutils.ResourceNameToProto(mybase), - MovementSensorName: rprotoutils.ResourceNameToProto(movementsensor.Named("my-movementsensor")), + ComponentName: mybase, + MovementSensorName: "my-movementsensor", Obstacles: []*commonpb.GeoGeometry{ { Location: &commonpb.GeoPoint{Latitude: 3, Longitude: 4}, @@ -1017,7 +1018,7 @@ func TestMoveOnGlobeReq(t *testing.T) { Heading: heading, Destination: dst, ComponentName: mybase, - MovementSensorName: movementsensor.Named("my-movementsensor"), + MovementSensorName: "my-movementsensor", BoundingRegions: []*spatialmath.GeoGeometry{ spatialmath.NewGeoGeometry(geo.NewPoint(1, 2), []spatialmath.Geometry{sphere}), }, @@ -1053,8 +1054,8 @@ func TestMoveOnGlobeReq(t *testing.T) { t.Run("nil heading is converted into a NaN heading", func(t *testing.T) { input := &pb.MoveOnGlobeRequest{ Destination: &commonpb.GeoPoint{Latitude: 1, Longitude: 2}, - ComponentName: rprotoutils.ResourceNameToProto(mybase), - MovementSensorName: rprotoutils.ResourceNameToProto(movementsensor.Named("my-movementsensor")), + ComponentName: mybase, + MovementSensorName: "my-movementsensor", } res, err := moveOnGlobeRequestFromProto(input) test.That(t, err, test.ShouldBeNil) @@ -1064,9 +1065,9 @@ func TestMoveOnGlobeReq(t *testing.T) { } func TestMoveOnMapReq(t *testing.T) { - visionCameraPairs := [][]resource.Name{ - {vision.Named("vision service 1"), camera.Named("camera 1")}, - {vision.Named("vision service 2"), camera.Named("camera 2")}, + visionCameraPairs := [][]string{ + {"vision service 1", "camera 1"}, + {"vision service 2", "camera 2"}, } obstacleDetectors := []ObstacleDetectorName{} for _, pair := range visionCameraPairs { @@ -1075,8 +1076,8 @@ func TestMoveOnMapReq(t *testing.T) { CameraName: pair[1], }) } - myBase := base.Named("mybase") - mySlam := slam.Named(("mySlam")) + myBase := "mybase" + mySlam := "mySlam" pollingFreq := 5. motionCfg := &MotionConfiguration{ ObstacleDetectors: obstacleDetectors, @@ -1099,8 +1100,8 @@ func TestMoveOnMapReq(t *testing.T) { validPbMoveOnMapRequest := &pb.MoveOnMapRequest{ Name: "bloop", Destination: spatialmath.PoseToProtobuf(spatialmath.NewZeroPose()), - ComponentName: rprotoutils.ResourceNameToProto(myBase), - SlamServiceName: rprotoutils.ResourceNameToProto(mySlam), + ComponentName: myBase, + SlamServiceName: mySlam, MotionConfiguration: motionCfg.toProto(), Extra: &structpb.Struct{}, } @@ -1154,8 +1155,8 @@ func TestMoveOnMapReq(t *testing.T) { result: &pb.MoveOnMapRequest{ Name: "bloop", Destination: spatialmath.PoseToProtobuf(spatialmath.NewZeroPose()), - ComponentName: rprotoutils.ResourceNameToProto(myBase), - SlamServiceName: rprotoutils.ResourceNameToProto(mySlam), + ComponentName: myBase, + SlamServiceName: mySlam, Extra: &structpb.Struct{}, }, err: nil, @@ -1173,8 +1174,8 @@ func TestMoveOnMapReq(t *testing.T) { result: &pb.MoveOnMapRequest{ Name: "bloop", Destination: spatialmath.PoseToProtobuf(spatialmath.NewZeroPose()), - ComponentName: rprotoutils.ResourceNameToProto(myBase), - SlamServiceName: rprotoutils.ResourceNameToProto(mySlam), + ComponentName: myBase, + SlamServiceName: mySlam, Obstacles: referenceframe.NewGeometriesToProto([]spatialmath.Geometry{spatialmath.NewPoint(r3.Vector{2, 2, 2}, "pt")}), Extra: &structpb.Struct{}, }, @@ -1225,17 +1226,17 @@ func TestMoveOnMapReq(t *testing.T) { Destination: spatialmath.PoseToProtobuf(spatialmath.NewZeroPose()), }, result: MoveOnMapReq{}, - err: errors.New("received nil *commonpb.ResourceName for component name"), + err: ErrEmptyComponentName, }, { description: "nil SlamName causes failure", input: &pb.MoveOnMapRequest{ Destination: spatialmath.PoseToProtobuf(spatialmath.NewZeroPose()), - ComponentName: rprotoutils.ResourceNameToProto(myBase), + ComponentName: myBase, }, result: MoveOnMapReq{}, - err: errors.New("received nil *commonpb.ResourceName for SlamService name"), + err: errors.New("SlamService name cannot be empty"), }, { description: "success", @@ -1248,8 +1249,8 @@ func TestMoveOnMapReq(t *testing.T) { input: &pb.MoveOnMapRequest{ Destination: spatialmath.PoseToProtobuf(spatialmath.NewPoseFromPoint(r3.Vector{2700, 0, 0})), - ComponentName: rprotoutils.ResourceNameToProto(myBase), - SlamServiceName: rprotoutils.ResourceNameToProto(mySlam), + ComponentName: myBase, + SlamServiceName: mySlam, }, result: MoveOnMapReq{ ComponentName: myBase, @@ -1265,8 +1266,8 @@ func TestMoveOnMapReq(t *testing.T) { description: "success - allow non-nil obstacles", input: &pb.MoveOnMapRequest{ Destination: spatialmath.PoseToProtobuf(spatialmath.NewPoseFromPoint(r3.Vector{2700, 0, 0})), - ComponentName: rprotoutils.ResourceNameToProto(myBase), - SlamServiceName: rprotoutils.ResourceNameToProto(mySlam), + ComponentName: myBase, + SlamServiceName: mySlam, Obstacles: referenceframe.NewGeometriesToProto( []spatialmath.Geometry{spatialmath.NewPoint(r3.Vector{X: 2, Y: 2, Z: 2}, "pt")}, ), @@ -1285,8 +1286,8 @@ func TestMoveOnMapReq(t *testing.T) { description: "fail - inconvertible geometry", input: &pb.MoveOnMapRequest{ Destination: spatialmath.PoseToProtobuf(spatialmath.NewPoseFromPoint(r3.Vector{2700, 0, 0})), - ComponentName: rprotoutils.ResourceNameToProto(myBase), - SlamServiceName: rprotoutils.ResourceNameToProto(mySlam), + ComponentName: myBase, + SlamServiceName: mySlam, Obstacles: []*commonpb.Geometry{{GeometryType: nil}}, }, result: MoveOnMapReq{}, @@ -1319,7 +1320,7 @@ func TestPlanHistoryReq(t *testing.T) { } executionID := uuid.New() - mybase := base.Named("mybase") + mybase := "mybase" executionIDStr := executionID.String() testCases := []testCase{ @@ -1329,7 +1330,7 @@ func TestPlanHistoryReq(t *testing.T) { name: "some name", result: &pb.GetPlanRequest{ Name: "some name", - ComponentName: rprotoutils.ResourceNameToProto(resource.Name{}), + ComponentName: "", Extra: &structpb.Struct{}, }, }, @@ -1343,7 +1344,7 @@ func TestPlanHistoryReq(t *testing.T) { name: "some name", result: &pb.GetPlanRequest{ Name: "some name", - ComponentName: rprotoutils.ResourceNameToProto(mybase), + ComponentName: mybase, ExecutionId: &executionIDStr, LastPlanOnly: true, Extra: &structpb.Struct{}, @@ -1374,7 +1375,7 @@ func TestPlanHistoryReq(t *testing.T) { } executionID := uuid.New() - mybase := base.Named("mybase") + mybase := "mybase" executionIDStr := executionID.String() testCases := []testCase{ @@ -1382,20 +1383,23 @@ func TestPlanHistoryReq(t *testing.T) { description: "returns an error if component name is nil", input: &pb.GetPlanRequest{}, result: PlanHistoryReq{}, - err: errors.New("received nil *commonpb.ResourceName"), + err: ErrEmptyComponentName, }, { description: "empty struct returns an empty struct", input: &pb.GetPlanRequest{ - ComponentName: rprotoutils.ResourceNameToProto(resource.Name{}), + ComponentName: "component_name", + }, + result: PlanHistoryReq{ + ComponentName: "component_name", + Extra: map[string]interface{}{}, }, - result: PlanHistoryReq{Extra: map[string]interface{}{}}, }, { description: "full struct returns a full struct", input: &pb.GetPlanRequest{ Name: "some name", - ComponentName: rprotoutils.ResourceNameToProto(mybase), + ComponentName: mybase, ExecutionId: &executionIDStr, LastPlanOnly: true, Extra: &structpb.Struct{}, @@ -1426,9 +1430,9 @@ func TestPlanHistoryReq(t *testing.T) { func validMoveOnGlobeRequest() MoveOnGlobeReq { dst := geo.NewPoint(1, 2) - visionCameraPairs := [][]resource.Name{ - {vision.Named("vision service 1"), camera.Named("camera 1")}, - {vision.Named("vision service 2"), camera.Named("camera 2")}, + visionCameraPairs := [][]string{ + {"vision service 1", "camera 1"}, + {"vision service 2", "camera 2"}, } obstacleDetectors := []ObstacleDetectorName{} for _, pair := range visionCameraPairs { @@ -1439,10 +1443,10 @@ func validMoveOnGlobeRequest() MoveOnGlobeReq { } pollingFreq := 5. return MoveOnGlobeReq{ - ComponentName: base.Named("my-base"), + ComponentName: "my-base", Destination: dst, Heading: 0.5, - MovementSensorName: movementsensor.Named("my-movementsensor"), + MovementSensorName: "my-movementsensor", Obstacles: nil, MotionCfg: &MotionConfiguration{ ObstacleDetectors: obstacleDetectors, diff --git a/services/motion/pbhelpers.go b/services/motion/pbhelpers.go index 1c197413d9c..b9b665388f1 100644 --- a/services/motion/pbhelpers.go +++ b/services/motion/pbhelpers.go @@ -11,11 +11,12 @@ import ( vprotoutils "go.viam.com/utils/protoutils" "go.viam.com/rdk/motionplan" - rprotoutils "go.viam.com/rdk/protoutils" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" ) +var ErrEmptyComponentName = errors.New("component name cannot be empty") + // ToProto converts a MoveReq to a pb.MoveRequest // the name argument should correspond to the name of the motion service the request will be used with. func (r MoveReq) ToProto(name string) (*pb.MoveRequest, error) { @@ -29,7 +30,7 @@ func (r MoveReq) ToProto(name string) (*pb.MoveRequest, error) { } reqPB := &pb.MoveRequest{ Name: name, - ComponentName: rprotoutils.ResourceNameToProto(r.ComponentName), + ComponentName: r.ComponentName, WorldState: worldStateMsg, Constraints: r.Constraints.ToProtobuf(), Extra: ext, @@ -56,7 +57,7 @@ func MoveReqFromProto(req *pb.MoveRequest) (MoveReq, error) { } return MoveReq{ - rprotoutils.ResourceNameFromProto(req.GetComponentName()), + req.GetComponentName(), destination, worldState, motionplan.ConstraintsFromProtobuf(req.GetConstraints()), @@ -129,13 +130,13 @@ func planStatusWithIDFromProto(ps *pb.PlanStatusWithID) (PlanStatusWithID, error return PlanStatusWithID{}, err } - if ps.ComponentName == nil { - return PlanStatusWithID{}, errors.New("received nil *commonpb.ResourceName") + if ps.ComponentName == "" { + return PlanStatusWithID{}, ErrEmptyComponentName } return PlanStatusWithID{ PlanID: planID, - ComponentName: rprotoutils.ResourceNameFromProto(ps.ComponentName), + ComponentName: ps.ComponentName, ExecutionID: executionID, Status: status, }, nil @@ -157,13 +158,13 @@ func planFromProto(p *pb.Plan) (PlanWithMetadata, error) { return PlanWithMetadata{}, err } - if p.ComponentName == nil { - return PlanWithMetadata{}, errors.New("received nil *pb.ResourceName") + if p.ComponentName == "" { + return PlanWithMetadata{}, ErrEmptyComponentName } plan := PlanWithMetadata{ ID: id, - ComponentName: rprotoutils.ResourceNameFromProto(p.ComponentName), + ComponentName: p.ComponentName, ExecutionID: executionID, } @@ -214,9 +215,9 @@ func (r MoveOnGlobeReq) toProto(name string) (*pb.MoveOnGlobeRequest, error) { req := &pb.MoveOnGlobeRequest{ Name: name, - ComponentName: rprotoutils.ResourceNameToProto(r.ComponentName), + ComponentName: r.ComponentName, Destination: &commonpb.GeoPoint{Latitude: r.Destination.Lat(), Longitude: r.Destination.Lng()}, - MovementSensorName: rprotoutils.ResourceNameToProto(r.MovementSensorName), + MovementSensorName: r.MovementSensorName, Extra: ext, } @@ -280,16 +281,16 @@ func moveOnGlobeRequestFromProto(req *pb.MoveOnGlobeRequest) (MoveOnGlobeReq, er } protoComponentName := req.GetComponentName() - if protoComponentName == nil { - return MoveOnGlobeReq{}, errors.New("received nil *commonpb.ResourceName") + if protoComponentName == "" { + return MoveOnGlobeReq{}, ErrEmptyComponentName } - componentName := rprotoutils.ResourceNameFromProto(protoComponentName) + componentName := protoComponentName destination := geo.NewPoint(req.GetDestination().GetLatitude(), req.GetDestination().GetLongitude()) protoMovementSensorName := req.GetMovementSensorName() - if protoMovementSensorName == nil { - return MoveOnGlobeReq{}, errors.New("received nil *commonpb.ResourceName") + if protoMovementSensorName == "" { + return MoveOnGlobeReq{}, ErrEmptyComponentName } - movementSensorName := rprotoutils.ResourceNameFromProto(protoMovementSensorName) + movementSensorName := protoMovementSensorName motionCfg := configurationFromProto(req.MotionConfiguration) return MoveOnGlobeReq{ @@ -317,7 +318,7 @@ func (req PlanHistoryReq) toProto(name string) (*pb.GetPlanRequest, error) { } return &pb.GetPlanRequest{ Name: name, - ComponentName: rprotoutils.ResourceNameToProto(req.ComponentName), + ComponentName: req.ComponentName, LastPlanOnly: req.LastPlanOnly, Extra: ext, ExecutionId: executionIDPtr, @@ -325,8 +326,8 @@ func (req PlanHistoryReq) toProto(name string) (*pb.GetPlanRequest, error) { } func getPlanRequestFromProto(req *pb.GetPlanRequest) (PlanHistoryReq, error) { - if req.GetComponentName() == nil { - return PlanHistoryReq{}, errors.New("received nil *commonpb.ResourceName") + if req.GetComponentName() == "" { + return PlanHistoryReq{}, ErrEmptyComponentName } executionID := uuid.Nil @@ -339,7 +340,7 @@ func getPlanRequestFromProto(req *pb.GetPlanRequest) (PlanHistoryReq, error) { } return PlanHistoryReq{ - ComponentName: rprotoutils.ResourceNameFromProto(req.GetComponentName()), + ComponentName: req.GetComponentName(), LastPlanOnly: req.GetLastPlanOnly(), ExecutionID: executionID, Extra: req.Extra.AsMap(), @@ -354,12 +355,12 @@ func moveOnMapRequestFromProto(req *pb.MoveOnMapRequest) (MoveOnMapReq, error) { return MoveOnMapReq{}, errors.New("received nil *commonpb.Pose for destination") } protoComponentName := req.GetComponentName() - if protoComponentName == nil { - return MoveOnMapReq{}, errors.New("received nil *commonpb.ResourceName for component name") + if protoComponentName == "" { + return MoveOnMapReq{}, ErrEmptyComponentName } protoSlamServiceName := req.GetSlamServiceName() - if protoSlamServiceName == nil { - return MoveOnMapReq{}, errors.New("received nil *commonpb.ResourceName for SlamService name") + if protoSlamServiceName == "" { + return MoveOnMapReq{}, errors.New("SlamService name cannot be empty") } geoms := []spatialmath.Geometry{} if obs := req.GetObstacles(); len(obs) > 0 { @@ -370,9 +371,9 @@ func moveOnMapRequestFromProto(req *pb.MoveOnMapRequest) (MoveOnMapReq, error) { geoms = convertedGeom } return MoveOnMapReq{ - ComponentName: rprotoutils.ResourceNameFromProto(protoComponentName), + ComponentName: protoComponentName, Destination: spatialmath.NewPoseFromProtobuf(req.GetDestination()), - SlamName: rprotoutils.ResourceNameFromProto(protoSlamServiceName), + SlamName: protoSlamServiceName, MotionCfg: configurationFromProto(req.MotionConfiguration), Obstacles: geoms, Extra: req.Extra.AsMap(), @@ -389,9 +390,9 @@ func (r MoveOnMapReq) toProto(name string) (*pb.MoveOnMapRequest, error) { } req := &pb.MoveOnMapRequest{ Name: name, - ComponentName: rprotoutils.ResourceNameToProto(r.ComponentName), + ComponentName: r.ComponentName, Destination: spatialmath.PoseToProtobuf(r.Destination), - SlamServiceName: rprotoutils.ResourceNameToProto(r.SlamName), + SlamServiceName: r.SlamName, Obstacles: referenceframe.NewGeometriesToProto(r.Obstacles), Extra: ext, } diff --git a/services/motion/server.go b/services/motion/server.go index 6520815816c..b1c26ada0aa 100644 --- a/services/motion/server.go +++ b/services/motion/server.go @@ -79,14 +79,14 @@ func (server *serviceServer) GetPose(ctx context.Context, req *pb.GetPoseRequest if err != nil { return nil, err } - if req.ComponentName == nil { + if req.ComponentName == "" { return nil, errors.New("must provide component name") } transforms, err := referenceframe.LinkInFramesFromTransformsProtobuf(req.GetSupplementalTransforms()) if err != nil { return nil, err } - pose, err := svc.GetPose(ctx, protoutils.ResourceNameFromProto(req.ComponentName), req.DestinationFrame, transforms, req.Extra.AsMap()) + pose, err := svc.GetPose(ctx, req.ComponentName, req.DestinationFrame, transforms, req.Extra.AsMap()) if err != nil { return nil, err } @@ -100,7 +100,7 @@ func (server *serviceServer) StopPlan(ctx context.Context, req *pb.StopPlanReque return nil, err } - componentName := protoutils.ResourceNameFromProto(req.GetComponentName()) + componentName := req.GetComponentName() r := StopPlanReq{ComponentName: componentName, Extra: req.Extra.AsMap()} err = svc.StopPlan(ctx, r) if err != nil { diff --git a/services/motion/server_test.go b/services/motion/server_test.go index 7b15ba0cbe2..94c2223ffbc 100644 --- a/services/motion/server_test.go +++ b/services/motion/server_test.go @@ -15,16 +15,11 @@ import ( "go.viam.com/test" vprotoutils "go.viam.com/utils/protoutils" - "go.viam.com/rdk/components/base" "go.viam.com/rdk/components/camera" - "go.viam.com/rdk/components/gripper" - "go.viam.com/rdk/components/movementsensor" "go.viam.com/rdk/motionplan" - "go.viam.com/rdk/protoutils" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" "go.viam.com/rdk/services/motion" - "go.viam.com/rdk/services/slam" "go.viam.com/rdk/services/vision" "go.viam.com/rdk/spatialmath" "go.viam.com/rdk/testutils" @@ -42,7 +37,7 @@ func newServer(resources map[resource.Name]motion.Service) (pb.MotionServiceServ func TestServerMove(t *testing.T) { grabRequest := &pb.MoveRequest{ Name: testMotionServiceName.ShortName(), - ComponentName: protoutils.ResourceNameToProto(gripper.Named("fake")), + ComponentName: "fake", Destination: referenceframe.PoseInFrameToProtobuf(referenceframe.NewPoseInFrame("", spatialmath.NewZeroPose())), } @@ -88,7 +83,7 @@ func TestServerMove(t *testing.T) { test.That(t, resp.GetSuccess(), test.ShouldBeTrue) grabRequest2 := &pb.MoveRequest{ Name: testMotionServiceName2.ShortName(), - ComponentName: protoutils.ResourceNameToProto(gripper.Named("fake")), + ComponentName: "fake", Destination: referenceframe.PoseInFrameToProtobuf(referenceframe.NewPoseInFrame("", spatialmath.NewZeroPose())), } resp, err = server.Move(context.Background(), grabRequest2) @@ -105,9 +100,9 @@ func TestServerMoveOnGlobe(t *testing.T) { test.That(t, err, test.ShouldBeNil) t.Run("returns error without calling MoveOnGlobe if req.Name doesn't map to a resource", func(t *testing.T) { moveOnGlobeRequest := &pb.MoveOnGlobeRequest{ - ComponentName: protoutils.ResourceNameToProto(base.Named("test-base")), + ComponentName: "test-base", Destination: &commonpb.GeoPoint{Latitude: 0.0, Longitude: 0.0}, - MovementSensorName: protoutils.ResourceNameToProto(movementsensor.Named("test-gps")), + MovementSensorName: "test-gps", } injectMS.MoveOnGlobeFunc = func(ctx context.Context, req motion.MoveOnGlobeReq) (motion.ExecutionID, error) { t.Log("should not be called") @@ -124,8 +119,8 @@ func TestServerMoveOnGlobe(t *testing.T) { t.Run("returns error if destination is nil without calling MoveOnGlobe", func(t *testing.T) { moveOnGlobeRequest := &pb.MoveOnGlobeRequest{ Name: testMotionServiceName.ShortName(), - ComponentName: protoutils.ResourceNameToProto(base.Named("test-base")), - MovementSensorName: protoutils.ResourceNameToProto(movementsensor.Named("test-gps")), + ComponentName: "test-base", + MovementSensorName: "test-gps", } injectMS.MoveOnGlobeFunc = func(ctx context.Context, req motion.MoveOnGlobeReq) (motion.ExecutionID, error) { t.Log("should not be called") @@ -141,9 +136,9 @@ func TestServerMoveOnGlobe(t *testing.T) { validMoveOnGlobeRequest := &pb.MoveOnGlobeRequest{ Name: testMotionServiceName.ShortName(), - ComponentName: protoutils.ResourceNameToProto(base.Named("test-base")), + ComponentName: "test-base", Destination: &commonpb.GeoPoint{Latitude: 0.0, Longitude: 0.0}, - MovementSensorName: protoutils.ResourceNameToProto(movementsensor.Named("test-gps")), + MovementSensorName: "test-gps", } t.Run("returns error when MoveOnGlobe returns an error", func(t *testing.T) { @@ -182,8 +177,8 @@ func TestServerMoveOnGlobe(t *testing.T) { }) t.Run("returns success when MoveOnGlobe returns success", func(t *testing.T) { - expectedComponentName := base.Named("test-base") - expectedMovSensorName := movementsensor.Named("test-gps") + expectedComponentName := "test-base" + expectedMovSensorName := "test-gps" reqHeading := 3. boxDims := r3.Vector{X: 5, Y: 50, Z: 10} @@ -221,21 +216,21 @@ func TestServerMoveOnGlobe(t *testing.T) { positionPollingFrequencyHz := 5. obstacleDetectorsPB := []*pb.ObstacleDetector{ { - VisionService: protoutils.ResourceNameToProto(vision.Named("vision service 1")), - Camera: protoutils.ResourceNameToProto(camera.Named("camera 1")), + VisionService: "vision service 1", + Camera: "camera 1", }, { - VisionService: protoutils.ResourceNameToProto(vision.Named("vision service 2")), - Camera: protoutils.ResourceNameToProto(camera.Named("camera 2")), + VisionService: "vision service 2", + Camera: "camera 2", }, } moveOnGlobeRequest := &pb.MoveOnGlobeRequest{ Name: testMotionServiceName.ShortName(), Heading: &reqHeading, - ComponentName: protoutils.ResourceNameToProto(expectedComponentName), + ComponentName: expectedComponentName, Destination: &commonpb.GeoPoint{Latitude: 1.0, Longitude: 2.0}, - MovementSensorName: protoutils.ResourceNameToProto(expectedMovSensorName), + MovementSensorName: expectedMovSensorName, Obstacles: obs, MotionConfiguration: &pb.MotionConfiguration{ AngularDegsPerSec: &angularDegsPerSec, @@ -264,10 +259,10 @@ func TestServerMoveOnGlobe(t *testing.T) { test.That(t, *req.MotionCfg.ObstaclePollingFreqHz, test.ShouldAlmostEqual, obstaclePollingFrequencyHz) test.That(t, *req.MotionCfg.PositionPollingFreqHz, test.ShouldAlmostEqual, positionPollingFrequencyHz) test.That(t, len(req.MotionCfg.ObstacleDetectors), test.ShouldAlmostEqual, 2) - test.That(t, req.MotionCfg.ObstacleDetectors[0].VisionServiceName, test.ShouldResemble, vision.Named("vision service 1")) - test.That(t, req.MotionCfg.ObstacleDetectors[0].CameraName, test.ShouldResemble, camera.Named("camera 1")) - test.That(t, req.MotionCfg.ObstacleDetectors[1].VisionServiceName, test.ShouldResemble, vision.Named("vision service 2")) - test.That(t, req.MotionCfg.ObstacleDetectors[1].CameraName, test.ShouldResemble, camera.Named("camera 2")) + test.That(t, req.MotionCfg.ObstacleDetectors[0].VisionServiceName, test.ShouldResemble, vision.Named("vision service 1").Name) + test.That(t, req.MotionCfg.ObstacleDetectors[0].CameraName, test.ShouldResemble, camera.Named("camera 1").Name) + test.That(t, req.MotionCfg.ObstacleDetectors[1].VisionServiceName, test.ShouldResemble, vision.Named("vision service 2").Name) + test.That(t, req.MotionCfg.ObstacleDetectors[1].CameraName, test.ShouldResemble, camera.Named("camera 2").Name) test.That(t, len(req.BoundingRegions), test.ShouldEqual, 1) test.That(t, req.BoundingRegions[0], test.ShouldResemble, geoGeometry3) return firstExecutionID, nil @@ -288,9 +283,9 @@ func TestServerMoveOnMap(t *testing.T) { t.Run("returns error without calling MoveOnMap if req.Name doesn't map to a resource", func(t *testing.T) { moveOnMapRequest := &pb.MoveOnMapRequest{ - ComponentName: protoutils.ResourceNameToProto(base.Named("test-base")), + ComponentName: "test-base", Destination: spatialmath.PoseToProtobuf(spatialmath.NewZeroPose()), - SlamServiceName: protoutils.ResourceNameToProto(slam.Named("test-slam")), + SlamServiceName: "test-slam", } injectMS.MoveOnMapFunc = func(ctx context.Context, req motion.MoveOnMapReq) (motion.ExecutionID, error) { t.Log("should not be called") @@ -307,9 +302,9 @@ func TestServerMoveOnMap(t *testing.T) { t.Run("returns error if destination is nil without calling MoveOnMap", func(t *testing.T) { moveOnMapRequest := &pb.MoveOnMapRequest{ Name: testMotionServiceName.ShortName(), - ComponentName: protoutils.ResourceNameToProto(base.Named("test-base")), + ComponentName: "test-base", Destination: nil, - SlamServiceName: protoutils.ResourceNameToProto(slam.Named("test-slam")), + SlamServiceName: "test-slam", } injectMS.MoveOnMapFunc = func(ctx context.Context, req motion.MoveOnMapReq) (motion.ExecutionID, error) { t.Log("should not be called") @@ -324,9 +319,9 @@ func TestServerMoveOnMap(t *testing.T) { validMoveOnMapRequest := &pb.MoveOnMapRequest{ Name: testMotionServiceName.ShortName(), - ComponentName: protoutils.ResourceNameToProto(base.Named("test-base")), + ComponentName: "test-base", Destination: spatialmath.PoseToProtobuf(spatialmath.NewZeroPose()), - SlamServiceName: protoutils.ResourceNameToProto(slam.Named("test-slam")), + SlamServiceName: "test-slam", } t.Run("returns error when MoveOnMap returns an error", func(t *testing.T) { @@ -342,8 +337,8 @@ func TestServerMoveOnMap(t *testing.T) { }) t.Run("returns success when MoveOnMap returns success", func(t *testing.T) { - expectedComponentName := base.Named("test-base") - expectedSlamName := slam.Named("test-slam") + expectedComponentName := "test-base" + expectedSlamName := "test-slam" expectedDestination := spatialmath.PoseToProtobuf(spatialmath.NewZeroPose()) angularDegsPerSec := 1. @@ -353,21 +348,21 @@ func TestServerMoveOnMap(t *testing.T) { positionPollingFrequencyHz := 5. obstacleDetectorsPB := []*pb.ObstacleDetector{ { - VisionService: protoutils.ResourceNameToProto(vision.Named("vision service 1")), - Camera: protoutils.ResourceNameToProto(camera.Named("camera 1")), + VisionService: "vision service 1", + Camera: "camera 1", }, { - VisionService: protoutils.ResourceNameToProto(vision.Named("vision service 2")), - Camera: protoutils.ResourceNameToProto(camera.Named("camera 2")), + VisionService: "vision service 2", + Camera: "camera 2", }, } moveOnMapRequest := &pb.MoveOnMapRequest{ Name: testMotionServiceName.ShortName(), - ComponentName: protoutils.ResourceNameToProto(expectedComponentName), + ComponentName: expectedComponentName, Destination: expectedDestination, - SlamServiceName: protoutils.ResourceNameToProto(expectedSlamName), + SlamServiceName: expectedSlamName, MotionConfiguration: &pb.MotionConfiguration{ AngularDegsPerSec: &angularDegsPerSec, @@ -394,10 +389,10 @@ func TestServerMoveOnMap(t *testing.T) { test.That(t, *req.MotionCfg.ObstaclePollingFreqHz, test.ShouldAlmostEqual, obstaclePollingFrequencyHz) test.That(t, *req.MotionCfg.PositionPollingFreqHz, test.ShouldAlmostEqual, positionPollingFrequencyHz) test.That(t, len(req.MotionCfg.ObstacleDetectors), test.ShouldAlmostEqual, 2) - test.That(t, req.MotionCfg.ObstacleDetectors[0].VisionServiceName, test.ShouldResemble, vision.Named("vision service 1")) - test.That(t, req.MotionCfg.ObstacleDetectors[0].CameraName, test.ShouldResemble, camera.Named("camera 1")) - test.That(t, req.MotionCfg.ObstacleDetectors[1].VisionServiceName, test.ShouldResemble, vision.Named("vision service 2")) - test.That(t, req.MotionCfg.ObstacleDetectors[1].CameraName, test.ShouldResemble, camera.Named("camera 2")) + test.That(t, req.MotionCfg.ObstacleDetectors[0].VisionServiceName, test.ShouldResemble, vision.Named("vision service 1").Name) + test.That(t, req.MotionCfg.ObstacleDetectors[0].CameraName, test.ShouldResemble, camera.Named("camera 1").Name) + test.That(t, req.MotionCfg.ObstacleDetectors[1].VisionServiceName, test.ShouldResemble, vision.Named("vision service 2").Name) + test.That(t, req.MotionCfg.ObstacleDetectors[1].CameraName, test.ShouldResemble, camera.Named("camera 2").Name) return firstExecutionID, nil } moveOnMapResponse, err := server.MoveOnMap(context.Background(), moveOnMapRequest) @@ -408,9 +403,9 @@ func TestServerMoveOnMap(t *testing.T) { t.Run("non-nil obstacles passes", func(t *testing.T) { moveOnMapReq := &pb.MoveOnMapRequest{ Name: testMotionServiceName.ShortName(), - ComponentName: protoutils.ResourceNameToProto(base.Named("test-base")), + ComponentName: "test-base", Destination: spatialmath.PoseToProtobuf(spatialmath.NewZeroPose()), - SlamServiceName: protoutils.ResourceNameToProto(slam.Named("test-slam")), + SlamServiceName: "test-slam", Obstacles: referenceframe.NewGeometriesToProto([]spatialmath.Geometry{spatialmath.NewPoint(r3.Vector{2, 2, 2}, "pt")}), } @@ -430,9 +425,9 @@ func TestServerMoveOnMap(t *testing.T) { t.Run("fails with inconvertible geometry", func(t *testing.T) { moveOnMapReq := &pb.MoveOnMapRequest{ Name: testMotionServiceName.ShortName(), - ComponentName: protoutils.ResourceNameToProto(base.Named("test-base")), + ComponentName: "test-base", Destination: spatialmath.PoseToProtobuf(spatialmath.NewZeroPose()), - SlamServiceName: protoutils.ResourceNameToProto(slam.Named("test-slam")), + SlamServiceName: "test-slam", Obstacles: []*commonpb.Geometry{{GeometryType: nil}}, } @@ -457,16 +452,16 @@ func TestServerStopPlan(t *testing.T) { server, err := newServer(resources) test.That(t, err, test.ShouldBeNil) - expectedComponentName := base.Named("test-base") + expectedComponentName := "test-base" validStopPlanRequest := &pb.StopPlanRequest{ - ComponentName: protoutils.ResourceNameToProto(expectedComponentName), + ComponentName: expectedComponentName, Name: testMotionServiceName.ShortName(), } t.Run("returns error without calling StopPlan if req.Name doesn't map to a resource", func(t *testing.T) { stopPlanRequest := &pb.StopPlanRequest{ - ComponentName: protoutils.ResourceNameToProto(expectedComponentName), + ComponentName: expectedComponentName, } injectMS.StopPlanFunc = func( @@ -565,7 +560,7 @@ func TestServerListPlanStatuses(t *testing.T) { planID3 := uuid.New() planID4 := uuid.New() - expectedComponentName := base.Named("test-base") + expectedComponentName := "test-base" failedReason := "some reason for failure" status1 := motion.PlanStatus{State: motion.PlanStateFailed, Timestamp: time.Now(), Reason: &failedReason} @@ -628,12 +623,12 @@ func TestServerGetPlan(t *testing.T) { server, err := newServer(resources) test.That(t, err, test.ShouldBeNil) - expectedComponentName := base.Named("test-base") + expectedComponentName := "test-base" uuidID := uuid.New() id := uuidID.String() validGetPlanRequest := &pb.GetPlanRequest{ - ComponentName: protoutils.ResourceNameToProto(expectedComponentName), + ComponentName: expectedComponentName, Name: testMotionServiceName.ShortName(), LastPlanOnly: false, ExecutionId: &id, @@ -671,8 +666,8 @@ func TestServerGetPlan(t *testing.T) { planID1 := uuid.New() planID2 := uuid.New() - base1 := base.Named("base1") - steps := []referenceframe.FrameSystemPoses{{base1.ShortName(): referenceframe.NewZeroPoseInFrame(referenceframe.World)}} + base1 := "base1" + steps := []referenceframe.FrameSystemPoses{{base1: referenceframe.NewZeroPoseInFrame(referenceframe.World)}} plan1 := motion.PlanWithMetadata{ ID: planID1, diff --git a/services/navigation/builtin/builtin.go b/services/navigation/builtin/builtin.go index 0b239954ba0..0b42464f730 100644 --- a/services/navigation/builtin/builtin.go +++ b/services/navigation/builtin/builtin.go @@ -234,7 +234,7 @@ type builtIn struct { fsService framesystem.Service base base.Base movementSensor movementsensor.MovementSensor - visionServicesByName map[resource.Name]vision.Service + visionServicesByName map[string]vision.Service motionService motion.Service obstacles []*spatialmath.GeoGeometry boundingRegions []*spatialmath.GeoGeometry @@ -340,7 +340,7 @@ func (svc *builtIn) Reconfigure(ctx context.Context, deps resource.Dependencies, } var obstacleDetectorNamePairs []motion.ObstacleDetectorName - visionServicesByName := make(map[resource.Name]vision.Service) + visionServicesByName := make(map[string]vision.Service) for _, pbObstacleDetectorPair := range svcConfig.ObstacleDetectors { visionSvc, err := vision.FromDependencies(deps, pbObstacleDetectorPair.VisionServiceName) if err != nil { @@ -351,9 +351,9 @@ func (svc *builtIn) Reconfigure(ctx context.Context, deps resource.Dependencies, return err } obstacleDetectorNamePairs = append(obstacleDetectorNamePairs, motion.ObstacleDetectorName{ - VisionServiceName: visionSvc.Name(), CameraName: camera.Name(), + VisionServiceName: visionSvc.Name().Name, CameraName: camera.Name().Name, }) - visionServicesByName[visionSvc.Name()] = visionSvc + visionServicesByName[visionSvc.Name().Name] = visionSvc } // Parse movement sensor from the configuration if map type is GPS @@ -544,10 +544,10 @@ func (svc *builtIn) Close(ctx context.Context) error { func (svc *builtIn) moveToWaypoint(ctx context.Context, wp navigation.Waypoint, extra map[string]interface{}) error { req := motion.MoveOnGlobeReq{ - ComponentName: svc.base.Name(), + ComponentName: svc.base.Name().Name, Destination: wp.ToPoint(), Heading: math.NaN(), - MovementSensorName: svc.movementSensor.Name(), + MovementSensorName: svc.movementSensor.Name().Name, Obstacles: svc.obstacles, MotionCfg: svc.motionCfg, BoundingRegions: svc.boundingRegions, @@ -669,25 +669,25 @@ func (svc *builtIn) Obstacles(ctx context.Context, extra map[string]interface{}) svc.logger.CDebugf( ctx, "proceeding to get detections from vision service: %s with camera: %s", - detector.VisionServiceName.ShortName(), - detector.CameraName.ShortName(), + detector.VisionServiceName, + detector.CameraName, ) // get the detections - detections, err := visSvc.GetObjectPointClouds(ctx, detector.CameraName.Name, nil) + detections, err := visSvc.GetObjectPointClouds(ctx, detector.CameraName, nil) if err != nil { return nil, err } // determine transform from camera to movement sensor movementsensorOrigin := referenceframe.NewPoseInFrame(svc.movementSensor.Name().ShortName(), spatialmath.NewZeroPose()) - cameraToMovementsensor, err := svc.fsService.TransformPose(ctx, movementsensorOrigin, detector.CameraName.ShortName(), nil) + cameraToMovementsensor, err := svc.fsService.TransformPose(ctx, movementsensorOrigin, detector.CameraName, nil) if err != nil { // here we make the assumption the movementsensor is coincident with the camera svc.logger.CDebugf( ctx, "we assume the movementsensor named: %s is coincident with the camera named: %s due to err: %v", - svc.movementSensor.Name().ShortName(), detector.CameraName.ShortName(), err.Error(), + svc.movementSensor.Name().Name, detector.CameraName, err.Error(), ) cameraToMovementsensor = movementsensorOrigin } @@ -707,14 +707,14 @@ func (svc *builtIn) Obstacles(ctx context.Context, extra map[string]interface{}) svc.logger.CDebugf(ctx, "baseToMovementSensor Pose: %v", baseToMovementSensor.Pose()) // determine transform from base to camera - cameraOrigin := referenceframe.NewPoseInFrame(detector.CameraName.ShortName(), spatialmath.NewZeroPose()) + cameraOrigin := referenceframe.NewPoseInFrame(detector.CameraName, spatialmath.NewZeroPose()) baseToCamera, err := svc.fsService.TransformPose(ctx, cameraOrigin, svc.base.Name().ShortName(), nil) if err != nil { // here we make the assumption the base is coincident with the camera svc.logger.CDebugf( ctx, "we assume the base named: %s is coincident with the camera named: %s due to err: %v", - svc.base.Name().ShortName(), detector.CameraName.ShortName(), err.Error(), + svc.base.Name().ShortName(), detector.CameraName, err.Error(), ) baseToCamera = cameraOrigin } @@ -779,7 +779,7 @@ func (svc *builtIn) Obstacles(ctx context.Context, extra map[string]interface{}) svc.logger.CDebugf(ctx, "obstacleGeoPose Location: %v, Heading: %v", *obstacleGeoPose.Location(), obstacleGeoPose.Heading()) // prefix the label of the geometry so we know it is transient and add extra info - label := "transient_" + strconv.Itoa(i) + "_" + detector.CameraName.Name + label := "transient_" + strconv.Itoa(i) + "_" + detector.CameraName if detection.Geometry.Label() != "" { label += "_" + detection.Geometry.Label() } @@ -822,7 +822,7 @@ func (svc *builtIn) Paths(ctx context.Context, extra map[string]interface{}) ([] } ph, err := svc.motionService.PlanHistory(ctx, motion.PlanHistoryReq{ - ComponentName: svc.base.Name(), + ComponentName: svc.base.Name().Name, ExecutionID: ewp.executionID, LastPlanOnly: true, }) diff --git a/services/navigation/builtin/builtin_test.go b/services/navigation/builtin/builtin_test.go index 8464e9fa9c8..b49a16c56c9 100644 --- a/services/navigation/builtin/builtin_test.go +++ b/services/navigation/builtin/builtin_test.go @@ -294,8 +294,8 @@ func TestNew(t *testing.T) { svcStruct := svc.(*builtIn) test.That(t, len(svcStruct.motionCfg.ObstacleDetectors), test.ShouldEqual, 1) - test.That(t, svcStruct.motionCfg.ObstacleDetectors[0].VisionServiceName.Name, test.ShouldEqual, "vision") - test.That(t, svcStruct.motionCfg.ObstacleDetectors[0].CameraName.Name, test.ShouldEqual, "camera") + test.That(t, svcStruct.motionCfg.ObstacleDetectors[0].VisionServiceName, test.ShouldEqual, "vision") + test.That(t, svcStruct.motionCfg.ObstacleDetectors[0].CameraName, test.ShouldEqual, "camera") test.That(t, svcStruct.motionCfg.AngularDegsPerSec, test.ShouldEqual, cfg.DegPerSec) test.That(t, svcStruct.motionCfg.LinearMPerSec, test.ShouldEqual, cfg.MetersPerSec) @@ -327,8 +327,8 @@ func TestNew(t *testing.T) { test.That(t, err, test.ShouldBeNil) svcStruct := svc.(*builtIn) - test.That(t, svcStruct.motionCfg.ObstacleDetectors[0].VisionServiceName.Name, test.ShouldEqual, "vision") - test.That(t, svcStruct.motionCfg.ObstacleDetectors[0].CameraName.Name, test.ShouldEqual, "camera") + test.That(t, svcStruct.motionCfg.ObstacleDetectors[0].VisionServiceName, test.ShouldEqual, "vision") + test.That(t, svcStruct.motionCfg.ObstacleDetectors[0].CameraName, test.ShouldEqual, "camera") test.That(t, svcStruct.replanCostFactor, test.ShouldEqual, cfg.ReplanCostFactor) }) @@ -435,8 +435,8 @@ func TestNew(t *testing.T) { test.That(t, err, test.ShouldBeNil) svcStruct := svc.(*builtIn) - test.That(t, svcStruct.motionCfg.ObstacleDetectors[0].VisionServiceName.Name, test.ShouldEqual, "vision") - test.That(t, svcStruct.motionCfg.ObstacleDetectors[0].CameraName.Name, test.ShouldEqual, "camera") + test.That(t, svcStruct.motionCfg.ObstacleDetectors[0].VisionServiceName, test.ShouldEqual, "vision") + test.That(t, svcStruct.motionCfg.ObstacleDetectors[0].CameraName, test.ShouldEqual, "camera") }) closeNavSvc() @@ -956,8 +956,8 @@ func TestStartWaypoint(t *testing.T) { AngularDegsPerSec: 1, ObstacleDetectors: []motion.ObstacleDetectorName{ { - VisionServiceName: vision.Named("vision"), - CameraName: camera.Named("camera"), + VisionServiceName: "vision", + CameraName: "camera", }, }, } @@ -981,9 +981,9 @@ func TestStartWaypoint(t *testing.T) { if len(s.sprs) == 2 { // MoveOnGlobe was called twice, once for each waypoint test.That(t, len(s.mogrs), test.ShouldEqual, 2) - test.That(t, s.mogrs[0].ComponentName, test.ShouldResemble, s.base.Name()) + test.That(t, s.mogrs[0].ComponentName, test.ShouldResemble, s.base.Name().Name) test.That(t, math.IsNaN(s.mogrs[0].Heading), test.ShouldBeTrue) - test.That(t, s.mogrs[0].MovementSensorName, test.ShouldResemble, s.movementSensor.Name()) + test.That(t, s.mogrs[0].MovementSensorName, test.ShouldResemble, s.movementSensor.Name().Name) test.That(t, s.mogrs[0].Extra, test.ShouldResemble, map[string]interface{}{ "motion_profile": "position_only", @@ -993,9 +993,9 @@ func TestStartWaypoint(t *testing.T) { // waypoint 1 test.That(t, s.mogrs[0].Destination, test.ShouldResemble, pt1) - test.That(t, s.mogrs[1].ComponentName, test.ShouldResemble, s.base.Name()) + test.That(t, s.mogrs[1].ComponentName, test.ShouldResemble, s.base.Name().Name) test.That(t, math.IsNaN(s.mogrs[1].Heading), test.ShouldBeTrue) - test.That(t, s.mogrs[1].MovementSensorName, test.ShouldResemble, s.movementSensor.Name()) + test.That(t, s.mogrs[1].MovementSensorName, test.ShouldResemble, s.movementSensor.Name().Name) test.That(t, s.mogrs[1].Extra, test.ShouldResemble, map[string]interface{}{ "motion_profile": "position_only", }) @@ -1005,11 +1005,11 @@ func TestStartWaypoint(t *testing.T) { test.That(t, s.mogrs[1].Destination, test.ShouldResemble, pt2) // PlanStop called twice, once for each waypoint - test.That(t, s.sprs[0].ComponentName, test.ShouldResemble, s.base.Name()) - test.That(t, s.sprs[1].ComponentName, test.ShouldResemble, s.base.Name()) + test.That(t, s.sprs[0].ComponentName, test.ShouldResemble, s.base.Name().Name) + test.That(t, s.sprs[1].ComponentName, test.ShouldResemble, s.base.Name().Name) // Motion reports that the last execution succeeded - ph, err := s.injectMS.PlanHistory(ctx, motion.PlanHistoryReq{ComponentName: s.base.Name()}) + ph, err := s.injectMS.PlanHistory(ctx, motion.PlanHistoryReq{ComponentName: s.base.Name().Name}) test.That(t, err, test.ShouldBeNil) test.That(t, len(ph), test.ShouldEqual, 1) test.That(t, ph[0].Plan.ExecutionID, test.ShouldResemble, executionIDs[1]) @@ -1256,7 +1256,7 @@ func TestStartWaypoint(t *testing.T) { test.That(t, s.mogrs[4].Destination, test.ShouldResemble, pt1) // Motion reports that the last execution succeeded - ph, err := s.injectMS.PlanHistory(ctx, motion.PlanHistoryReq{ComponentName: s.base.Name()}) + ph, err := s.injectMS.PlanHistory(ctx, motion.PlanHistoryReq{ComponentName: s.base.Name().Name}) test.That(t, err, test.ShouldBeNil) test.That(t, len(ph), test.ShouldEqual, 1) test.That(t, ph[0].Plan.ExecutionID, test.ShouldResemble, executionIDs[2]) @@ -1389,7 +1389,7 @@ func TestStartWaypoint(t *testing.T) { test.That(t, wpAfter, test.ShouldResemble, wpBefore) // check the last state of the execution - ph, err := s.injectMS.PlanHistory(ctx, motion.PlanHistoryReq{ComponentName: s.base.Name()}) + ph, err := s.injectMS.PlanHistory(ctx, motion.PlanHistoryReq{ComponentName: s.base.Name().Name}) test.That(t, err, test.ShouldBeNil) test.That(t, len(ph), test.ShouldEqual, 1) test.That(t, len(ph[0].StatusHistory), test.ShouldEqual, 2) @@ -1521,7 +1521,7 @@ func TestStartWaypoint(t *testing.T) { // Motion reports that the last execution succeeded s.RUnlock() - ph, err := s.injectMS.PlanHistory(ctx, motion.PlanHistoryReq{ComponentName: s.base.Name()}) + ph, err := s.injectMS.PlanHistory(ctx, motion.PlanHistoryReq{ComponentName: s.base.Name().Name}) test.That(t, err, test.ShouldBeNil) test.That(t, len(ph), test.ShouldEqual, 1) test.That(t, ph[0].Plan.ExecutionID, test.ShouldResemble, executionIDs[1]) @@ -1650,7 +1650,7 @@ func TestStartWaypoint(t *testing.T) { // Motion reports that the last execution succeeded s.RUnlock() - ph, err := s.injectMS.PlanHistory(ctx, motion.PlanHistoryReq{ComponentName: s.base.Name()}) + ph, err := s.injectMS.PlanHistory(ctx, motion.PlanHistoryReq{ComponentName: s.base.Name().Name}) test.That(t, err, test.ShouldBeNil) test.That(t, len(ph), test.ShouldEqual, 1) test.That(t, ph[0].Plan.ExecutionID, test.ShouldResemble, executionIDs[0]) diff --git a/testutils/inject/motion/motion_service.go b/testutils/inject/motion/motion_service.go index 1888af7abde..69e151c8b22 100644 --- a/testutils/inject/motion/motion_service.go +++ b/testutils/inject/motion/motion_service.go @@ -30,7 +30,7 @@ type MotionService struct { ) (motion.ExecutionID, error) GetPoseFunc func( ctx context.Context, - componentName resource.Name, + componentName string, destinationFrame string, supplementalTransforms []*referenceframe.LinkInFrame, extra map[string]interface{}, @@ -94,7 +94,7 @@ func (mgs *MotionService) MoveOnGlobe(ctx context.Context, req motion.MoveOnGlob // GetPose calls the injected GetPose or the real variant. func (mgs *MotionService) GetPose( ctx context.Context, - componentName resource.Name, + componentName string, destinationFrame string, supplementalTransforms []*referenceframe.LinkInFrame, extra map[string]interface{},