diff --git a/cli/data.go b/cli/data.go index 05e3f72c906..aed1be64e84 100644 --- a/cli/data.go +++ b/cli/data.go @@ -742,12 +742,12 @@ func (c *viamClient) tabularData(dest string, request *datapb.ExportTabularDataR ctx, cancel := context.WithCancel(context.Background()) defer func() { - writer.Flush() //nolint:errcheck,gosec - dataFile.Close() //nolint:errcheck,gosec + writer.Flush() //nolint:errcheck + dataFile.Close() //nolint:errcheck cancel() if exportErr != nil { - os.Remove(dataFile.Name()) //nolint:errcheck,gosec + os.Remove(dataFile.Name()) //nolint:errcheck } }() diff --git a/cli/module_build.go b/cli/module_build.go index 23e7097d2c2..bbf1a2c0422 100644 --- a/cli/module_build.go +++ b/cli/module_build.go @@ -235,7 +235,7 @@ func (c *viamClient) moduleBuildListAction(cCtx *cli.Context, args moduleBuildLi job.StartTime.AsTime().Format(time.RFC3339)) } // the table is not printed to stdout until the tabwriter is flushed - //nolint: errcheck,gosec + //nolint:errcheck w.Flush() return nil } @@ -700,7 +700,7 @@ func (c *viamClient) loadGitignorePatterns(repoPath string) (gitignore.Matcher, return nil, errors.Wrap(err, "failed to open .gitignore file") } defer func() { - //nolint:errcheck,gosec // Ignore close error for read-only file + //nolint:errcheck file.Close() }() diff --git a/components/arm/sim/kinematics.go b/components/arm/sim/kinematics.go index d2cee136116..307a8a5bd31 100644 --- a/components/arm/sim/kinematics.go +++ b/components/arm/sim/kinematics.go @@ -39,6 +39,8 @@ var xarm7JSON []byte //go:embed kinematics/lite6.json var lite6JSON []byte +// Re-Documenting each public arm API is zero value and onerous. This applies for the whole file. +// revive:disable:exported func modelFromName(model, name string) (referenceframe.Model, error) { switch model { case ur5eModel: @@ -74,7 +76,7 @@ func buildModel(resName string, conf *Config) (referenceframe.Model, error) { } } -func (sa *simulatedArm) Get3DModels( +func (sa *SimulatedArm) Get3DModels( ctx context.Context, extra map[string]interface{}, ) (map[string]*commonpb.Mesh, error) { models := make(map[string]*commonpb.Mesh) @@ -97,14 +99,14 @@ func (sa *simulatedArm) Get3DModels( return models, nil } -func (sa *simulatedArm) Kinematics(ctx context.Context) (referenceframe.Model, error) { +func (sa *SimulatedArm) Kinematics(ctx context.Context) (referenceframe.Model, error) { sa.mu.Lock() defer sa.mu.Unlock() return sa.model, nil } -func (sa *simulatedArm) Geometries( +func (sa *SimulatedArm) Geometries( ctx context.Context, extra map[string]interface{}, ) ([]spatialmath.Geometry, error) { inputs, err := sa.CurrentInputs(ctx) diff --git a/components/arm/sim/sim.go b/components/arm/sim/sim.go index e9499068edc..a562d1077f5 100644 --- a/components/arm/sim/sim.go +++ b/components/arm/sim/sim.go @@ -30,12 +30,16 @@ type operation struct { stopped bool } +// Re-Documenting each public arm API is zero value and onerous. This applies for the whole file. +// revive:disable:exported func (op operation) isMoving() bool { // If we have `targetInputs` and we are not done and not stopped - return op.targetInputs != nil && !op.done && !op.stopped + return !op.done && !op.stopped } -type simulatedArm struct { +// SimulatedArm is a fake arm and exposes an `UpdateForTime` method that can be used to explicitly +// control the arm position in tests as Move* API calls are made. +type SimulatedArm struct { resource.Named // logical properties @@ -90,10 +94,8 @@ func init() { }) } -// Model is the name used to refer to the simulated arm model. var Model = resource.DefaultModelFamily.WithModel("simulated") -// Validate ensures all parts of the config are valid. func (conf *Config) Validate(path string) ([]string, []string, error) { var err error switch { @@ -132,9 +134,9 @@ func NewArm(ctx context.Context, deps resource.Dependencies, resConf resource.Co func newArm( namedArm resource.Named, modelName string, model referenceframe.Model, speed float64, simulateTime bool, logger logging.Logger, -) *simulatedArm { +) *SimulatedArm { ctx, cancel := context.WithCancel(context.Background()) - ret := &simulatedArm{ + ret := &SimulatedArm{ Named: namedArm, logger: logger, @@ -146,6 +148,9 @@ func newArm( cancel: cancel, currInputs: make([]float64, len(model.DoF())), + operation: operation{ + done: true, + }, } if simulateTime { @@ -153,19 +158,20 @@ func newArm( // movement be unpredictable. ret.lastUpdated = time.Now() ret.timeSimulation = utils.NewStoppableWorkerWithTicker(10*time.Millisecond, func(_ context.Context) { - ret.updateForTime(time.Now()) + ret.UpdateForTime(time.Now()) }) } return ret } -// Simulated arms only update their position when `updateForTime` is called. This can be used by -// tests for deterministic passage of time. Or can be called by a background goroutine to follow a -// realtime clock. +// UpdateForTime updates the simulated joint positions as a function of the input time, configured +// speed and target joint positions from a running `Move*` API call. Simulated arms only update +// their position when `UpdateForTime` is called. This can be used by tests for deterministic +// passage of time. Or can be called by a background goroutine to follow a realtime clock. // -// But direct `arm.Arm` API calls will _not_ update the position under the hood. -func (sa *simulatedArm) updateForTime(now time.Time) { +// Direct `arm.Arm` API calls will _not_ update the position under the hood. +func (sa *SimulatedArm) UpdateForTime(now time.Time) { sa.mu.Lock() defer sa.mu.Unlock() @@ -223,7 +229,7 @@ func (sa *simulatedArm) updateForTime(now time.Time) { } } -func (sa *simulatedArm) EndPosition( +func (sa *SimulatedArm) EndPosition( ctx context.Context, extra map[string]interface{}, ) (spatialmath.Pose, error) { sa.mu.Lock() @@ -232,17 +238,17 @@ func (sa *simulatedArm) EndPosition( return sa.model.Transform(sa.currInputs) } -func (sa *simulatedArm) MoveToPosition( +func (sa *SimulatedArm) MoveToPosition( ctx context.Context, pose spatialmath.Pose, extra map[string]interface{}, ) error { return errors.New("unimplemented -- must call with explicit joint positions") } -func (sa *simulatedArm) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) { +func (sa *SimulatedArm) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) { return sa.JointPositions(ctx, nil) } -func (sa *simulatedArm) JointPositions( +func (sa *SimulatedArm) JointPositions( ctx context.Context, extra map[string]interface{}, ) ([]referenceframe.Input, error) { sa.mu.Lock() @@ -254,7 +260,7 @@ func (sa *simulatedArm) JointPositions( return ret, nil } -func (sa *simulatedArm) MoveToJointPositions( +func (sa *SimulatedArm) MoveToJointPositions( ctx context.Context, target []referenceframe.Input, extra map[string]interface{}, ) error { if err := arm.CheckDesiredJointPositions(ctx, sa, target); err != nil { @@ -284,7 +290,6 @@ func (sa *simulatedArm) MoveToJointPositions( default: // Poll for completion: sa.mu.Lock() - // Calls to `updateForTime` will nil out `targetInputs` when a movement is completed. done, stopped := sa.operation.done, sa.operation.stopped sa.mu.Unlock() @@ -305,7 +310,7 @@ func (sa *simulatedArm) MoveToJointPositions( } } -func (sa *simulatedArm) MoveThroughJointPositions( +func (sa *SimulatedArm) MoveThroughJointPositions( ctx context.Context, positions [][]referenceframe.Input, _ *arm.MoveOptions, @@ -320,18 +325,18 @@ func (sa *simulatedArm) MoveThroughJointPositions( return nil } -func (sa *simulatedArm) GoToInputs(ctx context.Context, inputSteps ...[]referenceframe.Input) error { +func (sa *SimulatedArm) GoToInputs(ctx context.Context, inputSteps ...[]referenceframe.Input) error { return sa.MoveThroughJointPositions(ctx, inputSteps, nil, nil) } -func (sa *simulatedArm) IsMoving(ctx context.Context) (bool, error) { +func (sa *SimulatedArm) IsMoving(ctx context.Context) (bool, error) { sa.mu.Lock() defer sa.mu.Unlock() return sa.operation.isMoving(), nil } -func (sa *simulatedArm) Stop(ctx context.Context, extra map[string]any) error { +func (sa *SimulatedArm) Stop(ctx context.Context, extra map[string]any) error { sa.mu.Lock() defer sa.mu.Unlock() @@ -346,7 +351,7 @@ func (sa *simulatedArm) Stop(ctx context.Context, extra map[string]any) error { return nil } -func (sa *simulatedArm) Close(ctx context.Context) error { +func (sa *SimulatedArm) Close(ctx context.Context) error { sa.closed.Store(true) sa.cancel() if sa.timeSimulation != nil { diff --git a/components/arm/sim/sim_test.go b/components/arm/sim/sim_test.go index 52ceef08e69..3294f32ca1b 100644 --- a/components/arm/sim/sim_test.go +++ b/components/arm/sim/sim_test.go @@ -28,7 +28,7 @@ func TestBasic(t *testing.T) { simArmI, err := NewArm(ctx, nil, resConf, logger) test.That(t, err, test.ShouldBeNil) - simArm := simArmI.(*simulatedArm) + simArm := simArmI.(*SimulatedArm) // Assert the starting joint position is all zeroes. currInputs, err := simArm.CurrentInputs(ctx) @@ -60,7 +60,7 @@ func TestBasic(t *testing.T) { // Advance time by one second. clock := simArm.lastUpdated clock = clock.Add(time.Second) - simArm.updateForTime(clock) + simArm.UpdateForTime(clock) // Assert the joint position for first two joints changed to `1`. currInputs, err = simArm.CurrentInputs(ctx) @@ -76,7 +76,7 @@ func TestBasic(t *testing.T) { // Advance time by another second. Assert the joint position matches the target. Assert the // operation is considered done. clock = clock.Add(time.Second) - simArm.updateForTime(clock) + simArm.UpdateForTime(clock) currInputs, err = simArm.CurrentInputs(ctx) test.That(t, err, test.ShouldBeNil) @@ -104,7 +104,7 @@ func TestStop(t *testing.T) { simArmI, err := NewArm(ctx, nil, resConf, logger) test.That(t, err, test.ShouldBeNil) - simArm := simArmI.(*simulatedArm) + simArm := simArmI.(*SimulatedArm) // Set up a move that will take "2 seconds" to complete. `MoveToJointPositions` is blocking and // time must be advanced manually. Hence the goroutine. @@ -158,7 +158,7 @@ func TestTimeSimulation(t *testing.T) { simArmI, err := NewArm(ctx, nil, resConf, logger) test.That(t, err, test.ShouldBeNil) - simArm := simArmI.(*simulatedArm) + simArm := simArmI.(*SimulatedArm) defer func() { // When `SimulateTime` is true, we must call `Close` to wait on background goroutines. err = simArm.Close(ctx) diff --git a/components/board/genericlinux/buses/i2c.go b/components/board/genericlinux/buses/i2c.go index 45bbd83139f..5c9f0be4d41 100644 --- a/components/board/genericlinux/buses/i2c.go +++ b/components/board/genericlinux/buses/i2c.go @@ -16,7 +16,6 @@ import ( func init() { if _, err := host.Init(); err != nil { - //nolint:gosec fmtnolint.Println("Error initializing host:", err) } } diff --git a/etc/.golangci.yaml b/etc/.golangci.yaml index 5d6899c99b6..5eaa6640af9 100644 --- a/etc/.golangci.yaml +++ b/etc/.golangci.yaml @@ -60,6 +60,7 @@ linters: - testpackage - thelper # false positives - tparallel # there are valid reasons to have top-level t.Parallel() but serial subtests + - unparam # not useful - varnamelen - wrapcheck - wsl @@ -81,6 +82,7 @@ linters-settings: - composites gosec: excludes: + - G104 # Unhandled errors. These are already linted on, no need to double lint. - G115 # TODO(go1.23): maybe reenable revive: # Unfortunately configuring a single rules disables all other rules, even @@ -120,6 +122,14 @@ linters-settings: - ifElseChain gomoddirectives: replace-allow-list: [github.com/hashicorp/go-getter] + stylecheck: + checks: + - all + - "-ST1005" # Low value complaint. Can remove when gosimple rewrites code in place instead of failing. + gosimple: + checks: + - all + - "-S1002" # Low value complaint. Can remove when gosimple rewrites code in place instead of failing. issues: exclude-rules: - path: _test\.go$ diff --git a/ftdc/custom_format.go b/ftdc/custom_format.go index 6e349806e00..4564d6de3ff 100644 --- a/ftdc/custom_format.go +++ b/ftdc/custom_format.go @@ -63,7 +63,6 @@ func writeSchema(schema *schema, output io.Writer) error { func writeDatum(time int64, prev, curr []float32, output io.Writer) error { numPts := len(curr) if len(prev) != 0 && numPts != len(prev) { - //nolint:stylecheck return fmt.Errorf("Bad input sizes. Prev: %v Curr: %v", len(prev), len(curr)) } @@ -234,7 +233,7 @@ func flattenMap(mValue reflect.Value) ([]string, []float32, error) { } numbers = append(numbers, subNumbers...) case isNumeric(value.Kind()): - //nolint:stylecheck + return nil, nil, fmt.Errorf("A numeric type was forgotten to be included. Kind: %v", value.Kind()) default: // Getting the keys for a structure will ignore these types. Such as the antagonistic @@ -298,7 +297,7 @@ func flattenStruct(value reflect.Value) ([]string, []float32, error) { numbers = append(numbers, subNumbers...) case isNumeric(rField.Kind()): - //nolint:stylecheck + return nil, nil, fmt.Errorf("A numeric type was forgotten to be included. Kind: %v", rField.Kind()) default: // Getting the keys for a structure will ignore these types. Such as the antagonistic @@ -599,7 +598,6 @@ func readDiffBits(reader *bufio.Reader, schema *schema) ([]int, error) { // is the post-hydration list and consequently matches the `schema.fieldOrder` size. func readData(reader *bufio.Reader, schema *schema, diffedFields []int, prevValues []float32) ([]float32, error) { if prevValues != nil && len(prevValues) != len(schema.fieldOrder) { - //nolint return nil, fmt.Errorf("Parser error. Mismatched `prevValues` and schema size. PrevValues: %d Schema: %d", len(prevValues), len(schema.fieldOrder)) } diff --git a/module/module.go b/module/module.go index 3d642baf0a5..cc64532ef74 100644 --- a/module/module.go +++ b/module/module.go @@ -194,7 +194,7 @@ func NewModule(ctx context.Context, address string, logger logging.Logger) (*Mod if err != nil { return nil, err } - //nolint: errcheck, gosec + //nolint:errcheck trace.SetProvider( ctx, sdktrace.WithResource( diff --git a/motionplan/armplanning/api.go b/motionplan/armplanning/api.go index dad5a7c296d..fcd5b574721 100644 --- a/motionplan/armplanning/api.go +++ b/motionplan/armplanning/api.go @@ -4,6 +4,7 @@ package armplanning import ( "context" "encoding/json" + "fmt" "os" "path/filepath" "strings" @@ -231,6 +232,56 @@ func PlanMotion(ctx context.Context, parentLogger logging.Logger, request *PlanR return t, meta, nil } +// ValidatePlan returns nil if the input `executingPlan` is still valid. Non-nil is returned if the +// `worldStateOverride` would result in a collision. +func ValidatePlan( + ctx context.Context, + priorPlan motionplan.Plan, + request *PlanRequest, + parentLogger logging.Logger, +) error { + logger := parentLogger.Sublogger("mp") + + planMeta := &PlanMeta{} + // Assume request is valid? + sfPlanner, err := newPlanManager(ctx, logger, request, planMeta) + if err != nil { + return err + } + + if numGoals := len(request.Goals); numGoals > 1 { + panic(fmt.Sprintf("Only works with one goal? Num: %v", numGoals)) + } + + psc, err := newPlanSegmentContext( + ctx, sfPlanner.pc, request.StartState.LinearConfiguration(), request.Goals[0].Poses()) + if err != nil { + panic(fmt.Sprintf("Programmer error? %v", err)) + } + + trajectory := priorPlan.Trajectory() + for idx, inputs := range trajectory { + // Prune inputs we've already executed. Somehow. We're not privy to when execution moved + // through a waypoint. Yet. + if idx+1 == len(trajectory) { + break + } + + start, end := inputs.ToLinearInputs(), trajectory[idx+1].ToLinearInputs() + segmentToCheck := motionplan.SegmentFS{ + StartConfiguration: start, + EndConfiguration: end, + FS: request.FrameSystem, + } + + // Dan: Unclear what exactly this does. + const checkFinal = true + _, err = psc.checker.CheckStateConstraintsAcrossSegmentFS(ctx, &segmentToCheck, 0, checkFinal) + } + + return err +} + var defaultArmPlannerOptions = &motionplan.Constraints{ LinearConstraint: []motionplan.LinearConstraint{}, } diff --git a/motionplan/armplanning/cmd-plan/cmd-plan.go b/motionplan/armplanning/cmd-plan/cmd-plan.go index e6a851b6f5f..eeb6765457e 100644 --- a/motionplan/armplanning/cmd-plan/cmd-plan.go +++ b/motionplan/armplanning/cmd-plan/cmd-plan.go @@ -131,7 +131,7 @@ func realMain() error { } spansExporter := perf.NewOtelDevelopmentExporter() - //nolint: errcheck, gosec + //nolint:errcheck trace.SetProvider(ctx, sdktrace.WithResource(otelresource.Empty())) trace.AddExporters(spansExporter) diff --git a/motionplan/constraint_checker.go b/motionplan/constraint_checker.go index 7c9285b45cd..d4bea0f8c29 100644 --- a/motionplan/constraint_checker.go +++ b/motionplan/constraint_checker.go @@ -287,8 +287,10 @@ func (c *ConstraintChecker) CheckStateFSConstraints(ctx context.Context, state * return closest, nil } -// InterpolateSegmentFS is a helper function which produces a list of intermediate inputs, between the start and end -// configuration of a segment at a given resolution value. +// InterpolateSegmentFS is a helper function which produces a list of intermediate inputs, between +// the start and end configuration of a segment at a given resolution value. The resolution is +// actually a step size. The higher the resolution the less interpolation frames this checks +// against. func InterpolateSegmentFS(ci *SegmentFS, resolution float64) ([]*referenceframe.LinearInputs, error) { // Find the frame with the most steps by calculating steps for each frame maxSteps := defaultMinStepCount @@ -349,10 +351,11 @@ func InterpolateSegmentFS(ci *SegmentFS, resolution float64) ([]*referenceframe. return interpolatedConfigurations, nil } -// CheckStateConstraintsAcrossSegmentFS will interpolate the given input from the StartConfiguration to the EndConfiguration, and ensure -// that all intermediate states as well as both endpoints satisfy all state constraints. If all constraints are satisfied, then this will -// return `true, nil`. If any constraints fail, this will return false, and an SegmentFS representing the valid portion of the segment, -// if any. If no part of the segment is valid, then `false, nil` is returned. +// CheckStateConstraintsAcrossSegmentFS will interpolate the given input from the StartConfiguration +// to the EndConfiguration, and ensure that all intermediate states as well as both endpoints +// satisfy all state constraints. If all constraints are satisfied, then this will return `nil, +// nil`. If any constraints fail, this will return a SegmentFS representing the valid portion of the +// segment, if any, and an error. If no part of the segment is valid, then `nil, error` is returned. func (c *ConstraintChecker) CheckStateConstraintsAcrossSegmentFS( ctx context.Context, ci *SegmentFS, diff --git a/referenceframe/frame_system.go b/referenceframe/frame_system.go index 4e39f739f02..d5e9a6a8c6d 100644 --- a/referenceframe/frame_system.go +++ b/referenceframe/frame_system.go @@ -83,7 +83,6 @@ func NewFrameSystem(name string, parts []*FrameSystemPart, additionalTransforms strs[idx] = part.FrameConfig.Name() } - //nolint return nil, fmt.Errorf("Cannot construct frame system. Some parts are not linked to the world frame. Parts: %v", strs) } diff --git a/referenceframe/worldstate.go b/referenceframe/worldstate.go index 5e86d9174ea..e052f66d5df 100644 --- a/referenceframe/worldstate.go +++ b/referenceframe/worldstate.go @@ -40,8 +40,13 @@ func NewWorldState(obstacles []*GeometriesInFrame, transforms []*LinkInFrame) (* unnamedCount := 0 for _, gf := range obstacles { geometries := gf.Geometries() - checkedGeometries := make([]spatialmath.Geometry, 0, len(geometries)) + if len(geometries) == 0 { + // Avoid adding a bag of empty geometries. Such that `len(worldState.Obstacles())` + // returns 0 iff there are no obstacles. + continue + } + checkedGeometries := make([]spatialmath.Geometry, 0, len(geometries)) // iterate over geometries and make sure that each one that is added to the WorldState has a unique name for _, geometry := range geometries { name := geometry.Label() @@ -81,6 +86,55 @@ func WorldStateFromProtobuf(proto *commonpb.WorldState) (*WorldState, error) { return NewWorldState(allGeometries, transforms) } +// Merge creates a shallow copy of the `WorldState` merged with the input geometries. +func (ws *WorldState) Merge(geomsInFrame *GeometriesInFrame) *WorldState { + ret := &WorldState{ + obstacleNames: make(map[string]bool), + } + + // It's unclear how geometry names might be re-used across multiple snapshots of the world frame + // when executing a replannable motion plan. We assume duplicates can exist, and the input + // `geomsInFrame` has precedence (they were returned from a fresh vision service query to get + // objects). + // + // First add the input geometries. + if len(geomsInFrame.Geometries()) > 0 { + // Avoid adding a bag of empty geometries. Such that `len(worldState.Obstacles())` returns 0 + // iff there are no obstacles. + ret.obstacles = append(ret.obstacles, geomsInFrame) + for _, geom := range geomsInFrame.Geometries() { + ret.obstacleNames[geom.Label()] = true + } + } + + // Followed by filling in leftover geometries from the `ws` object. + for _, geomsInFrame := range ws.obstacles { + geomsToKeep := make([]spatialmath.Geometry, 0, len(geomsInFrame.Geometries())) + for _, geom := range geomsInFrame.Geometries() { + if _, exists := ret.obstacleNames[geom.Label()]; exists { + continue + } + + ret.obstacleNames[geom.Label()] = true + geomsToKeep = append(geomsToKeep, geom) + } + + if len(geomsToKeep) > 0 { + // Similar to above, avoid adding a bag of empty geometries. + ret.obstacles = append(ret.obstacles, + NewGeometriesInFrame(geomsInFrame.Parent(), geomsToKeep)) + } + } + + // Fill in the rest of `obstacleNames`. This is presumably just the names of `transforms`. + for name := range ws.obstacleNames { + ret.obstacleNames[name] = true + } + ret.transforms = append(ret.transforms, ws.transforms...) + + return ret +} + // ToProtobuf takes an rdk WorldState and converts it to the protobuf definition of a WorldState. func (ws *WorldState) ToProtobuf() (*commonpb.WorldState, error) { if ws == nil { diff --git a/robot/impl/local_robot.go b/robot/impl/local_robot.go index ca482997f6a..c5bc7215b1f 100644 --- a/robot/impl/local_robot.go +++ b/robot/impl/local_robot.go @@ -70,7 +70,7 @@ func init() { // Unfortunately Otel SDK doesn't have a way to reconfigure the resource // information so we need to set it here before any of the gRPC servers // access the global tracer provider. - //nolint: errcheck, gosec + //nolint:errcheck trace.SetProvider( context.Background(), sdktrace.WithResource( @@ -1664,7 +1664,7 @@ func (r *localRobot) reconfigureTracing(ctx context.Context, newConfig *config.C if !newTracingCfg.IsEnabled() { prevExporters := trace.ClearExporters() for _, ex := range prevExporters { - //nolint: errcheck, gosec + //nolint:errcheck ex.Shutdown(ctx) } r.traceClients.Store(nil) diff --git a/robot/impl/replanning_test.go b/robot/impl/replanning_test.go new file mode 100644 index 00000000000..e60bccde160 --- /dev/null +++ b/robot/impl/replanning_test.go @@ -0,0 +1,330 @@ +package robotimpl + +import ( + "context" + "sync" + "testing" + "time" + + "github.com/go-viper/mapstructure/v2" + "github.com/golang/geo/r3" + viz "github.com/viam-labs/motion-tools/client/client" + "go.viam.com/test" + "go.viam.com/utils/protoutils" + "golang.org/x/sync/errgroup" + "google.golang.org/protobuf/encoding/protojson" + + "go.viam.com/rdk/components/arm" + "go.viam.com/rdk/components/arm/sim" + "go.viam.com/rdk/components/camera" + fakecamera "go.viam.com/rdk/components/camera/fake" + "go.viam.com/rdk/config" + "go.viam.com/rdk/logging" + "go.viam.com/rdk/motionplan" + "go.viam.com/rdk/pointcloud" + "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/resource" + "go.viam.com/rdk/robot/framesystem" + motionservice "go.viam.com/rdk/services/motion" + _ "go.viam.com/rdk/services/motion/builtin" + visionservice "go.viam.com/rdk/services/vision" + "go.viam.com/rdk/spatialmath" + "go.viam.com/rdk/vision" +) + +type customSegmenter struct { + mu sync.Mutex + objects []*vision.Object +} + +func (cs *customSegmenter) GetObjects(ctx context.Context, src camera.Camera) ([]*vision.Object, error) { + cs.mu.Lock() + defer cs.mu.Unlock() + return cs.objects, nil +} + +type replanningTestObstacleServiceConfig struct{} + +func (rsConfig *replanningTestObstacleServiceConfig) Validate(path string) ([]string, []string, error) { + return nil, nil, nil +} + +func newObstacleVisionService( + ctx context.Context, deps resource.Dependencies, conf resource.Config, logger logging.Logger, + segmenter *customSegmenter, + defaultCameraName string, +) (visionservice.Service, error) { + return visionservice.NewService(conf.ResourceName(), deps, logger, nil, nil, nil, + segmenter.GetObjects, + defaultCameraName) +} + +func TestMotionServiceReplanningOnObstacle(t *testing.T) { + // TODO: Test currently requires the motion-tools visualization webserver to be running with a + // connected client. + logger := logging.NewTestLogger(t) + ctx := context.Background() + + // Create a "mocked" segmenter. This will back the `GetObjectPointClouds` method of the vision + // service that a motion service request can use. The motion service will poll + // `GetObjectPointClouds` and potentially halt a plan that is currently executing because a new + // obstacle appeared, invalidating assumptions the plan had made. + segmenter := &customSegmenter{} + replanningTestObstacleServiceModel := resource.ModelNamespaceRDK.WithFamily("testing").WithModel("replanning") + resource.RegisterService( + visionservice.API, + replanningTestObstacleServiceModel, + resource.Registration[visionservice.Service, *replanningTestObstacleServiceConfig]{ + Constructor: func( + ctx context.Context, deps resource.Dependencies, conf resource.Config, logger logging.Logger, + ) (visionservice.Service, error) { + return newObstacleVisionService(ctx, deps, conf, logger, segmenter, "camera") + }, + }) + + // Create a robot with the minimal components for replanning. An arm, a camera, a motion service + // and a vision detector. + cfg := &config.Config{ + Components: []resource.Config{ + { + Name: "arm", + API: arm.API, + Model: sim.Model, + Frame: &referenceframe.LinkConfig{ + Translation: r3.Vector{X: 0, Y: 0, Z: 0}, + Parent: "world", + }, + ConvertedAttributes: &sim.Config{ + Model: "lite6", + // At a `Speed` of 1.0, the test takes about two simulated seconds to complete. + Speed: 0.2, + }, + }, + { + Name: "camera", + API: camera.API, + Model: fakecamera.Model, + Frame: &referenceframe.LinkConfig{ + Geometry: &spatialmath.GeometryConfig{ + Type: spatialmath.BoxType, + X: 10, Y: 20, Z: 10, + Label: "cameraBox", + }, + // The arm's `wrist_link` has an X length of 75 and the camera's center is 5 + // away from the X border. + Translation: r3.Vector{X: (-75 / 2) - 5, Y: 0, Z: 0}, + Parent: "arm", + }, + ConvertedAttributes: &fakecamera.Config{ + Width: 100, + Height: 100, + }, + }, + }, + Services: []resource.Config{ + { + Name: "motionService", + API: motionservice.API, + Model: resource.DefaultServiceModel, + }, + { + Name: "obstacleService", + API: visionservice.API, + Model: replanningTestObstacleServiceModel, + DependsOn: []string{"camera"}, + }, + }, + } + + robot := setupLocalRobot(t, ctx, cfg, logger) + + // Assert all of the components/services are properly instantiated. + armI, err := arm.FromProvider(robot, "arm") + test.That(t, err, test.ShouldBeNil) + simArm := armI.(*sim.SimulatedArm) + + camera, err := camera.FromProvider(robot, "camera") + test.That(t, err, test.ShouldBeNil) + _ = camera + + motion, err := motionservice.FromProvider(robot, "motionService") + test.That(t, err, test.ShouldBeNil) + _ = motion + + obstacleService, err := visionservice.FromProvider(robot, "obstacleService") + test.That(t, err, test.ShouldBeNil) + _ = obstacleService + + robotFsI, err := robot.GetResource(framesystem.InternalServiceName) + test.That(t, err, test.ShouldBeNil) + robotFs := robotFsI.(framesystem.Service) + + // Assert the vision service `GetObjectPointClouds` does not return any obstacles. + objects, err := obstacleService.GetObjectPointClouds(ctx, "", nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, objects, test.ShouldHaveLength, 0) + + startArmPose, err := armI.EndPosition(ctx, nil) + test.That(t, err, test.ShouldBeNil) + + // Create a goal 200 points away on the Z-coordinate. + armGoal := spatialmath.Compose(startArmPose, spatialmath.NewPoseFromPoint(r3.Vector{X: -300, Y: 0, Z: 0})) + logger.Info("Start arm pose:", startArmPose, "Goal:", armGoal) + + emptyWorldState, err := referenceframe.NewWorldState([]*referenceframe.GeometriesInFrame{}, nil) + test.That(t, err, test.ShouldBeNil) + + moveRequest := &motionservice.MoveReq{ + ComponentName: "arm", + Destination: referenceframe.NewPoseInFrame("world", armGoal), + WorldState: emptyWorldState, + Extra: map[string]any{ + "obstacleVisionService": "obstacleService", + }, + } + + // Turn that request into the "map" that the motion service `DoCommand` accepts. Yes, it is this + // many steps. + moveRequestProto, err := moveRequest.ToProto("motionService") + test.That(t, err, test.ShouldBeNil) + + moveRequestProtoBytes, err := protojson.Marshal(moveRequestProto) + test.That(t, err, test.ShouldBeNil) + + testClock, ctx := errgroup.WithContext(ctx) + defer testClock.Wait() + + doneMovingCtx, doneMoving := context.WithCancel(ctx) + start := time.Now() + testClock.Go(func() error { + for doneMovingCtx.Err() == nil { + now := time.Now() + simArm.UpdateForTime(now) + + // Introduce an obstacle just after the beginning of the simulation. The obstacle is + // halfway between the start and endpoint. + if time.Since(start) > 1000*time.Millisecond { + segmenter.mu.Lock() + if len(segmenter.objects) == 0 { + // Add a 10x10x10 cube obstacle centered between the arm's start position and the goal. Assert + // the vision service `GetObjectPointClouds` returns this obstacle. + logger.Info("Adding object") + segmenter.objects = []*vision.Object{ + { + PointCloud: pointcloud.NewBasicPointCloud(10), + Geometry: spatialmath.NewBoxGoodInput( + spatialmath.Compose(startArmPose, spatialmath.NewPoseFromPoint(r3.Vector{X: -150, Y: 0, Z: 0})), + r3.Vector{X: 50, Y: 50, Z: 50}, + "box1"), + }, + } + } + segmenter.mu.Unlock() + + // `GetObjectPointClouds` acquires the `segmenter.mu`. Assert after releasing lock + // held for modification. + objects, err = obstacleService.GetObjectPointClouds(ctx, "", nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, objects, test.ShouldHaveLength, 1) + } + + time.Sleep(time.Millisecond) + } + + logger.Info("Exiting at:", time.Since(start)) + return nil + }) + + testClock.Go(func() error { + fs, err := framesystem.NewFromService(doneMovingCtx, robotFs, nil) + if err != nil { + return err + } + + return visualize(doneMovingCtx, fs, moveRequest, obstacleService, armI, armGoal, logger) + }) + + moveCmdProto, err := protoutils.StructToStructPb(map[string]any{ + "replannable": string(moveRequestProtoBytes), + // "plan": string(moveRequestProtoBytes), + }) + test.That(t, err, test.ShouldBeNil) + + resMap, err := motion.DoCommand(ctx, moveCmdProto.AsMap()) + test.That(t, err, test.ShouldBeNil) + + var trajectory motionplan.Trajectory + err = mapstructure.Decode(resMap["plan"], &trajectory) + test.That(t, err, test.ShouldBeNil) + logger.Infof("Num waypoints: %v Traj: %v", len(trajectory), trajectory) + + doneMoving() + test.That(t, testClock.Wait(), test.ShouldBeNil) +} + +func visualize( + ctx context.Context, + fs *referenceframe.FrameSystem, + req *motionservice.MoveReq, + obstacleVisionService visionservice.Service, + arm arm.Arm, + goalPose spatialmath.Pose, + logger logging.Logger, +) error { + if err := viz.RemoveAllSpatialObjects(); err != nil { + panic(err) + } + + const arrowHeadAtPose = true + if err := viz.DrawPoses([]spatialmath.Pose{goalPose}, []string{"blue"}, arrowHeadAtPose); err != nil { + panic(err) + } + + var logOnce sync.Once + for ctx.Err() == nil { + armInputs, err := arm.CurrentInputs(ctx) + if err != nil { + panic(err) + } + + fsi := make(referenceframe.FrameSystemInputs) + fsi["arm"] = armInputs + + currObjects, err := obstacleVisionService.GetObjectPointClouds(ctx, "", nil) + if err != nil { + panic(err) + } + + if len(currObjects) > 0 { + logOnce.Do(func() { + logger.Info("Obstacle appeared") + }) + } + + geoms := make([]spatialmath.Geometry, len(currObjects)) + for idx, newObstacle := range currObjects { + geoms[idx] = newObstacle.Geometry + } + geomsInFrame := referenceframe.NewGeometriesInFrame("world", geoms) + worldState := req.WorldState.Merge(geomsInFrame) + + if geoms, err := worldState.ObstaclesInWorldFrame(fs, fsi); err != nil { + panic(err) + } else if len(geoms.Geometries()) > 0 { + // `DrawWorldState` just draws the obstacles. I think the FrameSystem/Path are necessary + // because obstacles can be in terms of reference frames contained within the frame + // system. Such as a camera attached to an arm. + if err := viz.DrawWorldState(worldState, fs, fsi); err != nil { + panic(err) + } + } + + if err := viz.DrawFrameSystem(fs, fsi); err != nil { + panic(err) + } + + time.Sleep(10 * time.Millisecond) + } + + return nil +} diff --git a/services/motion/builtin/builtin.go b/services/motion/builtin/builtin.go index 37dfbf7c34d..ffda4c4face 100644 --- a/services/motion/builtin/builtin.go +++ b/services/motion/builtin/builtin.go @@ -55,6 +55,7 @@ const ( DoPlan = "plan" DoExecute = "execute" DoExecuteCheckStart = "executeCheckStart" + DoReplan = "replannable" ) const ( @@ -270,37 +271,25 @@ func (ms *builtIn) PlanHistory( // input value: a motionplan.Trajectory // output value: a bool func (ms *builtIn) DoCommand(ctx context.Context, cmd map[string]interface{}) (map[string]interface{}, error) { + if reqI, ok := cmd[DoReplan]; ok { + return ms.doReplannable(ctx, reqI) + } + ms.mu.RLock() defer ms.mu.RUnlock() resp := make(map[string]interface{}, 0) - if req, ok := cmd[DoPlan]; ok { - s, err := utils.AssertType[string](req) - if err != nil { - return nil, err - } - var moveReqProto pb.MoveRequest - err = protojson.Unmarshal([]byte(s), &moveReqProto) + if reqI, ok := cmd[DoPlan]; ok { + moveReq, err := stringToMoveRequest(reqI) if err != nil { return nil, err } - fields := moveReqProto.Extra.AsMap() - if extra, err := utils.AssertType[map[string]interface{}](fields["fields"]); err == nil { - v, err := structpb.NewStruct(extra) - if err != nil { - return nil, err - } - moveReqProto.Extra = v - } + // Special handling: we want to observe the logs just for the DoCommand obsLogger := ms.logger.Sublogger("observed-" + uuid.New().String()) observerCore, observedLogs := observer.New(zap.LevelEnablerFunc(zapcore.InfoLevel.Enabled)) obsLogger.AddAppender(observerCore) - moveReq, err := motion.MoveReqFromProto(&moveReqProto) - if err != nil { - return nil, err - } - plan, err := ms.plan(ctx, moveReq, obsLogger) + plan, err := ms.plan(ctx, *moveReq, obsLogger) if err != nil { return nil, err } @@ -426,11 +415,6 @@ 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) - if movingFrame == nil { - return nil, fmt.Errorf("component named %s not found in robot frame system", req.ComponentName) - } - startState, waypoints, err := waypointsFromRequest(req, fsInputs) if err != nil { return nil, err @@ -698,3 +682,32 @@ func (ms *builtIn) writePlanRequest(req *armplanning.PlanRequest, plan motionpla ms.logger.Infof("writing plan to %s", fn) return req.WriteToFile(fn) } + +func stringToMoveRequest(reqI any) (*motion.MoveReq, error) { + reqStr, err := utils.AssertType[string](reqI) + if err != nil { + return nil, err + } + + var moveReqProto pb.MoveRequest + err = protojson.Unmarshal([]byte(reqStr), &moveReqProto) + if err != nil { + return nil, err + } + + fields := moveReqProto.Extra.AsMap() + if extra, err := utils.AssertType[map[string]interface{}](fields["fields"]); err == nil { + v, err := structpb.NewStruct(extra) + if err != nil { + return nil, err + } + moveReqProto.Extra = v + } + + moveReq, err := motion.MoveReqFromProto(&moveReqProto) + if err != nil { + return nil, err + } + + return &moveReq, nil +} diff --git a/services/motion/builtin/replan.go b/services/motion/builtin/replan.go new file mode 100644 index 00000000000..523c4a25f12 --- /dev/null +++ b/services/motion/builtin/replan.go @@ -0,0 +1,192 @@ +package builtin + +import ( + "context" + "errors" + "fmt" + "time" + + "golang.org/x/sync/errgroup" + + "go.viam.com/rdk/motionplan/armplanning" + "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/robot/framesystem" + "go.viam.com/rdk/services/vision" + "go.viam.com/rdk/spatialmath" + "go.viam.com/rdk/utils" +) + +func getExtraObstacles(ctx context.Context, vs vision.Service) (*referenceframe.GeometriesInFrame, error) { + if vs == nil { + return nil, nil + } + + const hardCodedCameraNameFromTest = "" + currObjects, err := vs.GetObjectPointClouds(ctx, hardCodedCameraNameFromTest, nil) + if err != nil { + return nil, err + } + + geoms := make([]spatialmath.Geometry, len(currObjects)) + for idx, newObstacle := range currObjects { + geoms[idx] = newObstacle.Geometry + } + + // Dan: TODO: Test is hardcoded to return objects in the world frame. + return referenceframe.NewGeometriesInFrame("world", geoms), nil +} + +func (ms *builtIn) doReplannable(ctx context.Context, reqI any) (map[string]any, error) { + moveRequest, err := stringToMoveRequest(reqI) + if err != nil { + return nil, err + } + + frameSys, err := ms.getFrameSystem(ctx, moveRequest.WorldState.Transforms()) + if err != nil { + return nil, err + } + + var obstacleVisionService vision.Service + obsVisSvcNameI, exists := moveRequest.Extra["obstacleVisionService"] + if exists { + obsVisSvcName, ok := obsVisSvcNameI.(string) + if !ok { + return nil, fmt.Errorf("MoveRequest `obstacleVisionService` param is not a string: %T", obsVisSvcNameI) + } + + obstacleVisionService, exists = ms.visionServices[obsVisSvcName] + if !exists { + return nil, fmt.Errorf("MoveRequest `obstacleVisionService` does not exist. Name: %v", obsVisSvcName) + } + } + + baseWorldState := moveRequest.WorldState + var extraObstacles *referenceframe.GeometriesInFrame + for ctx.Err() == nil { + // build maps of relevant components and inputs from initial inputs + fsInputs, err := ms.fsService.CurrentInputs(ctx) + if err != nil { + return nil, err + } + + extraObstacles, err = getExtraObstacles(ctx, obstacleVisionService) + if err != nil { + ms.logger.Warnw("Error querying for obstacles. Delaying motion.", "err", err) + time.Sleep(10 * time.Millisecond) + continue + } + + worldState := baseWorldState.Merge(extraObstacles) + // Generate a plan for execution. + motionPlanRequest := &armplanning.PlanRequest{ + FrameSystem: frameSys, + StartState: armplanning.NewPlanState(nil, fsInputs), + Goals: []*armplanning.PlanState{ + armplanning.NewPlanState(referenceframe.FrameSystemPoses{ + moveRequest.ComponentName: moveRequest.Destination, + }, nil), + }, + WorldState: worldState, + Constraints: moveRequest.Constraints, + PlannerOptions: nil, + } + + plan, _, err := armplanning.PlanMotion(ctx, ms.logger, motionPlanRequest) + ms.logger.CDebugf(ctx, + "Replannable motion planning request. Start: %v Goal: %v NumObstacles: %v NumPlannedWaypoints: %v Err: %v", + fsInputs[moveRequest.ComponentName], moveRequest.Destination, len(worldState.Obstacles()), + plan.Trajectory(), err) + if err != nil { + return nil, err + } + + // While executing, the world state might change (an obstacle later comes into existence + // that must be avoided). If an `obstacleVisionService` exists, set up a background + // goroutine for polling the vision service for new obstacles. And validate if the executing + // plan continues to be safe. + // + // If the plan is no longer safe, the `executeCtx` will be canceled. Stopping the + // arm/actuator and allowing us to replan with the new information. + obstacleAvoidance, executeCtx := errgroup.WithContext(ctx) + executeCtx, executeDone := context.WithCancel(executeCtx) + defer func() { + executeDone() + //nolint:errcheck + obstacleAvoidance.Wait() + }() + + if obstacleVisionService != nil { + obstacleAvoidance.Go(func() error { + for executeCtx.Err() == nil { + // TODO: Vision service objects are returned from the camera reference frame. + geomsInFrame, err := getExtraObstacles(ctx, obstacleVisionService) + if err != nil { + ms.logger.Warnw("Getting extra obstacles error", "err", err) + return err + } + + motionPlanRequest.WorldState = baseWorldState.Merge(geomsInFrame) + // Dan: Is it safe to overwrite the world state field on the motion plan + // request? My expectation is that this goroutine must exit before the parent + // goroutine will re-invoke `PlanMotion`. Can always make some shallow-ish copy, + // but would prefer to avoid as that would be brittle. + // + // If the `motionPlanRequest` used `PlannerOptions.MeshesAsOctrees`, make sure + // validation uses an octree representation. With the exception of new obstacles + // found in the `mergedWorldState`. + validateErr := armplanning.ValidatePlan(ctx, plan, motionPlanRequest, ms.logger) + if validateErr != nil { + ms.logger.Infow("Validate plan returned error. Canceling execution.", "err", validateErr) + return validateErr + } + + time.Sleep(10 * time.Millisecond) + } + + return nil + }) + } + + executeLoop: + for _, traj := range plan.Trajectory() { + for actuatorName, inputs := range traj { + if len(inputs) == 0 { + continue + } + + actuator, ok := ms.components[actuatorName] + if !ok { + return nil, fmt.Errorf("Actuator in plan to move does not exist. Name: %v", actuatorName) + } + + ie, err := utils.AssertType[framesystem.InputEnabled](actuator) + if err != nil { + return nil, err + } + + ms.logger.CDebugf(ctx, "Issuing GoToInputs. Actuator: %v Inputs: %v", actuatorName, inputs) + if err = ie.GoToInputs(executeCtx, inputs); err != nil { + ms.logger.Debug("DBG. GoToInputs error:", err, "Ctx:", ctx.Err(), "ExecuteCtx:", executeCtx.Err()) + if errors.Is(err, context.Canceled) { + break executeLoop + } + + return nil, err + } + } + } + + if executeCtx.Err() == nil { + // We've completed executing through `Trajectory` without being interrupted. We're at + // our destination and can return success. + return nil, nil + } + + // If `executeCtx` had an error, the `obstacleAvoidance` background goroutine returned an + // error. We do not need to explicitly `Wait` on it. + } + + // If the `executeCtx` was canceled because `ctx` was canceled. We return with an error. + return nil, ctx.Err() +} diff --git a/spatialmath/box.go b/spatialmath/box.go index ade1b6b11ec..97f192e422b 100644 --- a/spatialmath/box.go +++ b/spatialmath/box.go @@ -62,6 +62,23 @@ type box struct { once sync.Once } +// NewBoxGoodInput instantiates a new box Geometry and panics if the inputs are bad. +func NewBoxGoodInput(pose Pose, dims r3.Vector, label string) Geometry { + // Negative dimensions not allowed. Zero dimensions are allowed for bounding boxes, etc. + if dims.X < 0 || dims.Y < 0 || dims.Z < 0 { + panic("you promised good input") + } + + halfSize := dims.Mul(0.5) + return &box{ + center: pose, + centerPt: pose.Point(), + halfSize: [3]float64{halfSize.X, halfSize.Y, halfSize.Z}, + boundingSphereR: halfSize.Norm(), + label: label, + } +} + // NewBox instantiates a new box Geometry. func NewBox(pose Pose, dims r3.Vector, label string) (Geometry, error) { // Negative dimensions not allowed. Zero dimensions are allowed for bounding boxes, etc. diff --git a/web/cmd/droid/droid.go b/web/cmd/droid/droid.go index 149685f7c88..2886e78c160 100644 --- a/web/cmd/droid/droid.go +++ b/web/cmd/droid/droid.go @@ -28,7 +28,7 @@ func MainEntry(configPath, writeablePath, osEnv string) { os.Args = append(os.Args, "-config", configPath) for _, envEntry := range strings.Split(osEnv, "\n") { entryParts := strings.SplitN(envEntry, "=", 2) - os.Setenv(entryParts[0], entryParts[1]) //nolint:errcheck,gosec + os.Setenv(entryParts[0], entryParts[1]) //nolint:errcheck } utils.ContextualMain(server.RunServer, logger) } diff --git a/web/networkcheck/network-check.go b/web/networkcheck/network-check.go index 82392fc1790..a2a6ccf92e8 100644 --- a/web/networkcheck/network-check.go +++ b/web/networkcheck/network-check.go @@ -131,7 +131,7 @@ func testDNSServerConnectivity(ctx context.Context, dnsServer string) *DNSResult case <-ctx.Done(): case <-testDNSServerConnectivityDone: } - conn.Close() //nolint:gosec,errcheck + conn.Close() //nolint:errcheck }() // Send a simple DNS query for "google.com"'s A record. @@ -438,7 +438,7 @@ func testUDP(ctx context.Context, logger logging.Logger) error { case <-ctx.Done(): case <-testUDPDone: } - conn.Close() //nolint:gosec,errcheck + conn.Close() //nolint:errcheck }() // Build a STUN binding request to be used against all STUN servers. @@ -599,7 +599,7 @@ func testTCP(ctx context.Context, logger logging.Logger) error { } connMu.Lock() if conn != nil { - conn.Close() //nolint:gosec,errcheck + conn.Close() //nolint:errcheck } connMu.Unlock() }() @@ -651,7 +651,7 @@ func testTCP(ctx context.Context, logger logging.Logger) error { stunResponses = append(stunResponses, stunResponse) connMu.Lock() - conn.Close() //nolint:gosec,errcheck + conn.Close() //nolint:errcheck connMu.Unlock() }