Skip to content
27 changes: 5 additions & 22 deletions cli/motion.go
Original file line number Diff line number Diff line change
Expand Up @@ -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"
)
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
}
Expand Down Expand Up @@ -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
}
Expand Down Expand Up @@ -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)
Expand Down
30 changes: 15 additions & 15 deletions services/motion/builtin/builtin.go
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -326,15 +326,15 @@ 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{},
) (*referenceframe.PoseInFrame, error) {
ms.logger.Warn("GetPose is deprecated. Please switch to using the GetPose method defined on the FrameSystem service")
ms.mu.RLock()
defer ms.mu.RUnlock()
return ms.fsService.GetPose(ctx, componentName.ShortName(), destinationFrame, supplementalTransforms, extra)
return ms.fsService.GetPose(ctx, componentName, destinationFrame, supplementalTransforms, extra)
}

func (ms *builtIn) StopPlan(
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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
Expand Down
Loading
Loading