diff --git a/.artifact/tree.json b/.artifact/tree.json index 3cfc67945bb..658824be885 100644 --- a/.artifact/tree.json +++ b/.artifact/tree.json @@ -52067,6 +52067,10 @@ "size": 10484 } }, + "exampleDepth.png": { + "hash": "8a00e4dd4e779d185dfc26116e6d239a", + "size": 58418 + }, "keypoints": { "chess.jpg": { "hash": "66c0c0baa3b39ac23f90e7f0493e5d55", diff --git a/.github/workflows/cli.yml b/.github/workflows/cli.yml new file mode 100644 index 00000000000..5f3d30f829d --- /dev/null +++ b/.github/workflows/cli.yml @@ -0,0 +1,66 @@ +name: Viam CLI + +on: + workflow_dispatch: + inputs: + release_type: + required: true + type: string + default: latest + workflow_call: + inputs: + release_type: + required: true + type: string + secrets: + GCP_CREDENTIALS: + required: true + +jobs: + viam-cli: + runs-on: ubuntu-latest + steps: + - uses: actions/setup-go@v4 + with: + go-version: '1.20.x' + + - name: Check out code + if: github.event_name == 'workflow_dispatch' || github.event_name == 'push' + uses: actions/checkout@v3 + - name: Check out PR branch code + if: github.event_name == 'pull_request_target' + uses: actions/checkout@v3 + with: + ref: ${{ github.event.pull_request.head.sha }} + + - name: build + env: + CI_RELEASE: ${{ inputs.release_type }} + run: | + GOOS=linux GOARCH=amd64 make cli-ci + GOOS=linux GOARCH=arm64 make cli-ci + GOOS=darwin GOARCH=amd64 make cli-ci + GOOS=darwin GOARCH=arm64 make cli-ci + + - name: tagged alias + env: + CI_RELEASE: ${{ github.ref_name }} + if: inputs.release_type == 'stable' + run: | + GOOS=linux GOARCH=amd64 make cli-ci + GOOS=linux GOARCH=arm64 make cli-ci + GOOS=darwin GOARCH=amd64 make cli-ci + GOOS=darwin GOARCH=arm64 make cli-ci + + - name: Authorize GCP Upload + uses: google-github-actions/auth@v1 + with: + credentials_json: '${{ secrets.GCP_CREDENTIALS }}' + - name: upload + uses: google-github-actions/upload-cloud-storage@v0.10.4 + with: + headers: "cache-control: no-cache" + path: 'bin/deploy-ci' + glob: 'viam-cli-*' + destination: 'packages.viam.com/apps/viam-cli/' + parent: false diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index d1b4b44523c..9a16870435b 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -1,6 +1,6 @@ name: Build and Publish Latest -concurrency: +concurrency: group: ${{ github.workflow }}-${{ github.ref }} on: @@ -36,6 +36,13 @@ jobs: secrets: GCP_CREDENTIALS: ${{ secrets.GCP_CREDENTIALS }} + cli: + uses: viamrobotics/rdk/.github/workflows/cli.yml@main + with: + release_type: 'latest' + secrets: + GCP_CREDENTIALS: ${{ secrets.GCP_CREDENTIALS }} + npm_publish: uses: viamrobotics/rdk/.github/workflows/npm-publish.yml@main needs: test diff --git a/.github/workflows/stable.yml b/.github/workflows/stable.yml index 9b31b0d965e..badb4b1dd9b 100644 --- a/.github/workflows/stable.yml +++ b/.github/workflows/stable.yml @@ -34,6 +34,14 @@ jobs: secrets: GCP_CREDENTIALS: ${{ secrets.GCP_CREDENTIALS }} + cli: + needs: test + uses: viamrobotics/rdk/.github/workflows/cli.yml@main + with: + release_type: 'stable' + secrets: + GCP_CREDENTIALS: ${{ secrets.GCP_CREDENTIALS }} + slack-workflow-status: if: ${{ failure() }} name: Post Workflow Status To Slack diff --git a/.gitignore b/.gitignore index f90e5937b9e..2dce370f0a5 100644 --- a/.gitignore +++ b/.gitignore @@ -42,6 +42,7 @@ coverage2.txt coverage.xml profile*.pdf code-coverage-results.md +metadata *~ *# diff --git a/Makefile b/Makefile index 983600bd819..7ebdeabb937 100644 --- a/Makefile +++ b/Makefile @@ -18,6 +18,21 @@ build: build-web build-go build-go: go build ./... +GOOS ?= $(shell go env GOOS) +GOARCH ?= $(shell go env GOARCH) +bin/$(GOOS)-$(GOARCH)/viam-cli: + go build $(LDFLAGS) -tags osusergo,netgo -o $@ ./cli/viam + +.PHONY: cli +cli: bin/$(GOOS)-$(GOARCH)/viam-cli + +.PHONY: cli-ci +cli-ci: bin/$(GOOS)-$(GOARCH)/viam-cli + if [ -n "$(CI_RELEASE)" ]; then \ + mkdir -p bin/deploy-ci/; \ + cp $< bin/deploy-ci/viam-cli-$(CI_RELEASE)-$(GOOS)-$(GOARCH); \ + fi + build-web: web/runtime-shared/static/control.js # only generate static files when source has changed. diff --git a/cli/README.md b/cli/README.md index 8dac1c88086..8afdf8684a7 100644 --- a/cli/README.md +++ b/cli/README.md @@ -2,7 +2,7 @@ This is an experimental feature, so things may change without notice. -Install with `go build -o ~/go/bin/viam cli/cmd/main.go` +Install with `go build -o ~/go/bin/viam cli/viam/main.go` ### Getting Started Enter `viam auth` and follow instructions to authenticate. diff --git a/cli/app.go b/cli/app.go new file mode 100644 index 00000000000..ef0b7ffc3fc --- /dev/null +++ b/cli/app.go @@ -0,0 +1,725 @@ +package cli + +import ( + "fmt" + "io" + + "github.com/urfave/cli/v2" +) + +// CLI flags. +const ( + baseURLFlag = "base-url" + configFlag = "config" + debugFlag = "debug" + organizationFlag = "organization" + locationFlag = "location" + robotFlag = "robot" + partFlag = "part" + + logsFlagErrors = "errors" + logsFlagTail = "tail" + + runFlagData = "data" + runFlagStream = "stream" + + apiKeyCreateFlagOrgID = "org-id" + apiKeyCreateFlagName = "name" + + loginFlagKeyID = "key-id" + loginFlagKey = "key" + + moduleFlagName = "name" + moduleFlagPublicNamespace = "public-namespace" + moduleFlagOrgID = "org-id" + moduleFlagPath = "module" + moduleFlagVersion = "version" + moduleFlagPlatform = "platform" + moduleFlagForce = "force" + + dataFlagDestination = "destination" + dataFlagDataType = "data-type" + dataFlagOrgIDs = "org-ids" + dataFlagLocationIDs = "location-ids" + dataFlagRobotID = "robot-id" + dataFlagPartID = "part-id" + dataFlagRobotName = "robot-name" + dataFlagPartName = "part-name" + dataFlagComponentType = "component-type" + dataFlagComponentName = "component-name" + dataFlagMethod = "method" + dataFlagMimeTypes = "mime-types" + dataFlagStart = "start" + dataFlagEnd = "end" + dataFlagParallelDownloads = "parallel" + dataFlagTags = "tags" + dataFlagBboxLabels = "bbox-labels" + dataFlagOrgID = "org-id" + dataFlagDeleteTabularDataOlderThanDays = "delete-older-than-days" + + boardFlagName = "name" + boardFlagPath = "path" + boardFlagVersion = "version" +) + +var app = &cli.App{ + Name: "viam", + Usage: "interact with your Viam robots", + HideHelpCommand: true, + Flags: []cli.Flag{ + &cli.StringFlag{ + Name: baseURLFlag, + Hidden: true, + Usage: "base URL of app", + }, + &cli.StringFlag{ + Name: configFlag, + Aliases: []string{"c"}, + Usage: "load configuration from `FILE`", + }, + &cli.BoolFlag{ + Name: debugFlag, + Aliases: []string{"vvv"}, + Usage: "enable debug logging", + }, + }, + Commands: []*cli.Command{ + { + Name: "login", + // NOTE(benjirewis): maintain `auth` as an alias for backward compatibility. + Aliases: []string{"auth"}, + Usage: "login to app.viam.com", + HideHelpCommand: true, + Action: LoginAction, + Subcommands: []*cli.Command{ + { + Name: "print-access-token", + Usage: "print the access token associated with current credentials", + Action: PrintAccessTokenAction, + }, + { + Name: "api-key", + Usage: "authenticate with an api key", + Flags: []cli.Flag{ + &cli.StringFlag{ + Name: loginFlagKeyID, + Required: true, + Usage: "id of the key to authenticate with", + }, + &cli.StringFlag{ + Name: loginFlagKey, + Required: true, + Usage: "key to authenticate with", + }, + }, + Action: LoginWithAPIKeyAction, + }, + }, + }, + { + Name: "logout", + Usage: "logout from current session", + Action: LogoutAction, + }, + { + Name: "whoami", + Usage: "get currently logged-in user", + Action: WhoAmIAction, + }, + { + Name: "organizations", + Usage: "work with organizations", + HideHelpCommand: true, + Subcommands: []*cli.Command{ + { + Name: "list", + Usage: "list organizations for the current user", + Action: ListOrganizationsAction, + }, + }, + }, + { + Name: "organization", + Usage: "work with a organization", + HideHelpCommand: true, + Subcommands: []*cli.Command{ + { + Name: "api-key", + Usage: "work with an organization's api keys", + Subcommands: []*cli.Command{ + { + Name: "create", + Usage: "create an api key for your organization", + Flags: []cli.Flag{ + &cli.StringFlag{ + Name: apiKeyCreateFlagOrgID, + Required: true, + Usage: "the org to create an api key for", + }, + &cli.StringFlag{ + Name: apiKeyCreateFlagName, + Usage: "the name of the key (defaults to your login info with the current time)", + }, + }, + Action: OrganizationAPIKeyCreateAction, + }, + }, + }, + }, + }, + { + Name: "locations", + Usage: "work with locations", + HideHelpCommand: true, + Subcommands: []*cli.Command{ + { + Name: "list", + Usage: "list locations for the current user", + ArgsUsage: "[organization]", + Action: ListLocationsAction, + }, + }, + }, + { + Name: "data", + Usage: "work with data", + HideHelpCommand: true, + Subcommands: []*cli.Command{ + { + Name: "export", + Usage: "download data from Viam cloud", + UsageText: fmt.Sprintf("viam data export <%s> <%s> [other options]", + dataFlagDestination, dataFlagDataType), + Flags: []cli.Flag{ + &cli.PathFlag{ + Name: dataFlagDestination, + Required: true, + Usage: "output directory for downloaded data", + }, + &cli.StringFlag{ + Name: dataFlagDataType, + Required: true, + Usage: "data type to be downloaded: either binary or tabular", + }, + &cli.StringSliceFlag{ + Name: dataFlagOrgIDs, + Usage: "orgs filter", + }, + &cli.StringSliceFlag{ + Name: dataFlagLocationIDs, + Usage: "locations filter", + }, + &cli.StringFlag{ + Name: dataFlagRobotID, + Usage: "robot-id filter", + }, + &cli.StringFlag{ + Name: dataFlagPartID, + Usage: "part id filter", + }, + &cli.StringFlag{ + Name: dataFlagRobotName, + Usage: "robot name filter", + }, + &cli.StringFlag{ + Name: dataFlagPartName, + Usage: "part name filter", + }, + &cli.StringFlag{ + Name: dataFlagComponentType, + Usage: "component type filter", + }, + &cli.StringFlag{ + Name: dataFlagComponentName, + Usage: "component name filter", + }, + &cli.StringFlag{ + Name: dataFlagMethod, + Usage: "method filter", + }, + &cli.StringSliceFlag{ + Name: dataFlagMimeTypes, + Usage: "mime types filter", + }, + &cli.UintFlag{ + Name: dataFlagParallelDownloads, + Usage: "number of download requests to make in parallel", + DefaultText: "10", + }, + &cli.StringFlag{ + Name: dataFlagStart, + Usage: "ISO-8601 timestamp indicating the start of the interval filter", + }, + &cli.StringFlag{ + Name: dataFlagEnd, + Usage: "ISO-8601 timestamp indicating the end of the interval filter", + }, + &cli.StringSliceFlag{ + Name: dataFlagTags, + Usage: "tags filter. " + + "accepts tagged for all tagged data, untagged for all untagged data, or a list of tags for all data matching any of the tags", + }, + &cli.StringSliceFlag{ + Name: dataFlagBboxLabels, + Usage: "bbox labels filter. " + + "accepts string labels corresponding to bounding boxes within images", + }, + }, + Action: DataExportAction, + }, + { + Name: "delete", + Usage: "delete binary data from Viam cloud", + UsageText: fmt.Sprintf("viam data delete <%s> [other options]", dataFlagDataType), + Flags: []cli.Flag{ + &cli.StringFlag{ + Name: dataFlagDataType, + Required: true, + Usage: "data type to be deleted. should only be binary. if tabular, use delete-tabular instead.", + }, + &cli.StringSliceFlag{ + Name: dataFlagOrgIDs, + Usage: "orgs filter", + }, + &cli.StringSliceFlag{ + Name: dataFlagLocationIDs, + Usage: "locations filter", + }, + &cli.StringFlag{ + Name: dataFlagRobotID, + Usage: "robot id filter", + }, + &cli.StringFlag{ + Name: dataFlagPartID, + Usage: "part id filter", + }, + &cli.StringFlag{ + Name: dataFlagRobotName, + Usage: "robot name filter", + }, + &cli.StringFlag{ + Name: dataFlagPartName, + Usage: "part name filter", + }, + &cli.StringFlag{ + Name: dataFlagComponentType, + Usage: "component type filter", + }, + &cli.StringFlag{ + Name: dataFlagComponentName, + Usage: "component name filter", + }, + &cli.StringFlag{ + Name: dataFlagMethod, + Usage: "method filter", + }, + &cli.StringSliceFlag{ + Name: dataFlagMimeTypes, + Usage: "mime types filter", + }, + &cli.StringFlag{ + Name: dataFlagStart, + Usage: "ISO-8601 timestamp indicating the start of the interval filter", + }, + &cli.StringFlag{ + Name: dataFlagEnd, + Usage: "ISO-8601 timestamp indicating the end of the interval filter", + }, + }, + Action: DataDeleteBinaryAction, + }, + { + Name: "delete-tabular", + Usage: "delete tabular data from Viam cloud", + UsageText: "viam data delete-tabular [other options]", + Flags: []cli.Flag{ + &cli.StringFlag{ + Name: dataFlagOrgID, + Usage: "org", + Required: true, + }, + &cli.IntFlag{ + Name: dataFlagDeleteTabularDataOlderThanDays, + Usage: "delete any tabular data that is older than X calendar days before now. 0 deletes all data.", + Required: true, + }, + }, + Action: DataDeleteTabularAction, + }, + }, + }, + { + Name: "robots", + Usage: "work with robots", + HideHelpCommand: true, + Subcommands: []*cli.Command{ + { + Name: "list", + Usage: "list robots in an organization and location", + Flags: []cli.Flag{ + &cli.StringFlag{ + Name: organizationFlag, + DefaultText: "first organization alphabetically", + }, + &cli.StringFlag{ + Name: locationFlag, + DefaultText: "first location alphabetically", + }, + }, + Action: ListRobotsAction, + }, + }, + }, + { + Name: "robot", + Usage: "work with a robot", + HideHelpCommand: true, + Subcommands: []*cli.Command{ + { + Name: "status", + Usage: "display robot status", + UsageText: "viam robot status [other options]", + Flags: []cli.Flag{ + &cli.StringFlag{ + Name: organizationFlag, + DefaultText: "first organization alphabetically", + }, + &cli.StringFlag{ + Name: locationFlag, + DefaultText: "first location alphabetically", + }, + &cli.StringFlag{ + Name: robotFlag, + Required: true, + }, + }, + Action: RobotStatusAction, + }, + { + Name: "logs", + Usage: "display robot logs", + UsageText: "viam robot logs [other options]", + Flags: []cli.Flag{ + &cli.StringFlag{ + Name: organizationFlag, + DefaultText: "first organization alphabetically", + }, + &cli.StringFlag{ + Name: locationFlag, + DefaultText: "first location alphabetically", + }, + &cli.StringFlag{ + Name: robotFlag, + Required: true, + }, + &cli.BoolFlag{ + Name: logsFlagErrors, + Usage: "show only errors", + }, + }, + Action: RobotLogsAction, + }, + { + Name: "part", + Usage: "work with a robot part", + HideHelpCommand: true, + Subcommands: []*cli.Command{ + { + Name: "status", + Usage: "display part status", + UsageText: "viam robot part status [other options]", + Flags: []cli.Flag{ + &cli.StringFlag{ + Name: organizationFlag, + DefaultText: "first organization alphabetically", + }, + &cli.StringFlag{ + Name: locationFlag, + DefaultText: "first location alphabetically", + }, + &cli.StringFlag{ + Name: robotFlag, + Required: true, + }, + &cli.StringFlag{ + Name: partFlag, + Required: true, + }, + }, + Action: RobotPartStatusAction, + }, + { + Name: "logs", + Usage: "display part logs", + UsageText: "viam robot part logs [other options]", + Flags: []cli.Flag{ + &cli.StringFlag{ + Name: organizationFlag, + DefaultText: "first organization alphabetically", + }, + &cli.StringFlag{ + Name: locationFlag, + DefaultText: "first location alphabetically", + }, + &cli.StringFlag{ + Name: robotFlag, + Required: true, + }, + &cli.StringFlag{ + Name: partFlag, + Required: true, + }, + &cli.BoolFlag{ + Name: logsFlagErrors, + Usage: "show only errors", + }, + &cli.BoolFlag{ + Name: logsFlagTail, + Aliases: []string{"f"}, + Usage: "follow logs", + }, + }, + Action: RobotPartLogsAction, + }, + { + Name: "run", + Usage: "run a command on a robot part", + UsageText: "viam robot part run [other options] ", + Flags: []cli.Flag{ + &cli.StringFlag{ + Name: organizationFlag, + Required: true, + }, + &cli.StringFlag{ + Name: locationFlag, + Required: true, + }, + &cli.StringFlag{ + Name: robotFlag, + Required: true, + }, + &cli.StringFlag{ + Name: partFlag, + Required: true, + }, + &cli.StringFlag{ + Name: runFlagData, + Aliases: []string{"d"}, + }, + &cli.DurationFlag{ + Name: runFlagStream, + Aliases: []string{"s"}, + }, + }, + Action: RobotPartRunAction, + }, + { + Name: "shell", + Usage: "start a shell on a robot part", + Description: `In order to use the shell command, the robot must have a valid shell type service.`, + UsageText: "viam robot part shell ", + Flags: []cli.Flag{ + &cli.StringFlag{ + Name: organizationFlag, + Required: true, + }, + &cli.StringFlag{ + Name: locationFlag, + Required: true, + }, + &cli.StringFlag{ + Name: robotFlag, + Required: true, + }, + &cli.StringFlag{ + Name: partFlag, + Required: true, + }, + }, + Action: RobotPartShellAction, + }, + }, + }, + }, + }, + { + Name: "module", + Usage: "manage your modules in Viam's registry", + HideHelpCommand: true, + Subcommands: []*cli.Command{ + { + Name: "create", + Usage: "create & register a module on app.viam.com", + Description: `Creates a module in app.viam.com to simplify code deployment. +Ex: 'viam module create --name my-great-module --org-id ' +Will create the module and a corresponding meta.json file in the current directory. + +If your org has set a namespace in app.viam.com then your module name will be 'my-namespace:my-great-module' and +you won't have to pass a namespace or org-id in future commands. Otherwise there will be no namespace +and you will have to provide the org-id to future cli commands. You cannot make your module public until you claim an org-id. + +After creation, use 'viam module update' to push your new module to app.viam.com.`, + UsageText: "viam module create [other options]", + Flags: []cli.Flag{ + &cli.StringFlag{ + Name: moduleFlagName, + Usage: "name of your module (cannot be changed once set)", + Required: true, + }, + &cli.StringFlag{ + Name: moduleFlagPublicNamespace, + Usage: "the public namespace where the module will reside (alternative way of specifying the org id)", + }, + &cli.StringFlag{ + Name: moduleFlagOrgID, + Usage: "id of the organization that will host the module", + }, + }, + Action: CreateModuleAction, + }, + { + Name: "update", + Usage: "update a module's metadata on app.viam.com", + Flags: []cli.Flag{ + &cli.StringFlag{ + Name: moduleFlagPath, + Usage: "path to meta.json", + DefaultText: "./meta.json", + TakesFile: true, + }, + &cli.StringFlag{ + Name: moduleFlagPublicNamespace, + Usage: "the public namespace where the module resides (alternative way of specifying the org id)", + }, + &cli.StringFlag{ + Name: moduleFlagOrgID, + Usage: "id of the organization that hosts the module", + }, + }, + Action: UpdateModuleAction, + }, + { + Name: "upload", + Usage: "upload a new version of your module", + Description: `Upload an archive containing your module's file(s) for a specified platform + +Example for linux/amd64: +tar -czf packaged-module.tar.gz my-binary # the meta.json entrypoint is relative to the root of the archive, so it should be "./my-binary" +viam module upload --version "0.1.0" --platform "linux/amd64" packaged-module.tar.gz + `, + UsageText: "viam module upload [other options] ", + Flags: []cli.Flag{ + &cli.StringFlag{ + Name: moduleFlagPath, + Usage: "path to meta.json", + DefaultText: "./meta.json", + TakesFile: true, + }, + &cli.StringFlag{ + Name: moduleFlagPublicNamespace, + Usage: "the public namespace where the module resides (alternative way of specifying the org id)", + }, + &cli.StringFlag{ + Name: moduleFlagOrgID, + Usage: "id of the organization that hosts the module", + }, + &cli.StringFlag{ + Name: moduleFlagName, + Usage: "name of the module (used if you don't have a meta.json)", + }, + &cli.StringFlag{ + Name: moduleFlagVersion, + Usage: "version of the module to upload (semver2.0) ex: \"0.1.0\"", + Required: true, + }, + &cli.StringFlag{ + Name: moduleFlagPlatform, + Usage: `platform of the binary you are uploading. Must be one of: + linux/amd64 + linux/arm64 + darwin/amd64 (for intel macs) + darwin/arm64 (for non-intel macs)`, + Required: true, + }, + &cli.BoolFlag{ + Name: moduleFlagForce, + Usage: "skip validation (may result in non-functional versions)", + }, + }, + Action: UploadModuleAction, + }, + }, + }, + { + Name: "version", + Usage: "print version info for this program", + Action: VersionAction, + }, + { + Name: "board", + Usage: "manage your board definition files", + HideHelpCommand: true, + Subcommands: []*cli.Command{ + { + Name: "upload", + Usage: "upload a board definition file", + Description: `Upload a json board definition file for linux boards. +Example: +viam board upload --name=orin --org="my org" --version=1.0.0 file.json`, + UsageText: "viam board upload [other options] ", + Flags: []cli.Flag{ + &cli.StringFlag{ + Name: boardFlagName, + Usage: "name of your board definition file (cannot be changed once set)", + Required: true, + }, + &cli.StringFlag{ + Name: organizationFlag, + Usage: "organization that will host the board definitions file. This can be the org's ID or name", + Required: true, + }, + &cli.StringFlag{ + Name: boardFlagVersion, + Usage: "version of the file to upload (semver2.0) ex: \"0.1.0\"", + Required: true, + }, + }, + Action: UploadBoardDefsAction, + }, + { + Name: "download", + Usage: "download a board definitions package", + Description: `download a json board definitions file for generic linux boards. +Example: +viam board download --name=test --organization="my org" --version=1.0.0`, + UsageText: "viam board download [other options]", + Flags: []cli.Flag{ + &cli.StringFlag{ + Name: boardFlagName, + Usage: "name of the board definitions file to download", + Required: true, + }, + &cli.StringFlag{ + Name: organizationFlag, + Usage: "organization that hosts the board definitions file", + Required: true, + }, + &cli.StringFlag{ + Name: boardFlagVersion, + Usage: "version of the file to download. defaults to latest if not set.", + }, + }, + Action: DownloadBoardDefsAction, + }, + }, + }, + }, +} + +// NewApp returns a new app with the CLI API, Writer set to out, and ErrWriter +// set to errOut. +func NewApp(out, errOut io.Writer) *cli.App { + app.Writer = out + app.ErrWriter = errOut + return app +} diff --git a/cli/auth.go b/cli/auth.go index 39bbf4d6f3f..b33ef2cb910 100644 --- a/cli/auth.go +++ b/cli/auth.go @@ -7,6 +7,7 @@ import ( "io" "net/http" "net/url" + "os" "os/exec" "runtime" "strconv" @@ -16,7 +17,12 @@ import ( "github.com/edaniels/golog" "github.com/golang-jwt/jwt/v4" "github.com/pkg/errors" + "github.com/urfave/cli/v2" + datapb "go.viam.com/api/app/data/v1" + packagepb "go.viam.com/api/app/packages/v1" + apppb "go.viam.com/api/app/v1" "go.viam.com/utils" + "go.viam.com/utils/rpc" ) type authFlow struct { @@ -77,8 +83,18 @@ type tokenResponse struct { TokenType string `json:"token_type"` } -// Token contains an authorization token and details once logged in. -type Token struct { +type authMethod interface { + fmt.Stringer + dialOpts() rpc.DialOption +} + +var ( + _ authMethod = (*token)(nil) // Verify that *token implements authMethod. + _ authMethod = (*apiKey)(nil) // Verify that *apiKey implements authMethod. +) + +// token contains an authorization token and details once logged in. +type token struct { AccessToken string `json:"access_token"` RefreshToken string `json:"refresh_token"` IDToken string `json:"id_token"` @@ -87,21 +103,348 @@ type Token struct { TokenURL string `json:"token_url"` ClientID string `json:"client_id"` - User UserData `json:"user_data"` + User userData `json:"user_data"` +} + +// apiKey holds an id/value pair used to authenticate with app.viam. +type apiKey struct { + KeyID string `json:"key_id"` + KeyCrypto string `json:"key_crypto"` +} + +// LoginAction is the corresponding Action for 'login'. +func LoginAction(cCtx *cli.Context) error { + c, err := newViamClient(cCtx) + if err != nil { + return err + } + return c.loginAction(cCtx) +} + +func (c *viamClient) loginAction(cCtx *cli.Context) error { + loggedInMessage := func(t *token, alreadyLoggedIn bool) { + already := "Already l" + if !alreadyLoggedIn { + already = "L" + viamLogo(cCtx.App.Writer) + } + + printf(cCtx.App.Writer, "%sogged in as %q, expires %s", already, t.User.Email, + t.ExpiresAt.Format("Mon Jan 2 15:04:05 MST 2006")) + } + + if _, isAPIKey := c.conf.Auth.(*apiKey); isAPIKey { + warningf(c.c.App.Writer, "was logged in with an api-key. logging out") + utils.UncheckedError(c.logout()) + } + currentToken, _ := c.conf.Auth.(*token) // currentToken can be nil + if currentToken != nil && !currentToken.isExpired() { + loggedInMessage(currentToken, true) + return nil + } + + var t *token + var err error + if currentToken != nil && currentToken.canRefresh() { + t, err = c.authFlow.refreshToken(c.c.Context, currentToken) + if err != nil { + utils.UncheckedError(c.logout()) + return err + } + } else { + t, err = c.authFlow.loginAsUser(c.c.Context) + if err != nil { + return err + } + } + + // write token to config. + c.conf.Auth = t + if err := storeConfigToCache(c.conf); err != nil { + return err + } + + loggedInMessage(t, false) + return nil +} + +// LoginWithAPIKeyAction is the corresponding Action for `login api-key`. +func LoginWithAPIKeyAction(cCtx *cli.Context) error { + c, err := newViamClient(cCtx) + if err != nil { + return err + } + return c.loginWithAPIKeyAction(cCtx) } -// IsExpired returns true if the token is expired. -func (t *Token) IsExpired() bool { +func (c viamClient) loginWithAPIKeyAction(cCtx *cli.Context) error { + key := apiKey{ + KeyID: cCtx.String(loginFlagKeyID), + KeyCrypto: cCtx.String(loginFlagKey), + } + c.conf.Auth = &key + if err := storeConfigToCache(c.conf); err != nil { + return err + } + // test the connection + if _, err := c.listOrganizations(); err != nil { + return errors.Wrapf(err, "unable to connect to %q using the provided api key", c.conf.BaseURL) + } + printf(cCtx.App.Writer, "Successfully logged in with api key %q", key.KeyID) + return nil +} + +// PrintAccessTokenAction is the corresponding Action for 'print-access-token'. +func PrintAccessTokenAction(cCtx *cli.Context) error { + c, err := newViamClient(cCtx) + if err != nil { + return err + } + return c.printAccessTokenAction(cCtx) +} + +func (c *viamClient) printAccessTokenAction(cCtx *cli.Context) error { + if err := c.ensureLoggedIn(); err != nil { + return err + } + + if token, ok := c.conf.Auth.(*token); ok { + printf(cCtx.App.Writer, token.AccessToken) + } else { + return errors.New("not logged in as a user. Cannot print access token. Run \"viam login\" to sign in with your account") + } + return nil +} + +// LogoutAction is the corresponding Action for 'logout'. +func LogoutAction(cCtx *cli.Context) error { + // Create basic viam client; no need to check base URL. + conf, err := configFromCache() + if err != nil { + if !os.IsNotExist(err) { + return err + } + conf = &config{} + } + + vc := &viamClient{ + c: cCtx, + conf: conf, + } + return vc.logoutAction(cCtx) +} + +func (c *viamClient) logoutAction(cCtx *cli.Context) error { + auth := c.conf.Auth + if auth == nil { + printf(cCtx.App.Writer, "Already logged out") + return nil + } + if err := c.logout(); err != nil { + return errors.Wrap(err, "could not logout") + } + printf(cCtx.App.Writer, "Logged out from %q", auth) + return nil +} + +// WhoAmIAction is the corresponding Action for 'whoami'. +func WhoAmIAction(cCtx *cli.Context) error { + c, err := newViamClient(cCtx) + if err != nil { + return err + } + return c.whoAmIAction(cCtx) +} + +func (c *viamClient) whoAmIAction(cCtx *cli.Context) error { + auth := c.conf.Auth + if auth == nil { + warningf(cCtx.App.Writer, "Not logged in. Run \"login\" command") + return nil + } + printf(cCtx.App.Writer, "%s", auth) + return nil +} + +// OrganizationAPIKeyCreateAction corresponds to `organization api-key create`. +func OrganizationAPIKeyCreateAction(cCtx *cli.Context) error { + c, err := newViamClient(cCtx) + if err != nil { + return err + } + return c.organizationAPIKeyCreateAction(cCtx) +} + +func (c *viamClient) organizationAPIKeyCreateAction(cCtx *cli.Context) error { + if err := c.ensureLoggedIn(); err != nil { + return err + } + orgID := cCtx.String(apiKeyCreateFlagOrgID) + keyName := cCtx.String(apiKeyCreateFlagName) + if keyName == "" { + // Default name is in the form myusername@gmail.com-2009-11-10T23:00:00Z + // or key-uuid-2009-11-10T23:00:00Z if it was created by a key + keyName = fmt.Sprintf("%s-%s", c.conf.Auth, time.Now().Format(time.RFC3339)) + infof(cCtx.App.Writer, "Using default key name of %q", keyName) + } + resp, err := c.createOrganizationAPIKey(orgID, keyName) + if err != nil { + return err + } + infof(cCtx.App.Writer, "Successfully created key:") + printf(cCtx.App.Writer, "Key ID: %s\n", resp.GetId()) + printf(cCtx.App.Writer, "Key Value: %s\n\n", resp.GetKey()) + warningf(cCtx.App.Writer, "Keep this key somewhere safe; it has full write access to your organization") + return nil +} + +func (c *viamClient) createOrganizationAPIKey(orgID, keyName string) (*apppb.CreateKeyResponse, error) { + if err := c.ensureLoggedIn(); err != nil { + return nil, err + } + + req := &apppb.CreateKeyRequest{ + Authorizations: []*apppb.Authorization{ + { + AuthorizationType: "role", + AuthorizationId: "organization_owner", + ResourceType: "organization", + ResourceId: orgID, + IdentityId: "", + OrganizationId: orgID, + IdentityType: "api-key", + }, + }, + Name: keyName, + } + return c.client.CreateKey(c.c.Context, req) +} + +func (c *viamClient) ensureLoggedIn() error { + if c.client != nil { + return nil + } + + if c.conf.Auth == nil { + return errors.New("not logged in: run the following command to login:\n\tviam login") + } + + authToken, ok := c.conf.Auth.(*token) + if ok && authToken.isExpired() { + if !authToken.canRefresh() { + utils.UncheckedError(c.logout()) + return errors.New("token expired and cannot refresh") + } + + // expired. + newToken, err := c.authFlow.refreshToken(c.c.Context, authToken) + if err != nil { + utils.UncheckedError(c.logout()) // clear cache if failed to refresh + return errors.Wrapf(err, "error while refreshing token") + } + + // write token to config. + c.conf.Auth = newToken + if err := storeConfigToCache(c.conf); err != nil { + return err + } + } + + rpcOpts := append(c.copyRPCOpts(), c.conf.Auth.dialOpts()) + + conn, err := rpc.DialDirectGRPC( + c.c.Context, + c.baseURL.Host, + nil, + rpcOpts..., + ) + if err != nil { + return err + } + + c.client = apppb.NewAppServiceClient(conn) + c.dataClient = datapb.NewDataServiceClient(conn) + c.packageClient = packagepb.NewPackageServiceClient(conn) + return nil +} + +// logout logs out the client and clears the config. +func (c *viamClient) logout() error { + if err := removeConfigFromCache(); err != nil && !os.IsNotExist(err) { + return err + } + c.conf = &config{} + return nil +} + +func (c *viamClient) prepareDial( + orgStr, locStr, robotStr, partStr string, + debug bool, +) (context.Context, string, []rpc.DialOption, error) { + if err := c.ensureLoggedIn(); err != nil { + return nil, "", nil, err + } + if err := c.selectOrganization(orgStr); err != nil { + return nil, "", nil, err + } + if err := c.selectLocation(locStr); err != nil { + return nil, "", nil, err + } + + part, err := c.robotPart(c.selectedOrg.Id, c.selectedLoc.Id, robotStr, partStr) + if err != nil { + return nil, "", nil, err + } + + rpcDialer := rpc.NewCachedDialer() + defer func() { + utils.UncheckedError(rpcDialer.Close()) + }() + dialCtx := rpc.ContextWithDialer(c.c.Context, rpcDialer) + + rpcOpts := append(c.copyRPCOpts(), + rpc.WithExternalAuth(c.baseURL.Host, part.Fqdn), + c.conf.Auth.dialOpts(), + ) + + if debug { + rpcOpts = append(rpcOpts, rpc.WithDialDebug()) + } + + return dialCtx, part.Fqdn, rpcOpts, nil +} + +func (t *token) isExpired() bool { return t.ExpiresAt.Before(time.Now().Add(10 * time.Second)) } -// CanRefresh returns true if the token can be refreshed. -func (t *Token) CanRefresh() bool { +func (t *token) canRefresh() bool { return t.RefreshToken != "" && t.TokenURL != "" && t.ClientID != "" } -// UserData user details from login. -type UserData struct { +func (t *token) dialOpts() rpc.DialOption { + return rpc.WithStaticAuthenticationMaterial(t.AccessToken) +} + +func (t *token) String() string { + return t.User.Email +} + +func (k *apiKey) dialOpts() rpc.DialOption { + return rpc.WithEntityCredentials( + k.KeyID, + rpc.Credentials{ + Type: "api-key", + Payload: k.KeyCrypto, + }, + ) +} + +func (k *apiKey) String() string { + return fmt.Sprintf("key-%s", k.KeyID) +} + +type userData struct { jwt.Claims Email string `json:"email"` @@ -134,7 +477,7 @@ func newCLIAuthFlowWithAuthDomain(authDomain, audience, clientID string, console } } -func (a *authFlow) Login(ctx context.Context) (*Token, error) { +func (a *authFlow) loginAsUser(ctx context.Context) (*token, error) { discovery, err := a.loadOIDiscoveryEndpoint(ctx) if err != nil { return nil, errors.Wrapf(err, "failed retrieving discovery endpoint") @@ -142,7 +485,7 @@ func (a *authFlow) Login(ctx context.Context) (*Token, error) { deviceCode, err := a.makeDeviceCodeRequest(ctx, discovery) if err != nil { - return nil, errors.Wrapf(err, "failed return device code") + return nil, errors.Wrapf(err, "failed to return device code") } err = a.directUser(deviceCode) @@ -157,18 +500,18 @@ func (a *authFlow) Login(ctx context.Context) (*Token, error) { return buildToken(token, discovery.TokenEndPoint, a.clientID) } -func buildToken(token *tokenResponse, tokenURL, clientID string) (*Token, error) { - userData, err := userDataFromIDToken(token.IDToken) +func buildToken(t *tokenResponse, tokenURL, clientID string) (*token, error) { + userData, err := userDataFromIDToken(t.IDToken) if err != nil { return nil, err } - return &Token{ + return &token{ TokenType: tokenTypeUserOAuthToken, - AccessToken: token.AccessToken, - RefreshToken: token.RefreshToken, - IDToken: token.IDToken, - ExpiresAt: time.Now().Add(time.Second * time.Duration(token.ExpiresIn)), + AccessToken: t.AccessToken, + RefreshToken: t.RefreshToken, + IDToken: t.IDToken, + ExpiresAt: time.Now().Add(time.Second * time.Duration(t.ExpiresIn)), User: *userData, TokenURL: tokenURL, ClientID: clientID, @@ -214,7 +557,9 @@ func (a *authFlow) makeDeviceCodeRequest(ctx context.Context, discovery *openIDD } func (a *authFlow) directUser(code *deviceCodeResponse) error { - fmt.Fprintf(a.console, "To authorize this device, visit:\n\t%s\n", code.VerificationURIComplete) + infof(a.console, `You can log into Viam through the opened browser window or follow the URL below. +Ensure the code in the URL matches the one shown in your browser. + %s`, code.VerificationURIComplete) if a.disableBrowserOpen { return nil @@ -230,7 +575,7 @@ func (a *authFlow) waitForUser(ctx context.Context, code *deviceCodeResponse, di waitInterval := defaultWaitInterval for { if !utils.SelectContextOrWait(ctxWithTimeout, waitInterval) { - return nil, errors.New("timed out getting token ") + return nil, fmt.Errorf("timed out getting token after %f seconds", waitInterval.Seconds()) } data := url.Values{} @@ -305,8 +650,8 @@ func openbrowser(url string) error { return err } -func userDataFromIDToken(token string) (*UserData, error) { - userData := UserData{} +func userDataFromIDToken(token string) (*userData, error) { + userData := userData{} jwtParser := jwt.NewParser() // We assume the ID token returned form the authorization endpoint is going to give @@ -328,17 +673,13 @@ func userDataFromIDToken(token string) (*UserData, error) { return &userData, nil } -func (a *authFlow) Refresh(ctx context.Context, token *Token) (*Token, error) { - return refreshToken(ctx, a.httpClient, token) -} - -func refreshToken(ctx context.Context, httpClient *http.Client, token *Token) (*Token, error) { +func (a *authFlow) refreshToken(ctx context.Context, t *token) (*token, error) { data := url.Values{} - data.Set("client_id", token.ClientID) + data.Set("client_id", t.ClientID) data.Set("grant_type", "refresh_token") - data.Set("refresh_token", token.RefreshToken) + data.Set("refresh_token", t.RefreshToken) - req, err := http.NewRequestWithContext(ctx, http.MethodPost, token.TokenURL, strings.NewReader(data.Encode())) + req, err := http.NewRequestWithContext(ctx, http.MethodPost, t.TokenURL, strings.NewReader(data.Encode())) if err != nil { return nil, err } @@ -346,7 +687,7 @@ func refreshToken(ctx context.Context, httpClient *http.Client, token *Token) (* req.Header.Add("Content-Length", strconv.Itoa(len(data.Encode()))) //nolint:bodyclose // processTokenResponse() closes it - res, err := httpClient.Do(req) + res, err := a.httpClient.Do(req) if err != nil { return nil, err } @@ -358,7 +699,7 @@ func refreshToken(ctx context.Context, httpClient *http.Client, token *Token) (* return nil, errors.New("expecting new token") } - return buildToken(resp, token.TokenURL, token.ClientID) + return buildToken(resp, t.TokenURL, t.ClientID) } func processTokenResponse(res *http.Response) (*tokenResponse, error) { diff --git a/cli/auth_test.go b/cli/auth_test.go new file mode 100644 index 00000000000..eba5d8be0ca --- /dev/null +++ b/cli/auth_test.go @@ -0,0 +1,120 @@ +package cli + +import ( + "context" + "encoding/json" + "fmt" + "strings" + "testing" + + apppb "go.viam.com/api/app/v1" + "go.viam.com/test" + "google.golang.org/grpc" + + "go.viam.com/rdk/testutils/inject" +) + +func TestLoginAction(t *testing.T) { + cCtx, ac, out, errOut := setup(nil, nil) + + test.That(t, ac.loginAction(cCtx), test.ShouldBeNil) + test.That(t, len(errOut.messages), test.ShouldEqual, 0) + test.That(t, len(out.messages), test.ShouldEqual, 1) + test.That(t, out.messages[0], test.ShouldContainSubstring, + fmt.Sprintf("Already logged in as %q", testEmail)) +} + +func TestPrintAccessTokenAction(t *testing.T) { + // AppServiceClient needed for any Action that calls ensureLoggedIn. + cCtx, ac, out, errOut := setup(&inject.AppServiceClient{}, nil) + + test.That(t, ac.printAccessTokenAction(cCtx), test.ShouldBeNil) + test.That(t, len(errOut.messages), test.ShouldEqual, 0) + test.That(t, len(out.messages), test.ShouldEqual, 1) + test.That(t, out.messages[0], test.ShouldContainSubstring, testToken) +} + +func TestAPIKeyCreateAction(t *testing.T) { + createKeyFunc := func(ctx context.Context, in *apppb.CreateKeyRequest, + opts ...grpc.CallOption, + ) (*apppb.CreateKeyResponse, error) { + return &apppb.CreateKeyResponse{Id: "id-xxx", Key: "key-yyy"}, nil + } + asc := &inject.AppServiceClient{ + CreateKeyFunc: createKeyFunc, + } + cCtx, ac, out, errOut := setup(asc, nil) + + test.That(t, ac.organizationAPIKeyCreateAction(cCtx), test.ShouldBeNil) + test.That(t, len(errOut.messages), test.ShouldEqual, 0) + test.That(t, len(out.messages), test.ShouldEqual, 8) + test.That(t, strings.Join(out.messages, ""), test.ShouldContainSubstring, "id-xxx") + test.That(t, strings.Join(out.messages, ""), test.ShouldContainSubstring, "key-yyy") +} + +func TestLogoutAction(t *testing.T) { + cCtx, ac, out, errOut := setup(nil, nil) + + test.That(t, ac.logoutAction(cCtx), test.ShouldBeNil) + test.That(t, len(errOut.messages), test.ShouldEqual, 0) + test.That(t, len(out.messages), test.ShouldEqual, 1) + test.That(t, out.messages[0], test.ShouldContainSubstring, + fmt.Sprintf("Logged out from %q", testEmail)) +} + +func TestWhoAmIAction(t *testing.T) { + cCtx, ac, out, errOut := setup(nil, nil) + + test.That(t, ac.whoAmIAction(cCtx), test.ShouldBeNil) + test.That(t, len(errOut.messages), test.ShouldEqual, 0) + test.That(t, len(out.messages), test.ShouldEqual, 1) + test.That(t, out.messages[0], test.ShouldContainSubstring, testEmail) +} + +func TestConfigMarshalling(t *testing.T) { + t.Run("token config", func(t *testing.T) { + conf := config{ + BaseURL: "https://guthib.com:443", + Auth: &token{ + AccessToken: "secret-token", + User: userData{ + Email: "tipsy@viam.com", + Subject: "MAIV", + }, + }, + } + + bytes, err := json.Marshal(conf) + test.That(t, err, test.ShouldBeNil) + var newConf config + test.That(t, newConf.tryUnmarshallWithAPIKey(bytes), test.ShouldBeError) + test.That(t, newConf.tryUnmarshallWithToken(bytes), test.ShouldBeNil) + test.That(t, newConf.BaseURL, test.ShouldEqual, "https://guthib.com:443") + auth, ok := newConf.Auth.(*token) + test.That(t, ok, test.ShouldBeTrue) + test.That(t, auth.AccessToken, test.ShouldEqual, "secret-token") + test.That(t, auth.User.Email, test.ShouldEqual, "tipsy@viam.com") + test.That(t, auth.User.Subject, test.ShouldEqual, "MAIV") + }) + + t.Run("api-key config", func(t *testing.T) { + conf := config{ + BaseURL: "https://docs.viam.com:443", + Auth: &apiKey{ + KeyID: "42", + KeyCrypto: "secret", + }, + } + + bytes, err := json.Marshal(conf) + test.That(t, err, test.ShouldBeNil) + var newConf config + test.That(t, newConf.tryUnmarshallWithToken(bytes), test.ShouldBeError) + test.That(t, newConf.tryUnmarshallWithAPIKey(bytes), test.ShouldBeNil) + test.That(t, newConf.BaseURL, test.ShouldEqual, "https://docs.viam.com:443") + auth, ok := newConf.Auth.(*apiKey) + test.That(t, ok, test.ShouldBeTrue) + test.That(t, auth.KeyID, test.ShouldEqual, "42") + test.That(t, auth.KeyCrypto, test.ShouldEqual, "secret") + }) +} diff --git a/cli/boards.go b/cli/boards.go new file mode 100644 index 00000000000..7d75756d2be --- /dev/null +++ b/cli/boards.go @@ -0,0 +1,388 @@ +package cli + +import ( + "archive/tar" + "bufio" + "bytes" + "compress/gzip" + "context" + "fmt" + "io" + "net/http" + "os" + "path/filepath" + "regexp" + "strings" + "time" + + "github.com/pkg/errors" + "github.com/urfave/cli/v2" + "go.uber.org/multierr" + packagepb "go.viam.com/api/app/packages/v1" + "go.viam.com/utils" +) + +// supportedVersionRegex validates that the board version is semver 2.0.0 specification. +var supportedVersionRegex = regexp.MustCompile(`^(?P0|[1-9]\d*)\.(?P0|[1-9]\d*)\.(?P0|[1-9]\d*)` + + `(?:-(?P(?:0|[1-9]\d*|\d*[a-zA-Z-][0-9a-zA-Z-]*)` + + `(?:\.(?:0|[1-9]\d*|\d*[a-zA-Z-][0-9a-zA-Z-]*))*))?(?:\+(?P[0-9a-zA-Z-]+(?:\.[0-9a-zA-Z-]+)*))?$`) + +const boardUploadMaximumSize = 32 * 1024 + +// UploadBoardDefsAction is the corresponding action for "board upload". +func UploadBoardDefsAction(ctx *cli.Context) error { + orgArg := ctx.String(organizationFlag) + nameArg := ctx.String(boardFlagName) + versionArg := ctx.String(boardFlagVersion) + if ctx.Args().Len() > 1 { + return errors.New("too many arguments passed to upload command. " + + "make sure to specify flag and optional arguments before the required positional package argument") + } + + jsonPath := ctx.Args().First() + + if jsonPath == "" { + return errors.New("no package to upload -- please provide a path containing your json file. use --help for more information") + } + + // Validate the version is valid. + if !supportedVersionRegex.MatchString(versionArg) { + return fmt.Errorf("invalid version %s. Must use semver 2.0.0 specification for versions", versionArg) + } + + client, err := newViamClient(ctx) + if err != nil { + return err + } + + // get the org from the name or id. + org, err := client.getOrg(orgArg) + if err != nil { + return err + } + + // check if a package with this name and version already exists. + err = client.boardDefsVersionExists(ctx, org.Id, nameArg, versionArg) + if err != nil { + return err + } + + if !strings.HasSuffix(jsonPath, ".json") { + return errors.New("The board definition file must be a .json") + } + + _, err = client.uploadBoardDefsFile(nameArg, versionArg, org.Id, jsonPath) + if err != nil { + return err + } + + printf(ctx.App.Writer, "Board definitions file was successfully uploaded!") + return nil +} + +// DownloadBoardDefsAction is the corresponding action for "board download". +func DownloadBoardDefsAction(c *cli.Context) error { + orgArg := c.String(organizationFlag) + nameArg := c.String(boardFlagName) + versionArg := c.String(boardFlagVersion) + + if versionArg == "" { + versionArg = "latest" + } + + client, err := newViamClient(c) + if err != nil { + return err + } + + // get the org from the name or id. + org, err := client.getOrg(orgArg) + if err != nil { + return err + } + + err = client.downloadBoardDefsFile(nameArg, versionArg, org.Id) + if err != nil { + return err + } + + printf(c.App.Writer, "%s board definitions successfully downloaded!", nameArg) + + return nil +} + +func (c *viamClient) uploadBoardDefsFile( + name string, + version string, + orgID string, + jsonPath string, +) (*packagepb.CreatePackageResponse, error) { + if err := c.ensureLoggedIn(); err != nil { + return nil, err + } + ctx := c.c.Context + + jsonFile, err := os.Open(filepath.Clean(jsonPath)) + if err != nil { + return nil, err + } + + // Create an archive tar.gz file (required for packages). + file, err := createArchive(jsonFile) + if err != nil { + return nil, errors.Wrap(err, "error creating archive") + } + + // The board defs packages are small and never expected to be larger than the upload chunk size, + // so we are sending in one chunk. + // If the file is too big, return error. + if file.Len() > boardUploadMaximumSize { + return nil, fmt.Errorf("file is too large, must be under %d bytes", boardUploadMaximumSize) + } + + stream, err := c.packageClient.CreatePackage(ctx) + if err != nil { + return nil, errors.Wrapf(err, "error starting CreatePackage stream") + } + + stats, err := jsonFile.Stat() + if err != nil { + return nil, err + } + boardDefsFile := []*packagepb.FileInfo{{Name: name, Size: uint64(stats.Size())}} + + packageInfo := &packagepb.PackageInfo{ + OrganizationId: orgID, + Name: name, + Version: version, + Type: packagepb.PackageType_PACKAGE_TYPE_BOARD_DEFS, + Files: boardDefsFile, + Metadata: nil, + } + + // send the package requests + var errs error + if err := sendPackageRequests(stream, file, packageInfo); err != nil { + errs = multierr.Combine(errs, errors.Wrapf(err, "error syncing package")) + } + + // close the stream and receive a response when finished. + resp, err := stream.CloseAndRecv() + if err != nil { + errs = multierr.Combine(errs, errors.Wrapf(err, "received error response while syncing package")) + } + if errs != nil { + return nil, errs + } + + return resp, nil +} + +func (c *viamClient) downloadBoardDefsFile( + name string, + version string, + orgID string, +) error { + if err := c.ensureLoggedIn(); err != nil { + return err + } + ctx := c.c.Context + + includeURL := true + packageType := packagepb.PackageType_PACKAGE_TYPE_BOARD_DEFS + + // the packageID is the orgid/name. + packageID := fmt.Sprintf("%s/%s", orgID, name) + + req := &packagepb.GetPackageRequest{ + Id: packageID, + Version: version, + Type: &packageType, + IncludeUrl: &includeURL, + } + + response, err := c.packageClient.GetPackage(ctx, req) + if err != nil { + return errors.Wrap(err, "could not retrieve the requested package") + } + + currentDir, err := os.Getwd() + if err != nil { + return err + } + + // download the file from the gcs url into the current directory. + err = downloadFile(ctx, currentDir, response.Package.Url) + if err != nil { + return err + } + + return nil +} + +// helper function to check if a package with this name and version already exists. +func (c *viamClient) boardDefsVersionExists(ctx *cli.Context, orgID, name, version string) error { + // the packageID is the orgid/name + packageID := fmt.Sprintf("%s/%s", orgID, name) + + req := packagepb.GetPackageRequest{ + Id: packageID, + Version: version, + } + + _, err := c.packageClient.GetPackage(ctx.Context, &req) + + if err == nil { + return fmt.Errorf("a package with name %s and version %s already exists", name, version) + } + return nil +} + +func sendPackageRequests(stream packagepb.PackageService_CreatePackageClient, + f *bytes.Buffer, packageInfo *packagepb.PackageInfo, +) error { + defer utils.UncheckedErrorFunc(stream.CloseSend) + + req := &packagepb.CreatePackageRequest{ + Package: &packagepb.CreatePackageRequest_Info{Info: packageInfo}, + } + if err := stream.Send(req); err != nil { + return err + } + + req = &packagepb.CreatePackageRequest{ + Package: &packagepb.CreatePackageRequest_Contents{Contents: f.Bytes()}, + } + + if err := stream.Send(req); err != nil { + return err + } + return nil +} + +// createArchive creates a tar.gz from the file provided. +func createArchive(file *os.File) (*bytes.Buffer, error) { + // Create output buffer + out := new(bytes.Buffer) + + // These writers are chained. Writing to the tar writer will + // write to the gzip writer which in turn will write to + // the "out" writer + gw := gzip.NewWriter(out) + defer utils.UncheckedErrorFunc(gw.Close) + tw := tar.NewWriter(gw) + defer utils.UncheckedErrorFunc(tw.Close) + + // Get FileInfo about our file providing file size, mode, etc. + info, err := file.Stat() + if err != nil { + return nil, err + } + + // the raw file can be 100 times more than the max TAR size. + if info.Size() > 100*boardUploadMaximumSize { + return nil, errors.New("the json file is too large") + } + // Create a tar Header from the FileInfo data + header, err := tar.FileInfoHeader(info, info.Name()) + if err != nil { + return nil, err + } + + // Write file header to the tar archive + err = tw.WriteHeader(header) + if err != nil { + return nil, err + } + + // Read the file into a byte slice + bytes := make([]byte, info.Size()) + _, err = bufio.NewReader(file).Read(bytes) + if err != nil && !errors.Is(err, io.EOF) { + return nil, err + } + + // Copy file content to tar archive + if _, err := tw.Write(bytes); err != nil { + return nil, err + } + + return out, nil +} + +// helper function to download a url to a local file. +func downloadFile(ctx context.Context, filepath, url string) error { + getReq, err := http.NewRequestWithContext(ctx, http.MethodGet, url, nil) + if err != nil { + return err + } + httpClient := &http.Client{Timeout: time.Second * 30} + + //nolint:bodyclose /// closed in UncheckedErrorFunc + resp, err := httpClient.Do(getReq) + if err != nil { + return errors.Wrap(err, "error downloading the requested package") + } + + defer utils.UncheckedErrorFunc(resp.Body.Close) + + err = untar(filepath, resp.Body) + if err != nil { + return errors.Wrap(err, "error extracting the tar file") + } + return nil +} + +// untar extracts the tar.gz file, keeping file directory structure. +func untar(dst string, r io.Reader) error { + gzr, err := gzip.NewReader(r) + if err != nil { + return err + } + defer utils.UncheckedErrorFunc(gzr.Close) + tr := tar.NewReader(gzr) + + for { + header, err := tr.Next() + switch { + // if no more files are found return + case errors.Is(err, io.EOF): + return nil + // return any other error + case err != nil: + return err + } + + // the target location where the dir/file should be created. + //nolint: gosec + path := filepath.Join(dst, header.Name) + + // check the file type. + switch header.Typeflag { + // if its a dir and it doesn't exist create it. + case tar.TypeDir: + if _, err := os.Stat(path); err != nil { + if err := os.MkdirAll(path, 0o750); err != nil { + return err + } + } + // if it's a file create it. + case tar.TypeReg: + f, err := os.OpenFile(filepath.Clean(path), os.O_CREATE|os.O_RDWR, os.FileMode(header.Mode)) + if err != nil { + return err + } + + // copy over file contents. + //nolint:gosec + if _, err := io.Copy(f, tr); err != nil { + return err + } + + err = f.Close() + if err != nil { + return err + } + } + } +} diff --git a/cli/client.go b/cli/client.go index 1a247f9e50b..9b638872781 100644 --- a/cli/client.go +++ b/cli/client.go @@ -3,13 +3,13 @@ package cli import ( "context" - "encoding/json" "fmt" "io" - "net/http" + "net" "net/url" "os" "os/exec" + "runtime/debug" "strings" "time" @@ -19,7 +19,9 @@ import ( "github.com/jhump/protoreflect/grpcreflect" "github.com/pkg/errors" "github.com/urfave/cli/v2" + "go.uber.org/zap" datapb "go.viam.com/api/app/data/v1" + packagepb "go.viam.com/api/app/packages/v1" apppb "go.viam.com/api/app/v1" "go.viam.com/utils" "go.viam.com/utils/rpc" @@ -27,22 +29,24 @@ import ( "google.golang.org/grpc/metadata" reflectpb "google.golang.org/grpc/reflection/grpc_reflection_v1alpha" + rconfig "go.viam.com/rdk/config" "go.viam.com/rdk/grpc" "go.viam.com/rdk/resource" "go.viam.com/rdk/robot/client" "go.viam.com/rdk/services/shell" ) -// The AppClient provides all the CLI command functionality needed to talk -// to the app service but not directly to robot parts. -type AppClient struct { - c *cli.Context - conf *Config - client apppb.AppServiceClient - dataClient datapb.DataServiceClient - baseURL *url.URL - rpcOpts []rpc.DialOption - authFlow *authFlow +// viamClient wraps a cli.Context and provides all the CLI command functionality +// needed to talk to the app and data services but not directly to robot parts. +type viamClient struct { + c *cli.Context + conf *config + client apppb.AppServiceClient + dataClient datapb.DataServiceClient + packageClient packagepb.PackageServiceClient + baseURL *url.URL + rpcOpts []rpc.DialOption + authFlow *authFlow selectedOrg *apppb.Organization selectedLoc *apppb.Location @@ -52,196 +56,463 @@ type AppClient struct { locs *[]*apppb.Location } -func checkBaseURL(c *cli.Context) (*url.URL, []rpc.DialOption, error) { - baseURL := c.String("base-url") - baseURLParsed, err := url.Parse(baseURL) +// ListOrganizationsAction is the corresponding Action for 'organizations list'. +func ListOrganizationsAction(cCtx *cli.Context) error { + c, err := newViamClient(cCtx) if err != nil { - return nil, nil, err + return err } + return c.listOrganizationsAction(cCtx) +} - if baseURLParsed.Scheme == "https" { - return baseURLParsed, nil, nil +func (c *viamClient) listOrganizationsAction(cCtx *cli.Context) error { + orgs, err := c.listOrganizations() + if err != nil { + return errors.Wrap(err, "could not list organizations") } - return baseURLParsed, []rpc.DialOption{ - rpc.WithInsecure(), - rpc.WithAllowInsecureWithCredentialsDowngrade(), - }, nil + for i, org := range orgs { + if i == 0 { + printf(cCtx.App.Writer, "Organizations for %q:", c.conf.Auth) + } + printf(cCtx.App.Writer, "\t%s (id: %s)", org.Name, org.Id) + } + return nil } -func isProdBaseURL(baseURL *url.URL) bool { - return strings.HasSuffix(baseURL.Hostname(), "viam.com") +// ListLocationsAction is the corresponding Action for 'locations list'. +func ListLocationsAction(c *cli.Context) error { + client, err := newViamClient(c) + if err != nil { + return err + } + orgStr := c.Args().First() + listLocations := func(orgID string) error { + locs, err := client.listLocations(orgID) + if err != nil { + return errors.Wrap(err, "could not list locations") + } + for _, loc := range locs { + printf(c.App.Writer, "\t%s (id: %s)", loc.Name, loc.Id) + } + return nil + } + if orgStr == "" { + orgs, err := client.listOrganizations() + if err != nil { + return errors.Wrap(err, "could not list organizations") + } + for i, org := range orgs { + if i == 0 { + printf(c.App.Writer, "Locations for %q:", client.conf.Auth) + } + printf(c.App.Writer, "%s:", org.Name) + if err := listLocations(org.Id); err != nil { + return err + } + } + return nil + } + return listLocations(orgStr) } -// NewAppClient returns a new app client that may already -// be authenticated. -func NewAppClient(c *cli.Context) (*AppClient, error) { - baseURL, rpcOpts, err := checkBaseURL(c) +// ListRobotsAction is the corresponding Action for 'robots list'. +func ListRobotsAction(c *cli.Context) error { + client, err := newViamClient(c) if err != nil { - return nil, err + return err + } + orgStr := c.String(organizationFlag) + locStr := c.String(locationFlag) + robots, err := client.listRobots(orgStr, locStr) + if err != nil { + return errors.Wrap(err, "could not list robots") } - var authFlow *authFlow - if isProdBaseURL(baseURL) { - authFlow = newCLIAuthFlow(c.App.Writer) - } else { - authFlow = newStgCLIAuthFlow(c.App.Writer) + if orgStr == "" || locStr == "" { + printf(c.App.Writer, "%s -> %s", client.selectedOrg.Name, client.selectedLoc.Name) } - conf, err := configFromCache() + for _, robot := range robots { + printf(c.App.Writer, "%s (id: %s)", robot.Name, robot.Id) + } + return nil +} + +// RobotStatusAction is the corresponding Action for 'robot status'. +func RobotStatusAction(c *cli.Context) error { + client, err := newViamClient(c) if err != nil { - if !os.IsNotExist(err) { - return nil, err + return err + } + + orgStr := c.String(organizationFlag) + locStr := c.String(locationFlag) + robot, err := client.robot(orgStr, locStr, c.String(robotFlag)) + if err != nil { + return err + } + parts, err := client.robotParts(client.selectedOrg.Id, client.selectedLoc.Id, robot.Id) + if err != nil { + return errors.Wrap(err, "could not get robot parts") + } + + if orgStr == "" || locStr == "" { + printf(c.App.Writer, "%s -> %s", client.selectedOrg.Name, client.selectedLoc.Name) + } + + printf( + c.App.Writer, + "ID: %s\nName: %s\nLast Access: %s (%s ago)", + robot.Id, + robot.Name, + robot.LastAccess.AsTime().Format(time.UnixDate), + time.Since(robot.LastAccess.AsTime()), + ) + + if len(parts) != 0 { + printf(c.App.Writer, "Parts:") + } + for i, part := range parts { + name := part.Name + if part.MainPart { + name += " (main)" + } + printf( + c.App.Writer, + "\tID: %s\n\tName: %s\n\tLast Access: %s (%s ago)", + part.Id, + name, + part.LastAccess.AsTime().Format(time.UnixDate), + time.Since(part.LastAccess.AsTime()), + ) + if i != len(parts)-1 { + printf(c.App.Writer, "") } - conf = &Config{} } - return &AppClient{ - c: c, - conf: conf, - baseURL: baseURL, - rpcOpts: rpcOpts, - selectedOrg: &apppb.Organization{}, - selectedLoc: &apppb.Location{}, - authFlow: authFlow, - }, nil + return nil } -// Login goes through the CLI login flow using a device code and browser. Once logged in the access token and user details -// are cached on disk. -func (c *AppClient) Login() error { - var token *Token - var err error - if c.conf.Auth != nil && c.conf.Auth.CanRefresh() { - token, err = c.authFlow.Refresh(c.c.Context, c.conf.Auth) - if err != nil { - utils.UncheckedError(c.Logout()) - return err +// RobotLogsAction is the corresponding Action for 'robot logs'. +func RobotLogsAction(c *cli.Context) error { + client, err := newViamClient(c) + if err != nil { + return err + } + + orgStr := c.String(organizationFlag) + locStr := c.String(locationFlag) + robotStr := c.String(robotFlag) + robot, err := client.robot(orgStr, locStr, robotStr) + if err != nil { + return errors.Wrap(err, "could not get robot") + } + + parts, err := client.robotParts(orgStr, locStr, robotStr) + if err != nil { + return errors.Wrap(err, "could not get robot parts") + } + + for i, part := range parts { + if i != 0 { + printf(c.App.Writer, "") } - } else { - token, err = c.authFlow.Login(c.c.Context) - if err != nil { - return err + + var header string + if orgStr == "" || locStr == "" || robotStr == "" { + header = fmt.Sprintf("%s -> %s -> %s -> %s", client.selectedOrg.Name, client.selectedLoc.Name, robot.Name, part.Name) + } else { + header = part.Name + } + if err := client.printRobotPartLogs( + orgStr, locStr, robotStr, part.Id, + c.Bool(logsFlagErrors), + "\t", + header, + ); err != nil { + return errors.Wrap(err, "could not print robot logs") } } - // write token to config. - c.conf.Auth = token - - return storeConfigToCache(c.conf) + return nil } -// Config returns the current config. -func (c *AppClient) Config() *Config { - return c.conf -} +// RobotPartStatusAction is the corresponding Action for 'robot part status'. +func RobotPartStatusAction(c *cli.Context) error { + client, err := newViamClient(c) + if err != nil { + return err + } -// RPCOpts returns RPC dial options dervied from the base URL -// being used in the current invocation of the CLI. -func (c *AppClient) RPCOpts() []rpc.DialOption { - rpcOpts := make([]rpc.DialOption, len(c.rpcOpts)) - copy(rpcOpts, c.rpcOpts) - return rpcOpts -} + orgStr := c.String(organizationFlag) + locStr := c.String(locationFlag) + robotStr := c.String(robotFlag) + robot, err := client.robot(orgStr, locStr, robotStr) + if err != nil { + return errors.Wrap(err, "could not get robot") + } -// SelectedOrg returns the currently selected organization, possibly zero initialized. -func (c *AppClient) SelectedOrg() *apppb.Organization { - return c.selectedOrg -} + part, err := client.robotPart(orgStr, locStr, robotStr, c.String(partFlag)) + if err != nil { + return errors.Wrap(err, "could not get robot part") + } -// SelectedLoc returns the currently selected location, possibly zero initialized. -func (c *AppClient) SelectedLoc() *apppb.Location { - return c.selectedLoc + if orgStr == "" || locStr == "" || robotStr == "" { + printf(c.App.Writer, "%s -> %s -> %s", client.selectedOrg.Name, client.selectedLoc.Name, robot.Name) + } + + name := part.Name + if part.MainPart { + name += " (main)" + } + printf( + c.App.Writer, + "ID: %s\nName: %s\nLast Access: %s (%s ago)", + part.Id, + name, + part.LastAccess.AsTime().Format(time.UnixDate), + time.Since(part.LastAccess.AsTime()), + ) + + return nil } -func (c *AppClient) ensureLoggedIn() error { - if c.client != nil { - return nil +// RobotPartLogsAction is the corresponding Action for 'robot part logs'. +func RobotPartLogsAction(c *cli.Context) error { + client, err := newViamClient(c) + if err != nil { + return err } - if c.conf.Auth == nil { - return errors.New("not logged in: run the following command to authenticate:\n\tviam auth") + orgStr := c.String(organizationFlag) + locStr := c.String(locationFlag) + robotStr := c.String(robotFlag) + robot, err := client.robot(orgStr, locStr, robotStr) + if err != nil { + return errors.Wrap(err, "could not get robot") } - if c.conf.Auth.IsExpired() { - if !c.conf.Auth.CanRefresh() { - utils.UncheckedError(c.Logout()) - return errors.New("token expired and cannot refresh") - } + var header string + if orgStr == "" || locStr == "" || robotStr == "" { + header = fmt.Sprintf("%s -> %s -> %s", client.selectedOrg.Name, client.selectedLoc.Name, robot.Name) + } + if c.Bool(logsFlagTail) { + return client.tailRobotPartLogs( + orgStr, locStr, robotStr, c.String(partFlag), + c.Bool(logsFlagErrors), + "", + header, + ) + } + return client.printRobotPartLogs( + orgStr, locStr, robotStr, c.String(partFlag), + c.Bool(logsFlagErrors), + "", + header, + ) +} - // expired. - newToken, err := c.authFlow.Refresh(c.c.Context, c.conf.Auth) - if err != nil { - utils.UncheckedError(c.Logout()) // clear cache if failed to refresh - return errors.Wrapf(err, "error while refrshing token") - } +// RobotPartRunAction is the corresponding Action for 'robot part run'. +func RobotPartRunAction(c *cli.Context) error { + svcMethod := c.Args().First() + if svcMethod == "" { + return errors.New("service method required") + } - // write token to config. - c.conf.Auth = newToken - if err := storeConfigToCache(c.conf); err != nil { - return err - } + client, err := newViamClient(c) + if err != nil { + return err } - rpcOpts := append(c.RPCOpts(), rpc.WithStaticAuthenticationMaterial(c.conf.Auth.AccessToken)) + // Create logger based on presence of debugFlag. + logger := zap.NewNop().Sugar() + if c.Bool(debugFlag) { + logger = golog.NewDebugLogger("cli") + } - conn, err := rpc.DialDirectGRPC( - c.c.Context, - c.baseURL.Host, - nil, - rpcOpts..., + return client.runRobotPartCommand( + c.String(organizationFlag), + c.String(locationFlag), + c.String(robotFlag), + c.String(partFlag), + svcMethod, + c.String(runFlagData), + c.Duration(runFlagStream), + c.Bool(debugFlag), + logger, ) +} + +// RobotPartShellAction is the corresponding Action for 'robot part shell'. +func RobotPartShellAction(c *cli.Context) error { + infof(c.App.Writer, "Ensure robot part has a valid shell type service") + + client, err := newViamClient(c) if err != nil { return err } - c.client = apppb.NewAppServiceClient(conn) - c.dataClient = datapb.NewDataServiceClient(conn) + // Create logger based on presence of debugFlag. + logger := zap.NewNop().Sugar() + if c.Bool(debugFlag) { + logger = golog.NewDebugLogger("cli") + } + + return client.startRobotPartShell( + c.String(organizationFlag), + c.String(locationFlag), + c.String(robotFlag), + c.String(partFlag), + c.Bool(debugFlag), + logger, + ) +} + +// VersionAction is the corresponding Action for 'version'. +func VersionAction(c *cli.Context) error { + info, ok := debug.ReadBuildInfo() + if !ok { + return errors.New("error reading build info") + } + if c.Bool(debugFlag) { + printf(c.App.Writer, "%s", info.String()) + } + settings := make(map[string]string, len(info.Settings)) + for _, setting := range info.Settings { + settings[setting.Key] = setting.Value + } + version := "?" + if rev, ok := settings["vcs.revision"]; ok { + version = rev[:8] + if settings["vcs.modified"] == "true" { + version += "+" + } + } + deps := make(map[string]*debug.Module, len(info.Deps)) + for _, dep := range info.Deps { + deps[dep.Path] = dep + } + apiVersion := "?" + if dep, ok := deps["go.viam.com/api"]; ok { + apiVersion = dep.Version + } + appVersion := rconfig.Version + if appVersion == "" { + appVersion = "(dev)" + } + printf(c.App.Writer, "Version %s Git=%s API=%s", appVersion, version, apiVersion) return nil } -// PrepareAuthorization prepares authorization for this device and returns -// the device token to late authenticate with and the URL to authorize the device -// at. -func (c *AppClient) PrepareAuthorization() (string, string, error) { - req, err := http.NewRequest( - http.MethodPost, fmt.Sprintf("%s/auth/device", c.baseURL), nil) +var defaultBaseURL = "https://app.viam.com:443" + +func parseBaseURL(baseURL string, verifyConnection bool) (*url.URL, []rpc.DialOption, error) { + baseURLParsed, err := url.Parse(baseURL) if err != nil { - return "", "", err + return nil, nil, err } - req = req.WithContext(c.c.Context) - resp, err := http.DefaultClient.Do(req) - if err != nil { - return "", "", err + + // Go URL parsing can place the host in Path if no scheme is provided; place + // Path in Host in this case. + if baseURLParsed.Host == "" && baseURLParsed.Path != "" { + baseURLParsed.Host = baseURLParsed.Path + baseURLParsed.Path = "" } - defer func() { - utils.UncheckedError(resp.Body.Close()) - }() - if resp.StatusCode != http.StatusOK { - return "", "", fmt.Errorf("unexpected status code %d", resp.StatusCode) + // Assume "https" scheme if none is provided, and assume 8080 port for "http" + // scheme and 443 port for "https" scheme. + var secure bool + switch baseURLParsed.Scheme { + case "http": + if baseURLParsed.Port() == "" { + baseURLParsed.Host = baseURLParsed.Host + ":" + "8080" + } + case "https", "": + secure = true + baseURLParsed.Scheme = "https" + if baseURLParsed.Port() == "" { + baseURLParsed.Host = baseURLParsed.Host + ":" + "443" + } } - var deviceData struct { - Token string `json:"token"` - URL string `json:"url"` + if verifyConnection { + // Check if URL is even valid with a TCP dial. + conn, err := net.DialTimeout("tcp", baseURLParsed.Host, 3*time.Second) + if err != nil { + return nil, nil, fmt.Errorf("base URL %q (needed for auth) is currently unreachable. "+ + "Ensure URL is valid and you are connected to internet", baseURLParsed.Host) + } + utils.UncheckedError(conn.Close()) } - dec := json.NewDecoder(resp.Body) - if err := dec.Decode(&deviceData); err != nil { - return "", "", err + + if secure { + return baseURLParsed, nil, nil } - return deviceData.Token, deviceData.URL, nil + return baseURLParsed, []rpc.DialOption{ + rpc.WithInsecure(), + rpc.WithAllowInsecureWithCredentialsDowngrade(), + }, nil } -// Logout logs out the client and clears the config. -func (c *AppClient) Logout() error { - if err := removeConfigFromCache(); err != nil && !os.IsNotExist(err) { - return err +func isProdBaseURL(baseURL *url.URL) bool { + return strings.HasSuffix(baseURL.Hostname(), "viam.com") +} + +func newViamClient(c *cli.Context) (*viamClient, error) { + conf, err := configFromCache() + if err != nil { + if !os.IsNotExist(err) { + return nil, err + } + conf = &config{} } - c.conf = &Config{} - return nil + + // If base URL was not specified, assume cached base URL. If no base URL is + // cached, assume default base URL. + baseURLArg := c.String(baseURLFlag) + switch { + case conf.BaseURL == "" && baseURLArg == "": + conf.BaseURL = defaultBaseURL + case conf.BaseURL == "" && baseURLArg != "": + conf.BaseURL = baseURLArg + case baseURLArg != "" && conf.BaseURL != "" && conf.BaseURL != baseURLArg: + return nil, fmt.Errorf("cached base URL for this session is %q. "+ + "Please logout and login again to use provided base URL %q", conf.BaseURL, baseURLArg) + } + + if conf.BaseURL != defaultBaseURL { + infof(c.App.Writer, "Using %q as base URL value", conf.BaseURL) + } + baseURL, rpcOpts, err := parseBaseURL(conf.BaseURL, true) + if err != nil { + return nil, err + } + + var authFlow *authFlow + if isProdBaseURL(baseURL) { + authFlow = newCLIAuthFlow(c.App.Writer) + } else { + authFlow = newStgCLIAuthFlow(c.App.Writer) + } + + return &viamClient{ + c: c, + conf: conf, + baseURL: baseURL, + rpcOpts: rpcOpts, + selectedOrg: &apppb.Organization{}, + selectedLoc: &apppb.Location{}, + authFlow: authFlow, + }, nil } -func (c *AppClient) loadOrganizations() error { +func (c *viamClient) copyRPCOpts() []rpc.DialOption { + rpcOpts := make([]rpc.DialOption, len(c.rpcOpts)) + copy(rpcOpts, c.rpcOpts) + return rpcOpts +} + +func (c *viamClient) loadOrganizations() error { resp, err := c.client.ListOrganizations(c.c.Context, &apppb.ListOrganizationsRequest{}) if err != nil { return err @@ -250,7 +521,10 @@ func (c *AppClient) loadOrganizations() error { return nil } -func (c *AppClient) selectOrganization(orgStr string) error { +func (c *viamClient) selectOrganization(orgStr string) error { + if err := c.ensureLoggedIn(); err != nil { + return err + } if orgStr != "" && (c.selectedOrg.Id == orgStr || c.selectedOrg.Name == orgStr) { return nil } @@ -292,8 +566,53 @@ func (c *AppClient) selectOrganization(orgStr string) error { return nil } -// ListOrganizations returns all organizations belonging to the currently authenticated user. -func (c *AppClient) ListOrganizations() ([]*apppb.Organization, error) { +// getOrg gets an org by an indentifying string. If the orgStr is an +// org UUID, then this matchs on organization ID, otherwise this will match +// on organization name. +func (c *viamClient) getOrg(orgStr string) (*apppb.Organization, error) { + if err := c.ensureLoggedIn(); err != nil { + return nil, err + } + resp, err := c.client.ListOrganizations(c.c.Context, &apppb.ListOrganizationsRequest{}) + if err != nil { + return nil, err + } + organizations := resp.GetOrganizations() + var orgIsID bool + if _, err := uuid.Parse(orgStr); err == nil { + orgIsID = true + } + for _, org := range organizations { + if orgIsID { + if org.Id == orgStr { + return org, nil + } + } else if org.Name == orgStr { + return org, nil + } + } + return nil, errors.Errorf("no organization found for %q", orgStr) +} + +// getUserOrgByPublicNamespace searches the logged in users orgs to see +// if any have a matching public namespace. +func (c *viamClient) getUserOrgByPublicNamespace(publicNamespace string) (*apppb.Organization, error) { + if err := c.ensureLoggedIn(); err != nil { + return nil, err + } + + if err := c.loadOrganizations(); err != nil { + return nil, err + } + for _, org := range *c.orgs { + if org.PublicNamespace == publicNamespace { + return org, nil + } + } + return nil, errors.Errorf("none of your organizations have a public namespace of %q", publicNamespace) +} + +func (c *viamClient) listOrganizations() ([]*apppb.Organization, error) { if err := c.ensureLoggedIn(); err != nil { return nil, err } @@ -303,7 +622,7 @@ func (c *AppClient) ListOrganizations() ([]*apppb.Organization, error) { return (*c.orgs), nil } -func (c *AppClient) loadLocations() error { +func (c *viamClient) loadLocations() error { if c.selectedOrg.Id == "" { return errors.New("must select organization first") } @@ -315,7 +634,7 @@ func (c *AppClient) loadLocations() error { return nil } -func (c *AppClient) selectLocation(locStr string) error { +func (c *viamClient) selectLocation(locStr string) error { if locStr != "" && (c.selectedLoc.Id == locStr || c.selectedLoc.Name == locStr) { return nil } @@ -349,8 +668,7 @@ func (c *AppClient) selectLocation(locStr string) error { return nil } -// ListLocations returns all locations in the given organizationbelonging to the currently authenticated user. -func (c *AppClient) ListLocations(orgID string) ([]*apppb.Location, error) { +func (c *viamClient) listLocations(orgID string) ([]*apppb.Location, error) { if err := c.ensureLoggedIn(); err != nil { return nil, err } @@ -363,8 +681,7 @@ func (c *AppClient) ListLocations(orgID string) ([]*apppb.Location, error) { return (*c.locs), nil } -// ListRobots returns all robots in the given location. -func (c *AppClient) ListRobots(orgStr, locStr string) ([]*apppb.Robot, error) { +func (c *viamClient) listRobots(orgStr, locStr string) ([]*apppb.Robot, error) { if err := c.ensureLoggedIn(); err != nil { return nil, err } @@ -383,13 +700,12 @@ func (c *AppClient) ListRobots(orgStr, locStr string) ([]*apppb.Robot, error) { return resp.Robots, nil } -// Robot returns the given robot. -func (c *AppClient) Robot(orgStr, locStr, robotStr string) (*apppb.Robot, error) { +func (c *viamClient) robot(orgStr, locStr, robotStr string) (*apppb.Robot, error) { if err := c.ensureLoggedIn(); err != nil { return nil, err } - robots, err := c.ListRobots(orgStr, locStr) + robots, err := c.listRobots(orgStr, locStr) if err != nil { return nil, err } @@ -402,12 +718,11 @@ func (c *AppClient) Robot(orgStr, locStr, robotStr string) (*apppb.Robot, error) return nil, errors.Errorf("no robot found for %q", robotStr) } -// RobotPart returns the given robot part belonging to a robot. -func (c *AppClient) RobotPart(orgStr, locStr, robotStr, partStr string) (*apppb.RobotPart, error) { +func (c *viamClient) robotPart(orgStr, locStr, robotStr, partStr string) (*apppb.RobotPart, error) { if err := c.ensureLoggedIn(); err != nil { return nil, err } - parts, err := c.RobotParts(orgStr, locStr, robotStr) + parts, err := c.robotParts(orgStr, locStr, robotStr) if err != nil { return nil, err } @@ -419,8 +734,8 @@ func (c *AppClient) RobotPart(orgStr, locStr, robotStr, partStr string) (*apppb. return nil, errors.Errorf("no robot part found for %q", partStr) } -func (c *AppClient) robotPartLogs(orgStr, locStr, robotStr, partStr string, errorsOnly bool) ([]*apppb.LogEntry, error) { - part, err := c.RobotPart(orgStr, locStr, robotStr, partStr) +func (c *viamClient) robotPartLogs(orgStr, locStr, robotStr, partStr string, errorsOnly bool) ([]*apppb.LogEntry, error) { + part, err := c.robotPart(orgStr, locStr, robotStr, partStr) if err != nil { return nil, err } @@ -435,28 +750,11 @@ func (c *AppClient) robotPartLogs(orgStr, locStr, robotStr, partStr string, erro return resp.Logs, nil } -// RobotPartLogs returns recent logs for the given robot part. -func (c *AppClient) RobotPartLogs(orgStr, locStr, robotStr, partStr string) ([]*apppb.LogEntry, error) { - if err := c.ensureLoggedIn(); err != nil { - return nil, err - } - return c.robotPartLogs(orgStr, locStr, robotStr, partStr, false) -} - -// RobotPartLogsErrors returns recent error logs for the given robot part. -func (c *AppClient) RobotPartLogsErrors(orgStr, locStr, robotStr, partStr string) ([]*apppb.LogEntry, error) { +func (c *viamClient) robotParts(orgStr, locStr, robotStr string) ([]*apppb.RobotPart, error) { if err := c.ensureLoggedIn(); err != nil { return nil, err } - return c.robotPartLogs(orgStr, locStr, robotStr, partStr, true) -} - -// RobotParts returns all parts of the given robot. -func (c *AppClient) RobotParts(orgStr, locStr, robotStr string) ([]*apppb.RobotPart, error) { - if err := c.ensureLoggedIn(); err != nil { - return nil, err - } - robot, err := c.Robot(orgStr, locStr, robotStr) + robot, err := c.robot(orgStr, locStr, robotStr) if err != nil { return nil, err } @@ -469,11 +767,11 @@ func (c *AppClient) RobotParts(orgStr, locStr, robotStr string) ([]*apppb.RobotP return resp.Parts, nil } -func (c *AppClient) printRobotPartLogsInternal(logs []*apppb.LogEntry, indent string) { +func (c *viamClient) printRobotPartLogsInner(logs []*apppb.LogEntry, indent string) { for _, log := range logs { - fmt.Fprintf( + printf( c.c.App.Writer, - "%s%s\t%s\t%s\t%s\n", + "%s%s\t%s\t%s\t%s", indent, log.Time.AsTime().Format("2006-01-02T15:04:05.000Z0700"), log.Level, @@ -483,27 +781,26 @@ func (c *AppClient) printRobotPartLogsInternal(logs []*apppb.LogEntry, indent st } } -// PrintRobotPartLogs prints logs for the given robot part. -func (c *AppClient) PrintRobotPartLogs(orgStr, locStr, robotStr, partStr string, errorsOnly bool, indent, header string) error { +func (c *viamClient) printRobotPartLogs(orgStr, locStr, robotStr, partStr string, errorsOnly bool, indent, header string) error { logs, err := c.robotPartLogs(orgStr, locStr, robotStr, partStr, errorsOnly) if err != nil { return err } if header != "" { - fmt.Fprintln(c.c.App.Writer, header) + printf(c.c.App.Writer, header) } if len(logs) == 0 { - fmt.Fprintf(c.c.App.Writer, "%sno recent logs\n", indent) + printf(c.c.App.Writer, "%sNo recent logs", indent) return nil } - c.printRobotPartLogsInternal(logs, indent) + c.printRobotPartLogsInner(logs, indent) return nil } -// TailRobotPartLogs tails and prints logs for the given robot part. -func (c *AppClient) TailRobotPartLogs(orgStr, locStr, robotStr, partStr string, errorsOnly bool, indent, header string) error { - part, err := c.RobotPart(orgStr, locStr, robotStr, partStr) +// tailRobotPartLogs tails and prints logs for the given robot part. +func (c *viamClient) tailRobotPartLogs(orgStr, locStr, robotStr, partStr string, errorsOnly bool, indent, header string) error { + part, err := c.robotPart(orgStr, locStr, robotStr, partStr) if err != nil { return err } @@ -516,7 +813,7 @@ func (c *AppClient) TailRobotPartLogs(orgStr, locStr, robotStr, partStr string, } if header != "" { - fmt.Fprintln(c.c.App.Writer, header) + printf(c.c.App.Writer, header) } for { @@ -527,49 +824,11 @@ func (c *AppClient) TailRobotPartLogs(orgStr, locStr, robotStr, partStr string, } return err } - c.printRobotPartLogsInternal(resp.Logs, indent) - } -} - -func (c *AppClient) prepareDial( - orgStr, locStr, robotStr, partStr string, - debug bool, -) (context.Context, string, []rpc.DialOption, error) { - if err := c.ensureLoggedIn(); err != nil { - return nil, "", nil, err - } - if err := c.selectOrganization(orgStr); err != nil { - return nil, "", nil, err - } - if err := c.selectLocation(locStr); err != nil { - return nil, "", nil, err - } - - part, err := c.RobotPart(c.selectedOrg.Id, c.selectedLoc.Id, robotStr, partStr) - if err != nil { - return nil, "", nil, err + c.printRobotPartLogsInner(resp.Logs, indent) } - - rpcDialer := rpc.NewCachedDialer() - defer func() { - utils.UncheckedError(rpcDialer.Close()) - }() - dialCtx := rpc.ContextWithDialer(c.c.Context, rpcDialer) - - rpcOpts := append(c.RPCOpts(), - rpc.WithExternalAuth(c.baseURL.Host, part.Fqdn), - rpc.WithStaticExternalAuthenticationMaterial(c.conf.Auth.AccessToken), - ) - - if debug { - rpcOpts = append(rpcOpts, rpc.WithDialDebug()) - } - - return dialCtx, part.Fqdn, rpcOpts, nil } -// RunRobotPartCommand runs the given command on a robot part. -func (c *AppClient) RunRobotPartCommand( +func (c *viamClient) runRobotPartCommand( orgStr, locStr, robotStr, partStr string, svcMethod, data string, streamDur time.Duration, @@ -666,8 +925,7 @@ func (c *AppClient) RunRobotPartCommand( } } -// StartRobotPartShell starts a shell on a robot part. -func (c *AppClient) StartRobotPartShell( +func (c *viamClient) startRobotPartShell( orgStr, locStr, robotStr, partStr string, debug bool, logger golog.Logger, @@ -678,13 +936,11 @@ func (c *AppClient) StartRobotPartShell( } if debug { - fmt.Fprintln(c.c.App.Writer, "establishing connection...") + printf(c.c.App.Writer, "Establishing connection...") } robotClient, err := client.New(dialCtx, fqdn, logger, client.WithDialOptions(rpcOpts...)) if err != nil { - fmt.Fprintln(c.c.App.ErrWriter, err) - cli.OsExiter(1) - return nil + return errors.Wrap(err, "could not connect to robot part") } defer func() { @@ -701,46 +957,39 @@ func (c *AppClient) StartRobotPartShell( } } if found == nil { - fmt.Fprintln(c.c.App.ErrWriter, "shell service is not enabled") - cli.OsExiter(1) - return nil + return errors.New("shell service is not enabled on this robot part") } shellRes, err := robotClient.ResourceByName(*found) if err != nil { - fmt.Fprintln(c.c.App.ErrWriter, err) - cli.OsExiter(1) - return nil + return errors.Wrap(err, "could not get shell service from robot part") } shellSvc, ok := shellRes.(shell.Service) if !ok { - fmt.Fprintln(c.c.App.ErrWriter, "shell service is not a shell service") - cli.OsExiter(1) - return nil + return errors.New("could not get shell service from robot part") } input, output, err := shellSvc.Shell(c.c.Context, map[string]interface{}{}) if err != nil { - fmt.Fprintln(c.c.App.ErrWriter, err) - cli.OsExiter(1) - return nil + return err } setRaw := func(isRaw bool) error { - r := "raw" + // NOTE(benjirewis): Linux systems seem to need both "raw" (no processing) and "-echo" + // (no echoing back inputted characters) in order to allow the input and output loops + // below to completely control the terminal. + args := []string{"raw", "-echo"} if !isRaw { - r = "-raw" + args = []string{"-raw", "echo"} } - rawMode := exec.Command("stty", r) + rawMode := exec.Command("stty", args...) rawMode.Stdin = os.Stdin return rawMode.Run() } if err := setRaw(true); err != nil { - fmt.Fprintln(c.c.App.ErrWriter, err) - cli.OsExiter(1) - return nil + return err } defer func() { utils.UncheckedError(setRaw(false)) @@ -778,10 +1027,10 @@ func (c *AppClient) StartRobotPartShell( case outputData, ok := <-output: if ok { if outputData.Output != "" { - fmt.Fprint(c.c.App.Writer, outputData.Output) + fmt.Fprint(c.c.App.Writer, outputData.Output) // no newline } if outputData.Error != "" { - fmt.Fprint(c.c.App.ErrWriter, outputData.Error) + fmt.Fprint(c.c.App.ErrWriter, outputData.Error) // no newline } if outputData.EOF { return diff --git a/cli/client_test.go b/cli/client_test.go new file mode 100644 index 00000000000..203b49307eb --- /dev/null +++ b/cli/client_test.go @@ -0,0 +1,194 @@ +package cli + +import ( + "context" + "flag" + "fmt" + "os" + "testing" + "time" + + "github.com/urfave/cli/v2" + datapb "go.viam.com/api/app/data/v1" + apppb "go.viam.com/api/app/v1" + "go.viam.com/test" + "go.viam.com/utils/protoutils" + "google.golang.org/grpc" + + "go.viam.com/rdk/testutils/inject" + "go.viam.com/rdk/utils" +) + +var ( + testEmail = "grogu@viam.com" + testToken = "thisistheway" +) + +type testWriter struct { + messages []string +} + +// Write implements io.Writer. +func (tw *testWriter) Write(b []byte) (int, error) { + tw.messages = append(tw.messages, string(b)) + return len(b), nil +} + +// setup creates a new cli.Context and viamClient with fake auth and the passed +// in AppServiceClient and DataServiceClient. It also returns testWriters that capture Stdout and +// Stdin. +func setup(asc apppb.AppServiceClient, dataClient datapb.DataServiceClient) (*cli.Context, *viamClient, *testWriter, *testWriter) { + out := &testWriter{} + errOut := &testWriter{} + flags := &flag.FlagSet{} + if dataClient != nil { + // these flags are only relevant when testing a dataClient + flags.String(dataFlagDataType, dataTypeTabular, "") + flags.String(dataFlagDestination, utils.ResolveFile(""), "") + } + cCtx := cli.NewContext(NewApp(out, errOut), flags, nil) + conf := &config{ + Auth: &token{ + AccessToken: testToken, + ExpiresAt: time.Now().Add(time.Hour), + User: userData{ + Email: testEmail, + }, + }, + } + ac := &viamClient{ + client: asc, + conf: conf, + c: cCtx, + dataClient: dataClient, + } + return cCtx, ac, out, errOut +} + +func TestListOrganizationsAction(t *testing.T) { + listOrganizationsFunc := func(ctx context.Context, in *apppb.ListOrganizationsRequest, + opts ...grpc.CallOption, + ) (*apppb.ListOrganizationsResponse, error) { + orgs := []*apppb.Organization{{Name: "jedi"}, {Name: "mandalorians"}} + return &apppb.ListOrganizationsResponse{Organizations: orgs}, nil + } + asc := &inject.AppServiceClient{ + ListOrganizationsFunc: listOrganizationsFunc, + } + cCtx, ac, out, errOut := setup(asc, nil) + + test.That(t, ac.listOrganizationsAction(cCtx), test.ShouldBeNil) + test.That(t, len(errOut.messages), test.ShouldEqual, 0) + test.That(t, len(out.messages), test.ShouldEqual, 3) + test.That(t, out.messages[0], test.ShouldEqual, fmt.Sprintf("Organizations for %q:\n", testEmail)) + test.That(t, out.messages[1], test.ShouldContainSubstring, "jedi") + test.That(t, out.messages[2], test.ShouldContainSubstring, "mandalorians") +} + +func TestTabularDataByFilterAction(t *testing.T) { + pbStruct, err := protoutils.StructToStructPb(map[string]interface{}{"bool": true, "string": "true", "float": float64(1)}) + test.That(t, err, test.ShouldBeNil) + + // calls to `TabularDataByFilter` will repeat so long as data continue to be returned, + // so we need a way of telling our injected method when data has already been sent so we + // can send an empty response + var dataRequested bool + tabularDataByFilterFunc := func(ctx context.Context, in *datapb.TabularDataByFilterRequest, opts ...grpc.CallOption, + ) (*datapb.TabularDataByFilterResponse, error) { + if dataRequested { + return &datapb.TabularDataByFilterResponse{}, nil + } + dataRequested = true + return &datapb.TabularDataByFilterResponse{ + Data: []*datapb.TabularData{{Data: pbStruct}}, + Metadata: []*datapb.CaptureMetadata{{LocationId: "loc-id"}}, + }, nil + } + + dsc := &inject.DataServiceClient{ + TabularDataByFilterFunc: tabularDataByFilterFunc, + } + + cCtx, ac, out, errOut := setup(&inject.AppServiceClient{}, dsc) + + test.That(t, ac.dataExportAction(cCtx), test.ShouldBeNil) + test.That(t, len(errOut.messages), test.ShouldEqual, 0) + test.That(t, len(out.messages), test.ShouldEqual, 4) + test.That(t, out.messages[0], test.ShouldEqual, "Downloading..") + test.That(t, out.messages[1], test.ShouldEqual, ".") + test.That(t, out.messages[2], test.ShouldEqual, ".") + test.That(t, out.messages[3], test.ShouldEqual, "\n") + + // expectedDataSize is the expected string length of the data returned by the injected call + expectedDataSize := 98 + b := make([]byte, expectedDataSize) + + // `data.ndjson` is the standardized name of the file data is written to in the `tabularData` call + filePath := utils.ResolveFile("data/data.ndjson") + file, err := os.Open(filePath) + test.That(t, err, test.ShouldBeNil) + + dataSize, err := file.Read(b) + test.That(t, err, test.ShouldBeNil) + test.That(t, dataSize, test.ShouldEqual, expectedDataSize) + + savedData := string(b) + expectedData := "{\"MetadataIndex\":0,\"TimeReceived\":null,\"TimeRequested\":null,\"bool\":true,\"float\":1,\"string\":\"true\"}" + test.That(t, savedData, test.ShouldEqual, expectedData) + + expectedMetadataSize := 23 + b = make([]byte, expectedMetadataSize) + + // metadata is named `0.json` based on its index in the metadata array + filePath = utils.ResolveFile("metadata/0.json") + file, err = os.Open(filePath) + test.That(t, err, test.ShouldBeNil) + + metadataSize, err := file.Read(b) + test.That(t, err, test.ShouldBeNil) + test.That(t, metadataSize, test.ShouldEqual, expectedMetadataSize) + + savedMetadata := string(b) + test.That(t, savedMetadata, test.ShouldEqual, "{\"locationId\":\"loc-id\"}") +} + +func TestBaseURLParsing(t *testing.T) { + // Test basic parsing + url, rpcOpts, err := parseBaseURL("https://app.viam.com:443", false) + test.That(t, err, test.ShouldBeNil) + test.That(t, url.Port(), test.ShouldEqual, "443") + test.That(t, url.Scheme, test.ShouldEqual, "https") + test.That(t, url.Hostname(), test.ShouldEqual, "app.viam.com") + test.That(t, rpcOpts, test.ShouldBeNil) + + // Test parsing without a port + url, _, err = parseBaseURL("https://app.viam.com", false) + test.That(t, err, test.ShouldBeNil) + test.That(t, url.Port(), test.ShouldEqual, "443") + test.That(t, url.Hostname(), test.ShouldEqual, "app.viam.com") + + // Test parsing locally + url, rpcOpts, err = parseBaseURL("http://127.0.0.1:8081", false) + test.That(t, err, test.ShouldBeNil) + test.That(t, url.Scheme, test.ShouldEqual, "http") + test.That(t, url.Port(), test.ShouldEqual, "8081") + test.That(t, url.Hostname(), test.ShouldEqual, "127.0.0.1") + test.That(t, rpcOpts, test.ShouldHaveLength, 2) + + // Test localhost:8080 + url, _, err = parseBaseURL("http://localhost:8080", false) + test.That(t, err, test.ShouldBeNil) + test.That(t, url.Port(), test.ShouldEqual, "8080") + test.That(t, url.Hostname(), test.ShouldEqual, "localhost") + test.That(t, rpcOpts, test.ShouldHaveLength, 2) + + // Test no scheme remote + url, _, err = parseBaseURL("app.viam.com", false) + test.That(t, err, test.ShouldBeNil) + test.That(t, url.Scheme, test.ShouldEqual, "https") + test.That(t, url.Hostname(), test.ShouldEqual, "app.viam.com") + + // Test invalid url + _, _, err = parseBaseURL(":5", false) + test.That(t, fmt.Sprint(err), test.ShouldContainSubstring, "missing protocol scheme") +} diff --git a/cli/config.go b/cli/config.go index e60d9524665..c5222876831 100644 --- a/cli/config.go +++ b/cli/config.go @@ -4,6 +4,9 @@ import ( "encoding/json" "os" "path/filepath" + + "github.com/pkg/errors" + "go.uber.org/multierr" ) var viamDotDir = filepath.Join(os.Getenv("HOME"), ".viam") @@ -12,24 +15,30 @@ func getCLICachePath() string { return filepath.Join(viamDotDir, "cached_cli_config.json") } -func configFromCache() (*Config, error) { +func configFromCache() (*config, error) { rd, err := os.ReadFile(getCLICachePath()) if err != nil { return nil, err } - var conf Config - if err := json.Unmarshal(rd, &conf); err != nil { - return nil, err + var conf config + + tokenErr := conf.tryUnmarshallWithToken(rd) + if tokenErr == nil { + return &conf, nil + } + apiKeyErr := conf.tryUnmarshallWithAPIKey(rd) + if apiKeyErr == nil { + return &conf, nil } - return &conf, nil + return nil, errors.Wrap(multierr.Combine(tokenErr, apiKeyErr), "failed to read config from cache") } func removeConfigFromCache() error { return os.Remove(getCLICachePath()) } -func storeConfigToCache(cfg *Config) error { +func storeConfigToCache(cfg *config) error { if err := os.MkdirAll(viamDotDir, 0o700); err != nil { return err } @@ -41,7 +50,29 @@ func storeConfigToCache(cfg *Config) error { return os.WriteFile(getCLICachePath(), md, 0o640) } -// Config contains stored config information for the CLI. -type Config struct { - Auth *Token `json:"auth"` +type config struct { + BaseURL string `json:"base_url"` + Auth authMethod `json:"auth"` +} + +func (conf *config) tryUnmarshallWithToken(configBytes []byte) error { + conf.Auth = &token{} + if err := json.Unmarshal(configBytes, &conf); err != nil { + return err + } + if conf.Auth != nil && conf.Auth.(*token).User.Email != "" { + return nil + } + return errors.New("config did not contain a user token") +} + +func (conf *config) tryUnmarshallWithAPIKey(configBytes []byte) error { + conf.Auth = &apiKey{} + if err := json.Unmarshal(configBytes, &conf); err != nil { + return err + } + if conf.Auth != nil && conf.Auth.(*apiKey).KeyID != "" { + return nil + } + return errors.New("config did not contain an api key") } diff --git a/cli/data.go b/cli/data.go index 6c4041c596c..c8a3898f3a7 100644 --- a/cli/data.go +++ b/cli/data.go @@ -17,8 +17,10 @@ import ( "time" "github.com/pkg/errors" + "github.com/urfave/cli/v2" datapb "go.viam.com/api/app/data/v1" "google.golang.org/protobuf/encoding/protojson" + "google.golang.org/protobuf/types/known/timestamppb" ) const ( @@ -28,16 +30,167 @@ const ( maxRetryCount = 5 logEveryN = 100 maxLimit = 100 + + dataTypeBinary = "binary" + dataTypeTabular = "tabular" ) +// DataExportAction is the corresponding action for 'data export'. +func DataExportAction(c *cli.Context) error { + client, err := newViamClient(c) + if err != nil { + return err + } + + return client.dataExportAction(c) +} + +func (c *viamClient) dataExportAction(cCtx *cli.Context) error { + filter, err := createDataFilter(cCtx) + if err != nil { + return err + } + + switch cCtx.String(dataFlagDataType) { + case dataTypeBinary: + if err := c.binaryData(cCtx.Path(dataFlagDestination), filter, cCtx.Uint(dataFlagParallelDownloads)); err != nil { + return err + } + case dataTypeTabular: + if err := c.tabularData(cCtx.Path(dataFlagDestination), filter); err != nil { + return err + } + default: + return errors.Errorf("%s must be binary or tabular, got %q", dataFlagDataType, cCtx.String(dataFlagDataType)) + } + return nil +} + +// DataDeleteBinaryAction is the corresponding action for 'data delete'. +func DataDeleteBinaryAction(c *cli.Context) error { + client, err := newViamClient(c) + if err != nil { + return err + } + + switch c.String(dataFlagDataType) { + case dataTypeBinary: + filter, err := createDataFilter(c) + if err != nil { + return err + } + if err := client.deleteBinaryData(filter); err != nil { + return err + } + case dataTypeTabular: + return errors.New("use `delete-tabular` action instead of `delete`") + default: + return errors.Errorf("%s must be binary or tabular, got %q", dataFlagDataType, c.String(dataFlagDataType)) + } + + return nil +} + +// DataDeleteTabularAction is the corresponding action for 'data delete-tabular'. +func DataDeleteTabularAction(c *cli.Context) error { + client, err := newViamClient(c) + if err != nil { + return err + } + + if err := client.deleteTabularData(c.String(dataFlagOrgID), c.Int(dataFlagDeleteTabularDataOlderThanDays)); err != nil { + return err + } + return nil +} + +func createDataFilter(c *cli.Context) (*datapb.Filter, error) { + filter := &datapb.Filter{} + + if c.StringSlice(dataFlagOrgIDs) != nil { + filter.OrganizationIds = c.StringSlice(dataFlagOrgIDs) + } + if c.StringSlice(dataFlagLocationIDs) != nil { + filter.LocationIds = c.StringSlice(dataFlagLocationIDs) + } + if c.String(dataFlagRobotID) != "" { + filter.RobotId = c.String(dataFlagRobotID) + } + if c.String(dataFlagPartID) != "" { + filter.PartId = c.String(dataFlagPartID) + } + if c.String(dataFlagRobotName) != "" { + filter.RobotName = c.String(dataFlagRobotName) + } + if c.String(dataFlagPartName) != "" { + filter.PartName = c.String(dataFlagPartName) + } + if c.String(dataFlagComponentType) != "" { + filter.ComponentType = c.String(dataFlagComponentType) + } + if c.String(dataFlagComponentName) != "" { + filter.ComponentName = c.String(dataFlagComponentName) + } + if c.String(dataFlagMethod) != "" { + filter.Method = c.String(dataFlagMethod) + } + if len(c.StringSlice(dataFlagMimeTypes)) != 0 { + filter.MimeType = c.StringSlice(dataFlagMimeTypes) + } + if c.StringSlice(dataFlagTags) != nil { + switch { + case len(c.StringSlice(dataFlagTags)) == 1 && c.StringSlice(dataFlagTags)[0] == "tagged": + filter.TagsFilter = &datapb.TagsFilter{ + Type: datapb.TagsFilterType_TAGS_FILTER_TYPE_TAGGED, + } + case len(c.StringSlice(dataFlagTags)) == 1 && c.StringSlice(dataFlagTags)[0] == "untagged": + filter.TagsFilter = &datapb.TagsFilter{ + Type: datapb.TagsFilterType_TAGS_FILTER_TYPE_UNTAGGED, + } + default: + filter.TagsFilter = &datapb.TagsFilter{ + Type: datapb.TagsFilterType_TAGS_FILTER_TYPE_MATCH_BY_OR, + Tags: c.StringSlice(dataFlagTags), + } + } + } + if len(c.StringSlice(dataFlagBboxLabels)) != 0 { + filter.BboxLabels = c.StringSlice(dataFlagBboxLabels) + } + var start *timestamppb.Timestamp + var end *timestamppb.Timestamp + timeLayout := time.RFC3339 + if c.String(dataFlagStart) != "" { + t, err := time.Parse(timeLayout, c.String(dataFlagStart)) + if err != nil { + return nil, errors.Wrap(err, "could not parse start flag") + } + start = timestamppb.New(t) + } + if c.String(dataFlagEnd) != "" { + t, err := time.Parse(timeLayout, c.String(dataFlagEnd)) + if err != nil { + return nil, errors.Wrap(err, "could not parse end flag") + } + end = timestamppb.New(t) + } + if start != nil || end != nil { + filter.Interval = &datapb.CaptureInterval{ + Start: start, + End: end, + } + } + return filter, nil +} + // BinaryData downloads binary data matching filter to dst. -func (c *AppClient) BinaryData(dst string, filter *datapb.Filter, parallelDownloads uint) error { +func (c *viamClient) binaryData(dst string, filter *datapb.Filter, parallelDownloads uint) error { if err := c.ensureLoggedIn(); err != nil { return err } if err := makeDestinationDirs(dst); err != nil { - return errors.Wrapf(err, "error creating destination directories") + return errors.Wrapf(err, "could not create destination directories") } if parallelDownloads == 0 { @@ -105,7 +258,7 @@ func (c *AppClient) BinaryData(dst string, filter *datapb.Filter, parallelDownlo } numFilesDownloaded.Add(1) if numFilesDownloaded.Load()%logEveryN == 0 { - fmt.Fprintf(c.c.App.Writer, "downloaded %d files\n", numFilesDownloaded.Load()) + printf(c.c.App.Writer, "Downloaded %d files", numFilesDownloaded.Load()) } }(nextID) } @@ -115,7 +268,7 @@ func (c *AppClient) BinaryData(dst string, filter *datapb.Filter, parallelDownlo } } if numFilesDownloaded.Load()%logEveryN != 0 { - fmt.Fprintf(c.c.App.Writer, "downloaded %d files to %s\n", numFilesDownloaded.Load(), dst) + printf(c.c.App.Writer, "Downloaded %d files to %s", numFilesDownloaded.Load(), dst) } }() wg.Wait() @@ -213,16 +366,20 @@ func downloadBinary(ctx context.Context, client datapb.DataServiceClient, dst st return err } - gzippedBytes := datum.GetBinary() - r, err := gzip.NewReader(bytes.NewBuffer(gzippedBytes)) - if err != nil { - return err + bin := datum.GetBinary() + + r := io.NopCloser(bytes.NewReader(bin)) + if datum.GetMetadata().GetFileExt() == ".gz" { + r, err = gzip.NewReader(r) + if err != nil { + return err + } } //nolint:gosec dataFile, err := os.Create(filepath.Join(dst, dataDir, fileName+datum.GetMetadata().GetFileExt())) if err != nil { - return errors.Wrapf(err, fmt.Sprintf("error creating file for datum %s", datum.GetMetadata().GetId())) + return errors.Wrapf(err, fmt.Sprintf("could not create file for datum %s", datum.GetMetadata().GetId())) } //nolint:gosec if _, err := io.Copy(dataFile, r); err != nil { @@ -234,27 +391,27 @@ func downloadBinary(ctx context.Context, client datapb.DataServiceClient, dst st return nil } -// TabularData downloads binary data matching filter to dst. -func (c *AppClient) TabularData(dst string, filter *datapb.Filter) error { +// tabularData downloads binary data matching filter to dst. +func (c *viamClient) tabularData(dst string, filter *datapb.Filter) error { if err := c.ensureLoggedIn(); err != nil { return err } if err := makeDestinationDirs(dst); err != nil { - return errors.Wrapf(err, "error creating destination directories") + return errors.Wrapf(err, "could not create destination directories") } var err error var resp *datapb.TabularDataByFilterResponse - // TODO: [DATA-640] Support export in additional formats. + // TODO(DATA-640): Support export in additional formats. //nolint:gosec dataFile, err := os.Create(filepath.Join(dst, dataDir, "data.ndjson")) if err != nil { - return errors.Wrapf(err, "error creating data file") + return errors.Wrapf(err, "could not create data file") } w := bufio.NewWriter(dataFile) - fmt.Fprintf(c.c.App.Writer, "Downloading..") + fmt.Fprintf(c.c.App.Writer, "Downloading..") // no newline var last string mdIndexes := make(map[string]int) mdIndex := 0 @@ -268,7 +425,7 @@ func (c *AppClient) TabularData(dst string, filter *datapb.Filter) error { }, CountOnly: false, }) - fmt.Fprintf(c.c.App.Writer, ".") + fmt.Fprintf(c.c.App.Writer, ".") // no newline if err == nil { break } @@ -295,18 +452,18 @@ func (c *AppClient) TabularData(dst string, filter *datapb.Filter) error { mdJSONBytes, err := protojson.Marshal(md) if err != nil { - return errors.Wrap(err, "error marshaling metadata") + return errors.Wrap(err, "could not marshal metadata") } //nolint:gosec mdFile, err := os.Create(filepath.Join(dst, metadataDir, strconv.Itoa(mdIndex)+".json")) if err != nil { - return errors.Wrapf(err, fmt.Sprintf("error creating metadata file for metadata index %d", mdIndex)) + return errors.Wrapf(err, fmt.Sprintf("could not create metadata file for metadata index %d", mdIndex)) } if _, err := mdFile.Write(mdJSONBytes); err != nil { - return errors.Wrapf(err, "error writing metadata file %s", mdFile.Name()) + return errors.Wrapf(err, "could not write to metadata file %s", mdFile.Name()) } if err := mdFile.Close(); err != nil { - return errors.Wrapf(err, "error closing metadata file %s", mdFile.Name()) + return errors.Wrapf(err, "could not close metadata file %s", mdFile.Name()) } mdIndex++ } @@ -324,18 +481,18 @@ func (c *AppClient) TabularData(dst string, filter *datapb.Filter) error { m["MetadataIndex"] = localToGlobalMDIndex[int(datum.GetMetadataIndex())] j, err := json.Marshal(m) if err != nil { - return errors.Wrap(err, "error marshaling json response") + return errors.Wrap(err, "could not marshal JSON response") } _, err = w.Write(append(j, []byte("\n")...)) if err != nil { - return errors.Wrapf(err, "error writing reading to file %s", dataFile.Name()) + return errors.Wrapf(err, "could not write to file %s", dataFile.Name()) } } } - fmt.Fprintf(c.c.App.Writer, "\n") + printf(c.c.App.Writer, "") // newline if err := w.Flush(); err != nil { - return errors.Wrapf(err, "error flushing writer for %s", dataFile.Name()) + return errors.Wrapf(err, "could not flush writer for %s", dataFile.Name()) } return nil @@ -350,3 +507,30 @@ func makeDestinationDirs(dst string) error { } return nil } + +func (c *viamClient) deleteBinaryData(filter *datapb.Filter) error { + if err := c.ensureLoggedIn(); err != nil { + return err + } + resp, err := c.dataClient.DeleteBinaryDataByFilter(context.Background(), + &datapb.DeleteBinaryDataByFilterRequest{Filter: filter}) + if err != nil { + return errors.Wrapf(err, "received error from server") + } + printf(c.c.App.Writer, "Deleted %d files", resp.GetDeletedCount()) + return nil +} + +// deleteTabularData delete tabular data matching filter. +func (c *viamClient) deleteTabularData(orgID string, deleteOlderThanDays int) error { + if err := c.ensureLoggedIn(); err != nil { + return err + } + resp, err := c.dataClient.DeleteTabularData(context.Background(), + &datapb.DeleteTabularDataRequest{OrganizationId: orgID, DeleteOlderThanDays: uint32(deleteOlderThanDays)}) + if err != nil { + return errors.Wrapf(err, "received error from server") + } + printf(c.c.App.Writer, "Deleted %d datapoints", resp.GetDeletedCount()) + return nil +} diff --git a/cli/delete.go b/cli/delete.go deleted file mode 100644 index 119c95ef273..00000000000 --- a/cli/delete.go +++ /dev/null @@ -1,37 +0,0 @@ -package cli - -import ( - "context" - "fmt" - - "github.com/pkg/errors" - datapb "go.viam.com/api/app/data/v1" -) - -// DeleteBinaryData deletes binary data matching filter. -func (c *AppClient) DeleteBinaryData(filter *datapb.Filter) error { - if err := c.ensureLoggedIn(); err != nil { - return err - } - resp, err := c.dataClient.DeleteBinaryDataByFilter(context.Background(), - &datapb.DeleteBinaryDataByFilterRequest{Filter: filter}) - if err != nil { - return errors.Wrapf(err, "received error from server") - } - fmt.Fprintf(c.c.App.Writer, "deleted %d files\n", resp.GetDeletedCount()) - return nil -} - -// DeleteTabularData delete tabular data matching filter. -func (c *AppClient) DeleteTabularData(filter *datapb.Filter) error { - if err := c.ensureLoggedIn(); err != nil { - return err - } - resp, err := c.dataClient.DeleteTabularDataByFilter(context.Background(), - &datapb.DeleteTabularDataByFilterRequest{Filter: filter}) - if err != nil { - return errors.Wrapf(err, "received error from server") - } - fmt.Fprintf(c.c.App.Writer, "deleted %d datapoints\n", resp.GetDeletedCount()) - return nil -} diff --git a/cli/module_registry.go b/cli/module_registry.go new file mode 100644 index 00000000000..50406cc5036 --- /dev/null +++ b/cli/module_registry.go @@ -0,0 +1,638 @@ +package cli + +import ( + "archive/tar" + "compress/gzip" + "context" + "encoding/json" + "fmt" + "io" + "io/fs" + "math" + "os" + "path/filepath" + "strings" + + "github.com/google/uuid" + "github.com/pkg/errors" + "github.com/urfave/cli/v2" + "go.uber.org/multierr" + apppb "go.viam.com/api/app/v1" +) + +// moduleUploadChunkSize sets the number of bytes included in each chunk of the upload stream. +var moduleUploadChunkSize = 32 * 1024 + +// moduleVisibility determines whether modules are public or private. +type moduleVisibility string + +// Permissions enumeration. +const ( + moduleVisibilityPrivate moduleVisibility = "private" + moduleVisibilityPublic moduleVisibility = "public" +) + +// moduleComponent represents an api - model pair. +type moduleComponent struct { + API string `json:"api"` + Model string `json:"model"` +} + +// moduleID represents a prefix:name pair where prefix can be either an org id or a namespace. +type moduleID struct { + prefix string + name string +} + +// moduleManifest is used to create & parse manifest.json. +type moduleManifest struct { + // for backward compatibility - DO NOT SET as will be deprecated + Name string `json:"name,omitempty"` + ModuleID string `json:"module_id"` + Visibility moduleVisibility `json:"visibility"` + URL string `json:"url"` + Description string `json:"description"` + Models []moduleComponent `json:"models"` + Entrypoint string `json:"entrypoint"` +} + +const ( + defaultManifestFilename = "meta.json" +) + +// CreateModuleAction is the corresponding Action for 'module create'. It runs +// the command to create a module. This includes both a gRPC call to register +// the module on app.viam.com and creating the manifest file. +func CreateModuleAction(c *cli.Context) error { + moduleNameArg := c.String(moduleFlagName) + publicNamespaceArg := c.String(moduleFlagPublicNamespace) + orgIDArg := c.String(moduleFlagOrgID) + + client, err := newViamClient(c) + if err != nil { + return err + } + org, err := resolveOrg(client, publicNamespaceArg, orgIDArg) + if err != nil { + return err + } + // Check to make sure the user doesn't accidentally overwrite a module manifest + if _, err := os.Stat(defaultManifestFilename); err == nil { + return errors.New("another module's meta.json already exists in the current directory. Delete it and try again") + } + + response, err := client.createModule(moduleNameArg, org.GetId()) + if err != nil { + return errors.Wrap(err, "failed to register the module on app.viam.com") + } + + returnedModuleID, err := parseModuleID(response.GetModuleId()) + if err != nil { + return err + } + + printf(c.App.Writer, "Successfully created '%s'", returnedModuleID.String()) + if response.GetUrl() != "" { + printf(c.App.Writer, "You can view it here: %s", response.GetUrl()) + } + emptyManifest := moduleManifest{ + ModuleID: returnedModuleID.String(), + Visibility: "", + // This is done so that the json has an empty example + Models: []moduleComponent{ + {}, + }, + } + if err := writeManifest(defaultManifestFilename, emptyManifest); err != nil { + return err + } + printf(c.App.Writer, "Configuration for the module has been written to meta.json\n") + return nil +} + +// UpdateModuleAction is the corresponding Action for 'module update'. It runs +// the command to update a module. This includes updating the meta.json to +// include the public namespace (if set on the org). +func UpdateModuleAction(c *cli.Context) error { + publicNamespaceArg := c.String(moduleFlagPublicNamespace) + orgIDArg := c.String(moduleFlagOrgID) + manifestPathArg := c.String(moduleFlagPath) + var moduleID moduleID + + manifestPath := defaultManifestFilename + if manifestPathArg != "" { + manifestPath = manifestPathArg + } + + client, err := newViamClient(c) + if err != nil { + return err + } + + manifest, err := loadManifest(manifestPath) + if err != nil { + return err + } + + // for backwards compatibility this could be empty + if manifest.ModuleID != "" { + moduleID, err = validateModuleID(c, client, manifest.ModuleID, publicNamespaceArg, orgIDArg) + if err != nil { + return err + } + } else { + moduleID, err = validateModuleID(c, client, manifest.Name, publicNamespaceArg, orgIDArg) + if err != nil { + return err + } + } + + response, err := client.updateModule(moduleID, manifest) + if err != nil { + return err + } + printf(c.App.Writer, "Module successfully updated! You can view your changes online here: %s\n", response.GetUrl()) + + // if we have gotten this far it means that moduleID will have a prefix in it + // because the validate command resolves the orgId or namespace to the moduleID with the namespace as the priority + + // TODO: Will remove in a few week + if manifest.Name != "" || manifest.ModuleID == "" { + manifest.Name = "" + manifest.ModuleID = moduleID.String() + if err := writeManifest(manifestPath, manifest); err != nil { + return errors.Wrap(err, "failed to update meta.json with new information from Viam") + } + } + + return nil +} + +// UploadModuleAction is the corresponding action for 'module upload'. +func UploadModuleAction(c *cli.Context) error { + manifestPathArg := c.String(moduleFlagPath) + publicNamespaceArg := c.String(moduleFlagPublicNamespace) + orgIDArg := c.String(moduleFlagOrgID) + nameArg := c.String(moduleFlagName) + versionArg := c.String(moduleFlagVersion) + platformArg := c.String(moduleFlagPlatform) + forceUploadArg := c.Bool(moduleFlagForce) + tarballPath := c.Args().First() + if c.Args().Len() > 1 { + return errors.New("too many arguments passed to upload command. " + + "Make sure to specify flag and optional arguments before the required positional package argument") + } + if tarballPath == "" { + return errors.New("no package to upload -- please provide an archive containing your module. Use --help for more information") + } + + client, err := newViamClient(c) + if err != nil { + return err + } + + manifestPath := defaultManifestFilename + if manifestPathArg != "" { + manifestPath = manifestPathArg + } + var moduleID moduleID + // if the manifest cant be found + if _, err := os.Stat(manifestPath); err != nil { + // no manifest found. + if nameArg == "" || (publicNamespaceArg == "" && orgIDArg == "") { + return errors.New("unable to find the meta.json. " + + "If you want to upload a version without a meta.json, you must supply a module name and namespace (or module name and org-id)", + ) + } + } else { + // if we can find a manifest, use that + manifest, err := loadManifest(manifestPath) + var IDFromField string + if err != nil { + return err + } + + if manifest.ModuleID != "" { + IDFromField = manifest.ModuleID + } else { + IDFromField = manifest.Name + } + + moduleID, err = parseModuleID(IDFromField) + if err != nil { + return err + } + if nameArg != "" && (nameArg != moduleID.name) { + // This is almost certainly a mistake we want to catch + return errors.Errorf("module name %q was supplied on the command line but the meta.json has a module ID of %q", nameArg, + moduleID.name) + } + // set name arg from the manifest file rather than what is passed in + nameArg = IDFromField + } + + moduleID, err = validateModuleID(c, client, nameArg, publicNamespaceArg, orgIDArg) + if err != nil { + return err + } + + if !forceUploadArg { + if err := validateModuleFile(client, moduleID, tarballPath, versionArg); err != nil { + return fmt.Errorf( + "error validating module: %w. For more details, please visit: https://docs.viam.com/manage/cli/#command-options-3 ", + err) + } + } + + response, err := client.uploadModuleFile(moduleID, versionArg, platformArg, tarballPath) + if err != nil { + return err + } + + printf(c.App.Writer, "Version successfully uploaded! you can view your changes online here: %s", response.GetUrl()) + + return nil +} + +func (c *viamClient) createModule(moduleName, organizationID string) (*apppb.CreateModuleResponse, error) { + if err := c.ensureLoggedIn(); err != nil { + return nil, err + } + req := apppb.CreateModuleRequest{ + Name: moduleName, + OrganizationId: organizationID, + } + return c.client.CreateModule(c.c.Context, &req) +} + +func (c *viamClient) getModule(moduleID moduleID) (*apppb.GetModuleResponse, error) { + if err := c.ensureLoggedIn(); err != nil { + return nil, err + } + req := apppb.GetModuleRequest{ + ModuleId: moduleID.String(), + } + return c.client.GetModule(c.c.Context, &req) +} + +func (c *viamClient) updateModule(moduleID moduleID, manifest moduleManifest) (*apppb.UpdateModuleResponse, error) { + if err := c.ensureLoggedIn(); err != nil { + return nil, err + } + var models []*apppb.Model + for _, moduleComponent := range manifest.Models { + models = append(models, moduleComponentToProto(moduleComponent)) + } + visibility, err := visibilityToProto(manifest.Visibility) + if err != nil { + return nil, err + } + req := apppb.UpdateModuleRequest{ + ModuleId: moduleID.String(), + Visibility: visibility, + Url: manifest.URL, + Description: manifest.Description, + Models: models, + Entrypoint: manifest.Entrypoint, + } + return c.client.UpdateModule(c.c.Context, &req) +} + +func (c *viamClient) uploadModuleFile( + moduleID moduleID, + version, + platform string, + tarballPath string, +) (*apppb.UploadModuleFileResponse, error) { + if err := c.ensureLoggedIn(); err != nil { + return nil, err + } + + //nolint:gosec + file, err := os.Open(tarballPath) + if err != nil { + return nil, err + } + ctx := c.c.Context + + stream, err := c.client.UploadModuleFile(ctx) + if err != nil { + return nil, err + } + moduleFileInfo := apppb.ModuleFileInfo{ + ModuleId: moduleID.String(), + Version: version, + Platform: platform, + } + req := &apppb.UploadModuleFileRequest{ + ModuleFile: &apppb.UploadModuleFileRequest_ModuleFileInfo{ModuleFileInfo: &moduleFileInfo}, + } + if err := stream.Send(req); err != nil { + return nil, err + } + + var errs error + // We do not add the EOF as an error because all server-side errors trigger an EOF on the stream + // This results in extra clutter to the error msg + if err := sendModuleUploadRequests(ctx, stream, file, c.c.App.Writer); err != nil && !errors.Is(err, io.EOF) { + errs = multierr.Combine(errs, errors.Wrapf(err, "could not upload %s", file.Name())) + } + + resp, closeErr := stream.CloseAndRecv() + errs = multierr.Combine(errs, closeErr) + return resp, errs +} + +func sendModuleUploadRequests(ctx context.Context, stream apppb.AppService_UploadModuleFileClient, file *os.File, stdout io.Writer) error { + stat, err := file.Stat() + if err != nil { + return err + } + fileSize := stat.Size() + uploadedBytes := 0 + // Close the line with the progress reading + defer printf(stdout, "") + + //nolint:errcheck + defer stream.CloseSend() + // Loop until there is no more content to be read from file or the context expires. + for { + if ctx.Err() != nil { + return ctx.Err() + } + // Get the next UploadRequest from the file. + uploadReq, err := getNextModuleUploadRequest(file) + + // EOF means we've completed successfully. + if errors.Is(err, io.EOF) { + return nil + } + + if err != nil { + return errors.Wrap(err, "could not read file") + } + + if err = stream.Send(uploadReq); err != nil { + return err + } + uploadedBytes += len(uploadReq.GetFile()) + // Simple progress reading until we have a proper tui library + uploadPercent := int(math.Ceil(100 * float64(uploadedBytes) / float64(fileSize))) + fmt.Fprintf(stdout, "\rUploading... %d%% (%d/%d bytes)", uploadPercent, uploadedBytes, fileSize) // no newline + } +} + +func getNextModuleUploadRequest(file *os.File) (*apppb.UploadModuleFileRequest, error) { + // get the next chunk of bytes from the file + byteArr := make([]byte, moduleUploadChunkSize) + numBytesRead, err := file.Read(byteArr) + if err != nil { + return nil, err + } + if numBytesRead < moduleUploadChunkSize { + byteArr = byteArr[:numBytesRead] + } + return &apppb.UploadModuleFileRequest{ + ModuleFile: &apppb.UploadModuleFileRequest_File{ + File: byteArr, + }, + }, nil +} + +func validateModuleFile(client *viamClient, moduleID moduleID, tarballPath, version string) error { + getModuleResp, err := client.getModule(moduleID) + if err != nil { + return err + } + entrypoint, err := getEntrypointForVersion(getModuleResp.Module, version) + if err != nil { + return err + } + //nolint:gosec + file, err := os.Open(tarballPath) + if err != nil { + return err + } + // TODO(APP-2226): support .tar.xz + if !strings.HasSuffix(file.Name(), ".tar.gz") { + return errors.New("you must upload your module in the form of a .tar.gz") + } + archive, err := gzip.NewReader(file) + if err != nil { + return err + } + + tarReader := tar.NewReader(archive) + filesWithSameNameAsEntrypoint := []string{} + for { + if err := client.c.Context.Err(); err != nil { + return err + } + header, err := tarReader.Next() + if errors.Is(err, io.EOF) { + break + } + if err != nil { + return errors.Wrapf(err, "error reading %s", file.Name()) + } + path := header.Name + + // if path == entrypoint, we have found the right file + if filepath.Clean(path) == filepath.Clean(entrypoint) { + info := header.FileInfo() + if info.Mode().Perm()&0o100 == 0 { + return errors.Errorf( + "the provided tarball %q contained a file at the entrypoint %q, but that file is not marked as executable", + tarballPath, entrypoint) + } + // executable file at entrypoint. validation succeeded. + return nil + } + if filepath.Base(path) == filepath.Base(entrypoint) { + filesWithSameNameAsEntrypoint = append(filesWithSameNameAsEntrypoint, path) + } + } + extraErrInfo := "" + if len(filesWithSameNameAsEntrypoint) > 0 { + extraErrInfo = fmt.Sprintf(". Did you mean to set your entrypoint to %v?", filesWithSameNameAsEntrypoint) + } + return errors.Errorf("the provided tarball %q does not contain a file at the desired entrypoint %q%s", + tarballPath, entrypoint, extraErrInfo) +} + +func visibilityToProto(visibility moduleVisibility) (apppb.Visibility, error) { + switch visibility { + case moduleVisibilityPrivate: + return apppb.Visibility_VISIBILITY_PRIVATE, nil + case moduleVisibilityPublic: + return apppb.Visibility_VISIBILITY_PUBLIC, nil + default: + return apppb.Visibility_VISIBILITY_UNSPECIFIED, + errors.Errorf("invalid module visibility. must be either %q or %q", moduleVisibilityPublic, moduleVisibilityPrivate) + } +} + +func moduleComponentToProto(moduleComponent moduleComponent) *apppb.Model { + return &apppb.Model{ + Api: moduleComponent.API, + Model: moduleComponent.Model, + } +} + +func parseModuleID(id string) (moduleID, error) { + // This parsing is intentionally lenient so that the backend does the real validation + // We also allow for empty prefixes here (unlike the backend) to simplify the flexible way to parse user input + splitModuleName := strings.Split(id, ":") + switch len(splitModuleName) { + case 1: + return moduleID{prefix: "", name: id}, nil + case 2: + return moduleID{prefix: splitModuleName[0], name: splitModuleName[1]}, nil + default: + return moduleID{}, errors.Errorf("invalid module name '%s'."+ + " Module name must be in the form 'prefix:module-name' for public modules"+ + " or just 'module-name' for private modules in organizations without a public namespace", id) + } +} + +func (m *moduleID) String() string { + if m.prefix == "" { + return m.name + } + return fmt.Sprintf("%s:%s", m.prefix, m.name) +} + +// validateModuleID tries to parse the manifestModuleID to see if it is a valid moduleID with a prefix +// if it is not, it uses the publicNamespaceArg and orgIDArg to determine what the moduleID prefix should be. +func validateModuleID( + c *cli.Context, + client *viamClient, + manifestModuleID, + publicNamespaceArg, + orgIDArg string, +) (moduleID, error) { + mid, err := parseModuleID(manifestModuleID) + if err != nil { + return moduleID{}, err + } + + if mid.prefix != "" { + if publicNamespaceArg != "" || orgIDArg != "" { + org, err := resolveOrg(client, publicNamespaceArg, orgIDArg) + if err != nil { + return moduleID{}, err + } + expectedOrg, err := getOrgByModuleIDPrefix(client, mid.prefix) + if err != nil { + return moduleID{}, err + } + if org.GetId() != expectedOrg.GetId() { + // This is almost certainly a user mistake + // Preferring org name rather than orgid here because the manifest probably has it specified in terms of + // public_namespace so returning the ids would be frustrating + return moduleID{}, errors.Errorf("the meta.json specifies a different org %q than the one provided via args %q", + org.GetName(), expectedOrg.GetName()) + } + printf(c.App.Writer, "the module's meta.json already specifies a full module id. Ignoring public-namespace and org-id arg") + } + return mid, nil + } + // moduleID.Prefix is empty. Need to use orgIDArg and publicNamespaceArg to figure out what it should be + org, err := resolveOrg(client, publicNamespaceArg, orgIDArg) + if err != nil { + return moduleID{}, err + } + if org.PublicNamespace != "" { + mid.prefix = org.PublicNamespace + } else { + mid.prefix = org.Id + } + return mid, nil +} + +// resolveOrg accepts either an orgID or a publicNamespace (one must be an empty string). +// If orgID is an empty string, it will use the publicNamespace to resolve it. +func resolveOrg(client *viamClient, publicNamespace, orgID string) (*apppb.Organization, error) { + if orgID != "" { + if publicNamespace != "" { + return nil, errors.New("cannot specify both org-id and public-namespace") + } + if !isValidOrgID(orgID) { + return nil, errors.Errorf("provided org-id %q is not a valid org-id", orgID) + } + org, err := client.getOrg(orgID) + if err != nil { + return nil, err + } + return org, nil + } + // Use publicNamespace to back-derive what the org is + if publicNamespace == "" { + return nil, errors.New("must provide either org-id or public-namespace") + } + org, err := client.getUserOrgByPublicNamespace(publicNamespace) + if err != nil { + return nil, err + } + return org, nil +} + +func getOrgByModuleIDPrefix(client *viamClient, moduleIDPrefix string) (*apppb.Organization, error) { + if isValidOrgID(moduleIDPrefix) { + return client.getOrg(moduleIDPrefix) + } + return client.getUserOrgByPublicNamespace(moduleIDPrefix) +} + +// isValidOrgID checks if the str is a valid uuid. +func isValidOrgID(str string) bool { + _, err := uuid.Parse(str) + return err == nil +} + +func loadManifest(manifestPath string) (moduleManifest, error) { + //nolint:gosec + manifestBytes, err := os.ReadFile(manifestPath) + if err != nil { + if errors.Is(err, fs.ErrNotExist) { + return moduleManifest{}, errors.Wrapf(err, "cannot find %s", manifestPath) + } + return moduleManifest{}, err + } + var manifest moduleManifest + if err := json.Unmarshal(manifestBytes, &manifest); err != nil { + return moduleManifest{}, err + } + return manifest, nil +} + +func writeManifest(manifestPath string, manifest moduleManifest) error { + manifestBytes, err := json.MarshalIndent(manifest, "", " ") + if err != nil { + return err + } + //nolint:gosec + manifestFile, err := os.Create(manifestPath) + if err != nil { + return errors.Wrapf(err, "failed to create %s", manifestPath) + } + if _, err := manifestFile.Write(manifestBytes); err != nil { + return errors.Wrapf(err, "failed to write manifest to %s", manifestPath) + } + + return nil +} + +// getEntrypointForVersion returns the entrypoint associated with the provided version, or the last updated entrypoint if it doesnt exit. +func getEntrypointForVersion(mod *apppb.Module, version string) (string, error) { + for _, ver := range mod.Versions { + if ver.Version == version { + return ver.Entrypoint, nil + } + } + if mod.Entrypoint == "" { + return "", errors.New("no entrypoint has been set for your module. add one to your meta.json and then update your module") + } + // if there is no entrypoint set yet, use the last uploaded entrypoint + return mod.Entrypoint, nil +} diff --git a/cli/print.go b/cli/print.go new file mode 100644 index 00000000000..10bf467236f --- /dev/null +++ b/cli/print.go @@ -0,0 +1,71 @@ +package cli + +import ( + "fmt" + "io" + "log" + "os" + "unicode" + "unicode/utf8" + + "github.com/fatih/color" +) + +const asciiViam = ` +@@BO.. "%@@B^%@@< .}j. !B@B$v'. 'nB$$$! +.*@$%l f$$@X %$$+ &@$$ l$$$$@@^ "B$$$$@! + (@$$0'M@$@: B$$~ ~B$$@$@1 !$$BQ@@@q.0@$$0@$$! + 'WB$$B@p B$$~ qB$%-!@@@o. l$@B..B@$@$@%; @$@! + .u$$$! B$$~ ;@@@& ... oB$@~!!$$@. z$$$z'. $$$! + :h' B$$~L%@$|| -@@$bi$@B 'M' $$$! + +` + +// printf prints a message with no prefix. +func printf(w io.Writer, format string, a ...interface{}) { + fmt.Fprintf(w, format+"\n", a...) +} + +// infof prints a message prefixed with a bold cyan "Info: ". +func infof(w io.Writer, format string, a ...interface{}) { + // NOTE(benjirewis): for some reason, both errcheck and gosec complain about + // Fprint's "unchecked error" here. Fatally log any errors write errors here + // and below. + if _, err := color.New(color.Bold, color.FgCyan).Fprint(w, "Info: "); err != nil { + log.Fatal(err) + } + fmt.Fprintf(w, format+"\n", a...) +} + +// warningf prints a message prefixed with a bold yellow "Warning: ". +func warningf(w io.Writer, format string, a ...interface{}) { + if _, err := color.New(color.Bold, color.FgYellow).Fprint(w, "Warning: "); err != nil { + log.Fatal(err) + } + fmt.Fprintf(w, format+"\n", a...) +} + +// Errorf prints a message prefixed with a bold red "Error: " prefix and exits with 1. +// It also capitalizes the first letter of the message. +func Errorf(w io.Writer, format string, a ...interface{}) { + if _, err := color.New(color.Bold, color.FgRed).Fprint(w, "Error: "); err != nil { + log.Fatal(err) + } + + toPrint := fmt.Sprintf(format+"\n", a...) + r, i := utf8.DecodeRuneInString(toPrint) + if r == utf8.RuneError { + log.Fatal("Malformed error message:", toPrint) + } + upperR := unicode.ToUpper(r) + fmt.Fprintf(w, string(upperR)+toPrint[i:]) + + os.Exit(1) +} + +// viamLogo prints an ASCII Viam logo. +func viamLogo(w io.Writer) { + if _, err := color.New(color.Bold, color.FgWhite).Fprint(w, asciiViam); err != nil { + log.Fatal(err) + } +} diff --git a/cli/verify_main_test.go b/cli/verify_main_test.go new file mode 100644 index 00000000000..1f48ab185bc --- /dev/null +++ b/cli/verify_main_test.go @@ -0,0 +1,12 @@ +package cli + +import ( + "testing" + + testutilsext "go.viam.com/utils/testutils/ext" +) + +// TestMain is used to control the execution of all tests run within this package (including _test packages). +func TestMain(m *testing.M) { + testutilsext.VerifyTestMain(m) +} diff --git a/cli/viam/main.go b/cli/viam/main.go index 22c786504a9..eb5cea77cb0 100644 --- a/cli/viam/main.go +++ b/cli/viam/main.go @@ -2,945 +2,14 @@ package main import ( - "fmt" - "log" "os" - "time" - "github.com/edaniels/golog" - "github.com/pkg/errors" - "github.com/urfave/cli/v2" - "go.uber.org/zap" - datapb "go.viam.com/api/app/data/v1" - "google.golang.org/protobuf/types/known/timestamppb" - - rdkcli "go.viam.com/rdk/cli" -) - -const ( - // Flags. - dataFlagDestination = "destination" - dataFlagDataType = "data_type" - dataFlagOrgIDs = "org_ids" - dataFlagLocationIDs = "location_ids" - dataFlagRobotID = "robot_id" - dataFlagPartID = "part_id" - dataFlagRobotName = "robot_name" - dataFlagPartName = "part_name" - dataFlagComponentType = "component_type" - dataFlagComponentName = "component_name" - dataFlagMethod = "method" - dataFlagMimeTypes = "mime_types" - dataFlagStart = "start" - dataFlagEnd = "end" - dataFlagParallelDownloads = "parallel" - dataFlagTags = "tags" - dataFlagBboxLabels = "bbox_labels" - - dataTypeBinary = "binary" - dataTypeTabular = "tabular" + "go.viam.com/rdk/cli" ) func main() { - var logger golog.Logger - - app := &cli.App{ - Name: "viam", - Usage: "interact with your robots", - Flags: []cli.Flag{ - &cli.StringFlag{ - Name: "base-url", - Hidden: true, - Value: "https://app.viam.com:443", - Usage: "base URL of app", - }, - &cli.StringFlag{ - Name: "config", - Aliases: []string{"c"}, - Usage: "Load configuration from `FILE`", - }, - &cli.BoolFlag{ - Name: "debug", - Aliases: []string{"vvv"}, - Usage: "enable debug logging", - }, - }, - Before: func(c *cli.Context) error { - if c.Bool("debug") { - logger = golog.NewDebugLogger("cli") - } else { - logger = zap.NewNop().Sugar() - } - - return nil - }, - Commands: []*cli.Command{ - { - Name: "auth", - Usage: "authenticate to app.viam.com", - Action: func(c *cli.Context) error { - client, err := rdkcli.NewAppClient(c) - if err != nil { - return err - } - - loggedInMessage := func(token *rdkcli.Token) { - fmt.Fprintf(c.App.Writer, "Already authenticated as %q expires at %s\n", token.User.Email, token.ExpiresAt) - } - - if client.Config().Auth != nil && !client.Config().Auth.IsExpired() { - loggedInMessage(client.Config().Auth) - return nil - } - - if err := client.Login(); err != nil { - return err - } - - loggedInMessage(client.Config().Auth) - return nil - }, - Subcommands: []*cli.Command{ - { - Name: "print-access-token", - Usage: "print-access-token - print an access token for your current credentials", - Action: func(c *cli.Context) error { - client, err := rdkcli.NewAppClient(c) - if err != nil { - return err - } - - if client.Config().Auth == nil || client.Config().Auth.IsExpired() { - return errors.New("not authenticated. run \"auth\" command") - } - - fmt.Fprintln(c.App.Writer, client.Config().Auth.AccessToken) - - return nil - }, - }, - }, - }, - { - Name: "logout", - Usage: "logout from current session", - Action: func(c *cli.Context) error { - client, err := rdkcli.NewAppClient(c) - if err != nil { - return err - } - auth := client.Config().Auth - if auth == nil { - fmt.Fprintf(c.App.Writer, "Already logged out\n") - return nil - } - if err := client.Logout(); err != nil { - return err - } - fmt.Fprintf(c.App.Writer, "Logged out from %q\n", auth.User.Email) - return nil - }, - }, - { - Name: "whoami", - Usage: "get currently authenticated user", - Action: func(c *cli.Context) error { - client, err := rdkcli.NewAppClient(c) - if err != nil { - return err - } - auth := client.Config().Auth - if auth == nil { - fmt.Fprintf(c.App.Writer, "Not logged in\n") - return nil - } - fmt.Fprintf(c.App.Writer, "%s\n", auth.User.Email) - return nil - }, - }, - { - Name: "organizations", - Usage: "work with organizations", - Subcommands: []*cli.Command{ - { - Name: "list", - Usage: "list organizations", - Action: func(c *cli.Context) error { - client, err := rdkcli.NewAppClient(c) - if err != nil { - return err - } - orgs, err := client.ListOrganizations() - if err != nil { - return err - } - for _, org := range orgs { - fmt.Fprintf(c.App.Writer, "%s (id: %s)\n", org.Name, org.Id) - } - return nil - }, - }, - }, - }, - { - Name: "locations", - Usage: "work with locations", - Subcommands: []*cli.Command{ - { - Name: "list", - Usage: "list locations", - ArgsUsage: "[organization]", - Action: func(c *cli.Context) error { - client, err := rdkcli.NewAppClient(c) - if err != nil { - return err - } - orgStr := c.Args().First() - listLocations := func(orgID string) error { - locs, err := client.ListLocations(orgID) - if err != nil { - return err - } - for _, loc := range locs { - fmt.Fprintf(c.App.Writer, "%s (id: %s)\n", loc.Name, loc.Id) - } - return nil - } - if orgStr == "" { - orgs, err := client.ListOrganizations() - if err != nil { - return err - } - for i, org := range orgs { - if i != 0 { - fmt.Fprintln(c.App.Writer, "") - } - - fmt.Fprintf(c.App.Writer, "%s:\n", org.Name) - if err := listLocations(org.Id); err != nil { - return err - } - } - return nil - } - return listLocations(orgStr) - }, - }, - }, - }, - { - Name: "data", - Usage: "work with data", - Subcommands: []*cli.Command{ - { - Name: "export", - Usage: "download data from Viam cloud", - UsageText: fmt.Sprintf("viam data export <%s> <%s> [%s] [%s] [%s] [%s] [%s] [%s] [%s] [%s] [%s] [%s] [%s] [%s] [%s] [%s]", - dataFlagDestination, dataFlagDataType, dataFlagOrgIDs, dataFlagLocationIDs, dataFlagRobotID, dataFlagRobotName, - dataFlagPartID, dataFlagPartName, dataFlagComponentType, dataFlagComponentName, - dataFlagStart, dataFlagEnd, dataFlagMethod, dataFlagMimeTypes, dataFlagParallelDownloads, dataFlagTags), - Flags: []cli.Flag{ - &cli.PathFlag{ - Name: dataFlagDestination, - Required: true, - Usage: "output directory for downloaded data", - }, - &cli.StringFlag{ - Name: dataFlagDataType, - Required: true, - Usage: "data type to be downloaded: either binary or tabular", - }, - &cli.StringSliceFlag{ - Name: dataFlagOrgIDs, - Required: false, - Usage: "orgs filter", - }, - &cli.StringSliceFlag{ - Name: dataFlagLocationIDs, - Required: false, - Usage: "locations filter", - }, - &cli.StringFlag{ - Name: dataFlagRobotID, - Required: false, - Usage: "robot_id filter", - }, - &cli.StringFlag{ - Name: dataFlagPartID, - Required: false, - Usage: "part_id filter", - }, - &cli.StringFlag{ - Name: dataFlagRobotName, - Required: false, - Usage: "robot_name filter", - }, - &cli.StringFlag{ - Name: dataFlagPartName, - Required: false, - Usage: "part_name filter", - }, - &cli.StringFlag{ - Name: dataFlagComponentType, - Required: false, - Usage: "component_type filter", - }, - &cli.StringFlag{ - Name: dataFlagComponentName, - Required: false, - Usage: "component_name filter", - }, - &cli.StringFlag{ - Name: dataFlagMethod, - Required: false, - Usage: "method filter", - }, - &cli.StringSliceFlag{ - Name: dataFlagMimeTypes, - Required: false, - Usage: "mime_types filter", - }, - &cli.UintFlag{ - Name: dataFlagParallelDownloads, - Required: false, - Usage: "number of download requests to make in parallel, with a default value of 10", - }, - &cli.StringFlag{ - Name: dataFlagStart, - Required: false, - Usage: "ISO-8601 timestamp indicating the start of the interval filter", - }, - &cli.StringFlag{ - Name: dataFlagEnd, - Required: false, - Usage: "ISO-8601 timestamp indicating the end of the interval filter", - }, - &cli.StringSliceFlag{ - Name: dataFlagTags, - Required: false, - Usage: "tags filter. " + - "accepts tagged for all tagged data, untagged for all untagged data, or a list of tags for all data matching any of the tags", - }, - &cli.StringSliceFlag{ - Name: dataFlagBboxLabels, - Required: false, - Usage: "bbox labels filter. " + - "accepts string labels corresponding to bounding boxes within images", - }, - }, - Action: DataCommand, - }, - { - Name: "delete", - Usage: "delete data from Viam cloud", - UsageText: fmt.Sprintf("viam data delete [%s] [%s] [%s] [%s] [%s] [%s] [%s] [%s] [%s] [%s] [%s] [%s] [%s]", - dataFlagDataType, dataFlagOrgIDs, dataFlagLocationIDs, dataFlagRobotID, dataFlagRobotName, - dataFlagPartID, dataFlagPartName, dataFlagComponentType, dataFlagComponentName, - dataFlagStart, dataFlagEnd, dataFlagMethod, dataFlagMimeTypes), - Flags: []cli.Flag{ - &cli.StringFlag{ - Name: dataFlagDataType, - Required: false, - Usage: "data type to be deleted: either binary or tabular", - }, - &cli.StringSliceFlag{ - Name: dataFlagOrgIDs, - Required: false, - Usage: "orgs filter", - }, - &cli.StringSliceFlag{ - Name: dataFlagLocationIDs, - Required: false, - Usage: "locations filter", - }, - &cli.StringFlag{ - Name: dataFlagRobotID, - Required: false, - Usage: "robot_id filter", - }, - &cli.StringFlag{ - Name: dataFlagPartID, - Required: false, - Usage: "part_id filter", - }, - &cli.StringFlag{ - Name: dataFlagRobotName, - Required: false, - Usage: "robot_name filter", - }, - &cli.StringFlag{ - Name: dataFlagPartName, - Required: false, - Usage: "part_name filter", - }, - &cli.StringFlag{ - Name: dataFlagComponentType, - Required: false, - Usage: "component_type filter", - }, - &cli.StringFlag{ - Name: dataFlagComponentName, - Required: false, - Usage: "component_name filter", - }, - &cli.StringFlag{ - Name: dataFlagMethod, - Required: false, - Usage: "method filter", - }, - &cli.StringSliceFlag{ - Name: dataFlagMimeTypes, - Required: false, - Usage: "mime_types filter", - }, - &cli.StringFlag{ - Name: dataFlagStart, - Required: false, - Usage: "ISO-8601 timestamp indicating the start of the interval filter", - }, - &cli.StringFlag{ - Name: dataFlagEnd, - Required: false, - Usage: "ISO-8601 timestamp indicating the end of the interval filter", - }, - }, - Action: DeleteCommand, - }, - }, - }, - { - Name: "robots", - Usage: "work with robots", - Subcommands: []*cli.Command{ - { - Name: "list", - Usage: "list robots", - Flags: []cli.Flag{ - &cli.StringFlag{ - Name: "organization", - }, - &cli.StringFlag{ - Name: "location", - }, - }, - Action: func(c *cli.Context) error { - client, err := rdkcli.NewAppClient(c) - if err != nil { - return err - } - orgStr := c.String("organization") - locStr := c.String("location") - robots, err := client.ListRobots(orgStr, locStr) - if err != nil { - return err - } - - if orgStr == "" || locStr == "" { - fmt.Fprintf(c.App.Writer, "%s -> %s\n", client.SelectedOrg().Name, client.SelectedLoc().Name) - } - - for _, robot := range robots { - fmt.Fprintf(c.App.Writer, "%s (id: %s)\n", robot.Name, robot.Id) - } - return nil - }, - }, - }, - }, - { - Name: "robot", - Usage: "work with a robot", - Subcommands: []*cli.Command{ - { - Name: "status", - Usage: "display robot status", - Flags: []cli.Flag{ - &cli.StringFlag{ - Name: "organization", - }, - &cli.StringFlag{ - Name: "location", - }, - &cli.StringFlag{ - Name: "robot", - Required: true, - }, - }, - Action: func(c *cli.Context) error { - client, err := rdkcli.NewAppClient(c) - if err != nil { - return err - } - - orgStr := c.String("organization") - locStr := c.String("location") - robot, err := client.Robot(orgStr, locStr, c.String("robot")) - if err != nil { - return err - } - parts, err := client.RobotParts(client.SelectedOrg().Id, client.SelectedLoc().Id, robot.Id) - if err != nil { - return err - } - - if orgStr == "" || locStr == "" { - fmt.Fprintf(c.App.Writer, "%s -> %s\n", client.SelectedOrg().Name, client.SelectedLoc().Name) - } - - fmt.Fprintf( - c.App.Writer, - "ID: %s\nName: %s\nLast Access: %s (%s ago)\n", - robot.Id, - robot.Name, - robot.LastAccess.AsTime().Format(time.UnixDate), - time.Since(robot.LastAccess.AsTime()), - ) - - if len(parts) != 0 { - fmt.Fprintln(c.App.Writer, "Parts:") - } - for i, part := range parts { - name := part.Name - if part.MainPart { - name += " (main)" - } - fmt.Fprintf( - c.App.Writer, - "\tID: %s\n\tName: %s\n\tLast Access: %s (%s ago)\n", - part.Id, - name, - part.LastAccess.AsTime().Format(time.UnixDate), - time.Since(part.LastAccess.AsTime()), - ) - if i != len(parts)-1 { - fmt.Fprintln(c.App.Writer, "") - } - } - - return nil - }, - }, - { - Name: "logs", - Usage: "display robot logs", - Flags: []cli.Flag{ - &cli.StringFlag{ - Name: "organization", - }, - &cli.StringFlag{ - Name: "location", - }, - &cli.StringFlag{ - Name: "robot", - Required: true, - }, - &cli.BoolFlag{ - Name: "errors", - Usage: "show only errors", - }, - }, - Action: func(c *cli.Context) error { - client, err := rdkcli.NewAppClient(c) - if err != nil { - return err - } - - orgStr := c.String("organization") - locStr := c.String("location") - robotStr := c.String("robot") - robot, err := client.Robot(orgStr, locStr, robotStr) - if err != nil { - return err - } - - parts, err := client.RobotParts(orgStr, locStr, robotStr) - if err != nil { - return err - } - - for i, part := range parts { - if i != 0 { - fmt.Fprintln(c.App.Writer, "") - } - - var header string - if orgStr == "" || locStr == "" || robotStr == "" { - header = fmt.Sprintf("%s -> %s -> %s -> %s", client.SelectedOrg().Name, client.SelectedLoc().Name, robot.Name, part.Name) - } else { - header = part.Name - } - if err := client.PrintRobotPartLogs( - orgStr, locStr, robotStr, part.Id, - c.Bool("errors"), - "\t", - header, - ); err != nil { - return err - } - } - - return nil - }, - }, - { - Name: "part", - Usage: "work with robot part", - Subcommands: []*cli.Command{ - { - Name: "status", - Usage: "display part status", - Flags: []cli.Flag{ - &cli.StringFlag{ - Name: "organization", - }, - &cli.StringFlag{ - Name: "location", - }, - &cli.StringFlag{ - Name: "robot", - Required: true, - }, - &cli.StringFlag{ - Name: "part", - Required: true, - }, - }, - Action: func(c *cli.Context) error { - client, err := rdkcli.NewAppClient(c) - if err != nil { - return err - } - - orgStr := c.String("organization") - locStr := c.String("location") - robotStr := c.String("robot") - robot, err := client.Robot(orgStr, locStr, robotStr) - if err != nil { - return err - } - - part, err := client.RobotPart(orgStr, locStr, robotStr, c.String("part")) - if err != nil { - return err - } - - if orgStr == "" || locStr == "" || robotStr == "" { - fmt.Fprintf(c.App.Writer, "%s -> %s -> %s\n", client.SelectedOrg().Name, client.SelectedLoc().Name, robot.Name) - } - - name := part.Name - if part.MainPart { - name += " (main)" - } - fmt.Fprintf( - c.App.Writer, - "ID: %s\nName: %s\nLast Access: %s (%s ago)\n", - part.Id, - name, - part.LastAccess.AsTime().Format(time.UnixDate), - time.Since(part.LastAccess.AsTime()), - ) - - return nil - }, - }, - { - Name: "logs", - Usage: "display part logs", - Flags: []cli.Flag{ - &cli.StringFlag{ - Name: "organization", - }, - &cli.StringFlag{ - Name: "location", - }, - &cli.StringFlag{ - Name: "robot", - Required: true, - }, - &cli.StringFlag{ - Name: "part", - Required: true, - }, - &cli.BoolFlag{ - Name: "errors", - Usage: "show only errors", - }, - &cli.BoolFlag{ - Name: "tail", - Aliases: []string{"f"}, - Usage: "follow logs", - }, - }, - Action: func(c *cli.Context) error { - client, err := rdkcli.NewAppClient(c) - if err != nil { - return err - } - - orgStr := c.String("organization") - locStr := c.String("location") - robotStr := c.String("robot") - robot, err := client.Robot(orgStr, locStr, robotStr) - if err != nil { - return err - } - - var header string - if orgStr == "" || locStr == "" || robotStr == "" { - header = fmt.Sprintf("%s -> %s -> %s", client.SelectedOrg().Name, client.SelectedLoc().Name, robot.Name) - } - if c.Bool("tail") { - return client.TailRobotPartLogs( - orgStr, locStr, robotStr, c.String("part"), - c.Bool("errors"), - "", - header, - ) - } - return client.PrintRobotPartLogs( - orgStr, locStr, robotStr, c.String("part"), - c.Bool("errors"), - "", - header, - ) - }, - }, - { - Name: "run", - Usage: "run a command on a robot part", - ArgsUsage: "", - Flags: []cli.Flag{ - &cli.StringFlag{ - Name: "organization", - Required: true, - }, - &cli.StringFlag{ - Name: "location", - Required: true, - }, - &cli.StringFlag{ - Name: "robot", - Required: true, - }, - &cli.StringFlag{ - Name: "part", - Required: true, - }, - &cli.StringFlag{ - Name: "data", - Aliases: []string{"d"}, - }, - &cli.DurationFlag{ - Name: "stream", - Aliases: []string{"s"}, - }, - }, - Action: func(c *cli.Context) error { - svcMethod := c.Args().First() - if svcMethod == "" { - fmt.Fprintln(c.App.ErrWriter, "service method required") - cli.ShowSubcommandHelpAndExit(c, 1) - return nil - } - - client, err := rdkcli.NewAppClient(c) - if err != nil { - return err - } - - return client.RunRobotPartCommand( - c.String("organization"), - c.String("location"), - c.String("robot"), - c.String("part"), - svcMethod, - c.String("data"), - c.Duration("stream"), - c.Bool("debug"), - logger, - ) - }, - }, - { - Name: "shell", - Usage: "start a shell on a robot part", - Flags: []cli.Flag{ - &cli.StringFlag{ - Name: "organization", - Required: true, - }, - &cli.StringFlag{ - Name: "location", - Required: true, - }, - &cli.StringFlag{ - Name: "robot", - Required: true, - }, - &cli.StringFlag{ - Name: "part", - Required: true, - }, - }, - Action: func(c *cli.Context) error { - client, err := rdkcli.NewAppClient(c) - if err != nil { - return err - } - - return client.StartRobotPartShell( - c.String("organization"), - c.String("location"), - c.String("robot"), - c.String("part"), - c.Bool("debug"), - logger, - ) - }, - }, - }, - }, - }, - }, - }, - } - + app := cli.NewApp(os.Stdout, os.Stderr) if err := app.Run(os.Args); err != nil { - log.Fatal(err) - } -} - -// DataCommand runs the data command for downloading data from the Viam cloud. -func DataCommand(c *cli.Context) error { - filter, err := createDataFilter(c) - if err != nil { - return err - } - - client, err := rdkcli.NewAppClient(c) - if err != nil { - return err - } - - switch c.String(dataFlagDataType) { - case dataTypeBinary: - if err := client.BinaryData(c.Path(dataFlagDestination), filter, c.Uint(dataFlagParallelDownloads)); err != nil { - return err - } - case dataTypeTabular: - if err := client.TabularData(c.Path(dataFlagDestination), filter); err != nil { - return err - } - default: - return errors.Errorf("type must be binary or tabular, got %s", c.String("type")) - } - return nil -} - -// DeleteCommand runs the command for deleting data from the Viam cloud. -func DeleteCommand(c *cli.Context) error { - filter, err := createDataFilter(c) - if err != nil { - return err - } - - client, err := rdkcli.NewAppClient(c) - if err != nil { - return err - } - - switch c.String(dataFlagDataType) { - case dataTypeBinary: - if err := client.DeleteBinaryData(filter); err != nil { - return err - } - case dataTypeTabular: - if err := client.DeleteTabularData(filter); err != nil { - return err - } - default: - return errors.Errorf("type must be binary or tabular, got %s", c.String("type")) - } - - return nil -} - -func createDataFilter(c *cli.Context) (*datapb.Filter, error) { - filter := &datapb.Filter{} - - if c.StringSlice(dataFlagOrgIDs) != nil { - filter.OrganizationIds = c.StringSlice(dataFlagOrgIDs) - } - if c.StringSlice(dataFlagLocationIDs) != nil { - filter.LocationIds = c.StringSlice(dataFlagLocationIDs) - } - if c.String(dataFlagRobotID) != "" { - filter.RobotId = c.String(dataFlagRobotID) - } - if c.String(dataFlagPartID) != "" { - filter.PartId = c.String(dataFlagPartID) - } - if c.String(dataFlagRobotName) != "" { - filter.RobotName = c.String(dataFlagRobotName) - } - if c.String(dataFlagPartName) != "" { - filter.PartName = c.String(dataFlagPartName) - } - if c.String(dataFlagComponentType) != "" { - filter.ComponentType = c.String(dataFlagComponentType) - } - if c.String(dataFlagComponentName) != "" { - filter.ComponentName = c.String(dataFlagComponentName) - } - if c.String(dataFlagMethod) != "" { - filter.Method = c.String(dataFlagMethod) - } - if len(c.StringSlice(dataFlagMimeTypes)) != 0 { - filter.MimeType = c.StringSlice(dataFlagMimeTypes) - } - if c.StringSlice(dataFlagTags) != nil { - switch { - case len(c.StringSlice(dataFlagTags)) == 1 && c.StringSlice(dataFlagTags)[0] == "tagged": - filter.TagsFilter = &datapb.TagsFilter{ - Type: datapb.TagsFilterType_TAGS_FILTER_TYPE_TAGGED, - } - case len(c.StringSlice(dataFlagTags)) == 1 && c.StringSlice(dataFlagTags)[0] == "untagged": - filter.TagsFilter = &datapb.TagsFilter{ - Type: datapb.TagsFilterType_TAGS_FILTER_TYPE_UNTAGGED, - } - default: - filter.TagsFilter = &datapb.TagsFilter{ - Type: datapb.TagsFilterType_TAGS_FILTER_TYPE_MATCH_BY_OR, - Tags: c.StringSlice(dataFlagTags), - } - } - } - if len(c.StringSlice(dataFlagBboxLabels)) != 0 { - filter.BboxLabels = c.StringSlice(dataFlagBboxLabels) - } - var start *timestamppb.Timestamp - var end *timestamppb.Timestamp - timeLayout := time.RFC3339 - if c.String(dataFlagStart) != "" { - t, err := time.Parse(timeLayout, c.String(dataFlagStart)) - if err != nil { - return nil, errors.Wrap(err, "error parsing start flag") - } - start = timestamppb.New(t) - } - if c.String(dataFlagEnd) != "" { - t, err := time.Parse(timeLayout, c.String(dataFlagEnd)) - if err != nil { - return nil, errors.Wrap(err, "error parsing end flag") - } - end = timestamppb.New(t) - } - if start != nil || end != nil { - filter.Interval = &datapb.CaptureInterval{ - Start: start, - End: end, - } + cli.Errorf(app.ErrWriter, err.Error()) } - return filter, nil } diff --git a/components/arm/arm.go b/components/arm/arm.go index e8f1a7553b8..d1f1d9f6232 100644 --- a/components/arm/arm.go +++ b/components/arm/arm.go @@ -8,6 +8,7 @@ import ( "strings" "github.com/edaniels/golog" + v1 "go.viam.com/api/common/v1" pb "go.viam.com/api/component/arm/v1" motionpb "go.viam.com/api/service/motion/v1" @@ -83,9 +84,6 @@ type Arm interface { JointPositions(ctx context.Context, extra map[string]interface{}) (*pb.JointPositions, error) } -// ErrStopUnimplemented is used for when Stop is unimplemented. -var ErrStopUnimplemented = errors.New("Stop unimplemented") - // FromDependencies is a helper for getting the named arm from a collection of // dependencies. func FromDependencies(deps resource.Dependencies, name string) (Arm, error) { @@ -110,15 +108,17 @@ func CreateStatus(ctx context.Context, a Arm) (*pb.Status, error) { return nil, err } model := a.ModelFrame() - endPosition, err := motionplan.ComputeOOBPosition(model, jointPositions) - if err != nil { - return nil, err + + var endPosition *v1.Pose + if endPose, err := motionplan.ComputeOOBPosition(model, jointPositions); err == nil { + endPosition = spatialmath.PoseToProtobuf(endPose) } + isMoving, err := a.IsMoving(ctx) if err != nil { return nil, err } - return &pb.Status{EndPosition: spatialmath.PoseToProtobuf(endPosition), JointPositions: jointPositions, IsMoving: isMoving}, nil + return &pb.Status{EndPosition: endPosition, JointPositions: jointPositions, IsMoving: isMoving}, nil } // Move is a helper function to abstract away movement for general arms. diff --git a/components/arm/arm_test.go b/components/arm/arm_test.go index 0b99af83267..36260551075 100644 --- a/components/arm/arm_test.go +++ b/components/arm/arm_test.go @@ -60,97 +60,156 @@ func TestStatusValid(t *testing.T) { } func TestCreateStatus(t *testing.T) { - testPose := spatialmath.NewPose( + successfulPose := spatialmath.NewPose( r3.Vector{-802.801508917897990613710135, -248.284077946287368376943050, 9.115758604150467903082244}, &spatialmath.R4AA{1.5810814917942602, 0.992515011486776, -0.0953988491934626, 0.07624310818669232}, ) - status := &pb.Status{ - EndPosition: spatialmath.PoseToProtobuf(testPose), + successfulStatus := &pb.Status{ + EndPosition: spatialmath.PoseToProtobuf(successfulPose), JointPositions: &pb.JointPositions{Values: []float64{1.1, 2.2, 3.3, 1.1, 2.2, 3.3}}, IsMoving: true, } injectArm := &inject.Arm{} - injectArm.EndPositionFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.Pose, error) { - return pose, nil - } - injectArm.JointPositionsFunc = func(ctx context.Context, extra map[string]interface{}) (*pb.JointPositions, error) { - return &pb.JointPositions{Values: status.JointPositions.Values}, nil + + //nolint:unparam + successfulJointPositionsFunc := func(context.Context, map[string]interface{}) (*pb.JointPositions, error) { + return successfulStatus.JointPositions, nil } - injectArm.IsMovingFunc = func(context.Context) (bool, error) { + + successfulIsMovingFunc := func(context.Context) (bool, error) { return true, nil } - injectArm.ModelFrameFunc = func() referenceframe.Model { + + successfulModelFrameFunc := func() referenceframe.Model { model, _ := ur.MakeModelFrame("ur5e") return model } t.Run("working", func(t *testing.T) { - status1, err := arm.CreateStatus(context.Background(), injectArm) + injectArm.JointPositionsFunc = successfulJointPositionsFunc + injectArm.IsMovingFunc = successfulIsMovingFunc + injectArm.ModelFrameFunc = successfulModelFrameFunc + + expectedPose := successfulPose + expectedStatus := successfulStatus + + actualStatus, err := arm.CreateStatus(context.Background(), injectArm) test.That(t, err, test.ShouldBeNil) - test.That(t, status1.IsMoving, test.ShouldResemble, status.IsMoving) - test.That(t, status1.JointPositions, test.ShouldResemble, status.JointPositions) - pose1 := spatialmath.NewPoseFromProtobuf(status1.EndPosition) - pose2 := spatialmath.NewPoseFromProtobuf(status.EndPosition) - test.That(t, spatialmath.PoseAlmostEqualEps(pose1, pose2, 0.01), test.ShouldBeTrue) + test.That(t, actualStatus.IsMoving, test.ShouldEqual, expectedStatus.IsMoving) + test.That(t, actualStatus.JointPositions, test.ShouldResemble, expectedStatus.JointPositions) + + actualPose := spatialmath.NewPoseFromProtobuf(actualStatus.EndPosition) + test.That(t, spatialmath.PoseAlmostEqualEps(actualPose, expectedPose, 0.01), test.ShouldBeTrue) resourceAPI, ok, err := resource.LookupAPIRegistration[arm.Arm](arm.API) test.That(t, err, test.ShouldBeNil) test.That(t, ok, test.ShouldBeTrue) - status2, err := resourceAPI.Status(context.Background(), injectArm) + statusInterface, err := resourceAPI.Status(context.Background(), injectArm) test.That(t, err, test.ShouldBeNil) - statusMap, err := protoutils.InterfaceToMap(status2) + statusMap, err := protoutils.InterfaceToMap(statusInterface) test.That(t, err, test.ShouldBeNil) endPosMap, err := protoutils.InterfaceToMap(statusMap["end_position"]) test.That(t, err, test.ShouldBeNil) - pose3 := spatialmath.NewPose( + actualPose = spatialmath.NewPose( r3.Vector{endPosMap["x"].(float64), endPosMap["y"].(float64), endPosMap["z"].(float64)}, &spatialmath.OrientationVectorDegrees{ endPosMap["theta"].(float64), endPosMap["o_x"].(float64), endPosMap["o_y"].(float64), endPosMap["o_z"].(float64), }, ) - test.That(t, spatialmath.PoseAlmostEqualEps(pose3, pose2, 0.01), test.ShouldBeTrue) + test.That(t, spatialmath.PoseAlmostEqualEps(actualPose, expectedPose, 0.01), test.ShouldBeTrue) moving := statusMap["is_moving"].(bool) - test.That(t, moving, test.ShouldResemble, status.IsMoving) + test.That(t, moving, test.ShouldEqual, expectedStatus.IsMoving) jPosFace := statusMap["joint_positions"].(map[string]interface{})["values"].([]interface{}) - jPos := []float64{ + actualJointPositions := []float64{ jPosFace[0].(float64), jPosFace[1].(float64), jPosFace[2].(float64), jPosFace[3].(float64), jPosFace[4].(float64), jPosFace[5].(float64), } - test.That(t, jPos, test.ShouldResemble, status.JointPositions.Values) + test.That(t, actualJointPositions, test.ShouldResemble, expectedStatus.JointPositions.Values) }) t.Run("not moving", func(t *testing.T) { + injectArm.JointPositionsFunc = successfulJointPositionsFunc + injectArm.ModelFrameFunc = successfulModelFrameFunc + injectArm.IsMovingFunc = func(context.Context) (bool, error) { return false, nil } - status2 := &pb.Status{ - EndPosition: spatialmath.PoseToProtobuf(testPose), - JointPositions: &pb.JointPositions{Values: []float64{1.1, 2.2, 3.3, 1.1, 2.2, 3.3}}, + expectedPose := successfulPose + expectedStatus := &pb.Status{ + EndPosition: successfulStatus.EndPosition, //nolint:govet + JointPositions: successfulStatus.JointPositions, IsMoving: false, } - status1, err := arm.CreateStatus(context.Background(), injectArm) + + actualStatus, err := arm.CreateStatus(context.Background(), injectArm) test.That(t, err, test.ShouldBeNil) - test.That(t, status1.IsMoving, test.ShouldResemble, status2.IsMoving) - test.That(t, status1.JointPositions, test.ShouldResemble, status2.JointPositions) - pose1 := spatialmath.NewPoseFromProtobuf(status1.EndPosition) - pose2 := spatialmath.NewPoseFromProtobuf(status2.EndPosition) - test.That(t, spatialmath.PoseAlmostEqualEps(pose1, pose2, 0.01), test.ShouldBeTrue) + test.That(t, actualStatus.IsMoving, test.ShouldEqual, expectedStatus.IsMoving) + test.That(t, actualStatus.JointPositions, test.ShouldResemble, expectedStatus.JointPositions) + actualPose := spatialmath.NewPoseFromProtobuf(actualStatus.EndPosition) + test.That(t, spatialmath.PoseAlmostEqualEps(actualPose, expectedPose, 0.01), test.ShouldBeTrue) }) t.Run("fail on JointPositions", func(t *testing.T) { + injectArm.IsMovingFunc = successfulIsMovingFunc + injectArm.ModelFrameFunc = successfulModelFrameFunc + errFail := errors.New("can't get joint positions") injectArm.JointPositionsFunc = func(ctx context.Context, extra map[string]interface{}) (*pb.JointPositions, error) { return nil, errFail } - _, err := arm.CreateStatus(context.Background(), injectArm) + + actualStatus, err := arm.CreateStatus(context.Background(), injectArm) test.That(t, err, test.ShouldBeError, errFail) + test.That(t, actualStatus, test.ShouldBeNil) + }) + + t.Run("nil JointPositions", func(t *testing.T) { + injectArm.IsMovingFunc = successfulIsMovingFunc + injectArm.ModelFrameFunc = successfulModelFrameFunc + + injectArm.JointPositionsFunc = func(ctx context.Context, extra map[string]interface{}) (*pb.JointPositions, error) { + return nil, nil //nolint:nilnil + } + + expectedStatus := &pb.Status{ + EndPosition: nil, + JointPositions: nil, + IsMoving: successfulStatus.IsMoving, + } + + actualStatus, err := arm.CreateStatus(context.Background(), injectArm) + test.That(t, err, test.ShouldBeNil) + test.That(t, actualStatus.EndPosition, test.ShouldEqual, expectedStatus.EndPosition) + test.That(t, actualStatus.JointPositions, test.ShouldEqual, expectedStatus.JointPositions) + test.That(t, actualStatus.IsMoving, test.ShouldEqual, expectedStatus.IsMoving) + }) + + t.Run("nil model frame", func(t *testing.T) { + injectArm.IsMovingFunc = successfulIsMovingFunc + injectArm.JointPositionsFunc = successfulJointPositionsFunc + + injectArm.ModelFrameFunc = func() referenceframe.Model { + return nil + } + + expectedStatus := &pb.Status{ + EndPosition: nil, + JointPositions: successfulStatus.JointPositions, + IsMoving: successfulStatus.IsMoving, + } + + actualStatus, err := arm.CreateStatus(context.Background(), injectArm) + test.That(t, err, test.ShouldBeNil) + test.That(t, actualStatus.EndPosition, test.ShouldEqual, expectedStatus.EndPosition) + test.That(t, actualStatus.JointPositions, test.ShouldResemble, expectedStatus.JointPositions) + test.That(t, actualStatus.IsMoving, test.ShouldEqual, expectedStatus.IsMoving) }) } diff --git a/components/arm/client.go b/components/arm/client.go index 22a94813ab7..132fa07e680 100644 --- a/components/arm/client.go +++ b/components/arm/client.go @@ -46,9 +46,9 @@ func NewClientFromConn( client: pbClient, logger: logger, } - clientFrame, err := c.updateKinematics(ctx) + clientFrame, err := c.updateKinematics(ctx, nil) if err != nil { - logger.Errorw("error getting model for arm; will not allow certain methods") + logger.Errorw("error getting model for arm; will not allow certain methods", "err", err) } else { c.model = clientFrame } @@ -157,16 +157,30 @@ func (c *client) IsMoving(ctx context.Context) (bool, error) { return resp.IsMoving, nil } -func (c *client) Geometries(ctx context.Context) ([]spatialmath.Geometry, error) { - resp, err := c.client.GetGeometries(ctx, &commonpb.GetGeometriesRequest{Name: c.name}) +func (c *client) Geometries(ctx context.Context, extra map[string]interface{}) ([]spatialmath.Geometry, error) { + ext, err := protoutils.StructToStructPb(extra) + if err != nil { + return nil, err + } + resp, err := c.client.GetGeometries(ctx, &commonpb.GetGeometriesRequest{ + Name: c.name, + Extra: ext, + }) if err != nil { return nil, err } return spatialmath.NewGeometriesFromProto(resp.GetGeometries()) } -func (c *client) updateKinematics(ctx context.Context) (referenceframe.Model, error) { - resp, err := c.client.GetKinematics(ctx, &commonpb.GetKinematicsRequest{Name: c.name}) +func (c *client) updateKinematics(ctx context.Context, extra map[string]interface{}) (referenceframe.Model, error) { + ext, err := protoutils.StructToStructPb(extra) + if err != nil { + return nil, err + } + resp, err := c.client.GetKinematics(ctx, &commonpb.GetKinematicsRequest{ + Name: c.name, + Extra: ext, + }) if err != nil { return nil, err } diff --git a/components/arm/client_test.go b/components/arm/client_test.go index 12ab1dbe4e4..ae7657def43 100644 --- a/components/arm/client_test.go +++ b/components/arm/client_test.go @@ -60,7 +60,7 @@ func TestClient(t *testing.T) { } injectArm.StopFunc = func(ctx context.Context, extra map[string]interface{}) error { extraOptions = extra - return arm.ErrStopUnimplemented + return errStopUnimplemented } injectArm.ModelFrameFunc = func() referenceframe.Model { data := []byte("{\"links\": [{\"parent\": \"world\"}]}") @@ -130,7 +130,7 @@ func TestClient(t *testing.T) { cancel() _, err = viamgrpc.Dial(cancelCtx, listener1.Addr().String(), logger) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "canceled") + test.That(t, err, test.ShouldBeError, context.Canceled) }) // working @@ -169,7 +169,7 @@ func TestClient(t *testing.T) { err = arm1Client.Stop(context.Background(), map[string]interface{}{"foo": "Stop"}) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, arm.ErrStopUnimplemented.Error()) + test.That(t, err.Error(), test.ShouldContainSubstring, errStopUnimplemented.Error()) test.That(t, extraOptions, test.ShouldResemble, map[string]interface{}{"foo": "Stop"}) test.That(t, arm1Client.Close(context.Background()), test.ShouldBeNil) diff --git a/components/arm/collectors.go b/components/arm/collectors.go index d6736d34520..fa8c8582925 100644 --- a/components/arm/collectors.go +++ b/components/arm/collectors.go @@ -2,6 +2,7 @@ package arm import ( "context" + "errors" "google.golang.org/protobuf/types/known/anypb" @@ -32,8 +33,13 @@ func newEndPositionCollector(resource interface{}, params data.CollectorParams) } cFunc := data.CaptureFunc(func(ctx context.Context, _ map[string]*anypb.Any) (interface{}, error) { - v, err := arm.EndPosition(ctx, nil) + v, err := arm.EndPosition(ctx, data.FromDMExtraMap) if err != nil { + // A modular filter component can be created to filter the readings from a component. The error ErrNoCaptureToStore + // is used in the datamanager to exclude readings from being captured and stored. + if errors.Is(err, data.ErrNoCaptureToStore) { + return nil, err + } return nil, data.FailedToReadErr(params.ComponentName, endPosition.String(), err) } return v, nil @@ -48,8 +54,13 @@ func newJointPositionsCollector(resource interface{}, params data.CollectorParam } cFunc := data.CaptureFunc(func(ctx context.Context, _ map[string]*anypb.Any) (interface{}, error) { - v, err := arm.JointPositions(ctx, nil) + v, err := arm.JointPositions(ctx, data.FromDMExtraMap) if err != nil { + // A modular filter component can be created to filter the readings from a component. The error ErrNoCaptureToStore + // is used in the datamanager to exclude readings from being captured and stored. + if errors.Is(err, data.ErrNoCaptureToStore) { + return nil, err + } return nil, data.FailedToReadErr(params.ComponentName, jointPositions.String(), err) } return v, nil diff --git a/components/arm/eva/eva.go b/components/arm/eva/eva.go index a747baa553b..4908bf73883 100644 --- a/components/arm/eva/eva.go +++ b/components/arm/eva/eva.go @@ -91,7 +91,7 @@ type eva struct { frameJSON []byte - opMgr operation.SingleOperationManager + opMgr *operation.SingleOperationManager } // NewEva TODO. @@ -114,6 +114,7 @@ func NewEva(ctx context.Context, conf resource.Config, logger golog.Logger) (arm logger: logger, model: model, frameJSON: evamodeljson, + opMgr: operation.NewSingleOperationManager(), } name, err := e.apiName(ctx) @@ -405,7 +406,7 @@ func MakeModelFrame(name string) (referenceframe.Model, error) { // Geometries returns the list of geometries associated with the resource, in any order. The poses of the geometries reflect their // current location relative to the frame of the resource. -func (e *eva) Geometries(ctx context.Context) ([]spatialmath.Geometry, error) { +func (e *eva) Geometries(ctx context.Context, extra map[string]interface{}) ([]spatialmath.Geometry, error) { inputs, err := e.CurrentInputs(ctx) if err != nil { return nil, err diff --git a/components/arm/fake/fake.go b/components/arm/fake/fake.go index 9f9a4e2c840..23f8df4e4ab 100644 --- a/components/arm/fake/fake.go +++ b/components/arm/fake/fake.go @@ -219,7 +219,7 @@ func (a *Arm) Close(ctx context.Context) error { // Geometries returns the list of geometries associated with the resource, in any order. The poses of the geometries reflect their // current location relative to the frame of the resource. -func (a *Arm) Geometries(ctx context.Context) ([]spatialmath.Geometry, error) { +func (a *Arm) Geometries(ctx context.Context, extra map[string]interface{}) ([]spatialmath.Geometry, error) { inputs, err := a.CurrentInputs(ctx) if err != nil { return nil, err diff --git a/components/arm/server.go b/components/arm/server.go index 25cb408ffdc..3a22b4cd155 100644 --- a/components/arm/server.go +++ b/components/arm/server.go @@ -113,7 +113,7 @@ func (s *serviceServer) Geometries(ctx context.Context, req *commonpb.GetGeometr if err != nil { return nil, err } - geometries, err := res.Geometries(ctx) + geometries, err := res.Geometries(ctx, req.Extra.AsMap()) if err != nil { return nil, err } diff --git a/components/arm/server_test.go b/components/arm/server_test.go index 82518b5c81b..0d95c24a629 100644 --- a/components/arm/server_test.go +++ b/components/arm/server_test.go @@ -17,6 +17,15 @@ import ( "go.viam.com/rdk/testutils/inject" ) +var ( + errGetPoseFailed = errors.New("can't get pose") + errGetJointsFailed = errors.New("can't get joint positions") + errMoveToPositionFailed = errors.New("can't move to pose") + errMoveToJointPositionFailed = errors.New("can't move to joint positions") + errStopUnimplemented = errors.New("Stop unimplemented") + errArmUnimplemented = errors.New("not found") +) + func newServer() (pb.ArmServiceServer, *inject.Arm, *inject.Arm, error) { injectArm := &inject.Arm{} injectArm2 := &inject.Arm{} @@ -70,28 +79,28 @@ func TestServer(t *testing.T) { pose2 := &commonpb.Pose{X: 4, Y: 5, Z: 6} positionDegs2 := &pb.JointPositions{Values: []float64{4.0, 5.0, 6.0}} injectArm2.EndPositionFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.Pose, error) { - return nil, errors.New("can't get pose") + return nil, errGetPoseFailed } injectArm2.JointPositionsFunc = func(ctx context.Context, extra map[string]interface{}) (*pb.JointPositions, error) { - return nil, errors.New("can't get joint positions") + return nil, errGetJointsFailed } injectArm2.MoveToPositionFunc = func(ctx context.Context, ap spatialmath.Pose, extra map[string]interface{}) error { capArmPos = ap - return errors.New("can't move to pose") + return errMoveToPositionFailed } injectArm2.MoveToJointPositionsFunc = func(ctx context.Context, jp *pb.JointPositions, extra map[string]interface{}) error { capArmJointPos = jp - return errors.New("can't move to joint positions") + return errMoveToJointPositionFailed } injectArm2.StopFunc = func(ctx context.Context, extra map[string]interface{}) error { - return arm.ErrStopUnimplemented + return errStopUnimplemented } t.Run("arm position", func(t *testing.T) { _, err := armServer.GetEndPosition(context.Background(), &pb.GetEndPositionRequest{Name: missingArmName}) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "not found") + test.That(t, err.Error(), test.ShouldContainSubstring, errArmUnimplemented.Error()) ext, err := protoutils.StructToStructPb(map[string]interface{}{"foo": "EndPosition"}) test.That(t, err, test.ShouldBeNil) @@ -103,13 +112,13 @@ func TestServer(t *testing.T) { _, err = armServer.GetEndPosition(context.Background(), &pb.GetEndPositionRequest{Name: failArmName}) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "can't get pose") + test.That(t, err.Error(), test.ShouldContainSubstring, errGetPoseFailed.Error()) }) t.Run("move to position", func(t *testing.T) { _, err = armServer.MoveToPosition(context.Background(), &pb.MoveToPositionRequest{Name: missingArmName, To: pose2}) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "not found") + test.That(t, err.Error(), test.ShouldContainSubstring, errArmUnimplemented.Error()) ext, err := protoutils.StructToStructPb(map[string]interface{}{"foo": "MoveToPosition"}) test.That(t, err, test.ShouldBeNil) @@ -123,14 +132,14 @@ func TestServer(t *testing.T) { To: spatialmath.PoseToProtobuf(pose1), }) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "can't move to pose") + test.That(t, err.Error(), test.ShouldContainSubstring, errMoveToPositionFailed.Error()) test.That(t, spatialmath.PoseAlmostCoincident(capArmPos, pose1), test.ShouldBeTrue) }) t.Run("arm joint position", func(t *testing.T) { _, err := armServer.GetJointPositions(context.Background(), &pb.GetJointPositionsRequest{Name: missingArmName}) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "not found") + test.That(t, err.Error(), test.ShouldContainSubstring, errArmUnimplemented.Error()) ext, err := protoutils.StructToStructPb(map[string]interface{}{"foo": "JointPositions"}) test.That(t, err, test.ShouldBeNil) @@ -141,7 +150,7 @@ func TestServer(t *testing.T) { _, err = armServer.GetJointPositions(context.Background(), &pb.GetJointPositionsRequest{Name: failArmName}) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "can't get joint positions") + test.That(t, err.Error(), test.ShouldContainSubstring, errGetJointsFailed.Error()) }) t.Run("move to joint position", func(t *testing.T) { @@ -150,7 +159,7 @@ func TestServer(t *testing.T) { &pb.MoveToJointPositionsRequest{Name: missingArmName, Positions: positionDegs2}, ) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "not found") + test.That(t, err.Error(), test.ShouldContainSubstring, errArmUnimplemented.Error()) ext, err := protoutils.StructToStructPb(map[string]interface{}{"foo": "MoveToJointPositions"}) test.That(t, err, test.ShouldBeNil) @@ -167,14 +176,14 @@ func TestServer(t *testing.T) { &pb.MoveToJointPositionsRequest{Name: failArmName, Positions: positionDegs1}, ) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "can't move to joint positions") + test.That(t, err.Error(), test.ShouldContainSubstring, errMoveToJointPositionFailed.Error()) test.That(t, capArmJointPos.String(), test.ShouldResemble, positionDegs1.String()) }) t.Run("stop", func(t *testing.T) { _, err = armServer.Stop(context.Background(), &pb.StopRequest{Name: missingArmName}) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "not found") + test.That(t, err.Error(), test.ShouldContainSubstring, errArmUnimplemented.Error()) ext, err := protoutils.StructToStructPb(map[string]interface{}{"foo": "Stop"}) test.That(t, err, test.ShouldBeNil) @@ -184,6 +193,6 @@ func TestServer(t *testing.T) { _, err = armServer.Stop(context.Background(), &pb.StopRequest{Name: failArmName}) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err, test.ShouldBeError, arm.ErrStopUnimplemented) + test.That(t, err.Error(), test.ShouldContainSubstring, errStopUnimplemented.Error()) }) } diff --git a/components/arm/universalrobots/ur.go b/components/arm/universalrobots/ur.go index 474a2b87d9b..ad9e87c2782 100644 --- a/components/arm/universalrobots/ur.go +++ b/components/arm/universalrobots/ur.go @@ -80,7 +80,7 @@ type URArm struct { cancel func() activeBackgroundWorkers sync.WaitGroup model referenceframe.Model - opMgr operation.SingleOperationManager + opMgr *operation.SingleOperationManager mu sync.Mutex state RobotState @@ -91,6 +91,7 @@ type URArm struct { dashboardConnection net.Conn readRobotStateConnection net.Conn host string + isConnected bool } const waitBackgroundWorkersDur = 5 * time.Second @@ -193,11 +194,13 @@ func URArmConnect(ctx context.Context, conf resource.Config, logger golog.Logger logger: logger, cancel: cancel, model: model, + opMgr: operation.NewSingleOperationManager(), urHostedKinematics: newConf.ArmHostedKinematics, inRemoteMode: false, readRobotStateConnection: connReadRobotState, dashboardConnection: connDashboard, host: newConf.Host, + isConnected: true, } newArm.activeBackgroundWorkers.Add(1) @@ -214,12 +217,18 @@ func URArmConnect(ctx context.Context, conf resource.Config, logger golog.Logger return } logger.Debug("attempting to reconnect to ur arm dashboard") + time.Sleep(1 * time.Second) connDashboard, err = d.DialContext(cancelCtx, "tcp", newArm.host+":29999") if err == nil { newArm.mu.Lock() newArm.dashboardConnection = connDashboard + newArm.isConnected = true newArm.mu.Unlock() break + } else { + newArm.mu.Lock() + newArm.isConnected = false + newArm.mu.Unlock() } if !goutils.SelectContextOrWait(cancelCtx, 1*time.Second) { return @@ -227,6 +236,9 @@ func URArmConnect(ctx context.Context, conf resource.Config, logger golog.Logger } } else if err != nil { logger.Errorw("dashboard reader failed", "error", err) + newArm.mu.Lock() + newArm.isConnected = false + newArm.mu.Unlock() return } } @@ -489,7 +501,7 @@ func (ua *URArm) GoToInputs(ctx context.Context, goal []referenceframe.Input) er // Geometries returns the list of geometries associated with the resource, in any order. The poses of the geometries reflect their // current location relative to the frame of the resource. -func (ua *URArm) Geometries(ctx context.Context) ([]spatialmath.Geometry, error) { +func (ua *URArm) Geometries(ctx context.Context, extra map[string]interface{}) ([]spatialmath.Geometry, error) { // TODO (pl): RSDK-3316 abstract this to general arm function inputs, err := ua.CurrentInputs(ctx) if err != nil { diff --git a/components/arm/universalrobots/ur5e_test.go b/components/arm/universalrobots/ur5e_test.go index ef9932f36f0..d0d17a4bd27 100644 --- a/components/arm/universalrobots/ur5e_test.go +++ b/components/arm/universalrobots/ur5e_test.go @@ -3,7 +3,6 @@ package universalrobots import ( "bufio" "context" - "errors" "fmt" "math" "net" @@ -18,6 +17,7 @@ import ( "go.viam.com/test" goutils "go.viam.com/utils" "go.viam.com/utils/artifact" + "go.viam.com/utils/testutils" "gonum.org/v1/gonum/mat" "gonum.org/v1/gonum/num/quat" @@ -204,35 +204,23 @@ func computeUR5ePosition(t *testing.T, jointRadians []float64) spatialmath.Pose ) } -func selectChanOrTimeout(c <-chan struct{}, timeout time.Duration) error { - timer := time.NewTimer(timeout) - select { - case <-timer.C: - return errors.New("timeout") - case <-c: - return nil - } -} - func setupListeners(ctx context.Context, statusBlob []byte, remote *atomic.Bool, -) (func(), chan struct{}, chan struct{}, error) { +) (func(), error) { listener29999, err := net.Listen("tcp", "localhost:29999") if err != nil { - return nil, nil, nil, err + return nil, err } listener30001, err := net.Listen("tcp", "localhost:30001") if err != nil { - return nil, nil, nil, err + return nil, err } listener30011, err := net.Listen("tcp", "localhost:30011") if err != nil { - return nil, nil, nil, err + return nil, err } - dashboardChan := make(chan struct{}) - remoteConnChan := make(chan struct{}) goutils.PanicCapturingGo(func() { for { @@ -264,8 +252,6 @@ func setupListeners(ctx context.Context, statusBlob []byte, } timeout := time.NewTimer(100 * time.Millisecond) select { - case dashboardChan <- struct{}{}: - continue case <-ctx.Done(): return case <-timeout.C: @@ -282,7 +268,6 @@ func setupListeners(ctx context.Context, statusBlob []byte, if _, err := listener30001.Accept(); err != nil { break } - remoteConnChan <- struct{}{} } }) goutils.PanicCapturingGo(func() { @@ -314,10 +299,11 @@ func setupListeners(ctx context.Context, statusBlob []byte, listener29999.Close() listener30011.Close() } - return closer, dashboardChan, remoteConnChan, nil + return closer, nil } func TestArmReconnection(t *testing.T) { + t.Skip() var remote atomic.Bool remote.Store(false) @@ -330,7 +316,7 @@ func TestArmReconnection(t *testing.T) { defer cancel() ctx, childCancel := context.WithCancel(parentCtx) - closer, dashboardChan, remoteConnChan, err := setupListeners(ctx, statusBlob, &remote) + closer, err := setupListeners(ctx, statusBlob, &remote) test.That(t, err, test.ShouldBeNil) cfg := resource.Config{ @@ -348,44 +334,59 @@ func TestArmReconnection(t *testing.T) { ua, ok := arm.(*URArm) test.That(t, ok, test.ShouldBeTrue) - test.That(t, selectChanOrTimeout(dashboardChan, time.Second*60), test.ShouldBeNil) - test.That(t, ua.inRemoteMode, test.ShouldBeFalse) + testutils.WaitForAssertion(t, func(tb testing.TB) { + tb.Helper() + ua.mu.Lock() + test.That(tb, ua.isConnected, test.ShouldBeTrue) + test.That(tb, ua.inRemoteMode, test.ShouldBeFalse) + ua.mu.Unlock() + }) remote.Store(true) - test.That(t, selectChanOrTimeout(dashboardChan, time.Second*60), test.ShouldBeNil) - test.That(t, selectChanOrTimeout(dashboardChan, time.Second*60), test.ShouldBeNil) - - test.That(t, ua.inRemoteMode, test.ShouldBeTrue) - test.That(t, selectChanOrTimeout(remoteConnChan, time.Millisecond*900), test.ShouldBeNil) + testutils.WaitForAssertion(t, func(tb testing.TB) { + tb.Helper() + ua.mu.Lock() + test.That(tb, ua.isConnected, test.ShouldBeTrue) + test.That(tb, ua.inRemoteMode, test.ShouldBeTrue) + ua.mu.Unlock() + }) remote.Store(false) - test.That(t, selectChanOrTimeout(dashboardChan, time.Second*60), test.ShouldBeNil) - test.That(t, selectChanOrTimeout(dashboardChan, time.Second*60), test.ShouldBeNil) - - test.That(t, ua.inRemoteMode, test.ShouldBeFalse) - test.That(t, selectChanOrTimeout(remoteConnChan, time.Millisecond*900), test.ShouldNotBeNil) + testutils.WaitForAssertion(t, func(tb testing.TB) { + tb.Helper() + ua.mu.Lock() + test.That(tb, ua.isConnected, test.ShouldBeTrue) + test.That(tb, ua.inRemoteMode, test.ShouldBeFalse) + ua.mu.Unlock() + }) closer() childCancel() test.That(t, goutils.SelectContextOrWait(parentCtx, time.Millisecond*500), test.ShouldBeTrue) - _ = selectChanOrTimeout(dashboardChan, time.Millisecond*200) + testutils.WaitForAssertion(t, func(tb testing.TB) { + tb.Helper() + ua.mu.Lock() + test.That(tb, ua.isConnected, test.ShouldBeFalse) + ua.mu.Unlock() + }) - test.That(t, selectChanOrTimeout(dashboardChan, time.Second*1), test.ShouldNotBeNil) ctx, childCancel = context.WithCancel(parentCtx) - closer, dashboardChan, remoteConnChan, err = setupListeners(ctx, statusBlob, &remote) + closer, err = setupListeners(ctx, statusBlob, &remote) test.That(t, err, test.ShouldBeNil) remote.Store(true) - test.That(t, selectChanOrTimeout(dashboardChan, time.Second*60), test.ShouldBeNil) - test.That(t, selectChanOrTimeout(dashboardChan, time.Second*60), test.ShouldBeNil) - - test.That(t, ua.inRemoteMode, test.ShouldBeTrue) - test.That(t, selectChanOrTimeout(remoteConnChan, time.Millisecond*900), test.ShouldBeNil) + testutils.WaitForAssertion(t, func(tb testing.TB) { + tb.Helper() + ua.mu.Lock() + test.That(tb, ua.isConnected, test.ShouldBeTrue) + test.That(tb, ua.inRemoteMode, test.ShouldBeTrue) + ua.mu.Unlock() + }) closer() childCancel() diff --git a/components/arm/wrapper/wrapper.go b/components/arm/wrapper/wrapper.go index 2248f082ff1..443319254f9 100644 --- a/components/arm/wrapper/wrapper.go +++ b/components/arm/wrapper/wrapper.go @@ -49,7 +49,7 @@ type Arm struct { resource.Named resource.TriviallyCloseable logger golog.Logger - opMgr operation.SingleOperationManager + opMgr *operation.SingleOperationManager mu sync.RWMutex model referenceframe.Model @@ -61,6 +61,7 @@ func NewWrapperArm(ctx context.Context, deps resource.Dependencies, conf resourc a := &Arm{ Named: conf.ResourceName().AsNamed(), logger: logger, + opMgr: operation.NewSingleOperationManager(), } if err := a.Reconfigure(ctx, deps, conf); err != nil { return nil, err @@ -182,7 +183,7 @@ func (wrapper *Arm) GoToInputs(ctx context.Context, goal []referenceframe.Input) // Geometries returns the list of geometries associated with the resource, in any order. The poses of the geometries reflect their // current location relative to the frame of the resource. -func (wrapper *Arm) Geometries(ctx context.Context) ([]spatialmath.Geometry, error) { +func (wrapper *Arm) Geometries(ctx context.Context, extra map[string]interface{}) ([]spatialmath.Geometry, error) { inputs, err := wrapper.CurrentInputs(ctx) if err != nil { return nil, err diff --git a/components/arm/xarm/xarm.go b/components/arm/xarm/xarm.go index 3485e9e3eeb..66e6b584822 100644 --- a/components/arm/xarm/xarm.go +++ b/components/arm/xarm/xarm.go @@ -55,7 +55,7 @@ type xArm struct { moveLock sync.Mutex model referenceframe.Model started bool - opMgr operation.SingleOperationManager + opMgr *operation.SingleOperationManager logger golog.Logger mu sync.RWMutex @@ -123,6 +123,7 @@ func NewxArm(ctx context.Context, conf resource.Config, logger golog.Logger, mod moveHZ: defaultMoveHz, model: model, started: false, + opMgr: operation.NewSingleOperationManager(), logger: logger, } @@ -196,7 +197,7 @@ func (x *xArm) GoToInputs(ctx context.Context, goal []referenceframe.Input) erro return x.MoveToJointPositions(ctx, positionDegs, nil) } -func (x *xArm) Geometries(ctx context.Context) ([]spatialmath.Geometry, error) { +func (x *xArm) Geometries(ctx context.Context, extra map[string]interface{}) ([]spatialmath.Geometry, error) { inputs, err := x.CurrentInputs(ctx) if err != nil { return nil, err diff --git a/components/arm/xarm/xarm_test.go b/components/arm/xarm/xarm_test.go index 3a8df9a9ee7..c12c3dd99fe 100644 --- a/components/arm/xarm/xarm_test.go +++ b/components/arm/xarm/xarm_test.go @@ -73,20 +73,30 @@ func TestWriteViam(t *testing.T) { seedMap[m.Name()] = home7 - steps, err := motionplan.PlanMotion(ctx, logger, frame.NewPoseInFrame(fs.World().Name(), goal), moveFrame, seedMap, fs, nil, nil, nil) + plan, err := motionplan.PlanMotion(ctx, &motionplan.PlanRequest{ + Logger: logger, + Goal: frame.NewPoseInFrame(frame.World, goal), + Frame: moveFrame, + StartConfiguration: seedMap, + FrameSystem: fs, + }) test.That(t, err, test.ShouldBeNil) opt := map[string]interface{}{"motion_profile": motionplan.LinearMotionProfile} - goToGoal := func(seedMap map[string][]frame.Input, goal spatial.Pose) map[string][]frame.Input { - goalPiF := frame.NewPoseInFrame(fs.World().Name(), goal) - - waysteps, err := motionplan.PlanMotion(ctx, logger, goalPiF, moveFrame, seedMap, fs, nil, nil, opt) + plan, err := motionplan.PlanMotion(ctx, &motionplan.PlanRequest{ + Logger: logger, + Goal: frame.NewPoseInFrame(fs.World().Name(), goal), + Frame: moveFrame, + StartConfiguration: seedMap, + FrameSystem: fs, + Options: opt, + }) test.That(t, err, test.ShouldBeNil) - return waysteps[len(waysteps)-1] + return plan[len(plan)-1] } - seed := steps[len(steps)-1] + seed := plan[len(plan)-1] for _, goal = range viamPoints { seed = goToGoal(seed, goal) } diff --git a/components/arm/yahboom/dofbot.go b/components/arm/yahboom/dofbot.go index ce60e509de2..738dd6320a1 100644 --- a/components/arm/yahboom/dofbot.go +++ b/components/arm/yahboom/dofbot.go @@ -103,7 +103,7 @@ type Dofbot struct { mu sync.Mutex muMove sync.Mutex logger golog.Logger - opMgr operation.SingleOperationManager + opMgr *operation.SingleOperationManager stopped bool } @@ -119,6 +119,7 @@ func NewDofBot(ctx context.Context, deps resource.Dependencies, conf resource.Co a := Dofbot{ Named: conf.ResourceName().AsNamed(), logger: logger, + opMgr: operation.NewSingleOperationManager(), } b, err := board.FromDependencies(deps, newConf.Board) @@ -442,7 +443,7 @@ func (a *Dofbot) Close(ctx context.Context) error { // Geometries returns the list of geometries associated with the resource, in any order. The poses of the geometries reflect their // current location relative to the frame of the resource. -func (a *Dofbot) Geometries(ctx context.Context) ([]spatialmath.Geometry, error) { +func (a *Dofbot) Geometries(ctx context.Context, extra map[string]interface{}) ([]spatialmath.Geometry, error) { inputs, err := a.CurrentInputs(ctx) if err != nil { return nil, err diff --git a/components/base/agilex/limo_base.go b/components/base/agilex/limo_base.go index a23c6832ffd..3e36969dc26 100644 --- a/components/base/agilex/limo_base.go +++ b/components/base/agilex/limo_base.go @@ -22,6 +22,7 @@ import ( "go.viam.com/rdk/operation" "go.viam.com/rdk/resource" "go.viam.com/rdk/spatialmath" + rdkutils "go.viam.com/rdk/utils" ) // default port for limo serial comm. @@ -71,7 +72,7 @@ type limoBase struct { resource.Named resource.AlwaysRebuild driveMode string - opMgr operation.SingleOperationManager + opMgr *operation.SingleOperationManager cancel context.CancelFunc waitGroup sync.WaitGroup width int @@ -122,11 +123,12 @@ func createLimoBase(ctx context.Context, _ resource.Dependencies, conf resource. lb := &limoBase{ Named: conf.ResourceName().AsNamed(), driveMode: newConf.DriveMode, + opMgr: operation.NewSingleOperationManager(), testChan: newConf.TestChan, // for testing only logger: logger, width: defaultBaseTreadMm, wheelbase: 200, - maxLinearVelocity: 1200, + maxLinearVelocity: 3000, maxAngularVelocity: 180, maxInnerAngle: .48869, // 28 degrees in radians rightAngleScale: 1.64, @@ -330,12 +332,20 @@ func (lb *limoBase) setMotionCommand(linearVel float64, return nil } +// positive angleDeg spins base left. degsPerSec is a positive angular velocity. func (lb *limoBase) Spin(ctx context.Context, angleDeg, degsPerSec float64, extra map[string]interface{}) error { lb.logger.Debugf("Spin(%f, %f)", angleDeg, degsPerSec) + if degsPerSec <= 0 { + return errors.New("degrees per second must be a positive, non-zero value") + } secsToRun := math.Abs(angleDeg / degsPerSec) var err error if lb.driveMode == DIFFERENTIAL.String() || lb.driveMode == OMNI.String() { - err = lb.SetVelocity(ctx, r3.Vector{}, r3.Vector{Z: degsPerSec}, extra) + dir := 1.0 + if math.Signbit(angleDeg) { + dir = -1.0 + } + err = lb.SetVelocity(ctx, r3.Vector{}, r3.Vector{Z: dir * degsPerSec}, extra) } else if lb.driveMode == ACKERMANN.String() { // TODO: this is not the correct math linear := float64(lb.maxLinearVelocity) * (degsPerSec / 360) * math.Pi @@ -369,6 +379,7 @@ func (lb *limoBase) MoveStraight(ctx context.Context, distanceMm int, mmPerSec f } // linear is in mm/sec, angular in degrees/sec. +// positive angular velocity turns base left. func (lb *limoBase) SetVelocity(ctx context.Context, linear, angular r3.Vector, extra map[string]interface{}) error { lb.logger.Debugf("Will set linear velocity %f angular velocity %f", linear, angular) @@ -376,7 +387,7 @@ func (lb *limoBase) SetVelocity(ctx context.Context, linear, angular r3.Vector, defer done() // this lb expects angular velocity to be expressed in .001 radians/sec, convert - angular.Z = (angular.Z / 57.2958) * 1000 + angular.Z = rdkutils.DegToRad(-angular.Z) * 1000 lb.stateMutex.Lock() lb.state.velocityLinearGoal = linear @@ -390,7 +401,7 @@ func (lb *limoBase) SetPower(ctx context.Context, linear, angular r3.Vector, ext lb.logger.Debugf("Will set power linear %f angular %f", linear, angular) linY := linear.Y * float64(lb.maxLinearVelocity) angZ := angular.Z * float64(lb.maxAngularVelocity) - err := lb.SetVelocity(ctx, r3.Vector{Y: linY}, r3.Vector{Z: -angZ}, extra) + err := lb.SetVelocity(ctx, r3.Vector{Y: linY}, r3.Vector{Z: angZ}, extra) if err != nil { return err } @@ -428,12 +439,13 @@ func (lb *limoBase) Properties(ctx context.Context, extra map[string]interface{} } return base.Properties{ - TurningRadiusMeters: lbTurnRadiusM, - WidthMeters: float64(lb.width) * 0.001, // convert from mm to meters + TurningRadiusMeters: lbTurnRadiusM, + WidthMeters: float64(lb.width) * 0.001, // convert from mm to meters + WheelCircumferenceMeters: 0, // no access to individual motors, so wheel circumference cannot be used for odometry }, nil } -func (lb *limoBase) Geometries(ctx context.Context) ([]spatialmath.Geometry, error) { +func (lb *limoBase) Geometries(ctx context.Context, extra map[string]interface{}) ([]spatialmath.Geometry, error) { return lb.geometries, nil } diff --git a/components/base/client.go b/components/base/client.go index 145f7df065c..a452f70a826 100644 --- a/components/base/client.go +++ b/components/base/client.go @@ -150,8 +150,15 @@ func (c *client) Properties(ctx context.Context, extra map[string]interface{}) ( return ProtoFeaturesToProperties(resp), nil } -func (c *client) Geometries(ctx context.Context) ([]spatialmath.Geometry, error) { - resp, err := c.client.GetGeometries(ctx, &commonpb.GetGeometriesRequest{Name: c.name}) +func (c *client) Geometries(ctx context.Context, extra map[string]interface{}) ([]spatialmath.Geometry, error) { + ext, err := protoutils.StructToStructPb(extra) + if err != nil { + return nil, err + } + resp, err := c.client.GetGeometries(ctx, &commonpb.GetGeometriesRequest{ + Name: c.name, + Extra: ext, + }) if err != nil { return nil, err } diff --git a/components/base/client_test.go b/components/base/client_test.go index b9010c0cbf2..10ba6366cb5 100644 --- a/components/base/client_test.go +++ b/components/base/client_test.go @@ -7,7 +7,6 @@ import ( "github.com/edaniels/golog" "github.com/golang/geo/r3" - "github.com/pkg/errors" "go.viam.com/test" "go.viam.com/utils/rpc" @@ -54,34 +53,27 @@ func setupWorkingBase( } } -const ( - errMsgMoveStraight = "critical failure in MoveStraight" - errMsgSpin = "critical failure in Spin" - errMsgStop = "critical failure in Stop" - errMsgProperties = "critical failure in Properties" -) - func setupBrokenBase(brokenBase *inject.Base) { brokenBase.MoveStraightFunc = func( ctx context.Context, distanceMm int, mmPerSec float64, extra map[string]interface{}, ) error { - return errors.New(errMsgMoveStraight) + return errMoveStraight } brokenBase.SpinFunc = func( ctx context.Context, angleDeg, degsPerSec float64, extra map[string]interface{}, ) error { - return errors.New(errMsgSpin) + return errSpinFailed } brokenBase.StopFunc = func(ctx context.Context, extra map[string]interface{}) error { - return errors.New(errMsgStop) + return errStopFailed } brokenBase.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (base.Properties, error) { - return base.Properties{}, errors.New(errMsgProperties) + return base.Properties{}, errPropertiesFailed } } @@ -128,7 +120,7 @@ func TestClient(t *testing.T) { cancel() _, err = viamgrpc.Dial(cancelCtx, listener1.Addr().String(), logger) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "canceled") + test.That(t, err, test.ShouldBeError, context.Canceled) }) conn, err := viamgrpc.Dial(context.Background(), listener1.Addr().String(), logger) test.That(t, err, test.ShouldBeNil) @@ -188,7 +180,7 @@ func TestClient(t *testing.T) { }) t.Run("working Geometries", func(t *testing.T) { - geometries, err := workingBaseClient.Geometries(context.Background()) + geometries, err := workingBaseClient.Geometries(context.Background(), nil) test.That(t, err, test.ShouldBeNil) for i, geometry := range geometries { test.That(t, geometry.AlmostEqual(expectedGeometries[i]), test.ShouldBeTrue) @@ -221,16 +213,16 @@ func TestClient(t *testing.T) { test.That(t, err, test.ShouldBeNil) err = failingBaseClient.MoveStraight(context.Background(), 42, 42.0, nil) - test.That(t, err.Error(), test.ShouldContainSubstring, errMsgMoveStraight) + test.That(t, err.Error(), test.ShouldContainSubstring, errMoveStraight.Error()) err = failingBaseClient.Spin(context.Background(), 42.0, 42.0, nil) - test.That(t, err.Error(), test.ShouldContainSubstring, errMsgSpin) + test.That(t, err.Error(), test.ShouldContainSubstring, errSpinFailed.Error()) _, err = failingBaseClient.Properties(context.Background(), nil) - test.That(t, err.Error(), test.ShouldContainSubstring, errMsgProperties) + test.That(t, err.Error(), test.ShouldContainSubstring, errPropertiesFailed.Error()) err = failingBaseClient.Stop(context.Background(), nil) - test.That(t, err.Error(), test.ShouldContainSubstring, errMsgStop) + test.That(t, err.Error(), test.ShouldContainSubstring, errStopFailed.Error()) test.That(t, failingBaseClient.Close(context.Background()), test.ShouldBeNil) test.That(t, conn.Close(), test.ShouldBeNil) diff --git a/components/base/fake/fake.go b/components/base/fake/fake.go index 42bbb8868e1..01d3adaa14a 100644 --- a/components/base/fake/fake.go +++ b/components/base/fake/fake.go @@ -23,23 +23,27 @@ func init() { const ( defaultWidthMm = 600 defaultMinimumTurningRadiusM = 0 + defaultWheelCircumferenceM = 3 ) // Base is a fake base that returns what it was provided in each method. type Base struct { resource.Named resource.TriviallyReconfigurable - CloseCount int - WidthMeters float64 - TurningRadius float64 - Geometry []spatialmath.Geometry + CloseCount int + WidthMeters float64 + TurningRadius float64 + WheelCircumferenceMeters float64 + Geometry []spatialmath.Geometry + logger golog.Logger } // NewBase instantiates a new base of the fake model type. -func NewBase(_ context.Context, _ resource.Dependencies, conf resource.Config, _ golog.Logger) (base.Base, error) { +func NewBase(_ context.Context, _ resource.Dependencies, conf resource.Config, logger golog.Logger) (base.Base, error) { b := &Base{ Named: conf.ResourceName().AsNamed(), Geometry: []spatialmath.Geometry{}, + logger: logger, } if conf.Frame != nil && conf.Frame.Geometry != nil { geometry, err := conf.Frame.Geometry.ParseConfig() @@ -92,12 +96,13 @@ func (b *Base) Close(ctx context.Context) error { // Properties returns the base's properties. func (b *Base) Properties(ctx context.Context, extra map[string]interface{}) (base.Properties, error) { return base.Properties{ - TurningRadiusMeters: b.TurningRadius, - WidthMeters: b.WidthMeters, + TurningRadiusMeters: b.TurningRadius, + WidthMeters: b.WidthMeters, + WheelCircumferenceMeters: b.WheelCircumferenceMeters, }, nil } // Geometries returns the geometries associated with the fake base. -func (b *Base) Geometries(ctx context.Context) ([]spatialmath.Geometry, error) { +func (b *Base) Geometries(ctx context.Context, extra map[string]interface{}) ([]spatialmath.Geometry, error) { return b.Geometry, nil } diff --git a/components/base/kinematicbase/differentialDrive.go b/components/base/kinematicbase/differentialDrive.go index 02fb1fa02df..aaaae2c4d3a 100644 --- a/components/base/kinematicbase/differentialDrive.go +++ b/components/base/kinematicbase/differentialDrive.go @@ -5,178 +5,234 @@ package kinematicbase import ( "context" "errors" + "fmt" "math" + "time" + "github.com/edaniels/golog" "github.com/golang/geo/r3" + utils "go.viam.com/utils" "go.viam.com/rdk/components/base" + "go.viam.com/rdk/motionplan/ik" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/services/motion" "go.viam.com/rdk/spatialmath" ) -const ( - distThresholdMM = 100 - headingThresholdDegrees = 15 - defaultAngularVelocity = 60 // degrees per second - defaultLinearVelocity = 300 // mm per second - deviationThreshold = 300.0 // mm -) +// ErrMovementTimeout is used for when a movement call times out after no movement for some time. +var ErrMovementTimeout = errors.New("movement has timed out") // wrapWithDifferentialDriveKinematics takes a wheeledBase component and adds a localizer to it // It also adds kinematic model so that it can be controlled. func wrapWithDifferentialDriveKinematics( ctx context.Context, b base.Base, + logger golog.Logger, localizer motion.Localizer, limits []referenceframe.Limit, - maxLinearVelocityMillisPerSec float64, - maxAngularVelocityDegsPerSec float64, + options Options, ) (KinematicBase, error) { - geometries, err := b.Geometries(ctx) + ddk := &differentialDriveKinematics{ + Base: b, + Localizer: localizer, + logger: logger, + options: options, + } + + geometries, err := b.Geometries(ctx, nil) if err != nil { return nil, err } - + // RSDK-4131 will update this so it is no longer necessary var geometry spatialmath.Geometry + if len(geometries) > 1 { + ddk.logger.Warn("multiple geometries specified for differential drive kinematic base, only can use the first at this time") + } if len(geometries) > 0 { geometry = geometries[0] } - model, err := referenceframe.New2DMobileModelFrame(b.Name().ShortName(), limits, geometry) + ddk.executionFrame, err = referenceframe.New2DMobileModelFrame(b.Name().ShortName(), limits, geometry) if err != nil { return nil, err } - fs := referenceframe.NewEmptyFrameSystem("") - if err := fs.AddFrame(model, fs.World()); err != nil { - return nil, err + if options.PositionOnlyMode { + ddk.planningFrame, err = referenceframe.New2DMobileModelFrame(b.Name().ShortName(), limits[:2], geometry) + if err != nil { + return nil, err + } + } else { + ddk.planningFrame = ddk.executionFrame } - - return &differentialDriveKinematics{ - Base: b, - localizer: localizer, - model: model, - fs: fs, - maxLinearVelocityMillisPerSec: maxLinearVelocityMillisPerSec, - maxAngularVelocityDegsPerSec: maxAngularVelocityDegsPerSec, - }, nil + return ddk, nil } type differentialDriveKinematics struct { base.Base - localizer motion.Localizer - model referenceframe.Model - fs referenceframe.FrameSystem - maxLinearVelocityMillisPerSec float64 - maxAngularVelocityDegsPerSec float64 + motion.Localizer + logger golog.Logger + planningFrame, executionFrame referenceframe.Model + options Options } func (ddk *differentialDriveKinematics) Kinematics() referenceframe.Frame { - return ddk.model + return ddk.planningFrame } func (ddk *differentialDriveKinematics) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) { // TODO(rb): make a transformation from the component reference to the base frame - pif, err := ddk.localizer.CurrentPosition(ctx) + pif, err := ddk.CurrentPosition(ctx) if err != nil { return nil, err } pt := pif.Pose().Point() - theta := math.Mod(pif.Pose().Orientation().OrientationVectorRadians().Theta, 2*math.Pi) - math.Pi + // We should not have a problem with Gimbal lock by looking at yaw in the domain that most bases will be moving. + // This could potentially be made more robust in the future, though. + theta := math.Mod(pif.Pose().Orientation().EulerAngles().Yaw, 2*math.Pi) return []referenceframe.Input{{Value: pt.X}, {Value: pt.Y}, {Value: theta}}, nil } func (ddk *differentialDriveKinematics) GoToInputs(ctx context.Context, desired []referenceframe.Input) (err error) { // create capsule which defines the valid region for a base to be when driving to desired waypoint // deviationThreshold defines max distance base can be from path without error being thrown - current, err := ddk.CurrentInputs(ctx) - if err != nil { - return err + current, inputsErr := ddk.CurrentInputs(ctx) + if inputsErr != nil { + return inputsErr } - validRegion, err := ddk.newValidRegionCapsule(current, desired) - if err != nil { - return err + validRegion, capsuleErr := ddk.newValidRegionCapsule(current, desired) + if capsuleErr != nil { + return capsuleErr } + movementErr := make(chan error, 1) + defer close(movementErr) - // this loop polls the error state and issues a corresponding command to move the base to the objective - // when the base is within the positional threshold of the goal, exit the loop - for err = ctx.Err(); err == nil; err = ctx.Err() { - col, err := validRegion.CollidesWith(spatialmath.NewPoint(r3.Vector{X: current[0].Value, Y: current[1].Value}, "")) - if err != nil { - return err - } - if !col { - return errors.New("base has deviated too far from path") - } + cancelContext, cancel := context.WithCancel(ctx) + defer cancel() - // get to the x, y location first - note that from the base's perspective +y is forward - desiredHeading := math.Atan2(current[1].Value-desired[1].Value, current[0].Value-desired[0].Value) - commanded, err := ddk.issueCommand(ctx, current, []referenceframe.Input{desired[0], desired[1], {desiredHeading}}) - if err != nil { - return err - } + utils.PanicCapturingGo(func() { + // this loop polls the error state and issues a corresponding command to move the base to the objective + // when the base is within the positional threshold of the goal, exit the loop + for err := cancelContext.Err(); err == nil; err = cancelContext.Err() { + utils.SelectContextOrWait(ctx, 10*time.Millisecond) + col, err := validRegion.CollidesWith(spatialmath.NewPoint(r3.Vector{X: current[0].Value, Y: current[1].Value}, "")) + if err != nil { + movementErr <- err + return + } + if !col { + movementErr <- errors.New("base has deviated too far from path") + return + } + + // get to the x, y location first - note that from the base's perspective +y is forward + desiredHeading := math.Atan2(desired[1].Value-current[1].Value, desired[0].Value-current[0].Value) + commanded, err := ddk.issueCommand(cancelContext, current, []referenceframe.Input{desired[0], desired[1], {desiredHeading}}) + if err != nil { + movementErr <- err + return + } - if !commanded { - // no command to move to the x, y location was issued, correct the heading and then exit - if commanded, err := ddk.issueCommand(ctx, current, []referenceframe.Input{current[0], current[1], desired[2]}); err == nil { - if !commanded { - return nil + if !commanded { + // no command to move to the x, y location was issued, correct the heading and then exit + // 2DOF model indicates position-only mode so heading doesn't need to be corrected, exit function + if len(ddk.planningFrame.DoF()) == 2 { + movementErr <- err + return } - } else { - return err + if commanded, err := ddk.issueCommand(cancelContext, current, []referenceframe.Input{current[0], current[1], desired[2]}); err == nil { + if !commanded { + movementErr <- nil + return + } + } else { + movementErr <- err + return + } + } + + current, err = ddk.CurrentInputs(cancelContext) + if err != nil { + movementErr <- err + return } + ddk.logger.Infof("current inputs: %v", current) } + movementErr <- err + }) + + // watching for movement timeout + lastUpdate := time.Now() + var prevInputs []referenceframe.Input - current, err = ddk.CurrentInputs(ctx) + for { + utils.SelectContextOrWait(ctx, 100*time.Millisecond) + select { + case err := <-movementErr: + return err + default: + } + currentInputs, err := ddk.CurrentInputs(ctx) if err != nil { + cancel() + <-movementErr return err } + if prevInputs == nil { + prevInputs = currentInputs + } + positionChange := ik.L2InputMetric(&ik.Segment{ + StartConfiguration: prevInputs, + EndConfiguration: currentInputs, + }) + if positionChange > ddk.options.MinimumMovementThresholdMM { + lastUpdate = time.Now() + prevInputs = currentInputs + } else if time.Since(lastUpdate) > ddk.options.Timeout { + cancel() + <-movementErr + return ErrMovementTimeout + } } - return err } // issueCommand issues a relevant command to move the base to the given desired inputs and returns the boolean describing // if it issued a command successfully. If it is already at the location it will not need to issue another command and can therefore // return a false. func (ddk *differentialDriveKinematics) issueCommand(ctx context.Context, current, desired []referenceframe.Input) (bool, error) { - distErr, headingErr, err := ddk.errorState(current, desired) + distErr, headingErr, err := ddk.inputDiff(current, desired) if err != nil { return false, err } - if distErr > distThresholdMM && math.Abs(headingErr) > headingThresholdDegrees { + ddk.logger.Debug("distErr: %f\theadingErr %f", distErr, headingErr) + if distErr > ddk.options.GoalRadiusMM && math.Abs(headingErr) > ddk.options.HeadingThresholdDegrees { // base is headed off course; spin to correct - return true, ddk.Spin(ctx, -headingErr, ddk.maxAngularVelocityDegsPerSec, nil) - } else if distErr > distThresholdMM { + return true, ddk.Spin(ctx, math.Min(headingErr, ddk.options.MaxSpinAngleDeg), ddk.options.AngularVelocityDegsPerSec, nil) + } else if distErr > ddk.options.GoalRadiusMM { // base is pointed the correct direction but not there yet; forge onward - return true, ddk.MoveStraight(ctx, distErr, ddk.maxLinearVelocityMillisPerSec, nil) + return true, ddk.MoveStraight(ctx, int(math.Min(distErr, ddk.options.MaxMoveStraightMM)), ddk.options.LinearVelocityMMPerSec, nil) } return false, nil } // create a function for the error state, which is defined as [positional error, heading error]. -func (ddk *differentialDriveKinematics) errorState(current, desired []referenceframe.Input) (int, float64, error) { +func (ddk *differentialDriveKinematics) inputDiff(current, desired []referenceframe.Input) (float64, float64, error) { // create a goal pose in the world frame - goal := referenceframe.NewPoseInFrame( - referenceframe.World, - spatialmath.NewPose( - r3.Vector{X: desired[0].Value, Y: desired[1].Value}, - &spatialmath.OrientationVector{OZ: 1, Theta: desired[2].Value}, - ), + goal := spatialmath.NewPose( + r3.Vector{X: desired[0].Value, Y: desired[1].Value}, + &spatialmath.OrientationVector{OZ: 1, Theta: desired[2].Value}, ) // transform the goal pose such that it is in the base frame - tf, err := ddk.fs.Transform(map[string][]referenceframe.Input{ddk.model.Name(): current}, goal, ddk.model.Name()) + currentPose, err := ddk.executionFrame.Transform(current) if err != nil { return 0, 0, err } - delta, ok := tf.(*referenceframe.PoseInFrame) - if !ok { - return 0, 0, errors.New("can't interpret transformable as a pose in frame") - } + delta := spatialmath.PoseBetween(currentPose, goal) // calculate the error state - headingErr := math.Mod(delta.Pose().Orientation().OrientationVectorDegrees().Theta, 360) - positionErr := int(delta.Pose().Point().Norm()) + headingErr := math.Mod(delta.Orientation().OrientationVectorDegrees().Theta, 360) + positionErr := delta.Point().Norm() return positionErr, headingErr, nil } @@ -223,7 +279,7 @@ func CollisionGeometry(cfg *referenceframe.LinkConfig) ([]spatialmath.Geometry, // too far from its path. func (ddk *differentialDriveKinematics) newValidRegionCapsule(starting, desired []referenceframe.Input) (spatialmath.Geometry, error) { pt := r3.Vector{X: (desired[0].Value + starting[0].Value) / 2, Y: (desired[1].Value + starting[1].Value) / 2} - positionErr, _, err := ddk.errorState(starting, desired) + positionErr, _, err := ddk.inputDiff(starting, []referenceframe.Input{desired[0], desired[1], {0}}) if err != nil { return nil, err } @@ -244,8 +300,8 @@ func (ddk *differentialDriveKinematics) newValidRegionCapsule(starting, desired center := spatialmath.NewPose(pt, r) capsule, err := spatialmath.NewCapsule( center, - deviationThreshold, - 2*deviationThreshold+float64(positionErr), + ddk.options.PlanDeviationThresholdMM, + 2*ddk.options.PlanDeviationThresholdMM+positionErr, "") if err != nil { return nil, err @@ -253,3 +309,43 @@ func (ddk *differentialDriveKinematics) newValidRegionCapsule(starting, desired return capsule, nil } + +func (ddk *differentialDriveKinematics) ErrorState( + ctx context.Context, + plan [][]referenceframe.Input, + currentNode int, +) (spatialmath.Pose, error) { + if currentNode <= 0 || currentNode >= len(plan) { + return nil, fmt.Errorf("cannot get ErrorState for node %d, must be > 0 and less than plan length %d", currentNode, len(plan)) + } + + // Get pose-in-frame of the base via its localizer. The offset between the localizer and its base should already be accounted for. + actualPIF, err := ddk.CurrentPosition(ctx) + if err != nil { + return nil, err + } + + var nominalPose spatialmath.Pose + + // Determine the nominal pose, that is, the pose where the robot ought be if it had followed the plan perfectly up until this point. + // This is done differently depending on what sort of frame we are working with. + if len(plan) < 2 { + return nil, errors.New("diff drive motion plan must have at least two waypoints") + } + nominalPose, err = ddk.executionFrame.Transform(plan[currentNode]) + if err != nil { + return nil, err + } + pastPose, err := ddk.executionFrame.Transform(plan[currentNode-1]) + if err != nil { + return nil, err + } + // diff drive bases don't have a notion of "distance along the trajectory between waypoints", so instead we compare to the + // nearest point on the straight line path. + nominalPoint := spatialmath.ClosestPointSegmentPoint(pastPose.Point(), nominalPose.Point(), actualPIF.Pose().Point()) + pointDiff := nominalPose.Point().Sub(pastPose.Point()) + desiredHeading := math.Atan2(pointDiff.Y, pointDiff.X) + nominalPose = spatialmath.NewPose(nominalPoint, &spatialmath.OrientationVector{OZ: 1, Theta: desiredHeading}) + + return spatialmath.PoseBetween(nominalPose, actualPIF.Pose()), nil +} diff --git a/components/base/kinematicbase/differentialDrive_test.go b/components/base/kinematicbase/differentialDrive_test.go index e4bf2da450b..2ce309afde6 100644 --- a/components/base/kinematicbase/differentialDrive_test.go +++ b/components/base/kinematicbase/differentialDrive_test.go @@ -2,6 +2,7 @@ package kinematicbase import ( "context" + "math" "testing" "github.com/edaniels/golog" @@ -20,11 +21,6 @@ import ( "go.viam.com/rdk/utils" ) -const ( - defaultAngularVelocityDegsPerSec = 60 // degrees per second - defaultLinearVelocityMillisPerSec = 300 // mm per second -) - func testConfig() resource.Config { return resource.Config{ Name: "test", @@ -64,17 +60,17 @@ func TestWrapWithDifferentialDriveKinematics(t *testing.T) { t.Run(string(tc.geoType), func(t *testing.T) { testCfg := testConfig() testCfg.Frame.Geometry.Type = tc.geoType - ddk, err := buildTestDDK(ctx, testCfg, defaultLinearVelocityMillisPerSec, defaultAngularVelocityDegsPerSec, logger) + ddk, err := buildTestDDK(ctx, testCfg, defaultLinearVelocityMMPerSec, defaultAngularVelocityDegsPerSec, logger) test.That(t, err == nil, test.ShouldEqual, tc.success) if err != nil { return } - limits := ddk.model.DoF() + limits := ddk.executionFrame.DoF() test.That(t, limits[0].Min, test.ShouldBeLessThan, 0) test.That(t, limits[1].Min, test.ShouldBeLessThan, 0) test.That(t, limits[0].Max, test.ShouldBeGreaterThan, 0) test.That(t, limits[1].Max, test.ShouldBeGreaterThan, 0) - geometry, err := ddk.model.(*referenceframe.SimpleModel).Geometries(make([]referenceframe.Input, len(limits))) + geometry, err := ddk.executionFrame.(*referenceframe.SimpleModel).Geometries(make([]referenceframe.Input, len(limits))) test.That(t, err, test.ShouldBeNil) equivalent := geometry.GeometryByName(testCfg.Name + ":" + testCfg.Frame.Geometry.Label).AlmostEqual(expectedSphere) test.That(t, equivalent, test.ShouldBeTrue) @@ -95,8 +91,8 @@ func TestWrapWithDifferentialDriveKinematics(t *testing.T) { for _, vels := range velocities { ddk, err := buildTestDDK(ctx, testConfig(), vels.linear, vels.angular, logger) test.That(t, err, test.ShouldBeNil) - test.That(t, ddk.maxLinearVelocityMillisPerSec, test.ShouldAlmostEqual, vels.linear) - test.That(t, ddk.maxAngularVelocityDegsPerSec, test.ShouldAlmostEqual, vels.angular) + test.That(t, ddk.options.LinearVelocityMMPerSec, test.ShouldAlmostEqual, vels.linear) + test.That(t, ddk.options.AngularVelocityDegsPerSec, test.ShouldAlmostEqual, vels.angular) } }) } @@ -105,7 +101,7 @@ func TestCurrentInputs(t *testing.T) { ctx := context.Background() logger := golog.NewTestLogger(t) ddk, err := buildTestDDK(ctx, testConfig(), - defaultLinearVelocityMillisPerSec, defaultAngularVelocityDegsPerSec, logger) + defaultLinearVelocityMMPerSec, defaultAngularVelocityDegsPerSec, logger) test.That(t, err, test.ShouldBeNil) for i := 0; i < 10; i++ { _, err := ddk.CurrentInputs(ctx) @@ -113,26 +109,24 @@ func TestCurrentInputs(t *testing.T) { } } -func TestErrorState(t *testing.T) { +func TestInputDiff(t *testing.T) { ctx := context.Background() // make injected slam service slam := inject.NewSLAMService("the slammer") - slam.GetPositionFunc = func(ctx context.Context) (spatialmath.Pose, string, error) { + slam.PositionFunc = func(ctx context.Context) (spatialmath.Pose, string, error) { return spatialmath.NewZeroPose(), "", nil } - localizer, err := motion.NewLocalizer(ctx, slam) - test.That(t, err, test.ShouldBeNil) // build base logger := golog.NewTestLogger(t) ddk, err := buildTestDDK(ctx, testConfig(), - defaultLinearVelocityMillisPerSec, defaultAngularVelocityDegsPerSec, logger) + defaultLinearVelocityMMPerSec, defaultAngularVelocityDegsPerSec, logger) test.That(t, err, test.ShouldBeNil) - ddk.localizer = localizer + ddk.Localizer = motion.NewSLAMLocalizer(slam) desiredInput := []referenceframe.Input{{3}, {4}, {utils.DegToRad(30)}} - distErr, headingErr, err := ddk.errorState(make([]referenceframe.Input, 3), desiredInput) + distErr, headingErr, err := ddk.inputDiff(make([]referenceframe.Input, 3), desiredInput) test.That(t, err, test.ShouldBeNil) test.That(t, distErr, test.ShouldEqual, r3.Vector{desiredInput[0].Value, desiredInput[1].Value, 0}.Norm()) test.That(t, headingErr, test.ShouldAlmostEqual, 30) @@ -157,22 +151,20 @@ func buildTestDDK( // make a SLAM service and get its limits fakeSLAM := fake.NewSLAM(slam.Named("test"), logger) - limits, err := fakeSLAM.GetLimits(ctx) + limits, err := fakeSLAM.Limits(ctx) if err != nil { return nil, err } + limits = append(limits, referenceframe.Limit{-2 * math.Pi, 2 * math.Pi}) - // construct localizer - localizer, err := motion.NewLocalizer(ctx, fakeSLAM) + // construct differential drive kinematic base + options := NewKinematicBaseOptions() + options.LinearVelocityMMPerSec = linVel + options.AngularVelocityDegsPerSec = angVel + kb, err := wrapWithDifferentialDriveKinematics(ctx, b, logger, motion.NewSLAMLocalizer(fakeSLAM), limits, options) if err != nil { return nil, err } - - kb, err := wrapWithDifferentialDriveKinematics(ctx, b, localizer, limits, linVel, angVel) - if err != nil { - return nil, err - } - ddk, ok := kb.(*differentialDriveKinematics) if !ok { return nil, err @@ -183,8 +175,7 @@ func buildTestDDK( func TestNewValidRegionCapsule(t *testing.T) { ctx := context.Background() logger := golog.NewTestLogger(t) - ddk, err := buildTestDDK(ctx, testConfig(), - defaultLinearVelocityMillisPerSec, defaultAngularVelocityDegsPerSec, logger) + ddk, err := buildTestDDK(ctx, testConfig(), defaultLinearVelocityMMPerSec, defaultAngularVelocityDegsPerSec, logger) test.That(t, err, test.ShouldBeNil) starting := referenceframe.FloatsToInputs([]float64{400, 0, 0}) @@ -194,9 +185,9 @@ func TestNewValidRegionCapsule(t *testing.T) { col, err := c.CollidesWith(spatialmath.NewPoint(r3.Vector{-176, 576, 0}, "")) test.That(t, err, test.ShouldBeNil) - test.That(t, col, test.ShouldBeTrue) // TODO: FAILING after change made to transformations method + test.That(t, col, test.ShouldBeTrue) - col, err = c.CollidesWith(spatialmath.NewPoint(r3.Vector{-200, -200, 0}, "")) + col, err = c.CollidesWith(spatialmath.NewPoint(r3.Vector{-defaultPlanDeviationThresholdMM, -defaultPlanDeviationThresholdMM, 0}, "")) test.That(t, err, test.ShouldBeNil) test.That(t, col, test.ShouldBeFalse) } diff --git a/components/base/kinematicbase/fake_kinematics.go b/components/base/kinematicbase/fake_kinematics.go index 3f53c24a644..4516ad4384b 100644 --- a/components/base/kinematicbase/fake_kinematics.go +++ b/components/base/kinematicbase/fake_kinematics.go @@ -2,6 +2,8 @@ package kinematicbase import ( "context" + "sync" + "time" "go.viam.com/rdk/components/base/fake" "go.viam.com/rdk/referenceframe" @@ -11,9 +13,11 @@ import ( type fakeKinematics struct { *fake.Base - model referenceframe.Model - localizer motion.Localizer - inputs []referenceframe.Input + motion.Localizer + planningFrame, executionFrame referenceframe.Frame + inputs []referenceframe.Input + options Options + lock sync.Mutex } // WrapWithFakeKinematics creates a KinematicBase from the fake Base so that it satisfies the ModelFramer and InputEnabled interfaces. @@ -22,33 +26,69 @@ func WrapWithFakeKinematics( b *fake.Base, localizer motion.Localizer, limits []referenceframe.Limit, + options Options, ) (KinematicBase, error) { + position, err := localizer.CurrentPosition(ctx) + if err != nil { + return nil, err + } + pt := position.Pose().Point() + fk := &fakeKinematics{ + Base: b, + Localizer: localizer, + inputs: []referenceframe.Input{{pt.X}, {pt.Y}}, + } var geometry spatialmath.Geometry - if b.Geometry != nil { - geometry = b.Geometry[0] + if len(fk.Base.Geometry) != 0 { + geometry = fk.Base.Geometry[0] } - model, err := referenceframe.New2DMobileModelFrame(b.Name().ShortName(), limits, geometry) + + fk.executionFrame, err = referenceframe.New2DMobileModelFrame(b.Name().ShortName(), limits, geometry) if err != nil { return nil, err } - return &fakeKinematics{ - Base: b, - model: model, - localizer: localizer, - inputs: make([]referenceframe.Input, len(model.DoF())), - }, nil + + if options.PositionOnlyMode { + fk.planningFrame, err = referenceframe.New2DMobileModelFrame(b.Name().ShortName(), limits[:2], geometry) + if err != nil { + return nil, err + } + } else { + fk.planningFrame = fk.executionFrame + } + + fk.options = options + return fk, nil } func (fk *fakeKinematics) Kinematics() referenceframe.Frame { - return fk.model + return fk.planningFrame } func (fk *fakeKinematics) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) { + fk.lock.Lock() + defer fk.lock.Unlock() return fk.inputs, nil } func (fk *fakeKinematics) GoToInputs(ctx context.Context, inputs []referenceframe.Input) error { - _, err := fk.model.Transform(inputs) + _, err := fk.planningFrame.Transform(inputs) + if err != nil { + return err + } + fk.lock.Lock() fk.inputs = inputs - return err + fk.lock.Unlock() + + // Sleep for a short amount to time to simulate a base taking some amount of time to reach the inputs + time.Sleep(150 * time.Millisecond) + return nil +} + +func (fk *fakeKinematics) ErrorState( + ctx context.Context, + plan [][]referenceframe.Input, + currentNode int, +) (spatialmath.Pose, error) { + return spatialmath.NewZeroPose(), nil } diff --git a/components/base/kinematicbase/fake_kinematics_test.go b/components/base/kinematicbase/fake_kinematics_test.go index 313df6e3e08..ff4e3015024 100644 --- a/components/base/kinematicbase/fake_kinematics_test.go +++ b/components/base/kinematicbase/fake_kinematics_test.go @@ -5,15 +5,16 @@ import ( "testing" "github.com/edaniels/golog" + geo "github.com/kellydunn/golang-geo" "go.viam.com/test" fakebase "go.viam.com/rdk/components/base/fake" + "go.viam.com/rdk/components/movementsensor" "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/slam/fake" "go.viam.com/rdk/spatialmath" + "go.viam.com/rdk/testutils/inject" ) func TestNewFakeKinematics(t *testing.T) { @@ -31,16 +32,24 @@ func TestNewFakeKinematics(t *testing.T) { logger := golog.NewTestLogger(t) b, err := fakebase.NewBase(ctx, resource.Dependencies{}, conf, logger) test.That(t, err, test.ShouldBeNil) - fakeSLAM := fake.NewSLAM(slam.Named("test"), logger) - limits, err := fakeSLAM.GetLimits(ctx) - test.That(t, err, test.ShouldBeNil) - - localizer, err := motion.NewLocalizer(ctx, fakeSLAM) - test.That(t, err, test.ShouldBeNil) + ms := inject.NewMovementSensor("test") + ms.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { + return geo.NewPoint(0, 0), 0, nil + } + ms.CompassHeadingFunc = func(ctx context.Context, extra map[string]interface{}) (float64, error) { + return 0, nil + } + ms.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { + return &movementsensor.Properties{CompassHeadingSupported: true}, nil + } + localizer := motion.NewMovementSensorLocalizer(ms, geo.NewPoint(0, 0), spatialmath.NewZeroPose()) + limits := []referenceframe.Limit{{Min: -100, Max: 100}, {Min: -100, Max: 100}} - kb, err := WrapWithFakeKinematics(ctx, b.(*fakebase.Base), localizer, limits) + options := NewKinematicBaseOptions() + options.PositionOnlyMode = false + kb, err := WrapWithFakeKinematics(ctx, b.(*fakebase.Base), localizer, limits, options) test.That(t, err, test.ShouldBeNil) - expected := referenceframe.FloatsToInputs([]float64{10, 11, 0}) + expected := referenceframe.FloatsToInputs([]float64{10, 11}) test.That(t, kb.GoToInputs(ctx, expected), test.ShouldBeNil) inputs, err := kb.CurrentInputs(ctx) test.That(t, err, test.ShouldBeNil) diff --git a/components/base/kinematicbase/kinematics.go b/components/base/kinematicbase/kinematics.go index c060ef6bc18..ee0deb53d7d 100644 --- a/components/base/kinematicbase/kinematics.go +++ b/components/base/kinematicbase/kinematics.go @@ -4,18 +4,118 @@ package kinematicbase import ( "context" + "time" + + "github.com/edaniels/golog" "go.viam.com/rdk/components/base" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/services/motion" + "go.viam.com/rdk/spatialmath" ) // KinematicBase is an interface for Bases that also satisfy the ModelFramer and InputEnabled interfaces. type KinematicBase interface { base.Base + motion.Localizer referenceframe.InputEnabled Kinematics() referenceframe.Frame + // ErrorState takes a complete motionplan, as well as the index of the currently-executing set of inputs, and computes the pose + // difference between where the robot in fact is, and where it ought to be. + ErrorState(context.Context, [][]referenceframe.Input, int) (spatialmath.Pose, error) +} + +const ( + // LinearVelocityMMPerSec is the linear velocity the base will drive at in mm/s. + defaultLinearVelocityMMPerSec = 200 + + // AngularVelocityMMPerSec is the angular velocity the base will turn with in deg/s. + defaultAngularVelocityDegsPerSec = 60 + + // distThresholdMM is used when the base is moving to a goal. It is considered successful if it is within this radius. + defaultGoalRadiusMM = 300 + + // headingThresholdDegrees is used when the base is moving to a goal. + // If its heading is within this angle it is considered on the correct path. + defaultHeadingThresholdDegrees = 8 + + // planDeviationThresholdMM is the amount that the base is allowed to deviate from the straight line path it is intended to travel. + // If it ever exceeds this amount the movement will fail and an error will be returned. + defaultPlanDeviationThresholdMM = 600.0 // mm + + // timeout is the maximum amount of time that the base is allowed to remain stationary during a movement, else an error is thrown. + defaultTimeout = time.Second * 10 + + // minimumMovementThresholdMM is the amount that a base needs to move for it not to be considered stationary. + defaultMinimumMovementThresholdMM = 20 // mm + + // maxMoveStraightMM is the maximum distance the base should move with a single MoveStraight command. + // used to break up large driving segments to prevent error from building up due to slightly incorrect angle. + defaultMaxMoveStraightMM = 1000 + + // maxSpinAngleDeg is the maximum amount of degrees the base should turn with a single Spin command. + // used to break up large turns into smaller chunks to prevent error from building up. + defaultMaxSpinAngleDeg = 45 + + // positionOnlyMode defines whether motion planning should be done in 2DOF or 3DOF. + defaultPositionOnlyMode = true +) + +// Options contains values used for execution of base movement. +type Options struct { + // LinearVelocityMMPerSec is the linear velocity the base will drive at in mm/s + LinearVelocityMMPerSec float64 + + // AngularVelocityMMPerSec is the angular velocity the base will turn with in deg/s + AngularVelocityDegsPerSec float64 + + // GoalRadiusMM is used when the base is moving to a goal. It is considered successful if it is within this radius. + GoalRadiusMM float64 + + // HeadingThresholdDegrees is used when the base is moving to a goal. + // If its heading is within this angle it is considered to be on the correct path. + HeadingThresholdDegrees float64 + + // PlanDeviationThresholdMM is the amount that the base is allowed to deviate from the straight line path it is intended to travel. + // If it ever exceeds this amount the movement will fail and an error will be returned. + PlanDeviationThresholdMM float64 + + // Timeout is the maximum amount of time that the base is allowed to remain stationary during a movement, else an error is thrown. + Timeout time.Duration + + // MinimumMovementThresholdMM is the amount that a base needs to move for it not to be considered stationary. + MinimumMovementThresholdMM float64 + + // MaxMoveStraightMM is the maximum distance the base should move with a single MoveStraight command. + // used to break up large driving segments to prevent error from building up due to slightly incorrect angle. + MaxMoveStraightMM float64 + + // MaxSpinAngleDeg is the maximum amount of degrees the base should turn with a single Spin command. + // used to break up large turns into smaller chunks to prevent error from building up. + MaxSpinAngleDeg float64 + + // PositionOnlyMode defines whether motion planning should be done in 2DOF or 3DOF. + // If value is true, planning is done in [x,y]. If value is false, planning is done in [x,y,theta]. + PositionOnlyMode bool +} + +// NewKinematicBaseOptions creates a struct with values used for execution of base movement. +// all values are pre-set to reasonable default values and can be changed if desired. +func NewKinematicBaseOptions() Options { + options := Options{ + LinearVelocityMMPerSec: defaultLinearVelocityMMPerSec, + AngularVelocityDegsPerSec: defaultAngularVelocityDegsPerSec, + GoalRadiusMM: defaultGoalRadiusMM, + HeadingThresholdDegrees: defaultHeadingThresholdDegrees, + PlanDeviationThresholdMM: defaultPlanDeviationThresholdMM, + Timeout: defaultTimeout, + MinimumMovementThresholdMM: defaultMinimumMovementThresholdMM, + MaxMoveStraightMM: defaultMaxMoveStraightMM, + MaxSpinAngleDeg: defaultMaxSpinAngleDeg, + PositionOnlyMode: defaultPositionOnlyMode, + } + return options } // WrapWithKinematics will wrap a Base with the appropriate type of kinematics, allowing it to provide a Frame which can be planned with @@ -23,11 +123,15 @@ type KinematicBase interface { func WrapWithKinematics( ctx context.Context, b base.Base, + logger golog.Logger, localizer motion.Localizer, limits []referenceframe.Limit, - maxLinearVelocityMillisPerSec float64, - maxAngularVelocityDegsPerSec float64, + options Options, ) (KinematicBase, error) { + if kb, ok := b.(KinematicBase); ok { + return kb, nil + } + properties, err := b.Properties(ctx, nil) if err != nil { return nil, err @@ -35,7 +139,7 @@ func WrapWithKinematics( // TP-space PTG planning does not yet support 0 turning radius if properties.TurningRadiusMeters == 0 { - return wrapWithDifferentialDriveKinematics(ctx, b, localizer, limits, maxLinearVelocityMillisPerSec, maxAngularVelocityDegsPerSec) + return wrapWithDifferentialDriveKinematics(ctx, b, logger, localizer, limits, options) } - return wrapWithPTGKinematics(ctx, b, maxLinearVelocityMillisPerSec, maxAngularVelocityDegsPerSec) + return wrapWithPTGKinematics(ctx, b, logger, localizer, options) } diff --git a/components/base/kinematicbase/ptgKinematics.go b/components/base/kinematicbase/ptgKinematics.go index a70a882ecc0..d9b486a473b 100644 --- a/components/base/kinematicbase/ptgKinematics.go +++ b/components/base/kinematicbase/ptgKinematics.go @@ -5,9 +5,12 @@ package kinematicbase import ( "context" "errors" + "fmt" "math" + "sync" "time" + "github.com/edaniels/golog" "github.com/golang/geo/r3" "go.uber.org/multierr" utils "go.viam.com/utils" @@ -15,12 +18,16 @@ import ( "go.viam.com/rdk/components/base" "go.viam.com/rdk/motionplan/tpspace" "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/services/motion" + "go.viam.com/rdk/spatialmath" rdkutils "go.viam.com/rdk/utils" ) // Define a default speed to target for the base in the case where one is not provided. const defaultBaseMMps = 600. +var zeroInput = make([]referenceframe.Input, 3) + const ( ptgIndex int = iota trajectoryIndexWithinPTG @@ -29,17 +36,22 @@ const ( type ptgBaseKinematics struct { base.Base - frame referenceframe.Frame - fs referenceframe.FrameSystem - ptgs []tpspace.PTG + motion.Localizer + logger golog.Logger + frame referenceframe.Frame + fs referenceframe.FrameSystem + ptgs []tpspace.PTG + inputLock sync.RWMutex + currentInput []referenceframe.Input } // wrapWithPTGKinematics takes a Base component and adds a PTG kinematic model so that it can be controlled. func wrapWithPTGKinematics( ctx context.Context, b base.Base, - maxLinearVelocityMillisPerSec float64, - maxAngularVelocityDegsPerSec float64, + logger golog.Logger, + localizer motion.Localizer, + options Options, ) (KinematicBase, error) { properties, err := b.Properties(ctx, nil) if err != nil { @@ -47,31 +59,37 @@ func wrapWithPTGKinematics( } baseMillimetersPerSecond := defaultBaseMMps - if maxLinearVelocityMillisPerSec > 0 { - baseMillimetersPerSecond = maxLinearVelocityMillisPerSec + if options.LinearVelocityMMPerSec > 0 { + baseMillimetersPerSecond = options.LinearVelocityMMPerSec } baseTurningRadius := properties.TurningRadiusMeters - if maxAngularVelocityDegsPerSec > 0 { + if options.AngularVelocityDegsPerSec > 0 { // Compute smallest allowable turning radius permitted by the given speeds. Use the greater of the two. - calcTurnRadius := (baseMillimetersPerSecond / rdkutils.DegToRad(maxAngularVelocityDegsPerSec)) / 1000. + calcTurnRadius := (baseMillimetersPerSecond / rdkutils.DegToRad(options.AngularVelocityDegsPerSec)) / 1000. baseTurningRadius = math.Max(baseTurningRadius, calcTurnRadius) } + logger.Infof( + "using baseMillimetersPerSecond %f and baseTurningRadius %f for PTG base kinematics", + baseMillimetersPerSecond, + baseTurningRadius, + ) if baseTurningRadius <= 0 { return nil, errors.New("can only wrap with PTG kinematics if turning radius is greater than zero") } - geometries, err := b.Geometries(ctx) + geometries, err := b.Geometries(ctx, nil) if err != nil { return nil, err } - frame, err := referenceframe.NewPTGFrameFromTurningRadius( + frame, err := tpspace.NewPTGFrameFromTurningRadius( b.Name().ShortName(), + logger, baseMillimetersPerSecond, baseTurningRadius, - 0, // pass 0 to use the default + 0, // pass 0 to use the default refDist geometries, ) if err != nil { @@ -90,10 +108,13 @@ func wrapWithPTGKinematics( ptgs := ptgProv.PTGs() return &ptgBaseKinematics{ - Base: b, - frame: frame, - fs: fs, - ptgs: ptgs, + Base: b, + Localizer: localizer, + logger: logger, + frame: frame, + fs: fs, + ptgs: ptgs, + currentInput: zeroInput, }, nil } @@ -103,7 +124,9 @@ func (ptgk *ptgBaseKinematics) Kinematics() referenceframe.Frame { func (ptgk *ptgBaseKinematics) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) { // A PTG frame is always at its own origin, so current inputs are always all zero/not meaningful - return []referenceframe.Input{{Value: 0}, {Value: 0}, {Value: 0}}, nil + ptgk.inputLock.RLock() + defer ptgk.inputLock.RUnlock() + return ptgk.currentInput, nil } func (ptgk *ptgBaseKinematics) GoToInputs(ctx context.Context, inputs []referenceframe.Input) (err error) { @@ -111,19 +134,41 @@ func (ptgk *ptgBaseKinematics) GoToInputs(ctx context.Context, inputs []referenc return errors.New("inputs to ptg kinematic base must be length 3") } + defer func() { + ptgk.inputLock.Lock() + ptgk.currentInput = zeroInput + ptgk.inputLock.Unlock() + }() + + ptgk.logger.Debugf("GoToInputs going to %v", inputs) + selectedPTG := ptgk.ptgs[int(math.Round(inputs[ptgIndex].Value))] - selectedTraj := selectedPTG.Trajectory(uint(math.Round(inputs[trajectoryIndexWithinPTG].Value))) + selectedTraj, err := selectedPTG.Trajectory(inputs[trajectoryIndexWithinPTG].Value, inputs[distanceAlongTrajectoryIndex].Value) + if err != nil { + return multierr.Combine(err, ptgk.Base.Stop(ctx, nil)) + } + lastDist := 0. lastTime := 0. for _, trajNode := range selectedTraj { - if trajNode.Dist > inputs[distanceAlongTrajectoryIndex].Value { - // We have reached the desired distance along the given trajectory - break - } - timestep := time.Duration(trajNode.Time-lastTime) * time.Second + ptgk.inputLock.Lock() // In the case where there's actual contention here, this could cause timing issues; how to solve? + ptgk.currentInput = []referenceframe.Input{inputs[0], inputs[1], {lastDist}} + ptgk.inputLock.Unlock() + lastDist = trajNode.Dist + // TODO: Most trajectories update their velocities infrequently, or sometimes never. + // This function could be improved by looking ahead through the trajectory and minimizing the amount of SetVelocity calls. + timestep := time.Duration((trajNode.Time-lastTime)*1000*1000) * time.Microsecond lastTime = trajNode.Time linVel := r3.Vector{0, trajNode.LinVelMMPS, 0} angVel := r3.Vector{0, 0, rdkutils.RadToDeg(trajNode.AngVelRPS)} + + ptgk.logger.Debugf( + "setting velocity to linear %v angular %v and running velocity step for %s", + linVel, + angVel, + timestep, + ) + err := ptgk.Base.SetVelocity( ctx, linVel, @@ -138,3 +183,43 @@ func (ptgk *ptgBaseKinematics) GoToInputs(ctx context.Context, inputs []referenc return ptgk.Base.Stop(ctx, nil) } + +func (ptgk *ptgBaseKinematics) ErrorState(ctx context.Context, plan [][]referenceframe.Input, currentNode int) (spatialmath.Pose, error) { + if currentNode < 0 || currentNode >= len(plan) { + return nil, fmt.Errorf("cannot get ErrorState for node %d, must be >= 0 and less than plan length %d", currentNode, len(plan)) + } + + // Get pose-in-frame of the base via its localizer. The offset between the localizer and its base should already be accounted for. + actualPIF, err := ptgk.CurrentPosition(ctx) + if err != nil { + return nil, err + } + + var nominalPose spatialmath.Pose + + // Determine the nominal pose, that is, the pose where the robot ought be if it had followed the plan perfectly up until this point. + // This is done differently depending on what sort of frame we are working with. + // TODO: The `rectifyTPspacePath` in motionplan does basically this. Deduplicate. + runningPose := spatialmath.NewZeroPose() + for i := 0; i < currentNode; i++ { + wp := plan[i] + wpPose, err := ptgk.frame.Transform(wp) + if err != nil { + return nil, err + } + runningPose = spatialmath.Compose(runningPose, wpPose) + } + + // Determine how far through the current trajectory we are + currentInputs, err := ptgk.CurrentInputs(ctx) + if err != nil { + return nil, err + } + currPose, err := ptgk.frame.Transform(currentInputs) + if err != nil { + return nil, err + } + nominalPose = spatialmath.Compose(runningPose, currPose) + + return spatialmath.PoseBetween(nominalPose, actualPIF.Pose()), nil +} diff --git a/components/base/kinematicbase/ptgKinematics_test.go b/components/base/kinematicbase/ptgKinematics_test.go index e065b3b690a..0678bdb05ed 100644 --- a/components/base/kinematicbase/ptgKinematics_test.go +++ b/components/base/kinematicbase/ptgKinematics_test.go @@ -32,7 +32,7 @@ func TestPTGKinematics(t *testing.T) { ctx := context.Background() - kb, err := WrapWithKinematics(ctx, b, nil, nil, 0, 0) + kb, err := WrapWithKinematics(ctx, b, logger, nil, nil, NewKinematicBaseOptions()) test.That(t, err, test.ShouldBeNil) test.That(t, kb, test.ShouldNotBeNil) ptgBase, ok := kb.(*ptgBaseKinematics) @@ -42,10 +42,75 @@ func TestPTGKinematics(t *testing.T) { dstPIF := referenceframe.NewPoseInFrame(referenceframe.World, spatialmath.NewPoseFromPoint(r3.Vector{X: 999, Y: 0, Z: 0})) fs := referenceframe.NewEmptyFrameSystem("test") - fs.AddFrame(kb.Kinematics(), fs.World()) + f := kb.Kinematics() + test.That(t, err, test.ShouldBeNil) + fs.AddFrame(f, fs.World()) inputMap := referenceframe.StartPositions(fs) - plan, err := motionplan.PlanMotion(ctx, logger, dstPIF, kb.Kinematics(), inputMap, fs, nil, nil, nil) + plan, err := motionplan.PlanMotion(ctx, &motionplan.PlanRequest{ + Logger: logger, + Goal: dstPIF, + Frame: f, + StartConfiguration: inputMap, + FrameSystem: fs, + }) + test.That(t, err, test.ShouldBeNil) + test.That(t, plan, test.ShouldNotBeNil) +} + +func TestPTGKinematicsWithGeom(t *testing.T) { + logger := golog.NewTestLogger(t) + + name, err := resource.NewFromString("is:a:fakebase") + test.That(t, err, test.ShouldBeNil) + + baseGeom, err := spatialmath.NewBox(spatialmath.NewZeroPose(), r3.Vector{1, 1, 1}, "") + test.That(t, err, test.ShouldBeNil) + + b := &fake.Base{ + Named: name.AsNamed(), + Geometry: []spatialmath.Geometry{baseGeom}, + WidthMeters: 0.2, + TurningRadius: 0.3, + } + + ctx := context.Background() + + kbOpt := NewKinematicBaseOptions() + kbOpt.AngularVelocityDegsPerSec = 0 + kb, err := WrapWithKinematics(ctx, b, logger, nil, nil, kbOpt) + test.That(t, err, test.ShouldBeNil) + test.That(t, kb, test.ShouldNotBeNil) + ptgBase, ok := kb.(*ptgBaseKinematics) + test.That(t, ok, test.ShouldBeTrue) + test.That(t, ptgBase, test.ShouldNotBeNil) + + dstPIF := referenceframe.NewPoseInFrame(referenceframe.World, spatialmath.NewPoseFromPoint(r3.Vector{X: 2000, Y: 0, Z: 0})) + + fs := referenceframe.NewEmptyFrameSystem("test") + f := kb.Kinematics() + test.That(t, err, test.ShouldBeNil) + fs.AddFrame(f, fs.World()) + inputMap := referenceframe.StartPositions(fs) + + obstacle, err := spatialmath.NewBox(spatialmath.NewPoseFromPoint(r3.Vector{1000, 0, 0}), r3.Vector{1, 1, 1}, "") + test.That(t, err, test.ShouldBeNil) + + geoms := []spatialmath.Geometry{obstacle} + worldState, err := referenceframe.NewWorldState( + []*referenceframe.GeometriesInFrame{referenceframe.NewGeometriesInFrame(referenceframe.World, geoms)}, + nil, + ) + test.That(t, err, test.ShouldBeNil) + + plan, err := motionplan.PlanMotion(ctx, &motionplan.PlanRequest{ + Logger: logger, + Goal: dstPIF, + Frame: f, + StartConfiguration: inputMap, + FrameSystem: fs, + WorldState: worldState, + }) test.That(t, err, test.ShouldBeNil) test.That(t, plan, test.ShouldNotBeNil) } diff --git a/components/base/properties.go b/components/base/properties.go index a56a81083ee..da5052f8a6b 100644 --- a/components/base/properties.go +++ b/components/base/properties.go @@ -6,8 +6,9 @@ import pb "go.viam.com/api/component/base/v1" // Properties is a structure representing features // of a base. type Properties struct { - TurningRadiusMeters float64 - WidthMeters float64 + TurningRadiusMeters float64 + WidthMeters float64 + WheelCircumferenceMeters float64 } // ProtoFeaturesToProperties takes a GetPropertiesResponse and returns @@ -21,6 +22,8 @@ func ProtoFeaturesToProperties(resp *pb.GetPropertiesResponse) Properties { TurningRadiusMeters: resp.TurningRadiusMeters, // the width of the base's wheelbase WidthMeters: resp.WidthMeters, + // the circumference of the wheels + WheelCircumferenceMeters: resp.WheelCircumferenceMeters, } } @@ -30,7 +33,8 @@ func PropertiesToProtoResponse( features Properties, ) (*pb.GetPropertiesResponse, error) { return &pb.GetPropertiesResponse{ - TurningRadiusMeters: features.TurningRadiusMeters, - WidthMeters: features.WidthMeters, + TurningRadiusMeters: features.TurningRadiusMeters, + WidthMeters: features.WidthMeters, + WheelCircumferenceMeters: features.WheelCircumferenceMeters, }, nil } diff --git a/components/base/wheeled/sensorbase.go b/components/base/sensorbase/sensorbase.go similarity index 63% rename from components/base/wheeled/sensorbase.go rename to components/base/sensorbase/sensorbase.go index 8539fbf4750..caac9a9d6c6 100644 --- a/components/base/wheeled/sensorbase.go +++ b/components/base/sensorbase/sensorbase.go @@ -1,4 +1,5 @@ -package wheeled +// Package sensorcontrolled base implements a base with feedback control from a movement sensor +package sensorcontrolled import ( "context" @@ -20,31 +21,86 @@ import ( ) const ( - yawPollTime = 5 * time.Millisecond - boundCheckTurn = 2.0 - boundCheckTarget = 5.0 - oneTurn = 360.0 - increment = 0.01 - sensorDebug = false + yawPollTime = 5 * time.Millisecond + velocitiesPollTime = 5 * time.Millisecond + boundCheckTurn = 2.0 + boundCheckTarget = 5.0 + oneTurn = 360.0 + increment = 0.01 + sensorDebug = false ) +var ( + // Model is the name of the sensor_controlled model of a base component. + Model = resource.DefaultModelFamily.WithModel("sensor-controlled") + errNoGoodSensor = errors.New("no appropriate sensor for orientaiton or velocity feedback") +) + +// Config configures a sencor controlled base. +type Config struct { + MovementSensor []string `json:"movement_sensor"` + Base string `json:"base"` +} + +// Validate validates all parts of the sensor controlled base config. +func (cfg *Config) Validate(path string) ([]string, error) { + deps := []string{} + if len(cfg.MovementSensor) == 0 { + return nil, utils.NewConfigValidationError(path, errors.New("need at least one movement sensor for base")) + } + + deps = append(deps, cfg.MovementSensor...) + if cfg.Base == "" { + return nil, utils.NewConfigValidationFieldRequiredError(path, "base") + } + + deps = append(deps, cfg.Base) + return deps, nil +} + type sensorBase struct { resource.Named - // resource.AlwaysRebuild // TODO (rh) implement reconfigure logger golog.Logger mu sync.Mutex activeBackgroundWorkers sync.WaitGroup - wBase base.Base // the inherited wheeled base + controlledBase base.Base // the inherited wheeled base sensorLoopMu sync.Mutex sensorLoopDone func() sensorLoopPolling bool - opMgr operation.SingleOperationManager + opMgr *operation.SingleOperationManager allSensors []movementsensor.MovementSensor orientation movementsensor.MovementSensor + velocities movementsensor.MovementSensor +} + +func init() { + resource.RegisterComponent( + base.API, + Model, + resource.Registration[base.Base, *Config]{Constructor: createSensorBase}) +} + +func createSensorBase( + ctx context.Context, + deps resource.Dependencies, + conf resource.Config, + logger golog.Logger, +) (base.Base, error) { + sb := &sensorBase{ + logger: logger, + Named: conf.ResourceName().AsNamed(), + opMgr: operation.NewSingleOperationManager(), + } + + if err := sb.Reconfigure(ctx, deps, conf); err != nil { + return nil, err + } + + return sb, nil } func (sb *sensorBase) Reconfigure(ctx context.Context, deps resource.Dependencies, conf resource.Config) error { @@ -56,40 +112,49 @@ func (sb *sensorBase) Reconfigure(ctx context.Context, deps resource.Dependencie sb.mu.Lock() defer sb.mu.Unlock() - if len(sb.allSensors) != len(newConf.MovementSensor) { - for _, name := range newConf.MovementSensor { - ms, err := movementsensor.FromDependencies(deps, name) - if err != nil { - return errors.Wrapf(err, "no movement sensor named (%s)", name) - } - sb.allSensors = append(sb.allSensors, ms) - } - } else { - // Compare each element of the slices - for i := range sb.allSensors { - if sb.allSensors[i].Name().String() != newConf.MovementSensor[i] { - for _, name := range newConf.MovementSensor { - ms, err := movementsensor.FromDependencies(deps, name) - if err != nil { - return errors.Wrapf(err, "no movement sensor named (%s)", name) - } - sb.allSensors[i] = ms - } - break - } + // reset all sensors + sb.allSensors = nil + sb.velocities = nil + sb.orientation = nil + sb.controlledBase = nil + + for _, name := range newConf.MovementSensor { + ms, err := movementsensor.FromDependencies(deps, name) + if err != nil { + return errors.Wrapf(err, "no movement sensor named (%s)", name) } + sb.allSensors = append(sb.allSensors, ms) } - var oriMsName string for _, ms := range sb.allSensors { props, err := ms.Properties(context.Background(), nil) - if props.OrientationSupported && err == nil { + if err == nil && props.OrientationSupported { + // return first sensor that does not error that satisfies the properties wanted sb.orientation = ms - oriMsName = ms.Name().ShortName() + sb.logger.Infof("using sensor %s as orientation sensor for base", sb.orientation.Name().ShortName()) + break + } + } + + for _, ms := range sb.allSensors { + props, err := ms.Properties(context.Background(), nil) + if err == nil && props.AngularVelocitySupported && props.LinearVelocitySupported { + // return first sensor that does not error that satisfies the properties wanted + sb.velocities = ms + sb.logger.Infof("using sensor %s as velocity sensor for base", sb.velocities.Name().ShortName()) + break } } - sb.logger.Infof("using sensor %s as orientation sensor for base", oriMsName) + if sb.orientation == nil && sb.velocities == nil { + return errNoGoodSensor + } + + sb.controlledBase, err = base.FromDependencies(deps, newConf.Base) + if err != nil { + return errors.Wrapf(err, "no base named (%s)", newConf.Base) + } + return nil } @@ -114,7 +179,7 @@ func (sb *sensorBase) Spin(ctx context.Context, angleDeg, degsPerSec float64, ex if int(angleDeg) >= 360 { sb.setPolling(false) sb.logger.Warn("feedback for spin calls over 360 not supported yet, spinning without sensor") - return sb.wBase.Spin(ctx, angleDeg, degsPerSec, nil) + return sb.controlledBase.Spin(ctx, angleDeg, degsPerSec, nil) } ctx, done := sb.opMgr.New(ctx) defer done() @@ -137,23 +202,23 @@ func (sb *sensorBase) Spin(ctx context.Context, angleDeg, degsPerSec float64, ex return err } - wb := sb.wBase.(*wheeledBase) - motor := wb.allMotors[0] - if err := sb.opMgr.WaitTillNotPowered(ctx, 500*time.Millisecond, motor, - func(context.Context, map[string]interface{}) error { - return nil - }, - ); err != nil { - return err + // IsMoving returns true when moving, which is not a success condition for our control loop + baseStopped := func(ctx context.Context) (bool, error) { + moving, err := sb.IsMoving(ctx) + return !moving, err } - return nil + return sb.opMgr.WaitForSuccess( + ctx, + yawPollTime, + baseStopped, + ) } func (sb *sensorBase) startRunningMotors(ctx context.Context, angleDeg, degsPerSec float64) error { if math.Signbit(angleDeg) != math.Signbit(degsPerSec) { degsPerSec *= -1 } - return sb.wBase.SetVelocity(ctx, + return sb.controlledBase.SetVelocity(ctx, r3.Vector{X: 0, Y: 0, Z: 0}, r3.Vector{X: 0, Y: 0, Z: degsPerSec}, nil) } @@ -348,15 +413,74 @@ func (sb *sensorBase) MoveStraight( ctx, done := sb.opMgr.New(ctx) defer done() sb.setPolling(false) - return sb.wBase.MoveStraight(ctx, distanceMm, mmPerSec, extra) + return sb.controlledBase.MoveStraight(ctx, distanceMm, mmPerSec, extra) } func (sb *sensorBase) SetVelocity( ctx context.Context, linear, angular r3.Vector, extra map[string]interface{}, ) error { sb.opMgr.CancelRunning(ctx) - sb.setPolling(false) - return sb.wBase.SetVelocity(ctx, linear, angular, extra) + // check if a sensor context has been started + if sb.sensorLoopDone != nil { + sb.sensorLoopDone() + } + + sb.setPolling(true) + // start a sensor context for the sensor loop based on the longstanding base + // creator context, and add a timeout for the context + timeOut := 10 * time.Second + var sensorCtx context.Context + sensorCtx, sb.sensorLoopDone = context.WithTimeout(context.Background(), timeOut) + + if sb.velocities != nil { + sb.logger.Warn("not using sensor for SetVelocityfeedback, this feature will be implemented soon") + // TODO RSDK-3695 implement control loop here instead of placeholder sensor pllling function + sb.pollsensors(sensorCtx, extra) + return errors.New( + "setvelocity with sensor feedback not currently implemented, remove movement sensor reporting linear and angular velocity ") + } + return sb.controlledBase.SetVelocity(ctx, linear, angular, extra) +} + +func (sb *sensorBase) pollsensors(ctx context.Context, extra map[string]interface{}) { + sb.activeBackgroundWorkers.Add(1) + utils.ManagedGo(func() { + ticker := time.NewTicker(velocitiesPollTime) + defer ticker.Stop() + + for { + // check if we want to poll the sensor at all + // other API calls set this to false so that this for loop stops + if !sb.isPolling() { + ticker.Stop() + } + + if err := ctx.Err(); err != nil { + return + } + + select { + case <-ctx.Done(): + return + case <-ticker.C: + linvel, err := sb.velocities.LinearVelocity(ctx, extra) + if err != nil { + sb.logger.Error(err) + return + } + + angvel, err := sb.velocities.AngularVelocity(ctx, extra) + if err != nil { + sb.logger.Error(err) + return + } + + if sensorDebug { + sb.logger.Infof("sensor readings: linear: %#v, angular %#v", linvel, angvel) + } + } + } + }, sb.activeBackgroundWorkers.Done) } func (sb *sensorBase) SetPower( @@ -364,25 +488,25 @@ func (sb *sensorBase) SetPower( ) error { sb.opMgr.CancelRunning(ctx) sb.setPolling(false) - return sb.wBase.SetPower(ctx, linear, angular, extra) + return sb.controlledBase.SetPower(ctx, linear, angular, extra) } func (sb *sensorBase) Stop(ctx context.Context, extra map[string]interface{}) error { sb.opMgr.CancelRunning(ctx) sb.setPolling(false) - return sb.wBase.Stop(ctx, extra) + return sb.controlledBase.Stop(ctx, extra) } func (sb *sensorBase) IsMoving(ctx context.Context) (bool, error) { - return sb.wBase.IsMoving(ctx) + return sb.controlledBase.IsMoving(ctx) } func (sb *sensorBase) Properties(ctx context.Context, extra map[string]interface{}) (base.Properties, error) { - return sb.wBase.Properties(ctx, extra) + return sb.controlledBase.Properties(ctx, extra) } -func (sb *sensorBase) Geometries(ctx context.Context) ([]spatialmath.Geometry, error) { - return sb.wBase.Geometries(ctx) +func (sb *sensorBase) Geometries(ctx context.Context, extra map[string]interface{}) ([]spatialmath.Geometry, error) { + return sb.controlledBase.Geometries(ctx, extra) } func (sb *sensorBase) Close(ctx context.Context) error { diff --git a/components/base/wheeled/sensorbase_test.go b/components/base/sensorbase/sensorbase_test.go similarity index 54% rename from components/base/wheeled/sensorbase_test.go rename to components/base/sensorbase/sensorbase_test.go index e268188636f..95c962a168e 100644 --- a/components/base/wheeled/sensorbase_test.go +++ b/components/base/sensorbase/sensorbase_test.go @@ -1,9 +1,11 @@ -package wheeled +package sensorcontrolled import ( "context" + "errors" "math" "strconv" + "strings" "sync" "testing" @@ -15,8 +17,9 @@ import ( "go.viam.com/rdk/components/movementsensor" "go.viam.com/rdk/resource" "go.viam.com/rdk/spatialmath" + "go.viam.com/rdk/testutils" "go.viam.com/rdk/testutils/inject" - rdkutils "go.viam.com/rdk/utils" + "go.viam.com/rdk/utils" ) func TestSpinWithMSMath(t *testing.T) { @@ -96,7 +99,7 @@ func TestSpinWithMSMath(t *testing.T) { ori, err := ms.Orientation(ctx, nil) test.That(t, err, test.ShouldBeNil) - calcYaw := addAnglesInDomain(rdkutils.RadToDeg(ori.EulerAngles().Yaw), 0) + calcYaw := addAnglesInDomain(utils.RadToDeg(ori.EulerAngles().Yaw), 0) measYaw, err := getCurrentYaw(ms) test.That(t, measYaw, test.ShouldEqual, calcYaw) test.That(t, measYaw > 0, test.ShouldBeTrue) @@ -267,7 +270,7 @@ func TestSpinWithMovementSensor(t *testing.T) { sensorCtx, sensorCancel := context.WithCancel(ctx) sensorBase := &sensorBase{ logger: logger, - wBase: wb, + controlledBase: wb, sensorLoopMu: sync.Mutex{}, sensorLoopDone: sensorCancel, allSensors: []movementsensor.MovementSensor{ms}, @@ -288,15 +291,71 @@ func sConfig() resource.Config { API: base.API, Model: resource.Model{Name: "wheeled_base"}, ConvertedAttributes: &Config{ - WidthMM: 100, - WheelCircumferenceMM: 1000, - Left: []string{"fl-m", "bl-m"}, - Right: []string{"fr-m", "br-m"}, - MovementSensor: []string{"ms"}, + MovementSensor: []string{"ms"}, + Base: "test_base", }, } } +func createDependencies(t *testing.T) resource.Dependencies { + t.Helper() + deps := make(resource.Dependencies) + + counter := 0 + + deps[movementsensor.Named("ms")] = &inject.MovementSensor{ + PropertiesFuncExtraCap: map[string]interface{}{}, + PropertiesFunc: func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { + return &movementsensor.Properties{OrientationSupported: true}, nil + }, + OrientationFunc: func(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) { + counter++ + return &spatialmath.EulerAngles{Roll: 0, Pitch: 0, Yaw: utils.RadToDeg(float64(counter))}, nil + }, + } + + deps = addBaseDependency(deps) + + return deps +} + +func addBaseDependency(deps resource.Dependencies) resource.Dependencies { + deps[base.Named(("test_base"))] = &inject.Base{ + DoFunc: testutils.EchoFunc, + MoveStraightFunc: func(ctx context.Context, distanceMm int, mmPerSec float64, extra map[string]interface{}) error { + return nil + }, + SpinFunc: func(ctx context.Context, angleDeg, degsPerSec float64, extra map[string]interface{}) error { + return nil + }, + StopFunc: func(ctx context.Context, extra map[string]interface{}) error { + return nil + }, + IsMovingFunc: func(context.Context) (bool, error) { + return false, nil + }, + CloseFunc: func(ctx context.Context) error { + return nil + }, + SetPowerFunc: func(ctx context.Context, linear, angular r3.Vector, extra map[string]interface{}) error { + return nil + }, + SetVelocityFunc: func(ctx context.Context, linear, angular r3.Vector, extra map[string]interface{}) error { + return nil + }, + PropertiesFunc: func(ctx context.Context, extra map[string]interface{}) (base.Properties, error) { + return base.Properties{ + TurningRadiusMeters: 0.1, + WidthMeters: 0.1, + }, nil + }, + GeometriesFunc: func(ctx context.Context) ([]spatialmath.Geometry, error) { + return nil, nil + }, + } + return deps +} + func TestSensorBase(t *testing.T) { ctx := context.Background() logger := golog.NewTestLogger(t) @@ -305,15 +364,140 @@ func TestSensorBase(t *testing.T) { test.That(t, ok, test.ShouldBeTrue) deps, err := conf.Validate("path") test.That(t, err, test.ShouldBeNil) - msDeps := fakeMotorDependencies(t, deps) - msDeps[movementsensor.Named("ms")] = &inject.MovementSensor{ - PropertiesFuncExtraCap: map[string]interface{}{}, - PropertiesFunc: func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { - return &movementsensor.Properties{OrientationSupported: true}, nil + test.That(t, deps, test.ShouldResemble, []string{"ms", "test_base"}) + sbDeps := createDependencies(t) + + sb, err := createSensorBase(ctx, sbDeps, testCfg, logger) + test.That(t, err, test.ShouldBeNil) + test.That(t, sb, test.ShouldNotBeNil) + + moving, err := sb.IsMoving(ctx) + test.That(t, err, test.ShouldBeNil) + test.That(t, moving, test.ShouldBeFalse) + + props, err := sb.Properties(ctx, nil) + test.That(t, props.WidthMeters, test.ShouldResemble, 0.1) + test.That(t, err, test.ShouldBeNil) + + geometries, err := sb.Geometries(ctx, nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, geometries, test.ShouldBeNil) + + test.That(t, sb.SetPower(ctx, r3.Vector{X: 0, Y: 10, Z: 0}, r3.Vector{X: 0, Y: 0, Z: 0}, nil), test.ShouldBeNil) + test.That(t, sb.SetVelocity(ctx, r3.Vector{X: 0, Y: 100, Z: 0}, r3.Vector{X: 0, Y: 100, Z: 0}, nil), test.ShouldBeNil) + test.That(t, sb.MoveStraight(ctx, 10, 10, nil), test.ShouldBeNil) + test.That(t, sb.Spin(ctx, 2, 10, nil), test.ShouldBeNil) + test.That(t, sb.Stop(ctx, nil), test.ShouldBeNil) + + test.That(t, sb.Close(ctx), test.ShouldBeNil) +} + +func sBaseTestConfig(msNames []string) resource.Config { + return resource.Config{ + Name: "test", + API: base.API, + Model: resource.Model{Name: "controlled_base"}, + ConvertedAttributes: &Config{ + MovementSensor: msNames, + Base: "test_base", }, } +} + +func msDependencies(t *testing.T, msNames []string, +) (resource.Dependencies, resource.Config) { + t.Helper() + + cfg := sBaseTestConfig(msNames) + + deps := make(resource.Dependencies) + + for _, msName := range msNames { + ms := inject.NewMovementSensor(msName) + switch { + case strings.Contains(msName, "orientation"): + ms.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { + return &movementsensor.Properties{ + OrientationSupported: true, + }, nil + } + deps[movementsensor.Named(msName)] = ms + + case strings.Contains(msName, "setvel"): + ms.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { + return &movementsensor.Properties{ + AngularVelocitySupported: true, + LinearVelocitySupported: true, + }, nil + } + deps[movementsensor.Named(msName)] = ms + + case strings.Contains(msName, "Bad"): + ms.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { + return &movementsensor.Properties{ + OrientationSupported: true, + AngularVelocitySupported: true, + LinearVelocitySupported: true, + }, errors.New("bad sensor") + } + deps[movementsensor.Named(msName)] = ms + + default: + } + } - wheeled, err := createWheeledBase(ctx, msDeps, testCfg, logger) + deps = addBaseDependency(deps) + + return deps, cfg +} + +func TestReconfig(t *testing.T) { + ctx := context.Background() + logger := golog.NewTestLogger(t) + + deps, cfg := msDependencies(t, []string{"orientation"}) + + b, err := createSensorBase(ctx, deps, cfg, logger) + test.That(t, err, test.ShouldBeNil) + sb, ok := b.(*sensorBase) + test.That(t, ok, test.ShouldBeTrue) + test.That(t, sb.orientation.Name().ShortName(), test.ShouldResemble, "orientation") + + deps, cfg = msDependencies(t, []string{"orientation1"}) + err = b.Reconfigure(ctx, deps, cfg) + test.That(t, err, test.ShouldBeNil) + test.That(t, sb.orientation.Name().ShortName(), test.ShouldResemble, "orientation1") + + deps, cfg = msDependencies(t, []string{"orientation2"}) + err = b.Reconfigure(ctx, deps, cfg) test.That(t, err, test.ShouldBeNil) - test.That(t, wheeled, test.ShouldNotBeNil) + test.That(t, sb.orientation.Name().ShortName(), test.ShouldResemble, "orientation2") + + deps, cfg = msDependencies(t, []string{"setvel1"}) + err = b.Reconfigure(ctx, deps, cfg) + test.That(t, err, test.ShouldBeNil) + test.That(t, sb.velocities.Name().ShortName(), test.ShouldResemble, "setvel1") + + deps, cfg = msDependencies(t, []string{"setvel2"}) + err = b.Reconfigure(ctx, deps, cfg) + test.That(t, err, test.ShouldBeNil) + test.That(t, sb.velocities.Name().ShortName(), test.ShouldResemble, "setvel2") + + deps, cfg = msDependencies(t, []string{"orientation3", "setvel3", "Bad"}) + err = b.Reconfigure(ctx, deps, cfg) + test.That(t, err, test.ShouldBeNil) + test.That(t, sb.orientation.Name().ShortName(), test.ShouldResemble, "orientation3") + test.That(t, sb.velocities.Name().ShortName(), test.ShouldResemble, "setvel3") + + deps, cfg = msDependencies(t, []string{"Bad", "orientation4", "setvel4", "orientation5", "setvel5"}) + err = b.Reconfigure(ctx, deps, cfg) + test.That(t, err, test.ShouldBeNil) + test.That(t, sb.orientation.Name().ShortName(), test.ShouldResemble, "orientation4") + test.That(t, sb.velocities.Name().ShortName(), test.ShouldResemble, "setvel4") + + deps, cfg = msDependencies(t, []string{"Bad"}) + err = b.Reconfigure(ctx, deps, cfg) + test.That(t, sb.orientation, test.ShouldBeNil) + test.That(t, sb.velocities, test.ShouldBeNil) + test.That(t, err, test.ShouldBeError, errNoGoodSensor) } diff --git a/components/base/server.go b/components/base/server.go index 35a02f481ce..132d5e5d1d2 100644 --- a/components/base/server.go +++ b/components/base/server.go @@ -158,7 +158,7 @@ func (s *serviceServer) GetGeometries(ctx context.Context, req *commonpb.GetGeom if err != nil { return nil, err } - geometries, err := res.Geometries(ctx) + geometries, err := res.Geometries(ctx, req.Extra.AsMap()) if err != nil { return nil, err } diff --git a/components/base/server_test.go b/components/base/server_test.go index 69928a53ec4..8abef1f1f40 100644 --- a/components/base/server_test.go +++ b/components/base/server_test.go @@ -13,6 +13,13 @@ import ( "go.viam.com/rdk/testutils/inject" ) +var ( + errMoveStraight = errors.New("critical failure in MoveStraight") + errSpinFailed = errors.New("critical failure in Spin") + errPropertiesFailed = errors.New("critical failure in Properties") + errStopFailed = errors.New("critical failure in Stop") +) + func newServer() (pb.BaseServiceServer, *inject.Base, *inject.Base, error) { workingBase := &inject.Base{} brokenBase := &inject.Base{} @@ -53,13 +60,12 @@ func TestServer(t *testing.T) { test.That(t, resp, test.ShouldResemble, &pb.MoveStraightResponse{}) // on failing move straight - errMsg := "move straight failed" brokenBase.MoveStraightFunc = func( ctx context.Context, distanceMm int, mmPerSec float64, extra map[string]interface{}, ) error { - return errors.New(errMsg) + return errMoveStraight } req = &pb.MoveStraightRequest{ Name: failBaseName, @@ -68,7 +74,7 @@ func TestServer(t *testing.T) { } resp, err = server.MoveStraight(context.Background(), req) test.That(t, resp, test.ShouldBeNil) - test.That(t, err, test.ShouldBeError, errors.New(errMsg)) + test.That(t, err, test.ShouldBeError, errMoveStraight) // failure on unfound base req = &pb.MoveStraightRequest{ @@ -102,13 +108,12 @@ func TestServer(t *testing.T) { test.That(t, resp, test.ShouldResemble, &pb.SpinResponse{}) // on failing spin - errMsg := "spin failed" brokenBase.SpinFunc = func( ctx context.Context, angleDeg, degsPerSec float64, extra map[string]interface{}, ) error { - return errors.New(errMsg) + return errSpinFailed } req = &pb.SpinRequest{ Name: failBaseName, @@ -117,7 +122,7 @@ func TestServer(t *testing.T) { } resp, err = server.Spin(context.Background(), req) test.That(t, resp, test.ShouldBeNil) - test.That(t, err, test.ShouldBeError, errors.New(errMsg)) + test.That(t, err, test.ShouldBeError, errSpinFailed) // failure on unfound base req = &pb.SpinRequest{ @@ -146,15 +151,13 @@ func TestServer(t *testing.T) { test.That(t, err, test.ShouldBeNil) // on a failing get properties - errMsg := "properties not found" - brokenBase.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (base.Properties, error) { - return base.Properties{}, errors.New(errMsg) + return base.Properties{}, errPropertiesFailed } req = &pb.GetPropertiesRequest{Name: failBaseName} resp, err = server.GetProperties(context.Background(), req) test.That(t, resp, test.ShouldBeNil) - test.That(t, err, test.ShouldBeError, errors.New(errMsg)) + test.That(t, err, test.ShouldBeError, errPropertiesFailed) // failure on base not found req = &pb.GetPropertiesRequest{Name: "dne"} @@ -174,14 +177,13 @@ func TestServer(t *testing.T) { test.That(t, resp, test.ShouldResemble, &pb.StopResponse{}) // on failing stop - errMsg := "stop failed" brokenBase.StopFunc = func(ctx context.Context, extra map[string]interface{}) error { - return errors.New(errMsg) + return errStopFailed } req = &pb.StopRequest{Name: failBaseName} resp, err = server.Stop(context.Background(), req) test.That(t, resp, test.ShouldBeNil) - test.That(t, err, test.ShouldBeError, errors.New(errMsg)) + test.That(t, err, test.ShouldBeError, errStopFailed) // failure on base not found req = &pb.StopRequest{Name: "dne"} diff --git a/components/base/wheeled/wheeled_base.go b/components/base/wheeled/wheeled_base.go index 3519b08af3e..9603dcda806 100644 --- a/components/base/wheeled/wheeled_base.go +++ b/components/base/wheeled/wheeled_base.go @@ -62,7 +62,6 @@ type Config struct { SpinSlipFactor float64 `json:"spin_slip_factor,omitempty"` Left []string `json:"left"` Right []string `json:"right"` - MovementSensor []string `json:"movement_sensor,omitempty"` } // Validate ensures all parts of the config are valid. @@ -93,10 +92,6 @@ func (cfg *Config) Validate(path string) ([]string, error) { deps = append(deps, cfg.Left...) deps = append(deps, cfg.Right...) - if len(cfg.MovementSensor) != 0 { - deps = append(deps, cfg.MovementSensor...) - } - return deps, nil } @@ -115,7 +110,7 @@ type wheeledBase struct { right []motor.Motor allMotors []motor.Motor - opMgr operation.SingleOperationManager + opMgr *operation.SingleOperationManager logger golog.Logger mu sync.Mutex @@ -139,69 +134,58 @@ func (wb *wheeledBase) Reconfigure(ctx context.Context, deps resource.Dependenci } if newConf.SpinSlipFactor == 0 { - newConf.SpinSlipFactor = 1 + wb.spinSlipFactor = 1 + } else { + wb.spinSlipFactor = newConf.SpinSlipFactor } - wb.spinSlipFactor = newConf.SpinSlipFactor - - // Check if wb.left is different from newConf.Left before changing wb.left - if len(wb.left) != len(newConf.Left) { - // Resetting the left motor list - wb.left = make([]motor.Motor, 0) - - for _, name := range newConf.Left { - m, err := motor.FromDependencies(deps, name) - if err != nil { - return errors.Wrapf(err, "no left motor named (%s)", name) + updateMotors := func(curr []motor.Motor, fromConfig []string, whichMotor string) ([]motor.Motor, error) { + newMotors := make([]motor.Motor, 0) + if len(curr) != len(fromConfig) { + for _, name := range fromConfig { + select { + case <-ctx.Done(): + return newMotors, resource.NewBuildTimeoutError(wb.Name()) + default: + } + m, err := motor.FromDependencies(deps, name) + if err != nil { + return newMotors, errors.Wrapf(err, "no %s motor named (%s)", whichMotor, name) + } + newMotors = append(newMotors, m) } - wb.left = append(wb.left, m) - } - } else { - // Compare each element of the slices - for i := range wb.left { - if wb.left[i].Name().String() != newConf.Left[i] { - // Resetting the left motor list - wb.left = make([]motor.Motor, 0) - - for _, name := range newConf.Left { - m, err := motor.FromDependencies(deps, name) - if err != nil { - return errors.Wrapf(err, "no left motor named (%s)", name) + } else { + // Compare each element of the slices + for i := range curr { + select { + case <-ctx.Done(): + return newMotors, resource.NewBuildTimeoutError(wb.Name()) + default: + } + if (curr)[i].Name().String() != (fromConfig)[i] { + for _, name := range fromConfig { + m, err := motor.FromDependencies(deps, name) + if err != nil { + return newMotors, errors.Wrapf(err, "no %s motor named (%s)", whichMotor, name) + } + newMotors = append(newMotors, m) } - wb.left = append(wb.left, m) + break } - break } } + return newMotors, nil } - if len(wb.right) != len(newConf.Right) { - // Resetting the left motor list - wb.right = make([]motor.Motor, 0) - - for _, name := range newConf.Right { - m, err := motor.FromDependencies(deps, name) - if err != nil { - return errors.Wrapf(err, "no right motor named (%s)", name) - } - wb.right = append(wb.right, m) - } - } else { - // Compare each element of the slices - for i := range wb.right { - if wb.right[i].Name().String() != newConf.Right[i] { - wb.right = make([]motor.Motor, 0) - - for _, name := range newConf.Right { - m, err := motor.FromDependencies(deps, name) - if err != nil { - return errors.Wrapf(err, "no right motor named (%s)", name) - } - wb.right = append(wb.right, m) - } - break - } - } + left, err := updateMotors(wb.left, newConf.Left, "left") + wb.left = left + if err != nil { + return err + } + right, err := updateMotors(wb.right, newConf.Right, "right") + wb.right = right + if err != nil { + return err } wb.allMotors = append(wb.allMotors, wb.left...) @@ -235,6 +219,7 @@ func createWheeledBase( widthMm: newConf.WidthMM, wheelCircumferenceMm: newConf.WheelCircumferenceMM, spinSlipFactor: newConf.SpinSlipFactor, + opMgr: operation.NewSingleOperationManager(), logger: logger, name: conf.Name, } @@ -243,14 +228,6 @@ func createWheeledBase( return nil, err } - if len(newConf.MovementSensor) != 0 { - sb := sensorBase{wBase: &wb, logger: logger, Named: conf.ResourceName().AsNamed()} - if err := sb.Reconfigure(ctx, deps, conf); err != nil { - return nil, err - } - return &sb, nil - } - return &wb, nil } @@ -296,20 +273,30 @@ func (wb *wheeledBase) MoveStraight(ctx context.Context, distanceMm int, mmPerSe return wb.runAll(ctx, rpm, rotations, rpm, rotations) } -// runAll executes motor commands in parallel for left and right motors, +// runAll executes `motor.GoFor` commands in parallel for left and right motors, // with specified speeds and rotations and stops the base if an error occurs. +// All callers must register an operation via `wb.opMgr.New` to ensure the left and right motors +// receive consistent instructions. func (wb *wheeledBase) runAll(ctx context.Context, leftRPM, leftRotations, rightRPM, rightRotations float64) error { - fs := []rdkutils.SimpleFunc{} - - for _, m := range wb.left { - fs = append(fs, func(ctx context.Context) error { return m.GoFor(ctx, leftRPM, leftRotations, nil) }) - } + goForFuncs := func() []rdkutils.SimpleFunc { + ret := []rdkutils.SimpleFunc{} + + // These reads of `wb.left` and `wb.right` can race with `Reconfigure`. + wb.mu.Lock() + defer wb.mu.Unlock() + for _, m := range wb.left { + motor := m + ret = append(ret, func(ctx context.Context) error { return motor.GoFor(ctx, leftRPM, leftRotations, nil) }) + } - for _, m := range wb.right { - fs = append(fs, func(ctx context.Context) error { return m.GoFor(ctx, rightRPM, rightRotations, nil) }) - } + for _, m := range wb.right { + motor := m + ret = append(ret, func(ctx context.Context) error { return motor.GoFor(ctx, rightRPM, rightRotations, nil) }) + } + return ret + }() - if _, err := rdkutils.RunInParallel(ctx, fs); err != nil { + if _, err := rdkutils.RunInParallel(ctx, goForFuncs); err != nil { return multierr.Combine(err, wb.Stop(ctx, nil)) } return nil @@ -356,16 +343,38 @@ func (wb *wheeledBase) differentialDrive(forward, left float64) (float64, float6 // SetVelocity commands the base to move at the input linear and angular velocities. func (wb *wheeledBase) SetVelocity(ctx context.Context, linear, angular r3.Vector, extra map[string]interface{}) error { - wb.opMgr.CancelRunning(ctx) - wb.logger.Debugf( "received a SetVelocity with linear.X: %.2f, linear.Y: %.2f linear.Z: %.2f(mmPerSec),"+ " angular.X: %.2f, angular.Y: %.2f, angular.Z: %.2f", linear.X, linear.Y, linear.Z, angular.X, angular.Y, angular.Z) - l, r := wb.velocityMath(linear.Y, angular.Z) + leftRPM, rightRPM := wb.velocityMath(linear.Y, angular.Z) + // Passing zero revolutions to `motor.GoFor` will have the motor run until + // interrupted. Moreover, `motor.GoFor` will return immediately when given zero revolutions. + const numRevolutions = 0 + errs := make([]error, 0) + + wb.mu.Lock() + // Because `SetVelocity` does not create a new operation, canceling must be done atomically with + // engaging the underlying motors. Otherwise, for example: + // + // 1) A new `Spin` command can register an operation + // 2) but the motor instructions get overwritten by `SetVelocity` + // + // Resulting in the spin operation being "leaked" and/or the encoders trying to measure when to + // finish "spinning" have undefined behavior due to the motors actually running at a + // speed/direction that was not intended. + wb.opMgr.CancelRunning(ctx) + defer wb.mu.Unlock() + for _, m := range wb.left { + errs = append(errs, m.GoFor(ctx, leftRPM, numRevolutions, nil)) + } + + for _, m := range wb.right { + errs = append(errs, m.GoFor(ctx, rightRPM, numRevolutions, nil)) + } - return wb.runAll(ctx, l, 0, r, 0) + return multierr.Combine(errs...) } // SetPower commands the base motors to run at powers corresponding to input linear and angular powers. @@ -380,15 +389,23 @@ func (wb *wheeledBase) SetPower(ctx context.Context, linear, angular r3.Vector, lPower, rPower := wb.differentialDrive(linear.Y, angular.Z) // Send motor commands - var err error - for _, m := range wb.left { - err = multierr.Combine(err, m.SetPower(ctx, lPower, extra)) - } + err := func() error { + var err error + + // `wheeledBase.SetPower` does not create a new operation via the `opMgr`. Set the + // underlying motor powers atomically. + wb.mu.Lock() + defer wb.mu.Unlock() + for _, m := range wb.left { + err = multierr.Combine(err, m.SetPower(ctx, lPower, extra)) + } - for _, m := range wb.right { - err = multierr.Combine(err, m.SetPower(ctx, rPower, extra)) - } + for _, m := range wb.right { + err = multierr.Combine(err, m.SetPower(ctx, rPower, extra)) + } + return err + }() if err != nil { return multierr.Combine(err, wb.Stop(ctx, nil)) } @@ -467,11 +484,12 @@ func (wb *wheeledBase) Close(ctx context.Context) error { func (wb *wheeledBase) Properties(ctx context.Context, extra map[string]interface{}) (base.Properties, error) { return base.Properties{ - TurningRadiusMeters: 0.0, - WidthMeters: float64(wb.widthMm) * 0.001, // convert to meters from mm + TurningRadiusMeters: 0.0, + WidthMeters: float64(wb.widthMm) * 0.001, // convert to meters from mm + WheelCircumferenceMeters: float64(wb.wheelCircumferenceMm) * 0.001, // convert to meters from mm }, nil } -func (wb *wheeledBase) Geometries(ctx context.Context) ([]spatialmath.Geometry, error) { +func (wb *wheeledBase) Geometries(ctx context.Context, extra map[string]interface{}) ([]spatialmath.Geometry, error) { return wb.geometries, nil } diff --git a/components/base/wheeled/wheeled_base_test.go b/components/base/wheeled/wheeled_base_test.go index 1d9046cf64d..87725fedba0 100644 --- a/components/base/wheeled/wheeled_base_test.go +++ b/components/base/wheeled/wheeled_base_test.go @@ -7,12 +7,14 @@ import ( "time" "github.com/edaniels/golog" + "github.com/golang/geo/r3" "go.viam.com/test" "go.viam.com/utils" "go.viam.com/rdk/components/base" "go.viam.com/rdk/components/motor" "go.viam.com/rdk/components/motor/fake" + "go.viam.com/rdk/operation" "go.viam.com/rdk/resource" ) @@ -39,6 +41,7 @@ func fakeMotorDependencies(t *testing.T, deps []string) resource.Dependencies { result[motor.Named(dep)] = &fake.Motor{ Named: motor.Named(dep).AsNamed(), MaxRPM: 60, + OpMgr: operation.NewSingleOperationManager(), Logger: logger, } } @@ -63,6 +66,29 @@ func TestWheelBaseMath(t *testing.T) { props, err := wb.Properties(ctx, nil) test.That(t, err, test.ShouldBeNil) test.That(t, props.WidthMeters, test.ShouldEqual, 100*0.001) + + geometries, err := wb.Geometries(ctx, nil) + test.That(t, geometries, test.ShouldBeNil) + test.That(t, err, test.ShouldBeNil) + + err = wb.SetVelocity(ctx, r3.Vector{X: 0, Y: 10, Z: 0}, r3.Vector{X: 0, Y: 0, Z: 10}, nil) + test.That(t, err.Error(), test.ShouldContainSubstring, "0 RPM") + + err = wb.SetVelocity(ctx, r3.Vector{X: 0, Y: 100, Z: 0}, r3.Vector{X: 0, Y: 0, Z: 100}, nil) + test.That(t, err, test.ShouldBeNil) + + moving, err := wb.IsMoving(ctx) + test.That(t, err, test.ShouldBeNil) + test.That(t, moving, test.ShouldBeTrue) + + err = wb.SetPower(ctx, r3.Vector{X: 0, Y: 10, Z: 0}, r3.Vector{X: 0, Y: 0, Z: 10}, nil) + test.That(t, err, test.ShouldBeNil) + + test.That(t, wb.Stop(ctx, nil), test.ShouldBeNil) + + moving, err = wb.IsMoving(ctx) + test.That(t, err, test.ShouldBeNil) + test.That(t, moving, test.ShouldBeFalse) }) t.Run("math_straight", func(t *testing.T) { @@ -334,6 +360,66 @@ func TestWheeledBaseConstructor(t *testing.T) { test.That(t, len(wb.allMotors), test.ShouldEqual, 4) } +func TestWheeledBaseReconfigure(t *testing.T) { + ctx := context.Background() + logger := golog.NewTestLogger(t) + + // valid config + testCfg := newTestCfg() + deps, err := testCfg.Validate("path", resource.APITypeComponentName) + test.That(t, err, test.ShouldBeNil) + motorDeps := fakeMotorDependencies(t, deps) + + newBase, err := createWheeledBase(ctx, motorDeps, testCfg, logger) + test.That(t, err, test.ShouldBeNil) + wb, ok := newBase.(*wheeledBase) + test.That(t, ok, test.ShouldBeTrue) + test.That(t, len(wb.left), test.ShouldEqual, 2) + test.That(t, len(wb.right), test.ShouldEqual, 2) + test.That(t, len(wb.allMotors), test.ShouldEqual, 4) + + // invert the motors to confirm that Reconfigure still occurs when array order/naming changes + newTestConf := newTestCfg() + newTestConf.ConvertedAttributes = &Config{ + WidthMM: 100, + WheelCircumferenceMM: 1000, + Left: []string{"fr-m", "br-m"}, + Right: []string{"fl-m", "bl-m"}, + } + deps, err = newTestConf.Validate("path", resource.APITypeComponentName) + test.That(t, err, test.ShouldBeNil) + motorDeps = fakeMotorDependencies(t, deps) + test.That(t, wb.Reconfigure(ctx, motorDeps, newTestConf), test.ShouldBeNil) + + // Add a new motor to Left only to confirm that Reconfigure is impossible because cfg validation fails + newerTestCfg := newTestCfg() + newerTestCfg.ConvertedAttributes = &Config{ + WidthMM: 100, + WheelCircumferenceMM: 1000, + Left: []string{"fl-m", "bl-m", "ml-m"}, + Right: []string{"fr-m", "br-m"}, + } + + deps, err = newerTestCfg.Validate("path", resource.APITypeComponentName) + test.That(t, err.Error(), test.ShouldContainSubstring, "left and right need to have the same number of motors") + test.That(t, deps, test.ShouldBeNil) + + // Add a motor to Right so Left and Right are now the same size again, confirm that Reconfigure + // occurs after the motor array size change + newestTestCfg := newTestCfg() + newestTestCfg.ConvertedAttributes = &Config{ + WidthMM: 100, + WheelCircumferenceMM: 1000, + Left: []string{"fl-m", "bl-m", "ml-m"}, + Right: []string{"fr-m", "br-m", "mr-m"}, + } + + deps, err = newestTestCfg.Validate("path", resource.APITypeComponentName) + test.That(t, err, test.ShouldBeNil) + motorDeps = fakeMotorDependencies(t, deps) + test.That(t, wb.Reconfigure(ctx, motorDeps, newestTestCfg), test.ShouldBeNil) +} + func TestValidate(t *testing.T) { cfg := &Config{} deps, err := cfg.Validate("path") diff --git a/components/board/beaglebone/board.go b/components/board/beaglebone/board.go index 46d95762e95..b30d221cc0d 100644 --- a/components/board/beaglebone/board.go +++ b/components/board/beaglebone/board.go @@ -22,6 +22,5 @@ func init() { golog.Global().Debugw("error getting beaglebone GPIO board mapping", "error", err) } - // The false on this line means we're not using Periph. This lets us enable hardware PWM pins. - genericlinux.RegisterBoard(modelName, gpioMappings, false) + genericlinux.RegisterBoard(modelName, gpioMappings) } diff --git a/components/board/beaglebone/data.go b/components/board/beaglebone/data.go index 627e1c322c6..f0dcd17ca9c 100644 --- a/components/board/beaglebone/data.go +++ b/components/board/beaglebone/data.go @@ -7,7 +7,6 @@ const bbAi = "bb_Ai64" var boardInfoMappings = map[string]genericlinux.BoardInformation{ bbAi: { PinDefinitions: []genericlinux.PinDefinition{ - // GPIOChipRelativeIDs {NGPIO: LINENUM} -> {128: 93} // PinNumberBoard {914} -> PinNameCVM3 "P9_14" // ******** DATA MAPPING ******************************** @@ -23,80 +22,84 @@ var boardInfoMappings = map[string]genericlinux.BoardInformation{ // beaglebone 600000.gpio/ (128 lines) corresponds to gpiochip1 and /sys/class/gpio/gpiochip300/ // beaglebone 601000.gpio/ (36 lines) corresponds to gpiochip2 and /sys/class/gpio/gpiochip264/ - {map[int]int{128: 20}, map[int]string{}, "600000.gpio", 803, 0, "P8_03", "", "", -1}, - {map[int]int{128: 48}, map[int]string{}, "600000.gpio", 804, 0, "P8_04", "", "", -1}, // BOOTMODE2 - {map[int]int{128: 33}, map[int]string{}, "600000.gpio", 805, 0, "P8_05", "", "", -1}, - {map[int]int{128: 34}, map[int]string{}, "600000.gpio", 806, 0, "P8_06", "", "", -1}, - {map[int]int{128: 15}, map[int]string{}, "600000.gpio", 807, 0, "P8_07", "", "", -1}, - {map[int]int{128: 14}, map[int]string{}, "600000.gpio", 808, 0, "P8_08", "", "", -1}, - {map[int]int{128: 17}, map[int]string{}, "600000.gpio", 809, 0, "P8_09", "", "", -1}, - {map[int]int{128: 16}, map[int]string{}, "600000.gpio", 810, 0, "P8_10", "", "", -1}, - {map[int]int{128: 60}, map[int]string{}, "600000.gpio", 811, 0, "P8_11", "", "", -1}, // BOOTMODE7 - {map[int]int{128: 59}, map[int]string{}, "600000.gpio", 812, 0, "P8_12", "", "", -1}, - {map[int]int{128: 89}, map[int]string{}, "600000.gpio", 813, 0, "P8_13", "", "3000000.pwm", 1}, // pwmchip0 V27 EHRPWM0_A - {map[int]int{128: 75}, map[int]string{}, "600000.gpio", 814, 0, "P8_14", "", "", -1}, - {map[int]int{128: 61}, map[int]string{}, "600000.gpio", 815, 0, "P8_15", "", "", -1}, - {map[int]int{128: 62}, map[int]string{}, "600000.gpio", 816, 0, "P8_16", "", "", -1}, - {map[int]int{128: 3}, map[int]string{}, "600000.gpio", 817, 0, "P8_17", "", "", -1}, - {map[int]int{128: 4}, map[int]string{}, "600000.gpio", 818, 0, "P8_18", "", "", -1}, - {map[int]int{128: 88}, map[int]string{}, "600000.gpio", 819, 0, "P8_19", "", "3000000.pwm", 0}, // pwmchip0 V29 EHRPWM0_B - {map[int]int{128: 76}, map[int]string{}, "600000.gpio", 820, 0, "P8_20", "", "", -1}, - {map[int]int{128: 30}, map[int]string{}, "600000.gpio", 821, 0, "P8_21", "", "", -1}, - {map[int]int{128: 5}, map[int]string{}, "600000.gpio", 822, 0, "P8_22", "", "", -1}, - {map[int]int{128: 31}, map[int]string{}, "600000.gpio", 823, 0, "P8_23", "", "", -1}, - {map[int]int{128: 6}, map[int]string{}, "600000.gpio", 824, 0, "P8_24", "", "", -1}, - {map[int]int{128: 35}, map[int]string{}, "600000.gpio", 825, 0, "P8_25", "", "", -1}, - {map[int]int{128: 51}, map[int]string{}, "600000.gpio", 826, 0, "P8_26", "", "", -1}, - {map[int]int{128: 71}, map[int]string{}, "600000.gpio", 827, 0, "P8_27", "", "", -1}, - {map[int]int{128: 72}, map[int]string{}, "600000.gpio", 828, 0, "P8_28", "", "", -1}, - {map[int]int{128: 73}, map[int]string{}, "600000.gpio", 829, 0, "P8_29", "", "", -1}, - {map[int]int{128: 74}, map[int]string{}, "600000.gpio", 830, 0, "P8_30", "", "", -1}, - {map[int]int{128: 32}, map[int]string{}, "600000.gpio", 831, 0, "P8_31", "", "", -1}, - {map[int]int{128: 26}, map[int]string{}, "600000.gpio", 832, 0, "P8_32", "", "", -1}, // Timer-based PWM - {map[int]int{128: 25}, map[int]string{}, "600000.gpio", 833, 0, "P8_33", "", "", -1}, // Timer-based PWM - {map[int]int{128: 7}, map[int]string{}, "600000.gpio", 834, 0, "P8_34", "", "", -1}, - {map[int]int{128: 24}, map[int]string{}, "600000.gpio", 835, 0, "P8_35", "", "", -1}, // Timer-based PWM - {map[int]int{128: 8}, map[int]string{}, "600000.gpio", 836, 0, "P8_36", "", "", -1}, - {map[int]int{128: 11}, map[int]string{}, "600000.gpio", 837, 0, "P8_37", "", "", -1}, // Timer-based PWM - {map[int]int{128: 9}, map[int]string{}, "600000.gpio", 838, 0, "P8_38", "", "", -1}, - {map[int]int{128: 69}, map[int]string{}, "600000.gpio", 839, 0, "P8_39", "", "", -1}, - {map[int]int{128: 70}, map[int]string{}, "600000.gpio", 840, 0, "P8_40", "", "", -1}, - {map[int]int{128: 67}, map[int]string{}, "600000.gpio", 841, 0, "P8_41", "", "", -1}, - {map[int]int{128: 68}, map[int]string{}, "600000.gpio", 842, 0, "P8_42", "", "", -1}, // BOOTMODE6 - {map[int]int{128: 65}, map[int]string{}, "600000.gpio", 843, 0, "P8_43", "", "", -1}, - {map[int]int{128: 66}, map[int]string{}, "600000.gpio", 844, 0, "P8_44", "", "", -1}, - {map[int]int{128: 79}, map[int]string{}, "600000.gpio", 845, 0, "P8_45", "", "", -1}, - {map[int]int{128: 80}, map[int]string{}, "600000.gpio", 846, 0, "P8_46", "", "", -1}, // BOOTMODE3 - {map[int]int{128: 1}, map[int]string{}, "600000.gpio", 911, 0, "P9_11", "", "", -1}, - {map[int]int{128: 45}, map[int]string{}, "600000.gpio", 912, 0, "P9_12", "", "", -1}, - {map[int]int{128: 2}, map[int]string{}, "600000.gpio", 913, 0, "P9_13", "", "", -1}, - {map[int]int{128: 93}, map[int]string{}, "600000.gpio", 914, 0, "P9_14", "", "3020000.pwm", 0}, // pwmchip4 U27 EHRPWM2_A - {map[int]int{128: 47}, map[int]string{}, "600000.gpio", 915, 0, "P9_15", "", "", -1}, - {map[int]int{128: 94}, map[int]string{}, "600000.gpio", 916, 0, "P9_16", "", "3020000.pwm", 1}, // pwmchip4 U24 EHRPWM2_B - {map[int]int{128: 28}, map[int]string{}, "600000.gpio", 917, 0, "P9_17", "", "", -1}, - {map[int]int{128: 40}, map[int]string{}, "600000.gpio", 918, 0, "P9_18", "", "", -1}, - {map[int]int{128: 78}, map[int]string{}, "600000.gpio", 919, 0, "P9_19", "", "", -1}, - {map[int]int{128: 77}, map[int]string{}, "600000.gpio", 920, 0, "P9_20", "", "", -1}, - {map[int]int{128: 39}, map[int]string{}, "600000.gpio", 921, 0, "P9_21", "", "3010000.pwm", 0}, // pwmchip2 - {map[int]int{128: 38}, map[int]string{}, "600000.gpio", 922, 0, "P9_22", "", "3010000.pwm", 1}, // pwmchip2 BOOTMODE1 - {map[int]int{128: 10}, map[int]string{}, "600000.gpio", 923, 0, "P9_23", "", "", -1}, - {map[int]int{128: 13}, map[int]string{}, "600000.gpio", 924, 0, "P9_24", "", "", -1}, - {map[int]int{128: 127}, map[int]string{}, "600000.gpio", 925, 0, "P9_25", "", "", -1}, - {map[int]int{128: 12}, map[int]string{}, "600000.gpio", 926, 0, "P9_26", "", "", -1}, - {map[int]int{128: 46}, map[int]string{}, "600000.gpio", 927, 0, "P9_27", "", "", -1}, // Timer-based PWM - {map[int]int{128: 43}, map[int]string{}, "600000.gpio", 928, 0, "P9_28", "", "", -1}, - {map[int]int{36: 14}, map[int]string{}, "601000.gpio", 929, 0, "P9_29", "", "", -1}, // Timer-based PWM - {map[int]int{36: 13}, map[int]string{}, "601000.gpio", 930, 0, "P9_30", "", "", -1}, // Timer-based PWM - {map[int]int{128: 52}, map[int]string{}, "600000.gpio", 931, 0, "P9_31", "", "", -1}, - {map[int]int{128: 50}, map[int]string{}, "600000.gpio", 933, 0, "P9_33", "", "", -1}, - {map[int]int{128: 55}, map[int]string{}, "600000.gpio", 935, 0, "P9_35", "", "", -1}, - {map[int]int{128: 56}, map[int]string{}, "600000.gpio", 936, 0, "P9_36", "", "", -1}, - {map[int]int{128: 57}, map[int]string{}, "600000.gpio", 937, 0, "P9_37", "", "", -1}, - {map[int]int{128: 58}, map[int]string{}, "600000.gpio", 938, 0, "P9_38", "", "", -1}, - {map[int]int{128: 54}, map[int]string{}, "600000.gpio", 939, 0, "P9_39", "", "", -1}, - {map[int]int{128: 81}, map[int]string{}, "600000.gpio", 940, 0, "P9_40", "", "", -1}, - {map[int]int{36: 0}, map[int]string{}, "601000.gpio", 941, 0, "P9_41", "", "", -1}, - {map[int]int{128: 123}, map[int]string{}, "600000.gpio", 942, 0, "P9_42", "", "", -1}, // Timer-based PWM + {Name: "803", DeviceName: "gpiochip1", LineNumber: 20, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "804", DeviceName: "gpiochip1", LineNumber: 48, PwmChipSysfsDir: "", PwmID: -1}, // BOOTMODE2 + {Name: "805", DeviceName: "gpiochip1", LineNumber: 33, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "806", DeviceName: "gpiochip1", LineNumber: 34, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "807", DeviceName: "gpiochip1", LineNumber: 15, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "808", DeviceName: "gpiochip1", LineNumber: 14, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "809", DeviceName: "gpiochip1", LineNumber: 17, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "810", DeviceName: "gpiochip1", LineNumber: 16, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "811", DeviceName: "gpiochip1", LineNumber: 60, PwmChipSysfsDir: "", PwmID: -1}, // BOOTMODE7 + {Name: "812", DeviceName: "gpiochip1", LineNumber: 59, PwmChipSysfsDir: "", PwmID: -1}, + // We're unable to get GPIO to work on pin 813 despite Beaglebone's docs. PWM works. + {Name: "813", DeviceName: "gpiochip1", LineNumber: 89, PwmChipSysfsDir: "3000000.pwm", PwmID: 1}, // pwmchip0 V27 EHRPWM0_A + {Name: "814", DeviceName: "gpiochip1", LineNumber: 75, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "815", DeviceName: "gpiochip1", LineNumber: 61, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "816", DeviceName: "gpiochip1", LineNumber: 62, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "817", DeviceName: "gpiochip1", LineNumber: 3, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "818", DeviceName: "gpiochip1", LineNumber: 4, PwmChipSysfsDir: "", PwmID: -1}, + // We're unable to get GPIO to work on pin 819 despite Beaglebone's docs. PWM works. + {Name: "819", DeviceName: "gpiochip1", LineNumber: 88, PwmChipSysfsDir: "3000000.pwm", PwmID: 0}, // pwmchip0 V29 EHRPWM0_B + {Name: "820", DeviceName: "gpiochip1", LineNumber: 76, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "821", DeviceName: "gpiochip1", LineNumber: 30, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "822", DeviceName: "gpiochip1", LineNumber: 5, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "823", DeviceName: "gpiochip1", LineNumber: 31, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "824", DeviceName: "gpiochip1", LineNumber: 6, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "825", DeviceName: "gpiochip1", LineNumber: 35, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "826", DeviceName: "gpiochip1", LineNumber: 51, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "827", DeviceName: "gpiochip1", LineNumber: 71, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "828", DeviceName: "gpiochip1", LineNumber: 72, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "829", DeviceName: "gpiochip1", LineNumber: 73, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "830", DeviceName: "gpiochip1", LineNumber: 74, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "831", DeviceName: "gpiochip1", LineNumber: 32, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "832", DeviceName: "gpiochip1", LineNumber: 26, PwmChipSysfsDir: "", PwmID: -1}, // Timer-based PWM + {Name: "833", DeviceName: "gpiochip1", LineNumber: 25, PwmChipSysfsDir: "", PwmID: -1}, // Timer-based PWM + {Name: "834", DeviceName: "gpiochip1", LineNumber: 7, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "835", DeviceName: "gpiochip1", LineNumber: 24, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "836", DeviceName: "gpiochip1", LineNumber: 8, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "837", DeviceName: "gpiochip1", LineNumber: 11, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "838", DeviceName: "gpiochip1", LineNumber: 9, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "839", DeviceName: "gpiochip1", LineNumber: 69, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "840", DeviceName: "gpiochip1", LineNumber: 70, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "841", DeviceName: "gpiochip1", LineNumber: 67, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "842", DeviceName: "gpiochip1", LineNumber: 68, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "843", DeviceName: "gpiochip1", LineNumber: 65, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "844", DeviceName: "gpiochip1", LineNumber: 66, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "845", DeviceName: "gpiochip1", LineNumber: 79, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "846", DeviceName: "gpiochip1", LineNumber: 80, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "911", DeviceName: "gpiochip1", LineNumber: 1, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "912", DeviceName: "gpiochip1", LineNumber: 45, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "913", DeviceName: "gpiochip1", LineNumber: 2, PwmChipSysfsDir: "", PwmID: -1}, + // We're unable to get GPIO to work on pin 914 despite Beaglebone's docs. PWM works. + {Name: "914", DeviceName: "gpiochip1", LineNumber: 93, PwmChipSysfsDir: "3020000.pwm", PwmID: 0}, + {Name: "915", DeviceName: "gpiochip1", LineNumber: 47, PwmChipSysfsDir: "", PwmID: -1}, + // We're unable to get GPIO to work on pin 916 despite Beaglebone's docs. PWM works. + {Name: "916", DeviceName: "gpiochip1", LineNumber: 94, PwmChipSysfsDir: "3020000.pwm", PwmID: 1}, + {Name: "917", DeviceName: "gpiochip1", LineNumber: 28, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "918", DeviceName: "gpiochip1", LineNumber: 40, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "919", DeviceName: "gpiochip1", LineNumber: 78, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "920", DeviceName: "gpiochip1", LineNumber: 77, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "921", DeviceName: "gpiochip1", LineNumber: 39, PwmChipSysfsDir: "3010000.pwm", PwmID: 0}, + {Name: "922", DeviceName: "gpiochip1", LineNumber: 38, PwmChipSysfsDir: "3010000.pwm", PwmID: 1}, + {Name: "923", DeviceName: "gpiochip1", LineNumber: 10, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "924", DeviceName: "gpiochip1", LineNumber: 13, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "925", DeviceName: "gpiochip1", LineNumber: 127, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "926", DeviceName: "gpiochip1", LineNumber: 12, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "927", DeviceName: "gpiochip1", LineNumber: 46, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "928", DeviceName: "gpiochip1", LineNumber: 43, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "929", DeviceName: "gpiochip2", LineNumber: 14, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "930", DeviceName: "gpiochip2", LineNumber: 13, PwmChipSysfsDir: "", PwmID: -1}, // Timer-based PWM + {Name: "931", DeviceName: "gpiochip1", LineNumber: 52, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "933", DeviceName: "gpiochip1", LineNumber: 50, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "935", DeviceName: "gpiochip1", LineNumber: 55, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "936", DeviceName: "gpiochip1", LineNumber: 56, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "937", DeviceName: "gpiochip1", LineNumber: 57, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "938", DeviceName: "gpiochip1", LineNumber: 58, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "939", DeviceName: "gpiochip1", LineNumber: 54, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "940", DeviceName: "gpiochip1", LineNumber: 81, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "941", DeviceName: "gpiochip2", LineNumber: 0, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "942", DeviceName: "gpiochip1", LineNumber: 123, PwmChipSysfsDir: "", PwmID: -1}, // Timer-based PWM }, Compats: []string{"beagle,j721e-beagleboneai64", "ti,j721e"}, }, diff --git a/components/board/client_test.go b/components/board/client_test.go index a31c696351c..a14d26edc67 100644 --- a/components/board/client_test.go +++ b/components/board/client_test.go @@ -7,7 +7,6 @@ import ( "time" "github.com/edaniels/golog" - "github.com/pkg/errors" commonpb "go.viam.com/api/common/v1" boardpb "go.viam.com/api/component/board/v1" "go.viam.com/test" @@ -57,7 +56,7 @@ func TestFailingClient(t *testing.T) { _, err := viamgrpc.Dial(cancelCtx, listener.Addr().String(), logger) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "canceled") + test.That(t, err, test.ShouldBeError, context.Canceled) } func TestWorkingClient(t *testing.T) { @@ -256,9 +255,6 @@ func TestClientWithoutStatus(t *testing.T) { logger := golog.NewTestLogger(t) injectBoard := &inject.Board{} - injectBoard.StatusFunc = func(ctx context.Context, extra map[string]interface{}) (*commonpb.BoardStatus, error) { - return nil, errors.New("no status") - } listener1, err := net.Listen("tcp", "localhost:0") test.That(t, err, test.ShouldBeNil) diff --git a/components/board/collector.go b/components/board/collector.go index 1db0de1287a..670a9459583 100644 --- a/components/board/collector.go +++ b/components/board/collector.go @@ -3,6 +3,7 @@ package board import ( "context" + "github.com/pkg/errors" "google.golang.org/protobuf/types/known/anypb" "go.viam.com/rdk/data" @@ -57,8 +58,13 @@ func newAnalogCollector(resource interface{}, params data.CollectorParams) (data var readings []AnalogRecord for k := range arg { if reader, ok := board.AnalogReaderByName(k); ok { - value, err := reader.Read(ctx, nil) + value, err := reader.Read(ctx, data.FromDMExtraMap) if err != nil { + // A modular filter component can be created to filter the readings from a component. The error ErrNoCaptureToStore + // is used in the datamanager to exclude readings from being captured and stored. + if errors.Is(err, data.ErrNoCaptureToStore) { + return nil, err + } return nil, data.FailedToReadErr(params.ComponentName, analogs.String(), err) } readings = append(readings, AnalogRecord{AnalogName: k, AnalogValue: value}) @@ -79,8 +85,13 @@ func newGPIOCollector(resource interface{}, params data.CollectorParams) (data.C var readings []GpioRecord for k := range arg { if gpio, err := board.GPIOPinByName(k); err == nil { - value, err := gpio.Get(ctx, nil) + value, err := gpio.Get(ctx, data.FromDMExtraMap) if err != nil { + // A modular filter component can be created to filter the readings from a component. The error ErrNoCaptureToStore + // is used in the datamanager to exclude readings from being captured and stored. + if errors.Is(err, data.ErrNoCaptureToStore) { + return nil, err + } return nil, data.FailedToReadErr(params.ComponentName, gpios.String(), err) } readings = append(readings, GpioRecord{GPIOName: k, GPIOValue: value}) diff --git a/components/board/customlinux/README.md b/components/board/customlinux/README.md new file mode 100644 index 00000000000..32eeaca280f --- /dev/null +++ b/components/board/customlinux/README.md @@ -0,0 +1,18 @@ +## [EXPERIMENTAL] Configuring a custom Linux board +This component supports a board running Linux and requires the user to provide a map of gpio pin names to the corresponding gpio chip and line number. The mappings should be provided in a json file in this format: +```json +{ + "pins": [ + { + "name": "string", + "device_name": "string", + "line_number": "int", + "pwm_chip_sysfs_dir": "string", + "pwm_id": "int" + } + ] +} +``` + +`pwm_chip_sysfs_dir` and `pwm_id` are optional fields. +To configure a new board with these mappings, set the `pin_config_file_path` attribute to the filepath to your json configuration file. diff --git a/components/board/customlinux/board.go b/components/board/customlinux/board.go new file mode 100644 index 00000000000..1d6d8c5d6c8 --- /dev/null +++ b/components/board/customlinux/board.go @@ -0,0 +1,106 @@ +//go:build linux + +// Package customlinux implements a board running Linux. +// This is an Experimental package +package customlinux + +import ( + "context" + "encoding/json" + "os" + "path/filepath" + + "github.com/edaniels/golog" + "go.uber.org/multierr" + "periph.io/x/host/v3" + + "go.viam.com/rdk/components/board" + "go.viam.com/rdk/components/board/genericlinux" + "go.viam.com/rdk/resource" +) + +const modelName = "customlinux" + +func init() { + if _, err := host.Init(); err != nil { + golog.Global().Debugw("error initializing host", "error", err) + } + + resource.RegisterComponent( + board.API, + resource.DefaultModelFamily.WithModel(modelName), + resource.Registration[board.Board, *Config]{ + Constructor: createNewBoard, + }) +} + +func createNewBoard( + ctx context.Context, + _ resource.Dependencies, + conf resource.Config, + logger golog.Logger, +) (board.Board, error) { + return genericlinux.NewBoard(ctx, conf, pinDefsFromFile, logger) +} + +// This is a ConfigConverter which loads pin definitions from a file, assuming that the config +// passed in is a customlinux.Config underneath. +func pinDefsFromFile(conf resource.Config) (*genericlinux.LinuxBoardConfig, error) { + newConf, err := resource.NativeConfig[*Config](conf) + if err != nil { + return nil, err + } + + pinDefs, err := parsePinConfig(newConf.PinConfigFilePath) + if err != nil { + return nil, err + } + + gpioMappings, err := genericlinux.GetGPIOBoardMappingFromPinDefs(pinDefs) + if err != nil { + return nil, err + } + + return &genericlinux.LinuxBoardConfig{ + I2Cs: newConf.I2Cs, + SPIs: newConf.SPIs, + Analogs: newConf.Analogs, + DigitalInterrupts: newConf.DigitalInterrupts, + GpioMappings: gpioMappings, + }, nil +} + +func parsePinConfig(filePath string) ([]genericlinux.PinDefinition, error) { + pinData, err := os.ReadFile(filepath.Clean(filePath)) + if err != nil { + return nil, err + } + + return parseRawPinData(pinData, filePath) +} + +// filePath passed in for logging purposes. +func parseRawPinData(pinData []byte, filePath string) ([]genericlinux.PinDefinition, error) { + var parsedPinData genericlinux.PinDefinitions + if err := json.Unmarshal(pinData, &parsedPinData); err != nil { + return nil, err + } + + var err error + for _, pin := range parsedPinData.Pins { + err = multierr.Combine(err, pin.Validate(filePath)) + } + if err != nil { + return nil, err + } + return parsedPinData.Pins, nil +} + +func createGenericLinuxConfig(conf *Config) genericlinux.Config { + return genericlinux.Config{ + I2Cs: conf.I2Cs, + SPIs: conf.SPIs, + Analogs: conf.Analogs, + DigitalInterrupts: conf.DigitalInterrupts, + } +} diff --git a/components/board/customlinux/board_nonlinux.go b/components/board/customlinux/board_nonlinux.go new file mode 100644 index 00000000000..ec65213a8b3 --- /dev/null +++ b/components/board/customlinux/board_nonlinux.go @@ -0,0 +1,5 @@ +//go:build !linux + +// Package customlinux implements a board running Linux. This file, however, is +// a placeholder for when you build the server in a non-Linux environment. +package customlinux diff --git a/components/board/customlinux/setup.go b/components/board/customlinux/setup.go new file mode 100644 index 00000000000..9dac225a1c8 --- /dev/null +++ b/components/board/customlinux/setup.go @@ -0,0 +1,32 @@ +//go:build linux + +// Package customlinux implements a board running Linux +package customlinux + +import ( + "os" + + "go.viam.com/rdk/components/board" +) + +// A Config describes the configuration of a board and all of its connected parts. +type Config struct { + PinConfigFilePath string `json:"pin_config_file_path"` + I2Cs []board.I2CConfig `json:"i2cs,omitempty"` + SPIs []board.SPIConfig `json:"spis,omitempty"` + Analogs []board.AnalogConfig `json:"analogs,omitempty"` + DigitalInterrupts []board.DigitalInterruptConfig `json:"digital_interrupts,omitempty"` +} + +// Validate ensures all parts of the config are valid. +func (conf *Config) Validate(path string) ([]string, error) { + if _, err := os.Stat(conf.PinConfigFilePath); err != nil { + return nil, err + } + + boardConfig := createGenericLinuxConfig(conf) + if deps, err := boardConfig.Validate(path); err != nil { + return deps, err + } + return nil, nil +} diff --git a/components/board/customlinux/setup_test.go b/components/board/customlinux/setup_test.go new file mode 100644 index 00000000000..8d69f1eab76 --- /dev/null +++ b/components/board/customlinux/setup_test.go @@ -0,0 +1,66 @@ +//go:build linux + +// Package customlinux implements a board running linux +package customlinux + +import ( + "testing" + + "go.viam.com/test" + + "go.viam.com/rdk/components/board" + "go.viam.com/rdk/components/board/genericlinux" +) + +func TestConfigParse(t *testing.T) { + emptyConfig := []byte(`{"pins": [{}]}`) + _, err := parseRawPinData(emptyConfig, "path") + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, `"name" is required`) + + emptyPWMID := []byte(`{"pins": [{"name": "7", "device_name": "gpiochip1", "line_number": 71, "pwm_chip_sysfs_dir": "hi"}]}`) + _, err = parseRawPinData(emptyPWMID, "path") + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, "must supply pwm_id for the pwm chip") + + invalidLineNumber := []byte(`{"pins": [{"name": "7", "device_name": "gpiochip1", "line_number": -2}]}`) + _, err = parseRawPinData(invalidLineNumber, "path") + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, "line_number on gpio chip must be at least zero") + + validConfig := []byte(`{"pins": [{"name": "7", "device_name": "gpiochip1", "line_number": 80}]}`) + data, err := parseRawPinData(validConfig, "path") + correctData := make([]genericlinux.PinDefinition, 1) + correctData[0] = genericlinux.PinDefinition{ + Name: "7", + DeviceName: "gpiochip1", + LineNumber: 80, + PwmID: -1, + } + test.That(t, err, test.ShouldBeNil) + test.That(t, data, test.ShouldResemble, correctData) +} + +func TestConfigValidate(t *testing.T) { + validConfig := Config{} + + _, err := validConfig.Validate("path") + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, "no such file or directory") + + validConfig.PinConfigFilePath = "./" + _, err = validConfig.Validate("path") + test.That(t, err, test.ShouldBeNil) + + validConfig.DigitalInterrupts = []board.DigitalInterruptConfig{{}} + _, err = validConfig.Validate("path") + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, "path.digital_interrupts.0") + test.That(t, err.Error(), test.ShouldContainSubstring, `"name" is required`) + + validConfig.DigitalInterrupts = []board.DigitalInterruptConfig{{Name: "20"}} + _, err = validConfig.Validate("path") + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, "path.digital_interrupts.0") + test.That(t, err.Error(), test.ShouldContainSubstring, `"pin" is required`) +} diff --git a/components/board/fake/board.go b/components/board/fake/board.go index 3711e32a8dd..c68f62c26e8 100644 --- a/components/board/fake/board.go +++ b/components/board/fake/board.go @@ -88,6 +88,7 @@ func NewBoard(ctx context.Context, conf resource.Config, logger golog.Logger) (* Analogs: map[string]*Analog{}, Digitals: map[string]*DigitalInterruptWrapper{}, GPIOPins: map[string]*GPIOPin{}, + logger: logger, } if err := b.processConfig(conf); err != nil { @@ -204,6 +205,7 @@ type Board struct { Analogs map[string]*Analog Digitals map[string]*DigitalInterruptWrapper GPIOPins map[string]*GPIOPin + logger golog.Logger CloseCount int } @@ -497,10 +499,15 @@ type GPIOPin struct { high bool pwm float64 pwmFreq uint + + mu sync.Mutex } // Set sets the pin to either low or high. func (gp *GPIOPin) Set(ctx context.Context, high bool, extra map[string]interface{}) error { + gp.mu.Lock() + defer gp.mu.Unlock() + gp.high = high gp.pwm = 0 gp.pwmFreq = 0 @@ -509,27 +516,42 @@ func (gp *GPIOPin) Set(ctx context.Context, high bool, extra map[string]interfac // Get gets the high/low state of the pin. func (gp *GPIOPin) Get(ctx context.Context, extra map[string]interface{}) (bool, error) { + gp.mu.Lock() + defer gp.mu.Unlock() + return gp.high, nil } // PWM gets the pin's given duty cycle. func (gp *GPIOPin) PWM(ctx context.Context, extra map[string]interface{}) (float64, error) { + gp.mu.Lock() + defer gp.mu.Unlock() + return gp.pwm, nil } // SetPWM sets the pin to the given duty cycle. func (gp *GPIOPin) SetPWM(ctx context.Context, dutyCyclePct float64, extra map[string]interface{}) error { + gp.mu.Lock() + defer gp.mu.Unlock() + gp.pwm = dutyCyclePct return nil } // PWMFreq gets the PWM frequency of the pin. func (gp *GPIOPin) PWMFreq(ctx context.Context, extra map[string]interface{}) (uint, error) { + gp.mu.Lock() + defer gp.mu.Unlock() + return gp.pwmFreq, nil } // SetPWMFreq sets the given pin to the given PWM frequency. func (gp *GPIOPin) SetPWMFreq(ctx context.Context, freqHz uint, extra map[string]interface{}) error { + gp.mu.Lock() + defer gp.mu.Unlock() + gp.pwmFreq = freqHz return nil } diff --git a/components/board/genericlinux/board.go b/components/board/genericlinux/board.go index c398e73eefa..8c7616b9578 100644 --- a/components/board/genericlinux/board.go +++ b/components/board/genericlinux/board.go @@ -18,9 +18,6 @@ import ( commonpb "go.viam.com/api/common/v1" pb "go.viam.com/api/component/board/v1" goutils "go.viam.com/utils" - "periph.io/x/conn/v3/gpio" - "periph.io/x/conn/v3/gpio/gpioreg" - "periph.io/x/conn/v3/physic" "go.viam.com/rdk/components/board" "go.viam.com/rdk/grpc" @@ -28,7 +25,7 @@ import ( ) // RegisterBoard registers a sysfs based board of the given model. -func RegisterBoard(modelName string, gpioMappings map[int]GPIOBoardMapping, usePeriphGpio bool) { +func RegisterBoard(modelName string, gpioMappings map[string]GPIOBoardMapping) { resource.RegisterComponent( board.API, resource.DefaultModelFamily.WithModel(modelName), @@ -39,81 +36,175 @@ func RegisterBoard(modelName string, gpioMappings map[int]GPIOBoardMapping, useP conf resource.Config, logger golog.Logger, ) (board.Board, error) { - return newBoard(ctx, conf, gpioMappings, usePeriphGpio, logger) + return NewBoard(ctx, conf, ConstPinDefs(gpioMappings), logger) }, }) } -func newBoard( +// NewBoard is the constructor for a Board. +func NewBoard( ctx context.Context, conf resource.Config, - gpioMappings map[int]GPIOBoardMapping, - usePeriphGpio bool, + convertConfig ConfigConverter, logger golog.Logger, ) (board.Board, error) { cancelCtx, cancelFunc := context.WithCancel(context.Background()) - b := sysfsBoard{ + + b := &Board{ Named: conf.ResourceName().AsNamed(), - usePeriphGpio: usePeriphGpio, - gpioMappings: gpioMappings, - logger: logger, - cancelCtx: cancelCtx, - cancelFunc: cancelFunc, - - spis: map[string]*spiBus{}, - analogs: map[string]*wrappedAnalog{}, - // this is not yet modified during reconfiguration but maybe should be - pwms: map[string]pwmSetting{}, + convertConfig: convertConfig, + + logger: logger, + cancelCtx: cancelCtx, + cancelFunc: cancelFunc, + + spis: map[string]*spiBus{}, + analogs: map[string]*wrappedAnalog{}, i2cs: map[string]*I2cBus{}, gpios: map[string]*gpioPin{}, interrupts: map[string]*digitalInterrupt{}, } - for pinNumber, mapping := range gpioMappings { - b.gpios[fmt.Sprintf("%d", pinNumber)] = b.createGpioPin(mapping) - } - if err := b.Reconfigure(ctx, nil, conf); err != nil { return nil, err } - return &b, nil + return b, nil } -func (b *sysfsBoard) Reconfigure( +// Reconfigure reconfigures the board with interrupt pins, spi and i2c, and analogs. +func (b *Board) Reconfigure( ctx context.Context, _ resource.Dependencies, conf resource.Config, ) error { + newConf, err := b.convertConfig(conf) + if err != nil { + return err + } + b.mu.Lock() defer b.mu.Unlock() - newConf, err := resource.NativeConfig[*Config](conf) - if err != nil { + if err := b.reconfigureGpios(newConf); err != nil { return err } - if err := b.reconfigureSpis(newConf); err != nil { return err } - if err := b.reconfigureI2cs(newConf); err != nil { return err } - if err := b.reconfigureAnalogs(ctx, newConf); err != nil { return err } - if err := b.reconfigureInterrupts(newConf); err != nil { return err } + return nil +} + +// This is a helper function used to reconfigure the GPIO pins. It looks for the key in the map +// whose value resembles the target pin definition. +func getMatchingPin(target GPIOBoardMapping, mapping map[string]GPIOBoardMapping) (string, bool) { + for name, def := range mapping { + if target == def { + return name, true + } + } + return "", false +} + +func (b *Board) reconfigureGpios(newConf *LinuxBoardConfig) error { + // First, find old pins that are no longer defined, and destroy them. + for oldName, mapping := range b.gpioMappings { + if _, ok := getMatchingPin(mapping, newConf.GpioMappings); ok { + continue // This pin is in the new mapping, so don't destroy it. + } + + // Otherwise, remove the pin because it's not in the new mapping. + if pin, ok := b.gpios[oldName]; ok { + if err := pin.Close(); err != nil { + return err + } + delete(b.gpios, oldName) + continue + } + + // If we get here, the old pin definition exists, but the old pin does not. Check if it's a + // digital interrupt. + if interrupt, ok := b.interrupts[oldName]; ok { + if err := interrupt.Close(); err != nil { + return err + } + delete(b.interrupts, oldName) + continue + } + + // If we get here, there is a logic bug somewhere. but failing to delete a nonexistent pin + // seemingly doesn't hurt anything, so just log the error and continue. + b.logger.Errorf("During reconfiguration, old pin '%s' should be destroyed, but "+ + "it doesn't exist!?", oldName) + } + + // Next, compare the new pin definitions to the old ones, to build up 2 sets: pins to rename, + // and new pins to create. Don't actually create any yet, in case you'd overwrite a pin that + // should be renamed out of the way first. + toRename := map[string]string{} // Maps old names for pins to new names + toCreate := map[string]GPIOBoardMapping{} + for newName, mapping := range newConf.GpioMappings { + if oldName, ok := getMatchingPin(mapping, b.gpioMappings); ok { + if oldName != newName { + toRename[oldName] = newName + } + } else { + toCreate[newName] = mapping + } + } + // Rename the ones whose name changed. The ordering here is tricky: if B should be renamed to C + // while A should be renamed to B, we need to make sure we don't overwrite B with A and then + // rename it to C. To avoid this, move all the pins to rename into a temporary data structure, + // then move them all back again afterward. + tempGpios := map[string]*gpioPin{} + tempInterrupts := map[string]*digitalInterrupt{} + for oldName, newName := range toRename { + if pin, ok := b.gpios[oldName]; ok { + tempGpios[newName] = pin + delete(b.gpios, oldName) + continue + } + + // If we get here, again check if the missing pin is a digital interrupt. + if interrupt, ok := b.interrupts[oldName]; ok { + tempInterrupts[newName] = interrupt + delete(b.interrupts, oldName) + continue + } + + return fmt.Errorf("during reconfiguration, old pin '%s' should be renamed to '%s', but "+ + "it doesn't exist!?", oldName, newName) + } + + // Now move all the pins back from the temporary data structures. + for newName, pin := range tempGpios { + b.gpios[newName] = pin + } + for newName, interrupt := range tempInterrupts { + b.interrupts[newName] = interrupt + } + + // Finally, create the new pins. + for newName, mapping := range toCreate { + b.gpios[newName] = b.createGpioPin(mapping) + } + + b.gpioMappings = newConf.GpioMappings return nil } // This never returns errors, but we give it the same function signature as the other // reconfiguration helpers for consistency. -func (b *sysfsBoard) reconfigureSpis(newConf *Config) error { +func (b *Board) reconfigureSpis(newConf *LinuxBoardConfig) error { stillExists := map[string]struct{}{} for _, c := range newConf.SPIs { stillExists[c.Name] = struct{}{} @@ -136,7 +227,7 @@ func (b *sysfsBoard) reconfigureSpis(newConf *Config) error { return nil } -func (b *sysfsBoard) reconfigureI2cs(newConf *Config) error { +func (b *Board) reconfigureI2cs(newConf *LinuxBoardConfig) error { stillExists := map[string]struct{}{} for _, c := range newConf.I2Cs { stillExists[c.Name] = struct{}{} @@ -171,7 +262,7 @@ func (b *sysfsBoard) reconfigureI2cs(newConf *Config) error { return nil } -func (b *sysfsBoard) reconfigureAnalogs(ctx context.Context, newConf *Config) error { +func (b *Board) reconfigureAnalogs(ctx context.Context, newConf *LinuxBoardConfig) error { stillExists := map[string]struct{}{} for _, c := range newConf.Analogs { channel, err := strconv.Atoi(c.Pin) @@ -209,9 +300,9 @@ func (b *sysfsBoard) reconfigureAnalogs(ctx context.Context, newConf *Config) er // This helper function is used while reconfiguring digital interrupts. It finds the new config (if // any) for a pre-existing digital interrupt. func findNewDigIntConfig( - interrupt *digitalInterrupt, newConf *Config, logger golog.Logger, + interrupt *digitalInterrupt, confs []board.DigitalInterruptConfig, logger golog.Logger, ) *board.DigitalInterruptConfig { - for _, newConfig := range newConf.DigitalInterrupts { + for _, newConfig := range confs { if newConfig.Pin == interrupt.config.Pin { return &newConfig } @@ -220,7 +311,7 @@ func findNewDigIntConfig( // This interrupt is named identically to its pin. It was probably created on the fly // by some other component (an encoder?). Unless there's now some other config with the // same name but on a different pin, keep it initialized as-is. - for _, intConfig := range newConf.DigitalInterrupts { + for _, intConfig := range confs { if intConfig.Name == interrupt.config.Name { // The name of this interrupt is defined in the new config, but on a different // pin. This interrupt should be closed. @@ -236,33 +327,23 @@ func findNewDigIntConfig( return nil } -func (b *sysfsBoard) reconfigureInterrupts(newConf *Config) error { - if b.usePeriphGpio { - if len(newConf.DigitalInterrupts) != 0 { - return errors.New("digital interrupts on Periph GPIO pins are not yet supported") - } - return nil // No digital interrupts to reconfigure. - } - - // If we get here, we need to reconfigure b.interrupts. Any pin that already exists in the - // right configuration should just be copied over; closing and re-opening it risks losing its - // state. +func (b *Board) reconfigureInterrupts(newConf *LinuxBoardConfig) error { + // Any pin that already exists in the right configuration should just be copied over; closing + // and re-opening it risks losing its state. newInterrupts := make(map[string]*digitalInterrupt, len(newConf.DigitalInterrupts)) // Reuse any old interrupts that have new configs for _, oldInterrupt := range b.interrupts { - if newConfig := findNewDigIntConfig(oldInterrupt, newConf, b.logger); newConfig == nil { + if newConfig := findNewDigIntConfig(oldInterrupt, newConf.DigitalInterrupts, b.logger); newConfig == nil { // The old interrupt shouldn't exist any more, but it probably became a GPIO pin. if err := oldInterrupt.Close(); err != nil { return err // This should never happen, but the linter worries anyway. } - if pinInt, err := strconv.Atoi(oldInterrupt.config.Pin); err == nil { - if newGpioConfig, ok := b.gpioMappings[pinInt]; ok { - // See gpio.go for createGpioPin. - b.gpios[oldInterrupt.config.Pin] = b.createGpioPin(newGpioConfig) - } + if newGpioConfig, ok := b.gpioMappings[oldInterrupt.config.Pin]; ok { + // See gpio.go for createGpioPin. + b.gpios[oldInterrupt.config.Pin] = b.createGpioPin(newGpioConfig) } else { - b.logger.Warnf("Unable to reinterpret old interrupt pin '%s' as GPIO, ignoring.", + b.logger.Warnf("Old interrupt pin was on nonexistent GPIO pin '%s', ignoring", oldInterrupt.config.Pin) } } else { // The old interrupt should stick around. @@ -356,18 +437,18 @@ func (a *wrappedAnalog) reset(ctx context.Context, chipSelect string, reader *bo a.chipSelect = chipSelect } -type sysfsBoard struct { +// Board implements a component for a Linux machine. +type Board struct { resource.Named - mu sync.RWMutex - gpioMappings map[int]GPIOBoardMapping + mu sync.RWMutex + convertConfig ConfigConverter + + gpioMappings map[string]GPIOBoardMapping spis map[string]*spiBus analogs map[string]*wrappedAnalog - pwms map[string]pwmSetting i2cs map[string]*I2cBus logger golog.Logger - usePeriphGpio bool - // These next two are only used for non-periph.io pins gpios map[string]*gpioPin interrupts map[string]*digitalInterrupt @@ -376,31 +457,26 @@ type sysfsBoard struct { activeBackgroundWorkers sync.WaitGroup } -type pwmSetting struct { - dutyCycle gpio.Duty - frequency physic.Frequency -} - -func (b *sysfsBoard) SPIByName(name string) (board.SPI, bool) { +// SPIByName returns the SPI by the given name if it exists. +func (b *Board) SPIByName(name string) (board.SPI, bool) { s, ok := b.spis[name] return s, ok } -func (b *sysfsBoard) I2CByName(name string) (board.I2C, bool) { +// I2CByName returns the i2c by the given name if it exists. +func (b *Board) I2CByName(name string) (board.I2C, bool) { i, ok := b.i2cs[name] return i, ok } -func (b *sysfsBoard) AnalogReaderByName(name string) (board.AnalogReader, bool) { +// AnalogReaderByName returns the analog reader by the given name if it exists. +func (b *Board) AnalogReaderByName(name string) (board.AnalogReader, bool) { a, ok := b.analogs[name] return a, ok } -func (b *sysfsBoard) DigitalInterruptByName(name string) (board.DigitalInterrupt, bool) { - if b.usePeriphGpio { - return nil, false // Digital interrupts aren't supported. - } - +// DigitalInterruptByName returns the interrupt by the given name if it exists. +func (b *Board) DigitalInterruptByName(name string) (board.DigitalInterrupt, bool) { b.mu.Lock() defer b.mu.Unlock() @@ -438,7 +514,8 @@ func (b *sysfsBoard) DigitalInterruptByName(name string) (board.DigitalInterrupt return interrupt.interrupt, true } -func (b *sysfsBoard) SPINames() []string { +// SPINames returns the names of all known SPIs. +func (b *Board) SPINames() []string { if len(b.spis) == 0 { return nil } @@ -449,7 +526,8 @@ func (b *sysfsBoard) SPINames() []string { return names } -func (b *sysfsBoard) I2CNames() []string { +// I2CNames returns the names of all known I2Cs. +func (b *Board) I2CNames() []string { if len(b.i2cs) == 0 { return nil } @@ -460,7 +538,8 @@ func (b *sysfsBoard) I2CNames() []string { return names } -func (b *sysfsBoard) AnalogReaderNames() []string { +// AnalogReaderNames returns the names of all known analog readers. +func (b *Board) AnalogReaderNames() []string { names := []string{} for k := range b.analogs { names = append(names, k) @@ -468,7 +547,8 @@ func (b *sysfsBoard) AnalogReaderNames() []string { return names } -func (b *sysfsBoard) DigitalInterruptNames() []string { +// DigitalInterruptNames returns the names of all known digital interrupts. +func (b *Board) DigitalInterruptNames() []string { if b.interrupts == nil { return nil } @@ -480,37 +560,25 @@ func (b *sysfsBoard) DigitalInterruptNames() []string { return names } -func (b *sysfsBoard) GPIOPinNames() []string { +// GPIOPinNames returns the names of all known GPIO pins. +func (b *Board) GPIOPinNames() []string { if b.gpioMappings == nil { return nil } names := []string{} for k := range b.gpioMappings { - names = append(names, fmt.Sprintf("%d", k)) + names = append(names, k) } return names } -func (b *sysfsBoard) getGPIOLine(hwPin string) (gpio.PinIO, error) { - pinName := hwPin - - pin := gpioreg.ByName(pinName) - if pin == nil { - return nil, errors.Errorf("no global pin found for %q", pinName) - } - return pin, nil -} - -func (b *sysfsBoard) GPIOPinByName(pinName string) (board.GPIOPin, error) { - if b.usePeriphGpio { - return b.periphGPIOPinByName(pinName) - } - // Otherwise, the pins are stored in b.gpios. +// GPIOPinByName returns a GPIOPin by name. +func (b *Board) GPIOPinByName(pinName string) (board.GPIOPin, error) { if pin, ok := b.gpios[pinName]; ok { return pin, nil } - // check if pin is a digital interrupt + // Check if pin is a digital interrupt: those can still be used as inputs. if interrupt, interruptOk := b.interrupts[pinName]; interruptOk { return &gpioInterruptWrapperPin{*interrupt}, nil } @@ -518,82 +586,34 @@ func (b *sysfsBoard) GPIOPinByName(pinName string) (board.GPIOPin, error) { return nil, errors.Errorf("cannot find GPIO for unknown pin: %s", pinName) } -func (b *sysfsBoard) periphGPIOPinByName(pinName string) (board.GPIOPin, error) { - pin, err := b.getGPIOLine(pinName) - hwPWMSupported := false - if err != nil { - return nil, err - } - - return periphGpioPin{b, pin, pinName, hwPWMSupported}, nil -} - -// expects to already have lock acquired. -func (b *sysfsBoard) startSoftwarePWMLoop(gp periphGpioPin) { - b.activeBackgroundWorkers.Add(1) - goutils.ManagedGo(func() { - b.softwarePWMLoop(b.cancelCtx, gp) - }, b.activeBackgroundWorkers.Done) +// Status returns the current status of the board. +func (b *Board) Status(ctx context.Context, extra map[string]interface{}) (*commonpb.BoardStatus, error) { + return board.CreateStatus(ctx, b, extra) } -func (b *sysfsBoard) softwarePWMLoop(ctx context.Context, gp periphGpioPin) { - for { - cont := func() bool { - b.mu.RLock() - defer b.mu.RUnlock() - pwmSetting, ok := b.pwms[gp.pinName] - if !ok { - b.logger.Debug("pwm setting deleted; stopping") - return false - } - - if err := gp.set(true); err != nil { - b.logger.Errorw("error setting pin", "pin_name", gp.pinName, "error", err) - return true - } - onPeriod := time.Duration( - int64((float64(pwmSetting.dutyCycle) / float64(gpio.DutyMax)) * float64(pwmSetting.frequency.Period())), - ) - if !goutils.SelectContextOrWait(ctx, onPeriod) { - return false - } - if err := gp.set(false); err != nil { - b.logger.Errorw("error setting pin", "pin_name", gp.pinName, "error", err) - return true - } - offPeriod := pwmSetting.frequency.Period() - onPeriod - - return goutils.SelectContextOrWait(ctx, offPeriod) - }() - if !cont { - return - } - } -} - -func (b *sysfsBoard) Status(ctx context.Context, extra map[string]interface{}) (*commonpb.BoardStatus, error) { - return &commonpb.BoardStatus{}, nil -} - -func (b *sysfsBoard) ModelAttributes() board.ModelAttributes { +// ModelAttributes returns attributes related to the model of this board. +func (b *Board) ModelAttributes() board.ModelAttributes { return board.ModelAttributes{} } -func (b *sysfsBoard) SetPowerMode(ctx context.Context, mode pb.PowerMode, duration *time.Duration) error { +// SetPowerMode sets the board to the given power mode. If provided, +// the board will exit the given power mode after the specified +// duration. +func (b *Board) SetPowerMode( + ctx context.Context, + mode pb.PowerMode, + duration *time.Duration, +) error { return grpc.UnimplementedError } -func (b *sysfsBoard) Close(ctx context.Context) error { +// Close attempts to cleanly close each part of the board. +func (b *Board) Close(ctx context.Context) error { b.mu.Lock() b.cancelFunc() b.mu.Unlock() b.activeBackgroundWorkers.Wait() - // For non-Periph boards, shut down all our open pins so we don't leak file descriptors - if b.usePeriphGpio { - return nil - } - var err error for _, pin := range b.gpios { err = multierr.Combine(err, pin.Close()) diff --git a/components/board/genericlinux/board_nonlinux.go b/components/board/genericlinux/board_nonlinux.go index 869d4843ee2..ede0a28bb6b 100644 --- a/components/board/genericlinux/board_nonlinux.go +++ b/components/board/genericlinux/board_nonlinux.go @@ -16,7 +16,7 @@ import ( // RegisterBoard would register a sysfs based board of the given model. However, this one never // creates a board, and instead returns errors about making a Linux board on a non-Linux OS. -func RegisterBoard(modelName string, gpioMappings map[int]GPIOBoardMapping, usePeriphGpio bool) { +func RegisterBoard(modelName string, gpioMappings map[string]GPIOBoardMapping) { resource.RegisterComponent( board.API, resource.DefaultModelFamily.WithModel(modelName), @@ -33,6 +33,6 @@ func RegisterBoard(modelName string, gpioMappings map[int]GPIOBoardMapping, useP } // GetGPIOBoardMappings attempts to find a compatible GPIOBoardMapping for the given board. -func GetGPIOBoardMappings(modelName string, boardInfoMappings map[string]BoardInformation) (map[int]GPIOBoardMapping, error) { +func GetGPIOBoardMappings(modelName string, boardInfoMappings map[string]BoardInformation) (map[string]GPIOBoardMapping, error) { return nil, errors.New("linux boards are not supported on non-linux OSes") } diff --git a/components/board/genericlinux/board_test.go b/components/board/genericlinux/board_test.go index c0abd801ad2..a11a2cf29a1 100644 --- a/components/board/genericlinux/board_test.go +++ b/components/board/genericlinux/board_test.go @@ -9,33 +9,24 @@ package genericlinux import ( "context" "testing" - "time" "github.com/edaniels/golog" - commonpb "go.viam.com/api/common/v1" "go.viam.com/test" - "periph.io/x/conn/v3/gpio/gpiotest" "go.viam.com/rdk/components/board" ) -func TestRegisterBoard(t *testing.T) { - RegisterBoard("test", map[int]GPIOBoardMapping{}, true) -} - func TestGenericLinux(t *testing.T) { ctx := context.Background() - gp1 := &periphGpioPin{b: &sysfsBoard{ + b := &Board{ logger: golog.NewTestLogger(t), - }} + } t.Run("test empty sysfs board", func(t *testing.T) { - test.That(t, gp1.b.GPIOPinNames(), test.ShouldBeNil) - test.That(t, gp1.b.SPINames(), test.ShouldBeNil) - _, err := gp1.PWM(ctx, nil) - test.That(t, err, test.ShouldNotBeNil) - _, err = gp1.b.GPIOPinByName("10") + test.That(t, b.GPIOPinNames(), test.ShouldBeNil) + test.That(t, b.SPINames(), test.ShouldBeNil) + _, err := b.GPIOPinByName("10") test.That(t, err, test.ShouldNotBeNil) }) @@ -54,103 +45,65 @@ func TestGenericLinux(t *testing.T) { boardSPIs["open"].bus.Store(&twoStr) boardSPIs["open"].openHandle.bus.bus.Store(&twoStr) - gp2 := &periphGpioPin{ - b: &sysfsBoard{ - Named: board.Named("foo").AsNamed(), - gpioMappings: nil, - spis: boardSPIs, - analogs: map[string]*wrappedAnalog{"an": {}}, - pwms: map[string]pwmSetting{ - "10": {dutyCycle: 1, frequency: 1}, - }, - logger: golog.NewTestLogger(t), - cancelCtx: ctx, - cancelFunc: func() { - }, + b = &Board{ + Named: board.Named("foo").AsNamed(), + gpioMappings: nil, + spis: boardSPIs, + analogs: map[string]*wrappedAnalog{"an": {}}, + logger: golog.NewTestLogger(t), + cancelCtx: ctx, + cancelFunc: func() { }, - pinName: "10", - pin: &gpiotest.Pin{N: "10", Num: 10}, - hwPWMSupported: false, } t.Run("test analogs spis i2cs digital-interrupts and gpio names", func(t *testing.T) { - ans := gp2.b.AnalogReaderNames() + ans := b.AnalogReaderNames() test.That(t, ans, test.ShouldResemble, []string{"an"}) - an1, ok := gp2.b.AnalogReaderByName("an") + an1, ok := b.AnalogReaderByName("an") test.That(t, an1, test.ShouldHaveSameTypeAs, &wrappedAnalog{}) test.That(t, ok, test.ShouldBeTrue) - an2, ok := gp2.b.AnalogReaderByName("missing") + an2, ok := b.AnalogReaderByName("missing") test.That(t, an2, test.ShouldBeNil) test.That(t, ok, test.ShouldBeFalse) - sns := gp2.b.SPINames() + sns := b.SPINames() test.That(t, len(sns), test.ShouldEqual, 2) - sn1, ok := gp2.b.SPIByName("closed") + sn1, ok := b.SPIByName("closed") test.That(t, sn1, test.ShouldHaveSameTypeAs, &spiBus{}) test.That(t, ok, test.ShouldBeTrue) - sn2, ok := gp2.b.SPIByName("missing") + sn2, ok := b.SPIByName("missing") test.That(t, sn2, test.ShouldBeNil) test.That(t, ok, test.ShouldBeFalse) - ins := gp2.b.I2CNames() + ins := b.I2CNames() test.That(t, ins, test.ShouldBeNil) - in1, ok := gp2.b.I2CByName("in") + in1, ok := b.I2CByName("in") test.That(t, in1, test.ShouldBeNil) test.That(t, ok, test.ShouldBeFalse) - dns := gp2.b.DigitalInterruptNames() + dns := b.DigitalInterruptNames() test.That(t, dns, test.ShouldBeNil) - dn1, ok := gp2.b.DigitalInterruptByName("dn") + dn1, ok := b.DigitalInterruptByName("dn") test.That(t, dn1, test.ShouldBeNil) test.That(t, ok, test.ShouldBeFalse) - gns := gp2.b.GPIOPinNames() + gns := b.GPIOPinNames() test.That(t, gns, test.ShouldResemble, []string(nil)) - gn1, err := gp2.b.GPIOPinByName("10") + gn1, err := b.GPIOPinByName("10") test.That(t, err, test.ShouldNotBeNil) test.That(t, gn1, test.ShouldBeNil) }) - t.Run("test genericlinux gpio pin functionality", func(t *testing.T) { - err := gp2.SetPWM(ctx, 50, nil) - test.That(t, err, test.ShouldBeNil) - - err = gp2.SetPWMFreq(ctx, 1000, nil) - test.That(t, err, test.ShouldBeNil) - - freq, err := gp2.PWMFreq(ctx, nil) - test.That(t, err, test.ShouldBeNil) - test.That(t, freq, test.ShouldEqual, 1000) - - duty, err := gp2.PWM(ctx, nil) - test.That(t, err, test.ShouldBeNil) - test.That(t, duty, test.ShouldEqual, 50) - - err = gp2.Set(ctx, true, nil) - test.That(t, err, test.ShouldBeNil) - - high, err := gp2.Get(ctx, nil) - test.That(t, err, test.ShouldBeNil) - test.That(t, high, test.ShouldBeTrue) - - bs, err := gp2.b.Status(ctx, nil) - test.That(t, err, test.ShouldBeNil) - test.That(t, bs, test.ShouldResemble, &commonpb.BoardStatus{}) - - bma := gp2.b.ModelAttributes() - test.That(t, bma, test.ShouldResemble, board.ModelAttributes{}) - }) - t.Run("test spi functionality", func(t *testing.T) { - spi1 := gp2.b.spis["closed"] - spi2 := gp2.b.spis["open"] + spi1 := b.spis["closed"] + spi2 := b.spis["open"] sph1, err := spi1.OpenHandle() test.That(t, sph1, test.ShouldHaveSameTypeAs, &spiHandle{}) test.That(t, err, test.ShouldBeNil) @@ -164,24 +117,6 @@ func TestGenericLinux(t *testing.T) { test.That(t, err.Error(), test.ShouldContainSubstring, "closed") test.That(t, rx, test.ShouldBeNil) }) - - t.Run("test software pwm loop", func(t *testing.T) { - newCtx, cancel := context.WithTimeout(ctx, time.Duration(10)) - defer cancel() - gp2.b.softwarePWMLoop(newCtx, *gp2) - - gp2.b.pwms = map[string]pwmSetting{ - "10": {dutyCycle: 1, frequency: 1}, - } - gp2.b.startSoftwarePWMLoop(*gp2) - - gp2.b.softwarePWMLoop(newCtx, *gp2) - }) - - t.Run("test getGPIOLine", func(t *testing.T) { - _, err := gp2.b.getGPIOLine("10") - test.That(t, err.Error(), test.ShouldContainSubstring, "no global pin") - }) } func TestConfigValidate(t *testing.T) { diff --git a/components/board/genericlinux/config.go b/components/board/genericlinux/config.go index efea0114c7d..60b5a4691c7 100644 --- a/components/board/genericlinux/config.go +++ b/components/board/genericlinux/config.go @@ -4,6 +4,7 @@ import ( "fmt" "go.viam.com/rdk/components/board" + "go.viam.com/rdk/resource" "go.viam.com/rdk/utils" ) @@ -40,3 +41,45 @@ func (conf *Config) Validate(path string) ([]string, error) { } return nil, nil } + +// LinuxBoardConfig is a struct containing absolutely everything a genericlinux board might need +// configured. It is a union of the configs for the customlinux boards and the genericlinux boards +// with static pin definitions, because those components all use the same underlying code but have +// different config types (e.g., only genericlinux has named I2C and SPI buses, while only +// customlinux can change its pin definitions during reconfiguration). The LinuxBoardConfig struct +// is a unification of the two of them. Whenever we go through reconfiguration, we convert the +// provided config into this type, and then reconfigure based on this. +type LinuxBoardConfig struct { + I2Cs []board.I2CConfig + SPIs []board.SPIConfig + Analogs []board.AnalogConfig + DigitalInterrupts []board.DigitalInterruptConfig + GpioMappings map[string]GPIOBoardMapping +} + +// ConfigConverter is a type synonym for a function to turn whatever config we get during +// reconfiguration into a LinuxBoardConfig, so that we can reconfigure based on that. We return a +// pointer to a LinuxBoardConfig instead of the struct itself so that we can return nil if we +// encounter an error. +type ConfigConverter = func(resource.Config) (*LinuxBoardConfig, error) + +// ConstPinDefs takes in a map from pin names to GPIOBoardMapping structs, and returns a +// ConfigConverter that will use these pin definitions in the underlying config. It is intended to +// be used for board components whose pin definitions are built into the RDK, such as the +// BeagleBone or Jetson boards. +func ConstPinDefs(gpioMappings map[string]GPIOBoardMapping) ConfigConverter { + return func(conf resource.Config) (*LinuxBoardConfig, error) { + newConf, err := resource.NativeConfig[*Config](conf) + if err != nil { + return nil, err + } + + return &LinuxBoardConfig{ + I2Cs: newConf.I2Cs, + SPIs: newConf.SPIs, + Analogs: newConf.Analogs, + DigitalInterrupts: newConf.DigitalInterrupts, + GpioMappings: gpioMappings, + }, nil + } +} diff --git a/components/board/genericlinux/data.go b/components/board/genericlinux/data.go index 7d42e1638a1..9fd60943bac 100644 --- a/components/board/genericlinux/data.go +++ b/components/board/genericlinux/data.go @@ -10,7 +10,6 @@ import ( "strings" "github.com/edaniels/golog" - "github.com/mkch/gpio" rdkutils "go.viam.com/rdk/utils" ) @@ -29,16 +28,17 @@ type pwmChipData struct { } // GetGPIOBoardMappings attempts to find a compatible GPIOBoardMapping for the given board. -func GetGPIOBoardMappings(modelName string, boardInfoMappings map[string]BoardInformation) (map[int]GPIOBoardMapping, error) { +func GetGPIOBoardMappings(modelName string, boardInfoMappings map[string]BoardInformation) (map[string]GPIOBoardMapping, error) { pinDefs, err := getCompatiblePinDefs(modelName, boardInfoMappings) if err != nil { return nil, err } - gpioChipsInfo, err := getGpioChipDefs(pinDefs) - if err != nil { - return nil, err - } + return GetGPIOBoardMappingFromPinDefs(pinDefs) +} + +// GetGPIOBoardMappingFromPinDefs attempts to find a compatible board-pin mapping using the pin definitions. +func GetGPIOBoardMappingFromPinDefs(pinDefs []PinDefinition) (map[string]GPIOBoardMapping, error) { pwmChipsInfo, err := getPwmChipDefs(pinDefs) if err != nil { // Try continuing on without hardware PWM support. Many boards do not have it enabled by @@ -47,8 +47,7 @@ func GetGPIOBoardMappings(modelName string, boardInfoMappings map[string]BoardIn pwmChipsInfo = map[string]pwmChipData{} } - mapping, err := getBoardMapping(pinDefs, gpioChipsInfo, pwmChipsInfo) - return mapping, err + return getBoardMapping(pinDefs, pwmChipsInfo) } // getCompatiblePinDefs returns a list of pin definitions, from the first BoardInformation struct @@ -86,60 +85,15 @@ func readIntFile(filePath string) (int, error) { return int(resultInt64), err } -// getGpioChipDefs returns map of chip ngpio# to the corresponding gpio chip name. -func getGpioChipDefs(pinDefs []PinDefinition) (map[int]string, error) { - allDevices := gpio.ChipDevices() - ngpioToChipName := make(map[int]string, len(allDevices)) // maps chipNgpio -> string gpiochip# - for _, dev := range allDevices { - chip, err := gpio.OpenChip(dev) - if err != nil { - return nil, err - } - - chipInfo, err := chip.Info() - if err != nil { - return nil, err - } - - // should not have two chips with same ngpio # - if _, ok := ngpioToChipName[int(chipInfo.NumLines)]; ok { - golog.Global().Errorf("Board has multiple GPIO chips with the same ngpio value, %d!", chipInfo.NumLines) - } - ngpioToChipName[int(chipInfo.NumLines)] = chipInfo.Name - } - - expectedNgpios := make(map[int]struct{}, len(pinDefs)) - for _, pinDef := range pinDefs { - for n := range pinDef.GPIOChipRelativeIDs { - expectedNgpios[n] = struct{}{} // get a "set" of all ngpio numbers on the board - } - } - - gpioChipsInfo := map[int]string{} - // for each chip in the board config, find the right gpioChip dir - for chipNgpio := range expectedNgpios { - dir, ok := ngpioToChipName[chipNgpio] - - if !ok { - return nil, fmt.Errorf("unknown GPIO device with ngpio %d", - chipNgpio) - } - - gpioChipsInfo[chipNgpio] = dir - } - - return gpioChipsInfo, nil -} - func getPwmChipDefs(pinDefs []PinDefinition) (map[string]pwmChipData, error) { // First, collect the names of all relevant PWM chips with duplicates removed. Go doesn't have // native set objects, so we use a map whose values are ignored. pwmChipNames := make(map[string]struct{}, len(pinDefs)) for _, pinDef := range pinDefs { - if pinDef.PWMChipSysFSDir == "" { + if pinDef.PwmChipSysfsDir == "" { continue } - pwmChipNames[pinDef.PWMChipSysFSDir] = struct{}{} + pwmChipNames[pinDef.PwmChipSysfsDir] = struct{}{} } // Now, look for all chips whose names we found. @@ -185,58 +139,38 @@ func getPwmChipDefs(pinDefs []PinDefinition) (map[string]pwmChipData, error) { return pwmChipsInfo, nil } -func getBoardMapping(pinDefs []PinDefinition, gpioChipsInfo map[int]string, - pwmChipsInfo map[string]pwmChipData, -) (map[int]GPIOBoardMapping, error) { - data := make(map[int]GPIOBoardMapping, len(pinDefs)) +func getBoardMapping(pinDefs []PinDefinition, pwmChipsInfo map[string]pwmChipData, +) (map[string]GPIOBoardMapping, error) { + data := make(map[string]GPIOBoardMapping, len(pinDefs)) // For "use" on pins that don't have hardware PWMs dummyPwmInfo := pwmChipData{Dir: "", Npwm: -1} for _, pinDef := range pinDefs { - key := pinDef.PinNumberBoard - - var ngpio int - for n := range pinDef.GPIOChipRelativeIDs { - ngpio = n - break // each gpio pin should only be associated with one gpiochip in the config - } - - gpioChipDir, ok := gpioChipsInfo[ngpio] - if !ok { - return nil, fmt.Errorf("unknown GPIO device for chip with ngpio %d, pin %d", - ngpio, key) - } - - pwmChipInfo, ok := pwmChipsInfo[pinDef.PWMChipSysFSDir] + pwmChipInfo, ok := pwmChipsInfo[pinDef.PwmChipSysfsDir] if ok { - if pinDef.PWMID >= pwmChipInfo.Npwm { - return nil, fmt.Errorf("too high PWM ID %d for pin %d (npwm is %d for chip %s)", - pinDef.PWMID, key, pwmChipInfo.Npwm, pinDef.PWMChipSysFSDir) + if pinDef.PwmID >= pwmChipInfo.Npwm { + return nil, fmt.Errorf("too high PWM ID %d for pin %s (npwm is %d for chip %s)", + pinDef.PwmID, pinDef.Name, pwmChipInfo.Npwm, pinDef.PwmChipSysfsDir) } } else { - if pinDef.PWMChipSysFSDir == "" { + if pinDef.PwmChipSysfsDir == "" { // This pin isn't supposed to have hardware PWM support; all is well. pwmChipInfo = dummyPwmInfo } else { golog.Global().Errorw( - "cannot find expected hardware PWM chip, continuing without it", "pin", key) + "cannot find expected hardware PWM chip, continuing without it", "pin", pinDef.Name) pwmChipInfo = dummyPwmInfo } } - chipRelativeID, ok := pinDef.GPIOChipRelativeIDs[ngpio] - if !ok { - chipRelativeID = pinDef.GPIOChipRelativeIDs[-1] - } - - data[key] = GPIOBoardMapping{ - GPIOChipDev: gpioChipDir, - GPIO: chipRelativeID, - GPIOName: pinDef.PinNameCVM, + data[pinDef.Name] = GPIOBoardMapping{ + GPIOChipDev: pinDef.DeviceName, + GPIO: pinDef.LineNumber, + GPIOName: pinDef.Name, PWMSysFsDir: pwmChipInfo.Dir, - PWMID: pinDef.PWMID, - HWPWMSupported: pinDef.PWMID != -1, + PWMID: pinDef.PwmID, + HWPWMSupported: pinDef.PwmID != -1, } } return data, nil diff --git a/components/board/genericlinux/digital_interrupts.go b/components/board/genericlinux/digital_interrupts.go index 3ec315d1dc9..83e9359d803 100644 --- a/components/board/genericlinux/digital_interrupts.go +++ b/components/board/genericlinux/digital_interrupts.go @@ -6,7 +6,7 @@ package genericlinux import ( "context" - "strconv" + "sync" "github.com/mkch/gpio" "github.com/pkg/errors" @@ -17,28 +17,24 @@ import ( ) type digitalInterrupt struct { - parentBoard *sysfsBoard - interrupt board.ReconfigurableDigitalInterrupt - line *gpio.LineWithEvent - cancelCtx context.Context - cancelFunc func() - config *board.DigitalInterruptConfig + boardWorkers *sync.WaitGroup + interrupt board.ReconfigurableDigitalInterrupt + line *gpio.LineWithEvent + cancelCtx context.Context + cancelFunc func() + config *board.DigitalInterruptConfig } -func (b *sysfsBoard) createDigitalInterrupt( +func (b *Board) createDigitalInterrupt( ctx context.Context, config board.DigitalInterruptConfig, - gpioMappings map[int]GPIOBoardMapping, + gpioMappings map[string]GPIOBoardMapping, // If we are reconfiguring a board, we might already have channels subscribed and listening for // updates from an old interrupt that we're creating on a new pin. In that case, reuse the part // that holds the callbacks. oldCallbackHolder board.ReconfigurableDigitalInterrupt, ) (*digitalInterrupt, error) { - pinInt, err := strconv.Atoi(config.Pin) - if err != nil { - return nil, errors.Errorf("pin numbers must be numerical, not '%s'", config.Pin) - } - mapping, ok := gpioMappings[pinInt] + mapping, ok := gpioMappings[config.Pin] if !ok { return nil, errors.Errorf("unknown interrupt pin %s", config.Pin) } @@ -70,19 +66,19 @@ func (b *sysfsBoard) createDigitalInterrupt( cancelCtx, cancelFunc := context.WithCancel(ctx) result := digitalInterrupt{ - parentBoard: b, - interrupt: interrupt, - line: line, - cancelCtx: cancelCtx, - cancelFunc: cancelFunc, - config: &config, + boardWorkers: &b.activeBackgroundWorkers, + interrupt: interrupt, + line: line, + cancelCtx: cancelCtx, + cancelFunc: cancelFunc, + config: &config, } result.startMonitor() return &result, nil } func (di *digitalInterrupt) startMonitor() { - di.parentBoard.activeBackgroundWorkers.Add(1) + di.boardWorkers.Add(1) utils.ManagedGo(func() { for { select { @@ -93,7 +89,7 @@ func (di *digitalInterrupt) startMonitor() { di.cancelCtx, event.RisingEdge, uint64(event.Time.UnixNano()))) } } - }, di.parentBoard.activeBackgroundWorkers.Done) + }, di.boardWorkers.Done) } func (di *digitalInterrupt) Close() error { diff --git a/components/board/genericlinux/gpio.go b/components/board/genericlinux/gpio.go index 7364c2b873c..005937017ce 100644 --- a/components/board/genericlinux/gpio.go +++ b/components/board/genericlinux/gpio.go @@ -15,7 +15,7 @@ import ( ) type gpioPin struct { - parentBoard *sysfsBoard + boardWorkers *sync.WaitGroup // These values should both be considered immutable. devicePath string @@ -180,8 +180,8 @@ func (pin *gpioPin) startSoftwarePWM() error { } pin.swPwmRunning = true - pin.parentBoard.activeBackgroundWorkers.Add(1) - utils.ManagedGo(pin.softwarePwmLoop, pin.parentBoard.activeBackgroundWorkers.Done) + pin.boardWorkers.Add(1) + utils.ManagedGo(pin.softwarePwmLoop, pin.boardWorkers.Done) return nil } @@ -283,13 +283,13 @@ func (pin *gpioPin) Close() error { return pin.closeGpioFd() } -func (b *sysfsBoard) createGpioPin(mapping GPIOBoardMapping) *gpioPin { +func (b *Board) createGpioPin(mapping GPIOBoardMapping) *gpioPin { pin := gpioPin{ - parentBoard: b, - devicePath: mapping.GPIOChipDev, - offset: uint32(mapping.GPIO), - cancelCtx: b.cancelCtx, - logger: b.logger, + boardWorkers: &b.activeBackgroundWorkers, + devicePath: mapping.GPIOChipDev, + offset: uint32(mapping.GPIO), + cancelCtx: b.cancelCtx, + logger: b.logger, } if mapping.HWPWMSupported { pin.hwPwm = newPwmDevice(mapping.PWMSysFsDir, mapping.PWMID, b.logger) diff --git a/components/board/genericlinux/hw_pwm.go b/components/board/genericlinux/hw_pwm.go index 35e40640469..df568fd5b8a 100644 --- a/components/board/genericlinux/hw_pwm.go +++ b/components/board/genericlinux/hw_pwm.go @@ -9,6 +9,7 @@ import ( "fmt" "os" "sync" + "time" "github.com/edaniels/golog" "github.com/pkg/errors" @@ -87,6 +88,11 @@ func (pwm *pwmDevice) unexport() error { // device results in an error. We don't care if there's an error: it should be disabled no // matter what. goutils.UncheckedError(pwm.disable()) + + // On boards like the Odroid C4, there is a race condition in the kernel where, if you unexport + // the pin too quickly after changing something else about it (e.g., disabling it), the whole + // PWM system gets corrupted. Sleep for a small amount of time to avoid this. + time.Sleep(time.Microsecond) if err := pwm.writeChip("unexport", uint64(pwm.line)); err != nil { return err } diff --git a/components/board/genericlinux/i2c.go b/components/board/genericlinux/i2c.go index f23feed7a55..363cf24c68e 100644 --- a/components/board/genericlinux/i2c.go +++ b/components/board/genericlinux/i2c.go @@ -45,11 +45,17 @@ func NewI2cBus(deviceName string) (*I2cBus, error) { } func (bus *I2cBus) reset(deviceName string) error { - newBus, err := i2creg.Open(deviceName) - if err != nil { - return err + bus.mu.Lock() + defer bus.mu.Unlock() + + if bus.closeableBus != nil { // Close any old bus we used to have + if err := bus.closeableBus.Close(); err != nil { + return err + } + bus.closeableBus = nil } - bus.closeableBus = newBus + + bus.deviceName = deviceName return nil } @@ -57,7 +63,18 @@ func (bus *I2cBus) reset(deviceName string) error { // communicating with a device at a specific I2C handle. Opening a handle locks the I2C bus so // nothing else can use it, and closing the handle unlocks the bus again. func (bus *I2cBus) OpenHandle(addr byte) (board.I2CHandle, error) { - bus.mu.Lock() // Lock the bus so no other handle can use it until this one is closed. + bus.mu.Lock() // Lock the bus so no other handle can use it until this handle is closed. + + // If we haven't yet connected to the bus itself, do so now. + if bus.closeableBus == nil { + newBus, err := i2creg.Open(bus.deviceName) + if err != nil { + bus.mu.Unlock() // We never created a handle, so unlock the bus for next time. + return nil, err + } + bus.closeableBus = newBus + } + return &I2cHandle{device: &i2c.Dev{Bus: bus.closeableBus, Addr: uint16(addr)}, parentBus: bus}, nil } diff --git a/components/board/genericlinux/periph_gpio.go b/components/board/genericlinux/periph_gpio.go deleted file mode 100644 index 705326efb3a..00000000000 --- a/components/board/genericlinux/periph_gpio.go +++ /dev/null @@ -1,114 +0,0 @@ -//go:build linux - -package genericlinux - -import ( - "context" - "fmt" - - "github.com/pkg/errors" - "periph.io/x/conn/v3/gpio" - "periph.io/x/conn/v3/physic" -) - -type periphGpioPin struct { - b *sysfsBoard - pin gpio.PinIO - pinName string - hwPWMSupported bool -} - -func (gp periphGpioPin) Set(ctx context.Context, high bool, extra map[string]interface{}) error { - gp.b.mu.Lock() - defer gp.b.mu.Unlock() - - delete(gp.b.pwms, gp.pinName) - - return gp.set(high) -} - -// This function is separate from Set(), above, because this one does not remove the pin from the -// board's pwms map. When simulating PWM in software, we use this function to turn the pin on and -// off while continuing to treat it as a PWM pin. -func (gp periphGpioPin) set(high bool) error { - l := gpio.Low - if high { - l = gpio.High - } - return gp.pin.Out(l) -} - -func (gp periphGpioPin) Get(ctx context.Context, extra map[string]interface{}) (bool, error) { - return gp.pin.Read() == gpio.High, nil -} - -func (gp periphGpioPin) PWM(ctx context.Context, extra map[string]interface{}) (float64, error) { - gp.b.mu.RLock() - defer gp.b.mu.RUnlock() - - pwm, ok := gp.b.pwms[gp.pinName] - if !ok { - return 0, fmt.Errorf("missing pin %s", gp.pinName) - } - return float64(pwm.dutyCycle) / float64(gpio.DutyMax), nil -} - -func (gp periphGpioPin) SetPWM(ctx context.Context, dutyCyclePct float64, extra map[string]interface{}) error { - gp.b.mu.Lock() - defer gp.b.mu.Unlock() - - last, alreadySet := gp.b.pwms[gp.pinName] - var freqHz physic.Frequency - if last.frequency != 0 { - freqHz = last.frequency - } - duty := gpio.Duty(dutyCyclePct * float64(gpio.DutyMax)) - last.dutyCycle = duty - gp.b.pwms[gp.pinName] = last - - if gp.hwPWMSupported { - err := gp.pin.PWM(duty, freqHz) - // TODO: [RSDK-569] (rh) find or implement a PWM sysfs that works with hardware pwm mappings - // periph.io does not implement PWM - if err != nil { - return errors.New("sysfs PWM not currently supported, use another pin for software PWM loops") - } - } - - if !alreadySet { - gp.b.startSoftwarePWMLoop(gp) - } - - return nil -} - -func (gp periphGpioPin) PWMFreq(ctx context.Context, extra map[string]interface{}) (uint, error) { - gp.b.mu.RLock() - defer gp.b.mu.RUnlock() - - return uint(gp.b.pwms[gp.pinName].frequency / physic.Hertz), nil -} - -func (gp periphGpioPin) SetPWMFreq(ctx context.Context, freqHz uint, extra map[string]interface{}) error { - gp.b.mu.Lock() - defer gp.b.mu.Unlock() - - last, alreadySet := gp.b.pwms[gp.pinName] - var duty gpio.Duty - if last.dutyCycle != 0 { - duty = last.dutyCycle - } - frequency := physic.Hertz * physic.Frequency(freqHz) - last.frequency = frequency - gp.b.pwms[gp.pinName] = last - - if gp.hwPWMSupported { - return gp.pin.PWM(duty, frequency) - } - - if !alreadySet { - gp.b.startSoftwarePWMLoop(gp) - } - - return nil -} diff --git a/components/board/genericlinux/pin_types.go b/components/board/genericlinux/pin_types.go index 6e36e020d5b..6bc4b6e1f69 100644 --- a/components/board/genericlinux/pin_types.go +++ b/components/board/genericlinux/pin_types.go @@ -1,6 +1,12 @@ package genericlinux -import "fmt" +import ( + "encoding/json" + "fmt" + + "github.com/pkg/errors" + "go.viam.com/utils" +) // GPIOBoardMapping represents a GPIO pin's location locally within a GPIO chip // and globally within sysfs. @@ -13,18 +19,58 @@ type GPIOBoardMapping struct { HWPWMSupported bool } -// PinDefinition describes board specific information on how a particular pin can be accessed -// via sysfs along with information about its PWM capabilities. +// PinDefinition describes a gpio pin on a linux board. type PinDefinition struct { - GPIOChipRelativeIDs map[int]int // ngpio -> relative id - GPIONames map[int]string // e.g. ngpio=169=PQ.06 for claraAGXXavier - GPIOChipSysFSDir string - PinNumberBoard int - PinNumberBCM int - PinNameCVM string - PinNameTegraSOC string - PWMChipSysFSDir string // empty for none - PWMID int // -1 for none + Name string `json:"name"` + DeviceName string `json:"device_name"` // name of the pin's chip's device, within /dev + LineNumber int `json:"line_number"` // relative line number on chip + PwmChipSysfsDir string `json:"pwm_chip_sysfs_dir,omitempty"` + PwmID int `json:"pwm_id,omitempty"` +} + +// PinDefinitions describes a list of pins on a linux board. +type PinDefinitions struct { + Pins []PinDefinition `json:"pins"` +} + +// UnmarshalJSON handles setting defaults for pin configs. +// Int values default to -1. +func (conf *PinDefinition) UnmarshalJSON(text []byte) error { + type TempPin PinDefinition // needed to prevent infinite recursive calls to UnmarshalJSON + aux := TempPin{ + LineNumber: -1, + PwmID: -1, + } + if err := json.Unmarshal(text, &aux); err != nil { + return err + } + *conf = PinDefinition(aux) + return nil +} + +// Validate ensures all parts of the config are valid. +func (conf *PinDefinition) Validate(path string) error { + if conf.Name == "" { + return utils.NewConfigValidationFieldRequiredError(path, "name") + } + + if conf.DeviceName == "" { + return utils.NewConfigValidationFieldRequiredError(path, "device_name") + } + + if conf.LineNumber == -1 { + return utils.NewConfigValidationFieldRequiredError(path, "line_number") + } + + if conf.LineNumber < 0 { + return utils.NewConfigValidationError(path, errors.New("line_number on gpio chip must be at least zero")) + } + + if conf.PwmChipSysfsDir != "" && conf.PwmID == -1 { + return utils.NewConfigValidationError(path, errors.New("must supply pwm_id for the pwm chip")) + } + + return nil } // BoardInformation details pin definitions and device compatibility for a particular board. diff --git a/components/board/hat/pca9685/pca9685.go b/components/board/hat/pca9685/pca9685.go index 277e416bb22..4425614a980 100644 --- a/components/board/hat/pca9685/pca9685.go +++ b/components/board/hat/pca9685/pca9685.go @@ -65,7 +65,7 @@ func init() { conf resource.Config, logger golog.Logger, ) (board.Board, error) { - return New(ctx, deps, conf) + return New(ctx, deps, conf, logger) }, }) } @@ -83,6 +83,7 @@ type PCA9685 struct { gpioPins [16]gpioPin boardName string i2cName string + logger golog.Logger } const ( @@ -96,10 +97,11 @@ const ( var defaultAddr = 0x40 // New returns a new PCA9685 residing on the given bus and address. -func New(ctx context.Context, deps resource.Dependencies, conf resource.Config) (*PCA9685, error) { +func New(ctx context.Context, deps resource.Dependencies, conf resource.Config, logger golog.Logger) (*PCA9685, error) { pca := PCA9685{ Named: conf.ResourceName().AsNamed(), referenceClockSpeed: defaultReferenceClockSpeed, + logger: logger, } // each PWM combination spans 4 bytes startAddr := byte(0x06) diff --git a/components/board/jetson/board.go b/components/board/jetson/board.go index 2c617c080bb..2c19c4ef7bf 100644 --- a/components/board/jetson/board.go +++ b/components/board/jetson/board.go @@ -22,5 +22,5 @@ func init() { golog.Global().Debugw("error getting jetson GPIO board mapping", "error", err) } - genericlinux.RegisterBoard(modelName, gpioMappings, false) + genericlinux.RegisterBoard(modelName, gpioMappings) } diff --git a/components/board/jetson/data.go b/components/board/jetson/data.go index 6768df7aa9a..a8f888de08a 100644 --- a/components/board/jetson/data.go +++ b/components/board/jetson/data.go @@ -17,308 +17,204 @@ const ( jetsonOrinNano = "jetson_orin_nano" ) -// TODO [RSDK-3596]: fix ngpio numbers in pin definitions for the jetsonTX1, jetsonNano. +// TODO [RSDK-3596]: fix device names in pin definitions for the jetsonTX1, jetsonNano. var claraAGXXavierPins = []genericlinux.PinDefinition{ - {map[int]int{169: 106}, map[int]string{169: "PQ.06"}, "2200000.gpio", 7, 4, "MCLK05", "SOC_GPIO42", "", -1}, - {map[int]int{169: 112}, map[int]string{169: "PR.04"}, "2200000.gpio", 11, 17, "UART1_RTS", "UART1_RTS", "", -1}, - {map[int]int{169: 51}, map[int]string{169: "PH.07"}, "2200000.gpio", 12, 18, "I2S2_CLK", "DAP2_SCLK", "", -1}, - {map[int]int{169: 96}, map[int]string{169: "PP.04"}, "2200000.gpio", 13, 27, "GPIO32", "SOC_GPIO04", "", -1}, + {Name: "7", DeviceName: "UNKNOWN169", LineNumber: 106, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "11", DeviceName: "UNKNOWN169", LineNumber: 112, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "12", DeviceName: "UNKNOWN169", LineNumber: 51, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "13", DeviceName: "UNKNOWN169", LineNumber: 96, PwmChipSysfsDir: "", PwmID: -1}, // Older versions of L4T don"t enable this PWM controller in DT, so this PWM // channel may not be available. - { - map[int]int{169: 84}, - map[int]string{169: "PN.01"}, - "2200000.gpio", - 15, - 22, - "GPIO27", - "SOC_GPIO54", - "3280000.pwm", - 0, - }, - {map[int]int{40: 8, 30: 8}, map[int]string{30: "PBB.00"}, "c2f0000.gpio", 16, 23, "GPIO8", "CAN1_STB", "", -1}, - { - map[int]int{169: 44}, - map[int]string{169: "PH.00"}, - "2200000.gpio", - 18, - 24, - "GPIO35", - "SOC_GPIO12", - "32c0000.pwm", - 0, - }, - {map[int]int{169: 162}, map[int]string{169: "PZ.05"}, "2200000.gpio", 19, 10, "SPI1_MOSI", "SPI1_MOSI", "", -1}, - {map[int]int{169: 161}, map[int]string{169: "PZ.04"}, "2200000.gpio", 21, 9, "SPI1_MISO", "SPI1_MISO", "", -1}, - {map[int]int{169: 101}, map[int]string{169: "PQ.01"}, "2200000.gpio", 22, 25, "GPIO17", "SOC_GPIO21", "", -1}, - {map[int]int{169: 160}, map[int]string{169: "PZ.03"}, "2200000.gpio", 23, 11, "SPI1_CLK", "SPI1_SCK", "", -1}, - {map[int]int{169: 163}, map[int]string{169: "PZ.06"}, "2200000.gpio", 24, 8, "SPI1_CS0_N", "SPI1_CS0_N", "", -1}, - {map[int]int{169: 164}, map[int]string{169: "PZ.07"}, "2200000.gpio", 26, 7, "SPI1_CS1_N", "SPI1_CS1_N", "", -1}, - {map[int]int{30: 3}, map[int]string{30: "PAA.03"}, "c2f0000.gpio", 29, 5, "CAN0_DIN", "CAN0_DIN", "", -1}, - {map[int]int{30: 2}, map[int]string{30: "PAA.02"}, "c2f0000.gpio", 31, 6, "CAN0_DOUT", "CAN0_DOUT", "", -1}, - {map[int]int{30: 9}, map[int]string{30: "PBB.01"}, "c2f0000.gpio", 32, 12, "GPIO9", "CAN1_EN", "", -1}, - {map[int]int{30: 0}, map[int]string{30: "PAA.00"}, "c2f0000.gpio", 33, 13, "CAN1_DOUT", "CAN1_DOUT", "", -1}, - {map[int]int{69: 54}, map[int]string{169: "PI.02"}, "2200000.gpio", 35, 19, "I2S2_FS", "DAP2_FS", "", -1}, + {Name: "15", DeviceName: "UNKNOWN169", LineNumber: 84, PwmChipSysfsDir: "3280000.pwm", PwmID: 0}, + {Name: "16", DeviceName: "UNKNOWN30", LineNumber: 8, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "18", DeviceName: "UNKNOWN169", LineNumber: 44, PwmChipSysfsDir: "32c0000.pwm", PwmID: 0}, + {Name: "19", DeviceName: "UNKNOWN169", LineNumber: 162, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "21", DeviceName: "UNKNOWN169", LineNumber: 161, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "22", DeviceName: "UNKNOWN169", LineNumber: 101, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "23", DeviceName: "UNKNOWN169", LineNumber: 160, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "24", DeviceName: "UNKNOWN169", LineNumber: 163, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "26", DeviceName: "UNKNOWN169", LineNumber: 164, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "29", DeviceName: "UNKNOWN30", LineNumber: 3, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "31", DeviceName: "UNKNOWN30", LineNumber: 2, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "32", DeviceName: "UNKNOWN30", LineNumber: 9, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "33", DeviceName: "UNKNOWN30", LineNumber: 0, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "35", DeviceName: "UNKNOWN69", LineNumber: 54, PwmChipSysfsDir: "", PwmID: -1}, // Input-only (due to base board) - {map[int]int{169: 113}, map[int]string{169: "PR.05"}, "2200000.gpio", 36, 16, "UART1_CTS", "UART1_CTS", "", -1}, - {map[int]int{30: 1}, map[int]string{30: "PAA.01"}, "c2f0000.gpio", 37, 26, "CAN1_DIN", "CAN1_DIN", "", -1}, - {map[int]int{69: 53}, map[int]string{169: "PI.01"}, "2200000.gpio", 38, 20, "I2S2_DIN", "DAP2_DIN", "", -1}, - {map[int]int{69: 52}, map[int]string{169: "PI.00"}, "2200000.gpio", 40, 21, "I2S2_DOUT", "DAP2_DOUT", "", -1}, + {Name: "36", DeviceName: "UNKNOWN169", LineNumber: 113, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "37", DeviceName: "UNKNOWN30", LineNumber: 1, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "38", DeviceName: "UNKNOWN69", LineNumber: 53, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "40", DeviceName: "UNKNOWN69", LineNumber: 52, PwmChipSysfsDir: "", PwmID: -1}, } //nolint:dupl // This is not actually a duplicate of jetsonTX2NXPins despite what the linter thinks var jetsonNXPins = []genericlinux.PinDefinition{ - {map[int]int{169: 118}, map[int]string{169: "PS.04"}, "2200000.gpio", 7, 4, "GPIO09", "AUD_MCLK", "", -1}, - {map[int]int{169: 112}, map[int]string{169: "PR.04"}, "2200000.gpio", 11, 17, "UART1_RTS", "UART1_RTS", "", -1}, - {map[int]int{169: 127}, map[int]string{169: "PT.05"}, "2200000.gpio", 12, 18, "I2S0_SCLK", "DAP5_SCLK", "", -1}, - {map[int]int{169: 149}, map[int]string{169: "PY.00"}, "2200000.gpio", 13, 27, "SPI1_SCK", "SPI3_SCK", "", -1}, - {map[int]int{30: 16}, map[int]string{30: "PCC.04"}, "c2f0000.gpio", 15, 22, "GPIO12", "TOUCH_CLK", "", -1}, - {map[int]int{169: 153}, map[int]string{169: "PY.04"}, "2200000.gpio", 16, 23, "SPI1_CS1", "SPI3_CS1_N", "", -1}, - {map[int]int{169: 152}, map[int]string{169: "PY.03"}, "2200000.gpio", 18, 24, "SPI1_CS0", "SPI3_CS0_N", "", -1}, - {map[int]int{169: 162}, map[int]string{169: "PZ.05"}, "2200000.gpio", 19, 10, "SPI0_MOSI", "SPI1_MOSI", "", -1}, - {map[int]int{169: 161}, map[int]string{169: "PZ.04"}, "2200000.gpio", 21, 9, "SPI0_MISO", "SPI1_MISO", "", -1}, - {map[int]int{169: 150}, map[int]string{169: "PY.01"}, "2200000.gpio", 22, 25, "SPI1_MISO", "SPI3_MISO", "", -1}, - {map[int]int{169: 160}, map[int]string{169: "PZ.03"}, "2200000.gpio", 23, 11, "SPI0_SCK", "SPI1_SCK", "", -1}, - {map[int]int{169: 163}, map[int]string{169: "PZ.06"}, "2200000.gpio", 24, 8, "SPI0_CS0", "SPI1_CS0_N", "", -1}, - {map[int]int{169: 164}, map[int]string{169: "PZ.07"}, "2200000.gpio", 26, 7, "SPI0_CS1", "SPI1_CS1_N", "", -1}, - {map[int]int{169: 105}, map[int]string{169: "PQ.05"}, "2200000.gpio", 29, 5, "GPIO01", "SOC_GPIO41", "", -1}, - {map[int]int{169: 106}, map[int]string{169: "PQ.06"}, "2200000.gpio", 31, 6, "GPIO11", "SOC_GPIO42", "", -1}, - { - map[int]int{169: 108}, - map[int]string{169: "PR.00"}, - "2200000.gpio", - 32, - 12, - "GPIO07", - "SOC_GPIO44", - "32f0000.pwm", - 0, - }, - { - map[int]int{169: 84}, - map[int]string{169: "PN.01"}, - "2200000.gpio", - 33, - 13, - "GPIO13", - "SOC_GPIO54", - "3280000.pwm", - 0, - }, - {map[int]int{169: 130}, map[int]string{169: "PU.00"}, "2200000.gpio", 35, 19, "I2S0_FS", "DAP5_FS", "", -1}, - {map[int]int{169: 113}, map[int]string{169: "PR.05"}, "2200000.gpio", 36, 16, "UART1_CTS", "UART1_CTS", "", -1}, - {map[int]int{169: 151}, map[int]string{169: "PY.02"}, "2200000.gpio", 37, 26, "SPI1_MOSI", "SPI3_MOSI", "", -1}, - {map[int]int{169: 129}, map[int]string{169: "PT.07"}, "2200000.gpio", 38, 20, "I2S0_DIN", "DAP5_DIN", "", -1}, - {map[int]int{169: 128}, map[int]string{169: "PT.06"}, "2200000.gpio", 40, 21, "I2S0_DOUT", "DAP5_DOUT", "", -1}, + {Name: "7", DeviceName: "UNKNOWN169", LineNumber: 118, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "11", DeviceName: "UNKNOWN169", LineNumber: 112, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "12", DeviceName: "UNKNOWN169", LineNumber: 127, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "13", DeviceName: "UNKNOWN169", LineNumber: 149, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "15", DeviceName: "UNKNOWN30", LineNumber: 16, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "16", DeviceName: "UNKNOWN169", LineNumber: 153, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "18", DeviceName: "UNKNOWN169", LineNumber: 152, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "19", DeviceName: "UNKNOWN169", LineNumber: 162, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "21", DeviceName: "UNKNOWN169", LineNumber: 161, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "22", DeviceName: "UNKNOWN169", LineNumber: 150, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "23", DeviceName: "UNKNOWN169", LineNumber: 160, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "24", DeviceName: "UNKNOWN169", LineNumber: 163, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "26", DeviceName: "UNKNOWN169", LineNumber: 164, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "29", DeviceName: "UNKNOWN169", LineNumber: 105, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "31", DeviceName: "UNKNOWN169", LineNumber: 106, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "32", DeviceName: "UNKNOWN169", LineNumber: 108, PwmChipSysfsDir: "32f0000.pwm", PwmID: 0}, + {Name: "33", DeviceName: "UNKNOWN169", LineNumber: 84, PwmChipSysfsDir: "3280000.pwm", PwmID: 0}, + {Name: "35", DeviceName: "UNKNOWN169", LineNumber: 130, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "36", DeviceName: "UNKNOWN169", LineNumber: 113, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "37", DeviceName: "UNKNOWN169", LineNumber: 151, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "38", DeviceName: "UNKNOWN169", LineNumber: 129, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "40", DeviceName: "UNKNOWN169", LineNumber: 128, PwmChipSysfsDir: "", PwmID: -1}, } +// It's not clear that this has ever worked properly: back when we identified chips by the ngpio +// number, the two chips supposedly had 169 and 30 lines on them, but folks online seem to claim +// that the Xavier's chips have 223 and 39 lines. So, these are probably broken right now but could +// be fixed with some inspection of an actual Xavier board (which we don't have in the office). +// More likely, they'll be fixed when someone creates a customlinux pin definition file. var jetsonXavierPins = []genericlinux.PinDefinition{ - {map[int]int{169: 106}, map[int]string{169: "PQ.06"}, "2200000.gpio", 7, 4, "MCLK05", "SOC_GPIO42", "", -1}, - {map[int]int{169: 112}, map[int]string{169: "PR.04"}, "2200000.gpio", 11, 17, "UART1_RTS", "UART1_RTS", "", -1}, - {map[int]int{169: 51}, map[int]string{169: "PH.07"}, "2200000.gpio", 12, 18, "I2S2_CLK", "DAP2_SCLK", "", -1}, - { - map[int]int{169: 108}, - map[int]string{169: "PR.00"}, - "2200000.gpio", - 13, - 27, - "PWM01", - "SOC_GPIO44", - "32f0000.pwm", - 0, - }, + {Name: "7", DeviceName: "UNKNOWN169", LineNumber: 106, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "11", DeviceName: "UNKNOWN169", LineNumber: 112, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "12", DeviceName: "UNKNOWN169", LineNumber: 51, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "13", DeviceName: "UNKNOWN169", LineNumber: 108, PwmChipSysfsDir: "32f0000.pwm", PwmID: 0}, // Older versions of L4T don't enable this PWM controller in DT, so this PWM // channel may not be available. - { - map[int]int{169: 84}, - map[int]string{169: "PN.01"}, - "2200000.gpio", - 15, - 22, - "GPIO27", - "SOC_GPIO54", - "3280000.pwm", - 0, - }, - {map[int]int{30: 8}, map[int]string{30: "PBB.00"}, "c2f0000.gpio", 16, 23, "GPIO8", "CAN1_STB", "", -1}, - { - map[int]int{169: 44}, - map[int]string{169: "PH.00"}, - "2200000.gpio", - 18, - 24, - "GPIO35", - "SOC_GPIO12", - "32c0000.pwm", - 0, - }, - {map[int]int{169: 162}, map[int]string{169: "PZ.05"}, "2200000.gpio", 19, 10, "SPI1_MOSI", "SPI1_MOSI", "", -1}, - {map[int]int{169: 161}, map[int]string{169: "PZ.04"}, "2200000.gpio", 21, 9, "SPI1_MISO", "SPI1_MISO", "", -1}, - {map[int]int{169: 101}, map[int]string{169: "PQ.01"}, "2200000.gpio", 22, 25, "GPIO17", "SOC_GPIO21", "", -1}, - {map[int]int{169: 160}, map[int]string{169: "PZ.03"}, "2200000.gpio", 23, 11, "SPI1_CLK", "SPI1_SCK", "", -1}, - {map[int]int{169: 163}, map[int]string{169: "PZ.06"}, "2200000.gpio", 24, 8, "SPI1_CS0_N", "SPI1_CS0_N", "", -1}, - {map[int]int{169: 164}, map[int]string{169: "PZ.07"}, "2200000.gpio", 26, 7, "SPI1_CS1_N", "SPI1_CS1_N", "", -1}, - {map[int]int{30: 3}, map[int]string{30: "PAA.03"}, "c2f0000.gpio", 29, 5, "CAN0_DIN", "CAN0_DIN", "", -1}, - {map[int]int{30: 2}, map[int]string{30: "PAA.02"}, "c2f0000.gpio", 31, 6, "CAN0_DOUT", "CAN0_DOUT", "", -1}, - {map[int]int{30: 9}, map[int]string{30: "PBB.01"}, "c2f0000.gpio", 32, 12, "GPIO9", "CAN1_EN", "", -1}, - {map[int]int{30: 0}, map[int]string{30: "PAA.00"}, "c2f0000.gpio", 33, 13, "CAN1_DOUT", "CAN1_DOUT", "", -1}, - {map[int]int{169: 54}, map[int]string{169: "PI.02"}, "2200000.gpio", 35, 19, "I2S2_FS", "DAP2_FS", "", -1}, + {Name: "15", DeviceName: "UNKNOWN169", LineNumber: 84, PwmChipSysfsDir: "3280000.pwm", PwmID: 0}, + {Name: "16", DeviceName: "UNKNOWN30", LineNumber: 8, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "18", DeviceName: "UNKNOWN169", LineNumber: 44, PwmChipSysfsDir: "32c0000.pwm", PwmID: 0}, + {Name: "19", DeviceName: "UNKNOWN169", LineNumber: 162, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "21", DeviceName: "UNKNOWN169", LineNumber: 161, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "22", DeviceName: "UNKNOWN169", LineNumber: 101, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "23", DeviceName: "UNKNOWN169", LineNumber: 160, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "24", DeviceName: "UNKNOWN169", LineNumber: 163, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "26", DeviceName: "UNKNOWN169", LineNumber: 164, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "29", DeviceName: "UNKNOWN30", LineNumber: 3, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "31", DeviceName: "UNKNOWN30", LineNumber: 2, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "32", DeviceName: "UNKNOWN30", LineNumber: 9, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "33", DeviceName: "UNKNOWN30", LineNumber: 0, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "35", DeviceName: "UNKNOWN169", LineNumber: 54, PwmChipSysfsDir: "", PwmID: -1}, // Input-only (due to base board) - {map[int]int{169: 113}, map[int]string{169: "PR.05"}, "2200000.gpio", 36, 16, "UART1_CTS", "UART1_CTS", "", -1}, - {map[int]int{30: 1}, map[int]string{30: "PAA.01"}, "c2f0000.gpio", 37, 26, "CAN1_DIN", "CAN1_DIN", "", -1}, - {map[int]int{169: 53}, map[int]string{169: "PI.01"}, "2200000.gpio", 38, 20, "I2S2_DIN", "DAP2_DIN", "", -1}, - {map[int]int{169: 52}, map[int]string{169: "PI.00"}, "2200000.gpio", 40, 21, "I2S2_DOUT", "DAP2_DOUT", "", -1}, + {Name: "36", DeviceName: "UNKNOWN169", LineNumber: 113, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "37", DeviceName: "UNKNOWN30", LineNumber: 1, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "38", DeviceName: "UNKNOWN169", LineNumber: 53, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "40", DeviceName: "UNKNOWN169", LineNumber: 52, PwmChipSysfsDir: "", PwmID: -1}, } -//nolint:dupl // This is not actually a duplicate of jetsonNXPins despite wht the linter thinks +//nolint:dupl // This is not actually a duplicate of jetsonNXPins despite what the linter thinks var jetsonTX2NXPins = []genericlinux.PinDefinition{ - {map[int]int{192: 76}, map[int]string{140: "PJ.04"}, "2200000.gpio", 7, 4, "GPIO09", "AUD_MCLK", "", -1}, - {map[int]int{64: 28}, map[int]string{47: "PW.04"}, "c2f0000.gpio", 11, 17, "UART1_RTS", "UART3_RTS", "", -1}, - {map[int]int{192: 72}, map[int]string{140: "PJ.00"}, "2200000.gpio", 12, 18, "I2S0_SCLK", "DAP1_SCLK", "", -1}, - {map[int]int{64: 17}, map[int]string{47: "PV.01"}, "c2f0000.gpio", 13, 27, "SPI1_SCK", "GPIO_SEN1", "", -1}, - {map[int]int{192: 18}, map[int]string{140: "PC.02"}, "2200000.gpio", 15, 22, "GPIO12", "DAP2_DOUT", "", -1}, - {map[int]int{192: 19}, map[int]string{140: "PC.03"}, "2200000.gpio", 16, 23, "SPI1_CS1", "DAP2_DIN", "", -1}, - {map[int]int{64: 20}, map[int]string{47: "PV.04"}, "c2f0000.gpio", 18, 24, "SPI1_CS0", "GPIO_SEN4", "", -1}, - {map[int]int{192: 58}, map[int]string{140: "PH.02"}, "2200000.gpio", 19, 10, "SPI0_MOSI", "GPIO_WAN7", "", -1}, - {map[int]int{192: 57}, map[int]string{140: "PH.01"}, "2200000.gpio", 21, 9, "SPI0_MISO", "GPIO_WAN6", "", -1}, - {map[int]int{64: 18}, map[int]string{47: "PV.02"}, "c2f0000.gpio", 22, 25, "SPI1_MISO", "GPIO_SEN2", "", -1}, - {map[int]int{192: 56}, map[int]string{140: "PH.00"}, "2200000.gpio", 23, 11, "SPI1_CLK", "GPIO_WAN5", "", -1}, - {map[int]int{192: 59}, map[int]string{140: "PH.03"}, "2200000.gpio", 24, 8, "SPI0_CS0", "GPIO_WAN8", "", -1}, - {map[int]int{192: 163}, map[int]string{140: "PY.03"}, "2200000.gpio", 26, 7, "SPI0_CS1", "GPIO_MDM4", "", -1}, - {map[int]int{192: 105}, map[int]string{140: "PN.01"}, "2200000.gpio", 29, 5, "GPIO01", "GPIO_CAM2", "", -1}, - {map[int]int{64: 50}, map[int]string{47: "PEE.02"}, "c2f0000.gpio", 31, 6, "GPIO11", "TOUCH_CLK", "", -1}, - {map[int]int{64: 8}, map[int]string{47: "PU.00"}, "c2f0000.gpio", 32, 12, "GPIO07", "GPIO_DIS0", "3280000.pwm", 0}, - {map[int]int{64: 13}, map[int]string{47: "PU.05"}, "c2f0000.gpio", 33, 13, "GPIO13", "GPIO_DIS5", "32a0000.pwm", 0}, - {map[int]int{192: 75}, map[int]string{140: "PJ.03"}, "2200000.gpio", 35, 19, "I2S0_FS", "DAP1_FS", "", -1}, - {map[int]int{64: 29}, map[int]string{47: "PW.05"}, "c2f0000.gpio", 36, 16, "UART1_CTS", "UART3_CTS", "", -1}, - {map[int]int{64: 19}, map[int]string{47: "PV.03"}, "c2f0000.gpio", 37, 26, "SPI1_MOSI", "GPIO_SEN3", "", -1}, - {map[int]int{192: 74}, map[int]string{140: "PJ.02"}, "2200000.gpio", 38, 20, "I2S0_DIN", "DAP1_DIN", "", -1}, - {map[int]int{192: 73}, map[int]string{140: "PJ.01"}, "2200000.gpio", 40, 21, "I2S0_DOUT", "DAP1_DOUT", "", -1}, + {Name: "7", DeviceName: "gpiochip0", LineNumber: 76, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "11", DeviceName: "gpiochip1", LineNumber: 28, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "12", DeviceName: "gpiochip0", LineNumber: 72, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "13", DeviceName: "gpiochip1", LineNumber: 17, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "15", DeviceName: "gpiochip0", LineNumber: 18, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "16", DeviceName: "gpiochip0", LineNumber: 19, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "18", DeviceName: "gpiochip1", LineNumber: 20, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "19", DeviceName: "gpiochip0", LineNumber: 58, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "21", DeviceName: "gpiochip0", LineNumber: 57, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "22", DeviceName: "gpiochip1", LineNumber: 18, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "23", DeviceName: "gpiochip0", LineNumber: 56, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "24", DeviceName: "gpiochip0", LineNumber: 59, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "26", DeviceName: "gpiochip0", LineNumber: 163, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "29", DeviceName: "gpiochip0", LineNumber: 105, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "31", DeviceName: "gpiochip1", LineNumber: 50, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "32", DeviceName: "gpiochip1", LineNumber: 8, PwmChipSysfsDir: "3280000.pwm", PwmID: 0}, + {Name: "33", DeviceName: "gpiochip1", LineNumber: 13, PwmChipSysfsDir: "32a0000.pwm", PwmID: 0}, + {Name: "35", DeviceName: "gpiochip0", LineNumber: 75, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "36", DeviceName: "gpiochip1", LineNumber: 29, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "37", DeviceName: "gpiochip1", LineNumber: 19, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "38", DeviceName: "gpiochip0", LineNumber: 74, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "40", DeviceName: "gpiochip0", LineNumber: 73, PwmChipSysfsDir: "", PwmID: -1}, } var jetsonTX2Pins = []genericlinux.PinDefinition{ - {map[int]int{192: 76}, map[int]string{140: "PJ.04"}, "2200000.gpio", 7, 4, "PAUDIO_MCLK", "AUD_MCLK", "", -1}, + {Name: "7", DeviceName: "gpiochip0", LineNumber: 76, PwmChipSysfsDir: "", PwmID: -1}, // Output-only (due to base board) - {map[int]int{192: 146}, map[int]string{140: "PT.02"}, "2200000.gpio", 11, 17, "PUART0_RTS", "UART1_RTS", "", -1}, - {map[int]int{192: 72}, map[int]string{140: "PJ.00"}, "2200000.gpio", 12, 18, "PI2S0_CLK", "DAP1_SCLK", "", -1}, - { - map[int]int{192: 77}, - map[int]string{140: "PJ.05"}, - "2200000.gpio", - 13, - 27, - "PGPIO20_AUD_INT", - "GPIO_AUD0", - "", - -1, - }, - {map[int]int{-1: 15}, nil, "3160000.i2c/i2c-0/0-0074", 15, 22, "GPIO_EXP_P17", "GPIO_EXP_P17", "", -1}, + {Name: "11", DeviceName: "gpiochip0", LineNumber: 146, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "12", DeviceName: "gpiochip0", LineNumber: 72, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "13", DeviceName: "gpiochip0", LineNumber: 77, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "15", DeviceName: "UNKNOWN", LineNumber: 15, PwmChipSysfsDir: "", PwmID: -1}, // Input-only (due to module): - {map[int]int{64: 40}, map[int]string{47: "PAA.00"}, "c2f0000.gpio", 16, 23, "AO_DMIC_IN_DAT", "CAN_GPIO0", "", -1}, - { - map[int]int{192: 161}, - map[int]string{140: "PY.01"}, - "2200000.gpio", - 18, - 24, - "GPIO16_MDM_WAKE_AP", - "GPIO_MDM2", - "", - -1, - }, - {map[int]int{192: 109}, map[int]string{140: "PN.05"}, "2200000.gpio", 19, 10, "SPI1_MOSI", "GPIO_CAM6", "", -1}, - {map[int]int{192: 108}, map[int]string{140: "PN.04"}, "2200000.gpio", 21, 9, "SPI1_MISO", "GPIO_CAM5", "", -1}, - {map[int]int{-1: 14}, nil, "3160000.i2c/i2c-0/0-0074", 22, 25, "GPIO_EXP_P16", "GPIO_EXP_P16", "", -1}, - {map[int]int{192: 107}, map[int]string{140: "PN.03"}, "2200000.gpio", 23, 11, "SPI1_CLK", "GPIO_CAM4", "", -1}, - {map[int]int{192: 110}, map[int]string{140: "PN.06"}, "2200000.gpio", 24, 8, "SPI1_CS0", "GPIO_CAM7", "", -1}, + {Name: "16", DeviceName: "gpiochip1", LineNumber: 40, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "18", DeviceName: "gpiochip0", LineNumber: 161, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "19", DeviceName: "gpiochip0", LineNumber: 109, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "21", DeviceName: "gpiochip0", LineNumber: 108, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "22", DeviceName: "UNKNOWN", LineNumber: 14, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "23", DeviceName: "gpiochip0", LineNumber: 107, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "24", DeviceName: "gpiochip0", LineNumber: 110, PwmChipSysfsDir: "", PwmID: -1}, // Board pin 26 is not available on this board - {map[int]int{192: 78}, map[int]string{140: "PJ.06"}, "2200000.gpio", 29, 5, "GPIO19_AUD_RST", "GPIO_AUD1", "", -1}, - {map[int]int{64: 42}, map[int]string{47: "PAA.02"}, "c2f0000.gpio", 31, 6, "GPIO9_MOTION_INT", "CAN_GPIO2", "", -1}, + {Name: "29", DeviceName: "gpiochip0", LineNumber: 78, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "31", DeviceName: "gpiochip1", LineNumber: 42, PwmChipSysfsDir: "", PwmID: -1}, // Output-only (due to module): - {map[int]int{64: 41}, map[int]string{47: "PAA.01"}, "c2f0000.gpio", 32, 12, "AO_DMIC_IN_CLK", "CAN_GPIO1", "", -1}, - { - map[int]int{192: 69}, - map[int]string{140: "PI.05"}, - "2200000.gpio", - 33, - 13, - "GPIO11_AP_WAKE_BT", - "GPIO_PQ5", - "", - -1, - }, - {map[int]int{192: 75}, map[int]string{140: "PJ.03"}, "2200000.gpio", 35, 19, "I2S0_LRCLK", "DAP1_FS", "", -1}, + {Name: "32", DeviceName: "gpiochip1", LineNumber: 41, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "33", DeviceName: "gpiochip0", LineNumber: 69, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "35", DeviceName: "gpiochip0", LineNumber: 75, PwmChipSysfsDir: "", PwmID: -1}, // Input-only (due to base board) IF NVIDIA debug card NOT plugged in // Output-only (due to base board) IF NVIDIA debug card plugged in - {map[int]int{192: 147}, map[int]string{140: "PT.03"}, "2200000.gpio", 36, 16, "UART0_CTS", "UART1_CTS", "", -1}, - { - map[int]int{192: 68}, - map[int]string{140: "PI.04"}, - "2200000.gpio", - 37, - 26, - "GPIO8_ALS_PROX_INT", - "GPIO_PQ4", - "", - -1, - }, - {map[int]int{192: 74}, map[int]string{140: "PJ.02"}, "2200000.gpio", 38, 20, "I2S0_SDIN", "DAP1_DIN", "", -1}, - {map[int]int{192: 73}, map[int]string{140: "PJ.01"}, "2200000.gpio", 40, 21, "I2S0_SDOUT", "DAP1_DOUT", "", -1}, + {Name: "36", DeviceName: "gpiochip0", LineNumber: 147, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "37", DeviceName: "gpiochip0", LineNumber: 68, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "38", DeviceName: "gpiochip0", LineNumber: 74, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "40", DeviceName: "gpiochip0", LineNumber: 73, PwmChipSysfsDir: "", PwmID: -1}, } var jetsonTX1Pins = []genericlinux.PinDefinition{ - {map[int]int{-1: 216}, nil, "6000d000.gpio", 7, 4, "AUDIO_MCLK", "AUD_MCLK", "", -1}, + {Name: "7", DeviceName: "UNKNOWN", LineNumber: 216, PwmChipSysfsDir: "", PwmID: -1}, // Output-only (due to base board) - {map[int]int{-1: 162}, nil, "6000d000.gpio", 11, 17, "UART0_RTS", "UART1_RTS", "", -1}, - {map[int]int{-1: 11}, nil, "6000d000.gpio", 12, 18, "I2S0_CLK", "DAP1_SCLK", "", -1}, - {map[int]int{-1: 38}, nil, "6000d000.gpio", 13, 27, "GPIO20_AUD_INT", "GPIO_PE6", "", -1}, - {map[int]int{-1: 15}, nil, "7000c400.i2c/i2c-1/1-0074", 15, 22, "GPIO_EXP_P17", "GPIO_EXP_P17", "", -1}, - {map[int]int{-1: 37}, nil, "6000d000.gpio", 16, 23, "AO_DMIC_IN_DAT", "DMIC3_DAT", "", -1}, - {map[int]int{-1: 184}, nil, "6000d000.gpio", 18, 24, "GPIO16_MDM_WAKE_AP", "MODEM_WAKE_AP", "", -1}, - {map[int]int{-1: 16}, nil, "6000d000.gpio", 19, 10, "SPI1_MOSI", "SPI1_MOSI", "", -1}, - {map[int]int{-1: 17}, nil, "6000d000.gpio", 21, 9, "SPI1_MISO", "SPI1_MISO", "", -1}, - {map[int]int{-1: 14}, nil, "7000c400.i2c/i2c-1/1-0074", 22, 25, "GPIO_EXP_P16", "GPIO_EXP_P16", "", -1}, - {map[int]int{-1: 18}, nil, "6000d000.gpio", 23, 11, "SPI1_CLK", "SPI1_SCK", "", -1}, - {map[int]int{-1: 19}, nil, "6000d000.gpio", 24, 8, "SPI1_CS0", "SPI1_CS0", "", -1}, - {map[int]int{-1: 20}, nil, "6000d000.gpio", 26, 7, "SPI1_CS1", "SPI1_CS1", "", -1}, - {map[int]int{-1: 219}, nil, "6000d000.gpio", 29, 5, "GPIO19_AUD_RST", "GPIO_X1_AUD", "", -1}, - {map[int]int{-1: 186}, nil, "6000d000.gpio", 31, 6, "GPIO9_MOTION_INT", "MOTION_INT", "", -1}, - {map[int]int{-1: 36}, nil, "6000d000.gpio", 32, 12, "AO_DMIC_IN_CLK", "DMIC3_CLK", "", -1}, - {map[int]int{-1: 63}, nil, "6000d000.gpio", 33, 13, "GPIO11_AP_WAKE_BT", "AP_WAKE_NFC", "", -1}, - {map[int]int{-1: 8}, nil, "6000d000.gpio", 35, 19, "I2S0_LRCLK", "DAP1_FS", "", -1}, + {Name: "11", DeviceName: "UNKNOWN", LineNumber: 162, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "12", DeviceName: "UNKNOWN", LineNumber: 11, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "13", DeviceName: "UNKNOWN", LineNumber: 38, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "15", DeviceName: "UNKNOWN", LineNumber: 15, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "16", DeviceName: "UNKNOWN", LineNumber: 37, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "18", DeviceName: "UNKNOWN", LineNumber: 184, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "19", DeviceName: "UNKNOWN", LineNumber: 16, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "21", DeviceName: "UNKNOWN", LineNumber: 17, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "22", DeviceName: "UNKNOWN", LineNumber: 14, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "23", DeviceName: "UNKNOWN", LineNumber: 18, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "24", DeviceName: "UNKNOWN", LineNumber: 19, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "26", DeviceName: "UNKNOWN", LineNumber: 20, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "29", DeviceName: "UNKNOWN", LineNumber: 219, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "31", DeviceName: "UNKNOWN", LineNumber: 186, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "32", DeviceName: "UNKNOWN", LineNumber: 36, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "33", DeviceName: "UNKNOWN", LineNumber: 63, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "35", DeviceName: "UNKNOWN", LineNumber: 8, PwmChipSysfsDir: "", PwmID: -1}, // Input-only (due to base board) IF NVIDIA debug card NOT plugged in // Input-only (due to base board) (always reads fixed value) IF NVIDIA debug card plugged in - {map[int]int{-1: 163}, nil, "6000d000.gpio", 36, 16, "UART0_CTS", "UART1_CTS", "", -1}, - {map[int]int{-1: 187}, nil, "6000d000.gpio", 37, 26, "GPIO8_ALS_PROX_INT", "ALS_PROX_INT", "", -1}, - {map[int]int{-1: 9}, nil, "6000d000.gpio", 38, 20, "I2S0_SDIN", "DAP1_DIN", "", -1}, - {map[int]int{-1: 10}, nil, "6000d000.gpio", 40, 21, "I2S0_SDOUT", "DAP1_DOUT", "", -1}, + {Name: "36", DeviceName: "UNKNOWN", LineNumber: 163, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "37", DeviceName: "UNKNOWN", LineNumber: 187, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "38", DeviceName: "UNKNOWN", LineNumber: 9, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "40", DeviceName: "UNKNOWN", LineNumber: 10, PwmChipSysfsDir: "", PwmID: -1}, } +//nolint:dupl // This is not actually a duplicate of jetsonNXPins despite what the linter thinks var jetsonNanoPins = []genericlinux.PinDefinition{ - {map[int]int{-1: 216}, nil, "6000d000.gpio", 7, 4, "GPIO9", "AUD_MCLK", "", -1}, - {map[int]int{-1: 50}, nil, "6000d000.gpio", 11, 17, "UART1_RTS", "UART2_RTS", "", -1}, - {map[int]int{-1: 79}, nil, "6000d000.gpio", 12, 18, "I2S0_SCLK", "DAP4_SCLK", "", -1}, - {map[int]int{-1: 14}, nil, "6000d000.gpio", 13, 27, "SPI1_SCK", "SPI2_SCK", "", -1}, - {map[int]int{-1: 194}, nil, "6000d000.gpio", 15, 22, "GPIO12", "LCD_TE", "", -1}, - {map[int]int{-1: 232}, nil, "6000d000.gpio", 16, 23, "SPI1_CS1", "SPI2_CS1", "", -1}, - {map[int]int{-1: 15}, nil, "6000d000.gpio", 18, 24, "SPI1_CS0", "SPI2_CS0", "", -1}, - {map[int]int{-1: 16}, nil, "6000d000.gpio", 19, 10, "SPI0_MOSI", "SPI1_MOSI", "", -1}, - {map[int]int{-1: 17}, nil, "6000d000.gpio", 21, 9, "SPI0_MISO", "SPI1_MISO", "", -1}, - {map[int]int{-1: 13}, nil, "6000d000.gpio", 22, 25, "SPI1_MISO", "SPI2_MISO", "", -1}, - {map[int]int{-1: 18}, nil, "6000d000.gpio", 23, 11, "SPI0_SCK", "SPI1_SCK", "", -1}, - {map[int]int{-1: 19}, nil, "6000d000.gpio", 24, 8, "SPI0_CS0", "SPI1_CS0", "", -1}, - {map[int]int{-1: 20}, nil, "6000d000.gpio", 26, 7, "SPI0_CS1", "SPI1_CS1", "", -1}, - {map[int]int{-1: 149}, nil, "6000d000.gpio", 29, 5, "GPIO01", "CAM_AF_EN", "", -1}, - {map[int]int{-1: 200}, nil, "6000d000.gpio", 31, 6, "GPIO11", "GPIO_PZ0", "", -1}, + {Name: "7", DeviceName: "gpiochip0", LineNumber: 216, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "11", DeviceName: "gpiochip0", LineNumber: 50, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "12", DeviceName: "gpiochip0", LineNumber: 79, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "13", DeviceName: "gpiochip0", LineNumber: 14, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "15", DeviceName: "gpiochip0", LineNumber: 194, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "16", DeviceName: "gpiochip0", LineNumber: 232, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "18", DeviceName: "gpiochip0", LineNumber: 15, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "19", DeviceName: "gpiochip0", LineNumber: 16, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "21", DeviceName: "gpiochip0", LineNumber: 17, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "22", DeviceName: "gpiochip0", LineNumber: 13, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "23", DeviceName: "gpiochip0", LineNumber: 18, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "24", DeviceName: "gpiochip0", LineNumber: 19, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "26", DeviceName: "gpiochip0", LineNumber: 20, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "29", DeviceName: "gpiochip0", LineNumber: 149, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "31", DeviceName: "gpiochip0", LineNumber: 200, PwmChipSysfsDir: "", PwmID: -1}, // Older versions of L4T have a DT bug which instantiates a bogus device // which prevents this library from using this PWM channel. - {map[int]int{-1: 168}, nil, "6000d000.gpio", 32, 12, "GPIO07", "LCD_BL_PW", "7000a000.pwm", 0}, - {map[int]int{-1: 38}, nil, "6000d000.gpio", 33, 13, "GPIO13", "GPIO_PE6", "7000a000.pwm", 2}, - {map[int]int{-1: 76}, nil, "6000d000.gpio", 35, 19, "I2S0_FS", "DAP4_FS", "", -1}, - {map[int]int{-1: 51}, nil, "6000d000.gpio", 36, 16, "UART1_CTS", "UART2_CTS", "", -1}, - {map[int]int{-1: 12}, nil, "6000d000.gpio", 37, 26, "SPI1_MOSI", "SPI2_MOSI", "", -1}, - {map[int]int{-1: 77}, nil, "6000d000.gpio", 38, 20, "I2S0_DIN", "DAP4_DIN", "", -1}, - {map[int]int{-1: 78}, nil, "6000d000.gpio", 40, 21, "I2S0_DOUT", "DAP4_DOUT", "", -1}, + {Name: "32", DeviceName: "gpiochip0", LineNumber: 168, PwmChipSysfsDir: "7000a000.pwm", PwmID: 0}, + {Name: "33", DeviceName: "gpiochip0", LineNumber: 38, PwmChipSysfsDir: "7000a000.pwm", PwmID: 2}, + {Name: "35", DeviceName: "gpiochip0", LineNumber: 76, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "36", DeviceName: "gpiochip0", LineNumber: 51, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "37", DeviceName: "gpiochip0", LineNumber: 12, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "38", DeviceName: "gpiochip0", LineNumber: 77, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "40", DeviceName: "gpiochip0", LineNumber: 78, PwmChipSysfsDir: "", PwmID: -1}, } // There are 6 pins whose Broadcom SOC channel is -1 (pins 3, 5, 8, 10, 27, and 28). We @@ -327,64 +223,65 @@ var jetsonNanoPins = []genericlinux.PinDefinition{ // We were unable to find the broadcom channel numbers for these pins, but (as of April // 2023) Viam doesn't use those values for anything anyway. var jetsonOrinAGXPins = []genericlinux.PinDefinition{ - {map[int]int{32: 22}, map[int]string{32: "PDD.02"}, "c2f0000.gpio", 3, -1, "I2C4_DAT", "GP16_I2C8_DAT", "", -1}, - {map[int]int{32: 21}, map[int]string{32: "PDD.01"}, "c2f0000.gpio", 5, -1, "I2C4_CLK", "GP81_I2C9_CLK", "", -1}, - {map[int]int{164: 106}, map[int]string{164: "PQ.06"}, "2200000.gpio", 7, 4, "MCLK05", "GP66", "", -1}, + {Name: "3", DeviceName: "gpiochip1", LineNumber: 22, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "5", DeviceName: "gpiochip1", LineNumber: 21, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "7", DeviceName: "gpiochip0", LineNumber: 106, PwmChipSysfsDir: "", PwmID: -1}, // Output-only (due to hardware limitation) - {map[int]int{164: 110}, map[int]string{164: "PR.02"}, "2200000.gpio", 8, -1, "UART1_TX", "GP70_UART1_TXD_BOOT2_STRAP", "", -1}, + {Name: "8", DeviceName: "gpiochip0", LineNumber: 110, PwmChipSysfsDir: "", PwmID: -1}, // Input-only (due to hardware limitation) - {map[int]int{164: 111}, map[int]string{164: "PR.03"}, "2200000.gpio", 10, -1, "UART1_RX", "GP71_UART1_RXD", "", -1}, + {Name: "10", DeviceName: "gpiochip0", LineNumber: 111, PwmChipSysfsDir: "", PwmID: -1}, // Output-only (due to hardware limitation) - {map[int]int{164: 112}, map[int]string{164: "PR.04"}, "2200000.gpio", 11, 17, "UART1_RTS", "GP72_UART1_RTS_N", "", -1}, - {map[int]int{164: 50}, map[int]string{164: "PH.07"}, "2200000.gpio", 12, 18, "I2S2_CLK", "GP122", "", -1}, - {map[int]int{164: 108}, map[int]string{164: "PR.00"}, "2200000.gpio", 13, 27, "PWM01", "GP68", "", -1}, - {map[int]int{164: 85}, map[int]string{164: "PN.01"}, "2200000.gpio", 15, 22, "GPIO27", "GP88_PWM1", "3280000.pwm", 0}, - {map[int]int{32: 9}, map[int]string{32: "PBB.01"}, "c2f0000.gpio", 16, 23, "GPIO08", "GP26", "", -1}, - {map[int]int{164: 43}, map[int]string{164: "PH.00"}, "2200000.gpio", 18, 24, "GPIO35", "GP115", "32c0000.pwm", 0}, - {map[int]int{164: 135}, map[int]string{164: "PZ.05"}, "2200000.gpio", 19, 10, "SPI1_MOSI", "GP49_SPI1_MOSI", "", -1}, - {map[int]int{164: 134}, map[int]string{164: "PZ.04"}, "2200000.gpio", 21, 9, "SPI1_MISO", "GP48_SPI1_MISO", "", -1}, - {map[int]int{164: 96}, map[int]string{164: "PP.04"}, "2200000.gpio", 22, 25, "GPIO17", "GP56", "", -1}, - {map[int]int{164: 133}, map[int]string{164: "PZ.03"}, "2200000.gpio", 23, 11, "SPI1_CLK", "GP47_SPI1_CLK", "", -1}, - {map[int]int{164: 136}, map[int]string{164: "PZ.06"}, "2200000.gpio", 24, 8, "SPI1_CS0_N", "GP50_SPI1_CS0_N", "", -1}, - {map[int]int{164: 137}, map[int]string{164: "PZ.07"}, "2200000.gpio", 26, 7, "SPI1_CS1_N", "GP51_SPI1_CS1_N", "", -1}, - {map[int]int{32: 20}, map[int]string{32: "PDD.00"}, "c2f0000.gpio", 27, -1, "I2C2_DAT", "GP14_I2C2_DAT", "", -1}, - {map[int]int{32: 19}, map[int]string{32: "PCC.07"}, "c2f0000.gpio", 28, -1, "I2C2_CLK", "GP13_I2C2_CLK", "", -1}, - {map[int]int{32: 1}, map[int]string{32: "PAA.01"}, "c2f0000.gpio", 29, 5, "CAN0_DIN", "GP18_CAN0_DIN", "", -1}, - {map[int]int{32: 0}, map[int]string{32: "PAA.00"}, "c2f0000.gpio", 31, 6, "CAN0_DOUT", "GP17_CAN0_DOUT", "", -1}, - {map[int]int{32: 8}, map[int]string{32: "PBB.00"}, "c2f0000.gpio", 32, 12, "GPIO09", "GP25", "", -1}, - {map[int]int{32: 2}, map[int]string{32: "PAA.02"}, "c2f0000.gpio", 33, 13, "CAN1_DOUT", "GP19_CAN1_DOUT", "", -1}, - {map[int]int{164: 53}, map[int]string{164: "PI.02"}, "2200000.gpio", 35, 19, "I2S2_FS", "GP125", "", -1}, + {Name: "11", DeviceName: "gpiochip0", LineNumber: 112, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "12", DeviceName: "gpiochip0", LineNumber: 50, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "13", DeviceName: "gpiochip0", LineNumber: 108, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "15", DeviceName: "gpiochip0", LineNumber: 85, PwmChipSysfsDir: "3280000.pwm", PwmID: 0}, + {Name: "16", DeviceName: "gpiochip1", LineNumber: 9, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "18", DeviceName: "gpiochip0", LineNumber: 43, PwmChipSysfsDir: "32c0000.pwm", PwmID: 0}, + {Name: "19", DeviceName: "gpiochip0", LineNumber: 135, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "21", DeviceName: "gpiochip0", LineNumber: 134, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "22", DeviceName: "gpiochip0", LineNumber: 96, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "23", DeviceName: "gpiochip0", LineNumber: 133, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "24", DeviceName: "gpiochip0", LineNumber: 136, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "26", DeviceName: "gpiochip0", LineNumber: 137, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "27", DeviceName: "gpiochip1", LineNumber: 20, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "28", DeviceName: "gpiochip1", LineNumber: 19, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "29", DeviceName: "gpiochip1", LineNumber: 1, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "31", DeviceName: "gpiochip1", LineNumber: 0, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "32", DeviceName: "gpiochip1", LineNumber: 8, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "33", DeviceName: "gpiochip1", LineNumber: 2, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "35", DeviceName: "gpiochip0", LineNumber: 53, PwmChipSysfsDir: "", PwmID: -1}, // Input-only (due to hardware limitation) - {map[int]int{164: 113}, map[int]string{164: "PR.05"}, "2200000.gpio", 36, 16, "UART1_CTS", "GP73_UART1_CTS_N", "", -1}, - {map[int]int{32: 3}, map[int]string{32: "PAA.03"}, "c2f0000.gpio", 37, 26, "CAN1_DIN", "GP20_CAN1_DIN", "", -1}, - {map[int]int{164: 52}, map[int]string{164: "PI.01"}, "2200000.gpio", 38, 20, "I2S2_DIN", "GP124", "", -1}, - {map[int]int{164: 51}, map[int]string{164: "PI.00"}, "2200000.gpio", 40, 21, "I2S2_DOUT", "GP123", "", -1}, + {Name: "36", DeviceName: "gpiochip0", LineNumber: 113, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "37", DeviceName: "gpiochip1", LineNumber: 3, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "38", DeviceName: "gpiochip0", LineNumber: 52, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "40", DeviceName: "gpiochip0", LineNumber: 51, PwmChipSysfsDir: "", PwmID: -1}, } // This pin mapping is used for both the Jetson Orin NX and the Jetson Orin Nano. var jetsonOrinNXPins = []genericlinux.PinDefinition{ - {map[int]int{164: 144}, map[int]string{164: "PAC.06"}, "2200000.gpio", 7, 4, "GPIO09", "GP167", "", -1}, - {map[int]int{164: 112}, map[int]string{164: "PR.04"}, "2200000.gpio", 11, 17, "UART1_RTS", "GP72_UART1_RTS_N", "", -1}, - {map[int]int{164: 50}, map[int]string{164: "PH.07"}, "2200000.gpio", 12, 18, "I2S0_SCLK", "GP122", "", -1}, - {map[int]int{164: 122}, map[int]string{164: "PY.00"}, "2200000.gpio", 13, 27, "SPI1_SCK", "GP36_SPI3_CLK", "", -1}, - {map[int]int{164: 85}, map[int]string{164: "PN.01"}, "2200000.gpio", 15, 22, "GPIO12", "GP88_PWM1", "3280000.pwm", 0}, - {map[int]int{164: 126}, map[int]string{164: "PY.04"}, "2200000.gpio", 16, 23, "SPI1_CS1", "GP40_SPI3_CS1_N", "", -1}, - {map[int]int{164: 125}, map[int]string{164: "PY.03"}, "2200000.gpio", 18, 24, "SPI1_CS0", "GP39_SPI3_CS0_N", "", -1}, - {map[int]int{164: 135}, map[int]string{164: "PZ.05"}, "2200000.gpio", 19, 10, "SPI0_MOSI", "GP49_SPI1_MOSI", "", -1}, - {map[int]int{164: 134}, map[int]string{164: "PZ.04"}, "2200000.gpio", 21, 9, "SPI0_MISO", "GP48_SPI1_MISO", "", -1}, - {map[int]int{164: 123}, map[int]string{164: "PY.01"}, "2200000.gpio", 22, 25, "SPI1_MISO", "GP37_SPI3_MISO", "", -1}, - {map[int]int{164: 133}, map[int]string{164: "PZ.03"}, "2200000.gpio", 23, 11, "SPI0_SCK", "GP47_SPI1_CLK", "", -1}, - {map[int]int{164: 136}, map[int]string{164: "PZ.06"}, "2200000.gpio", 24, 8, "SPI0_CS0", "GP50_SPI1_CS0_N", "", -1}, - {map[int]int{164: 137}, map[int]string{164: "PZ.07"}, "2200000.gpio", 26, 7, "SPI0_CS1", "GP51_SPI1_CS1_N", "", -1}, - {map[int]int{164: 105}, map[int]string{164: "PQ.05"}, "2200000.gpio", 29, 5, "GPIO01", "GP65", "", -1}, - {map[int]int{164: 106}, map[int]string{164: "PQ.06"}, "2200000.gpio", 31, 6, "GPIO11", "GP66", "", -1}, - {map[int]int{164: 41}, map[int]string{164: "PG.06"}, "2200000.gpio", 32, 12, "GPIO07", "GP113_PWM7", "", -1}, - {map[int]int{164: 43}, map[int]string{164: "PH.00"}, "2200000.gpio", 33, 13, "GPIO13", "GP115", "32c0000.pwm", 0}, - {map[int]int{164: 53}, map[int]string{164: "PI.02"}, "2200000.gpio", 35, 19, "I2S0_FS", "GP125", "", -1}, - {map[int]int{164: 113}, map[int]string{164: "PR.05"}, "2200000.gpio", 36, 16, "UART1_CTS", "GP73_UART1_CTS_N", "", -1}, - {map[int]int{164: 124}, map[int]string{164: "PY.02"}, "2200000.gpio", 37, 26, "SPI1_MOSI", "GP38_SPI3_MOSI", "", -1}, - {map[int]int{164: 52}, map[int]string{164: "PI.01"}, "2200000.gpio", 38, 20, "I2S0_SDIN", "GP124", "", -1}, - {map[int]int{164: 51}, map[int]string{164: "PI.00"}, "2200000.gpio", 40, 21, "I2S0_SDOUT", "GP123", "", -1}, + {Name: "7", DeviceName: "gpiochip0", LineNumber: 144, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "11", DeviceName: "gpiochip0", LineNumber: 112, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "12", DeviceName: "gpiochip0", LineNumber: 50, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "13", DeviceName: "gpiochip0", LineNumber: 122, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "15", DeviceName: "gpiochip0", LineNumber: 85, PwmChipSysfsDir: "3280000.pwm", PwmID: 0}, + {Name: "16", DeviceName: "gpiochip0", LineNumber: 126, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "18", DeviceName: "gpiochip0", LineNumber: 125, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "19", DeviceName: "gpiochip0", LineNumber: 135, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "21", DeviceName: "gpiochip0", LineNumber: 134, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "22", DeviceName: "gpiochip0", LineNumber: 123, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "23", DeviceName: "gpiochip0", LineNumber: 133, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "24", DeviceName: "gpiochip0", LineNumber: 136, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "26", DeviceName: "gpiochip0", LineNumber: 137, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "29", DeviceName: "gpiochip0", LineNumber: 105, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "31", DeviceName: "gpiochip0", LineNumber: 106, PwmChipSysfsDir: "", PwmID: -1}, + // Pin 32 supposedly has hardware PWM support, but we've been unable to turn it on. + {Name: "32", DeviceName: "gpiochip0", LineNumber: 41, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "33", DeviceName: "gpiochip0", LineNumber: 43, PwmChipSysfsDir: "32c0000.pwm", PwmID: 0}, + {Name: "35", DeviceName: "gpiochip0", LineNumber: 53, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "36", DeviceName: "gpiochip0", LineNumber: 113, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "37", DeviceName: "gpiochip0", LineNumber: 124, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "38", DeviceName: "gpiochip0", LineNumber: 52, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "40", DeviceName: "gpiochip0", LineNumber: 51, PwmChipSysfsDir: "", PwmID: -1}, } var boardInfoMappings = map[string]genericlinux.BoardInformation{ diff --git a/components/board/nanopi/board.go b/components/board/nanopi/board.go deleted file mode 100644 index 4a5ec7757aa..00000000000 --- a/components/board/nanopi/board.go +++ /dev/null @@ -1,21 +0,0 @@ -// Package nanopi implements a nanopi based board. -// This is an experimental package. -// Supported functionality: GPIO pins, I2C, SPI, Software PWM -// Unsupported functionality: Digital Interrupts, Hardware PWM -package nanopi - -import ( - "github.com/edaniels/golog" - "periph.io/x/host/v3" - - "go.viam.com/rdk/components/board/genericlinux" -) - -const modelName = "nanopi" - -func init() { - if _, err := host.Init(); err != nil { - golog.Global().Debugw("error initializing host", "error", err) - } - genericlinux.RegisterBoard(modelName, nil, true) -} diff --git a/components/board/pi/impl/board.go b/components/board/pi/impl/board.go index f9b05dd3db3..8e69cad5f85 100644 --- a/components/board/pi/impl/board.go +++ b/components/board/pi/impl/board.go @@ -224,7 +224,7 @@ func (pi *piPigpio) reconfigureAnalogs(ctx context.Context, cfg *genericlinux.Co return errors.Errorf("bad analog pin (%s)", ac.Pin) } - bus, have := pi.SPIByName(ac.SPIBus) + bus, have := pi.spis[ac.SPIBus] if !have { return errors.Errorf("can't find SPI bus (%s) requested by AnalogReader", ac.SPIBus) } diff --git a/components/board/pi/impl/board_test.go b/components/board/pi/impl/board_test.go index b94d1f6937b..efb0bad6098 100644 --- a/components/board/pi/impl/board_test.go +++ b/components/board/pi/impl/board_test.go @@ -15,6 +15,7 @@ import ( "go.viam.com/rdk/components/board/genericlinux" picommon "go.viam.com/rdk/components/board/pi/common" "go.viam.com/rdk/components/servo" + "go.viam.com/rdk/operation" "go.viam.com/rdk/resource" ) @@ -272,7 +273,7 @@ func TestServoFunctions(t *testing.T) { t.Run(("check Move IsMoving ande pigpio errors"), func(t *testing.T) { ctx := context.Background() - s := &piPigpioServo{pinname: "1", maxRotation: 180} + s := &piPigpioServo{pinname: "1", maxRotation: 180, opMgr: operation.NewSingleOperationManager()} s.res = -93 err := s.pigpioErrors(int(s.res)) diff --git a/components/board/pi/impl/servo.go b/components/board/pi/impl/servo.go index 46410fc0193..0dac9f05300 100644 --- a/components/board/pi/impl/servo.go +++ b/components/board/pi/impl/servo.go @@ -50,8 +50,10 @@ func init() { } theServo := &piPigpioServo{ - Named: conf.ResourceName().AsNamed(), - pin: C.uint(bcom), + Named: conf.ResourceName().AsNamed(), + logger: logger, + pin: C.uint(bcom), + opMgr: operation.NewSingleOperationManager(), } if newConf.Min > 0 { theServo.min = uint32(newConf.Min) @@ -104,11 +106,12 @@ type piPigpioServo struct { resource.Named resource.AlwaysRebuild resource.TriviallyCloseable + logger golog.Logger pin C.uint pinname string res C.int min, max uint32 - opMgr operation.SingleOperationManager + opMgr *operation.SingleOperationManager pulseWidth int // pulsewidth value, 500-2500us is 0-180 degrees, 0 is off holdPos bool maxRotation uint32 diff --git a/components/board/register/register.go b/components/board/register/register.go index aa0cccfcdba..32ebf628e2f 100644 --- a/components/board/register/register.go +++ b/components/board/register/register.go @@ -7,8 +7,8 @@ import ( _ "go.viam.com/rdk/components/board/fake" _ "go.viam.com/rdk/components/board/hat/pca9685" _ "go.viam.com/rdk/components/board/jetson" - _ "go.viam.com/rdk/components/board/nanopi" _ "go.viam.com/rdk/components/board/numato" _ "go.viam.com/rdk/components/board/pi" _ "go.viam.com/rdk/components/board/ti" + _ "go.viam.com/rdk/components/board/upboard" ) diff --git a/components/board/server_test.go b/components/board/server_test.go index 0603ad00a67..1121cd657ce 100644 --- a/components/board/server_test.go +++ b/components/board/server_test.go @@ -15,7 +15,10 @@ import ( "go.viam.com/rdk/testutils/inject" ) -var errFoo = errors.New("whoops") +var ( + errFoo = errors.New("whoops") + errUnimplemented = errors.New("not found") +) func newServer() (pb.BoardServiceServer, *inject.Board, error) { injectBoard := &inject.Board{} @@ -61,7 +64,7 @@ func TestServerStatus(t *testing.T) { req: &request{Name: missingBoardName}, expCapArgs: []interface{}(nil), expResp: nil, - expRespErr: "not found", + expRespErr: errUnimplemented.Error(), }, { injectResult: status, @@ -125,7 +128,7 @@ func TestServerSetGPIO(t *testing.T) { injectErr: nil, req: &request{Name: missingBoardName}, expCapArgs: []interface{}(nil), - expRespErr: "not found", + expRespErr: errUnimplemented.Error(), }, { injectErr: errFoo, @@ -194,7 +197,7 @@ func TestServerGetGPIO(t *testing.T) { req: &request{Name: missingBoardName}, expCapArgs: []interface{}(nil), expResp: nil, - expRespErr: "not found", + expRespErr: errUnimplemented.Error(), }, { injectResult: false, @@ -269,7 +272,7 @@ func TestServerPWM(t *testing.T) { req: &request{Name: missingBoardName}, expCapArgs: []interface{}(nil), expResp: nil, - expRespErr: "not found", + expRespErr: errUnimplemented.Error(), }, { injectResult: 0, @@ -337,7 +340,7 @@ func TestServerSetPWM(t *testing.T) { injectErr: nil, req: &request{Name: missingBoardName}, expCapArgs: []interface{}(nil), - expRespErr: "not found", + expRespErr: errUnimplemented.Error(), }, { injectErr: errFoo, @@ -407,7 +410,7 @@ func TestServerPWMFrequency(t *testing.T) { req: &request{Name: missingBoardName}, expCapArgs: []interface{}(nil), expResp: nil, - expRespErr: "not found", + expRespErr: errUnimplemented.Error(), }, { injectResult: 0, @@ -475,7 +478,7 @@ func TestServerSetPWMFrequency(t *testing.T) { injectErr: nil, req: &request{Name: missingBoardName}, expCapArgs: []interface{}(nil), - expRespErr: "not found", + expRespErr: errUnimplemented.Error(), }, { injectErr: errFoo, @@ -551,7 +554,7 @@ func TestServerReadAnalogReader(t *testing.T) { expCapAnalogReaderArgs: []interface{}(nil), expCapArgs: []interface{}(nil), expResp: nil, - expRespErr: "not found", + expRespErr: errUnimplemented.Error(), }, { injectAnalogReader: nil, @@ -650,7 +653,7 @@ func TestServerGetDigitalInterruptValue(t *testing.T) { expCapDigitalInterruptArgs: []interface{}(nil), expCapArgs: []interface{}(nil), expResp: nil, - expRespErr: "not found", + expRespErr: errUnimplemented.Error(), }, { injectDigitalInterrupt: nil, diff --git a/components/board/ti/board.go b/components/board/ti/board.go index 36ec63c8d00..701cbe06c89 100644 --- a/components/board/ti/board.go +++ b/components/board/ti/board.go @@ -22,5 +22,5 @@ func init() { golog.Global().Debugw("error getting ti GPIO board mapping", "error", err) } - genericlinux.RegisterBoard(modelName, gpioMappings, false) + genericlinux.RegisterBoard(modelName, gpioMappings) } diff --git a/components/board/ti/data.go b/components/board/ti/data.go index 185b99065c7..70bb7e3ee44 100644 --- a/components/board/ti/data.go +++ b/components/board/ti/data.go @@ -9,33 +9,33 @@ var boardInfoMappings = map[string]genericlinux.BoardInformation{ []genericlinux.PinDefinition{ // Pins 3 and 5 don't work as GPIO by default; you might need to disable the I2C bus to // use them. - {map[int]int{128: 84}, map[int]string{}, "600000.gpio", 3, 2, "GPIO0_84", "", "", -1}, - {map[int]int{128: 83}, map[int]string{}, "600000.gpio", 5, 3, "GPIO0_83", "", "", -1}, + {Name: "3", DeviceName: "gpiochip1", LineNumber: 84, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "5", DeviceName: "gpiochip1", LineNumber: 83, PwmChipSysfsDir: "", PwmID: -1}, // Pin 7 appears to be input-only, due to some sort of hardware limitation. - {map[int]int{128: 7}, map[int]string{}, "600000.gpio", 7, 4, "GPIO0_7", "", "", -1}, - {map[int]int{128: 70}, map[int]string{}, "600000.gpio", 8, 14, "GPIO0_70", "", "", -1}, - {map[int]int{128: 81}, map[int]string{}, "600000.gpio", 10, 15, "GPIO0_81", "", "", -1}, - {map[int]int{128: 71}, map[int]string{}, "600000.gpio", 11, 17, "GPIO0_71", "", "", -1}, - {map[int]int{128: 1}, map[int]string{}, "600000.gpio", 12, 18, "GPIO0_1", "", "", -1}, - {map[int]int{128: 82}, map[int]string{}, "600000.gpio", 13, 27, "GPIO0_82", "", "", -1}, - {map[int]int{128: 11}, map[int]string{}, "600000.gpio", 15, 22, "GPIO0_11", "", "", -1}, - {map[int]int{128: 5}, map[int]string{}, "600000.gpio", 16, 23, "GPIO0_5", "", "", -1}, - {map[int]int{36: 12}, map[int]string{}, "601000.gpio", 18, 24, "GPIO0_12", "", "", -1}, - {map[int]int{128: 101}, map[int]string{}, "600000.gpio", 19, 10, "GPIO0_101", "", "", -1}, - {map[int]int{128: 107}, map[int]string{}, "600000.gpio", 21, 9, "GPIO0_107", "", "", -1}, - {map[int]int{128: 8}, map[int]string{}, "600000.gpio", 22, 25, "GPIO0_8", "", "", -1}, - {map[int]int{128: 103}, map[int]string{}, "600000.gpio", 23, 11, "GPIO0_103", "", "", -1}, - {map[int]int{128: 102}, map[int]string{}, "600000.gpio", 24, 8, "GPIO0_102", "", "", -1}, - {map[int]int{128: 108}, map[int]string{}, "600000.gpio", 26, 7, "GPIO0_108", "", "", -1}, - {map[int]int{128: 93}, map[int]string{}, "600000.gpio", 29, 5, "GPIO0_93", "", "3020000.pwm", 0}, - {map[int]int{128: 94}, map[int]string{}, "600000.gpio", 31, 6, "GPIO0_94", "", "3020000.pwm", 1}, - {map[int]int{128: 98}, map[int]string{}, "600000.gpio", 32, 12, "GPIO0_98", "", "3030000.pwm", 0}, - {map[int]int{128: 99}, map[int]string{}, "600000.gpio", 33, 13, "GPIO0_99", "", "3030000.pwm", 1}, - {map[int]int{128: 2}, map[int]string{}, "600000.gpio", 35, 19, "GPIO0_2", "", "", -1}, - {map[int]int{128: 97}, map[int]string{}, "600000.gpio", 36, 16, "GPIO0_97", "", "", -1}, - {map[int]int{128: 115}, map[int]string{}, "600000.gpio", 37, 26, "GPIO0_115", "", "", -1}, - {map[int]int{128: 3}, map[int]string{}, "600000.gpio", 38, 20, "GPIO0_3", "", "", -1}, - {map[int]int{128: 4}, map[int]string{}, "600000.gpio", 40, 21, "GPIO0_4", "", "", -1}, + {Name: "7", DeviceName: "gpiochip1", LineNumber: 7, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "8", DeviceName: "gpiochip1", LineNumber: 70, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "10", DeviceName: "gpiochip1", LineNumber: 81, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "11", DeviceName: "gpiochip1", LineNumber: 71, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "12", DeviceName: "gpiochip1", LineNumber: 1, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "13", DeviceName: "gpiochip1", LineNumber: 82, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "15", DeviceName: "gpiochip1", LineNumber: 11, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "16", DeviceName: "gpiochip1", LineNumber: 5, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "18", DeviceName: "gpiochip2", LineNumber: 12, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "19", DeviceName: "gpiochip1", LineNumber: 101, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "21", DeviceName: "gpiochip1", LineNumber: 107, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "22", DeviceName: "gpiochip1", LineNumber: 8, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "23", DeviceName: "gpiochip1", LineNumber: 103, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "24", DeviceName: "gpiochip1", LineNumber: 102, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "26", DeviceName: "gpiochip1", LineNumber: 108, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "29", DeviceName: "gpiochip1", LineNumber: 93, PwmChipSysfsDir: "3020000.pwm", PwmID: 0}, + {Name: "31", DeviceName: "gpiochip1", LineNumber: 94, PwmChipSysfsDir: "3020000.pwm", PwmID: 1}, + {Name: "32", DeviceName: "gpiochip1", LineNumber: 98, PwmChipSysfsDir: "3030000.pwm", PwmID: 0}, + {Name: "33", DeviceName: "gpiochip1", LineNumber: 99, PwmChipSysfsDir: "3030000.pwm", PwmID: 1}, + {Name: "35", DeviceName: "gpiochip1", LineNumber: 2, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "36", DeviceName: "gpiochip1", LineNumber: 97, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "37", DeviceName: "gpiochip1", LineNumber: 115, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "38", DeviceName: "gpiochip1", LineNumber: 3, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "40", DeviceName: "gpiochip1", LineNumber: 4, PwmChipSysfsDir: "", PwmID: -1}, }, []string{"ti,j721e-sk", "ti,j721e"}, }, diff --git a/components/board/upboard/board.go b/components/board/upboard/board.go index c00142505cd..8b553e00ca9 100644 --- a/components/board/upboard/board.go +++ b/components/board/upboard/board.go @@ -28,6 +28,5 @@ func init() { golog.Global().Debugw("error getting up board GPIO board mapping", "error", err) } - // Not using Periph io for GPIO - genericlinux.RegisterBoard(modelName, gpioMappings, false) + genericlinux.RegisterBoard(modelName, gpioMappings) } diff --git a/components/board/upboard/data.go b/components/board/upboard/data.go index a3ab1d50530..d69b98a3966 100644 --- a/components/board/upboard/data.go +++ b/components/board/upboard/data.go @@ -11,45 +11,38 @@ var boardInfoMappings = map[string]genericlinux.BoardInformation{ []genericlinux.PinDefinition{ /* pinout for up4000: https://github.com/up-board/up-community/wiki/Pinout_UP4000 - GPIOChipRelativeIDs: {ngpio : base-linux_gpio_number} GPIOChipSysFSDir: path to the directory of a chip. Can be found from the output of gpiodetect */ // GPIO pin definition - {map[int]int{78: 73}, map[int]string{}, "INT3452:01", 29, 0, "GPIO10", "", "", -1}, - {map[int]int{77: 46}, map[int]string{}, "INT3452:01", 31, 0, "BCM26", "", "", -1}, - {map[int]int{77: 48}, map[int]string{}, "INT3452:01", 18, 0, "BCM24", "", "", -1}, - {map[int]int{77: 45}, map[int]string{}, "INT3452:01", 22, 0, "BCM25", "", "", -1}, - {map[int]int{77: 46}, map[int]string{}, "INT3452:01", 37, 0, "BCM26", "", "", -1}, - {map[int]int{47: 17}, map[int]string{}, "INT3452:02", 35, 0, "BCM19", "", "", -1}, - {map[int]int{77: 75}, map[int]string{}, "INT3452:01", 13, 0, "BMC27", "", "", -1}, - - // ttyS4 UART - {map[int]int{78: 43}, map[int]string{}, "INT3452:00", 8, 0, "BCM14_TXD", "", "", -1}, - {map[int]int{78: 42}, map[int]string{}, "INT3452:00", 10, 0, "BCM15_RXD", "", "", -1}, - {map[int]int{78: 44}, map[int]string{}, "INT3452:00", 11, 0, "BCM17", "", "", -1}, - {map[int]int{78: 45}, map[int]string{}, "INT3452:00", 36, 0, "BMC16", "", "", -1}, - - // I2c - {map[int]int{78: 28}, map[int]string{}, "INT3452:00", 3, 0, "BCM2_SDA", "", "", -1}, - {map[int]int{78: 29}, map[int]string{}, "INT3452:00", 5, 0, "BVM3_SCL", "", "", -1}, - {map[int]int{78: 31}, map[int]string{}, "INT3452:00", 28, 0, "BMC1_ID_SCL", "", "", -1}, - - // pwm - {map[int]int{78: 35}, map[int]string{}, "INT3452:00", 33, 0, "BMC13_PWM1", "", "0000:00:1a.0", 0}, - {map[int]int{78: 34}, map[int]string{}, "INT3452:00", 32, 0, "BCM12_PWM0", "", "0000:00:1a.0", 1}, - {map[int]int{78: 37}, map[int]string{}, "INT3452:00", 16, 0, "BCM23", "", "0000:00:1a.0", 3}, - - {map[int]int{77: 76}, map[int]string{}, "INT3452:01", 7, 0, "BCM4", "", "", -1}, - {map[int]int{77: 65}, map[int]string{}, "INT3452:01", 19, 0, "BCM10_MOSI", "", "", -1}, - {map[int]int{77: 64}, map[int]string{}, "INT3452:01", 21, 0, "BCM9_MISO", "", "", -1}, - {map[int]int{77: 61}, map[int]string{}, "INT3452:01", 23, 0, "BCM11_SCLK", "", "", -1}, - {map[int]int{78: 30}, map[int]string{}, "INT3452:00", 27, 0, "BCM0_ID_SD", "", "", -1}, - {map[int]int{47: 16}, map[int]string{}, "INT3452:02", 12, 0, "BCM15_RXD", "", "", -1}, - {map[int]int{77: 62}, map[int]string{}, "INT3452:01", 24, 0, "BCM8_CE0", "", "", -1}, - {map[int]int{77: 63}, map[int]string{}, "INT3452:01", 26, 0, "BCM7_CE1", "", "", -1}, - {map[int]int{47: 18}, map[int]string{}, "INT3452:02", 38, 0, "BCM20", "", "", -1}, - {map[int]int{47: 19}, map[int]string{}, "INT3452:02", 40, 0, "BCM21", "", "", -1}, - {map[int]int{77: 74}, map[int]string{}, "INT3452:01", 15, 0, "BCM22", "", "", -1}, + {Name: "3", DeviceName: "gpiochip4", LineNumber: 2, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "5", DeviceName: "gpiochip4", LineNumber: 3, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "7", DeviceName: "gpiochip4", LineNumber: 4, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "8", DeviceName: "gpiochip4", LineNumber: 14, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "10", DeviceName: "gpiochip4", LineNumber: 15, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "11", DeviceName: "gpiochip4", LineNumber: 17, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "12", DeviceName: "gpiochip4", LineNumber: 18, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "13", DeviceName: "gpiochip4", LineNumber: 27, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "15", DeviceName: "gpiochip4", LineNumber: 22, PwmChipSysfsDir: "", PwmID: -1}, + // Pin 16 supposedly has hardware PWM from pwmID 3, but we haven't gotten it to work. + {Name: "16", DeviceName: "gpiochip4", LineNumber: 23, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "18", DeviceName: "gpiochip4", LineNumber: 24, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "19", DeviceName: "gpiochip4", LineNumber: 10, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "21", DeviceName: "gpiochip4", LineNumber: 9, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "22", DeviceName: "gpiochip4", LineNumber: 25, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "23", DeviceName: "gpiochip4", LineNumber: 11, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "24", DeviceName: "gpiochip4", LineNumber: 8, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "26", DeviceName: "gpiochip4", LineNumber: 7, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "27", DeviceName: "gpiochip4", LineNumber: 0, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "28", DeviceName: "gpiochip4", LineNumber: 1, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "29", DeviceName: "gpiochip4", LineNumber: 5, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "31", DeviceName: "gpiochip4", LineNumber: 6, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "32", DeviceName: "gpiochip4", LineNumber: 12, PwmChipSysfsDir: "0000:00:1a.0", PwmID: 0}, + {Name: "33", DeviceName: "gpiochip4", LineNumber: 13, PwmChipSysfsDir: "0000:00:1a.0", PwmID: 1}, + {Name: "35", DeviceName: "gpiochip4", LineNumber: 19, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "36", DeviceName: "gpiochip4", LineNumber: 16, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "37", DeviceName: "gpiochip4", LineNumber: 26, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "38", DeviceName: "gpiochip4", LineNumber: 20, PwmChipSysfsDir: "", PwmID: -1}, + {Name: "40", DeviceName: "gpiochip4", LineNumber: 21, PwmChipSysfsDir: "", PwmID: -1}, }, []string{"UP-APL03"}, }, diff --git a/components/camera/camera.go b/components/camera/camera.go index 7612ab20726..38c49b1db25 100644 --- a/components/camera/camera.go +++ b/components/camera/camera.go @@ -5,6 +5,7 @@ import ( "context" "image" "sync" + "time" "github.com/pion/mediadevices/pkg/prop" "github.com/pkg/errors" @@ -62,6 +63,12 @@ type Properties struct { DistortionParams transform.Distorter } +// NamedImage is a struct that associates the source from where the image came from to the Image. +type NamedImage struct { + Image image.Image + SourceName string +} + // A Camera is a resource that can capture frames. type Camera interface { resource.Resource @@ -72,6 +79,9 @@ type Camera interface { type VideoSource interface { projectorProvider + // Images is used for getting simultaneous images from different imagers, + // along with associated metadata (just timestamp for now). It's not for getting a time series of images from the same imager. + Images(ctx context.Context) ([]NamedImage, resource.ResponseMetadata, error) // Stream returns a stream that makes a best effort to return consecutive images // that may have a MIME type hint dictated in the context via gostream.WithMIMETypeHint. Stream(ctx context.Context, errHandlers ...gostream.ErrorHandler) (gostream.VideoStream, error) @@ -99,6 +109,11 @@ type PointCloudSource interface { NextPointCloud(ctx context.Context) (pointcloud.PointCloud, error) } +// A ImagesSource is a source that can return a list of images with timestamp. +type ImagesSource interface { + Images(ctx context.Context) ([]NamedImage, resource.ResponseMetadata, error) +} + // FromVideoSource creates a Camera resource from a VideoSource. // Note: this strips away Reconfiguration and DoCommand abilities. // If needed, implement the Camera another way. For example, a webcam @@ -230,6 +245,28 @@ func (vs *videoSource) Stream(ctx context.Context, errHandlers ...gostream.Error return vs.videoSource.Stream(ctx, errHandlers...) } +// Images is for getting simultaneous images from different sensors +// If the underlying source did not specify an Images function, a default is applied. +// The default returns a list of 1 image from ReadImage, and the current time. +func (vs *videoSource) Images(ctx context.Context) ([]NamedImage, resource.ResponseMetadata, error) { + ctx, span := trace.StartSpan(ctx, "camera::videoSource::Images") + defer span.End() + if c, ok := vs.actualSource.(ImagesSource); ok { + return c.Images(ctx) + } + img, release, err := ReadImage(ctx, vs.videoSource) + if err != nil { + return nil, resource.ResponseMetadata{}, errors.Wrap(err, "videoSource: call to get Images failed") + } + defer func() { + if release != nil { + release() + } + }() + ts := time.Now() + return []NamedImage{{img, ""}}, resource.ResponseMetadata{CapturedAt: ts}, nil +} + // NextPointCloud returns the next PointCloud from the camera, or will error if not supported. func (vs *videoSource) NextPointCloud(ctx context.Context) (pointcloud.PointCloud, error) { ctx, span := trace.StartSpan(ctx, "camera::videoSource::NextPointCloud") diff --git a/components/camera/camera_test.go b/components/camera/camera_test.go index be891589b47..b155f7d6409 100644 --- a/components/camera/camera_test.go +++ b/components/camera/camera_test.go @@ -243,6 +243,13 @@ func TestCameraWithProjector(t *testing.T) { test.That(t, err, test.ShouldBeNil) test.That(t, depthImg.Bounds().Dx(), test.ShouldEqual, 1280) test.That(t, depthImg.Bounds().Dy(), test.ShouldEqual, 720) + // cam2 should implement a default GetImages, that just returns the one image + images, _, err := cam2.Images(context.Background()) + test.That(t, err, test.ShouldBeNil) + test.That(t, len(images), test.ShouldEqual, 1) + test.That(t, images[0].Image, test.ShouldHaveSameTypeAs, &rimage.DepthMap{}) + test.That(t, images[0].Image.Bounds().Dx(), test.ShouldEqual, 1280) + test.That(t, images[0].Image.Bounds().Dy(), test.ShouldEqual, 720) test.That(t, cam2.Close(context.Background()), test.ShouldBeNil) } diff --git a/components/camera/client.go b/components/camera/client.go index ed4efa62e9b..9a16a1640ff 100644 --- a/components/camera/client.go +++ b/components/camera/client.go @@ -8,12 +8,14 @@ import ( "sync" "github.com/edaniels/golog" + "github.com/pkg/errors" "github.com/viamrobotics/gostream" "go.opencensus.io/trace" pb "go.viam.com/api/component/camera/v1" goutils "go.viam.com/utils" "go.viam.com/utils/rpc" + "go.viam.com/rdk/data" "go.viam.com/rdk/pointcloud" "go.viam.com/rdk/protoutils" "go.viam.com/rdk/resource" @@ -63,9 +65,16 @@ func (c *client) Read(ctx context.Context) (image.Image, func(), error) { defer span.End() mimeType := gostream.MIMETypeHint(ctx, "") expectedType, _ := utils.CheckLazyMIMEType(mimeType) + + ext, err := data.GetExtraFromContext(ctx) + if err != nil { + return nil, nil, err + } + resp, err := c.client.GetImage(ctx, &pb.GetImageRequest{ Name: c.name, MimeType: expectedType, + Extra: ext, }) if err != nil { return nil, nil, err @@ -136,14 +145,56 @@ func (c *client) Stream( return stream, nil } +func (c *client) Images(ctx context.Context) ([]NamedImage, resource.ResponseMetadata, error) { + ctx, span := trace.StartSpan(ctx, "camera::client::Images") + defer span.End() + + resp, err := c.client.GetImages(ctx, &pb.GetImagesRequest{ + Name: c.name, + }) + if err != nil { + return nil, resource.ResponseMetadata{}, errors.Wrap(err, "camera client: could not gets images from the camera") + } + + images := make([]NamedImage, 0, len(resp.Images)) + // keep everything lazy encoded by default, if type is unknown, attempt to decode it + for _, img := range resp.Images { + var rdkImage image.Image + switch img.Format { + case pb.Format_FORMAT_RAW_RGBA: + rdkImage = rimage.NewLazyEncodedImage(img.Image, utils.MimeTypeRawRGBA) + case pb.Format_FORMAT_RAW_DEPTH: + rdkImage = rimage.NewLazyEncodedImage(img.Image, utils.MimeTypeRawDepth) + case pb.Format_FORMAT_JPEG: + rdkImage = rimage.NewLazyEncodedImage(img.Image, utils.MimeTypeJPEG) + case pb.Format_FORMAT_PNG: + rdkImage = rimage.NewLazyEncodedImage(img.Image, utils.MimeTypePNG) + case pb.Format_FORMAT_UNSPECIFIED: + rdkImage, _, err = image.Decode(bytes.NewReader(img.Image)) + if err != nil { + return nil, resource.ResponseMetadata{}, err + } + } + images = append(images, NamedImage{rdkImage, img.SourceName}) + } + return images, resource.ResponseMetadataFromProto(resp.ResponseMetadata), nil +} + func (c *client) NextPointCloud(ctx context.Context) (pointcloud.PointCloud, error) { ctx, span := trace.StartSpan(ctx, "camera::client::NextPointCloud") defer span.End() ctx, getPcdSpan := trace.StartSpan(ctx, "camera::client::NextPointCloud::GetPointCloud") + + ext, err := data.GetExtraFromContext(ctx) + if err != nil { + return nil, err + } + resp, err := c.client.GetPointCloud(ctx, &pb.GetPointCloudRequest{ Name: c.name, MimeType: utils.MimeTypePCD, + Extra: ext, }) getPcdSpan.End() if err != nil { @@ -200,6 +251,9 @@ func (c *client) Properties(ctx context.Context) (Properties, error) { if resp.DistortionParameters == nil { return result, nil } + if resp.DistortionParameters.Model == "" { // same as if nil + return result, nil + } // switch distortion model based on model name model := transform.DistortionType(resp.DistortionParameters.Model) distorter, err := transform.NewDistorter(model, resp.DistortionParameters.Parameters) diff --git a/components/camera/client_test.go b/components/camera/client_test.go index fe017ad2820..f0d4f3b8ade 100644 --- a/components/camera/client_test.go +++ b/components/camera/client_test.go @@ -3,12 +3,13 @@ package camera_test import ( "bytes" "context" - "errors" "image" + "image/color" "image/png" "net" "sync" "testing" + "time" "github.com/edaniels/golog" "github.com/viamrobotics/gostream" @@ -74,6 +75,18 @@ func TestClient(t *testing.T) { injectCamera.ProjectorFunc = func(ctx context.Context) (transform.Projector, error) { return projA, nil } + injectCamera.ImagesFunc = func(ctx context.Context) ([]camera.NamedImage, resource.ResponseMetadata, error) { + images := []camera.NamedImage{} + // one color image + color := rimage.NewImage(40, 50) + images = append(images, camera.NamedImage{color, "color"}) + // one depth image + depth := rimage.NewEmptyDepthMap(10, 20) + images = append(images, camera.NamedImage{depth, "depth"}) + // a timestamp of 12345 + ts := time.UnixMilli(12345) + return images, resource.ResponseMetadata{ts}, nil + } injectCamera.StreamFunc = func(ctx context.Context, errHandlers ...gostream.ErrorHandler) (gostream.VideoStream, error) { return gostream.NewEmbeddedVideoStreamFromReader(gostream.VideoReaderFunc(func(ctx context.Context) (image.Image, func(), error) { imageReleasedMu.Lock() @@ -113,16 +126,16 @@ func TestClient(t *testing.T) { // bad camera injectCamera2 := &inject.Camera{} injectCamera2.NextPointCloudFunc = func(ctx context.Context) (pointcloud.PointCloud, error) { - return nil, errors.New("can't generate next point cloud") + return nil, errGeneratePointCloudFailed } injectCamera2.PropertiesFunc = func(ctx context.Context) (camera.Properties, error) { - return camera.Properties{}, errors.New("can't get camera properties") + return camera.Properties{}, errPropertiesFailed } injectCamera2.ProjectorFunc = func(ctx context.Context) (transform.Projector, error) { - return nil, errors.New("can't get camera properties") + return nil, errCameraProjectorFailed } injectCamera2.StreamFunc = func(ctx context.Context, errHandlers ...gostream.ErrorHandler) (gostream.VideoStream, error) { - return nil, errors.New("can't generate stream") + return nil, errStreamFailed } resources := map[resource.Name]camera.Camera{ @@ -147,7 +160,7 @@ func TestClient(t *testing.T) { cancel() _, err := viamgrpc.Dial(cancelCtx, listener1.Addr().String(), logger) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "canceled") + test.That(t, err, test.ShouldBeError, context.Canceled) }) t.Run("camera client 1", func(t *testing.T) { @@ -179,6 +192,21 @@ func TestClient(t *testing.T) { test.That(t, propsB.SupportsPCD, test.ShouldBeTrue) test.That(t, propsB.IntrinsicParams, test.ShouldResemble, intrinsics) + images, meta, err := camera1Client.Images(context.Background()) + test.That(t, err, test.ShouldBeNil) + test.That(t, meta.CapturedAt, test.ShouldEqual, time.UnixMilli(12345)) + test.That(t, len(images), test.ShouldEqual, 2) + test.That(t, images[0].SourceName, test.ShouldEqual, "color") + test.That(t, images[0].Image.Bounds().Dx(), test.ShouldEqual, 40) + test.That(t, images[0].Image.Bounds().Dy(), test.ShouldEqual, 50) + test.That(t, images[0].Image, test.ShouldHaveSameTypeAs, &rimage.LazyEncodedImage{}) + test.That(t, images[0].Image.ColorModel(), test.ShouldHaveSameTypeAs, color.RGBAModel) + test.That(t, images[1].SourceName, test.ShouldEqual, "depth") + test.That(t, images[1].Image.Bounds().Dx(), test.ShouldEqual, 10) + test.That(t, images[1].Image.Bounds().Dy(), test.ShouldEqual, 20) + test.That(t, images[1].Image, test.ShouldHaveSameTypeAs, &rimage.LazyEncodedImage{}) + test.That(t, images[1].Image.ColorModel(), test.ShouldHaveSameTypeAs, color.Gray16Model) + // Do resp, err := camera1Client.DoCommand(context.Background(), testutils.TestCommand) test.That(t, err, test.ShouldBeNil) @@ -217,19 +245,19 @@ func TestClient(t *testing.T) { _, _, err = camera.ReadImage(context.Background(), client2) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "can't generate stream") + test.That(t, err.Error(), test.ShouldContainSubstring, errStreamFailed.Error()) _, err = client2.NextPointCloud(context.Background()) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "can't generate next point cloud") + test.That(t, err.Error(), test.ShouldContainSubstring, errGeneratePointCloudFailed.Error()) _, err = client2.Projector(context.Background()) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "can't get camera properties") + test.That(t, err.Error(), test.ShouldContainSubstring, errCameraProjectorFailed.Error()) _, err = client2.Properties(context.Background()) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "can't get camera properties") + test.That(t, err.Error(), test.ShouldContainSubstring, errPropertiesFailed.Error()) test.That(t, conn.Close(), test.ShouldBeNil) }) @@ -348,7 +376,7 @@ func TestClientLazyImage(t *testing.T) { case rutils.MimeTypePNG: return imgPng, func() {}, nil default: - return nil, nil, errors.New("invalid mime type") + return nil, nil, errInvalidMimeType } })), nil } diff --git a/components/camera/collectors.go b/components/camera/collectors.go index 95a825bbdf3..a3f5c7f6219 100644 --- a/components/camera/collectors.go +++ b/components/camera/collectors.go @@ -44,8 +44,15 @@ func newNextPointCloudCollector(resource interface{}, params data.CollectorParam _, span := trace.StartSpan(ctx, "camera::data::collector::CaptureFunc::NextPointCloud") defer span.End() + ctx = context.WithValue(ctx, data.FromDMContextKey{}, true) + v, err := camera.NextPointCloud(ctx) if err != nil { + // A modular filter component can be created to filter the readings from a component. The error ErrNoCaptureToStore + // is used in the datamanager to exclude readings from being captured and stored. + if errors.Is(err, data.ErrNoCaptureToStore) { + return nil, err + } return nil, data.FailedToReadErr(params.ComponentName, nextPointCloud.String(), err) } @@ -81,8 +88,16 @@ func newReadImageCollector(resource interface{}, params data.CollectorParams) (d _, span := trace.StartSpan(ctx, "camera::data::collector::CaptureFunc::ReadImage") defer span.End() + ctx = context.WithValue(ctx, data.FromDMContextKey{}, true) + img, release, err := ReadImage(ctx, camera) if err != nil { + // A modular filter component can be created to filter the readings from a component. The error ErrNoCaptureToStore + // is used in the datamanager to exclude readings from being captured and stored. + if errors.Is(err, data.ErrNoCaptureToStore) { + return nil, err + } + return nil, data.FailedToReadErr(params.ComponentName, readImage.String(), err) } defer func() { diff --git a/components/camera/replaypcd/replaypcd.go b/components/camera/replaypcd/replaypcd.go index 5e662a4754a..806bf58883e 100644 --- a/components/camera/replaypcd/replaypcd.go +++ b/components/camera/replaypcd/replaypcd.go @@ -35,8 +35,10 @@ const ( var ( // model is the model of a replay camera. - model = resource.DefaultModelFamily.WithModel("replay_pcd") - errEndOfDataset = errors.New("reached end of dataset") + model = resource.DefaultModelFamily.WithModel("replay_pcd") + + // ErrEndOfDataset represents that the replay sensor has reached the end of the dataset. + ErrEndOfDataset = errors.New("reached end of dataset") ) func init() { @@ -47,10 +49,12 @@ func init() { // Config describes how to configure the replay camera component. type Config struct { - Source string `json:"source,omitempty"` - RobotID string `json:"robot_id,omitempty"` - Interval TimeInterval `json:"time_interval,omitempty"` - BatchSize *uint64 `json:"batch_size,omitempty"` + Source string `json:"source,omitempty"` + RobotID string `json:"robot_id,omitempty"` + LocationID string `json:"location_id,omitempty"` + OrganizationID string `json:"organization_id,omitempty"` + Interval TimeInterval `json:"time_interval,omitempty"` + BatchSize *uint64 `json:"batch_size,omitempty"` } // TimeInterval holds the start and end time used to filter data. @@ -75,6 +79,18 @@ func (cfg *Config) Validate(path string) ([]string, error) { return nil, goutils.NewConfigValidationFieldRequiredError(path, "source") } + if cfg.RobotID == "" { + return nil, goutils.NewConfigValidationFieldRequiredError(path, "robot_id") + } + + if cfg.LocationID == "" { + return nil, goutils.NewConfigValidationFieldRequiredError(path, "location_id") + } + + if cfg.OrganizationID == "" { + return nil, goutils.NewConfigValidationFieldRequiredError(path, "organization_id") + } + var err error var startTime time.Time if cfg.Interval.Start != "" { @@ -177,7 +193,7 @@ func (replay *pcdCamera) NextPointCloud(ctx context.Context) (pointcloud.PointCl } if len(resp.GetData()) == 0 { - return nil, errEndOfDataset + return nil, ErrEndOfDataset } replay.lastData = resp.GetLast() @@ -287,10 +303,17 @@ func addGRPCMetadata(ctx context.Context, timeRequested, timeReceived *timestamp return nil } -// Properties is a part of the camera interface but is not implemented for replay. +// Images is a part of the camera interface but is not implemented for replay. +func (replay *pcdCamera) Images(ctx context.Context) ([]camera.NamedImage, resource.ResponseMetadata, error) { + return nil, resource.ResponseMetadata{}, errors.New("Images is unimplemented") +} + +// Properties is a part of the camera interface and returns the camera.Properties struct with SupportsPCD set to true. func (replay *pcdCamera) Properties(ctx context.Context) (camera.Properties, error) { - var props camera.Properties - return props, errors.New("Properties is unimplemented") + props := camera.Properties{ + SupportsPCD: true, + } + return props, nil } // Projector is a part of the camera interface but is not implemented for replay. @@ -354,10 +377,12 @@ func (replay *pcdCamera) Reconfigure(ctx context.Context, deps resource.Dependen replay.cache = nil replay.filter = &datapb.Filter{ - ComponentName: replayCamConfig.Source, - RobotId: replayCamConfig.RobotID, - MimeType: []string{"pointcloud/pcd"}, - Interval: &datapb.CaptureInterval{}, + ComponentName: replayCamConfig.Source, + RobotId: replayCamConfig.RobotID, + LocationIds: []string{replayCamConfig.LocationID}, + OrganizationIds: []string{replayCamConfig.OrganizationID}, + MimeType: []string{"pointcloud/pcd"}, + Interval: &datapb.CaptureInterval{}, } replay.lastData = "" diff --git a/components/camera/replaypcd/replaypcd_test.go b/components/camera/replaypcd/replaypcd_test.go index 16690033084..6bbe004225f 100644 --- a/components/camera/replaypcd/replaypcd_test.go +++ b/components/camera/replaypcd/replaypcd_test.go @@ -17,7 +17,13 @@ import ( "go.viam.com/rdk/utils/contextutils" ) -const datasetDirectory = "slam/mock_lidar/%d.pcd" +const ( + datasetDirectory = "slam/mock_lidar/%d.pcd" + validSource = "source" + validRobotID = "robot_id" + validOrganizationID = "organization_id" + validLocationID = "location_id" +) var ( numPCDFiles = 15 @@ -30,7 +36,7 @@ var ( batchSizeTooLarge = uint64(1000) ) -func TestNewReplayPCD(t *testing.T) { +func TestReplayPCDNew(t *testing.T) { ctx := context.Background() cases := []struct { @@ -42,14 +48,20 @@ func TestNewReplayPCD(t *testing.T) { { description: "valid config with internal cloud service", cfg: &Config{ - Source: "source", + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, }, validCloudConnection: true, }, { description: "bad internal cloud service", cfg: &Config{ - Source: "source", + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, }, validCloudConnection: false, expectedErr: errors.New("failure to connect to the cloud: cloud connection error"), @@ -57,7 +69,10 @@ func TestNewReplayPCD(t *testing.T) { { description: "bad start timestamp", cfg: &Config{ - Source: "source", + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, Interval: TimeInterval{ Start: "bad timestamp", }, @@ -68,7 +83,10 @@ func TestNewReplayPCD(t *testing.T) { { description: "bad end timestamp", cfg: &Config{ - Source: "source", + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, Interval: TimeInterval{ End: "bad timestamp", }, @@ -99,7 +117,7 @@ func TestNewReplayPCD(t *testing.T) { } } -func TestNextPointCloud(t *testing.T) { +func TestReplayPCDNextPointCloud(t *testing.T) { ctx := context.Background() cases := []struct { @@ -111,7 +129,10 @@ func TestNextPointCloud(t *testing.T) { { description: "Calling NextPointCloud no filter", cfg: &Config{ - Source: "source", + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, }, startFileNum: 0, endFileNum: numPCDFiles, @@ -119,25 +140,43 @@ func TestNextPointCloud(t *testing.T) { { description: "Calling NextPointCloud with bad source", cfg: &Config{ - Source: "bad_source", + Source: "bad_source", + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, }, startFileNum: -1, endFileNum: -1, }, { - description: "Calling NextPointCloud with robot_id", + description: "Calling NextPointCloud with bad robot_id", cfg: &Config{ - Source: "source", - RobotID: "robot_id", + Source: validSource, + RobotID: "bad_robot_id", + LocationID: validLocationID, + OrganizationID: validOrganizationID, }, - startFileNum: 0, - endFileNum: numPCDFiles, + startFileNum: -1, + endFileNum: -1, }, { - description: "Calling NextPointCloud with bad robot_id", + description: "Calling NextPointCloud with bad location_id", + cfg: &Config{ + Source: validSource, + RobotID: validRobotID, + LocationID: "bad_location_id", + OrganizationID: validOrganizationID, + }, + startFileNum: -1, + endFileNum: -1, + }, + { + description: "Calling NextPointCloud with bad organization_id", cfg: &Config{ - Source: "source", - RobotID: "bad_robot_id", + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: "bad_organization_id", }, startFileNum: -1, endFileNum: -1, @@ -145,8 +184,11 @@ func TestNextPointCloud(t *testing.T) { { description: "Calling NextPointCloud with filter no data", cfg: &Config{ - Source: "source", - BatchSize: &batchSize1, + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + BatchSize: &batchSize1, Interval: TimeInterval{ Start: "2000-01-01T12:00:30Z", End: "2000-01-01T12:00:40Z", @@ -158,8 +200,11 @@ func TestNextPointCloud(t *testing.T) { { description: "Calling NextPointCloud with end filter", cfg: &Config{ - Source: "source", - BatchSize: &batchSize1, + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + BatchSize: &batchSize1, Interval: TimeInterval{ End: "2000-01-01T12:00:10Z", }, @@ -170,8 +215,11 @@ func TestNextPointCloud(t *testing.T) { { description: "Calling NextPointCloud with start filter", cfg: &Config{ - Source: "source", - BatchSize: &batchSize1, + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + BatchSize: &batchSize1, Interval: TimeInterval{ Start: "2000-01-01T12:00:05Z", }, @@ -182,8 +230,11 @@ func TestNextPointCloud(t *testing.T) { { description: "Calling NextPointCloud with start and end filter", cfg: &Config{ - Source: "source", - BatchSize: &batchSize1, + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + BatchSize: &batchSize1, Interval: TimeInterval{ Start: "2000-01-01T12:00:05Z", End: "2000-01-01T12:00:10Z", @@ -195,8 +246,11 @@ func TestNextPointCloud(t *testing.T) { { description: "Calling NextPointCloud with non-divisible batch size, last batch size 1", cfg: &Config{ - Source: "source", - BatchSize: &batchSize2, + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + BatchSize: &batchSize2, }, startFileNum: 0, endFileNum: numPCDFiles, @@ -204,8 +258,11 @@ func TestNextPointCloud(t *testing.T) { { description: "Calling NextPointCloud with non-divisible batch size, last batch > 1", cfg: &Config{ - Source: "source", - BatchSize: &batchSize4, + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + BatchSize: &batchSize4, }, startFileNum: 0, endFileNum: numPCDFiles, @@ -213,8 +270,11 @@ func TestNextPointCloud(t *testing.T) { { description: "Calling NextPointCloud with divisible batch size", cfg: &Config{ - Source: "source", - BatchSize: &batchSize3, + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + BatchSize: &batchSize3, }, startFileNum: 0, endFileNum: numPCDFiles, @@ -222,8 +282,11 @@ func TestNextPointCloud(t *testing.T) { { description: "Calling NextPointCloud with batching and a start and end filter", cfg: &Config{ - Source: "source", - BatchSize: &batchSize2, + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + BatchSize: &batchSize2, Interval: TimeInterval{ Start: "2000-01-01T12:00:05Z", End: "2000-01-01T12:00:10Z", @@ -235,8 +298,11 @@ func TestNextPointCloud(t *testing.T) { { description: "Calling NextPointCloud with a large batch size", cfg: &Config{ - Source: "source", - BatchSize: &batchSizeLarge, + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + BatchSize: &batchSizeLarge, }, startFileNum: 0, endFileNum: numPCDFiles, @@ -268,7 +334,7 @@ func TestNextPointCloud(t *testing.T) { // Confirm the end of the dataset was reached when expected pc, err := replayCamera.NextPointCloud(ctx) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, errEndOfDataset.Error()) + test.That(t, err.Error(), test.ShouldContainSubstring, ErrEndOfDataset.Error()) test.That(t, pc, test.ShouldBeNil) err = replayCamera.Close(ctx) @@ -282,7 +348,7 @@ func TestNextPointCloud(t *testing.T) { // TestLiveNextPointCloud checks the replay pcd camera's ability to handle new data being added to the // database the pool during a session, proving that NextPointCloud can return new data even after // returning errEndOfDataset. -func TestLiveNextPointCloud(t *testing.T) { +func TestReplayPCDLiveNextPointCloud(t *testing.T) { ctx := context.Background() numPCDFilesOriginal := numPCDFiles @@ -290,7 +356,10 @@ func TestLiveNextPointCloud(t *testing.T) { defer func() { numPCDFiles = numPCDFilesOriginal }() cfg := &Config{ - Source: "source", + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, } replayCamera, _, serverClose, err := createNewReplayPCDCamera(ctx, t, cfg, true) @@ -303,7 +372,7 @@ func TestLiveNextPointCloud(t *testing.T) { pc, err := replayCamera.NextPointCloud(ctx) if i == numPCDFiles { test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, errEndOfDataset.Error()) + test.That(t, err.Error(), test.ShouldContainSubstring, ErrEndOfDataset.Error()) test.That(t, pc, test.ShouldBeNil) // Add new files for future processing @@ -331,7 +400,7 @@ func TestLiveNextPointCloud(t *testing.T) { test.That(t, serverClose(), test.ShouldBeNil) } -func TestConfigValidation(t *testing.T) { +func TestReplayPCDConfigValidation(t *testing.T) { cases := []struct { description string cfg *Config @@ -341,23 +410,61 @@ func TestConfigValidation(t *testing.T) { { description: "Valid config with source and no timestamp", cfg: &Config{ - Source: "source", - Interval: TimeInterval{}, + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + Interval: TimeInterval{}, }, expectedDeps: []string{cloud.InternalServiceName.String()}, }, { - description: "Valid config with source and any robot id", + description: "Valid config with no source", cfg: &Config{ - Source: "source", - RobotID: "source", + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + Interval: TimeInterval{}, }, - expectedDeps: []string{cloud.InternalServiceName.String()}, + expectedErr: utils.NewConfigValidationFieldRequiredError("", validSource), + }, + { + description: "Valid config with no robot_id", + cfg: &Config{ + Source: validSource, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + Interval: TimeInterval{}, + }, + expectedErr: utils.NewConfigValidationFieldRequiredError("", validRobotID), + }, + { + description: "Valid config with no location_id", + cfg: &Config{ + Source: validSource, + RobotID: validRobotID, + OrganizationID: validOrganizationID, + Interval: TimeInterval{}, + }, + expectedErr: utils.NewConfigValidationFieldRequiredError("", validLocationID), + }, + { + description: "Valid config with no organization_id", + cfg: &Config{ + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + Interval: TimeInterval{}, + }, + expectedErr: utils.NewConfigValidationFieldRequiredError("", validOrganizationID), }, { description: "Valid config with start timestamp", cfg: &Config{ - Source: "source", + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, Interval: TimeInterval{ Start: "2000-01-01T12:00:00Z", }, @@ -367,7 +474,10 @@ func TestConfigValidation(t *testing.T) { { description: "Valid config with end timestamp", cfg: &Config{ - Source: "source", + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, Interval: TimeInterval{ End: "2000-01-01T12:00:00Z", }, @@ -377,7 +487,10 @@ func TestConfigValidation(t *testing.T) { { description: "Valid config with start and end timestamps", cfg: &Config{ - Source: "source", + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, Interval: TimeInterval{ Start: "2000-01-01T12:00:00Z", End: "2000-01-01T12:00:01Z", @@ -385,18 +498,13 @@ func TestConfigValidation(t *testing.T) { }, expectedDeps: []string{cloud.InternalServiceName.String()}, }, - { - description: "Invalid config no source and no timestamp", - cfg: &Config{ - Source: "", - Interval: TimeInterval{}, - }, - expectedErr: utils.NewConfigValidationFieldRequiredError("", "source"), - }, { description: "Invalid config with bad start timestamp format", cfg: &Config{ - Source: "source", + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, Interval: TimeInterval{ Start: "gibberish", }, @@ -406,7 +514,10 @@ func TestConfigValidation(t *testing.T) { { description: "Invalid config with bad end timestamp format", cfg: &Config{ - Source: "source", + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, Interval: TimeInterval{ End: "gibberish", }, @@ -416,7 +527,10 @@ func TestConfigValidation(t *testing.T) { { description: "Invalid config with bad start timestamp", cfg: &Config{ - Source: "source", + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, Interval: TimeInterval{ Start: "3000-01-01T12:00:00Z", }, @@ -426,7 +540,10 @@ func TestConfigValidation(t *testing.T) { { description: "Invalid config with bad end timestamp", cfg: &Config{ - Source: "source", + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, Interval: TimeInterval{ End: "3000-01-01T12:00:00Z", }, @@ -436,7 +553,10 @@ func TestConfigValidation(t *testing.T) { { description: "Invalid config with start after end timestamps", cfg: &Config{ - Source: "source", + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, Interval: TimeInterval{ Start: "2000-01-01T12:00:01Z", End: "2000-01-01T12:00:00Z", @@ -447,7 +567,10 @@ func TestConfigValidation(t *testing.T) { { description: "Invalid config with batch size above max", cfg: &Config{ - Source: "source", + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, Interval: TimeInterval{ Start: "2000-01-01T12:00:00Z", End: "2000-01-01T12:00:01Z", @@ -459,7 +582,10 @@ func TestConfigValidation(t *testing.T) { { description: "Invalid config with batch size 0", cfg: &Config{ - Source: "source", + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, Interval: TimeInterval{ Start: "2000-01-01T12:00:00Z", End: "2000-01-01T12:00:01Z", @@ -483,10 +609,15 @@ func TestConfigValidation(t *testing.T) { } } -func TestUnimplementedFunctions(t *testing.T) { +func TestReplayPCDUnimplementedFunctions(t *testing.T) { ctx := context.Background() - replayCamCfg := &Config{Source: "source"} + replayCamCfg := &Config{ + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + } replayCamera, _, serverClose, err := createNewReplayPCDCamera(ctx, t, replayCamCfg, true) test.That(t, err, test.ShouldBeNil) @@ -495,11 +626,6 @@ func TestUnimplementedFunctions(t *testing.T) { test.That(t, err.Error(), test.ShouldEqual, "Stream is unimplemented") }) - t.Run("Properties", func(t *testing.T) { - _, err := replayCamera.Properties(ctx) - test.That(t, err.Error(), test.ShouldEqual, "Properties is unimplemented") - }) - t.Run("Projector", func(t *testing.T) { _, err := replayCamera.Projector(ctx) test.That(t, err.Error(), test.ShouldEqual, "Projector is unimplemented") @@ -511,9 +637,7 @@ func TestUnimplementedFunctions(t *testing.T) { test.That(t, serverClose(), test.ShouldBeNil) } -// TestNextPointCloudTimestamps tests that calls to NextPointCloud on the replay camera will inject -// the time received and time requested metadata into the gRPC response header. -func TestNextPointCloudTimestamps(t *testing.T) { +func TestReplayPCDTimestamps(t *testing.T) { testCameraWithCfg := func(cfg *Config) { // Construct replay camera. ctx := context.Background() @@ -544,7 +668,7 @@ func TestNextPointCloudTimestamps(t *testing.T) { // Confirm the end of the dataset was reached when expected pc, err := replayCamera.NextPointCloud(ctx) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, errEndOfDataset.Error()) + test.That(t, err.Error(), test.ShouldContainSubstring, ErrEndOfDataset.Error()) test.That(t, pc, test.ShouldBeNil) err = replayCamera.Close(ctx) @@ -554,18 +678,58 @@ func TestNextPointCloudTimestamps(t *testing.T) { } t.Run("no batching", func(t *testing.T) { - cfg := &Config{Source: "source"} + cfg := &Config{ + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + } testCameraWithCfg(cfg) }) t.Run("with batching", func(t *testing.T) { - cfg := &Config{Source: "source", BatchSize: &batchSize2} + cfg := &Config{ + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + BatchSize: &batchSize2, + } testCameraWithCfg(cfg) }) } -func TestReconfigure(t *testing.T) { +func TestReplayPCDProperties(t *testing.T) { + // Construct replay camera. + ctx := context.Background() + cfg := &Config{ + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + BatchSize: &batchSize1, + } + replayCamera, _, serverClose, err := createNewReplayPCDCamera(ctx, t, cfg, true) + test.That(t, err, test.ShouldBeNil) + test.That(t, replayCamera, test.ShouldNotBeNil) + + props, err := replayCamera.Properties(ctx) + test.That(t, err, test.ShouldBeNil) + test.That(t, props.SupportsPCD, test.ShouldBeTrue) + + err = replayCamera.Close(ctx) + test.That(t, err, test.ShouldBeNil) + + test.That(t, serverClose(), test.ShouldBeNil) +} + +func TestReplayPCDReconfigure(t *testing.T) { // Construct replay camera - cfg := &Config{Source: "source"} + cfg := &Config{ + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + } ctx := context.Background() replayCamera, deps, serverClose, err := createNewReplayPCDCamera(ctx, t, cfg, true) test.That(t, err, test.ShouldBeNil) @@ -581,7 +745,7 @@ func TestReconfigure(t *testing.T) { } // Reconfigure with a new batch size - cfg = &Config{Source: "source", BatchSize: &batchSize4} + cfg = &Config{Source: validSource, BatchSize: &batchSize4} replayCamera.Reconfigure(ctx, deps, resource.Config{ConvertedAttributes: cfg}) // Call NextPointCloud a couple more times, ensuring that we start over from the beginning @@ -595,7 +759,7 @@ func TestReconfigure(t *testing.T) { } // Reconfigure again, batch size 1 - cfg = &Config{Source: "source", BatchSize: &batchSize1} + cfg = &Config{Source: validSource, BatchSize: &batchSize1} replayCamera.Reconfigure(ctx, deps, resource.Config{ConvertedAttributes: cfg}) // Again verify dataset starts from beginning @@ -610,7 +774,7 @@ func TestReconfigure(t *testing.T) { // Confirm the end of the dataset was reached when expected pc, err := replayCamera.NextPointCloud(ctx) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, errEndOfDataset.Error()) + test.That(t, err.Error(), test.ShouldContainSubstring, ErrEndOfDataset.Error()) test.That(t, pc, test.ShouldBeNil) err = replayCamera.Close(ctx) diff --git a/components/camera/replaypcd/replaypcd_utils_test.go b/components/camera/replaypcd/replaypcd_utils_test.go index da0e734afb1..d0be745d74f 100644 --- a/components/camera/replaypcd/replaypcd_utils_test.go +++ b/components/camera/replaypcd/replaypcd_utils_test.go @@ -236,13 +236,29 @@ func resourcesFromDeps(t *testing.T, r robot.Robot, deps []string) resource.Depe // the provided filter and last returned artifact. func getNextDataAfterFilter(filter *datapb.Filter, last string) (int, error) { // Basic component part (source) filter - if filter.ComponentName != "" && filter.ComponentName != "source" { - return 0, errEndOfDataset + if filter.ComponentName != "" && filter.ComponentName != validSource { + return 0, ErrEndOfDataset } // Basic robot_id filter - if filter.RobotId != "" && filter.RobotId != "robot_id" { - return 0, errEndOfDataset + if filter.RobotId != "" && filter.RobotId != validRobotID { + return 0, ErrEndOfDataset + } + + // Basic location_id filter + if len(filter.LocationIds) == 0 { + return 0, errors.New("LocationIds in filter is empty") + } + if filter.LocationIds[0] != "" && filter.LocationIds[0] != validLocationID { + return 0, ErrEndOfDataset + } + + // Basic organization_id filter + if len(filter.OrganizationIds) == 0 { + return 0, errors.New("OrganizationIds in filter is empty") + } + if filter.OrganizationIds[0] != "" && filter.OrganizationIds[0] != validOrganizationID { + return 0, ErrEndOfDataset } // Apply the time-based filter based on the seconds value in the start and end fields. Because artifacts @@ -274,7 +290,7 @@ func getFile(i, end int) (int, error) { if i < end { return i, nil } - return 0, errEndOfDataset + return 0, ErrEndOfDataset } // getCompressedBytesFromArtifact will return an array of bytes from the @@ -282,12 +298,12 @@ func getFile(i, end int) (int, error) { func getCompressedBytesFromArtifact(inputPath string) ([]byte, error) { artifactPath, err := artifact.Path(inputPath) if err != nil { - return nil, errEndOfDataset + return nil, ErrEndOfDataset } path := filepath.Clean(artifactPath) data, err := os.ReadFile(path) if err != nil { - return nil, errEndOfDataset + return nil, ErrEndOfDataset } var dataBuf bytes.Buffer diff --git a/components/camera/server.go b/components/camera/server.go index 693b27cb104..1dcd2f2f8dc 100644 --- a/components/camera/server.go +++ b/components/camera/server.go @@ -3,14 +3,17 @@ package camera import ( "bytes" "context" + "image" "github.com/edaniels/golog" + "github.com/pkg/errors" "github.com/viamrobotics/gostream" "go.opencensus.io/trace" commonpb "go.viam.com/api/common/v1" pb "go.viam.com/api/component/camera/v1" "google.golang.org/genproto/googleapis/api/httpbody" + "go.viam.com/rdk/data" "go.viam.com/rdk/pointcloud" "go.viam.com/rdk/protoutils" "go.viam.com/rdk/resource" @@ -69,6 +72,12 @@ func (s *serviceServer) GetImage( } req.MimeType = utils.WithLazyMIMEType(req.MimeType) + + // Add 'fromDataManagement' to context to avoid threading extra through gostream API. + if req.Extra.AsMap()[data.FromDMString] == true { + ctx = context.WithValue(ctx, data.FromDMContextKey{}, true) + } + img, release, err := ReadImage(gostream.WithMIMETypeHint(ctx, req.MimeType), cam) if err != nil { return nil, err @@ -90,6 +99,85 @@ func (s *serviceServer) GetImage( return &resp, nil } +// GetImages returns a list of images and metadata from a camera of the underlying robot. +func (s *serviceServer) GetImages( + ctx context.Context, + req *pb.GetImagesRequest, +) (*pb.GetImagesResponse, error) { + ctx, span := trace.StartSpan(ctx, "camera::server::GetImages") + defer span.End() + cam, err := s.coll.Resource(req.Name) + if err != nil { + return nil, errors.Wrap(err, "camera server GetImages had an error getting the camera component") + } + // request the images, and then check to see what the underlying type is to determine + // what to encode as. If it's color, just encode as JPEG. + imgs, metadata, err := cam.Images(ctx) + if err != nil { + return nil, errors.Wrap(err, "camera server GetImages could not call Images on the camera") + } + imagesMessage := make([]*pb.Image, 0, len(imgs)) + for _, img := range imgs { + format, outBytes, err := encodeImageFromUnderlyingType(ctx, img.Image) + if err != nil { + return nil, errors.Wrap(err, "camera server GetImages could not encode the images") + } + imgMes := &pb.Image{ + SourceName: img.SourceName, + Format: format, + Image: outBytes, + } + imagesMessage = append(imagesMessage, imgMes) + } + // right now the only metadata is timestamp + resp := &pb.GetImagesResponse{ + Images: imagesMessage, + ResponseMetadata: metadata.AsProto(), + } + + return resp, nil +} + +func encodeImageFromUnderlyingType(ctx context.Context, img image.Image) (pb.Format, []byte, error) { + switch v := img.(type) { + case *rimage.LazyEncodedImage: + format := pb.Format_FORMAT_UNSPECIFIED + switch v.MIMEType() { + case utils.MimeTypeRawDepth: + format = pb.Format_FORMAT_RAW_DEPTH + case utils.MimeTypeRawRGBA: + format = pb.Format_FORMAT_RAW_RGBA + case utils.MimeTypeJPEG: + format = pb.Format_FORMAT_JPEG + case utils.MimeTypePNG: + format = pb.Format_FORMAT_PNG + default: + } + return format, v.RawData(), nil + case *rimage.DepthMap: + format := pb.Format_FORMAT_RAW_DEPTH + outBytes, err := rimage.EncodeImage(ctx, v, utils.MimeTypeRawDepth) + if err != nil { + return pb.Format_FORMAT_UNSPECIFIED, nil, err + } + return format, outBytes, nil + case *image.Gray16: + format := pb.Format_FORMAT_PNG + outBytes, err := rimage.EncodeImage(ctx, v, utils.MimeTypePNG) + if err != nil { + return pb.Format_FORMAT_UNSPECIFIED, nil, err + } + return format, outBytes, nil + default: + format := pb.Format_FORMAT_JPEG + outBytes, err := rimage.EncodeImage(ctx, v, utils.MimeTypeJPEG) + if err != nil { + return pb.Format_FORMAT_UNSPECIFIED, nil, err + } + return format, outBytes, nil + } +} + // RenderFrame renders a frame from a camera of the underlying robot to an HTTP response. A specific MIME type // can be requested but may not necessarily be the same one returned. func (s *serviceServer) RenderFrame( diff --git a/components/camera/server_test.go b/components/camera/server_test.go index 6f65aeb22ff..04e4188451b 100644 --- a/components/camera/server_test.go +++ b/components/camera/server_test.go @@ -8,6 +8,7 @@ import ( "image/png" "sync" "testing" + "time" "github.com/viamrobotics/gostream" pb "go.viam.com/api/component/camera/v1" @@ -22,6 +23,15 @@ import ( "go.viam.com/rdk/utils" ) +var ( + errInvalidMimeType = errors.New("invalid mime type") + errGeneratePointCloudFailed = errors.New("can't generate next point cloud") + errPropertiesFailed = errors.New("can't get camera properties") + errCameraProjectorFailed = errors.New("can't get camera properties") + errStreamFailed = errors.New("can't generate stream") + errCameraUnimplemented = errors.New("not found") +) + func newServer() (pb.CameraServiceServer, *inject.Camera, *inject.Camera, *inject.Camera, error) { injectCamera := &inject.Camera{} injectCameraDepth := &inject.Camera{} @@ -80,6 +90,18 @@ func TestServer(t *testing.T) { IntrinsicParams: intrinsics, }, nil } + injectCamera.ImagesFunc = func(ctx context.Context) ([]camera.NamedImage, resource.ResponseMetadata, error) { + images := []camera.NamedImage{} + // one color image + color := rimage.NewImage(40, 50) + images = append(images, camera.NamedImage{color, "color"}) + // one depth image + depth := rimage.NewEmptyDepthMap(10, 20) + images = append(images, camera.NamedImage{depth, "depth"}) + // a timestamp of 12345 + ts := time.UnixMilli(12345) + return images, resource.ResponseMetadata{ts}, nil + } injectCamera.ProjectorFunc = func(ctx context.Context) (transform.Projector, error) { return projA, nil } @@ -100,7 +122,7 @@ func TestServer(t *testing.T) { case "image/woohoo": return rimage.NewLazyEncodedImage([]byte{1, 2, 3}, mimeType), func() {}, nil default: - return nil, nil, errors.New("invalid mime type") + return nil, nil, errInvalidMimeType } })), nil } @@ -136,22 +158,22 @@ func TestServer(t *testing.T) { } // bad camera injectCamera2.NextPointCloudFunc = func(ctx context.Context) (pointcloud.PointCloud, error) { - return nil, errors.New("can't generate next point cloud") + return nil, errGeneratePointCloudFailed } injectCamera2.PropertiesFunc = func(ctx context.Context) (camera.Properties, error) { - return camera.Properties{}, errors.New("can't get camera properties") + return camera.Properties{}, errPropertiesFailed } injectCamera2.ProjectorFunc = func(ctx context.Context) (transform.Projector, error) { - return nil, errors.New("can't get camera properties") + return nil, errCameraProjectorFailed } injectCamera2.StreamFunc = func(ctx context.Context, errHandlers ...gostream.ErrorHandler) (gostream.VideoStream, error) { - return nil, errors.New("can't generate stream") + return nil, errStreamFailed } // does a depth camera transfer its depth image properly t.Run("GetImage", func(t *testing.T) { _, err := cameraServer.GetImage(context.Background(), &pb.GetImageRequest{Name: missingCameraName}) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "not found") + test.That(t, err.Error(), test.ShouldContainSubstring, errCameraUnimplemented.Error()) // color camera // ensure that explicit RawRGBA mimetype request will return RawRGBA mimetype response @@ -212,7 +234,7 @@ func TestServer(t *testing.T) { MimeType: "image/who", }) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "invalid mime type") + test.That(t, err.Error(), test.ShouldContainSubstring, errInvalidMimeType.Error()) // depth camera imageReleasedMu.Lock() @@ -254,7 +276,7 @@ func TestServer(t *testing.T) { // bad camera _, err = cameraServer.GetImage(context.Background(), &pb.GetImageRequest{Name: failCameraName, MimeType: utils.MimeTypeRawRGBA}) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "can't generate stream") + test.That(t, err.Error(), test.ShouldContainSubstring, errStreamFailed.Error()) }) t.Run("GetImage with lazy", func(t *testing.T) { @@ -272,7 +294,7 @@ func TestServer(t *testing.T) { MimeType: "image/notwoo", }) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "invalid mime type") + test.That(t, err.Error(), test.ShouldContainSubstring, errInvalidMimeType.Error()) }) t.Run("GetImage with +lazy default", func(t *testing.T) { @@ -295,7 +317,7 @@ func TestServer(t *testing.T) { t.Run("RenderFrame", func(t *testing.T) { _, err := cameraServer.RenderFrame(context.Background(), &pb.RenderFrameRequest{Name: missingCameraName}) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "not found") + test.That(t, err.Error(), test.ShouldContainSubstring, errCameraUnimplemented.Error()) resp, err := cameraServer.RenderFrame(context.Background(), &pb.RenderFrameRequest{ Name: testCameraName, @@ -330,20 +352,20 @@ func TestServer(t *testing.T) { MimeType: "image/who", }) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "invalid mime type") + test.That(t, err.Error(), test.ShouldContainSubstring, errInvalidMimeType.Error()) imageReleasedMu.Lock() test.That(t, imageReleased, test.ShouldBeTrue) imageReleasedMu.Unlock() _, err = cameraServer.RenderFrame(context.Background(), &pb.RenderFrameRequest{Name: failCameraName}) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "can't generate stream") + test.That(t, err.Error(), test.ShouldContainSubstring, errStreamFailed.Error()) }) t.Run("GetPointCloud", func(t *testing.T) { _, err := cameraServer.GetPointCloud(context.Background(), &pb.GetPointCloudRequest{Name: missingCameraName}) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "not found") + test.That(t, err.Error(), test.ShouldContainSubstring, errCameraUnimplemented.Error()) pcA := pointcloud.New() err = pcA.Set(pointcloud.NewVector(5, 5, 5), nil) @@ -361,13 +383,27 @@ func TestServer(t *testing.T) { Name: failCameraName, }) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "can't generate next point cloud") + test.That(t, err.Error(), test.ShouldContainSubstring, errGeneratePointCloudFailed.Error()) + }) + t.Run("GetImages", func(t *testing.T) { + _, err := cameraServer.GetImages(context.Background(), &pb.GetImagesRequest{Name: missingCameraName}) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, errCameraUnimplemented.Error()) + + resp, err := cameraServer.GetImages(context.Background(), &pb.GetImagesRequest{Name: testCameraName}) + test.That(t, err, test.ShouldBeNil) + test.That(t, resp.ResponseMetadata.CapturedAt.AsTime(), test.ShouldEqual, time.UnixMilli(12345)) + test.That(t, len(resp.Images), test.ShouldEqual, 2) + test.That(t, resp.Images[0].Format, test.ShouldEqual, pb.Format_FORMAT_JPEG) + test.That(t, resp.Images[0].SourceName, test.ShouldEqual, "color") + test.That(t, resp.Images[1].Format, test.ShouldEqual, pb.Format_FORMAT_RAW_DEPTH) + test.That(t, resp.Images[1].SourceName, test.ShouldEqual, "depth") }) t.Run("GetProperties", func(t *testing.T) { _, err := cameraServer.GetProperties(context.Background(), &pb.GetPropertiesRequest{Name: missingCameraName}) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "not found") + test.That(t, err.Error(), test.ShouldContainSubstring, errCameraUnimplemented.Error()) resp, err := cameraServer.GetProperties(context.Background(), &pb.GetPropertiesRequest{Name: testCameraName}) test.That(t, err, test.ShouldBeNil) diff --git a/components/camera/transformpipeline/mods.go b/components/camera/transformpipeline/mods.go index 3178aa5ba3f..62c275b0ed7 100644 --- a/components/camera/transformpipeline/mods.go +++ b/components/camera/transformpipeline/mods.go @@ -18,15 +18,30 @@ import ( "go.viam.com/rdk/utils" ) +// rotateConfig are the attributes for a rotate transform. +type rotateConfig struct { + Angle float64 `json:"angle_degs"` +} + // rotateSource is the source to be rotated and the kind of image type. type rotateSource struct { originalStream gostream.VideoStream stream camera.ImageType + angle float64 } // newRotateTransform creates a new rotation transform. -func newRotateTransform(ctx context.Context, source gostream.VideoSource, stream camera.ImageType, +func newRotateTransform(ctx context.Context, source gostream.VideoSource, stream camera.ImageType, am utils.AttributeMap, ) (gostream.VideoSource, camera.ImageType, error) { + conf, err := resource.TransformAttributeMap[*rotateConfig](am) + if err != nil { + return nil, camera.UnspecifiedStream, errors.Wrap(err, "cannot parse rotate attribute map") + } + + if !am.Has("angle_degs") { + conf.Angle = 180 // Default to 180 for backwards-compatibility + } + props, err := propsFromVideoSource(ctx, source) if err != nil { return nil, camera.UnspecifiedStream, err @@ -37,7 +52,7 @@ func newRotateTransform(ctx context.Context, source gostream.VideoSource, stream if props.DistortionParams != nil { cameraModel.Distortion = props.DistortionParams } - reader := &rotateSource{gostream.NewEmbeddedVideoStream(source), stream} + reader := &rotateSource{gostream.NewEmbeddedVideoStream(source), stream, conf.Angle} src, err := camera.NewVideoSourceFromReader(ctx, reader, &cameraModel, stream) if err != nil { return nil, camera.UnspecifiedStream, err @@ -55,13 +70,15 @@ func (rs *rotateSource) Read(ctx context.Context) (image.Image, func(), error) { } switch rs.stream { case camera.ColorStream, camera.UnspecifiedStream: - return imaging.Rotate(orig, 180, color.Black), release, nil + // imaging.Rotate rotates an image counter-clockwise but our rotate function rotates in the + // clockwise direction. The angle is negated here for consistency. + return imaging.Rotate(orig, -(rs.angle), color.Black), release, nil case camera.DepthStream: dm, err := rimage.ConvertImageToDepthMap(ctx, orig) if err != nil { return nil, nil, err } - return dm.Rotate(180), release, nil + return dm.Rotate(int(rs.angle)), release, nil default: return nil, nil, camera.NewUnsupportedImageTypeError(rs.stream) } diff --git a/components/camera/transformpipeline/mods_test.go b/components/camera/transformpipeline/mods_test.go index 2836ddefc23..2ab19ac6dc7 100644 --- a/components/camera/transformpipeline/mods_test.go +++ b/components/camera/transformpipeline/mods_test.go @@ -155,7 +155,10 @@ func TestRotateColorSource(t *testing.T) { test.That(t, err, test.ShouldBeNil) source := gostream.NewVideoSource(&videosource.StaticSource{ColorImg: img}, prop.Video{}) - rs, stream, err := newRotateTransform(context.Background(), source, camera.ColorStream) + am := utils.AttributeMap{ + "angle_degs": 180, + } + rs, stream, err := newRotateTransform(context.Background(), source, camera.ColorStream, am) test.That(t, err, test.ShouldBeNil) test.That(t, stream, test.ShouldEqual, camera.ColorStream) @@ -168,12 +171,157 @@ func TestRotateColorSource(t *testing.T) { img2 := rimage.ConvertImage(rawImage) + am = utils.AttributeMap{ + // defaults to 180 + } + + rsDefault, stream, err := newRotateTransform(context.Background(), source, camera.ColorStream, am) + test.That(t, err, test.ShouldBeNil) + test.That(t, stream, test.ShouldEqual, camera.ColorStream) + + rawImageDefault, _, err := camera.ReadImage(context.Background(), rsDefault) + test.That(t, err, test.ShouldBeNil) + test.That(t, rs.Close(context.Background()), test.ShouldBeNil) + + err = rimage.WriteImageToFile(t.TempDir()+"/test_rotate_color_source.png", rawImageDefault) + test.That(t, err, test.ShouldBeNil) + + img3 := rimage.ConvertImage(rawImageDefault) + for x := 0; x < img.Width(); x++ { p1 := image.Point{x, 0} p2 := image.Point{img.Width() - x - 1, img.Height() - 1} + p3 := image.Point{img.Width() - x - 1, img.Height() - 1} a := img.Get(p1) b := img2.Get(p2) + c := img3.Get(p3) + + d := a.Distance(b) + test.That(t, d, test.ShouldEqual, 0) + + d = a.Distance(c) + test.That(t, d, test.ShouldEqual, 0) + } + + test.That(t, rs.Close(context.Background()), test.ShouldBeNil) + test.That(t, source.Close(context.Background()), test.ShouldBeNil) + + source = gostream.NewVideoSource(&videosource.StaticSource{ColorImg: img}, prop.Video{}) + am = utils.AttributeMap{ + "angle_degs": 90, + } + rs, stream, err = newRotateTransform(context.Background(), source, camera.ColorStream, am) + test.That(t, err, test.ShouldBeNil) + test.That(t, stream, test.ShouldEqual, camera.ColorStream) + + rawImage, _, err = camera.ReadImage(context.Background(), rs) + test.That(t, err, test.ShouldBeNil) + test.That(t, rs.Close(context.Background()), test.ShouldBeNil) + + err = rimage.WriteImageToFile(t.TempDir()+"/test_rotate_color_source.png", rawImage) + test.That(t, err, test.ShouldBeNil) + + img2 = rimage.ConvertImage(rawImage) + + for x := 0; x < img.Width(); x++ { + p1 := image.Point{X: x} + p2 := image.Point{X: img2.Width() - 1, Y: x} + + a := img.Get(p1) + b := img2.Get(p2) + + d := a.Distance(b) + test.That(t, d, test.ShouldEqual, 0) + } + + test.That(t, rs.Close(context.Background()), test.ShouldBeNil) + test.That(t, source.Close(context.Background()), test.ShouldBeNil) + + source = gostream.NewVideoSource(&videosource.StaticSource{ColorImg: img}, prop.Video{}) + am = utils.AttributeMap{ + "angle_degs": -90, + } + rs, stream, err = newRotateTransform(context.Background(), source, camera.ColorStream, am) + test.That(t, err, test.ShouldBeNil) + test.That(t, stream, test.ShouldEqual, camera.ColorStream) + + rawImage, _, err = camera.ReadImage(context.Background(), rs) + test.That(t, err, test.ShouldBeNil) + test.That(t, rs.Close(context.Background()), test.ShouldBeNil) + + err = rimage.WriteImageToFile(t.TempDir()+"/test_rotate_color_source.png", rawImage) + test.That(t, err, test.ShouldBeNil) + + img2 = rimage.ConvertImage(rawImage) + + for x := 0; x < img.Width(); x++ { + p1 := image.Point{X: x} + p2 := image.Point{Y: img2.Height() - 1 - x} + + a := img.Get(p1) + b := img2.Get(p2) + + d := a.Distance(b) + test.That(t, d, test.ShouldEqual, 0) + } + + test.That(t, rs.Close(context.Background()), test.ShouldBeNil) + test.That(t, source.Close(context.Background()), test.ShouldBeNil) + + source = gostream.NewVideoSource(&videosource.StaticSource{ColorImg: img}, prop.Video{}) + am = utils.AttributeMap{ + "angle_degs": 270, + } + rs, stream, err = newRotateTransform(context.Background(), source, camera.ColorStream, am) + test.That(t, err, test.ShouldBeNil) + test.That(t, stream, test.ShouldEqual, camera.ColorStream) + + rawImage, _, err = camera.ReadImage(context.Background(), rs) + test.That(t, err, test.ShouldBeNil) + test.That(t, rs.Close(context.Background()), test.ShouldBeNil) + + err = rimage.WriteImageToFile(t.TempDir()+"/test_rotate_color_source.png", rawImage) + test.That(t, err, test.ShouldBeNil) + + img2 = rimage.ConvertImage(rawImage) + + for x := 0; x < img.Width(); x++ { + p1 := image.Point{X: x} + p2 := image.Point{Y: img2.Height() - 1 - x} + + a := img.Get(p1) + b := img2.Get(p2) + + d := a.Distance(b) + test.That(t, d, test.ShouldEqual, 0) + } + + test.That(t, rs.Close(context.Background()), test.ShouldBeNil) + test.That(t, source.Close(context.Background()), test.ShouldBeNil) + + source = gostream.NewVideoSource(&videosource.StaticSource{ColorImg: img}, prop.Video{}) + am = utils.AttributeMap{ + "angle_degs": 0, // no-op + } + rs, stream, err = newRotateTransform(context.Background(), source, camera.ColorStream, am) + test.That(t, err, test.ShouldBeNil) + test.That(t, stream, test.ShouldEqual, camera.ColorStream) + + rawImage, _, err = camera.ReadImage(context.Background(), rs) + test.That(t, err, test.ShouldBeNil) + test.That(t, rs.Close(context.Background()), test.ShouldBeNil) + + err = rimage.WriteImageToFile(t.TempDir()+"/test_rotate_color_source.png", rawImage) + test.That(t, err, test.ShouldBeNil) + + img2 = rimage.ConvertImage(rawImage) + + for x := 0; x < img.Width(); x++ { + p := image.Point{X: x} + + a := img.Get(p) + b := img2.Get(p) d := a.Distance(b) test.That(t, d, test.ShouldEqual, 0) @@ -189,7 +337,10 @@ func TestRotateDepthSource(t *testing.T) { test.That(t, err, test.ShouldBeNil) source := gostream.NewVideoSource(&videosource.StaticSource{DepthImg: pc}, prop.Video{}) - rs, stream, err := newRotateTransform(context.Background(), source, camera.DepthStream) + am := utils.AttributeMap{ + "angle_degs": 180, + } + rs, stream, err := newRotateTransform(context.Background(), source, camera.DepthStream, am) test.That(t, err, test.ShouldBeNil) test.That(t, stream, test.ShouldEqual, camera.DepthStream) @@ -203,9 +354,123 @@ func TestRotateDepthSource(t *testing.T) { dm, err := rimage.ConvertImageToDepthMap(context.Background(), rawImage) test.That(t, err, test.ShouldBeNil) + am = utils.AttributeMap{ + // defaults to 180 + } + + rsDefault, stream, err := newRotateTransform(context.Background(), source, camera.DepthStream, am) + test.That(t, err, test.ShouldBeNil) + test.That(t, stream, test.ShouldEqual, camera.DepthStream) + + rawImageDefault, _, err := camera.ReadImage(context.Background(), rsDefault) + test.That(t, err, test.ShouldBeNil) + test.That(t, rs.Close(context.Background()), test.ShouldBeNil) + + err = rimage.WriteImageToFile(t.TempDir()+"/test_rotate_depth_source.png", rawImageDefault) + test.That(t, err, test.ShouldBeNil) + + dmDefault, err := rimage.ConvertImageToDepthMap(context.Background(), rawImageDefault) + test.That(t, err, test.ShouldBeNil) + for x := 0; x < pc.Width(); x++ { p1 := image.Point{x, 0} p2 := image.Point{pc.Width() - x - 1, pc.Height() - 1} + p3 := image.Point{pc.Width() - x - 1, pc.Height() - 1} + + d1 := pc.Get(p1) + d2 := dm.Get(p2) + d3 := dmDefault.Get(p3) + + test.That(t, d1, test.ShouldEqual, d2) + test.That(t, d1, test.ShouldEqual, d3) + } + + test.That(t, rs.Close(context.Background()), test.ShouldBeNil) + test.That(t, source.Close(context.Background()), test.ShouldBeNil) + + source = gostream.NewVideoSource(&videosource.StaticSource{DepthImg: pc}, prop.Video{}) + am = utils.AttributeMap{ + "angle_degs": 90, + } + rs, stream, err = newRotateTransform(context.Background(), source, camera.DepthStream, am) + test.That(t, err, test.ShouldBeNil) + test.That(t, stream, test.ShouldEqual, camera.DepthStream) + + rawImage, _, err = camera.ReadImage(context.Background(), rs) + test.That(t, err, test.ShouldBeNil) + test.That(t, rs.Close(context.Background()), test.ShouldBeNil) + + err = rimage.WriteImageToFile(t.TempDir()+"/test_rotate_depth_source.png", rawImage) + test.That(t, err, test.ShouldBeNil) + + dm, err = rimage.ConvertImageToDepthMap(context.Background(), rawImage) + test.That(t, err, test.ShouldBeNil) + + for x := 0; x < pc.Width(); x++ { + p1 := image.Point{X: x} + p2 := image.Point{X: dm.Width() - 1, Y: x} + + d1 := pc.Get(p1) + d2 := dm.Get(p2) + + test.That(t, d1, test.ShouldEqual, d2) + } + + test.That(t, rs.Close(context.Background()), test.ShouldBeNil) + test.That(t, source.Close(context.Background()), test.ShouldBeNil) + + source = gostream.NewVideoSource(&videosource.StaticSource{DepthImg: pc}, prop.Video{}) + am = utils.AttributeMap{ + "angle_degs": -90, + } + rs, stream, err = newRotateTransform(context.Background(), source, camera.DepthStream, am) + test.That(t, err, test.ShouldBeNil) + test.That(t, stream, test.ShouldEqual, camera.DepthStream) + + rawImage, _, err = camera.ReadImage(context.Background(), rs) + test.That(t, err, test.ShouldBeNil) + test.That(t, rs.Close(context.Background()), test.ShouldBeNil) + + err = rimage.WriteImageToFile(t.TempDir()+"/test_rotate_depth_source.png", rawImage) + test.That(t, err, test.ShouldBeNil) + + dm, err = rimage.ConvertImageToDepthMap(context.Background(), rawImage) + test.That(t, err, test.ShouldBeNil) + + for x := 0; x < pc.Width(); x++ { + p1 := image.Point{X: x} + p2 := image.Point{Y: dm.Height() - 1 - x} + + d1 := pc.Get(p1) + d2 := dm.Get(p2) + + test.That(t, d1, test.ShouldEqual, d2) + } + + test.That(t, rs.Close(context.Background()), test.ShouldBeNil) + test.That(t, source.Close(context.Background()), test.ShouldBeNil) + + source = gostream.NewVideoSource(&videosource.StaticSource{DepthImg: pc}, prop.Video{}) + am = utils.AttributeMap{ + "angle_degs": 270, + } + rs, stream, err = newRotateTransform(context.Background(), source, camera.DepthStream, am) + test.That(t, err, test.ShouldBeNil) + test.That(t, stream, test.ShouldEqual, camera.DepthStream) + + rawImage, _, err = camera.ReadImage(context.Background(), rs) + test.That(t, err, test.ShouldBeNil) + test.That(t, rs.Close(context.Background()), test.ShouldBeNil) + + err = rimage.WriteImageToFile(t.TempDir()+"/test_rotate_depth_source.png", rawImage) + test.That(t, err, test.ShouldBeNil) + + dm, err = rimage.ConvertImageToDepthMap(context.Background(), rawImage) + test.That(t, err, test.ShouldBeNil) + + for x := 0; x < pc.Width(); x++ { + p1 := image.Point{X: x} + p2 := image.Point{Y: dm.Height() - 1 - x} d1 := pc.Get(p1) d2 := dm.Get(p2) @@ -215,6 +480,36 @@ func TestRotateDepthSource(t *testing.T) { test.That(t, rs.Close(context.Background()), test.ShouldBeNil) test.That(t, source.Close(context.Background()), test.ShouldBeNil) + + source = gostream.NewVideoSource(&videosource.StaticSource{DepthImg: pc}, prop.Video{}) + am = utils.AttributeMap{ + "angle_degs": 0, // no-op + } + rs, stream, err = newRotateTransform(context.Background(), source, camera.DepthStream, am) + test.That(t, err, test.ShouldBeNil) + test.That(t, stream, test.ShouldEqual, camera.DepthStream) + + rawImage, _, err = camera.ReadImage(context.Background(), rs) + test.That(t, err, test.ShouldBeNil) + test.That(t, rs.Close(context.Background()), test.ShouldBeNil) + + err = rimage.WriteImageToFile(t.TempDir()+"/test_rotate_depth_source.png", rawImage) + test.That(t, err, test.ShouldBeNil) + + dm, err = rimage.ConvertImageToDepthMap(context.Background(), rawImage) + test.That(t, err, test.ShouldBeNil) + + for x := 0; x < pc.Width(); x++ { + p := image.Point{X: x} + + d1 := pc.Get(p) + d2 := dm.Get(p) + + test.That(t, d1, test.ShouldEqual, d2) + } + + test.That(t, rs.Close(context.Background()), test.ShouldBeNil) + test.That(t, source.Close(context.Background()), test.ShouldBeNil) } func BenchmarkColorRotate(b *testing.B) { @@ -224,7 +519,10 @@ func BenchmarkColorRotate(b *testing.B) { source := gostream.NewVideoSource(&videosource.StaticSource{ColorImg: img}, prop.Video{}) src, err := camera.WrapVideoSourceWithProjector(context.Background(), source, nil, camera.ColorStream) test.That(b, err, test.ShouldBeNil) - rs, stream, err := newRotateTransform(context.Background(), src, camera.ColorStream) + am := utils.AttributeMap{ + "angle_degs": 180, + } + rs, stream, err := newRotateTransform(context.Background(), src, camera.ColorStream, am) test.That(b, err, test.ShouldBeNil) test.That(b, stream, test.ShouldEqual, camera.ColorStream) @@ -245,7 +543,10 @@ func BenchmarkDepthRotate(b *testing.B) { source := gostream.NewVideoSource(&videosource.StaticSource{DepthImg: img}, prop.Video{}) src, err := camera.WrapVideoSourceWithProjector(context.Background(), source, nil, camera.DepthStream) test.That(b, err, test.ShouldBeNil) - rs, stream, err := newRotateTransform(context.Background(), src, camera.DepthStream) + am := utils.AttributeMap{ + "angle_degs": 180, + } + rs, stream, err := newRotateTransform(context.Background(), src, camera.DepthStream, am) test.That(b, err, test.ShouldBeNil) test.That(b, stream, test.ShouldEqual, camera.DepthStream) diff --git a/components/camera/transformpipeline/pipeline.go b/components/camera/transformpipeline/pipeline.go index f72a9775570..84d3354f032 100644 --- a/components/camera/transformpipeline/pipeline.go +++ b/components/camera/transformpipeline/pipeline.go @@ -16,6 +16,7 @@ import ( goutils "go.viam.com/utils" "go.viam.com/rdk/components/camera" + "go.viam.com/rdk/pointcloud" "go.viam.com/rdk/resource" "go.viam.com/rdk/rimage" "go.viam.com/rdk/rimage/transform" @@ -92,23 +93,25 @@ func newTransformPipeline( } // check if the source produces a depth image or color image img, release, err := camera.ReadImage(ctx, source) - if err != nil { - return nil, err - } + var streamType camera.ImageType - if _, ok := img.(*rimage.DepthMap); ok { + if err != nil { + streamType = camera.UnspecifiedStream + } else if _, ok := img.(*rimage.DepthMap); ok { streamType = camera.DepthStream } else if _, ok := img.(*image.Gray16); ok { streamType = camera.DepthStream } else { streamType = camera.ColorStream } - release() + if release != nil { + release() + } // loop through the pipeline and create the image flow pipeline := make([]gostream.VideoSource, 0, len(cfg.Pipeline)) lastSource := source for _, tr := range cfg.Pipeline { - src, newStreamType, err := buildTransform(ctx, r, lastSource, streamType, tr) + src, newStreamType, err := buildTransform(ctx, r, lastSource, streamType, tr, cfg.Source) if err != nil { return nil, err } @@ -138,6 +141,19 @@ func (tp transformPipeline) Read(ctx context.Context) (image.Image, func(), erro return tp.stream.Next(ctx) } +func (tp transformPipeline) NextPointCloud(ctx context.Context) (pointcloud.PointCloud, error) { + ctx, span := trace.StartSpan(ctx, "camera::transformpipeline::NextPointCloud") + defer span.End() + if lastElem, ok := tp.pipeline[len(tp.pipeline)-1].(camera.PointCloudSource); ok { + pc, err := lastElem.NextPointCloud(ctx) + if err != nil { + return nil, errors.Wrap(err, "function NextPointCloud not defined for last videosource in transform pipeline") + } + return pc, nil + } + return nil, errors.New("function NextPointCloud not defined for last videosource in transform pipeline") +} + func (tp transformPipeline) Close(ctx context.Context) error { var errs error for _, src := range tp.pipeline { diff --git a/components/camera/transformpipeline/pipeline_test.go b/components/camera/transformpipeline/pipeline_test.go index 0081f6d427f..7f421d3c42a 100644 --- a/components/camera/transformpipeline/pipeline_test.go +++ b/components/camera/transformpipeline/pipeline_test.go @@ -93,8 +93,9 @@ func TestTransformPipelineDepth(t *testing.T) { test.That(t, err, test.ShouldBeNil) test.That(t, prop, test.ShouldResemble, intrinsics) outPc, err := depth.NextPointCloud(context.Background()) - test.That(t, err, test.ShouldBeNil) - test.That(t, outPc, test.ShouldNotBeNil) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, "not defined for last videosource") + test.That(t, outPc, test.ShouldBeNil) test.That(t, depth.Close(context.Background()), test.ShouldBeNil) test.That(t, source.Close(context.Background()), test.ShouldBeNil) diff --git a/components/camera/transformpipeline/segmenter.go b/components/camera/transformpipeline/segmenter.go new file mode 100644 index 00000000000..7a1e99f29b6 --- /dev/null +++ b/components/camera/transformpipeline/segmenter.go @@ -0,0 +1,122 @@ +package transformpipeline + +import ( + "context" + "fmt" + "image" + + "github.com/viamrobotics/gostream" + "go.opencensus.io/trace" + goutils "go.viam.com/utils" + + "go.viam.com/rdk/components/camera" + "go.viam.com/rdk/pointcloud" + "go.viam.com/rdk/resource" + "go.viam.com/rdk/robot" + "go.viam.com/rdk/services/vision" + "go.viam.com/rdk/spatialmath" + "go.viam.com/rdk/utils" +) + +// segmenterConfig is the attribute struct for segementers (their name as found in the vision service). +type segmenterConfig struct { + SegmenterName string `json:"segmenter_name"` +} + +// segmenterSource takes a pointcloud from the camera and applies a segmenter to it. +type segmenterSource struct { + stream gostream.VideoStream + cameraName string + segmenterName string + r robot.Robot +} + +func newSegmentationsTransform( + ctx context.Context, + source gostream.VideoSource, + r robot.Robot, + am utils.AttributeMap, + sourceString string, +) (gostream.VideoSource, camera.ImageType, error) { + conf, err := resource.TransformAttributeMap[*segmenterConfig](am) + if err != nil { + return nil, camera.UnspecifiedStream, err + } + + props, err := propsFromVideoSource(ctx, source) + if err != nil { + return nil, camera.UnspecifiedStream, err + } + + segmenter := &segmenterSource{ + gostream.NewEmbeddedVideoStream(source), + sourceString, + conf.SegmenterName, + r, + } + src, err := camera.NewVideoSourceFromReader(ctx, segmenter, nil, props.ImageType) + if err != nil { + return nil, camera.UnspecifiedStream, err + } + + return src, props.ImageType, err +} + +// Validate ensures all parts of the config are valid. +func (cfg *segmenterConfig) Validate(path string) ([]string, error) { + var deps []string + if len(cfg.SegmenterName) == 0 { + return nil, goutils.NewConfigValidationFieldRequiredError(path, "segmenter_name") + } + return deps, nil +} + +// NextPointCloud function calls a segmenter service on the underlying camera and returns a pointcloud. +func (ss *segmenterSource) NextPointCloud(ctx context.Context) (pointcloud.PointCloud, error) { + ctx, span := trace.StartSpan(ctx, "camera::transformpipeline::segmenter::NextPointCloud") + defer span.End() + + // get the service + srv, err := vision.FromRobot(ss.r, ss.segmenterName) + if err != nil { + return nil, fmt.Errorf("source_segmenter cant find vision service: %w", err) + } + + // apply service + clouds, err := srv.GetObjectPointClouds(ctx, ss.cameraName, map[string]interface{}{}) + if err != nil { + return nil, fmt.Errorf("could not get point clouds: %w", err) + } + if clouds == nil { + return pointcloud.New(), nil + } + + // merge pointclouds + cloudsWithOffset := make([]pointcloud.CloudAndOffsetFunc, 0, len(clouds)) + for _, cloud := range clouds { + cloudCopy := cloud + cloudFunc := func(ctx context.Context) (pointcloud.PointCloud, spatialmath.Pose, error) { + return cloudCopy, nil, nil + } + cloudsWithOffset = append(cloudsWithOffset, cloudFunc) + } + mergedCloud, err := pointcloud.MergePointClouds(context.Background(), cloudsWithOffset, nil) + if err != nil { + return nil, fmt.Errorf("could not merge point clouds: %w", err) + } + return mergedCloud, nil +} + +// Read returns the image if the stream is valid, else error. +func (ss *segmenterSource) Read(ctx context.Context) (image.Image, func(), error) { + img, release, err := ss.stream.Next(ctx) + if err != nil { + return nil, nil, fmt.Errorf("could not get next source image: %w", err) + } + return img, release, nil +} + +// Close closes the underlying stream. +func (ss *segmenterSource) Close(ctx context.Context) error { + return ss.stream.Close(ctx) +} diff --git a/components/camera/transformpipeline/segmenter_test.go b/components/camera/transformpipeline/segmenter_test.go new file mode 100644 index 00000000000..6c112e4269f --- /dev/null +++ b/components/camera/transformpipeline/segmenter_test.go @@ -0,0 +1,163 @@ +package transformpipeline + +import ( + "context" + "image" + "image/color" + "testing" + + "github.com/viamrobotics/gostream" + "go.viam.com/test" + + "go.viam.com/rdk/components/camera" + pc "go.viam.com/rdk/pointcloud" + "go.viam.com/rdk/resource" + vizservices "go.viam.com/rdk/services/vision" + "go.viam.com/rdk/testutils/inject" + "go.viam.com/rdk/utils" + vision "go.viam.com/rdk/vision" + segment "go.viam.com/rdk/vision/segmentation" +) + +func TestTransformSegmenterProps(t *testing.T) { + r := &inject.Robot{} + cam := &inject.Camera{} + vizServ := &inject.VisionService{} + + cam.StreamFunc = func(ctx context.Context, + errHandlers ...gostream.ErrorHandler, + ) (gostream.MediaStream[image.Image], error) { + return &streamTest{}, nil + } + cam.PropertiesFunc = func(ctx context.Context) (camera.Properties, error) { + return camera.Properties{}, nil + } + + r.ResourceByNameFunc = func(n resource.Name) (resource.Resource, error) { + switch n.Name { + case "fakeCamera": + return cam, nil + case "fakeVizService": + return vizServ, nil + default: + return nil, resource.NewNotFoundError(n) + } + } + + transformConf := &transformConfig{ + Source: "fakeCamera", + Pipeline: []Transformation{ + { + Type: "segmentations", Attributes: utils.AttributeMap{ + "segmenter_name": "fakeVizService", + }, + }, + }, + } + + am := transformConf.Pipeline[0].Attributes + conf, err := resource.TransformAttributeMap[*segmenterConfig](am) + test.That(t, err, test.ShouldBeNil) + _, err = conf.Validate("path") + test.That(t, err, test.ShouldBeNil) + + _, err = newTransformPipeline(context.Background(), cam, transformConf, r) + test.That(t, err, test.ShouldBeNil) + + transformConf = &transformConfig{ + Pipeline: []Transformation{ + { + Type: "segmentations", Attributes: utils.AttributeMap{}, + }, + }, + } + + am = transformConf.Pipeline[0].Attributes + conf, err = resource.TransformAttributeMap[*segmenterConfig](am) + test.That(t, err, test.ShouldBeNil) + _, err = conf.Validate("path") + test.That(t, err, test.ShouldNotBeNil) +} + +func TestTransformSegmenterFunctionality(t *testing.T) { + // TODO(RSDK-1200): remove skip when complete + t.Skip("remove skip once RSDK-1200 improvement is complete") + + r := &inject.Robot{} + cam := &inject.Camera{} + vizServ := &inject.VisionService{} + + cam.StreamFunc = func(ctx context.Context, + errHandlers ...gostream.ErrorHandler, + ) (gostream.MediaStream[image.Image], error) { + return &streamTest{}, nil + } + cam.PropertiesFunc = func(ctx context.Context) (camera.Properties, error) { + return camera.Properties{}, nil + } + + vizServ.GetObjectPointCloudsFunc = func(ctx context.Context, cameraName string, + extra map[string]interface{}, + ) ([]*vision.Object, error) { + segments := make([]pc.PointCloud, 3) + segments[0] = pc.New() + err := segments[0].Set(pc.NewVector(0, 0, 1), pc.NewColoredData(color.NRGBA{255, 0, 0, 255})) + if err != nil { + return nil, err + } + segments[1] = pc.New() + err = segments[1].Set(pc.NewVector(0, 1, 0), pc.NewColoredData(color.NRGBA{255, 0, 0, 255})) + if err != nil { + return nil, err + } + segments[2] = pc.New() + err = segments[2].Set(pc.NewVector(1, 0, 0), pc.NewColoredData(color.NRGBA{255, 0, 0, 255})) + if err != nil { + return nil, err + } + + objects, err := segment.NewSegmentsFromSlice(segments, "fake") + if err != nil { + return nil, err + } + return objects.Objects, nil + } + + r.ResourceNamesFunc = func() []resource.Name { + return []resource.Name{camera.Named("fakeCamera"), vizservices.Named("fakeVizService")} + } + r.ResourceByNameFunc = func(n resource.Name) (resource.Resource, error) { + switch n.Name { + case "fakeCamera": + return cam, nil + case "fakeVizService": + return vizServ, nil + default: + return nil, resource.NewNotFoundError(n) + } + } + + transformConf := &transformConfig{ + Source: "fakeCamera", + Pipeline: []Transformation{ + { + Type: "segmentations", Attributes: utils.AttributeMap{ + "segmenter_name": "fakeVizService", + }, + }, + }, + } + + pipeline, err := newTransformPipeline(context.Background(), cam, transformConf, r) + test.That(t, err, test.ShouldBeNil) + + pc, err := pipeline.NextPointCloud(context.Background()) + test.That(t, err, test.ShouldBeNil) + test.That(t, pc, test.ShouldNotBeNil) + _, isValid := pc.At(0, 0, 1) + test.That(t, isValid, test.ShouldBeTrue) + _, isValid = pc.At(1, 0, 0) + test.That(t, isValid, test.ShouldBeTrue) + _, isValid = pc.At(0, 1, 0) + test.That(t, isValid, test.ShouldBeTrue) +} diff --git a/components/camera/transformpipeline/transform.go b/components/camera/transformpipeline/transform.go index 69616c9b2d4..5f764ae38a9 100644 --- a/components/camera/transformpipeline/transform.go +++ b/components/camera/transformpipeline/transform.go @@ -27,6 +27,7 @@ const ( transformTypeUndistort = transformType("undistort") transformTypeDetections = transformType("detections") transformTypeClassifications = transformType("classifications") + transformTypeSegmentations = transformType("segmentations") transformTypeDepthEdges = transformType("depth_edges") transformTypeDepthPreprocess = transformType("depth_preprocess") ) @@ -88,6 +89,11 @@ var registeredTransformConfigs = map[transformType]*transformRegistration{ &classifierConfig{}, "Overlays image classifications on the image. Can use any classifier registered in the vision service.", }, + transformTypeSegmentations: { + string(transformTypeSegmentations), + &segmenterConfig{}, + "Segments the camera's point cloud. Can use any segmenter registered in the vision service.", + }, transformTypeDepthEdges: { string(transformTypeDepthEdges), &depthEdgesConfig{}, @@ -128,12 +134,13 @@ func buildTransform( source gostream.VideoSource, stream camera.ImageType, tr Transformation, + sourceString string, ) (gostream.VideoSource, camera.ImageType, error) { switch transformType(tr.Type) { case transformTypeUnspecified, transformTypeIdentity: return source, stream, nil case transformTypeRotate: - return newRotateTransform(ctx, source, stream) + return newRotateTransform(ctx, source, stream, tr.Attributes) case transformTypeResize: return newResizeTransform(ctx, source, stream, tr.Attributes) case transformTypeCrop: @@ -148,6 +155,8 @@ func buildTransform( return newDetectionsTransform(ctx, source, r, tr.Attributes) case transformTypeClassifications: return newClassificationsTransform(ctx, source, r, tr.Attributes) + case transformTypeSegmentations: + return newSegmentationsTransform(ctx, source, r, tr.Attributes, sourceString) case transformTypeDepthEdges: return newDepthEdgesTransform(ctx, source, tr.Attributes) case transformTypeDepthPreprocess: diff --git a/components/camera/ultrasonic/ultrasonic.go b/components/camera/ultrasonic/ultrasonic.go index 47d0c87db6d..f0eadb3ac4c 100644 --- a/components/camera/ultrasonic/ultrasonic.go +++ b/components/camera/ultrasonic/ultrasonic.go @@ -36,15 +36,15 @@ func init() { if err != nil { return nil, err } - return newCamera(ctx, deps, conf.ResourceName(), newConf) + return newCamera(ctx, deps, conf.ResourceName(), newConf, logger) }, }) } func newCamera(ctx context.Context, deps resource.Dependencies, name resource.Name, - newConf *ultrasense.Config, + newConf *ultrasense.Config, logger golog.Logger, ) (camera.Camera, error) { - usSensor, err := ultrasense.NewSensor(ctx, deps, name, newConf) + usSensor, err := ultrasense.NewSensor(ctx, deps, name, newConf, logger) if err != nil { return nil, err } diff --git a/components/camera/ultrasonic/ultrasonic_test.go b/components/camera/ultrasonic/ultrasonic_test.go index 94319b167d4..1f1806c524b 100644 --- a/components/camera/ultrasonic/ultrasonic_test.go +++ b/components/camera/ultrasonic/ultrasonic_test.go @@ -4,6 +4,7 @@ import ( "context" "testing" + "github.com/edaniels/golog" "github.com/golang/geo/r3" "go.viam.com/test" @@ -52,7 +53,8 @@ func TestNewCamera(t *testing.T) { name := resource.Name{API: camera.API} ctx := context.Background() deps := setupDependencies(t) - _, err := newCamera(ctx, deps, name, fakecfg) + logger := golog.NewTestLogger(t) + _, err := newCamera(ctx, deps, name, fakecfg, logger) test.That(t, err, test.ShouldBeNil) } diff --git a/components/camera/videosource/logging/logger.go b/components/camera/videosource/logging/logger.go new file mode 100644 index 00000000000..6e41a378669 --- /dev/null +++ b/components/camera/videosource/logging/logger.go @@ -0,0 +1,322 @@ +// Package logging is a thread-safe way to log video device information to a file. On startup, this package creates a +// unique filename and uses that filename throughout the lifetime of the program to log information such as which video +// devices are V4L2 compatible and the current operating system. +package logging + +import ( + "context" + "fmt" + "io/fs" + "log" + "os" + "os/exec" + "path/filepath" + "reflect" + "runtime" + "strings" + "sync/atomic" + "time" + + "github.com/edaniels/golog" + "github.com/jedib0t/go-pretty/v6/table" + "github.com/pkg/errors" + "go.viam.com/utils" + + "go.viam.com/rdk/config" +) + +var ( + // GLoggerCamComp is the global logger-to-file for camera components. + GLoggerCamComp *Logger + filePath string +) + +func init() { + t := time.Now().UTC().Format(time.RFC3339) + filePath = filepath.Join(config.ViamDotDir, "debug", "components", "camera", fmt.Sprintf("%s.txt", t)) + + var err error + if GLoggerCamComp, err = NewLogger(); err != nil && !errors.Is(err, UnsupportedError{}) { + log.Println("cannot create new logger: ", err) + } +} + +// InfoMap is a map of information to be written to the log. +type InfoMap = map[string]string + +// Logger is a thread-safe logger that manages a single log file in config.ViamDotDir. +type Logger struct { + infoCh chan info + logger golog.Logger + isRunning atomic.Bool + seenPath map[string]bool + seenMap map[string]InfoMap +} + +type info struct { + title string + m InfoMap +} + +const ( + // keep at most 3 log files in dir. + maxFiles = 3 + linux = "linux" +) + +// UnsupportedError indicates this feature is not supported on the current platform. +type UnsupportedError struct{} + +func (e UnsupportedError) Error() string { + return "unsupported OS: cannot emit logs to file for camera component" +} + +// NewLogger creates a new logger. Call Logger.Start to start logging. +func NewLogger() (*Logger, error) { + // TODO: support non-Linux platforms + if runtime.GOOS != linux { + return nil, UnsupportedError{} + } + + dir := filepath.Dir(filePath) + if err := os.MkdirAll(dir, os.ModePerm); err != nil { + return nil, errors.Wrap(err, "camera logger: cannot mkdir "+dir) + } + + // remove enough entries to keep the number of files <= maxFiles + for entries, err := os.ReadDir(dir); len(entries) >= maxFiles; entries, err = os.ReadDir(dir) { + if err != nil { + utils.UncheckedError(errors.Wrap(err, "camera logger: cannot read directory "+dir)) + break + } + + // because entries are sorted by name (timestamp), earlier entries are removed first + if err = os.Remove(filepath.Join(dir, entries[0].Name())); err != nil { + utils.UncheckedError(errors.Wrap(err, "camera logger: cannot remove file "+filepath.Join(dir, entries[0].Name()))) + break + } + } + + cfg := golog.NewDevelopmentLoggerConfig() + cfg.OutputPaths = []string{filePath} + + // only keep message + cfg.EncoderConfig.TimeKey = "" + cfg.EncoderConfig.LevelKey = "" + cfg.EncoderConfig.NameKey = "" + cfg.EncoderConfig.CallerKey = "" + cfg.EncoderConfig.StacktraceKey = "" + + logger, err := cfg.Build() + if err != nil { + return nil, err + } + + return &Logger{ + infoCh: make(chan info), + logger: logger.Sugar().Named("camera_debugger"), + }, nil +} + +// Start creates and initializes the logging file and periodically emits logs to it. This method is thread-safe. +func (l *Logger) Start(ctx context.Context) error { + // TODO: support non-Linux platforms + if runtime.GOOS != linux { + return UnsupportedError{} + } + + if l == nil { + return nil + } + + if prevVal := l.isRunning.Swap(true); prevVal { + return nil // already running; nothing to do + } + + utils.PanicCapturingGo(func() { + log.Println("started global logger") + defer log.Println("terminated global logger") + + l.init() + ticker := time.NewTicker(1 * time.Second) + shouldReset := time.NewTimer(12 * time.Hour) + for { + select { + case <-ctx.Done(): + return + case <-shouldReset.C: + l.init() + default: + } + + select { + case <-ctx.Done(): + return + case info := <-l.infoCh: + l.write(info.title, info.m) + case <-ticker.C: + l.captureV4L2info() + } + } + }) + return nil +} + +// Log emits the data stored in the given InfoMap with the given title to the log file. This method is thread-safe. +func (l *Logger) Log(title string, m InfoMap) error { + // TODO: support non-Linux platforms + if runtime.GOOS != linux { + return UnsupportedError{} + } + + if l == nil { + return nil + } + + if !l.isRunning.Load() { + return errors.New("must start logger") + } + + l.infoCh <- info{title, m} + return nil +} + +func (l *Logger) captureV4L2info() { + v4l2Info := make(InfoMap) + v4l2Compliance := make(InfoMap) + err := filepath.Walk("/dev", func(path string, info fs.FileInfo, err error) error { + if err != nil { + return err + } + + if !strings.HasPrefix(path, "/dev/video") { + return nil + } + if l.seenPath[path] { + return nil + } + + // some devices may not have a symbolic link under /dev/v4l so we log info from all /dev/videoN paths we find. + v4l2Info[path] = runCommand("v4l2-ctl", "--device", path, "--all") + v4l2Compliance[path] = runCommand("v4l2-compliance", "--device", path) + l.seenPath[path] = true + return nil + }) + l.logError(err, "cannot walk filepath") + + v4l2Path := make(InfoMap) + err = filepath.Walk("/dev/v4l/by-path", func(path string, info fs.FileInfo, err error) error { + if err != nil { + return err + } + + if l.seenPath[path] { + return nil + } + v4l2Path["by-path"] = filepath.Base(path) + l.seenPath[path] = true + return nil + }) + l.logError(err, "cannot walk filepath") + + v4l2ID := make(InfoMap) + err = filepath.Walk("/dev/v4l/by-id", func(path string, info fs.FileInfo, err error) error { + if err != nil { + return err + } + + if l.seenPath[path] { + return nil + } + v4l2ID["by-id"] = filepath.Base(path) + l.seenPath[path] = true + return nil + }) + l.logError(err, "cannot walk filepath") + + // Video capture and overlay devices' minor numbers range from [0,63] + // https://www.kernel.org/doc/html/v4.16/media/uapi/v4l/diff-v4l.html + for n := 0; n < 63; n++ { + path := fmt.Sprintf("/dev/video%d", n) + if _, err := os.Stat(path); os.IsNotExist(err) { + // when this file is re-created we won't know whether it's the same device. Better to assume not. + l.seenPath[path] = false + } + } + + l.write("v4l2 control", v4l2Info) + l.write("v4l2 compliance", v4l2Compliance) + l.write("v4l2 paths", v4l2Path) + l.write("v4l2 ID", v4l2ID) +} + +func (l *Logger) init() { + err := os.Truncate(filePath, 0) + l.logError(err, "cannot truncate file") + + l.seenPath = make(map[string]bool) + l.seenMap = make(map[string]InfoMap) + l.write("system information", InfoMap{ + "kernel": runCommand("uname", "--kernel-name"), + "machine": runCommand("uname", "--machine"), + "processor": runCommand("uname", "--processor"), + "platform": runCommand("uname", "--hardware-platform"), + "OS": runCommand("uname", "--operating-system"), + "lscpu": runCommand("lscpu"), + "model": runCommand("cat", "/proc/device-tree/model"), + }) +} + +func runCommand(name string, args ...string) string { + //nolint:errcheck + out, _ := exec.Command(name, args...).CombinedOutput() + return string(out) +} + +func (l *Logger) write(title string, m InfoMap) { + if len(m) == 0 { + return + } + if oldM, ok := l.seenMap[title]; ok && reflect.DeepEqual(oldM, m) { + return // don't log the same info twice + } + + l.seenMap[title] = m + t := table.NewWriter() + t.SetAllowedRowLength(120) + t.SuppressEmptyColumns() + t.SetStyle(table.StyleLight) + t.SetTitle(strings.ToUpper(title)) + + splitLine := func(line string) table.Row { + var row table.Row + for _, ele := range strings.Split(line, ":") { + row = append(row, strings.TrimSpace(ele)) + } + return row + } + + for k, v := range m { + lines := strings.Split(v, "\n") + t.AppendRow(append(table.Row{k}, splitLine(lines[0])...)) + for i := 1; i < len(lines); i++ { + line := lines[i] + if strings.ReplaceAll(line, " ", "") == "" { + continue + } + + t.AppendRow(append(table.Row{""}, splitLine(line)...)) + } + t.AppendSeparator() + } + + t.AppendFooter(table.Row{time.Now().UTC().Format(time.RFC3339)}) + l.logger.Infoln(t.Render()) + l.logger.Infoln(fmt.Sprintln()) +} + +func (l *Logger) logError(err error, msg string) { + if l != nil && err != nil { + l.write("error", InfoMap{msg: err.Error()}) + } +} diff --git a/components/camera/videosource/static.go b/components/camera/videosource/static.go index 6484737ebe3..f94cb7bfbbc 100644 --- a/components/camera/videosource/static.go +++ b/components/camera/videosource/static.go @@ -27,31 +27,38 @@ func init() { if err != nil { return nil, err } - videoSrc := &fileSource{newConf.Color, newConf.Depth, newConf.CameraParameters} - imgType := camera.ColorStream - if newConf.Color == "" { - imgType = camera.DepthStream - } - cameraModel := camera.NewPinholeModelWithBrownConradyDistortion(newConf.CameraParameters, newConf.DistortionParameters) - src, err := camera.NewVideoSourceFromReader( - ctx, - videoSrc, - &cameraModel, - imgType, - ) - if err != nil { - return nil, err - } - return camera.FromVideoSource(conf.ResourceName(), src), nil + return newCamera(context.Background(), conf.ResourceName(), newConf) }, }) } -// fileSource stores the paths to a color and depth image. +func newCamera(ctx context.Context, name resource.Name, newConf *fileSourceConfig) (camera.Camera, error) { + videoSrc := &fileSource{newConf.Color, newConf.Depth, newConf.PointCloud, newConf.CameraParameters, nil, nil} + imgType := camera.ColorStream + if newConf.Color == "" { + imgType = camera.DepthStream + } + cameraModel := camera.NewPinholeModelWithBrownConradyDistortion(newConf.CameraParameters, newConf.DistortionParameters) + src, err := camera.NewVideoSourceFromReader( + ctx, + videoSrc, + &cameraModel, + imgType, + ) + if err != nil { + return nil, err + } + return camera.FromVideoSource(name, src), nil +} + +// fileSource stores the paths to a color and depth image and a pointcloud. type fileSource struct { - ColorFN string - DepthFN string - Intrinsics *transform.PinholeCameraIntrinsics + ColorFN string + DepthFN string + PointCloudFN string + Intrinsics *transform.PinholeCameraIntrinsics + colorImg image.Image + pc pointcloud.PointCloud } // fileSourceConfig is the attribute struct for fileSource. @@ -62,20 +69,66 @@ type fileSourceConfig struct { Debug bool `json:"debug,omitempty"` Color string `json:"color_image_file_path,omitempty"` Depth string `json:"depth_image_file_path,omitempty"` + PointCloud string `json:"pointcloud_file_path,omitempty"` } // Read returns just the RGB image if it is present, or the depth map if the RGB image is not present. func (fs *fileSource) Read(ctx context.Context) (image.Image, func(), error) { + if fs.ColorFN == "" && fs.DepthFN == "" { + return nil, nil, errors.New("no image file to read, so not implemented") + } if fs.ColorFN == "" { // only depth info img, err := rimage.NewDepthMapFromFile(context.Background(), fs.DepthFN) + if err != nil { + return nil, nil, err + } return img, func() {}, err } + + if fs.colorImg != nil { + return fs.colorImg, func() {}, nil + } + img, err := rimage.NewImageFromFile(fs.ColorFN) - return img, func() {}, err + if err != nil { + return nil, nil, err + } + + // x264 only supports even resolutions. Not every call to this function will + // be in the context of an x264 stream, but we crop every image to even + // dimensions anyways. + oddWidth := img.Bounds().Dx()%2 != 0 + oddHeight := img.Bounds().Dy()%2 != 0 + if oddWidth || oddHeight { + newWidth := img.Bounds().Dx() + newHeight := img.Bounds().Dy() + if oddWidth { + newWidth-- + } + if oddHeight { + newHeight-- + } + fs.colorImg = img.SubImage(image.Rect(0, 0, newWidth, newHeight)) + } else { + fs.colorImg = img + } + + return fs.colorImg, func() {}, err } -// NextPointCloud returns the point cloud from projecting the rgb and depth image using the intrinsic parameters. +// NextPointCloud returns the point cloud from projecting the rgb and depth image using the intrinsic parameters, +// or the pointcloud from file if set. func (fs *fileSource) NextPointCloud(ctx context.Context) (pointcloud.PointCloud, error) { + if fs.PointCloudFN != "" && fs.pc == nil { + newPc, err := pointcloud.NewFromFile(fs.PointCloudFN, nil) + if err != nil { + return nil, err + } + fs.pc = newPc + } + if fs.pc != nil { + return fs.pc, nil + } if fs.Intrinsics == nil { return nil, transform.NewNoIntrinsicsError("camera intrinsics not found in config") } diff --git a/components/camera/videosource/static_test.go b/components/camera/videosource/static_test.go new file mode 100644 index 00000000000..ee6192e9e36 --- /dev/null +++ b/components/camera/videosource/static_test.go @@ -0,0 +1,122 @@ +package videosource + +import ( + "context" + "image" + "image/color" + "image/jpeg" + "os" + "path/filepath" + "testing" + + "go.viam.com/test" + "go.viam.com/utils/artifact" + + "go.viam.com/rdk/components/camera" + "go.viam.com/rdk/resource" + "go.viam.com/rdk/rimage" +) + +func TestPCD(t *testing.T) { + pcdPath := filepath.Clean(artifact.MustPath("pointcloud/octagonspace.pcd")) + cfg := &fileSourceConfig{PointCloud: pcdPath} + ctx := context.Background() + cam, err := newCamera(ctx, resource.Name{API: camera.API}, cfg) + test.That(t, err, test.ShouldBeNil) + + _, err = cam.Stream(ctx) + test.That(t, err, test.ShouldBeNil) + + pc, err := cam.NextPointCloud(ctx) + test.That(t, err, test.ShouldBeNil) + test.That(t, pc.Size(), test.ShouldEqual, 628) + + pc, err = cam.NextPointCloud(ctx) + test.That(t, err, test.ShouldBeNil) + test.That(t, pc.Size(), test.ShouldEqual, 628) + + err = cam.Close(ctx) + test.That(t, err, test.ShouldBeNil) + + colorImgPath := artifact.MustPath("vision/objectdetection/detection_test.jpg") + cfg.Color = colorImgPath + cam, err = newCamera(ctx, resource.Name{API: camera.API}, cfg) + test.That(t, err, test.ShouldBeNil) + + stream, err := cam.Stream(ctx) + test.That(t, err, test.ShouldBeNil) + + readInImage, err := rimage.NewImageFromFile(artifact.MustPath("vision/objectdetection/detection_test.jpg")) + test.That(t, err, test.ShouldBeNil) + + strmImg, _, err := stream.Next(ctx) + test.That(t, err, test.ShouldBeNil) + test.That(t, strmImg, test.ShouldResemble, readInImage) + test.That(t, strmImg.Bounds(), test.ShouldResemble, readInImage.Bounds()) + + err = cam.Close(ctx) + test.That(t, err, test.ShouldBeNil) +} + +func TestColor(t *testing.T) { + colorImgPath := artifact.MustPath("vision/objectdetection/detection_test.jpg") + cfg := &fileSourceConfig{Color: colorImgPath} + ctx := context.Background() + cam, err := newCamera(ctx, resource.Name{API: camera.API}, cfg) + test.That(t, err, test.ShouldBeNil) + + stream, err := cam.Stream(ctx) + test.That(t, err, test.ShouldBeNil) + + _, err = cam.NextPointCloud(ctx) + test.That(t, err, test.ShouldNotBeNil) + + readInImage, err := rimage.NewImageFromFile(artifact.MustPath("vision/objectdetection/detection_test.jpg")) + test.That(t, err, test.ShouldBeNil) + + strmImg, _, err := stream.Next(ctx) + test.That(t, err, test.ShouldBeNil) + test.That(t, strmImg, test.ShouldResemble, readInImage) + test.That(t, strmImg.Bounds(), test.ShouldResemble, readInImage.Bounds()) + + err = cam.Close(ctx) + test.That(t, err, test.ShouldBeNil) +} + +func TestColorOddResolution(t *testing.T) { + imgFilePath := t.TempDir() + "/test_img.jpg" + imgFile, err := os.Create(imgFilePath) + test.That(t, err, test.ShouldBeNil) + + img := image.NewRGBA(image.Rect(0, 0, 3, 3)) + for x := 0; x < img.Bounds().Dx(); x++ { + for y := 0; y < img.Bounds().Dy(); y++ { + img.Set(x, y, color.White) + } + } + err = jpeg.Encode(imgFile, img, nil) + test.That(t, err, test.ShouldBeNil) + err = imgFile.Close() + test.That(t, err, test.ShouldBeNil) + + cfg := &fileSourceConfig{Color: imgFilePath} + ctx := context.Background() + cam, err := newCamera(ctx, resource.Name{API: camera.API}, cfg) + test.That(t, err, test.ShouldBeNil) + + stream, err := cam.Stream(ctx) + test.That(t, err, test.ShouldBeNil) + + readInImage, err := rimage.NewImageFromFile(imgFilePath) + test.That(t, err, test.ShouldBeNil) + + strmImg, _, err := stream.Next(ctx) + test.That(t, err, test.ShouldBeNil) + + expectedBounds := image.Rect(0, 0, readInImage.Bounds().Dx()-1, readInImage.Bounds().Dy()-1) + test.That(t, strmImg, test.ShouldResemble, readInImage.SubImage(expectedBounds)) + test.That(t, strmImg.Bounds(), test.ShouldResemble, expectedBounds) + + err = cam.Close(ctx) + test.That(t, err, test.ShouldBeNil) +} diff --git a/components/camera/videosource/webcam.go b/components/camera/videosource/webcam.go index c1e74edfea2..f463b4a1802 100644 --- a/components/camera/videosource/webcam.go +++ b/components/camera/videosource/webcam.go @@ -2,6 +2,7 @@ package videosource import ( "context" + "fmt" "image" "path/filepath" "strings" @@ -23,6 +24,7 @@ import ( "go.viam.com/rdk/components/camera" jetsoncamera "go.viam.com/rdk/components/camera/platforms/jetson" + debugLogger "go.viam.com/rdk/components/camera/videosource/logging" "go.viam.com/rdk/pointcloud" "go.viam.com/rdk/resource" "go.viam.com/rdk/rimage/transform" @@ -107,9 +109,32 @@ func Discover(_ context.Context, getDrivers func() []driver.Driver, logger golog } webcams = append(webcams, wc) } + + goutils.UncheckedError(debugLogger.GLoggerCamComp.Log("discovery service", webcamsToMap(webcams))) return &pb.Webcams{Webcams: webcams}, nil } +func webcamsToMap(webcams []*pb.Webcam) debugLogger.InfoMap { + info := make(debugLogger.InfoMap) + for _, w := range webcams { + k := w.Name + v := fmt.Sprintf("ID: %s\n", w.Id) + v += fmt.Sprintf("Status: %s\n", w.Status) + v += fmt.Sprintf("Label: %s\n", w.Label) + v += "Properties:" + for _, p := range w.Properties { + v += fmt.Sprintf(" :%s=%-4d | %s=%-4d | %s=%-5s | %s=%-4.2f\n", + "width_px", p.GetWidthPx(), + "height_px", p.GetHeightPx(), + "frame_format", p.GetFrameFormat(), + "frame_rate", p.GetFrameRate(), + ) + } + info[k] = v + } + return info +} + func getProperties(d driver.Driver) (_ []prop.Media, err error) { // Need to open driver to get properties if d.Status() == driver.StateClosed { @@ -250,6 +275,27 @@ func NewWebcam( return nil, err } cam.Monitor() + + s, err := cam.Stream(ctx) + if err != nil { + goutils.UncheckedError(debugLogger.GLoggerCamComp.Log("camera test results", + debugLogger.InfoMap{ + "name": cam.Name().Name, + "error": fmt.Sprint(err), + }, + )) + return cam, nil + } + + img, _, err := s.Next(ctx) + goutils.UncheckedError(debugLogger.GLoggerCamComp.Log("camera test results", + debugLogger.InfoMap{ + "camera name": cam.Name().Name, + "has non-nil image?": fmt.Sprintf("%t", img != nil), + "error:": fmt.Sprintf("%s", err), + }, + )) + return cam, nil } @@ -514,6 +560,22 @@ func (c *monitoredWebcam) Projector(ctx context.Context) (transform.Projector, e return c.exposedProjector.Projector(ctx) } +func (c *monitoredWebcam) Images(ctx context.Context) ([]camera.NamedImage, resource.ResponseMetadata, error) { + if c, ok := c.underlyingSource.(camera.ImagesSource); ok { + return c.Images(ctx) + } + img, release, err := camera.ReadImage(ctx, c.underlyingSource) + if err != nil { + return nil, resource.ResponseMetadata{}, errors.Wrap(err, "monitoredWebcam: call to get Images failed") + } + defer func() { + if release != nil { + release() + } + }() + return []camera.NamedImage{{img, c.Name().Name}}, resource.ResponseMetadata{time.Now()}, nil +} + func (c *monitoredWebcam) Stream(ctx context.Context, errHandlers ...gostream.ErrorHandler) (gostream.VideoStream, error) { c.mu.RLock() defer c.mu.RUnlock() diff --git a/components/encoder/ams/ams_as5048.go b/components/encoder/ams/ams_as5048.go index 4ea17b8b017..72f9cb19b87 100644 --- a/components/encoder/ams/ams_as5048.go +++ b/components/encoder/ams/ams_as5048.go @@ -319,10 +319,10 @@ func (enc *Encoder) ResetPosition( } // Properties returns a list of all the position types that are supported by a given encoder. -func (enc *Encoder) Properties(ctx context.Context, extra map[string]interface{}) (map[encoder.Feature]bool, error) { - return map[encoder.Feature]bool{ - encoder.TicksCountSupported: true, - encoder.AngleDegreesSupported: true, +func (enc *Encoder) Properties(ctx context.Context, extra map[string]interface{}) (encoder.Properties, error) { + return encoder.Properties{ + TicksCountSupported: true, + AngleDegreesSupported: true, }, nil } diff --git a/components/encoder/client.go b/components/encoder/client.go index 1ec529957bf..bbff2d22ca1 100644 --- a/components/encoder/client.go +++ b/components/encoder/client.go @@ -75,17 +75,17 @@ func (c *client) ResetPosition(ctx context.Context, extra map[string]interface{} } // Properties returns a list of all the position types that are supported by a given encoder. -func (c *client) Properties(ctx context.Context, extra map[string]interface{}) (map[Feature]bool, error) { +func (c *client) Properties(ctx context.Context, extra map[string]interface{}) (Properties, error) { ext, err := structpb.NewStruct(extra) if err != nil { - return nil, err + return Properties{}, err } req := &pb.GetPropertiesRequest{Name: c.name, Extra: ext} resp, err := c.client.GetProperties(ctx, req) if err != nil { - return nil, err + return Properties{}, err } - return ProtoFeaturesToMap(resp), nil + return ProtoFeaturesToProperties(resp), nil } func (c *client) DoCommand(ctx context.Context, cmd map[string]interface{}) (map[string]interface{}, error) { diff --git a/components/encoder/client_test.go b/components/encoder/client_test.go index e75800fcdf1..ed0745adce3 100644 --- a/components/encoder/client_test.go +++ b/components/encoder/client_test.go @@ -2,7 +2,6 @@ package encoder_test import ( "context" - "errors" "net" "testing" @@ -48,26 +47,26 @@ func TestClient(t *testing.T) { actualExtra = extra return 42.0, encoder.PositionTypeUnspecified, nil } - workingEncoder.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (map[encoder.Feature]bool, error) { + workingEncoder.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (encoder.Properties, error) { actualExtra = extra - return map[encoder.Feature]bool{ - encoder.TicksCountSupported: true, - encoder.AngleDegreesSupported: false, + return encoder.Properties{ + TicksCountSupported: true, + AngleDegreesSupported: false, }, nil } failingEncoder.ResetPositionFunc = func(ctx context.Context, extra map[string]interface{}) error { - return errors.New("set to zero failed") + return errSetToZeroFailed } failingEncoder.PositionFunc = func( ctx context.Context, positionType encoder.PositionType, extra map[string]interface{}, ) (float64, encoder.PositionType, error) { - return 0, encoder.PositionTypeUnspecified, errors.New("position unavailable") + return 0, encoder.PositionTypeUnspecified, errPositionUnavailable } - failingEncoder.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (map[encoder.Feature]bool, error) { - return nil, errors.New("get properties failed") + failingEncoder.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (encoder.Properties, error) { + return encoder.Properties{}, errGetPropertiesFailed } resourceMap := map[resource.Name]encoder.Encoder{ @@ -91,7 +90,7 @@ func TestClient(t *testing.T) { cancel() _, err := viamgrpc.Dial(cancelCtx, listener1.Addr().String(), logger) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "canceled") + test.That(t, err, test.ShouldBeError, context.Canceled) }) conn, err := viamgrpc.Dial(context.Background(), listener1.Addr().String(), logger) @@ -132,9 +131,11 @@ func TestClient(t *testing.T) { t.Run("client tests for failing encoder", func(t *testing.T) { err = failingEncoderClient.ResetPosition(context.Background(), nil) test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, errSetToZeroFailed.Error()) pos, _, err := failingEncoderClient.Position(context.Background(), encoder.PositionTypeUnspecified, nil) test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, errPositionUnavailable.Error()) test.That(t, pos, test.ShouldEqual, 0.0) test.That(t, failingEncoderClient.Close(context.Background()), test.ShouldBeNil) diff --git a/components/encoder/collectors.go b/components/encoder/collectors.go index 4e702b8cdf8..f178477a1ca 100644 --- a/components/encoder/collectors.go +++ b/components/encoder/collectors.go @@ -2,6 +2,7 @@ package encoder import ( "context" + "errors" "google.golang.org/protobuf/types/known/anypb" @@ -33,8 +34,13 @@ func newTicksCountCollector(resource interface{}, params data.CollectorParams) ( } cFunc := data.CaptureFunc(func(ctx context.Context, _ map[string]*anypb.Any) (interface{}, error) { - v, _, err := encoder.Position(ctx, PositionTypeUnspecified, nil) + v, _, err := encoder.Position(ctx, PositionTypeUnspecified, data.FromDMExtraMap) if err != nil { + // A modular filter component can be created to filter the readings from a component. The error ErrNoCaptureToStore + // is used in the datamanager to exclude readings from being captured and stored. + if errors.Is(err, data.ErrNoCaptureToStore) { + return nil, err + } return nil, data.FailedToReadErr(params.ComponentName, ticksCount.String(), err) } return Ticks{Ticks: int64(v)}, nil diff --git a/components/encoder/encoder.go b/components/encoder/encoder.go index 82c93da4637..46e49863f94 100644 --- a/components/encoder/encoder.go +++ b/components/encoder/encoder.go @@ -68,7 +68,7 @@ type Encoder interface { ResetPosition(ctx context.Context, extra map[string]interface{}) error // Properties returns a list of all the position types that are supported by a given encoder - Properties(ctx context.Context, extra map[string]interface{}) (map[Feature]bool, error) + Properties(ctx context.Context, extra map[string]interface{}) (Properties, error) } // Named is a helper for getting the named Encoder's typed resource name. diff --git a/components/encoder/errors.go b/components/encoder/errors.go index 3ee6f4571a9..bf12299422f 100644 --- a/components/encoder/errors.go +++ b/components/encoder/errors.go @@ -10,8 +10,8 @@ func NewPositionTypeUnsupportedError(positionType PositionType) error { // NewEncodedMotorPositionTypeUnsupportedError returns a standard error for when // an encoded motor tries to use an encoder that doesn't support Ticks. -func NewEncodedMotorPositionTypeUnsupportedError(props map[Feature]bool) error { - if props[AngleDegreesSupported] { +func NewEncodedMotorPositionTypeUnsupportedError(props Properties) error { + if props.AngleDegreesSupported { return errors.New( "encoder position type is Angle Degrees, need an encoder that supports Ticks") } diff --git a/components/encoder/fake/encoder.go b/components/encoder/fake/encoder.go index a27630104f1..32e3042b312 100644 --- a/components/encoder/fake/encoder.go +++ b/components/encoder/fake/encoder.go @@ -24,7 +24,7 @@ func init() { conf resource.Config, logger golog.Logger, ) (encoder.Encoder, error) { - return NewEncoder(ctx, conf) + return NewEncoder(ctx, conf, logger) }, }) } @@ -33,11 +33,13 @@ func init() { func NewEncoder( ctx context.Context, cfg resource.Config, + logger golog.Logger, ) (encoder.Encoder, error) { e := &fakeEncoder{ Named: cfg.ResourceName().AsNamed(), position: 0, positionType: encoder.PositionTypeTicks, + logger: logger, } if err := e.Reconfigure(ctx, nil, cfg); err != nil { return nil, err @@ -82,6 +84,7 @@ type fakeEncoder struct { positionType encoder.PositionType activeBackgroundWorkers sync.WaitGroup + logger golog.Logger mu sync.RWMutex position int64 @@ -139,10 +142,10 @@ func (e *fakeEncoder) ResetPosition(ctx context.Context, extra map[string]interf } // Properties returns a list of all the position types that are supported by a given encoder. -func (e *fakeEncoder) Properties(ctx context.Context, extra map[string]interface{}) (map[encoder.Feature]bool, error) { - return map[encoder.Feature]bool{ - encoder.TicksCountSupported: true, - encoder.AngleDegreesSupported: false, +func (e *fakeEncoder) Properties(ctx context.Context, extra map[string]interface{}) (encoder.Properties, error) { + return encoder.Properties{ + TicksCountSupported: true, + AngleDegreesSupported: false, }, nil } diff --git a/components/encoder/fake/encoder_test.go b/components/encoder/fake/encoder_test.go index 6687e820e84..60b6e9a18ed 100644 --- a/components/encoder/fake/encoder_test.go +++ b/components/encoder/fake/encoder_test.go @@ -4,6 +4,7 @@ import ( "context" "testing" + "github.com/edaniels/golog" "go.viam.com/test" "go.viam.com/utils/testutils" @@ -17,7 +18,8 @@ func TestEncoder(t *testing.T) { UpdateRate: 100, } cfg := resource.Config{Name: "enc1", ConvertedAttributes: &ic} - e, _ := NewEncoder(ctx, cfg) + logger := golog.NewTestLogger(t) + e, _ := NewEncoder(ctx, cfg, logger) // Get and set position t.Run("get and set position", func(t *testing.T) { @@ -60,8 +62,8 @@ func TestEncoder(t *testing.T) { tb.Helper() props, err := e.Properties(ctx, nil) test.That(tb, err, test.ShouldBeNil) - test.That(tb, props[encoder.TicksCountSupported], test.ShouldBeTrue) - test.That(tb, props[encoder.AngleDegreesSupported], test.ShouldBeFalse) + test.That(tb, props.TicksCountSupported, test.ShouldBeTrue) + test.That(tb, props.AngleDegreesSupported, test.ShouldBeFalse) }) }) diff --git a/components/encoder/features.go b/components/encoder/features.go deleted file mode 100644 index 6ab9b422f34..00000000000 --- a/components/encoder/features.go +++ /dev/null @@ -1,36 +0,0 @@ -// Package encoder contains an enum representing optional encoder features -package encoder - -import ( - pb "go.viam.com/api/component/encoder/v1" -) - -// Feature is an enum representing an optional encoder feature. -type Feature string - -// TicksCountSupported and AngleDegreesSupported represesnts the feature -// of an encoder being able to report ticks and/or degrees, respectively. -const ( - TicksCountSupported Feature = "Ticks" - AngleDegreesSupported Feature = "Degrees" -) - -// ProtoFeaturesToMap takes a GetPropertiesResponse and returns -// an equivalent Feature-to-boolean map. -func ProtoFeaturesToMap(resp *pb.GetPropertiesResponse) map[Feature]bool { - return map[Feature]bool{ - TicksCountSupported: resp.TicksCountSupported, - AngleDegreesSupported: resp.AngleDegreesSupported, - } -} - -// FeatureMapToProtoResponse takes a map of features to booleans (indicating -// whether the feature is supported) and converts it to a GetPropertiesResponse. -func FeatureMapToProtoResponse( - featureMap map[Feature]bool, -) (*pb.GetPropertiesResponse, error) { - return &pb.GetPropertiesResponse{ - TicksCountSupported: featureMap[TicksCountSupported], - AngleDegreesSupported: featureMap[AngleDegreesSupported], - }, nil -} diff --git a/components/encoder/incremental/incremental_encoder.go b/components/encoder/incremental/incremental_encoder.go index f4cf2e4907f..2a82a186156 100644 --- a/components/encoder/incremental/incremental_encoder.go +++ b/components/encoder/incremental/incremental_encoder.go @@ -49,9 +49,8 @@ type Encoder struct { encBName string logger golog.Logger - // TODO(RSDK-2672): This is exposed for tests and should be unexported with - // the constructor being used instead. - CancelCtx context.Context + + cancelCtx context.Context cancelFunc func() activeBackgroundWorkers sync.WaitGroup } @@ -98,7 +97,7 @@ func NewIncrementalEncoder( e := &Encoder{ Named: conf.ResourceName().AsNamed(), logger: logger, - CancelCtx: cancelCtx, + cancelCtx: cancelCtx, cancelFunc: cancelFunc, position: 0, positionType: encoder.PositionTypeTicks, @@ -154,7 +153,7 @@ func (e *Encoder) Reconfigure( } utils.UncheckedError(e.Close(ctx)) cancelCtx, cancelFunc := context.WithCancel(context.Background()) - e.CancelCtx = cancelCtx + e.cancelCtx = cancelCtx e.cancelFunc = cancelFunc e.mu.Lock() @@ -239,7 +238,7 @@ func (e *Encoder) Start(ctx context.Context) { // statement guarantees that we'll return if we're supposed to, regardless of whether // there's data in the other channels. select { - case <-e.CancelCtx.Done(): + case <-e.cancelCtx.Done(): return default: } @@ -247,7 +246,7 @@ func (e *Encoder) Start(ctx context.Context) { var tick board.Tick select { - case <-e.CancelCtx.Done(): + case <-e.cancelCtx.Done(): return case tick = <-chanA: aLevel = 0 @@ -311,10 +310,10 @@ func (e *Encoder) ResetPosition(ctx context.Context, extra map[string]interface{ } // Properties returns a list of all the position types that are supported by a given encoder. -func (e *Encoder) Properties(ctx context.Context, extra map[string]interface{}) (map[encoder.Feature]bool, error) { - return map[encoder.Feature]bool{ - encoder.TicksCountSupported: true, - encoder.AngleDegreesSupported: false, +func (e *Encoder) Properties(ctx context.Context, extra map[string]interface{}) (encoder.Properties, error) { + return encoder.Properties{ + TicksCountSupported: true, + AngleDegreesSupported: false, }, nil } diff --git a/components/encoder/incremental/incremental_test.go b/components/encoder/incremental/incremental_test.go index 026603decd6..76d0f24ef30 100644 --- a/components/encoder/incremental/incremental_test.go +++ b/components/encoder/incremental/incremental_test.go @@ -154,8 +154,8 @@ func TestEncoder(t *testing.T) { tb.Helper() props, err := enc.Properties(ctx, nil) test.That(tb, err, test.ShouldBeNil) - test.That(tb, props[encoder.TicksCountSupported], test.ShouldBeTrue) - test.That(tb, props[encoder.AngleDegreesSupported], test.ShouldBeFalse) + test.That(tb, props.TicksCountSupported, test.ShouldBeTrue) + test.That(tb, props.AngleDegreesSupported, test.ShouldBeFalse) }) }) } diff --git a/components/encoder/properties.go b/components/encoder/properties.go new file mode 100644 index 00000000000..5962f7606ea --- /dev/null +++ b/components/encoder/properties.go @@ -0,0 +1,29 @@ +package encoder + +import pb "go.viam.com/api/component/encoder/v1" + +// Properties holds the properties of the encoder. +type Properties struct { + TicksCountSupported bool + AngleDegreesSupported bool +} + +// ProtoFeaturesToProperties takes a GetPropertiesResponse and returns +// an equivalent Properties struct. +func ProtoFeaturesToProperties(resp *pb.GetPropertiesResponse) Properties { + return Properties{ + TicksCountSupported: resp.TicksCountSupported, + AngleDegreesSupported: resp.AngleDegreesSupported, + } +} + +// PropertiesToProtoResponse takes a properties struct and converts it +// to a GetPropertiesResponse. +func PropertiesToProtoResponse( + props Properties, +) (*pb.GetPropertiesResponse, error) { + return &pb.GetPropertiesResponse{ + TicksCountSupported: props.TicksCountSupported, + AngleDegreesSupported: props.AngleDegreesSupported, + }, nil +} diff --git a/components/encoder/server.go b/components/encoder/server.go index d577d5018b1..a7e1ab5be10 100644 --- a/components/encoder/server.go +++ b/components/encoder/server.go @@ -70,7 +70,7 @@ func (s *serviceServer) GetProperties( if err != nil { return nil, err } - return FeatureMapToProtoResponse(features) + return PropertiesToProtoResponse(features) } // DoCommand receives arbitrary commands. diff --git a/components/encoder/server_test.go b/components/encoder/server_test.go index 7f08d983c73..74927e564cf 100644 --- a/components/encoder/server_test.go +++ b/components/encoder/server_test.go @@ -14,6 +14,13 @@ import ( "go.viam.com/rdk/testutils/inject" ) +var ( + errPositionUnavailable = errors.New("position unavailable") + errSetToZeroFailed = errors.New("set to zero failed") + errPropertiesNotFound = errors.New("properties not found") + errGetPropertiesFailed = errors.New("get properties failed") +) + func newServer() (pb.EncoderServiceServer, *inject.Encoder, *inject.Encoder, error) { injectEncoder1 := &inject.Encoder{} injectEncoder2 := &inject.Encoder{} @@ -38,18 +45,21 @@ func TestServerGetPosition(t *testing.T) { resp, err := encoderServer.GetPosition(context.Background(), &req) test.That(t, resp, test.ShouldBeNil) test.That(t, err, test.ShouldNotBeNil) + test.That(t, resource.IsNotFoundError(err), test.ShouldBeTrue) failingEncoder.PositionFunc = func( ctx context.Context, positionType encoder.PositionType, extra map[string]interface{}, ) (float64, encoder.PositionType, error) { - return 0, encoder.PositionTypeUnspecified, errors.New("position unavailable") + return 0, encoder.PositionTypeUnspecified, errPositionUnavailable } + + // Position unavailable test req = pb.GetPositionRequest{Name: failEncoderName} resp, err = encoderServer.GetPosition(context.Background(), &req) test.That(t, resp, test.ShouldBeNil) - test.That(t, err, test.ShouldNotBeNil) + test.That(t, err, test.ShouldBeError, errPositionUnavailable) workingEncoder.PositionFunc = func( ctx context.Context, @@ -74,12 +84,13 @@ func TestServerResetPosition(t *testing.T) { test.That(t, err, test.ShouldNotBeNil) failingEncoder.ResetPositionFunc = func(ctx context.Context, extra map[string]interface{}) error { - return errors.New("set to zero failed") + return errSetToZeroFailed } req = pb.ResetPositionRequest{Name: failEncoderName} resp, err = encoderServer.ResetPosition(context.Background(), &req) test.That(t, resp, test.ShouldNotBeNil) test.That(t, err, test.ShouldNotBeNil) + test.That(t, err, test.ShouldBeError, errSetToZeroFailed) workingEncoder.ResetPositionFunc = func(ctx context.Context, extra map[string]interface{}) error { return nil @@ -99,18 +110,19 @@ func TestServerGetProperties(t *testing.T) { test.That(t, resp, test.ShouldBeNil) test.That(t, err, test.ShouldNotBeNil) - failingEncoder.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (map[encoder.Feature]bool, error) { - return nil, errors.New("properties not found") + failingEncoder.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (encoder.Properties, error) { + return encoder.Properties{}, errPropertiesNotFound } req = pb.GetPropertiesRequest{Name: failEncoderName} resp, err = encoderServer.GetProperties(context.Background(), &req) test.That(t, resp, test.ShouldBeNil) test.That(t, err, test.ShouldNotBeNil) + test.That(t, err, test.ShouldBeError, errPropertiesNotFound) - workingEncoder.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (map[encoder.Feature]bool, error) { - return map[encoder.Feature]bool{ - encoder.TicksCountSupported: true, - encoder.AngleDegreesSupported: false, + workingEncoder.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (encoder.Properties, error) { + return encoder.Properties{ + TicksCountSupported: true, + AngleDegreesSupported: false, }, nil } req = pb.GetPropertiesRequest{Name: testEncoderName} diff --git a/components/encoder/single/single_encoder.go b/components/encoder/single/single_encoder.go index ed997b91df7..05cac8bcc9a 100644 --- a/components/encoder/single/single_encoder.go +++ b/components/encoder/single/single_encoder.go @@ -67,9 +67,8 @@ type Encoder struct { positionType encoder.PositionType logger golog.Logger - // TODO(RSDK-2672): This is exposed for tests and should be unexported with - // the constructor being used instead. - CancelCtx context.Context + + cancelCtx context.Context cancelFunc func() activeBackgroundWorkers sync.WaitGroup } @@ -119,7 +118,7 @@ func NewSingleEncoder( e := &Encoder{ Named: conf.ResourceName().AsNamed(), logger: logger, - CancelCtx: cancelCtx, + cancelCtx: cancelCtx, cancelFunc: cancelFunc, position: 0, positionType: encoder.PositionTypeTicks, @@ -164,7 +163,7 @@ func (e *Encoder) Reconfigure( } utils.UncheckedError(e.Close(ctx)) cancelCtx, cancelFunc := context.WithCancel(context.Background()) - e.CancelCtx = cancelCtx + e.cancelCtx = cancelCtx e.cancelFunc = cancelFunc e.mu.Lock() @@ -189,13 +188,13 @@ func (e *Encoder) Start(ctx context.Context) { defer e.I.RemoveCallback(encoderChannel) for { select { - case <-e.CancelCtx.Done(): + case <-e.cancelCtx.Done(): return default: } select { - case <-e.CancelCtx.Done(): + case <-e.cancelCtx.Done(): return case <-encoderChannel: } @@ -237,10 +236,10 @@ func (e *Encoder) ResetPosition(ctx context.Context, extra map[string]interface{ } // Properties returns a list of all the position types that are supported by a given encoder. -func (e *Encoder) Properties(ctx context.Context, extra map[string]interface{}) (map[encoder.Feature]bool, error) { - return map[encoder.Feature]bool{ - encoder.TicksCountSupported: true, - encoder.AngleDegreesSupported: false, +func (e *Encoder) Properties(ctx context.Context, extra map[string]interface{}) (encoder.Properties, error) { + return encoder.Properties{ + TicksCountSupported: true, + AngleDegreesSupported: false, }, nil } diff --git a/components/encoder/single/single_encoder_test.go b/components/encoder/single/single_encoder_test.go index 3b4bbbd32a6..1cfc07e6298 100644 --- a/components/encoder/single/single_encoder_test.go +++ b/components/encoder/single/single_encoder_test.go @@ -225,8 +225,8 @@ func TestEncoder(t *testing.T) { tb.Helper() props, err := enc.Properties(ctx, nil) test.That(tb, err, test.ShouldBeNil) - test.That(tb, props[encoder.TicksCountSupported], test.ShouldBeTrue) - test.That(tb, props[encoder.AngleDegreesSupported], test.ShouldBeFalse) + test.That(tb, props.TicksCountSupported, test.ShouldBeTrue) + test.That(tb, props.AngleDegreesSupported, test.ShouldBeFalse) }) }) } diff --git a/components/gantry/client_test.go b/components/gantry/client_test.go index 1a40fa07278..61c621f055d 100644 --- a/components/gantry/client_test.go +++ b/components/gantry/client_test.go @@ -6,7 +6,6 @@ import ( "testing" "github.com/edaniels/golog" - "github.com/pkg/errors" "go.viam.com/test" "go.viam.com/utils/rpc" @@ -47,7 +46,7 @@ func TestClient(t *testing.T) { } injectGantry.StopFunc = func(ctx context.Context, extra map[string]interface{}) error { extra1 = extra - return errors.New("no stop") + return errStopFailed } injectGantry.HomeFunc = func(ctx context.Context, extra map[string]interface{}) (bool, error) { extra1 = extra @@ -79,7 +78,7 @@ func TestClient(t *testing.T) { } injectGantry2.HomeFunc = func(ctx context.Context, extra map[string]interface{}) (bool, error) { extra2 = extra - return false, errors.New("Home error") + return false, errHomingFailed } gantrySvc, err := resource.NewAPIResourceCollection( @@ -103,7 +102,7 @@ func TestClient(t *testing.T) { cancel() _, err := viamgrpc.Dial(cancelCtx, listener1.Addr().String(), logger) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "canceled") + test.That(t, err, test.ShouldBeError, context.Canceled) }) // working @@ -142,7 +141,7 @@ func TestClient(t *testing.T) { err = gantry1Client.Stop(context.Background(), map[string]interface{}{"foo": 456, "bar": "567"}) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "no stop") + test.That(t, err.Error(), test.ShouldContainSubstring, errStopFailed.Error()) test.That(t, extra1, test.ShouldResemble, map[string]interface{}{"foo": 456., "bar": "567"}) test.That(t, gantry1Client.Close(context.Background()), test.ShouldBeNil) @@ -161,7 +160,7 @@ func TestClient(t *testing.T) { test.That(t, extra2, test.ShouldResemble, map[string]interface{}{"foo": "123", "bar": 234.}) homed, err := client2.Home(context.Background(), map[string]interface{}{"foo": 345, "bar": "456"}) - test.That(t, err.Error(), test.ShouldContainSubstring, "Home error") + test.That(t, err.Error(), test.ShouldContainSubstring, errHomingFailed.Error()) test.That(t, homed, test.ShouldBeFalse) test.That(t, extra2, test.ShouldResemble, map[string]interface{}{"foo": 345., "bar": "456"}) diff --git a/components/gantry/collectors.go b/components/gantry/collectors.go index 24665aeb273..26a3c56aae2 100644 --- a/components/gantry/collectors.go +++ b/components/gantry/collectors.go @@ -2,6 +2,7 @@ package gantry import ( "context" + "errors" "google.golang.org/protobuf/types/known/anypb" @@ -37,8 +38,13 @@ func newPositionCollector(resource interface{}, params data.CollectorParams) (da } cFunc := data.CaptureFunc(func(ctx context.Context, _ map[string]*anypb.Any) (interface{}, error) { - v, err := gantry.Position(ctx, nil) + v, err := gantry.Position(ctx, data.FromDMExtraMap) if err != nil { + // A modular filter component can be created to filter the readings from a component. The error ErrNoCaptureToStore + // is used in the datamanager to exclude readings from being captured and stored. + if errors.Is(err, data.ErrNoCaptureToStore) { + return nil, err + } return nil, data.FailedToReadErr(params.ComponentName, position.String(), err) } return Position{Position: v}, nil @@ -58,8 +64,13 @@ func newLengthsCollector(resource interface{}, params data.CollectorParams) (dat } cFunc := data.CaptureFunc(func(ctx context.Context, _ map[string]*anypb.Any) (interface{}, error) { - v, err := gantry.Lengths(ctx, nil) + v, err := gantry.Lengths(ctx, data.FromDMExtraMap) if err != nil { + // A modular filter component can be created to filter the readings from a component. The error ErrNoCaptureToStore + // is used in the datamanager to exclude readings from being captured and stored. + if errors.Is(err, data.ErrNoCaptureToStore) { + return nil, err + } return nil, data.FailedToReadErr(params.ComponentName, lengths.String(), err) } return Lengths{Lengths: v}, nil diff --git a/components/gantry/fake/gantry.go b/components/gantry/fake/gantry.go index ee4bc70228d..af551a81f4e 100644 --- a/components/gantry/fake/gantry.go +++ b/components/gantry/fake/gantry.go @@ -25,13 +25,13 @@ func init() { conf resource.Config, logger golog.Logger, ) (gantry.Gantry, error) { - return NewGantry(conf.ResourceName()), nil + return NewGantry(conf.ResourceName(), logger), nil }, }) } // NewGantry returns a new fake gantry. -func NewGantry(name resource.Name) gantry.Gantry { +func NewGantry(name resource.Name, logger golog.Logger) gantry.Gantry { return &Gantry{ testutils.NewUnimplementedResource(name), resource.TriviallyReconfigurable{}, @@ -41,6 +41,7 @@ func NewGantry(name resource.Name) gantry.Gantry { []float64{5}, 2, r3.Vector{X: 1, Y: 0, Z: 0}, + logger, } } @@ -54,6 +55,7 @@ type Gantry struct { lengths []float64 lengthMeters float64 frame r3.Vector + logger golog.Logger } // Position returns the position in meters. @@ -68,6 +70,7 @@ func (g *Gantry) Lengths(ctx context.Context, extra map[string]interface{}) ([]f // Home runs the homing sequence of the gantry and returns true once completed. func (g *Gantry) Home(ctx context.Context, extra map[string]interface{}) (bool, error) { + g.logger.Info("homing") return true, nil } diff --git a/components/gantry/multiaxis/multiaxis.go b/components/gantry/multiaxis/multiaxis.go index 597fee07f6f..bc83a53d060 100644 --- a/components/gantry/multiaxis/multiaxis.go +++ b/components/gantry/multiaxis/multiaxis.go @@ -7,12 +7,14 @@ import ( "github.com/edaniels/golog" "github.com/pkg/errors" + "go.uber.org/multierr" "go.viam.com/utils" "go.viam.com/rdk/components/gantry" "go.viam.com/rdk/operation" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" + rdkutils "go.viam.com/rdk/utils" ) var model = resource.DefaultModelFamily.WithModel("multi-axis") @@ -31,7 +33,7 @@ type multiAxis struct { logger golog.Logger moveSimultaneously bool model referenceframe.Model - opMgr operation.SingleOperationManager + opMgr *operation.SingleOperationManager workers sync.WaitGroup } @@ -68,6 +70,7 @@ func newMultiAxis( mAx := &multiAxis{ Named: conf.ResourceName().AsNamed(), logger: logger, + opMgr: operation.NewSingleOperationManager(), } for _, s := range newConf.SubAxes { @@ -121,6 +124,7 @@ func (g *multiAxis) MoveToPosition(ctx context.Context, positions, speeds []floa ) } + fs := []rdkutils.SimpleFunc{} idx := 0 for _, subAx := range g.subAxes { subAxNum, err := subAx.Lengths(ctx, extra) @@ -138,9 +142,19 @@ func (g *multiAxis) MoveToPosition(ctx context.Context, positions, speeds []floa } idx += len(subAxNum) - err = subAx.MoveToPosition(ctx, pos, speed, extra) - if err != nil && !errors.Is(err, context.Canceled) { - return err + if g.moveSimultaneously { + singleGantry := subAx + fs = append(fs, func(ctx context.Context) error { return singleGantry.MoveToPosition(ctx, pos, speed, nil) }) + } else { + err = subAx.MoveToPosition(ctx, pos, speed, extra) + if err != nil && !errors.Is(err, context.Canceled) { + return err + } + } + } + if g.moveSimultaneously { + if _, err := rdkutils.RunInParallel(ctx, fs); err != nil { + return multierr.Combine(err, g.Stop(ctx, nil)) } } return nil diff --git a/components/gantry/multiaxis/multiaxis_test.go b/components/gantry/multiaxis/multiaxis_test.go index 0b45b129050..af0b1b2be41 100644 --- a/components/gantry/multiaxis/multiaxis_test.go +++ b/components/gantry/multiaxis/multiaxis_test.go @@ -10,6 +10,7 @@ import ( "go.viam.com/rdk/components/gantry" "go.viam.com/rdk/components/motor" fm "go.viam.com/rdk/components/motor/fake" + "go.viam.com/rdk/operation" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" "go.viam.com/rdk/testutils/inject" @@ -123,17 +124,27 @@ func TestMoveToPosition(t *testing.T) { positions := []float64{} speeds := []float64{} - fakemultiaxis := &multiAxis{} + fakemultiaxis := &multiAxis{ + opMgr: operation.NewSingleOperationManager(), + } err := fakemultiaxis.MoveToPosition(ctx, positions, speeds, nil) test.That(t, err, test.ShouldNotBeNil) - fakemultiaxis = &multiAxis{subAxes: threeAxes, lengthsMm: []float64{1, 2, 3}} + fakemultiaxis = &multiAxis{ + subAxes: threeAxes, + lengthsMm: []float64{1, 2, 3}, + opMgr: operation.NewSingleOperationManager(), + } positions = []float64{1, 2, 3} speeds = []float64{100, 200, 300} err = fakemultiaxis.MoveToPosition(ctx, positions, speeds, nil) test.That(t, err, test.ShouldBeNil) - fakemultiaxis = &multiAxis{subAxes: twoAxes, lengthsMm: []float64{1, 2}} + fakemultiaxis = &multiAxis{ + subAxes: twoAxes, + lengthsMm: []float64{1, 2}, + opMgr: operation.NewSingleOperationManager(), + } positions = []float64{1, 2} speeds = []float64{100, 200} err = fakemultiaxis.MoveToPosition(ctx, positions, speeds, nil) @@ -144,16 +155,26 @@ func TestGoToInputs(t *testing.T) { ctx := context.Background() inputs := []referenceframe.Input{} - fakemultiaxis := &multiAxis{} + fakemultiaxis := &multiAxis{ + opMgr: operation.NewSingleOperationManager(), + } err := fakemultiaxis.GoToInputs(ctx, inputs) test.That(t, err, test.ShouldNotBeNil) - fakemultiaxis = &multiAxis{subAxes: threeAxes, lengthsMm: []float64{1, 2, 3}} + fakemultiaxis = &multiAxis{ + subAxes: threeAxes, + lengthsMm: []float64{1, 2, 3}, + opMgr: operation.NewSingleOperationManager(), + } inputs = []referenceframe.Input{{Value: 1}, {Value: 2}, {Value: 3}} err = fakemultiaxis.GoToInputs(ctx, inputs) test.That(t, err, test.ShouldBeNil) - fakemultiaxis = &multiAxis{subAxes: twoAxes, lengthsMm: []float64{1, 2}} + fakemultiaxis = &multiAxis{ + subAxes: twoAxes, + lengthsMm: []float64{1, 2}, + opMgr: operation.NewSingleOperationManager(), + } inputs = []referenceframe.Input{{Value: 1}, {Value: 2}} err = fakemultiaxis.GoToInputs(ctx, inputs) test.That(t, err, test.ShouldBeNil) @@ -162,12 +183,18 @@ func TestGoToInputs(t *testing.T) { func TestPosition(t *testing.T) { ctx := context.Background() - fakemultiaxis := &multiAxis{subAxes: threeAxes} + fakemultiaxis := &multiAxis{ + subAxes: threeAxes, + opMgr: operation.NewSingleOperationManager(), + } pos, err := fakemultiaxis.Position(ctx, nil) test.That(t, err, test.ShouldBeNil) test.That(t, pos, test.ShouldResemble, []float64{1, 5, 9}) - fakemultiaxis = &multiAxis{subAxes: twoAxes} + fakemultiaxis = &multiAxis{ + subAxes: twoAxes, + opMgr: operation.NewSingleOperationManager(), + } pos, err = fakemultiaxis.Position(ctx, nil) test.That(t, err, test.ShouldBeNil) test.That(t, pos, test.ShouldResemble, []float64{1, 5}) @@ -175,17 +202,25 @@ func TestPosition(t *testing.T) { func TestLengths(t *testing.T) { ctx := context.Background() - fakemultiaxis := &multiAxis{} + fakemultiaxis := &multiAxis{ + opMgr: operation.NewSingleOperationManager(), + } lengths, err := fakemultiaxis.Lengths(ctx, nil) test.That(t, err, test.ShouldBeNil) test.That(t, lengths, test.ShouldResemble, []float64{}) - fakemultiaxis = &multiAxis{subAxes: threeAxes} + fakemultiaxis = &multiAxis{ + subAxes: threeAxes, + opMgr: operation.NewSingleOperationManager(), + } lengths, err = fakemultiaxis.Lengths(ctx, nil) test.That(t, err, test.ShouldBeNil) test.That(t, lengths, test.ShouldResemble, []float64{1, 2, 3}) - fakemultiaxis = &multiAxis{subAxes: twoAxes} + fakemultiaxis = &multiAxis{ + subAxes: twoAxes, + opMgr: operation.NewSingleOperationManager(), + } lengths, err = fakemultiaxis.Lengths(ctx, nil) test.That(t, err, test.ShouldBeNil) @@ -194,17 +229,25 @@ func TestLengths(t *testing.T) { func TestHome(t *testing.T) { ctx := context.Background() - fakemultiaxis := &multiAxis{} + fakemultiaxis := &multiAxis{ + opMgr: operation.NewSingleOperationManager(), + } homed, err := fakemultiaxis.Home(ctx, nil) test.That(t, err, test.ShouldBeNil) test.That(t, homed, test.ShouldBeTrue) - fakemultiaxis = &multiAxis{subAxes: threeAxes} + fakemultiaxis = &multiAxis{ + subAxes: threeAxes, + opMgr: operation.NewSingleOperationManager(), + } homed, err = fakemultiaxis.Home(ctx, nil) test.That(t, err, test.ShouldBeNil) test.That(t, homed, test.ShouldBeTrue) - fakemultiaxis = &multiAxis{subAxes: twoAxes} + fakemultiaxis = &multiAxis{ + subAxes: twoAxes, + opMgr: operation.NewSingleOperationManager(), + } homed, err = fakemultiaxis.Home(ctx, nil) test.That(t, err, test.ShouldBeNil) @@ -213,29 +256,45 @@ func TestHome(t *testing.T) { func TestStop(t *testing.T) { ctx := context.Background() - fakemultiaxis := &multiAxis{} + fakemultiaxis := &multiAxis{ + opMgr: operation.NewSingleOperationManager(), + } test.That(t, fakemultiaxis.Stop(ctx, nil), test.ShouldBeNil) - fakemultiaxis = &multiAxis{subAxes: threeAxes} + fakemultiaxis = &multiAxis{ + subAxes: threeAxes, + opMgr: operation.NewSingleOperationManager(), + } test.That(t, fakemultiaxis.Stop(ctx, nil), test.ShouldBeNil) - fakemultiaxis = &multiAxis{subAxes: twoAxes} + fakemultiaxis = &multiAxis{ + subAxes: twoAxes, + opMgr: operation.NewSingleOperationManager(), + } test.That(t, fakemultiaxis.Stop(ctx, nil), test.ShouldBeNil) } func TestCurrentInputs(t *testing.T) { ctx := context.Background() - fakemultiaxis := &multiAxis{} + fakemultiaxis := &multiAxis{ + opMgr: operation.NewSingleOperationManager(), + } inputs, err := fakemultiaxis.CurrentInputs(ctx) test.That(t, err, test.ShouldNotBeNil) test.That(t, inputs, test.ShouldResemble, []referenceframe.Input(nil)) - fakemultiaxis = &multiAxis{subAxes: threeAxes} + fakemultiaxis = &multiAxis{ + subAxes: threeAxes, + opMgr: operation.NewSingleOperationManager(), + } inputs, err = fakemultiaxis.CurrentInputs(ctx) test.That(t, err, test.ShouldBeNil) test.That(t, inputs, test.ShouldResemble, []referenceframe.Input{{Value: 1}, {Value: 5}, {Value: 9}}) - fakemultiaxis = &multiAxis{subAxes: twoAxes} + fakemultiaxis = &multiAxis{ + subAxes: twoAxes, + opMgr: operation.NewSingleOperationManager(), + } inputs, err = fakemultiaxis.CurrentInputs(ctx) test.That(t, err, test.ShouldBeNil) test.That(t, inputs, test.ShouldResemble, []referenceframe.Input{{Value: 1}, {Value: 5}}) @@ -246,6 +305,7 @@ func TestModelFrame(t *testing.T) { Named: gantry.Named("foo").AsNamed(), subAxes: twoAxes, lengthsMm: []float64{1, 1}, + opMgr: operation.NewSingleOperationManager(), } model := fakemultiaxis.ModelFrame() test.That(t, model, test.ShouldNotBeNil) @@ -254,6 +314,7 @@ func TestModelFrame(t *testing.T) { Named: gantry.Named("foo").AsNamed(), subAxes: threeAxes, lengthsMm: []float64{1, 1, 1}, + opMgr: operation.NewSingleOperationManager(), } model = fakemultiaxis.ModelFrame() test.That(t, model, test.ShouldNotBeNil) diff --git a/components/gantry/server_test.go b/components/gantry/server_test.go index 30b9b6ae94b..8f246813df1 100644 --- a/components/gantry/server_test.go +++ b/components/gantry/server_test.go @@ -14,6 +14,15 @@ import ( "go.viam.com/rdk/testutils/inject" ) +var ( + errPositionFailed = errors.New("couldn't get position") + errHomingFailed = errors.New("homing unsuccessful") + errMoveToPositionFailed = errors.New("couldn't move to position") + errLengthsFailed = errors.New("couldn't get lengths") + errStopFailed = errors.New("couldn't stop") + errGantryNotFound = errors.New("not found") +) + func newServer() (pb.GantryServiceServer, *inject.Gantry, *inject.Gantry, error) { injectGantry := &inject.Gantry{} injectGantry2 := &inject.Gantry{} @@ -65,29 +74,29 @@ func TestServer(t *testing.T) { pos2 := []float64{4.0, 5.0, 6.0} speed2 := []float64{100.0, 80.0, 120.0} injectGantry2.PositionFunc = func(ctx context.Context, extra map[string]interface{}) ([]float64, error) { - return nil, errors.New("can't get position") + return nil, errPositionFailed } injectGantry2.HomeFunc = func(ctx context.Context, extra map[string]interface{}) (bool, error) { extra1 = extra - return false, errors.New("homing unsuccessful") + return false, errHomingFailed } injectGantry2.MoveToPositionFunc = func(ctx context.Context, pos, speed []float64, extra map[string]interface{}) error { gantryPos = pos gantrySpeed = speed - return errors.New("can't move to position") + return errMoveToPositionFailed } injectGantry2.LengthsFunc = func(ctx context.Context, extra map[string]interface{}) ([]float64, error) { - return nil, errors.New("can't get lengths") + return nil, errLengthsFailed } injectGantry2.StopFunc = func(ctx context.Context, extra map[string]interface{}) error { - return errors.New("no stop") + return errStopFailed } //nolint:dupl t.Run("gantry position", func(t *testing.T) { _, err := gantryServer.GetPosition(context.Background(), &pb.GetPositionRequest{Name: missingGantryName}) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "not found") + test.That(t, err.Error(), test.ShouldContainSubstring, errGantryNotFound.Error()) ext, err := protoutils.StructToStructPb(map[string]interface{}{"foo": "123", "bar": 234}) test.That(t, err, test.ShouldBeNil) @@ -98,7 +107,7 @@ func TestServer(t *testing.T) { _, err = gantryServer.GetPosition(context.Background(), &pb.GetPositionRequest{Name: failGantryName}) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "can't get position") + test.That(t, err.Error(), test.ShouldContainSubstring, errPositionFailed.Error()) }) t.Run("move to position", func(t *testing.T) { @@ -107,7 +116,7 @@ func TestServer(t *testing.T) { &pb.MoveToPositionRequest{Name: missingGantryName, PositionsMm: pos2, SpeedsMmPerSec: speed2}, ) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "not found") + test.That(t, err.Error(), test.ShouldContainSubstring, errGantryNotFound.Error()) ext, err := protoutils.StructToStructPb(map[string]interface{}{"foo": "234", "bar": 345}) test.That(t, err, test.ShouldBeNil) @@ -125,7 +134,7 @@ func TestServer(t *testing.T) { &pb.MoveToPositionRequest{Name: failGantryName, PositionsMm: pos1, SpeedsMmPerSec: speed1}, ) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "can't move to position") + test.That(t, err.Error(), test.ShouldContainSubstring, errMoveToPositionFailed.Error()) test.That(t, gantryPos, test.ShouldResemble, pos1) test.That(t, gantrySpeed, test.ShouldResemble, speed1) }) @@ -134,7 +143,7 @@ func TestServer(t *testing.T) { t.Run("lengths", func(t *testing.T) { _, err := gantryServer.GetLengths(context.Background(), &pb.GetLengthsRequest{Name: missingGantryName}) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "not found") + test.That(t, err.Error(), test.ShouldContainSubstring, errGantryNotFound.Error()) ext, err := protoutils.StructToStructPb(map[string]interface{}{"foo": 123, "bar": "234"}) test.That(t, err, test.ShouldBeNil) @@ -145,13 +154,13 @@ func TestServer(t *testing.T) { _, err = gantryServer.GetLengths(context.Background(), &pb.GetLengthsRequest{Name: failGantryName}) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "can't get lengths") + test.That(t, err.Error(), test.ShouldContainSubstring, errLengthsFailed.Error()) }) t.Run("home", func(t *testing.T) { _, err := gantryServer.Home(context.Background(), &pb.HomeRequest{Name: missingGantryName}) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "not found") + test.That(t, err.Error(), test.ShouldContainSubstring, errGantryNotFound.Error()) ext, err := protoutils.StructToStructPb(map[string]interface{}{"foo": 123, "bar": "234"}) test.That(t, err, test.ShouldBeNil) @@ -163,12 +172,13 @@ func TestServer(t *testing.T) { resp, err = gantryServer.Home(context.Background(), &pb.HomeRequest{Name: failGantryName}) test.That(t, err, test.ShouldNotBeNil) test.That(t, resp.Homed, test.ShouldBeFalse) + test.That(t, err.Error(), test.ShouldContainSubstring, errHomingFailed.Error()) }) t.Run("stop", func(t *testing.T) { _, err = gantryServer.Stop(context.Background(), &pb.StopRequest{Name: missingGantryName}) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "not found") + test.That(t, err.Error(), test.ShouldContainSubstring, errGantryNotFound.Error()) ext, err := protoutils.StructToStructPb(map[string]interface{}{"foo": 234, "bar": "123"}) test.That(t, err, test.ShouldBeNil) @@ -178,7 +188,6 @@ func TestServer(t *testing.T) { _, err = gantryServer.Stop(context.Background(), &pb.StopRequest{Name: failGantryName}) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "no stop") + test.That(t, err.Error(), test.ShouldContainSubstring, errStopFailed.Error()) }) } diff --git a/components/gantry/singleaxis/singleaxis.go b/components/gantry/singleaxis/singleaxis.go index db7cf74996d..6d91ffe06ea 100644 --- a/components/gantry/singleaxis/singleaxis.go +++ b/components/gantry/singleaxis/singleaxis.go @@ -104,7 +104,7 @@ type singleAxis struct { cancelFunc func() logger golog.Logger - opMgr operation.SingleOperationManager + opMgr *operation.SingleOperationManager activeBackgroundWorkers sync.WaitGroup } @@ -113,6 +113,7 @@ func newSingleAxis(ctx context.Context, deps resource.Dependencies, conf resourc sAx := &singleAxis{ Named: conf.ResourceName().AsNamed(), logger: logger, + opMgr: operation.NewSingleOperationManager(), } if err := sAx.Reconfigure(ctx, deps, conf); err != nil { @@ -181,13 +182,13 @@ func (g *singleAxis) Reconfigure(ctx context.Context, deps resource.Dependencies if err != nil { return err } - features, err := motorDep.Properties(ctx, nil) + properties, err := motorDep.Properties(ctx, nil) if err != nil { return err } - ok := features[motor.PositionReporting] + ok := properties.PositionReporting if !ok { - return motor.NewFeatureUnsupportedError(motor.PositionReporting, newConf.Motor) + return motor.NewPropertyUnsupportedError(properties, newConf.Motor) } g.motor = motorDep } @@ -294,7 +295,13 @@ func (g *singleAxis) moveAway(ctx context.Context, pin int) error { if err := g.motor.GoFor(ctx, dir*g.rpm, 0, nil); err != nil { return err } + defer utils.UncheckedErrorFunc(func() error { + return g.motor.Stop(ctx, nil) + }) for { + if ctx.Err() != nil { + return ctx.Err() + } hit, err := g.limitHit(ctx, pin) if err != nil { return err @@ -336,7 +343,7 @@ func (g *singleAxis) doHome(ctx context.Context) (bool, error) { } func (g *singleAxis) homeLimSwitch(ctx context.Context) error { - var positionA, positionB, start float64 + var positionA, positionB float64 positionA, err := g.testLimit(ctx, 0) if err != nil { return err @@ -348,12 +355,10 @@ func (g *singleAxis) homeLimSwitch(ctx context.Context) error { if err != nil { return err } - start = 0.8 } else { // Only one limit switch, calculate positionB revPerLength := g.lengthMm / g.mmPerRevolution positionB = positionA + revPerLength - start = 0.2 } g.positionLimits = []float64{positionA, positionB} @@ -364,8 +369,9 @@ func (g *singleAxis) homeLimSwitch(ctx context.Context) error { g.logger.Debugf("positionA: %0.2f positionB: %0.2f range: %0.2f", positionA, positionB, g.positionRange) } - // Go to start position so limit stops are not hit. - if err = g.goToStart(ctx, start); err != nil { + // Go to start position at the middle of the axis. + x := g.gantryToMotorPosition(0.5 * g.lengthMm) + if err := g.motor.GoTo(ctx, g.rpm, x, nil); err != nil { return err } @@ -389,14 +395,6 @@ func (g *singleAxis) homeEncoder(ctx context.Context) error { return nil } -func (g *singleAxis) goToStart(ctx context.Context, percent float64) error { - x := g.gantryToMotorPosition(percent * g.lengthMm) - if err := g.motor.GoTo(ctx, g.rpm, x, nil); err != nil { - return err - } - return nil -} - func (g *singleAxis) gantryToMotorPosition(positions float64) float64 { x := positions / g.lengthMm x = g.positionLimits[0] + (x * g.positionRange) @@ -412,10 +410,11 @@ func (g *singleAxis) testLimit(ctx context.Context, pin int) (float64, error) { defer utils.UncheckedErrorFunc(func() error { return g.motor.Stop(ctx, nil) }) - + wrongPin := 1 d := -1.0 if pin != 0 { d = 1 + wrongPin = 0 } err := g.motor.GoFor(ctx, d*g.rpm, 0, nil) @@ -437,6 +436,22 @@ func (g *singleAxis) testLimit(ctx context.Context, pin int) (float64, error) { break } + // check if the wrong limit switch was hit + wrongHit, err := g.limitHit(ctx, wrongPin) + if err != nil { + return 0, err + } + if wrongHit { + err = g.motor.Stop(ctx, nil) + if err != nil { + return 0, err + } + return 0, errors.Errorf( + "expected limit switch %v but hit limit switch %v, try switching the order in the config", + pin, + wrongPin) + } + elapsed := start.Sub(start) if elapsed > (time.Second * 15) { return 0, errors.New("gantry timed out testing limit") @@ -520,16 +535,12 @@ func (g *singleAxis) MoveToPosition(ctx context.Context, positions, speeds []flo if len(g.limitSwitchPins) > 0 { // Stops if position x is past the 0 limit switch if x <= (g.positionLimits[0] + limitErrorMargin) { - g.logger.Debugf("limit: %.2f", g.positionLimits[0]+limitErrorMargin) - g.logger.Debugf("position x: %.2f", x) g.logger.Error("Cannot move past limit switch!") return g.motor.Stop(ctx, extra) } // Stops if position x is past the at-length limit switch if x >= (g.positionLimits[1] - limitErrorMargin) { - g.logger.Debugf("limit: %.2f", g.positionLimits[1]-limitErrorMargin) - g.logger.Debugf("position x: %.2f", x) g.logger.Error("Cannot move past limit switch!") return g.motor.Stop(ctx, extra) } diff --git a/components/gantry/singleaxis/singleaxis_test.go b/components/gantry/singleaxis/singleaxis_test.go index 955909e0735..098a04aa8f7 100644 --- a/components/gantry/singleaxis/singleaxis_test.go +++ b/components/gantry/singleaxis/singleaxis_test.go @@ -12,6 +12,7 @@ import ( "go.viam.com/rdk/components/board" "go.viam.com/rdk/components/motor" + "go.viam.com/rdk/operation" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" "go.viam.com/rdk/testutils/inject" @@ -29,12 +30,15 @@ var fakeFrame = &referenceframe.LinkConfig{ var badFrame = &referenceframe.LinkConfig{} -var count = 0 +var ( + count = 0 + pinValues = []int{1, 1, 0} +) func createFakeMotor() motor.Motor { return &inject.Motor{ - PropertiesFunc: func(ctx context.Context, extra map[string]interface{}) (map[motor.Feature]bool, error) { - return map[motor.Feature]bool{motor.PositionReporting: true}, nil + PropertiesFunc: func(ctx context.Context, extra map[string]interface{}) (motor.Properties, error) { + return motor.Properties{PositionReporting: true}, nil }, PositionFunc: func(ctx context.Context, extra map[string]interface{}) (float64, error) { return float64(count + 1), nil @@ -59,11 +63,14 @@ func createFakeBoard() board.Board { pinCount := 0 injectGPIOPin := &inject.GPIOPin{ GetFunc: func(ctx context.Context, extra map[string]interface{}) (bool, error) { + if pinValues[pinCount] == 1 { + return true, nil + } pinCount++ - if pinCount%2 == 0 { - return false, nil + if pinCount == len(pinValues) { + pinCount = 0 } - return true, nil + return false, nil }, SetFunc: func(ctx context.Context, high bool, extra map[string]interface{}) error { return nil }, } @@ -191,9 +198,9 @@ func TestNewSingleAxis(t *testing.T) { test.That(t, err.Error(), test.ShouldContainSubstring, "missing from dependencies") injectMotor := &inject.Motor{ - PropertiesFunc: func(ctx context.Context, extra map[string]interface{}) (map[motor.Feature]bool, error) { - return map[motor.Feature]bool{ - motor.PositionReporting: true, + PropertiesFunc: func(ctx context.Context, extra map[string]interface{}) (motor.Properties, error) { + return motor.Properties{ + PositionReporting: true, }, nil }, } @@ -205,9 +212,9 @@ func TestNewSingleAxis(t *testing.T) { test.That(t, err.Error(), test.ShouldContainSubstring, "invalid gantry type") injectMotor = &inject.Motor{ - PropertiesFunc: func(ctx context.Context, extra map[string]interface{}) (map[motor.Feature]bool, error) { - return map[motor.Feature]bool{ - motor.PositionReporting: false, + PropertiesFunc: func(ctx context.Context, extra map[string]interface{}) (motor.Properties, error) { + return motor.Properties{ + PositionReporting: false, }, nil }, } @@ -215,8 +222,9 @@ func TestNewSingleAxis(t *testing.T) { deps = make(resource.Dependencies) deps[motor.Named(motorName)] = injectMotor deps[board.Named(boardName)] = createFakeBoard() + properties, _ := injectMotor.Properties(ctx, nil) _, err = newSingleAxis(ctx, deps, fakecfg, logger) - expectedErr := motor.NewFeatureUnsupportedError(motor.PositionReporting, motorName) + expectedErr := motor.NewPropertyUnsupportedError(properties, motorName) test.That(t, err, test.ShouldBeError, expectedErr) } @@ -273,6 +281,7 @@ func TestHome(t *testing.T) { logger: logger, rpm: float64(300), limitSwitchPins: []string{"1"}, + opMgr: operation.NewSingleOperationManager(), } homed, err := fakegantry.Home(ctx, nil) test.That(t, err, test.ShouldBeNil) @@ -281,9 +290,9 @@ func TestHome(t *testing.T) { goForErr := errors.New("GoFor failed") posErr := errors.New("Position fail") fakeMotor := &inject.Motor{ - PropertiesFunc: func(ctx context.Context, extra map[string]interface{}) (map[motor.Feature]bool, error) { - return map[motor.Feature]bool{ - motor.PositionReporting: false, + PropertiesFunc: func(ctx context.Context, extra map[string]interface{}) (motor.Properties, error) { + return motor.Properties{ + PositionReporting: false, }, nil }, GoForFunc: func(ctx context.Context, rpm, rotations float64, extra map[string]interface{}) error { @@ -299,6 +308,7 @@ func TestHome(t *testing.T) { fakegantry = &singleAxis{ motor: fakeMotor, logger: logger, + opMgr: operation.NewSingleOperationManager(), } homed, err = fakegantry.Home(ctx, nil) test.That(t, err, test.ShouldBeError, posErr) @@ -311,6 +321,7 @@ func TestHome(t *testing.T) { logger: logger, rpm: float64(300), limitSwitchPins: []string{"1", "2"}, + opMgr: operation.NewSingleOperationManager(), } homed, err = fakegantry.Home(ctx, nil) test.That(t, err, test.ShouldBeNil) @@ -318,6 +329,7 @@ func TestHome(t *testing.T) { fakegantry = &singleAxis{ motor: fakeMotor, + opMgr: operation.NewSingleOperationManager(), } homed, err = fakegantry.Home(ctx, nil) test.That(t, err, test.ShouldBeError, posErr) @@ -330,6 +342,7 @@ func TestHome(t *testing.T) { logger: logger, rpm: float64(300), limitSwitchPins: []string{"1", "2"}, + opMgr: operation.NewSingleOperationManager(), } homed, err = fakegantry.Home(ctx, nil) test.That(t, err, test.ShouldBeNil) @@ -346,6 +359,7 @@ func TestHomeLimitSwitch(t *testing.T) { logger: logger, rpm: float64(300), limitSwitchPins: []string{"1", "2"}, + opMgr: operation.NewSingleOperationManager(), } err := fakegantry.homeLimSwitch(ctx) @@ -362,9 +376,9 @@ func TestHomeLimitSwitch(t *testing.T) { test.That(t, err, test.ShouldBeError, getPosErr) fakegantry.motor = &inject.Motor{ - PropertiesFunc: func(ctx context.Context, extra map[string]interface{}) (map[motor.Feature]bool, error) { - return map[motor.Feature]bool{ - motor.PositionReporting: true, + PropertiesFunc: func(ctx context.Context, extra map[string]interface{}) (motor.Properties, error) { + return motor.Properties{ + PositionReporting: true, }, nil }, GoForFunc: func(ctx context.Context, rpm, rotations float64, extra map[string]interface{}) error { @@ -376,9 +390,9 @@ func TestHomeLimitSwitch(t *testing.T) { test.That(t, err, test.ShouldNotBeNil) fakegantry.motor = &inject.Motor{ - PropertiesFunc: func(ctx context.Context, extra map[string]interface{}) (map[motor.Feature]bool, error) { - return map[motor.Feature]bool{ - motor.PositionReporting: true, + PropertiesFunc: func(ctx context.Context, extra map[string]interface{}) (motor.Properties, error) { + return motor.Properties{ + PositionReporting: true, }, nil }, GoForFunc: func(ctx context.Context, rpm, rotations float64, extra map[string]interface{}) error { @@ -390,9 +404,9 @@ func TestHomeLimitSwitch(t *testing.T) { test.That(t, err, test.ShouldNotBeNil) fakegantry.motor = &inject.Motor{ - PropertiesFunc: func(ctx context.Context, extra map[string]interface{}) (map[motor.Feature]bool, error) { - return map[motor.Feature]bool{ - motor.PositionReporting: true, + PropertiesFunc: func(ctx context.Context, extra map[string]interface{}) (motor.Properties, error) { + return motor.Properties{ + PositionReporting: true, }, nil }, GoForFunc: func(ctx context.Context, rpm, rotations float64, extra map[string]interface{}) error { @@ -444,6 +458,7 @@ func TestHomeLimitSwitch2(t *testing.T) { limitSwitchPins: []string{"1"}, lengthMm: float64(1), mmPerRevolution: float64(.1), + opMgr: operation.NewSingleOperationManager(), } err := fakegantry.homeLimSwitch(ctx) @@ -466,9 +481,9 @@ func TestHomeLimitSwitch2(t *testing.T) { test.That(t, err, test.ShouldBeError, getPosErr) fakegantry.motor = &inject.Motor{ - PropertiesFunc: func(ctx context.Context, extra map[string]interface{}) (map[motor.Feature]bool, error) { - return map[motor.Feature]bool{ - motor.PositionReporting: true, + PropertiesFunc: func(ctx context.Context, extra map[string]interface{}) (motor.Properties, error) { + return motor.Properties{ + PositionReporting: true, }, nil }, GoForFunc: func(ctx context.Context, rpm, rotations float64, extra map[string]interface{}) error { @@ -494,7 +509,7 @@ func TestHomeLimitSwitch2(t *testing.T) { } func TestHomeEncoder(t *testing.T) { - fakegantry := &singleAxis{} + fakegantry := &singleAxis{opMgr: operation.NewSingleOperationManager()} resetZeroErr := errors.New("failed to set zero") injMotor := &inject.Motor{ @@ -524,6 +539,7 @@ func TestTestLimit(t *testing.T) { board: createLimitBoard(), rpm: float64(300), limitHigh: true, + opMgr: operation.NewSingleOperationManager(), } pos, err := fakegantry.testLimit(ctx, 0) test.That(t, err, test.ShouldBeNil) @@ -536,6 +552,7 @@ func TestLimitHit(t *testing.T) { limitSwitchPins: []string{"1", "2", "3"}, board: createLimitBoard(), limitHigh: true, + opMgr: operation.NewSingleOperationManager(), } hit, err := fakegantry.limitHit(ctx, 0) @@ -548,9 +565,9 @@ func TestPosition(t *testing.T) { ctx := context.Background() fakegantry := &singleAxis{ motor: &inject.Motor{ - PropertiesFunc: func(ctx context.Context, extra map[string]interface{}) (map[motor.Feature]bool, error) { - return map[motor.Feature]bool{ - motor.PositionReporting: false, + PropertiesFunc: func(ctx context.Context, extra map[string]interface{}) (motor.Properties, error) { + return motor.Properties{ + PositionReporting: false, }, nil }, PositionFunc: func(ctx context.Context, extra map[string]interface{}) (float64, error) { return 1, nil }, @@ -561,6 +578,7 @@ func TestPosition(t *testing.T) { limitHigh: true, limitSwitchPins: []string{"1", "2"}, logger: logger, + opMgr: operation.NewSingleOperationManager(), } positions, err := fakegantry.Position(ctx, nil) test.That(t, err, test.ShouldBeNil) @@ -568,8 +586,8 @@ func TestPosition(t *testing.T) { fakegantry = &singleAxis{ motor: &inject.Motor{ - PropertiesFunc: func(ctx context.Context, extra map[string]interface{}) (map[motor.Feature]bool, error) { - return nil, errors.New("not supported") + PropertiesFunc: func(ctx context.Context, extra map[string]interface{}) (motor.Properties, error) { + return motor.Properties{}, errors.New("not supported") }, PositionFunc: func(ctx context.Context, extra map[string]interface{}) (float64, error) { return 1, errors.New("not supported") @@ -580,6 +598,7 @@ func TestPosition(t *testing.T) { limitSwitchPins: []string{"1", "2"}, positionLimits: []float64{0, 1}, logger: logger, + opMgr: operation.NewSingleOperationManager(), } positions, err = fakegantry.Position(ctx, nil) test.That(t, positions, test.ShouldResemble, []float64{}) @@ -589,6 +608,7 @@ func TestPosition(t *testing.T) { func TestLengths(t *testing.T) { fakegantry := &singleAxis{ lengthMm: float64(1.0), + opMgr: operation.NewSingleOperationManager(), } ctx := context.Background() fakelengths, err := fakegantry.Lengths(ctx, nil) @@ -605,6 +625,7 @@ func TestMoveToPosition(t *testing.T) { motor: createFakeMotor(), limitHigh: true, positionRange: 10, + opMgr: operation.NewSingleOperationManager(), } pos := []float64{1, 2} speed := []float64{100, 200} @@ -700,6 +721,7 @@ func TestStop(t *testing.T) { limitSwitchPins: []string{"1", "2"}, lengthMm: float64(200), positionLimits: []float64{0, 2}, + opMgr: operation.NewSingleOperationManager(), } test.That(t, fakegantry.Stop(ctx, nil), test.ShouldBeNil) @@ -719,6 +741,7 @@ func TestCurrentInputs(t *testing.T) { lengthMm: float64(200), positionLimits: []float64{0, 2}, positionRange: 2.0, + opMgr: operation.NewSingleOperationManager(), } input, err := fakegantry.CurrentInputs(ctx) @@ -735,6 +758,7 @@ func TestCurrentInputs(t *testing.T) { lengthMm: float64(200), positionLimits: []float64{0, 2}, positionRange: 2.0, + opMgr: operation.NewSingleOperationManager(), } input, err = fakegantry.CurrentInputs(ctx) @@ -754,6 +778,7 @@ func TestCurrentInputs(t *testing.T) { rpm: float64(300), lengthMm: float64(200), positionLimits: []float64{0, 0.5}, + opMgr: operation.NewSingleOperationManager(), } input, err = fakegantry.CurrentInputs(ctx) @@ -793,6 +818,7 @@ func TestGoToInputs(t *testing.T) { positionLimits: []float64{1, 2}, model: nil, logger: logger, + opMgr: operation.NewSingleOperationManager(), } fakegantry.positionRange = 10 diff --git a/components/generic/client_test.go b/components/generic/client_test.go index 44009ef9a85..384f74a7194 100644 --- a/components/generic/client_test.go +++ b/components/generic/client_test.go @@ -2,7 +2,6 @@ package generic_test import ( "context" - "errors" "net" "testing" @@ -40,7 +39,7 @@ func TestClient(t *testing.T) { map[string]interface{}, error, ) { - return nil, errors.New("do failed") + return nil, errDoFailed } resourceMap := map[resource.Name]resource.Resource{ @@ -62,7 +61,7 @@ func TestClient(t *testing.T) { cancel() _, err = viamgrpc.Dial(cancelCtx, listener1.Addr().String(), logger) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "canceled") + test.That(t, err, test.ShouldBeError, context.Canceled) }) t.Run("client tests for working generic", func(t *testing.T) { @@ -88,6 +87,7 @@ func TestClient(t *testing.T) { _, err = failingGenericClient.DoCommand(context.Background(), testutils.TestCommand) test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, errDoFailed.Error()) test.That(t, failingGenericClient.Close(context.Background()), test.ShouldBeNil) test.That(t, conn.Close(), test.ShouldBeNil) diff --git a/components/generic/fake/generic.go b/components/generic/fake/generic.go index d01f75e4613..1270fb57e4b 100644 --- a/components/generic/fake/generic.go +++ b/components/generic/fake/generic.go @@ -20,12 +20,12 @@ func init() { conf resource.Config, logger golog.Logger, ) (resource.Resource, error) { - return newGeneric(conf.ResourceName()), nil + return newGeneric(conf.ResourceName(), logger), nil }}) } -func newGeneric(name resource.Name) resource.Resource { - return &Generic{Named: name.AsNamed()} +func newGeneric(name resource.Name, logger golog.Logger) resource.Resource { + return &Generic{Named: name.AsNamed(), logger: logger} } // Generic is a fake Generic device that always echos inputs back to the caller. @@ -33,4 +33,5 @@ type Generic struct { resource.Named resource.TriviallyReconfigurable resource.TriviallyCloseable + logger golog.Logger } diff --git a/components/generic/server_test.go b/components/generic/server_test.go index 95513b8cec3..6c05f1ba06a 100644 --- a/components/generic/server_test.go +++ b/components/generic/server_test.go @@ -16,6 +16,8 @@ import ( "go.viam.com/rdk/testutils/inject" ) +var errDoFailed = errors.New("do failed") + func newServer() (genericpb.GenericServiceServer, *inject.Generic, *inject.Generic, error) { injectGeneric := &inject.Generic{} injectGeneric2 := &inject.Generic{} @@ -50,7 +52,7 @@ func TestGenericDo(t *testing.T) { map[string]interface{}, error, ) { - return nil, errors.New("do failed") + return nil, errDoFailed } commandStruct, err := protoutils.StructToStructPb(testutils.TestCommand) @@ -66,5 +68,6 @@ func TestGenericDo(t *testing.T) { req = commonpb.DoCommandRequest{Name: failGenericName, Command: commandStruct} resp, err = genericServer.DoCommand(context.Background(), &req) test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, errDoFailed.Error()) test.That(t, resp, test.ShouldBeNil) } diff --git a/components/gripper/client.go b/components/gripper/client.go index f6d237819c7..efd8fcb5b2c 100644 --- a/components/gripper/client.go +++ b/components/gripper/client.go @@ -5,6 +5,7 @@ import ( "context" "github.com/edaniels/golog" + commonpb "go.viam.com/api/common/v1" pb "go.viam.com/api/component/gripper/v1" "go.viam.com/utils/protoutils" "go.viam.com/utils/rpc" @@ -12,6 +13,7 @@ import ( rprotoutils "go.viam.com/rdk/protoutils" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" + "go.viam.com/rdk/spatialmath" ) // client implements GripperServiceClient. @@ -96,3 +98,18 @@ func (c *client) IsMoving(ctx context.Context) (bool, error) { } return resp.IsMoving, nil } + +func (c *client) Geometries(ctx context.Context, extra map[string]interface{}) ([]spatialmath.Geometry, error) { + ext, err := protoutils.StructToStructPb(extra) + if err != nil { + return nil, err + } + resp, err := c.client.GetGeometries(ctx, &commonpb.GetGeometriesRequest{ + Name: c.name, + Extra: ext, + }) + if err != nil { + return nil, err + } + return spatialmath.NewGeometriesFromProto(resp.GetGeometries()) +} diff --git a/components/gripper/client_test.go b/components/gripper/client_test.go index 9ea98801c23..0c09653ae84 100644 --- a/components/gripper/client_test.go +++ b/components/gripper/client_test.go @@ -6,7 +6,6 @@ import ( "testing" "github.com/edaniels/golog" - "github.com/pkg/errors" "go.viam.com/test" "go.viam.com/utils/rpc" @@ -46,13 +45,13 @@ func TestClient(t *testing.T) { injectGripper2 := &inject.Gripper{} injectGripper2.OpenFunc = func(ctx context.Context, extra map[string]interface{}) error { gripperOpen = failGripperName - return errors.New("can't open") + return errCantOpen } injectGripper2.GrabFunc = func(ctx context.Context, extra map[string]interface{}) (bool, error) { - return false, errors.New("can't grab") + return false, errCantGrab } injectGripper2.StopFunc = func(ctx context.Context, extra map[string]interface{}) error { - return gripper.ErrStopUnimplemented + return errStopUnimplemented } gripperSvc, err := resource.NewAPIResourceCollection( @@ -75,7 +74,7 @@ func TestClient(t *testing.T) { cancel() _, err := viamgrpc.Dial(cancelCtx, listener1.Addr().String(), logger) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "canceled") + test.That(t, err, test.ShouldBeError, context.Canceled) }) // working @@ -121,17 +120,17 @@ func TestClient(t *testing.T) { extra := map[string]interface{}{} err = client2.Open(context.Background(), extra) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "can't open") + test.That(t, err.Error(), test.ShouldContainSubstring, errCantOpen.Error()) test.That(t, gripperOpen, test.ShouldEqual, failGripperName) grabbed, err := client2.Grab(context.Background(), extra) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "can't grab") + test.That(t, err.Error(), test.ShouldContainSubstring, errCantGrab.Error()) test.That(t, grabbed, test.ShouldEqual, false) err = client2.Stop(context.Background(), extra) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, gripper.ErrStopUnimplemented.Error()) + test.That(t, err.Error(), test.ShouldContainSubstring, errStopUnimplemented.Error()) test.That(t, client2.Close(context.Background()), test.ShouldBeNil) test.That(t, conn.Close(), test.ShouldBeNil) diff --git a/components/gripper/fake/gripper.go b/components/gripper/fake/gripper.go index 7400f8985ac..12c4af9bd9a 100644 --- a/components/gripper/fake/gripper.go +++ b/components/gripper/fake/gripper.go @@ -3,12 +3,14 @@ package fake import ( "context" + "sync" "github.com/edaniels/golog" "go.viam.com/rdk/components/gripper" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" + "go.viam.com/rdk/spatialmath" ) var model = resource.DefaultModelFamily.WithModel("fake") @@ -19,20 +21,44 @@ type Config struct { } func init() { - resource.RegisterComponent(gripper.API, model, resource.Registration[gripper.Gripper, *Config]{ - Constructor: func(ctx context.Context, _ resource.Dependencies, conf resource.Config, logger golog.Logger) (gripper.Gripper, error) { - return &Gripper{ - Named: conf.ResourceName().AsNamed(), - }, nil - }, - }) + resource.RegisterComponent(gripper.API, model, resource.Registration[gripper.Gripper, *Config]{Constructor: NewGripper}) } // Gripper is a fake gripper that can simply read and set properties. type Gripper struct { resource.Named - resource.TriviallyReconfigurable resource.TriviallyCloseable + geometries []spatialmath.Geometry + mu sync.Mutex + logger golog.Logger +} + +// NewGripper instantiates a new gripper of the fake model type. +func NewGripper(ctx context.Context, deps resource.Dependencies, conf resource.Config, logger golog.Logger) (gripper.Gripper, error) { + g := &Gripper{ + Named: conf.ResourceName().AsNamed(), + geometries: []spatialmath.Geometry{}, + logger: logger, + } + if err := g.Reconfigure(ctx, deps, conf); err != nil { + return nil, err + } + return g, nil +} + +// Reconfigure reconfigures the gripper atomically and in place. +func (g *Gripper) Reconfigure(_ context.Context, _ resource.Dependencies, conf resource.Config) error { + g.mu.Lock() + defer g.mu.Unlock() + + if conf.Frame != nil && conf.Frame.Geometry != nil { + geometry, err := conf.Frame.Geometry.ParseConfig() + if err != nil { + return err + } + g.geometries = []spatialmath.Geometry{geometry} + } + return nil } // ModelFrame returns the dynamic frame of the model. @@ -59,3 +85,10 @@ func (g *Gripper) Stop(ctx context.Context, extra map[string]interface{}) error func (g *Gripper) IsMoving(ctx context.Context) (bool, error) { return false, nil } + +// Geometries returns the geometries associated with the fake base. +func (g *Gripper) Geometries(ctx context.Context, extra map[string]interface{}) ([]spatialmath.Geometry, error) { + g.mu.Lock() + defer g.mu.Unlock() + return g.geometries, nil +} diff --git a/components/gripper/gripper.go b/components/gripper/gripper.go index 36ab68ee83f..448dceb53bb 100644 --- a/components/gripper/gripper.go +++ b/components/gripper/gripper.go @@ -4,7 +4,6 @@ package gripper import ( "context" - "github.com/pkg/errors" commonpb "go.viam.com/api/common/v1" pb "go.viam.com/api/component/gripper/v1" @@ -37,6 +36,7 @@ func Named(name string) resource.Name { // A Gripper represents a physical robotic gripper. type Gripper interface { resource.Resource + resource.Shaped resource.Actuator referenceframe.ModelFramer @@ -50,9 +50,6 @@ type Gripper interface { Grab(ctx context.Context, extra map[string]interface{}) (bool, error) } -// ErrStopUnimplemented is used for when Stop is unimplemented. -var ErrStopUnimplemented = errors.New("Stop unimplemented") - // FromRobot is a helper for getting the named Gripper from the given Robot. func FromRobot(r robot.Robot, name string) (Gripper, error) { return robot.ResourceFromRobot[Gripper](r, Named(name)) diff --git a/components/gripper/gripper_test.go b/components/gripper/gripper_test.go index 520ae18d788..b65e69073ba 100644 --- a/components/gripper/gripper_test.go +++ b/components/gripper/gripper_test.go @@ -4,11 +4,15 @@ import ( "context" "testing" + "github.com/golang/geo/r3" commonpb "go.viam.com/api/common/v1" "go.viam.com/test" "go.viam.com/rdk/components/gripper" + "go.viam.com/rdk/components/gripper/fake" + "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" + "go.viam.com/rdk/spatialmath" "go.viam.com/rdk/testutils/inject" ) @@ -55,3 +59,18 @@ func TestCreateStatus(t *testing.T) { test.That(t, status1, test.ShouldResemble, status) }) } + +func TestGetGeometries(t *testing.T) { + cfg := resource.Config{ + Name: "fakeGripper", + API: gripper.API, + Frame: &referenceframe.LinkConfig{Geometry: &spatialmath.GeometryConfig{X: 10, Y: 5, Z: 10}}, + } + gripper, err := fake.NewGripper(context.Background(), nil, cfg, nil) + test.That(t, err, test.ShouldBeNil) + + geometries, err := gripper.Geometries(context.Background(), nil) + expected, _ := spatialmath.NewBox(spatialmath.NewZeroPose(), r3.Vector{X: 10, Y: 5, Z: 10}, "") + test.That(t, err, test.ShouldBeNil) + test.That(t, geometries, test.ShouldResemble, []spatialmath.Geometry{expected}) +} diff --git a/components/gripper/robotiq/gripper.go b/components/gripper/robotiq/gripper.go index effabadf10a..90e4b6c813d 100644 --- a/components/gripper/robotiq/gripper.go +++ b/components/gripper/robotiq/gripper.go @@ -18,6 +18,7 @@ import ( "go.viam.com/rdk/operation" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" + "go.viam.com/rdk/spatialmath" ) var model = resource.DefaultModelFamily.WithModel("robotiq") @@ -42,7 +43,7 @@ func init() { if err != nil { return nil, err } - return newGripper(ctx, conf.ResourceName(), newConf.Host, logger) + return newGripper(ctx, conf, newConf.Host, logger) }, }) } @@ -57,23 +58,25 @@ type robotiqGripper struct { openLimit string closeLimit string logger golog.Logger - opMgr operation.SingleOperationManager + opMgr *operation.SingleOperationManager + geometries []spatialmath.Geometry } -// newGripper TODO. -func newGripper(ctx context.Context, name resource.Name, host string, logger golog.Logger) (gripper.Gripper, error) { +// newGripper instantiates a new Gripper of robotiqGripper type. +func newGripper(ctx context.Context, conf resource.Config, host string, logger golog.Logger) (gripper.Gripper, error) { conn, err := net.Dial("tcp", host+":63352") if err != nil { return nil, err } g := &robotiqGripper{ - name.AsNamed(), + conf.ResourceName().AsNamed(), resource.AlwaysRebuild{}, conn, "0", "255", logger, - operation.SingleOperationManager{}, + operation.NewSingleOperationManager(), + []spatialmath.Geometry{}, } init := [][]string{ @@ -92,6 +95,14 @@ func newGripper(ctx context.Context, name resource.Name, host string, logger gol return nil, err } + if conf.Frame != nil && conf.Frame.Geometry != nil { + geometry, err := conf.Frame.Geometry.ParseConfig() + if err != nil { + return nil, err + } + g.geometries = []spatialmath.Geometry{geometry} + } + return g, nil } @@ -287,3 +298,8 @@ func (g *robotiqGripper) IsMoving(ctx context.Context) (bool, error) { func (g *robotiqGripper) ModelFrame() referenceframe.Model { return nil } + +// Geometries returns the geometries associated with robotiqGripper. +func (g *robotiqGripper) Geometries(ctx context.Context, extra map[string]interface{}) ([]spatialmath.Geometry, error) { + return g.geometries, nil +} diff --git a/components/gripper/server.go b/components/gripper/server.go index 16b6b276288..59f42b0d2d1 100644 --- a/components/gripper/server.go +++ b/components/gripper/server.go @@ -10,6 +10,7 @@ import ( "go.viam.com/rdk/operation" "go.viam.com/rdk/protoutils" "go.viam.com/rdk/resource" + "go.viam.com/rdk/spatialmath" ) // serviceServer implements the GripperService from gripper.proto. @@ -79,3 +80,15 @@ func (s *serviceServer) DoCommand(ctx context.Context, } return protoutils.DoFromResourceServer(ctx, gripper, req) } + +func (s *serviceServer) Geometries(ctx context.Context, req *commonpb.GetGeometriesRequest) (*commonpb.GetGeometriesResponse, error) { + res, err := s.coll.Resource(req.GetName()) + if err != nil { + return nil, err + } + geometries, err := res.Geometries(ctx, req.Extra.AsMap()) + if err != nil { + return nil, err + } + return &commonpb.GetGeometriesResponse{Geometries: spatialmath.NewGeometriesToProto(geometries)}, nil +} diff --git a/components/gripper/server_test.go b/components/gripper/server_test.go index e53bca46e89..248b22ac24e 100644 --- a/components/gripper/server_test.go +++ b/components/gripper/server_test.go @@ -14,6 +14,13 @@ import ( "go.viam.com/rdk/testutils/inject" ) +var ( + errCantOpen = errors.New("can't open") + errCantGrab = errors.New("can't grab") + errStopUnimplemented = errors.New("stop unimplemented") + errGripperNotFound = errors.New("not found") +) + func newServer() (pb.GripperServiceServer, *inject.Gripper, *inject.Gripper, error) { injectGripper := &inject.Gripper{} injectGripper2 := &inject.Gripper{} @@ -52,19 +59,19 @@ func TestServer(t *testing.T) { injectGripper2.OpenFunc = func(ctx context.Context, extra map[string]interface{}) error { gripperOpen = testGripperName2 - return errors.New("can't open") + return errCantOpen } injectGripper2.GrabFunc = func(ctx context.Context, extra map[string]interface{}) (bool, error) { - return false, errors.New("can't grab") + return false, errCantGrab } injectGripper2.StopFunc = func(ctx context.Context, extra map[string]interface{}) error { - return gripper.ErrStopUnimplemented + return errStopUnimplemented } t.Run("open", func(t *testing.T) { _, err := gripperServer.Open(context.Background(), &pb.OpenRequest{Name: missingGripperName}) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "not found") + test.That(t, err.Error(), test.ShouldContainSubstring, errGripperNotFound.Error()) extra := map[string]interface{}{"foo": "Open"} ext, err := protoutils.StructToStructPb(extra) @@ -76,14 +83,14 @@ func TestServer(t *testing.T) { _, err = gripperServer.Open(context.Background(), &pb.OpenRequest{Name: testGripperName2}) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "can't open") + test.That(t, err.Error(), test.ShouldContainSubstring, errCantOpen.Error()) test.That(t, gripperOpen, test.ShouldEqual, testGripperName2) }) t.Run("grab", func(t *testing.T) { _, err := gripperServer.Grab(context.Background(), &pb.GrabRequest{Name: missingGripperName}) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "not found") + test.That(t, err.Error(), test.ShouldContainSubstring, errGripperNotFound.Error()) extra := map[string]interface{}{"foo": "Grab"} ext, err := protoutils.StructToStructPb(extra) @@ -95,14 +102,14 @@ func TestServer(t *testing.T) { resp, err = gripperServer.Grab(context.Background(), &pb.GrabRequest{Name: testGripperName2}) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "can't grab") + test.That(t, err.Error(), test.ShouldContainSubstring, errCantGrab.Error()) test.That(t, resp, test.ShouldBeNil) }) t.Run("stop", func(t *testing.T) { _, err = gripperServer.Stop(context.Background(), &pb.StopRequest{Name: missingGripperName}) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "not found") + test.That(t, err.Error(), test.ShouldContainSubstring, errGripperNotFound.Error()) extra := map[string]interface{}{"foo": "Stop"} ext, err := protoutils.StructToStructPb(extra) @@ -113,6 +120,6 @@ func TestServer(t *testing.T) { _, err = gripperServer.Stop(context.Background(), &pb.StopRequest{Name: testGripperName2}) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err, test.ShouldBeError, gripper.ErrStopUnimplemented) + test.That(t, err, test.ShouldBeError, errStopUnimplemented) }) } diff --git a/components/gripper/softrobotics/gripper.go b/components/gripper/softrobotics/gripper.go index 9d09e43707b..5c47a944eee 100644 --- a/components/gripper/softrobotics/gripper.go +++ b/components/gripper/softrobotics/gripper.go @@ -15,6 +15,7 @@ import ( "go.viam.com/rdk/operation" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" + "go.viam.com/rdk/spatialmath" ) var model = resource.DefaultModelFamily.WithModel("softrobotics") @@ -84,11 +85,12 @@ type softGripper struct { pinOpen, pinClose, pinPower board.GPIOPin - logger golog.Logger - opMgr operation.SingleOperationManager + logger golog.Logger + opMgr *operation.SingleOperationManager + geometries []spatialmath.Geometry } -// newGripper TODO. +// newGripper instantiates a new Gripper of softGripper type. func newGripper(b board.Board, conf resource.Config, logger golog.Logger) (gripper.Gripper, error) { newConf, err := resource.NativeConfig[*Config](conf) if err != nil { @@ -120,12 +122,21 @@ func newGripper(b board.Board, conf resource.Config, logger golog.Logger) (gripp pinClose: pinClose, pinPower: pinPower, logger: logger, + opMgr: operation.NewSingleOperationManager(), } if theGripper.psi == nil { return nil, errors.New("no psi analog reader") } + if conf.Frame != nil && conf.Frame.Geometry != nil { + geometry, err := conf.Frame.Geometry.ParseConfig() + if err != nil { + return nil, err + } + theGripper.geometries = []spatialmath.Geometry{geometry} + } + return theGripper, nil } @@ -219,3 +230,8 @@ func (g *softGripper) IsMoving(ctx context.Context) (bool, error) { func (g *softGripper) ModelFrame() referenceframe.Model { return nil } + +// Geometries returns the geometries associated with the softGripper. +func (g *softGripper) Geometries(ctx context.Context, extra map[string]interface{}) ([]spatialmath.Geometry, error) { + return g.geometries, nil +} diff --git a/components/gripper/yahboom/dofbot.go b/components/gripper/yahboom/dofbot.go index bd8728fd1b5..77174382915 100644 --- a/components/gripper/yahboom/dofbot.go +++ b/components/gripper/yahboom/dofbot.go @@ -18,6 +18,7 @@ import ( "go.viam.com/rdk/operation" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" + "go.viam.com/rdk/spatialmath" ) var model = resource.DefaultModelFamily.WithModel("yahboom-dofbot") @@ -45,12 +46,13 @@ func init() { conf resource.Config, logger golog.Logger, ) (gripper.Gripper, error) { - return newGripper(deps, conf) + return newGripper(deps, conf, logger) }, }) } -func newGripper(deps resource.Dependencies, conf resource.Config) (gripper.Gripper, error) { +// newGripper instantiates a new Gripper of dofGripper type. +func newGripper(deps resource.Dependencies, conf resource.Config, logger golog.Logger) (gripper.Gripper, error) { newConf, err := resource.NativeConfig[*Config](conf) if err != nil { return nil, err @@ -69,8 +71,19 @@ func newGripper(deps resource.Dependencies, conf resource.Config) (gripper.Gripp return nil, fmt.Errorf("yahboom-dofbot gripper got not a dofbot arm, got %T", myArm) } g := &dofGripper{ - Named: conf.ResourceName().AsNamed(), - dofArm: dofArm, + Named: conf.ResourceName().AsNamed(), + dofArm: dofArm, + opMgr: operation.NewSingleOperationManager(), + geometries: []spatialmath.Geometry{}, + logger: logger, + } + + if conf.Frame != nil && conf.Frame.Geometry != nil { + geometry, err := conf.Frame.Geometry.ParseConfig() + if err != nil { + return nil, err + } + g.geometries = []spatialmath.Geometry{geometry} } return g, nil @@ -80,8 +93,10 @@ type dofGripper struct { resource.Named resource.AlwaysRebuild resource.TriviallyCloseable - dofArm *yahboom.Dofbot - opMgr operation.SingleOperationManager + dofArm *yahboom.Dofbot + opMgr *operation.SingleOperationManager + geometries []spatialmath.Geometry + logger golog.Logger } func (g *dofGripper) Open(ctx context.Context, extra map[string]interface{}) error { @@ -111,3 +126,8 @@ func (g *dofGripper) IsMoving(ctx context.Context) (bool, error) { func (g *dofGripper) ModelFrame() referenceframe.Model { return g.dofArm.ModelFrame() } + +// Geometries returns the geometries associated with the dofGripper. +func (g *dofGripper) Geometries(ctx context.Context, extra map[string]interface{}) ([]spatialmath.Geometry, error) { + return g.geometries, nil +} diff --git a/components/input/client_test.go b/components/input/client_test.go index 1b6a7641ee2..61ecd1e0ec9 100644 --- a/components/input/client_test.go +++ b/components/input/client_test.go @@ -7,7 +7,6 @@ import ( "time" "github.com/edaniels/golog" - "github.com/pkg/errors" "go.viam.com/test" "go.viam.com/utils/rpc" @@ -62,10 +61,10 @@ func TestClient(t *testing.T) { injectInputController2 := &inject.InputController{} injectInputController2.ControlsFunc = func(ctx context.Context, extra map[string]interface{}) ([]input.Control, error) { - return nil, errors.New("can't get controls") + return nil, errControlsFailed } injectInputController2.EventsFunc = func(ctx context.Context, extra map[string]interface{}) (map[input.Control]input.Event, error) { - return nil, errors.New("can't get last events") + return nil, errEventsFailed } resources := map[resource.Name]input.Controller{ @@ -89,7 +88,7 @@ func TestClient(t *testing.T) { cancel() _, err := viamgrpc.Dial(cancelCtx, listener1.Addr().String(), logger) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "canceled") + test.That(t, err, test.ShouldBeError, context.Canceled) }) t.Run("input controller client 1", func(t *testing.T) { @@ -207,7 +206,7 @@ func TestClient(t *testing.T) { test.That(t, btnEv.Time.Before(time.Now()), test.ShouldBeTrue) injectInputController.TriggerEventFunc = func(ctx context.Context, event input.Event, extra map[string]interface{}) error { - return errors.New("can't inject event") + return errTriggerEvent } event1 := input.Event{ Time: time.Now().UTC(), @@ -219,7 +218,7 @@ func TestClient(t *testing.T) { test.That(t, ok, test.ShouldBeTrue) err = injectable.TriggerEvent(context.Background(), event1, map[string]interface{}{}) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "can't inject event") + test.That(t, err.Error(), test.ShouldContainSubstring, errTriggerEvent.Error()) var injectedEvent input.Event extra = map[string]interface{}{"foo": "TriggerEvent"} @@ -246,11 +245,11 @@ func TestClient(t *testing.T) { _, err = client2.Controls(context.Background(), map[string]interface{}{}) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "can't get controls") + test.That(t, err.Error(), test.ShouldContainSubstring, errControlsFailed.Error()) _, err = client2.Events(context.Background(), map[string]interface{}{}) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "can't get last events") + test.That(t, err.Error(), test.ShouldContainSubstring, errEventsFailed.Error()) event1 := input.Event{ Time: time.Now().UTC(), diff --git a/components/input/fake/input.go b/components/input/fake/input.go index 31543b55db9..b4139cdfec9 100644 --- a/components/input/fake/input.go +++ b/components/input/fake/input.go @@ -22,8 +22,8 @@ func init() { input.API, model, resource.Registration[input.Controller, *Config]{ - Constructor: func(ctx context.Context, _ resource.Dependencies, conf resource.Config, _ golog.Logger) (input.Controller, error) { - return NewInputController(ctx, conf) + Constructor: func(ctx context.Context, _ resource.Dependencies, conf resource.Config, logger golog.Logger) (input.Controller, error) { + return NewInputController(ctx, conf, logger) }, }, ) @@ -49,14 +49,15 @@ type callback struct { } // NewInputController returns a fake input.Controller. -func NewInputController(ctx context.Context, conf resource.Config) (input.Controller, error) { - closeCtx, cancelFunc := context.WithCancel(ctx) +func NewInputController(ctx context.Context, conf resource.Config, logger golog.Logger) (input.Controller, error) { + closeCtx, cancelFunc := context.WithCancel(context.Background()) c := &InputController{ Named: conf.ResourceName().AsNamed(), closeCtx: closeCtx, cancelFunc: cancelFunc, callbacks: make([]callback, 0), + logger: logger, } if err := c.Reconfigure(ctx, nil, conf); err != nil { @@ -85,6 +86,7 @@ type InputController struct { eventValue *float64 callbackDelay *time.Duration callbacks []callback + logger golog.Logger } // Reconfigure updates the config of the controller. diff --git a/components/input/fake/input_test.go b/components/input/fake/input_test.go index 3c0bc4f55b4..03578e57cbf 100644 --- a/components/input/fake/input_test.go +++ b/components/input/fake/input_test.go @@ -5,6 +5,7 @@ import ( "testing" "time" + "github.com/edaniels/golog" "github.com/pkg/errors" "go.viam.com/test" @@ -35,7 +36,8 @@ func setupDefinedInput(t *testing.T) *InputController { func setupInputWithCfg(t *testing.T, conf Config) *InputController { t.Helper() - input, err := NewInputController(context.Background(), resource.Config{ConvertedAttributes: &conf}) + logger := golog.NewTestLogger(t) + input, err := NewInputController(context.Background(), resource.Config{ConvertedAttributes: &conf}, logger) test.That(t, err, test.ShouldBeNil) return input.(*InputController) } diff --git a/components/input/mux/mux.go b/components/input/mux/mux.go index 504aa77807c..40263bb1e10 100644 --- a/components/input/mux/mux.go +++ b/components/input/mux/mux.go @@ -46,6 +46,7 @@ func NewController( cancelFunc: cancel, ctxWithCancel: ctxWithCancel, eventsChan: make(chan input.Event, 1024), + logger: logger, } for _, s := range newConf.Sources { @@ -84,6 +85,7 @@ type mux struct { cancelFunc func() callbacks map[input.Control]map[input.EventType]input.ControlFunction eventsChan chan input.Event + logger golog.Logger } func (m *mux) makeCallbacks(eventOut input.Event) { diff --git a/components/input/server_test.go b/components/input/server_test.go index 0146345cb03..f2955c7af5b 100644 --- a/components/input/server_test.go +++ b/components/input/server_test.go @@ -17,6 +17,15 @@ import ( "go.viam.com/rdk/testutils/inject" ) +var ( + errControlsFailed = errors.New("can't get controls") + errEventsFailed = errors.New("can't get last events") + errTriggerEvent = errors.New("can't inject event") + errSendFailed = errors.New("send fail") + errRegisterFailed = errors.New("can't register callbacks") + errNotFound = errors.New("not found") +) + type streamServer struct { grpc.ServerStream ctx context.Context @@ -30,7 +39,7 @@ func (x *streamServer) Context() context.Context { func (x *streamServer) Send(m *pb.StreamEventsResponse) error { if x.fail { - return errors.New("send fail") + return errSendFailed } if x.messageCh == nil { return nil @@ -83,10 +92,10 @@ func TestServer(t *testing.T) { } injectInputController2.ControlsFunc = func(ctx context.Context, extra map[string]interface{}) ([]input.Control, error) { - return nil, errors.New("can't get controls") + return nil, errControlsFailed } injectInputController2.EventsFunc = func(ctx context.Context, extra map[string]interface{}) (map[input.Control]input.Event, error) { - return nil, errors.New("can't get last events") + return nil, errEventsFailed } injectInputController2.RegisterControlCallbackFunc = func( ctx context.Context, @@ -95,7 +104,7 @@ func TestServer(t *testing.T) { ctrlFunc input.ControlFunction, extra map[string]interface{}, ) error { - return errors.New("can't register callbacks") + return errRegisterFailed } t.Run("GetControls", func(t *testing.T) { @@ -104,7 +113,7 @@ func TestServer(t *testing.T) { &pb.GetControlsRequest{Controller: missingInputControllerName}, ) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "not found") + test.That(t, err.Error(), test.ShouldContainSubstring, errNotFound.Error()) extra := map[string]interface{}{"foo": "Controls"} ext, err := protoutils.StructToStructPb(extra) @@ -122,7 +131,7 @@ func TestServer(t *testing.T) { &pb.GetControlsRequest{Controller: failInputControllerName}, ) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "can't get controls") + test.That(t, err.Error(), test.ShouldContainSubstring, errControlsFailed.Error()) }) t.Run("GetEvents", func(t *testing.T) { @@ -131,7 +140,7 @@ func TestServer(t *testing.T) { &pb.GetEventsRequest{Controller: missingInputControllerName}, ) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "not found") + test.That(t, err.Error(), test.ShouldContainSubstring, errNotFound.Error()) extra := map[string]interface{}{"foo": "Events"} ext, err := protoutils.StructToStructPb(extra) @@ -171,7 +180,7 @@ func TestServer(t *testing.T) { &pb.GetEventsRequest{Controller: failInputControllerName}, ) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "can't get last events") + test.That(t, err.Error(), test.ShouldContainSubstring, errEventsFailed.Error()) }) t.Run("StreamEvents", func(t *testing.T) { @@ -186,7 +195,7 @@ func TestServer(t *testing.T) { startTime := time.Now() err := inputControllerServer.StreamEvents(&pb.StreamEventsRequest{Controller: missingInputControllerName}, s) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "not found") + test.That(t, err.Error(), test.ShouldContainSubstring, errNotFound.Error()) extra := map[string]interface{}{"foo": "StreamEvents"} ext, err := protoutils.StructToStructPb(extra) @@ -228,7 +237,7 @@ func TestServer(t *testing.T) { err = inputControllerServer.StreamEvents(eventReqList, s) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "send fail") + test.That(t, err.Error(), test.ShouldContainSubstring, errSendFailed.Error()) var streamErr error done := make(chan struct{}) @@ -255,7 +264,7 @@ func TestServer(t *testing.T) { eventReqList.Controller = failInputControllerName err = inputControllerServer.StreamEvents(eventReqList, s) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "can't register callbacks") + test.That(t, err.Error(), test.ShouldContainSubstring, errRegisterFailed.Error()) }) t.Run("TriggerEvent", func(t *testing.T) { @@ -264,7 +273,7 @@ func TestServer(t *testing.T) { &pb.TriggerEventRequest{Controller: missingInputControllerName}, ) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "not found") + test.That(t, err.Error(), test.ShouldContainSubstring, errNotFound.Error()) injectInputController.TriggerEventFunc = func(ctx context.Context, event input.Event, extra map[string]interface{}) error { return errors.New("can't inject event") @@ -290,7 +299,7 @@ func TestServer(t *testing.T) { }, ) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "can't inject event") + test.That(t, err.Error(), test.ShouldContainSubstring, errTriggerEvent.Error()) var injectedEvent input.Event diff --git a/components/input/webgamepad/webgamepad.go b/components/input/webgamepad/webgamepad.go index 485f6cebe99..30cfa62f232 100644 --- a/components/input/webgamepad/webgamepad.go +++ b/components/input/webgamepad/webgamepad.go @@ -34,6 +34,7 @@ func NewController(ctx context.Context, _ resource.Dependencies, conf resource.C input.ButtonLT, input.ButtonRT, input.ButtonLThumb, input.ButtonRThumb, input.ButtonSelect, input.ButtonStart, input.ButtonMenu, }, + logger: logger, }, nil } @@ -46,6 +47,7 @@ type webGamepad struct { lastEvents map[input.Control]input.Event mu sync.RWMutex callbacks map[input.Control]map[input.EventType]input.ControlFunction + logger golog.Logger } func (w *webGamepad) makeCallbacks(ctx context.Context, eventOut input.Event) { diff --git a/components/motor/client.go b/components/motor/client.go index 5fc57bdb4f6..614b42bb694 100644 --- a/components/motor/client.go +++ b/components/motor/client.go @@ -111,17 +111,17 @@ func (c *client) Position(ctx context.Context, extra map[string]interface{}) (fl return resp.GetPosition(), nil } -func (c *client) Properties(ctx context.Context, extra map[string]interface{}) (map[Feature]bool, error) { +func (c *client) Properties(ctx context.Context, extra map[string]interface{}) (Properties, error) { ext, err := protoutils.StructToStructPb(extra) if err != nil { - return nil, err + return Properties{}, err } req := &pb.GetPropertiesRequest{Name: c.name, Extra: ext} resp, err := c.client.GetProperties(ctx, req) if err != nil { - return nil, err + return Properties{}, err } - return ProtoFeaturesToMap(resp), nil + return ProtoFeaturesToProperties(resp), nil } func (c *client) Stop(ctx context.Context, extra map[string]interface{}) error { diff --git a/components/motor/client_test.go b/components/motor/client_test.go index 03dca71cab6..beb0a74d941 100644 --- a/components/motor/client_test.go +++ b/components/motor/client_test.go @@ -2,7 +2,6 @@ package motor_test import ( "context" - "errors" "net" "testing" @@ -51,10 +50,10 @@ func TestClient(t *testing.T) { actualExtra = extra return 42.0, nil } - workingMotor.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (map[motor.Feature]bool, error) { + workingMotor.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (motor.Properties, error) { actualExtra = extra - return map[motor.Feature]bool{ - motor.PositionReporting: true, + return motor.Properties{ + PositionReporting: true, }, nil } workingMotor.StopFunc = func(ctx context.Context, extra map[string]interface{}) error { @@ -67,28 +66,28 @@ func TestClient(t *testing.T) { } failingMotor.SetPowerFunc = func(ctx context.Context, powerPct float64, extra map[string]interface{}) error { - return errors.New("set power failed") + return errSetPowerFailed } failingMotor.GoForFunc = func(ctx context.Context, rpm, rotations float64, extra map[string]interface{}) error { - return errors.New("go for failed") + return errGoForFailed } failingMotor.GoToFunc = func(ctx context.Context, rpm, position float64, extra map[string]interface{}) error { - return errors.New("go to failed") + return errGoToFailed } failingMotor.ResetZeroPositionFunc = func(ctx context.Context, offset float64, extra map[string]interface{}) error { - return errors.New("set to zero failed") + return errResetZeroFailed } failingMotor.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (float64, error) { - return 0, errors.New("position unavailable") + return 0, errPositionUnavailable } - failingMotor.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (map[motor.Feature]bool, error) { - return nil, errors.New("supported features unavailable") + failingMotor.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (motor.Properties, error) { + return motor.Properties{}, errPropertiesNotFound } failingMotor.StopFunc = func(ctx context.Context, extra map[string]interface{}) error { - return errors.New("stop failed") + return errStopFailed } failingMotor.IsPoweredFunc = func(ctx context.Context, extra map[string]interface{}) (bool, float64, error) { - return false, 0.0, errors.New("is on unavailable") + return false, 0.0, errIsPoweredFailed } resourceMap := map[resource.Name]motor.Motor{ @@ -112,7 +111,7 @@ func TestClient(t *testing.T) { cancel() _, err := viamgrpc.Dial(cancelCtx, listener1.Addr().String(), logger) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "canceled") + test.That(t, err, test.ShouldBeError, context.Canceled) }) conn, err := viamgrpc.Dial(context.Background(), listener1.Addr().String(), logger) @@ -143,8 +142,8 @@ func TestClient(t *testing.T) { test.That(t, err, test.ShouldBeNil) test.That(t, pos, test.ShouldEqual, 42.0) - features, err := workingMotorClient.Properties(context.Background(), nil) - test.That(t, features[motor.PositionReporting], test.ShouldBeTrue) + properties, err := workingMotorClient.Properties(context.Background(), nil) + test.That(t, properties.PositionReporting, test.ShouldBeTrue) test.That(t, err, test.ShouldBeNil) err = workingMotorClient.Stop(context.Background(), nil) @@ -171,31 +170,39 @@ func TestClient(t *testing.T) { t.Run("client tests for failing motor", func(t *testing.T) { err := failingMotorClient.GoTo(context.Background(), 42.0, 42.0, nil) test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, errGoToFailed.Error()) err = failingMotorClient.ResetZeroPosition(context.Background(), 0.5, nil) test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, errResetZeroFailed.Error()) pos, err := failingMotorClient.Position(context.Background(), nil) test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, errPositionUnavailable.Error()) test.That(t, pos, test.ShouldEqual, 0.0) err = failingMotorClient.SetPower(context.Background(), 42.0, nil) test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, errSetPowerFailed.Error()) err = failingMotorClient.GoFor(context.Background(), 42.0, 42.0, nil) test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, errGoForFailed.Error()) - features, err := failingMotorClient.Properties(context.Background(), nil) - test.That(t, features, test.ShouldBeNil) + properties, err := failingMotorClient.Properties(context.Background(), nil) + test.That(t, properties.PositionReporting, test.ShouldBeFalse) test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, errPropertiesNotFound.Error()) isOn, powerPct, err := failingMotorClient.IsPowered(context.Background(), nil) test.That(t, isOn, test.ShouldBeFalse) test.That(t, powerPct, test.ShouldEqual, 0.0) test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, errIsPoweredFailed.Error()) err = failingMotorClient.Stop(context.Background(), nil) test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, errStopFailed.Error()) test.That(t, failingMotorClient.Close(context.Background()), test.ShouldBeNil) }) @@ -210,8 +217,8 @@ func TestClient(t *testing.T) { test.That(t, err, test.ShouldBeNil) test.That(t, pos, test.ShouldEqual, 42.0) - features, err := workingMotorDialedClient.Properties(context.Background(), nil) - test.That(t, features[motor.PositionReporting], test.ShouldBeTrue) + properties, err := workingMotorDialedClient.Properties(context.Background(), nil) + test.That(t, properties.PositionReporting, test.ShouldBeTrue) test.That(t, err, test.ShouldBeNil) err = workingMotorDialedClient.GoTo(context.Background(), 42.0, 42.0, nil) @@ -240,15 +247,18 @@ func TestClient(t *testing.T) { err = failingMotorDialedClient.SetPower(context.Background(), 39.2, nil) test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, errSetPowerFailed.Error()) - features, err := failingMotorDialedClient.Properties(context.Background(), nil) - test.That(t, features, test.ShouldBeNil) + properties, err := failingMotorDialedClient.Properties(context.Background(), nil) + test.That(t, properties.PositionReporting, test.ShouldBeFalse) test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, errPropertiesNotFound.Error()) isOn, powerPct, err := failingMotorDialedClient.IsPowered(context.Background(), nil) test.That(t, isOn, test.ShouldBeFalse) test.That(t, powerPct, test.ShouldEqual, 0.0) test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, errIsPoweredFailed.Error()) test.That(t, failingMotorDialedClient.Close(context.Background()), test.ShouldBeNil) test.That(t, conn.Close(), test.ShouldBeNil) diff --git a/components/motor/collectors.go b/components/motor/collectors.go index feb5155f5bf..3fda71f3cd3 100644 --- a/components/motor/collectors.go +++ b/components/motor/collectors.go @@ -2,6 +2,7 @@ package motor import ( "context" + "errors" "google.golang.org/protobuf/types/known/anypb" @@ -37,8 +38,13 @@ func newPositionCollector(resource interface{}, params data.CollectorParams) (da } cFunc := data.CaptureFunc(func(ctx context.Context, _ map[string]*anypb.Any) (interface{}, error) { - v, err := motor.Position(ctx, nil) + v, err := motor.Position(ctx, data.FromDMExtraMap) if err != nil { + // A modular filter component can be created to filter the readings from a component. The error ErrNoCaptureToStore + // is used in the datamanager to exclude readings from being captured and stored. + if errors.Is(err, data.ErrNoCaptureToStore) { + return nil, err + } return nil, data.FailedToReadErr(params.ComponentName, position.String(), err) } return Position{Position: v}, nil @@ -59,8 +65,13 @@ func newIsPoweredCollector(resource interface{}, params data.CollectorParams) (d } cFunc := data.CaptureFunc(func(ctx context.Context, _ map[string]*anypb.Any) (interface{}, error) { - v, powerPct, err := motor.IsPowered(ctx, nil) + v, powerPct, err := motor.IsPowered(ctx, data.FromDMExtraMap) if err != nil { + // A modular filter component can be created to filter the readings from a component. The error ErrNoCaptureToStore + // is used in the datamanager to exclude readings from being captured and stored. + if errors.Is(err, data.ErrNoCaptureToStore) { + return nil, err + } return nil, data.FailedToReadErr(params.ComponentName, isPowered.String(), err) } return Powered{IsPowered: v, PowerPct: powerPct}, nil diff --git a/components/motor/dimensionengineering/sabertooth.go b/components/motor/dimensionengineering/sabertooth.go index b7d037b5ee0..d5c5c7db11a 100644 --- a/components/motor/dimensionengineering/sabertooth.go +++ b/components/motor/dimensionengineering/sabertooth.go @@ -47,6 +47,7 @@ type Motor struct { resource.Named resource.AlwaysRebuild + logger golog.Logger // A reference to the actual controller that needs to be commanded for the motor to run c *controller // which channel the motor is connected to on the controller @@ -66,7 +67,7 @@ type Motor struct { maxRPM float64 // A manager to ensure only a single operation is happening at any given time since commands could overlap on the serial port - opMgr operation.SingleOperationManager + opMgr *operation.SingleOperationManager } // Config adds DimensionEngineering-specific config options. @@ -244,6 +245,8 @@ func NewMotor(ctx context.Context, c *Config, name resource.Name, logger golog.L minPowerPct: c.MinPowerPct, maxPowerPct: c.MaxPowerPct, maxRPM: c.MaxRPM, + opMgr: operation.NewSingleOperationManager(), + logger: logger, } if err := m.configure(c); err != nil { @@ -460,9 +463,9 @@ func (m *Motor) DoCommand(ctx context.Context, cmd map[string]interface{}) (map[ return nil, fmt.Errorf("no such command: %s", name) } -// Properties returns the additional features supported by this motor. -func (m *Motor) Properties(ctx context.Context, extra map[string]interface{}) (map[motor.Feature]bool, error) { - return map[motor.Feature]bool{"PositionReporting": false}, nil +// Properties returns the additional properties supported by this motor. +func (m *Motor) Properties(ctx context.Context, extra map[string]interface{}) (motor.Properties, error) { + return motor.Properties{PositionReporting: false}, nil } type command struct { diff --git a/components/motor/dimensionengineering/sabertooth_test.go b/components/motor/dimensionengineering/sabertooth_test.go index 66ba5e06348..5148d5664b5 100644 --- a/components/motor/dimensionengineering/sabertooth_test.go +++ b/components/motor/dimensionengineering/sabertooth_test.go @@ -59,9 +59,9 @@ func TestSabertoothMotor(t *testing.T) { test.That(t, ok, test.ShouldBeTrue) t.Run("motor supports position reporting", func(t *testing.T) { - features, err := motor1.Properties(ctx, nil) + properties, err := motor1.Properties(ctx, nil) test.That(t, err, test.ShouldBeNil) - test.That(t, features[motor.PositionReporting], test.ShouldBeFalse) + test.That(t, properties.PositionReporting, test.ShouldBeFalse) }) t.Run("motor SetPower testing", func(t *testing.T) { @@ -114,9 +114,9 @@ func TestSabertoothMotor(t *testing.T) { test.That(t, ok, test.ShouldBeTrue) t.Run("motor supports position reporting", func(t *testing.T) { - features, err := motor2.Properties(ctx, nil) + properties, err := motor2.Properties(ctx, nil) test.That(t, err, test.ShouldBeNil) - test.That(t, features[motor.PositionReporting], test.ShouldBeFalse) + test.That(t, properties.PositionReporting, test.ShouldBeFalse) }) t.Run("motor SetPower testing", func(t *testing.T) { @@ -232,9 +232,9 @@ func TestSabertoothMotorDirectionFlip(t *testing.T) { test.That(t, ok, test.ShouldBeTrue) t.Run("motor supports position reporting", func(t *testing.T) { - features, err := motor2.Properties(ctx, nil) + properties, err := motor2.Properties(ctx, nil) test.That(t, err, test.ShouldBeNil) - test.That(t, features[motor.PositionReporting], test.ShouldBeFalse) + test.That(t, properties.PositionReporting, test.ShouldBeFalse) }) t.Run("motor SetPower testing", func(t *testing.T) { diff --git a/components/motor/dmc4000/dmc.go b/components/motor/dmc4000/dmc.go index fe2f94effb6..4d20a1f5f87 100644 --- a/components/motor/dmc4000/dmc.go +++ b/components/motor/dmc4000/dmc.go @@ -58,8 +58,9 @@ type Motor struct { MaxAcceleration float64 HomeRPM float64 jogging bool - opMgr operation.SingleOperationManager + opMgr *operation.SingleOperationManager powerPct float64 + logger golog.Logger } // Config adds DMC-specific config options. @@ -153,6 +154,8 @@ func NewMotor(ctx context.Context, c *Config, name resource.Name, logger golog.L MaxAcceleration: c.MaxAcceleration, HomeRPM: c.HomeRPM, powerPct: 0.0, + opMgr: operation.NewSingleOperationManager(), + logger: logger, } if m.maxRPM <= 0 { @@ -811,7 +814,7 @@ func (m *Motor) DoCommand(ctx context.Context, cmd map[string]interface{}) (map[ } } -// Properties returns the additional features supported by this motor. -func (m *Motor) Properties(ctx context.Context, extra map[string]interface{}) (map[motor.Feature]bool, error) { - return map[motor.Feature]bool{motor.PositionReporting: true}, nil +// Properties returns the additional properties supported by this motor. +func (m *Motor) Properties(ctx context.Context, extra map[string]interface{}) (motor.Properties, error) { + return motor.Properties{PositionReporting: true}, nil } diff --git a/components/motor/dmc4000/dmc4000_test.go b/components/motor/dmc4000/dmc4000_test.go index 3d20aabb4dc..4f5c622393b 100644 --- a/components/motor/dmc4000/dmc4000_test.go +++ b/components/motor/dmc4000/dmc4000_test.go @@ -126,9 +126,9 @@ func TestDMC4000Motor(t *testing.T) { waitTx(t, resChan) t.Run("motor supports position reporting", func(t *testing.T) { - features, err := motorDep.Properties(ctx, nil) + properties, err := motorDep.Properties(ctx, nil) test.That(t, err, test.ShouldBeNil) - test.That(t, features[motor.PositionReporting], test.ShouldBeTrue) + test.That(t, properties.PositionReporting, test.ShouldBeTrue) }) t.Run("motor SetPower testing", func(t *testing.T) { diff --git a/components/motor/errors.go b/components/motor/errors.go index a2132f3c600..04d0fef0687 100644 --- a/components/motor/errors.go +++ b/components/motor/errors.go @@ -8,10 +8,10 @@ func NewResetZeroPositionUnsupportedError(motorName string) error { return errors.Errorf("motor with name %s does not support ResetZeroPosition", motorName) } -// NewFeatureUnsupportedError returns an error representing the need -// for a motor to support a particular feature. -func NewFeatureUnsupportedError(feature Feature, motorName string) error { - return errors.Errorf("motor named %s must support feature motor.%s", motorName, feature) +// NewPropertyUnsupportedError returns an error representing the need +// for a motor to support a particular property. +func NewPropertyUnsupportedError(prop Properties, motorName string) error { + return errors.Errorf("motor named %s must support property motor.%v", motorName, prop) } // NewZeroRPMError returns an error representing a request to move a motor at diff --git a/components/motor/fake/motor.go b/components/motor/fake/motor.go index 3da93b3d36e..a37a7bc7d80 100644 --- a/components/motor/fake/motor.go +++ b/components/motor/fake/motor.go @@ -68,6 +68,7 @@ func init() { m := &Motor{ Named: conf.ResourceName().AsNamed(), Logger: logger, + OpMgr: operation.NewSingleOperationManager(), } if err := m.Reconfigure(ctx, deps, conf); err != nil { return nil, err @@ -93,7 +94,7 @@ type Motor struct { DirFlip bool TicksPerRotation int - opMgr operation.SingleOperationManager + OpMgr *operation.SingleOperationManager Logger golog.Logger } @@ -172,10 +173,10 @@ func (m *Motor) Position(ctx context.Context, extra map[string]interface{}) (flo return ticks / float64(m.TicksPerRotation), nil } -// Properties returns the status of whether the motor supports certain optional features. -func (m *Motor) Properties(ctx context.Context, extra map[string]interface{}) (map[motor.Feature]bool, error) { - return map[motor.Feature]bool{ - motor.PositionReporting: m.PositionReporting, +// Properties returns the status of whether the motor supports certain optional properties. +func (m *Motor) Properties(ctx context.Context, extra map[string]interface{}) (motor.Properties, error) { + return motor.Properties{ + PositionReporting: m.PositionReporting, }, nil } @@ -184,7 +185,7 @@ func (m *Motor) SetPower(ctx context.Context, powerPct float64, extra map[string m.mu.Lock() defer m.mu.Unlock() - m.opMgr.CancelRunning(ctx) + m.OpMgr.CancelRunning(ctx) m.Logger.Debugf("Motor SetPower %f", powerPct) m.setPowerPct(powerPct) @@ -285,7 +286,7 @@ func (m *Motor) GoFor(ctx context.Context, rpm, revolutions float64, extra map[s return nil } - if m.opMgr.NewTimedWaitOp(ctx, waitDur) { + if m.OpMgr.NewTimedWaitOp(ctx, waitDur) { err = m.Stop(ctx, nil) if err != nil { return err @@ -333,7 +334,7 @@ func (m *Motor) GoTo(ctx context.Context, rpm, pos float64, extra map[string]int return nil } - if m.opMgr.NewTimedWaitOp(ctx, waitDur) { + if m.OpMgr.NewTimedWaitOp(ctx, waitDur) { err = m.Stop(ctx, nil) if err != nil { return err diff --git a/components/motor/fake/motor_test.go b/components/motor/fake/motor_test.go index 8d533f622f7..a9ae27ac6d5 100644 --- a/components/motor/fake/motor_test.go +++ b/components/motor/fake/motor_test.go @@ -11,6 +11,7 @@ import ( "go.viam.com/rdk/components/encoder/fake" "go.viam.com/rdk/components/motor" + "go.viam.com/rdk/operation" "go.viam.com/rdk/resource" ) @@ -20,7 +21,7 @@ func TestMotorInit(t *testing.T) { enc, err := fake.NewEncoder(context.Background(), resource.Config{ ConvertedAttributes: &fake.Config{}, - }) + }, logger) test.That(t, err, test.ShouldBeNil) m := &Motor{ Encoder: enc.(fake.Encoder), @@ -28,15 +29,16 @@ func TestMotorInit(t *testing.T) { PositionReporting: true, MaxRPM: 60, TicksPerRotation: 1, + OpMgr: operation.NewSingleOperationManager(), } pos, err := m.Position(ctx, nil) test.That(t, err, test.ShouldBeNil) test.That(t, pos, test.ShouldEqual, 0) - featureMap, err := m.Properties(ctx, nil) + properties, err := m.Properties(ctx, nil) test.That(t, err, test.ShouldBeNil) - test.That(t, featureMap[motor.PositionReporting], test.ShouldBeTrue) + test.That(t, properties.PositionReporting, test.ShouldBeTrue) } func TestGoFor(t *testing.T) { @@ -45,7 +47,7 @@ func TestGoFor(t *testing.T) { enc, err := fake.NewEncoder(context.Background(), resource.Config{ ConvertedAttributes: &fake.Config{}, - }) + }, logger) test.That(t, err, test.ShouldBeNil) m := &Motor{ Encoder: enc.(fake.Encoder), @@ -53,6 +55,7 @@ func TestGoFor(t *testing.T) { PositionReporting: true, MaxRPM: 60, TicksPerRotation: 1, + OpMgr: operation.NewSingleOperationManager(), } err = m.GoFor(ctx, 0, 1, nil) @@ -81,7 +84,7 @@ func TestGoTo(t *testing.T) { enc, err := fake.NewEncoder(context.Background(), resource.Config{ ConvertedAttributes: &fake.Config{}, - }) + }, logger) test.That(t, err, test.ShouldBeNil) m := &Motor{ Encoder: enc.(fake.Encoder), @@ -89,6 +92,7 @@ func TestGoTo(t *testing.T) { PositionReporting: true, MaxRPM: 60, TicksPerRotation: 1, + OpMgr: operation.NewSingleOperationManager(), } err = m.GoTo(ctx, 60, 1, nil) @@ -117,7 +121,7 @@ func TestResetZeroPosition(t *testing.T) { enc, err := fake.NewEncoder(context.Background(), resource.Config{ ConvertedAttributes: &fake.Config{}, - }) + }, logger) test.That(t, err, test.ShouldBeNil) m := &Motor{ Encoder: enc.(fake.Encoder), @@ -125,6 +129,7 @@ func TestResetZeroPosition(t *testing.T) { PositionReporting: true, MaxRPM: 60, TicksPerRotation: 1, + OpMgr: operation.NewSingleOperationManager(), } err = m.ResetZeroPosition(ctx, 0, nil) @@ -141,7 +146,7 @@ func TestPower(t *testing.T) { enc, err := fake.NewEncoder(context.Background(), resource.Config{ ConvertedAttributes: &fake.Config{}, - }) + }, logger) test.That(t, err, test.ShouldBeNil) m := &Motor{ Encoder: enc.(fake.Encoder), @@ -149,6 +154,7 @@ func TestPower(t *testing.T) { PositionReporting: true, MaxRPM: 60, TicksPerRotation: 1, + OpMgr: operation.NewSingleOperationManager(), } err = m.SetPower(ctx, 1.0, nil) diff --git a/components/motor/features.go b/components/motor/features.go deleted file mode 100644 index 5d5b07916f8..00000000000 --- a/components/motor/features.go +++ /dev/null @@ -1,31 +0,0 @@ -// Package motor contains an enum representing optional motor features -package motor - -import ( - pb "go.viam.com/api/component/motor/v1" -) - -// Feature is an enum representing an optional motor feature. -type Feature string - -// PositionReporting represesnts the feature of a motor being -// able to report its own position. -const PositionReporting Feature = "PositionReporting" - -// ProtoFeaturesToMap takes a GetPropertiesResponse and returns -// an equivalent Feature-to-boolean map. -func ProtoFeaturesToMap(resp *pb.GetPropertiesResponse) map[Feature]bool { - return map[Feature]bool{ - PositionReporting: resp.PositionReporting, - } -} - -// FeatureMapToProtoResponse takes a map of features to booleans (indicating -// whether the feature is supported) and converts it to a GetPropertiesResponse. -func FeatureMapToProtoResponse( - featureMap map[Feature]bool, -) (*pb.GetPropertiesResponse, error) { - return &pb.GetPropertiesResponse{ - PositionReporting: featureMap[PositionReporting], - }, nil -} diff --git a/components/motor/gpio/basic.go b/components/motor/gpio/basic.go index 5ba4c1dc12c..1e703ffc28d 100644 --- a/components/motor/gpio/basic.go +++ b/components/motor/gpio/basic.go @@ -46,6 +46,7 @@ func NewMotor(b board.Board, mc Config, name resource.Name, logger golog.Logger) maxRPM: mc.MaxRPM, dirFlip: mc.DirectionFlip, logger: logger, + opMgr: operation.NewSingleOperationManager(), } if mc.Pins.A != "" { @@ -101,7 +102,7 @@ type Motor struct { resource.TriviallyCloseable mu sync.Mutex - opMgr operation.SingleOperationManager + opMgr *operation.SingleOperationManager logger golog.Logger // config Board board.Board @@ -123,10 +124,10 @@ func (m *Motor) Position(ctx context.Context, extra map[string]interface{}) (flo return 0, nil } -// Properties returns the status of whether the motor supports certain optional features. -func (m *Motor) Properties(ctx context.Context, extra map[string]interface{}) (map[motor.Feature]bool, error) { - return map[motor.Feature]bool{ - motor.PositionReporting: false, +// Properties returns the status of whether the motor supports certain optional properties. +func (m *Motor) Properties(ctx context.Context, extra map[string]interface{}) (motor.Properties, error) { + return motor.Properties{ + PositionReporting: false, }, nil } diff --git a/components/motor/gpio/basic_test.go b/components/motor/gpio/basic_test.go index 6ec4b6b4e3a..c04dd12e2da 100644 --- a/components/motor/gpio/basic_test.go +++ b/components/motor/gpio/basic_test.go @@ -129,9 +129,9 @@ func TestMotorABPWM(t *testing.T) { test.That(t, err, test.ShouldBeNil) test.That(t, pos, test.ShouldEqual, 0.0) - features, err := m.Properties(ctx, nil) + properties, err := m.Properties(ctx, nil) test.That(t, err, test.ShouldBeNil) - test.That(t, features[motor.PositionReporting], test.ShouldBeFalse) + test.That(t, properties.PositionReporting, test.ShouldBeFalse) }) t.Run("motor (A/B/PWM) Set PWM frequency testing", func(t *testing.T) { @@ -213,9 +213,9 @@ func TestMotorDirPWM(t *testing.T) { test.That(t, err, test.ShouldBeNil) test.That(t, pos, test.ShouldEqual, 0.0) - features, err := m.Properties(ctx, nil) + properties, err := m.Properties(ctx, nil) test.That(t, err, test.ShouldBeNil) - test.That(t, features[motor.PositionReporting], test.ShouldBeFalse) + test.That(t, properties.PositionReporting, test.ShouldBeFalse) }) t.Run("motor (DIR/PWM) Set PWM frequency testing", func(t *testing.T) { @@ -288,9 +288,9 @@ func TestMotorAB(t *testing.T) { test.That(t, err, test.ShouldBeNil) test.That(t, pos, test.ShouldEqual, 0.0) - features, err := m.Properties(ctx, nil) + properties, err := m.Properties(ctx, nil) test.That(t, err, test.ShouldBeNil) - test.That(t, features[motor.PositionReporting], test.ShouldBeFalse) + test.That(t, properties.PositionReporting, test.ShouldBeFalse) }) t.Run("motor (A/B) Set PWM frequency testing", func(t *testing.T) { diff --git a/components/motor/gpio/motor_encoder.go b/components/motor/gpio/motor_encoder.go index fc46f680736..a50b9b19c0b 100644 --- a/components/motor/gpio/motor_encoder.go +++ b/components/motor/gpio/motor_encoder.go @@ -122,13 +122,14 @@ func newEncodedMotor( rampRate: motorConfig.RampRate, maxPowerPct: motorConfig.MaxPowerPct, logger: logger, + opMgr: operation.NewSingleOperationManager(), } props, err := realEncoder.Properties(context.Background(), nil) if err != nil { return nil, errors.New("cannot get encoder properties") } - if !props[encoder.TicksCountSupported] { + if !props.TicksCountSupported { return nil, encoder.NewEncodedMotorPositionTypeUnsupportedError(props) } @@ -199,7 +200,7 @@ type EncodedMotor struct { cancelCtx context.Context cancel func() loop *control.Loop - opMgr operation.SingleOperationManager + opMgr *operation.SingleOperationManager } // EncodedMotorState is the core, non-statistical state for the motor. @@ -241,10 +242,10 @@ func (m *EncodedMotor) directionMovingInLock() int64 { return sign(m.state.lastPowerPct) } -// Properties returns the status of whether the motor supports certain optional features. -func (m *EncodedMotor) Properties(ctx context.Context, extra map[string]interface{}) (map[motor.Feature]bool, error) { - return map[motor.Feature]bool{ - motor.PositionReporting: true, +// Properties returns the status of whether the motor supports certain optional properties. +func (m *EncodedMotor) Properties(ctx context.Context, extra map[string]interface{}) (motor.Properties, error) { + return motor.Properties{ + PositionReporting: true, }, nil } diff --git a/components/motor/gpio/motor_encoder_test.go b/components/motor/gpio/motor_encoder_test.go index 79703323ade..56d88e5323e 100644 --- a/components/motor/gpio/motor_encoder_test.go +++ b/components/motor/gpio/motor_encoder_test.go @@ -17,6 +17,7 @@ import ( "go.viam.com/rdk/components/encoder/single" "go.viam.com/rdk/components/motor" fakemotor "go.viam.com/rdk/components/motor/fake" + "go.viam.com/rdk/operation" "go.viam.com/rdk/resource" ) @@ -34,8 +35,52 @@ func (f *fakeDirectionAware) DirectionMoving() int64 { return int64(f.m.Direction()) } +func MakeSingleBoard(t *testing.T) *fakeboard.Board { + interrupt, _ := fakeboard.NewDigitalInterruptWrapper(board.DigitalInterruptConfig{ + Name: "10", + Pin: "10", + Type: "basic", + }) + + interrupts := map[string]*fakeboard.DigitalInterruptWrapper{ + "10": interrupt, + } + + b := fakeboard.Board{ + GPIOPins: map[string]*fakeboard.GPIOPin{}, + Digitals: interrupts, + } + + return &b +} + +func MakeIncrementalBoard(t *testing.T) *fakeboard.Board { + interrupt11, _ := fakeboard.NewDigitalInterruptWrapper(board.DigitalInterruptConfig{ + Name: "11", + Pin: "11", + Type: "basic", + }) + + interrupt13, _ := fakeboard.NewDigitalInterruptWrapper(board.DigitalInterruptConfig{ + Name: "13", + Pin: "13", + Type: "basic", + }) + + interrupts := map[string]*fakeboard.DigitalInterruptWrapper{ + "11": interrupt11, + "13": interrupt13, + } + + b := fakeboard.Board{ + GPIOPins: map[string]*fakeboard.GPIOPin{}, + Digitals: interrupts, + } + + return &b +} + func TestMotorEncoder1(t *testing.T) { - t.Skip() logger := golog.NewTestLogger(t) undo := SetRPMSleepDebug(1, false) defer undo() @@ -45,15 +90,32 @@ func TestMotorEncoder1(t *testing.T) { MaxRPM: 100, Logger: logger, TicksPerRotation: 100, + OpMgr: operation.NewSingleOperationManager(), } interrupt := &board.BasicDigitalInterrupt{} - e := &single.Encoder{I: interrupt, CancelCtx: context.Background()} - e.AttachDirectionalAwareness(&fakeDirectionAware{m: fakeMotor}) - e.Start(context.Background()) + ctx := context.Background() + b := MakeSingleBoard(t) + deps := make(resource.Dependencies) + deps[board.Named("main")] = b + + ic := single.Config{ + BoardName: "main", + Pins: single.Pin{I: "10"}, + } + + rawcfg := resource.Config{Name: "enc1", ConvertedAttributes: &ic} + e, err := single.NewSingleEncoder(ctx, deps, rawcfg, golog.NewTestLogger(t)) + test.That(t, err, test.ShouldBeNil) + enc := e.(*single.Encoder) + defer enc.Close(context.Background()) + + enc.AttachDirectionalAwareness(&fakeDirectionAware{m: fakeMotor}) dirFMotor, err := NewEncodedMotor(resource.Config{}, cfg, fakeMotor, e, logger) test.That(t, err, test.ShouldBeNil) + defer dirFMotor.Close(context.Background()) motorDep, ok := dirFMotor.(*EncodedMotor) + defer motorDep.Close(context.Background()) test.That(t, ok, test.ShouldBeTrue) defer func() { test.That(t, motorDep.Close(context.Background()), test.ShouldBeNil) @@ -64,9 +126,9 @@ func TestMotorEncoder1(t *testing.T) { test.That(t, err, test.ShouldBeNil) test.That(t, isOn, test.ShouldBeFalse) test.That(t, powerPct, test.ShouldEqual, 0.0) - features, err := motorDep.Properties(context.Background(), nil) + properties, err := motorDep.Properties(context.Background(), nil) test.That(t, err, test.ShouldBeNil) - test.That(t, features[motor.PositionReporting], test.ShouldBeTrue) + test.That(t, properties.PositionReporting, test.ShouldBeTrue) }) t.Run("encoded motor testing regulation", func(t *testing.T) { @@ -93,7 +155,6 @@ func TestMotorEncoder1(t *testing.T) { }) t.Run("encoded motor testing SetPower interrupt GoFor", func(t *testing.T) { - t.Skip() test.That(t, motorDep.goForInternal(context.Background(), 1000, 1000), test.ShouldBeNil) test.That(t, fakeMotor.Direction(), test.ShouldEqual, 1) test.That(t, fakeMotor.PowerPct(), test.ShouldBeGreaterThan, float32(0)) @@ -131,32 +192,18 @@ func TestMotorEncoder1(t *testing.T) { }) test.That(t, motorDep.Stop(context.Background(), nil), test.ShouldBeNil) - - testutils.WaitForAssertion(t, func(tb testing.TB) { - tb.Helper() - pos, err := motorDep.Position(context.Background(), nil) - test.That(tb, err, test.ShouldBeNil) - test.That(tb, pos, test.ShouldAlmostEqual, 10, 0.01) - }) }) t.Run("encoded motor testing GoFor (RPM + | REV +)", func(t *testing.T) { test.That(t, motorDep.goForInternal(context.Background(), 1000, 1), test.ShouldBeNil) - test.That(t, fakeMotor.Direction(), test.ShouldEqual, 1) - test.That(t, fakeMotor.PowerPct(), test.ShouldBeGreaterThan, 0) + test.That(t, motorDep.DirectionMoving(), test.ShouldEqual, 1) - test.That(t, interrupt.Ticks(context.Background(), 99, nowNanosTest()), test.ShouldBeNil) + test.That(t, enc.I.Tick(context.Background(), true, nowNanosTest()), test.ShouldBeNil) testutils.WaitForAssertion(t, func(tb testing.TB) { tb.Helper() test.That(tb, fakeMotor.Direction(), test.ShouldEqual, 1) }) - test.That(t, interrupt.Tick(context.Background(), true, nowNanosTest()), test.ShouldBeNil) - testutils.WaitForAssertion(t, func(tb testing.TB) { - tb.Helper() - test.That(tb, fakeMotor.Direction(), test.ShouldEqual, 0) - }) - test.That(t, motorDep.Stop(context.Background(), nil), test.ShouldBeNil) test.That(t, motorDep.goForInternal(context.Background(), 1000, 1), test.ShouldBeNil) @@ -175,18 +222,12 @@ func TestMotorEncoder1(t *testing.T) { test.That(t, fakeMotor.Direction(), test.ShouldEqual, -1) test.That(t, fakeMotor.PowerPct(), test.ShouldBeLessThan, 0) - test.That(t, interrupt.Ticks(context.Background(), 99, nowNanosTest()), test.ShouldBeNil) + test.That(t, enc.I.Tick(context.Background(), true, nowNanosTest()), test.ShouldBeNil) testutils.WaitForAssertion(t, func(tb testing.TB) { tb.Helper() test.That(tb, fakeMotor.Direction(), test.ShouldEqual, -1) }) - test.That(t, interrupt.Tick(context.Background(), true, nowNanosTest()), test.ShouldBeNil) - testutils.WaitForAssertion(t, func(tb testing.TB) { - tb.Helper() - test.That(tb, fakeMotor.Direction(), test.ShouldEqual, 0) - }) - test.That(t, motorDep.Stop(context.Background(), nil), test.ShouldBeNil) test.That(t, motorDep.goForInternal(context.Background(), -1000, 1), test.ShouldBeNil) @@ -210,12 +251,6 @@ func TestMotorEncoder1(t *testing.T) { test.That(tb, fakeMotor.Direction(), test.ShouldEqual, -1) }) - test.That(t, interrupt.Tick(context.Background(), true, nowNanosTest()), test.ShouldBeNil) - testutils.WaitForAssertion(t, func(tb testing.TB) { - tb.Helper() - test.That(tb, fakeMotor.Direction(), test.ShouldEqual, 0) - }) - test.That(t, motorDep.Stop(context.Background(), nil), test.ShouldBeNil) test.That(t, motorDep.goForInternal(context.Background(), 1000, -1), test.ShouldBeNil) @@ -240,12 +275,6 @@ func TestMotorEncoder1(t *testing.T) { test.That(tb, fakeMotor.Direction(), test.ShouldEqual, 1) }) - test.That(t, interrupt.Tick(context.Background(), true, nowNanosTest()), test.ShouldBeNil) - testutils.WaitForAssertion(t, func(tb testing.TB) { - tb.Helper() - test.That(tb, fakeMotor.Direction(), test.ShouldEqual, 0) - }) - test.That(t, motorDep.Stop(context.Background(), nil), test.ShouldBeNil) test.That(t, motorDep.goForInternal(context.Background(), -1000, -1), test.ShouldBeNil) @@ -277,7 +306,6 @@ func TestMotorEncoder1(t *testing.T) { } func TestMotorEncoderIncremental(t *testing.T) { - // t.Skip() logger := golog.NewTestLogger(t) undo := SetRPMSleepDebug(1, false) defer undo() @@ -297,11 +325,23 @@ func TestMotorEncoderIncremental(t *testing.T) { MaxRPM: 100, Logger: logger, TicksPerRotation: 100, + OpMgr: operation.NewSingleOperationManager(), + } + + ctx := context.Background() + b := MakeIncrementalBoard(t) + deps := make(resource.Dependencies) + deps[board.Named("main")] = b + + ic := incremental.Config{ + BoardName: "main", + Pins: incremental.Pins{A: "11", B: "13"}, } - encA := &board.BasicDigitalInterrupt{} - encB := &board.BasicDigitalInterrupt{} - enc := &incremental.Encoder{A: encA, B: encB, CancelCtx: context.Background()} - enc.Start(context.Background()) + + rawcfg := resource.Config{Name: "enc1", ConvertedAttributes: &ic} + e, err := incremental.NewIncrementalEncoder(ctx, deps, rawcfg, golog.NewTestLogger(t)) + test.That(t, err, test.ShouldBeNil) + enc := e.(*incremental.Encoder) motorIfc, err := NewEncodedMotor(resource.Config{}, cfg, fakeMotor, enc, logger) test.That(t, err, test.ShouldBeNil) @@ -318,8 +358,8 @@ func TestMotorEncoderIncremental(t *testing.T) { return testHarness{ enc, - encA, - encB, + enc.A, + enc.B, fakeMotor, motor, func() { test.That(t, motor.Close(context.Background()), test.ShouldBeNil) }, @@ -584,7 +624,9 @@ func TestWrapMotorWithEncoder(t *testing.T) { logger := golog.NewTestLogger(t) t.Run("wrap motor no encoder", func(t *testing.T) { - fakeMotor := &fakemotor.Motor{} + fakeMotor := &fakemotor.Motor{ + OpMgr: operation.NewSingleOperationManager(), + } m, err := WrapMotorWithEncoder( context.Background(), nil, @@ -598,17 +640,30 @@ func TestWrapMotorWithEncoder(t *testing.T) { }) t.Run("wrap motor with single encoder", func(t *testing.T) { - b, err := fakeboard.NewBoard(context.Background(), resource.Config{ConvertedAttributes: &fakeboard.Config{}}, logger) - test.That(t, err, test.ShouldBeNil) - fakeMotor := &fakemotor.Motor{} - b.Digitals["a"], err = fakeboard.NewDigitalInterruptWrapper(board.DigitalInterruptConfig{ + b := MakeSingleBoard(t) + fakeMotor := &fakemotor.Motor{ + OpMgr: operation.NewSingleOperationManager(), + } + b.Digitals["a"], _ = fakeboard.NewDigitalInterruptWrapper(board.DigitalInterruptConfig{ Type: "basic", }) + + ctx := context.Background() + deps := make(resource.Dependencies) + deps[board.Named("main")] = b + + ic := single.Config{ + BoardName: "main", + Pins: single.Pin{I: "10"}, + } + + rawcfg := resource.Config{Name: "enc1", ConvertedAttributes: &ic} + e, err := single.NewSingleEncoder(ctx, deps, rawcfg, golog.NewTestLogger(t)) test.That(t, err, test.ShouldBeNil) - e := &single.Encoder{I: b.Digitals["a"], CancelCtx: context.Background()} - e.AttachDirectionalAwareness(&fakeDirectionAware{m: fakeMotor}) - e.Start(context.Background()) + enc := e.(*single.Encoder) + defer enc.Close(context.Background()) + enc.AttachDirectionalAwareness(&fakeDirectionAware{m: fakeMotor}) m, err := WrapMotorWithEncoder( context.Background(), e, @@ -621,25 +676,37 @@ func TestWrapMotorWithEncoder(t *testing.T) { logger, ) test.That(t, err, test.ShouldBeNil) + defer m.Close(context.Background()) _, ok := m.(*EncodedMotor) test.That(t, ok, test.ShouldBeTrue) test.That(t, m.Close(context.Background()), test.ShouldBeNil) }) t.Run("wrap motor with hall encoder", func(t *testing.T) { - b, err := fakeboard.NewBoard(context.Background(), resource.Config{ConvertedAttributes: &fakeboard.Config{}}, logger) - test.That(t, err, test.ShouldBeNil) - fakeMotor := &fakemotor.Motor{} - b.Digitals["a"], err = fakeboard.NewDigitalInterruptWrapper(board.DigitalInterruptConfig{ + b := MakeIncrementalBoard(t) + fakeMotor := &fakemotor.Motor{ + OpMgr: operation.NewSingleOperationManager(), + } + b.Digitals["a"], _ = fakeboard.NewDigitalInterruptWrapper(board.DigitalInterruptConfig{ Type: "basic", }) - test.That(t, err, test.ShouldBeNil) - b.Digitals["b"], err = fakeboard.NewDigitalInterruptWrapper(board.DigitalInterruptConfig{ + b.Digitals["b"], _ = fakeboard.NewDigitalInterruptWrapper(board.DigitalInterruptConfig{ Type: "basic", }) + + ctx := context.Background() + deps := make(resource.Dependencies) + deps[board.Named("main")] = b + + ic := incremental.Config{ + BoardName: "main", + Pins: incremental.Pins{A: "11", B: "13"}, + } + + rawcfg := resource.Config{Name: "enc1", ConvertedAttributes: &ic} + + e, err := incremental.NewIncrementalEncoder(ctx, deps, rawcfg, golog.NewTestLogger(t)) test.That(t, err, test.ShouldBeNil) - e := &incremental.Encoder{A: b.Digitals["a"], B: b.Digitals["b"], CancelCtx: context.Background()} - e.Start(context.Background()) m, err := WrapMotorWithEncoder( context.Background(), @@ -653,6 +720,7 @@ func TestWrapMotorWithEncoder(t *testing.T) { logger, ) test.That(t, err, test.ShouldBeNil) + defer m.Close(context.Background()) _, ok := m.(*EncodedMotor) test.That(t, ok, test.ShouldBeTrue) test.That(t, m.Close(context.Background()), test.ShouldBeNil) @@ -667,16 +735,33 @@ func TestDirFlipMotor(t *testing.T) { Logger: logger, TicksPerRotation: 100, DirFlip: true, + OpMgr: operation.NewSingleOperationManager(), } - interrupt := &board.BasicDigitalInterrupt{} + defer dirflipFakeMotor.Close(context.Background()) + + ctx := context.Background() + b := MakeSingleBoard(t) + deps := make(resource.Dependencies) + deps[board.Named("main")] = b + + ic := single.Config{ + BoardName: "main", + Pins: single.Pin{I: "10"}, + } + + rawcfg := resource.Config{Name: "enc1", ConvertedAttributes: &ic} + e, err := single.NewSingleEncoder(ctx, deps, rawcfg, golog.NewTestLogger(t)) + test.That(t, err, test.ShouldBeNil) + enc := e.(*single.Encoder) + defer enc.Close(context.Background()) - e := &single.Encoder{I: interrupt, CancelCtx: context.Background()} - e.AttachDirectionalAwareness(&fakeDirectionAware{m: dirflipFakeMotor}) - e.Start(context.Background()) + enc.AttachDirectionalAwareness(&fakeDirectionAware{m: dirflipFakeMotor}) dirFMotor, err := NewEncodedMotor(resource.Config{}, cfg, dirflipFakeMotor, e, logger) test.That(t, err, test.ShouldBeNil) + defer dirFMotor.Close(context.Background()) _dirFMotor, ok := dirFMotor.(*EncodedMotor) test.That(t, ok, test.ShouldBeTrue) + defer _dirFMotor.Close(context.Background()) t.Run("Direction flip RPM + | REV + ", func(t *testing.T) { test.That(t, _dirFMotor.goForInternal(context.Background(), 1000, 1), test.ShouldBeNil) diff --git a/components/motor/gpiostepper/gpiostepper.go b/components/motor/gpiostepper/gpiostepper.go index 787e7b1e6dc..2769fec3aa5 100644 --- a/components/motor/gpiostepper/gpiostepper.go +++ b/components/motor/gpiostepper/gpiostepper.go @@ -123,6 +123,10 @@ func newGPIOStepper( name resource.Name, logger golog.Logger, ) (motor.Motor, error) { + if b == nil { + return nil, errors.New("board is required") + } + if mc.TicksPerRotation == 0 { return nil, errors.New("expected ticks_per_rotation in config for motor") } @@ -132,6 +136,7 @@ func newGPIOStepper( theBoard: b, stepsPerRotation: mc.TicksPerRotation, logger: logger, + opMgr: operation.NewSingleOperationManager(), } var err error @@ -165,7 +170,12 @@ func newGPIOStepper( m.minDelay = time.Duration(mc.StepperDelay * int(time.Microsecond)) } - m.startThread(ctx) + err = m.enable(ctx, false) + if err != nil { + return nil, err + } + + m.startThread() return m, nil } @@ -182,15 +192,17 @@ type gpioStepper struct { enablePinHigh, enablePinLow board.GPIOPin stepPin, dirPin board.GPIOPin logger golog.Logger - motorName string // state lock sync.Mutex - opMgr operation.SingleOperationManager + opMgr *operation.SingleOperationManager stepPosition int64 threadStarted bool targetStepPosition int64 + + cancel context.CancelFunc + waitGroup sync.WaitGroup } // SetPower sets the percentage of power the motor should employ between 0-1. @@ -200,10 +212,10 @@ func (m *gpioStepper) SetPower(ctx context.Context, powerPct float64, extra map[ return nil } - return errors.Errorf("gpioStepper doesn't support raw power mode in motor (%s)", m.motorName) + return errors.Errorf("gpioStepper doesn't support raw power mode in motor (%s)", m.Name().Name) } -func (m *gpioStepper) startThread(ctx context.Context) { +func (m *gpioStepper) startThread() { m.lock.Lock() defer m.lock.Unlock() @@ -211,21 +223,26 @@ func (m *gpioStepper) startThread(ctx context.Context) { return } - m.threadStarted = true - go m.doRun(ctx) -} + m.logger.Debugf("starting control thread for motor (%s)", m.Name().Name) -func (m *gpioStepper) doRun(ctx context.Context) { - for { - sleep, err := m.doCycle(ctx) - if err != nil { - m.logger.Warnf("error cycling gpioStepper (%s) %s", m.motorName, err.Error()) - } + var ctxWG context.Context + ctxWG, m.cancel = context.WithCancel(context.Background()) + m.threadStarted = true + m.waitGroup.Add(1) + go func() { + defer m.waitGroup.Done() + for { + sleep, err := m.doCycle(ctxWG) + if err != nil { + m.logger.Warnf("error cycling gpioStepper (%s) %s", m.Name().Name, err.Error()) + } - if !utils.SelectContextOrWait(ctx, sleep) { - return + if !utils.SelectContextOrWait(ctxWG, sleep) { + // context done + return + } } - } + }() } func (m *gpioStepper) doCycle(ctx context.Context) (time.Duration, error) { @@ -244,7 +261,7 @@ func (m *gpioStepper) doCycle(ctx context.Context) (time.Duration, error) { // reporting err := m.doStep(ctx, m.stepPosition < m.targetStepPosition) if err != nil { - return time.Second, fmt.Errorf("error stepping %w", err) + return time.Second, fmt.Errorf("error stepping motor (%s) %w", m.Name().Name, err) } // wait the stepper delay to return from the doRun for loop or select @@ -255,7 +272,6 @@ func (m *gpioStepper) doCycle(ctx context.Context) (time.Duration, error) { // have to be locked to call. func (m *gpioStepper) doStep(ctx context.Context, forward bool) error { err := multierr.Combine( - m.enable(ctx, true), m.dirPin.Set(ctx, forward, nil), m.stepPin.Set(ctx, true, nil)) if err != nil { @@ -288,16 +304,26 @@ func (m *gpioStepper) GoFor(ctx context.Context, rpm, revolutions float64, extra ctx, done := m.opMgr.New(ctx) defer done() - err := m.goForInternal(ctx, rpm, revolutions) + err := m.enable(ctx, true) if err != nil { - return errors.Wrapf(err, "error in GoFor from motor (%s)", m.motorName) + return errors.Wrapf(err, "error enabling motor in GoFor from motor (%s)", m.Name().Name) } + err = m.goForInternal(ctx, rpm, revolutions) + if err != nil { + return multierr.Combine( + m.enable(ctx, false), + errors.Wrapf(err, "error in GoFor from motor (%s)", m.Name().Name)) + } + + // this is a long-running operation, do not wait for Stop, do not disable enable pins if revolutions == 0 { return nil } - return m.opMgr.WaitTillNotPowered(ctx, time.Millisecond, m, m.Stop) + return multierr.Combine( + m.opMgr.WaitTillNotPowered(ctx, time.Millisecond, m, m.Stop), + m.enable(ctx, false)) } func (m *gpioStepper) goForInternal(ctx context.Context, rpm, revolutions float64) error { @@ -344,18 +370,18 @@ func (m *gpioStepper) goForInternal(ctx context.Context, rpm, revolutions float6 func (m *gpioStepper) GoTo(ctx context.Context, rpm, positionRevolutions float64, extra map[string]interface{}) error { curPos, err := m.Position(ctx, extra) if err != nil { - return errors.Wrapf(err, "error in GoTo from motor (%s)", m.motorName) + return errors.Wrapf(err, "error in GoTo from motor (%s)", m.Name().Name) } moveDistance := positionRevolutions - curPos // if you call GoFor with 0 revolutions, the motor will spin forever. If we are at the target, // we must avoid this by not calling GoFor. if rdkutils.Float64AlmostEqual(moveDistance, 0, 0.1) { - m.logger.Debugf("GoTo distance nearly zero for motor (%s), not moving", m.motorName) + m.logger.Debugf("GoTo distance nearly zero for motor (%s), not moving", m.Name().Name) return nil } - m.logger.Debugf("motor (%s) going to %.2f at rpm %.2f", m.motorName, moveDistance, math.Abs(rpm)) + m.logger.Debugf("motor (%s) going to %.2f at rpm %.2f", m.Name().Name, moveDistance, math.Abs(rpm)) return m.GoFor(ctx, math.Abs(rpm), moveDistance, extra) } @@ -376,10 +402,10 @@ func (m *gpioStepper) Position(ctx context.Context, extra map[string]interface{} return float64(m.stepPosition) / float64(m.stepsPerRotation), nil } -// Properties returns the status of whether the motor supports certain optional features. -func (m *gpioStepper) Properties(ctx context.Context, extra map[string]interface{}) (map[motor.Feature]bool, error) { - return map[motor.Feature]bool{ - motor.PositionReporting: true, +// Properties returns the status of whether the motor supports certain optional properties. +func (m *gpioStepper) Properties(ctx context.Context, extra map[string]interface{}) (motor.Properties, error) { + return motor.Properties{ + PositionReporting: true, }, nil } @@ -410,7 +436,7 @@ func (m *gpioStepper) stop() { func (m *gpioStepper) IsPowered(ctx context.Context, extra map[string]interface{}) (bool, float64, error) { on, err := m.IsMoving(ctx) if err != nil { - return on, 0.0, errors.Wrapf(err, "error in IsPowered from motor (%s)", m.motorName) + return on, 0.0, errors.Wrapf(err, "error in IsPowered from motor (%s)", m.Name().Name) } percent := 0.0 if on { @@ -420,13 +446,30 @@ func (m *gpioStepper) IsPowered(ctx context.Context, extra map[string]interface{ } func (m *gpioStepper) enable(ctx context.Context, on bool) error { + var err error if m.enablePinHigh != nil { - return m.enablePinHigh.Set(ctx, on, nil) + err = multierr.Combine(err, m.enablePinHigh.Set(ctx, on, nil)) } if m.enablePinLow != nil { - return m.enablePinLow.Set(ctx, !on, nil) + err = multierr.Combine(err, m.enablePinLow.Set(ctx, !on, nil)) } - return nil + return err +} + +func (m *gpioStepper) Close(ctx context.Context) error { + err := m.Stop(ctx, nil) + + m.lock.Lock() + if m.cancel != nil { + m.logger.Debugf("stopping control thread for motor (%s)", m.Name().Name) + m.cancel() + m.cancel = nil + m.threadStarted = false + } + m.lock.Unlock() + m.waitGroup.Wait() + + return err } diff --git a/components/motor/gpiostepper/gpiostepper_test.go b/components/motor/gpiostepper/gpiostepper_test.go index 1b739e39aaa..87d2aba8778 100644 --- a/components/motor/gpiostepper/gpiostepper_test.go +++ b/components/motor/gpiostepper/gpiostepper_test.go @@ -5,81 +5,243 @@ import ( "fmt" "sync" "testing" + "time" "github.com/edaniels/golog" "go.viam.com/test" + "go.viam.com/utils" "go.viam.com/utils/testutils" fakeboard "go.viam.com/rdk/components/board/fake" - "go.viam.com/rdk/components/motor" "go.viam.com/rdk/resource" ) -func Test1(t *testing.T) { +func TestConfigs(t *testing.T) { ctx, cancel := context.WithCancel(context.Background()) - logger, obs := golog.NewObservedTestLogger(t) - - b := &fakeboard.Board{GPIOPins: make(map[string]*fakeboard.GPIOPin)} + defer cancel() - mc := Config{} + logger := golog.NewTestLogger(t) c := resource.Config{ Name: "fake_gpiostepper", } - // Create motor with no board and default config - t.Run("gpiostepper initializing test with no board and default config", func(t *testing.T) { - _, err := newGPIOStepper(ctx, nil, mc, c.ResourceName(), logger) + goodConfig := Config{ + Pins: PinConfig{Direction: "b", Step: "c", EnablePinHigh: "d", EnablePinLow: "e"}, + TicksPerRotation: 200, + BoardName: "brd", + StepperDelay: 30, + } + + pinB := &fakeboard.GPIOPin{} + pinC := &fakeboard.GPIOPin{} + pinD := &fakeboard.GPIOPin{} + pinE := &fakeboard.GPIOPin{} + pinMap := map[string]*fakeboard.GPIOPin{ + "b": pinB, + "c": pinC, + "d": pinD, + "e": pinE, + } + b := fakeboard.Board{GPIOPins: pinMap} + + t.Run("config validation good", func(t *testing.T) { + mc := goodConfig + + deps, err := mc.Validate("") + test.That(t, err, test.ShouldBeNil) + test.That(t, deps, test.ShouldResemble, []string{"brd"}) + + // remove optional fields + mc.StepperDelay = 0 + deps, err = mc.Validate("") + test.That(t, err, test.ShouldBeNil) + test.That(t, deps, test.ShouldResemble, []string{"brd"}) + + mc.Pins.EnablePinHigh = "" + mc.Pins.EnablePinLow = "" + deps, err = mc.Validate("") + test.That(t, err, test.ShouldBeNil) + test.That(t, deps, test.ShouldResemble, []string{"brd"}) + }) + + t.Run("config missing required pins", func(t *testing.T) { + mc := goodConfig + mc.Pins.Direction = "" + + _, err := mc.Validate("") test.That(t, err, test.ShouldNotBeNil) + test.That(t, err, test.ShouldBeError, utils.NewConfigValidationFieldRequiredError("", "dir")) + + mc = goodConfig + mc.Pins.Step = "" + _, err = mc.Validate("") + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err, test.ShouldBeError, utils.NewConfigValidationFieldRequiredError("", "step")) }) - // Create motor with board and default config - t.Run("gpiostepper initializing test with board and default config", func(t *testing.T) { - _, err := newGPIOStepper(ctx, b, mc, c.ResourceName(), logger) + t.Run("config missing ticks", func(t *testing.T) { + mc := goodConfig + mc.TicksPerRotation = 0 + + _, err := mc.Validate("") test.That(t, err, test.ShouldNotBeNil) + test.That(t, err, test.ShouldBeError, utils.NewConfigValidationFieldRequiredError("", "ticks_per_rotation")) + }) + + t.Run("config missing board", func(t *testing.T) { + mc := goodConfig + mc.BoardName = "" + + _, err := mc.Validate("") + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err, test.ShouldBeError, utils.NewConfigValidationFieldRequiredError("", "board")) + }) + + t.Run("initializing good with enable pins", func(t *testing.T) { + m, err := newGPIOStepper(ctx, &b, goodConfig, c.ResourceName(), logger) + s := m.(*gpioStepper) + + test.That(t, err, test.ShouldBeNil) + defer m.Close(ctx) + + test.That(t, s.minDelay, test.ShouldEqual, 30*time.Microsecond) + test.That(t, s.stepsPerRotation, test.ShouldEqual, 200) + test.That(t, s.dirPin, test.ShouldEqual, pinB) + test.That(t, s.stepPin, test.ShouldEqual, pinC) + test.That(t, s.enablePinHigh, test.ShouldEqual, pinD) + test.That(t, s.enablePinLow, test.ShouldEqual, pinE) }) - mc.Pins = PinConfig{Direction: "b"} + t.Run("initializing good without enable pins", func(t *testing.T) { + mc := goodConfig + mc.Pins.EnablePinHigh = "" + mc.Pins.EnablePinLow = "" + + m, err := newGPIOStepper(ctx, &b, mc, c.ResourceName(), logger) + s := m.(*gpioStepper) + + test.That(t, err, test.ShouldBeNil) + defer m.Close(ctx) + + test.That(t, s.dirPin, test.ShouldEqual, pinB) + test.That(t, s.stepPin, test.ShouldEqual, pinC) - _, err := newGPIOStepper(ctx, b, mc, c.ResourceName(), logger) - test.That(t, err, test.ShouldNotBeNil) + // fake board auto-creates new pins by default. just make sure they're not what they would normally be. + test.That(t, s.enablePinHigh, test.ShouldNotEqual, pinD) + test.That(t, s.enablePinLow, test.ShouldNotEqual, pinE) + }) - mc.Pins.Step = "c" + t.Run("initializing with no board", func(t *testing.T) { + _, err := newGPIOStepper(ctx, nil, goodConfig, c.ResourceName(), logger) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, "board is required") + }) - _, err = newGPIOStepper(ctx, b, mc, c.ResourceName(), logger) - test.That(t, err, test.ShouldNotBeNil) + t.Run("initializing without ticks per rotation", func(t *testing.T) { + mc := goodConfig + mc.TicksPerRotation = 0 - mc.TicksPerRotation = 200 + _, err := newGPIOStepper(ctx, &b, mc, c.ResourceName(), logger) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, "expected ticks_per_rotation") + }) - mm, err := newGPIOStepper(ctx, b, mc, c.ResourceName(), logger) - test.That(t, err, test.ShouldBeNil) + t.Run("initializing with negative stepper delay", func(t *testing.T) { + mc := goodConfig + mc.StepperDelay = -100 - m := mm.(*gpioStepper) + m, err := newGPIOStepper(ctx, &b, mc, c.ResourceName(), logger) + s := m.(*gpioStepper) - t.Run("motor test supports position reporting", func(t *testing.T) { - features, err := m.Properties(ctx, nil) test.That(t, err, test.ShouldBeNil) - test.That(t, features[motor.PositionReporting], test.ShouldBeTrue) + defer m.Close(ctx) + test.That(t, s.minDelay, test.ShouldEqual, 0*time.Microsecond) }) - t.Run("motor test isOn functionality", func(t *testing.T) { + t.Run("motor supports position reporting", func(t *testing.T) { + m, err := newGPIOStepper(ctx, &b, goodConfig, c.ResourceName(), logger) + test.That(t, err, test.ShouldBeNil) + defer m.Close(ctx) + + properties, err := m.Properties(ctx, nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, properties.PositionReporting, test.ShouldBeTrue) + }) +} + +// Warning: Tests that run goForInternal may be racy. +func TestRunning(t *testing.T) { + ctx, cancel := context.WithCancel(context.Background()) + defer cancel() + + logger, obs := golog.NewObservedTestLogger(t) + c := resource.Config{ + Name: "fake_gpiostepper", + } + + goodConfig := Config{ + Pins: PinConfig{Direction: "b", Step: "c", EnablePinHigh: "d", EnablePinLow: "e"}, + TicksPerRotation: 200, + BoardName: "brd", + StepperDelay: 30, + } + + pinB := &fakeboard.GPIOPin{} + pinC := &fakeboard.GPIOPin{} + pinD := &fakeboard.GPIOPin{} + pinE := &fakeboard.GPIOPin{} + pinMap := map[string]*fakeboard.GPIOPin{ + "b": pinB, + "c": pinC, + "d": pinD, + "e": pinE, + } + b := fakeboard.Board{GPIOPins: pinMap} + + t.Run("isPowered false after init", func(t *testing.T) { + m, err := newGPIOStepper(ctx, &b, goodConfig, c.ResourceName(), logger) + test.That(t, err, test.ShouldBeNil) + defer m.Close(ctx) + on, powerPct, err := m.IsPowered(ctx, nil) test.That(t, err, test.ShouldBeNil) test.That(t, on, test.ShouldEqual, false) test.That(t, powerPct, test.ShouldEqual, 0.0) + + h, err := pinD.Get(ctx, nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, h, test.ShouldBeFalse) + + l, err := pinE.Get(ctx, nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, l, test.ShouldBeTrue) }) - t.Run("motor testing with positive rpm and positive revolutions", func(t *testing.T) { - err = m.goForInternal(ctx, 100, 2) + t.Run("IsPowered true", func(t *testing.T) { + m, err := newGPIOStepper(ctx, &b, goodConfig, c.ResourceName(), logger) + s := m.(*gpioStepper) test.That(t, err, test.ShouldBeNil) + defer m.Close(ctx) + + // long running goFor + err = s.goForInternal(ctx, 100, 3) + defer m.Stop(ctx, nil) - on, powerPct, err := m.IsPowered(ctx, nil) test.That(t, err, test.ShouldBeNil) - test.That(t, on, test.ShouldEqual, true) - test.That(t, powerPct, test.ShouldEqual, 1.0) + // the motor is running testutils.WaitForAssertion(t, func(tb testing.TB) { tb.Helper() - on, powerPct, err = m.IsPowered(ctx, nil) + on, powerPct, err := m.IsPowered(ctx, nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, on, test.ShouldEqual, true) + test.That(t, powerPct, test.ShouldEqual, 1.0) + }) + + // the motor finished running + testutils.WaitForAssertionWithSleep(t, 100*time.Millisecond, 100, func(tb testing.TB) { + tb.Helper() + on, powerPct, err := m.IsPowered(ctx, nil) test.That(tb, err, test.ShouldBeNil) test.That(tb, on, test.ShouldEqual, false) test.That(tb, powerPct, test.ShouldEqual, 0.0) @@ -87,75 +249,172 @@ func Test1(t *testing.T) { pos, err := m.Position(ctx, nil) test.That(t, err, test.ShouldBeNil) - test.That(t, pos, test.ShouldEqual, 2) + test.That(t, pos, test.ShouldEqual, 3) + test.That(t, s.targetStepPosition, test.ShouldEqual, 600) }) - t.Run("motor testing with negative rpm and positive revolutions", func(t *testing.T) { - err = m.goForInternal(ctx, -100, 2) + t.Run("motor enable", func(t *testing.T) { + m, err := newGPIOStepper(ctx, &b, goodConfig, c.ResourceName(), logger) + s := m.(*gpioStepper) + test.That(t, err, test.ShouldBeNil) + defer m.Close(ctx) + + err = s.enable(ctx, true) + test.That(t, err, test.ShouldBeNil) + + h, err := pinD.Get(ctx, nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, h, test.ShouldBeTrue) + + l, err := pinE.Get(ctx, nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, l, test.ShouldBeFalse) + + err = s.enable(ctx, false) + test.That(t, err, test.ShouldBeNil) + + h, err = pinD.Get(ctx, nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, h, test.ShouldBeFalse) + + l, err = pinE.Get(ctx, nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, l, test.ShouldBeTrue) + }) + + t.Run("motor testing with positive rpm and positive revolutions", func(t *testing.T) { + m, err := newGPIOStepper(ctx, &b, goodConfig, c.ResourceName(), logger) + s := m.(*gpioStepper) + test.That(t, err, test.ShouldBeNil) + defer m.Close(ctx) + + err = s.GoFor(ctx, 10000, 1, nil) test.That(t, err, test.ShouldBeNil) on, powerPct, err := m.IsPowered(ctx, nil) test.That(t, err, test.ShouldBeNil) - test.That(t, on, test.ShouldEqual, true) - test.That(t, powerPct, test.ShouldEqual, 1.0) + test.That(t, on, test.ShouldEqual, false) + test.That(t, powerPct, test.ShouldEqual, 0.0) - testutils.WaitForAssertion(t, func(tb testing.TB) { - tb.Helper() - on, powerPct, err = m.IsPowered(ctx, nil) - test.That(tb, err, test.ShouldBeNil) - test.That(tb, on, test.ShouldEqual, false) - test.That(tb, powerPct, test.ShouldEqual, 0.0) - }) + pos, err := m.Position(ctx, nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, pos, test.ShouldEqual, 1) + test.That(t, s.targetStepPosition, test.ShouldEqual, 200) + }) + + t.Run("motor testing with negative rpm and positive revolutions", func(t *testing.T) { + m, err := newGPIOStepper(ctx, &b, goodConfig, c.ResourceName(), logger) + s := m.(*gpioStepper) + test.That(t, err, test.ShouldBeNil) + defer m.Close(ctx) + + err = m.GoFor(ctx, -10000, 1, nil) + test.That(t, err, test.ShouldBeNil) + + on, powerPct, err := m.IsPowered(ctx, nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, on, test.ShouldEqual, false) + test.That(t, powerPct, test.ShouldEqual, 0.0) pos, err := m.Position(ctx, nil) test.That(t, err, test.ShouldBeNil) - test.That(t, pos, test.ShouldEqual, 0) + test.That(t, pos, test.ShouldEqual, -1) + test.That(t, s.targetStepPosition, test.ShouldEqual, -200) }) t.Run("motor testing with positive rpm and negative revolutions", func(t *testing.T) { - err = m.goForInternal(ctx, 100, -2) + m, err := newGPIOStepper(ctx, &b, goodConfig, c.ResourceName(), logger) + s := m.(*gpioStepper) test.That(t, err, test.ShouldBeNil) + defer m.Close(ctx) - on, powerPct, err := m.IsPowered(ctx, nil) + err = m.GoFor(ctx, 10000, -1, nil) test.That(t, err, test.ShouldBeNil) - test.That(t, on, test.ShouldEqual, true) - test.That(t, powerPct, test.ShouldEqual, 1.0) - testutils.WaitForAssertion(t, func(tb testing.TB) { - tb.Helper() - on, powerPct, err = m.IsPowered(ctx, nil) - test.That(tb, err, test.ShouldBeNil) - test.That(tb, on, test.ShouldEqual, false) - test.That(tb, powerPct, test.ShouldEqual, 0.0) - }) + on, powerPct, err := m.IsPowered(ctx, nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, on, test.ShouldEqual, false) + test.That(t, powerPct, test.ShouldEqual, 0.0) pos, err := m.Position(ctx, nil) test.That(t, err, test.ShouldBeNil) - test.That(t, pos, test.ShouldEqual, -2) + test.That(t, pos, test.ShouldEqual, -1) + test.That(t, s.targetStepPosition, test.ShouldEqual, -200) }) t.Run("motor testing with negative rpm and negative revolutions", func(t *testing.T) { - err = m.goForInternal(ctx, -100, -2) + m, err := newGPIOStepper(ctx, &b, goodConfig, c.ResourceName(), logger) + s := m.(*gpioStepper) + test.That(t, err, test.ShouldBeNil) + defer m.Close(ctx) + + err = m.GoFor(ctx, -10000, -1, nil) test.That(t, err, test.ShouldBeNil) on, powerPct, err := m.IsPowered(ctx, nil) test.That(t, err, test.ShouldBeNil) - test.That(t, on, test.ShouldEqual, true) - test.That(t, powerPct, test.ShouldEqual, 1.0) + test.That(t, on, test.ShouldEqual, false) + test.That(t, powerPct, test.ShouldEqual, 0.0) + + pos, err := m.Position(ctx, nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, pos, test.ShouldEqual, 1) + test.That(t, s.targetStepPosition, test.ShouldEqual, 200) + }) + + t.Run("Ensure stop called when gofor is interrupted", func(t *testing.T) { + m, err := newGPIOStepper(ctx, &b, goodConfig, c.ResourceName(), logger) + s := m.(*gpioStepper) + test.That(t, err, test.ShouldBeNil) + defer m.Close(ctx) + + ctx := context.Background() + var wg sync.WaitGroup + ctx, cancel := context.WithCancel(ctx) + wg.Add(1) + go func() { + m.GoFor(ctx, 100, 100, map[string]interface{}{}) + wg.Done() + }() + + // Make sure it starts moving + testutils.WaitForAssertion(t, func(tb testing.TB) { + tb.Helper() + on, _, err := m.IsPowered(ctx, nil) + test.That(tb, err, test.ShouldBeNil) + test.That(tb, on, test.ShouldEqual, true) + + p, err := m.Position(ctx, nil) + test.That(tb, err, test.ShouldBeNil) + test.That(tb, p, test.ShouldBeGreaterThan, 0) + }) + + cancel() + wg.Wait() + // Make sure it stops moving testutils.WaitForAssertion(t, func(tb testing.TB) { tb.Helper() - on, powerPct, err = m.IsPowered(ctx, nil) + on, _, err := m.IsPowered(ctx, nil) test.That(tb, err, test.ShouldBeNil) test.That(tb, on, test.ShouldEqual, false) - test.That(tb, powerPct, test.ShouldEqual, 0.0) }) + test.That(t, ctx.Err(), test.ShouldNotBeNil) - pos, err := m.Position(ctx, nil) + p, err := m.Position(context.Background(), nil) test.That(t, err, test.ShouldBeNil) - test.That(t, pos, test.ShouldEqual, 0) + + // stop() sets targetStepPosition to the stepPostion value + test.That(t, s.targetStepPosition, test.ShouldEqual, s.stepPosition) + test.That(t, s.targetStepPosition, test.ShouldBeBetweenOrEqual, 1, 100*200) + test.That(t, p, test.ShouldBeBetween, 0, 100) }) - t.Run("Ensure stop called when gofor is interrupted", func(t *testing.T) { + + t.Run("enable pins handled properly during GoFor", func(t *testing.T) { + m, err := newGPIOStepper(ctx, &b, goodConfig, c.ResourceName(), logger) + test.That(t, err, test.ShouldBeNil) + defer m.Close(ctx) + ctx := context.Background() var wg sync.WaitGroup ctx, cancel := context.WithCancel(ctx) @@ -164,23 +423,60 @@ func Test1(t *testing.T) { m.GoFor(ctx, 100, 100, map[string]interface{}{}) wg.Done() }() + + // Make sure it starts moving + testutils.WaitForAssertion(t, func(tb testing.TB) { + tb.Helper() + on, _, err := m.IsPowered(ctx, nil) + test.That(tb, err, test.ShouldBeNil) + test.That(tb, on, test.ShouldEqual, true) + + h, err := pinD.Get(ctx, nil) + test.That(tb, err, test.ShouldBeNil) + test.That(tb, h, test.ShouldBeTrue) + + l, err := pinE.Get(ctx, nil) + test.That(tb, err, test.ShouldBeNil) + test.That(tb, l, test.ShouldBeFalse) + }) + cancel() wg.Wait() + // Make sure it stops moving + testutils.WaitForAssertion(t, func(tb testing.TB) { + tb.Helper() + on, _, err := m.IsPowered(ctx, nil) + test.That(tb, err, test.ShouldBeNil) + test.That(tb, on, test.ShouldEqual, false) + + h, err := pinD.Get(ctx, nil) + test.That(tb, err, test.ShouldBeNil) + test.That(tb, h, test.ShouldBeFalse) + + l, err := pinE.Get(ctx, nil) + test.That(tb, err, test.ShouldBeNil) + test.That(tb, l, test.ShouldBeTrue) + }) test.That(t, ctx.Err(), test.ShouldNotBeNil) }) t.Run("motor testing with large # of revolutions", func(t *testing.T) { - err = m.goForInternal(ctx, 100, 200) + m, err := newGPIOStepper(ctx, &b, goodConfig, c.ResourceName(), logger) + s := m.(*gpioStepper) test.That(t, err, test.ShouldBeNil) + defer m.Close(ctx) - on, powerPct, err := m.IsPowered(ctx, nil) + err = s.goForInternal(ctx, 1000, 200) test.That(t, err, test.ShouldBeNil) - test.That(t, on, test.ShouldEqual, true) - test.That(t, powerPct, test.ShouldEqual, 1.0) testutils.WaitForAssertion(t, func(tb testing.TB) { tb.Helper() + + on, _, err := m.IsPowered(ctx, nil) + test.That(tb, err, test.ShouldBeNil) + test.That(tb, on, test.ShouldEqual, true) + pos, err := m.Position(ctx, nil) test.That(tb, err, test.ShouldBeNil) test.That(tb, pos, test.ShouldBeGreaterThan, 2) @@ -189,6 +485,10 @@ func Test1(t *testing.T) { err = m.Stop(ctx, nil) test.That(t, err, test.ShouldBeNil) + on, _, err := m.IsPowered(ctx, nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, on, test.ShouldEqual, false) + pos, err := m.Position(ctx, nil) test.That(t, err, test.ShouldBeNil) test.That(t, pos, test.ShouldBeGreaterThan, 2) @@ -196,11 +496,19 @@ func Test1(t *testing.T) { }) t.Run("motor testing with 0 rpm", func(t *testing.T) { + m, err := newGPIOStepper(ctx, &b, goodConfig, c.ResourceName(), logger) + test.That(t, err, test.ShouldBeNil) + defer m.Close(ctx) + err = m.GoFor(ctx, 0, 1, nil) test.That(t, err, test.ShouldBeNil) allObs := obs.All() latestLoggedEntry := allObs[len(allObs)-1] test.That(t, fmt.Sprint(latestLoggedEntry), test.ShouldContainSubstring, "nearly 0") + + on, _, err := m.IsPowered(ctx, nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, on, test.ShouldEqual, false) }) cancel() diff --git a/components/motor/i2cmotors/ezopmp.go b/components/motor/i2cmotors/ezopmp.go index 7716acb52e9..2c889ccdee9 100644 --- a/components/motor/i2cmotors/ezopmp.go +++ b/components/motor/i2cmotors/ezopmp.go @@ -83,7 +83,7 @@ type Ezopmp struct { maxPowerPct float64 powerPct float64 maxFlowRate float64 - opMgr operation.SingleOperationManager + opMgr *operation.SingleOperationManager } // available commands. @@ -122,6 +122,7 @@ func NewMotor(ctx context.Context, deps resource.Dependencies, c *Config, name r logger: logger, maxPowerPct: 1.0, powerPct: 0.0, + opMgr: operation.NewSingleOperationManager(), } flowRate, err := m.findMaxFlowRate(ctx) @@ -332,10 +333,10 @@ func (m *Ezopmp) Position(ctx context.Context, extra map[string]interface{}) (fl return floatVal, err } -// Properties returns the status of optional features on the motor. -func (m *Ezopmp) Properties(ctx context.Context, extra map[string]interface{}) (map[motor.Feature]bool, error) { - return map[motor.Feature]bool{ - motor.PositionReporting: true, +// Properties returns the status of optional properties on the motor. +func (m *Ezopmp) Properties(ctx context.Context, extra map[string]interface{}) (motor.Properties, error) { + return motor.Properties{ + PositionReporting: true, }, nil } diff --git a/components/motor/motor.go b/components/motor/motor.go index 65a494ea5ba..b422a31ecb4 100644 --- a/components/motor/motor.go +++ b/components/motor/motor.go @@ -65,8 +65,8 @@ type Motor interface { // back into calls of GoFor. Position(ctx context.Context, extra map[string]interface{}) (float64, error) - // Properties returns whether or not the motor supports certain optional features. - Properties(ctx context.Context, extra map[string]interface{}) (map[Feature]bool, error) + // Properties returns whether or not the motor supports certain optional properties. + Properties(ctx context.Context, extra map[string]interface{}) (Properties, error) // IsPowered returns whether or not the motor is currently on, and the percent power (between 0 // and 1, if the motor is off then the percent power will be 0). @@ -100,12 +100,12 @@ func CreateStatus(ctx context.Context, m Motor) (*pb.Status, error) { if err != nil { return nil, err } - features, err := m.Properties(ctx, nil) + properties, err := m.Properties(ctx, nil) if err != nil { return nil, err } var position float64 - if features[PositionReporting] { + if properties.PositionReporting { position, err = m.Position(ctx, nil) if err != nil { return nil, err diff --git a/components/motor/motor_test.go b/components/motor/motor_test.go index 15649accb7f..9b61177bdf4 100644 --- a/components/motor/motor_test.go +++ b/components/motor/motor_test.go @@ -59,8 +59,8 @@ func TestCreateStatus(t *testing.T) { injectMotor.IsPoweredFunc = func(ctx context.Context, extra map[string]interface{}) (bool, float64, error) { return status.IsPowered, 1.0, nil } - injectMotor.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (map[motor.Feature]bool, error) { - return map[motor.Feature]bool{motor.PositionReporting: true}, nil + injectMotor.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (motor.Properties, error) { + return motor.Properties{PositionReporting: true}, nil } injectMotor.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (float64, error) { return status.Position, nil @@ -103,8 +103,8 @@ func TestCreateStatus(t *testing.T) { }) t.Run("position not supported", func(t *testing.T) { - injectMotor.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (map[motor.Feature]bool, error) { - return map[motor.Feature]bool{motor.PositionReporting: false}, nil + injectMotor.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (motor.Properties, error) { + return motor.Properties{PositionReporting: false}, nil } status1, err := motor.CreateStatus(context.Background(), injectMotor) @@ -113,9 +113,9 @@ func TestCreateStatus(t *testing.T) { }) t.Run("fail on Properties", func(t *testing.T) { - errFail := errors.New("can't get features") - injectMotor.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (map[motor.Feature]bool, error) { - return nil, errFail + errFail := errors.New("can't get properties") + injectMotor.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (motor.Properties, error) { + return motor.Properties{}, errFail } _, err := motor.CreateStatus(context.Background(), injectMotor) test.That(t, err, test.ShouldBeError, errFail) diff --git a/components/motor/properties.go b/components/motor/properties.go new file mode 100644 index 00000000000..7e7dae58af1 --- /dev/null +++ b/components/motor/properties.go @@ -0,0 +1,29 @@ +// Package motor contains a struct representing optional motor properties +package motor + +import ( + pb "go.viam.com/api/component/motor/v1" +) + +// Properties is struct contaning the motor properties. +type Properties struct { + PositionReporting bool +} + +// ProtoFeaturesToProperties takes a GetPropertiesResponse and returns +// an equivalent Properties struct. +func ProtoFeaturesToProperties(resp *pb.GetPropertiesResponse) Properties { + return Properties{ + PositionReporting: resp.PositionReporting, + } +} + +// PropertiesToProtoResponse takes a Properties struct (indicating +// whether the property is supported) and converts it to a GetPropertiesResponse. +func PropertiesToProtoResponse( + props Properties, +) (*pb.GetPropertiesResponse, error) { + return &pb.GetPropertiesResponse{ + PositionReporting: props.PositionReporting, + }, nil +} diff --git a/components/motor/roboclaw/roboclaw.go b/components/motor/roboclaw/roboclaw.go index 83a5fc915d6..ce8605623f2 100644 --- a/components/motor/roboclaw/roboclaw.go +++ b/components/motor/roboclaw/roboclaw.go @@ -198,6 +198,7 @@ func newRoboClaw(conf resource.Config, logger golog.Logger) (motor.Motor, error) conf: motorConfig, addr: uint8(motorConfig.Address), logger: logger, + opMgr: operation.NewSingleOperationManager(), maxRPM: maxRPM, }, nil } @@ -212,7 +213,7 @@ type roboclawMotor struct { maxRPM float64 logger golog.Logger - opMgr operation.SingleOperationManager + opMgr *operation.SingleOperationManager powerPct float64 } @@ -343,9 +344,9 @@ func (m *roboclawMotor) Position(ctx context.Context, extra map[string]interface return float64(ticks) / float64(m.conf.TicksPerRotation), nil } -func (m *roboclawMotor) Properties(ctx context.Context, extra map[string]interface{}) (map[motor.Feature]bool, error) { - return map[motor.Feature]bool{ - motor.PositionReporting: true, +func (m *roboclawMotor) Properties(ctx context.Context, extra map[string]interface{}) (motor.Properties, error) { + return motor.Properties{ + PositionReporting: true, }, nil } diff --git a/components/motor/server.go b/components/motor/server.go index 80dc33043b5..5cb4aa61dbd 100644 --- a/components/motor/server.go +++ b/components/motor/server.go @@ -73,7 +73,7 @@ func (server *serviceServer) GetPosition( return &pb.GetPositionResponse{Position: pos}, nil } -// GetProperties returns a message of booleans indicating which optional features the robot's motor supports. +// GetProperties returns a message of booleans indicating which optional properties the robot's motor supports. func (server *serviceServer) GetProperties( ctx context.Context, req *pb.GetPropertiesRequest, @@ -83,11 +83,11 @@ func (server *serviceServer) GetProperties( if err != nil { return nil, err } - features, err := motor.Properties(ctx, req.Extra.AsMap()) + props, err := motor.Properties(ctx, req.Extra.AsMap()) if err != nil { return nil, err } - return FeatureMapToProtoResponse(features) + return PropertiesToProtoResponse(props) } // Stop turns the motor of the underlying robot off. diff --git a/components/motor/server_test.go b/components/motor/server_test.go index cb22b5099e5..e3265f4e4fd 100644 --- a/components/motor/server_test.go +++ b/components/motor/server_test.go @@ -14,6 +14,18 @@ import ( "go.viam.com/rdk/testutils/inject" ) +var ( + errPositionUnavailable = errors.New("position unavailable") + errResetZeroFailed = errors.New("set to zero failed") + errPropertiesNotFound = errors.New("properties not found") + errGetPropertiesFailed = errors.New("get properties failed") + errSetPowerFailed = errors.New("set power failed") + errGoForFailed = errors.New("go for failed") + errStopFailed = errors.New("stop failed") + errIsPoweredFailed = errors.New("could not determine if motor is on") + errGoToFailed = errors.New("go to failed") +) + func newServer() (pb.MotorServiceServer, *inject.Motor, *inject.Motor, error) { injectMotor1 := &inject.Motor{} injectMotor2 := &inject.Motor{} @@ -41,7 +53,7 @@ func TestServerSetPower(t *testing.T) { test.That(t, err, test.ShouldNotBeNil) failingMotor.SetPowerFunc = func(ctx context.Context, powerPct float64, extra map[string]interface{}) error { - return errors.New("set power failed") + return errSetPowerFailed } req = pb.SetPowerRequest{Name: failMotorName, PowerPct: 0.5} resp, err = motorServer.SetPower(context.Background(), &req) @@ -68,7 +80,7 @@ func TestServerGoFor(t *testing.T) { test.That(t, err, test.ShouldNotBeNil) failingMotor.GoForFunc = func(ctx context.Context, rpm, rotations float64, extra map[string]interface{}) error { - return errors.New("go for failed") + return errGoForFailed } req = pb.GoForRequest{Name: failMotorName, Rpm: 42.0, Revolutions: 42.1} resp, err = motorServer.GoFor(context.Background(), &req) @@ -92,9 +104,10 @@ func TestServerPosition(t *testing.T) { resp, err := motorServer.GetPosition(context.Background(), &req) test.That(t, resp, test.ShouldBeNil) test.That(t, err, test.ShouldNotBeNil) + test.That(t, resource.IsNotFoundError(err), test.ShouldBeTrue) failingMotor.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (float64, error) { - return 0, errors.New("position unavailable") + return 0, errPositionUnavailable } req = pb.GetPositionRequest{Name: failMotorName} resp, err = motorServer.GetPosition(context.Background(), &req) @@ -119,17 +132,17 @@ func TestServerGetProperties(t *testing.T) { test.That(t, resp, test.ShouldBeNil) test.That(t, err, test.ShouldNotBeNil) - failingMotor.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (map[motor.Feature]bool, error) { - return nil, errors.New("unable to get supported features") + failingMotor.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (motor.Properties, error) { + return motor.Properties{}, errGetPropertiesFailed } req = pb.GetPropertiesRequest{Name: failMotorName} resp, err = motorServer.GetProperties(context.Background(), &req) test.That(t, resp, test.ShouldBeNil) test.That(t, err, test.ShouldNotBeNil) - workingMotor.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (map[motor.Feature]bool, error) { - return map[motor.Feature]bool{ - motor.PositionReporting: true, + workingMotor.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (motor.Properties, error) { + return motor.Properties{ + PositionReporting: true, }, nil } req = pb.GetPropertiesRequest{Name: testMotorName} @@ -148,7 +161,7 @@ func TestServerStop(t *testing.T) { test.That(t, err, test.ShouldNotBeNil) failingMotor.StopFunc = func(ctx context.Context, extra map[string]interface{}) error { - return errors.New("stop failed") + return errStopFailed } req = pb.StopRequest{Name: failMotorName} resp, err = motorServer.Stop(context.Background(), &req) @@ -174,7 +187,7 @@ func TestServerIsOn(t *testing.T) { test.That(t, err, test.ShouldNotBeNil) failingMotor.IsPoweredFunc = func(ctx context.Context, extra map[string]interface{}) (bool, float64, error) { - return false, 0.0, errors.New("could not determine if motor is on") + return false, 0.0, errIsPoweredFailed } req = pb.IsPoweredRequest{Name: failMotorName} resp, err = motorServer.IsPowered(context.Background(), &req) @@ -202,7 +215,7 @@ func TestServerGoTo(t *testing.T) { test.That(t, err, test.ShouldNotBeNil) failingMotor.GoToFunc = func(ctx context.Context, rpm, position float64, extra map[string]interface{}) error { - return errors.New("go to failed") + return errGoToFailed } req = pb.GoToRequest{Name: failMotorName, Rpm: 20.0, PositionRevolutions: 2.5} resp, err = motorServer.GoTo(context.Background(), &req) @@ -229,7 +242,7 @@ func TestServerResetZeroPosition(t *testing.T) { test.That(t, err, test.ShouldNotBeNil) failingMotor.ResetZeroPositionFunc = func(ctx context.Context, offset float64, extra map[string]interface{}) error { - return errors.New("set to zero failed") + return errResetZeroFailed } req = pb.ResetZeroPositionRequest{Name: failMotorName, Offset: 1.1} resp, err = motorServer.ResetZeroPosition(context.Background(), &req) diff --git a/components/motor/tmcstepper/stepper_motor_tmc.go b/components/motor/tmcstepper/stepper_motor_tmc.go index 6091c84da07..70621b6683d 100644 --- a/components/motor/tmcstepper/stepper_motor_tmc.go +++ b/components/motor/tmcstepper/stepper_motor_tmc.go @@ -97,7 +97,7 @@ type Motor struct { maxAcc float64 fClk float64 logger golog.Logger - opMgr operation.SingleOperationManager + opMgr *operation.SingleOperationManager powerPct float64 motorName string } @@ -186,6 +186,7 @@ func NewMotor(ctx context.Context, deps resource.Dependencies, c TMC5072Config, maxAcc: c.MaxAcceleration, fClk: baseClk / c.CalFactor, logger: logger, + opMgr: operation.NewSingleOperationManager(), motorName: name.ShortName(), } @@ -391,10 +392,10 @@ func (m *Motor) Position(ctx context.Context, extra map[string]interface{}) (flo return float64(rawPos) / float64(m.stepsPerRev), nil } -// Properties returns the status of optional features on the motor. -func (m *Motor) Properties(ctx context.Context, extra map[string]interface{}) (map[motor.Feature]bool, error) { - return map[motor.Feature]bool{ - motor.PositionReporting: true, +// Properties returns the status of optional properties on the motor. +func (m *Motor) Properties(ctx context.Context, extra map[string]interface{}) (motor.Properties, error) { + return motor.Properties{ + PositionReporting: true, }, nil } diff --git a/components/motor/tmcstepper/stepper_motor_tmc_test.go b/components/motor/tmcstepper/stepper_motor_tmc_test.go index 46646db7476..67bb63296b1 100644 --- a/components/motor/tmcstepper/stepper_motor_tmc_test.go +++ b/components/motor/tmcstepper/stepper_motor_tmc_test.go @@ -195,9 +195,9 @@ func TestTMCStepperMotor(t *testing.T) { test.That(t, ok, test.ShouldBeTrue) t.Run("motor supports position reporting", func(t *testing.T) { - features, err := motorDep.Properties(ctx, nil) + properties, err := motorDep.Properties(ctx, nil) test.That(t, err, test.ShouldBeNil) - test.That(t, features[motor.PositionReporting], test.ShouldBeTrue) + test.That(t, properties.PositionReporting, test.ShouldBeTrue) }) t.Run("motor SetPower testing", func(t *testing.T) { diff --git a/components/motor/ulnstepper/28byj-48.go b/components/motor/ulnstepper/28byj-48.go index cd0f4049d0d..cc1d0408bcf 100644 --- a/components/motor/ulnstepper/28byj-48.go +++ b/components/motor/ulnstepper/28byj-48.go @@ -121,6 +121,7 @@ func new28byj(ctx context.Context, deps resource.Dependencies, conf resource.Con ticksPerRotation: mc.TicksPerRotation, logger: logger, motorName: conf.Name, + opMgr: operation.NewSingleOperationManager(), } in1, err := b.GPIOPinByName(mc.Pins.In1) @@ -163,7 +164,7 @@ type uln28byj struct { // state lock sync.Mutex - opMgr operation.SingleOperationManager + opMgr *operation.SingleOperationManager stepPosition int64 stepperDelay time.Duration @@ -330,10 +331,10 @@ func (m *uln28byj) Position(ctx context.Context, extra map[string]interface{}) ( return float64(m.stepPosition) / float64(m.ticksPerRotation), nil } -// Properties returns the status of whether the motor supports certain optional features. -func (m *uln28byj) Properties(ctx context.Context, extra map[string]interface{}) (map[motor.Feature]bool, error) { - return map[motor.Feature]bool{ - motor.PositionReporting: true, +// Properties returns the status of whether the motor supports certain optional properties. +func (m *uln28byj) Properties(ctx context.Context, extra map[string]interface{}) (motor.Properties, error) { + return motor.Properties{ + PositionReporting: true, }, nil } diff --git a/components/motor/ulnstepper/28byj-48_test.go b/components/motor/ulnstepper/28byj-48_test.go index c09ef1239fb..16a278ec88d 100644 --- a/components/motor/ulnstepper/28byj-48_test.go +++ b/components/motor/ulnstepper/28byj-48_test.go @@ -11,7 +11,6 @@ import ( "go.viam.com/test" "go.viam.com/rdk/components/board" - "go.viam.com/rdk/components/motor" "go.viam.com/rdk/resource" "go.viam.com/rdk/testutils/inject" ) @@ -89,9 +88,9 @@ func TestValid(t *testing.T) { m := mm.(*uln28byj) t.Run("motor test supports position reporting", func(t *testing.T) { - features, err := m.Properties(ctx, nil) + properties, err := m.Properties(ctx, nil) test.That(t, err, test.ShouldBeNil) - test.That(t, features[motor.PositionReporting], test.ShouldBeTrue) + test.That(t, properties.PositionReporting, test.ShouldBeTrue) }) t.Run("motor test isOn functionality", func(t *testing.T) { diff --git a/components/movementsensor/cameramono/cameramono.go b/components/movementsensor/cameramono/cameramono.go deleted file mode 100644 index 5f9cc048eaa..00000000000 --- a/components/movementsensor/cameramono/cameramono.go +++ /dev/null @@ -1,337 +0,0 @@ -// Package cameramono implements a visual odemetry movement sensor based ona single camera stream -// This is an Experimental package -package cameramono - -import ( - "context" - "errors" - "sync" - "time" - - "github.com/edaniels/golog" - "github.com/golang/geo/r3" - geo "github.com/kellydunn/golang-geo" - "github.com/viamrobotics/gostream" - "go.viam.com/utils" - "gonum.org/v1/gonum/mat" - - "go.viam.com/rdk/components/camera" - "go.viam.com/rdk/components/movementsensor" - "go.viam.com/rdk/resource" - "go.viam.com/rdk/rimage" - "go.viam.com/rdk/spatialmath" - "go.viam.com/rdk/vision/odometry" -) - -var model = resource.DefaultModelFamily.WithModel("camera_mono") - -// Config is used for converting config attributes of a cameramono movement sensor. -type Config struct { - Camera string `json:"camera"` - MotionConfig *odometry.MotionEstimationConfig `json:"motion_estimation_config"` -} - -// Validate ensures all parts of the config are valid. -func (cfg *Config) Validate(path string) ([]string, error) { - var deps []string - if cfg.Camera == "" { - return nil, utils.NewConfigValidationError(path, - errors.New("single camera missing for visual odometry")) - } - deps = append(deps, cfg.Camera) - if cfg.MotionConfig == nil { - return nil, utils.NewConfigValidationError(path, - errors.New("no motion_estimation_config for visual odometry algorithm")) - } - - if cfg.MotionConfig.KeyPointCfg == nil { - return nil, utils.NewConfigValidationError(path, - errors.New("no kps config found in motion_estimation_config")) - } - - if cfg.MotionConfig.MatchingCfg == nil { - return nil, utils.NewConfigValidationError(path, - errors.New("no matching config found in motion_estimation_config")) - } - - if cfg.MotionConfig.CamIntrinsics == nil { - return nil, utils.NewConfigValidationError(path, - errors.New("no camera_instrinsics config found in motion_estimation_config")) - } - - if cfg.MotionConfig.ScaleEstimatorCfg == nil { - return nil, utils.NewConfigValidationError(path, - errors.New("no scale_estimator config found in motion_estimation_config")) - } - - if cfg.MotionConfig.CamHeightGround == 0 { - return nil, utils.NewConfigValidationError(path, - errors.New("set camera_height from ground to 0 by default")) - } - if cfg.MotionConfig.KeyPointCfg.BRIEFConf == nil { - return nil, utils.NewConfigValidationError(path, - errors.New("no BRIEF Config found in motion_estimation_config")) - } - - if cfg.MotionConfig.KeyPointCfg.DownscaleFactor <= 1 { - return nil, utils.NewConfigValidationError(path, - errors.New("downscale_factor in motion_estimation_config should be greater than 1")) - } - - return deps, nil -} - -func init() { - resource.RegisterComponent( - movementsensor.API, - model, - resource.Registration[movementsensor.MovementSensor, *Config]{ - Constructor: func( - ctx context.Context, - deps resource.Dependencies, - conf resource.Config, - logger golog.Logger, - ) (movementsensor.MovementSensor, error) { - return newCameraMono(deps, conf, logger) - }, - }) -} - -type cameramono struct { - resource.Named - resource.AlwaysRebuild - activeBackgroundWorkers sync.WaitGroup - cancelCtx context.Context - cancelFunc func() - motion *odometry.Motion3D - logger golog.Logger - result result - stream gostream.VideoStream - mu sync.RWMutex - err movementsensor.LastError -} - -type result struct { - dt float64 - trackedPos r3.Vector - trackedOrient spatialmath.Orientation - angVel spatialmath.AngularVelocity - linVel r3.Vector -} - -func newCameraMono( - deps resource.Dependencies, - conf resource.Config, - logger golog.Logger, -) (movementsensor.MovementSensor, error) { - logger.Info( - "visual odometry using one camera implements Position, Orientation, LinearVelocity and AngularVelocity", - ) - - newConf, err := resource.NativeConfig[*Config](conf) - if err != nil { - return nil, err - } - - cam, err := camera.FromDependencies(deps, newConf.Camera) - if err != nil { - return nil, err - } - - cancelCtx, cancelFunc := context.WithCancel(context.Background()) - - co := &cameramono{ - Named: conf.ResourceName().AsNamed(), - cancelCtx: cancelCtx, - cancelFunc: cancelFunc, - logger: logger, - motion: &odometry.Motion3D{ - Rotation: mat.NewDense(3, 3, []float64{1, 0, 0, 0, 1, 0, 0, 0, 1}), - Translation: mat.NewDense(3, 1, []float64{0, 0, 0}), - }, - result: result{ - trackedPos: r3.Vector{X: 0, Y: 0, Z: 0}, - trackedOrient: spatialmath.NewOrientationVector(), - }, - err: movementsensor.NewLastError(1, 1), - } - - co.stream = gostream.NewEmbeddedVideoStream(cam) - - err = co.backgroundWorker(co.stream, newConf.MotionConfig) - co.err.Set(err) - - return co, co.err.Get() -} - -func (co *cameramono) backgroundWorker(stream gostream.VideoStream, cfg *odometry.MotionEstimationConfig) error { - defer func() { utils.UncheckedError(stream.Close(context.Background())) }() - co.activeBackgroundWorkers.Add(1) - utils.ManagedGo(func() { - sT := time.Now() - sImg, _, err := stream.Next(co.cancelCtx) - if err != nil && errors.Is(err, context.Canceled) { - co.err.Set(err) - return - } - for { - eT := time.Now() - eImg, _, err := stream.Next(co.cancelCtx) - if err != nil { - co.err.Set(err) - return - } - - dt, moreThanZero := co.getDt(sT, eT) - co.result.dt = dt - if moreThanZero { - co.motion, err = co.extractMovementFromOdometer(rimage.ConvertImage(sImg), rimage.ConvertImage(eImg), dt, cfg) - if err != nil { - co.err.Set(err) - co.logger.Error(err) - continue - } - select { - case <-co.cancelCtx.Done(): - return - default: - } - } - - sImg = eImg - sT = eT - } - }, co.activeBackgroundWorkers.Done) - return co.err.Get() -} - -func (co *cameramono) extractMovementFromOdometer( - start, end *rimage.Image, - dt float64, - cfg *odometry.MotionEstimationConfig, -) (*odometry.Motion3D, error) { - motion, _, err := odometry.EstimateMotionFrom2Frames(start, end, cfg, co.logger) - if err != nil { - motion = co.motion - return motion, err - } - - rAng, cAng := motion.Rotation.Dims() - if rAng != 3 || cAng != 3 { - return nil, errors.New("rotation dims are not 3,3") - } - - rLin, cLin := motion.Translation.Dims() - if rLin != 3 || cLin != 1 { - return nil, errors.New("lin dims are not 3,1") - } - - rotMat, err := spatialmath.NewRotationMatrix(co.motion.Rotation.RawMatrix().Data) - if err != nil { - return nil, err - } - - co.mu.RLock() - defer co.mu.RUnlock() - co.result.trackedOrient = co.result.trackedOrient.RotationMatrix().LeftMatMul(*rotMat) - co.result.trackedPos = co.result.trackedPos.Add(translationToR3(co.motion)) - co.result.linVel = calculateLinVel(motion, dt) - co.result.angVel = spatialmath.OrientationToAngularVel(rotMat.EulerAngles(), dt) - - return motion, err -} - -func (co *cameramono) getDt(startTime, endTime time.Time) (float64, bool) { - duration := endTime.Sub(startTime) - dt := float64(duration/time.Millisecond) / 1000 - moreThanZero := dt > 0 - return dt, moreThanZero -} - -// Close closes all the channels and threads. -func (co *cameramono) Close(ctx context.Context) error { - co.cancelFunc() - co.activeBackgroundWorkers.Wait() - err := co.stream.Close(co.cancelCtx) - co.err.Set(err) - return nil -} - -// Position gets the position of the moving object calculated by visual odometry. -func (co *cameramono) Position(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { - co.mu.RLock() - defer co.mu.RUnlock() - return geo.NewPoint(co.result.trackedPos.X, co.result.trackedPos.Y), co.result.trackedPos.Z, nil -} - -// Oritentation gets the position of the moving object calculated by visual odometry. -func (co *cameramono) Orientation(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) { - co.mu.RLock() - defer co.mu.RUnlock() - return co.result.trackedOrient, nil -} - -// Readings gets the position of the moving object calculated by visual odometry. -func (co *cameramono) Readings(ctx context.Context, extra map[string]interface{}) (map[string]interface{}, error) { - return movementsensor.Readings(ctx, co, extra) -} - -// LinearVelocity gets the position of the moving object calculated by visual odometry. -func (co *cameramono) LinearVelocity(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { - co.mu.RLock() - defer co.mu.RUnlock() - return co.result.linVel, nil -} - -func (co *cameramono) LinearAcceleration(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { - return r3.Vector{}, movementsensor.ErrMethodUnimplementedLinearAcceleration -} - -// AngularVelocity gets the position of the moving object calculated by visual odometry. -func (co *cameramono) AngularVelocity(ctx context.Context, extra map[string]interface{}) (spatialmath.AngularVelocity, error) { - co.mu.RLock() - defer co.mu.RUnlock() - return co.result.angVel, nil -} - -// Properties gets the position of the moving object calculated by visual odometry. -func (co *cameramono) Properties(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { - return &movementsensor.Properties{ - PositionSupported: true, - OrientationSupported: true, - AngularVelocitySupported: true, - LinearVelocitySupported: true, - }, nil -} - -// helpers. -func translationToR3(motion *odometry.Motion3D) r3.Vector { - return r3.Vector{ - X: motion.Translation.At(0, 0), - Y: motion.Translation.At(1, 0), - Z: motion.Translation.At(2, 0), - } -} - -func calculateLinVel(motion *odometry.Motion3D, dt float64) r3.Vector { - tVec := translationToR3(motion) - return r3.Vector{ - X: tVec.X / dt, - Y: tVec.Y / dt, - Z: tVec.Z / dt, - } -} - -// unimplemented methods. - -// Accuracy gets the position of the moving object calculated by visual odometry. -func (co *cameramono) Accuracy(ctx context.Context, extra map[string]interface{}) (map[string]float32, error) { - return map[string]float32{}, movementsensor.ErrMethodUnimplementedAccuracy -} - -// CompassHeadings gets the position of the moving object calculated by visual odometry. -func (co *cameramono) CompassHeading(ctx context.Context, extra map[string]interface{}) (float64, error) { - co.mu.RLock() - defer co.mu.RUnlock() - return 0, movementsensor.ErrMethodUnimplementedCompassHeading -} diff --git a/components/movementsensor/cameramono/cameramono_test.go b/components/movementsensor/cameramono/cameramono_test.go deleted file mode 100644 index fbd6e15b746..00000000000 --- a/components/movementsensor/cameramono/cameramono_test.go +++ /dev/null @@ -1,198 +0,0 @@ -package cameramono - -import ( - "context" - "image" - "testing" - "time" - - "github.com/edaniels/golog" - "github.com/golang/geo/r3" - geo "github.com/kellydunn/golang-geo" - "github.com/viamrobotics/gostream" - "go.viam.com/test" - "go.viam.com/utils/artifact" - "gonum.org/v1/gonum/mat" - - "go.viam.com/rdk/components/camera" - "go.viam.com/rdk/components/movementsensor" - "go.viam.com/rdk/pointcloud" - "go.viam.com/rdk/resource" - "go.viam.com/rdk/rimage" - "go.viam.com/rdk/rimage/transform" - "go.viam.com/rdk/spatialmath" - "go.viam.com/rdk/testutils/inject" - "go.viam.com/rdk/vision/keypoints" - "go.viam.com/rdk/vision/odometry" -) - -var tCo = &cameramono{ - Named: camera.Named("foo").AsNamed(), - cancelCtx: context.Background(), - cancelFunc: func() { - }, - result: result{ - trackedPos: r3.Vector{X: 4, Y: 5, Z: 6}, - trackedOrient: &spatialmath.OrientationVector{Theta: 90, OX: 1, OY: 2, OZ: 3}, - angVel: spatialmath.AngularVelocity{X: 10, Y: 20, Z: 30}, - linVel: r3.Vector{X: 40, Y: 50, Z: 60}, - }, - err: movementsensor.NewLastError(1, 1), -} - -func TestInit(t *testing.T) { - camName := "cam" - conf := &Config{} - _, err := conf.Validate("") - test.That(t, err.Error(), test.ShouldContainSubstring, "single camera") - conf.Camera = camName - _, err = conf.Validate("") - test.That(t, err.Error(), test.ShouldContainSubstring, "motion_estimation_config") - - conf.MotionConfig = &odometry.MotionEstimationConfig{ - KeyPointCfg: &keypoints.ORBConfig{ - Layers: 1, - DownscaleFactor: 2, - FastConf: &keypoints.FASTConfig{ - NMatchesCircle: 1, - NMSWinSize: 1, - Threshold: 1, - Oriented: true, - Radius: 1, - }, - BRIEFConf: &keypoints.BRIEFConfig{ - N: 1, - Sampling: 1, - UseOrientation: true, - PatchSize: 1, - }, - }, - MatchingCfg: &keypoints.MatchingConfig{ - DoCrossCheck: true, - MaxDist: 1, - }, - CamIntrinsics: &transform.PinholeCameraIntrinsics{ - Width: 300, - Height: 240, - Fx: 1, - Fy: 1, - Ppx: 1, - Ppy: 1, - }, - ScaleEstimatorCfg: &odometry.ScaleEstimatorConfig{ - ThresholdNormalAngle: 0.1, - ThresholdPlaneInlier: 0.1, - }, - CamHeightGround: 0.1, - } - _, err = conf.Validate("") - test.That(t, err, test.ShouldBeNil) - - logger := golog.NewTestLogger(t) - _, err = newCameraMono(nil, resource.Config{}, logger) - test.That(t, err.Error(), test.ShouldContainSubstring, "Config") - goodC := resource.Config{ConvertedAttributes: conf} - _, err = newCameraMono(nil, goodC, logger) - test.That(t, err.Error(), test.ShouldContainSubstring, "missing from dependencies") - deps := make(resource.Dependencies) - - deps[camera.Named(camName)] = &inject.Camera{ - Camera: nil, - StreamFunc: func(ctx context.Context, errHandlers ...gostream.ErrorHandler) (gostream.VideoStream, error) { - return gostream.NewEmbeddedVideoStreamFromReader(gostream.VideoReaderFunc(func(ctx context.Context) (image.Image, func(), error) { - return image.NewGray(image.Rect(0, 0, 1, 1)), func() {}, nil - })), nil - }, - NextPointCloudFunc: func(ctx context.Context) (pointcloud.PointCloud, error) { - return nil, nil - }, - CloseFunc: func(ctx context.Context) error { - return nil - }, - } -} - -func TestFunctions(t *testing.T) { - xy, z, err := tCo.Position(tCo.cancelCtx, make(map[string]interface{})) - test.That(t, xy, test.ShouldResemble, geo.NewPoint(4, 5)) - test.That(t, z, test.ShouldEqual, 6) - test.That(t, err, test.ShouldBeNil) - - ori, err := tCo.Orientation(tCo.cancelCtx, make(map[string]interface{})) - test.That(t, ori, test.ShouldResemble, &spatialmath.OrientationVector{Theta: 90, OX: 1, OY: 2, OZ: 3}) - test.That(t, err, test.ShouldBeNil) - - linVel, err := tCo.LinearVelocity(tCo.cancelCtx, make(map[string]interface{})) - test.That(t, linVel, test.ShouldResemble, r3.Vector{X: 40, Y: 50, Z: 60}) - test.That(t, err, test.ShouldBeNil) - - angVel, err := tCo.AngularVelocity(tCo.cancelCtx, make(map[string]interface{})) - test.That(t, angVel, test.ShouldResemble, spatialmath.AngularVelocity{X: 10, Y: 20, Z: 30}) - test.That(t, err, test.ShouldBeNil) - test.That(t, err, test.ShouldBeNil) - - props, err := tCo.Properties(tCo.cancelCtx, make(map[string]interface{})) - test.That(t, props, test.ShouldResemble, &movementsensor.Properties{ - PositionSupported: true, - OrientationSupported: true, - LinearVelocitySupported: true, - AngularVelocitySupported: true, - }) - test.That(t, err, test.ShouldBeNil) - - acc, err := tCo.Accuracy(tCo.cancelCtx, make(map[string]interface{})) - test.That(t, acc, test.ShouldResemble, map[string]float32{}) - test.That(t, err, - test.ShouldResemble, - movementsensor.ErrMethodUnimplementedAccuracy) - - ch, err := tCo.CompassHeading(tCo.cancelCtx, make(map[string]interface{})) - test.That(t, ch, test.ShouldEqual, 0) - test.That(t, err, - test.ShouldResemble, - movementsensor.ErrMethodUnimplementedCompassHeading) - - read, err := tCo.Readings(tCo.cancelCtx, make(map[string]interface{})) - test.That(t, read["linear_velocity"], test.ShouldResemble, r3.Vector{X: 40, Y: 50, Z: 60}) - test.That(t, err, test.ShouldBeNil) -} - -func TestMathHelpers(t *testing.T) { - t.Run("test package math", func(t *testing.T) { - tMotion := &odometry.Motion3D{ - Rotation: mat.NewDense(3, 3, []float64{1, 2, 3, 4, 5, 6, 7, 8, 9}), - Translation: mat.NewDense(3, 1, []float64{12, 14, 16}), - } - dt := 2.0 - lVout := calculateLinVel(tMotion, dt) - test.That(t, lVout, test.ShouldResemble, r3.Vector{X: 6, Y: 7, Z: 8}) - - r3Out := translationToR3(tMotion) - test.That(t, r3Out, test.ShouldResemble, r3.Vector{X: 12, Y: 14, Z: 16}) - - start := time.Now() - time.Sleep(500 * time.Millisecond) - end := time.Now() - dt, moreThanZero := tCo.getDt(start, end) - test.That(t, dt, test.ShouldBeGreaterThan, 0.49) - test.That(t, moreThanZero, test.ShouldBeTrue) - }) - - t.Run("test extract images", func(t *testing.T) { - // TODO(RSDK-586): Re-enable after testing new field changes during hack-week. - t.Skip() - tCo.logger = golog.NewTestLogger(t) - // load cfg - cfg, err := odometry.LoadMotionEstimationConfig(artifact.MustPath("vision/odometry/vo_config.json")) - test.That(t, err, test.ShouldBeNil) - // load images - im1, err := rimage.NewImageFromFile(artifact.MustPath("vision/odometry/000001.png")) - test.That(t, err, test.ShouldBeNil) - im2, err := rimage.NewImageFromFile(artifact.MustPath("vision/odometry/000002.png")) - test.That(t, err, test.ShouldBeNil) - motion, err := tCo.extractMovementFromOdometer(im1, im2, 0.1, cfg) - test.That(t, err, test.ShouldBeNil) - test.That(t, motion.Translation.At(2, 0), test.ShouldBeLessThan, -0.8) - test.That(t, motion.Translation.At(1, 0), test.ShouldBeLessThan, 0.2) - }) -} diff --git a/components/movementsensor/client.go b/components/movementsensor/client.go index 75d9ec28675..f16dc87db06 100644 --- a/components/movementsensor/client.go +++ b/components/movementsensor/client.go @@ -135,7 +135,6 @@ func (c *client) CompassHeading(ctx context.Context, extra map[string]interface{ } func (c *client) Readings(ctx context.Context, extra map[string]interface{}) (map[string]interface{}, error) { - // Does not currently go over the network, you cannot call a remote movement sensor's Readings method return Readings(ctx, c, extra) } diff --git a/components/movementsensor/collectors.go b/components/movementsensor/collectors.go index d9b9dd725a0..84e22e0ffae 100644 --- a/components/movementsensor/collectors.go +++ b/components/movementsensor/collectors.go @@ -2,6 +2,7 @@ package movementsensor import ( "context" + "errors" "google.golang.org/protobuf/types/known/anypb" @@ -18,7 +19,7 @@ func assertMovementSensor(resource interface{}) (MovementSensor, error) { return ms, nil } -type lowLevelCollector func(ctx context.Context, ms MovementSensor) (interface{}, error) +type lowLevelCollector func(ctx context.Context, ms MovementSensor, extra map[string]interface{}) (interface{}, error) func registerCollector(name string, f lowLevelCollector) { data.RegisterCollector(data.MethodMetadata{ @@ -31,8 +32,13 @@ func registerCollector(name string, f lowLevelCollector) { } cFunc := data.CaptureFunc(func(ctx context.Context, extra map[string]*anypb.Any) (interface{}, error) { - v, err := f(ctx, ms) + v, err := f(ctx, ms, data.FromDMExtraMap) if err != nil { + // A modular filter component can be created to filter the readings from a component. The error ErrNoCaptureToStore + // is used in the datamanager to exclude readings from being captured and stored. + if errors.Is(err, data.ErrNoCaptureToStore) { + return nil, err + } return nil, data.FailedToReadErr(params.ComponentName, name, err) } return v, nil diff --git a/components/movementsensor/fake/movementsensor.go b/components/movementsensor/fake/movementsensor.go index 3c24d631533..83211c4dc30 100644 --- a/components/movementsensor/fake/movementsensor.go +++ b/components/movementsensor/fake/movementsensor.go @@ -18,31 +18,29 @@ var model = resource.DefaultModelFamily.WithModel("fake") // Config is used for converting fake movementsensor attributes. type Config struct { resource.TriviallyValidateConfig - ConnectionType string `json:"connection_type,omitempty"` } func init() { resource.RegisterComponent( movementsensor.API, model, - resource.Registration[movementsensor.MovementSensor, *Config]{ - Constructor: func( - ctx context.Context, - deps resource.Dependencies, - conf resource.Config, - logger golog.Logger, - ) (movementsensor.MovementSensor, error) { - return movementsensor.MovementSensor(&MovementSensor{ - Named: conf.ResourceName().AsNamed(), - }), nil - }, - }) + resource.Registration[movementsensor.MovementSensor, *Config]{Constructor: NewMovementSensor}) +} + +// NewMovementSensor makes a new fake movement sensor. +func NewMovementSensor(ctx context.Context, deps resource.Dependencies, conf resource.Config, logger golog.Logger, +) (movementsensor.MovementSensor, error) { + return &MovementSensor{ + Named: conf.ResourceName().AsNamed(), + logger: logger, + }, nil } // MovementSensor implements is a fake movement sensor interface. type MovementSensor struct { resource.Named resource.AlwaysRebuild + logger golog.Logger } // Position gets the position of a fake movementsensor. diff --git a/components/movementsensor/gpsnmea/gpsnmea.go b/components/movementsensor/gpsnmea/gpsnmea.go index a6586002118..12d3d2e8a3c 100644 --- a/components/movementsensor/gpsnmea/gpsnmea.go +++ b/components/movementsensor/gpsnmea/gpsnmea.go @@ -1,6 +1,17 @@ // Package gpsnmea implements an NMEA serial gps. package gpsnmea +/* + This package supports GPS NMEA over Serial or I2C. + + NMEA reference manual: + https://www.sparkfun.com/datasheets/GPS/NMEA%20Reference%20Manual-Rev2.1-Dec07.pdf + + Example GPS NMEA chip datasheet: + https://content.u-blox.com/sites/default/files/NEO-M9N-00B_DataSheet_UBX-19014285.pdf + +*/ + import ( "context" "strings" @@ -23,7 +34,6 @@ func connectionTypeError(connType, serialConn, i2cConn string) error { // Config is used for converting NMEA Movement Sensor attibutes. type Config struct { ConnectionType string `json:"connection_type"` - Board string `json:"board,omitempty"` DisableNMEA bool `json:"disable_nmea,omitempty"` *SerialConfig `json:"serial_attributes,omitempty"` @@ -32,10 +42,8 @@ type Config struct { // SerialConfig is used for converting Serial NMEA MovementSensor config attributes. type SerialConfig struct { - SerialPath string `json:"serial_path"` - SerialBaudRate int `json:"serial_baud_rate,omitempty"` - SerialCorrectionPath string `json:"serial_correction_path,omitempty"` - SerialCorrectionBaudRate int `json:"serial_correction_baud_rate,omitempty"` + SerialPath string `json:"serial_path"` + SerialBaudRate int `json:"serial_baud_rate,omitempty"` // TestChan is a fake "serial" path for test use only TestChan chan []uint8 `json:"-"` @@ -43,17 +51,15 @@ type SerialConfig struct { // I2CConfig is used for converting Serial NMEA MovementSensor config attributes. type I2CConfig struct { + Board string `json:"board"` I2CBus string `json:"i2c_bus"` - I2cAddr int `json:"i2c_addr"` + I2CAddr int `json:"i2c_addr"` I2CBaudRate int `json:"i2c_baud_rate,omitempty"` } // Validate ensures all parts of the config are valid. func (cfg *Config) Validate(path string) ([]string, error) { var deps []string - if cfg.Board == "" && cfg.ConnectionType == i2cStr { - return nil, utils.NewConfigValidationFieldRequiredError(path, "board") - } if cfg.ConnectionType == "" { return nil, utils.NewConfigValidationFieldRequiredError(path, "connection_type") @@ -65,28 +71,31 @@ func (cfg *Config) Validate(path string) ([]string, error) { return nil, utils.NewConfigValidationFieldRequiredError(path, "board") } deps = append(deps, cfg.Board) - return deps, cfg.I2CConfig.ValidateI2C(path) + return deps, cfg.I2CConfig.validateI2C(path) case serialStr: - return nil, cfg.SerialConfig.ValidateSerial(path) + return nil, cfg.SerialConfig.validateSerial(path) default: return nil, connectionTypeError(cfg.ConnectionType, serialStr, i2cStr) } } // ValidateI2C ensures all parts of the config are valid. -func (cfg *I2CConfig) ValidateI2C(path string) error { +func (cfg *I2CConfig) validateI2C(path string) error { if cfg.I2CBus == "" { return utils.NewConfigValidationFieldRequiredError(path, "i2c_bus") } - if cfg.I2cAddr == 0 { + if cfg.I2CAddr == 0 { return utils.NewConfigValidationFieldRequiredError(path, "i2c_addr") } + if cfg.Board == "" { + return utils.NewConfigValidationFieldRequiredError(path, "board") + } return nil } // ValidateSerial ensures all parts of the config are valid. -func (cfg *SerialConfig) ValidateSerial(path string) error { +func (cfg *SerialConfig) validateSerial(path string) error { if cfg.SerialPath == "" { return utils.NewConfigValidationFieldRequiredError(path, "serial_path") } diff --git a/components/movementsensor/gpsnmea/nmeaParser.go b/components/movementsensor/gpsnmea/nmeaParser.go index 44787f26123..48c65be5563 100644 --- a/components/movementsensor/gpsnmea/nmeaParser.go +++ b/components/movementsensor/gpsnmea/nmeaParser.go @@ -1,6 +1,7 @@ package gpsnmea import ( + "fmt" "math" "strconv" "strings" @@ -16,25 +17,29 @@ const ( kphToMPerSec = 0.27778 ) -type gpsData struct { - location *geo.Point - alt float64 - speed float64 // ground speed in m per sec - vDOP float64 // vertical accuracy - hDOP float64 // horizontal accuracy - satsInView int // quantity satellites in view - satsInUse int // quantity satellites in view - valid bool - fixQuality int +// GPSData struct combines various attributes related to GPS. +type GPSData struct { + Location *geo.Point + Alt float64 + Speed float64 // ground speed in m per sec + VDOP float64 // vertical accuracy + HDOP float64 // horizontal accuracy + SatsInView int // quantity satellites in view + SatsInUse int // quantity satellites in view + valid bool + FixQuality int + CompassHeading float64 // true compass heading in degree + isEast bool // direction for magnetic variation which outputs East or West. + validCompassHeading bool // true if we get course of direction instead of empty strings. } func errInvalidFix(sentenceType, badFix, goodFix string) error { return errors.Errorf("type %q sentence fix is not valid have: %q want %q", sentenceType, badFix, goodFix) } -// parseAndUpdate will attempt to parse a line to an NMEA sentence, and if valid, will try to update the given struct +// ParseAndUpdate will attempt to parse a line to an NMEA sentence, and if valid, will try to update the given struct // with the values for that line. Nothing will be updated if there is not a valid gps fix. -func (g *gpsData) parseAndUpdate(line string) error { +func (g *GPSData) ParseAndUpdate(line string) error { // add parsing to filter out corrupted data ind := strings.Index(line, "$G") if ind == -1 { @@ -48,85 +53,234 @@ func (g *gpsData) parseAndUpdate(line string) error { if err != nil { return multierr.Combine(errs, err) } - // Most receivers support at least the following sentence types: GSV, RMC, GSA, GGA, GLL, VTG, GNS - if gsv, ok := s.(nmea.GSV); ok { - // GSV provides the number of satellites in view - g.satsInView = int(gsv.NumberSVsInView) - } else if rmc, ok := s.(nmea.RMC); ok { - // RMC provides validity, lon/lat, and ground speed. - if rmc.Validity == "A" { - g.valid = true - } else if rmc.Validity == "V" { - g.valid = false - errs = multierr.Combine(errs, errInvalidFix(rmc.Type, rmc.Validity, "A")) + if s.DataType() == nmea.TypeRMC { + g.parseRMC(line) + } + errs = g.updateData(s) + + if g.Location == nil { + g.Location = geo.NewPoint(math.NaN(), math.NaN()) + errs = multierr.Combine(errs, errors.New("no Location parsed for nmea gps, using default value of lat: NaN, long: NaN")) + return errs + } + + return nil +} + +// given an NMEA sentense, updateData updates it. An error is returned if any of +// the function calls fails. +func (g *GPSData) updateData(s nmea.Sentence) error { + var errs error + + switch sentence := s.(type) { + case nmea.GSV: + if gsv, ok := s.(nmea.GSV); ok { + errs = g.updateGSV(gsv) } - if g.valid { - g.speed = rmc.Speed * knotsToMPerSec - g.location = geo.NewPoint(rmc.Latitude, rmc.Longitude) + case nmea.RMC: + if rmc, ok := s.(nmea.RMC); ok { + errs = g.updateRMC(rmc) } - } else if gsa, ok := s.(nmea.GSA); ok { - // GSA gives horizontal and vertical accuracy, and also describes the type of lock- invalid, 2d, or 3d. - switch gsa.FixType { - case "1": - // No fix - g.valid = false - errs = multierr.Combine(errs, errInvalidFix(gsa.Type, gsa.FixType, "1 or 2")) - case "2": - // 2d fix, valid lat/lon but invalid alt - g.valid = true - g.vDOP = -1 - case "3": - // 3d fix - g.valid = true + case nmea.GSA: + if gsa, ok := s.(nmea.GSA); ok { + errs = g.updateGSA(gsa) } - if g.valid { - g.vDOP = gsa.VDOP - g.hDOP = gsa.HDOP + case nmea.GGA: + if gga, ok := s.(nmea.GGA); ok { + errs = g.updateGGA(gga) } - g.satsInUse = len(gsa.SV) - } else if gga, ok := s.(nmea.GGA); ok { - // GGA provides validity, lon/lat, altitude, sats in use, and horizontal position error - g.fixQuality, err = strconv.Atoi(gga.FixQuality) - if err != nil { - return err + case nmea.GLL: + if gll, ok := s.(nmea.GLL); ok { + errs = g.updateGLL(gll) } - if gga.FixQuality == "0" { - g.valid = false - errs = multierr.Combine(errs, errInvalidFix(gga.Type, gga.FixQuality, "1 to 6")) - } else { - g.valid = true - g.location = geo.NewPoint(gga.Latitude, gga.Longitude) - g.satsInUse = int(gga.NumSatellites) - g.hDOP = gga.HDOP - g.alt = gga.Altitude + case nmea.VTG: + if vtg, ok := s.(nmea.VTG); ok { + errs = g.updateVTG(vtg) } - } else if gll, ok := s.(nmea.GLL); ok { - // GLL provides just lat/lon - now := toPoint(gll) - g.location = now - } else if vtg, ok := s.(nmea.VTG); ok { - // VTG provides ground speed - g.speed = vtg.GroundSpeedKPH * kphToMPerSec - } else if gns, ok := s.(nmea.GNS); ok { - // GNS Provides approximately the same information as GGA - for _, mode := range gns.Mode { - if mode == "N" { - g.valid = false - errs = multierr.Combine(errs, errInvalidFix(gns.Type, mode, " A, D, P, R, F, E, M or S")) - } + case nmea.GNS: + if gns, ok := s.(nmea.GNS); ok { + errs = g.updateGNS(gns) } - if g.valid { - g.location = geo.NewPoint(gns.Latitude, gns.Longitude) - g.satsInUse = int(gns.SVs) - g.hDOP = gns.HDOP - g.alt = gns.Altitude + case nmea.HDT: + if hdt, ok := s.(nmea.HDT); ok { + errs = g.updateHDT(hdt) } + default: + // Handle the case when the sentence type is not recognized + errs = fmt.Errorf("unrecognized sentence type: %T", sentence) } - if g.location == nil { - g.location = geo.NewPoint(math.NaN(), math.NaN()) - errs = multierr.Combine(errs, errors.New("no location parsed for nmea gps, using default value of lat: NaN, long: NaN")) - return errs + return errs +} + +// updateGSV updates g.SatsInView with the information from the provided +// GSV (GPS Satellites in View) data. +// +//nolint:all +func (g *GPSData) updateGSV(gsv nmea.GSV) error { + // GSV provides the number of satellites in view + + g.SatsInView = int(gsv.NumberSVsInView) + return nil +} + +// updateRMC updates the GPSData object with the information from the provided +// RMC (Recommended Minimum Navigation Information) data. +func (g *GPSData) updateRMC(rmc nmea.RMC) error { + if rmc.Validity == "A" { + g.valid = true + } else if rmc.Validity == "V" { + g.valid = false + err := errInvalidFix(rmc.Type, rmc.Validity, "A") + return err + } + if g.valid { + g.Speed = rmc.Speed * knotsToMPerSec + g.Location = geo.NewPoint(rmc.Latitude, rmc.Longitude) + + if g.validCompassHeading { + g.CompassHeading = calculateTrueHeading(rmc.Course, rmc.Variation, g.isEast) + } else { + g.CompassHeading = math.NaN() + } + } + return nil +} + +// updateGSA updates the GPSData object with the information from the provided +// GSA (GPS DOP and Active Satellites) data. +func (g *GPSData) updateGSA(gsa nmea.GSA) error { + switch gsa.FixType { + case "1": + // No fix + g.valid = false + err := errInvalidFix(gsa.Type, gsa.FixType, "1 or 2") + return err + case "2": + // 2d fix, valid lat/lon but invalid Alt + g.valid = true + g.VDOP = -1 + case "3": + // 3d fix + g.valid = true + } + + if g.valid { + g.VDOP = gsa.VDOP + g.HDOP = gsa.HDOP + } + g.SatsInUse = len(gsa.SV) + + return nil +} + +// updateGGA updates the GPSData object with the information from the provided +// GGA (Global Positioning System Fix Data) data. +func (g *GPSData) updateGGA(gga nmea.GGA) error { + var err error + + g.FixQuality, err = strconv.Atoi(gga.FixQuality) + if err != nil { + return err + } + + if gga.FixQuality == "0" { + g.valid = false + err = errInvalidFix(gga.Type, gga.FixQuality, "1 to 6") + } else { + g.valid = true + g.Location = geo.NewPoint(gga.Latitude, gga.Longitude) + g.SatsInUse = int(gga.NumSatellites) + g.HDOP = gga.HDOP + g.Alt = gga.Altitude } + return err +} + +// updateGLL updates g.Location with the location information from the provided +// GLL (Geographic Position - Latitude/Longitude) data. +// +//nolint:all +func (g *GPSData) updateGLL(gll nmea.GLL) error { + now := toPoint(gll) + g.Location = now + return nil +} + +// updateVTG updates g.Speed with the ground speed information from the provided +// VTG (Velocity Made Good) data. +// +//nolint:all +func (g *GPSData) updateVTG(vtg nmea.VTG) error { + // VTG provides ground speed + g.Speed = vtg.GroundSpeedKPH * kphToMPerSec return nil } + +// updateGNS updates the GPSData object with the information from the provided +// GNS (Global Navigation Satellite System) data. +func (g *GPSData) updateGNS(gns nmea.GNS) error { + for _, mode := range gns.Mode { + if mode == "N" { + g.valid = false + err := errInvalidFix(gns.Type, mode, " A, D, P, R, F, E, M or S") + return err + } + } + + if g.valid { + g.Location = geo.NewPoint(gns.Latitude, gns.Longitude) + g.SatsInUse = int(gns.SVs) + g.HDOP = gns.HDOP + g.Alt = gns.Altitude + } + + return nil +} + +//nolint:all +// updateHDT updaates g.CompassHeading with the ground speed information from the provided +func (g *GPSData) updateHDT(hdt nmea.HDT) error { + // HDT provides compass heading + g.CompassHeading = hdt.Heading + return nil +} + +// calculateTrueHeading is used to get true compass heading from RCM messages. +func calculateTrueHeading(heading, magneticDeclination float64, isEast bool) float64 { + var adjustment float64 + if isEast { + adjustment = magneticDeclination + } else { + adjustment = -magneticDeclination + } + + trueHeading := heading + adjustment + if trueHeading < 0 { + trueHeading += 360.0 + } else if trueHeading >= 360 { + trueHeading -= 360.0 + } + + return trueHeading +} + +// parseRMC sets g.isEast bool value by parsing the RMC message for compass heading +// and sets g.validCompassHeading bool since RMC message sends empty strings if +// there is no movement. +// go-nmea library does not provide this feature. +func (g *GPSData) parseRMC(message string) { + data := strings.Split(message, ",") + if len(data) < 10 { + return + } + + if data[8] == "" { + g.validCompassHeading = false + } else { + g.validCompassHeading = true + } + + // Check if the magnetic declination is East or West + g.isEast = strings.Contains(data[10], "E") +} diff --git a/components/movementsensor/gpsnmea/nmeaParser_test.go b/components/movementsensor/gpsnmea/nmeaParser_test.go index 67eeb9ccd20..b32c2752f0e 100644 --- a/components/movementsensor/gpsnmea/nmeaParser_test.go +++ b/components/movementsensor/gpsnmea/nmeaParser_test.go @@ -1,66 +1,128 @@ package gpsnmea import ( + "math" "testing" "go.viam.com/test" ) +func TestParse2(t *testing.T) { + var data GPSData + nmeaSentence := "$GBGSV,1,1,01,33,56,045,27,1*40" + err := data.ParseAndUpdate(nmeaSentence) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, data.Speed, test.ShouldAlmostEqual, 0) + test.That(t, math.IsNaN(data.Location.Lng()), test.ShouldBeTrue) + test.That(t, math.IsNaN(data.Location.Lat()), test.ShouldBeTrue) + + nmeaSentence = "$GNGLL,4046.43133,N,07358.90383,W,203755.00,A,A*6B" + err = data.ParseAndUpdate(nmeaSentence) + test.That(t, err, test.ShouldBeNil) + test.That(t, data.Location.Lat(), test.ShouldAlmostEqual, 40.773855499999996, 0.001) + test.That(t, data.Location.Lng(), test.ShouldAlmostEqual, -73.9817305, 0.001) + + nmeaSentence = "$GNRMC,203756.00,A,4046.43152,N,07358.90347,W,0.059,,120723,,,A,V*0D" + err = data.ParseAndUpdate(nmeaSentence) + test.That(t, err, test.ShouldBeNil) + test.That(t, data.Speed, test.ShouldAlmostEqual, 0.030351959999999997) + test.That(t, data.Location.Lat(), test.ShouldAlmostEqual, 40.77385866666667, 0.001) + test.That(t, data.Location.Lng(), test.ShouldAlmostEqual, -73.9817245, 0.001) + test.That(t, math.IsNaN(data.CompassHeading), test.ShouldBeTrue) + + nmeaSentence = "$GPRMC,210230,A,3855.4487,N,09446.0071,W,0.0,076.2,130495,003.8,E*69" + err = data.ParseAndUpdate(nmeaSentence) + test.That(t, err, test.ShouldBeNil) + test.That(t, data.Speed, test.ShouldAlmostEqual, 0) + test.That(t, data.Location.Lat(), test.ShouldAlmostEqual, 38.924144999999996, 0.001) + test.That(t, data.Location.Lng(), test.ShouldAlmostEqual, -94.76678500000001, 0.001) + test.That(t, data.CompassHeading, test.ShouldAlmostEqual, 72.4) + + nmeaSentence = "$GNVTG,,T,,M,0.059,N,0.108,K,A*38" + err = data.ParseAndUpdate(nmeaSentence) + test.That(t, err, test.ShouldBeNil) + test.That(t, data.Speed, test.ShouldEqual, 0.03000024) + + nmeaSentence = "$GNGGA,203756.00,4046.43152,N,07358.90347,W,1,05,4.65,141.4,M,-34.4,M,,*7E" + err = data.ParseAndUpdate(nmeaSentence) + test.That(t, err, test.ShouldBeNil) + test.That(t, data.valid, test.ShouldBeTrue) + test.That(t, data.Alt, test.ShouldEqual, 141.4) + test.That(t, data.SatsInUse, test.ShouldEqual, 5) + test.That(t, data.HDOP, test.ShouldEqual, 4.65) + test.That(t, data.Location.Lat(), test.ShouldAlmostEqual, 40.77385866666667, 0.001) + test.That(t, data.Location.Lng(), test.ShouldAlmostEqual, -73.9817245, 0.001) + + nmeaSentence = "$GNGSA,A,3,05,23,15,18,,,,,,,,,5.37,4.65,2.69,1*03" + err = data.ParseAndUpdate(nmeaSentence) + test.That(t, err, test.ShouldBeNil) + test.That(t, data.HDOP, test.ShouldEqual, 4.65) + test.That(t, data.VDOP, test.ShouldEqual, 2.69) +} + func TestParsing(t *testing.T) { - var data gpsData + var data GPSData // Test a GGA sentence nmeaSentence := "$GNGGA,191351.000,4403.4655,N,12118.7950,W,1,6,1.72,1094.5,M,-19.6,M,,*47" - err := data.parseAndUpdate(nmeaSentence) + err := data.ParseAndUpdate(nmeaSentence) test.That(t, err, test.ShouldBeNil) test.That(t, data.valid, test.ShouldBeTrue) - test.That(t, data.alt, test.ShouldEqual, 1094.5) - test.That(t, data.satsInUse, test.ShouldEqual, 6) - test.That(t, data.hDOP, test.ShouldEqual, 1.72) - test.That(t, data.location.Lat(), test.ShouldAlmostEqual, 44.05776, 0.001) - test.That(t, data.location.Lng(), test.ShouldAlmostEqual, -121.31325, 0.001) + test.That(t, data.Alt, test.ShouldEqual, 1094.5) + test.That(t, data.SatsInUse, test.ShouldEqual, 6) + test.That(t, data.HDOP, test.ShouldEqual, 1.72) + test.That(t, data.Location.Lat(), test.ShouldAlmostEqual, 44.05776, 0.001) + test.That(t, data.Location.Lng(), test.ShouldAlmostEqual, -121.31325, 0.001) // Test GSA, should update HDOP nmeaSentence = "$GPGSA,A,3,21,10,27,08,,,,,,,,,1.98,2.99,0.98*0E" - err = data.parseAndUpdate(nmeaSentence) + err = data.ParseAndUpdate(nmeaSentence) test.That(t, err, test.ShouldBeNil) - test.That(t, data.hDOP, test.ShouldEqual, 2.99) - test.That(t, data.vDOP, test.ShouldEqual, 0.98) + test.That(t, data.HDOP, test.ShouldEqual, 2.99) + test.That(t, data.VDOP, test.ShouldEqual, 0.98) // Test VTG, should update speed nmeaSentence = "$GNVTG,176.25,T,,M,0.13,N,0.25,K,A*21" - err = data.parseAndUpdate(nmeaSentence) + err = data.ParseAndUpdate(nmeaSentence) test.That(t, err, test.ShouldBeNil) - test.That(t, data.speed, test.ShouldEqual, 0.069445) + test.That(t, data.Speed, test.ShouldEqual, 0.069445) // Test RMC, should update speed and position nmeaSentence = "$GNRMC,191352.000,A,4503.4656,N,13118.7951,W,0.04,90.29,011021,,,A*59" - err = data.parseAndUpdate(nmeaSentence) + err = data.ParseAndUpdate(nmeaSentence) test.That(t, err, test.ShouldBeNil) - test.That(t, data.speed, test.ShouldAlmostEqual, 0.0205776) - test.That(t, data.location.Lat(), test.ShouldAlmostEqual, 45.05776, 0.001) - test.That(t, data.location.Lng(), test.ShouldAlmostEqual, -131.31325, 0.001) + test.That(t, data.Speed, test.ShouldAlmostEqual, 0.0205776) + test.That(t, data.Location.Lat(), test.ShouldAlmostEqual, 45.05776, 0.001) + test.That(t, data.Location.Lng(), test.ShouldAlmostEqual, -131.31325, 0.001) // Test GSV, should update total sats in view nmeaSentence = " $GLGSV,2,2,07,85,23,327,34,70,21,234,21,77,07,028,*50" - err = data.parseAndUpdate(nmeaSentence) + err = data.ParseAndUpdate(nmeaSentence) test.That(t, err, test.ShouldBeNil) - test.That(t, data.satsInView, test.ShouldEqual, 7) + test.That(t, data.SatsInView, test.ShouldEqual, 7) // Test GNS, should update same fields as GGA nmeaSentence = "$GNGNS,014035.00,4332.69262,S,17235.48549,E,RR,13,0.9,25.63,11.24,,*70" - err = data.parseAndUpdate(nmeaSentence) + err = data.ParseAndUpdate(nmeaSentence) test.That(t, err, test.ShouldBeNil) test.That(t, data.valid, test.ShouldBeTrue) - test.That(t, data.alt, test.ShouldEqual, 25.63) - test.That(t, data.satsInUse, test.ShouldEqual, 13) - test.That(t, data.hDOP, test.ShouldEqual, 0.9) - test.That(t, data.location.Lat(), test.ShouldAlmostEqual, -43.544877, 0.001) - test.That(t, data.location.Lng(), test.ShouldAlmostEqual, 172.59142, 0.001) + test.That(t, data.Alt, test.ShouldEqual, 25.63) + test.That(t, data.SatsInUse, test.ShouldEqual, 13) + test.That(t, data.HDOP, test.ShouldEqual, 0.9) + test.That(t, data.Location.Lat(), test.ShouldAlmostEqual, -43.544877, 0.001) + test.That(t, data.Location.Lng(), test.ShouldAlmostEqual, 172.59142, 0.001) // Test GLL, should update location nmeaSentence = "$GPGLL,4112.26,N,11332.22,E,213276,A,*05" - err = data.parseAndUpdate(nmeaSentence) + err = data.ParseAndUpdate(nmeaSentence) + test.That(t, err, test.ShouldBeNil) + test.That(t, data.Location.Lat(), test.ShouldAlmostEqual, 41.20433, 0.001) + test.That(t, data.Location.Lng(), test.ShouldAlmostEqual, 113.537, 0.001) + + nmeaSentence = "$GPRMC,123519,A,4807.038,N,01131.000,E,022.4,084.4,230394,003.1,W*6A" + err = data.ParseAndUpdate(nmeaSentence) test.That(t, err, test.ShouldBeNil) - test.That(t, data.location.Lat(), test.ShouldAlmostEqual, 41.20433, 0.001) - test.That(t, data.location.Lng(), test.ShouldAlmostEqual, 113.537, 0.001) + test.That(t, data.Speed, test.ShouldAlmostEqual, 11.523456) + test.That(t, data.Location.Lat(), test.ShouldAlmostEqual, 48.117299999, 0.001) + test.That(t, data.Location.Lng(), test.ShouldAlmostEqual, 11.516666666, 0.001) + test.That(t, data.CompassHeading, test.ShouldAlmostEqual, 87.5) } diff --git a/components/movementsensor/gpsnmea/pmtkI2C.go b/components/movementsensor/gpsnmea/pmtkI2C.go index 0940c2b539b..72f8e88e34b 100644 --- a/components/movementsensor/gpsnmea/pmtkI2C.go +++ b/components/movementsensor/gpsnmea/pmtkI2C.go @@ -4,6 +4,7 @@ import ( "context" "errors" "fmt" + "math" "sync" "github.com/edaniels/golog" @@ -25,12 +26,13 @@ type PmtkI2CNMEAMovementSensor struct { cancelCtx context.Context cancelFunc func() logger golog.Logger - data gpsData + data GPSData activeBackgroundWorkers sync.WaitGroup - disableNmea bool - err movementsensor.LastError - lastposition movementsensor.LastPosition + disableNmea bool + err movementsensor.LastError + lastPosition movementsensor.LastPosition + lastCompassHeading movementsensor.LastCompassHeading bus board.I2C addr byte @@ -57,7 +59,7 @@ func NewPmtkI2CGPSNMEA( if !ok { return nil, fmt.Errorf("gps init: failed to find i2c bus %s", conf.I2CConfig.I2CBus) } - addr := conf.I2CConfig.I2cAddr + addr := conf.I2CConfig.I2CAddr if addr == -1 { return nil, errors.New("must specify gps i2c address") } @@ -83,8 +85,9 @@ func NewPmtkI2CGPSNMEA( disableNmea: disableNmea, // Overloaded boards can have flaky I2C busses. Only report errors if at least 5 of the // last 10 attempts have failed. - err: movementsensor.NewLastError(10, 5), - lastposition: movementsensor.NewLastPosition(), + err: movementsensor.NewLastError(10, 5), + lastPosition: movementsensor.NewLastPosition(), + lastCompassHeading: movementsensor.NewLastCompassHeading(), } if err := g.Start(ctx); err != nil { @@ -166,7 +169,7 @@ func (g *PmtkI2CNMEAMovementSensor) Start(ctx context.Context) error { if b == 0x0D { if strBuf != "" { g.mu.Lock() - err = g.data.parseAndUpdate(strBuf) + err = g.data.ParseAndUpdate(strBuf) g.mu.Unlock() if err != nil { g.logger.Debugf("can't parse nmea : %s, %v", strBuf, err) @@ -192,47 +195,50 @@ func (g *PmtkI2CNMEAMovementSensor) GetBusAddr() (board.I2C, byte) { //nolint // Position returns the current geographic location of the MovementSensor. func (g *PmtkI2CNMEAMovementSensor) Position(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { - lastPosition := g.lastposition.GetLastPosition() + lastPosition := g.lastPosition.GetLastPosition() g.mu.RLock() defer g.mu.RUnlock() - currentPosition := g.data.location + currentPosition := g.data.Location if currentPosition == nil { return lastPosition, 0, errNilLocation } // if current position is (0,0) we will return the last non zero position - if g.lastposition.IsZeroPosition(currentPosition) && !g.lastposition.IsZeroPosition(lastPosition) { - return lastPosition, g.data.alt, g.err.Get() + if g.lastPosition.IsZeroPosition(currentPosition) && !g.lastPosition.IsZeroPosition(lastPosition) { + return lastPosition, g.data.Alt, g.err.Get() } - // updating lastposition if it is different from the current position - if !g.lastposition.ArePointsEqual(currentPosition, lastPosition) { - g.lastposition.SetLastPosition(currentPosition) + // updating lastPosition if it is different from the current position + if !g.lastPosition.ArePointsEqual(currentPosition, lastPosition) { + g.lastPosition.SetLastPosition(currentPosition) } // updating the last known valid position if the current position is non-zero - if !g.lastposition.IsZeroPosition(currentPosition) && !g.lastposition.IsPositionNaN(currentPosition) { - g.lastposition.SetLastPosition(currentPosition) + if !g.lastPosition.IsZeroPosition(currentPosition) && !g.lastPosition.IsPositionNaN(currentPosition) { + g.lastPosition.SetLastPosition(currentPosition) } - return currentPosition, g.data.alt, g.err.Get() + return currentPosition, g.data.Alt, g.err.Get() } // Accuracy returns the accuracy, hDOP and vDOP. func (g *PmtkI2CNMEAMovementSensor) Accuracy(ctx context.Context, extra map[string]interface{}) (map[string]float32, error) { g.mu.RLock() defer g.mu.RUnlock() - return map[string]float32{"hDOP": float32(g.data.hDOP), "vDOP": float32(g.data.vDOP)}, g.err.Get() + return map[string]float32{"hDOP": float32(g.data.HDOP), "vDOP": float32(g.data.HDOP)}, g.err.Get() } // LinearVelocity returns the current speed of the MovementSensor. func (g *PmtkI2CNMEAMovementSensor) LinearVelocity(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { g.mu.RLock() defer g.mu.RUnlock() - return r3.Vector{X: 0, Y: g.data.speed, Z: 0}, g.err.Get() + headingInRadians := g.data.CompassHeading * (math.Pi / 180) + xVelocity := g.data.Speed * math.Sin(headingInRadians) + yVelocity := g.data.Speed * math.Cos(headingInRadians) + return r3.Vector{X: xVelocity, Y: yVelocity, Z: 0}, g.err.Get() } // LinearAcceleration returns the current linear acceleration of the MovementSensor. @@ -252,11 +258,24 @@ func (g *PmtkI2CNMEAMovementSensor) AngularVelocity( return spatialmath.AngularVelocity{}, movementsensor.ErrMethodUnimplementedAngularVelocity } -// CompassHeading not supported. +// CompassHeading returns the compass heading in degree (0->360). func (g *PmtkI2CNMEAMovementSensor) CompassHeading(ctx context.Context, extra map[string]interface{}) (float64, error) { + lastHeading := g.lastCompassHeading.GetLastCompassHeading() + g.mu.RLock() defer g.mu.RUnlock() - return 0, g.err.Get() + + currentHeading := g.data.CompassHeading + + if !math.IsNaN(lastHeading) && math.IsNaN(currentHeading) { + return lastHeading, nil + } + + if !math.IsNaN(currentHeading) && currentHeading != lastHeading { + g.lastCompassHeading.SetLastCompassHeading(currentHeading) + } + + return currentHeading, nil } // Orientation not supporter. @@ -271,6 +290,7 @@ func (g *PmtkI2CNMEAMovementSensor) Properties(ctx context.Context, extra map[st return &movementsensor.Properties{ LinearVelocitySupported: true, PositionSupported: true, + CompassHeadingSupported: true, }, nil } @@ -278,7 +298,7 @@ func (g *PmtkI2CNMEAMovementSensor) Properties(ctx context.Context, extra map[st func (g *PmtkI2CNMEAMovementSensor) ReadFix(ctx context.Context) (int, error) { g.mu.RLock() defer g.mu.RUnlock() - return g.data.fixQuality, g.err.Get() + return g.data.FixQuality, g.err.Get() } // Readings will use return all of the MovementSensor Readings. diff --git a/components/movementsensor/gpsnmea/pmtkI2C_test.go b/components/movementsensor/gpsnmea/pmtkI2C_test.go index b92c44c13b7..5ac110f8cb7 100644 --- a/components/movementsensor/gpsnmea/pmtkI2C_test.go +++ b/components/movementsensor/gpsnmea/pmtkI2C_test.go @@ -50,15 +50,15 @@ func setupDependencies(t *testing.T) resource.Dependencies { } func TestValidateI2C(t *testing.T) { - fakecfg := &I2CConfig{I2CBus: "some-bus"} + fakecfg := &I2CConfig{Board: testBoardName, I2CBus: "some-bus"} path := "path" - err := fakecfg.ValidateI2C(path) + err := fakecfg.validateI2C(path) test.That(t, err, test.ShouldBeError, gutils.NewConfigValidationFieldRequiredError(path, "i2c_addr")) - fakecfg.I2cAddr = 66 - err = fakecfg.ValidateI2C(path) + fakecfg.I2CAddr = 66 + err = fakecfg.validateI2C(path) test.That(t, err, test.ShouldBeNil) } @@ -85,9 +85,8 @@ func TestNewI2CMovementSensor(t *testing.T) { API: movementsensor.API, ConvertedAttributes: &Config{ ConnectionType: "I2C", - Board: testBoardName, DisableNMEA: false, - I2CConfig: &I2CConfig{I2CBus: testBusName}, + I2CConfig: &I2CConfig{I2CBus: testBusName, Board: testBoardName}, }, } g, err = newNMEAGPS(ctx, deps, conf, logger) @@ -108,16 +107,16 @@ func TestReadingsI2C(t *testing.T) { cancelFunc: cancelFunc, logger: logger, } - g.data = gpsData{ - location: loc, - alt: alt, - speed: speed, - vDOP: vAcc, - hDOP: hAcc, - satsInView: totalSats, - satsInUse: activeSats, + g.data = GPSData{ + Location: loc, + Alt: alt, + Speed: speed, + VDOP: vAcc, + HDOP: hAcc, + SatsInView: totalSats, + SatsInUse: activeSats, valid: valid, - fixQuality: fix, + FixQuality: fix, } g.bus = nil diff --git a/components/movementsensor/gpsnmea/serial.go b/components/movementsensor/gpsnmea/serial.go index 03c6298a6dc..e4b7c7fc10f 100644 --- a/components/movementsensor/gpsnmea/serial.go +++ b/components/movementsensor/gpsnmea/serial.go @@ -6,6 +6,7 @@ import ( "context" "fmt" "io" + "math" "sync" "github.com/adrianmo/go-nmea" @@ -31,12 +32,14 @@ type SerialNMEAMovementSensor struct { cancelCtx context.Context cancelFunc func() logger golog.Logger - data gpsData + data GPSData activeBackgroundWorkers sync.WaitGroup - disableNmea bool - err movementsensor.LastError - lastposition movementsensor.LastPosition + disableNmea bool + err movementsensor.LastError + lastPosition movementsensor.LastPosition + lastCompassHeading movementsensor.LastCompassHeading + isClosed bool dev io.ReadWriteCloser path string @@ -51,21 +54,13 @@ func NewSerialGPSNMEA(ctx context.Context, name resource.Name, conf *Config, log if serialPath == "" { return nil, fmt.Errorf("SerialNMEAMovementSensor expected non-empty string for %q", conf.SerialConfig.SerialPath) } - correctionPath := conf.SerialConfig.SerialCorrectionPath - if correctionPath == "" { - correctionPath = serialPath - logger.Infof("SerialNMEAMovementSensor: correction_path using path: %s", correctionPath) - } + baudRate := conf.SerialConfig.SerialBaudRate if baudRate == 0 { baudRate = 38400 logger.Info("SerialNMEAMovementSensor: serial_baud_rate using default 38400") } - correctionBaudRate := conf.SerialConfig.SerialCorrectionBaudRate - if correctionBaudRate == 0 { - correctionBaudRate = baudRate - logger.Infof("SerialNMEAMovementSensor: correction_baud using baud_rate: %d", baudRate) - } + disableNmea := conf.DisableNMEA if disableNmea { logger.Info("SerialNMEAMovementSensor: NMEA reading disabled") @@ -92,12 +87,11 @@ func NewSerialGPSNMEA(ctx context.Context, name resource.Name, conf *Config, log cancelFunc: cancelFunc, logger: logger, path: serialPath, - correctionPath: correctionPath, baudRate: uint(baudRate), - correctionBaudRate: uint(correctionBaudRate), disableNmea: disableNmea, err: movementsensor.NewLastError(1, 1), - lastposition: movementsensor.NewLastPosition(), + lastPosition: movementsensor.NewLastPosition(), + lastCompassHeading: movementsensor.NewLastCompassHeading(), } if err := g.Start(ctx); err != nil { @@ -120,7 +114,7 @@ func (g *SerialNMEAMovementSensor) Start(ctx context.Context) error { default: } - if !g.disableNmea { + if !g.disableNmea && !g.isClosed { line, err := r.ReadString('\n') if err != nil { g.logger.Errorf("can't read gps serial %s", err) @@ -129,7 +123,7 @@ func (g *SerialNMEAMovementSensor) Start(ctx context.Context) error { } // Update our struct's gps data in-place g.mu.Lock() - err = g.data.parseAndUpdate(line) + err = g.data.ParseAndUpdate(line) g.mu.Unlock() if err != nil { g.logger.Warnf("can't parse nmea sentence: %#v", err) @@ -149,47 +143,50 @@ func (g *SerialNMEAMovementSensor) GetCorrectionInfo() (string, uint) { //nolint // Position position, altitide. func (g *SerialNMEAMovementSensor) Position(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { - lastPosition := g.lastposition.GetLastPosition() + lastPosition := g.lastPosition.GetLastPosition() g.mu.RLock() defer g.mu.RUnlock() - currentPosition := g.data.location + currentPosition := g.data.Location if currentPosition == nil { return lastPosition, 0, errNilLocation } // if current position is (0,0) we will return the last non zero position - if g.lastposition.IsZeroPosition(currentPosition) && !g.lastposition.IsZeroPosition(lastPosition) { - return lastPosition, g.data.alt, g.err.Get() + if g.lastPosition.IsZeroPosition(currentPosition) && !g.lastPosition.IsZeroPosition(lastPosition) { + return lastPosition, g.data.Alt, g.err.Get() } - // updating lastposition if it is different from the current position - if !g.lastposition.ArePointsEqual(currentPosition, lastPosition) { - g.lastposition.SetLastPosition(currentPosition) + // updating lastPosition if it is different from the current position + if !g.lastPosition.ArePointsEqual(currentPosition, lastPosition) { + g.lastPosition.SetLastPosition(currentPosition) } // updating the last known valid position if the current position is non-zero - if !g.lastposition.IsZeroPosition(currentPosition) && !g.lastposition.IsPositionNaN(currentPosition) { - g.lastposition.SetLastPosition(currentPosition) + if !g.lastPosition.IsZeroPosition(currentPosition) && !g.lastPosition.IsPositionNaN(currentPosition) { + g.lastPosition.SetLastPosition(currentPosition) } - return currentPosition, g.data.alt, g.err.Get() + return currentPosition, g.data.Alt, g.err.Get() } // Accuracy returns the accuracy, hDOP and vDOP. func (g *SerialNMEAMovementSensor) Accuracy(ctx context.Context, extra map[string]interface{}) (map[string]float32, error) { g.mu.RLock() defer g.mu.RUnlock() - return map[string]float32{"hDOP": float32(g.data.hDOP), "vDOP": float32(g.data.vDOP)}, nil + return map[string]float32{"hDOP": float32(g.data.HDOP), "vDOP": float32(g.data.VDOP)}, nil } // LinearVelocity linear velocity. func (g *SerialNMEAMovementSensor) LinearVelocity(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { g.mu.RLock() defer g.mu.RUnlock() - return r3.Vector{X: 0, Y: g.data.speed, Z: 0}, nil + headingInRadians := g.data.CompassHeading * (math.Pi / 180) + xVelocity := g.data.Speed * math.Sin(headingInRadians) + yVelocity := g.data.Speed * math.Cos(headingInRadians) + return r3.Vector{X: xVelocity, Y: yVelocity, Z: 0}, g.err.Get() } // LinearAcceleration linear acceleration. @@ -213,16 +210,29 @@ func (g *SerialNMEAMovementSensor) Orientation(ctx context.Context, extra map[st // CompassHeading 0->360. func (g *SerialNMEAMovementSensor) CompassHeading(ctx context.Context, extra map[string]interface{}) (float64, error) { + lastHeading := g.lastCompassHeading.GetLastCompassHeading() + g.mu.RLock() defer g.mu.RUnlock() - return 0, movementsensor.ErrMethodUnimplementedCompassHeading + + currentHeading := g.data.CompassHeading + + if !math.IsNaN(lastHeading) && math.IsNaN(currentHeading) { + return lastHeading, nil + } + + if !math.IsNaN(currentHeading) && currentHeading != lastHeading { + g.lastCompassHeading.SetLastCompassHeading(currentHeading) + } + + return currentHeading, nil } // ReadFix returns Fix quality of MovementSensor measurements. func (g *SerialNMEAMovementSensor) ReadFix(ctx context.Context) (int, error) { g.mu.RLock() defer g.mu.RUnlock() - return g.data.fixQuality, nil + return g.data.FixQuality, nil } // Readings will use return all of the MovementSensor Readings. @@ -247,6 +257,7 @@ func (g *SerialNMEAMovementSensor) Properties(ctx context.Context, extra map[str return &movementsensor.Properties{ LinearVelocitySupported: true, PositionSupported: true, + CompassHeadingSupported: true, }, nil } @@ -254,10 +265,11 @@ func (g *SerialNMEAMovementSensor) Properties(ctx context.Context, extra map[str func (g *SerialNMEAMovementSensor) Close(ctx context.Context) error { g.logger.Debug("Closing SerialNMEAMovementSensor") g.cancelFunc() - defer g.activeBackgroundWorkers.Wait() + g.activeBackgroundWorkers.Wait() g.mu.Lock() defer g.mu.Unlock() + g.isClosed = true if g.dev != nil { if err := g.dev.Close(); err != nil { return err diff --git a/components/movementsensor/gpsnmea/serial_test.go b/components/movementsensor/gpsnmea/serial_test.go index 96afa118317..4bbc353cc7b 100644 --- a/components/movementsensor/gpsnmea/serial_test.go +++ b/components/movementsensor/gpsnmea/serial_test.go @@ -30,11 +30,11 @@ var ( func TestValidateSerial(t *testing.T) { fakecfg := &SerialConfig{} path := "path" - err := fakecfg.ValidateSerial(path) + err := fakecfg.validateSerial(path) test.That(t, err, test.ShouldBeError, utils.NewConfigValidationFieldRequiredError(path, "serial_path")) fakecfg.SerialPath = "some-path" - err = fakecfg.ValidateSerial(path) + err = fakecfg.validateSerial(path) test.That(t, err, test.ShouldBeNil) } @@ -65,15 +65,12 @@ func TestNewSerialMovementSensor(t *testing.T) { API: movementsensor.API, ConvertedAttributes: &Config{ ConnectionType: "serial", - Board: "local", DisableNMEA: false, SerialConfig: &SerialConfig{ - SerialPath: path, - SerialBaudRate: 0, - SerialCorrectionPath: path, - SerialCorrectionBaudRate: 0, + SerialPath: path, + SerialBaudRate: 0, }, - I2CConfig: &I2CConfig{}, + I2CConfig: &I2CConfig{Board: "local"}, }, } g, err = newNMEAGPS(ctx, deps, cfig, logger) @@ -93,16 +90,16 @@ func TestReadingsSerial(t *testing.T) { cancelFunc: cancelFunc, logger: logger, } - g.data = gpsData{ - location: loc, - alt: alt, - speed: speed, - vDOP: vAcc, - hDOP: hAcc, - satsInView: totalSats, - satsInUse: activeSats, + g.data = GPSData{ + Location: loc, + Alt: alt, + Speed: speed, + VDOP: vAcc, + HDOP: hAcc, + SatsInView: totalSats, + SatsInUse: activeSats, valid: valid, - fixQuality: fix, + FixQuality: fix, } path := "somepath" diff --git a/components/movementsensor/gpsrtk/gpsrtk.go b/components/movementsensor/gpsrtk/gpsrtk.go deleted file mode 100644 index 0534d4114be..00000000000 --- a/components/movementsensor/gpsrtk/gpsrtk.go +++ /dev/null @@ -1,888 +0,0 @@ -// Package gpsrtk defines a gps and an rtk correction source -// which sends rtcm data to a child gps -// This is an Experimental package -package gpsrtk - -import ( - "bufio" - "bytes" - "context" - "errors" - "fmt" - "io" - "math" - "strings" - "sync" - - "github.com/de-bkg/gognss/pkg/ntrip" - "github.com/edaniels/golog" - "github.com/go-gnss/rtcm/rtcm3" - "github.com/golang/geo/r3" - slib "github.com/jacobsa/go-serial/serial" - geo "github.com/kellydunn/golang-geo" - "go.viam.com/utils" - - "go.viam.com/rdk/components/board" - "go.viam.com/rdk/components/movementsensor" - gpsnmea "go.viam.com/rdk/components/movementsensor/gpsnmea" - "go.viam.com/rdk/resource" - "go.viam.com/rdk/spatialmath" -) - -var ( - errCorrectionSourceValidation = fmt.Errorf("only serial, i2c, and ntrip are supported correction sources for %s", roverModel.Name) - errConnectionTypeValidation = fmt.Errorf("only serial and i2c are supported connection types for %s", roverModel.Name) - errInputProtocolValidation = fmt.Errorf("only serial and i2c are supported input protocols for %s", roverModel.Name) -) - -var roverModel = resource.DefaultModelFamily.WithModel("gps-rtk") - -const ( - i2cStr = "i2c" - serialStr = "serial" - ntripStr = "ntrip" -) - -// Config is used for converting NMEA MovementSensor with RTK capabilities config attributes. -type Config struct { - CorrectionSource string `json:"correction_source"` - ConnectionType string `json:"connection_type,omitempty"` - - *SerialConfig `json:"serial_attributes,omitempty"` - *I2CConfig `json:"i2c_attributes,omitempty"` - *NtripConfig `json:"ntrip_attributes,omitempty"` -} - -// NtripConfig is used for converting attributes for a correction source. -type NtripConfig struct { - NtripAddr string `json:"ntrip_addr"` - NtripConnectAttempts int `json:"ntrip_connect_attempts,omitempty"` - NtripMountpoint string `json:"ntrip_mountpoint,omitempty"` - NtripPass string `json:"ntrip_password,omitempty"` - NtripUser string `json:"ntrip_username,omitempty"` - NtripPath string `json:"ntrip_path,omitempty"` - NtripBaud int `json:"ntrip_baud,omitempty"` - NtripInputProtocol string `json:"ntrip_input_protocol,omitempty"` -} - -// SerialConfig is used for converting attributes for a correction source. -type SerialConfig struct { - SerialPath string `json:"serial_path"` - SerialBaudRate int `json:"serial_baud_rate,omitempty"` - SerialCorrectionPath string `json:"serial_correction_path,omitempty"` - SerialCorrectionBaudRate int `json:"serial_correction_baud_rate,omitempty"` - - // TestChan is a fake "serial" path for test use only - TestChan chan []uint8 `json:"-"` -} - -// I2CConfig is used for converting attributes for a correction source. -type I2CConfig struct { - Board string `json:"board"` - I2CBus string `json:"i2c_bus"` - I2cAddr int `json:"i2c_addr"` - I2CBaudRate int `json:"i2c_baud_rate,omitempty"` -} - -// Validate ensures all parts of the config are valid. -func (cfg *Config) Validate(path string) ([]string, error) { - var deps []string - - dep, err := cfg.validateCorrectionSource(path) - if err != nil { - return nil, err - } - if dep != nil { - deps = append(deps, dep...) - } - - dep, err = cfg.validateConnectionType(path) - if err != nil { - return nil, err - } - if dep != nil { - deps = append(deps, dep...) - } - - if cfg.CorrectionSource == ntripStr { - dep, err = cfg.validateNtripInputProtocol(path) - if err != nil { - return nil, err - } - } - if dep != nil { - deps = append(deps, dep...) - } - - return deps, nil -} - -func (cfg *Config) validateCorrectionSource(path string) ([]string, error) { - var deps []string - switch cfg.CorrectionSource { - case ntripStr: - return nil, cfg.NtripConfig.ValidateNtrip(path) - case i2cStr: - if cfg.Board == "" { - return nil, utils.NewConfigValidationFieldRequiredError(path, "board") - } - deps = append(deps, cfg.Board) - return deps, cfg.I2CConfig.ValidateI2C(path) - case serialStr: - return nil, cfg.SerialConfig.ValidateSerial(path) - case "": - return nil, utils.NewConfigValidationFieldRequiredError(path, "correction_source") - default: - return nil, errCorrectionSourceValidation - } -} - -func (cfg *Config) validateConnectionType(path string) ([]string, error) { - var deps []string - switch strings.ToLower(cfg.ConnectionType) { - case i2cStr: - if cfg.Board == "" { - return nil, utils.NewConfigValidationFieldRequiredError(path, "board") - } - deps = append(deps, cfg.Board) - return deps, cfg.I2CConfig.ValidateI2C(path) - case serialStr: - return nil, cfg.SerialConfig.ValidateSerial(path) - case "": - return nil, utils.NewConfigValidationFieldRequiredError(path, "connection_type") - default: - return nil, errConnectionTypeValidation - } -} - -func (cfg *Config) validateNtripInputProtocol(path string) ([]string, error) { - var deps []string - switch cfg.NtripInputProtocol { - case i2cStr: - if cfg.Board == "" { - return nil, utils.NewConfigValidationFieldRequiredError(path, "board") - } - deps = append(deps, cfg.Board) - return deps, cfg.I2CConfig.ValidateI2C(path) - case serialStr: - return nil, cfg.SerialConfig.ValidateSerial(path) - default: - return nil, errInputProtocolValidation - } -} - -// ValidateI2C ensures all parts of the config are valid. -func (cfg *I2CConfig) ValidateI2C(path string) error { - if cfg.I2CBus == "" { - return utils.NewConfigValidationFieldRequiredError(path, "i2c_bus") - } - if cfg.I2cAddr == 0 { - return utils.NewConfigValidationFieldRequiredError(path, "i2c_addr") - } - return nil -} - -// ValidateSerial ensures all parts of the config are valid. -func (cfg *SerialConfig) ValidateSerial(path string) error { - if cfg.SerialPath == "" { - return utils.NewConfigValidationFieldRequiredError(path, "serial_path") - } - return nil -} - -// ValidateNtrip ensures all parts of the config are valid. -func (cfg *NtripConfig) ValidateNtrip(path string) error { - if cfg.NtripAddr == "" { - return utils.NewConfigValidationFieldRequiredError(path, "ntrip_addr") - } - if cfg.NtripInputProtocol == "" { - return utils.NewConfigValidationFieldRequiredError(path, "ntrip_input_protocol") - } - return nil -} - -func init() { - resource.RegisterComponent( - movementsensor.API, - roverModel, - resource.Registration[movementsensor.MovementSensor, *Config]{ - Constructor: newRTKMovementSensor, - }) -} - -// A RTKMovementSensor is an NMEA MovementSensor model that can intake RTK correction data. -type RTKMovementSensor struct { - resource.Named - resource.AlwaysRebuild - logger golog.Logger - cancelCtx context.Context - cancelFunc func() - - activeBackgroundWorkers sync.WaitGroup - - ntripMu sync.Mutex - ntripClient *NtripInfo - ntripStatus bool - - err movementsensor.LastError - lastposition movementsensor.LastPosition - - Nmeamovementsensor gpsnmea.NmeaMovementSensor - InputProtocol string - CorrectionWriter io.ReadWriteCloser - - Bus board.I2C - Wbaud int - Addr byte // for i2c only - Writepath string -} - -func newRTKMovementSensor( - ctx context.Context, - deps resource.Dependencies, - conf resource.Config, - logger golog.Logger, -) (movementsensor.MovementSensor, error) { - newConf, err := resource.NativeConfig[*Config](conf) - if err != nil { - return nil, err - } - - cancelCtx, cancelFunc := context.WithCancel(context.Background()) - g := &RTKMovementSensor{ - Named: conf.ResourceName().AsNamed(), - cancelCtx: cancelCtx, - cancelFunc: cancelFunc, - logger: logger, - err: movementsensor.NewLastError(1, 1), - lastposition: movementsensor.NewLastPosition(), - } - - if newConf.CorrectionSource == ntripStr { - g.InputProtocol = strings.ToLower(newConf.NtripInputProtocol) - } else { - g.InputProtocol = newConf.CorrectionSource - } - - nmeaConf := &gpsnmea.Config{ - ConnectionType: newConf.ConnectionType, - DisableNMEA: false, - } - - // Init NMEAMovementSensor - switch strings.ToLower(newConf.ConnectionType) { - case serialStr: - var err error - nmeaConf.SerialConfig = (*gpsnmea.SerialConfig)(newConf.SerialConfig) - g.Nmeamovementsensor, err = gpsnmea.NewSerialGPSNMEA(ctx, conf.ResourceName(), nmeaConf, logger) - if err != nil { - return nil, err - } - case i2cStr: - var err error - nmeaConf.Board = newConf.I2CConfig.Board - nmeaConf.I2CConfig = &gpsnmea.I2CConfig{I2CBus: newConf.I2CBus, I2CBaudRate: newConf.I2CBaudRate, I2cAddr: newConf.I2cAddr} - g.Nmeamovementsensor, err = gpsnmea.NewPmtkI2CGPSNMEA(ctx, deps, conf.ResourceName(), nmeaConf, logger) - if err != nil { - return nil, err - } - default: - // Invalid protocol - return nil, fmt.Errorf("%s is not a valid connection type", newConf.ConnectionType) - } - - // Init ntripInfo from attributes - g.ntripClient, err = newNtripInfo(newConf.NtripConfig, g.logger) - if err != nil { - return nil, err - } - - // baud rate - if newConf.NtripBaud == 0 { - newConf.NtripBaud = 38400 - g.logger.Info("ntrip_baud using default baud rate 38400") - } - g.Wbaud = newConf.NtripBaud - - switch g.InputProtocol { - case serialStr: - switch newConf.NtripPath { - case "": - g.logger.Info("RTK will use the same serial path as the GPS data to write RCTM messages") - g.Writepath = newConf.SerialPath - default: - g.Writepath = newConf.NtripPath - } - case i2cStr: - g.Addr = byte(newConf.I2cAddr) - - b, err := board.FromDependencies(deps, newConf.Board) - if err != nil { - return nil, fmt.Errorf("gps init: failed to find board: %w", err) - } - localB, ok := b.(board.LocalBoard) - if !ok { - return nil, fmt.Errorf("board %s is not local", newConf.Board) - } - - i2cbus, ok := localB.I2CByName(newConf.I2CBus) - if !ok { - return nil, fmt.Errorf("gps init: failed to find i2c bus %s", newConf.I2CBus) - } - g.Bus = i2cbus - } - - if err := g.start(); err != nil { - return nil, err - } - return g, g.err.Get() -} - -// Start begins NTRIP receiver with specified protocol and begins reading/updating MovementSensor measurements. -func (g *RTKMovementSensor) start() error { - // TODO(RDK-1639): Test out what happens if we call this line and then the ReceiveAndWrite* - // correction data goes wrong. Could anything worse than uncorrected data occur? - if err := g.Nmeamovementsensor.Start(g.cancelCtx); err != nil { - g.lastposition.GetLastPosition() - return err - } - - switch g.InputProtocol { - case serialStr: - g.activeBackgroundWorkers.Add(1) - utils.PanicCapturingGo(g.receiveAndWriteSerial) - case i2cStr: - g.activeBackgroundWorkers.Add(1) - utils.PanicCapturingGo(func() { g.receiveAndWriteI2C(g.cancelCtx) }) - } - - return g.err.Get() -} - -// Connect attempts to connect to ntrip client until successful connection or timeout. -func (g *RTKMovementSensor) Connect(casterAddr, user, pwd string, maxAttempts int) error { - attempts := 0 - - var c *ntrip.Client - var err error - - g.logger.Debug("Connecting to NTRIP caster") - for attempts < maxAttempts { - select { - case <-g.cancelCtx.Done(): - return g.cancelCtx.Err() - default: - } - - c, err = ntrip.NewClient(casterAddr, ntrip.Options{Username: user, Password: pwd}) - if err == nil { - break - } - - attempts++ - } - - if err != nil { - g.logger.Errorf("Can't connect to NTRIP caster: %s", err) - return err - } - - g.logger.Debug("Connected to NTRIP caster") - g.ntripMu.Lock() - g.ntripClient.Client = c - g.ntripMu.Unlock() - return g.err.Get() -} - -// GetStream attempts to connect to ntrip streak until successful connection or timeout. -func (g *RTKMovementSensor) GetStream(mountPoint string, maxAttempts int) error { - success := false - attempts := 0 - - var rc io.ReadCloser - var err error - - g.logger.Debug("Getting NTRIP stream") - - for !success && attempts < maxAttempts { - select { - case <-g.cancelCtx.Done(): - return errors.New("Canceled") - default: - } - - rc, err = func() (io.ReadCloser, error) { - g.ntripMu.Lock() - defer g.ntripMu.Unlock() - return g.ntripClient.Client.GetStream(mountPoint) - }() - if err == nil { - success = true - } - attempts++ - } - - if err != nil { - g.logger.Errorf("Can't connect to NTRIP stream: %s", err) - return err - } - - g.logger.Debug("Connected to stream") - g.ntripMu.Lock() - defer g.ntripMu.Unlock() - - g.ntripClient.Stream = rc - return g.err.Get() -} - -// receiveAndWriteI2C connects to NTRIP receiver and sends correction stream to the MovementSensor through I2C protocol. -func (g *RTKMovementSensor) receiveAndWriteI2C(ctx context.Context) { - defer g.activeBackgroundWorkers.Done() - if err := g.cancelCtx.Err(); err != nil { - return - } - err := g.Connect(g.ntripClient.URL, g.ntripClient.Username, g.ntripClient.Password, g.ntripClient.MaxConnectAttempts) - if err != nil { - g.err.Set(err) - return - } - - if !g.ntripClient.Client.IsCasterAlive() { - g.logger.Infof("caster %s seems to be down", g.ntripClient.URL) - } - - // establish I2C connection - handle, err := g.Bus.OpenHandle(g.Addr) - if err != nil { - g.logger.Errorf("can't open gps i2c %s", err) - g.err.Set(err) - return - } - // Send GLL, RMC, VTG, GGA, GSA, and GSV sentences each 1000ms - baudcmd := fmt.Sprintf("PMTK251,%d", g.Wbaud) - cmd251 := addChk([]byte(baudcmd)) - cmd314 := addChk([]byte("PMTK314,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0")) - cmd220 := addChk([]byte("PMTK220,1000")) - - err = handle.Write(ctx, cmd251) - if err != nil { - g.logger.Debug("Failed to set baud rate") - } - err = handle.Write(ctx, cmd314) - if err != nil { - g.logger.Debug("failed to set NMEA output") - g.err.Set(err) - return - } - err = handle.Write(ctx, cmd220) - if err != nil { - g.logger.Debug("failed to set NMEA update rate") - g.err.Set(err) - return - } - - err = g.GetStream(g.ntripClient.MountPoint, g.ntripClient.MaxConnectAttempts) - if err != nil { - g.err.Set(err) - return - } - - // create a buffer - w := &bytes.Buffer{} - r := io.TeeReader(g.ntripClient.Stream, w) - - buf := make([]byte, 1100) - n, err := g.ntripClient.Stream.Read(buf) - if err != nil { - g.err.Set(err) - return - } - wI2C := addChk(buf[:n]) - - // port still open - err = handle.Write(ctx, wI2C) - if err != nil { - g.logger.Errorf("i2c handle write failed %s", err) - g.err.Set(err) - return - } - - scanner := rtcm3.NewScanner(r) - - g.ntripMu.Lock() - g.ntripStatus = true - g.ntripMu.Unlock() - - // It's okay to skip the mutex on this next line: g.ntripStatus can only be mutated by this - // goroutine itself. - for g.ntripStatus { - select { - case <-g.cancelCtx.Done(): - g.err.Set(err) - return - default: - } - - // establish I2C connection - handle, err := g.Bus.OpenHandle(g.Addr) - if err != nil { - g.logger.Errorf("can't open gps i2c %s", err) - g.err.Set(err) - return - } - - msg, err := scanner.NextMessage() - if err != nil { - g.ntripMu.Lock() - g.ntripStatus = false - g.ntripMu.Unlock() - - if msg == nil { - g.logger.Debug("No message... reconnecting to stream...") - err = g.GetStream(g.ntripClient.MountPoint, g.ntripClient.MaxConnectAttempts) - if err != nil { - g.err.Set(err) - return - } - - w = &bytes.Buffer{} - r = io.TeeReader(g.ntripClient.Stream, w) - - buf = make([]byte, 1100) - n, err := g.ntripClient.Stream.Read(buf) - if err != nil { - g.err.Set(err) - return - } - wI2C := addChk(buf[:n]) - - err = handle.Write(ctx, wI2C) - - if err != nil { - g.logger.Errorf("i2c handle write failed %s", err) - g.err.Set(err) - return - } - - scanner = rtcm3.NewScanner(r) - g.ntripMu.Lock() - g.ntripStatus = true - g.ntripMu.Unlock() - continue - } - } - // close I2C - err = handle.Close() - if err != nil { - g.logger.Debug("failed to close handle: %s", err) - g.err.Set(err) - return - } - } -} - -// receiveAndWriteSerial connects to NTRIP receiver and sends correction stream to the MovementSensor through serial. -func (g *RTKMovementSensor) receiveAndWriteSerial() { - defer g.activeBackgroundWorkers.Done() - if err := g.cancelCtx.Err(); err != nil { - return - } - err := g.Connect(g.ntripClient.URL, g.ntripClient.Username, g.ntripClient.Password, g.ntripClient.MaxConnectAttempts) - if err != nil { - g.err.Set(err) - return - } - - if !g.ntripClient.Client.IsCasterAlive() { - g.logger.Infof("caster %s seems to be down", g.ntripClient.URL) - } - - options := slib.OpenOptions{ - PortName: g.Writepath, - BaudRate: uint(g.Wbaud), - DataBits: 8, - StopBits: 1, - MinimumReadSize: 1, - } - - // Open the port. - g.ntripMu.Lock() - if err := g.cancelCtx.Err(); err != nil { - g.ntripMu.Unlock() - return - } - g.CorrectionWriter, err = slib.Open(options) - g.ntripMu.Unlock() - if err != nil { - g.logger.Errorf("serial.Open: %v", err) - g.err.Set(err) - return - } - - w := bufio.NewWriter(g.CorrectionWriter) - - err = g.GetStream(g.ntripClient.MountPoint, g.ntripClient.MaxConnectAttempts) - if err != nil { - g.err.Set(err) - return - } - - r := io.TeeReader(g.ntripClient.Stream, w) - scanner := rtcm3.NewScanner(r) - - g.ntripMu.Lock() - g.ntripStatus = true - g.ntripMu.Unlock() - - // It's okay to skip the mutex on this next line: g.ntripStatus can only be mutated by this - // goroutine itself. - for g.ntripStatus { - select { - case <-g.cancelCtx.Done(): - return - default: - } - - msg, err := scanner.NextMessage() - if err != nil { - g.ntripMu.Lock() - g.ntripStatus = false - g.ntripMu.Unlock() - - if msg == nil { - g.logger.Debug("No message... reconnecting to stream...") - err = g.GetStream(g.ntripClient.MountPoint, g.ntripClient.MaxConnectAttempts) - if err != nil { - g.err.Set(err) - return - } - - r = io.TeeReader(g.ntripClient.Stream, w) - scanner = rtcm3.NewScanner(r) - g.ntripMu.Lock() - g.ntripStatus = true - g.ntripMu.Unlock() - continue - } - } - } -} - -// NtripStatus returns true if connection to NTRIP stream is OK, false if not. -func (g *RTKMovementSensor) NtripStatus() (bool, error) { - g.ntripMu.Lock() - defer g.ntripMu.Unlock() - return g.ntripStatus, g.err.Get() -} - -// Position returns the current geographic location of the MOVEMENTSENSOR. -func (g *RTKMovementSensor) Position(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { - g.ntripMu.Lock() - lastError := g.err.Get() - if lastError != nil { - lastPosition := g.lastposition.GetLastPosition() - g.ntripMu.Unlock() - if lastPosition != nil { - return lastPosition, 0, nil - } - return geo.NewPoint(math.NaN(), math.NaN()), math.NaN(), lastError - } - g.ntripMu.Unlock() - - position, alt, err := g.Nmeamovementsensor.Position(ctx, extra) - if err != nil { - // Use the last known valid position if current position is (0,0)/ NaN. - if position != nil && (g.lastposition.IsZeroPosition(position) || g.lastposition.IsPositionNaN(position)) { - lastPosition := g.lastposition.GetLastPosition() - if lastPosition != nil { - return lastPosition, alt, nil - } - } - return geo.NewPoint(math.NaN(), math.NaN()), math.NaN(), err - } - - // Check if the current position is different from the last position and non-zero - lastPosition := g.lastposition.GetLastPosition() - if !g.lastposition.ArePointsEqual(position, lastPosition) { - g.lastposition.SetLastPosition(position) - } - - // Update the last known valid position if the current position is non-zero - if position != nil && !g.lastposition.IsZeroPosition(position) { - g.lastposition.SetLastPosition(position) - } - - return position, alt, nil -} - -// LinearVelocity passthrough. -func (g *RTKMovementSensor) LinearVelocity(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { - g.ntripMu.Lock() - lastError := g.err.Get() - if lastError != nil { - defer g.ntripMu.Unlock() - return r3.Vector{}, lastError - } - g.ntripMu.Unlock() - - return g.Nmeamovementsensor.LinearVelocity(ctx, extra) -} - -// LinearAcceleration passthrough. -func (g *RTKMovementSensor) LinearAcceleration(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { - lastError := g.err.Get() - if lastError != nil { - return r3.Vector{}, lastError - } - return g.Nmeamovementsensor.LinearAcceleration(ctx, extra) -} - -// AngularVelocity passthrough. -func (g *RTKMovementSensor) AngularVelocity(ctx context.Context, extra map[string]interface{}) (spatialmath.AngularVelocity, error) { - g.ntripMu.Lock() - lastError := g.err.Get() - if lastError != nil { - defer g.ntripMu.Unlock() - return spatialmath.AngularVelocity{}, lastError - } - g.ntripMu.Unlock() - - return g.Nmeamovementsensor.AngularVelocity(ctx, extra) -} - -// CompassHeading passthrough. -func (g *RTKMovementSensor) CompassHeading(ctx context.Context, extra map[string]interface{}) (float64, error) { - g.ntripMu.Lock() - lastError := g.err.Get() - if lastError != nil { - defer g.ntripMu.Unlock() - return 0, lastError - } - g.ntripMu.Unlock() - - return g.Nmeamovementsensor.CompassHeading(ctx, extra) -} - -// Orientation passthrough. -func (g *RTKMovementSensor) Orientation(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) { - g.ntripMu.Lock() - lastError := g.err.Get() - if lastError != nil { - defer g.ntripMu.Unlock() - return spatialmath.NewZeroOrientation(), lastError - } - g.ntripMu.Unlock() - - return g.Nmeamovementsensor.Orientation(ctx, extra) -} - -// ReadFix passthrough. -func (g *RTKMovementSensor) ReadFix(ctx context.Context) (int, error) { - g.ntripMu.Lock() - lastError := g.err.Get() - if lastError != nil { - defer g.ntripMu.Unlock() - return 0, lastError - } - g.ntripMu.Unlock() - - return g.Nmeamovementsensor.ReadFix(ctx) -} - -// Properties passthrough. -func (g *RTKMovementSensor) Properties(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { - lastError := g.err.Get() - if lastError != nil { - return &movementsensor.Properties{}, lastError - } - - return g.Nmeamovementsensor.Properties(ctx, extra) -} - -// Accuracy passthrough. -func (g *RTKMovementSensor) Accuracy(ctx context.Context, extra map[string]interface{}) (map[string]float32, error) { - lastError := g.err.Get() - if lastError != nil { - return map[string]float32{}, lastError - } - - return g.Nmeamovementsensor.Accuracy(ctx, extra) -} - -// Readings will use the default MovementSensor Readings if not provided. -func (g *RTKMovementSensor) Readings(ctx context.Context, extra map[string]interface{}) (map[string]interface{}, error) { - readings, err := movementsensor.Readings(ctx, g, extra) - if err != nil { - return nil, err - } - - fix, err := g.ReadFix(ctx) - if err != nil { - return nil, err - } - - readings["fix"] = fix - - return readings, nil -} - -// Close shuts down the RTKMOVEMENTSENSOR. -func (g *RTKMovementSensor) Close(ctx context.Context) error { - g.ntripMu.Lock() - g.cancelFunc() - - if err := g.Nmeamovementsensor.Close(ctx); err != nil { - g.ntripMu.Unlock() - return err - } - - // close ntrip writer - if g.CorrectionWriter != nil { - if err := g.CorrectionWriter.Close(); err != nil { - g.ntripMu.Unlock() - return err - } - g.CorrectionWriter = nil - } - - // close ntrip client and stream - if g.ntripClient.Client != nil { - g.ntripClient.Client.CloseIdleConnections() - g.ntripClient.Client = nil - } - - if g.ntripClient.Stream != nil { - if err := g.ntripClient.Stream.Close(); err != nil { - g.ntripMu.Unlock() - return err - } - g.ntripClient.Stream = nil - } - - g.ntripMu.Unlock() - g.activeBackgroundWorkers.Wait() - - if err := g.err.Get(); err != nil && !errors.Is(err, context.Canceled) { - return err - } - return nil -} - -// TODO: move these to utils -// PMTK checksums commands by XORing together each byte. -func addChk(data []byte) []byte { - chk := checksum(data) - newCmd := []byte("$") - newCmd = append(newCmd, data...) - newCmd = append(newCmd, []byte("*")...) - newCmd = append(newCmd, chk) - return newCmd -} - -func checksum(data []byte) byte { - var chk byte - for _, b := range data { - chk ^= b - } - return chk -} diff --git a/components/movementsensor/gpsrtkpmtk/gpsrtkpmtk.go b/components/movementsensor/gpsrtkpmtk/gpsrtkpmtk.go new file mode 100644 index 00000000000..9d131806441 --- /dev/null +++ b/components/movementsensor/gpsrtkpmtk/gpsrtkpmtk.go @@ -0,0 +1,684 @@ +// Package gpsrtkpmtk implements a gps using serial connection +package gpsrtkpmtk + +/* + This package supports GPS RTK (Real Time Kinematics), which takes in the normal signals + from the GNSS (Global Navigation Satellite Systems) along with a correction stream to achieve + positional accuracy (accuracy tbd), over I2C bus. + + Example GPS RTK chip datasheet: + https://content.u-blox.com/sites/default/files/ZED-F9P-04B_DataSheet_UBX-21044850.pdf + + Example configuration: + + { + "name": "my-gps-rtk", + "type": "movement_sensor", + "model": "gps-nmea-rtk-pmtk", + "attributes": { + "board": "local", + "i2c_addr": 66, + "i2c_baud_rate": 115200, + "i2c_bus": "default_bus", + "ntrip_connect_attempts": 12, + "ntrip_mountpoint": "MNTPT", + "ntrip_password": "pass", + "ntrip_url": "http://ntrip/url", + "ntrip_username": "usr" + }, + "depends_on": [], + } + +*/ + +import ( + "bytes" + "context" + "errors" + "fmt" + "io" + "math" + "sync" + + "github.com/de-bkg/gognss/pkg/ntrip" + "github.com/edaniels/golog" + "github.com/go-gnss/rtcm/rtcm3" + "github.com/golang/geo/r3" + geo "github.com/kellydunn/golang-geo" + "go.viam.com/utils" + + "go.viam.com/rdk/components/board" + "go.viam.com/rdk/components/movementsensor" + gpsnmea "go.viam.com/rdk/components/movementsensor/gpsnmea" + rtk "go.viam.com/rdk/components/movementsensor/rtkutils" + "go.viam.com/rdk/resource" + "go.viam.com/rdk/spatialmath" +) + +var rtkmodel = resource.DefaultModelFamily.WithModel("gps-nmea-rtk-pmtk") + +const i2cStr = "i2c" + +// Config is used for converting NMEA MovementSensor with RTK capabilities config attributes. +type Config struct { + Board string `json:"board"` + I2CBus string `json:"i2c_bus"` + I2CAddr int `json:"i2c_addr"` + I2CBaudRate int `json:"i2c_baud_rate,omitempty"` + + NtripURL string `json:"ntrip_url"` + NtripConnectAttempts int `json:"ntrip_connect_attempts,omitempty"` + NtripMountpoint string `json:"ntrip_mountpoint,omitempty"` + NtripPass string `json:"ntrip_password,omitempty"` + NtripUser string `json:"ntrip_username,omitempty"` +} + +// Validate ensures all parts of the config are valid. +func (cfg *Config) Validate(path string) ([]string, error) { + var deps []string + + err := cfg.validateI2C(path) + if err != nil { + return nil, err + } + + err = cfg.validateNtrip(path) + if err != nil { + return nil, err + } + + deps = append(deps, cfg.Board) + return deps, nil +} + +// validateI2C ensures all parts of the config are valid. +func (cfg *Config) validateI2C(path string) error { + if cfg.I2CBus == "" { + return utils.NewConfigValidationFieldRequiredError(path, "i2c_bus") + } + if cfg.I2CAddr == 0 { + return utils.NewConfigValidationFieldRequiredError(path, "i2c_addr") + } + return nil +} + +// validateNtrip ensures all parts of the config are valid. +func (cfg *Config) validateNtrip(path string) error { + if cfg.NtripURL == "" { + return utils.NewConfigValidationFieldRequiredError(path, "ntrip_url") + } + return nil +} + +func init() { + resource.RegisterComponent( + movementsensor.API, + rtkmodel, + resource.Registration[movementsensor.MovementSensor, *Config]{ + Constructor: newRTKI2C, + }) +} + +// rtkI2C is an nmea movementsensor model that can intake RTK correction data via I2C. +type rtkI2C struct { + resource.Named + resource.AlwaysRebuild + logger golog.Logger + cancelCtx context.Context + cancelFunc func() + + activeBackgroundWorkers sync.WaitGroup + + mu sync.Mutex + ntripMu sync.Mutex + ntripconfigMu sync.Mutex + ntripClient *rtk.NtripInfo + ntripStatus bool + + err movementsensor.LastError + lastposition movementsensor.LastPosition + + nmeamovementsensor gpsnmea.NmeaMovementSensor + correctionWriter io.ReadWriteCloser + + bus board.I2C + wbaud int + addr byte +} + +// Reconfigure reconfigures attributes. +func (g *rtkI2C) Reconfigure(ctx context.Context, deps resource.Dependencies, conf resource.Config) error { + g.mu.Lock() + defer g.mu.Unlock() + newConf, err := resource.NativeConfig[*Config](conf) + if err != nil { + return err + } + + if newConf.I2CBaudRate == 0 { + g.wbaud = 115200 + } else { + g.wbaud = newConf.I2CBaudRate + } + + g.addr = byte(newConf.I2CAddr) + + b, err := board.FromDependencies(deps, newConf.Board) + if err != nil { + return fmt.Errorf("gps init: failed to find board: %w", err) + } + localB, ok := b.(board.LocalBoard) + if !ok { + return fmt.Errorf("board %s is not local", newConf.Board) + } + + i2cbus, ok := localB.I2CByName(newConf.I2CBus) + if !ok { + return fmt.Errorf("gps init: failed to find i2c bus %s", newConf.I2CBus) + } + g.bus = i2cbus + + g.ntripconfigMu.Lock() + ntripConfig := &rtk.NtripConfig{ + NtripURL: newConf.NtripURL, + NtripUser: newConf.NtripUser, + NtripPass: newConf.NtripPass, + NtripMountpoint: newConf.NtripMountpoint, + NtripConnectAttempts: newConf.NtripConnectAttempts, + } + + // Init ntripInfo from attributes + tempNtripClient, err := rtk.NewNtripInfo(ntripConfig, g.logger) + if err != nil { + return err + } + + if g.ntripClient == nil { + g.ntripClient = tempNtripClient + } else { + tempNtripClient.Client = g.ntripClient.Client + tempNtripClient.Stream = g.ntripClient.Stream + + g.ntripClient = tempNtripClient + } + + g.ntripconfigMu.Unlock() + + g.logger.Debug("done reconfiguring") + + return nil +} + +func newRTKI2C( + ctx context.Context, + deps resource.Dependencies, + conf resource.Config, + logger golog.Logger, +) (movementsensor.MovementSensor, error) { + newConf, err := resource.NativeConfig[*Config](conf) + if err != nil { + return nil, err + } + + cancelCtx, cancelFunc := context.WithCancel(context.Background()) + g := &rtkI2C{ + Named: conf.ResourceName().AsNamed(), + cancelCtx: cancelCtx, + cancelFunc: cancelFunc, + logger: logger, + err: movementsensor.NewLastError(1, 1), + lastposition: movementsensor.NewLastPosition(), + } + + // reconfigure + if err = g.Reconfigure(ctx, deps, conf); err != nil { + return nil, err + } + + nmeaConf := &gpsnmea.Config{ + ConnectionType: i2cStr, + } + + // Init NMEAMovementSensor + nmeaConf.I2CConfig = &gpsnmea.I2CConfig{ + Board: newConf.Board, + I2CBus: newConf.I2CBus, + I2CBaudRate: newConf.I2CBaudRate, + I2CAddr: newConf.I2CAddr, + } + + if nmeaConf.I2CConfig.I2CBaudRate == 0 { + nmeaConf.I2CConfig.I2CBaudRate = 115200 + } + + g.nmeamovementsensor, err = gpsnmea.NewPmtkI2CGPSNMEA(ctx, deps, conf.ResourceName(), nmeaConf, logger) + if err != nil { + return nil, err + } + + if err := g.start(); err != nil { + return nil, err + } + + return g, g.err.Get() +} + +// Start begins NTRIP receiver with i2c protocol and begins reading/updating MovementSensor measurements. +func (g *rtkI2C) start() error { + // TODO(RDK-1639): Test out what happens if we call this line and then the ReceiveAndWrite* + // correction data goes wrong. Could anything worse than uncorrected data occur? + + if err := g.nmeamovementsensor.Start(g.cancelCtx); err != nil { + g.lastposition.GetLastPosition() + return err + } + + g.activeBackgroundWorkers.Add(1) + utils.PanicCapturingGo(func() { g.receiveAndWriteI2C(g.cancelCtx) }) + + return g.err.Get() +} + +// connect attempts to connect to ntrip client until successful connection or timeout. +func (g *rtkI2C) connect(casterAddr, user, pwd string, maxAttempts int) error { + g.logger.Info("starting connect") + for attempts := 0; attempts < maxAttempts; attempts++ { + ntripclient, err := ntrip.NewClient(casterAddr, ntrip.Options{Username: user, Password: pwd}) + if err == nil { + g.logger.Debug("Connected to NTRIP caster") + g.ntripMu.Lock() + g.ntripClient.Client = ntripclient + g.ntripMu.Unlock() + return g.err.Get() + } + } + + errMsg := fmt.Sprintf("Can't connect to NTRIP caster after %d attempts", maxAttempts) + return errors.New(errMsg) +} + +// getStream attempts to connect to ntrip stream until successful connection or timeout. +func (g *rtkI2C) getStream(mountPoint string, maxAttempts int) error { + success := false + attempts := 0 + + var rc io.ReadCloser + var err error + + g.logger.Debug("Getting NTRIP stream") + + for !success && attempts < maxAttempts { + select { + case <-g.cancelCtx.Done(): + return errors.New("Canceled") + default: + } + + rc, err = func() (io.ReadCloser, error) { + g.ntripMu.Lock() + defer g.ntripMu.Unlock() + return g.ntripClient.Client.GetStream(mountPoint) + }() + if err == nil { + success = true + } + attempts++ + } + + if err != nil { + g.logger.Errorf("Can't connect to NTRIP stream: %s", err) + return err + } + + g.logger.Debug("Connected to stream") + g.ntripMu.Lock() + defer g.ntripMu.Unlock() + + g.ntripClient.Stream = rc + return g.err.Get() +} + +// receiveAndWriteI2C connects to NTRIP receiver and sends correction stream to the MovementSensor through I2C protocol. +func (g *rtkI2C) receiveAndWriteI2C(ctx context.Context) { + defer g.activeBackgroundWorkers.Done() + if err := g.cancelCtx.Err(); err != nil { + return + } + err := g.connect(g.ntripClient.URL, g.ntripClient.Username, g.ntripClient.Password, g.ntripClient.MaxConnectAttempts) + if err != nil { + g.err.Set(err) + return + } + + if !g.ntripClient.Client.IsCasterAlive() { + g.logger.Infof("caster %s seems to be down", g.ntripClient.URL) + } + + // establish I2C connection + handle, err := g.bus.OpenHandle(g.addr) + if err != nil { + g.logger.Errorf("can't open gps i2c %s", err) + g.err.Set(err) + return + } + + // Send GLL, RMC, VTG, GGA, GSA, and GSV sentences each 1000ms + baudcmd := fmt.Sprintf("PMTK251,%d", g.wbaud) + cmd251 := movementsensor.PMTKAddChk([]byte(baudcmd)) + cmd314 := movementsensor.PMTKAddChk([]byte("PMTK314,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0")) + cmd220 := movementsensor.PMTKAddChk([]byte("PMTK220,1000")) + + err = handle.Write(ctx, cmd251) + if err != nil { + g.logger.Debug("Failed to set baud rate") + } + + err = handle.Write(ctx, cmd314) + if err != nil { + g.logger.Debug("failed to set NMEA output") + g.err.Set(err) + return + } + + err = handle.Write(ctx, cmd220) + if err != nil { + g.logger.Debug("failed to set NMEA update rate") + g.err.Set(err) + return + } + + err = g.getStream(g.ntripClient.MountPoint, g.ntripClient.MaxConnectAttempts) + if err != nil { + g.err.Set(err) + return + } + + // create a buffer + w := &bytes.Buffer{} + r := io.TeeReader(g.ntripClient.Stream, w) + + buf := make([]byte, 1100) + n, err := g.ntripClient.Stream.Read(buf) + if err != nil { + g.err.Set(err) + return + } + + wI2C := movementsensor.PMTKAddChk(buf[:n]) + + // port still open + err = handle.Write(ctx, wI2C) + if err != nil { + g.logger.Errorf("i2c handle write failed %s", err) + g.err.Set(err) + return + } + + scanner := rtcm3.NewScanner(r) + + g.ntripMu.Lock() + g.ntripStatus = true + g.ntripMu.Unlock() + + // It's okay to skip the mutex on this next line: g.ntripStatus can only be mutated by this + // goroutine itself. + for g.ntripStatus { + select { + case <-g.cancelCtx.Done(): + g.err.Set(err) + return + default: + } + + // establish I2C connection + handle, err := g.bus.OpenHandle(g.addr) + if err != nil { + g.logger.Errorf("can't open gps i2c %s", err) + g.err.Set(err) + return + } + + msg, err := scanner.NextMessage() + if err != nil { + g.ntripMu.Lock() + g.ntripStatus = false + g.ntripMu.Unlock() + + if msg == nil { + g.logger.Debug("No message... reconnecting to stream...") + err = g.getStream(g.ntripClient.MountPoint, g.ntripClient.MaxConnectAttempts) + if err != nil { + g.err.Set(err) + return + } + + w = &bytes.Buffer{} + r = io.TeeReader(g.ntripClient.Stream, w) + + buf = make([]byte, 1100) + n, err := g.ntripClient.Stream.Read(buf) + if err != nil { + g.err.Set(err) + return + } + wI2C := movementsensor.PMTKAddChk(buf[:n]) + + err = handle.Write(ctx, wI2C) + + if err != nil { + g.logger.Errorf("i2c handle write failed %s", err) + g.err.Set(err) + return + } + + scanner = rtcm3.NewScanner(r) + g.ntripMu.Lock() + g.ntripStatus = true + g.ntripMu.Unlock() + continue + } + } + // close I2C + err = handle.Close() + if err != nil { + g.logger.Debug("failed to close handle: %s", err) + g.err.Set(err) + return + } + } +} + +//nolint +// getNtripConnectionStatus returns true if connection to NTRIP stream is OK, false if not +func (g *rtkI2C) getNtripConnectionStatus() (bool, error) { + g.ntripMu.Lock() + defer g.ntripMu.Unlock() + return g.ntripStatus, g.err.Get() +} + +// Position returns the current geographic location of the MOVEMENTSENSOR. +func (g *rtkI2C) Position(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { + g.ntripMu.Lock() + lastError := g.err.Get() + if lastError != nil { + lastPosition := g.lastposition.GetLastPosition() + g.ntripMu.Unlock() + if lastPosition != nil { + return lastPosition, 0, nil + } + return geo.NewPoint(math.NaN(), math.NaN()), math.NaN(), lastError + } + g.ntripMu.Unlock() + + position, alt, err := g.nmeamovementsensor.Position(ctx, extra) + if err != nil { + // Use the last known valid position if current position is (0,0)/ NaN. + if position != nil && (g.lastposition.IsZeroPosition(position) || g.lastposition.IsPositionNaN(position)) { + lastPosition := g.lastposition.GetLastPosition() + if lastPosition != nil { + return lastPosition, alt, nil + } + } + return geo.NewPoint(math.NaN(), math.NaN()), math.NaN(), err + } + + if g.lastposition.IsPositionNaN(position) { + position = g.lastposition.GetLastPosition() + } + + return position, alt, nil +} + +// LinearVelocity passthrough. +func (g *rtkI2C) LinearVelocity(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { + g.ntripMu.Lock() + lastError := g.err.Get() + if lastError != nil { + defer g.ntripMu.Unlock() + return r3.Vector{}, lastError + } + g.ntripMu.Unlock() + + return g.nmeamovementsensor.LinearVelocity(ctx, extra) +} + +// LinearAcceleration passthrough. +func (g *rtkI2C) LinearAcceleration(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { + lastError := g.err.Get() + if lastError != nil { + return r3.Vector{}, lastError + } + return g.nmeamovementsensor.LinearAcceleration(ctx, extra) +} + +// AngularVelocity passthrough. +func (g *rtkI2C) AngularVelocity(ctx context.Context, extra map[string]interface{}) (spatialmath.AngularVelocity, error) { + g.ntripMu.Lock() + lastError := g.err.Get() + if lastError != nil { + defer g.ntripMu.Unlock() + return spatialmath.AngularVelocity{}, lastError + } + g.ntripMu.Unlock() + + return g.nmeamovementsensor.AngularVelocity(ctx, extra) +} + +// CompassHeading passthrough. +func (g *rtkI2C) CompassHeading(ctx context.Context, extra map[string]interface{}) (float64, error) { + g.ntripMu.Lock() + lastError := g.err.Get() + if lastError != nil { + defer g.ntripMu.Unlock() + return 0, lastError + } + g.ntripMu.Unlock() + + return g.nmeamovementsensor.CompassHeading(ctx, extra) +} + +// Orientation passthrough. +func (g *rtkI2C) Orientation(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) { + g.ntripMu.Lock() + lastError := g.err.Get() + if lastError != nil { + defer g.ntripMu.Unlock() + return spatialmath.NewZeroOrientation(), lastError + } + g.ntripMu.Unlock() + + return g.nmeamovementsensor.Orientation(ctx, extra) +} + +// readFix passthrough. +func (g *rtkI2C) readFix(ctx context.Context) (int, error) { + g.ntripMu.Lock() + lastError := g.err.Get() + if lastError != nil { + defer g.ntripMu.Unlock() + return 0, lastError + } + g.ntripMu.Unlock() + + return g.nmeamovementsensor.ReadFix(ctx) +} + +// Properties passthrough. +func (g *rtkI2C) Properties(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { + lastError := g.err.Get() + if lastError != nil { + return &movementsensor.Properties{}, lastError + } + + return g.nmeamovementsensor.Properties(ctx, extra) +} + +// Accuracy passthrough. +func (g *rtkI2C) Accuracy(ctx context.Context, extra map[string]interface{}) (map[string]float32, error) { + lastError := g.err.Get() + if lastError != nil { + return map[string]float32{}, lastError + } + + return g.nmeamovementsensor.Accuracy(ctx, extra) +} + +// Readings will use the default MovementSensor Readings if not provided. +func (g *rtkI2C) Readings(ctx context.Context, extra map[string]interface{}) (map[string]interface{}, error) { + readings, err := movementsensor.Readings(ctx, g, extra) + if err != nil { + return nil, err + } + + fix, err := g.readFix(ctx) + if err != nil { + return nil, err + } + + readings["fix"] = fix + + return readings, nil +} + +// Close shuts down the RTKMOVEMENTSENSOR. +func (g *rtkI2C) Close(ctx context.Context) error { + g.ntripMu.Lock() + g.cancelFunc() + + if err := g.nmeamovementsensor.Close(ctx); err != nil { + g.ntripMu.Unlock() + return err + } + + // close ntrip writer + if g.correctionWriter != nil { + if err := g.correctionWriter.Close(); err != nil { + g.ntripMu.Unlock() + return err + } + g.correctionWriter = nil + } + + // close ntrip client and stream + if g.ntripClient.Client != nil { + g.ntripClient.Client.CloseIdleConnections() + g.ntripClient.Client = nil + } + + if g.ntripClient.Stream != nil { + if err := g.ntripClient.Stream.Close(); err != nil { + g.ntripMu.Unlock() + return err + } + g.ntripClient.Stream = nil + } + + g.ntripMu.Unlock() + g.activeBackgroundWorkers.Wait() + + if err := g.err.Get(); err != nil && !errors.Is(err, context.Canceled) { + return err + } + + return nil +} diff --git a/components/movementsensor/gpsrtkpmtk/gpsrtkpmtk_test.go b/components/movementsensor/gpsrtkpmtk/gpsrtkpmtk_test.go new file mode 100644 index 00000000000..ab28c053582 --- /dev/null +++ b/components/movementsensor/gpsrtkpmtk/gpsrtkpmtk_test.go @@ -0,0 +1,246 @@ +package gpsrtkpmtk + +import ( + "context" + "math" + "testing" + + "github.com/edaniels/golog" + geo "github.com/kellydunn/golang-geo" + "go.viam.com/test" + "go.viam.com/utils" + + "go.viam.com/rdk/components/movementsensor/fake" + rtk "go.viam.com/rdk/components/movementsensor/rtkutils" + "go.viam.com/rdk/resource" +) + +const ( + testRoverName = "testRover" + testStationName = "testStation" + testBoardName = "board1" + testBusName = "bus1" + testi2cAddr = 44 +) + +func TestValidateRTK(t *testing.T) { + path := "path" + cfg := Config{ + NtripURL: "http//fakeurl", + NtripConnectAttempts: 10, + NtripPass: "somepass", + NtripUser: "someuser", + NtripMountpoint: "NYC", + Board: testBoardName, + I2CBus: testBusName, + I2CAddr: testi2cAddr, + } + t.Run("valid config", func(t *testing.T) { + _, err := cfg.Validate(path) + test.That(t, err, test.ShouldBeNil) + }) + + t.Run("invalid ntrip url", func(t *testing.T) { + cfg := Config{ + NtripURL: "", + NtripConnectAttempts: 10, + NtripPass: "somepass", + NtripUser: "someuser", + NtripMountpoint: "NYC", + Board: testBoardName, + I2CBus: testBusName, + I2CAddr: testi2cAddr, + } + _, err := cfg.Validate(path) + test.That(t, err, test.ShouldBeError, + utils.NewConfigValidationFieldRequiredError(path, "ntrip_url")) + }) + + t.Run("invalid i2c bus", func(t *testing.T) { + cfg := Config{ + I2CBus: "", + NtripURL: "http//fakeurl", + NtripConnectAttempts: 10, + NtripPass: "somepass", + NtripUser: "someuser", + NtripMountpoint: "NYC", + Board: testBoardName, + I2CAddr: testi2cAddr, + } + _, err := cfg.Validate(path) + test.That(t, err, test.ShouldBeError, + utils.NewConfigValidationFieldRequiredError(path, "i2c_bus")) + }) + + t.Run("invalid i2c addr", func(t *testing.T) { + cfg := Config{ + I2CAddr: 0, + NtripURL: "http//fakeurl", + NtripConnectAttempts: 10, + NtripPass: "somepass", + NtripUser: "someuser", + NtripMountpoint: "NYC", + Board: testBoardName, + I2CBus: testBusName, + } + _, err := cfg.Validate(path) + test.That(t, err, test.ShouldBeError, + utils.NewConfigValidationFieldRequiredError(path, "i2c_addr")) + }) +} + +func TestConnect(t *testing.T) { + logger := golog.NewTestLogger(t) + ctx := context.Background() + cancelCtx, cancelFunc := context.WithCancel(ctx) + g := rtkI2C{ + cancelCtx: cancelCtx, + cancelFunc: cancelFunc, + logger: logger, + } + + url := "http://fakeurl" + username := "user" + password := "pwd" + + // create new ntrip client and connect + err := g.connect("invalidurl", username, password, 10) + test.That(t, err.Error(), test.ShouldContainSubstring, `Can't connect to NTRIP caster`) + + g.ntripClient = makeMockNtripClient() + + err = g.connect(url, username, password, 10) + test.That(t, err, test.ShouldBeNil) +} + +func TestReadings(t *testing.T) { + var ( + alt = 50.5 + speed = 5.4 + loc = geo.NewPoint(40.7, -73.98) + ) + + logger := golog.NewTestLogger(t) + ctx := context.Background() + cancelCtx, cancelFunc := context.WithCancel(ctx) + g := rtkI2C{ + cancelCtx: cancelCtx, + cancelFunc: cancelFunc, + logger: logger, + } + + mockSensor := &CustomMovementSensor{ + MovementSensor: &fake.MovementSensor{}, + } + + mockSensor.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { + return loc, alt, nil + } + + g.nmeamovementsensor = mockSensor + + // Normal position + loc1, alt1, err := g.Position(ctx, make(map[string]interface{})) + test.That(t, err, test.ShouldBeNil) + test.That(t, loc1, test.ShouldResemble, loc) + test.That(t, alt1, test.ShouldEqual, alt) + + speed1, err := g.LinearVelocity(ctx, make(map[string]interface{})) + test.That(t, err, test.ShouldBeNil) + test.That(t, speed1.Y, test.ShouldEqual, speed) + test.That(t, speed1.X, test.ShouldEqual, 0) + test.That(t, speed1.Z, test.ShouldEqual, 0) + + // Zero position with latitude 0 and longitude 0. + mockSensor.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { + return geo.NewPoint(0, 0), 0, nil + } + + loc2, alt2, err := g.Position(ctx, make(map[string]interface{})) + test.That(t, err, test.ShouldBeNil) + test.That(t, loc2, test.ShouldResemble, geo.NewPoint(0, 0)) + test.That(t, alt2, test.ShouldEqual, 0) + + speed2, err := g.LinearVelocity(ctx, make(map[string]interface{})) + test.That(t, err, test.ShouldBeNil) + test.That(t, speed2.Y, test.ShouldEqual, speed) + test.That(t, speed2.X, test.ShouldEqual, 0) + test.That(t, speed2.Z, test.ShouldEqual, 0) + + // Position with NaN values. + mockSensor.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { + return geo.NewPoint(math.NaN(), math.NaN()), math.NaN(), nil + } + + g.lastposition.SetLastPosition(loc1) + + loc3, alt3, err := g.Position(ctx, make(map[string]interface{})) + test.That(t, err, test.ShouldBeNil) + + // last known valid position should be returned when current position is NaN() + test.That(t, loc3, test.ShouldResemble, loc1) + test.That(t, math.IsNaN(alt3), test.ShouldBeTrue) + + speed3, err := g.LinearVelocity(ctx, make(map[string]interface{})) + test.That(t, err, test.ShouldBeNil) + test.That(t, speed3.Y, test.ShouldEqual, speed) +} + +func TestReconfigure(t *testing.T) { + g := &rtkI2C{ + wbaud: 9600, + addr: byte(66), + } + conf := resource.Config{ + Name: "reconfig1", + ConvertedAttributes: &Config{ + NtripURL: "http//fakeurl", + NtripConnectAttempts: 10, + NtripPass: "somepass", + NtripUser: "someuser", + NtripMountpoint: "NYC", + Board: testBoardName, + I2CBus: testBusName, + I2CAddr: testi2cAddr, + I2CBaudRate: 115200, + }, + } + err := g.Reconfigure(context.Background(), nil, conf) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, g.wbaud, test.ShouldEqual, 115200) + test.That(t, g.addr, test.ShouldEqual, byte(44)) +} + +func TestCloseRTK(t *testing.T) { + logger := golog.NewTestLogger(t) + ctx := context.Background() + cancelCtx, cancelFunc := context.WithCancel(ctx) + g := rtkI2C{ + cancelCtx: cancelCtx, + cancelFunc: cancelFunc, + logger: logger, + } + g.ntripClient = &rtk.NtripInfo{} + g.nmeamovementsensor = &fake.MovementSensor{} + + err := g.Close(ctx) + test.That(t, err, test.ShouldBeNil) +} + +type CustomMovementSensor struct { + *fake.MovementSensor + PositionFunc func(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) +} + +func (c *CustomMovementSensor) Position(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { + if c.PositionFunc != nil { + return c.PositionFunc(ctx, extra) + } + // Fallback to the default implementation if PositionFunc is not set. + return c.MovementSensor.Position(ctx, extra) +} + +// mock ntripinfo client. +func makeMockNtripClient() *rtk.NtripInfo { + return &rtk.NtripInfo{} +} diff --git a/components/movementsensor/gpsrtkserial/gpsrtkserial.go b/components/movementsensor/gpsrtkserial/gpsrtkserial.go new file mode 100644 index 00000000000..c5672da9bb5 --- /dev/null +++ b/components/movementsensor/gpsrtkserial/gpsrtkserial.go @@ -0,0 +1,639 @@ +// Package gpsrtkserial implements a gps using serial connection +package gpsrtkserial + +/* + This package supports GPS RTK (Real Time Kinematics), which takes in the normal signals + from the GNSS (Global Navigation Satellite Systems) along with a correction stream to achieve + positional accuracy (accuracy tbd), over Serial. + + Example GPS RTK chip datasheet: + https://content.u-blox.com/sites/default/files/ZED-F9P-04B_DataSheet_UBX-21044850.pdf + + Example configuration: + { + "type": "movement_sensor", + "model": "gps-nmea-rtk-serial", + "name": "my-gps-rtk" + "attributes": { + "ntrip_url": "url", + "ntrip_username": "usr", + "ntrip_connect_attempts": 10, + "ntrip_mountpoint": "MTPT", + "ntrip_password": "pwd", + "serial_baud_rate": 115200, + "serial_path": "serial-path" + }, + "depends_on": [], + } + +*/ + +import ( + "bufio" + "context" + "errors" + "io" + "math" + "sync" + + "github.com/de-bkg/gognss/pkg/ntrip" + "github.com/edaniels/golog" + "github.com/go-gnss/rtcm/rtcm3" + "github.com/golang/geo/r3" + slib "github.com/jacobsa/go-serial/serial" + geo "github.com/kellydunn/golang-geo" + "go.viam.com/utils" + + "go.viam.com/rdk/components/movementsensor" + gpsnmea "go.viam.com/rdk/components/movementsensor/gpsnmea" + rtk "go.viam.com/rdk/components/movementsensor/rtkutils" + "go.viam.com/rdk/resource" + "go.viam.com/rdk/spatialmath" +) + +var rtkmodel = resource.DefaultModelFamily.WithModel("gps-nmea-rtk-serial") + +const ( + serialStr = "serial" + ntripStr = "ntrip" +) + +// Config is used for converting NMEA MovementSensor with RTK capabilities config attributes. +type Config struct { + SerialPath string `json:"serial_path"` + SerialBaudRate int `json:"serial_baud_rate,omitempty"` + + NtripURL string `json:"ntrip_url"` + NtripConnectAttempts int `json:"ntrip_connect_attempts,omitempty"` + NtripMountpoint string `json:"ntrip_mountpoint,omitempty"` + NtripPass string `json:"ntrip_password,omitempty"` + NtripUser string `json:"ntrip_username,omitempty"` +} + +// Validate ensures all parts of the config are valid. +func (cfg *Config) Validate(path string) ([]string, error) { + err := cfg.validateNtrip(path) + if err != nil { + return nil, err + } + + err = cfg.validateSerialPath(path) + if err != nil { + return nil, err + } + + return nil, nil +} + +// validateSerialPath ensures all parts of the config are valid. +func (cfg *Config) validateSerialPath(path string) error { + if cfg.SerialPath == "" { + return utils.NewConfigValidationFieldRequiredError(path, "serial_path") + } + return nil +} + +// validateNtrip ensures all parts of the config are valid. +func (cfg *Config) validateNtrip(path string) error { + if cfg.NtripURL == "" { + return utils.NewConfigValidationFieldRequiredError(path, "ntrip_url") + } + return nil +} + +func init() { + resource.RegisterComponent( + movementsensor.API, + rtkmodel, + resource.Registration[movementsensor.MovementSensor, *Config]{ + Constructor: newRTKSerial, + }) +} + +// rtkSerial is an nmea movementsensor model that can intake RTK correction data. +type rtkSerial struct { + resource.Named + resource.AlwaysRebuild + logger golog.Logger + cancelCtx context.Context + cancelFunc func() + + activeBackgroundWorkers sync.WaitGroup + + mu sync.Mutex + ntripMu sync.Mutex + ntripconfigMu sync.Mutex + ntripClient *rtk.NtripInfo + ntripStatus bool + isClosed bool + + err movementsensor.LastError + lastposition movementsensor.LastPosition + lastcompassheading movementsensor.LastCompassHeading + InputProtocol string + + nmeamovementsensor gpsnmea.NmeaMovementSensor + correctionWriter io.ReadWriteCloser + writePath string + wbaud int +} + +// Reconfigure reconfigures attributes. +func (g *rtkSerial) Reconfigure(ctx context.Context, deps resource.Dependencies, conf resource.Config) error { + g.mu.Lock() + defer g.mu.Unlock() + + newConf, err := resource.NativeConfig[*Config](conf) + if err != nil { + return err + } + + if newConf.SerialPath != "" { + g.writePath = newConf.SerialPath + g.logger.Infof("updated serial_path to #%v", newConf.SerialPath) + } + + if newConf.SerialBaudRate != 0 { + g.wbaud = newConf.SerialBaudRate + g.logger.Infof("updated serial_baud_rate to %v", newConf.SerialBaudRate) + } else { + g.wbaud = 38400 + g.logger.Info("serial_baud_rate using default baud rate 38400") + } + + g.ntripconfigMu.Lock() + ntripConfig := &rtk.NtripConfig{ + NtripURL: newConf.NtripURL, + NtripUser: newConf.NtripUser, + NtripPass: newConf.NtripPass, + NtripMountpoint: newConf.NtripMountpoint, + NtripConnectAttempts: newConf.NtripConnectAttempts, + } + + // Init ntripInfo from attributes + tempNtripClient, err := rtk.NewNtripInfo(ntripConfig, g.logger) + if err != nil { + return err + } + + if g.ntripClient == nil { + g.ntripClient = tempNtripClient + } else { + tempNtripClient.Client = g.ntripClient.Client + tempNtripClient.Stream = g.ntripClient.Stream + + g.ntripClient = tempNtripClient + } + + g.ntripconfigMu.Unlock() + + g.logger.Debug("done reconfiguring") + + return nil +} + +func newRTKSerial( + ctx context.Context, + deps resource.Dependencies, + conf resource.Config, + logger golog.Logger, +) (movementsensor.MovementSensor, error) { + newConf, err := resource.NativeConfig[*Config](conf) + if err != nil { + return nil, err + } + + cancelCtx, cancelFunc := context.WithCancel(context.Background()) + g := &rtkSerial{ + Named: conf.ResourceName().AsNamed(), + cancelCtx: cancelCtx, + cancelFunc: cancelFunc, + logger: logger, + err: movementsensor.NewLastError(1, 1), + lastposition: movementsensor.NewLastPosition(), + lastcompassheading: movementsensor.NewLastCompassHeading(), + } + + // reconfigure + if err := g.Reconfigure(ctx, deps, conf); err != nil { + return nil, err + } + + g.InputProtocol = serialStr + nmeaConf := &gpsnmea.Config{ + ConnectionType: serialStr, + } + + // Init NMEAMovementSensor + nmeaConf.SerialConfig = &gpsnmea.SerialConfig{ + SerialPath: newConf.SerialPath, + SerialBaudRate: newConf.SerialBaudRate, + } + g.nmeamovementsensor, err = gpsnmea.NewSerialGPSNMEA(ctx, conf.ResourceName(), nmeaConf, logger) + if err != nil { + return nil, err + } + + if err := g.start(); err != nil { + return nil, err + } + return g, g.err.Get() +} + +func (g *rtkSerial) start() error { + if err := g.nmeamovementsensor.Start(g.cancelCtx); err != nil { + g.lastposition.GetLastPosition() + return err + } + g.activeBackgroundWorkers.Add(1) + if !g.isClosed { + utils.PanicCapturingGo(g.receiveAndWriteSerial) + } + return g.err.Get() +} + +// connect attempts to connect to ntrip client until successful connection or timeout. +func (g *rtkSerial) connect(casterAddr, user, pwd string, maxAttempts int) error { + attempts := 0 + + var c *ntrip.Client + var err error + + g.logger.Debug("Connecting to NTRIP caster") + for attempts < maxAttempts { + select { + case <-g.cancelCtx.Done(): + return g.cancelCtx.Err() + default: + } + + c, err = ntrip.NewClient(casterAddr, ntrip.Options{Username: user, Password: pwd}) + if err == nil { + break + } + + attempts++ + } + + if err != nil { + g.logger.Errorf("Can't connect to NTRIP caster: %s", err) + return err + } + + g.logger.Debug("Connected to NTRIP caster") + g.ntripMu.Lock() + g.ntripClient.Client = c + g.ntripMu.Unlock() + return g.err.Get() +} + +// getStream attempts to connect to ntrip streak until successful connection or timeout. +func (g *rtkSerial) getStream(mountPoint string, maxAttempts int) error { + success := false + attempts := 0 + + var rc io.ReadCloser + var err error + + g.logger.Debug("Getting NTRIP stream") + + for !success && attempts < maxAttempts && !g.isClosed { + select { + case <-g.cancelCtx.Done(): + return errors.New("Canceled") + default: + } + + rc, err = func() (io.ReadCloser, error) { + g.ntripMu.Lock() + defer g.ntripMu.Unlock() + return g.ntripClient.Client.GetStream(mountPoint) + }() + if err == nil { + success = true + } + attempts++ + } + + if err != nil { + g.logger.Errorf("Can't connect to NTRIP stream: %s", err) + return err + } + + if success { + g.logger.Debug("Connected to stream") + } + + g.ntripMu.Lock() + defer g.ntripMu.Unlock() + + g.ntripClient.Stream = rc + return g.err.Get() +} + +// openPort opens the serial port for writing. +func (g *rtkSerial) openPort() error { + options := slib.OpenOptions{ + PortName: g.writePath, + BaudRate: uint(g.wbaud), + DataBits: 8, + StopBits: 1, + MinimumReadSize: 1, + } + + g.ntripMu.Lock() + defer g.ntripMu.Unlock() + + if err := g.cancelCtx.Err(); err != nil { + return err + } + + var err error + g.correctionWriter, err = slib.Open(options) + if err != nil { + g.logger.Errorf("serial.Open: %v", err) + return err + } + + return nil +} + +// closePort closes the serial port. +func (g *rtkSerial) closePort() { + g.ntripMu.Lock() + defer g.ntripMu.Unlock() + + if g.correctionWriter != nil { + err := g.correctionWriter.Close() + if err != nil { + g.logger.Errorf("Error closing port: %v", err) + } + } +} + +// receiveAndWriteSerial connects to NTRIP receiver and sends correction stream to the MovementSensor through serial. +func (g *rtkSerial) receiveAndWriteSerial() { + defer g.activeBackgroundWorkers.Done() + if err := g.cancelCtx.Err(); err != nil { + return + } + err := g.connect(g.ntripClient.URL, g.ntripClient.Username, g.ntripClient.Password, g.ntripClient.MaxConnectAttempts) + if err != nil { + g.err.Set(err) + return + } + + if !g.ntripClient.Client.IsCasterAlive() { + g.logger.Infof("caster %s seems to be down", g.ntripClient.URL) + } + + err = g.openPort() + if err != nil { + g.err.Set(err) + return + } + + defer g.closePort() + + w := bufio.NewWriter(g.correctionWriter) + err = g.getStream(g.ntripClient.MountPoint, g.ntripClient.MaxConnectAttempts) + if err != nil { + g.err.Set(err) + return + } + + r := io.TeeReader(g.ntripClient.Stream, w) + scanner := rtcm3.NewScanner(r) + + g.ntripMu.Lock() + g.ntripStatus = true + g.ntripMu.Unlock() + + // It's okay to skip the mutex on this next line: g.ntripStatus can only be mutated by this + // goroutine itself + for g.ntripStatus && !g.isClosed { + select { + case <-g.cancelCtx.Done(): + return + default: + } + + msg, err := scanner.NextMessage() + if err != nil { + g.ntripMu.Lock() + g.ntripStatus = false + g.ntripMu.Unlock() + + if msg == nil { + if g.isClosed { + return + } + g.logger.Debug("No message... reconnecting to stream...") + err = g.getStream(g.ntripClient.MountPoint, g.ntripClient.MaxConnectAttempts) + if err != nil { + g.err.Set(err) + return + } + + r = io.TeeReader(g.ntripClient.Stream, w) + scanner = rtcm3.NewScanner(r) + g.ntripMu.Lock() + g.ntripStatus = true + g.ntripMu.Unlock() + continue + } + } + } +} + +// getNtripConnectionStatus returns true if connection to NTRIP stream is OK, false if not. +// +//nolint:all +func (g *rtkSerial) getNtripConnectionStatus() (bool, error) { + g.ntripMu.Lock() + defer g.ntripMu.Unlock() + return g.ntripStatus, g.err.Get() +} + +// Position returns the current geographic location of the MOVEMENTSENSOR. +func (g *rtkSerial) Position(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { + g.ntripMu.Lock() + lastError := g.err.Get() + if lastError != nil { + lastPosition := g.lastposition.GetLastPosition() + g.ntripMu.Unlock() + if lastPosition != nil { + return lastPosition, 0, nil + } + return geo.NewPoint(math.NaN(), math.NaN()), math.NaN(), lastError + } + g.ntripMu.Unlock() + + position, alt, err := g.nmeamovementsensor.Position(ctx, extra) + if err != nil { + // Use the last known valid position if current position is (0,0)/ NaN. + if position != nil && (g.lastposition.IsZeroPosition(position) || g.lastposition.IsPositionNaN(position)) { + lastPosition := g.lastposition.GetLastPosition() + if lastPosition != nil { + return lastPosition, alt, nil + } + } + return geo.NewPoint(math.NaN(), math.NaN()), math.NaN(), err + } + + if g.lastposition.IsPositionNaN(position) { + position = g.lastposition.GetLastPosition() + } + return position, alt, nil +} + +// LinearVelocity passthrough. +func (g *rtkSerial) LinearVelocity(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { + g.ntripMu.Lock() + lastError := g.err.Get() + if lastError != nil { + defer g.ntripMu.Unlock() + return r3.Vector{}, lastError + } + g.ntripMu.Unlock() + + return g.nmeamovementsensor.LinearVelocity(ctx, extra) +} + +// LinearAcceleration passthrough. +func (g *rtkSerial) LinearAcceleration(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { + lastError := g.err.Get() + if lastError != nil { + return r3.Vector{}, lastError + } + return g.nmeamovementsensor.LinearAcceleration(ctx, extra) +} + +// AngularVelocity passthrough. +func (g *rtkSerial) AngularVelocity(ctx context.Context, extra map[string]interface{}) (spatialmath.AngularVelocity, error) { + g.ntripMu.Lock() + lastError := g.err.Get() + if lastError != nil { + defer g.ntripMu.Unlock() + return spatialmath.AngularVelocity{}, lastError + } + g.ntripMu.Unlock() + + return g.nmeamovementsensor.AngularVelocity(ctx, extra) +} + +// CompassHeading passthrough. +func (g *rtkSerial) CompassHeading(ctx context.Context, extra map[string]interface{}) (float64, error) { + g.ntripMu.Lock() + lastError := g.err.Get() + if lastError != nil { + defer g.ntripMu.Unlock() + return 0, lastError + } + g.ntripMu.Unlock() + return g.nmeamovementsensor.CompassHeading(ctx, extra) +} + +// Orientation passthrough. +func (g *rtkSerial) Orientation(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) { + g.ntripMu.Lock() + lastError := g.err.Get() + if lastError != nil { + defer g.ntripMu.Unlock() + return spatialmath.NewZeroOrientation(), lastError + } + g.ntripMu.Unlock() + return g.nmeamovementsensor.Orientation(ctx, extra) +} + +// readFix passthrough. +func (g *rtkSerial) readFix(ctx context.Context) (int, error) { + g.ntripMu.Lock() + lastError := g.err.Get() + if lastError != nil { + defer g.ntripMu.Unlock() + return 0, lastError + } + g.ntripMu.Unlock() + return g.nmeamovementsensor.ReadFix(ctx) +} + +// Properties passthrough. +func (g *rtkSerial) Properties(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { + lastError := g.err.Get() + if lastError != nil { + return &movementsensor.Properties{}, lastError + } + + return g.nmeamovementsensor.Properties(ctx, extra) +} + +// Accuracy passthrough. +func (g *rtkSerial) Accuracy(ctx context.Context, extra map[string]interface{}) (map[string]float32, error) { + lastError := g.err.Get() + if lastError != nil { + return map[string]float32{}, lastError + } + + return g.nmeamovementsensor.Accuracy(ctx, extra) +} + +// Readings will use the default MovementSensor Readings if not provided. +func (g *rtkSerial) Readings(ctx context.Context, extra map[string]interface{}) (map[string]interface{}, error) { + readings, err := movementsensor.Readings(ctx, g, extra) + if err != nil { + return nil, err + } + + fix, err := g.readFix(ctx) + if err != nil { + return nil, err + } + + readings["fix"] = fix + + return readings, nil +} + +// Close shuts down the rtkSerial. +func (g *rtkSerial) Close(ctx context.Context) error { + g.ntripMu.Lock() + g.cancelFunc() + + if err := g.nmeamovementsensor.Close(ctx); err != nil { + g.ntripMu.Unlock() + return err + } + + // close ntrip writer + if g.correctionWriter != nil { + if err := g.correctionWriter.Close(); err != nil { + g.isClosed = true + g.ntripMu.Unlock() + return err + } + g.correctionWriter = nil + } + + // close ntrip client and stream + if g.ntripClient.Client != nil { + g.ntripClient.Client.CloseIdleConnections() + g.ntripClient.Client = nil + } + + if g.ntripClient.Stream != nil { + if err := g.ntripClient.Stream.Close(); err != nil { + g.ntripMu.Unlock() + return err + } + g.ntripClient.Stream = nil + } + + g.ntripMu.Unlock() + g.activeBackgroundWorkers.Wait() + + if err := g.err.Get(); err != nil && !errors.Is(err, context.Canceled) { + return err + } + return nil +} diff --git a/components/movementsensor/gpsrtkserial/gpsrtkserial_test.go b/components/movementsensor/gpsrtkserial/gpsrtkserial_test.go new file mode 100644 index 00000000000..2b7999d9992 --- /dev/null +++ b/components/movementsensor/gpsrtkserial/gpsrtkserial_test.go @@ -0,0 +1,225 @@ +package gpsrtkserial + +import ( + "context" + "math" + "testing" + + "github.com/edaniels/golog" + geo "github.com/kellydunn/golang-geo" + "go.viam.com/test" + "go.viam.com/utils" + + "go.viam.com/rdk/components/movementsensor/fake" + rtk "go.viam.com/rdk/components/movementsensor/rtkutils" + "go.viam.com/rdk/resource" +) + +func TestValidateRTK(t *testing.T) { + path := "path" + t.Run("valid config", func(t *testing.T) { + cfg := Config{ + NtripURL: "http//fakeurl", + NtripConnectAttempts: 10, + NtripPass: "somepass", + NtripUser: "someuser", + NtripMountpoint: "NYC", + SerialPath: path, + SerialBaudRate: 115200, + } + err := cfg.validateNtrip(path) + test.That(t, err, test.ShouldBeNil) + err = cfg.validateSerialPath(path) + test.That(t, err, test.ShouldBeNil) + _, err = cfg.Validate(path) + test.That(t, err, test.ShouldBeNil) + }) + + t.Run("invalid config", func(t *testing.T) { + cfg := Config{ + NtripURL: "", + NtripConnectAttempts: 10, + NtripPass: "somepass", + NtripUser: "someuser", + NtripMountpoint: "NYC", + SerialPath: path, + SerialBaudRate: 115200, + } + + _, err := cfg.Validate(path) + test.That(t, err, test.ShouldBeError, + utils.NewConfigValidationFieldRequiredError(path, "ntrip_url")) + }) + + t.Run("invalid config", func(t *testing.T) { + cfg := Config{ + NtripURL: "http//fakeurl", + NtripConnectAttempts: 10, + NtripPass: "somepass", + NtripUser: "someuser", + NtripMountpoint: "NYC", + SerialPath: "", + SerialBaudRate: 115200, + } + + _, err := cfg.Validate(path) + test.That(t, err, test.ShouldBeError, + utils.NewConfigValidationFieldRequiredError(path, "serial_path")) + }) +} + +func TestConnect(t *testing.T) { + logger := golog.NewTestLogger(t) + ctx := context.Background() + cancelCtx, cancelFunc := context.WithCancel(ctx) + g := rtkSerial{ + cancelCtx: cancelCtx, + cancelFunc: cancelFunc, + logger: logger, + } + + url := "http://fakeurl" + username := "user" + password := "pwd" + + err := g.connect("invalidurl", username, password, 10) + test.That(t, err.Error(), test.ShouldContainSubstring, `address must start with http://`) + + g.ntripClient = makeMockNtripClient() + + err = g.connect(url, username, password, 10) + test.That(t, err, test.ShouldBeNil) +} + +func TestReadings(t *testing.T) { + var ( + alt = 50.5 + speed = 5.4 + loc = geo.NewPoint(40.7, -73.98) + ) + + logger := golog.NewTestLogger(t) + ctx := context.Background() + cancelCtx, cancelFunc := context.WithCancel(ctx) + g := rtkSerial{ + cancelCtx: cancelCtx, + cancelFunc: cancelFunc, + logger: logger, + } + + mockSensor := &CustomMovementSensor{ + MovementSensor: &fake.MovementSensor{}, + } + + mockSensor.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { + return loc, alt, nil + } + + g.nmeamovementsensor = mockSensor + + // Normal position + loc1, alt1, err := g.Position(ctx, make(map[string]interface{})) + test.That(t, err, test.ShouldBeNil) + test.That(t, loc1, test.ShouldResemble, loc) + test.That(t, alt1, test.ShouldEqual, alt) + + speed1, err := g.LinearVelocity(ctx, make(map[string]interface{})) + test.That(t, err, test.ShouldBeNil) + test.That(t, speed1.Y, test.ShouldEqual, speed) + test.That(t, speed1.X, test.ShouldEqual, 0) + test.That(t, speed1.Z, test.ShouldEqual, 0) + + // Zero position with latitude 0 and longitude 0. + mockSensor.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { + return geo.NewPoint(0, 0), 0, nil + } + + loc2, alt2, err := g.Position(ctx, make(map[string]interface{})) + test.That(t, err, test.ShouldBeNil) + test.That(t, loc2, test.ShouldResemble, geo.NewPoint(0, 0)) + test.That(t, alt2, test.ShouldEqual, 0) + + speed2, err := g.LinearVelocity(ctx, make(map[string]interface{})) + test.That(t, err, test.ShouldBeNil) + test.That(t, speed2.Y, test.ShouldEqual, speed) + test.That(t, speed2.X, test.ShouldEqual, 0) + test.That(t, speed2.Z, test.ShouldEqual, 0) + + // Position with NaN values. + mockSensor.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { + return geo.NewPoint(math.NaN(), math.NaN()), math.NaN(), nil + } + g.lastposition.SetLastPosition(loc1) + + loc3, alt3, err := g.Position(ctx, make(map[string]interface{})) + test.That(t, err, test.ShouldBeNil) + + // last known valid position should be returned when current position is NaN() + test.That(t, loc3, test.ShouldResemble, loc1) + test.That(t, math.IsNaN(alt3), test.ShouldBeTrue) + + speed3, err := g.LinearVelocity(ctx, make(map[string]interface{})) + test.That(t, err, test.ShouldBeNil) + test.That(t, speed3.Y, test.ShouldEqual, speed) +} + +func TestReconfigure(t *testing.T) { + g := &rtkSerial{ + writePath: "/dev/ttyUSB0", + wbaud: 9600, + logger: golog.NewTestLogger(t), + } + + conf := resource.Config{ + Name: "reconfig1", + ConvertedAttributes: &Config{ + SerialPath: "/dev/ttyUSB1", + SerialBaudRate: 115200, + NtripURL: "http//fakeurl", + NtripConnectAttempts: 10, + NtripPass: "somepass", + NtripUser: "someuser", + NtripMountpoint: "NYC", + }, + } + + err := g.Reconfigure(context.Background(), nil, conf) + + test.That(t, err, test.ShouldBeNil) + test.That(t, g.writePath, test.ShouldResemble, "/dev/ttyUSB1") + test.That(t, g.wbaud, test.ShouldEqual, 115200) +} + +func TestCloseRTK(t *testing.T) { + logger := golog.NewTestLogger(t) + ctx := context.Background() + cancelCtx, cancelFunc := context.WithCancel(ctx) + g := rtkSerial{ + cancelCtx: cancelCtx, + cancelFunc: cancelFunc, + logger: logger, + } + g.ntripClient = &rtk.NtripInfo{} + g.nmeamovementsensor = &fake.MovementSensor{} + + err := g.Close(ctx) + test.That(t, err, test.ShouldBeNil) +} + +type CustomMovementSensor struct { + *fake.MovementSensor + PositionFunc func(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) +} + +func (c *CustomMovementSensor) Position(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { + if c.PositionFunc != nil { + return c.PositionFunc(ctx, extra) + } + // Fallback to the default implementation if PositionFunc is not set. + return c.MovementSensor.Position(ctx, extra) +} + +// mock ntripinfo client. +func makeMockNtripClient() *rtk.NtripInfo { + return &rtk.NtripInfo{} +} diff --git a/components/movementsensor/imuwit/imu.go b/components/movementsensor/imuwit/imu.go index a27145182ce..efe49dd32d2 100644 --- a/components/movementsensor/imuwit/imu.go +++ b/components/movementsensor/imuwit/imu.go @@ -69,22 +69,22 @@ func (cfg *Config) Validate(path string) ([]string, error) { func init() { resource.RegisterComponent(movementsensor.API, model, resource.Registration[movementsensor.MovementSensor, *Config]{ - Constructor: NewWit, + Constructor: newWit, }) } type wit struct { resource.Named resource.AlwaysRebuild - angularVelocity spatialmath.AngularVelocity - orientation spatialmath.EulerAngles - acceleration r3.Vector - magnetometer r3.Vector - numBadReadings uint32 - err movementsensor.LastError - - mu sync.Mutex - + angularVelocity spatialmath.AngularVelocity + orientation spatialmath.EulerAngles + acceleration r3.Vector + magnetometer r3.Vector + compassheading float64 + numBadReadings uint32 + err movementsensor.LastError + hasMagnetometer bool + mu sync.Mutex port io.ReadWriteCloser cancelFunc func() activeBackgroundWorkers sync.WaitGroup @@ -116,15 +116,34 @@ func (imu *wit) LinearAcceleration(ctx context.Context, extra map[string]interfa return imu.acceleration, imu.err.Get() } -// GetMagnetometer returns magnetic field in gauss. -func (imu *wit) GetMagnetometer(ctx context.Context) (r3.Vector, error) { +// getMagnetometer returns magnetic field in gauss. +func (imu *wit) getMagnetometer() (r3.Vector, error) { imu.mu.Lock() defer imu.mu.Unlock() return imu.magnetometer, imu.err.Get() } func (imu *wit) CompassHeading(ctx context.Context, extra map[string]interface{}) (float64, error) { - return 0, movementsensor.ErrMethodUnimplementedCompassHeading + imu.mu.Lock() + defer imu.mu.Unlock() + + var err error + // this only works when the imu is level to the surface of the earth, no inclines + // do not let the imu near permanent magnets or things that make a strong magnetic field + imu.compassheading = calculateCompassHeading(imu.magnetometer.X, imu.magnetometer.Y) + + return imu.compassheading, err +} + +func calculateCompassHeading(x, y float64) float64 { + // calculate -180 to 180 heading from radians + // North (y) is 0 so the π/2 - atan2(y, x) identity is used + // directl + rad := math.Atan2(y, x) // -180 to 180 heading + compass := rutils.RadToDeg(rad) + compass = math.Mod(compass, 360) + compass = math.Mod(compass+360, 360) + return compass // change domain to 0 to 360 } func (imu *wit) Position(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { @@ -141,7 +160,7 @@ func (imu *wit) Readings(ctx context.Context, extra map[string]interface{}) (map return nil, err } - mag, err := imu.GetMagnetometer(ctx) + mag, err := imu.getMagnetometer() if err != nil { return nil, err } @@ -151,15 +170,19 @@ func (imu *wit) Readings(ctx context.Context, extra map[string]interface{}) (map } func (imu *wit) Properties(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { + imu.mu.Lock() + defer imu.mu.Unlock() + return &movementsensor.Properties{ AngularVelocitySupported: true, OrientationSupported: true, LinearAccelerationSupported: true, + CompassHeadingSupported: imu.hasMagnetometer, }, nil } -// NewWit creates a new Wit IMU. -func NewWit( +// newWit creates a new Wit IMU. +func newWit( ctx context.Context, deps resource.Dependencies, conf resource.Config, @@ -204,6 +227,7 @@ func NewWit( } func (imu *wit) startUpdateLoop(ctx context.Context, portReader *bufio.Reader, logger golog.Logger) { + imu.hasMagnetometer = false ctx, imu.cancelFunc = context.WithCancel(ctx) imu.activeBackgroundWorkers.Add(1) utils.PanicCapturingGo(func() { @@ -259,12 +283,8 @@ func scale(a, b byte, r float64) float64 { return x } -func scalemag(a, b byte, r float64) float64 { - x := float64(int(b)<<8 | int(a)) // 0 -> 2 - x *= r // 0 -> 2r - x += r - x = math.Mod(x, r*2) - x -= r +func convertMagByteToTesla(a, b byte) float64 { + x := float64(int(int8(b))<<8 | int(a)) // 0 -> 2 return x } @@ -298,12 +318,13 @@ func (imu *wit) parseWIT(line string) error { } if line[0] == 0x54 { + imu.hasMagnetometer = true if len(line) < 7 { return fmt.Errorf("line is wrong for imu magnetometer %d %v", len(line), line) } - imu.magnetometer.X = scalemag(line[1], line[2], 1) // converts to gauss - imu.magnetometer.Y = scalemag(line[3], line[4], 1) - imu.magnetometer.Z = scalemag(line[5], line[6], 1) + imu.magnetometer.X = convertMagByteToTesla(line[1], line[2]) // converts uT (micro Tesla) + imu.magnetometer.Y = convertMagByteToTesla(line[3], line[4]) + imu.magnetometer.Z = convertMagByteToTesla(line[5], line[6]) } return nil diff --git a/components/movementsensor/merged/merged.go b/components/movementsensor/merged/merged.go index bed2be92253..62cea7aa727 100644 --- a/components/movementsensor/merged/merged.go +++ b/components/movementsensor/merged/merged.go @@ -29,7 +29,7 @@ type Config struct { CompassHeading []string `json:"compass_heading,omitempty"` LinearVelocity []string `json:"linear_velocity,omitempty"` AngularVelocity []string `json:"angular_velocity,omitempty"` - LinearAcceleration []string `json:"angular_acceleration,omitempty"` + LinearAcceleration []string `json:"linear_acceleration,omitempty"` } // Validate validates the merged model's configuration. @@ -71,6 +71,7 @@ func newMergedModel(ctx context.Context, deps resource.Dependencies, conf resour ) { m := merged{ logger: logger, + Named: conf.ResourceName().AsNamed(), } if err := m.Reconfigure(ctx, deps, conf); err != nil { diff --git a/components/movementsensor/merged/merged_test.go b/components/movementsensor/merged/merged_test.go index b46a6c727ee..0cea9f9590c 100644 --- a/components/movementsensor/merged/merged_test.go +++ b/components/movementsensor/merged/merged_test.go @@ -275,6 +275,9 @@ func TestCreation(t *testing.T) { err = ms.Reconfigure(ctx, deps, conf) test.That(t, err, test.ShouldBeNil) + res := ms.Name() + test.That(t, res, test.ShouldNotBeNil) + pos, alt, err = ms.Position(ctx, nil) test.That(t, err, test.ShouldBeNil) test.That(t, pos, test.ShouldEqual, testgeopoint) diff --git a/components/movementsensor/movementsensor.go b/components/movementsensor/movementsensor.go index cb59ff1231d..4ba1b41cc1f 100644 --- a/components/movementsensor/movementsensor.go +++ b/components/movementsensor/movementsensor.go @@ -3,7 +3,7 @@ package movementsensor import ( "context" - "errors" + "strings" "github.com/golang/geo/r3" geo "github.com/kellydunn/golang-geo" @@ -23,35 +23,41 @@ func init() { RPCClient: NewClientFromConn, }) - registerCollector("Position", func(ctx context.Context, ms MovementSensor) (interface{}, error) { + registerCollector("Position", func(ctx context.Context, ms MovementSensor, extra map[string]interface{}) (interface{}, error) { type Position struct { Lat float64 Lng float64 } - p, _, err := ms.Position(ctx, make(map[string]interface{})) - return Position{Lat: p.Lat(), Lng: p.Lng()}, err + p, _, err := ms.Position(ctx, extra) + if err != nil { + return nil, err + } + return Position{Lat: p.Lat(), Lng: p.Lng()}, nil }) - registerCollector("LinearVelocity", func(ctx context.Context, ms MovementSensor) (interface{}, error) { - v, err := ms.LinearVelocity(ctx, make(map[string]interface{})) + registerCollector("LinearVelocity", func(ctx context.Context, ms MovementSensor, extra map[string]interface{}) (interface{}, error) { + v, err := ms.LinearVelocity(ctx, extra) return v, err }) - registerCollector("AngularVelocity", func(ctx context.Context, ms MovementSensor) (interface{}, error) { - v, err := ms.AngularVelocity(ctx, make(map[string]interface{})) + registerCollector("AngularVelocity", func(ctx context.Context, ms MovementSensor, extra map[string]interface{}) (interface{}, error) { + v, err := ms.AngularVelocity(ctx, extra) return v, err }) - registerCollector("CompassHeading", func(ctx context.Context, ms MovementSensor) (interface{}, error) { + registerCollector("CompassHeading", func(ctx context.Context, ms MovementSensor, extra map[string]interface{}) (interface{}, error) { type Heading struct { Heading float64 } - h, err := ms.CompassHeading(ctx, make(map[string]interface{})) - return Heading{Heading: h}, err + h, err := ms.CompassHeading(ctx, extra) + if err != nil { + return nil, err + } + return Heading{Heading: h}, nil }) - registerCollector("LinearAcceleration", func(ctx context.Context, ms MovementSensor) (interface{}, error) { - v, err := ms.LinearAcceleration(ctx, make(map[string]interface{})) + registerCollector("LinearAcceleration", func(ctx context.Context, ms MovementSensor, extra map[string]interface{}) (interface{}, error) { + v, err := ms.LinearAcceleration(ctx, extra) return v, err }) - registerCollector("Orientation", func(ctx context.Context, ms MovementSensor) (interface{}, error) { - v, err := ms.Orientation(ctx, make(map[string]interface{})) + registerCollector("Orientation", func(ctx context.Context, ms MovementSensor, extra map[string]interface{}) (interface{}, error) { + v, err := ms.Orientation(ctx, extra) return v, err }) } @@ -102,7 +108,7 @@ func Readings(ctx context.Context, g MovementSensor, extra map[string]interface{ pos, altitude, err := g.Position(ctx, extra) if err != nil { - if !errors.Is(err, ErrMethodUnimplementedPosition) { + if !strings.Contains(err.Error(), ErrMethodUnimplementedPosition.Error()) { return nil, err } } else { @@ -112,7 +118,7 @@ func Readings(ctx context.Context, g MovementSensor, extra map[string]interface{ vel, err := g.LinearVelocity(ctx, extra) if err != nil { - if !errors.Is(err, ErrMethodUnimplementedLinearVelocity) { + if !strings.Contains(err.Error(), ErrMethodUnimplementedLinearVelocity.Error()) { return nil, err } } else { @@ -121,7 +127,7 @@ func Readings(ctx context.Context, g MovementSensor, extra map[string]interface{ la, err := g.LinearAcceleration(ctx, extra) if err != nil { - if !errors.Is(err, ErrMethodUnimplementedLinearAcceleration) { + if !strings.Contains(err.Error(), ErrMethodUnimplementedLinearAcceleration.Error()) { return nil, err } } else { @@ -130,7 +136,7 @@ func Readings(ctx context.Context, g MovementSensor, extra map[string]interface{ avel, err := g.AngularVelocity(ctx, extra) if err != nil { - if !errors.Is(err, ErrMethodUnimplementedAngularVelocity) { + if !strings.Contains(err.Error(), ErrMethodUnimplementedAngularVelocity.Error()) { return nil, err } } else { @@ -139,7 +145,7 @@ func Readings(ctx context.Context, g MovementSensor, extra map[string]interface{ compass, err := g.CompassHeading(ctx, extra) if err != nil { - if !errors.Is(err, ErrMethodUnimplementedCompassHeading) { + if !strings.Contains(err.Error(), ErrMethodUnimplementedCompassHeading.Error()) { return nil, err } } else { @@ -148,7 +154,7 @@ func Readings(ctx context.Context, g MovementSensor, extra map[string]interface{ ori, err := g.Orientation(ctx, extra) if err != nil { - if !errors.Is(err, ErrMethodUnimplementedOrientation) { + if !strings.Contains(err.Error(), ErrMethodUnimplementedOrientation.Error()) { return nil, err } } else { diff --git a/components/movementsensor/register/register.go b/components/movementsensor/register/register.go index e2358ba19ee..94a99372d2f 100644 --- a/components/movementsensor/register/register.go +++ b/components/movementsensor/register/register.go @@ -4,13 +4,14 @@ package register import ( // Load all movementsensors. _ "go.viam.com/rdk/components/movementsensor/adxl345" - _ "go.viam.com/rdk/components/movementsensor/cameramono" _ "go.viam.com/rdk/components/movementsensor/fake" _ "go.viam.com/rdk/components/movementsensor/gpsnmea" - _ "go.viam.com/rdk/components/movementsensor/gpsrtk" + _ "go.viam.com/rdk/components/movementsensor/gpsrtkpmtk" + _ "go.viam.com/rdk/components/movementsensor/gpsrtkserial" _ "go.viam.com/rdk/components/movementsensor/imuvectornav" _ "go.viam.com/rdk/components/movementsensor/imuwit" _ "go.viam.com/rdk/components/movementsensor/merged" _ "go.viam.com/rdk/components/movementsensor/mpu6050" - _ "go.viam.com/rdk/components/movementsensor/rtk_station" + _ "go.viam.com/rdk/components/movementsensor/replay" + _ "go.viam.com/rdk/components/movementsensor/wheeledodometry" ) diff --git a/components/movementsensor/replay/replay.go b/components/movementsensor/replay/replay.go new file mode 100644 index 00000000000..1f92e511a83 --- /dev/null +++ b/components/movementsensor/replay/replay.go @@ -0,0 +1,491 @@ +// Package replay implements a replay movement sensor that can return motion data. +package replay + +import ( + "context" + "sync" + "time" + + "github.com/edaniels/golog" + "github.com/golang/geo/r3" + geo "github.com/kellydunn/golang-geo" + "github.com/pkg/errors" + datapb "go.viam.com/api/app/data/v1" + goutils "go.viam.com/utils" + "go.viam.com/utils/rpc" + "google.golang.org/grpc" + "google.golang.org/grpc/metadata" + "google.golang.org/protobuf/types/known/structpb" + "google.golang.org/protobuf/types/known/timestamppb" + + "go.viam.com/rdk/components/movementsensor" + "go.viam.com/rdk/internal/cloud" + "go.viam.com/rdk/resource" + "go.viam.com/rdk/spatialmath" + "go.viam.com/rdk/utils/contextutils" +) + +const ( + timeFormat = time.RFC3339 + grpcConnectionTimeout = 10 * time.Second + downloadTimeout = 30 * time.Second + maxCacheSize = 1000 +) + +var ( + // model is the model of a replay movement sensor. + model = resource.DefaultModelFamily.WithModel("replay") + + // ErrEndOfDataset represents that the replay sensor has reached the end of the dataset. + ErrEndOfDataset = errors.New("reached end of dataset") + + // methodList is a list of all the base methods possible for a movement sensor to implement. + methodList = []string{"Position", "Orientation", "AngularVelocity", "LinearVelocity", "LinearAcceleration", "CompassHeading"} +) + +func init() { + resource.RegisterComponent(movementsensor.API, model, resource.Registration[movementsensor.MovementSensor, *Config]{ + Constructor: newReplayMovementSensor, + }) +} + +// Validate checks that the config attributes are valid for a replay movement sensor. +func (cfg *Config) Validate(path string) ([]string, error) { + if cfg.Source == "" { + return nil, goutils.NewConfigValidationFieldRequiredError(path, "source") + } + + if cfg.RobotID == "" { + return nil, goutils.NewConfigValidationFieldRequiredError(path, "robot_id") + } + + if cfg.LocationID == "" { + return nil, goutils.NewConfigValidationFieldRequiredError(path, "location_id") + } + + if cfg.OrganizationID == "" { + return nil, goutils.NewConfigValidationFieldRequiredError(path, "organization_id") + } + + var err error + var startTime time.Time + if cfg.Interval.Start != "" { + startTime, err = time.Parse(timeFormat, cfg.Interval.Start) + if err != nil { + return nil, errors.New("invalid time format for start time (UTC), use RFC3339") + } + if startTime.After(time.Now()) { + return nil, errors.New("invalid config, start time (UTC) must be in the past") + } + } + + var endTime time.Time + if cfg.Interval.End != "" { + endTime, err = time.Parse(timeFormat, cfg.Interval.End) + if err != nil { + return nil, errors.New("invalid time format for end time (UTC), use RFC3339") + } + if endTime.After(time.Now()) { + return nil, errors.New("invalid config, end time (UTC) must be in the past") + } + } + + if cfg.Interval.Start != "" && cfg.Interval.End != "" && startTime.After(endTime) { + return nil, errors.New("invalid config, end time (UTC) must be after start time (UTC)") + } + + if cfg.BatchSize != nil && (*cfg.BatchSize > uint64(maxCacheSize) || *cfg.BatchSize == 0) { + return nil, errors.Errorf("batch_size must be between 1 and %d", maxCacheSize) + } + + return []string{cloud.InternalServiceName.String()}, nil +} + +// Config describes how to configure the replay movement sensor. +type Config struct { + Source string `json:"source,omitempty"` + RobotID string `json:"robot_id,omitempty"` + LocationID string `json:"location_id,omitempty"` + OrganizationID string `json:"organization_id,omitempty"` + Interval TimeInterval `json:"time_interval,omitempty"` + BatchSize *uint64 `json:"batch_size,omitempty"` + Properties map[string]bool `json:"properties,omitempty"` +} + +// TimeInterval holds the start and end time used to filter data. +type TimeInterval struct { + Start string `json:"start,omitempty"` + End string `json:"end,omitempty"` +} + +// cacheEntry stores data that was downloaded from a previous operation but has not yet been passed +// to the caller. +type cacheEntry struct { + data *structpb.Struct + timeRequested *timestamppb.Timestamp + timeReceived *timestamppb.Timestamp +} + +// replayMovementSensor is a movement sensor model that plays back pre-captured movement sensor data. +type replayMovementSensor struct { + resource.Named + logger golog.Logger + + cloudConnSvc cloud.ConnectionService + cloudConn rpc.ClientConn + dataClient datapb.DataServiceClient + + lastData map[string]string + limit uint64 + filter *datapb.Filter + + cache map[string][]*cacheEntry + + mu sync.RWMutex + closed bool +} + +// newReplayMovementSensor creates a new replay movement sensor based on the inputted config and dependencies. +func newReplayMovementSensor(ctx context.Context, deps resource.Dependencies, conf resource.Config, logger golog.Logger) ( + movementsensor.MovementSensor, error, +) { + replay := &replayMovementSensor{ + Named: conf.ResourceName().AsNamed(), + logger: logger, + } + + if err := replay.Reconfigure(ctx, deps, conf); err != nil { + return nil, err + } + + return replay, nil +} + +// Position returns the next position from the cache, in the form of a geo.Point and altitude. +func (replay *replayMovementSensor) Position(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { + replay.mu.Lock() + defer replay.mu.Unlock() + if replay.closed { + return nil, 0, errors.New("session closed") + } + + data, err := replay.getDataFromCache(ctx, "Position") + if err != nil { + return nil, 0, err + } + + return geo.NewPoint( + data.GetFields()["Latitude"].GetNumberValue(), + data.GetFields()["Longitude"].GetNumberValue()), data.GetFields()["Altitude"].GetNumberValue(), nil +} + +// LinearVelocity returns the next linear velocity from the cache in the form of an r3.Vector. +func (replay *replayMovementSensor) LinearVelocity(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { + replay.mu.Lock() + defer replay.mu.Unlock() + if replay.closed { + return r3.Vector{}, errors.New("session closed") + } + + data, err := replay.getDataFromCache(ctx, "LinearVelocity") + if err != nil { + return r3.Vector{}, err + } + + return r3.Vector{ + X: data.GetFields()["X"].GetNumberValue(), + Y: data.GetFields()["Y"].GetNumberValue(), + Z: data.GetFields()["Z"].GetNumberValue(), + }, nil +} + +// AngularVelocity returns the next angular velocity from the cache in the form of a spatialmath.AngularVelocity (r3.Vector). +func (replay *replayMovementSensor) AngularVelocity(ctx context.Context, extra map[string]interface{}) ( + spatialmath.AngularVelocity, error, +) { + replay.mu.Lock() + defer replay.mu.Unlock() + if replay.closed { + return spatialmath.AngularVelocity{}, errors.New("session closed") + } + + data, err := replay.getDataFromCache(ctx, "AngularVelocity") + if err != nil { + return spatialmath.AngularVelocity{}, err + } + + return spatialmath.AngularVelocity{ + X: data.GetFields()["X"].GetNumberValue(), + Y: data.GetFields()["Y"].GetNumberValue(), + Z: data.GetFields()["Z"].GetNumberValue(), + }, nil +} + +// LinearAcceleration returns the next linear acceleration from the cache in the form of an r3.Vector. +func (replay *replayMovementSensor) LinearAcceleration(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { + replay.mu.Lock() + defer replay.mu.Unlock() + if replay.closed { + return r3.Vector{}, errors.New("session closed") + } + + data, err := replay.getDataFromCache(ctx, "LinearAcceleration") + if err != nil { + return r3.Vector{}, err + } + + return r3.Vector{ + X: data.GetFields()["X"].GetNumberValue(), + Y: data.GetFields()["Y"].GetNumberValue(), + Z: data.GetFields()["Z"].GetNumberValue(), + }, nil +} + +// CompassHeading returns the next compass heading from the cache as a float64. +func (replay *replayMovementSensor) CompassHeading(ctx context.Context, extra map[string]interface{}) (float64, error) { + replay.mu.Lock() + defer replay.mu.Unlock() + if replay.closed { + return 0., errors.New("session closed") + } + + data, err := replay.getDataFromCache(ctx, "CompassHeading") + if err != nil { + return 0., err + } + + return data.GetFields()["Compass"].GetNumberValue(), nil +} + +// Orientation returns the next orientation from the cache as a spatialmath.Orientation created from a spatialmath.OrientationVector. +func (replay *replayMovementSensor) Orientation(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) { + replay.mu.Lock() + defer replay.mu.Unlock() + if replay.closed { + return nil, errors.New("session closed") + } + + data, err := replay.getDataFromCache(ctx, "Orientation") + if err != nil { + return nil, err + } + + return &spatialmath.OrientationVector{ + OX: data.GetFields()["OX"].GetNumberValue(), + OY: data.GetFields()["OY"].GetNumberValue(), + OZ: data.GetFields()["OZ"].GetNumberValue(), + Theta: data.GetFields()["Theta"].GetNumberValue(), + }, nil +} + +// Properties returns the available properties for the given replay movement sensor. +func (replay *replayMovementSensor) Properties(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { + return &movementsensor.Properties{ + LinearVelocitySupported: true, + AngularVelocitySupported: true, + OrientationSupported: true, + PositionSupported: true, + CompassHeadingSupported: true, + LinearAccelerationSupported: true, + }, nil +} + +// Accuracy is currently not defined for replay movement sensors. +func (replay *replayMovementSensor) Accuracy(ctx context.Context, extra map[string]interface{}) (map[string]float32, error) { + return map[string]float32{}, movementsensor.ErrMethodUnimplementedAccuracy +} + +// Close stops the replay movement sensor, closes its channels and its connections to the cloud. +func (replay *replayMovementSensor) Close(ctx context.Context) error { + replay.mu.Lock() + defer replay.mu.Unlock() + + replay.closed = true + replay.closeCloudConnection(ctx) + return nil +} + +// Readings returns all available data from the next entry stored in the cache. +func (replay *replayMovementSensor) Readings(ctx context.Context, extra map[string]interface{}) (map[string]interface{}, error) { + return movementsensor.Readings(ctx, replay, extra) +} + +// Reconfigure finishes the bring up of the replay movement sensor by evaluating given arguments and setting up the required cloud +// connection as well as updates all required parameters upon a reconfiguration attempt, restarting the cloud connection in the process. +func (replay *replayMovementSensor) Reconfigure(ctx context.Context, deps resource.Dependencies, conf resource.Config) error { + replay.mu.Lock() + defer replay.mu.Unlock() + if replay.closed { + return errors.New("session closed") + } + + replayMovementSensorConfig, err := resource.NativeConfig[*Config](conf) + if err != nil { + return err + } + + cloudConnSvc, err := resource.FromDependencies[cloud.ConnectionService](deps, cloud.InternalServiceName) + if err != nil { + return err + } + + // Update cloud connection if needed + if replay.cloudConnSvc != cloudConnSvc { + replay.closeCloudConnection(ctx) + replay.cloudConnSvc = cloudConnSvc + + if err := replay.initCloudConnection(ctx); err != nil { + replay.closeCloudConnection(ctx) + return errors.Wrap(err, "failure to connect to the cloud") + } + } + + if replayMovementSensorConfig.BatchSize == nil { + replay.limit = 1 + } else { + replay.limit = *replayMovementSensorConfig.BatchSize + } + + replay.cache = map[string][]*cacheEntry{} + for _, k := range methodList { + replay.cache[k] = nil + } + + replay.lastData = map[string]string{} + for _, k := range methodList { + replay.lastData[k] = "" + } + + replay.filter = &datapb.Filter{ + ComponentName: replayMovementSensorConfig.Source, + RobotId: replayMovementSensorConfig.RobotID, + LocationIds: []string{replayMovementSensorConfig.LocationID}, + OrganizationIds: []string{replayMovementSensorConfig.OrganizationID}, + Interval: &datapb.CaptureInterval{}, + } + + if replayMovementSensorConfig.Interval.Start != "" { + startTime, err := time.Parse(timeFormat, replayMovementSensorConfig.Interval.Start) + if err != nil { + replay.closeCloudConnection(ctx) + return errors.New("invalid time format for start time, missed during config validation") + } + replay.filter.Interval.Start = timestamppb.New(startTime) + } + + if replayMovementSensorConfig.Interval.End != "" { + endTime, err := time.Parse(timeFormat, replayMovementSensorConfig.Interval.End) + if err != nil { + replay.closeCloudConnection(ctx) + return errors.New("invalid time format for end time, missed during config validation") + } + replay.filter.Interval.End = timestamppb.New(endTime) + } + + return nil +} + +// updateCache will update the cache with an additional batch of data downloaded from the cloud via TabularDataByFilter based on the given +// filter, and the last data accessed. +func (replay *replayMovementSensor) updateCache(ctx context.Context, method string) error { + filter := replay.filter + filter.Method = method + + // Retrieve data from the cloud + resp, err := replay.dataClient.TabularDataByFilter(ctx, &datapb.TabularDataByFilterRequest{ + DataRequest: &datapb.DataRequest{ + Filter: filter, + Limit: replay.limit, + Last: replay.lastData[method], + SortOrder: datapb.Order_ORDER_ASCENDING, + }, + CountOnly: false, + }) + if err != nil { + return err + } + + // Check if data exists + if len(resp.GetData()) == 0 { + return ErrEndOfDataset + } + replay.lastData[method] = resp.GetLast() + + // Add data to associated cache + for _, dataResponse := range resp.Data { + entry := &cacheEntry{ + data: dataResponse.Data, + timeRequested: dataResponse.GetTimeRequested(), + timeReceived: dataResponse.GetTimeReceived(), + } + replay.cache[method] = append(replay.cache[method], entry) + } + + return nil +} + +// addGRPCMetadata adds timestamps from the data response to the gRPC response header if one is found in the context. +func addGRPCMetadata(ctx context.Context, timeRequested, timeReceived *timestamppb.Timestamp) error { + if stream := grpc.ServerTransportStreamFromContext(ctx); stream != nil { + var grpcMetadata metadata.MD = make(map[string][]string) + if timeRequested != nil { + grpcMetadata.Set(contextutils.TimeRequestedMetadataKey, timeRequested.AsTime().Format(time.RFC3339Nano)) + } + if timeReceived != nil { + grpcMetadata.Set(contextutils.TimeReceivedMetadataKey, timeReceived.AsTime().Format(time.RFC3339Nano)) + } + if err := grpc.SetHeader(ctx, grpcMetadata); err != nil { + return err + } + } + + return nil +} + +// extractDataAndMetadata retrieves the next cached data and removes it from the cache. It assumes the write lock is being held. +func (replay *replayMovementSensor) getDataFromCache(ctx context.Context, method string) (*structpb.Struct, error) { + // If no data remains in the cache, download a new batch of data + if len(replay.cache[method]) == 0 { + if err := replay.updateCache(ctx, method); err != nil { + return nil, errors.Wrapf(err, "could not update the cache") + } + } + + // Grab the next cached data and update the associated cache + methodCache := replay.cache[method] + entry := methodCache[0] + replay.cache[method] = methodCache[1:] + + if err := addGRPCMetadata(ctx, entry.timeRequested, entry.timeReceived); err != nil { + return nil, errors.Wrapf(err, "adding GRPC metadata failed") + } + + return entry.data, nil +} + +// closeCloudConnection closes all parts of the cloud connection used by the replay movement sensor. +func (replay *replayMovementSensor) closeCloudConnection(ctx context.Context) { + if replay.cloudConn != nil { + goutils.UncheckedError(replay.cloudConn.Close()) + } + + if replay.cloudConnSvc != nil { + goutils.UncheckedError(replay.cloudConnSvc.Close(ctx)) + } +} + +// initCloudConnection creates a rpc client connection and data service. +func (replay *replayMovementSensor) initCloudConnection(ctx context.Context) error { + ctx, cancel := context.WithTimeout(ctx, grpcConnectionTimeout) + defer cancel() + + _, conn, err := replay.cloudConnSvc.AcquireConnection(ctx) + if err != nil { + return err + } + dataServiceClient := datapb.NewDataServiceClient(conn) + + replay.cloudConn = conn + replay.dataClient = dataServiceClient + return nil +} diff --git a/components/movementsensor/replay/replay_test.go b/components/movementsensor/replay/replay_test.go new file mode 100644 index 00000000000..e24a2d4df12 --- /dev/null +++ b/components/movementsensor/replay/replay_test.go @@ -0,0 +1,770 @@ +package replay + +import ( + "context" + "fmt" + "testing" + + "github.com/golang/geo/r3" + geo "github.com/kellydunn/golang-geo" + "github.com/pkg/errors" + "go.viam.com/test" + "go.viam.com/utils" + "google.golang.org/grpc" + + "go.viam.com/rdk/components/movementsensor" + "go.viam.com/rdk/internal/cloud" + "go.viam.com/rdk/resource" + "go.viam.com/rdk/spatialmath" + "go.viam.com/rdk/testutils" + "go.viam.com/rdk/utils/contextutils" +) + +var ( + validSource = "source" + validRobotID = "robot_id" + validOrganizationID = "organization_id" + validLocationID = "location_id" + + batchSizeZero = uint64(0) + batchSizeNonZero = uint64(5) + batchSize4 = uint64(4) + batchSizeOutOfBounds = uint64(50000) + + positionPointData = []*geo.Point{ + geo.NewPoint(0, 0), + geo.NewPoint(1, 0), + geo.NewPoint(.5, 0), + geo.NewPoint(0, .4), + geo.NewPoint(1, .4), + } + + positionAltitudeData = []float64{0, 1, 2, 3, 4} + + linearAccelerationData = []r3.Vector{ + {X: 0, Y: 0, Z: 0}, + {X: 1, Y: 0, Z: 0}, + {X: 0, Y: 1, Z: 0}, + {X: 0, Y: 2, Z: 0}, + {X: 0, Y: 3, Z: 3}, + } + + linearVelocityData = []r3.Vector{ + {X: 0, Y: 0, Z: 0}, + {X: 1, Y: 0, Z: 0}, + {X: 0, Y: 1, Z: 11}, + {X: 1, Y: 4, Z: 0}, + {X: 0, Y: 3, Z: 3}, + {X: 3, Y: 2, Z: 7}, + {X: 0, Y: 3, Z: 3}, + {X: 3, Y: 2, Z: 7}, + {X: 0, Y: 3, Z: 311}, + } + + angularVelocityData = []spatialmath.AngularVelocity{ + {X: 0, Y: 0, Z: 0}, + {X: 1, Y: 0, Z: 2}, + {X: 0, Y: 1, Z: 0}, + {X: 0, Y: 5, Z: 2}, + {X: 2, Y: 3, Z: 3}, + {X: 1, Y: 2, Z: 0}, + {X: 0, Y: 0, Z: 12}, + } + + compassHeadingData = []float64{0, 1, 2, 3, 4, 5, 6, 4, 3, 2, 1} + + orientationData = []*spatialmath.OrientationVector{ + {OX: 1, OY: 0, OZ: 1, Theta: 0}, + {OX: 2, OY: 1, OZ: 1, Theta: 0}, + } + + defaultMaxDataLength = map[string]int{ + "Position": len(positionPointData), + "LinearAcceleration": len(linearAccelerationData), + "AngularVelocity": len(angularVelocityData), + "LinearVelocity": len(linearVelocityData), + "Orientation": len(orientationData), + "CompassHeading": len(compassHeadingData), + } + + defaultMinDataLength = map[string]int{ + "Position": 0, + "LinearAcceleration": 0, + "AngularVelocity": 0, + "LinearVelocity": 0, + "Orientation": 0, + "CompassHeading": 0, + } + + defaultReplayMovementSensorFunction = "LinearAcceleration" +) + +func TestNewReplayMovementSensor(t *testing.T) { + ctx := context.Background() + + cases := []struct { + description string + cfg *Config + expectedErr error + validCloudConnection bool + }{ + { + description: "valid config with internal cloud service", + cfg: &Config{ + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + }, + validCloudConnection: true, + }, + { + description: "bad internal cloud service", + cfg: &Config{ + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + }, + validCloudConnection: false, + expectedErr: errors.New("failure to connect to the cloud: cloud connection error"), + }, + { + description: "bad start timestamp", + cfg: &Config{ + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + Interval: TimeInterval{ + Start: "bad timestamp", + }, + }, + validCloudConnection: true, + expectedErr: errors.New("invalid time format for start time, missed during config validation"), + }, + { + description: "bad end timestamp", + cfg: &Config{ + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + Interval: TimeInterval{ + End: "bad timestamp", + }, + }, + validCloudConnection: true, + expectedErr: errors.New("invalid time format for end time, missed during config validation"), + }, + } + + for _, tt := range cases { + t.Run(tt.description, func(t *testing.T) { + replay, _, serverClose, err := createNewReplayMovementSensor(ctx, t, tt.cfg, tt.validCloudConnection) + if err != nil { + test.That(t, err, test.ShouldBeError, tt.expectedErr) + test.That(t, replay, test.ShouldBeNil) + } else { + test.That(t, err, test.ShouldBeNil) + test.That(t, replay, test.ShouldNotBeNil) + + err = replay.Close(ctx) + test.That(t, err, test.ShouldBeNil) + } + + if tt.validCloudConnection { + test.That(t, serverClose(), test.ShouldBeNil) + } + }) + } +} + +func TestReplayMovementSensorFunctions(t *testing.T) { + ctx := context.Background() + + cases := []struct { + description string + methods []string + cfg *Config + startFileNum map[string]int + endFileNum map[string]int + }{ + { + description: "Calling method no filter", + cfg: &Config{ + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + }, + methods: []string{"LinearAcceleration", "AngularVelocity", "Position", "LinearVelocity", "CompassHeading", "Orientation"}, + startFileNum: defaultMinDataLength, + endFileNum: defaultMaxDataLength, + }, + { + description: "Calling method with valid filter", + cfg: &Config{ + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + }, + methods: []string{"LinearAcceleration", "AngularVelocity", "Position", "LinearVelocity", "CompassHeading", "Orientation"}, + startFileNum: defaultMinDataLength, + endFileNum: defaultMaxDataLength, + }, + { + description: "Calling method with bad source", + cfg: &Config{ + Source: "bad_source", + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + }, + methods: []string{"LinearAcceleration"}, + startFileNum: map[string]int{"LinearAcceleration": -1}, + endFileNum: map[string]int{"LinearAcceleration": -1}, + }, + { + description: "Calling method with bad robot_id", + cfg: &Config{ + Source: validSource, + RobotID: "bad_robot_id", + LocationID: validLocationID, + OrganizationID: validOrganizationID, + }, + methods: []string{"LinearAcceleration"}, + startFileNum: map[string]int{"LinearAcceleration": -1}, + endFileNum: map[string]int{"LinearAcceleration": -1}, + }, + { + description: "Calling method with bad location_id", + cfg: &Config{ + Source: validSource, + RobotID: validRobotID, + LocationID: "bad_location_id", + OrganizationID: validOrganizationID, + }, + methods: []string{"LinearAcceleration"}, + startFileNum: map[string]int{"LinearAcceleration": -1}, + endFileNum: map[string]int{"LinearAcceleration": -1}, + }, + { + description: "Calling method with bad organization_id", + cfg: &Config{ + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: "bad_organization_id", + }, + methods: []string{"LinearAcceleration"}, + startFileNum: map[string]int{"LinearAcceleration": -1}, + endFileNum: map[string]int{"LinearAcceleration": -1}, + }, + { + description: "Calling method with filter no data", + cfg: &Config{ + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + BatchSize: &batchSizeNonZero, + Interval: TimeInterval{ + Start: "2000-01-01T12:00:30Z", + End: "2000-01-01T12:00:40Z", + }, + }, + methods: []string{"LinearAcceleration"}, + startFileNum: map[string]int{"LinearAcceleration": -1}, + endFileNum: map[string]int{"LinearAcceleration": -1}, + }, + { + description: "Calling methods with end filter", + cfg: &Config{ + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + BatchSize: &batchSizeNonZero, + Interval: TimeInterval{ + End: "2000-01-01T12:00:03Z", + }, + }, + methods: []string{"LinearAcceleration", "AngularVelocity", "Position", "LinearVelocity", "CompassHeading", "Orientation"}, + startFileNum: defaultMinDataLength, + endFileNum: map[string]int{ + "LinearAcceleration": 3, + "AngularVelocity": 3, + "Position": 3, + "LinearVelocity": 3, + "CompassHeading": 3, + "Orientation": 2, + }, + }, + { + description: "Calling methods with start filter", + cfg: &Config{ + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + BatchSize: &batchSizeNonZero, + Interval: TimeInterval{ + Start: "2000-01-01T12:00:02Z", + }, + }, + methods: []string{"LinearAcceleration", "AngularVelocity", "Position", "LinearVelocity", "CompassHeading", "Orientation"}, + startFileNum: map[string]int{ + "LinearAcceleration": 2, + "AngularVelocity": 2, + "Position": 2, + "LinearVelocity": 2, + "CompassHeading": 2, + "Orientation": -1, + }, + endFileNum: map[string]int{ + "LinearAcceleration": defaultMaxDataLength["LinearAcceleration"], + "AngularVelocity": defaultMaxDataLength["AngularVelocity"], + "Position": defaultMaxDataLength["Position"], + "LinearVelocity": defaultMaxDataLength["LinearVelocity"], + "CompassHeading": defaultMaxDataLength["CompassHeading"], + "Orientation": -1, + }, + }, + { + description: "Calling methods with start and end filter", + cfg: &Config{ + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + BatchSize: &batchSizeNonZero, + Interval: TimeInterval{ + Start: "2000-01-01T12:00:01Z", + End: "2000-01-01T12:00:03Z", + }, + }, + methods: []string{"LinearAcceleration", "AngularVelocity", "Position", "LinearVelocity", "CompassHeading", "Orientation"}, + startFileNum: map[string]int{ + "LinearAcceleration": 1, + "AngularVelocity": 1, + "Position": 1, + "LinearVelocity": 1, + "CompassHeading": 1, + "Orientation": 1, + }, + endFileNum: map[string]int{ + "LinearAcceleration": 3, + "AngularVelocity": 3, + "Position": 3, + "LinearVelocity": 3, + "CompassHeading": 3, + "Orientation": defaultMaxDataLength["Orientation"], + }, + }, + } + + for _, tt := range cases { + t.Run(tt.description, func(t *testing.T) { + replay, _, serverClose, err := createNewReplayMovementSensor(ctx, t, tt.cfg, true) + test.That(t, err, test.ShouldBeNil) + test.That(t, replay, test.ShouldNotBeNil) + + for _, method := range tt.methods { + // Iterate through all files that meet the provided filter + if tt.startFileNum[method] != -1 { + for i := tt.startFileNum[method]; i < tt.endFileNum[method]; i++ { + testReplayMovementSensorMethod(ctx, t, replay, method, i, true) + } + } + // Confirm the end of the dataset was reached when expected + testReplayMovementSensorMethod(ctx, t, replay, method, -1, false) + } + + err = replay.Close(ctx) + test.That(t, err, test.ShouldBeNil) + + test.That(t, serverClose(), test.ShouldBeNil) + }) + } +} + +func TestReplayMovementSensorConfigValidation(t *testing.T) { + cases := []struct { + description string + cfg *Config + expectedDeps []string + expectedErr error + }{ + { + description: "Valid config and no timestamp", + cfg: &Config{ + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + Interval: TimeInterval{}, + }, + expectedDeps: []string{cloud.InternalServiceName.String()}, + }, + { + description: "Valid config with start timestamp", + cfg: &Config{ + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + Interval: TimeInterval{ + Start: "2000-01-01T12:00:00Z", + }, + }, + expectedDeps: []string{cloud.InternalServiceName.String()}, + }, + { + description: "Valid config with end timestamp", + cfg: &Config{ + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + Interval: TimeInterval{ + End: "2000-01-01T12:00:00Z", + }, + }, + expectedDeps: []string{cloud.InternalServiceName.String()}, + }, + { + description: "Valid config with start and end timestamps", + cfg: &Config{ + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + Interval: TimeInterval{ + Start: "2000-01-01T12:00:00Z", + End: "2000-01-01T12:00:01Z", + }, + }, + expectedDeps: []string{cloud.InternalServiceName.String()}, + }, + { + description: "Invalid config no source", + cfg: &Config{ + Interval: TimeInterval{}, + }, + expectedErr: utils.NewConfigValidationFieldRequiredError("", validSource), + }, + { + description: "Invalid config no robot_id", + cfg: &Config{ + Source: validSource, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + Interval: TimeInterval{}, + }, + expectedErr: utils.NewConfigValidationFieldRequiredError("", validRobotID), + }, + { + description: "Invalid config no location_id", + cfg: &Config{ + Source: validSource, + RobotID: validRobotID, + OrganizationID: validOrganizationID, + Interval: TimeInterval{}, + }, + expectedErr: utils.NewConfigValidationFieldRequiredError("", validLocationID), + }, + { + description: "Invalid config no organization_id", + cfg: &Config{ + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + Interval: TimeInterval{}, + }, + expectedErr: utils.NewConfigValidationFieldRequiredError("", validOrganizationID), + }, + { + description: "Invalid config with bad start timestamp format", + cfg: &Config{ + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + Interval: TimeInterval{ + Start: "gibberish", + }, + }, + expectedErr: errors.New("invalid time format for start time (UTC), use RFC3339"), + }, + { + description: "Invalid config with bad end timestamp format", + cfg: &Config{ + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + Interval: TimeInterval{ + End: "gibberish", + }, + }, + expectedErr: errors.New("invalid time format for end time (UTC), use RFC3339"), + }, + { + description: "Invalid config with bad start timestamp", + cfg: &Config{ + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + Interval: TimeInterval{ + Start: "3000-01-01T12:00:00Z", + }, + }, + expectedErr: errors.New("invalid config, start time (UTC) must be in the past"), + }, + { + description: "Invalid config with bad end timestamp", + cfg: &Config{ + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + Interval: TimeInterval{ + End: "3000-01-01T12:00:00Z", + }, + }, + expectedErr: errors.New("invalid config, end time (UTC) must be in the past"), + }, + { + description: "Invalid config with start after end timestamps", + cfg: &Config{ + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + Interval: TimeInterval{ + Start: "2000-01-01T12:00:01Z", + End: "2000-01-01T12:00:00Z", + }, + }, + expectedErr: errors.New("invalid config, end time (UTC) must be after start time (UTC)"), + }, + { + description: "Invalid config with batch size of 0", + cfg: &Config{ + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + Interval: TimeInterval{ + Start: "2000-01-01T12:00:00Z", + End: "2000-01-01T12:00:01Z", + }, + BatchSize: &batchSizeZero, + }, + expectedErr: errors.New("batch_size must be between 1 and 1000"), + }, + { + description: "Invalid config with batch size above max", + cfg: &Config{ + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + Interval: TimeInterval{ + Start: "2000-01-01T12:00:00Z", + End: "2000-01-01T12:00:01Z", + }, + BatchSize: &batchSizeOutOfBounds, + }, + expectedErr: errors.New("batch_size must be between 1 and 1000"), + }, + } + + for _, tt := range cases { + t.Run(tt.description, func(t *testing.T) { + deps, err := tt.cfg.Validate("") + if tt.expectedErr != nil { + test.That(t, err, test.ShouldBeError, tt.expectedErr) + } else { + test.That(t, err, test.ShouldBeNil) + } + test.That(t, deps, test.ShouldResemble, tt.expectedDeps) + }) + } +} + +func TestReplayMovementSensorProperties(t *testing.T) { + // Construct replay movement sensor. + ctx := context.Background() + cfg := &Config{ + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + BatchSize: &batchSizeNonZero, + } + replay, _, serverClose, err := createNewReplayMovementSensor(ctx, t, cfg, true) + test.That(t, err, test.ShouldBeNil) + test.That(t, replay, test.ShouldNotBeNil) + + props, err := replay.Properties(ctx, map[string]interface{}{}) + test.That(t, err, test.ShouldBeNil) + test.That(t, props.PositionSupported, test.ShouldBeTrue) + test.That(t, props.OrientationSupported, test.ShouldBeTrue) + test.That(t, props.AngularVelocitySupported, test.ShouldBeTrue) + test.That(t, props.LinearAccelerationSupported, test.ShouldBeTrue) + test.That(t, props.LinearVelocitySupported, test.ShouldBeTrue) + test.That(t, props.CompassHeadingSupported, test.ShouldBeTrue) + + err = replay.Close(ctx) + test.That(t, err, test.ShouldBeNil) + + test.That(t, serverClose(), test.ShouldBeNil) +} + +func TestUnimplementedFunctionAccuracy(t *testing.T) { + ctx := context.Background() + + cfg := &Config{ + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + } + replay, _, serverClose, err := createNewReplayMovementSensor(ctx, t, cfg, true) + test.That(t, err, test.ShouldBeNil) + + acc, err := replay.Accuracy(ctx, map[string]interface{}{}) + test.That(t, err, test.ShouldResemble, movementsensor.ErrMethodUnimplementedAccuracy) + test.That(t, acc, test.ShouldResemble, map[string]float32{}) + + err = replay.Close(ctx) + test.That(t, err, test.ShouldBeNil) + + test.That(t, serverClose(), test.ShouldBeNil) +} + +func TestReplayMovementSensorReadings(t *testing.T) { + ctx := context.Background() + + cfg := &Config{ + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + } + replay, _, serverClose, err := createNewReplayMovementSensor(ctx, t, cfg, true) + test.That(t, err, test.ShouldBeNil) + + // For loop depends on the data length of orientation as it has the fewest points of data + for i := 0; i < defaultMaxDataLength["Orientation"]; i++ { + readings, err := replay.Readings(ctx, map[string]interface{}{}) + test.That(t, err, test.ShouldBeNil) + test.That(t, readings["position"], test.ShouldResemble, positionPointData[i]) + test.That(t, readings["altitude"], test.ShouldResemble, positionAltitudeData[i]) + test.That(t, readings["linear_velocity"], test.ShouldResemble, linearVelocityData[i]) + test.That(t, readings["linear_acceleration"], test.ShouldResemble, linearAccelerationData[i]) + test.That(t, readings["angular_velocity"], test.ShouldResemble, angularVelocityData[i]) + test.That(t, readings["compass"], test.ShouldResemble, compassHeadingData[i]) + test.That(t, readings["orientation"], test.ShouldResemble, orientationData[i]) + } + + readings, err := replay.Readings(ctx, map[string]interface{}{}) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, ErrEndOfDataset.Error()) + test.That(t, readings, test.ShouldBeNil) + + err = replay.Close(ctx) + test.That(t, err, test.ShouldBeNil) + + test.That(t, serverClose(), test.ShouldBeNil) +} + +func TestReplayMovementSensorTimestampsMetadata(t *testing.T) { + // Construct replay movement sensor. + ctx := context.Background() + cfg := &Config{ + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + BatchSize: &batchSizeNonZero, + } + replay, _, serverClose, err := createNewReplayMovementSensor(ctx, t, cfg, true) + test.That(t, err, test.ShouldBeNil) + test.That(t, replay, test.ShouldNotBeNil) + + // Repeatedly call the default method, checking for timestamps in the gRPC header. + for i := 0; i < defaultMaxDataLength[defaultReplayMovementSensorFunction]; i++ { + serverStream := testutils.NewServerTransportStream() + ctx = grpc.NewContextWithServerTransportStream(ctx, serverStream) + + testReplayMovementSensorMethod(ctx, t, replay, defaultReplayMovementSensorFunction, i, true) + + expectedTimeReq := fmt.Sprintf(testTime, i) + expectedTimeRec := fmt.Sprintf(testTime, i+1) + + actualTimeReq := serverStream.Value(contextutils.TimeRequestedMetadataKey)[0] + actualTimeRec := serverStream.Value(contextutils.TimeReceivedMetadataKey)[0] + + test.That(t, expectedTimeReq, test.ShouldEqual, actualTimeReq) + test.That(t, expectedTimeRec, test.ShouldEqual, actualTimeRec) + } + + // Confirm the end of the dataset was reached when expected + testReplayMovementSensorMethod(ctx, t, replay, defaultReplayMovementSensorFunction, -1, false) + + err = replay.Close(ctx) + test.That(t, err, test.ShouldBeNil) + + test.That(t, serverClose(), test.ShouldBeNil) +} + +func TestReplayMovementSensorReconfigure(t *testing.T) { + // Construct replay movement sensor + cfg := &Config{ + Source: validSource, + RobotID: validRobotID, + LocationID: validLocationID, + OrganizationID: validOrganizationID, + } + ctx := context.Background() + replay, deps, serverClose, err := createNewReplayMovementSensor(ctx, t, cfg, true) + test.That(t, err, test.ShouldBeNil) + test.That(t, replay, test.ShouldNotBeNil) + + // Call default movement sensor function to iterate through a few files + for i := 0; i < 3; i++ { + testReplayMovementSensorMethod(ctx, t, replay, defaultReplayMovementSensorFunction, i, true) + } + + // Reconfigure with a new batch size + cfg = &Config{Source: validSource, BatchSize: &batchSize4} + replay.Reconfigure(ctx, deps, resource.Config{ConvertedAttributes: cfg}) + + // Call the default movement sensor function a couple more times, ensuring that we start over from + // the beginning of the dataset after calling Reconfigure + for i := 0; i < 5; i++ { + testReplayMovementSensorMethod(ctx, t, replay, defaultReplayMovementSensorFunction, i, true) + } + + // Reconfigure again, batch size 1 + cfg = &Config{Source: validSource, BatchSize: &batchSizeNonZero} + replay.Reconfigure(ctx, deps, resource.Config{ConvertedAttributes: cfg}) + + // Again verify dataset starts from beginning + for i := 0; i < defaultMaxDataLength[defaultReplayMovementSensorFunction]; i++ { + testReplayMovementSensorMethod(ctx, t, replay, defaultReplayMovementSensorFunction, i, true) + } + + // Confirm the end of the dataset was reached when expected + testReplayMovementSensorMethod(ctx, t, replay, defaultReplayMovementSensorFunction, -1, false) + + err = replay.Close(ctx) + test.That(t, err, test.ShouldBeNil) + + test.That(t, serverClose(), test.ShouldBeNil) +} diff --git a/components/movementsensor/replay/replay_utils_test.go b/components/movementsensor/replay/replay_utils_test.go new file mode 100644 index 00000000000..bd8047a8260 --- /dev/null +++ b/components/movementsensor/replay/replay_utils_test.go @@ -0,0 +1,340 @@ +package replay + +import ( + "context" + "fmt" + "math" + "net" + "strconv" + "testing" + "time" + + "github.com/edaniels/golog" + "github.com/golang/geo/r3" + "github.com/pkg/errors" + datapb "go.viam.com/api/app/data/v1" + "go.viam.com/test" + "go.viam.com/utils/rpc" + "google.golang.org/protobuf/types/known/structpb" + "google.golang.org/protobuf/types/known/timestamppb" + + "go.viam.com/rdk/components/movementsensor" + viamgrpc "go.viam.com/rdk/grpc" + "go.viam.com/rdk/internal/cloud" + cloudinject "go.viam.com/rdk/internal/testutils/inject" + "go.viam.com/rdk/resource" + "go.viam.com/rdk/robot" + "go.viam.com/rdk/spatialmath" + "go.viam.com/rdk/testutils/inject" +) + +const ( + testTime = "2000-01-01T12:00:%02dZ" +) + +// mockDataServiceServer is a struct that includes unimplemented versions of all the Data Service endpoints. These +// can be overwritten to allow developers to trigger desired behaviors during testing. +type mockDataServiceServer struct { + datapb.UnimplementedDataServiceServer +} + +// TabularDataByFilter is a mocked version of the Data Service function of a similar name. It returns a response with +// data corresponding to the stored data associated with that function and index. +func (mDServer *mockDataServiceServer) TabularDataByFilter(ctx context.Context, req *datapb.TabularDataByFilterRequest, +) (*datapb.TabularDataByFilterResponse, error) { + filter := req.DataRequest.GetFilter() + last := req.DataRequest.GetLast() + limit := req.DataRequest.GetLimit() + + var dataset []*datapb.TabularData + var dataIndex int + var err error + for i := 0; i < int(limit); i++ { + dataIndex, err = getNextDataAfterFilter(filter, last) + if err != nil { + if i == 0 { + return nil, err + } + continue + } + + // Call desired function + data := createDataByMovementSensorMethod(filter.Method, dataIndex) + + timeReq, timeRec, err := timestampsFromIndex(dataIndex) + if err != nil { + return nil, err + } + + last = fmt.Sprint(dataIndex) + + tabularData := &datapb.TabularData{ + Data: data, + TimeRequested: timeReq, + TimeReceived: timeRec, + } + dataset = append(dataset, tabularData) + } + + // Construct response + resp := &datapb.TabularDataByFilterResponse{ + Data: dataset, + Last: last, + } + + return resp, nil +} + +// timestampsFromIndex uses the index of the data to generate a timeReceived and timeRequested for testing. +func timestampsFromIndex(index int) (*timestamppb.Timestamp, *timestamppb.Timestamp, error) { + timeReq, err := time.Parse(time.RFC3339, fmt.Sprintf(testTime, index)) + if err != nil { + return nil, nil, errors.Wrap(err, "failed parsing time") + } + timeRec := timeReq.Add(time.Second) + return timestamppb.New(timeReq), timestamppb.New(timeRec), nil +} + +// getNextDataAfterFilter returns the index of the next data based on the provided. +func getNextDataAfterFilter(filter *datapb.Filter, last string) (int, error) { + // Basic component part (source) filter + if filter.ComponentName != "" && filter.ComponentName != validSource { + return 0, ErrEndOfDataset + } + + // Basic robot_id filter + if filter.RobotId != "" && filter.RobotId != validRobotID { + return 0, ErrEndOfDataset + } + + // Basic location_id filter + if len(filter.LocationIds) == 0 { + return 0, errors.New("issue occurred with transmitting LocationIds to the cloud") + } + if filter.LocationIds[0] != "" && filter.LocationIds[0] != validLocationID { + return 0, ErrEndOfDataset + } + + // Basic organization_id filter + if len(filter.OrganizationIds) == 0 { + return 0, errors.New("issue occurred with transmitting OrganizationIds to the cloud") + } + if filter.OrganizationIds[0] != "" && filter.OrganizationIds[0] != validOrganizationID { + return 0, ErrEndOfDataset + } + + // Apply the time-based filter based on the seconds value in the start and end fields. Because our mock data + // does not have timestamps associated with them but are ordered we can approximate the filtering + // by sorting for the data in the list whose index is after the start second count and before the end second count. + // For example, if there are 15 entries the start time is 2000-01-01T12:00:10Z and the end time is 2000-01-01T12:00:14Z, + // we will return data from indices 10 to 14. + startIntervalIndex := 0 + endIntervalIndex := math.MaxInt + availableDataNum := defaultMaxDataLength[filter.Method] + + if filter.Interval.Start != nil { + startIntervalIndex = filter.Interval.Start.AsTime().Second() + } + if filter.Interval.End != nil { + endIntervalIndex = filter.Interval.End.AsTime().Second() + } + if last == "" { + return checkDataEndCondition(startIntervalIndex, endIntervalIndex, availableDataNum) + } + lastFileNum, err := strconv.Atoi(last) + if err != nil { + return 0, err + } + + return checkDataEndCondition(lastFileNum+1, endIntervalIndex, availableDataNum) +} + +// checkDataEndCondition will return the index of the data to be returned after checking the amount of data available and the end +// internal condition. +func checkDataEndCondition(i, endIntervalIndex, availableDataNum int) (int, error) { + if i < endIntervalIndex && i < availableDataNum { + return i, nil + } + return 0, ErrEndOfDataset +} + +// createMockCloudDependencies creates a mockDataServiceServer and rpc client connection to it which is then +// stored in a mockCloudConnectionService. +func createMockCloudDependencies(ctx context.Context, t *testing.T, logger golog.Logger, b bool) (resource.Dependencies, func() error) { + listener, err := net.Listen("tcp", "localhost:0") + test.That(t, err, test.ShouldBeNil) + rpcServer, err := rpc.NewServer(logger, rpc.WithUnauthenticated()) + test.That(t, err, test.ShouldBeNil) + + test.That(t, rpcServer.RegisterServiceServer( + ctx, + &datapb.DataService_ServiceDesc, + &mockDataServiceServer{}, + datapb.RegisterDataServiceHandlerFromEndpoint, + ), test.ShouldBeNil) + + go rpcServer.Serve(listener) + + conn, err := viamgrpc.Dial(ctx, listener.Addr().String(), logger) + test.That(t, err, test.ShouldBeNil) + + mockCloudConnectionService := &cloudinject.CloudConnectionService{ + Named: cloud.InternalServiceName.AsNamed(), + Conn: conn, + } + if !b { + mockCloudConnectionService.AcquireConnectionErr = errors.New("cloud connection error") + } + + r := &inject.Robot{} + rs := map[resource.Name]resource.Resource{} + rs[cloud.InternalServiceName] = mockCloudConnectionService + r.MockResourcesFromMap(rs) + + return resourcesFromDeps(t, r, []string{cloud.InternalServiceName.String()}), rpcServer.Stop +} + +// createNewReplayMovementSensor will create a new replay movement sensor based on the provided config with either +// a valid or invalid data client. +func createNewReplayMovementSensor(ctx context.Context, t *testing.T, replayMovementSensorCfg *Config, validDeps bool, +) (movementsensor.MovementSensor, resource.Dependencies, func() error, error) { + logger := golog.NewTestLogger(t) + + resources, closeRPCFunc := createMockCloudDependencies(ctx, t, logger, validDeps) + + cfg := resource.Config{ConvertedAttributes: replayMovementSensorCfg} + replay, err := newReplayMovementSensor(ctx, resources, cfg, logger) + + return replay, resources, closeRPCFunc, err +} + +// resourcesFromDeps returns a list of dependencies from the provided robot. +func resourcesFromDeps(t *testing.T, r robot.Robot, deps []string) resource.Dependencies { + t.Helper() + resources := resource.Dependencies{} + for _, dep := range deps { + resName, err := resource.NewFromString(dep) + test.That(t, err, test.ShouldBeNil) + res, err := r.ResourceByName(resName) + if err == nil { + // some resources are weakly linked + resources[resName] = res + } + } + return resources +} + +// createDataByMovementSensorMethod will create the mocked structpb.Struct containing the next data returned by calls in tabular data. +func createDataByMovementSensorMethod(method string, index int) *structpb.Struct { + var data structpb.Struct + switch method { + case "Position": + data.Fields = map[string]*structpb.Value{ + "Latitude": structpb.NewNumberValue(positionPointData[index].Lat()), + "Longitude": structpb.NewNumberValue(positionPointData[index].Lng()), + "Altitude": structpb.NewNumberValue(positionAltitudeData[index]), + } + case "LinearAcceleration": + data.Fields = map[string]*structpb.Value{ + "X": structpb.NewNumberValue(linearAccelerationData[index].X), + "Y": structpb.NewNumberValue(linearAccelerationData[index].Y), + "Z": structpb.NewNumberValue(linearAccelerationData[index].Z), + } + case "AngularVelocity": + data.Fields = map[string]*structpb.Value{ + "X": structpb.NewNumberValue(angularVelocityData[index].X), + "Y": structpb.NewNumberValue(angularVelocityData[index].Y), + "Z": structpb.NewNumberValue(angularVelocityData[index].Z), + } + case "LinearVelocity": + data.Fields = map[string]*structpb.Value{ + "X": structpb.NewNumberValue(linearVelocityData[index].X), + "Y": structpb.NewNumberValue(linearVelocityData[index].Y), + "Z": structpb.NewNumberValue(linearVelocityData[index].Z), + } + case "Orientation": + data.Fields = map[string]*structpb.Value{ + "OX": structpb.NewNumberValue(orientationData[index].OX), + "OY": structpb.NewNumberValue(orientationData[index].OY), + "OZ": structpb.NewNumberValue(orientationData[index].OZ), + "Theta": structpb.NewNumberValue(orientationData[index].Theta), + } + case "CompassHeading": + data.Fields = map[string]*structpb.Value{ + "Compass": structpb.NewNumberValue(compassHeadingData[index]), + } + } + return &data +} + +// testReplayMovementSensorMethod tests the specified replay movement sensor function, both success and failure cases. +func testReplayMovementSensorMethod(ctx context.Context, t *testing.T, replay movementsensor.MovementSensor, method string, + i int, success bool, +) { + var extra map[string]interface{} + switch method { + case "Position": + point, altitude, err := replay.Position(ctx, extra) + if success { + test.That(t, err, test.ShouldBeNil) + test.That(t, point, test.ShouldResemble, positionPointData[i]) + test.That(t, altitude, test.ShouldResemble, positionAltitudeData[i]) + } else { + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, ErrEndOfDataset.Error()) + test.That(t, point, test.ShouldBeNil) + test.That(t, altitude, test.ShouldEqual, 0) + } + case "LinearVelocity": + data, err := replay.LinearVelocity(ctx, extra) + if success { + test.That(t, err, test.ShouldBeNil) + test.That(t, data, test.ShouldResemble, linearVelocityData[i]) + } else { + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, ErrEndOfDataset.Error()) + test.That(t, data, test.ShouldResemble, r3.Vector{}) + } + case "LinearAcceleration": + data, err := replay.LinearAcceleration(ctx, extra) + if success { + test.That(t, err, test.ShouldBeNil) + test.That(t, data, test.ShouldResemble, linearAccelerationData[i]) + } else { + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, ErrEndOfDataset.Error()) + test.That(t, data, test.ShouldResemble, r3.Vector{}) + } + case "AngularVelocity": + data, err := replay.AngularVelocity(ctx, extra) + if success { + test.That(t, err, test.ShouldBeNil) + test.That(t, data, test.ShouldResemble, angularVelocityData[i]) + } else { + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, ErrEndOfDataset.Error()) + test.That(t, data, test.ShouldResemble, spatialmath.AngularVelocity{}) + } + case "CompassHeading": + data, err := replay.CompassHeading(ctx, extra) + if success { + test.That(t, err, test.ShouldBeNil) + test.That(t, data, test.ShouldEqual, compassHeadingData[i]) + } else { + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, ErrEndOfDataset.Error()) + test.That(t, data, test.ShouldEqual, 0) + } + case "Orientation": + data, err := replay.Orientation(ctx, extra) + if success { + test.That(t, err, test.ShouldBeNil) + test.That(t, err, test.ShouldBeNil) + test.That(t, data, test.ShouldResemble, orientationData[i]) + } else { + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, ErrEndOfDataset.Error()) + test.That(t, data, test.ShouldBeNil) + } + } +} diff --git a/components/movementsensor/rtk_station/configure.go b/components/movementsensor/rtk_station/configure.go deleted file mode 100644 index 2eb959f1339..00000000000 --- a/components/movementsensor/rtk_station/configure.go +++ /dev/null @@ -1,399 +0,0 @@ -package rtkstation - -import ( - "context" - "fmt" - "io" - - "github.com/jacobsa/go-serial/serial" - "github.com/pkg/errors" - - "go.viam.com/rdk/resource" -) - -const ( - ubxSynch1 = 0xB5 - ubxSynch2 = 0x62 - ubxRtcm1005 = 0x05 // Stationary RTK reference ARP - ubxRtcm1074 = 0x4A // GPS MSM4 - ubxRtcm1084 = 0x54 // GLONASS MSM4 - ubxRtcm1094 = 0x5E // Galileo MSM4 - ubxRtcm1124 = 0x7C // BeiDou MSM4 - ubxRtcm1230 = 0xE6 // GLONASS code-phase biases, set to once every 10 seconds - uart2 = 2 - usb = 3 - ubxRtcmMsb = 0xF5 - ubxClassCfg = 0x06 - ubxCfgMsg = 0x01 - ubxCfgTmode3 = 0x71 - maxPayloadSize = 256 - ubxCfgCfg = 0x09 - - ubxNmeaMsb = 0xF0 // All NMEA enable commands have 0xF0 as MSB. Equal to UBX_CLASS_NMEA - ubxNmeaGga = 0x00 // GxGGA (Global positioning system fix data) - ubxNmeaGll = 0x01 // GxGLL (latitude and long, with time of position fix and status) - ubxNmeaGsa = 0x02 // GxGSA (GNSS DOP and Active satellites) - ubxNmeaGsv = 0x03 // GxGSV (GNSS satellites in view) - ubxNmeaRmc = 0x04 // GxRMC (Recommended minimum data) - ubxNmeaVtg = 0x05 // GxVTG (course over ground and Ground speed) - - svinModeEnable = 0x01 - svinModeDisable = 0x00 -) - -var rtcmMsgs = map[int]int{ - ubxRtcm1005: 1, - ubxRtcm1074: 1, - ubxRtcm1084: 1, - ubxRtcm1094: 1, - ubxRtcm1124: 1, - ubxRtcm1230: 5, -} - -var nmeaMsgs = map[int]int{ - ubxNmeaGll: 1, - ubxNmeaGsa: 1, - ubxNmeaGsv: 1, - ubxNmeaRmc: 1, - ubxNmeaVtg: 1, - ubxNmeaGga: 1, -} - -type configCommand struct { - correctionType string - portName string - baudRate uint - surveyIn string - - requiredAcc float64 - observationTime int - - msgsToEnable map[int]int - msgsToDisable map[int]int - - portID int - writePort io.ReadWriteCloser -} - -// ConfigureBaseRTKStation configures an RTK chip to act as a base station and send correction data. -func ConfigureBaseRTKStation(conf resource.Config) error { - newConf, err := resource.NativeConfig[*StationConfig](conf) - if err != nil { - return err - } - correctionType := newConf.CorrectionSource - requiredAcc := newConf.RequiredAccuracy - observationTime := newConf.RequiredTime - - c := &configCommand{ - correctionType: correctionType, - requiredAcc: requiredAcc, - observationTime: observationTime, - msgsToEnable: rtcmMsgs, // defaults - msgsToDisable: nmeaMsgs, // defaults - } - - // already configured - if c.surveyIn != timeMode { - return nil - } - - switch c.correctionType { - case serialStr: - err := c.serialConfigure(conf) - if err != nil { - return err - } - default: - return errors.Errorf("configuration not supported for %s", correctionType) - } - - if err := c.enableAll(ubxRtcmMsb); err != nil { - return err - } - - err = c.disableAll(ubxNmeaMsb) - if err != nil { - return err - } - - err = c.enableSVIN() - if err != nil { - return err - } - - return nil -} - -func (c *configCommand) serialConfigure(conf resource.Config) error { - newConf, err := resource.NativeConfig[*StationConfig](conf) - if err != nil { - return err - } - portName := newConf.SerialConfig.SerialCorrectionPath - if portName == "" { - return fmt.Errorf("serialCorrectionSource expected non-empty string for %q", correctionPathName) - } - c.portName = portName - - baudRate := newConf.SerialConfig.SerialCorrectionBaudRate - if baudRate == 0 { - baudRate = 9600 - } - c.baudRate = uint(baudRate) - c.portID = uart2 - - options := serial.OpenOptions{ - PortName: c.portName, - BaudRate: c.baudRate, - DataBits: 8, - StopBits: 1, - MinimumReadSize: 1, - } - - // Open the port - writePort, err := serial.Open(options) - if err != nil { - return err - } - c.writePort = writePort - - return nil -} - -func (c *configCommand) sendCommand(cls, id, msgLen int, payloadCfg []byte) error { - switch c.correctionType { - case serialStr: - _, err := c.sendCommandSerial(cls, id, msgLen, payloadCfg) - return err - default: - return errors.Errorf("configuration not supported for %s", c.correctionType) - } -} - -func (c *configCommand) sendCommandSerial(cls, id, msgLen int, payloadCfg []byte) ([]byte, error) { - checksumA, checksumB := calcChecksum(cls, id, msgLen, payloadCfg) - - // build packet to send over serial - byteSize := msgLen + 8 // header+checksum+payload - packet := make([]byte, byteSize) - - // header bytes - packet[0] = byte(ubxSynch1) - packet[1] = byte(ubxSynch2) - packet[2] = byte(cls) - packet[3] = byte(id) - packet[4] = byte(msgLen & 0xFF) // LSB - packet[5] = byte(msgLen >> 8) // MSB - - ind := 6 - for i := 0; i < msgLen; i++ { - packet[ind+i] = payloadCfg[i] - } - packet[len(packet)-1] = byte(checksumB) - packet[len(packet)-2] = byte(checksumA) - - _, err := c.writePort.Write(packet) - if err != nil { - return nil, err - } - - // then wait to capture a byte - buf := make([]byte, maxPayloadSize) - n, err := c.writePort.Read(buf) - if err != nil { - return nil, err - } - return buf[:n], nil -} - -func (c *configCommand) disableAll(msb int) error { - for msg := range c.msgsToDisable { - err := c.disableMessageCommand(msb, msg, c.portID) - if err != nil { - return err - } - } - err := c.saveAllConfigs() - if err != nil { - return err - } - return nil -} - -func (c *configCommand) enableAll(msb int) error { - for msg, sendRate := range c.msgsToEnable { - err := c.enableMessageCommand(msb, msg, c.portID, sendRate) - if err != nil { - return err - } - } - err := c.saveAllConfigs() - if err != nil { - return err - } - return nil -} - -//nolint:unused -func (c *configCommand) getSurveyMode() error { - cls := ubxClassCfg - id := ubxCfgTmode3 - payloadCfg := make([]byte, 40) - return c.sendCommand(cls, id, 0, payloadCfg) // set payloadcfg -} - -func (c *configCommand) enableSVIN() error { - err := c.setSurveyMode(svinModeEnable, c.requiredAcc, c.observationTime) - if err != nil { - return err - } - - err = c.saveAllConfigs() - if err != nil { - return err - } - return nil -} - -func (c *configCommand) setSurveyMode(mode int, requiredAccuracy float64, observationTime int) error { - payloadCfg := make([]byte, 40) - if len(payloadCfg) == 0 { - return errors.New("must specify payload") - } - - cls := ubxClassCfg - id := ubxCfgTmode3 - msgLen := 40 - - // payloadCfg should be loaded with poll response. Now modify only the bits we care about - payloadCfg[2] = byte(mode) // Set mode. Survey-In and Disabled are most common. Use ECEF (not LAT/LON/ALT). - - // svinMinDur is U4 (uint32_t) in seconds - payloadCfg[24] = byte(observationTime & 0xFF) // svinMinDur in seconds - payloadCfg[25] = byte((observationTime >> 8) & 0xFF) - payloadCfg[26] = byte((observationTime >> 16) & 0xFF) - payloadCfg[27] = byte((observationTime >> 24) & 0xFF) - - // svinAccLimit is U4 (uint32_t) in 0.1mm. - svinAccLimit := uint32(requiredAccuracy * 10000.0) // Convert m to 0.1mm - - payloadCfg[28] = byte(svinAccLimit & 0xFF) // svinAccLimit in 0.1mm increments - payloadCfg[29] = byte((svinAccLimit >> 8) & 0xFF) - payloadCfg[30] = byte((svinAccLimit >> 16) & 0xFF) - payloadCfg[31] = byte((svinAccLimit >> 24) & 0xFF) - - return c.sendCommand(cls, id, msgLen, payloadCfg) -} - -//nolint:lll,unused -func (c *configCommand) setStaticPosition(ecefXOrLat, ecefXOrLatHP, ecefYOrLon, ecefYOrLonHP, ecefZOrAlt, ecefZOrAltHP int, latLong bool) error { - cls := ubxClassCfg - id := ubxCfgTmode3 - msgLen := 40 - - payloadCfg := make([]byte, maxPayloadSize) - payloadCfg[2] = byte(2) - - if latLong { - payloadCfg[3] = (1 << 0) // Set mode to fixed. Use LAT/LON/ALT. - } - - // Set ECEF X or Lat - payloadCfg[4] = byte((ecefXOrLat >> 8 * 0) & 0xFF) // LSB - payloadCfg[5] = byte((ecefXOrLat >> 8 * 1) & 0xFF) - payloadCfg[6] = byte((ecefXOrLat >> 8 * 2) & 0xFF) - payloadCfg[7] = byte((ecefXOrLat >> 8 * 3) & 0xFF) // MSB - - // Set ECEF Y or Long - payloadCfg[8] = byte((ecefYOrLon >> 8 * 0) & 0xFF) // LSB - payloadCfg[9] = byte((ecefYOrLon >> 8 * 1) & 0xFF) - payloadCfg[10] = byte((ecefYOrLon >> 8 * 2) & 0xFF) - payloadCfg[11] = byte((ecefYOrLon >> 8 * 3) & 0xFF) // MSB - - // Set ECEF Z or Altitude - payloadCfg[12] = byte((ecefZOrAlt >> 8 * 0) & 0xFF) // LSB - payloadCfg[13] = byte((ecefZOrAlt >> 8 * 1) & 0xFF) - payloadCfg[14] = byte((ecefZOrAlt >> 8 * 2) & 0xFF) - payloadCfg[15] = byte((ecefZOrAlt >> 8 * 3) & 0xFF) // MSB - - // Set high precision parts - payloadCfg[16] = byte(ecefXOrLatHP) - payloadCfg[17] = byte(ecefYOrLonHP) - payloadCfg[18] = byte(ecefZOrAltHP) - - return c.sendCommand(cls, id, msgLen, payloadCfg) -} - -func (c *configCommand) disableMessageCommand(msgClass, messageNumber, portID int) error { - err := c.enableMessageCommand(msgClass, messageNumber, portID, 0) - if err != nil { - return err - } - return nil -} - -func (c *configCommand) enableMessageCommand(msgClass, messageNumber, portID, sendRate int) error { - // dont use current port settings actually - payloadCfg := make([]byte, maxPayloadSize) - - cls := ubxClassCfg - id := ubxCfgMsg - msgLen := 8 - - payloadCfg[0] = byte(msgClass) - payloadCfg[1] = byte(messageNumber) - payloadCfg[2+portID] = byte(sendRate) - // default to enable usb on with same sendRate - payloadCfg[2+usb] = byte(sendRate) - - return c.sendCommand(cls, id, msgLen, payloadCfg) -} - -func (c *configCommand) saveAllConfigs() error { - cls := ubxClassCfg - id := ubxCfgCfg - msgLen := 12 - - payloadCfg := make([]byte, maxPayloadSize) - - payloadCfg[4] = 0xFF - payloadCfg[5] = 0xFF - - return c.sendCommand(cls, id, msgLen, payloadCfg) -} - -// Close closes all open ports used in configuration. -func (c *configCommand) Close(ctx context.Context) error { - // close port reader if serial - if c.writePort != nil { - if err := c.writePort.Close(); err != nil { - return err - } - c.writePort = nil - } - return nil -} - -func calcChecksum(cls, id, msgLen int, payload []byte) (checksumA, checksumB int) { - checksumA = 0 - checksumB = 0 - - checksumA += cls - checksumB += checksumA - - checksumA += id - checksumB += checksumA - - checksumA += (msgLen & 0xFF) - checksumB += checksumA - - checksumA += (msgLen >> 8) - checksumB += checksumA - - for i := 0; i < msgLen; i++ { - checksumA += int(payload[i]) - checksumB += checksumA - } - return checksumA, checksumB -} diff --git a/components/movementsensor/rtk_station/i2c.go b/components/movementsensor/rtk_station/i2c.go deleted file mode 100644 index 7f7f381f131..00000000000 --- a/components/movementsensor/rtk_station/i2c.go +++ /dev/null @@ -1,194 +0,0 @@ -package rtkstation - -import ( - "context" - "errors" - "fmt" - "io" - "sync" - - "github.com/edaniels/golog" - "go.viam.com/utils" - - "go.viam.com/rdk/components/board" - "go.viam.com/rdk/components/movementsensor" - "go.viam.com/rdk/resource" -) - -type i2cCorrectionSource struct { - logger golog.Logger - bus board.I2C - addr byte - - cancelCtx context.Context - cancelFunc func() - activeBackgroundWorkers sync.WaitGroup - - correctionReaderMu sync.Mutex - correctionReader io.ReadCloser // reader for rctm corrections only - - err movementsensor.LastError -} - -func newI2CCorrectionSource( - deps resource.Dependencies, - conf *StationConfig, - logger golog.Logger, -) (correctionSource, error) { - b, err := board.FromDependencies(deps, conf.I2CConfig.Board) - if err != nil { - return nil, fmt.Errorf("gps init: failed to find board: %w", err) - } - localB, ok := b.(board.LocalBoard) - if !ok { - return nil, fmt.Errorf("board %s is not local", conf.Board) - } - i2cbus, ok := localB.I2CByName(conf.I2CBus) - if !ok { - return nil, fmt.Errorf("gps init: failed to find i2c bus %s", conf.I2CBus) - } - addr := conf.I2cAddr - if addr == -1 { - return nil, errors.New("must specify gps i2c address") - } - - cancelCtx, cancelFunc := context.WithCancel(context.Background()) - - s := &i2cCorrectionSource{ - bus: i2cbus, - addr: byte(addr), - cancelCtx: cancelCtx, - cancelFunc: cancelFunc, - logger: logger, - // Overloaded boards can have flaky I2C busses. Only report errors if at least 5 of the - // last 10 attempts have failed. - err: movementsensor.NewLastError(10, 5), - } - - return s, s.err.Get() -} - -// Start reads correction data from the i2c address and sends it into the correctionReader. -func (s *i2cCorrectionSource) Start(ready chan<- bool) { - s.activeBackgroundWorkers.Add(1) - utils.PanicCapturingGo(func() { - defer s.activeBackgroundWorkers.Done() - - // currently not checking if rtcm message is valid, need to figure out how to integrate constant I2C byte message with rtcm3 scanner - if err := s.cancelCtx.Err(); err != nil { - return - } - - var w *io.PipeWriter - s.correctionReaderMu.Lock() - if err := s.cancelCtx.Err(); err != nil { - s.correctionReaderMu.Unlock() - return - } - s.correctionReader, w = io.Pipe() - s.correctionReaderMu.Unlock() - select { - case ready <- true: - case <-s.cancelCtx.Done(): - return - } - - // open I2C handle every time - handle, err := s.bus.OpenHandle(s.addr) - // Record the error value no matter what. If it's nil, this will prevent us from reporting - // ephemeral errors later. - s.err.Set(err) - if err != nil { - s.logger.Errorf("can't open gps i2c handle: %s", err) - return - } - - // read from handle and pipe to correctionSource - buffer, err := handle.Read(context.Background(), 1024) - if err != nil { - s.logger.Debug("Could not read from handle") - } - _, err = w.Write(buffer) - s.err.Set(err) - if err != nil { - s.logger.Errorf("Error writing RTCM message: %s", err) - return - } - - // close I2C handle - err = handle.Close() - s.err.Set(err) - if err != nil { - s.logger.Debug("failed to close handle: %s", err) - return - } - - for err == nil { - select { - case <-s.cancelCtx.Done(): - return - default: - } - - // Open I2C handle every time - handle, err := s.bus.OpenHandle(s.addr) - s.err.Set(err) - if err != nil { - s.logger.Errorf("can't open gps i2c handle: %s", err) - return - } - - // read from handle and pipe to correctionSource - buffer, err := handle.Read(context.Background(), 1024) - if err != nil { - s.logger.Debug("Could not read from handle") - } - _, err = w.Write(buffer) - s.err.Set(err) - if err != nil { - s.logger.Errorf("Error writing RTCM message: %s", err) - return - } - - // close I2C handle - err = handle.Close() - s.err.Set(err) - if err != nil { - s.logger.Debug("failed to close handle: %s", err) - return - } - } - }) -} - -// Reader returns the i2cCorrectionSource's correctionReader if it exists. -func (s *i2cCorrectionSource) Reader() (io.ReadCloser, error) { - if s.correctionReader == nil { - return nil, errors.New("no stream") - } - - return s.correctionReader, s.err.Get() -} - -// Close shuts down the i2cCorrectionSource. -func (s *i2cCorrectionSource) Close(ctx context.Context) error { - s.correctionReaderMu.Lock() - s.cancelFunc() - - // close correction reader - if s.correctionReader != nil { - if err := s.correctionReader.Close(); err != nil { - s.correctionReaderMu.Unlock() - return err - } - s.correctionReader = nil - } - - s.correctionReaderMu.Unlock() - s.activeBackgroundWorkers.Wait() - - if err := s.err.Get(); err != nil && !errors.Is(err, context.Canceled) { - return err - } - return nil -} diff --git a/components/movementsensor/rtk_station/rtk_station.go b/components/movementsensor/rtk_station/rtk_station.go deleted file mode 100644 index 6c0384cf28e..00000000000 --- a/components/movementsensor/rtk_station/rtk_station.go +++ /dev/null @@ -1,403 +0,0 @@ -// Package rtkstation defines a gps rtk correction source -// This is an Experimental package -package rtkstation - -import ( - "context" - "fmt" - "io" - "sync" - - "github.com/edaniels/golog" - "github.com/golang/geo/r3" - "github.com/jacobsa/go-serial/serial" - geo "github.com/kellydunn/golang-geo" - "github.com/pkg/errors" - "go.viam.com/utils" - - "go.viam.com/rdk/components/board" - "go.viam.com/rdk/components/movementsensor" - gpsrtk "go.viam.com/rdk/components/movementsensor/gpsrtk" - "go.viam.com/rdk/resource" - "go.viam.com/rdk/spatialmath" -) - -// StationConfig is used for converting RTK MovementSensor config attributes. -type StationConfig struct { - CorrectionSource string `json:"correction_source"` - Children []string `json:"children,omitempty"` - Board string `json:"board,omitempty"` - - RequiredAccuracy float64 `json:"required_accuracy,omitempty"` // fixed number 1-5, 5 being the highest accuracy - RequiredTime int `json:"required_time_sec,omitempty"` - - *SerialConfig `json:"serial_attributes,omitempty"` - *I2CConfig `json:"i2c_attributes,omitempty"` -} - -// SerialConfig is used for converting attributes for a correction source. -type SerialConfig struct { - SerialPath string `json:"serial_path"` - SerialBaudRate int `json:"serial_baud_rate,omitempty"` - SerialCorrectionPath string `json:"serial_correction_path,omitempty"` - SerialCorrectionBaudRate int `json:"serial_correction_baud_rate,omitempty"` - - // TestChan is a fake "serial" path for test use only - TestChan chan []uint8 `json:"-"` -} - -// I2CConfig is used for converting attributes for a correction source. -type I2CConfig struct { - Board string `json:"board"` - I2CBus string `json:"i2c_bus"` - I2cAddr int `json:"i2c_addr"` - I2CBaudRate int `json:"i2c_baud_rate,omitempty"` -} - -const ( - i2cStr = "i2c" - serialStr = "serial" - ntripStr = "ntrip" - timeMode = "time" -) - -var stationModel = resource.DefaultModelFamily.WithModel("rtk-station") - -// ErrStationValidation contains the model substring for the available correction source types. -var ( - ErrStationValidation = fmt.Errorf("only serial, I2C are supported for %s", stationModel.Name) - errRequiredAccuracy = errors.New("required accuracy can be a fixed number 1-5, 5 being the highest accuracy") -) - -// Validate ensures all parts of the config are valid. -func (cfg *StationConfig) Validate(path string) ([]string, error) { - var deps []string - if cfg.RequiredAccuracy == 0 { - return nil, utils.NewConfigValidationFieldRequiredError(path, "required_accuracy") - } - if cfg.RequiredAccuracy < 0 || cfg.RequiredAccuracy > 5 { - return nil, errRequiredAccuracy - } - if cfg.RequiredTime == 0 { - return nil, utils.NewConfigValidationFieldRequiredError(path, "required_time") - } - - switch cfg.CorrectionSource { - case i2cStr: - if cfg.I2CConfig.Board == "" { - return nil, utils.NewConfigValidationFieldRequiredError(path, "board") - } - deps = append(deps, cfg.I2CConfig.Board) - return deps, cfg.I2CConfig.ValidateI2C(path) - case serialStr: - if cfg.SerialConfig.SerialCorrectionPath == "" { - return nil, utils.NewConfigValidationFieldRequiredError(path, "serial_correction_path") - } - case "": - return nil, utils.NewConfigValidationFieldRequiredError(path, "correction_source") - default: - return nil, ErrStationValidation - } - - deps = append(deps, cfg.Children...) - return deps, nil -} - -// ValidateI2C ensures all parts of the config are valid. -func (cfg *I2CConfig) ValidateI2C(path string) error { - if cfg.I2CBus == "" { - return utils.NewConfigValidationFieldRequiredError(path, "i2c_bus") - } - if cfg.I2cAddr == 0 { - return utils.NewConfigValidationFieldRequiredError(path, "i2c_addr") - } - - return nil -} - -// ValidateSerial ensures all parts of the config are valid. -func (cfg *SerialConfig) ValidateSerial(path string) error { - if cfg.SerialPath == "" { - return utils.NewConfigValidationFieldRequiredError(path, "serial_path") - } - return nil -} - -func init() { - resource.RegisterComponent( - movementsensor.API, - stationModel, - resource.Registration[movementsensor.MovementSensor, *StationConfig]{ - Constructor: newRTKStation, - }) -} - -type rtkStation struct { - resource.Named - resource.AlwaysRebuild - logger golog.Logger - correctionSource correctionSource - protocol string - i2cPaths []i2cBusAddr - serialPorts []io.Writer - serialWriter io.Writer - movementsensorNames []string - - cancelCtx context.Context - cancelFunc func() - activeBackgroundWorkers sync.WaitGroup - - err movementsensor.LastError - - testChan chan []byte -} - -type correctionSource interface { - Reader() (io.ReadCloser, error) - Start(ready chan<- bool) - Close(ctx context.Context) error -} - -type i2cBusAddr struct { - bus board.I2C - addr byte -} - -func newRTKStation( - ctx context.Context, - deps resource.Dependencies, - conf resource.Config, - logger golog.Logger, -) (movementsensor.MovementSensor, error) { - newConf, err := resource.NativeConfig[*StationConfig](conf) - if err != nil { - return nil, err - } - - cancelCtx, cancelFunc := context.WithCancel(context.Background()) - - r := &rtkStation{ - Named: conf.ResourceName().AsNamed(), - cancelCtx: cancelCtx, - cancelFunc: cancelFunc, - logger: logger, - err: movementsensor.NewLastError(1, 1), - } - - r.protocol = newConf.CorrectionSource - - // Init correction source - switch r.protocol { - case serialStr: - r.correctionSource, err = newSerialCorrectionSource(newConf, logger) - if err != nil { - return nil, err - } - case i2cStr: - r.correctionSource, err = newI2CCorrectionSource(deps, newConf, logger) - if err != nil { - return nil, err - } - default: - // Invalid protocol - return nil, fmt.Errorf("%s is not a valid correction source protocol", r.protocol) - } - - r.movementsensorNames = newConf.Children - - err = ConfigureBaseRTKStation(conf) - if err != nil { - r.logger.Info("rtk base station could not be configured") - return r, err - } - - // Init movementsensor correction input addresses - r.logger.Debug("Init movementsensor") - - for _, movementsensorName := range r.movementsensorNames { - movementSensor, err := movementsensor.FromDependencies(deps, movementsensorName) - if err != nil { - return nil, err - } - rtkgps := movementSensor.(*gpsrtk.RTKMovementSensor) - - switch rtkgps.InputProtocol { - case serialStr: - r.serialPorts = make([]io.Writer, 0) - path := rtkgps.Writepath - baudRate := rtkgps.Wbaud - if newConf.SerialConfig.TestChan != nil { - r.testChan = newConf.SerialConfig.TestChan - } else { - options := serial.OpenOptions{ - PortName: path, - BaudRate: uint(baudRate), - DataBits: 8, - StopBits: 1, - MinimumReadSize: 4, - } - - port, err := serial.Open(options) - if err != nil { - return nil, err - } - - r.serialPorts = append(r.serialPorts, port) - - r.logger.Debug("Init multiwriter") - r.serialWriter = io.MultiWriter(r.serialPorts...) - } - - case i2cStr: - bus := rtkgps.Bus - addr := rtkgps.Addr - busAddr := i2cBusAddr{bus: bus, addr: addr} - - r.i2cPaths = append(r.i2cPaths, busAddr) - default: - return nil, errors.New("child is not valid gpsrtk type") - } - } - r.logger.Debug("Starting") - - r.Start(ctx) - return r, r.err.Get() -} - -// Start starts reading from the correction source and sends corrections to the child movementsensor's. -func (r *rtkStation) Start(ctx context.Context) { - r.activeBackgroundWorkers.Add(1) - utils.PanicCapturingGo(func() { - defer r.activeBackgroundWorkers.Done() - - if err := r.cancelCtx.Err(); err != nil { - return - } - - // read from correction source - ready := make(chan bool) - r.correctionSource.Start(ready) - - select { - case <-ready: - case <-r.cancelCtx.Done(): - return - } - stream, err := r.correctionSource.Reader() - if err != nil { - r.logger.Errorf("Unable to get reader: %s", err) - r.err.Set(err) - return - } - - reader := io.TeeReader(stream, r.serialWriter) - - // write corrections to all open ports and i2c handles - for { - select { - case <-r.cancelCtx.Done(): - return - default: - } - - buf := make([]byte, 1100) - n, err := reader.Read(buf) - r.logger.Debugf("Reading %d bytes", n) - if err != nil { - if err.Error() == "io: read/write on closed pipe" { - r.logger.Debug("Pipe closed") - return - } - r.logger.Errorf("Unable to read stream: %s", err) - r.err.Set(err) - return - } - - // write buf to all i2c handles - for _, busAddr := range r.i2cPaths { - // open handle - handle, err := busAddr.bus.OpenHandle(busAddr.addr) - if err != nil { - r.logger.Errorf("can't open movementsensor i2c handle: %s", err) - r.err.Set(err) - return - } - // write to i2c handle - err = handle.Write(ctx, buf) - if err != nil { - r.logger.Errorf("i2c handle write failed %s", err) - r.err.Set(err) - return - } - // close i2c handle - err = handle.Close() - if err != nil { - r.logger.Errorf("failed to close handle: %s", err) - r.err.Set(err) - return - } - } - } - }) -} - -// Close shuts down the rtkStation. -func (r *rtkStation) Close(ctx context.Context) error { - r.cancelFunc() - r.activeBackgroundWorkers.Wait() - - // close correction source - err := r.correctionSource.Close(ctx) - if err != nil { - return err - } - - // close all ports in slice - for _, port := range r.serialPorts { - err := port.(io.ReadWriteCloser).Close() - if err != nil { - return err - } - } - - if err := r.err.Get(); err != nil && !errors.Is(err, context.Canceled) { - return err - } - return nil -} - -func (r *rtkStation) Position(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { - return &geo.Point{}, 0, movementsensor.ErrMethodUnimplementedPosition -} - -func (r *rtkStation) LinearVelocity(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { - return r3.Vector{}, movementsensor.ErrMethodUnimplementedLinearVelocity -} - -func (r *rtkStation) LinearAcceleration(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { - return r3.Vector{}, movementsensor.ErrMethodUnimplementedLinearAcceleration -} - -func (r *rtkStation) AngularVelocity(ctx context.Context, extra map[string]interface{}) (spatialmath.AngularVelocity, error) { - return spatialmath.AngularVelocity{}, movementsensor.ErrMethodUnimplementedAngularVelocity -} - -func (r *rtkStation) Orientation(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) { - return spatialmath.NewZeroOrientation(), movementsensor.ErrMethodUnimplementedOrientation -} - -func (r *rtkStation) CompassHeading(ctx context.Context, extra map[string]interface{}) (float64, error) { - return 0, movementsensor.ErrMethodUnimplementedCompassHeading -} - -func (r *rtkStation) Readings(ctx context.Context, extra map[string]interface{}) (map[string]interface{}, error) { - return map[string]interface{}{}, movementsensor.ErrMethodUnimplementedReadings -} - -func (r *rtkStation) Accuracy(ctx context.Context, extra map[string]interface{}) (map[string]float32, error) { - return map[string]float32{}, movementsensor.ErrMethodUnimplementedAccuracy -} - -func (r *rtkStation) Properties(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { - return &movementsensor.Properties{}, r.err.Get() -} diff --git a/components/movementsensor/rtk_station/serial.go b/components/movementsensor/rtk_station/serial.go deleted file mode 100644 index 7c3b1e969ae..00000000000 --- a/components/movementsensor/rtk_station/serial.go +++ /dev/null @@ -1,206 +0,0 @@ -package rtkstation - -import ( - "context" - "fmt" - "io" - "sync" - - "github.com/edaniels/golog" - "github.com/go-gnss/rtcm/rtcm3" - "github.com/jacobsa/go-serial/serial" - "github.com/pkg/errors" - "go.viam.com/utils" - - "go.viam.com/rdk/components/movementsensor" -) - -type serialCorrectionSource struct { - port io.ReadCloser // reads all messages from port - logger golog.Logger - - cancelCtx context.Context - cancelFunc func() - activeBackgroundWorkers sync.WaitGroup - - correctionReaderMu sync.Mutex - correctionReader io.ReadCloser // reader for rctm corrections only - - err movementsensor.LastError - - // TestChan is a fake "serial" path for test use only - TestChan chan []uint8 `json:"-"` -} - -type pipeReader struct { - pr *io.PipeReader -} - -func (r pipeReader) Read(p []byte) (int, error) { - return r.pr.Read(p) -} - -func (r pipeReader) Close() error { - return r.pr.Close() -} - -type pipeWriter struct { - pw *io.PipeWriter -} - -func (r pipeWriter) Write(p []byte) (int, error) { - return r.pw.Write(p) -} - -func (r pipeWriter) Close() error { - return r.pw.Close() -} - -const ( - correctionPathName = "correction_path" - baudRateName = "correction_baud" -) - -func newSerialCorrectionSource(conf *StationConfig, logger golog.Logger) (correctionSource, error) { - cancelCtx, cancelFunc := context.WithCancel(context.Background()) - - s := &serialCorrectionSource{ - cancelCtx: cancelCtx, - cancelFunc: cancelFunc, - logger: logger, - err: movementsensor.NewLastError(1, 1), - } - - serialPath := conf.SerialCorrectionPath - if serialPath == "" { - return nil, fmt.Errorf("serialCorrectionSource expected non-empty string for %q", correctionPathName) - } - - baudRate := conf.SerialCorrectionBaudRate - if baudRate == 0 { - baudRate = 9600 - s.logger.Info("SerialCorrectionSource: correction_baud using default 9600") - } - - if conf.SerialConfig.TestChan != nil { - s.TestChan = conf.SerialConfig.TestChan - } else { - options := serial.OpenOptions{ - PortName: serialPath, - BaudRate: uint(baudRate), - DataBits: 8, - StopBits: 1, - MinimumReadSize: 4, - } - - var err error - s.port, err = serial.Open(options) - if err != nil { - return nil, err - } - } - - return s, s.err.Get() -} - -// Start reads correction data from the serial port and sends it into the correctionReader. -func (s *serialCorrectionSource) Start(ready chan<- bool) { - s.activeBackgroundWorkers.Add(1) - utils.PanicCapturingGo(func() { - defer s.activeBackgroundWorkers.Done() - - if err := s.cancelCtx.Err(); err != nil { - return - } - - var w io.WriteCloser - pr, pw := io.Pipe() - s.correctionReaderMu.Lock() - if err := s.cancelCtx.Err(); err != nil { - return - } - s.correctionReader = pipeReader{pr: pr} - s.correctionReaderMu.Unlock() - w = pipeWriter{pw: pw} - select { - case ready <- true: - case <-s.cancelCtx.Done(): - return - } - - // read from s.port and write rctm messages into w, discard other messages in loop - scanner := rtcm3.NewScanner(s.port) - - for { - select { - case <-s.cancelCtx.Done(): - err := w.Close() - if err != nil { - s.logger.Errorf("Unable to close writer: %s", err) - s.err.Set(err) - return - } - return - default: - } - - msg, err := scanner.NextMessage() - if err != nil { - s.logger.Errorf("Error reading RTCM message: %s", err) - s.err.Set(err) - return - } - switch msg.(type) { - case rtcm3.MessageUnknown: - continue - default: - frame := rtcm3.EncapsulateMessage(msg) - byteMsg := frame.Serialize() - _, err := w.Write(byteMsg) - if err != nil { - s.logger.Errorf("Error writing RTCM message: %s", err) - s.err.Set(err) - return - } - } - } - }) -} - -// Reader returns the serialCorrectionSource's correctionReader if it exists. -func (s *serialCorrectionSource) Reader() (io.ReadCloser, error) { - if s.correctionReader == nil { - return nil, errors.New("no stream") - } - - return s.correctionReader, s.err.Get() -} - -// Close shuts down the serialCorrectionSource and closes s.port. -func (s *serialCorrectionSource) Close(ctx context.Context) error { - s.correctionReaderMu.Lock() - s.cancelFunc() - - // close port reader - if s.port != nil { - if err := s.port.Close(); err != nil { - s.correctionReaderMu.Unlock() - return err - } - s.port = nil - } - - // close correction reader - if s.correctionReader != nil { - if err := s.correctionReader.Close(); err != nil { - s.correctionReaderMu.Unlock() - return err - } - s.correctionReader = nil - } - - s.correctionReaderMu.Unlock() - s.activeBackgroundWorkers.Wait() - - return s.err.Get() -} diff --git a/components/movementsensor/rtkutils/gpsrtk.go b/components/movementsensor/rtkutils/gpsrtk.go new file mode 100644 index 00000000000..6f057488e57 --- /dev/null +++ b/components/movementsensor/rtkutils/gpsrtk.go @@ -0,0 +1,290 @@ +// Package rtkutils defines a gps and an rtk correction source +// which sends rtcm data to a child gps +// This is an Experimental package +package rtkutils + +import ( + "context" + "errors" + "io" + "math" + "sync" + + "github.com/edaniels/golog" + "github.com/golang/geo/r3" + geo "github.com/kellydunn/golang-geo" + + "go.viam.com/rdk/components/board" + "go.viam.com/rdk/components/movementsensor" + gpsnmea "go.viam.com/rdk/components/movementsensor/gpsnmea" + "go.viam.com/rdk/resource" + "go.viam.com/rdk/spatialmath" +) + +// A RTKMovementSensor is an NMEA MovementSensor model that can intake RTK correction data. +type RTKMovementSensor struct { + resource.Named + resource.AlwaysRebuild + logger golog.Logger + cancelCtx context.Context + cancelFunc func() + + activeBackgroundWorkers sync.WaitGroup + + ntripMu sync.Mutex + ntripClient *NtripInfo + ntripStatus bool + + err movementsensor.LastError + lastposition movementsensor.LastPosition + + Nmeamovementsensor gpsnmea.NmeaMovementSensor + InputProtocol string + CorrectionWriter io.ReadWriteCloser + + Bus board.I2C + Wbaud int + Addr byte // for i2c only + Writepath string +} + +// GetStream attempts to connect to ntrip streak until successful connection or timeout. +func (g *RTKMovementSensor) GetStream(mountPoint string, maxAttempts int) error { + success := false + attempts := 0 + + var rc io.ReadCloser + var err error + + g.logger.Debug("Getting NTRIP stream") + + for !success && attempts < maxAttempts { + select { + case <-g.cancelCtx.Done(): + return errors.New("Canceled") + default: + } + + rc, err = func() (io.ReadCloser, error) { + g.ntripMu.Lock() + defer g.ntripMu.Unlock() + return g.ntripClient.Client.GetStream(mountPoint) + }() + if err == nil { + success = true + } + attempts++ + } + + if err != nil { + g.logger.Errorf("Can't connect to NTRIP stream: %s", err) + return err + } + + g.logger.Debug("Connected to stream") + g.ntripMu.Lock() + defer g.ntripMu.Unlock() + + g.ntripClient.Stream = rc + return g.err.Get() +} + +// NtripStatus returns true if connection to NTRIP stream is OK, false if not. +func (g *RTKMovementSensor) NtripStatus() (bool, error) { + g.ntripMu.Lock() + defer g.ntripMu.Unlock() + return g.ntripStatus, g.err.Get() +} + +// Position returns the current geographic location of the MOVEMENTSENSOR. +func (g *RTKMovementSensor) Position(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { + g.ntripMu.Lock() + lastError := g.err.Get() + if lastError != nil { + lastPosition := g.lastposition.GetLastPosition() + g.ntripMu.Unlock() + if lastPosition != nil { + return lastPosition, 0, nil + } + return geo.NewPoint(math.NaN(), math.NaN()), math.NaN(), lastError + } + g.ntripMu.Unlock() + + position, alt, err := g.Nmeamovementsensor.Position(ctx, extra) + if err != nil { + // Use the last known valid position if current position is (0,0)/ NaN. + if position != nil && (g.lastposition.IsZeroPosition(position) || g.lastposition.IsPositionNaN(position)) { + lastPosition := g.lastposition.GetLastPosition() + if lastPosition != nil { + return lastPosition, alt, nil + } + } + return geo.NewPoint(math.NaN(), math.NaN()), math.NaN(), err + } + + // Check if the current position is different from the last position and non-zero + lastPosition := g.lastposition.GetLastPosition() + if !g.lastposition.ArePointsEqual(position, lastPosition) { + g.lastposition.SetLastPosition(position) + } + + // Update the last known valid position if the current position is non-zero + if position != nil && !g.lastposition.IsZeroPosition(position) { + g.lastposition.SetLastPosition(position) + } + + return position, alt, nil +} + +// LinearVelocity passthrough. +func (g *RTKMovementSensor) LinearVelocity(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { + g.ntripMu.Lock() + lastError := g.err.Get() + if lastError != nil { + defer g.ntripMu.Unlock() + return r3.Vector{}, lastError + } + g.ntripMu.Unlock() + + return g.Nmeamovementsensor.LinearVelocity(ctx, extra) +} + +// LinearAcceleration passthrough. +func (g *RTKMovementSensor) LinearAcceleration(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { + lastError := g.err.Get() + if lastError != nil { + return r3.Vector{}, lastError + } + return g.Nmeamovementsensor.LinearAcceleration(ctx, extra) +} + +// AngularVelocity passthrough. +func (g *RTKMovementSensor) AngularVelocity(ctx context.Context, extra map[string]interface{}) (spatialmath.AngularVelocity, error) { + g.ntripMu.Lock() + lastError := g.err.Get() + if lastError != nil { + defer g.ntripMu.Unlock() + return spatialmath.AngularVelocity{}, lastError + } + g.ntripMu.Unlock() + + return g.Nmeamovementsensor.AngularVelocity(ctx, extra) +} + +// CompassHeading passthrough. +func (g *RTKMovementSensor) CompassHeading(ctx context.Context, extra map[string]interface{}) (float64, error) { + g.ntripMu.Lock() + lastError := g.err.Get() + if lastError != nil { + defer g.ntripMu.Unlock() + return 0, lastError + } + g.ntripMu.Unlock() + + return g.Nmeamovementsensor.CompassHeading(ctx, extra) +} + +// Orientation passthrough. +func (g *RTKMovementSensor) Orientation(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) { + g.ntripMu.Lock() + lastError := g.err.Get() + if lastError != nil { + defer g.ntripMu.Unlock() + return spatialmath.NewZeroOrientation(), lastError + } + g.ntripMu.Unlock() + + return g.Nmeamovementsensor.Orientation(ctx, extra) +} + +// ReadFix passthrough. +func (g *RTKMovementSensor) ReadFix(ctx context.Context) (int, error) { + g.ntripMu.Lock() + lastError := g.err.Get() + if lastError != nil { + defer g.ntripMu.Unlock() + return 0, lastError + } + g.ntripMu.Unlock() + + return g.Nmeamovementsensor.ReadFix(ctx) +} + +// Properties passthrough. +func (g *RTKMovementSensor) Properties(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { + lastError := g.err.Get() + if lastError != nil { + return &movementsensor.Properties{}, lastError + } + + return g.Nmeamovementsensor.Properties(ctx, extra) +} + +// Accuracy passthrough. +func (g *RTKMovementSensor) Accuracy(ctx context.Context, extra map[string]interface{}) (map[string]float32, error) { + lastError := g.err.Get() + if lastError != nil { + return map[string]float32{}, lastError + } + + return g.Nmeamovementsensor.Accuracy(ctx, extra) +} + +// Readings will use the default MovementSensor Readings if not provided. +func (g *RTKMovementSensor) Readings(ctx context.Context, extra map[string]interface{}) (map[string]interface{}, error) { + readings, err := movementsensor.Readings(ctx, g, extra) + if err != nil { + return nil, err + } + + fix, err := g.ReadFix(ctx) + if err != nil { + return nil, err + } + + readings["fix"] = fix + + return readings, nil +} + +// Close shuts down the RTKMOVEMENTSENSOR. +func (g *RTKMovementSensor) Close(ctx context.Context) error { + g.ntripMu.Lock() + g.cancelFunc() + + if err := g.Nmeamovementsensor.Close(ctx); err != nil { + g.ntripMu.Unlock() + return err + } + + // close ntrip writer + if g.CorrectionWriter != nil { + if err := g.CorrectionWriter.Close(); err != nil { + g.ntripMu.Unlock() + return err + } + g.CorrectionWriter = nil + } + + // close ntrip client and stream + if g.ntripClient.Client != nil { + g.ntripClient.Client.CloseIdleConnections() + g.ntripClient.Client = nil + } + + if g.ntripClient.Stream != nil { + if err := g.ntripClient.Stream.Close(); err != nil { + g.ntripMu.Unlock() + return err + } + g.ntripClient.Stream = nil + } + + g.ntripMu.Unlock() + g.activeBackgroundWorkers.Wait() + + if err := g.err.Get(); err != nil && !errors.Is(err, context.Canceled) { + return err + } + return nil +} diff --git a/components/movementsensor/gpsrtk/gpsrtk_test.go b/components/movementsensor/rtkutils/gpsrtk_test.go similarity index 83% rename from components/movementsensor/gpsrtk/gpsrtk_test.go rename to components/movementsensor/rtkutils/gpsrtk_test.go index 2e116f42905..06d6db56c80 100644 --- a/components/movementsensor/gpsrtk/gpsrtk_test.go +++ b/components/movementsensor/rtkutils/gpsrtk_test.go @@ -1,4 +1,4 @@ -package gpsrtk +package rtkutils import ( "context" @@ -7,7 +7,6 @@ import ( "github.com/edaniels/golog" geo "github.com/kellydunn/golang-geo" "go.viam.com/test" - "go.viam.com/utils" "go.viam.com/rdk/components/movementsensor" "go.viam.com/rdk/components/movementsensor/fake" @@ -127,57 +126,57 @@ func TestModelTypeCreators(t *testing.T) { test.That(t, err, test.ShouldBeNil) } -func TestValidateRTK(t *testing.T) { - path := "path" - fakecfg := &Config{NtripConfig: &NtripConfig{}, ConnectionType: "serial", SerialConfig: &SerialConfig{SerialPath: "some-path"}} - _, err := fakecfg.Validate(path) - - test.That(t, err, test.ShouldBeError, - utils.NewConfigValidationFieldRequiredError(path, "correction_source")) - - fakecfg.CorrectionSource = "ntrip" - _, err = fakecfg.Validate(path) - test.That(t, err, test.ShouldBeError, utils.NewConfigValidationFieldRequiredError(path, "ntrip_addr")) - - fakecfg.NtripConfig.NtripAddr = "http://fakeurl" - _, err = fakecfg.Validate(path) - test.That( - t, - err, - test.ShouldBeError, - utils.NewConfigValidationFieldRequiredError(path, "ntrip_input_protocol"), - ) - fakecfg.NtripInputProtocol = "serial" - _, err = fakecfg.Validate("path") - test.That(t, err, test.ShouldBeNil) -} - -func TestConnect(t *testing.T) { - logger := golog.NewTestLogger(t) - ctx := context.Background() - cancelCtx, cancelFunc := context.WithCancel(ctx) - g := RTKMovementSensor{ - cancelCtx: cancelCtx, - cancelFunc: cancelFunc, - logger: logger, - } - - url := "http://fakeurl" - username := "user" - password := "pwd" - - // create new ntrip client and connect - err := g.Connect("invalidurl", username, password, 10) - g.ntripClient = makeMockNtripClient() - - test.That(t, err, test.ShouldNotBeNil) - - err = g.Connect(url, username, password, 10) - test.That(t, err, test.ShouldBeNil) - - err = g.GetStream("", 10) - test.That(t, err, test.ShouldNotBeNil) -} +// func TestValidateRTK(t *testing.T) { +// path := "path" +// fakecfg := &Config{NtripConfig: &NtripConfig{}, ConnectionType: "serial", SerialConfig: &SerialConfig{SerialPath: "some-path"}} +// _, err := fakecfg.Validate(path) + +// test.That(t, err, test.ShouldBeError, +// utils.NewConfigValidationFieldRequiredError(path, "correction_source")) + +// fakecfg.CorrectionSource = "ntrip" +// _, err = fakecfg.Validate(path) +// test.That(t, err, test.ShouldBeError, utils.NewConfigValidationFieldRequiredError(path, "ntrip_url")) + +// fakecfg.NtripConfig.NtripURL = "http://fakeurl" +// _, err = fakecfg.Validate(path) +// test.That( +// t, +// err, +// test.ShouldBeError, +// utils.NewConfigValidationFieldRequiredError(path, "ntrip_input_protocol"), +// ) +// fakecfg.NtripInputProtocol = "serial" +// _, err = fakecfg.Validate("path") +// test.That(t, err, test.ShouldBeNil) +// } + +// func TestConnect(t *testing.T) { +// logger := golog.NewTestLogger(t) +// ctx := context.Background() +// cancelCtx, cancelFunc := context.WithCancel(ctx) +// g := RTKMovementSensor{ +// cancelCtx: cancelCtx, +// cancelFunc: cancelFunc, +// logger: logger, +// } + +// url := "http://fakeurl" +// username := "user" +// password := "pwd" + +// // create new ntrip client and connect +// err := g.Connect("invalidurl", username, password, 10) +// g.ntripClient = makeMockNtripClient() + +// test.That(t, err, test.ShouldNotBeNil) + +// err = g.Connect(url, username, password, 10) +// test.That(t, err, test.ShouldBeNil) + +// err = g.GetStream("", 10) +// test.That(t, err, test.ShouldNotBeNil) +// } // TODO: RSDK-3264, This needs to be cleaned up as we stablize gpsrtk /* func TestNewRTKMovementSensor(t *testing.T) { @@ -208,7 +207,7 @@ func TestConnect(t *testing.T) { SerialCorrectionBaudRate: 0, }, NtripConfig: &NtripConfig{ - NtripAddr: "some_ntrip_address", + NtripURL: "some_ntrip_address", NtripConnectAttempts: 10, NtripMountpoint: "", NtripPass: "", @@ -257,7 +256,7 @@ func TestConnect(t *testing.T) { I2CBaudRate: 115200, }, NtripConfig: &NtripConfig{ - NtripAddr: "http://some_ntrip_address", + NtripURL: "http://some_ntrip_address", }, }, } @@ -367,7 +366,7 @@ func TestCloseRTK(t *testing.T) { // Helpers -// mock ntripinfo client. -func makeMockNtripClient() *NtripInfo { - return &NtripInfo{} -} +// // mock ntripinfo client. +// func makeMockNtripClient() *NtripInfo { +// return &NtripInfo{} +// } diff --git a/components/movementsensor/gpsrtk/model_test.go b/components/movementsensor/rtkutils/model_test.go similarity index 90% rename from components/movementsensor/gpsrtk/model_test.go rename to components/movementsensor/rtkutils/model_test.go index a5081962ab4..c87050ca856 100644 --- a/components/movementsensor/gpsrtk/model_test.go +++ b/components/movementsensor/rtkutils/model_test.go @@ -1,4 +1,4 @@ -package gpsrtk_test +package rtkutils import ( "context" @@ -30,6 +30,6 @@ func TestGPSModels(t *testing.T) { ] }` _, err := config.FromReader(ctx, "", strings.NewReader(cfg1), logger) - test.That(t, err, test.ShouldNotBeNil) + test.That(t, err, test.ShouldBeNil) }) } diff --git a/components/movementsensor/gpsrtk/ntrip.go b/components/movementsensor/rtkutils/ntrip.go similarity index 62% rename from components/movementsensor/gpsrtk/ntrip.go rename to components/movementsensor/rtkutils/ntrip.go index 7442f4ffc1f..9f53c8fd5a9 100644 --- a/components/movementsensor/gpsrtk/ntrip.go +++ b/components/movementsensor/rtkutils/ntrip.go @@ -1,4 +1,4 @@ -package gpsrtk +package rtkutils import ( "fmt" @@ -19,13 +19,23 @@ type NtripInfo struct { MaxConnectAttempts int } -func newNtripInfo(cfg *NtripConfig, logger golog.Logger) (*NtripInfo, error) { +// NtripConfig is used for converting attributes for a correction source. +type NtripConfig struct { + NtripURL string `json:"ntrip_url"` + NtripConnectAttempts int `json:"ntrip_connect_attempts,omitempty"` + NtripMountpoint string `json:"ntrip_mountpoint,omitempty"` + NtripPass string `json:"ntrip_password,omitempty"` + NtripUser string `json:"ntrip_username,omitempty"` +} + +// NewNtripInfo function validates and sets NtripConfig arributes and returns NtripInfo. +func NewNtripInfo(cfg *NtripConfig, logger golog.Logger) (*NtripInfo, error) { n := &NtripInfo{} // Init NtripInfo from attributes - n.URL = cfg.NtripAddr + n.URL = cfg.NtripURL if n.URL == "" { - return nil, fmt.Errorf("NTRIP expected non-empty string for %q", cfg.NtripAddr) + return nil, fmt.Errorf("NTRIP expected non-empty string for %q", cfg.NtripURL) } n.Username = cfg.NtripUser if n.Username == "" { diff --git a/components/movementsensor/utils.go b/components/movementsensor/utils.go index db3a1313555..e290c2725ab 100644 --- a/components/movementsensor/utils.go +++ b/components/movementsensor/utils.go @@ -84,38 +84,12 @@ type LastError struct { count int // How many items in errs are non-nil } -// LastPosition stores the last position seen by the movement sensor. -type LastPosition struct { - lastposition *geo.Point - mu sync.Mutex -} - // NewLastError creates a LastError object which will let you retrieve the most recent error if at // least `threshold` of the most recent `size` items put into it are non-nil. func NewLastError(size, threshold int) LastError { return LastError{errs: make([]error, size), threshold: threshold} } -// NewLastPosition creates a new point that's (NaN, NaN) -// go-staticcheck. -func NewLastPosition() LastPosition { - return LastPosition{lastposition: geo.NewPoint(math.NaN(), math.NaN())} -} - -// GetLastPosition returns the last known position. -func (lp *LastPosition) GetLastPosition() *geo.Point { - lp.mu.Lock() - defer lp.mu.Unlock() - return lp.lastposition -} - -// SetLastPosition updates the last known position. -func (lp *LastPosition) SetLastPosition(position *geo.Point) { - lp.mu.Lock() - defer lp.mu.Unlock() - lp.lastposition = position -} - // Set stores an error to be retrieved later. func (le *LastError) Set(err error) { le.mu.Lock() @@ -161,6 +135,32 @@ func (le *LastError) Get() error { return errToReturn } +// LastPosition stores the last position seen by the movement sensor. +type LastPosition struct { + lastposition *geo.Point + mu sync.Mutex +} + +// NewLastPosition creates a new point that's (NaN, NaN) +// go-staticcheck. +func NewLastPosition() LastPosition { + return LastPosition{lastposition: geo.NewPoint(math.NaN(), math.NaN())} +} + +// GetLastPosition returns the last known position. +func (lp *LastPosition) GetLastPosition() *geo.Point { + lp.mu.Lock() + defer lp.mu.Unlock() + return lp.lastposition +} + +// SetLastPosition updates the last known position. +func (lp *LastPosition) SetLastPosition(position *geo.Point) { + lp.mu.Lock() + defer lp.mu.Unlock() + lp.lastposition = position +} + // ArePointsEqual checks if two geo.Point instances are equal. func (lp *LastPosition) ArePointsEqual(p1, p2 *geo.Point) bool { if p1 == nil || p2 == nil { @@ -178,3 +178,49 @@ func (lp *LastPosition) IsZeroPosition(p *geo.Point) bool { func (lp *LastPosition) IsPositionNaN(p *geo.Point) bool { return math.IsNaN(p.Lng()) && math.IsNaN(p.Lat()) } + +// LastCompassHeading store the last valid compass heading seen by the movement sensor. +// This is really just an atomic float64, analogous to the atomic ints provided in the +// "sync/atomic" package. +type LastCompassHeading struct { + lastcompassheading float64 + mu sync.Mutex +} + +// NewLastCompassHeading create a new LastCompassHeading. +func NewLastCompassHeading() LastCompassHeading { + return LastCompassHeading{lastcompassheading: math.NaN()} +} + +// GetLastCompassHeading returns the last compass heading stored. +func (lch *LastCompassHeading) GetLastCompassHeading() float64 { + lch.mu.Lock() + defer lch.mu.Unlock() + return lch.lastcompassheading +} + +// SetLastCompassHeading sets lastcompassheading to the value given in the function. +func (lch *LastCompassHeading) SetLastCompassHeading(compassheading float64) { + lch.mu.Lock() + defer lch.mu.Unlock() + lch.lastcompassheading = compassheading +} + +// PMTKAddChk adds PMTK checksums to commands by XORing the bytes together. +func PMTKAddChk(data []byte) []byte { + chk := PMTKChecksum(data) + newCmd := []byte("$") + newCmd = append(newCmd, data...) + newCmd = append(newCmd, []byte("*")...) + newCmd = append(newCmd, chk) + return newCmd +} + +// PMTKChecksum calculates the checksum of a byte array by performing an XOR operation on each byte. +func PMTKChecksum(data []byte) byte { + var chk byte + for _, b := range data { + chk ^= b + } + return chk +} diff --git a/components/movementsensor/utils_test.go b/components/movementsensor/utils_test.go index 4c51b00c5a5..78894b6a241 100644 --- a/components/movementsensor/utils_test.go +++ b/components/movementsensor/utils_test.go @@ -2,65 +2,68 @@ package movementsensor import ( "errors" + "math" "testing" geo "github.com/kellydunn/golang-geo" "go.viam.com/test" ) +var ( + testPos1 = geo.NewPoint(8.46696, -17.03663) + testPos2 = geo.NewPoint(65.35996, -17.03663) + zeroPos = geo.NewPoint(0, 0) + nanPos = geo.NewPoint(math.NaN(), math.NaN()) +) + func TestGetHeading(t *testing.T) { // test case 1, standard bearing = 0, heading = 270 - var ( - GPS1 = geo.NewPoint(8.46696, -17.03663) - GPS2 = geo.NewPoint(65.35996, -17.03663) - ) - - bearing, heading, standardBearing := GetHeading(GPS1, GPS2, 90) + bearing, heading, standardBearing := GetHeading(testPos1, testPos2, 90) test.That(t, bearing, test.ShouldAlmostEqual, 0) test.That(t, heading, test.ShouldAlmostEqual, 270) test.That(t, standardBearing, test.ShouldAlmostEqual, 0) // test case 2, reversed test case 1. - GPS1 = geo.NewPoint(65.35996, -17.03663) - GPS2 = geo.NewPoint(8.46696, -17.03663) + testPos1 = geo.NewPoint(65.35996, -17.03663) + testPos2 = geo.NewPoint(8.46696, -17.03663) - bearing, heading, standardBearing = GetHeading(GPS1, GPS2, 90) + bearing, heading, standardBearing = GetHeading(testPos1, testPos2, 90) test.That(t, bearing, test.ShouldAlmostEqual, 180) test.That(t, heading, test.ShouldAlmostEqual, 90) test.That(t, standardBearing, test.ShouldAlmostEqual, 180) // test case 2.5, changed yaw offsets - GPS1 = geo.NewPoint(65.35996, -17.03663) - GPS2 = geo.NewPoint(8.46696, -17.03663) + testPos1 = geo.NewPoint(65.35996, -17.03663) + testPos2 = geo.NewPoint(8.46696, -17.03663) - bearing, heading, standardBearing = GetHeading(GPS1, GPS2, 270) + bearing, heading, standardBearing = GetHeading(testPos1, testPos2, 270) test.That(t, bearing, test.ShouldAlmostEqual, 180) test.That(t, heading, test.ShouldAlmostEqual, 270) test.That(t, standardBearing, test.ShouldAlmostEqual, 180) // test case 3 - GPS1 = geo.NewPoint(8.46696, -17.03663) - GPS2 = geo.NewPoint(56.74367734077241, 29.369620000000015) + testPos1 = geo.NewPoint(8.46696, -17.03663) + testPos2 = geo.NewPoint(56.74367734077241, 29.369620000000015) - bearing, heading, standardBearing = GetHeading(GPS1, GPS2, 90) + bearing, heading, standardBearing = GetHeading(testPos1, testPos2, 90) test.That(t, bearing, test.ShouldAlmostEqual, 27.2412, 1e-3) test.That(t, heading, test.ShouldAlmostEqual, 297.24126, 1e-3) test.That(t, standardBearing, test.ShouldAlmostEqual, 27.24126, 1e-3) // test case 4, reversed coordinates - GPS1 = geo.NewPoint(56.74367734077241, 29.369620000000015) - GPS2 = geo.NewPoint(8.46696, -17.03663) + testPos1 = geo.NewPoint(56.74367734077241, 29.369620000000015) + testPos2 = geo.NewPoint(8.46696, -17.03663) - bearing, heading, standardBearing = GetHeading(GPS1, GPS2, 90) + bearing, heading, standardBearing = GetHeading(testPos1, testPos2, 90) test.That(t, bearing, test.ShouldAlmostEqual, 235.6498, 1e-3) test.That(t, heading, test.ShouldAlmostEqual, 145.6498, 1e-3) test.That(t, standardBearing, test.ShouldAlmostEqual, -124.3501, 1e-3) // test case 4.5, changed yaw Offset - GPS1 = geo.NewPoint(56.74367734077241, 29.369620000000015) - GPS2 = geo.NewPoint(8.46696, -17.03663) + testPos1 = geo.NewPoint(56.74367734077241, 29.369620000000015) + testPos2 = geo.NewPoint(8.46696, -17.03663) - bearing, heading, standardBearing = GetHeading(GPS1, GPS2, 270) + bearing, heading, standardBearing = GetHeading(testPos1, testPos2, 270) test.That(t, bearing, test.ShouldAlmostEqual, 235.6498, 1e-3) test.That(t, heading, test.ShouldAlmostEqual, 325.6498, 1e-3) test.That(t, standardBearing, test.ShouldAlmostEqual, -124.3501, 1e-3) @@ -109,3 +112,36 @@ func TestSuppressRareErrors(t *testing.T) { // and now that we've returned an error, the history is cleared out again. test.That(t, le.Get(), test.ShouldBeNil) } + +func TestLastPosition(t *testing.T) { + lp := NewLastPosition() + lp.SetLastPosition(testPos2) + test.That(t, lp.lastposition, test.ShouldEqual, testPos2) + + lp.SetLastPosition(testPos1) + getPos := lp.GetLastPosition() + test.That(t, getPos, test.ShouldEqual, testPos1) +} + +func TestPositionLogic(t *testing.T) { + lp := NewLastPosition() + + test.That(t, lp.ArePointsEqual(testPos2, testPos2), test.ShouldBeTrue) + test.That(t, lp.ArePointsEqual(testPos2, testPos1), test.ShouldBeFalse) + + test.That(t, lp.IsZeroPosition(zeroPos), test.ShouldBeTrue) + test.That(t, lp.IsZeroPosition(testPos2), test.ShouldBeFalse) + + test.That(t, lp.IsPositionNaN(nanPos), test.ShouldBeTrue) + test.That(t, lp.IsPositionNaN(testPos1), test.ShouldBeFalse) +} + +func TestPMTKFunctions(t *testing.T) { + var ( + expectedValue = ([]uint8{36, 80, 77, 84, 75, 50, 50, 48, 44, 49, 48, 48, 48, 42, 31}) + testValue = ([]byte("PMTK220,1000")) + expectedChecksum = 31 + ) + test.That(t, PMTKChecksum(testValue), test.ShouldEqual, expectedChecksum) + test.That(t, PMTKAddChk(testValue), test.ShouldResemble, expectedValue) +} diff --git a/components/movementsensor/wheeledodometry/wheeledodometry.go b/components/movementsensor/wheeledodometry/wheeledodometry.go new file mode 100644 index 00000000000..4260139d2fd --- /dev/null +++ b/components/movementsensor/wheeledodometry/wheeledodometry.go @@ -0,0 +1,367 @@ +// Package wheeledodometry implements an odometery estimate from an encoder wheeled base. +package wheeledodometry + +import ( + "context" + "errors" + "math" + "sync" + "time" + + "github.com/edaniels/golog" + "github.com/golang/geo/r3" + geo "github.com/kellydunn/golang-geo" + "go.viam.com/utils" + + "go.viam.com/rdk/components/base" + "go.viam.com/rdk/components/motor" + "go.viam.com/rdk/components/movementsensor" + "go.viam.com/rdk/resource" + "go.viam.com/rdk/spatialmath" + rdkutils "go.viam.com/rdk/utils" +) + +var model = resource.DefaultModelFamily.WithModel("wheeled-odometry") + +const ( + defaultTimeIntervalMSecs = 500 + oneTurn = 2 * math.Pi +) + +// Config is the config for a wheeledodometry MovementSensor. +type Config struct { + LeftMotors []string `json:"left_motors"` + RightMotors []string `json:"right_motors"` + Base string `json:"base"` + TimeIntervalMSecs float64 `json:"time_interval_msecs,omitempty"` +} + +type motorPair struct { + left motor.Motor + right motor.Motor +} + +type odometry struct { + resource.Named + resource.AlwaysRebuild + + lastLeftPos float64 + lastRightPos float64 + baseWidth float64 + wheelCircumference float64 + timeIntervalMSecs float64 + + motors []motorPair + + angularVelocity spatialmath.AngularVelocity + linearVelocity r3.Vector + position r3.Vector + orientation spatialmath.EulerAngles + + cancelFunc func() + activeBackgroundWorkers sync.WaitGroup + mu sync.Mutex + logger golog.Logger +} + +func init() { + resource.RegisterComponent( + movementsensor.API, + model, + resource.Registration[movementsensor.MovementSensor, *Config]{Constructor: newWheeledOdometry}) +} + +// Validate ensures all parts of the config are valid. +func (cfg *Config) Validate(path string) ([]string, error) { + var deps []string + + if cfg.Base == "" { + return nil, utils.NewConfigValidationFieldRequiredError(path, "base") + } + deps = append(deps, cfg.Base) + + if len(cfg.LeftMotors) == 0 { + return nil, utils.NewConfigValidationFieldRequiredError(path, "left motors") + } + deps = append(deps, cfg.LeftMotors...) + + if len(cfg.RightMotors) == 0 { + return nil, utils.NewConfigValidationFieldRequiredError(path, "right motors") + } + deps = append(deps, cfg.RightMotors...) + + if len(cfg.LeftMotors) != len(cfg.RightMotors) { + return nil, errors.New("mismatch number of left and right motors") + } + + // Temporary validation check until support for more than one left and right motor each is added. + if len(cfg.LeftMotors) > 1 || len(cfg.RightMotors) > 1 { + return nil, errors.New("wheeled odometry only supports one left and right motor each") + } + + return deps, nil +} + +// Reconfigure automatically reconfigures this movement sensor based on the updated config. +func (o *odometry) Reconfigure(ctx context.Context, deps resource.Dependencies, conf resource.Config) error { + if len(o.motors) > 0 { + if err := o.motors[0].left.Stop(ctx, nil); err != nil { + return err + } + if err := o.motors[0].right.Stop(ctx, nil); err != nil { + return err + } + } + + if o.cancelFunc != nil { + o.cancelFunc() + o.activeBackgroundWorkers.Wait() + } + + o.mu.Lock() + defer o.mu.Unlock() + + newConf, err := resource.NativeConfig[*Config](conf) + if err != nil { + return err + } + + // set the new timeIntervalMSecs + o.timeIntervalMSecs = newConf.TimeIntervalMSecs + if o.timeIntervalMSecs == 0 { + o.timeIntervalMSecs = defaultTimeIntervalMSecs + } + if o.timeIntervalMSecs > 1000 { + o.logger.Warn("if the time interval is more than 1000 ms, be sure to move the base slowly for better accuracy") + } + + // set baseWidth and wheelCircumference from the new base properties + newBase, err := base.FromDependencies(deps, newConf.Base) + if err != nil { + return err + } + props, err := newBase.Properties(ctx, nil) + if err != nil { + return err + } + o.baseWidth = props.WidthMeters + o.wheelCircumference = props.WheelCircumferenceMeters + if o.baseWidth == 0 || o.wheelCircumference == 0 { + return errors.New("base width or wheel circumference are 0, movement sensor cannot be created") + } + + // check if new motors have been added, or the existing motors have been changed, and update the motorPairs accorodingly + for i := range newConf.LeftMotors { + var motorLeft, motorRight motor.Motor + if i >= len(o.motors) || o.motors[i].left.Name().ShortName() != newConf.LeftMotors[i] { + motorLeft, err = motor.FromDependencies(deps, newConf.LeftMotors[i]) + if err != nil { + return err + } + properties, err := motorLeft.Properties(ctx, nil) + if err != nil { + return err + } + if !properties.PositionReporting { + return motor.NewPropertyUnsupportedError(properties, newConf.LeftMotors[i]) + } + } + + if i >= len(o.motors) || o.motors[i].right.Name().ShortName() != newConf.RightMotors[i] { + motorRight, err = motor.FromDependencies(deps, newConf.RightMotors[i]) + if err != nil { + return err + } + properties, err := motorRight.Properties(ctx, nil) + if err != nil { + return err + } + if !properties.PositionReporting { + return motor.NewPropertyUnsupportedError(properties, newConf.LeftMotors[i]) + } + } + + // append if motors have been added, replace if motors have changed + thisPair := motorPair{left: motorLeft, right: motorRight} + if i >= len(o.motors) { + o.motors = append(o.motors, thisPair) + } else { + o.motors[i].left = motorLeft + o.motors[i].right = motorRight + } + } + + if len(o.motors) > 1 { + o.logger.Warn("odometry will not be accurate if the left and right motors that are paired are not listed in the same order") + } + + o.orientation.Yaw = 0 + ctx, cancelFunc := context.WithCancel(context.Background()) + o.cancelFunc = cancelFunc + o.trackPosition(ctx) + + return nil +} + +// newWheeledOdometry returns a new wheeled encoder movement sensor defined by the given config. +func newWheeledOdometry( + ctx context.Context, + deps resource.Dependencies, + conf resource.Config, + logger golog.Logger, +) (movementsensor.MovementSensor, error) { + o := &odometry{ + Named: conf.ResourceName().AsNamed(), + lastLeftPos: 0.0, + lastRightPos: 0.0, + logger: logger, + } + + if err := o.Reconfigure(ctx, deps, conf); err != nil { + return nil, err + } + + return o, nil +} + +func (o *odometry) AngularVelocity(ctx context.Context, extra map[string]interface{}) (spatialmath.AngularVelocity, error) { + o.mu.Lock() + defer o.mu.Unlock() + return o.angularVelocity, nil +} + +func (o *odometry) LinearAcceleration(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { + return r3.Vector{}, movementsensor.ErrMethodUnimplementedAngularVelocity +} + +func (o *odometry) Orientation(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) { + o.mu.Lock() + defer o.mu.Unlock() + ov := &spatialmath.OrientationVector{Theta: o.orientation.Yaw, OX: 0, OY: 0, OZ: 1} + return ov, nil +} + +func (o *odometry) CompassHeading(ctx context.Context, extra map[string]interface{}) (float64, error) { + o.mu.Lock() + defer o.mu.Unlock() + return 0, movementsensor.ErrMethodUnimplementedCompassHeading +} + +func (o *odometry) LinearVelocity(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { + o.mu.Lock() + defer o.mu.Unlock() + return o.linearVelocity, nil +} + +func (o *odometry) Position(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { + o.mu.Lock() + defer o.mu.Unlock() + return geo.NewPoint(o.position.X, o.position.Y), o.position.Z, nil +} + +func (o *odometry) Readings(ctx context.Context, extra map[string]interface{}) (map[string]interface{}, error) { + return movementsensor.Readings(ctx, o, extra) +} + +func (o *odometry) Accuracy(ctx context.Context, extra map[string]interface{}) (map[string]float32, error) { + return map[string]float32{}, movementsensor.ErrMethodUnimplementedAccuracy +} + +func (o *odometry) Properties(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { + return &movementsensor.Properties{ + LinearVelocitySupported: true, + AngularVelocitySupported: true, + OrientationSupported: true, + PositionSupported: true, + }, nil +} + +func (o *odometry) Close(ctx context.Context) error { + o.cancelFunc() + o.activeBackgroundWorkers.Wait() + return nil +} + +// trackPosition uses the motor positions to calculate an estimation of the position, orientation, +// linear velocity, and angular velocity of the wheeled base. +// The estimations in this function are based on the math outlined in this article: +// https://stuff.mit.edu/afs/athena/course/6/6.186/OldFiles/2005/doc/odomtutorial/odomtutorial.pdf +func (o *odometry) trackPosition(ctx context.Context) { + o.activeBackgroundWorkers.Add(1) + utils.PanicCapturingGo(func() { + defer o.activeBackgroundWorkers.Done() + ticker := time.NewTicker(time.Duration(o.timeIntervalMSecs) * time.Millisecond) + for { + select { + case <-ctx.Done(): + return + default: + } + + // Sleep until it's time to update the position again. + select { + case <-ctx.Done(): + return + case <-ticker.C: + } + + // Use GetInParallel to ensure the left and right motors are polled at the same time. + positionFuncs := func() []rdkutils.FloatFunc { + fs := []rdkutils.FloatFunc{} + + // Always use the first pair until more than one pair of motors is supported in this model. + fs = append(fs, func(ctx context.Context) (float64, error) { return o.motors[0].left.Position(ctx, nil) }) + fs = append(fs, func(ctx context.Context) (float64, error) { return o.motors[0].right.Position(ctx, nil) }) + + return fs + } + + _, positions, err := rdkutils.GetInParallel(ctx, positionFuncs()) + if err != nil { + o.logger.Error(err) + continue + } + + // Current position of the left and right motors in revolutions. + if len(positions) != len(o.motors)*2 { + o.logger.Error("error getting both motor positions, trying again") + continue + } + left := positions[0] + right := positions[1] + + // Difference in the left and right motors since the last iteration, in mm. + leftDist := (left - o.lastLeftPos) * o.wheelCircumference + rightDist := (right - o.lastRightPos) * o.wheelCircumference + + // Update lastLeftPos and lastRightPos to be the current position in mm. + o.lastLeftPos = left + o.lastRightPos = right + + // Linear and angular distance the center point has traveled. This works based on + // the assumption that the time interval between calulations is small enough that + // the inner angle of the rotation will be small enough that it can be accurately + // estimated using the below equations. + centerDist := (leftDist + rightDist) / 2 + centerAngle := (rightDist - leftDist) / o.baseWidth + + // Update the position and orientation values accordingly. + o.mu.Lock() + o.orientation.Yaw += centerAngle + + // Limit the yaw to a range of positive 0 to 360 degrees. + o.orientation.Yaw = math.Mod(o.orientation.Yaw, oneTurn) + o.orientation.Yaw = math.Mod(o.orientation.Yaw+oneTurn, oneTurn) + + // Calculate X and Y by using centerDist and the current orientation yaw (theta). + o.position.X += (centerDist * math.Cos(o.orientation.Yaw)) + o.position.Y += (centerDist * math.Sin(o.orientation.Yaw)) + + // Update the linear and angular velocity values using the provided time interval. + o.linearVelocity.Y = centerDist / (o.timeIntervalMSecs / 1000) + o.angularVelocity.Z = centerAngle * (180 / math.Pi) / (o.timeIntervalMSecs / 1000) + + o.mu.Unlock() + } + }) +} diff --git a/components/movementsensor/wheeledodometry/wheeledodometry_test.go b/components/movementsensor/wheeledodometry/wheeledodometry_test.go new file mode 100644 index 00000000000..1f65925ed68 --- /dev/null +++ b/components/movementsensor/wheeledodometry/wheeledodometry_test.go @@ -0,0 +1,489 @@ +// Package wheeledodometry implements an odometery estimate from an encoder wheeled base +package wheeledodometry + +import ( + "context" + "errors" + "math" + "sync" + "testing" + "time" + + "github.com/edaniels/golog" + "go.viam.com/test" + "go.viam.com/utils" + + "go.viam.com/rdk/components/base" + "go.viam.com/rdk/components/motor" + "go.viam.com/rdk/resource" + "go.viam.com/rdk/testutils/inject" +) + +const ( + leftMotorName = "left" + rightMotorName = "right" + baseName = "base" + testSensorName = "name" + newLeftMotorName = "new_left" + newRightMotorName = "new_right" + newBaseName = "new_base" +) + +type positions struct { + mu sync.Mutex + leftPos float64 + rightPos float64 +} + +var position = positions{ + leftPos: 0.0, + rightPos: 0.0, +} + +func createFakeMotor(dir bool) motor.Motor { + return &inject.Motor{ + PropertiesFunc: func(ctx context.Context, extra map[string]interface{}) (motor.Properties, error) { + return motor.Properties{PositionReporting: true}, nil + }, + PositionFunc: func(ctx context.Context, extra map[string]interface{}) (float64, error) { + position.mu.Lock() + defer position.mu.Unlock() + if dir { + return position.leftPos, nil + } + return position.rightPos, nil + }, + IsMovingFunc: func(ctx context.Context) (bool, error) { + position.mu.Lock() + defer position.mu.Unlock() + if dir && math.Abs(position.leftPos) > 0 { + return true, nil + } + if !dir && math.Abs(position.rightPos) > 0 { + return true, nil + } + return false, nil + }, + ResetZeroPositionFunc: func(ctx context.Context, offset float64, extra map[string]interface{}) error { + position.mu.Lock() + defer position.mu.Unlock() + position.leftPos = 0 + position.rightPos = 0 + return nil + }, + StopFunc: func(ctx context.Context, extra map[string]interface{}) error { + return nil + }, + } +} + +func createFakeBase(circ, width, rad float64) base.Base { + return &inject.Base{ + PropertiesFunc: func(ctx context.Context, extra map[string]interface{}) (base.Properties, error) { + return base.Properties{WheelCircumferenceMeters: circ, WidthMeters: width, TurningRadiusMeters: rad}, nil + }, + } +} + +func setPositions(left, right float64) { + position.mu.Lock() + defer position.mu.Unlock() + position.leftPos += left + position.rightPos += right +} + +func TestNewWheeledOdometry(t *testing.T) { + ctx := context.Background() + logger := golog.NewTestLogger(t) + + deps := make(resource.Dependencies) + deps[base.Named(baseName)] = createFakeBase(0.1, 0.1, 0.1) + deps[motor.Named(leftMotorName)] = createFakeMotor(true) + deps[motor.Named(rightMotorName)] = createFakeMotor(false) + + fakecfg := resource.Config{ + Name: testSensorName, + ConvertedAttributes: &Config{ + LeftMotors: []string{leftMotorName}, + RightMotors: []string{rightMotorName}, + Base: baseName, + TimeIntervalMSecs: 500, + }, + } + fakeSensor, err := newWheeledOdometry(ctx, deps, fakecfg, logger) + test.That(t, err, test.ShouldBeNil) + _, ok := fakeSensor.(*odometry) + test.That(t, ok, test.ShouldBeTrue) +} + +func TestReconfigure(t *testing.T) { + ctx := context.Background() + logger := golog.NewTestLogger(t) + + deps := make(resource.Dependencies) + deps[base.Named(baseName)] = createFakeBase(0.1, 0.1, 0) + deps[motor.Named(leftMotorName)] = createFakeMotor(true) + deps[motor.Named(rightMotorName)] = createFakeMotor(false) + + fakecfg := resource.Config{ + Name: testSensorName, + ConvertedAttributes: &Config{ + LeftMotors: []string{leftMotorName}, + RightMotors: []string{rightMotorName}, + Base: baseName, + TimeIntervalMSecs: 500, + }, + } + fakeSensor, err := newWheeledOdometry(ctx, deps, fakecfg, logger) + test.That(t, err, test.ShouldBeNil) + od, ok := fakeSensor.(*odometry) + test.That(t, ok, test.ShouldBeTrue) + + newDeps := make(resource.Dependencies) + newDeps[base.Named(newBaseName)] = createFakeBase(0.2, 0.2, 0) + newDeps[motor.Named(newLeftMotorName)] = createFakeMotor(true) + newDeps[motor.Named(rightMotorName)] = createFakeMotor(false) + + newconf := resource.Config{ + Name: testSensorName, + ConvertedAttributes: &Config{ + LeftMotors: []string{newLeftMotorName}, + RightMotors: []string{rightMotorName}, + Base: newBaseName, + TimeIntervalMSecs: 500, + }, + } + + err = fakeSensor.Reconfigure(ctx, newDeps, newconf) + test.That(t, err, test.ShouldBeNil) + test.That(t, od.timeIntervalMSecs, test.ShouldEqual, 500) + test.That(t, od.baseWidth, test.ShouldEqual, 0.2) + test.That(t, od.wheelCircumference, test.ShouldEqual, 0.2) + + newDeps = make(resource.Dependencies) + newDeps[base.Named(newBaseName)] = createFakeBase(0.2, 0.2, 0) + newDeps[motor.Named(newLeftMotorName)] = createFakeMotor(true) + newDeps[motor.Named(newRightMotorName)] = createFakeMotor(false) + + newconf = resource.Config{ + Name: testSensorName, + ConvertedAttributes: &Config{ + LeftMotors: []string{newLeftMotorName}, + RightMotors: []string{newRightMotorName}, + Base: newBaseName, + TimeIntervalMSecs: 200, + }, + } + + err = fakeSensor.Reconfigure(ctx, newDeps, newconf) + test.That(t, err, test.ShouldBeNil) + test.That(t, od.timeIntervalMSecs, test.ShouldEqual, 200) + test.That(t, od.baseWidth, test.ShouldEqual, 0.2) + test.That(t, od.wheelCircumference, test.ShouldEqual, 0.2) +} + +func TestValidateConfig(t *testing.T) { + cfg := Config{ + LeftMotors: []string{leftMotorName}, + RightMotors: []string{rightMotorName}, + Base: "", + TimeIntervalMSecs: 500, + } + + deps, err := cfg.Validate("path") + expectedErr := utils.NewConfigValidationFieldRequiredError("path", "base") + test.That(t, err, test.ShouldBeError, expectedErr) + test.That(t, deps, test.ShouldBeEmpty) + + cfg = Config{ + LeftMotors: []string{}, + RightMotors: []string{rightMotorName}, + Base: baseName, + TimeIntervalMSecs: 500, + } + + deps, err = cfg.Validate("path") + expectedErr = utils.NewConfigValidationFieldRequiredError("path", "left motors") + test.That(t, err, test.ShouldBeError, expectedErr) + test.That(t, deps, test.ShouldBeEmpty) + + cfg = Config{ + LeftMotors: []string{leftMotorName}, + RightMotors: []string{}, + Base: baseName, + TimeIntervalMSecs: 500, + } + + deps, err = cfg.Validate("path") + expectedErr = utils.NewConfigValidationFieldRequiredError("path", "right motors") + test.That(t, err, test.ShouldBeError, expectedErr) + test.That(t, deps, test.ShouldBeEmpty) + + cfg = Config{ + LeftMotors: []string{leftMotorName, leftMotorName}, + RightMotors: []string{rightMotorName}, + Base: baseName, + TimeIntervalMSecs: 500, + } + + deps, err = cfg.Validate("path") + expectedErr = errors.New("mismatch number of left and right motors") + test.That(t, err, test.ShouldBeError, expectedErr) + test.That(t, deps, test.ShouldBeEmpty) + + cfg = Config{ + LeftMotors: []string{leftMotorName, leftMotorName}, + RightMotors: []string{rightMotorName, rightMotorName}, + Base: baseName, + TimeIntervalMSecs: 500, + } + + deps, err = cfg.Validate("path") + expectedErr = errors.New("wheeled odometry only supports one left and right motor each") + test.That(t, err, test.ShouldBeError, expectedErr) + test.That(t, deps, test.ShouldBeEmpty) +} + +func TestSpin(t *testing.T) { + left := createFakeMotor(true) + right := createFakeMotor(false) + ctx := context.Background() + _ = left.ResetZeroPosition(ctx, 0, nil) + _ = right.ResetZeroPosition(ctx, 0, nil) + + od := &odometry{ + lastLeftPos: 0, + lastRightPos: 0, + baseWidth: 1, + wheelCircumference: 1, + timeIntervalMSecs: 500, + } + od.motors = append(od.motors, motorPair{left, right}) + od.trackPosition(context.Background()) + + // turn 90 degrees + setPositions(-1*(math.Pi/4), 1*(math.Pi/4)) + // sleep for slightly longer than time interval to ensure trackPosition has enough time to run + time.Sleep(time.Duration(od.timeIntervalMSecs*1.15) * time.Millisecond) + + pos, _, _ := od.Position(ctx, nil) + or, err := od.Orientation(ctx, nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, or.OrientationVectorDegrees().Theta, test.ShouldAlmostEqual, 90, 0.1) + test.That(t, pos.Lat(), test.ShouldAlmostEqual, 0, 0.1) + test.That(t, pos.Lng(), test.ShouldAlmostEqual, 0, 0.1) + + // turn negative 180 degrees + setPositions(1*(math.Pi/2), -1*(math.Pi/2)) + time.Sleep(time.Duration(od.timeIntervalMSecs*1.15) * time.Millisecond) + + pos, _, _ = od.Position(ctx, nil) + or, err = od.Orientation(context.Background(), nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, or.OrientationVectorDegrees().Theta, test.ShouldAlmostEqual, 270, 0.1) + test.That(t, pos.Lat(), test.ShouldAlmostEqual, 0, 0.1) + test.That(t, pos.Lng(), test.ShouldAlmostEqual, 0, 0.1) + + // turn another 360 degrees + setPositions(-1*math.Pi, 1*math.Pi) + time.Sleep(time.Duration(od.timeIntervalMSecs*1.15) * time.Millisecond) + + pos, _, _ = od.Position(ctx, nil) + or, err = od.Orientation(context.Background(), nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, or.OrientationVectorDegrees().Theta, test.ShouldAlmostEqual, 270, 0.1) + test.That(t, pos.Lat(), test.ShouldAlmostEqual, 0, 0.1) + test.That(t, pos.Lng(), test.ShouldAlmostEqual, 0, 0.1) +} + +func TestMoveStraight(t *testing.T) { + left := createFakeMotor(true) + right := createFakeMotor(false) + ctx := context.Background() + _ = left.ResetZeroPosition(ctx, 0, nil) + _ = right.ResetZeroPosition(ctx, 0, nil) + + od := &odometry{ + lastLeftPos: 0, + lastRightPos: 0, + baseWidth: 1, + wheelCircumference: 1, + timeIntervalMSecs: 500, + } + od.motors = append(od.motors, motorPair{left, right}) + od.trackPosition(context.Background()) + + // move straight 5 m + setPositions(5, 5) + time.Sleep(time.Duration(od.timeIntervalMSecs*1.15) * time.Millisecond) + + pos, _, err := od.Position(ctx, nil) + or, _ := od.Orientation(context.Background(), nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, or.OrientationVectorDegrees().Theta, test.ShouldAlmostEqual, 0, 0.1) + test.That(t, pos.Lat(), test.ShouldAlmostEqual, 5, 0.1) + test.That(t, pos.Lng(), test.ShouldAlmostEqual, 0, 0.1) + + // move backwards 10 m + setPositions(-10, -10) + time.Sleep(time.Duration(od.timeIntervalMSecs*1.15) * time.Millisecond) + + pos, _, err = od.Position(ctx, nil) + or, _ = od.Orientation(context.Background(), nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, or.OrientationVectorDegrees().Theta, test.ShouldAlmostEqual, 0, 0.1) + test.That(t, pos.Lat(), test.ShouldAlmostEqual, -5, 0.1) + test.That(t, pos.Lng(), test.ShouldAlmostEqual, 0, 0.1) +} + +func TestComplicatedPath(t *testing.T) { + left := createFakeMotor(true) + right := createFakeMotor(false) + ctx := context.Background() + _ = left.ResetZeroPosition(ctx, 0, nil) + _ = right.ResetZeroPosition(ctx, 0, nil) + + od := &odometry{ + lastLeftPos: 0, + lastRightPos: 0, + baseWidth: 1, + wheelCircumference: 1, + timeIntervalMSecs: 500, + } + od.motors = append(od.motors, motorPair{left, right}) + od.trackPosition(context.Background()) + + // move straight 5 m + setPositions(5, 5) + time.Sleep(time.Duration(od.timeIntervalMSecs*1.15) * time.Millisecond) + + pos, _, err := od.Position(ctx, nil) + test.That(t, err, test.ShouldBeNil) + or, _ := od.Orientation(context.Background(), nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, or.OrientationVectorDegrees().Theta, test.ShouldAlmostEqual, 0, 0.1) + test.That(t, pos.Lat(), test.ShouldAlmostEqual, 5, 0.1) + test.That(t, pos.Lng(), test.ShouldAlmostEqual, 0, 0.1) + + // spin negative 90 degrees + setPositions(1*(math.Pi/4), -1*(math.Pi/4)) + time.Sleep(time.Duration(od.timeIntervalMSecs*1.15) * time.Millisecond) + + pos, _, err = od.Position(ctx, nil) + test.That(t, err, test.ShouldBeNil) + or, err = od.Orientation(context.Background(), nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, or.OrientationVectorDegrees().Theta, test.ShouldAlmostEqual, 270, 0.1) + test.That(t, pos.Lat(), test.ShouldAlmostEqual, 5, 0.1) + test.That(t, pos.Lng(), test.ShouldAlmostEqual, 0, 0.1) + + // move forward another 5 m + setPositions(5, 5) + time.Sleep(time.Duration(od.timeIntervalMSecs*1.15) * time.Millisecond) + + pos, _, err = od.Position(ctx, nil) + test.That(t, err, test.ShouldBeNil) + or, err = od.Orientation(context.Background(), nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, or.OrientationVectorDegrees().Theta, test.ShouldAlmostEqual, 270, 0.1) + test.That(t, pos.Lat(), test.ShouldAlmostEqual, 5, 0.1) + test.That(t, pos.Lng(), test.ShouldAlmostEqual, -5, 0.1) + + // spin positive 45 degrees + setPositions(-1*(math.Pi/8), 1*(math.Pi/8)) + time.Sleep(time.Duration(od.timeIntervalMSecs*1.15) * time.Millisecond) + + pos, _, err = od.Position(ctx, nil) + test.That(t, err, test.ShouldBeNil) + or, err = od.Orientation(context.Background(), nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, or.OrientationVectorDegrees().Theta, test.ShouldAlmostEqual, 315, 0.1) + test.That(t, pos.Lat(), test.ShouldAlmostEqual, 5, 0.1) + test.That(t, pos.Lng(), test.ShouldAlmostEqual, -5, 0.1) + + // move forward 2 m + setPositions(2, 2) + time.Sleep(time.Duration(od.timeIntervalMSecs*1.15) * time.Millisecond) + + pos, _, err = od.Position(ctx, nil) + test.That(t, err, test.ShouldBeNil) + or, err = od.Orientation(context.Background(), nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, or.OrientationVectorDegrees().Theta, test.ShouldAlmostEqual, 315, 0.1) + test.That(t, pos.Lat(), test.ShouldAlmostEqual, 6.4, 0.1) + test.That(t, pos.Lng(), test.ShouldAlmostEqual, -6.4, 0.1) + + // travel in an arc + setPositions(1*(math.Pi/4), 2*(math.Pi/4)) + time.Sleep(time.Duration(od.timeIntervalMSecs*1.15) * time.Millisecond) + + pos, _, err = od.Position(ctx, nil) + test.That(t, err, test.ShouldBeNil) + or, err = od.Orientation(context.Background(), nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, or.OrientationVectorDegrees().Theta, test.ShouldAlmostEqual, 0, 0.1) + test.That(t, pos.Lat(), test.ShouldAlmostEqual, 7.6, 0.1) + test.That(t, pos.Lng(), test.ShouldAlmostEqual, -6.4, 0.1) +} + +func TestVelocities(t *testing.T) { + left := createFakeMotor(true) + right := createFakeMotor(false) + ctx := context.Background() + _ = left.ResetZeroPosition(ctx, 0, nil) + _ = right.ResetZeroPosition(ctx, 0, nil) + + od := &odometry{ + lastLeftPos: 0, + lastRightPos: 0, + baseWidth: 1, + wheelCircumference: 1, + timeIntervalMSecs: 500, + } + od.motors = append(od.motors, motorPair{left, right}) + od.trackPosition(context.Background()) + + // move forward 10 m + setPositions(10, 10) + time.Sleep(time.Duration(od.timeIntervalMSecs*1.15) * time.Millisecond) + + linVel, err := od.LinearVelocity(ctx, nil) + test.That(t, err, test.ShouldBeNil) + angVel, err := od.AngularVelocity(ctx, nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, linVel.Y, test.ShouldAlmostEqual, 20, 0.1) + test.That(t, angVel.Z, test.ShouldAlmostEqual, 0, 0.1) + + // spin 45 degrees + setPositions(-1*(math.Pi/8), 1*(math.Pi/8)) + time.Sleep(time.Duration(od.timeIntervalMSecs*1.15) * time.Millisecond) + + linVel, err = od.LinearVelocity(ctx, nil) + test.That(t, err, test.ShouldBeNil) + angVel, err = od.AngularVelocity(ctx, nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, linVel.Y, test.ShouldAlmostEqual, 0, 0.1) + test.That(t, angVel.Z, test.ShouldAlmostEqual, 90, 0.1) + + // spin back 45 degrees + setPositions(1*(math.Pi/8), -1*(math.Pi/8)) + time.Sleep(time.Duration(od.timeIntervalMSecs*1.15) * time.Millisecond) + + linVel, err = od.LinearVelocity(ctx, nil) + test.That(t, err, test.ShouldBeNil) + angVel, err = od.AngularVelocity(ctx, nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, linVel.Y, test.ShouldAlmostEqual, 0, 0.1) + test.That(t, angVel.Z, test.ShouldAlmostEqual, -90, 0.1) + + // move backwards 5 m + setPositions(-5, -5) + time.Sleep(time.Duration(od.timeIntervalMSecs*1.15) * time.Millisecond) + + linVel, err = od.LinearVelocity(ctx, nil) + test.That(t, err, test.ShouldBeNil) + angVel, err = od.AngularVelocity(ctx, nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, linVel.Y, test.ShouldAlmostEqual, -10, 0.1) + test.That(t, angVel.Z, test.ShouldAlmostEqual, 0, 0.1) +} diff --git a/components/posetracker/client_test.go b/components/posetracker/client_test.go index c96d25e9d68..e924ff20796 100644 --- a/components/posetracker/client_test.go +++ b/components/posetracker/client_test.go @@ -2,7 +2,6 @@ package posetracker_test import ( "context" - "errors" "math" "net" "testing" @@ -70,7 +69,7 @@ func TestClient(t *testing.T) { failingPT.PosesFunc = func(ctx context.Context, bodyNames []string, extra map[string]interface{}) ( posetracker.BodyToPoseInFrame, error, ) { - return nil, errors.New("failure to get poses") + return nil, errPoseFailed } resourceMap := map[resource.Name]posetracker.PoseTracker{ @@ -94,7 +93,7 @@ func TestClient(t *testing.T) { cancel() _, err := viamgrpc.Dial(cancelCtx, listener1.Addr().String(), logger) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "canceled") + test.That(t, err, test.ShouldBeError, context.Canceled) }) conn, err := viamgrpc.Dial(context.Background(), listener1.Addr().String(), logger) @@ -142,6 +141,7 @@ func TestClient(t *testing.T) { bodyToPoseInFrame, err := failingPTDialedClient.Poses(context.Background(), []string{}, nil) test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, errPoseFailed.Error()) test.That(t, bodyToPoseInFrame, test.ShouldBeNil) test.That(t, conn.Close(), test.ShouldBeNil) }) diff --git a/components/posetracker/server_test.go b/components/posetracker/server_test.go index dfcfb94ab5a..4e89b456c04 100644 --- a/components/posetracker/server_test.go +++ b/components/posetracker/server_test.go @@ -16,6 +16,8 @@ import ( "go.viam.com/rdk/testutils/inject" ) +var errPoseFailed = errors.New("failure to get poses") + const ( workingPTName = "workingPT" failingPTName = "failingPT" @@ -53,11 +55,11 @@ func TestGetPoses(t *testing.T) { bodyName: referenceframe.NewPoseInFrame(bodyFrame, zeroPose), }, nil } - poseFailureErr := errors.New("failure to get poses") + failingPT.PosesFunc = func(ctx context.Context, bodyNames []string, extra map[string]interface{}) ( posetracker.BodyToPoseInFrame, error, ) { - return nil, poseFailureErr + return nil, errPoseFailed } t.Run("get poses fails on failing pose tracker", func(t *testing.T) { @@ -65,7 +67,7 @@ func TestGetPoses(t *testing.T) { Name: failingPTName, BodyNames: []string{bodyName}, } resp, err := ptServer.GetPoses(context.Background(), &req) - test.That(t, err, test.ShouldBeError, poseFailureErr) + test.That(t, err, test.ShouldBeError, errPoseFailed) test.That(t, resp, test.ShouldBeNil) }) diff --git a/components/powersensor/client.go b/components/powersensor/client.go new file mode 100644 index 00000000000..5a97636992b --- /dev/null +++ b/components/powersensor/client.go @@ -0,0 +1,100 @@ +package powersensor + +import ( + "context" + + "github.com/edaniels/golog" + pb "go.viam.com/api/component/powersensor/v1" + "go.viam.com/utils/rpc" + "google.golang.org/protobuf/types/known/structpb" + + "go.viam.com/rdk/protoutils" + "go.viam.com/rdk/resource" +) + +// client implements PowerSensorServiceClient. +type client struct { + resource.Named + resource.TriviallyReconfigurable + resource.TriviallyCloseable + name string + client pb.PowerSensorServiceClient + logger golog.Logger +} + +// NewClientFromConn constructs a new client from connection passed in. +func NewClientFromConn( + ctx context.Context, + conn rpc.ClientConn, + remoteName string, + name resource.Name, + logger golog.Logger, +) (PowerSensor, error) { + c := pb.NewPowerSensorServiceClient(conn) + return &client{ + Named: name.PrependRemote(remoteName).AsNamed(), + name: name.ShortName(), + client: c, + logger: logger, + }, nil +} + +// Voltage returns the voltage reading in volts and a bool returning true if the voltage is AC. +func (c *client) Voltage(ctx context.Context, extra map[string]interface{}) (float64, bool, error) { + ext, err := structpb.NewStruct(extra) + if err != nil { + return 0, false, err + } + resp, err := c.client.GetVoltage(ctx, &pb.GetVoltageRequest{ + Name: c.name, + Extra: ext, + }) + if err != nil { + return 0, false, err + } + return resp.Volts, + resp.IsAc, + nil +} + +// Current returns the current reading in amperes and a bool returning true if the current is AC. +func (c *client) Current(ctx context.Context, extra map[string]interface{}) (float64, bool, error) { + ext, err := structpb.NewStruct(extra) + if err != nil { + return 0, false, err + } + resp, err := c.client.GetCurrent(ctx, &pb.GetCurrentRequest{ + Name: c.name, + Extra: ext, + }) + if err != nil { + return 0, false, err + } + return resp.Amperes, + resp.IsAc, + nil +} + +// Power returns the power reading in watts. +func (c *client) Power(ctx context.Context, extra map[string]interface{}) (float64, error) { + ext, err := structpb.NewStruct(extra) + if err != nil { + return 0, err + } + resp, err := c.client.GetPower(ctx, &pb.GetPowerRequest{ + Name: c.name, + Extra: ext, + }) + if err != nil { + return 0, err + } + return resp.Watts, nil +} + +func (c *client) Readings(ctx context.Context, extra map[string]interface{}) (map[string]interface{}, error) { + return Readings(ctx, c, extra) +} + +func (c *client) DoCommand(ctx context.Context, cmd map[string]interface{}) (map[string]interface{}, error) { + return protoutils.DoFromResourceClient(ctx, c.client, c.name, cmd) +} diff --git a/components/powersensor/client_test.go b/components/powersensor/client_test.go new file mode 100644 index 00000000000..3a45ba56a1e --- /dev/null +++ b/components/powersensor/client_test.go @@ -0,0 +1,140 @@ +// package powersensor_test contains tests for powersensor +package powersensor_test + +import ( + "context" + "net" + "testing" + + "github.com/edaniels/golog" + "go.viam.com/test" + "go.viam.com/utils/rpc" + + "go.viam.com/rdk/components/motor" + "go.viam.com/rdk/components/powersensor" + viamgrpc "go.viam.com/rdk/grpc" + "go.viam.com/rdk/resource" + "go.viam.com/rdk/testutils" + "go.viam.com/rdk/testutils/inject" +) + +func TestClient(t *testing.T) { + logger := golog.NewTestLogger(t) + listener1, err := net.Listen("tcp", "localhost:0") + test.That(t, err, test.ShouldBeNil) + rpcServer, err := rpc.NewServer(logger, rpc.WithUnauthenticated()) + test.That(t, err, test.ShouldBeNil) + + testVolts := 4.8 + testAmps := 3.8 + testWatts := 2.8 + testIsAC := false + + workingPowerSensor := &inject.PowerSensor{} + failingPowerSensor := &inject.PowerSensor{} + + workingPowerSensor.VoltageFunc = func(ctx context.Context, extra map[string]interface{}) (float64, bool, error) { + return testVolts, testIsAC, nil + } + + workingPowerSensor.CurrentFunc = func(ctx context.Context, extra map[string]interface{}) (float64, bool, error) { + return testAmps, testIsAC, nil + } + + workingPowerSensor.PowerFunc = func(ctx context.Context, extra map[string]interface{}) (float64, error) { + return testWatts, nil + } + + failingPowerSensor.VoltageFunc = func(ctx context.Context, extra map[string]interface{}) (float64, bool, error) { + return 0, false, errVoltageFailed + } + + failingPowerSensor.CurrentFunc = func(ctx context.Context, extra map[string]interface{}) (float64, bool, error) { + return 0, false, errCurrentFailed + } + + failingPowerSensor.PowerFunc = func(ctx context.Context, extra map[string]interface{}) (float64, error) { + return 0, errPowerFailed + } + + resourceMap := map[resource.Name]powersensor.PowerSensor{ + motor.Named(workingPowerSensorName): workingPowerSensor, + motor.Named(failingPowerSensorName): failingPowerSensor, + } + powersensorSvc, err := resource.NewAPIResourceCollection(powersensor.API, resourceMap) + test.That(t, err, test.ShouldBeNil) + resourceAPI, ok, err := resource.LookupAPIRegistration[powersensor.PowerSensor](powersensor.API) + test.That(t, err, test.ShouldBeNil) + test.That(t, ok, test.ShouldBeTrue) + test.That(t, resourceAPI.RegisterRPCService(context.Background(), rpcServer, powersensorSvc), test.ShouldBeNil) + + workingPowerSensor.DoFunc = testutils.EchoFunc + + go rpcServer.Serve(listener1) + defer rpcServer.Stop() + + // failing client + t.Run("Failing client", func(t *testing.T) { + cancelCtx, cancel := context.WithCancel(context.Background()) + cancel() + _, err := viamgrpc.Dial(cancelCtx, listener1.Addr().String(), logger) + test.That(t, err, test.ShouldBeError, context.Canceled) + }) + + conn, err := viamgrpc.Dial(context.Background(), listener1.Addr().String(), logger) + test.That(t, err, test.ShouldBeNil) + client, err := powersensor.NewClientFromConn(context.Background(), conn, "", motor.Named(workingPowerSensorName), logger) + test.That(t, err, test.ShouldBeNil) + + t.Run("client tests with working power sensor", func(t *testing.T) { + // DoCommand + resp, err := client.DoCommand(context.Background(), testutils.TestCommand) + test.That(t, err, test.ShouldBeNil) + test.That(t, resp["command"], test.ShouldEqual, testutils.TestCommand["command"]) + test.That(t, resp["data"], test.ShouldEqual, testutils.TestCommand["data"]) + + volts, isAC, err := client.Voltage(context.Background(), make(map[string]interface{})) + test.That(t, err, test.ShouldBeNil) + test.That(t, volts, test.ShouldEqual, testVolts) + test.That(t, isAC, test.ShouldEqual, testIsAC) + + amps, isAC, err := client.Current(context.Background(), make(map[string]interface{})) + test.That(t, err, test.ShouldBeNil) + test.That(t, amps, test.ShouldEqual, testAmps) + test.That(t, isAC, test.ShouldEqual, testIsAC) + + watts, err := client.Power(context.Background(), make(map[string]interface{})) + test.That(t, err, test.ShouldBeNil) + test.That(t, watts, test.ShouldEqual, testWatts) + + test.That(t, client.Close(context.Background()), test.ShouldBeNil) + test.That(t, conn.Close(), test.ShouldBeNil) + }) + + conn, err = viamgrpc.Dial(context.Background(), listener1.Addr().String(), logger) + test.That(t, err, test.ShouldBeNil) + client, err = powersensor.NewClientFromConn(context.Background(), conn, "", powersensor.Named(failingPowerSensorName), logger) + test.That(t, err, test.ShouldBeNil) + + t.Run("client tests with failing power sensor", func(t *testing.T) { + volts, isAC, err := client.Voltage(context.Background(), make(map[string]interface{})) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, errVoltageFailed.Error()) + test.That(t, volts, test.ShouldEqual, 0) + test.That(t, isAC, test.ShouldEqual, false) + + amps, isAC, err := client.Current(context.Background(), make(map[string]interface{})) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, errCurrentFailed.Error()) + test.That(t, amps, test.ShouldEqual, 0) + test.That(t, isAC, test.ShouldEqual, false) + + watts, err := client.Power(context.Background(), make(map[string]interface{})) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, errPowerFailed.Error()) + test.That(t, watts, test.ShouldEqual, 0) + + test.That(t, client.Close(context.Background()), test.ShouldBeNil) + test.That(t, conn.Close(), test.ShouldBeNil) + }) +} diff --git a/components/powersensor/collectors.go b/components/powersensor/collectors.go new file mode 100644 index 00000000000..ddbbe34d116 --- /dev/null +++ b/components/powersensor/collectors.go @@ -0,0 +1,47 @@ +package powersensor + +import ( + "context" + "errors" + + "google.golang.org/protobuf/types/known/anypb" + + "go.viam.com/rdk/data" +) + +func assertPowerSensor(resource interface{}) (PowerSensor, error) { + ps, ok := resource.(PowerSensor) + if !ok { + return nil, data.InvalidInterfaceErr(API) + } + return ps, nil +} + +type lowLevelCollector func(ctx context.Context, ps PowerSensor, extra map[string]interface{}) (interface{}, error) + +func registerCollector(name string, f lowLevelCollector) { + data.RegisterCollector(data.MethodMetadata{ + API: API, + MethodName: name, + }, func(resource interface{}, params data.CollectorParams) (data.Collector, error) { + ps, err := assertPowerSensor(resource) + if err != nil { + return nil, err + } + + cFunc := data.CaptureFunc(func(ctx context.Context, extra map[string]*anypb.Any) (interface{}, error) { + v, err := f(ctx, ps, data.FromDMExtraMap) + if err != nil { + // A modular filter component can be created to filter the readings from a component. The error ErrNoCaptureToStore + // is used in the datamanager to exclude readings from being captured and stored. + if errors.Is(err, data.ErrNoCaptureToStore) { + return nil, err + } + return nil, data.FailedToReadErr(params.ComponentName, name, err) + } + return v, nil + }) + return data.NewCollector(cFunc, params) + }, + ) +} diff --git a/components/powersensor/errors.go b/components/powersensor/errors.go new file mode 100644 index 00000000000..3bf3afe196a --- /dev/null +++ b/components/powersensor/errors.go @@ -0,0 +1,12 @@ +package powersensor + +import "errors" + +var ( + // ErrMethodUnimplementedVoltage returns error if the Voltage method is unimplemented. + ErrMethodUnimplementedVoltage = errors.New("Voltage Unimplemented") + // ErrMethodUnimplementedCurrent returns error if the Current method is unimplemented. + ErrMethodUnimplementedCurrent = errors.New("Current Unimplemented") + // ErrMethodUnimplementedPower returns error if the Power method is unimplemented. + ErrMethodUnimplementedPower = errors.New("Power Unimplemented") +) diff --git a/components/powersensor/fake/powersensor.go b/components/powersensor/fake/powersensor.go new file mode 100644 index 00000000000..6e540643693 --- /dev/null +++ b/components/powersensor/fake/powersensor.go @@ -0,0 +1,72 @@ +// Package fake is a fake PowerSensor for testing +package fake + +import ( + "context" + + "github.com/edaniels/golog" + + "go.viam.com/rdk/components/powersensor" + "go.viam.com/rdk/resource" +) + +var model = resource.DefaultModelFamily.WithModel("fake") + +// Config is used for converting fake movementsensor attributes. +type Config struct { + resource.TriviallyValidateConfig +} + +func init() { + resource.RegisterComponent( + powersensor.API, + model, + resource.Registration[powersensor.PowerSensor, *Config]{ + Constructor: newFakePowerSensorModel, + }) +} + +func newFakePowerSensorModel(_ context.Context, _ resource.Dependencies, conf resource.Config, logger golog.Logger, +) (powersensor.PowerSensor, error) { + return powersensor.PowerSensor(&PowerSensor{ + Named: conf.ResourceName().AsNamed(), + logger: logger, + }), nil +} + +// PowerSensor implements a fake PowerSensor interface. +type PowerSensor struct { + resource.Named + resource.AlwaysRebuild + logger golog.Logger +} + +// DoCommand uses a map string to run custom functionality of a fake powersensor. +func (f *PowerSensor) DoCommand(ctx context.Context, cmd map[string]interface{}) (map[string]interface{}, error) { + return map[string]interface{}{}, nil +} + +// Voltage gets the voltage and isAC of a fake powersensor. +func (f *PowerSensor) Voltage(ctx context.Context, cmd map[string]interface{}) (float64, bool, error) { + return 1.5, true, nil +} + +// Current gets the current and isAC of a fake powersensor. +func (f *PowerSensor) Current(ctx context.Context, cmd map[string]interface{}) (float64, bool, error) { + return 2.2, true, nil +} + +// Power gets the power of a fake powersensor. +func (f *PowerSensor) Power(ctx context.Context, cmd map[string]interface{}) (float64, error) { + return 9.8, nil +} + +// Readings gets the readings of a fake powersensor. +func (f *PowerSensor) Readings(ctx context.Context, extra map[string]interface{}) (map[string]interface{}, error) { + return powersensor.Readings(ctx, f, extra) +} + +// Close closes the fake powersensor. +func (f *PowerSensor) Close(ctx context.Context) error { + return nil +} diff --git a/components/powersensor/ina/ina.go b/components/powersensor/ina/ina.go new file mode 100644 index 00000000000..358893f316e --- /dev/null +++ b/components/powersensor/ina/ina.go @@ -0,0 +1,329 @@ +//go:build linux + +// Package ina implements ina power sensors to measure voltage, current, and power +// INA219 datasheet: https://www.ti.com/lit/ds/symlink/ina219.pdf +// Example repo: https://github.com/periph/devices/blob/main/ina219/ina219.go +// INA226 datasheet: https://www.ti.com/lit/ds/symlink/ina226.pdf + +// The voltage, current and power can be read as +// 16 bit big endian integers from their given registers. +// This value is multiplied by the register LSB to get the reading in nanounits. + +// Voltage LSB: 1.25 mV for INA226, 4 mV for INA219 +// Current LSB: maximum expected current of the system / (1 << 15) +// Power LSB: 25*CurrentLSB for INA226, 20*CurrentLSB for INA219 + +// The calibration register is programmed to measure current and power properly. +// The calibration register is set to: calibratescale / (currentLSB * senseResistor) + +package ina + +import ( + "context" + "errors" + "fmt" + + "github.com/d2r2/go-i2c" + i2clog "github.com/d2r2/go-logger" + "github.com/edaniels/golog" + "go.viam.com/utils" + + "go.viam.com/rdk/components/powersensor" + "go.viam.com/rdk/resource" +) + +const ( + modelName219 = "ina219" + modelName226 = "ina226" + defaultI2Caddr = 0x40 + configRegister = 0x00 + shuntVoltageRegister = 0x01 + busVoltageRegister = 0x02 + powerRegister = 0x03 + currentRegister = 0x04 + calibrationRegister = 0x05 +) + +// values for inas in nano units so need to convert. +var ( + senseResistor = toNano(0.1) // .1 ohm + maxCurrent219 = toNano(3.2) // 3.2 amp + maxCurrent226 = toNano(20) // 20 amp +) + +// need to scale, making sure to not overflow int64. +var ( + calibratescale219 = (toNano(1) * toNano(1) / 100000) << 12 // .04096 is internal fixed value for ina219 + calibrateScale226 = (toNano(1) * toNano(1) / 100000) << 9 // .00512 is internal fixed value for ina226 +) + +var inaModels = []string{modelName219, modelName226} + +// Config is used for converting config attributes. +type Config struct { + I2CBus int `json:"i2c_bus"` + I2cAddr int `json:"i2c_addr,omitempty"` + MaxCurrent float64 `json:"max_current_amps,omitempty"` + ShuntResistance float64 `json:"shunt_resistance,omitempty"` +} + +// Validate ensures all parts of the config are valid. +func (conf *Config) Validate(path string) ([]string, error) { + var deps []string + if conf.I2CBus == 0 { + return nil, utils.NewConfigValidationFieldRequiredError(path, "i2c_bus") + } + return deps, nil +} + +func init() { + for _, modelName := range inaModels { + localModelName := modelName + inaModel := resource.DefaultModelFamily.WithModel(modelName) + resource.RegisterComponent( + powersensor.API, + inaModel, + resource.Registration[powersensor.PowerSensor, *Config]{ + Constructor: func( + ctx context.Context, + deps resource.Dependencies, + conf resource.Config, + logger golog.Logger, + ) (powersensor.PowerSensor, error) { + newConf, err := resource.NativeConfig[*Config](conf) + if err != nil { + return nil, err + } + return newINA(conf.ResourceName(), newConf, logger, localModelName) + }, + }) + } +} + +func newINA( + name resource.Name, + conf *Config, + logger golog.Logger, + modelName string, +) (powersensor.PowerSensor, error) { + err := i2clog.ChangePackageLogLevel("i2c", i2clog.InfoLevel) + if err != nil { + return nil, err + } + + addr := conf.I2cAddr + if addr == 0 { + addr = defaultI2Caddr + logger.Infof("using i2c address : %d", defaultI2Caddr) + } + + maxCurrent := toNano(conf.MaxCurrent) + if maxCurrent == 0 { + switch modelName { + case modelName219: + maxCurrent = maxCurrent219 + logger.Info("using default max current 3.2A") + case modelName226: + maxCurrent = maxCurrent226 + logger.Info("using default max current 20A") + } + } + + resistance := toNano(conf.ShuntResistance) + if resistance == 0 { + resistance = senseResistor + logger.Info("using default resistor value 0.1 ohms") + } + + s := &ina{ + Named: name.AsNamed(), + logger: logger, + model: modelName, + bus: conf.I2CBus, + addr: byte(addr), + maxCurrent: maxCurrent, + resistance: resistance, + } + + err = s.setCalibrationScale(modelName) + if err != nil { + return nil, err + } + + return s, nil +} + +// ina is a i2c sensor device that reports voltage, current and power. +type ina struct { + resource.Named + resource.AlwaysRebuild + resource.TriviallyCloseable + logger golog.Logger + model string + bus int + addr byte + currentLSB int64 + powerLSB int64 + cal uint16 + maxCurrent int64 + resistance int64 +} + +func (d *ina) setCalibrationScale(modelName string) error { + var calibratescale int64 + d.currentLSB = d.maxCurrent / (1 << 15) + switch modelName { + case modelName219: + calibratescale = calibratescale219 + d.powerLSB = (d.maxCurrent*20 + (1 << 14)) / (1 << 15) + case modelName226: + calibratescale = calibrateScale226 + d.powerLSB = 25 * d.currentLSB + default: + return errors.New("ina model not supported") + } + + // Calibration Register = calibration scale / (current LSB * Shunt Resistance) + // Where lsb is in Amps and resistance is in ohms. + // Calibration register is 16 bits. + cal := calibratescale / (d.currentLSB * d.resistance) + if cal >= (1 << 16) { + return fmt.Errorf("ina calibrate: calibration register value invalid %d", cal) + } + d.cal = uint16(cal) + + return nil +} + +func (d *ina) calibrate() error { + handle, err := i2c.NewI2C(d.addr, d.bus) + if err != nil { + d.logger.Errorf("can't open ina i2c: %s", err) + return err + } + defer utils.UncheckedErrorFunc(handle.Close) + + // use the calibration result to set the scaling factor + // of the current and power registers for the maximum resolution + err = handle.WriteRegU16BE(calibrationRegister, d.cal) + if err != nil { + return err + } + + // set the config register to all default values. + err = handle.WriteRegU16BE(configRegister, uint16(0x399F)) + if err != nil { + return err + } + return nil +} + +func (d *ina) Voltage(ctx context.Context, extra map[string]interface{}) (float64, bool, error) { + handle, err := i2c.NewI2C(d.addr, d.bus) + if err != nil { + d.logger.Errorf("can't open ina i2c: %s", err) + return 0, false, err + } + defer utils.UncheckedErrorFunc(handle.Close) + + bus, err := handle.ReadRegS16BE(busVoltageRegister) + if err != nil { + return 0, false, err + } + + var voltage float64 + switch d.model { + case modelName226: + // voltage is 1.25 mV/bit for the ina226 + voltage = float64(bus) * 1.25e-3 + case modelName219: + // lsb is 4mV, must shift right 3 bits + voltage = float64(bus>>3) * 4 / 1000 + default: + } + + isAC := false + return voltage, isAC, nil +} + +func (d *ina) Current(ctx context.Context, extra map[string]interface{}) (float64, bool, error) { + handle, err := i2c.NewI2C(d.addr, d.bus) + if err != nil { + d.logger.Errorf("can't open ina i2c: %s", err) + return 0, false, err + } + defer utils.UncheckedErrorFunc(handle.Close) + + // Calibrate each time the current value is read, so if anything else is also writing to these registers + // we have the correct value. + err = d.calibrate() + if err != nil { + return 0, false, err + } + + rawCur, err := handle.ReadRegS16BE(currentRegister) + if err != nil { + return 0, false, err + } + + current := fromNano(float64(int64(rawCur) * d.currentLSB)) + isAC := false + return current, isAC, nil +} + +func (d *ina) Power(ctx context.Context, extra map[string]interface{}) (float64, error) { + handle, err := i2c.NewI2C(d.addr, d.bus) + if err != nil { + d.logger.Errorf("can't open ina i2c handle: %s", err) + return 0, err + } + defer utils.UncheckedErrorFunc(handle.Close) + + // Calibrate each time the power value is read, so if anything else is also writing to these registers + // we have the correct value. + err = d.calibrate() + if err != nil { + return 0, err + } + + pow, err := handle.ReadRegS16BE(powerRegister) + if err != nil { + return 0, err + } + power := fromNano(float64(int64(pow) * d.powerLSB)) + return power, nil +} + +// Readings returns a map with voltage, current, power and isAC. +func (d *ina) Readings(ctx context.Context, extra map[string]interface{}) (map[string]interface{}, error) { + volts, isAC, err := d.Voltage(ctx, nil) + if err != nil { + d.logger.Errorf("failed to get voltage reading: %s", err.Error()) + } + + amps, _, err := d.Current(ctx, nil) + if err != nil { + d.logger.Errorf("failed to get current reading: %s", err.Error()) + } + + watts, err := d.Power(ctx, nil) + if err != nil { + d.logger.Errorf("failed to get power reading: %s", err.Error()) + } + return map[string]interface{}{ + "volts": volts, + "amps": amps, + "is_ac": isAC, + "watts": watts, + }, nil +} + +func toNano(value float64) int64 { + nano := value * 1e9 + return int64(nano) +} + +func fromNano(value float64) float64 { + unit := value / 1e9 + return unit +} diff --git a/components/powersensor/ina/ina_nonlinux.go b/components/powersensor/ina/ina_nonlinux.go new file mode 100644 index 00000000000..10d6ac02206 --- /dev/null +++ b/components/powersensor/ina/ina_nonlinux.go @@ -0,0 +1,2 @@ +// Package ina this is blank for mac +package ina diff --git a/components/powersensor/powersensor.go b/components/powersensor/powersensor.go new file mode 100644 index 00000000000..4d2138e483d --- /dev/null +++ b/components/powersensor/powersensor.go @@ -0,0 +1,129 @@ +// Package powersensor defines the interfaces of a powersensor +package powersensor + +import ( + "context" + "strings" + + pb "go.viam.com/api/component/powersensor/v1" + + "go.viam.com/rdk/components/sensor" + "go.viam.com/rdk/resource" + "go.viam.com/rdk/robot" +) + +func init() { + resource.RegisterAPI(API, resource.APIRegistration[PowerSensor]{ + RPCServiceServerConstructor: NewRPCServiceServer, + RPCServiceHandler: pb.RegisterPowerSensorServiceHandlerFromEndpoint, + RPCServiceDesc: &pb.PowerSensorService_ServiceDesc, + RPCClient: NewClientFromConn, + }) + + registerCollector("Voltage", func(ctx context.Context, ps PowerSensor, extra map[string]interface{}) (interface{}, error) { + type Voltage struct { + Volts float64 + IsAc bool + } + v, ac, err := ps.Voltage(ctx, extra) + if err != nil { + return nil, err + } + + return Voltage{Volts: v, IsAc: ac}, nil + }) + + registerCollector("Current", func(ctx context.Context, ps PowerSensor, extra map[string]interface{}) (interface{}, error) { + type Current struct { + Amperes float64 + IsAc bool + } + c, ac, err := ps.Current(ctx, extra) + if err != nil { + return nil, err + } + return Current{Amperes: c, IsAc: ac}, nil + }) + + registerCollector("Power", func(ctx context.Context, ps PowerSensor, extra map[string]interface{}) (interface{}, error) { + type Power struct { + Watts float64 + } + p, err := ps.Power(ctx, extra) + if err != nil { + return nil, err + } + return Power{Watts: p}, nil + }) +} + +// SubtypeName is a constant that identifies the component resource API string "power_sensor". +const SubtypeName = "power_sensor" + +// API is a variable that identifies the component resource API. +var API = resource.APINamespaceRDK.WithComponentType(SubtypeName) + +// Named is a helper for getting the named PowerSensor's typed resource name. +func Named(name string) resource.Name { + return resource.NewName(API, name) +} + +// A PowerSensor reports information about voltage, current and power. +type PowerSensor interface { + sensor.Sensor + Voltage(ctx context.Context, extra map[string]interface{}) (float64, bool, error) + Current(ctx context.Context, extra map[string]interface{}) (float64, bool, error) + Power(ctx context.Context, extra map[string]interface{}) (float64, error) +} + +// FromDependencies is a helper for getting the named PowerSensor from a collection of +// dependencies. +func FromDependencies(deps resource.Dependencies, name string) (PowerSensor, error) { + return resource.FromDependencies[PowerSensor](deps, Named(name)) +} + +// FromRobot is a helper for getting the named PowerSensor from the given Robot. +func FromRobot(r robot.Robot, name string) (PowerSensor, error) { + return robot.ResourceFromRobot[PowerSensor](r, Named(name)) +} + +// NamesFromRobot is a helper for getting all PowerSensor names from the given Robot. +func NamesFromRobot(r robot.Robot) []string { + return robot.NamesByAPI(r, API) +} + +// Readings is a helper for getting all readings from a PowerSensor. +func Readings(ctx context.Context, g PowerSensor, extra map[string]interface{}) (map[string]interface{}, error) { + readings := map[string]interface{}{} + + vol, isAC, err := g.Voltage(ctx, extra) + if err != nil { + if !strings.Contains(err.Error(), ErrMethodUnimplementedVoltage.Error()) { + return nil, err + } + } else { + readings["voltage"] = vol + readings["is_ac"] = isAC + } + + cur, isAC, err := g.Current(ctx, extra) + if err != nil { + if !strings.Contains(err.Error(), ErrMethodUnimplementedCurrent.Error()) { + return nil, err + } + } else { + readings["current"] = cur + readings["is_ac"] = isAC + } + + pow, err := g.Power(ctx, extra) + if err != nil { + if !strings.Contains(err.Error(), ErrMethodUnimplementedPower.Error()) { + return nil, err + } + } else { + readings["power"] = pow + } + + return readings, nil +} diff --git a/components/powersensor/register/register.go b/components/powersensor/register/register.go new file mode 100644 index 00000000000..89abb2b1f37 --- /dev/null +++ b/components/powersensor/register/register.go @@ -0,0 +1,9 @@ +// Package register registers all relevant motors +package register + +import ( + // register all powersensors. + _ "go.viam.com/rdk/components/powersensor/fake" + _ "go.viam.com/rdk/components/powersensor/ina" + _ "go.viam.com/rdk/components/powersensor/renogy" +) diff --git a/components/powersensor/renogy/renogy.go b/components/powersensor/renogy/renogy.go new file mode 100644 index 00000000000..2e96889b785 --- /dev/null +++ b/components/powersensor/renogy/renogy.go @@ -0,0 +1,259 @@ +// Package renogy implements the renogy charge controller sensor for DC batteries. +// Tested with renogy wanderer model +// Wanderer Manual: https://www.renogy.com/content/RNG-CTRL-WND30-LI/WND30-LI-Manual.pdf +// LCD Wanderer Manual: https://ca.renogy.com/content/manual/RNG-CTRL-WND10-Manual.pdf +package renogy + +import ( + "context" + "encoding/binary" + "math" + "sync" + "time" + + "github.com/edaniels/golog" + "github.com/goburrow/modbus" + + "go.viam.com/rdk/components/powersensor" + "go.viam.com/rdk/resource" +) + +var ( + model = resource.DefaultModelFamily.WithModel("renogy") + readings map[string]interface{} +) + +const ( + // defaults assume the device is connected via UART serial. + pathDefault = "/dev/serial0" + baudDefault = 9600 + modbusIDDefault = 1 + + solarVoltReg = 263 + solarAmpReg = 264 + solarWattReg = 265 + loadVoltReg = 260 + loadAmpReg = 261 + loadWattReg = 262 + battVoltReg = 257 + battChargePctReg = 256 + controllerDegCReg = 259 + maxSolarTodayWattReg = 271 + minSolarTodayWattReg = 272 + maxBattTodayVoltReg = 268 + minBattTodayVoltReg = 267 + maxSolarTodayAmpReg = 269 + minSolarTodayAmpReg = 270 + chargeTodayWattHrsReg = 273 + dischargeTodayWattHrsReg = 274 + chargeTodayAmpHrsReg = 275 + dischargeTodayAmpHrsReg = 276 + totalBattOverChargesReg = 278 + totalBattFullChargesReg = 279 + + isAc = false +) + +// Config is used for converting config attributes. +type Config struct { + resource.TriviallyValidateConfig + Path string `json:"serial_path,omitempty"` + Baud int `json:"serial_baud_rate,omitempty"` + ModbusID byte `json:"modbus_id,omitempty"` +} + +func init() { + resource.RegisterComponent( + powersensor.API, + model, + resource.Registration[powersensor.PowerSensor, *Config]{ + Constructor: newRenogy, + }) +} + +func newRenogy(_ context.Context, _ resource.Dependencies, conf resource.Config, logger golog.Logger) (powersensor.PowerSensor, error) { + newConf, err := resource.NativeConfig[*Config](conf) + if err != nil { + return nil, err + } + + if newConf.Path == "" { + newConf.Path = pathDefault + } + if newConf.Baud == 0 { + newConf.Baud = baudDefault + } + if newConf.ModbusID == 0 { + newConf.ModbusID = modbusIDDefault + } + + r := &Renogy{ + Named: conf.ResourceName().AsNamed(), + logger: logger, + path: newConf.Path, + baud: newConf.Baud, + modbusID: newConf.ModbusID, + } + + r.handler = r.getHandler() + + err = r.handler.Connect() + if err != nil { + return nil, err + } + r.client = modbus.NewClient(r.handler) + + return r, nil +} + +// Renogy is a serial charge controller. +type Renogy struct { + resource.Named + resource.AlwaysRebuild + logger golog.Logger + mu sync.Mutex + path string + baud int + modbusID byte + handler *modbus.RTUClientHandler + client modbus.Client +} + +// getHandler is a helper function to create the modbus handler. +func (r *Renogy) getHandler() *modbus.RTUClientHandler { + handler := modbus.NewRTUClientHandler(r.path) + handler.BaudRate = r.baud + handler.DataBits = 8 + handler.Parity = "N" + handler.StopBits = 1 + handler.SlaveId = r.modbusID + handler.Timeout = 1 * time.Second + return handler +} + +// Voltage returns the voltage of the battery and a boolean IsAc. +func (r *Renogy) Voltage(ctx context.Context, extra map[string]interface{}) (float64, bool, error) { + // Read the battery voltage. + volts, err := r.readRegister(r.client, battVoltReg, 1) + if err != nil { + return 0, false, err + } + return float64(volts), isAc, nil +} + +// Current returns the load's current and boolean isAC. +// If the controller does not have a load input, will return zero. +func (r *Renogy) Current(ctx context.Context, extra map[string]interface{}) (float64, bool, error) { + // read the load current. + loadCurrent, err := r.readRegister(r.client, loadAmpReg, 2) + if err != nil { + return 0, false, err + } + + return float64(loadCurrent), isAc, nil +} + +// Power returns the power of the load. If the controller does not have a load input, will return zero. +func (r *Renogy) Power(ctx context.Context, extra map[string]interface{}) (float64, error) { + // reads the load wattage. + loadPower, err := r.readRegister(r.client, loadWattReg, 1) + if err != nil { + return 0, err + } + + return float64(loadPower), err +} + +// Readings returns a list of all readings from the sensor. +func (r *Renogy) Readings(ctx context.Context, extra map[string]interface{}) (map[string]interface{}, error) { + readings = make(map[string]interface{}) + + // add all readings. + r.addReading(solarVoltReg, 1, "SolarVolt") + r.addReading(solarAmpReg, 2, "SolarAmp") + r.addReading(solarWattReg, 0, "SolarWatt") + r.addReading(loadVoltReg, 1, "LoadVolt") + r.addReading(loadAmpReg, 2, "LoadAmp") + r.addReading(loadWattReg, 0, "LoadWatt") + r.addReading(battVoltReg, 1, "BattVolt") + r.addReading(battChargePctReg, 0, "BattChargePct") + r.addReading(maxSolarTodayWattReg, 0, "MaxSolarTodayWatt") + r.addReading(minSolarTodayWattReg, 0, "MinSolarTodayWatt") + r.addReading(maxBattTodayVoltReg, 1, "MaxBattTodayVolt") + r.addReading(minBattTodayVoltReg, 1, "MinBattTodayVolt") + r.addReading(maxSolarTodayAmpReg, 2, "MaxSolarTodayAmp") + r.addReading(minSolarTodayAmpReg, 1, "MinSolarTodayAmp") + r.addReading(chargeTodayAmpHrsReg, 0, "ChargeTodayAmpHrs") + r.addReading(dischargeTodayAmpHrsReg, 0, "DischargeTodayAmpHrs") + r.addReading(chargeTodayWattHrsReg, 0, "ChargeTodayWattHrs") + r.addReading(dischargeTodayWattHrsReg, 0, "DischargeTodayWattHrs") + r.addReading(totalBattOverChargesReg, 0, "TotalBattOverCharges") + r.addReading(totalBattFullChargesReg, 0, "TotalBattFullCharges") + + // Controller and battery temperates require math on controller deg register. + tempReading, err := r.readRegister(r.client, controllerDegCReg, 0) + if err != nil { + return readings, err + } + + battTempSign := (int16(tempReading) & 0b0000000010000000) >> 7 + battTemp := int16(tempReading) & 0b0000000001111111 + if battTempSign == 1 { + battTemp = -battTemp + } + + readings["BattDegC"] = int32(battTemp) + + ctlTempSign := (int32(tempReading) & 0b1000000000000000) >> 15 + ctlTemp := (int16(tempReading) & 0b0111111100000000) >> 8 + if ctlTempSign == 1 { + ctlTemp = -ctlTemp + } + readings["ControllerDegC"] = int32(ctlTemp) + + return readings, nil +} + +func (r *Renogy) addReading(register uint16, precision uint, reading string) { + value, err := r.readRegister(r.client, register, precision) + if err != nil { + r.logger.Errorf("error getting reading: %s : %v", reading, err) + } else { + readings[reading] = value + } +} + +func (r *Renogy) readRegister(client modbus.Client, register uint16, precision uint) (result float32, err error) { + r.mu.Lock() + b, err := client.ReadHoldingRegisters(register, 1) + r.mu.Unlock() + if err != nil { + return 0, err + } + if len(b) > 0 { + result = float32FromBytes(b, precision) + } else { + result = 0 + } + return result, nil +} + +func float32FromBytes(bytes []byte, precision uint) float32 { + i := binary.BigEndian.Uint16(bytes) + ratio := math.Pow(10, float64(precision)) + return float32(float64(i) / ratio) +} + +// Close closes the renogy modbus. +func (r *Renogy) Close(ctx context.Context) error { + r.mu.Lock() + if r.handler != nil { + err := r.handler.Close() + if err != nil { + r.mu.Unlock() + return err + } + } + r.mu.Unlock() + return nil +} diff --git a/components/powersensor/server.go b/components/powersensor/server.go new file mode 100644 index 00000000000..ca89c3a2df0 --- /dev/null +++ b/components/powersensor/server.go @@ -0,0 +1,85 @@ +package powersensor + +import ( + "context" + + commonpb "go.viam.com/api/common/v1" + pb "go.viam.com/api/component/powersensor/v1" + + "go.viam.com/rdk/protoutils" + "go.viam.com/rdk/resource" +) + +type serviceServer struct { + pb.UnimplementedPowerSensorServiceServer + coll resource.APIResourceCollection[PowerSensor] +} + +// NewRPCServiceServer constructs a PowerSesnsor gRPC service serviceServer. +func NewRPCServiceServer(coll resource.APIResourceCollection[PowerSensor]) interface{} { + return &serviceServer{coll: coll} +} + +func (s *serviceServer) GetVoltage( + ctx context.Context, + req *pb.GetVoltageRequest, +) (*pb.GetVoltageResponse, error) { + psDevice, err := s.coll.Resource(req.Name) + if err != nil { + return nil, err + } + voltage, isAc, err := psDevice.Voltage(ctx, req.Extra.AsMap()) + if err != nil { + return nil, err + } + return &pb.GetVoltageResponse{ + Volts: voltage, + IsAc: isAc, + }, nil +} + +func (s *serviceServer) GetCurrent( + ctx context.Context, + req *pb.GetCurrentRequest, +) (*pb.GetCurrentResponse, error) { + psDevice, err := s.coll.Resource(req.Name) + if err != nil { + return nil, err + } + current, isAc, err := psDevice.Current(ctx, req.Extra.AsMap()) + if err != nil { + return nil, err + } + return &pb.GetCurrentResponse{ + Amperes: current, + IsAc: isAc, + }, nil +} + +func (s *serviceServer) GetPower( + ctx context.Context, + req *pb.GetPowerRequest, +) (*pb.GetPowerResponse, error) { + psDevice, err := s.coll.Resource(req.Name) + if err != nil { + return nil, err + } + power, err := psDevice.Power(ctx, req.Extra.AsMap()) + if err != nil { + return nil, err + } + return &pb.GetPowerResponse{ + Watts: power, + }, nil +} + +// DoCommand receives arbitrary commands. +func (s *serviceServer) DoCommand(ctx context.Context, + req *commonpb.DoCommandRequest, +) (*commonpb.DoCommandResponse, error) { + psDevice, err := s.coll.Resource(req.GetName()) + if err != nil { + return nil, err + } + return protoutils.DoFromResourceServer(ctx, psDevice, req) +} diff --git a/components/powersensor/server_test.go b/components/powersensor/server_test.go new file mode 100644 index 00000000000..cffab33fdf3 --- /dev/null +++ b/components/powersensor/server_test.go @@ -0,0 +1,142 @@ +package powersensor_test + +import ( + "context" + "errors" + "testing" + + pb "go.viam.com/api/component/powersensor/v1" + "go.viam.com/test" + + "go.viam.com/rdk/components/powersensor" + "go.viam.com/rdk/components/sensor" + "go.viam.com/rdk/resource" + "go.viam.com/rdk/testutils/inject" +) + +var ( + workingPowerSensorName = "workingPS" + failingPowerSensorName = "failingPS" + missingPowerSensorName = "missingPS" + errVoltageFailed = errors.New("can't get voltage") + errCurrentFailed = errors.New("can't get current") + errPowerFailed = errors.New("can't get power") + errPowerSensorNotFound = errors.New("not found") +) + +func newServer() (pb.PowerSensorServiceServer, *inject.PowerSensor, *inject.PowerSensor, error) { + workingPowerSensor := &inject.PowerSensor{} + failingPowerSensor := &inject.PowerSensor{} + powerSensors := map[resource.Name]powersensor.PowerSensor{ + powersensor.Named(workingPowerSensorName): workingPowerSensor, + powersensor.Named(failingPowerSensorName): failingPowerSensor, + } + + powerSensorSvc, err := resource.NewAPIResourceCollection(sensor.API, powerSensors) + if err != nil { + return nil, nil, nil, err + } + + server := powersensor.NewRPCServiceServer(powerSensorSvc).(pb.PowerSensorServiceServer) + + return server, workingPowerSensor, failingPowerSensor, nil +} + +//nolint:dupl +func TestServerGetVoltage(t *testing.T) { + powerSensorServer, testPowerSensor, failingPowerSensor, err := newServer() + test.That(t, err, test.ShouldBeNil) + volts := 4.8 + isAC := false + + // successful + testPowerSensor.VoltageFunc = func(ctx context.Context, extra map[string]interface{}) (float64, bool, error) { + return volts, isAC, nil + } + req := &pb.GetVoltageRequest{Name: workingPowerSensorName} + resp, err := powerSensorServer.GetVoltage(context.Background(), req) + test.That(t, err, test.ShouldBeNil) + test.That(t, resp.Volts, test.ShouldEqual, volts) + test.That(t, resp.IsAc, test.ShouldEqual, isAC) + + // fails on bad power sensor + failingPowerSensor.VoltageFunc = func(ctx context.Context, extra map[string]interface{}) (float64, bool, error) { + return 0, false, errVoltageFailed + } + req = &pb.GetVoltageRequest{Name: failingPowerSensorName} + resp, err = powerSensorServer.GetVoltage(context.Background(), req) + test.That(t, err, test.ShouldBeError, errVoltageFailed) + test.That(t, resp, test.ShouldBeNil) + + // missing power sensor + req = &pb.GetVoltageRequest{Name: missingPowerSensorName} + resp, err = powerSensorServer.GetVoltage(context.Background(), req) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, errPowerSensorNotFound.Error()) + test.That(t, resp, test.ShouldBeNil) +} + +//nolint:dupl +func TestServerGetCurrent(t *testing.T) { + powerSensorServer, testPowerSensor, failingPowerSensor, err := newServer() + test.That(t, err, test.ShouldBeNil) + amps := 4.8 + isAC := false + + // successful + testPowerSensor.CurrentFunc = func(ctx context.Context, extra map[string]interface{}) (float64, bool, error) { + return amps, isAC, nil + } + req := &pb.GetCurrentRequest{Name: workingPowerSensorName} + resp, err := powerSensorServer.GetCurrent(context.Background(), req) + test.That(t, err, test.ShouldBeNil) + test.That(t, resp.Amperes, test.ShouldEqual, amps) + test.That(t, resp.IsAc, test.ShouldEqual, isAC) + + // fails on bad power sensor + failingPowerSensor.CurrentFunc = func(ctx context.Context, extra map[string]interface{}) (float64, bool, error) { + return 0, false, errCurrentFailed + } + req = &pb.GetCurrentRequest{Name: failingPowerSensorName} + resp, err = powerSensorServer.GetCurrent(context.Background(), req) + test.That(t, err, test.ShouldBeError, errCurrentFailed) + test.That(t, resp, test.ShouldBeNil) + + // missing power sensor + req = &pb.GetCurrentRequest{Name: missingPowerSensorName} + resp, err = powerSensorServer.GetCurrent(context.Background(), req) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, errPowerSensorNotFound.Error()) + test.That(t, resp, test.ShouldBeNil) +} + +func TestServerGetPower(t *testing.T) { + powerSensorServer, testPowerSensor, failingPowerSensor, err := newServer() + test.That(t, err, test.ShouldBeNil) + watts := 4.8 + + // successful + testPowerSensor.PowerFunc = func(ctx context.Context, extra map[string]interface{}) (float64, error) { + return watts, nil + } + req := &pb.GetPowerRequest{Name: workingPowerSensorName} + resp, err := powerSensorServer.GetPower(context.Background(), req) + test.That(t, err, test.ShouldBeNil) + test.That(t, resp.Watts, test.ShouldEqual, watts) + + // fails on bad power sensor + failingPowerSensor.PowerFunc = func(ctx context.Context, extra map[string]interface{}) (float64, error) { + return 0, errPowerFailed + } + req = &pb.GetPowerRequest{Name: failingPowerSensorName} + resp, err = powerSensorServer.GetPower(context.Background(), req) + test.That(t, err, test.ShouldBeError, errPowerFailed) + test.That(t, resp, test.ShouldBeNil) + + // missing power sensor + req = &pb.GetPowerRequest{Name: missingPowerSensorName} + resp, err = powerSensorServer.GetPower(context.Background(), req) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, errPowerSensorNotFound.Error()) + test.That(t, resp, test.ShouldBeNil) +} diff --git a/components/register/all.go b/components/register/all.go index 6f03c3c8efe..60fc57f21a7 100644 --- a/components/register/all.go +++ b/components/register/all.go @@ -17,6 +17,7 @@ import ( _ "go.viam.com/rdk/components/movementsensor/register" // register APIs without implementations directly. _ "go.viam.com/rdk/components/posetracker" + _ "go.viam.com/rdk/components/powersensor/register" _ "go.viam.com/rdk/components/sensor/register" _ "go.viam.com/rdk/components/servo/register" ) diff --git a/components/sensor/bme280/bme280.go b/components/sensor/bme280/bme280.go index ee166addfe7..00ddf91967e 100644 --- a/components/sensor/bme280/bme280.go +++ b/components/sensor/bme280/bme280.go @@ -436,8 +436,8 @@ func (s *bme280) setOverSample(ctx context.Context, addr, offset, val byte) erro if err != nil { return err } - err = s.setMode(ctx, 0b00) - if err != nil { + + if err = s.setMode(ctx, 0b00); err != nil { return err } @@ -453,16 +453,19 @@ func (s *bme280) setOverSample(ctx context.Context, addr, offset, val byte) erro controlData &= ^((byte(1) << (offset + 2)) | (byte(1) << (offset + 1)) | (byte(1) << offset)) controlData |= (val << offset) - err = handle.WriteByteData(ctx, addr, controlData) - if err != nil { + if err = handle.WriteByteData(ctx, addr, controlData); err != nil { return err } - err = s.setMode(ctx, mode) - if err != nil { + + if err := handle.Close(); err != nil { return err } - return handle.Close() + if err = s.setMode(ctx, mode); err != nil { + return err + } + + return nil } // setupCalibration sets up all calibration data for the chip. diff --git a/components/sensor/charge/renogy.go b/components/sensor/charge/renogy.go deleted file mode 100644 index 10d293e8352..00000000000 --- a/components/sensor/charge/renogy.go +++ /dev/null @@ -1,207 +0,0 @@ -// Package charge implements a charge controller sensor -package charge - -import ( - "context" - "encoding/binary" - "encoding/json" - "math" - "sync" - "time" - - "github.com/edaniels/golog" - "github.com/goburrow/modbus" - - "go.viam.com/rdk/components/sensor" - "go.viam.com/rdk/resource" -) - -var globalMu sync.Mutex - -// defaults assume the device is connected via UART serial. -const ( - pathDefault = "/dev/serial0" - baudDefault = 9600 - modbusIDDefault = 1 -) - -var model = resource.DefaultModelFamily.WithModel("renogy") - -// Config is used for converting config attributes. -type Config struct { - resource.TriviallyValidateConfig - Path string `json:"serial_path"` - Baud int `json:"serial_baud_rate"` - ModbusID byte `json:"modbus_id"` -} - -// Charge represents a charge state. -type Charge struct { - SolarVolt float32 - SolarAmp float32 - SolarWatt float32 - LoadVolt float32 - LoadAmp float32 - LoadWatt float32 - BattVolt float32 - BattChargePct float32 - BattDegC int16 - ControllerDegC int16 - MaxSolarTodayWatt float32 - MinSolarTodayWatt float32 - MaxBattTodayVolt float32 - MinBattTodayVolt float32 - MaxSolarTodayAmp float32 - MinSolarTodayAmp float32 - ChargeTodayWattHrs float32 - DischargeTodayWattHrs float32 - ChargeTodayAmpHrs float32 - DischargeTodayAmpHrs float32 - TotalBattOverCharges float32 - TotalBattFullCharges float32 -} - -func init() { - resource.RegisterComponent( - sensor.API, - model, - resource.Registration[sensor.Sensor, *Config]{ - Constructor: func( - ctx context.Context, - deps resource.Dependencies, - conf resource.Config, - logger golog.Logger, - ) (sensor.Sensor, error) { - newConf, err := resource.NativeConfig[*Config](conf) - if err != nil { - return nil, err - } - return newSensor(conf.ResourceName(), newConf.Path, - newConf.Baud, newConf.ModbusID), nil - }, - }) -} - -func newSensor(name resource.Name, path string, baud int, modbusID byte) sensor.Sensor { - if path == "" { - path = pathDefault - } - if baud == 0 { - baud = baudDefault - } - if modbusID == 0 { - modbusID = modbusIDDefault - } - - return &Sensor{ - Named: name.AsNamed(), - path: path, - baud: baud, - modbusID: modbusID, - } -} - -// Sensor is a serial charge controller. -type Sensor struct { - resource.Named - resource.AlwaysRebuild - resource.TriviallyCloseable - path string - baud int - modbusID byte -} - -// Readings returns a list containing single item (current temperature). -func (s *Sensor) Readings(ctx context.Context, extra map[string]interface{}) (map[string]interface{}, error) { - readings, err := s.GetControllerOutput(ctx) - if err != nil { - return nil, err - } - m := make(map[string]interface{}) - j, err := json.Marshal(readings) - if err != nil { - return nil, err - } - err = json.Unmarshal(j, &m) - if err != nil { - return nil, err - } - return m, nil -} - -// GetControllerOutput returns current readings from the charge controller. -func (s *Sensor) GetControllerOutput(ctx context.Context) (Charge, error) { - var chargeRes Charge - handler := modbus.NewRTUClientHandler(s.path) - handler.BaudRate = s.baud - handler.DataBits = 8 - handler.Parity = "N" - handler.StopBits = 1 - handler.SlaveId = s.modbusID - handler.Timeout = 1 * time.Second - - err := handler.Connect() - if err != nil { - err = handler.Close() - return chargeRes, err - } - client := modbus.NewClient(handler) - chargeRes.SolarVolt = readRegister(client, 263, 1) - chargeRes.SolarAmp = readRegister(client, 264, 2) - chargeRes.SolarWatt = readRegister(client, 265, 0) - chargeRes.LoadVolt = readRegister(client, 260, 1) - chargeRes.LoadAmp = readRegister(client, 261, 2) - chargeRes.LoadWatt = readRegister(client, 262, 0) - chargeRes.BattVolt = readRegister(client, 257, 1) - chargeRes.BattChargePct = readRegister(client, 256, 0) - tempReading := readRegister(client, 259, 0) - battTempSign := (int16(tempReading) & 0b0000000010000000) >> 7 - battTemp := int16(tempReading) & 0b0000000001111111 - if battTempSign == 1 { - battTemp = -battTemp - } - chargeRes.BattDegC = battTemp - ctlTempSign := (int32(tempReading) & 0b1000000000000000) >> 15 - ctlTemp := (int16(tempReading) & 0b0111111100000000) >> 8 - if ctlTempSign == 1 { - ctlTemp = -ctlTemp - } - chargeRes.ControllerDegC = ctlTemp - chargeRes.MaxSolarTodayWatt = readRegister(client, 271, 0) - chargeRes.MinSolarTodayWatt = readRegister(client, 272, 0) - chargeRes.MaxBattTodayVolt = readRegister(client, 268, 1) - chargeRes.MinBattTodayVolt = readRegister(client, 267, 1) - chargeRes.MaxSolarTodayAmp = readRegister(client, 269, 2) - chargeRes.MinSolarTodayAmp = readRegister(client, 270, 1) - chargeRes.ChargeTodayAmpHrs = readRegister(client, 273, 0) - chargeRes.DischargeTodayAmpHrs = readRegister(client, 274, 0) - chargeRes.ChargeTodayWattHrs = readRegister(client, 275, 0) - chargeRes.DischargeTodayWattHrs = readRegister(client, 276, 0) - chargeRes.TotalBattOverCharges = readRegister(client, 278, 0) - chargeRes.TotalBattFullCharges = readRegister(client, 279, 0) - - err = handler.Close() - return chargeRes, err -} - -func readRegister(client modbus.Client, register uint16, precision uint) (result float32) { - globalMu.Lock() - b, err := client.ReadHoldingRegisters(register, 1) - globalMu.Unlock() - if err != nil { - result = 0 - } else { - if len(b) > 0 { - result = float32FromBytes(b, precision) - } else { - result = 0 - } - } - return result -} - -func float32FromBytes(bytes []byte, precision uint) float32 { - i := binary.BigEndian.Uint16(bytes) - ratio := math.Pow(10, float64(precision)) - return float32(float64(i) / ratio) -} diff --git a/components/sensor/client_test.go b/components/sensor/client_test.go index 6da08f9dc8f..856902a8f04 100644 --- a/components/sensor/client_test.go +++ b/components/sensor/client_test.go @@ -2,7 +2,6 @@ package sensor_test import ( "context" - "errors" "net" "testing" @@ -42,7 +41,7 @@ func TestClient(t *testing.T) { injectSensor2 := &inject.Sensor{} injectSensor2.ReadingsFunc = func(ctx context.Context, extra map[string]interface{}) (map[string]interface{}, error) { - return nil, errors.New("can't get readings") + return nil, errReadingsFailed } sensorSvc, err := resource.NewAPIResourceCollection( @@ -66,7 +65,7 @@ func TestClient(t *testing.T) { cancel() _, err := viamgrpc.Dial(cancelCtx, listener1.Addr().String(), logger) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "canceled") + test.That(t, err, test.ShouldBeError, context.Canceled) }) t.Run("Sensor client 1", func(t *testing.T) { @@ -105,7 +104,7 @@ func TestClient(t *testing.T) { _, err = client2.Readings(context.Background(), make(map[string]interface{})) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "can't get readings") + test.That(t, err.Error(), test.ShouldContainSubstring, errReadingsFailed.Error()) test.That(t, client2.Close(context.Background()), test.ShouldBeNil) test.That(t, conn.Close(), test.ShouldBeNil) diff --git a/components/sensor/collector.go b/components/sensor/collector.go index 5947829a2b4..f02f2d966b7 100644 --- a/components/sensor/collector.go +++ b/components/sensor/collector.go @@ -2,6 +2,7 @@ package sensor import ( "context" + "errors" "google.golang.org/protobuf/types/known/anypb" @@ -40,8 +41,13 @@ func newSensorCollector(resource interface{}, params data.CollectorParams) (data cFunc := data.CaptureFunc(func(ctx context.Context, arg map[string]*anypb.Any) (interface{}, error) { var records []ReadingRecord - values, err := sensorResource.Readings(ctx, nil) // TODO (RSDK-1972): pass in something here from the config rather than nil? + values, err := sensorResource.Readings(ctx, data.FromDMExtraMap) // TODO (RSDK-1972): pass in something here from the config? if err != nil { + // A modular filter component can be created to filter the readings from a component. The error ErrNoCaptureToStore + // is used in the datamanager to exclude readings from being captured and stored. + if errors.Is(err, data.ErrNoCaptureToStore) { + return nil, err + } return nil, data.FailedToReadErr(params.ComponentName, readings.String(), err) } for name, value := range values { diff --git a/components/sensor/ds18b20/ds18b20.go b/components/sensor/ds18b20/ds18b20.go index 63ab05ecf37..51432ed12c9 100644 --- a/components/sensor/ds18b20/ds18b20.go +++ b/components/sensor/ds18b20/ds18b20.go @@ -40,15 +40,16 @@ func init() { if err != nil { return nil, err } - return newSensor(conf.ResourceName(), newConf.UniqueID), nil + return newSensor(conf.ResourceName(), newConf.UniqueID, logger), nil }, }) } -func newSensor(name resource.Name, id string) sensor.Sensor { +func newSensor(name resource.Name, id string, logger golog.Logger) sensor.Sensor { // temp sensors are in family 28 return &Sensor{ Named: name.AsNamed(), + logger: logger, OneWireID: id, OneWireFamily: "28", } @@ -61,6 +62,7 @@ type Sensor struct { resource.TriviallyCloseable OneWireID string OneWireFamily string + logger golog.Logger } // ReadTemperatureCelsius returns current temperature in celsius. diff --git a/components/sensor/fake/sensor.go b/components/sensor/fake/sensor.go index 1b5e8cf4269..b01ea877d73 100644 --- a/components/sensor/fake/sensor.go +++ b/components/sensor/fake/sensor.go @@ -21,13 +21,14 @@ func init() { conf resource.Config, logger golog.Logger, ) (sensor.Sensor, error) { - return newSensor(conf.ResourceName()), nil + return newSensor(conf.ResourceName(), logger), nil }}) } -func newSensor(name resource.Name) sensor.Sensor { +func newSensor(name resource.Name, logger golog.Logger) sensor.Sensor { return &Sensor{ - Named: name.AsNamed(), + Named: name.AsNamed(), + logger: logger, } } @@ -37,6 +38,7 @@ type Sensor struct { resource.Named resource.TriviallyReconfigurable resource.TriviallyCloseable + logger golog.Logger } // Readings always returns the set values. diff --git a/components/sensor/power_ina219/power_ina219.go b/components/sensor/power_ina219/power_ina219.go deleted file mode 100644 index a07c789dcb8..00000000000 --- a/components/sensor/power_ina219/power_ina219.go +++ /dev/null @@ -1,225 +0,0 @@ -// Package ina219 implements an ina219 voltage/current/power monitor sensor - -// typically used for battery state monitoring. -// Datasheet can be found at: https://www.ti.com/lit/ds/symlink/ina219.pdf -// Example repo: https://github.com/periph/devices/blob/main/ina219/ina219.go -package ina219 - -import ( - "context" - "encoding/binary" - "fmt" - - "github.com/edaniels/golog" - "go.viam.com/utils" - - "go.viam.com/rdk/components/board" - "go.viam.com/rdk/components/sensor" - "go.viam.com/rdk/resource" -) - -var model = resource.DefaultModelFamily.WithModel("power_ina219") - -const ( - milliAmp = 1000 * 1000 // milliAmp = 1000 microAmpere * 1000 nanoAmpere - milliOhm = 1000 * 1000 // milliOhm = 1000 microOhm * 1000 nanoOhm - defaultI2Caddr = 0x40 - senseResistor int64 = 100 * milliOhm // .1 ohm - maxCurrent int64 = 3200 * milliAmp // 3.2 amp - calibratescale = ((int64(1000*milliAmp) * int64(1000*milliOhm)) / 100000) << 12 - configRegister = 0x00 - shuntVoltageRegister = 0x01 - busVoltageRegister = 0x02 - powerRegister = 0x03 - currentRegister = 0x04 - calibrationRegister = 0x05 -) - -// Config is used for converting config attributes. -type Config struct { - Board string `json:"board"` - I2CBus string `json:"i2c_bus"` - I2cAddr int `json:"i2c_addr,omitempty"` -} - -// Validate ensures all parts of the config are valid. -func (conf *Config) Validate(path string) ([]string, error) { - var deps []string - if len(conf.Board) == 0 { - return nil, utils.NewConfigValidationFieldRequiredError(path, "board") - } - deps = append(deps, conf.Board) - if len(conf.I2CBus) == 0 { - return nil, utils.NewConfigValidationFieldRequiredError(path, "i2c_bus") - } - return deps, nil -} - -func init() { - resource.RegisterComponent( - sensor.API, - model, - resource.Registration[sensor.Sensor, *Config]{ - Constructor: func( - ctx context.Context, - deps resource.Dependencies, - conf resource.Config, - logger golog.Logger, - ) (sensor.Sensor, error) { - newConf, err := resource.NativeConfig[*Config](conf) - if err != nil { - return nil, err - } - return newSensor(deps, conf.ResourceName(), newConf, logger) - }, - }) -} - -func newSensor( - deps resource.Dependencies, - name resource.Name, - conf *Config, - logger golog.Logger, -) (sensor.Sensor, error) { - b, err := board.FromDependencies(deps, conf.Board) - if err != nil { - return nil, fmt.Errorf("ina219 init: failed to find board: %w", err) - } - localB, ok := b.(board.LocalBoard) - if !ok { - return nil, fmt.Errorf("board %s is not local", conf.Board) - } - i2cbus, ok := localB.I2CByName(conf.I2CBus) - if !ok { - return nil, fmt.Errorf("ina219 init: failed to find i2c bus %s", conf.I2CBus) - } - addr := conf.I2cAddr - if addr == 0 { - addr = defaultI2Caddr - logger.Infof("using i2c address : %d", defaultI2Caddr) - } - - s := &ina219{ - Named: name.AsNamed(), - logger: logger, - bus: i2cbus, - addr: byte(addr), - } - - err = s.calibrate() - if err != nil { - return nil, err - } - - return s, nil -} - -// ina219 is a i2c sensor device that reports voltage, current and power. -type ina219 struct { - resource.Named - resource.AlwaysRebuild - resource.TriviallyCloseable - logger golog.Logger - bus board.I2C - addr byte - currentLSB int64 - powerLSB int64 - cal uint16 -} - -type powerMonitor struct { - Shunt int64 - Voltage float64 - Current float64 - Power float64 -} - -func (d *ina219) calibrate() error { - if senseResistor <= 0 { - return fmt.Errorf("ina219 calibrate: senseResistor value invalid %d", senseResistor) - } - if maxCurrent <= 0 { - return fmt.Errorf("ina219 calibrate: maxCurrent value invalid %d", maxCurrent) - } - - d.currentLSB = maxCurrent / (1 << 15) - d.powerLSB = (maxCurrent*20 + (1 << 14)) / (1 << 15) - // Calibration Register = 0.04096 / (current LSB * Shunt Resistance) - // Where lsb is in Amps and resistance is in ohms. - // Calibration register is 16 bits. - cal := calibratescale / (d.currentLSB * senseResistor) - if cal >= (1 << 16) { - return fmt.Errorf("ina219 calibrate: calibration register value invalid %d", cal) - } - d.cal = uint16(cal) - - return nil -} - -// Readings returns a list containing three items (voltage, current, and power). -func (d *ina219) Readings(ctx context.Context, extra map[string]interface{}) (map[string]interface{}, error) { - handle, err := d.bus.OpenHandle(d.addr) - if err != nil { - d.logger.Errorf("can't open ina219 i2c: %s", err) - return nil, err - } - defer utils.UncheckedErrorFunc(handle.Close) - - // use the calibration result to set the scaling factor - // of the current and power registers for the maximum resolution - buf := make([]byte, 2) - binary.BigEndian.PutUint16(buf, d.cal) - err = handle.WriteBlockData(ctx, calibrationRegister, buf) - if err != nil { - return nil, err - } - - buf = make([]byte, 2) - binary.BigEndian.PutUint16(buf, uint16(0x1FFF)) - err = handle.WriteBlockData(ctx, configRegister, buf) - if err != nil { - return nil, err - } - - var pm powerMonitor - - // get shunt voltage - currently we are not returning - is it useful? - shunt, err := handle.ReadBlockData(ctx, shuntVoltageRegister, 2) - if err != nil { - return nil, err - } - - // Least significant bit is 10µV. - pm.Shunt = int64(binary.BigEndian.Uint16(shunt)) * 10 * 1000 - d.logger.Debugf("ina219 shunt : %d", pm.Shunt) - - bus, err := handle.ReadBlockData(ctx, busVoltageRegister, 2) - if err != nil { - return nil, err - } - - // Check if bit zero is set, if set the ADC has overflowed. - if binary.BigEndian.Uint16(bus)&1 > 0 { - return nil, fmt.Errorf("ina219 bus voltage register overflow, register: %d", busVoltageRegister) - } - - pm.Voltage = float64(binary.BigEndian.Uint16(bus)>>3) * 4 / 1000 - - current, err := handle.ReadBlockData(ctx, currentRegister, 2) - if err != nil { - return nil, err - } - - pm.Current = float64(int64(binary.BigEndian.Uint16(current))*d.currentLSB) / 1000000000 - - power, err := handle.ReadBlockData(ctx, powerRegister, 2) - if err != nil { - return nil, err - } - pm.Power = float64(int64(binary.BigEndian.Uint16(power))*d.powerLSB) / 1000000000 - - return map[string]interface{}{ - "volts": pm.Voltage, - "amps": pm.Current, - "watts": pm.Power, - }, nil -} diff --git a/components/sensor/register/register.go b/components/sensor/register/register.go index c5f35a35211..7d4f5a991e0 100644 --- a/components/sensor/register/register.go +++ b/components/sensor/register/register.go @@ -4,10 +4,8 @@ package register import ( // for Sensors. _ "go.viam.com/rdk/components/sensor/bme280" - _ "go.viam.com/rdk/components/sensor/charge" _ "go.viam.com/rdk/components/sensor/ds18b20" _ "go.viam.com/rdk/components/sensor/fake" - _ "go.viam.com/rdk/components/sensor/power_ina219" _ "go.viam.com/rdk/components/sensor/sht3xd" _ "go.viam.com/rdk/components/sensor/ultrasonic" ) diff --git a/components/sensor/server_test.go b/components/sensor/server_test.go index 654187aea7e..11e5ec961a3 100644 --- a/components/sensor/server_test.go +++ b/components/sensor/server_test.go @@ -15,6 +15,8 @@ import ( "go.viam.com/rdk/testutils/inject" ) +var errReadingsFailed = errors.New("can't get readings") + func newServer() (pb.SensorServiceServer, *inject.Sensor, *inject.Sensor, error) { injectSensor := &inject.Sensor{} injectSensor2 := &inject.Sensor{} @@ -42,7 +44,7 @@ func TestServer(t *testing.T) { } injectSensor2.ReadingsFunc = func(ctx context.Context, extra map[string]interface{}) (map[string]interface{}, error) { - return nil, errors.New("can't get readings") + return nil, errReadingsFailed } t.Run("GetReadings", func(t *testing.T) { @@ -62,7 +64,7 @@ func TestServer(t *testing.T) { _, err = sensorServer.GetReadings(context.Background(), &pb.GetReadingsRequest{Name: failSensorName}) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "can't get readings") + test.That(t, err.Error(), test.ShouldContainSubstring, errReadingsFailed.Error()) _, err = sensorServer.GetReadings(context.Background(), &pb.GetReadingsRequest{Name: missingSensorName}) test.That(t, err, test.ShouldNotBeNil) diff --git a/components/sensor/sht3xd/sht3xd.go b/components/sensor/sht3xd/sht3xd.go index c9cfcbf8e6b..3d3008d2c63 100644 --- a/components/sensor/sht3xd/sht3xd.go +++ b/components/sensor/sht3xd/sht3xd.go @@ -1,3 +1,5 @@ +//go:build linux + // Package sht3xd implements a sht3x-d sensor for temperature and humidity // datasheet can be found at: https://cdn-shop.adafruit.com/product-files/2857/Sensirion_Humidity_SHT3x_Datasheet_digital-767294.pdf // example repo: https://github.com/esphome/esphome/tree/dev/esphome/components/sht3xd @@ -14,6 +16,7 @@ import ( "go.viam.com/utils" "go.viam.com/rdk/components/board" + "go.viam.com/rdk/components/board/genericlinux" "go.viam.com/rdk/components/sensor" "go.viam.com/rdk/resource" ) @@ -31,7 +34,7 @@ const ( // Config is used for converting config attributes. type Config struct { - Board string `json:"board"` + Board string `json:"board,omitempty"` I2CBus string `json:"i2c_bus"` I2cAddr int `json:"i2c_addr,omitempty"` } @@ -39,10 +42,9 @@ type Config struct { // Validate ensures all parts of the config are valid. func (conf *Config) Validate(path string) ([]string, error) { var deps []string - if len(conf.Board) == 0 { - return nil, utils.NewConfigValidationFieldRequiredError(path, "board") + if len(conf.Board) != 0 { + deps = append(deps, conf.Board) } - deps = append(deps, conf.Board) if len(conf.I2CBus) == 0 { return nil, utils.NewConfigValidationFieldRequiredError(path, "i2c_bus") } @@ -76,17 +78,26 @@ func newSensor( conf *Config, logger golog.Logger, ) (sensor.Sensor, error) { - b, err := board.FromDependencies(deps, conf.Board) - if err != nil { - return nil, fmt.Errorf("sht3xd init: failed to find board: %w", err) - } - localB, ok := b.(board.LocalBoard) - if !ok { - return nil, fmt.Errorf("board %s is not local", conf.Board) - } - i2cbus, ok := localB.I2CByName(conf.I2CBus) - if !ok { - return nil, fmt.Errorf("sht3xd init: failed to find i2c bus %s", conf.I2CBus) + var i2cbus board.I2C + var err error + if conf.Board != "" { + b, err := board.FromDependencies(deps, conf.Board) + if err != nil { + return nil, fmt.Errorf("sht3xd init: failed to find board: %w", err) + } + localB, ok := b.(board.LocalBoard) + if !ok { + return nil, fmt.Errorf("board %s is not local", conf.Board) + } + i2cbus, ok = localB.I2CByName(conf.I2CBus) + if !ok { + return nil, fmt.Errorf("sht3xd init: failed to find i2c bus %s", conf.I2CBus) + } + } else { + i2cbus, err = genericlinux.NewI2cBus(conf.I2CBus) + if err != nil { + return nil, fmt.Errorf("sht3xd init: failed to find i2c bus %s", conf.I2CBus) + } } addr := conf.I2cAddr if addr == 0 { diff --git a/components/sensor/sht3xd/sht3xd_nonlinux.go b/components/sensor/sht3xd/sht3xd_nonlinux.go new file mode 100644 index 00000000000..2dafdfab2e9 --- /dev/null +++ b/components/sensor/sht3xd/sht3xd_nonlinux.go @@ -0,0 +1,2 @@ +// Package sht3xd This is blank for non linux +package sht3xd diff --git a/components/sensor/ultrasonic/ultrasonic.go b/components/sensor/ultrasonic/ultrasonic.go index 6d1407da1a3..93ff458edcb 100644 --- a/components/sensor/ultrasonic/ultrasonic.go +++ b/components/sensor/ultrasonic/ultrasonic.go @@ -57,15 +57,18 @@ func init() { if err != nil { return nil, err } - return NewSensor(ctx, deps, conf.ResourceName(), newConf) + return NewSensor(ctx, deps, conf.ResourceName(), newConf, logger) }, }) } // NewSensor creates and configures a new ultrasonic sensor. -func NewSensor(ctx context.Context, deps resource.Dependencies, name resource.Name, config *Config) (sensor.Sensor, error) { +func NewSensor(ctx context.Context, deps resource.Dependencies, + name resource.Name, config *Config, logger golog.Logger, +) (sensor.Sensor, error) { s := &Sensor{ Named: name.AsNamed(), + logger: logger, config: config, } cancelCtx, cancelFunc := context.WithCancel(context.Background()) @@ -115,6 +118,7 @@ type Sensor struct { timeoutMs uint cancelCtx context.Context cancelFunc func() + logger golog.Logger } func (s *Sensor) namedError(err error) error { diff --git a/components/sensor/ultrasonic/ultrasonic_test.go b/components/sensor/ultrasonic/ultrasonic_test.go index a51787c15dc..49f58dfb062 100644 --- a/components/sensor/ultrasonic/ultrasonic_test.go +++ b/components/sensor/ultrasonic/ultrasonic_test.go @@ -4,6 +4,7 @@ import ( "context" "testing" + "github.com/edaniels/golog" "go.viam.com/test" "go.viam.com/rdk/components/board" @@ -66,7 +67,8 @@ func TestNewSensor(t *testing.T) { fakecfg := &Config{TriggerPin: triggerPin, EchoInterrupt: echoInterrupt, Board: board1} ctx := context.Background() deps := setupDependencies(t) + logger := golog.NewTestLogger(t) - _, err := NewSensor(ctx, deps, sensor.Named(testSensorName), fakecfg) + _, err := NewSensor(ctx, deps, sensor.Named(testSensorName), fakecfg, logger) test.That(t, err, test.ShouldBeNil) } diff --git a/components/servo/client_test.go b/components/servo/client_test.go index 36c81e19abf..6fed3093dc2 100644 --- a/components/servo/client_test.go +++ b/components/servo/client_test.go @@ -6,7 +6,6 @@ import ( "testing" "github.com/edaniels/golog" - "github.com/pkg/errors" "go.viam.com/test" "go.viam.com/utils/rpc" @@ -49,13 +48,13 @@ func TestClient(t *testing.T) { } failingServo.MoveFunc = func(ctx context.Context, angle uint32, extra map[string]interface{}) error { - return errors.New("move failed") + return errMoveFailed } failingServo.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (uint32, error) { - return 0, errors.New("current angle not readable") + return 0, errPositionUnreadable } failingServo.StopFunc = func(ctx context.Context, extra map[string]interface{}) error { - return errors.New("no stop") + return errStopFailed } resourceMap := map[resource.Name]servo.Servo{ @@ -79,7 +78,7 @@ func TestClient(t *testing.T) { cancel() _, err := viamgrpc.Dial(cancelCtx, listener1.Addr().String(), logger) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "canceled") + test.That(t, err, test.ShouldBeError, context.Canceled) }) t.Run("client tests for working servo", func(t *testing.T) { @@ -119,13 +118,15 @@ func TestClient(t *testing.T) { err = failingServoClient.Move(context.Background(), 20, nil) test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, errMoveFailed.Error()) _, err = failingServoClient.Position(context.Background(), nil) test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, errPositionUnreadable.Error()) err = failingServoClient.Stop(context.Background(), nil) test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "no stop") + test.That(t, err.Error(), test.ShouldContainSubstring, errStopFailed.Error()) test.That(t, failingServoClient.Close(context.Background()), test.ShouldBeNil) test.That(t, conn.Close(), test.ShouldBeNil) diff --git a/components/servo/collectors.go b/components/servo/collectors.go index 187f4a57756..e95e3708cf6 100644 --- a/components/servo/collectors.go +++ b/components/servo/collectors.go @@ -2,6 +2,7 @@ package servo import ( "context" + "errors" "google.golang.org/protobuf/types/known/anypb" @@ -33,8 +34,13 @@ func newPositionCollector(resource interface{}, params data.CollectorParams) (da } cFunc := data.CaptureFunc(func(ctx context.Context, _ map[string]*anypb.Any) (interface{}, error) { - v, err := servo.Position(ctx, nil) + v, err := servo.Position(ctx, data.FromDMExtraMap) if err != nil { + // A modular filter component can be created to filter the readings from a component. The error ErrNoCaptureToStore + // is used in the datamanager to exclude readings from being captured and stored. + if errors.Is(err, data.ErrNoCaptureToStore) { + return nil, err + } return nil, data.FailedToReadErr(params.ComponentName, position.String(), err) } return Position{Position: v}, nil diff --git a/components/servo/fake/servo.go b/components/servo/fake/servo.go index 97312a257dd..a7658ded45a 100644 --- a/components/servo/fake/servo.go +++ b/components/servo/fake/servo.go @@ -17,7 +17,8 @@ func init() { resource.Registration[servo.Servo, resource.NoNativeConfig]{ Constructor: func(ctx context.Context, _ resource.Dependencies, conf resource.Config, logger golog.Logger) (servo.Servo, error) { return &Servo{ - Named: conf.ResourceName().AsNamed(), + Named: conf.ResourceName().AsNamed(), + logger: logger, }, nil }, }) @@ -29,6 +30,7 @@ type Servo struct { resource.Named resource.TriviallyReconfigurable resource.TriviallyCloseable + logger golog.Logger } // Move sets the given angle. diff --git a/components/servo/gpio/servo.go b/components/servo/gpio/servo.go index 9f9135893c9..82841d705fd 100644 --- a/components/servo/gpio/servo.go +++ b/components/servo/gpio/servo.go @@ -103,7 +103,7 @@ type servoGPIO struct { minDeg float64 maxDeg float64 logger golog.Logger - opMgr operation.SingleOperationManager + opMgr *operation.SingleOperationManager frequency uint minUs uint maxUs uint @@ -188,6 +188,7 @@ func newGPIOServo(ctx context.Context, deps resource.Dependencies, conf resource frequency: frequency, pin: pin, logger: logger, + opMgr: operation.NewSingleOperationManager(), minUs: minUs, maxUs: maxUs, currPct: 0, diff --git a/components/servo/server_test.go b/components/servo/server_test.go index 9ea3c809917..83813c7ec01 100644 --- a/components/servo/server_test.go +++ b/components/servo/server_test.go @@ -14,6 +14,12 @@ import ( "go.viam.com/rdk/testutils/inject" ) +var ( + errMoveFailed = errors.New("move failed") + errPositionUnreadable = errors.New("current angle not readable") + errStopFailed = errors.New("stop failed") +) + func newServer() (pb.ServoServiceServer, *inject.Servo, *inject.Servo, error) { injectServo := &inject.Servo{} injectServo2 := &inject.Servo{} @@ -39,7 +45,7 @@ func TestServoMove(t *testing.T) { return nil } failingServo.MoveFunc = func(ctx context.Context, angle uint32, extra map[string]interface{}) error { - return errors.New("move failed") + return errMoveFailed } extra := map[string]interface{}{"foo": "Move"} @@ -56,6 +62,7 @@ func TestServoMove(t *testing.T) { resp, err = servoServer.Move(context.Background(), &req) test.That(t, resp, test.ShouldNotBeNil) test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, errMoveFailed.Error()) req = pb.MoveRequest{Name: fakeServoName} resp, err = servoServer.Move(context.Background(), &req) @@ -73,7 +80,7 @@ func TestServoGetPosition(t *testing.T) { return 20, nil } failingServo.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (uint32, error) { - return 0, errors.New("current angle not readable") + return 0, errPositionUnreadable } extra := map[string]interface{}{"foo": "Move"} @@ -90,6 +97,7 @@ func TestServoGetPosition(t *testing.T) { resp, err = servoServer.GetPosition(context.Background(), &req) test.That(t, resp, test.ShouldBeNil) test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, errPositionUnreadable.Error()) req = pb.GetPositionRequest{Name: fakeServoName} resp, err = servoServer.GetPosition(context.Background(), &req) @@ -107,7 +115,7 @@ func TestServoStop(t *testing.T) { return nil } failingServo.StopFunc = func(ctx context.Context, extra map[string]interface{}) error { - return errors.New("no stop") + return errStopFailed } extra := map[string]interface{}{"foo": "Move"} @@ -122,6 +130,7 @@ func TestServoStop(t *testing.T) { req = pb.StopRequest{Name: failServoName} _, err = servoServer.Stop(context.Background(), &req) test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, errStopFailed.Error()) req = pb.StopRequest{Name: fakeServoName} _, err = servoServer.Stop(context.Background(), &req) diff --git a/config/config.go b/config/config.go index 1db40260282..4c6d1cd5c62 100644 --- a/config/config.go +++ b/config/config.go @@ -6,8 +6,10 @@ import ( "encoding/json" "fmt" "net" + "os" + "path/filepath" "reflect" - "regexp" + "strings" "sync" "time" @@ -17,6 +19,7 @@ import ( "go.viam.com/utils/jwks" "go.viam.com/utils/pexec" "go.viam.com/utils/rpc" + "golang.org/x/exp/slices" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" @@ -855,12 +858,27 @@ func ProcessConfig(in *Config, tlsCfg *TLSConfig) (*Config, error) { return &out, nil } -// Regex to match if a config is referencing a Package. Group is the package name. -var packageReferenceRegex = regexp.MustCompile(`^\$\{packages\.([A-Za-z0-9_\/-]+)}(.*)`) - // DefaultPackageVersionValue default value of the package version used when empty. const DefaultPackageVersionValue = "latest" +// PackageType indicates the type of the package +// This is used to replace placeholder strings in the config. +type PackageType string + +const ( + // PackageTypeMlModel represents an ML model. + PackageTypeMlModel PackageType = "ml_model" + // PackageTypeModule represents a module type. + PackageTypeModule PackageType = "module" + // PackageTypeSlamMap represents a slam internal state. + PackageTypeSlamMap PackageType = "slam_map" + // PackageTypeBoardDefs represents a linux board definition file. + PackageTypeBoardDefs PackageType = "board_defs" +) + +// SupportedPackageTypes is a list of all of the valid package types. +var SupportedPackageTypes = []PackageType{PackageTypeMlModel, PackageTypeModule, PackageTypeSlamMap, PackageTypeBoardDefs} + // A PackageConfig describes the configuration of a Package. type PackageConfig struct { // Name is the local name of the package on the RDK. Must be unique across Packages. Must not be empty. @@ -869,10 +887,24 @@ type PackageConfig struct { Package string `json:"package"` // Version of the package ID hosted by a remote PackageService. If not specified "latest" is assumed. Version string `json:"version,omitempty"` + // Types of the Package. If not specified it is assumed to be ml_model. + Type PackageType `json:"type,omitempty"` + + alreadyValidated bool + cachedErr error } // Validate package config is valid. func (p *PackageConfig) Validate(path string) error { + if p.alreadyValidated { + return p.cachedErr + } + p.cachedErr = p.validate(path) + p.alreadyValidated = true + return p.cachedErr +} + +func (p *PackageConfig) validate(path string) error { if p.Name == "" { return utils.NewConfigValidationError(path, errors.New("empty package name")) } @@ -881,36 +913,57 @@ func (p *PackageConfig) Validate(path string) error { return utils.NewConfigValidationError(path, errors.New("empty package id")) } + if p.Type == "" { + // for backwards compatibility + p.Type = PackageTypeMlModel + } + + if !slices.Contains(SupportedPackageTypes, p.Type) { + return utils.NewConfigValidationError(path, errors.Errorf("unsupported package type %q. Must be one of: %v", + p.Type, SupportedPackageTypes)) + } + if !rutils.ValidNameRegex.MatchString(p.Name) { - return rutils.ErrInvalidName(p.Name) + return utils.NewConfigValidationError(path, rutils.ErrInvalidName(p.Name)) } return nil } -// GetPackageReference a PackageReference if the given path has a Package reference eg. ${packages.some-package}/path. -// Returns nil if no package reference is found. -func GetPackageReference(path string) *PackageReference { - // return early before regex match - if len(path) == 0 || path[0] != '$' { - return nil - } +// Equals checks if the two configs are deeply equal to each other. +func (p PackageConfig) Equals(other PackageConfig) bool { + p.alreadyValidated = false + p.cachedErr = nil + other.alreadyValidated = false + other.cachedErr = nil + //nolint:govet + return reflect.DeepEqual(p, other) +} - match := packageReferenceRegex.FindStringSubmatch(path) - if match == nil { - return nil - } +// LocalDataDirectory returns the folder where the package should be extracted. +// Ex: /home/user/.viam/packages/.data/ml_model/orgid_ballClassifier_0.1.2. +func (p *PackageConfig) LocalDataDirectory(packagesDir string) string { + return filepath.Join(p.LocalDataParentDirectory(packagesDir), p.SanitizedName()) +} - if len(match) != 3 { - return nil - } +// LocalDownloadPath returns the file where the archive should be downloaded before extraction. +func (p *PackageConfig) LocalDownloadPath(packagesDir string) string { + return filepath.Join(p.LocalDataParentDirectory(packagesDir), fmt.Sprintf("%s.download", p.SanitizedName())) +} + +// LocalDataParentDirectory returns the folder that will contain the all packages of this type. +// Ex: /home/user/.viam/packages/.data/ml_model. +func (p *PackageConfig) LocalDataParentDirectory(packagesDir string) string { + return filepath.Join(packagesDir, ".data", string(p.Type)) +} - return &PackageReference{Package: match[1], PathInPackage: match[2]} +// SanitizedName returns the package name for the symlink/filepath of the package on the system. +func (p *PackageConfig) SanitizedName() string { + return fmt.Sprintf("%s-%s", strings.ReplaceAll(p.Package, string(os.PathSeparator), "-"), p.sanitizedVersion()) } -// PackageReference contains the deconstructed parts of a package reference in the config. -// Eg: ${packages.some-package}/path/a/b/c -> {"some-package", "/path/a/b/c"}. -type PackageReference struct { - Package string - PathInPackage string +// sanitizedVersion returns a cleaned version of the version so it is file-system-safe. +func (p *PackageConfig) sanitizedVersion() string { + // replaces all the . if they exist with _ + return strings.ReplaceAll(p.Version, ".", "_") } diff --git a/config/config_test.go b/config/config_test.go index defe2e1da3d..2e2e13a1c71 100644 --- a/config/config_test.go +++ b/config/config_test.go @@ -9,6 +9,7 @@ import ( "fmt" "net" "os" + "path/filepath" "testing" "time" @@ -266,10 +267,16 @@ func TestConfigEnsure(t *testing.T) { test.That(t, err, test.ShouldNotBeNil) test.That(t, err.Error(), test.ShouldContainSubstring, `processes.0`) test.That(t, err.Error(), test.ShouldContainSubstring, `"id" is required`) - invalidProcesses.Processes[0].ID = "bar" + invalidProcesses = config.Config{ + DisablePartialStart: true, + Processes: []pexec.ProcessConfig{{ID: "bar"}}, + } err = invalidProcesses.Ensure(false, logger) test.That(t, err.Error(), test.ShouldContainSubstring, `"name" is required`) - invalidProcesses.Processes[0].Name = "foo" + invalidProcesses = config.Config{ + DisablePartialStart: true, + Processes: []pexec.ProcessConfig{{ID: "bar", Name: "foo"}}, + } test.That(t, invalidProcesses.Ensure(false, logger), test.ShouldBeNil) invalidNetwork := config.Config{ @@ -960,30 +967,87 @@ func keysetToAttributeMap(t *testing.T, keyset jwks.KeySet) rutils.AttributeMap return jwksAsInterface } -func TestGetPackageReference(t *testing.T) { - t.Run("non reference", func(t *testing.T) { - test.That(t, config.GetPackageReference("/a/path"), test.ShouldBeNil) - }) - - t.Run("non reference with ${package.some}", func(t *testing.T) { - test.That(t, config.GetPackageReference("/a/path/${packages.some}"), test.ShouldBeNil) - }) - - t.Run("non reference with ${package.some", func(t *testing.T) { - test.That(t, config.GetPackageReference("${packages.some"), test.ShouldBeNil) - }) - - t.Run("non reference with ${package.some} empty space", func(t *testing.T) { - test.That(t, config.GetPackageReference(" ${packages.some-package}"), test.ShouldBeNil) - }) +func TestPackageConfig(t *testing.T) { + homeDir, _ := os.UserHomeDir() + viamDotDir := filepath.Join(homeDir, ".viam") - t.Run("valid package reference", func(t *testing.T) { - test.That(t, config.GetPackageReference("${packages.some-package}/some/path"), test.ShouldResemble, - &config.PackageReference{Package: "some-package", PathInPackage: "/some/path"}) - }) + packageTests := []struct { + config config.PackageConfig + shouldFailValidation bool + expectedRealFilePath string + }{ + { + config: config.PackageConfig{ + Name: "my_package", + Package: "my_org/my_package", + Version: "0", + }, + expectedRealFilePath: filepath.Join(viamDotDir, "packages", ".data", "ml_model", "my_org-my_package-0"), + }, + { + config: config.PackageConfig{ + Name: "my_module", + Type: config.PackageTypeModule, + Package: "my_org/my_module", + Version: "1.2", + }, + expectedRealFilePath: filepath.Join(viamDotDir, "packages", ".data", "module", "my_org-my_module-1_2"), + }, + { + config: config.PackageConfig{ + Name: "my_ml_model", + Type: config.PackageTypeMlModel, + Package: "my_org/my_ml_model", + Version: "latest", + }, + expectedRealFilePath: filepath.Join(viamDotDir, "packages", ".data", "ml_model", "my_org-my_ml_model-latest"), + }, + { + config: config.PackageConfig{ + Name: "my_slam_map", + Type: config.PackageTypeSlamMap, + Package: "my_org/my_slam_map", + Version: "latest", + }, + expectedRealFilePath: filepath.Join(viamDotDir, "packages", ".data", "slam_map", "my_org-my_slam_map-latest"), + }, + { + config: config.PackageConfig{ + Name: "my_board_defs", + Type: config.PackageTypeBoardDefs, + Package: "my_org/my_board_defs", + Version: "latest", + }, + expectedRealFilePath: filepath.Join(viamDotDir, "packages", ".data", "board_defs", "my_org-my_board_defs-latest"), + }, + { + config: config.PackageConfig{ + Name: "::::", + Type: config.PackageTypeMlModel, + Package: "my_org/my_ml_model", + Version: "latest", + }, + shouldFailValidation: true, + }, + { + config: config.PackageConfig{ + Name: "my_ml_model", + Type: config.PackageType("willfail"), + Package: "my_org/my_ml_model", + Version: "latest", + }, + shouldFailValidation: true, + }, + } - t.Run("valid package reference no path", func(t *testing.T) { - test.That(t, config.GetPackageReference("${packages.some-package}"), test.ShouldResemble, - &config.PackageReference{Package: "some-package", PathInPackage: ""}) - }) + for _, pt := range packageTests { + err := pt.config.Validate("") + if pt.shouldFailValidation { + test.That(t, err, test.ShouldBeError) + continue + } + test.That(t, err, test.ShouldBeNil) + actualFilepath := pt.config.LocalDataDirectory(filepath.Join(viamDotDir, "packages")) + test.That(t, actualFilepath, test.ShouldEqual, pt.expectedRealFilePath) + } } diff --git a/config/data/diff_config_1.json b/config/data/diff_config_1.json index 614c6f9aa38..dd074ff79cf 100644 --- a/config/data/diff_config_1.json +++ b/config/data/diff_config_1.json @@ -2,7 +2,7 @@ "modules": [ { "name": "my-module", - "executable_path": "path/to/my-module", + "executable_path": ".", "log_level": "info" } ], diff --git a/config/data/diff_config_2.json b/config/data/diff_config_2.json index 7369726b6ec..87e6e4bb130 100644 --- a/config/data/diff_config_2.json +++ b/config/data/diff_config_2.json @@ -2,7 +2,7 @@ "modules": [ { "name": "my-module", - "executable_path": "new/path/to/my-module", + "executable_path": "..", "log_level": "debug" } ], diff --git a/config/diff.go b/config/diff.go index 174266d36ba..4057e9f5815 100644 --- a/config/diff.go +++ b/config/diff.go @@ -294,7 +294,7 @@ func diffProcesses(left, right []pexec.ProcessConfig, diff *Diff) bool { } func diffProcess(left, right pexec.ProcessConfig, diff *Diff) bool { - if reflect.DeepEqual(left, right) { + if left.Equals(right) { return false } diff.Modified.Processes = append(diff.Modified.Processes, right) @@ -336,7 +336,7 @@ func diffPackages(left, right []PackageConfig, diff *Diff) bool { } func diffPackage(left, right PackageConfig, diff *Diff) bool { - if reflect.DeepEqual(left, right) { + if left.Equals(right) { return false } diff.Modified.Packages = append(diff.Modified.Packages, right) @@ -489,7 +489,7 @@ func diffModules(leftModules, rightModules []Module, diff *Diff) bool { } func diffModule(left, right Module, diff *Diff) bool { - if reflect.DeepEqual(left, right) { + if left.Equals(right) { return false } diff.Modified.Modules = append(diff.Modified.Modules, right) diff --git a/config/diff_test.go b/config/diff_test.go index 7319a609bb9..642c6f17018 100644 --- a/config/diff_test.go +++ b/config/diff_test.go @@ -30,7 +30,7 @@ func TestDiffConfigs(t *testing.T) { Modules: []config.Module{ { Name: "my-module", - ExePath: "path/to/my-module", + ExePath: ".", LogLevel: "info", }, }, @@ -103,7 +103,7 @@ func TestDiffConfigs(t *testing.T) { Modules: []config.Module{ { Name: "my-module", - ExePath: "new/path/to/my-module", + ExePath: "..", LogLevel: "debug", }, }, @@ -314,7 +314,7 @@ func TestDiffConfigs(t *testing.T) { Modules: []config.Module{ { Name: "my-module", - ExePath: "path/to/my-module", + ExePath: ".", LogLevel: "info", }, }, @@ -664,5 +664,11 @@ func modifiedConfigDiffValidate(c *config.ModifiedConfigDiff) error { } } + for idx := 0; idx < len(c.Modules); idx++ { + if err := c.Modules[idx].Validate(fmt.Sprintf("%s.%d", "modules", idx)); err != nil { + return err + } + } + return nil } diff --git a/config/module.go b/config/module.go index 51e4423dfd6..8d88cf00028 100644 --- a/config/module.go +++ b/config/module.go @@ -2,12 +2,14 @@ package config import ( "os" + "reflect" "regexp" + "strings" "github.com/pkg/errors" ) -var moduleNameRegEx = regexp.MustCompile(`^[A-Za-z0-9_-]+$`) +var moduleNameRegEx = regexp.MustCompile(`^[\w-]+$`) const reservedModuleName = "parent" @@ -25,13 +27,30 @@ type Module struct { // value besides "" or "debug" is used for LogLevel ("log_level" in JSON). In other words, setting a LogLevel // of something like "info" will ignore the debug setting on the server. LogLevel string `json:"log_level"` + + alreadyValidated bool + cachedErr error } // Validate checks if the config is valid. func (m *Module) Validate(path string) error { - _, err := os.Stat(m.ExePath) - if err != nil { - return errors.Wrapf(err, "module %s executable path error", path) + if m.alreadyValidated { + return m.cachedErr + } + m.cachedErr = m.validate(path) + m.alreadyValidated = true + return m.cachedErr +} + +func (m *Module) validate(path string) error { + // Only check if the path exists during validation for local modules because the packagemanager may not have downloaded + // the package yet. + // As of 2023-08, modules can't know if they were originally registry modules, so this roundabout check is required + if !(ContainsPlaceholder(m.ExePath) || strings.HasPrefix(m.ExePath, viamPackagesDir)) { + _, err := os.Stat(m.ExePath) + if err != nil { + return errors.Wrapf(err, "module %s executable path error", path) + } } // the module name is used to create the socket path @@ -45,3 +64,13 @@ func (m *Module) Validate(path string) error { return nil } + +// Equals checks if the two modules are deeply equal to each other. +func (m Module) Equals(other Module) bool { + m.alreadyValidated = false + m.cachedErr = nil + other.alreadyValidated = false + other.cachedErr = nil + //nolint:govet + return reflect.DeepEqual(m, other) +} diff --git a/config/placeholder_replacement.go b/config/placeholder_replacement.go new file mode 100644 index 00000000000..f1ef6df45e8 --- /dev/null +++ b/config/placeholder_replacement.go @@ -0,0 +1,188 @@ +package config + +import ( + "fmt" + "reflect" + "regexp" + + "github.com/pkg/errors" + "go.uber.org/multierr" + + "go.viam.com/rdk/utils" +) + +// placeholderRegexp matches on all strings that satisfy our criteria for a placeholder +// Example string satisfying the regex: +// ${hello}. +var placeholderRegexp = regexp.MustCompile(`\$\{(?P[^\}]*)\}`) + +// packagePlaceholderRegexp matches on all valid ways of specifying one of our package placeholders +// Example strings satisfying the regex: +// packages.my-COOL-ml-model/__89 +// packages.modules.intel:CameraThatRocks +// packages.FutureP4ckge_Ty-pe.name. +var packagePlaceholderRegexp = regexp.MustCompile(`^packages(\.(?P[^\.]+))?\.(?P[\w:/-]+)$`) + +// ContainsPlaceholder returns true if the passed string contains a placeholder. +func ContainsPlaceholder(s string) bool { + return placeholderRegexp.MatchString(s) +} + +// ReplacePlaceholders traverses parts of the config to replace placeholders with their resolved values. +func (c *Config) ReplacePlaceholders() error { + var allErrs, err error + visitor := newPlaceholderReplacementVisitor(c) + + for i, service := range c.Services { + // this nil check may seem superfluous, however, the walking & casting will transform a + // utils.AttributeMap(nil) into a utils.AttributeMap{} which causes config diffs + if service.Attributes == nil { + continue + } + c.Services[i].Attributes, err = walkTypedAttributes(visitor, service.Attributes) + allErrs = multierr.Append(allErrs, err) + } + + for i, component := range c.Components { + if component.Attributes == nil { + continue + } + c.Components[i].Attributes, err = walkTypedAttributes(visitor, component.Attributes) + allErrs = multierr.Append(allErrs, err) + } + + for i, module := range c.Modules { + c.Modules[i].ExePath, err = visitor.replacePlaceholders(module.ExePath) + allErrs = multierr.Append(allErrs, err) + } + + return multierr.Append(visitor.AllErrors, allErrs) +} + +func walkTypedAttributes[T any](visitor *placeholderReplacementVisitor, attributes T) (T, error) { + var asIfc interface{} = attributes + if walker, ok := asIfc.(utils.Walker); ok { + newAttrs, err := walker.Walk(visitor) + if err != nil { + return attributes, err + } + newAttrsTyped, err := utils.AssertType[T](newAttrs) + if err != nil { + return attributes, err + } + return newAttrsTyped, nil + } + return attributes, errors.New("placeholder replacement tried to walk an unwalkable type") +} + +// placeholderReplacementVisitor is a visitor that replaces strings containing placeholder values with their desired values. +type placeholderReplacementVisitor struct { + // Map of packageName -> packageConfig + packages map[string]PackageConfig + // Accumulation of all that occurred during traversal + AllErrors error +} + +// newPlaceholderReplacementVisitor creates a new PlaceholderReplacementVisitor. +func newPlaceholderReplacementVisitor(cfg *Config) *placeholderReplacementVisitor { + // Create the map of packages that will be used for replacement + packages := map[string]PackageConfig{} + for _, config := range cfg.Packages { + packages[config.Name] = config + } + + return &placeholderReplacementVisitor{ + packages: packages, + AllErrors: nil, + } +} + +// Visit implements config.Visitor. +// Importantly, this function will never error. Instead, all errors are accumulated on the PlaceholderReplacementVisitor.AllErrors object. +// +// Returning an error causes the walker to prematurely stop traversing the tree. This is undesirable because it means a single invalid +// placeholder causes otherwise valid placeholders to appear invalid to the user (there is also no guaranteed order that an +// attribute map is traversed, so if there is a single invalid placeholder, the set of other placeholders that fail to be resolved would +// be non-deterministic). +func (v *placeholderReplacementVisitor) Visit(data interface{}) (interface{}, error) { + t := reflect.TypeOf(data) + + var s string + switch { + case t.Kind() == reflect.String: + s = data.(string) + case t.Kind() == reflect.Ptr && t.Elem().Kind() == reflect.String: + s = *data.(*string) + default: + return data, nil + } + + withReplacedRefs, err := v.replacePlaceholders(s) + v.AllErrors = multierr.Append(v.AllErrors, err) + + // If the input was a pointer, return a pointer. + if t.Kind() == reflect.Ptr { + return &withReplacedRefs, nil + } + return withReplacedRefs, nil +} + +// replacePlaceholders tries to replace all placeholders in a given string using a two step process: +// First, match anything that could be a placeholder (ex: ${hello}) +// Second, attempt to match those placeholder keys (ex: "hello") against the some known set of valid placeholders and perform replacement +// +// This is done so that misspellings like ${package.module.name} wont be silently ignored and +// so that it is easy to add additional placeholder types in the future (like environment variables). +func (v *placeholderReplacementVisitor) replacePlaceholders(s string) (string, error) { + var replacementErrors error + // First, match all possible placeholders (ex: ${hello}) + patchedStr := placeholderRegexp.ReplaceAllFunc([]byte(s), func(placeholder []byte) []byte { + matches := placeholderRegexp.FindSubmatch(placeholder) + if matches == nil { + replacementErrors = multierr.Append(replacementErrors, errors.Errorf("failed to find substring matches for %q", string(placeholder))) + return placeholder + } + placeholderKey := matches[placeholderRegexp.SubexpIndex("placeholder_key")] + + // Now, match against every way we know of doing placeholder replacement + if packagePlaceholderRegexp.Match(placeholderKey) { + replaced, err := v.replacePackagePlaceholder(string(placeholderKey)) + if err != nil { + replacementErrors = multierr.Append(replacementErrors, err) + return placeholder + } + return []byte(replaced) + } + + replacementErrors = multierr.Append(replacementErrors, errors.Errorf("invalid placeholder %q", string(placeholder))) + return placeholder + }) + + return string(patchedStr), replacementErrors +} + +func (v *placeholderReplacementVisitor) replacePackagePlaceholder(toReplace string) (string, error) { + matches := packagePlaceholderRegexp.FindStringSubmatch(toReplace) + if matches == nil { + return toReplace, errors.Errorf("failed to find substring matches for %q", toReplace) + } + packageType := matches[packagePlaceholderRegexp.SubexpIndex("type")] + packageName := matches[packagePlaceholderRegexp.SubexpIndex("name")] + + if packageType == "" { + // for backwards compatibility + packageType = string(PackageTypeMlModel) + } + packageConfig, isPresent := v.packages[packageName] + if !isPresent { + return toReplace, errors.Errorf("failed to find a package named %q for placeholder %q", + packageName, toReplace) + } + if packageType != string(packageConfig.Type) { + expectedPlaceholder := fmt.Sprintf("packages.%s.%s", string(packageConfig.Type), packageName) + return toReplace, + errors.Errorf("placeholder %q is looking for a package of type %q but a package of type %q was found. Try %q", + toReplace, packageType, string(packageConfig.Type), expectedPlaceholder) + } + return packageConfig.LocalDataDirectory(viamPackagesDir), nil +} diff --git a/config/placeholder_replacement_test.go b/config/placeholder_replacement_test.go new file mode 100644 index 00000000000..ae72d1b555f --- /dev/null +++ b/config/placeholder_replacement_test.go @@ -0,0 +1,171 @@ +package config_test + +import ( + "fmt" + "os" + "path/filepath" + "testing" + + "go.viam.com/test" + + "go.viam.com/rdk/config" + "go.viam.com/rdk/resource" + "go.viam.com/rdk/utils" +) + +func TestPlaceholderReplacement(t *testing.T) { + homeDir, _ := os.UserHomeDir() + viamPackagesDir := filepath.Join(homeDir, ".viam", "packages") + t.Run("placeholder replacement", func(t *testing.T) { + cfg := &config.Config{ + Components: []resource.Config{ + { + Name: "m", + Attributes: utils.AttributeMap{ + "should_equal_1": "${packages.coolpkg}", + "should_equal_2": "${packages.ml_model.coolpkg}", + "mid_string_replace": "Hello ${packages.coolpkg} Friends!", + "module_replace": "${packages.module.coolmod}", + "multi_replace": "${packages.coolpkg} ${packages.module.coolmod}", + }, + }, + }, + Services: []resource.Config{ + { + Name: "m", + Attributes: utils.AttributeMap{ + "apply_to_services_too": "${packages.coolpkg}", + }, + }, + }, + Modules: []config.Module{ + { + ExePath: "${packages.module.coolmod}/bin", + }, + }, + Packages: []config.PackageConfig{ + { + Name: "coolpkg", + Package: "orgid/pkg", + Type: "ml_model", + Version: "0.4.0", + }, + { + Name: "coolmod", + Package: "orgid/mod", + Type: "module", + Version: "0.5.0", + }, + }, + } + err := cfg.ReplacePlaceholders() + test.That(t, err, test.ShouldBeNil) + dirForMlModel := cfg.Packages[0].LocalDataDirectory(viamPackagesDir) + dirForModule := cfg.Packages[1].LocalDataDirectory(viamPackagesDir) + // components + attrMap := cfg.Components[0].Attributes + test.That(t, attrMap["should_equal_1"], test.ShouldResemble, attrMap["should_equal_2"]) + test.That(t, attrMap["should_equal_1"], test.ShouldResemble, dirForMlModel) + test.That(t, attrMap["mid_string_replace"], test.ShouldResemble, fmt.Sprintf("Hello %s Friends!", dirForMlModel)) + test.That(t, attrMap["module_replace"], test.ShouldResemble, dirForModule) + test.That(t, attrMap["multi_replace"], test.ShouldResemble, fmt.Sprintf("%s %s", dirForMlModel, dirForModule)) + // services + attrMap = cfg.Services[0].Attributes + test.That(t, attrMap["apply_to_services_too"], test.ShouldResemble, dirForMlModel) + // module + exePath := cfg.Modules[0].ExePath + test.That(t, exePath, test.ShouldResemble, fmt.Sprintf("%s/bin", dirForModule)) + }) + t.Run("placeholder typos", func(t *testing.T) { + // Unknown type of placeholder + cfg := &config.Config{ + Components: []resource.Config{ + { + Attributes: utils.AttributeMap{ + "a": "${invalidplaceholder}", + }, + }, + }, + } + err := cfg.ReplacePlaceholders() + test.That(t, fmt.Sprint(err), test.ShouldContainSubstring, "invalidplaceholder") + // Test that the attribute is left unchanged if replacement failed + test.That(t, cfg.Components[0].Attributes["a"], test.ShouldResemble, "${invalidplaceholder}") + + // Empy placeholder + cfg = &config.Config{ + Components: []resource.Config{ + { + Attributes: utils.AttributeMap{ + "a": "${}", + }, + }, + }, + } + err = cfg.ReplacePlaceholders() + test.That(t, fmt.Sprint(err), test.ShouldContainSubstring, "invalid placeholder") + // Test that the attribute is left unchanged if replacement failed + test.That(t, cfg.Components[0].Attributes["a"], test.ShouldResemble, "${}") + + // Package placeholder with no equivalent pkg + cfg = &config.Config{ + Components: []resource.Config{ + { + Attributes: utils.AttributeMap{ + "a": "${packages.ml_model.chicken}", + }, + }, + }, + } + err = cfg.ReplacePlaceholders() + test.That(t, fmt.Sprint(err), test.ShouldContainSubstring, "package named \"chicken\"") + // Test that the attribute is left unchanged if replacement failed + test.That(t, cfg.Components[0].Attributes["a"], test.ShouldResemble, "${packages.ml_model.chicken}") + + // Package placeholder with wrong type + cfg = &config.Config{ + Components: []resource.Config{ + { + Attributes: utils.AttributeMap{ + "a": "${packages.ml_model.chicken}", + }, + }, + }, + Packages: []config.PackageConfig{ + { + Name: "chicken", + Package: "orgid/pkg", + Type: "module", + Version: "0.4.0", + }, + }, + } + err = cfg.ReplacePlaceholders() + test.That(t, fmt.Sprint(err), test.ShouldContainSubstring, "looking for a package of type \"ml_model\"") + + // Half successful string replacement + cfg = &config.Config{ + Components: []resource.Config{ + { + Attributes: utils.AttributeMap{ + "a": "${packages.module.chicken}/${invalidplaceholder}", + }, + }, + }, + Packages: []config.PackageConfig{ + { + Name: "chicken", + Package: "orgid/pkg", + Type: "module", + Version: "0.4.0", + }, + }, + } + err = cfg.ReplacePlaceholders() + test.That(t, fmt.Sprint(err), test.ShouldContainSubstring, "invalidplaceholder") + test.That(t, fmt.Sprint(err), test.ShouldNotContainSubstring, "chicken") + + test.That(t, cfg.Components[0].Attributes["a"], test.ShouldResemble, + fmt.Sprintf("%s/${invalidplaceholder}", cfg.Packages[0].LocalDataDirectory(viamPackagesDir))) + }) +} diff --git a/config/proto_conversions.go b/config/proto_conversions.go index 36a5dde2d65..3e9b9abb3ee 100644 --- a/config/proto_conversions.go +++ b/config/proto_conversions.go @@ -7,6 +7,7 @@ import ( "github.com/edaniels/golog" "github.com/golang/geo/r3" "github.com/pkg/errors" + packagespb "go.viam.com/api/app/packages/v1" pb "go.viam.com/api/app/v1" "go.viam.com/utils/pexec" "go.viam.com/utils/protoutils" @@ -179,14 +180,20 @@ func ServiceConfigToProto(conf *resource.Config) (*pb.ServiceConfig, error) { return nil, err } + serviceConfigs, err := mapSliceWithErrors(conf.AssociatedResourceConfigs, AssociatedResourceConfigToProto) + if err != nil { + return nil, errors.Wrap(err, "failed to convert service configs") + } + protoConf := pb.ServiceConfig{ - Name: conf.Name, - Namespace: string(conf.API.Type.Namespace), - Type: conf.API.SubtypeName, - Api: conf.API.String(), - Model: conf.Model.String(), - Attributes: attributes, - DependsOn: conf.DependsOn, + Name: conf.Name, + Namespace: string(conf.API.Type.Namespace), + Type: conf.API.SubtypeName, + Api: conf.API.String(), + Model: conf.Model.String(), + Attributes: attributes, + DependsOn: conf.DependsOn, + ServiceConfigs: serviceConfigs, } return &protoConf, nil @@ -200,6 +207,11 @@ func ServiceConfigFromProto(protoConf *pb.ServiceConfig) (*resource.Config, erro attrs = nil } + serviceConfigs, err := mapSliceWithErrors(protoConf.ServiceConfigs, AssociatedResourceConfigFromProto) + if err != nil { + return nil, errors.Wrap(err, "failed to convert service configs") + } + api, err := resource.NewAPIFromString(protoConf.GetApi()) if err != nil { return nil, err @@ -211,11 +223,12 @@ func ServiceConfigFromProto(protoConf *pb.ServiceConfig) (*resource.Config, erro } conf := resource.Config{ - Name: protoConf.GetName(), - API: api, - Model: model, - Attributes: attrs, - DependsOn: protoConf.GetDependsOn(), + Name: protoConf.GetName(), + API: api, + Model: model, + Attributes: attrs, + DependsOn: protoConf.GetDependsOn(), + AssociatedResourceConfigs: serviceConfigs, } return &conf, nil @@ -815,6 +828,7 @@ func PackageConfigToProto(cfg *PackageConfig) (*pb.PackageConfig, error) { Name: cfg.Name, Package: cfg.Package, Version: cfg.Version, + Type: string(cfg.Type), }, nil } @@ -824,5 +838,26 @@ func PackageConfigFromProto(proto *pb.PackageConfig) (*PackageConfig, error) { Name: proto.Name, Package: proto.Package, Version: proto.Version, + Type: PackageType(proto.Type), }, nil } + +// PackageTypeToProto converts a config PackageType to its proto equivalent +// This is required be because app/packages uses a PackageType enum but app/PackageConfig uses a string Type. +func PackageTypeToProto(t PackageType) (*packagespb.PackageType, error) { + switch t { + case "": + // for backwards compatibility + fallthrough + case PackageTypeMlModel: + return packagespb.PackageType_PACKAGE_TYPE_ML_MODEL.Enum(), nil + case PackageTypeModule: + return packagespb.PackageType_PACKAGE_TYPE_MODULE.Enum(), nil + case PackageTypeSlamMap: + return packagespb.PackageType_PACKAGE_TYPE_SLAM_MAP.Enum(), nil + case PackageTypeBoardDefs: + return packagespb.PackageType_PACKAGE_TYPE_BOARD_DEFS.Enum(), nil + default: + return packagespb.PackageType_PACKAGE_TYPE_UNSPECIFIED.Enum(), errors.Errorf("unknown package type %q", t) + } +} diff --git a/config/proto_conversions_test.go b/config/proto_conversions_test.go index 2191eb27efb..f0e248a9ce2 100644 --- a/config/proto_conversions_test.go +++ b/config/proto_conversions_test.go @@ -4,6 +4,7 @@ import ( "crypto/rand" "crypto/rsa" "encoding/json" + "fmt" "syscall" "testing" "time" @@ -11,6 +12,7 @@ import ( "github.com/edaniels/golog" "github.com/golang/geo/r3" "github.com/lestrrat-go/jwx/jwk" + packagespb "go.viam.com/api/app/packages/v1" pb "go.viam.com/api/app/v1" "go.viam.com/test" "go.viam.com/utils/jwks" @@ -115,6 +117,14 @@ var testService = resource.Config{ "attr1": 1, }, DependsOn: []string{"some-depends-on"}, + AssociatedResourceConfigs: []resource.AssociatedResourceConfig{ + { + API: resource.APINamespaceRDK.WithServiceType("some-type-1"), + Attributes: utils.AttributeMap{ + "attr1": 1, + }, + }, + }, } var testProcessConfig = pexec.ProcessConfig{ @@ -183,6 +193,7 @@ var testPackageConfig = PackageConfig{ Name: "package-name", Package: "some/package", Version: "v1", + Type: PackageTypeModule, } var ( @@ -521,33 +532,53 @@ func TestServiceConfigToProto(t *testing.T) { { Name: "basic component with internal API", Conf: resource.Config{ - Name: "foo", - API: resource.APINamespaceRDK.WithServiceType("base"), - Model: resource.DefaultModelFamily.WithModel("fake"), + Name: "foo", + API: resource.APINamespaceRDK.WithServiceType("base"), + Model: resource.DefaultModelFamily.WithModel("fake"), + AssociatedResourceConfigs: []resource.AssociatedResourceConfig{}, }, }, { Name: "basic component with external API", Conf: resource.Config{ - Name: "foo", - API: resource.NewAPI("acme", "service", "gizmo"), - Model: resource.DefaultModelFamily.WithModel("fake"), + Name: "foo", + API: resource.NewAPI("acme", "service", "gizmo"), + Model: resource.DefaultModelFamily.WithModel("fake"), + AssociatedResourceConfigs: []resource.AssociatedResourceConfig{}, }, }, { Name: "basic component with external model", Conf: resource.Config{ - Name: "foo", - API: resource.NewAPI("acme", "service", "gizmo"), - Model: resource.NewModel("acme", "test", "model"), + Name: "foo", + API: resource.NewAPI("acme", "service", "gizmo"), + Model: resource.NewModel("acme", "test", "model"), + AssociatedResourceConfigs: []resource.AssociatedResourceConfig{}, }, }, { Name: "empty model name", + Conf: resource.Config{ + Name: "foo", + API: resource.NewAPI("acme", "service", "gizmo"), + Model: resource.Model{}, + AssociatedResourceConfigs: []resource.AssociatedResourceConfig{}, + }, + }, + { + Name: "associated service config", Conf: resource.Config{ Name: "foo", API: resource.NewAPI("acme", "service", "gizmo"), Model: resource.Model{}, + AssociatedResourceConfigs: []resource.AssociatedResourceConfig{ + { + API: resource.APINamespaceRDK.WithServiceType("some-type-1"), + Attributes: utils.AttributeMap{ + "attr1": 1, + }, + }, + }, }, }, } { @@ -559,7 +590,7 @@ func TestServiceConfigToProto(t *testing.T) { test.That(t, err, test.ShouldBeNil) test.That(t, out, test.ShouldNotBeNil) - test.That(t, out, test.ShouldResemble, &tc.Conf) + test.That(t, out.String(), test.ShouldResemble, tc.Conf.String()) _, err = out.Validate("test", resource.APITypeServiceName) test.That(t, err, test.ShouldBeNil) }) @@ -885,3 +916,21 @@ func TestDisablePartialStart(t *testing.T) { test.That(t, err, test.ShouldNotBeNil) test.That(t, out, test.ShouldBeNil) } + +func TestPackageTypeConversion(t *testing.T) { + emptyType := PackageType("") + converted, err := PackageTypeToProto(emptyType) + test.That(t, err, test.ShouldBeNil) + test.That(t, converted, test.ShouldResemble, packagespb.PackageType_PACKAGE_TYPE_ML_MODEL.Enum()) + + moduleType := PackageType("module") + converted, err = PackageTypeToProto(moduleType) + test.That(t, err, test.ShouldBeNil) + test.That(t, converted, test.ShouldResemble, packagespb.PackageType_PACKAGE_TYPE_MODULE.Enum()) + + badType := PackageType("invalid-package-type") + converted, err = PackageTypeToProto(badType) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, fmt.Sprint(err), test.ShouldContainSubstring, "invalid-package-type") + test.That(t, converted, test.ShouldResemble, packagespb.PackageType_PACKAGE_TYPE_UNSPECIFIED.Enum()) +} diff --git a/config/reader.go b/config/reader.go index 8f62dd29a7e..cfc3b2f0927 100644 --- a/config/reader.go +++ b/config/reader.go @@ -39,6 +39,7 @@ func getAgentInfo() (*apppb.AgentInfo, error) { if err != nil { return nil, err } + platform := fmt.Sprintf("%s/%s", runtime.GOOS, runtime.GOARCH) return &apppb.AgentInfo{ Host: hostname, @@ -46,13 +47,25 @@ func getAgentInfo() (*apppb.AgentInfo, error) { Os: runtime.GOOS, Version: Version, GitRevision: GitRevision, + Platform: &platform, }, nil } -var viamDotDir = filepath.Join(os.Getenv("HOME"), ".viam") +var ( + // ViamDotDir is the directory for Viam's cached files. + ViamDotDir string + viamPackagesDir string +) + +func init() { + //nolint:errcheck + home, _ := os.UserHomeDir() + ViamDotDir = filepath.Join(home, ".viam") + viamPackagesDir = filepath.Join(ViamDotDir, "packages") +} func getCloudCacheFilePath(id string) string { - return filepath.Join(viamDotDir, fmt.Sprintf("cached_cloud_config_%s.json", id)) + return filepath.Join(ViamDotDir, fmt.Sprintf("cached_cloud_config_%s.json", id)) } func readFromCache(id string) (*Config, error) { @@ -75,7 +88,7 @@ func readFromCache(id string) (*Config, error) { } func storeToCache(id string, cfg *Config) error { - if err := os.MkdirAll(viamDotDir, 0o700); err != nil { + if err := os.MkdirAll(ViamDotDir, 0o700); err != nil { return err } @@ -371,6 +384,10 @@ func processConfig(unprocessedConfig *Config, fromCloud bool, logger golog.Logge return nil, errors.Wrap(err, "error copying config") } + if err := cfg.ReplacePlaceholders(); err != nil { + logger.Errorw("error during placeholder replacement", "err", err) + } + // Copy does not presve ConfigFilePath and we need to pass it along manually cfg.ConfigFilePath = unprocessedConfig.ConfigFilePath diff --git a/data/collector.go b/data/collector.go index 1f13e304747..21cbdf607a5 100644 --- a/data/collector.go +++ b/data/collector.go @@ -15,7 +15,10 @@ import ( v1 "go.viam.com/api/app/datasync/v1" "go.viam.com/utils" "go.viam.com/utils/protoutils" + "google.golang.org/grpc/codes" + "google.golang.org/grpc/status" "google.golang.org/protobuf/types/known/anypb" + "google.golang.org/protobuf/types/known/structpb" "google.golang.org/protobuf/types/known/timestamppb" "go.viam.com/rdk/resource" @@ -28,6 +31,18 @@ var sleepCaptureCutoff = 2 * time.Millisecond // CaptureFunc allows the creation of simple Capturers with anonymous functions. type CaptureFunc func(ctx context.Context, params map[string]*anypb.Any) (interface{}, error) +// FromDMContextKey is used to check whether the context is from data management. +type FromDMContextKey struct{} + +// FromDMString is used to access the 'fromDataManagement' value from a request's Extra struct. +const FromDMString = "fromDataManagement" + +// FromDMExtraMap is a map with 'fromDataManagement' set to true. +var FromDMExtraMap = map[string]interface{}{FromDMString: true} + +// ErrNoCaptureToStore is returned when a modular filter resource filters the capture coming from the base resource. +var ErrNoCaptureToStore = status.Error(codes.FailedPrecondition, "no capture from filter module") + // Collector collects data to some target. type Collector interface { Close() @@ -186,6 +201,10 @@ func (c *collector) getAndPushNextReading() { reading, err := c.captureFunc(c.cancelCtx, c.params) timeReceived := timestamppb.New(c.clock.Now().UTC()) if err != nil { + if errors.Is(err, ErrNoCaptureToStore) { + c.logger.Debugln("capture filtered out by modular resource") + return + } c.captureErrors <- errors.Wrap(err, "error while capturing data") return } @@ -290,3 +309,12 @@ func InvalidInterfaceErr(api resource.API) error { func FailedToReadErr(component, method string, err error) error { return errors.Errorf("failed to get reading of method %s of component %s: %v", method, component, err) } + +// GetExtraFromContext sets the extra struct with "fromDataManagement": true if the flag is true in the context. +func GetExtraFromContext(ctx context.Context) (*structpb.Struct, error) { + extra := make(map[string]interface{}) + if ctx.Value(FromDMContextKey{}) == true { + extra[FromDMString] = true + } + return protoutils.StructToStructPb(extra) +} diff --git a/etc/configs/fake.json b/etc/configs/fake.json index aa7ccdac23a..193482528a5 100644 --- a/etc/configs/fake.json +++ b/etc/configs/fake.json @@ -97,7 +97,11 @@ "type": "movement_sensor", "model": "fake" }, - + { + "name": "power_sensor1", + "type": "power_sensor", + "model": "fake" + }, { "name": "sensor1", "type": "sensor", @@ -140,6 +144,39 @@ "movement_sensor": "movement_sensor1", "base": "base1", "obstacles": [ + { + "location": { + "latitude": 40.6705209, + "longitude": -73.9659182 + }, + "geometries": [ + { + "r": 2000 + } + ] + }, + { + "location": { + "longitude": -73.976472, + "latitude": 40.693268 + }, + "geometries": [ + { + "r": 2000000 + } + ] + }, + { + "geometries": [ + { + "r": 3700 + } + ], + "location": { + "latitude": 40.772239, + "longitude": -73.98241 + } + }, { "location": { "latitude": 40.6759, @@ -148,10 +185,19 @@ "geometries": [ { "type": "capsule", - "r": 10, - "l": 30, - "translation": { "x": 0, "y": 0, "z": 0 } - } + "r": 50000, + "l": 300000, + "translation": { "x": 0, "y": 0, "z": 0 }, + "orientation": { + "type": "ov_degrees", + "value": { + "x": 0, + "y": 0, + "z": 1, + "th": 0 + } + } + } ] }, { @@ -162,7 +208,7 @@ "geometries": [ { "type": "sphere", - "r": 1000, + "r": 20000, "translation": { "x": 0, "y": 0, "z": 0 } } ] @@ -175,9 +221,9 @@ "geometries": [ { "type": "box", - "x": 100, - "y": 100, - "z": 100, + "x": 50000, + "y": 50000, + "z": 50000, "translation": { "x": 0, "y": 0, "z": 0 } } ] diff --git a/examples/apis.json b/examples/apis.json index 213f77883d5..9799627b5e2 100644 --- a/examples/apis.json +++ b/examples/apis.json @@ -52,6 +52,10 @@ "func": "LinearAcceleration", "args": ["context.Background()", "map[string]interface{}{}"] }, + "power_sensor": { + "func": "Power", + "args": ["context.Background()", "map[string]interface{}{}"] + }, "pose_tracker": { "func": "Poses", "args": [] diff --git a/examples/customresources/demos/complexmodule/client/client.go b/examples/customresources/demos/complexmodule/client/client.go index cff8aa40199..684934de3fa 100644 --- a/examples/customresources/demos/complexmodule/client/client.go +++ b/examples/customresources/demos/complexmodule/client/client.go @@ -99,12 +99,12 @@ func main() { if err != nil { logger.Fatal(err) } - loc, err := nav.Location(context.Background(), nil) + geoPose, err := nav.Location(context.Background(), nil) if err != nil { logger.Fatal(err) } - logger.Infof("denali service reports its location as %0.8f, %0.8f", loc.Lat(), loc.Lng()) + logger.Infof("denali service reports its location as %0.8f, %0.8f", geoPose.Location().Lat(), geoPose.Location().Lng()) err = nav.AddWaypoint(context.Background(), geo.NewPoint(55.1, 22.2), nil) if err != nil { @@ -157,14 +157,14 @@ func main() { time.Sleep(time.Second) logger.Info("spin left") - err = mybase.SetPower(context.Background(), r3.Vector{}, r3.Vector{Z: -1}, nil) + err = mybase.SetPower(context.Background(), r3.Vector{}, r3.Vector{Z: 1}, nil) if err != nil { logger.Fatal(err) } time.Sleep(time.Second) logger.Info("spin right") - err = mybase.SetPower(context.Background(), r3.Vector{}, r3.Vector{Z: 1}, nil) + err = mybase.SetPower(context.Background(), r3.Vector{}, r3.Vector{Z: -1}, nil) if err != nil { logger.Fatal(err) } diff --git a/examples/customresources/demos/complexmodule/moduletest/module_test.go b/examples/customresources/demos/complexmodule/moduletest/module_test.go index 258cfdc1559..a839c849fda 100644 --- a/examples/customresources/demos/complexmodule/moduletest/module_test.go +++ b/examples/customresources/demos/complexmodule/moduletest/module_test.go @@ -41,9 +41,12 @@ func TestComplexModule(t *testing.T) { cfgFilename, port, err := modifyCfg(t, utils.ResolveFile("examples/customresources/demos/complexmodule/module.json"), logger) test.That(t, err, test.ShouldBeNil) + serverPath, err := testutils.BuildTempModule(t, "web/cmd/server/") + test.That(t, err, test.ShouldBeNil) + server := pexec.NewManagedProcess(pexec.ProcessConfig{ - Name: "bash", - Args: []string{"-c", "exec bin/`uname`-`uname -m`/viam-server -config " + cfgFilename}, + Name: serverPath, + Args: []string{"-config", cfgFilename}, CWD: utils.ResolveFile("./"), Log: true, }, logger) @@ -260,10 +263,10 @@ func TestComplexModule(t *testing.T) { test.That(t, err, test.ShouldBeNil) nav := res.(navigation.Service) - loc, err := nav.Location(context.Background(), nil) + geoPose, err := nav.Location(context.Background(), nil) test.That(t, err, test.ShouldBeNil) - test.That(t, loc.Lat(), test.ShouldAlmostEqual, 63.0691739667009) - test.That(t, loc.Lng(), test.ShouldAlmostEqual, -151.00698515692034) + test.That(t, geoPose.Location().Lat(), test.ShouldAlmostEqual, 63.0691739667009) + test.That(t, geoPose.Location().Lng(), test.ShouldAlmostEqual, -151.00698515692034) err = nav.AddWaypoint(context.Background(), geo.NewPoint(55.1, 22.2), nil) test.That(t, err, test.ShouldBeNil) @@ -362,9 +365,12 @@ func TestValidationFailure(t *testing.T) { utils.ResolveFile("examples/customresources/demos/complexmodule/moduletest/bad_modular_validation.json"), logger) test.That(t, err, test.ShouldBeNil) + serverPath, err := testutils.BuildTempModule(t, "web/cmd/server/") + test.That(t, err, test.ShouldBeNil) + server := pexec.NewManagedProcess(pexec.ProcessConfig{ - Name: "bash", - Args: []string{"-c", "exec bin/`uname`-`uname -m`/viam-server -config " + cfgFilename}, + Name: serverPath, + Args: []string{"-config", cfgFilename}, CWD: utils.ResolveFile("./"), Log: true, }, logger) diff --git a/examples/customresources/models/mybase/mybase.go b/examples/customresources/models/mybase/mybase.go index ae76d5b0d2e..2753493e919 100644 --- a/examples/customresources/models/mybase/mybase.go +++ b/examples/customresources/models/mybase/mybase.go @@ -164,7 +164,7 @@ func (b *myBase) Properties(ctx context.Context, extra map[string]interface{}) ( } // Geometries returns physical dimensions. -func (b *myBase) Geometries(ctx context.Context) ([]spatialmath.Geometry, error) { +func (b *myBase) Geometries(ctx context.Context, extra map[string]interface{}) ([]spatialmath.Geometry, error) { return b.geometries, nil } diff --git a/examples/customresources/models/mynavigation/mynavigation.go b/examples/customresources/models/mynavigation/mynavigation.go index 4e7f03daba4..f44ffa9e302 100644 --- a/examples/customresources/models/mynavigation/mynavigation.go +++ b/examples/customresources/models/mynavigation/mynavigation.go @@ -11,6 +11,7 @@ import ( "go.viam.com/rdk/resource" "go.viam.com/rdk/services/navigation" + "go.viam.com/rdk/spatialmath" ) // Model is the full model definition. @@ -54,10 +55,11 @@ func (svc *navSvc) SetMode(ctx context.Context, mode navigation.Mode, extra map[ return nil } -func (svc *navSvc) Location(ctx context.Context, extra map[string]interface{}) (*geo.Point, error) { +func (svc *navSvc) Location(ctx context.Context, extra map[string]interface{}) (*spatialmath.GeoPose, error) { svc.waypointsMu.RLock() defer svc.waypointsMu.RUnlock() - return svc.loc, nil + geoPose := spatialmath.NewGeoPose(svc.loc, 0) + return geoPose, nil } func (svc *navSvc) Waypoints(ctx context.Context, extra map[string]interface{}) ([]navigation.Waypoint, error) { @@ -88,3 +90,7 @@ func (svc *navSvc) RemoveWaypoint(ctx context.Context, id primitive.ObjectID, ex svc.waypoints = newWps return nil } + +func (svc *navSvc) GetObstacles(ctx context.Context, extra map[string]interface{}) ([]*spatialmath.GeoObstacle, error) { + return []*spatialmath.GeoObstacle{}, nil +} diff --git a/go.mod b/go.mod index 9205668040c..43626c37d6d 100644 --- a/go.mod +++ b/go.mod @@ -16,6 +16,8 @@ require ( github.com/bep/debounce v1.2.1 github.com/bufbuild/buf v1.6.0 github.com/creack/pty v1.1.19-0.20220421211855-0d412c9fbeb1 + github.com/d2r2/go-i2c v0.0.0-20191123181816-73a8a799d6bc + github.com/d2r2/go-logger v0.0.0-20210606094344-60e9d1233e22 github.com/de-bkg/gognss v0.0.0-20220601150219-24ccfdcdbb5d github.com/disintegration/imaging v1.6.2 github.com/docker/go-units v0.5.0 @@ -24,6 +26,7 @@ require ( github.com/edaniels/golog v0.0.0-20230215213219-28954395e8d0 github.com/edaniels/lidario v0.0.0-20220607182921-5879aa7b96dd github.com/erh/scheme v0.0.0-20210304170849-99d295c6ce9a + github.com/fatih/color v1.15.0 github.com/fogleman/gg v1.3.0 github.com/fsnotify/fsnotify v1.6.0 github.com/fullstorydev/grpcurl v1.8.6 @@ -60,9 +63,9 @@ require ( github.com/muesli/clusters v0.0.0-20200529215643-2700303c1762 github.com/muesli/kmeans v0.3.1 github.com/nfnt/resize v0.0.0-20180221191011-83c6a9932646 - github.com/pion/mediadevices v0.4.1-0.20230605163757-e64f0d8697f9 + github.com/pion/mediadevices v0.5.1-0.20230724160738-03c44ee80347 github.com/pion/rtp v1.7.13 - github.com/pion/webrtc/v3 v3.2.6 + github.com/pion/webrtc/v3 v3.2.11 github.com/rhysd/actionlint v1.6.24 github.com/rs/cors v1.9.0 github.com/sergi/go-diff v1.3.1 @@ -71,7 +74,7 @@ require ( github.com/urfave/cli/v2 v2.10.3 github.com/viam-labs/go-libjpeg v0.3.1 github.com/viamrobotics/evdev v0.1.3 - github.com/viamrobotics/gostream v0.0.0-20230609200515-c5d67c29ed25 + github.com/viamrobotics/gostream v0.0.0-20230725145737-ed58004e202e github.com/xfmoulet/qoi v0.2.0 go-hep.org/x/hep v0.32.1 go.einride.tech/vlp16 v0.7.0 @@ -80,11 +83,11 @@ require ( go.uber.org/atomic v1.10.0 go.uber.org/multierr v1.11.0 go.uber.org/zap v1.24.0 - go.viam.com/api v0.1.143 + go.viam.com/api v0.1.186 go.viam.com/test v1.1.1-0.20220913152726-5da9916c08a2 - go.viam.com/utils v0.1.37 + go.viam.com/utils v0.1.43 goji.io v2.0.2+incompatible - golang.org/x/image v0.7.0 + golang.org/x/image v0.8.0 golang.org/x/tools v0.8.0 gonum.org/v1/gonum v0.12.0 gonum.org/v1/plot v0.12.0 @@ -93,6 +96,7 @@ require ( google.golang.org/grpc/cmd/protoc-gen-go-grpc v1.2.0 google.golang.org/protobuf v1.30.0 gopkg.in/src-d/go-billy.v4 v4.3.2 + gorgonia.org/tensor v0.9.24 gotest.tools/gotestsum v1.10.0 periph.io/x/conn/v3 v3.7.0 periph.io/x/host/v3 v3.8.1-0.20230331112814-9f0d9f7d76db @@ -124,13 +128,14 @@ require ( github.com/alecthomas/participle/v2 v2.0.0-alpha3 // indirect github.com/alexkohler/prealloc v1.0.0 // indirect github.com/alingse/asasalint v0.0.11 // indirect + github.com/apache/arrow/go/arrow v0.0.0-20201229220542-30ce2eb5d4dc // indirect github.com/ashanbrown/forbidigo v1.4.0 // indirect github.com/ashanbrown/makezero v1.1.1 // indirect github.com/aws/aws-sdk-go v1.38.20 // indirect github.com/bamiaux/iobit v0.0.0-20170418073505-498159a04883 // indirect github.com/beorn7/perks v1.0.1 // indirect github.com/bkielbasa/cyclop v1.2.0 // indirect - github.com/blackjack/webcam v0.0.0-20230502173554-3b52e93e8607 // indirect + github.com/blackjack/webcam v0.0.0-20230509180125-87693b3f29dc // indirect github.com/blizzy78/varnamelen v0.8.0 // indirect github.com/bombsimon/wsl/v3 v3.4.0 // indirect github.com/breml/bidichk v0.2.3 // indirect @@ -145,6 +150,8 @@ require ( github.com/cespare/xxhash/v2 v2.2.0 // indirect github.com/charithe/durationcheck v0.0.9 // indirect github.com/chavacava/garif v0.0.0-20221024190013-b3ef35877348 // indirect + github.com/chewxy/hm v1.0.0 // indirect + github.com/chewxy/math32 v1.0.8 // indirect github.com/cncf/udpa/go v0.0.0-20220112060539-c52dc94e7fbe // indirect github.com/cncf/xds/go v0.0.0-20230105202645-06c439db220b // indirect github.com/cpuguy83/go-md2man/v2 v2.0.2 // indirect @@ -161,7 +168,6 @@ require ( github.com/esimonov/ifshort v1.0.4 // indirect github.com/ettle/strcase v0.1.1 // indirect github.com/fatih/camelcase v1.0.0 // indirect - github.com/fatih/color v1.15.0 // indirect github.com/fatih/structtag v1.2.0 // indirect github.com/firefart/nonamedreturns v1.0.4 // indirect github.com/fzipp/gocyclo v0.6.0 // indirect @@ -187,6 +193,7 @@ require ( github.com/goccy/go-json v0.10.2 // indirect github.com/gofrs/flock v0.8.1 // indirect github.com/gofrs/uuid v4.2.0+incompatible // indirect + github.com/gogo/protobuf v1.3.2 // indirect github.com/golang/glog v1.0.0 // indirect github.com/golang/groupcache v0.0.0-20210331224755-41bb18bfe9da // indirect github.com/golang/snappy v0.0.4 // indirect @@ -229,7 +236,6 @@ require ( github.com/jingyugao/rowserrcheck v1.1.1 // indirect github.com/jirfag/go-printf-func-name v0.0.0-20200119135958-7558a9eaa5af // indirect github.com/jmespath/go-jmespath v0.4.0 // indirect - github.com/jonboulle/clockwork v0.3.0 // indirect github.com/julz/importas v0.1.0 // indirect github.com/junk1tm/musttag v0.4.5 // indirect github.com/kisielk/errcheck v1.6.3 // indirect @@ -275,7 +281,7 @@ require ( github.com/pierrec/lz4 v2.0.5+incompatible // indirect github.com/pion/datachannel v1.5.5 // indirect github.com/pion/dtls/v2 v2.2.7 // indirect - github.com/pion/ice/v2 v2.3.5 // indirect + github.com/pion/ice/v2 v2.3.9 // indirect github.com/pion/interceptor v0.1.17 // indirect github.com/pion/logging v0.2.2 // indirect github.com/pion/mdns v0.0.8-0.20230502060824-17c664ea7d5c // indirect @@ -284,9 +290,9 @@ require ( github.com/pion/sctp v1.8.7 // indirect github.com/pion/sdp/v3 v3.0.6 // indirect github.com/pion/srtp/v2 v2.0.15 // indirect - github.com/pion/stun v0.6.0 // indirect + github.com/pion/stun v0.6.1 // indirect github.com/pion/transport/v2 v2.2.1 // indirect - github.com/pion/turn/v2 v2.1.0 // indirect + github.com/pion/turn/v2 v2.1.2 // indirect github.com/pkg/browser v0.0.0-20210911075715-681adbf594b8 // indirect github.com/pkg/profile v1.6.0 // indirect github.com/pmezard/go-difflib v1.0.0 // indirect @@ -345,21 +351,23 @@ require ( github.com/xdg-go/scram v1.1.2 // indirect github.com/xdg-go/stringprep v1.0.4 // indirect github.com/xrash/smetrics v0.0.0-20201216005158-039620a65673 // indirect + github.com/xtgo/set v1.0.0 // indirect github.com/yagipy/maintidx v1.0.0 // indirect github.com/yeya24/promlinter v0.2.0 // indirect github.com/youmark/pkcs8 v0.0.0-20201027041543-1326539a0a0a // indirect github.com/zitadel/oidc v1.13.4 // indirect gitlab.com/bosi/decorder v0.2.3 // indirect go.uber.org/goleak v1.2.1 // indirect - golang.org/x/crypto v0.9.0 // indirect + go4.org/unsafe/assume-no-moving-gc v0.0.0-20230525183740-e7c30c78aeb2 // indirect + golang.org/x/crypto v0.10.0 // indirect golang.org/x/exp/typeparams v0.0.0-20230203172020-98cc5a0785f9 // indirect golang.org/x/mod v0.10.0 // indirect - golang.org/x/net v0.10.0 // indirect + golang.org/x/net v0.11.0 // indirect golang.org/x/oauth2 v0.7.0 // indirect golang.org/x/sync v0.1.0 // indirect - golang.org/x/sys v0.8.0 // indirect - golang.org/x/term v0.8.0 // indirect - golang.org/x/text v0.9.0 // indirect + golang.org/x/sys v0.9.0 // indirect + golang.org/x/term v0.9.0 // indirect + golang.org/x/text v0.10.0 // indirect golang.org/x/xerrors v0.0.0-20220907171357-04be3eba64a2 // indirect google.golang.org/api v0.114.0 // indirect google.golang.org/appengine v1.6.7 // indirect @@ -367,6 +375,8 @@ require ( gopkg.in/square/go-jose.v2 v2.6.0 // indirect gopkg.in/yaml.v2 v2.4.0 // indirect gopkg.in/yaml.v3 v3.0.1 // indirect + gorgonia.org/vecf32 v0.9.0 // indirect + gorgonia.org/vecf64 v0.9.0 // indirect honnef.co/go/tools v0.4.2 // indirect howett.net/plist v1.0.0 // indirect mvdan.cc/gofumpt v0.4.0 // indirect diff --git a/go.sum b/go.sum index 67620ec98d6..1bed976a348 100644 --- a/go.sum +++ b/go.sum @@ -144,6 +144,8 @@ github.com/anmitsu/go-shlex v0.0.0-20161002113705-648efa622239/go.mod h1:2FmKhYU github.com/antihax/optional v0.0.0-20180407024304-ca021399b1a6/go.mod h1:V8iCPQYkqmusNa815XgQio277wI47sdRh1dUOLdyC6Q= github.com/antihax/optional v1.0.0/go.mod h1:uupD/76wgC+ih3iEmQUL+0Ugr19nfwCT1kdvxnR2qWY= github.com/aokoli/goutils v1.0.1/go.mod h1:SijmP0QR8LtwsmDs8Yii5Z/S4trXFGFC2oO5g9DP+DQ= +github.com/apache/arrow/go/arrow v0.0.0-20201229220542-30ce2eb5d4dc h1:zvQ6w7KwtQWgMQiewOF9tFtundRMVZFSAksNV6ogzuY= +github.com/apache/arrow/go/arrow v0.0.0-20201229220542-30ce2eb5d4dc/go.mod h1:c9sxoIT3YgLxH4UhLOCKaBlEojuMhVYpk4Ntv3opUTQ= github.com/apache/thrift v0.0.0-20181112125854-24918abba929/go.mod h1:cp2SuWMxlEZw2r+iP2GNCdIi4C1qmUzdZFSVb+bacwQ= github.com/apache/thrift v0.12.0/go.mod h1:cp2SuWMxlEZw2r+iP2GNCdIi4C1qmUzdZFSVb+bacwQ= github.com/apache/thrift v0.13.0/go.mod h1:cp2SuWMxlEZw2r+iP2GNCdIi4C1qmUzdZFSVb+bacwQ= @@ -188,8 +190,8 @@ github.com/bgentry/speakeasy v0.1.0/go.mod h1:+zsyZBPWlz7T6j88CTgSN5bM796AkVf0kB github.com/bketelsen/crypt v0.0.3-0.20200106085610-5cbc8cc4026c/go.mod h1:MKsuJmJgSg28kpZDP6UIiPt0e0Oz0kqKNGyRaWEPv84= github.com/bkielbasa/cyclop v1.2.0 h1:7Jmnh0yL2DjKfw28p86YTd/B4lRGcNuu12sKE35sM7A= github.com/bkielbasa/cyclop v1.2.0/go.mod h1:qOI0yy6A7dYC4Zgsa72Ppm9kONl0RoIlPbzot9mhmeI= -github.com/blackjack/webcam v0.0.0-20230502173554-3b52e93e8607 h1:KG44gkEm6X8qGbJnv9Ef02OSWYtP0pGnu5Pw8QiWxys= -github.com/blackjack/webcam v0.0.0-20230502173554-3b52e93e8607/go.mod h1:G0X+rEqYPWSq0dG8OMf8M446MtKytzpPjgS3HbdOJZ4= +github.com/blackjack/webcam v0.0.0-20230509180125-87693b3f29dc h1:7cMZ/f4xwkD3FUOcThPAm0uecSP5kSTUU/3RWsrmcww= +github.com/blackjack/webcam v0.0.0-20230509180125-87693b3f29dc/go.mod h1:G0X+rEqYPWSq0dG8OMf8M446MtKytzpPjgS3HbdOJZ4= github.com/blend/go-sdk v1.1.1/go.mod h1:IP1XHXFveOXHRnojRJO7XvqWGqyzevtXND9AdSztAe8= github.com/blizzy78/varnamelen v0.8.0 h1:oqSblyuQvFsW1hbBHh1zfwrKe3kcSj0rnXkKzsQ089M= github.com/blizzy78/varnamelen v0.8.0/go.mod h1:V9TzQZ4fLJ1DSrjVDfl89H7aMnTvKkApdHeyESmyR7k= @@ -235,6 +237,11 @@ github.com/charithe/durationcheck v0.0.9 h1:mPP4ucLrf/rKZiIG/a9IPXHGlh8p4CzgpyTy github.com/charithe/durationcheck v0.0.9/go.mod h1:SSbRIBVfMjCi/kEB6K65XEA83D6prSM8ap1UCpNKtgg= github.com/chavacava/garif v0.0.0-20221024190013-b3ef35877348 h1:cy5GCEZLUCshCGCRRUjxHrDUqkB4l5cuUt3ShEckQEo= github.com/chavacava/garif v0.0.0-20221024190013-b3ef35877348/go.mod h1:f/miWtG3SSuTxKsNK3o58H1xl+XV6ZIfbC6p7lPPB8U= +github.com/chewxy/hm v1.0.0 h1:zy/TSv3LV2nD3dwUEQL2VhXeoXbb9QkpmdRAVUFiA6k= +github.com/chewxy/hm v1.0.0/go.mod h1:qg9YI4q6Fkj/whwHR1D+bOGeF7SniIP40VweVepLjg0= +github.com/chewxy/math32 v1.0.0/go.mod h1:Miac6hA1ohdDUTagnvJy/q+aNnEk16qWUdb8ZVhvCN0= +github.com/chewxy/math32 v1.0.8 h1:fU5E4Ec4Z+5RtRAi3TovSxUjQPkgRh+HbP7tKB2OFbM= +github.com/chewxy/math32 v1.0.8/go.mod h1:dOB2rcuFrCn6UHrze36WSLVPKtzPMRAQvBvUwkSsLqs= github.com/chzyer/logex v1.1.10/go.mod h1:+Ywpsq7O8HXn0nuIou7OrIPyXbp3wmkHB+jjWRnGsAI= github.com/chzyer/readline v0.0.0-20180603132655-2972be24d48e/go.mod h1:nSuG5e5PlCu98SY8svDHJxuZscDgtXS6KTTbou5AhLI= github.com/chzyer/test v0.0.0-20180213035817-a1ea475d72b1/go.mod h1:Q3SI9o4m/ZMnBNeIyt5eFwwo7qiLfzFZmjNmxjkiQlU= @@ -282,6 +289,10 @@ github.com/creack/pty v1.1.19-0.20220421211855-0d412c9fbeb1 h1:Tw0uuY+3UWYiSbR0+ github.com/creack/pty v1.1.19-0.20220421211855-0d412c9fbeb1/go.mod h1:MOBLtS5ELjhRRrroQr9kyvTxUAFNvYEK993ew/Vr4O4= github.com/curioswitch/go-reassign v0.2.0 h1:G9UZyOcpk/d7Gd6mqYgd8XYWFMw/znxwGDUstnC9DIo= github.com/curioswitch/go-reassign v0.2.0/go.mod h1:x6OpXuWvgfQaMGks2BZybTngWjT84hqJfKoO8Tt/Roc= +github.com/d2r2/go-i2c v0.0.0-20191123181816-73a8a799d6bc h1:HLRSIWzUGMLCq4ldt0W1GLs3nnAxa5EGoP+9qHgh6j0= +github.com/d2r2/go-i2c v0.0.0-20191123181816-73a8a799d6bc/go.mod h1:AwxDPnsgIpy47jbGXZHA9Rv7pDkOJvQbezPuK1Y+nNk= +github.com/d2r2/go-logger v0.0.0-20210606094344-60e9d1233e22 h1:nO+SY4KOMsF/LsZ5EtbSKhiT3M6sv/igo2PEru/xEHI= +github.com/d2r2/go-logger v0.0.0-20210606094344-60e9d1233e22/go.mod h1:eSx+YfcVy5vCjRZBNIhpIpfCGFMQ6XSOSQkDk7+VCpg= github.com/daixiang0/gci v0.2.8/go.mod h1:+4dZ7TISfSmqfAGv59ePaHfNzgGtIkHAhhdKggP1JAc= github.com/daixiang0/gci v0.9.1 h1:jBrwBmBZTDsGsXiaCTLIe9diotp1X4X64zodFrh7l+c= github.com/daixiang0/gci v0.9.1/go.mod h1:EpVfrztufwVgQRXjnX4zuNinEpLj5OmMjtu/+MB0V0c= @@ -596,6 +607,7 @@ github.com/google/btree v0.0.0-20180813153112-4030bb1f1f0c/go.mod h1:lNA+9X1NB3Z github.com/google/btree v1.0.0/go.mod h1:lNA+9X1NB3Zf8V7Ke586lFgjr2dZNuvo3lPJSGZ5JPQ= github.com/google/certificate-transparency-go v1.0.21/go.mod h1:QeJfpSbVSfYc7RgB3gJFj9cbuQMMchQxrWXz8Ruopmg= github.com/google/certificate-transparency-go v1.1.1/go.mod h1:FDKqPvSXawb2ecErVRrD+nfy23RCzyl7eqVCEmlT1Zs= +github.com/google/flatbuffers v1.11.0/go.mod h1:1AeVuKshWv4vARoZatz6mlQ0JxURH0Kv5+zNeJKJCa8= github.com/google/flatbuffers v2.0.6+incompatible h1:XHFReMv7nFFusa+CEokzWbzaYocKXI6C7hdU5Kgh9Lw= github.com/google/flatbuffers v2.0.6+incompatible/go.mod h1:1AeVuKshWv4vARoZatz6mlQ0JxURH0Kv5+zNeJKJCa8= github.com/google/go-cmp v0.2.0/go.mod h1:oXzfMopK8JAjlY9xF4vHSVASa0yLyX7SntLO5aqRK0M= @@ -799,7 +811,6 @@ github.com/jmoiron/sqlx v1.2.0/go.mod h1:1FEQNm3xlJgrMD+FBdI9+xvCksHtbpVBBw5dYhB github.com/jonboulle/clockwork v0.1.0/go.mod h1:Ii8DK3G1RaLaWxj9trq07+26W01tbo22gdxWY5EU2bo= github.com/jonboulle/clockwork v0.2.0/go.mod h1:Pkfl5aHPm1nk2H9h0bjmnJD/BcgbGXUBGnn1kMkgxc8= github.com/jonboulle/clockwork v0.3.0 h1:9BSCMi8C+0qdApAp4auwX0RkLGUjs956h0EkuQymUhg= -github.com/jonboulle/clockwork v0.3.0/go.mod h1:Pkfl5aHPm1nk2H9h0bjmnJD/BcgbGXUBGnn1kMkgxc8= github.com/jpillora/backoff v1.0.0/go.mod h1:J/6gKK9jxlEcS3zixgDgUAsiuZ7yrSoa/FX5e0EB2j4= github.com/json-iterator/go v1.1.6/go.mod h1:+SdeFBvtyEkXs7REEP0seUULqWtbJapLOCVDaaPEHmU= github.com/json-iterator/go v1.1.7/go.mod h1:KdQUCv79m/52Kvf8AW2vK1V8akMuk1QjK/uOdHXbAo4= @@ -1130,11 +1141,11 @@ github.com/pierrec/lz4 v2.0.5+incompatible/go.mod h1:pdkljMzZIN41W+lC3N2tnIh5sFi github.com/pierrec/xxHash v0.1.1/go.mod h1:w2waW5Zoa/Wc4Yqe0wgrIYAGKqRMf7czn2HNKXmuL+I= github.com/pion/datachannel v1.5.5 h1:10ef4kwdjije+M9d7Xm9im2Y3O6A6ccQb0zcqZcJew8= github.com/pion/datachannel v1.5.5/go.mod h1:iMz+lECmfdCMqFRhXhcA/219B0SQlbpoR2V118yimL0= -github.com/pion/dtls/v2 v2.2.6/go.mod h1:t8fWJCIquY5rlQZwA2yWxUS1+OCrAdXrhVKXB5oD/wY= github.com/pion/dtls/v2 v2.2.7 h1:cSUBsETxepsCSFSxC3mc/aDo14qQLMSL+O6IjG28yV8= github.com/pion/dtls/v2 v2.2.7/go.mod h1:8WiMkebSHFD0T+dIU+UeBaoV7kDhOW5oDCzZ7WZ/F9s= -github.com/pion/ice/v2 v2.3.5 h1:Y8D/ZT+H4mo2DOl3YbCz5+FoXlYSYnziSJWWcvy4oOU= -github.com/pion/ice/v2 v2.3.5/go.mod h1:MloU1ypbcE3WBbETCkGw6rx9RcfeHdaBqsaB08R2tlI= +github.com/pion/ice/v2 v2.3.8/go.mod h1:DoMA9FvsfNTBVnjyRf2t4EhUkSp9tNrH77fMtPFYygQ= +github.com/pion/ice/v2 v2.3.9 h1:7yZpHf3PhPxJGT4JkMj1Y8Rl5cQ6fB709iz99aeMd/U= +github.com/pion/ice/v2 v2.3.9/go.mod h1:lT3kv5uUIlHfXHU/ZRD7uKD/ufM202+eTa3C/umgGf4= github.com/pion/interceptor v0.1.17 h1:prJtgwFh/gB8zMqGZoOgJPHivOwVAp61i2aG61Du/1w= github.com/pion/interceptor v0.1.17/go.mod h1:SY8kpmfVBvrbUzvj2bsXz7OJt5JvmVNZ+4Kjq7FcwrI= github.com/pion/logging v0.2.2 h1:M9+AIj/+pxNsDfAT64+MAVgJO0rsyLnoJKCqf//DoeY= @@ -1142,8 +1153,8 @@ github.com/pion/logging v0.2.2/go.mod h1:k0/tDVsRCX2Mb2ZEmTqNa7CWsQPc+YYCB7Q+5pa github.com/pion/mdns v0.0.7/go.mod h1:4iP2UbeFhLI/vWju/bw6ZfwjJzk0z8DNValjGxR/dD8= github.com/pion/mdns v0.0.8-0.20230502060824-17c664ea7d5c h1:yuxunGfPeXnWCP9ke+iS2nq7K/q+17ynSgOeLkd5f20= github.com/pion/mdns v0.0.8-0.20230502060824-17c664ea7d5c/go.mod h1:658EdZmbrzmxLVC2qYP6pRgHtZTAzMoLadjBUVetgH4= -github.com/pion/mediadevices v0.4.1-0.20230605163757-e64f0d8697f9 h1:nJ6sDIa0Z8uQG6G9f8MRIFYBxLBOPzuAw72wS9cCVyI= -github.com/pion/mediadevices v0.4.1-0.20230605163757-e64f0d8697f9/go.mod h1:3KYjLNRU8ZcYpNB+zcUMd2g3aEZyD/jzPFKnwkQZiqI= +github.com/pion/mediadevices v0.5.1-0.20230724160738-03c44ee80347 h1:JOP2KHuMtMz5E0+3wlhahETaHOH0YpV9fLDe3PJtGFo= +github.com/pion/mediadevices v0.5.1-0.20230724160738-03c44ee80347/go.mod h1:HeL/EIzoN/E2Qj9viyRvQoVjyOOa2xAXQGx33syV1mE= github.com/pion/randutil v0.1.0 h1:CFG1UdESneORglEsnimhUjf33Rwjubwj6xfiOXBa3mA= github.com/pion/randutil v0.1.0/go.mod h1:XcJrSMMbbMRhASFVOlj/5hQial/Y8oH/HVo7TBZq+j8= github.com/pion/rtcp v1.2.10 h1:nkr3uj+8Sp97zyItdN60tE/S6vk4al5CPRR6Gejsdjc= @@ -1158,22 +1169,21 @@ github.com/pion/sdp/v3 v3.0.6/go.mod h1:iiFWFpQO8Fy3S5ldclBkpXqmWy02ns78NOKoLLL0 github.com/pion/srtp/v2 v2.0.15 h1:+tqRtXGsGwHC0G0IUIAzRmdkHvriF79IHVfZGfHrQoA= github.com/pion/srtp/v2 v2.0.15/go.mod h1:b/pQOlDrbB0HEH5EUAQXzSYxikFbNcNuKmF8tM0hCtw= github.com/pion/stun v0.4.0/go.mod h1:QPsh1/SbXASntw3zkkrIk3ZJVKz4saBY2G7S10P3wCw= -github.com/pion/stun v0.5.2/go.mod h1:TNo1HjyjaFVpMZsvowqPeV8TfwRytympQC0//neaksA= -github.com/pion/stun v0.6.0 h1:JHT/2iyGDPrFWE8NNC15wnddBN8KifsEDw8swQmrEmU= github.com/pion/stun v0.6.0/go.mod h1:HPqcfoeqQn9cuaet7AOmB5e5xkObu9DwBdurwLKO9oA= +github.com/pion/stun v0.6.1 h1:8lp6YejULeHBF8NmV8e2787BogQhduZugh5PdhDyyN4= +github.com/pion/stun v0.6.1/go.mod h1:/hO7APkX4hZKu/D0f2lHzNyvdkTGtIy3NDmLR7kSz/8= github.com/pion/transport v0.14.1 h1:XSM6olwW+o8J4SCmOBb/BpwZypkHeyM0PGFCxNQBr40= github.com/pion/transport v0.14.1/go.mod h1:4tGmbk00NeYA3rUa9+n+dzCCoKkcy3YlYb99Jn2fNnI= github.com/pion/transport/v2 v2.0.0/go.mod h1:HS2MEBJTwD+1ZI2eSXSvHJx/HnzQqRy2/LXxt6eVMHc= -github.com/pion/transport/v2 v2.0.2/go.mod h1:vrz6bUbFr/cjdwbnxq8OdDDzHf7JJfGsIRkxfpZoTA0= github.com/pion/transport/v2 v2.1.0/go.mod h1:AdSw4YBZVDkZm8fpoz+fclXyQwANWmZAlDuQdctTThQ= github.com/pion/transport/v2 v2.2.0/go.mod h1:AdSw4YBZVDkZm8fpoz+fclXyQwANWmZAlDuQdctTThQ= github.com/pion/transport/v2 v2.2.1 h1:7qYnCBlpgSJNYMbLCKuSY9KbQdBFoETvPNETv0y4N7c= github.com/pion/transport/v2 v2.2.1/go.mod h1:cXXWavvCnFF6McHTft3DWS9iic2Mftcz1Aq29pGcU5g= -github.com/pion/turn/v2 v2.1.0 h1:5wGHSgGhJhP/RpabkUb/T9PdsAjkGLS6toYz5HNzoSI= github.com/pion/turn/v2 v2.1.0/go.mod h1:yrT5XbXSGX1VFSF31A3c1kCNB5bBZgk/uu5LET162qs= -github.com/pion/udp/v2 v2.0.1/go.mod h1:B7uvTMP00lzWdyMr/1PVZXtV3wpPIxBRd4Wl6AksXn8= -github.com/pion/webrtc/v3 v3.2.6 h1:6pHQacytdvCbWSJT1IHMi6d67FuAEu9jEQ98GqKNgJU= -github.com/pion/webrtc/v3 v3.2.6/go.mod h1:+//AwYJLlhHRyoNyuRUBs7Pw1jyZHCxQCuu2ZUNSBaw= +github.com/pion/turn/v2 v2.1.2 h1:wj0cAoGKltaZ790XEGW9HwoUewqjliwmhtxCuB2ApyM= +github.com/pion/turn/v2 v2.1.2/go.mod h1:1kjnPkBcex3dhCU2Am+AAmxDcGhLX3WnMfmkNpvSTQU= +github.com/pion/webrtc/v3 v3.2.11 h1:lfGKYZcG7ghCTQWn+zsD+icIIWL3qIfclEjBGk537+s= +github.com/pion/webrtc/v3 v3.2.11/go.mod h1:fejQio1v8tKG4ntq4u8H4uDHsCNX6eX7bT093t4H+0E= github.com/pkg/browser v0.0.0-20210911075715-681adbf594b8 h1:KoWmjvw+nsYOo29YJK9vDA65RGE3NrOnUtO7a+RF9HU= github.com/pkg/browser v0.0.0-20210911075715-681adbf594b8/go.mod h1:HKlIX3XHQyzLZPlr7++PzdhaXEj94dEiJgZDTsxEqUI= github.com/pkg/errors v0.8.0/go.mod h1:bwawxfHBFNV+L2hUp1rHADufV3IMtnDRdf1r5NINEl0= @@ -1391,6 +1401,7 @@ github.com/stretchr/objx v0.5.0 h1:1zr/of2m5FGMsad5YfcqgdqdWrIhu+EBEJRhR1U7z/c= github.com/stretchr/objx v0.5.0/go.mod h1:Yh+to48EsGEfYuaHDzXPcE3xhTkx73EhmCGUpEOglKo= github.com/stretchr/testify v0.0.0-20170130113145-4d4bfba8f1d1/go.mod h1:a8OnRcib4nhh0OaRAV+Yts87kKdq0PP7pXfy6kDkUVs= github.com/stretchr/testify v1.1.4/go.mod h1:a8OnRcib4nhh0OaRAV+Yts87kKdq0PP7pXfy6kDkUVs= +github.com/stretchr/testify v1.2.0/go.mod h1:a8OnRcib4nhh0OaRAV+Yts87kKdq0PP7pXfy6kDkUVs= github.com/stretchr/testify v1.2.2/go.mod h1:a8OnRcib4nhh0OaRAV+Yts87kKdq0PP7pXfy6kDkUVs= github.com/stretchr/testify v1.3.0/go.mod h1:M5WIy9Dh21IEIfnGCwXGc5bZfKNJtfHm1UVUgZn+9EI= github.com/stretchr/testify v1.3.1-0.20190311161405-34c6fa2dc709/go.mod h1:M5WIy9Dh21IEIfnGCwXGc5bZfKNJtfHm1UVUgZn+9EI= @@ -1473,8 +1484,8 @@ github.com/viam-labs/go-libjpeg v0.3.1 h1:J/byavXHFqRI1PFPrnPbP+wFCr1y+Cn1CwKXrO github.com/viam-labs/go-libjpeg v0.3.1/go.mod h1:b0ISpf9lJv9MO1h1gXAmSA/osG19cKGYjfYc6aeEjqs= github.com/viamrobotics/evdev v0.1.3 h1:mR4HFafvbc5Wx4Vp1AUJp6/aITfVx9AKyXWx+rWjpfc= github.com/viamrobotics/evdev v0.1.3/go.mod h1:N6nuZmPz7HEIpM7esNWwLxbYzqWqLSZkfI/1Sccckqk= -github.com/viamrobotics/gostream v0.0.0-20230609200515-c5d67c29ed25 h1:U6dSI2rmUFtX3/gzZNTnLUKZZAxFIbn022xd60y6Aq0= -github.com/viamrobotics/gostream v0.0.0-20230609200515-c5d67c29ed25/go.mod h1:IIA5PHjXhVFVM8W/kYtU0030A90Q1QHwyIuwAkpLmz4= +github.com/viamrobotics/gostream v0.0.0-20230725145737-ed58004e202e h1:j7vCOikvAE3AxaFUtTfvRS9u6Agmvc5HbxI/TVzNEkg= +github.com/viamrobotics/gostream v0.0.0-20230725145737-ed58004e202e/go.mod h1:XbqZO/IuYMHmn2L0QWbEwiytLh20nj6gWIeoEHMWLo8= github.com/viki-org/dnscache v0.0.0-20130720023526-c70c1f23c5d8/go.mod h1:dniwbG03GafCjFohMDmz6Zc6oCuiqgH6tGNyXTkHzXE= github.com/wcharczuk/go-chart v2.0.1+incompatible/go.mod h1:PF5tmL4EIx/7Wf+hEkpCqYi5He4u90sw+0+6FhrryuE= github.com/wcharczuk/go-chart/v2 v2.1.0/go.mod h1:yx7MvAVNcP/kN9lKXM/NTce4au4DFN99j6i1OwDclNA= @@ -1497,6 +1508,8 @@ github.com/xitongsys/parquet-go-source v0.0.0-20200509081216-8db33acb0acf/go.mod github.com/xordataexchange/crypt v0.0.3-0.20170626215501-b2862e3d0a77/go.mod h1:aYKd//L2LvnjZzWKhF00oedf4jCCReLcmhLdhm1A27Q= github.com/xrash/smetrics v0.0.0-20201216005158-039620a65673 h1:bAn7/zixMGCfxrRTfdpNzjtPYqr8smhKouy9mxVdGPU= github.com/xrash/smetrics v0.0.0-20201216005158-039620a65673/go.mod h1:N3UwUGtsrSj3ccvlPHLoLsHnpR27oXr4ZE984MbSER8= +github.com/xtgo/set v1.0.0 h1:6BCNBRv3ORNDQ7fyoJXRv+tstJz3m1JVFQErfeZz2pY= +github.com/xtgo/set v1.0.0/go.mod h1:d3NHzGzSa0NmB2NhFyECA+QdRp29oEn2xbT+TpeFoM8= github.com/yagipy/maintidx v1.0.0 h1:h5NvIsCz+nRDapQ0exNv4aJ0yXSI0420omVANTv3GJM= github.com/yagipy/maintidx v1.0.0/go.mod h1:0qNf/I/CCZXSMhsRsrEPDZ+DkekpKLXAJfsTACwgXLk= github.com/yeya24/promlinter v0.2.0 h1:xFKDQ82orCU5jQujdaD8stOHiv8UN68BSdn2a8u8Y3o= @@ -1569,13 +1582,15 @@ go.uber.org/zap v1.18.1/go.mod h1:xg/QME4nWcxGxrpdeYfq7UvYrLh66cuVKdrbD1XF/NI= go.uber.org/zap v1.23.0/go.mod h1:D+nX8jyLsMHMYrln8A0rJjFt/T/9/bGgIhAqxv5URuY= go.uber.org/zap v1.24.0 h1:FiJd5l1UOLj0wCgbSE0rwwXHzEdAZS6hiiSnxJN/D60= go.uber.org/zap v1.24.0/go.mod h1:2kMP+WWQ8aoFoedH3T2sq6iJ2yDWpHbP0f6MQbS9Gkg= -go.viam.com/api v0.1.143 h1:P3f4it8ZtHPM+txJZ6f4s14H70hA9S/KC017DUs2yUM= -go.viam.com/api v0.1.143/go.mod h1:CwLz82Ie4+Z2lSH2F0oQGViP4/TV9uxjJs+rqHvFWE8= +go.viam.com/api v0.1.186 h1:yilwFO70Xbg2vtpMS7Nni6w/HV/0LrOiBg5fNzWbjpc= +go.viam.com/api v0.1.186/go.mod h1:CwLz82Ie4+Z2lSH2F0oQGViP4/TV9uxjJs+rqHvFWE8= go.viam.com/test v1.1.1-0.20220913152726-5da9916c08a2 h1:oBiK580EnEIzgFLU4lHOXmGAE3MxnVbeR7s1wp/F3Ps= go.viam.com/test v1.1.1-0.20220913152726-5da9916c08a2/go.mod h1:XM0tej6riszsiNLT16uoyq1YjuYPWlRBweTPRDanIts= -go.viam.com/utils v0.1.37 h1:AQgpbiHahZbcDqMAxfB1HYeH2txyQ+lP/Ob02i+sFOM= -go.viam.com/utils v0.1.37/go.mod h1:tjPInze4C0UYFRqL/FU96yqhJpHR1zjiNZ7qChTN/b8= +go.viam.com/utils v0.1.43 h1:ZNEqvicTBXJ/CGh897vNk6MVzGi98BOzdxTenFF0pv8= +go.viam.com/utils v0.1.43/go.mod h1:tjPInze4C0UYFRqL/FU96yqhJpHR1zjiNZ7qChTN/b8= go4.org v0.0.0-20180809161055-417644f6feb5/go.mod h1:MkTOUMDaeVYJUOUsaDXIhWPZYa1yOyC1qaOBpL57BhE= +go4.org/unsafe/assume-no-moving-gc v0.0.0-20230525183740-e7c30c78aeb2 h1:WJhcL4p+YeDxmZWg141nRm7XC8IDmhz7lk5GpadO1Sg= +go4.org/unsafe/assume-no-moving-gc v0.0.0-20230525183740-e7c30c78aeb2/go.mod h1:FftLjUGFEDu5k8lt0ddY+HcrH/qU/0qk+H8j9/nTl3E= gocv.io/x/gocv v0.25.0/go.mod h1:Rar2PS6DV+T4FL+PM535EImD/h13hGVaHhnCu1xarBs= goji.io v2.0.2+incompatible h1:uIssv/elbKRLznFUy3Xj4+2Mz/qKhek/9aZQDUMae7c= goji.io v2.0.2+incompatible/go.mod h1:sbqFwrtqZACxLBTQcdgVjFh54yGVCvwq8+w49MVMMIk= @@ -1603,10 +1618,10 @@ golang.org/x/crypto v0.0.0-20211108221036-ceb1ce70b4fa/go.mod h1:GvvjBRRGRdwPK5y golang.org/x/crypto v0.0.0-20220427172511-eb4f295cb31f/go.mod h1:IxCIyHEi3zRg3s0A5j5BB6A9Jmi73HwBIUl50j+osU4= golang.org/x/crypto v0.0.0-20220622213112-05595931fe9d/go.mod h1:IxCIyHEi3zRg3s0A5j5BB6A9Jmi73HwBIUl50j+osU4= golang.org/x/crypto v0.1.0/go.mod h1:RecgLatLF4+eUMCP1PoPZQb+cVrJcOPbHkTkbkB9sbw= -golang.org/x/crypto v0.5.0/go.mod h1:NK/OQwhpMQP3MwtdjgLlYHnH9ebylxKWv3e0fK+mkQU= golang.org/x/crypto v0.8.0/go.mod h1:mRqEX+O9/h5TFCrQhkgjo2yKi0yYA+9ecGkdQoHrywE= -golang.org/x/crypto v0.9.0 h1:LF6fAI+IutBocDJ2OT0Q1g8plpYljMZ4+lty+dsqw3g= golang.org/x/crypto v0.9.0/go.mod h1:yrmDGqONDYtNj3tH8X9dzUun2m2lzPa9ngI6/RUPGR0= +golang.org/x/crypto v0.10.0 h1:LKqV2xt9+kDzSTfOhx4FrkEBcMrAgHSYgzywV9zcGmM= +golang.org/x/crypto v0.10.0/go.mod h1:o4eNf7Ede1fv+hwOwZsTHl9EsPFO6q6ZvYR8vYfY45I= golang.org/x/exp v0.0.0-20180321215751-8460e604b9de/go.mod h1:CJ0aWSM057203Lf6IL+f9T1iT9GByDxfZKAQTCR3kQA= golang.org/x/exp v0.0.0-20180807140117-3d87b88a115f/go.mod h1:CJ0aWSM057203Lf6IL+f9T1iT9GByDxfZKAQTCR3kQA= golang.org/x/exp v0.0.0-20190121172915-509febef88a4/go.mod h1:CJ0aWSM057203Lf6IL+f9T1iT9GByDxfZKAQTCR3kQA= @@ -1635,8 +1650,8 @@ golang.org/x/image v0.0.0-20191009234506-e7c1f5e7dbb8/go.mod h1:FeLwcggjj3mMvU+o golang.org/x/image v0.0.0-20200927104501-e162460cd6b5/go.mod h1:FeLwcggjj3mMvU+oOTbSwawSJRM1uh48EjtB4UJZlP0= golang.org/x/image v0.0.0-20210607152325-775e3b0c77b9/go.mod h1:023OzeP/+EPmXeapQh35lcL3II3LrY8Ic+EFFKVhULM= golang.org/x/image v0.0.0-20211028202545-6944b10bf410/go.mod h1:023OzeP/+EPmXeapQh35lcL3II3LrY8Ic+EFFKVhULM= -golang.org/x/image v0.7.0 h1:gzS29xtG1J5ybQlv0PuyfE3nmc6R4qB73m6LUUmvFuw= -golang.org/x/image v0.7.0/go.mod h1:nd/q4ef1AKKYl/4kft7g+6UyGbdiqWqTP1ZAbRoV7Rg= +golang.org/x/image v0.8.0 h1:agUcRXV/+w6L9ryntYYsF2x9fQTMd4T8fiiYXAVW6Jg= +golang.org/x/image v0.8.0/go.mod h1:PwLxp3opCYg4WR2WO9P0L6ESnsD6bLTWcw8zanLMVFM= golang.org/x/lint v0.0.0-20181026193005-c67002cb31c3/go.mod h1:UVdnD1Gm6xHRNCYTkRU2/jEulfH38KcIWyp/GAMgvoE= golang.org/x/lint v0.0.0-20190227174305-5b3e6a55c961/go.mod h1:wehouNa3lNwaWXcvxsM5YxQ5yQlVC4a0KAMCusXpPoU= golang.org/x/lint v0.0.0-20190301231843-5614ed5bae6f/go.mod h1:UVdnD1Gm6xHRNCYTkRU2/jEulfH38KcIWyp/GAMgvoE= @@ -1708,6 +1723,7 @@ golang.org/x/net v0.0.0-20200602114024-627f9648deb9/go.mod h1:qpuaurCH72eLCgpAm/ golang.org/x/net v0.0.0-20200625001655-4c5254603344/go.mod h1:/O7V0waA8r7cgGh81Ro3o1hOxt32SMVPicZroKQ2sZA= golang.org/x/net v0.0.0-20200707034311-ab3426394381/go.mod h1:/O7V0waA8r7cgGh81Ro3o1hOxt32SMVPicZroKQ2sZA= golang.org/x/net v0.0.0-20200822124328-c89045814202/go.mod h1:/O7V0waA8r7cgGh81Ro3o1hOxt32SMVPicZroKQ2sZA= +golang.org/x/net v0.0.0-20200904194848-62affa334b73/go.mod h1:/O7V0waA8r7cgGh81Ro3o1hOxt32SMVPicZroKQ2sZA= golang.org/x/net v0.0.0-20201021035429-f5854403a974/go.mod h1:sp8m0HH+o8qH0wwXwYZr8TS3Oi6o0r6Gce1SSxlDquU= golang.org/x/net v0.0.0-20201031054903-ff519b6c9102/go.mod h1:sp8m0HH+o8qH0wwXwYZr8TS3Oi6o0r6Gce1SSxlDquU= golang.org/x/net v0.0.0-20201110031124-69a78807bb2b/go.mod h1:sp8m0HH+o8qH0wwXwYZr8TS3Oi6o0r6Gce1SSxlDquU= @@ -1731,11 +1747,11 @@ golang.org/x/net v0.2.0/go.mod h1:KqCZLdyyvdV855qA2rE3GC2aiw5xGR5TEjj8smXukLY= golang.org/x/net v0.3.0/go.mod h1:MBQ8lrhLObU/6UmLb4fmbmk5OcyYmqtbGd/9yIeKjEE= golang.org/x/net v0.5.0/go.mod h1:DivGGAXEgPSlEBzxGzZI+ZLohi+xUj054jfeKui00ws= golang.org/x/net v0.6.0/go.mod h1:2Tu9+aMcznHK/AK1HMvgo6xiTLG5rD5rZLDS+rp2Bjs= -golang.org/x/net v0.7.0/go.mod h1:2Tu9+aMcznHK/AK1HMvgo6xiTLG5rD5rZLDS+rp2Bjs= golang.org/x/net v0.8.0/go.mod h1:QVkue5JL9kW//ek3r6jTKnTFis1tRmNAW2P1shuFdJc= golang.org/x/net v0.9.0/go.mod h1:d48xBJpPfHeWQsugry2m+kC02ZBRGRgulfHnEXEuWns= -golang.org/x/net v0.10.0 h1:X2//UzNDwYmtCLn7To6G58Wr6f5ahEAQgKNzv9Y951M= golang.org/x/net v0.10.0/go.mod h1:0qNGK6F8kojg2nk9dLZ2mShWaEBan6FAoqfSigmmuDg= +golang.org/x/net v0.11.0 h1:Gi2tvZIJyBtO9SDr1q9h5hEQCp/4L2RQ+ar0qjx2oNU= +golang.org/x/net v0.11.0/go.mod h1:2L/ixqYpgIVXmeoSA/4Lu7BzTG4KIyPIryS4IsOd1oQ= golang.org/x/oauth2 v0.0.0-20180821212333-d2e6202438be/go.mod h1:N/0e6XlmueqKjAGxoOufVs8QHGRruUQn6yWY3a++T0U= golang.org/x/oauth2 v0.0.0-20181203162652-d668ce993890/go.mod h1:N/0e6XlmueqKjAGxoOufVs8QHGRruUQn6yWY3a++T0U= golang.org/x/oauth2 v0.0.0-20190226205417-e64efc72b421/go.mod h1:gOpvHmFTYa4IltrdGE7lF6nIHvwfUNPOp7c8zoXwtLw= @@ -1820,6 +1836,7 @@ golang.org/x/sys v0.0.0-20200615200032-f1bc736245b1/go.mod h1:h1NjWce9XRLGQEsW7w golang.org/x/sys v0.0.0-20200625212154-ddb9806d33ae/go.mod h1:h1NjWce9XRLGQEsW7wpKNCjG9DtNlClVuFLEZdDNbEs= golang.org/x/sys v0.0.0-20200803210538-64077c9b5642/go.mod h1:h1NjWce9XRLGQEsW7wpKNCjG9DtNlClVuFLEZdDNbEs= golang.org/x/sys v0.0.0-20200905004654-be1d3432aa8f/go.mod h1:h1NjWce9XRLGQEsW7wpKNCjG9DtNlClVuFLEZdDNbEs= +golang.org/x/sys v0.0.0-20200909081042-eff7692f9009/go.mod h1:h1NjWce9XRLGQEsW7wpKNCjG9DtNlClVuFLEZdDNbEs= golang.org/x/sys v0.0.0-20200930185726-fdedc70b468f/go.mod h1:h1NjWce9XRLGQEsW7wpKNCjG9DtNlClVuFLEZdDNbEs= golang.org/x/sys v0.0.0-20201018230417-eeed37f84f13/go.mod h1:h1NjWce9XRLGQEsW7wpKNCjG9DtNlClVuFLEZdDNbEs= golang.org/x/sys v0.0.0-20201024232916-9f70ab9862d5/go.mod h1:h1NjWce9XRLGQEsW7wpKNCjG9DtNlClVuFLEZdDNbEs= @@ -1863,8 +1880,9 @@ golang.org/x/sys v0.4.0/go.mod h1:oPkhp1MJrh7nUepCBck5+mAzfO9JrbApNNgaTdGDITg= golang.org/x/sys v0.5.0/go.mod h1:oPkhp1MJrh7nUepCBck5+mAzfO9JrbApNNgaTdGDITg= golang.org/x/sys v0.6.0/go.mod h1:oPkhp1MJrh7nUepCBck5+mAzfO9JrbApNNgaTdGDITg= golang.org/x/sys v0.7.0/go.mod h1:oPkhp1MJrh7nUepCBck5+mAzfO9JrbApNNgaTdGDITg= -golang.org/x/sys v0.8.0 h1:EBmGv8NaZBZTWvrbjNoL6HVt+IVy3QDQpJs7VRIw3tU= golang.org/x/sys v0.8.0/go.mod h1:oPkhp1MJrh7nUepCBck5+mAzfO9JrbApNNgaTdGDITg= +golang.org/x/sys v0.9.0 h1:KS/R3tvhPqvJvwcKfnBHJwwthS11LRhmM5D59eEXa0s= +golang.org/x/sys v0.9.0/go.mod h1:oPkhp1MJrh7nUepCBck5+mAzfO9JrbApNNgaTdGDITg= golang.org/x/term v0.0.0-20201117132131-f5c789dd3221/go.mod h1:Nr5EML6q2oocZ2LXRh80K7BxOlk5/8JxuGnuhpl+muw= golang.org/x/term v0.0.0-20201126162022-7de9c90e9dd1/go.mod h1:bj7SfCRtBDWHUb9snDiAeCFNEtKQo2Wmx5Cou7ajbmo= golang.org/x/term v0.0.0-20210927222741-03fcf44c2211/go.mod h1:jbD1KX2456YbFQfuXm/mYQcufACuNUgVhRMnK/tPxf8= @@ -1876,8 +1894,9 @@ golang.org/x/term v0.4.0/go.mod h1:9P2UbLfCdcvo3p/nzKvsmas4TnlujnuoV9hGgYzW1lQ= golang.org/x/term v0.5.0/go.mod h1:jMB1sMXY+tzblOD4FWmEbocvup2/aLOaQEp7JmGp78k= golang.org/x/term v0.6.0/go.mod h1:m6U89DPEgQRMq3DNkDClhWw02AUbt2daBVO4cn4Hv9U= golang.org/x/term v0.7.0/go.mod h1:P32HKFT3hSsZrRxla30E9HqToFYAQPCMs/zFMBUFqPY= -golang.org/x/term v0.8.0 h1:n5xxQn2i3PC0yLAbjTpNT85q/Kgzcr2gIoX9OrJUols= golang.org/x/term v0.8.0/go.mod h1:xPskH00ivmX89bAKVGSKKtLOWNx2+17Eiy94tnKShWo= +golang.org/x/term v0.9.0 h1:GRRCnKYhdQrD8kfRAdQ6Zcw1P0OcELxGLKJvtjVMZ28= +golang.org/x/term v0.9.0/go.mod h1:M6DEAAIenWoTxdKrOltXcmDY3rSplQUkrvaDU5FcQyo= golang.org/x/text v0.0.0-20170915032832-14c0d48ead0c/go.mod h1:NqM8EUOU14njkJ3fqMW+pc6Ldnwhi/IjpwHt7yyuwOQ= golang.org/x/text v0.3.0/go.mod h1:NqM8EUOU14njkJ3fqMW+pc6Ldnwhi/IjpwHt7yyuwOQ= golang.org/x/text v0.3.1-0.20180807135948-17ff2d5776d2/go.mod h1:NqM8EUOU14njkJ3fqMW+pc6Ldnwhi/IjpwHt7yyuwOQ= @@ -1893,8 +1912,9 @@ golang.org/x/text v0.5.0/go.mod h1:mrYo+phRRbMaCq/xk9113O4dZlRixOauAjOtrjsXDZ8= golang.org/x/text v0.6.0/go.mod h1:mrYo+phRRbMaCq/xk9113O4dZlRixOauAjOtrjsXDZ8= golang.org/x/text v0.7.0/go.mod h1:mrYo+phRRbMaCq/xk9113O4dZlRixOauAjOtrjsXDZ8= golang.org/x/text v0.8.0/go.mod h1:e1OnstbJyHTd6l/uOt8jFFHp6TRDWZR/bV3emEE/zU8= -golang.org/x/text v0.9.0 h1:2sjJmO8cDvYveuX97RDLsxlyUxLl+GHoLxBiRdHllBE= golang.org/x/text v0.9.0/go.mod h1:e1OnstbJyHTd6l/uOt8jFFHp6TRDWZR/bV3emEE/zU8= +golang.org/x/text v0.10.0 h1:UpjohKhiEgNc0CSauXmwYftY1+LlaC75SJwh0SgCX58= +golang.org/x/text v0.10.0/go.mod h1:TvPlkZtksWOMsz7fbANvkp4WM8x/WCo/om8BMLbz+aE= golang.org/x/time v0.0.0-20180412165947-fbb02b2291d2/go.mod h1:tRJNPiyCQ0inRvYxbN9jk5I+vvW/OXSQhTDSoE431IQ= golang.org/x/time v0.0.0-20181108054448-85acf8d2951c/go.mod h1:tRJNPiyCQ0inRvYxbN9jk5I+vvW/OXSQhTDSoE431IQ= golang.org/x/time v0.0.0-20190308202827-9d24e82272b4/go.mod h1:tRJNPiyCQ0inRvYxbN9jk5I+vvW/OXSQhTDSoE431IQ= @@ -2106,6 +2126,7 @@ google.golang.org/genproto v0.0.0-20200729003335-053ba62fc06f/go.mod h1:FWY/as6D google.golang.org/genproto v0.0.0-20200804131852-c06518451d9c/go.mod h1:FWY/as6DDZQgahTzZj3fqbO1CbirC29ZNUFHwi0/+no= google.golang.org/genproto v0.0.0-20200825200019-8632dd797987/go.mod h1:FWY/as6DDZQgahTzZj3fqbO1CbirC29ZNUFHwi0/+no= google.golang.org/genproto v0.0.0-20200904004341-0bd0a958aa1d/go.mod h1:FWY/as6DDZQgahTzZj3fqbO1CbirC29ZNUFHwi0/+no= +google.golang.org/genproto v0.0.0-20200911024640-645f7a48b24f/go.mod h1:FWY/as6DDZQgahTzZj3fqbO1CbirC29ZNUFHwi0/+no= google.golang.org/genproto v0.0.0-20201109203340-2640f1f9cdfb/go.mod h1:FWY/as6DDZQgahTzZj3fqbO1CbirC29ZNUFHwi0/+no= google.golang.org/genproto v0.0.0-20201201144952-b05cb90ed32e/go.mod h1:FWY/as6DDZQgahTzZj3fqbO1CbirC29ZNUFHwi0/+no= google.golang.org/genproto v0.0.0-20201210142538-e3217bee35cc/go.mod h1:FWY/as6DDZQgahTzZj3fqbO1CbirC29ZNUFHwi0/+no= @@ -2150,6 +2171,7 @@ google.golang.org/grpc v1.44.0/go.mod h1:k+4IHHFw41K8+bbowsex27ge2rCb65oeWqe4jJ5 google.golang.org/grpc v1.45.0/go.mod h1:lN7owxKUQEqMfSyQikvvk5tf/6zMPsrK+ONuO11+0rQ= google.golang.org/grpc v1.54.0 h1:EhTqbhiYeixwWQtAEZAxmV9MGqcjEU2mFx52xCzNyag= google.golang.org/grpc v1.54.0/go.mod h1:PUSEXI6iWghWaB6lXM4knEgpJNu2qUcKfDtNci3EC2g= +google.golang.org/grpc/cmd/protoc-gen-go-grpc v0.0.0-20200910201057-6591123024b3/go.mod h1:6Kw0yEErY5E/yWrBtf03jp27GLLJujG4z/JK95pnjjw= google.golang.org/grpc/cmd/protoc-gen-go-grpc v1.2.0 h1:TLkBREm4nIsEcexnCjgQd5GQWaHcqMzwQV0TX9pq8S0= google.golang.org/grpc/cmd/protoc-gen-go-grpc v1.2.0/go.mod h1:DNq5QpG7LJqD2AamLZ7zvKE0DEpVl2BSEVjFycAAjRY= google.golang.org/protobuf v0.0.0-20200109180630-ec00e32a8dfd/go.mod h1:DFci5gLYBciE7Vtevhsrf46CRTquxDuWsQurQQe4oz8= @@ -2218,6 +2240,12 @@ gopkg.in/yaml.v3 v3.0.0-20200313102051-9f266ea9e77c/go.mod h1:K4uyk7z7BCEPqu6E+C gopkg.in/yaml.v3 v3.0.0-20210107192922-496545a6307b/go.mod h1:K4uyk7z7BCEPqu6E+C64Yfv1cQ7kz7rIZviUmN+EgEM= gopkg.in/yaml.v3 v3.0.1 h1:fxVm/GzAzEWqLHuvctI91KS9hhNmmWOoWu0XTYJS7CA= gopkg.in/yaml.v3 v3.0.1/go.mod h1:K4uyk7z7BCEPqu6E+C64Yfv1cQ7kz7rIZviUmN+EgEM= +gorgonia.org/tensor v0.9.24 h1:8ahrfwO4iby+1ILObIqfjJa+wyA2RoCfJSS3LVERSRE= +gorgonia.org/tensor v0.9.24/go.mod h1:1dsOegMm2n1obs69YnVJdp2oPSKx9Q9Tco5i7GEaXRg= +gorgonia.org/vecf32 v0.9.0 h1:PClazic1r+JVJ1dEzRXgeiVl4g1/Hf/w+wUSqnco1Xg= +gorgonia.org/vecf32 v0.9.0/go.mod h1:NCc+5D2oxddRL11hd+pCB1PEyXWOyiQxfZ/1wwhOXCA= +gorgonia.org/vecf64 v0.9.0 h1:bgZDP5x0OzBF64PjMGC3EvTdOoMEcmfAh1VCUnZFm1A= +gorgonia.org/vecf64 v0.9.0/go.mod h1:hp7IOWCnRiVQKON73kkC/AUMtEXyf9kGlVrtPQ9ccVA= gotest.tools v2.2.0+incompatible h1:VsBPFP1AI068pPrMxtb/S8Zkgf9xEmTLJjfM+P5UIEo= gotest.tools v2.2.0+incompatible/go.mod h1:DsYFclhRJ6vuDpmuTbkuFWG+y2sxOXAzmJt81HFBacw= gotest.tools/gotestsum v1.10.0 h1:lVO4uQJoxdsJb7jgmr1fg8QW7zGQ/tuqvsq5fHKyoHQ= diff --git a/ml/api.go b/ml/api.go index 7df11623a09..3df5007d9f5 100644 --- a/ml/api.go +++ b/ml/api.go @@ -1,6 +1,12 @@ // Package ml provides some fundamental machine learning primitives. package ml +import "gorgonia.org/tensor" + +// Tensors are a data structure to hold the input and output map of tensors that will fed into a +// model, or come from the result of a model. +type Tensors map[string]*tensor.Dense + // TODO(erh): this is all wrong, I just need a pivot point in the sand // Classifier TODO. diff --git a/ml/inference/inference.go b/ml/inference/inference.go index cbe38c236db..c2a28142fed 100644 --- a/ml/inference/inference.go +++ b/ml/inference/inference.go @@ -4,14 +4,14 @@ package inference import ( "github.com/pkg/errors" - "go.viam.com/rdk/utils" + "go.viam.com/rdk/ml" ) // MLModel represents a trained machine learning model. type MLModel interface { // Infer takes an already ordered input tensor as an array, // and makes an inference on the model, returning an output tensor map - Infer(inputTensor interface{}) (utils.AttributeMap, error) + Infer(inputTensors ml.Tensors) (ml.Tensors, error) // Metadata gets the entire model metadata structure from file Metadata() (interface{}, error) diff --git a/ml/inference/tflite.go b/ml/inference/tflite.go index adfbcb43581..fd58de40ed5 100644 --- a/ml/inference/tflite.go +++ b/ml/inference/tflite.go @@ -3,6 +3,7 @@ package inference import ( + "fmt" "log" "os" "runtime" @@ -10,7 +11,9 @@ import ( tflite "github.com/mattn/go-tflite" "github.com/pkg/errors" + "gorgonia.org/tensor" + "go.viam.com/rdk/ml" tfliteSchema "go.viam.com/rdk/ml/inference/tflite" metadata "go.viam.com/rdk/ml/inference/tflite_metadata" ) @@ -174,63 +177,89 @@ func getInfo(inter Interpreter) *TFLiteInfo { return info } -// Infer takes an input array in desired type and returns an array of the output tensors. -func (model *TFLiteStruct) Infer(inputTensor interface{}) ([]interface{}, error) { +// Infer takes an input map of tensors and returns an output map of tensors. +func (model *TFLiteStruct) Infer(inputTensors ml.Tensors) (ml.Tensors, error) { model.mu.Lock() defer model.mu.Unlock() interpreter := model.interpreter - input := interpreter.GetInputTensor(0) - status := input.CopyFromBuffer(inputTensor) - if status != tflite.OK { - return nil, errors.New("copying to buffer failed") + inputCount := interpreter.GetInputTensorCount() + if inputCount == 1 && len(inputTensors) == 1 { // convenience function for underspecified names + input := interpreter.GetInputTensor(0) + for _, inpTensor := range inputTensors { // there is only one element in this map + status := input.CopyFromBuffer(inpTensor.Data()) + if status != tflite.OK { + return nil, errors.Errorf("copying from tensor buffer %q failed", input.Name()) + } + } + } else { + for i := 0; i < inputCount; i++ { + input := interpreter.GetInputTensor(i) + inpTensor, ok := inputTensors[input.Name()] + if !ok { + return nil, errors.Errorf("tflite model expected a tensor named %q, but no such input tensor found", input.Name()) + } + if inpTensor == nil { + continue + } + status := input.CopyFromBuffer(inpTensor.Data()) + if status != tflite.OK { + return nil, errors.Errorf("copying from tensor buffer named %q failed", input.Name()) + } + } } - status = interpreter.Invoke() + status := interpreter.Invoke() if status != tflite.OK { - return nil, errors.New("invoke failed") + return nil, errors.New("tflite invoke failed") } - var output []interface{} - numOutputTensors := interpreter.GetOutputTensorCount() - for i := 0; i < numOutputTensors; i++ { - var buf interface{} - currTensor := interpreter.GetOutputTensor(i) - if currTensor == nil { + output := ml.Tensors{} + for i := 0; i < interpreter.GetOutputTensorCount(); i++ { + t := interpreter.GetOutputTensor(i) + if t == nil { continue } - switch currTensor.Type() { - case tflite.Float32: - buf = currTensor.Float32s() - case tflite.UInt8: - buf = currTensor.UInt8s() - case tflite.Bool: - buf = make([]bool, currTensor.ByteSize()) - currTensor.CopyToBuffer(buf) - case tflite.Int8: - buf = currTensor.Int8s() - case tflite.Int16: - buf = currTensor.Int16s() - case tflite.Int32: - buf = currTensor.Int32s() - case tflite.Int64: - buf = currTensor.Int64s() - case tflite.Complex64: - buf = make([]complex64, currTensor.ByteSize()/8) - currTensor.CopyToBuffer(buf) - case tflite.String, tflite.NoType: - // TODO: find a model that outputs tflite.String to test - // if there is a better solution than this - buf = make([]byte, currTensor.ByteSize()) - currTensor.CopyToBuffer(buf) - default: - return nil, FailedToGetError("output tensor type") - } - output = append(output, buf) + tType := TFliteTensorToGorgoniaTensor(t.Type()) + outputTensor := tensor.New( + tensor.WithShape(t.Shape()...), + tensor.Of(tType), + tensor.FromMemory(uintptr(t.Data()), uintptr(t.ByteSize())), + ) + outName := fmt.Sprintf("%s:%v", t.Name(), i) + output[outName] = outputTensor } return output, nil } +// TFliteTensorToGorgoniaTensor converts the constants from one tensor library to another. +func TFliteTensorToGorgoniaTensor(t tflite.TensorType) tensor.Dtype { + switch t { + case tflite.NoType: + return tensor.Uintptr // just return is as a general pointer type... + case tflite.Float32: + return tensor.Float32 + case tflite.Int32: + return tensor.Int32 + case tflite.UInt8: + return tensor.Uint8 + case tflite.Int64: + return tensor.Int64 + case tflite.String: + return tensor.String + case tflite.Bool: + return tensor.Bool + case tflite.Int16: + return tensor.Int16 + case tflite.Complex64: + return tensor.Complex64 + case tflite.Int8: + return tensor.Int8 + default: // shouldn't reach here unless tflite adds more types + return tensor.Uintptr + } +} + // Metadata provides the metadata information based on the model flatbuffer file. func (model *TFLiteStruct) Metadata() (*metadata.ModelMetadataT, error) { b, err := getTFLiteMetadataBytes(model.modelPath) diff --git a/ml/inference/tflite_test.go b/ml/inference/tflite_test.go index d02b94930af..322fbba3728 100644 --- a/ml/inference/tflite_test.go +++ b/ml/inference/tflite_test.go @@ -6,6 +6,9 @@ import ( tflite "github.com/mattn/go-tflite" "go.viam.com/test" "go.viam.com/utils/artifact" + "gorgonia.org/tensor" + + "go.viam.com/rdk/ml" ) type fakeInterpreter struct{} @@ -66,7 +69,8 @@ func TestLoadModel(t *testing.T) { test.That(t, structInfo.OutputTensorTypes, test.ShouldResemble, []string{"Float32", "Float32", "Float32", "Float32"}) buf := make([]float32, c*h*w) - outTensors, err := tfliteStruct.Infer(buf) + tensors := ml.Tensors{"serving_default_input:0": tensor.New(tensor.WithShape(h, w, c), tensor.WithBacking(buf))} + outTensors, err := tfliteStruct.Infer(tensors) test.That(t, err, test.ShouldBeNil) test.That(t, outTensors, test.ShouldNotBeNil) test.That(t, len(outTensors), test.ShouldEqual, 4) diff --git a/module/modmanager/manager.go b/module/modmanager/manager.go index 4e265057141..e61456c9896 100644 --- a/module/modmanager/manager.go +++ b/module/modmanager/manager.go @@ -126,34 +126,33 @@ func (mgr *Manager) add(ctx context.Context, conf config.Module, conn *grpc.Clie name: conf.Name, exe: conf.ExePath, logLevel: conf.LogLevel, + conn: conn, resources: map[resource.Name]*addedResource{}, } - mgr.modules[conf.Name] = mod - - if err := mod.startProcess(ctx, mgr.parentAddr, - mgr.newOnUnexpectedExitHandler(mod), mgr.logger); err != nil { - return errors.WithMessage(err, "error while starting module "+mod.name) - } var success bool defer func() { if !success { - if err := mod.stopProcess(); err != nil { - mgr.logger.Error(err) - } + mod.cleanupAfterStartupFailure(mgr, false) } }() - // dial will re-use conn if it's non-nil (module being added in a Reconfigure). - if err := mod.dial(conn); err != nil { + if err := mod.startProcess(ctx, mgr.parentAddr, + mgr.newOnUnexpectedExitHandler(mod), mgr.logger); err != nil { + return errors.WithMessage(err, "error while starting module "+mod.name) + } + + // dial will re-use mod.conn if it's non-nil (module being added in a Reconfigure). + if err := mod.dial(); err != nil { return errors.WithMessage(err, "error while dialing module "+mod.name) } - if err := mod.checkReady(ctx, mgr.parentAddr); err != nil { + if err := mod.checkReady(ctx, mgr.parentAddr, mgr.logger); err != nil { return errors.WithMessage(err, "error while waiting for module to be ready "+mod.name) } mod.registerResources(mgr, mgr.logger) + mgr.modules[conf.Name] = mod success = true return nil @@ -444,6 +443,7 @@ func (mgr *Manager) newOnUnexpectedExitHandler(mod *module) func(exitCode int) b if mod.inRecovery.Load() { return false } + mod.inRecovery.Store(true) defer mod.inRecovery.Store(false) @@ -470,14 +470,13 @@ func (mgr *Manager) newOnUnexpectedExitHandler(mod *module) func(exitCode int) b } // Otherwise, add old module process' resources to new module; warn if new - // module cannot handle old resource, deregister that resource and remove - // it from mod.resources. Finally, handle orphaned resources. + // module cannot handle old resource and remove it from mod.resources. + // Finally, handle orphaned resources. var orphanedResourceNames []resource.Name for name, res := range mod.resources { if _, err := mgr.addResource(ctx, res.conf, res.deps); err != nil { mgr.logger.Warnw("error while re-adding resource to module", "resource", name, "module", mod.name, "error", err) - resource.Deregister(res.conf.API, res.conf.Model) delete(mod.resources, name) orphanedResourceNames = append(orphanedResourceNames, name) } @@ -497,6 +496,10 @@ func (mgr *Manager) attemptRestart(ctx context.Context, mod *module) []resource. mgr.mu.Lock() defer mgr.mu.Unlock() + // deregister crashed module's resources, and let later checkReady reset m.handles + // before reregistering. + mod.deregisterResources() + var orphanedResourceNames []resource.Name for name := range mod.resources { orphanedResourceNames = append(orphanedResourceNames, name) @@ -509,22 +512,7 @@ func (mgr *Manager) attemptRestart(ctx context.Context, mod *module) []resource. var success bool defer func() { if !success { - // Deregister module's resources, remove module, and close connection if - // restart fails. Process will already be stopped. - mod.deregisterResources() - for r, m := range mgr.rMap { - if m == mod { - delete(mgr.rMap, r) - } - } - delete(mgr.modules, mod.name) - if mod.conn != nil { - if err := mod.conn.Close(); err != nil { - mgr.logger.Errorw("error while closing connection from crashed module", - "module", mod.name, - "error", err) - } - } + mod.cleanupAfterStartupFailure(mgr, true) } }() @@ -549,37 +537,29 @@ func (mgr *Manager) attemptRestart(ctx context.Context, mod *module) []resource. utils.SelectContextOrWait(ctx, time.Duration(attempt)*oueRestartInterval) } - defer func() { - if !success { - // Stop restarted module process if there are later failures. - if err := mod.stopProcess(); err != nil { - mgr.logger.Error(err) - } - } - }() - - // dial will re-use connection; old connection can still be used when module + // dial will re-use mod.conn; old connection can still be used when module // crashes. - if err := mod.dial(mod.conn); err != nil { + if err := mod.dial(); err != nil { mgr.logger.Errorw("error while dialing restarted module", "module", mod.name, "error", err) return orphanedResourceNames } - if err := mod.checkReady(ctx, mgr.parentAddr); err != nil { + if err := mod.checkReady(ctx, mgr.parentAddr, mgr.logger); err != nil { mgr.logger.Errorw("error while waiting for restarted module to be ready", "module", mod.name, "error", err) return orphanedResourceNames } + mod.registerResources(mgr, mgr.logger) + success = true return nil } -// dial will use the passed-in connection to make a new module service client -// or Dial m.addr if the passed-in connection is nil. -func (m *module) dial(conn *grpc.ClientConn) error { - m.conn = conn +// dial will use m.conn to make a new module service client or Dial m.addr if +// m.conn is nil. +func (m *module) dial() error { if m.conn == nil { // TODO(PRODUCT-343): session support probably means interceptors here var err error @@ -603,8 +583,8 @@ func (m *module) dial(conn *grpc.ClientConn) error { return nil } -func (m *module) checkReady(ctx context.Context, parentAddr string) error { - ctxTimeout, cancelFunc := context.WithTimeout(ctx, time.Second*30) +func (m *module) checkReady(ctx context.Context, parentAddr string, logger golog.Logger) error { + ctxTimeout, cancelFunc := context.WithTimeout(ctx, rutils.GetResourceConfigurationTimeout(logger)) defer cancelFunc() for { @@ -655,7 +635,7 @@ func (m *module) startProcess( return errors.WithMessage(err, "module startup failed") } - ctxTimeout, cancel := context.WithTimeout(ctx, time.Second*30) + ctxTimeout, cancel := context.WithTimeout(ctx, rutils.GetResourceConfigurationTimeout(logger)) defer cancel() for { select { @@ -704,6 +684,7 @@ func (m *module) registerResources(mgr modmaninterface.ModuleManager, logger gol switch { case api.API.IsComponent(): for _, model := range models { + logger.Debugw("registering component from module", "module", m.name, "API", api.API, "model", model) resource.RegisterComponent(api.API, model, resource.Registration[resource.Resource, resource.NoNativeConfig]{ Constructor: func( ctx context.Context, @@ -717,6 +698,7 @@ func (m *module) registerResources(mgr modmaninterface.ModuleManager, logger gol } case api.API.IsService(): for _, model := range models { + logger.Debugw("registering service from module", "module", m.name, "API", api.API, "model", model) resource.RegisterService(api.API, model, resource.Registration[resource.Resource, resource.NoNativeConfig]{ Constructor: func( ctx context.Context, @@ -740,6 +722,36 @@ func (m *module) deregisterResources() { resource.Deregister(api.API, model) } } + m.handles = nil +} + +func (m *module) cleanupAfterStartupFailure(mgr *Manager, afterCrash bool) { + if err := m.stopProcess(); err != nil { + msg := "error while stopping process of module that failed to start" + if afterCrash { + msg = "error while stopping process of crashed module" + } + mgr.logger.Errorw(msg, "module", m.name, "error", err) + } + if m.conn != nil { + if err := m.conn.Close(); err != nil { + msg := "error while closing connection to module that failed to start" + if afterCrash { + msg = "error while closing connection to crashed module" + } + mgr.logger.Errorw(msg, "module", m.name, "error", err) + } + } + + // Remove module from rMap and mgr.modules if startup failure was after crash. + if afterCrash { + for r, mod := range mgr.rMap { + if mod == m { + delete(mgr.rMap, r) + } + } + delete(mgr.modules, m.name) + } } // DepsToNames converts a dependency list to a simple string slice. diff --git a/module/modmanager/manager_test.go b/module/modmanager/manager_test.go index affea369991..d4814f17c68 100644 --- a/module/modmanager/manager_test.go +++ b/module/modmanager/manager_test.go @@ -19,6 +19,7 @@ import ( modmanageroptions "go.viam.com/rdk/module/modmanager/options" "go.viam.com/rdk/resource" rtestutils "go.viam.com/rdk/testutils" + rutils "go.viam.com/rdk/utils" ) func TestModManagerFunctions(t *testing.T) { @@ -55,16 +56,16 @@ func TestModManagerFunctions(t *testing.T) { err = mod.startProcess(ctx, parentAddr, nil, logger) test.That(t, err, test.ShouldBeNil) - err = mod.dial(nil) + err = mod.dial() test.That(t, err, test.ShouldBeNil) // check that dial can re-use connections. oldConn := mod.conn - err = mod.dial(mod.conn) + err = mod.dial() test.That(t, err, test.ShouldBeNil) test.That(t, mod.conn, test.ShouldEqual, oldConn) - err = mod.checkReady(ctx, parentAddr) + err = mod.checkReady(ctx, parentAddr, logger) test.That(t, err, test.ShouldBeNil) mod.registerResources(mgr, logger) @@ -459,6 +460,52 @@ func TestModuleReloading(t *testing.T) { // Assert that RemoveOrphanedResources was called once. test.That(t, dummyRemoveOrphanedResourcesCallCount.Load(), test.ShouldEqual, 1) }) + t.Run("timed out module process is stopped", func(t *testing.T) { + logger, logs := golog.NewObservedTestLogger(t) + + modCfg.ExePath = rutils.ResolveFile("module/testmodule/fakemodule.sh") + + // Lower global timeout early to avoid race with actual restart code. + defer func(oriOrigVal time.Duration) { + oueRestartInterval = oriOrigVal + }(oueRestartInterval) + oueRestartInterval = 10 * time.Millisecond + + // Lower resource configuration timeout to avoid waiting for 60 seconds + // for manager.Add to time out waiting for module to start listening. + defer func() { + test.That(t, os.Unsetenv(rutils.ResourceConfigurationTimeoutEnvVar), + test.ShouldBeNil) + }() + test.That(t, os.Setenv(rutils.ResourceConfigurationTimeoutEnvVar, "10ms"), + test.ShouldBeNil) + + // This test neither uses a resource manager nor asserts anything about + // the existence of resources in the graph. Use a dummy + // RemoveOrphanedResources function so orphaned resource logic does not + // panic. + dummyRemoveOrphanedResources := func(context.Context, []resource.Name) {} + mgr := NewManager(parentAddr, logger, modmanageroptions.Options{ + UntrustedEnv: false, + RemoveOrphanedResources: dummyRemoveOrphanedResources, + }) + err = mgr.Add(ctx, modCfg) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, + "timed out waiting for module test-module to start listening") + + // Assert that number of "fakemodule is running" messages does not increase + // over time (the process was stopped). + msgNum := logs.FilterMessageSnippet("fakemodule is running").Len() + time.Sleep(100 * time.Millisecond) + test.That(t, logs.FilterMessageSnippet("fakemodule is running").Len(), test.ShouldEqual, msgNum) + + // Assert that manager removes module. + test.That(t, len(mgr.Configs()), test.ShouldEqual, 0) + + err = mgr.Close(ctx) + test.That(t, err, test.ShouldBeNil) + }) } func TestDebugModule(t *testing.T) { diff --git a/module/module_interceptors_test.go b/module/module_interceptors_test.go index 3bb11060c17..50864909fea 100644 --- a/module/module_interceptors_test.go +++ b/module/module_interceptors_test.go @@ -34,9 +34,13 @@ func TestOpID(t *testing.T) { defer func() { test.That(t, os.Remove(cfgFilename), test.ShouldBeNil) }() + + serverPath, err := rtestutils.BuildTempModule(t, "web/cmd/server/") + test.That(t, err, test.ShouldBeNil) + server := pexec.NewManagedProcess(pexec.ProcessConfig{ - Name: "bash", - Args: []string{"-c", "make server && exec bin/`uname`-`uname -m`/viam-server -config " + cfgFilename}, + Name: serverPath, + Args: []string{"-config", cfgFilename}, CWD: utils.ResolveFile("./"), Log: true, }, logger) diff --git a/module/testmodule/fakemodule.sh b/module/testmodule/fakemodule.sh new file mode 100755 index 00000000000..f89a7e4e668 --- /dev/null +++ b/module/testmodule/fakemodule.sh @@ -0,0 +1,10 @@ +#!/bin/bash + +# fakemodule is a completely fake module that repeatedly echos a message. Used +# to test that modules that never start listening are stopped. + +while : +do + echo "fakemodule is running" + sleep 0.01 +done diff --git a/module/testmodule/main.go b/module/testmodule/main.go index 07a84caed64..ca4ab156e39 100644 --- a/module/testmodule/main.go +++ b/module/testmodule/main.go @@ -12,13 +12,15 @@ import ( "go.viam.com/utils" "go.viam.com/rdk/components/generic" + "go.viam.com/rdk/components/motor" "go.viam.com/rdk/module" "go.viam.com/rdk/resource" ) var ( - myModel = resource.NewModel("rdk", "test", "helper") - myMod *module.Module + helperModel = resource.NewModel("rdk", "test", "helper") + testMotorModel = resource.NewModel("rdk", "test", "motor") + myMod *module.Module ) func main() { @@ -33,14 +35,25 @@ func mainWithArgs(ctx context.Context, args []string, logger golog.Logger) error if err != nil { return err } + resource.RegisterComponent( generic.API, - myModel, + helperModel, resource.Registration[resource.Resource, resource.NoNativeConfig]{Constructor: newHelper}) - err = myMod.AddModelFromRegistry(ctx, generic.API, myModel) + err = myMod.AddModelFromRegistry(ctx, generic.API, helperModel) + if err != nil { + return err + } + + resource.RegisterComponent( + motor.API, + testMotorModel, + resource.Registration[resource.Resource, resource.NoNativeConfig]{Constructor: newTestMotor}) + err = myMod.AddModelFromRegistry(ctx, motor.API, testMotorModel) if err != nil { return err } + err = myMod.Start(ctx) defer myMod.Close(ctx) if err != nil { @@ -95,3 +108,68 @@ func (h *helper) DoCommand(ctx context.Context, req map[string]interface{}) (map return nil, fmt.Errorf("unknown command string %s", cmd) } } + +func newTestMotor(ctx context.Context, deps resource.Dependencies, conf resource.Config, logger golog.Logger) (resource.Resource, error) { + return &testMotor{ + Named: conf.ResourceName().AsNamed(), + }, nil +} + +type testMotor struct { + resource.Named + resource.TriviallyReconfigurable + resource.TriviallyCloseable +} + +var _ motor.Motor = &testMotor{} + +// SetPower trivially implements motor.Motor. +func (tm *testMotor) SetPower(_ context.Context, _ float64, _ map[string]interface{}) error { + return nil +} + +// GoFor trivially implements motor.Motor. +func (tm *testMotor) GoFor(_ context.Context, _, _ float64, _ map[string]interface{}) error { + return nil +} + +// GoTo trivially implements motor.Motor. +func (tm *testMotor) GoTo(_ context.Context, _, _ float64, _ map[string]interface{}) error { + return nil +} + +// ResetZeroPosition trivially implements motor.Motor. +func (tm *testMotor) ResetZeroPosition(_ context.Context, _ float64, _ map[string]interface{}) error { + return nil +} + +// Position trivially implements motor.Motor. +func (tm *testMotor) Position(_ context.Context, _ map[string]interface{}) (float64, error) { + return 0.0, nil +} + +// Properties trivially implements motor.Motor. +func (tm *testMotor) Properties(_ context.Context, _ map[string]interface{}) (motor.Properties, error) { + return motor.Properties{}, nil +} + +// Stop trivially implements motor.Motor. +func (tm *testMotor) Stop(_ context.Context, _ map[string]interface{}) error { + return nil +} + +// IsPowered trivally implements motor.Motor. +func (tm *testMotor) IsPowered(_ context.Context, _ map[string]interface{}) (bool, float64, error) { + return false, 0.0, nil +} + +// DoCommand trivially implements motor.Motor. +func (tm *testMotor) DoCommand(_ context.Context, _ map[string]interface{}) (map[string]interface{}, error) { + //nolint:nilnil + return nil, nil +} + +// IsMoving trivially implements motor.Motor. +func (tm *testMotor) IsMoving(context.Context) (bool, error) { + return false, nil +} diff --git a/motionplan/cBiRRT.go b/motionplan/cBiRRT.go index 7535e851c07..8d4623cf381 100644 --- a/motionplan/cBiRRT.go +++ b/motionplan/cBiRRT.go @@ -13,20 +13,12 @@ import ( "github.com/edaniels/golog" "go.viam.com/utils" + "go.viam.com/rdk/motionplan/ik" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" ) const ( - // The maximum percent of a joints range of motion to allow per step. - defaultFrameStep = 0.015 - - // If the dot product between two sets of joint angles is less than this, consider them identical. - defaultJointSolveDist = 0.0001 - - // Number of iterations to run before beginning to accept randomly seeded locations. - defaultIterBeforeRand = 50 - // Maximum number of iterations that constrainNear will run before exiting nil. // Typically it will solve in the first five iterations, or not at all. maxNearIter = 20 @@ -36,34 +28,15 @@ const ( ) type cbirrtOptions struct { - // The maximum percent of a joints range of motion to allow per step. - FrameStep float64 `json:"frame_step"` - - // If the dot product between two sets of joint angles is less than this, consider them identical. - JointSolveDist float64 `json:"joint_solve_dist"` - // Number of IK solutions with which to seed the goal side of the bidirectional tree. SolutionsToSeed int `json:"solutions_to_seed"` - - // Number of iterations to mrun before beginning to accept randomly seeded locations. - IterBeforeRand int `json:"iter_before_rand"` - - // This is how far cbirrt will try to extend the map towards a goal per-step. Determined from FrameStep - qstep []float64 - - // Parameters common to all RRT implementations - *rrtOptions } // newCbirrtOptions creates a struct controlling the running of a single invocation of cbirrt. All values are pre-set to reasonable // defaults, but can be tweaked if needed. -func newCbirrtOptions(planOpts *plannerOptions, frame referenceframe.Frame) (*cbirrtOptions, error) { +func newCbirrtOptions(planOpts *plannerOptions) (*cbirrtOptions, error) { algOpts := &cbirrtOptions{ - FrameStep: defaultFrameStep, - JointSolveDist: defaultJointSolveDist, SolutionsToSeed: defaultSolutionsToSeed, - IterBeforeRand: defaultIterBeforeRand, - rrtOptions: newRRTOptions(), } // convert map to json jsonString, err := json.Marshal(planOpts.extra) @@ -75,8 +48,6 @@ func newCbirrtOptions(planOpts *plannerOptions, frame referenceframe.Frame) (*cb return nil, err } - algOpts.qstep = getFrameSteps(frame, algOpts.FrameStep) - return algOpts, nil } @@ -85,9 +56,8 @@ func newCbirrtOptions(planOpts *plannerOptions, frame referenceframe.Frame) (*cb // https://ieeexplore.ieee.org/document/5152399/ type cBiRRTMotionPlanner struct { *planner - fastGradDescent *NloptIK + fastGradDescent *ik.NloptIK algOpts *cbirrtOptions - corners map[node]bool } // newCBiRRTMotionPlannerWithSeed creates a cBiRRTMotionPlanner object with a user specified random seed. @@ -105,11 +75,11 @@ func newCBiRRTMotionPlanner( return nil, err } // nlopt should try only once - nlopt, err := CreateNloptIKSolver(frame, logger, 1, opt.GoalThreshold) + nlopt, err := ik.CreateNloptIKSolver(frame, logger, 1, true) if err != nil { return nil, err } - algOpts, err := newCbirrtOptions(opt, mp.frame) + algOpts, err := newCbirrtOptions(opt) if err != nil { return nil, err } @@ -117,24 +87,19 @@ func newCBiRRTMotionPlanner( planner: mp, fastGradDescent: nlopt, algOpts: algOpts, - corners: map[node]bool{}, }, nil } func (mp *cBiRRTMotionPlanner) plan(ctx context.Context, goal spatialmath.Pose, seed []referenceframe.Input, -) ([][]referenceframe.Input, error) { +) ([]node, error) { solutionChan := make(chan *rrtPlanReturn, 1) utils.PanicCapturingGo(func() { mp.rrtBackgroundRunner(ctx, seed, &rrtParallelPlannerShared{nil, nil, solutionChan}) }) - select { - case <-ctx.Done(): - return nil, ctx.Err() - case plan := <-solutionChan: - return plan.toInputs(), plan.err() - } + plan := <-solutionChan + return plan.steps, plan.err() } // rrtBackgroundRunner will execute the plan. Plan() will call rrtBackgroundRunner in a separate thread and wait for results. @@ -152,7 +117,6 @@ func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner( return } // initialize maps - corners := map[node]bool{} // TODO(rb) package neighborManager better nm1 := &neighborManager{nCPU: mp.planOpts.NumThreads} nm2 := &neighborManager{nCPU: mp.planOpts.NumThreads} @@ -168,7 +132,8 @@ func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner( } rrt.maps = planSeed.maps } - target := referenceframe.InterpolateInputs(seed, rrt.maps.optNode.Q(), 0.5) + mp.logger.Infof("goal node: %v\n", rrt.maps.optNode.Q()) + target := newConfigurationNode(referenceframe.InterpolateInputs(seed, rrt.maps.optNode.Q(), 0.5)) map1, map2 := rrt.maps.startMap, rrt.maps.goalMap @@ -190,7 +155,7 @@ func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner( len(rrt.maps.goalMap), ) - for i := 0; i < mp.algOpts.PlanIter; i++ { + for i := 0; i < mp.planOpts.PlanIter; i++ { select { case <-ctx.Done(): mp.logger.Debugf("CBiRRT timed out after %d iterations", i) @@ -199,13 +164,13 @@ func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner( default: } - tryExtend := func(target []referenceframe.Input) (node, node, error) { + tryExtend := func(target node) (node, node, error) { // attempt to extend maps 1 and 2 towards the target utils.PanicCapturingGo(func() { - m1chan <- nm1.nearestNeighbor(nmContext, mp.planOpts, newConfigurationNode(target), map1) + m1chan <- nm1.nearestNeighbor(nmContext, mp.planOpts, target, map1) }) utils.PanicCapturingGo(func() { - m2chan <- nm2.nearestNeighbor(nmContext, mp.planOpts, newConfigurationNode(target), map2) + m2chan <- nm2.nearestNeighbor(nmContext, mp.planOpts, target, map2) }) nearest1 := <-m1chan nearest2 := <-m2chan @@ -222,16 +187,16 @@ func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner( rseed2 := rand.New(rand.NewSource(int64(mp.randseed.Int()))) utils.PanicCapturingGo(func() { - mp.constrainedExtend(ctx, rseed1, map1, nearest1, newConfigurationNode(target), m1chan) + mp.constrainedExtend(ctx, rseed1, map1, nearest1, target, m1chan) }) utils.PanicCapturingGo(func() { - mp.constrainedExtend(ctx, rseed2, map2, nearest2, newConfigurationNode(target), m2chan) + mp.constrainedExtend(ctx, rseed2, map2, nearest2, target, m2chan) }) map1reached := <-m1chan map2reached := <-m2chan - corners[map1reached] = true - corners[map2reached] = true + map1reached.SetCorner(true) + map2reached.SetCorner(true) return map1reached, map2reached, nil } @@ -242,49 +207,39 @@ func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner( return } - reachedDelta := mp.planOpts.DistanceFunc(&Segment{StartConfiguration: map1reached.Q(), EndConfiguration: map2reached.Q()}) + reachedDelta := mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: map1reached.Q(), EndConfiguration: map2reached.Q()}) // Second iteration; extend maps 1 and 2 towards the halfway point between where they reached - if reachedDelta > mp.algOpts.JointSolveDist { - target = referenceframe.InterpolateInputs(map1reached.Q(), map2reached.Q(), 0.5) + if reachedDelta > mp.planOpts.JointSolveDist { + target = newConfigurationNode(referenceframe.InterpolateInputs(map1reached.Q(), map2reached.Q(), 0.5)) map1reached, map2reached, err = tryExtend(target) if err != nil { rrt.solutionChan <- &rrtPlanReturn{planerr: err, maps: rrt.maps} return } - reachedDelta = mp.planOpts.DistanceFunc(&Segment{StartConfiguration: map1reached.Q(), EndConfiguration: map2reached.Q()}) + reachedDelta = mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: map1reached.Q(), EndConfiguration: map2reached.Q()}) } // Solved! - if reachedDelta <= mp.algOpts.JointSolveDist { + if reachedDelta <= mp.planOpts.JointSolveDist { mp.logger.Debugf("CBiRRT found solution after %d iterations", i) cancel() - path := extractPath(rrt.maps.startMap, rrt.maps.goalMap, &nodePair{map1reached, map2reached}) + path := extractPath(rrt.maps.startMap, rrt.maps.goalMap, &nodePair{map1reached, map2reached}, true) rrt.solutionChan <- &rrtPlanReturn{steps: path, maps: rrt.maps} return } // sample near map 1 and switch which map is which to keep adding to them even - target = mp.sample(map1reached, i) + target, err = mp.sample(map1reached, i) + if err != nil { + rrt.solutionChan <- &rrtPlanReturn{planerr: err, maps: rrt.maps} + return + } map1, map2 = map2, map1 } rrt.solutionChan <- &rrtPlanReturn{planerr: errPlannerFailed, maps: rrt.maps} } -func (mp *cBiRRTMotionPlanner) sample(rSeed node, sampleNum int) []referenceframe.Input { - // If we have done more than 50 iterations, start seeding off completely random positions 2 at a time - // The 2 at a time is to ensure random seeds are added onto both the seed and goal maps. - if sampleNum >= mp.algOpts.IterBeforeRand && sampleNum%4 >= 2 { - return referenceframe.RandomFrameInputs(mp.frame, mp.randseed) - } - // Seeding nearby to valid points results in much faster convergence in less constrained space - q := referenceframe.RestrictedRandomFrameInputs(mp.frame, mp.randseed, 0.1) - for j, v := range rSeed.Q() { - q[j].Value += v.Value - } - return q -} - // constrainedExtend will try to extend the map towards the target while meeting constraints along the way. It will // return the closest solution to the target that it reaches, which may or may not actually be the target. func (mp *cBiRRTMotionPlanner) constrainedExtend( @@ -295,8 +250,8 @@ func (mp *cBiRRTMotionPlanner) constrainedExtend( mchan chan node, ) { // Allow qstep to be doubled as a means to escape from configurations which gradient descend to their seed - qstep := make([]float64, len(mp.algOpts.qstep)) - copy(qstep, mp.algOpts.qstep) + qstep := make([]float64, len(mp.planOpts.qstep)) + copy(qstep, mp.planOpts.qstep) doubled := false oldNear := near @@ -314,10 +269,10 @@ func (mp *cBiRRTMotionPlanner) constrainedExtend( default: } - dist := mp.planOpts.DistanceFunc(&Segment{StartConfiguration: near.Q(), EndConfiguration: target.Q()}) - oldDist := mp.planOpts.DistanceFunc(&Segment{StartConfiguration: oldNear.Q(), EndConfiguration: target.Q()}) + dist := mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: near.Q(), EndConfiguration: target.Q()}) + oldDist := mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: oldNear.Q(), EndConfiguration: target.Q()}) switch { - case dist < mp.algOpts.JointSolveDist: + case dist < mp.planOpts.JointSolveDist: mchan <- near return case dist > oldDist: @@ -327,26 +282,13 @@ func (mp *cBiRRTMotionPlanner) constrainedExtend( oldNear = near - newNear := make([]referenceframe.Input, 0, len(near.Q())) - - // alter near to be closer to target - for j, nearInput := range near.Q() { - if nearInput.Value == target.Q()[j].Value { - newNear = append(newNear, nearInput) - } else { - v1, v2 := nearInput.Value, target.Q()[j].Value - newVal := math.Min(qstep[j], math.Abs(v2-v1)) - // get correct sign - newVal *= (v2 - v1) / math.Abs(v2-v1) - newNear = append(newNear, referenceframe.Input{nearInput.Value + newVal}) - } - } + newNear := fixedStepInterpolation(near, target, mp.planOpts.qstep) // Check whether newNear meets constraints, and if not, update it to a configuration that does meet constraints (or nil) newNear = mp.constrainNear(ctx, randseed, oldNear.Q(), newNear) if newNear != nil { - nearDist := mp.planOpts.DistanceFunc(&Segment{StartConfiguration: oldNear.Q(), EndConfiguration: newNear}) - if nearDist < math.Pow(mp.algOpts.JointSolveDist, 3) { + nearDist := mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: oldNear.Q(), EndConfiguration: newNear}) + if nearDist < math.Pow(mp.planOpts.JointSolveDist, 3) { if !doubled { doubled = true // Check if doubling qstep will allow escape from the identical configuration @@ -364,7 +306,7 @@ func (mp *cBiRRTMotionPlanner) constrainedExtend( } } if doubled { - copy(qstep, mp.algOpts.qstep) + copy(qstep, mp.planOpts.qstep) doubled = false } // constrainNear will ensure path between oldNear and newNear satisfies constraints along the way @@ -402,7 +344,7 @@ func (mp *cBiRRTMotionPlanner) constrainNear( return nil } - newArc := &Segment{ + newArc := &ik.Segment{ StartPosition: seedPos, EndPosition: goalPos, StartConfiguration: seedInputs, @@ -415,30 +357,30 @@ func (mp *cBiRRTMotionPlanner) constrainNear( if ok { return target } - solutionGen := make(chan []referenceframe.Input, 1) + solutionGen := make(chan *ik.Solution, 1) // Spawn the IK solver to generate solutions until done err = mp.fastGradDescent.Solve(ctx, solutionGen, target, mp.planOpts.pathMetric, randseed.Int()) // We should have zero or one solutions - var solved []referenceframe.Input + var solved *ik.Solution select { case solved = <-solutionGen: default: } close(solutionGen) - if err != nil { + if err != nil || solved == nil { return nil } ok, failpos := mp.planOpts.CheckSegmentAndStateValidity( - &Segment{StartConfiguration: seedInputs, EndConfiguration: solved, Frame: mp.frame}, + &ik.Segment{StartConfiguration: seedInputs, EndConfiguration: solved.Configuration, Frame: mp.frame}, mp.planOpts.Resolution, ) if ok { - return solved + return solved.Configuration } if failpos != nil { - dist := mp.planOpts.DistanceFunc(&Segment{StartConfiguration: target, EndConfiguration: failpos.EndConfiguration}) - if dist > mp.algOpts.JointSolveDist { + dist := mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: target, EndConfiguration: failpos.EndConfiguration}) + if dist > mp.planOpts.JointSolveDist { // If we have a first failing position, and that target is updating (no infinite loop), then recurse seedInputs = failpos.StartConfiguration target = failpos.EndConfiguration @@ -461,100 +403,62 @@ func (mp *cBiRRTMotionPlanner) smoothPath( schan := make(chan node, 1) defer close(schan) - for iter := 0; iter < toIter && len(inputSteps) > 4; iter++ { - select { - case <-ctx.Done(): - mp.logger.Debug("CBiRRT timed out during smoothing, returning best path") - return inputSteps - default: - } - // Pick two random non-adjacent indices, excepting the ends - - j := 2 + mp.randseed.Intn(len(inputSteps)-3) - - i := mp.randseed.Intn(j) + 1 - - ok, hitCorners := smoothable(inputSteps, i, j, mp.corners) - if !ok { - continue - } - - shortcutGoal := make(map[node]node) - - iSol := inputSteps[i] - jSol := inputSteps[j] - shortcutGoal[jSol] = nil - - // extend backwards for convenience later. Should work equally well in both directions - mp.constrainedExtend(ctx, mp.randseed, shortcutGoal, jSol, iSol, schan) - reached := <-schan - - // Note this could technically replace paths with "longer" paths i.e. with more waypoints. - // However, smoothed paths are invariably more intuitive and smooth, and lend themselves to future shortening, - // so we allow elongation here. - dist := mp.planOpts.DistanceFunc(&Segment{StartConfiguration: inputSteps[i].Q(), EndConfiguration: reached.Q()}) - if dist < mp.algOpts.JointSolveDist && len(reached.Q()) < j-i { - mp.corners[iSol] = true - mp.corners[jSol] = true - for _, hitCorner := range hitCorners { - mp.corners[hitCorner] = false + for numCornersToPass := 2; numCornersToPass > 0; numCornersToPass-- { + for iter := 0; iter < toIter/2 && len(inputSteps) > 3; iter++ { + select { + case <-ctx.Done(): + return inputSteps + default: } - newInputSteps := append([]node{}, inputSteps[:i]...) - for reached != nil { - newInputSteps = append(newInputSteps, reached) - reached = shortcutGoal[reached] + // get start node of first edge. Cannot be either the last or second-to-last node. + // Intn will return an int in the half-open interval [0,n) + i := mp.randseed.Intn(len(inputSteps) - 2) + j := i + 1 + cornersPassed := 0 + hitCorners := []node{} + for (cornersPassed != numCornersToPass || !inputSteps[j].Corner()) && j < len(inputSteps)-1 { + j++ + if cornersPassed < numCornersToPass && inputSteps[j].Corner() { + cornersPassed++ + hitCorners = append(hitCorners, inputSteps[j]) + } + } + // no corners existed between i and end of inputSteps -> not good candidate for smoothing + if len(hitCorners) == 0 { + continue } - newInputSteps = append(newInputSteps, inputSteps[j+1:]...) - inputSteps = newInputSteps - } - } - return inputSteps -} + shortcutGoal := make(map[node]node) -// Check if there is more than one joint direction change. If not, then not a good candidate for smoothing. -func smoothable(inputSteps []node, i, j int, corners map[node]bool) (bool, []node) { - startPos := inputSteps[i] - nextPos := inputSteps[i+1] - // Whether joints are increasing - incDir := make([]int, 0, len(startPos.Q())) - hitCorners := []node{} - - check := func(v1, v2 float64) int { - if v1 > v2 { - return 1 - } else if v1 < v2 { - return -1 - } - return 0 - } + iSol := inputSteps[i] + jSol := inputSteps[j] + shortcutGoal[jSol] = nil - // Get initial directionality - for h, v := range startPos.Q() { - incDir = append(incDir, check(v.Value, nextPos.Q()[h].Value)) - } + mp.constrainedExtend(ctx, mp.randseed, shortcutGoal, jSol, iSol, schan) + reached := <-schan - // Check for any direction changes - changes := 0 - for k := i + 1; k < j; k++ { - for h, v := range nextPos.Q() { - // Get 1, 0, or -1 depending on directionality - newV := check(v.Value, inputSteps[k].Q()[h].Value) - if incDir[h] == 0 { - incDir[h] = newV - } else if incDir[h] == newV*-1 { - changes++ - } - if changes > 1 && len(hitCorners) > 0 { - return true, hitCorners + // Note this could technically replace paths with "longer" paths i.e. with more waypoints. + // However, smoothed paths are invariably more intuitive and smooth, and lend themselves to future shortening, + // so we allow elongation here. + dist := mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: inputSteps[i].Q(), EndConfiguration: reached.Q()}) + if dist < mp.planOpts.JointSolveDist { + for _, hitCorner := range hitCorners { + hitCorner.SetCorner(false) + } + + newInputSteps := append([]node{}, inputSteps[:i]...) + for reached != nil { + newInputSteps = append(newInputSteps, reached) + reached = shortcutGoal[reached] + } + newInputSteps[i].SetCorner(true) + newInputSteps[len(newInputSteps)-1].SetCorner(true) + newInputSteps = append(newInputSteps, inputSteps[j+1:]...) + inputSteps = newInputSteps } } - nextPos = inputSteps[k] - if corners[nextPos] { - hitCorners = append(hitCorners, nextPos) - } } - return false, hitCorners + return inputSteps } // getFrameSteps will return a slice of positive values representing the largest amount a particular DOF of a frame should diff --git a/motionplan/cBiRRT_test.go b/motionplan/cBiRRT_test.go index a1dfa0e2c52..6db3f5d9a5e 100644 --- a/motionplan/cBiRRT_test.go +++ b/motionplan/cBiRRT_test.go @@ -10,6 +10,7 @@ import ( "go.viam.com/test" "go.viam.com/utils" + "go.viam.com/rdk/motionplan/ik" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" rutils "go.viam.com/rdk/utils" @@ -37,14 +38,12 @@ func TestSimpleLinearMotion(t *testing.T) { goalPos := spatialmath.NewPose(r3.Vector{X: 206, Y: 100, Z: 120.5}, &spatialmath.OrientationVectorDegrees{OY: -1}) - opt := newBasicPlannerOptions() - opt.SetGoalMetric(NewSquaredNormMetric(goalPos)) + opt := newBasicPlannerOptions(m) + opt.SetGoalMetric(ik.NewSquaredNormMetric(goalPos)) mp, err := newCBiRRTMotionPlanner(m, rand.New(rand.NewSource(42)), logger, opt) test.That(t, err, test.ShouldBeNil) cbirrt, _ := mp.(*cBiRRTMotionPlanner) - corners := map[node]bool{} - solutions, err := mp.getSolutions(ctx, home7) test.That(t, err, test.ShouldBeNil) @@ -64,7 +63,7 @@ func TestSimpleLinearMotion(t *testing.T) { } nn := &neighborManager{nCPU: nCPU} - cOpt, err := newCbirrtOptions(opt, m) + _, err = newCbirrtOptions(opt) test.That(t, err, test.ShouldBeNil) m1chan := make(chan node, 1) @@ -82,11 +81,11 @@ func TestSimpleLinearMotion(t *testing.T) { cbirrt.constrainedExtend(ctx, cbirrt.randseed, goalMap, near2, seedReached, m1chan) }) goalReached := <-m1chan - dist := opt.DistanceFunc(&Segment{StartConfiguration: seedReached.Q(), EndConfiguration: goalReached.Q()}) - test.That(t, dist < cOpt.JointSolveDist, test.ShouldBeTrue) + dist := opt.DistanceFunc(&ik.Segment{StartConfiguration: seedReached.Q(), EndConfiguration: goalReached.Q()}) + test.That(t, dist < cbirrt.planOpts.JointSolveDist, test.ShouldBeTrue) - corners[seedReached] = true - corners[goalReached] = true + seedReached.SetCorner(true) + goalReached.SetCorner(true) // extract the path to the seed for seedReached != nil { @@ -107,4 +106,6 @@ func TestSimpleLinearMotion(t *testing.T) { unsmoothLen := len(inputSteps) finalSteps := cbirrt.smoothPath(ctx, inputSteps) test.That(t, len(finalSteps), test.ShouldBeLessThanOrEqualTo, unsmoothLen) + // Test that path has changed after smoothing was applied + test.That(t, finalSteps, test.ShouldNotResemble, inputSteps) } diff --git a/motionplan/constraint.go b/motionplan/constraint.go index 49358eacc43..4f44c17e2b6 100644 --- a/motionplan/constraint.go +++ b/motionplan/constraint.go @@ -7,24 +7,14 @@ import ( "github.com/golang/geo/r3" pb "go.viam.com/api/service/motion/v1" + "go.viam.com/rdk/motionplan/ik" "go.viam.com/rdk/pointcloud" "go.viam.com/rdk/referenceframe" spatial "go.viam.com/rdk/spatialmath" ) -// Segment contains all the information a constraint needs to determine validity for a movement. -// It contains the starting inputs, the ending inputs, corresponding poses, and the frame it refers to. -// Pose fields may be empty, and may be filled in by a constraint that needs them. -type Segment struct { - StartPosition spatial.Pose - EndPosition spatial.Pose - StartConfiguration []referenceframe.Input - EndConfiguration []referenceframe.Input - Frame referenceframe.Frame -} - // Given a constraint input with only frames and input positions, calculates the corresponding poses as needed. -func resolveSegmentsToPositions(segment *Segment) error { +func resolveSegmentsToPositions(segment *ik.Segment) error { if segment.StartPosition == nil { if segment.Frame != nil { if segment.StartConfiguration != nil { @@ -60,17 +50,8 @@ func resolveSegmentsToPositions(segment *Segment) error { return nil } -// State contains all the information a constraint needs to determine validity for a movement. -// It contains the starting inputs, the ending inputs, corresponding poses, and the frame it refers to. -// Pose fields may be empty, and may be filled in by a constraint that needs them. -type State struct { - Position spatial.Pose - Configuration []referenceframe.Input - Frame referenceframe.Frame -} - // Given a constraint input with only frames and input positions, calculates the corresponding poses as needed. -func resolveStatesToPositions(state *State) error { +func resolveStatesToPositions(state *ik.State) error { if state.Position == nil { if state.Frame != nil { if state.Configuration != nil { @@ -92,11 +73,11 @@ func resolveStatesToPositions(state *State) error { // SegmentConstraint tests whether a transition from a starting robot configuration to an ending robot configuration is valid. // If the returned bool is true, the constraint is satisfied and the segment is valid. -type SegmentConstraint func(*Segment) bool +type SegmentConstraint func(*ik.Segment) bool // StateConstraint tests whether a given robot configuration is valid // If the returned bool is true, the constraint is satisfied and the state is valid. -type StateConstraint func(*State) bool +type StateConstraint func(*ik.State) bool // ConstraintHandler is a convenient wrapper for constraint handling which is likely to be common among most motion // planners. Including a constraint handler as an anonymous struct member allows reuse. @@ -109,7 +90,7 @@ type ConstraintHandler struct { // Return values are: // -- a bool representing whether all constraints passed // -- if failing, a string naming the failed constraint. -func (c *ConstraintHandler) CheckStateConstraints(state *State) (bool, string) { +func (c *ConstraintHandler) CheckStateConstraints(state *ik.State) (bool, string) { for name, cFunc := range c.stateConstraints { pass := cFunc(state) if !pass { @@ -123,7 +104,7 @@ func (c *ConstraintHandler) CheckStateConstraints(state *State) (bool, string) { // Return values are: // -- a bool representing whether all constraints passed // -- if failing, a string naming the failed constraint. -func (c *ConstraintHandler) CheckSegmentConstraints(segment *Segment) (bool, string) { +func (c *ConstraintHandler) CheckSegmentConstraints(segment *ik.Segment) (bool, string) { for name, cFunc := range c.segmentConstraints { pass := cFunc(segment) if !pass { @@ -137,7 +118,7 @@ func (c *ConstraintHandler) CheckSegmentConstraints(segment *Segment) (bool, str // 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 Segment representing the valid portion of the segment, if any. If no // part of the segment is valid, then `false, nil` is returned. -func (c *ConstraintHandler) CheckStateConstraintsAcrossSegment(ci *Segment, resolution float64) (bool, *Segment) { +func (c *ConstraintHandler) CheckStateConstraintsAcrossSegment(ci *ik.Segment, resolution float64) (bool, *ik.Segment) { // ensure we have cartesian positions err := resolveSegmentsToPositions(ci) if err != nil { @@ -150,7 +131,7 @@ func (c *ConstraintHandler) CheckStateConstraintsAcrossSegment(ci *Segment, reso for i := 0; i <= steps; i++ { interp := float64(i) / float64(steps) interpConfig := referenceframe.InterpolateInputs(ci.StartConfiguration, ci.EndConfiguration, interp) - interpC := &State{Frame: ci.Frame, Configuration: interpConfig} + interpC := &ik.State{Frame: ci.Frame, Configuration: interpConfig} err = resolveStatesToPositions(interpC) if err != nil { return false, nil @@ -161,7 +142,7 @@ func (c *ConstraintHandler) CheckStateConstraintsAcrossSegment(ci *Segment, reso // fail on start pos return false, nil } - return false, &Segment{StartConfiguration: ci.StartConfiguration, EndConfiguration: lastGood} + return false, &ik.Segment{StartConfiguration: ci.StartConfiguration, EndConfiguration: lastGood} } lastGood = interpC.Configuration } @@ -172,7 +153,7 @@ func (c *ConstraintHandler) CheckStateConstraintsAcrossSegment(ci *Segment, reso // CheckSegmentAndStateValidity will check an segment input and confirm that it 1) meets all segment constraints, and 2) meets all // state constraints across the segment at some resolution. If it fails an intermediate state, it will return the shortest valid segment, // provided that segment also meets segment constraints. -func (c *ConstraintHandler) CheckSegmentAndStateValidity(segment *Segment, resolution float64) (bool, *Segment) { +func (c *ConstraintHandler) CheckSegmentAndStateValidity(segment *ik.Segment, resolution float64) (bool, *ik.Segment) { valid, subSegment := c.CheckStateConstraintsAcrossSegment(segment, resolution) if !valid { if subSegment != nil { @@ -336,7 +317,7 @@ func newCollisionConstraint( } // create constraint from reference collision graph - constraint := func(state *State) bool { + constraint := func(state *ik.State) bool { var internalGeoms []spatial.Geometry switch { case state.Configuration != nil: @@ -373,12 +354,12 @@ func newCollisionConstraint( // NewAbsoluteLinearInterpolatingConstraint provides a Constraint whose valid manifold allows a specified amount of deviation from the // shortest straight-line path between the start and the goal. linTol is the allowed linear deviation in mm, orientTol is the allowed // orientation deviation measured by norm of the R3AA orientation difference to the slerp path between start/goal orientations. -func NewAbsoluteLinearInterpolatingConstraint(from, to spatial.Pose, linTol, orientTol float64) (StateConstraint, StateMetric) { +func NewAbsoluteLinearInterpolatingConstraint(from, to spatial.Pose, linTol, orientTol float64) (StateConstraint, ik.StateMetric) { orientConstraint, orientMetric := NewSlerpOrientationConstraint(from, to, orientTol) lineConstraint, lineMetric := NewLineConstraint(from.Point(), to.Point(), linTol) - interpMetric := CombineMetrics(orientMetric, lineMetric) + interpMetric := ik.CombineMetrics(orientMetric, lineMetric) - f := func(state *State) bool { + f := func(state *ik.State) bool { return orientConstraint(state) && lineConstraint(state) } return f, interpMetric @@ -386,8 +367,8 @@ func NewAbsoluteLinearInterpolatingConstraint(from, to spatial.Pose, linTol, ori // NewProportionalLinearInterpolatingConstraint will provide the same metric and constraint as NewAbsoluteLinearInterpolatingConstraint, // except that allowable linear and orientation deviation is scaled based on the distance from start to goal. -func NewProportionalLinearInterpolatingConstraint(from, to spatial.Pose, epsilon float64) (StateConstraint, StateMetric) { - orientTol := epsilon * orientDist(from.Orientation(), to.Orientation()) +func NewProportionalLinearInterpolatingConstraint(from, to spatial.Pose, epsilon float64) (StateConstraint, ik.StateMetric) { + orientTol := epsilon * ik.OrientDist(from.Orientation(), to.Orientation()) linTol := epsilon * from.Point().Distance(to.Point()) return NewAbsoluteLinearInterpolatingConstraint(from, to, linTol, orientTol) @@ -396,22 +377,22 @@ func NewProportionalLinearInterpolatingConstraint(from, to spatial.Pose, epsilon // NewSlerpOrientationConstraint will measure the orientation difference between the orientation of two poses, and return a constraint that // returns whether a given orientation is within a given tolerance distance of the shortest segment between the two orientations, as // well as a metric which returns the distance to that valid region. -func NewSlerpOrientationConstraint(start, goal spatial.Pose, tolerance float64) (StateConstraint, StateMetric) { - origDist := math.Max(orientDist(start.Orientation(), goal.Orientation()), defaultEpsilon) +func NewSlerpOrientationConstraint(start, goal spatial.Pose, tolerance float64) (StateConstraint, ik.StateMetric) { + origDist := math.Max(ik.OrientDist(start.Orientation(), goal.Orientation()), defaultEpsilon) - gradFunc := func(state *State) float64 { - sDist := orientDist(start.Orientation(), state.Position.Orientation()) + gradFunc := func(state *ik.State) float64 { + sDist := ik.OrientDist(start.Orientation(), state.Position.Orientation()) gDist := 0. // If origDist is less than or equal to defaultEpsilon, then the starting and ending orientations are the same and we do not need // to compute the distance to the ending orientation if origDist > defaultEpsilon { - gDist = orientDist(goal.Orientation(), state.Position.Orientation()) + gDist = ik.OrientDist(goal.Orientation(), state.Position.Orientation()) } return (sDist + gDist) - origDist } - validFunc := func(state *State) bool { + validFunc := func(state *ik.State) bool { err := resolveStatesToPositions(state) if err != nil { return false @@ -427,7 +408,7 @@ func NewSlerpOrientationConstraint(start, goal spatial.Pose, tolerance float64) // which will bring a pose into the valid constraint space. The plane normal is assumed to point towards the valid area. // angle refers to the maximum unit sphere segment length deviation from the ov // epsilon refers to the closeness to the plane necessary to be a valid pose. -func NewPlaneConstraint(pNorm, pt r3.Vector, writingAngle, epsilon float64) (StateConstraint, StateMetric) { +func NewPlaneConstraint(pNorm, pt r3.Vector, writingAngle, epsilon float64) (StateConstraint, ik.StateMetric) { // get the constant value for the plane pConst := -pt.Dot(pNorm) @@ -435,7 +416,7 @@ func NewPlaneConstraint(pNorm, pt r3.Vector, writingAngle, epsilon float64) (Sta ov := &spatial.OrientationVector{OX: -pNorm.X, OY: -pNorm.Y, OZ: -pNorm.Z} ov.Normalize() - dFunc := orientDistToRegion(ov, writingAngle) + dFunc := ik.OrientDistToRegion(ov, writingAngle) // distance from plane to point planeDist := func(pt r3.Vector) float64 { @@ -443,13 +424,13 @@ func NewPlaneConstraint(pNorm, pt r3.Vector, writingAngle, epsilon float64) (Sta } // TODO: do we need to care about trajectory here? Probably, but not yet implemented - gradFunc := func(state *State) float64 { + gradFunc := func(state *ik.State) float64 { pDist := planeDist(state.Position.Point()) oDist := dFunc(state.Position.Orientation()) return pDist*pDist + oDist*oDist } - validFunc := func(state *State) bool { + validFunc := func(state *ik.State) bool { err := resolveStatesToPositions(state) if err != nil { return false @@ -464,16 +445,16 @@ func NewPlaneConstraint(pNorm, pt r3.Vector, writingAngle, epsilon float64) (Sta // function which will determine whether a point is on the line, and 2) a distance function // which will bring a pose into the valid constraint space. // tolerance refers to the closeness to the line necessary to be a valid pose in mm. -func NewLineConstraint(pt1, pt2 r3.Vector, tolerance float64) (StateConstraint, StateMetric) { +func NewLineConstraint(pt1, pt2 r3.Vector, tolerance float64) (StateConstraint, ik.StateMetric) { if pt1.Distance(pt2) < defaultEpsilon { tolerance = defaultEpsilon } - gradFunc := func(state *State) float64 { + gradFunc := func(state *ik.State) float64 { return math.Max(spatial.DistToLineSegment(pt1, pt2, state.Position.Point())-tolerance, 0) } - validFunc := func(state *State) bool { + validFunc := func(state *ik.State) bool { err := resolveStatesToPositions(state) if err != nil { return false @@ -488,7 +469,7 @@ func NewLineConstraint(pt1, pt2 r3.Vector, tolerance float64) (StateConstraint, // intersect with points in the octree. Threshold sets the confidence level required for a point to be considered, and buffer is the // distance to a point that is considered a collision in mm. func NewOctreeCollisionConstraint(octree *pointcloud.BasicOctree, threshold int, buffer float64) StateConstraint { - constraint := func(state *State) bool { + constraint := func(state *ik.State) bool { geometries, err := state.Frame.Geometries(state.Configuration) if err != nil && geometries == nil { return false diff --git a/motionplan/constraint_test.go b/motionplan/constraint_test.go index 35464bbdf85..ba1920d6e28 100644 --- a/motionplan/constraint_test.go +++ b/motionplan/constraint_test.go @@ -12,6 +12,7 @@ import ( commonpb "go.viam.com/api/common/v1" "go.viam.com/test" + "go.viam.com/rdk/motionplan/ik" frame "go.viam.com/rdk/referenceframe" spatial "go.viam.com/rdk/spatialmath" "go.viam.com/rdk/utils" @@ -22,7 +23,7 @@ func TestIKTolerances(t *testing.T) { m, err := frame.ParseModelJSONFile(utils.ResolveFile("referenceframe/testjson/ur5eDH.json"), "") test.That(t, err, test.ShouldBeNil) - mp, err := newCBiRRTMotionPlanner(m, rand.New(rand.NewSource(1)), logger, newBasicPlannerOptions()) + mp, err := newCBiRRTMotionPlanner(m, rand.New(rand.NewSource(1)), logger, newBasicPlannerOptions(m)) test.That(t, err, test.ShouldBeNil) // Test inability to arrive at another position due to orientation @@ -38,8 +39,8 @@ func TestIKTolerances(t *testing.T) { test.That(t, err, test.ShouldNotBeNil) // Now verify that setting tolerances to zero allows the same arm to reach that position - opt := newBasicPlannerOptions() - opt.SetGoalMetric(NewPositionOnlyMetric(pos)) + opt := newBasicPlannerOptions(m) + opt.SetGoalMetric(ik.NewPositionOnlyMetric(pos)) opt.SetMaxSolutions(50) mp, err = newCBiRRTMotionPlanner(m, rand.New(rand.NewSource(1)), logger, opt) test.That(t, err, test.ShouldBeNil) @@ -54,7 +55,7 @@ func TestConstraintPath(t *testing.T) { modelXarm, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "") test.That(t, err, test.ShouldBeNil) - ci := &Segment{StartConfiguration: homePos, EndConfiguration: toPos, Frame: modelXarm} + ci := &ik.Segment{StartConfiguration: homePos, EndConfiguration: toPos, Frame: modelXarm} err = resolveSegmentsToPositions(ci) test.That(t, err, test.ShouldBeNil) @@ -75,7 +76,7 @@ func TestConstraintPath(t *testing.T) { test.That(t, len(handler.StateConstraints()), test.ShouldEqual, 1) badInterpPos := frame.FloatsToInputs([]float64{6.2, 0, 0, 0, 0, 0}) - ciBad := &Segment{StartConfiguration: homePos, EndConfiguration: badInterpPos, Frame: modelXarm} + ciBad := &ik.Segment{StartConfiguration: homePos, EndConfiguration: badInterpPos, Frame: modelXarm} err = resolveSegmentsToPositions(ciBad) test.That(t, err, test.ShouldBeNil) ok, failCI = handler.CheckSegmentAndStateValidity(ciBad, 0.5) @@ -133,7 +134,7 @@ func TestLineFollow(t *testing.T) { validFunc, gradFunc := NewLineConstraint(p1.Point(), p2.Point(), 0.001) - pointGrad := gradFunc(&State{Position: query}) + pointGrad := gradFunc(&ik.State{Position: query}) test.That(t, pointGrad, test.ShouldBeLessThan, 0.001*0.001) fs := frame.NewEmptyFrameSystem("test") @@ -154,12 +155,12 @@ func TestLineFollow(t *testing.T) { sf, err := newSolverFrame(fs, markerFrame.Name(), goalFrame.Name(), frame.StartPositions(fs)) test.That(t, err, test.ShouldBeNil) - opt := newBasicPlannerOptions() + opt := newBasicPlannerOptions(sf) opt.SetPathMetric(gradFunc) opt.AddStateConstraint("whiteboard", validFunc) ok, lastGood := opt.CheckSegmentAndStateValidity( - &Segment{ + &ik.Segment{ StartConfiguration: sf.InputFromProtobuf(mp1), EndConfiguration: sf.InputFromProtobuf(mp2), Frame: sf, @@ -169,7 +170,7 @@ func TestLineFollow(t *testing.T) { test.That(t, ok, test.ShouldBeFalse) // lastGood.StartConfiguration and EndConfiguration should pass constraints lastGood.Frame = sf - stateCheck := &State{Configuration: lastGood.StartConfiguration, Frame: lastGood.Frame} + stateCheck := &ik.State{Configuration: lastGood.StartConfiguration, Frame: lastGood.Frame} pass, _ := opt.CheckStateConstraints(stateCheck) test.That(t, pass, test.ShouldBeTrue) @@ -226,7 +227,7 @@ func TestCollisionConstraints(t *testing.T) { // loop through cases and check constraint handler processes them correctly for i, c := range cases { t.Run(fmt.Sprintf("Test %d", i), func(t *testing.T) { - response, failName := handler.CheckStateConstraints(&State{Configuration: c.input, Frame: model}) + response, failName := handler.CheckStateConstraints(&ik.State{Configuration: c.input, Frame: model}) test.That(t, response, test.ShouldEqual, c.expected) test.That(t, failName, test.ShouldEqual, c.failName) }) @@ -266,7 +267,7 @@ func BenchmarkCollisionConstraints(b *testing.B) { // loop through cases and check constraint handler processes them correctly for n = 0; n < b.N; n++ { rfloats := frame.GenerateRandomConfiguration(model, rseed) - b1, _ = handler.CheckStateConstraints(&State{Configuration: frame.FloatsToInputs(rfloats), Frame: model}) + b1, _ = handler.CheckStateConstraints(&ik.State{Configuration: frame.FloatsToInputs(rfloats), Frame: model}) } bt = b1 } diff --git a/motionplan/errors.go b/motionplan/errors.go index 682c9daa020..db9cbe5f1c8 100644 --- a/motionplan/errors.go +++ b/motionplan/errors.go @@ -13,6 +13,12 @@ var ( errNoPlannerOptions = errors.New("PlannerOptions are required but have not been specified") errIKConstraint = "all IK solutions failed constraints. Failures: " + + errNoNeighbors = errors.New("no neighbors found") + + errInvalidCandidate = errors.New("candidate did not meet constraints") + + errNoCandidates = errors.New("no candidates passed in, skipping") ) func genIKConstraintErr(failures map[string]int, constraintFailCnt int) error { diff --git a/motionplan/combinedInverseKinematics.go b/motionplan/ik/combinedInverseKinematics.go similarity index 86% rename from motionplan/combinedInverseKinematics.go rename to motionplan/ik/combinedInverseKinematics.go index 6b528802961..2247b7d92a8 100644 --- a/motionplan/combinedInverseKinematics.go +++ b/motionplan/ik/combinedInverseKinematics.go @@ -1,6 +1,6 @@ //go:build !windows -package motionplan +package ik import ( "context" @@ -30,7 +30,7 @@ func CreateCombinedIKSolver(model referenceframe.Frame, logger golog.Logger, nCP nCPU = 1 } for i := 1; i <= nCPU; i++ { - nlopt, err := CreateNloptIKSolver(model, logger, -1, goalThreshold) + nlopt, err := CreateNloptIKSolver(model, logger, -1, true) nlopt.id = i if err != nil { return nil, err @@ -41,20 +41,10 @@ func CreateCombinedIKSolver(model referenceframe.Frame, logger golog.Logger, nCP return ik, nil } -func runSolver(ctx context.Context, - solver InverseKinematics, - c chan<- []referenceframe.Input, - seed []referenceframe.Input, - m StateMetric, - rseed int, -) error { - return solver.Solve(ctx, c, seed, m, rseed) -} - // Solve will initiate solving for the given position in all child solvers, seeding with the specified initial joint // positions. If unable to solve, the returned error will be non-nil. func (ik *CombinedIK) Solve(ctx context.Context, - c chan<- []referenceframe.Input, + c chan<- *Solution, seed []referenceframe.Input, m StateMetric, rseed int, @@ -75,7 +65,8 @@ func (ik *CombinedIK) Solve(ctx context.Context, utils.PanicCapturingGo(func() { defer activeSolvers.Done() - errChan <- runSolver(ctxWithCancel, thisSolver, c, seed, m, parseed) + + errChan <- thisSolver.Solve(ctxWithCancel, c, seed, m, parseed) }) } @@ -89,6 +80,7 @@ func (ik *CombinedIK) Solve(ctx context.Context, for !done { select { case <-ctx.Done(): + activeSolvers.Wait() return ctx.Err() default: } @@ -110,6 +102,7 @@ func (ik *CombinedIK) Solve(ctx context.Context, // Collect return errors from all solvers select { case <-ctx.Done(): + activeSolvers.Wait() return ctx.Err() default: } @@ -120,6 +113,7 @@ func (ik *CombinedIK) Solve(ctx context.Context, collectedErrs = multierr.Combine(collectedErrs, err) } } + activeSolvers.Wait() return collectedErrs } diff --git a/motionplan/combinedInverseKinematics_windows.go b/motionplan/ik/combinedInverseKinematics_windows.go similarity index 95% rename from motionplan/combinedInverseKinematics_windows.go rename to motionplan/ik/combinedInverseKinematics_windows.go index a95507c87dd..5475b9633d1 100644 --- a/motionplan/combinedInverseKinematics_windows.go +++ b/motionplan/ik/combinedInverseKinematics_windows.go @@ -1,6 +1,6 @@ //go:build windows -package motionplan +package ik import ( "github.com/edaniels/golog" diff --git a/motionplan/ik/inverseKinematics.go b/motionplan/ik/inverseKinematics.go new file mode 100644 index 00000000000..19993aa6d8f --- /dev/null +++ b/motionplan/ik/inverseKinematics.go @@ -0,0 +1,32 @@ +// Package ik contains tols for doing gradient-descent based inverse kinematics, allowing for the minimization of arbitrary metrics +// based on the output of calling `Transform` on the given frame. +package ik + +import ( + "context" + + "go.viam.com/rdk/referenceframe" +) + +const ( + // Default distance below which two distances are considered equal. + defaultEpsilon = 0.001 + + // default amount of closeness to get to the goal. + defaultGoalThreshold = defaultEpsilon * defaultEpsilon +) + +// InverseKinematics defines an interface which, provided with seed inputs and a Metric to minimize to zero, will output all found +// solutions to the provided channel until cancelled or otherwise completes. +type InverseKinematics interface { + // Solve receives a context, the goal arm position, and current joint angles. + Solve(context.Context, chan<- *Solution, []referenceframe.Input, StateMetric, int) error +} + +// Solution is the struct returned from an IK solver. It contains the solution configuration, the score of the solution, and a flag +// indicating whether that configuration and score met the solution criteria requested by the caller. +type Solution struct { + Configuration []referenceframe.Input + Score float64 + Exact bool +} diff --git a/motionplan/ik/inverseKinematics_test.go b/motionplan/ik/inverseKinematics_test.go new file mode 100644 index 00000000000..49831d4cfd2 --- /dev/null +++ b/motionplan/ik/inverseKinematics_test.go @@ -0,0 +1,117 @@ +package ik + +import ( + "context" + "errors" + "math" + "runtime" + "testing" + + "github.com/edaniels/golog" + "github.com/golang/geo/r3" + "go.viam.com/test" + + frame "go.viam.com/rdk/referenceframe" + spatial "go.viam.com/rdk/spatialmath" + "go.viam.com/rdk/utils" +) + +var ( + home = frame.FloatsToInputs([]float64{0, 0, 0, 0, 0, 0}) + nCPU = int(math.Max(1.0, float64(runtime.NumCPU()/4))) +) + +func TestCombinedIKinematics(t *testing.T) { + logger := golog.NewTestLogger(t) + m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "") + test.That(t, err, test.ShouldBeNil) + ik, err := CreateCombinedIKSolver(m, logger, nCPU, defaultGoalThreshold) + test.That(t, err, test.ShouldBeNil) + + // Test ability to arrive at another position + pos := spatial.NewPose( + r3.Vector{X: -46, Y: -133, Z: 372}, + &spatial.OrientationVectorDegrees{OX: 1.79, OY: -1.32, OZ: -1.11}, + ) + solution, err := solveTest(context.Background(), ik, pos, home) + test.That(t, err, test.ShouldBeNil) + + // Test moving forward 20 in X direction from previous position + pos = spatial.NewPose( + r3.Vector{X: -66, Y: -133, Z: 372}, + &spatial.OrientationVectorDegrees{OX: 1.78, OY: -3.3, OZ: -1.11}, + ) + _, err = solveTest(context.Background(), ik, pos, solution[0]) + test.That(t, err, test.ShouldBeNil) +} + +func TestUR5NloptIKinematics(t *testing.T) { + logger := golog.NewTestLogger(t) + + m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/universalrobots/ur5e.json"), "") + test.That(t, err, test.ShouldBeNil) + ik, err := CreateCombinedIKSolver(m, logger, nCPU, defaultGoalThreshold) + test.That(t, err, test.ShouldBeNil) + + goalJP := frame.JointPositionsFromRadians([]float64{-4.128, 2.71, 2.798, 2.3, 1.291, 0.62}) + goal, err := m.Transform(m.InputFromProtobuf(goalJP)) + test.That(t, err, test.ShouldBeNil) + _, err = solveTest(context.Background(), ik, goal, home) + test.That(t, err, test.ShouldBeNil) +} + +func TestCombinedCPUs(t *testing.T) { + logger := golog.NewTestLogger(t) + m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm7_kinematics.json"), "") + test.That(t, err, test.ShouldBeNil) + ik, err := CreateCombinedIKSolver(m, logger, runtime.NumCPU()/400000, defaultGoalThreshold) + test.That(t, err, test.ShouldBeNil) + test.That(t, len(ik.solvers), test.ShouldEqual, 1) +} + +func solveTest(ctx context.Context, solver InverseKinematics, goal spatial.Pose, seed []frame.Input) ([][]frame.Input, error) { + solutionGen := make(chan *Solution) + ikErr := make(chan error) + ctxWithCancel, cancel := context.WithCancel(ctx) + defer cancel() + + // Spawn the IK solver to generate solutions until done + go func() { + defer close(ikErr) + ikErr <- solver.Solve(ctxWithCancel, solutionGen, seed, NewSquaredNormMetric(goal), 1) + }() + + var solutions [][]frame.Input + + // Solve the IK solver. Loop labels are required because `break` etc in a `select` will break only the `select`. +IK: + for { + select { + case <-ctx.Done(): + return nil, ctx.Err() + default: + } + + select { + case step := <-solutionGen: + solutions = append(solutions, step.Configuration) + // Skip the return check below until we have nothing left to read from solutionGen + continue IK + default: + } + + select { + case <-ikErr: + // If we have a return from the IK solver, there are no more solutions, so we finish processing above + // until we've drained the channel + break IK + default: + } + } + cancel() + if len(solutions) == 0 { + return nil, errors.New("unable to solve for position") + } + + return solutions, nil +} diff --git a/motionplan/metrics.go b/motionplan/ik/metrics.go similarity index 71% rename from motionplan/metrics.go rename to motionplan/ik/metrics.go index 6dbcb62636a..38012b0ed2f 100644 --- a/motionplan/metrics.go +++ b/motionplan/ik/metrics.go @@ -1,4 +1,4 @@ -package motionplan +package ik import ( "math" @@ -10,6 +10,26 @@ import ( const orientationDistanceScaling = 10. +// Segment contains all the information a constraint needs to determine validity for a movement. +// It contains the starting inputs, the ending inputs, corresponding poses, and the frame it refers to. +// Pose fields may be empty, and may be filled in by a constraint that needs them. +type Segment struct { + StartPosition spatial.Pose + EndPosition spatial.Pose + StartConfiguration []referenceframe.Input + EndConfiguration []referenceframe.Input + Frame referenceframe.Frame +} + +// State contains all the information a constraint needs to determine validity for a movement. +// It contains the starting inputs, the ending inputs, corresponding poses, and the frame it refers to. +// Pose fields may be empty, and may be filled in by a constraint that needs them. +type State struct { + Position spatial.Pose + Configuration []referenceframe.Input + Frame referenceframe.Frame +} + // StateMetric are functions which, given a State, produces some score. Lower is better. // This is used for gradient descent to converge upon a goal pose, for example. type StateMetric func(*State) float64 @@ -42,15 +62,15 @@ func CombineMetrics(metrics ...StateMetric) StateMetric { return cm.combinedDist } -// orientDist returns the arclength between two orientations in degrees. -func orientDist(o1, o2 spatial.Orientation) float64 { +// OrientDist returns the arclength between two orientations in degrees. +func OrientDist(o1, o2 spatial.Orientation) float64 { return utils.RadToDeg(spatial.QuatToR4AA(spatial.OrientationBetween(o1, o2).Quaternion()).Theta) } -// orientDistToRegion will return a function which will tell you how far the unit sphere component of an orientation +// OrientDistToRegion will return a function which will tell you how far the unit sphere component of an orientation // vector is from a region defined by a point and an arclength around it. The theta value of OV is disregarded. // This is useful, for example, in defining the set of acceptable angles of attack for writing on a whiteboard. -func orientDistToRegion(goal spatial.Orientation, alpha float64) func(spatial.Orientation) float64 { +func OrientDistToRegion(goal spatial.Orientation, alpha float64) func(spatial.Orientation) float64 { ov1 := goal.OrientationVectorRadians() return func(o spatial.Orientation) float64 { ov2 := o.OrientationVectorRadians() @@ -81,7 +101,7 @@ func NewSquaredNormMetric(goal spatial.Pose) StateMetric { // NewPoseFlexOVMetric will provide a distance function which will converge on a pose with an OV within an arclength of `alpha` // of the ov of the goal given. func NewPoseFlexOVMetric(goal spatial.Pose, alpha float64) StateMetric { - oDistFunc := orientDistToRegion(goal.Orientation(), alpha) + oDistFunc := OrientDistToRegion(goal.Orientation(), alpha) return func(state *State) float64 { pDist := state.Position.Point().Distance(goal.Point()) oDist := oDistFunc(state.Position.Orientation()) @@ -113,11 +133,14 @@ func L2InputMetric(segment *Segment) float64 { return referenceframe.InputsL2Distance(segment.StartConfiguration, segment.EndConfiguration) } -// SquaredNormSegmentMetric is a metric which will return the cartesian distance between the two positions. -func SquaredNormSegmentMetric(segment *Segment) float64 { - delta := spatial.PoseDelta(segment.StartPosition, segment.EndPosition) - // Increase weight for orientation since it's a small number - return delta.Point().Norm2() + spatial.QuatToR3AA(delta.Orientation().Quaternion()).Mul(orientationDistanceScaling).Norm2() +// NewSquaredNormSegmentMetric returns a metric which will return the cartesian distance between the two positions. +// It allows the caller to choose the scaling level of orientation. +func NewSquaredNormSegmentMetric(orientationScaleFactor float64) SegmentMetric { + return func(segment *Segment) float64 { + delta := spatial.PoseDelta(segment.StartPosition, segment.EndPosition) + // Increase weight for orientation since it's a small number + return delta.Point().Norm2() + spatial.QuatToR3AA(delta.Orientation().Quaternion()).Mul(orientationScaleFactor).Norm2() + } } // SquaredNormNoOrientSegmentMetric is a metric which will return the cartesian distance between the two positions. diff --git a/motionplan/metrics_test.go b/motionplan/ik/metrics_test.go similarity index 98% rename from motionplan/metrics_test.go rename to motionplan/ik/metrics_test.go index 89fb7f6dd56..d41b470d769 100644 --- a/motionplan/metrics_test.go +++ b/motionplan/ik/metrics_test.go @@ -1,4 +1,4 @@ -package motionplan +package ik import ( "math" diff --git a/motionplan/nloptInverseKinematics.go b/motionplan/ik/nloptInverseKinematics.go similarity index 77% rename from motionplan/nloptInverseKinematics.go rename to motionplan/ik/nloptInverseKinematics.go index b5f8a43d6f8..a454f5a4771 100644 --- a/motionplan/nloptInverseKinematics.go +++ b/motionplan/ik/nloptInverseKinematics.go @@ -1,17 +1,19 @@ //go:build !windows -package motionplan +package ik import ( "context" "math" "math/rand" "strings" + "sync" "github.com/edaniels/golog" "github.com/go-nlopt/nlopt" "github.com/pkg/errors" "go.uber.org/multierr" + "go.viam.com/utils" "go.viam.com/rdk/referenceframe" ) @@ -35,23 +37,32 @@ type NloptIK struct { upperBound []float64 maxIterations int epsilon float64 - solveEpsilon float64 logger golog.Logger jump float64 + + // Nlopt will try to minimize a configuration for whatever is passed in. If exact is false, then the solver will emit partial + // solutions where it was not able to meet the goal criteria but still was able to improve upon the seed. + exact bool +} + +type optimizeReturn struct { + solution []float64 + score float64 + err error } // CreateNloptIKSolver creates an nloptIK object that can perform gradient descent on metrics for Frames. The parameters are the Frame on // which Transform() will be called, a logger, and the number of iterations to run. If the iteration count is less than 1, it will be set // to the default of 5000. -func CreateNloptIKSolver(mdl referenceframe.Frame, logger golog.Logger, iter int, solveEpsilon float64) (*NloptIK, error) { +func CreateNloptIKSolver(mdl referenceframe.Frame, logger golog.Logger, iter int, exact bool) (*NloptIK, error) { ik := &NloptIK{logger: logger} ik.model = mdl ik.id = 0 - // How close we want to get to the goal - ik.epsilon = defaultEpsilon + // Stop optimizing when iterations change by less than this much - ik.solveEpsilon = solveEpsilon + // Also, how close we want to get to the goal region. The metric should reflect any buffer. + ik.epsilon = defaultEpsilon * defaultEpsilon if iter < 1 { // default value iter = 5000 @@ -60,15 +71,16 @@ func CreateNloptIKSolver(mdl referenceframe.Frame, logger golog.Logger, iter int ik.lowerBound, ik.upperBound = limitsToArrays(mdl.DoF()) // How much to adjust joints to determine slope ik.jump = 0.00000001 + ik.exact = exact return ik, nil } // Solve runs the actual solver and sends any solutions found to the given channel. func (ik *NloptIK) Solve(ctx context.Context, - c chan<- []referenceframe.Input, + solutionChan chan<- *Solution, seed []referenceframe.Input, - m StateMetric, + solveMetric StateMetric, rseed int, ) error { //nolint: gosec @@ -90,6 +102,7 @@ func (ik *NloptIK) Solve(ctx context.Context, return errBadBounds } mInput := &State{Frame: ik.model} + var activeSolvers sync.WaitGroup // x is our joint positions // Gradient is, under the hood, a unsafe C structure that we are meant to mutate in place. @@ -109,7 +122,7 @@ func (ik *NloptIK) Solve(ctx context.Context, } mInput.Configuration = inputs mInput.Position = eePos - dist := m(mInput) + dist := solveMetric(mInput) if len(gradient) > 0 { for i := range gradient { @@ -125,7 +138,7 @@ func (ik *NloptIK) Solve(ctx context.Context, } mInput.Configuration = inputs mInput.Position = eePos - dist2 := m(mInput) + dist2 := solveMetric(mInput) gradient[i] = (dist2 - dist) / ik.jump } @@ -134,13 +147,13 @@ func (ik *NloptIK) Solve(ctx context.Context, } err = multierr.Combine( - opt.SetFtolAbs(ik.solveEpsilon), - opt.SetFtolRel(ik.solveEpsilon), + opt.SetFtolAbs(ik.epsilon), + opt.SetFtolRel(ik.epsilon), opt.SetLowerBounds(ik.lowerBound), - opt.SetStopVal(ik.epsilon*ik.epsilon), + opt.SetStopVal(ik.epsilon), opt.SetUpperBounds(ik.upperBound), - opt.SetXtolAbs1(ik.solveEpsilon), - opt.SetXtolRel(ik.solveEpsilon), + opt.SetXtolAbs1(ik.epsilon), + opt.SetXtolRel(ik.epsilon), opt.SetMinObjective(nloptMinFunc), opt.SetMaxEval(nloptStepsPerIter), ) @@ -165,32 +178,52 @@ func (ik *NloptIK) Solve(ctx context.Context, } } - select { - case <-ctx.Done(): - ik.logger.Info("solver halted before solving start; possibly solving twice in a row?") - return err - default: - } - + solveChan := make(chan *optimizeReturn, 1) + defer close(solveChan) for iterations < ik.maxIterations { select { case <-ctx.Done(): return ctx.Err() default: } + + var solutionRaw []float64 + var result float64 + var nloptErr error + iterations++ - solutionRaw, result, nloptErr := opt.Optimize(referenceframe.InputsToFloats(startingPos)) + activeSolvers.Add(1) + utils.PanicCapturingGo(func() { + defer activeSolvers.Done() + solutionRaw, result, nloptErr := opt.Optimize(referenceframe.InputsToFloats(startingPos)) + solveChan <- &optimizeReturn{solutionRaw, result, nloptErr} + }) + select { + case <-ctx.Done(): + err = multierr.Combine(err, opt.ForceStop()) + activeSolvers.Wait() + return multierr.Combine(err, ctx.Err()) + case solution := <-solveChan: + solutionRaw = solution.solution + result = solution.score + nloptErr = solution.err + } if nloptErr != nil { // This just *happens* sometimes due to weirdnesses in nonlinear randomized problems. // Ignore it, something else will find a solution err = multierr.Combine(err, nloptErr) } - if result < ik.epsilon*ik.epsilon { + if result < ik.epsilon || (solutionRaw != nil && !ik.exact) { select { case <-ctx.Done(): return err - case c <- referenceframe.FloatsToInputs(solutionRaw): + default: + } + solutionChan <- &Solution{ + Configuration: referenceframe.FloatsToInputs(solutionRaw), + Score: result, + Exact: result < ik.epsilon, } solutionsFound++ } @@ -266,3 +299,12 @@ func (ik *NloptIK) updateBounds(seed []referenceframe.Input, tries int, opt *nlo opt.SetUpperBounds(newUpper), ) } + +func limitsToArrays(limits []referenceframe.Limit) ([]float64, []float64) { + var min, max []float64 + for _, limit := range limits { + min = append(min, limit.Min) + max = append(max, limit.Max) + } + return min, max +} diff --git a/motionplan/nloptInverseKinematics_test.go b/motionplan/ik/nloptInverseKinematics_test.go similarity index 92% rename from motionplan/nloptInverseKinematics_test.go rename to motionplan/ik/nloptInverseKinematics_test.go index 9716e4e4d0f..7e0fcbe9f48 100644 --- a/motionplan/nloptInverseKinematics_test.go +++ b/motionplan/ik/nloptInverseKinematics_test.go @@ -1,4 +1,4 @@ -package motionplan +package ik import ( "context" @@ -18,7 +18,7 @@ func TestCreateNloptIKSolver(t *testing.T) { logger := golog.NewTestLogger(t) m, err := referenceframe.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "") test.That(t, err, test.ShouldBeNil) - ik, err := CreateNloptIKSolver(m, logger, -1, defaultGoalThreshold) + ik, err := CreateNloptIKSolver(m, logger, -1, false) test.That(t, err, test.ShouldBeNil) ik.id = 1 diff --git a/motionplan/inverseKinematics.go b/motionplan/inverseKinematics.go deleted file mode 100644 index d759acd3a2f..00000000000 --- a/motionplan/inverseKinematics.go +++ /dev/null @@ -1,23 +0,0 @@ -package motionplan - -import ( - "context" - - "go.viam.com/rdk/referenceframe" -) - -// InverseKinematics defines an interface which, provided with seed inputs and a Metric to minimize to zero, will output all found -// solutions to the provided channel until cancelled or otherwise completes. -type InverseKinematics interface { - // Solve receives a context, the goal arm position, and current joint angles. - Solve(context.Context, chan<- []referenceframe.Input, []referenceframe.Input, StateMetric, int) error -} - -func limitsToArrays(limits []referenceframe.Limit) ([]float64, []float64) { - var min, max []float64 - for _, limit := range limits { - min = append(min, limit.Min) - max = append(max, limit.Max) - } - return min, max -} diff --git a/motionplan/kinematic.go b/motionplan/kinematic.go index fc798a1641e..57f5b5e4588 100644 --- a/motionplan/kinematic.go +++ b/motionplan/kinematic.go @@ -36,6 +36,13 @@ func ComputePosition(model referenceframe.Frame, joints *pb.JointPositions) (spa // position of the end effector as a protobuf ArmPosition even when the arm is in an out of bounds state. // This is performed statelessly without changing any data. func ComputeOOBPosition(model referenceframe.Frame, joints *pb.JointPositions) (spatialmath.Pose, error) { + if joints == nil { + return nil, referenceframe.ErrNilJointPositions + } + if model == nil { + return nil, referenceframe.ErrNilModelFrame + } + if len(joints.Values) != len(model.DoF()) { return nil, errors.Errorf( "incorrect number of joints passed to ComputePosition. Want: %d, got: %d", diff --git a/motionplan/kinematic_test.go b/motionplan/kinematic_test.go index ef6a7d3e6d8..2bc36add521 100644 --- a/motionplan/kinematic_test.go +++ b/motionplan/kinematic_test.go @@ -1,14 +1,10 @@ package motionplan import ( - "context" - "errors" "math" "math/rand" - "runtime" "testing" - "github.com/edaniels/golog" "github.com/golang/geo/r3" pb "go.viam.com/api/component/arm/v1" "go.viam.com/test" @@ -19,11 +15,6 @@ import ( "go.viam.com/rdk/utils" ) -var ( - home = frame.FloatsToInputs([]float64{0, 0, 0, 0, 0, 0}) - nCPU = int(math.Max(1.0, float64(runtime.NumCPU()/4))) -) - func BenchmarkFK(b *testing.B) { m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm7_kinematics.json"), "") test.That(b, err, test.ShouldBeNil) @@ -285,45 +276,6 @@ func TestComplicatedDynamicFrameSystem(t *testing.T) { test.That(t, spatial.PoseAlmostCoincident(pointCamToXarm.(*frame.PoseInFrame).Pose(), spatial.NewZeroPose()), test.ShouldBeTrue) } -func TestCombinedIKinematics(t *testing.T) { - logger := golog.NewTestLogger(t) - m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "") - test.That(t, err, test.ShouldBeNil) - ik, err := CreateCombinedIKSolver(m, logger, nCPU, defaultGoalThreshold) - test.That(t, err, test.ShouldBeNil) - - // Test ability to arrive at another position - pos := spatial.NewPose( - r3.Vector{X: -46, Y: -133, Z: 372}, - &spatial.OrientationVectorDegrees{OX: 1.79, OY: -1.32, OZ: -1.11}, - ) - solution, err := solveTest(context.Background(), ik, pos, home) - test.That(t, err, test.ShouldBeNil) - - // Test moving forward 20 in X direction from previous position - pos = spatial.NewPose( - r3.Vector{X: -66, Y: -133, Z: 372}, - &spatial.OrientationVectorDegrees{OX: 1.78, OY: -3.3, OZ: -1.11}, - ) - _, err = solveTest(context.Background(), ik, pos, solution[0]) - test.That(t, err, test.ShouldBeNil) -} - -func TestUR5NloptIKinematics(t *testing.T) { - logger := golog.NewTestLogger(t) - - m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/universalrobots/ur5e.json"), "") - test.That(t, err, test.ShouldBeNil) - ik, err := CreateCombinedIKSolver(m, logger, nCPU, defaultGoalThreshold) - test.That(t, err, test.ShouldBeNil) - - goalJP := frame.JointPositionsFromRadians([]float64{-4.128, 2.71, 2.798, 2.3, 1.291, 0.62}) - goal, err := ComputePosition(m, goalJP) - test.That(t, err, test.ShouldBeNil) - _, err = solveTest(context.Background(), ik, goal, home) - test.That(t, err, test.ShouldBeNil) -} - func TestSVAvsDH(t *testing.T) { mSVA, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/universalrobots/ur5e.json"), "") test.That(t, err, test.ShouldBeNil) @@ -344,62 +296,6 @@ func TestSVAvsDH(t *testing.T) { } } -func TestCombinedCPUs(t *testing.T) { - logger := golog.NewTestLogger(t) - m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm7_kinematics.json"), "") - test.That(t, err, test.ShouldBeNil) - ik, err := CreateCombinedIKSolver(m, logger, runtime.NumCPU()/400000, defaultGoalThreshold) - test.That(t, err, test.ShouldBeNil) - test.That(t, len(ik.solvers), test.ShouldEqual, 1) -} - -func solveTest(ctx context.Context, solver InverseKinematics, goal spatial.Pose, seed []frame.Input) ([][]frame.Input, error) { - solutionGen := make(chan []frame.Input) - ikErr := make(chan error) - ctxWithCancel, cancel := context.WithCancel(ctx) - defer cancel() - - // Spawn the IK solver to generate solutions until done - go func() { - defer close(ikErr) - ikErr <- solver.Solve(ctxWithCancel, solutionGen, seed, NewSquaredNormMetric(goal), 1) - }() - - var solutions [][]frame.Input - - // Solve the IK solver. Loop labels are required because `break` etc in a `select` will break only the `select`. -IK: - for { - select { - case <-ctx.Done(): - return nil, ctx.Err() - default: - } - - select { - case step := <-solutionGen: - solutions = append(solutions, step) - // Skip the return check below until we have nothing left to read from solutionGen - continue IK - default: - } - - select { - case <-ikErr: - // If we have a return from the IK solver, there are no more solutions, so we finish processing above - // until we've drained the channel - break IK - default: - } - } - cancel() - if len(solutions) == 0 { - return nil, errors.New("unable to solve for position") - } - - return solutions, nil -} - // Test loading model kinematics of the same arm via ModelJSON parsing and URDF parsing and comparing results. func TestKinematicsJSONvsURDF(t *testing.T) { numTests := 100 @@ -421,3 +317,35 @@ func TestKinematicsJSONvsURDF(t *testing.T) { test.That(t, spatial.PoseAlmostEqual(posJSON, posURDF), test.ShouldBeTrue) } } + +func TestComputeOOBPosition(t *testing.T) { + model, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "foo") + test.That(t, err, test.ShouldBeNil) + test.That(t, model.Name(), test.ShouldEqual, "foo") + + jointPositions := &pb.JointPositions{Values: []float64{1.1, 2.2, 3.3, 1.1, 2.2, 3.3}} + + t.Run("succeed", func(t *testing.T) { + pose, err := ComputeOOBPosition(model, jointPositions) + test.That(t, err, test.ShouldBeNil) + test.That(t, pose, test.ShouldNotBeNil) + }) + + t.Run("fail when JointPositions are nil", func(t *testing.T) { + var NilJointPositions *pb.JointPositions + + pose, err := ComputeOOBPosition(model, NilJointPositions) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, pose, test.ShouldBeNil) + test.That(t, err, test.ShouldEqual, frame.ErrNilJointPositions) + }) + + t.Run("fail when model frame is nil", func(t *testing.T) { + var NilModel frame.Model + + pose, err := ComputeOOBPosition(NilModel, jointPositions) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, pose, test.ShouldBeNil) + test.That(t, err, test.ShouldEqual, frame.ErrNilModelFrame) + }) +} diff --git a/motionplan/motionPlanner.go b/motionplan/motionPlanner.go index daf984ae050..9fba9cf2dac 100644 --- a/motionplan/motionPlanner.go +++ b/motionplan/motionPlanner.go @@ -5,6 +5,7 @@ import ( "context" "math/rand" "sort" + "sync" "time" "github.com/edaniels/golog" @@ -12,6 +13,7 @@ import ( pb "go.viam.com/api/service/motion/v1" "go.viam.com/utils" + "go.viam.com/rdk/motionplan/ik" frame "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" ) @@ -23,7 +25,7 @@ const defaultRandomSeed = 0 type motionPlanner interface { // Plan will take a context, a goal position, and an input start state and return a series of state waypoints which // should be visited in order to arrive at the goal while satisfying all constraints - plan(context.Context, spatialmath.Pose, []frame.Input) ([][]frame.Input, error) + plan(context.Context, spatialmath.Pose, []frame.Input) ([]node, error) // Everything below this point should be covered by anything that wraps the generic `planner` smoothPath(context.Context, []node) []node @@ -31,75 +33,38 @@ type motionPlanner interface { checkInputs([]frame.Input) bool getSolutions(context.Context, []frame.Input) ([]node, error) opt() *plannerOptions + sample(node, int) (node, error) } type plannerConstructor func(frame.Frame, *rand.Rand, golog.Logger, *plannerOptions) (motionPlanner, error) -// PlanMotion plans a motion to destination for a given frame. It takes a given frame system, wraps it with a SolvableFS, and solves. -func PlanMotion(ctx context.Context, - logger golog.Logger, - dst *frame.PoseInFrame, - f frame.Frame, - seedMap map[string][]frame.Input, - fs frame.FrameSystem, - worldState *frame.WorldState, - constraintSpec *pb.Constraints, - planningOpts map[string]interface{}, -) ([]map[string][]frame.Input, error) { - return motionPlanInternal(ctx, logger, dst, f, seedMap, fs, worldState, constraintSpec, planningOpts) +// PlanRequest is a struct to store all the data necessary to make a call to PlanMotion. +type PlanRequest struct { + Logger golog.Logger + Goal *frame.PoseInFrame + Frame frame.Frame + FrameSystem frame.FrameSystem + StartConfiguration map[string][]frame.Input + WorldState *frame.WorldState + ConstraintSpecs *pb.Constraints + Options map[string]interface{} } -// PlanFrameMotion plans a motion to destination for a given frame with no frame system. It will create a new FS just for the plan. -// WorldState is not supported in the absence of a real frame system. -func PlanFrameMotion(ctx context.Context, - logger golog.Logger, - dst spatialmath.Pose, - f frame.Frame, - seed []frame.Input, - constraintSpec *pb.Constraints, - planningOpts map[string]interface{}, -) ([][]frame.Input, error) { - // ephemerally create a framesystem containing just the frame for the solve - fs := frame.NewEmptyFrameSystem("") - if err := fs.AddFrame(f, fs.World()); err != nil { - return nil, err - } - destination := frame.NewPoseInFrame(frame.World, dst) - seedMap := map[string][]frame.Input{f.Name(): seed} - solutionMap, err := motionPlanInternal(ctx, logger, destination, f, seedMap, fs, nil, constraintSpec, planningOpts) - if err != nil { - return nil, err - } - return FrameStepsFromRobotPath(f.Name(), solutionMap) -} - -// motionPlanInternal is the internal private function that all motion planning access calls. This will construct the plan manager for each -// waypoint, and return at the end. -func motionPlanInternal(ctx context.Context, - logger golog.Logger, - goal *frame.PoseInFrame, - f frame.Frame, - seedMap map[string][]frame.Input, - fs frame.FrameSystem, - worldState *frame.WorldState, - constraintSpec *pb.Constraints, - motionConfig map[string]interface{}, -) ([]map[string][]frame.Input, error) { - if goal == nil { +// PlanMotion plans a motion from a provided plan request. +func PlanMotion(ctx context.Context, request *PlanRequest) (Plan, error) { + if request.Goal == nil { return nil, errors.New("no destination passed to Motion") } - steps := []map[string][]frame.Input{} - // Create a frame to solve for, and an IK solver with that frame. - sf, err := newSolverFrame(fs, f.Name(), goal.Parent(), seedMap) + sf, err := newSolverFrame(request.FrameSystem, request.Frame.Name(), request.Goal.Parent(), request.StartConfiguration) if err != nil { return nil, err } if len(sf.DoF()) == 0 { return nil, errors.New("solver frame has no degrees of freedom, cannot perform inverse kinematics") } - seed, err := sf.mapToSlice(seedMap) + seed, err := sf.mapToSlice(request.StartConfiguration) if err != nil { return nil, err } @@ -108,42 +73,78 @@ func motionPlanInternal(ctx context.Context, return nil, err } - logger.Infof( - "planning motion for frame %s. Goal: %v Starting seed map %v, startPose %v, worldstate: %v", - f.Name(), - frame.PoseInFrameToProtobuf(goal), - seedMap, + request.Logger.Infof( + "planning motion for frame %s\nGoal: %v\nStarting seed map %v\n, startPose %v\n, worldstate: %v\n", + request.Frame.Name(), + frame.PoseInFrameToProtobuf(request.Goal), + request.StartConfiguration, spatialmath.PoseToProtobuf(startPose), - worldState.String(), + request.WorldState.String(), ) - logger.Debugf("constraint specs for this step: %v", constraintSpec) - logger.Debugf("motion config for this step: %v", motionConfig) + request.Logger.Debugf("constraint specs for this step: %v", request.ConstraintSpecs) + request.Logger.Debugf("motion config for this step: %v", request.Options) rseed := defaultRandomSeed - if seed, ok := motionConfig["rseed"].(int); ok { + if seed, ok := request.Options["rseed"].(int); ok { rseed = seed } - - sfPlanner, err := newPlanManager(sf, fs, logger, rseed) + sfPlanner, err := newPlanManager(sf, request.FrameSystem, request.Logger, rseed) if err != nil { return nil, err } - resultSlices, err := sfPlanner.PlanSingleWaypoint(ctx, seedMap, goal.Pose(), worldState, constraintSpec, motionConfig) + + resultSlices, err := sfPlanner.PlanSingleWaypoint( + ctx, + request.StartConfiguration, + request.Goal.Pose(), + request.WorldState, + request.ConstraintSpecs, + request.Options, + ) if err != nil { return nil, err } + plan := Plan{} for _, resultSlice := range resultSlices { stepMap := sf.sliceToMap(resultSlice) - steps = append(steps, stepMap) + plan = append(plan, stepMap) } + request.Logger.Debugf("final plan steps: %s", plan.String()) + return plan, nil +} - logger.Debugf("final plan steps: %v", steps) - - return steps, nil +// PlanFrameMotion plans a motion to destination for a given frame with no frame system. It will create a new FS just for the plan. +// WorldState is not supported in the absence of a real frame system. +func PlanFrameMotion(ctx context.Context, + logger golog.Logger, + dst spatialmath.Pose, + f frame.Frame, + seed []frame.Input, + constraintSpec *pb.Constraints, + planningOpts map[string]interface{}, +) ([][]frame.Input, error) { + // ephemerally create a framesystem containing just the frame for the solve + fs := frame.NewEmptyFrameSystem("") + if err := fs.AddFrame(f, fs.World()); err != nil { + return nil, err + } + plan, err := PlanMotion(ctx, &PlanRequest{ + Logger: logger, + Goal: frame.NewPoseInFrame(frame.World, dst), + Frame: f, + StartConfiguration: map[string][]frame.Input{f.Name(): seed}, + FrameSystem: fs, + ConstraintSpecs: constraintSpec, + Options: planningOpts, + }) + if err != nil { + return nil, err + } + return plan.GetFrameSteps(f.Name()) } type planner struct { - solver InverseKinematics + solver ik.InverseKinematics frame frame.Frame logger golog.Logger randseed *rand.Rand @@ -152,12 +153,12 @@ type planner struct { } func newPlanner(frame frame.Frame, seed *rand.Rand, logger golog.Logger, opt *plannerOptions) (*planner, error) { - ik, err := CreateCombinedIKSolver(frame, logger, opt.NumThreads, opt.GoalThreshold) + solver, err := ik.CreateCombinedIKSolver(frame, logger, opt.NumThreads, opt.GoalThreshold) if err != nil { return nil, err } mp := &planner{ - solver: ik, + solver: solver, frame: frame, logger: logger, randseed: seed, @@ -167,7 +168,7 @@ func newPlanner(frame frame.Frame, seed *rand.Rand, logger golog.Logger, opt *pl } func (mp *planner) checkInputs(inputs []frame.Input) bool { - ok, _ := mp.planOpts.CheckStateConstraints(&State{ + ok, _ := mp.planOpts.CheckStateConstraints(&ik.State{ Configuration: inputs, Frame: mp.frame, }) @@ -176,7 +177,7 @@ func (mp *planner) checkInputs(inputs []frame.Input) bool { func (mp *planner) checkPath(seedInputs, target []frame.Input) bool { ok, _ := mp.planOpts.CheckSegmentAndStateValidity( - &Segment{ + &ik.Segment{ StartConfiguration: seedInputs, EndConfiguration: target, Frame: mp.frame, @@ -186,6 +187,20 @@ func (mp *planner) checkPath(seedInputs, target []frame.Input) bool { return ok } +func (mp *planner) sample(rSeed node, sampleNum int) (node, error) { + // If we have done more than 50 iterations, start seeding off completely random positions 2 at a time + // The 2 at a time is to ensure random seeds are added onto both the seed and goal maps. + if sampleNum >= mp.planOpts.IterBeforeRand && sampleNum%4 >= 2 { + return newConfigurationNode(frame.RandomFrameInputs(mp.frame, mp.randseed)), nil + } + // Seeding nearby to valid points results in much faster convergence in less constrained space + q, err := frame.RestrictedRandomFrameInputs(mp.frame, mp.randseed, 0.1, rSeed.Q()) + if err != nil { + return nil, err + } + return newConfigurationNode(q), nil +} + func (mp *planner) opt() *plannerOptions { return mp.planOpts } @@ -217,7 +232,6 @@ func (mp *planner) smoothPath(ctx context.Context, path []node) []node { // Intn will return an int in the half-open interval half-open interval [0,n) firstEdge := mp.randseed.Intn(len(path) - 2) secondEdge := firstEdge + 1 + mp.randseed.Intn((len(path)-2)-firstEdge) - mp.logger.Debugf("checking shortcut between nodes %d and %d", firstEdge, secondEdge+1) wayPoint1 := frame.InterpolateInputs(path[firstEdge].Q(), path[firstEdge+1].Q(), waypoints[mp.randseed.Intn(3)]) wayPoint2 := frame.InterpolateInputs(path[secondEdge].Q(), path[secondEdge+1].Q(), waypoints[mp.randseed.Intn(3)]) @@ -255,11 +269,15 @@ func (mp *planner) getSolutions(ctx context.Context, seed []frame.Input) ([]node ctxWithCancel, cancel := context.WithCancel(ctx) defer cancel() - solutionGen := make(chan []frame.Input) + solutionGen := make(chan *ik.Solution, mp.planOpts.NumThreads*2) ikErr := make(chan error, 1) + var activeSolvers sync.WaitGroup + defer activeSolvers.Wait() + activeSolvers.Add(1) // Spawn the IK solver to generate solutions until done utils.PanicCapturingGo(func() { defer close(ikErr) + defer activeSolvers.Done() ikErr <- mp.solver.Solve(ctxWithCancel, solutionGen, seed, mp.planOpts.goalMetric, mp.randseed.Int()) }) @@ -279,14 +297,15 @@ IK: } select { - case step := <-solutionGen: + case stepSolution := <-solutionGen: + step := stepSolution.Configuration // Ensure the end state is a valid one - statePass, failName := mp.planOpts.CheckStateConstraints(&State{ + statePass, failName := mp.planOpts.CheckStateConstraints(&ik.State{ Configuration: step, Frame: mp.frame, }) if statePass { - stepArc := &Segment{ + stepArc := &ik.Segment{ StartConfiguration: seed, StartPosition: seedPos, EndConfiguration: step, @@ -332,6 +351,13 @@ IK: // Cancel any ongoing processing within the IK solvers if we're done receiving solutions cancel() + for done := false; !done; { + select { + case <-solutionGen: + default: + done = true + } + } if len(solutions) == 0 { // We have failed to produce a usable IK solution. Let the user know if zero IK solutions were produced, or if non-zero solutions diff --git a/motionplan/motionPlanner_test.go b/motionplan/motionPlanner_test.go index 449b48be337..685bf444ef3 100644 --- a/motionplan/motionPlanner_test.go +++ b/motionplan/motionPlanner_test.go @@ -13,6 +13,7 @@ import ( motionpb "go.viam.com/api/service/motion/v1" "go.viam.com/test" + "go.viam.com/rdk/motionplan/ik" frame "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" "go.viam.com/rdk/utils" @@ -95,18 +96,18 @@ func constrainedXArmMotion() (*planConfig, error) { // Test ability to arrive at another position pos := spatialmath.NewPoseFromProtobuf(&commonpb.Pose{X: -206, Y: 100, Z: 120, OZ: -1}) - opt := newBasicPlannerOptions() - orientMetric := NewPoseFlexOVMetric(pos, 0.09) + opt := newBasicPlannerOptions(model) + orientMetric := ik.NewPoseFlexOVMetric(pos, 0.09) - oFunc := orientDistToRegion(pos.Orientation(), 0.1) - oFuncMet := func(from *State) float64 { + oFunc := ik.OrientDistToRegion(pos.Orientation(), 0.1) + oFuncMet := func(from *ik.State) float64 { err := resolveStatesToPositions(from) if err != nil { return math.Inf(1) } return oFunc(from.Position.Orientation()) } - orientConstraint := func(cInput *State) bool { + orientConstraint := func(cInput *ik.State) bool { err := resolveStatesToPositions(cInput) if err != nil { return false @@ -141,17 +142,13 @@ func TestPlanningWithGripper(t *testing.T) { zeroPos := frame.StartPositions(fs) newPose := frame.NewPoseInFrame("gripper", spatialmath.NewPoseFromPoint(r3.Vector{100, 100, 0})) - solutionMap, err := PlanMotion( - context.Background(), - logger.Sugar(), - newPose, - gripper, - zeroPos, - fs, - nil, - nil, - nil, - ) + solutionMap, err := PlanMotion(context.Background(), &PlanRequest{ + Logger: logger.Sugar(), + Goal: newPose, + Frame: gripper, + StartConfiguration: zeroPos, + FrameSystem: fs, + }) test.That(t, err, test.ShouldBeNil) test.That(t, len(solutionMap), test.ShouldBeGreaterThanOrEqualTo, 2) } @@ -174,7 +171,7 @@ func TestPlanningWithGripper(t *testing.T) { // ------------------------. func simple2DMap() (*planConfig, error) { // build model - limits := []frame.Limit{{Min: -100, Max: 100}, {Min: -100, Max: 100}} + limits := []frame.Limit{{Min: -100, Max: 100}, {Min: -100, Max: 100}, {Min: -2 * math.Pi, Max: 2 * math.Pi}} physicalGeometry, err := spatialmath.NewBox(spatialmath.NewZeroPose(), r3.Vector{X: 10, Y: 10, Z: 10}, "") if err != nil { return nil, err @@ -205,11 +202,11 @@ func simple2DMap() (*planConfig, error) { } // setup planner options - opt := newBasicPlannerOptions() + opt := newBasicPlannerOptions(model) startInput := frame.StartPositions(fs) startInput[modelName] = frame.FloatsToInputs([]float64{-90., 90., 0}) goal := spatialmath.NewPoseFromPoint(r3.Vector{X: 90, Y: 90, Z: 0}) - opt.SetGoalMetric(NewSquaredNormMetric(goal)) + opt.SetGoalMetric(ik.NewSquaredNormMetric(goal)) sf, err := newSolverFrame(fs, modelName, frame.World, startInput) if err != nil { return nil, err @@ -246,8 +243,8 @@ func simpleXArmMotion() (*planConfig, error) { goal := spatialmath.NewPoseFromProtobuf(&commonpb.Pose{X: 206, Y: 100, Z: 120, OZ: -1}) // setup planner options - opt := newBasicPlannerOptions() - opt.SetGoalMetric(NewSquaredNormMetric(goal)) + opt := newBasicPlannerOptions(xarm) + opt.SetGoalMetric(ik.NewSquaredNormMetric(goal)) sf, err := newSolverFrame(fs, xarm.Name(), frame.World, frame.StartPositions(fs)) if err != nil { return nil, err @@ -281,8 +278,8 @@ func simpleUR5eMotion() (*planConfig, error) { goal := spatialmath.NewPoseFromProtobuf(&commonpb.Pose{X: -750, Y: -250, Z: 200, OX: -1}) // setup planner options - opt := newBasicPlannerOptions() - opt.SetGoalMetric(NewSquaredNormMetric(goal)) + opt := newBasicPlannerOptions(ur5e) + opt.SetGoalMetric(ik.NewSquaredNormMetric(goal)) sf, err := newSolverFrame(fs, ur5e.Name(), frame.World, frame.StartPositions(fs)) if err != nil { return nil, err @@ -313,13 +310,14 @@ func testPlanner(t *testing.T, plannerFunc plannerConstructor, config planConfig test.That(t, err, test.ShouldBeNil) mp, err := plannerFunc(cfg.RobotFrame, rand.New(rand.NewSource(int64(seed))), logger.Sugar(), cfg.Options) test.That(t, err, test.ShouldBeNil) - path, err := mp.plan(context.Background(), cfg.Goal, cfg.Start) + pathNodes, err := mp.plan(context.Background(), cfg.Goal, cfg.Start) test.That(t, err, test.ShouldBeNil) + path := nodesToInputs(pathNodes) // test that path doesn't violate constraints test.That(t, len(path), test.ShouldBeGreaterThanOrEqualTo, 2) for j := 0; j < len(path)-1; j++ { - ok, _ := cfg.Options.ConstraintHandler.CheckSegmentAndStateValidity(&Segment{ + ok, _ := cfg.Options.ConstraintHandler.CheckSegmentAndStateValidity(&ik.Segment{ StartConfiguration: path[j], EndConfiguration: path[j+1], Frame: cfg.RobotFrame, @@ -375,17 +373,13 @@ func TestArmOOBSolve(t *testing.T) { // Set a goal unreachable by the UR due to sheer distance goal1 := spatialmath.NewPose(r3.Vector{X: 257, Y: 21000, Z: -300}, &spatialmath.OrientationVectorDegrees{OZ: -1}) - _, err := PlanMotion( - context.Background(), - logger.Sugar(), - frame.NewPoseInFrame(frame.World, goal1), - fs.Frame("urCamera"), - positions, - fs, - nil, - nil, - nil, - ) + _, err := PlanMotion(context.Background(), &PlanRequest{ + Logger: logger.Sugar(), + Goal: frame.NewPoseInFrame(frame.World, goal1), + Frame: fs.Frame("urCamera"), + StartConfiguration: positions, + FrameSystem: fs, + }) test.That(t, err, test.ShouldNotBeNil) test.That(t, err.Error(), test.ShouldEqual, errIKSolve.Error()) } @@ -405,17 +399,14 @@ func TestArmObstacleSolve(t *testing.T) { // Set a goal unreachable by the UR goal1 := spatialmath.NewPose(r3.Vector{X: 257, Y: 210, Z: -300}, &spatialmath.OrientationVectorDegrees{OZ: -1}) - _, err = PlanMotion( - context.Background(), - logger.Sugar(), - frame.NewPoseInFrame(frame.World, goal1), - fs.Frame("urCamera"), - positions, - fs, - worldState, - nil, - nil, - ) + _, err = PlanMotion(context.Background(), &PlanRequest{ + Logger: logger.Sugar(), + Goal: frame.NewPoseInFrame(frame.World, goal1), + Frame: fs.Frame("urCamera"), + StartConfiguration: positions, + FrameSystem: fs, + WorldState: worldState, + }) test.That(t, err, test.ShouldNotBeNil) test.That(t, err.Error(), test.ShouldContainSubstring, errIKConstraint) } @@ -434,17 +425,13 @@ func TestArmAndGantrySolve(t *testing.T) { // Set a goal such that the gantry and arm must both be used to solve goal1 := spatialmath.NewPose(r3.Vector{X: 257, Y: 2100, Z: -300}, &spatialmath.OrientationVectorDegrees{OZ: -1}) - plan, err := PlanMotion( - context.Background(), - logger.Sugar(), - frame.NewPoseInFrame(frame.World, goal1), - fs.Frame("xArmVgripper"), - positions, - fs, - nil, - nil, - nil, - ) + plan, err := PlanMotion(context.Background(), &PlanRequest{ + Logger: logger.Sugar(), + Goal: frame.NewPoseInFrame(frame.World, goal1), + Frame: fs.Frame("xArmVgripper"), + StartConfiguration: positions, + FrameSystem: fs, + }) test.That(t, err, test.ShouldBeNil) solvedPose, err := fs.Transform( plan[len(plan)-1], @@ -460,17 +447,14 @@ func TestMultiArmSolve(t *testing.T) { positions := frame.StartPositions(fs) // Solve such that the ur5 and xArm are pointing at each other, 60mm from gripper to camera goal2 := spatialmath.NewPose(r3.Vector{Z: 60}, &spatialmath.OrientationVectorDegrees{OZ: -1}) - plan, err := PlanMotion( - context.Background(), - logger.Sugar(), - frame.NewPoseInFrame("urCamera", goal2), - fs.Frame("xArmVgripper"), - positions, - fs, - nil, - nil, - map[string]interface{}{"max_ik_solutions": 100, "timeout": 150.0}, - ) + plan, err := PlanMotion(context.Background(), &PlanRequest{ + Logger: logger.Sugar(), + Goal: frame.NewPoseInFrame("urCamera", goal2), + Frame: fs.Frame("xArmVgripper"), + StartConfiguration: positions, + FrameSystem: fs, + Options: map[string]interface{}{"max_ik_solutions": 100, "timeout": 150.0}, + }) test.That(t, err, test.ShouldBeNil) // Both frames should wind up at the goal relative to one another @@ -502,7 +486,15 @@ func TestReachOverArm(t *testing.T) { // plan to a location, it should interpolate to get there opts := map[string]interface{}{"max_ik_solutions": 100, "timeout": 150.0} - plan, err := PlanMotion(context.Background(), logger.Sugar(), goal, xarm, frame.StartPositions(fs), fs, nil, nil, opts) + plan, err := PlanMotion(context.Background(), &PlanRequest{ + Logger: logger.Sugar(), + Goal: goal, + Frame: xarm, + StartConfiguration: frame.StartPositions(fs), + FrameSystem: fs, + Options: opts, + }) + test.That(t, err, test.ShouldBeNil) test.That(t, len(plan), test.ShouldEqual, 2) @@ -513,7 +505,14 @@ func TestReachOverArm(t *testing.T) { // the plan should no longer be able to interpolate, but it should still be able to get there opts = map[string]interface{}{"max_ik_solutions": 100, "timeout": 150.0} - plan, err = PlanMotion(context.Background(), logger.Sugar(), goal, xarm, frame.StartPositions(fs), fs, nil, nil, opts) + plan, err = PlanMotion(context.Background(), &PlanRequest{ + Logger: logger.Sugar(), + Goal: goal, + Frame: xarm, + StartConfiguration: frame.StartPositions(fs), + FrameSystem: fs, + Options: opts, + }) test.That(t, err, test.ShouldBeNil) test.That(t, len(plan), test.ShouldBeGreaterThan, 2) } @@ -527,7 +526,7 @@ func TestPlanMapMotion(t *testing.T) { test.That(t, err, test.ShouldBeNil) model, err := frame.New2DMobileModelFrame( "test", - []frame.Limit{{-100, 100}, {-100, 100}}, + []frame.Limit{{-100, 100}, {-100, 100}, {-2 * math.Pi, 2 * math.Pi}}, sphere, ) test.That(t, err, test.ShouldBeNil) @@ -540,7 +539,6 @@ func TestPlanMapMotion(t *testing.T) { ) test.That(t, err, test.ShouldBeNil) - // TODO(RSDK-2314): when MoveOnMap is implemented this will need to change to PlanMapMotion PlanMapMotion := func( ctx context.Context, logger golog.Logger, @@ -556,11 +554,18 @@ func TestPlanMapMotion(t *testing.T) { } destination := frame.NewPoseInFrame(frame.World, dst) seedMap := map[string][]frame.Input{f.Name(): seed} - solutionMap, err := motionPlanInternal(ctx, logger, destination, f, seedMap, fs, worldState, nil, nil) + plan, err := PlanMotion(ctx, &PlanRequest{ + Logger: logger, + Goal: destination, + Frame: f, + StartConfiguration: seedMap, + FrameSystem: fs, + WorldState: worldState, + }) if err != nil { return nil, err } - return FrameStepsFromRobotPath(f.Name(), solutionMap) + return plan.GetFrameSteps(f.Name()) } plan, err := PlanMapMotion(ctx, logger, dst, model, make([]frame.Input, 3), worldState) @@ -620,17 +625,15 @@ func TestArmConstraintSpecificationSolve(t *testing.T) { checkReachable := func(worldState *frame.WorldState, constraints *motionpb.Constraints) error { goal := spatialmath.NewPose(r3.Vector{X: 600, Y: 100, Z: 300}, &spatialmath.OrientationVectorDegrees{OX: 1}) - _, err := PlanMotion( - context.Background(), - logger.Sugar(), - frame.NewPoseInFrame(frame.World, goal), - fs.Frame("xArmVgripper"), - frame.StartPositions(fs), - fs, - worldState, - constraints, - nil, - ) + _, err := PlanMotion(context.Background(), &PlanRequest{ + Logger: logger.Sugar(), + Goal: frame.NewPoseInFrame(frame.World, goal), + Frame: fs.Frame("xArmVgripper"), + FrameSystem: fs, + StartConfiguration: frame.StartPositions(fs), + WorldState: worldState, + ConstraintSpecs: constraints, + }) return err } diff --git a/motionplan/nearestNeighbor.go b/motionplan/nearestNeighbor.go index aeb371fb081..2ff20bc46e3 100644 --- a/motionplan/nearestNeighbor.go +++ b/motionplan/nearestNeighbor.go @@ -4,9 +4,10 @@ import ( "context" "math" "sort" - "sync" "go.viam.com/utils" + + "go.viam.com/rdk/motionplan/ik" ) const defaultNeighborsBeforeParallelization = 1000 @@ -14,9 +15,7 @@ const defaultNeighborsBeforeParallelization = 1000 type neighborManager struct { nnKeys chan node neighbors chan *neighbor - nnLock sync.RWMutex seedPos node - ready bool nCPU int parallelNeighbors int } @@ -27,20 +26,32 @@ type neighbor struct { } //nolint:revive -func kNearestNeighbors(planOpts *plannerOptions, rrtMap map[node]node, target node, neighborhoodSize int) []*neighbor { +func kNearestNeighbors(planOpts *plannerOptions, tree rrtMap, target node, neighborhoodSize int) []*neighbor { kNeighbors := neighborhoodSize - if neighborhoodSize > len(rrtMap) { - kNeighbors = len(rrtMap) + if neighborhoodSize > len(tree) { + kNeighbors = len(tree) } allCosts := make([]*neighbor, 0) - for rrtnode := range rrtMap { - dist := planOpts.DistanceFunc(&Segment{ + for rrtnode := range tree { + dist := planOpts.DistanceFunc(&ik.Segment{ StartConfiguration: target.Q(), EndConfiguration: rrtnode.Q(), }) allCosts = append(allCosts, &neighbor{dist: dist, node: rrtnode}) } + // sort neighbors by their distance to target first so that first nearest neighbor isn't always the start node of tree + sort.Slice(allCosts, func(i, j int) bool { + if !math.IsNaN(allCosts[i].node.Cost()) { + if !math.IsNaN(allCosts[j].node.Cost()) { + return allCosts[i].dist < allCosts[j].dist + } + } + return allCosts[i].dist < allCosts[j].dist + }) + allCosts = allCosts[:kNeighbors] + // sort k nearest distance neighbors by "total cost to target" metric so that target's nearest neighbor + // provides the smallest cost path from start node to target sort.Slice(allCosts, func(i, j int) bool { if !math.IsNaN(allCosts[i].node.Cost()) { if !math.IsNaN(allCosts[j].node.Cost()) { @@ -49,27 +60,28 @@ func kNearestNeighbors(planOpts *plannerOptions, rrtMap map[node]node, target no } return allCosts[i].dist < allCosts[j].dist }) - return allCosts[:kNeighbors] + return allCosts } +// Can return `nil` when the context is canceled during processing. func (nm *neighborManager) nearestNeighbor( ctx context.Context, planOpts *plannerOptions, seed node, - rrtMap map[node]node, + tree rrtMap, ) node { if nm.parallelNeighbors == 0 { nm.parallelNeighbors = defaultNeighborsBeforeParallelization } - if len(rrtMap) > nm.parallelNeighbors && nm.nCPU > 1 { + if len(tree) > nm.parallelNeighbors && nm.nCPU > 1 { // If the map is large, calculate distances in parallel - return nm.parallelNearestNeighbor(ctx, planOpts, seed, rrtMap) + return nm.parallelNearestNeighbor(ctx, planOpts, seed, tree) } bestDist := math.Inf(1) var best node - for k := range rrtMap { - seg := &Segment{ + for k := range tree { + seg := &ik.Segment{ StartConfiguration: seed.Q(), EndConfiguration: k.Q(), } @@ -92,93 +104,78 @@ func (nm *neighborManager) parallelNearestNeighbor( ctx context.Context, planOpts *plannerOptions, seed node, - rrtMap map[node]node, + tree rrtMap, ) node { - nm.ready = false nm.seedPos = seed - nm.startNNworkers(ctx, planOpts) - defer close(nm.nnKeys) + + nm.neighbors = make(chan *neighbor, nm.nCPU) + nm.nnKeys = make(chan node, len(tree)) defer close(nm.neighbors) - for k := range rrtMap { + for i := 0; i < nm.nCPU; i++ { + utils.PanicCapturingGo(func() { + nm.nnWorker(ctx, planOpts) + }) + } + + for k := range tree { nm.nnKeys <- k } - nm.nnLock.Lock() - nm.ready = true - nm.nnLock.Unlock() + close(nm.nnKeys) + + wasInterrupted := false var best node bestDist := math.Inf(1) - returned := 0 - for returned < nm.nCPU { - select { - case <-ctx.Done(): - return nil - default: + for workerIdx := 0; workerIdx < nm.nCPU; workerIdx++ { + candidate := <-nm.neighbors + if candidate == nil { + // Seeing a `nil` here implies the workers did not get to all of the candidate + // neighbors. And thus we don't have the right answer to return. + wasInterrupted = true + continue } - select { - case nn := <-nm.neighbors: - returned++ - if nn.dist < bestDist { - bestDist = nn.dist - best = nn.node - } - default: + if candidate.dist < bestDist { + bestDist = candidate.dist + best = candidate.node } } - return best -} - -func (nm *neighborManager) startNNworkers(ctx context.Context, planOpts *plannerOptions) { - nm.neighbors = make(chan *neighbor, nm.nCPU) - nm.nnKeys = make(chan node, nm.nCPU) - for i := 0; i < nm.nCPU; i++ { - utils.PanicCapturingGo(func() { - nm.nnWorker(ctx, planOpts) - }) + if wasInterrupted { + return nil } + + return best } func (nm *neighborManager) nnWorker(ctx context.Context, planOpts *plannerOptions) { var best node bestDist := math.Inf(1) - for { + for candidate := range nm.nnKeys { select { case <-ctx.Done(): + // We were interrupted, signal that to the caller by returning a `nil`. + nm.neighbors <- nil return default: } - select { - case k := <-nm.nnKeys: - if k != nil { - nm.nnLock.RLock() - seg := &Segment{ - StartConfiguration: nm.seedPos.Q(), - EndConfiguration: k.Q(), - } - if pose := nm.seedPos.Pose(); pose != nil { - seg.StartPosition = pose - } - if pose := k.Pose(); pose != nil { - seg.EndPosition = pose - } - dist := planOpts.DistanceFunc(seg) - nm.nnLock.RUnlock() - if dist < bestDist { - bestDist = dist - best = k - } - } - default: - nm.nnLock.RLock() - if nm.ready { - nm.nnLock.RUnlock() - nm.neighbors <- &neighbor{bestDist, best} - return - } - nm.nnLock.RUnlock() + seg := &ik.Segment{ + StartConfiguration: nm.seedPos.Q(), + EndConfiguration: candidate.Q(), + } + if pose := nm.seedPos.Pose(); pose != nil { + seg.StartPosition = pose + } + if pose := candidate.Pose(); pose != nil { + seg.EndPosition = pose + } + dist := planOpts.DistanceFunc(seg) + if dist < bestDist { + bestDist = dist + best = candidate } } + + nm.neighbors <- &neighbor{bestDist, best} } diff --git a/motionplan/nearestNeighbor_test.go b/motionplan/nearestNeighbor_test.go index 36e46d00e4c..c3a5996ff6a 100644 --- a/motionplan/nearestNeighbor_test.go +++ b/motionplan/nearestNeighbor_test.go @@ -2,6 +2,8 @@ package motionplan import ( "context" + "math" + "runtime" "testing" "go.viam.com/test" @@ -9,33 +11,35 @@ import ( "go.viam.com/rdk/referenceframe" ) +var nCPU = int(math.Max(1.0, float64(runtime.NumCPU()/4))) + func TestNearestNeighbor(t *testing.T) { - nm := &neighborManager{nCPU: 2} + nm := &neighborManager{nCPU: 2, parallelNeighbors: 1000} rrtMap := map[node]node{} j := &basicNode{q: []referenceframe.Input{{0.0}}} + // We add ~110 nodes to the set of candidates. This is smaller than the configured + // `parallelNeighbors` or 1000 meaning the `nearestNeighbor` call will be evaluated in series. for i := 1.0; i < 110.0; i++ { iSol := &basicNode{q: []referenceframe.Input{{i}}} rrtMap[iSol] = j j = iSol } ctx := context.Background() - m1chan := make(chan node, 1) - defer close(m1chan) seed := []referenceframe.Input{{23.1}} - // test serial NN - opt := newBasicPlannerOptions() + opt := newBasicPlannerOptions(referenceframe.NewZeroStaticFrame("test-frame")) nn := nm.nearestNeighbor(ctx, opt, &basicNode{q: seed}, rrtMap) test.That(t, nn.Q()[0].Value, test.ShouldAlmostEqual, 23.0) + // We add more nodes to trip the 1000 threshold. The `nearestNeighbor` call will use `nCPU` (2) + // goroutines for evaluation. for i := 120.0; i < 1100.0; i++ { iSol := &basicNode{q: []referenceframe.Input{{i}}} rrtMap[iSol] = j j = iSol } seed = []referenceframe.Input{{723.6}} - // test parallel NN nn = nm.nearestNeighbor(ctx, opt, &basicNode{q: seed}, rrtMap) test.That(t, nn.Q()[0].Value, test.ShouldAlmostEqual, 724.0) } diff --git a/motionplan/planManager.go b/motionplan/planManager.go index 24ee7c59af1..17e96229eaf 100644 --- a/motionplan/planManager.go +++ b/motionplan/planManager.go @@ -7,20 +7,23 @@ import ( "fmt" "math" "math/rand" + "sync" "time" "github.com/edaniels/golog" pb "go.viam.com/api/service/motion/v1" "go.viam.com/utils" + "go.viam.com/rdk/motionplan/ik" "go.viam.com/rdk/motionplan/tpspace" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" ) const ( - defaultOptimalityMultiple = 2.0 - defaultFallbackTimeout = 1.5 + defaultOptimalityMultiple = 2.0 + defaultFallbackTimeout = 1.5 + defaultTPspaceOrientationScale = 30. ) // planManager is intended to be the single entry point to motion planners, wrapping all others, dealing with fallbacks, etc. @@ -28,8 +31,9 @@ const ( // motionplan.PlanMotion() -> SolvableFrameSystem.SolveWaypointsWithOptions() -> planManager.planSingleWaypoint(). type planManager struct { *planner - frame *solverFrame - fs referenceframe.FrameSystem + frame *solverFrame + fs referenceframe.FrameSystem + activeBackgroundWorkers sync.WaitGroup useTPspace bool } @@ -58,11 +62,11 @@ func newPlanManager( } //nolint: gosec - p, err := newPlanner(frame, rand.New(rand.NewSource(int64(seed))), logger, newBasicPlannerOptions()) + p, err := newPlanner(frame, rand.New(rand.NewSource(int64(seed))), logger, newBasicPlannerOptions(frame)) if err != nil { return nil, err } - return &planManager{p, frame, fs, anyPTG}, nil + return &planManager{planner: p, frame: frame, fs: fs, useTPspace: anyPTG}, nil } // PlanSingleWaypoint will solve the solver frame to one individual pose. If you have multiple waypoints to hit, call this multiple times. @@ -172,6 +176,7 @@ func (pm *planManager) PlanSingleWaypoint(ctx context.Context, } resultSlices, err := pm.planAtomicWaypoints(ctx, goals, seed, planners) + pm.activeBackgroundWorkers.Wait() if err != nil { if len(goals) > 1 { err = fmt.Errorf("failed to plan path for valid goal: %w", err) @@ -242,7 +247,9 @@ func (pm *planManager) planSingleAtomicWaypoint( // for this solve will be rectified at the end. endpointPreview := make(chan node, 1) solutionChan := make(chan *rrtPlanReturn, 1) + pm.activeBackgroundWorkers.Add(1) utils.PanicCapturingGo(func() { + defer pm.activeBackgroundWorkers.Done() pm.planParallelRRTMotion(ctx, goal, seed, parPlan, endpointPreview, solutionChan, maps) }) select { @@ -258,7 +265,7 @@ func (pm *planManager) planSingleAtomicWaypoint( if planReturn.planerr != nil { return nil, nil, planReturn.planerr } - steps := planReturn.toInputs() + steps := nodesToInputs(planReturn.steps) return steps[len(steps)-1], &resultPromise{steps: steps}, nil case <-ctx.Done(): return nil, nil, ctx.Err() @@ -271,9 +278,12 @@ func (pm *planManager) planSingleAtomicWaypoint( if err != nil { return nil, nil, err } + + smoothedPath := nodesToInputs(pathPlanner.smoothPath(ctx, steps)) + // Update seed for the next waypoint to be the final configuration of this waypoint - seed = steps[len(steps)-1] - return seed, &resultPromise{steps: steps}, nil + seed = smoothedPath[len(smoothedPath)-1] + return seed, &resultPromise{steps: smoothedPath}, nil } } @@ -319,7 +329,9 @@ func (pm *planManager) planParallelRRTMotion( plannerChan := make(chan *rrtPlanReturn, 1) // start the planner + pm.activeBackgroundWorkers.Add(1) utils.PanicCapturingGo(func() { + defer pm.activeBackgroundWorkers.Done() pathPlanner.rrtBackgroundRunner(plannerctx, seed, &rrtParallelPlannerShared{maps, endpointPreview, plannerChan}) }) @@ -371,7 +383,9 @@ func (pm *planManager) planParallelRRTMotion( // Start smoothing before initializing the fallback plan. This allows both to run simultaneously. smoothChan := make(chan []node, 1) + pm.activeBackgroundWorkers.Add(1) utils.PanicCapturingGo(func() { + defer pm.activeBackgroundWorkers.Done() smoothChan <- pathPlanner.smoothPath(ctx, finalSteps.steps) }) var alternateFuture *resultPromise @@ -432,8 +446,8 @@ func (pm *planManager) plannerSetupFromMoveRequest( to = fixOvIncrement(to, from) // Start with normal options - opt := newBasicPlannerOptions() - opt.SetGoalMetric(NewSquaredNormMetric(to)) + opt := newBasicPlannerOptions(pm.frame) + opt.SetGoalMetric(ik.NewSquaredNormMetric(to)) opt.extra = planningOpts @@ -503,11 +517,8 @@ func (pm *planManager) plannerSetupFromMoveRequest( // overwrite default with TP space opt.PlannerConstructor = newTPSpaceMotionPlanner // Distances are computed in cartesian space rather than configuration space - opt.DistanceFunc = SquaredNormSegmentMetric + opt.DistanceFunc = ik.NewSquaredNormSegmentMetric(defaultTPspaceOrientationScale) - // TODO: instead of using a default this should be set from the TP frame as a function of the resolution of - // the simulated trajectories - opt.GoalThreshold = defaultTPSpaceGoalDist planAlg = "tpspace" } @@ -546,7 +557,7 @@ func (pm *planManager) plannerSetupFromMoveRequest( opt.AddStateConstraint(defaultOrientationConstraintDesc, constraint) opt.pathMetric = pathMetric case PositionOnlyMotionProfile: - opt.SetGoalMetric(NewPositionOnlyMetric(to)) + opt.SetGoalMetric(ik.NewPositionOnlyMetric(to)) case FreeMotionProfile: // No restrictions on motion fallthrough @@ -578,7 +589,7 @@ func goodPlan(pr *rrtPlanReturn, opt *plannerOptions) (bool, float64) { if pr.maps.optNode.Cost() <= 0 { return true, solutionCost } - solutionCost = EvaluatePlan(pr.toInputs(), opt.DistanceFunc) + solutionCost = EvaluatePlan(nodesToInputs(pr.steps), opt.DistanceFunc) if solutionCost < pr.maps.optNode.Cost()*defaultOptimalityMultiple { return true, solutionCost } diff --git a/motionplan/plannerOptions.go b/motionplan/plannerOptions.go index 5036af49370..1eb9a14b00f 100644 --- a/motionplan/plannerOptions.go +++ b/motionplan/plannerOptions.go @@ -5,6 +5,8 @@ import ( pb "go.viam.com/api/service/motion/v1" + "go.viam.com/rdk/motionplan/ik" + "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" ) @@ -37,10 +39,7 @@ const ( defaultTimeout = 300. // default number of times to try to smooth the path. - defaultSmoothIter = 20 - - // default amount of closeness to get to the goal. - defaultGoalThreshold = defaultEpsilon * defaultEpsilon + defaultSmoothIter = 200 // descriptions of constraints. defaultLinearConstraintDesc = "Constraint to follow linear path" @@ -69,11 +68,11 @@ const ( ) // NewBasicPlannerOptions specifies a set of basic options for the planner. -func newBasicPlannerOptions() *plannerOptions { +func newBasicPlannerOptions(frame referenceframe.Frame) *plannerOptions { opt := &plannerOptions{} - opt.goalArcScore = JointMetric - opt.DistanceFunc = L2InputMetric - opt.pathMetric = NewZeroMetric() // By default, the distance to the valid manifold is zero, unless constraints say otherwise + opt.goalArcScore = ik.JointMetric + opt.DistanceFunc = ik.L2InputMetric + opt.pathMetric = ik.NewZeroMetric() // By default, the distance to the valid manifold is zero, unless constraints say otherwise // opt.goalMetric is intentionally unset as it is likely dependent on the goal itself. // Set defaults @@ -81,7 +80,12 @@ func newBasicPlannerOptions() *plannerOptions { opt.MinScore = defaultMinIkScore opt.Resolution = defaultResolution opt.Timeout = defaultTimeout - opt.GoalThreshold = defaultGoalThreshold + + opt.PlanIter = defaultPlanIter + opt.FrameStep = defaultFrameStep + opt.JointSolveDist = defaultJointSolveDist + opt.IterBeforeRand = defaultIterBeforeRand + opt.qstep = getFrameSteps(frame, defaultFrameStep) // Note the direct reference to a default here. // This is due to a Go compiler issue where it will incorrectly refuse to compile with a circular reference error if this @@ -98,9 +102,9 @@ func newBasicPlannerOptions() *plannerOptions { // plannerOptions are a set of options to be passed to a planner which will specify how to solve a motion planning problem. type plannerOptions struct { ConstraintHandler - goalMetric StateMetric // Distance function which converges to the final goal position - goalArcScore SegmentMetric - pathMetric StateMetric // Distance function which converges on the valid manifold of intermediate path states + goalMetric ik.StateMetric // Distance function which converges to the final goal position + goalArcScore ik.SegmentMetric + pathMetric ik.StateMetric // Distance function which converges on the valid manifold of intermediate path states extra map[string]interface{} @@ -129,8 +133,23 @@ type plannerOptions struct { // How close to get to the goal GoalThreshold float64 `json:"goal_threshold"` + // Number of planner iterations before giving up. + PlanIter int `json:"plan_iter"` + + // The maximum percent of a joints range of motion to allow per step. + FrameStep float64 `json:"frame_step"` + + // If the dot product between two sets of joint angles is less than this, consider them identical. + JointSolveDist float64 `json:"joint_solve_dist"` + + // Number of iterations to mrun before beginning to accept randomly seeded locations. + IterBeforeRand int `json:"iter_before_rand"` + + // This is how far cbirrt will try to extend the map towards a goal per-step. Determined from FrameStep + qstep []float64 + // DistanceFunc is the function that the planner will use to measure the degree of "closeness" between two states of the robot - DistanceFunc SegmentMetric + DistanceFunc ik.SegmentMetric PlannerConstructor plannerConstructor @@ -138,12 +157,12 @@ type plannerOptions struct { } // SetMetric sets the distance metric for the solver. -func (p *plannerOptions) SetGoalMetric(m StateMetric) { +func (p *plannerOptions) SetGoalMetric(m ik.StateMetric) { p.goalMetric = m } // SetPathDist sets the distance metric for the solver to move a constraint-violating point into a valid manifold. -func (p *plannerOptions) SetPathMetric(m StateMetric) { +func (p *plannerOptions) SetPathMetric(m ik.StateMetric) { p.pathMetric = m } @@ -186,7 +205,7 @@ func (p *plannerOptions) addPbLinearConstraints(from, to spatialmath.Pose, pbCon constraint, pathDist := NewAbsoluteLinearInterpolatingConstraint(from, to, float64(linTol), float64(orientTol)) p.AddStateConstraint(defaultLinearConstraintDesc, constraint) - p.pathMetric = CombineMetrics(p.pathMetric, pathDist) + p.pathMetric = ik.CombineMetrics(p.pathMetric, pathDist) } func (p *plannerOptions) addPbOrientationConstraints(from, to spatialmath.Pose, pbConstraint *pb.OrientationConstraint) { @@ -196,5 +215,5 @@ func (p *plannerOptions) addPbOrientationConstraints(from, to spatialmath.Pose, } constraint, pathDist := NewSlerpOrientationConstraint(from, to, float64(orientTol)) p.AddStateConstraint(defaultOrientationConstraintDesc, constraint) - p.pathMetric = CombineMetrics(p.pathMetric, pathDist) + p.pathMetric = ik.CombineMetrics(p.pathMetric, pathDist) } diff --git a/motionplan/rrt.go b/motionplan/rrt.go index fe34ed6aabe..37f16329336 100644 --- a/motionplan/rrt.go +++ b/motionplan/rrt.go @@ -4,6 +4,7 @@ import ( "context" "math" + "go.viam.com/rdk/motionplan/ik" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" ) @@ -11,6 +12,15 @@ import ( const ( // Number of planner iterations before giving up. defaultPlanIter = 20000 + + // The maximum percent of a joints range of motion to allow per step. + defaultFrameStep = 0.015 + + // If the dot product between two sets of joint angles is less than this, consider them identical. + defaultJointSolveDist = 0.0001 + + // Number of iterations to run before beginning to accept randomly seeded locations. + defaultIterBeforeRand = 50 ) type rrtParallelPlanner interface { @@ -24,17 +34,6 @@ type rrtParallelPlannerShared struct { solutionChan chan *rrtPlanReturn } -type rrtOptions struct { - // Number of planner iterations before giving up. - PlanIter int `json:"plan_iter"` -} - -func newRRTOptions() *rrtOptions { - return &rrtOptions{ - PlanIter: defaultPlanIter, - } -} - type rrtMap map[node]node type rrtPlanReturn struct { @@ -43,9 +42,9 @@ type rrtPlanReturn struct { maps *rrtMaps } -func (plan *rrtPlanReturn) toInputs() [][]referenceframe.Input { - inputs := make([][]referenceframe.Input, 0, len(plan.steps)) - for _, step := range plan.steps { +func nodesToInputs(nodes []node) [][]referenceframe.Input { + inputs := make([][]referenceframe.Input, 0, len(nodes)) + for _, step := range nodes { inputs = append(inputs, step.Q()) } return inputs @@ -81,7 +80,7 @@ func initRRTSolutions(ctx context.Context, mp motionPlanner, seed []referencefra } // the smallest interpolated distance between the start and end input represents a lower bound on cost - optimalCost := mp.opt().DistanceFunc(&Segment{StartConfiguration: seed, EndConfiguration: solutions[0].Q()}) + optimalCost := mp.opt().DistanceFunc(&ik.Segment{StartConfiguration: seed, EndConfiguration: solutions[0].Q()}) rrt.maps.optNode = &basicNode{q: solutions[0].Q(), cost: optimalCost} // Check for direct interpolation for the subset of IK solutions within some multiple of optimal @@ -90,7 +89,7 @@ func initRRTSolutions(ctx context.Context, mp motionPlanner, seed []referencefra // initialize maps and check whether direct interpolation is an option for _, solution := range solutions { if canInterp { - cost := mp.opt().DistanceFunc(&Segment{StartConfiguration: seed, EndConfiguration: solution.Q()}) + cost := mp.opt().DistanceFunc(&ik.Segment{StartConfiguration: seed, EndConfiguration: solution.Q()}) if cost < optimalCost*defaultOptimalityMultiple { if mp.checkPath(seed, solution.Q()) { rrt.steps = []node{seedNode, solution} @@ -117,7 +116,25 @@ func shortestPath(maps *rrtMaps, nodePairs []*nodePair) *rrtPlanReturn { minIdx = i } } - return &rrtPlanReturn{steps: extractPath(maps.startMap, maps.goalMap, nodePairs[minIdx]), maps: maps} + return &rrtPlanReturn{steps: extractPath(maps.startMap, maps.goalMap, nodePairs[minIdx], true), maps: maps} +} + +// fixedStepInterpolation returns inputs at qstep distance along the path from start to target +// if start and target have the same Input value, then no step increment is made. +func fixedStepInterpolation(start, target node, qstep []float64) []referenceframe.Input { + newNear := make([]referenceframe.Input, 0, len(start.Q())) + for j, nearInput := range start.Q() { + if nearInput.Value == target.Q()[j].Value { + newNear = append(newNear, nearInput) + } else { + v1, v2 := nearInput.Value, target.Q()[j].Value + newVal := math.Min(qstep[j], math.Abs(v2-v1)) + // get correct sign + newVal *= (v2 - v1) / math.Abs(v2-v1) + newNear = append(newNear, referenceframe.Input{nearInput.Value + newVal}) + } + } + return newNear } // node interface is used to wrap a configuration for planning purposes. @@ -128,19 +145,23 @@ type node interface { Cost() float64 SetCost(float64) Pose() spatialmath.Pose + Corner() bool + SetCorner(bool) } type basicNode struct { - q []referenceframe.Input - cost float64 - pose spatialmath.Pose + q []referenceframe.Input + cost float64 + pose spatialmath.Pose + corner bool } // Special case constructors for nodes without costs to return NaN. func newConfigurationNode(q []referenceframe.Input) node { return &basicNode{ - q: q, - cost: math.NaN(), + q: q, + cost: math.NaN(), + corner: false, } } @@ -160,6 +181,14 @@ func (n *basicNode) Pose() spatialmath.Pose { return n.pose } +func (n *basicNode) Corner() bool { + return n.corner +} + +func (n *basicNode) SetCorner(corner bool) { + n.corner = corner +} + // nodePair groups together nodes in a tuple // TODO(rb): in the future we might think about making this into a list of nodes. type nodePair struct{ a, b node } @@ -176,7 +205,7 @@ func (np *nodePair) sumCosts() float64 { return aCost + bCost } -func extractPath(startMap, goalMap map[node]node, pair *nodePair) []node { +func extractPath(startMap, goalMap map[node]node, pair *nodePair, matched bool) []node { // need to figure out which of the two nodes is in the start map var startReached, goalReached node if _, ok := startMap[pair.a]; ok { @@ -198,8 +227,10 @@ func extractPath(startMap, goalMap map[node]node, pair *nodePair) []node { } if goalReached != nil { - // skip goalReached node and go directly to its parent in order to not repeat this node - goalReached = goalMap[goalReached] + if matched { + // skip goalReached node and go directly to its parent in order to not repeat this node + goalReached = goalMap[goalReached] + } // extract the path to the goal for goalReached != nil { @@ -209,3 +240,11 @@ func extractPath(startMap, goalMap map[node]node, pair *nodePair) []node { } return path } + +func sumCosts(path []node) float64 { + cost := 0. + for _, wp := range path { + cost += wp.Cost() + } + return cost +} diff --git a/motionplan/rrtStarConnect.go b/motionplan/rrtStarConnect.go index 2a20be678e6..56811915e28 100644 --- a/motionplan/rrtStarConnect.go +++ b/motionplan/rrtStarConnect.go @@ -3,13 +3,13 @@ package motionplan import ( "context" "encoding/json" - "math" "math/rand" "time" "github.com/edaniels/golog" "go.viam.com/utils" + "go.viam.com/rdk/motionplan/ik" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" ) @@ -26,9 +26,6 @@ const ( type rrtStarConnectOptions struct { // The number of nearest neighbors to consider when adding a new sample to the tree NeighborhoodSize int `json:"neighborhood_size"` - - // Parameters common to all RRT implementations - *rrtOptions } // newRRTStarConnectOptions creates a struct controlling the running of a single invocation of the algorithm. @@ -36,7 +33,6 @@ type rrtStarConnectOptions struct { func newRRTStarConnectOptions(planOpts *plannerOptions) (*rrtStarConnectOptions, error) { algOpts := &rrtStarConnectOptions{ NeighborhoodSize: defaultNeighborhoodSize, - rrtOptions: newRRTOptions(), } // convert map to json jsonString, err := json.Marshal(planOpts.extra) @@ -82,7 +78,7 @@ func newRRTStarConnectMotionPlanner( func (mp *rrtStarConnectMotionPlanner) plan(ctx context.Context, goal spatialmath.Pose, seed []referenceframe.Input, -) ([][]referenceframe.Input, error) { +) ([]node, error) { solutionChan := make(chan *rrtPlanReturn, 1) utils.PanicCapturingGo(func() { mp.rrtBackgroundRunner(ctx, seed, &rrtParallelPlannerShared{nil, nil, solutionChan}) @@ -91,7 +87,7 @@ func (mp *rrtStarConnectMotionPlanner) plan(ctx context.Context, case <-ctx.Done(): return nil, ctx.Err() case plan := <-solutionChan: - return plan.toInputs(), plan.err() + return plan.steps, plan.err() } } @@ -120,7 +116,8 @@ func (mp *rrtStarConnectMotionPlanner) rrtBackgroundRunner(ctx context.Context, } rrt.maps = planSeed.maps } - target := referenceframe.InterpolateInputs(seed, rrt.maps.optNode.Q(), 0.5) + target := newConfigurationNode(referenceframe.InterpolateInputs(seed, rrt.maps.optNode.Q(), 0.5)) + map1, map2 := rrt.maps.startMap, rrt.maps.goalMap // Keep a list of the node pairs that have the same inputs shared := make([]*nodePair, 0) @@ -132,7 +129,7 @@ func (mp *rrtStarConnectMotionPlanner) rrtBackgroundRunner(ctx context.Context, nSolved := 0 - for i := 0; i < mp.algOpts.PlanIter; i++ { + for i := 0; i < mp.planOpts.PlanIter; i++ { select { case <-ctx.Done(): // stop and return best path @@ -147,24 +144,55 @@ func (mp *rrtStarConnectMotionPlanner) rrtBackgroundRunner(ctx context.Context, default: } - // try to connect the target to map 1 - utils.PanicCapturingGo(func() { - mp.extend(rrt.maps.startMap, target, m1chan) - }) - utils.PanicCapturingGo(func() { - mp.extend(rrt.maps.goalMap, target, m2chan) - }) - map1reached := <-m1chan - map2reached := <-m2chan - - if map1reached != nil && map2reached != nil { + tryExtend := func(target node) (node, node, error) { + // attempt to extend maps 1 and 2 towards the target + // If ctx is done, nearest neighbors will be invalid and we want to return immediately + select { + case <-ctx.Done(): + return nil, nil, ctx.Err() + default: + } + + utils.PanicCapturingGo(func() { + mp.extend(ctx, map1, target, m1chan) + }) + utils.PanicCapturingGo(func() { + mp.extend(ctx, map2, target, m2chan) + }) + map1reached := <-m1chan + map2reached := <-m2chan + + return map1reached, map2reached, nil + } + + map1reached, map2reached, err := tryExtend(target) + if err != nil { + rrt.solutionChan <- &rrtPlanReturn{planerr: err, maps: rrt.maps} + return + } + + reachedDelta := mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: map1reached.Q(), EndConfiguration: map2reached.Q()}) + + // Second iteration; extend maps 1 and 2 towards the halfway point between where they reached + if reachedDelta > mp.planOpts.JointSolveDist { + target = newConfigurationNode(referenceframe.InterpolateInputs(map1reached.Q(), map2reached.Q(), 0.5)) + map1reached, map2reached, err = tryExtend(target) + if err != nil { + rrt.solutionChan <- &rrtPlanReturn{planerr: err, maps: rrt.maps} + return + } + reachedDelta = mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: map1reached.Q(), EndConfiguration: map2reached.Q()}) + } + + // Solved + if reachedDelta <= mp.planOpts.JointSolveDist { // target was added to both map shared = append(shared, &nodePair{map1reached, map2reached}) // Check if we can return if nSolved%defaultOptimalityCheckIter == 0 { solution := shortestPath(rrt.maps, shared) - solutionCost := EvaluatePlan(solution.toInputs(), mp.planOpts.DistanceFunc) + solutionCost := EvaluatePlan(nodesToInputs(solution.steps), mp.planOpts.DistanceFunc) if solutionCost-rrt.maps.optNode.Cost() < defaultOptimalityThreshold*rrt.maps.optNode.Cost() { mp.logger.Debug("RRT* progress: sufficiently optimal path found, exiting") rrt.solutionChan <- solution @@ -176,64 +204,78 @@ func (mp *rrtStarConnectMotionPlanner) rrtBackgroundRunner(ctx context.Context, } // get next sample, switch map pointers - target = referenceframe.RandomFrameInputs(mp.frame, mp.randseed) + target, err = mp.sample(map1reached, i) + if err != nil { + rrt.solutionChan <- &rrtPlanReturn{planerr: err, maps: rrt.maps} + return + } + map1, map2 = map2, map1 } mp.logger.Debug("RRT* exceeded max iter") rrt.solutionChan <- shortestPath(rrt.maps, shared) } func (mp *rrtStarConnectMotionPlanner) extend( - tree rrtMap, - target []referenceframe.Input, + ctx context.Context, + rrtMap map[node]node, + target node, mchan chan node, ) { - if validTarget := mp.checkInputs(target); !validTarget { - mchan <- nil - return - } - // iterate over the k nearest neighbors and find the minimum cost to connect the target node to the tree - neighbors := kNearestNeighbors(mp.planOpts, tree, &basicNode{q: target}, mp.algOpts.NeighborhoodSize) - minCost := math.Inf(1) - minIndex := -1 - for i, thisNeighbor := range neighbors { - cost := thisNeighbor.node.Cost() + thisNeighbor.dist - if mp.checkPath(thisNeighbor.node.Q(), target) { - minIndex = i - minCost = cost - // Neighbors are returned ordered by their costs. The first valid one we find is best, so break here. - break + // This should iterate until one of the following conditions: + // 1) we have reached the target + // 2) the request is cancelled/times out + // 3) we are no longer approaching the target and our "best" node is further away than the previous best + // 4) further iterations change our best node by close-to-zero amounts + // 5) we have iterated more than maxExtendIter times + near := kNearestNeighbors(mp.planOpts, rrtMap, &basicNode{q: target.Q()}, mp.algOpts.NeighborhoodSize)[0].node + oldNear := near + for i := 0; i < maxExtendIter; i++ { + select { + case <-ctx.Done(): + mchan <- oldNear + return + default: } - } - // add new node to tree as a child of the minimum cost neighbor node if it was reachable - if minIndex == -1 { - mchan <- nil - return - } - targetNode := &basicNode{q: target, cost: minCost} - tree[targetNode] = neighbors[minIndex].node - - // rewire the tree - for i, thisNeighbor := range neighbors { - // dont need to try to rewire minIndex, so skip it - if i == minIndex { - continue + dist := mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: near.Q(), EndConfiguration: target.Q()}) + if dist < mp.planOpts.JointSolveDist { + mchan <- near + return } - // check to see if a shortcut is possible, and rewire the node if it is - connectionCost := mp.planOpts.DistanceFunc(&Segment{ - StartConfiguration: thisNeighbor.node.Q(), - EndConfiguration: targetNode.Q(), - }) - cost := connectionCost + targetNode.Cost() - - // If 1) we have a lower cost, and 2) the putative updated path is valid - if cost < thisNeighbor.node.Cost() && mp.checkPath(target, thisNeighbor.node.Q()) { - // Alter the cost of the node - // This needs to edit the existing node, rather than make a new one, as there are pointers in the tree - thisNeighbor.node.SetCost(cost) - tree[thisNeighbor.node] = targetNode + oldNear = near + newNear := fixedStepInterpolation(near, target, mp.planOpts.qstep) + // Check whether oldNear -> newNear path is a valid segment, and if not then set to nil + if !mp.checkPath(oldNear.Q(), newNear) { + break + } + + neighbors := kNearestNeighbors(mp.planOpts, rrtMap, &basicNode{q: newNear}, mp.algOpts.NeighborhoodSize) + near = &basicNode{q: newNear, cost: neighbors[0].node.Cost() + neighbors[0].dist} + rrtMap[near] = oldNear + + // rewire the tree + for i, thisNeighbor := range neighbors { + // dont need to try to rewire nearest neighbor, so skip it + if i == 0 { + continue + } + + // check to see if a shortcut is possible, and rewire the node if it is + connectionCost := mp.planOpts.DistanceFunc(&ik.Segment{ + StartConfiguration: thisNeighbor.node.Q(), + EndConfiguration: near.Q(), + }) + cost := connectionCost + near.Cost() + + // If 1) we have a lower cost, and 2) the putative updated path is valid + if cost < thisNeighbor.node.Cost() && mp.checkPath(target.Q(), thisNeighbor.node.Q()) { + // Alter the cost of the node + // This needs to edit the existing node, rather than make a new one, as there are pointers in the tree + thisNeighbor.node.SetCost(cost) + rrtMap[thisNeighbor.node] = near + } } } - mchan <- targetNode + mchan <- oldNear } diff --git a/motionplan/tpSpaceRRT.go b/motionplan/tpSpaceRRT.go index b7e84fe9763..4f863fdade4 100644 --- a/motionplan/tpSpaceRRT.go +++ b/motionplan/tpSpaceRRT.go @@ -8,39 +8,104 @@ import ( "fmt" "math" "math/rand" + "sync" "github.com/edaniels/golog" "github.com/golang/geo/r3" + "go.uber.org/multierr" "go.viam.com/utils" + "go.viam.com/rdk/motionplan/ik" "go.viam.com/rdk/motionplan/tpspace" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" ) const ( - defaultGoalCheck = 5 // Check if the goal is reachable every this many iterations - defaultAutoBB = 1. // Automatic bounding box on driveable area as a multiple of start-goal distance + defaultGoalCheck = 5 // Check if the goal is reachable every this many iterations + defaultAutoBB = 0.3 // Automatic bounding box on driveable area as a multiple of start-goal distance // Note: while fully holonomic planners can use the limits of the frame as implicit boundaries, with non-holonomic motion // this is not the case, and the total workspace available to the planned frame is not directly related to the motion available // from a single set of inputs. // whether to add intermediate waypoints. - defaultAddInt = false + defaultAddInt = true // Add a subnode every this many mm along a valid trajectory. Large values run faster, small gives better paths // Meaningless if the above is false. - defaultAddNodeEvery = 50. - - // When getting the closest node to a pose, only look for nodes at least this far away. - defaultDuplicateNodeBuffer = 50. + defaultAddNodeEvery = 100. // Don't add new RRT tree nodes if there is an existing node within this distance. + // Consider nodes on trees to be connected if they are within this distance. defaultIdenticalNodeDistance = 5. - // Default distance in mm to get within for tp-space trajectories. - defaultTPSpaceGoalDist = 10. + // When extending the RRT tree towards some point, do not extend more than this many times in a single RRT invocation. + defaultMaxReseeds = 50 + + // For whatever `refDist` is used for the generation of the original path, scale that by this amount when smoothing. + // This can help to find paths. + defaultSmoothScaleFactor = 0.5 + + // Make an attempt to solve the tree every this many iterations + // For a unidirectional solve, this means attempting to reach the goal rather than a random point + // For a bidirectional solve, this means trying to connect the two trees directly. + defaultAttemptSolveEvery = 15 ) +type tpspaceOptions struct { + goalCheck int // Check if goal is reachable every this many iters + + // TODO: base this on frame limits? + autoBB float64 // Automatic bounding box on driveable area as a multiple of start-goal distance + + addIntermediate bool // whether to add intermediate waypoints. + // Add a subnode every this many mm along a valid trajectory. Large values run faster, small gives better paths + // Meaningless if the above is false. + addNodeEvery float64 + + // If the squared norm between two poses is less than this, consider them equal + poseSolveDist float64 + + // When smoothing, adjust the trajectory path length to be this proportion of the length used for solving + smoothScaleFactor float64 + + // Make an attempt to solve the tree every this many iterations + // For a unidirectional solve, this means attempting to reach the goal rather than a random point + // For a bidirectional solve, this means trying to connect the two trees directly + attemptSolveEvery int + + // Print very fine-grained debug info. Useful for observing the inner RRT tree structure directly + pathdebug bool + + // random value to seed the IK solver. Can be anything in the middle of the valid manifold. + ikSeed []referenceframe.Input + + // Cached functions for calculating TP-space distances for each PTG + distOptions map[tpspace.PTG]*plannerOptions + invertDistOptions map[tpspace.PTG]*plannerOptions +} + +// candidate is putative node which could be added to an RRT tree. It includes a distance score, the new node and its future parent. +type candidate struct { + dist float64 + treeNode node + newNode node + err error + lastInTraj bool +} + +type nodeAndError struct { + node + error +} + +// tpSpaceRRTMotionPlanner. +type tpSpaceRRTMotionPlanner struct { + *planner + mu sync.Mutex + algOpts *tpspaceOptions + tpFrame tpspace.PTGProvider +} + // newTPSpaceMotionPlanner creates a newTPSpaceMotionPlanner object with a user specified random seed. func newTPSpaceMotionPlanner( frame referenceframe.Frame, @@ -56,7 +121,6 @@ func newTPSpaceMotionPlanner( return nil, err } - // either the passed in frame, tpFrame, ok := mp.frame.(tpspace.PTGProvider) if !ok { return nil, fmt.Errorf("frame %v must be a PTGProvider", mp.frame) @@ -64,36 +128,26 @@ func newTPSpaceMotionPlanner( tpPlanner := &tpSpaceRRTMotionPlanner{ planner: mp, + tpFrame: tpFrame, } - tpPlanner.setupTPSpaceOptions(tpFrame) - return tpPlanner, nil -} + tpPlanner.setupTPSpaceOptions() -// tpSpaceRRTMotionPlanner. -type tpSpaceRRTMotionPlanner struct { - *planner - algOpts *tpspaceOptions -} + tpPlanner.algOpts.ikSeed = []referenceframe.Input{{math.Pi / 2}, {tpFrame.PTGs()[0].MaxDistance() / 2}} -// candidate is putative node which could be added to an RRT tree. It includes a distance score, the new node and its future parent. -type candidate struct { - dist float64 - treeNode node - newNode node - err error + return tpPlanner, nil } // TODO: seed is not immediately useful for TP-space. func (mp *tpSpaceRRTMotionPlanner) plan(ctx context.Context, goal spatialmath.Pose, seed []referenceframe.Input, -) ([][]referenceframe.Input, error) { +) ([]node, error) { solutionChan := make(chan *rrtPlanReturn, 1) seedPos := spatialmath.NewZeroPose() - startNode := &basicNode{make([]referenceframe.Input, len(mp.frame.DoF())), 0, seedPos} - goalNode := &basicNode{nil, 0, goal} + startNode := &basicNode{q: make([]referenceframe.Input, len(mp.frame.DoF())), cost: 0, pose: seedPos, corner: false} + goalNode := &basicNode{q: make([]referenceframe.Input, len(mp.frame.DoF())), cost: 0, pose: goal, corner: false} utils.PanicCapturingGo(func() { mp.planRunner(ctx, seed, &rrtParallelPlannerShared{ @@ -110,7 +164,7 @@ func (mp *tpSpaceRRTMotionPlanner) plan(ctx context.Context, return nil, ctx.Err() case plan := <-solutionChan: if plan != nil { - return plan.toInputs(), plan.err() + return plan.steps, plan.err() } return nil, errors.New("nil tp-space plan returned, unable to complete plan") } @@ -125,12 +179,6 @@ func (mp *tpSpaceRRTMotionPlanner) planRunner( ) { defer close(rrt.solutionChan) - tpFrame, ok := mp.frame.(tpspace.PTGProvider) - if !ok { - rrt.solutionChan <- &rrtPlanReturn{planerr: fmt.Errorf("frame %v must be a PTGProvider", mp.frame)} - return - } - // get start and goal poses var startPose spatialmath.Pose var goalPose spatialmath.Pose @@ -157,282 +205,619 @@ func (mp *tpSpaceRRTMotionPlanner) planRunner( } } - dist := math.Sqrt(mp.planOpts.DistanceFunc(&Segment{StartPosition: startPose, EndPosition: goalPose})) - midPt := goalPose.Point().Sub(startPose.Point()) + m1chan := make(chan *nodeAndError, 1) + m2chan := make(chan *nodeAndError, 1) + defer close(m1chan) + defer close(m2chan) - var successNode node - for iter := 0; iter < mp.algOpts.PlanIter; iter++ { + dist := math.Sqrt(mp.planOpts.DistanceFunc(&ik.Segment{StartPosition: startPose, EndPosition: goalPose})) + midptNode := &basicNode{pose: spatialmath.Interpolate(startPose, goalPose, 0.5), cost: dist} // used for initial seed + var randPosNode node = midptNode + + for iter := 0; iter < mp.planOpts.PlanIter; iter++ { if ctx.Err() != nil { mp.logger.Debugf("TP Space RRT timed out after %d iterations", iter) rrt.solutionChan <- &rrtPlanReturn{planerr: fmt.Errorf("TP Space RRT timeout %w", ctx.Err()), maps: rrt.maps} return } - // Get random cartesian configuration - var randPos spatialmath.Pose - tryGoal := true - // Check if we can reach the goal every N iters - if iter%mp.algOpts.goalCheck != 0 { - rDist := dist * (mp.algOpts.autoBB + float64(iter)/10.) - tryGoal = false - randPosX := float64(mp.randseed.Intn(int(rDist))) - randPosY := float64(mp.randseed.Intn(int(rDist))) - randPosTheta := math.Pi * (mp.randseed.Float64() - 0.5) - randPos = spatialmath.NewPose( - r3.Vector{midPt.X + (randPosX - rDist/2.), midPt.Y + (randPosY - rDist/2.), 0}, - &spatialmath.OrientationVector{OZ: 1, Theta: randPosTheta}, - ) - } else { - randPos = goalPose - } - randPosNode := &basicNode{nil, 0, randPos} - - candidateNodes := map[float64][2]node{} - - // Find the best traj point for each traj family, and store for later comparison - // TODO: run in parallel - for ptgNum, curPtg := range tpFrame.PTGs() { - cand := mp.getExtensionCandidate(ctx, randPosNode, ptgNum, curPtg, rrt) - if cand != nil { - if cand.err == nil { - atGoal := false - // If we tried the goal and have a close-enough XY location, check if the node is good enough to be a final goal - if tryGoal && cand.dist < mp.planOpts.GoalThreshold { - atGoal = mp.planOpts.GoalThreshold > mp.planOpts.goalMetric(&State{Position: cand.newNode.Pose(), Frame: mp.frame}) - } - if atGoal { - // If we've reached the goal, break out - // TODO: Currently only the *first* valid goal is considered and returned. It would be more optimal to take - // the best valid goal. - rrt.maps.startMap[cand.newNode] = cand.treeNode - successNode = cand.newNode - path := extractPath(rrt.maps.startMap, rrt.maps.goalMap, &nodePair{a: successNode}) - rrt.solutionChan <- &rrtPlanReturn{steps: path, maps: rrt.maps} - return + + utils.PanicCapturingGo(func() { + m1chan <- mp.attemptExtension(ctx, randPosNode, rrt.maps.startMap, false) + }) + utils.PanicCapturingGo(func() { + m2chan <- mp.attemptExtension(ctx, randPosNode, rrt.maps.goalMap, true) + }) + seedMapReached := <-m1chan + goalMapReached := <-m2chan + + seedMapNode := seedMapReached.node + goalMapNode := goalMapReached.node + err := multierr.Combine(seedMapReached.error, goalMapReached.error) + if err != nil { + rrt.solutionChan <- &rrtPlanReturn{planerr: err, maps: rrt.maps} + return + } + + if seedMapNode != nil && goalMapNode != nil { + seedReached := mp.attemptExtension(ctx, goalMapNode, rrt.maps.startMap, false) + if seedReached.error != nil { + rrt.solutionChan <- &rrtPlanReturn{planerr: seedReached.error, maps: rrt.maps} + return + } + if seedReached.node == nil { + continue + } + goalReached := mp.attemptExtension(ctx, seedReached.node, rrt.maps.goalMap, true) + if goalReached.error != nil { + rrt.solutionChan <- &rrtPlanReturn{planerr: goalReached.error, maps: rrt.maps} + return + } + if goalReached.node == nil { + continue + } + reachedDelta := mp.planOpts.DistanceFunc(&ik.Segment{StartPosition: seedReached.node.Pose(), EndPosition: goalReached.node.Pose()}) + if reachedDelta <= mp.algOpts.poseSolveDist { + // If we've reached the goal, extract the path from the RRT trees and return + path := extractPath(rrt.maps.startMap, rrt.maps.goalMap, &nodePair{a: seedReached.node, b: goalReached.node}, false) + rrt.solutionChan <- &rrtPlanReturn{steps: path, maps: rrt.maps} + return + } + } + if iter%mp.algOpts.attemptSolveEvery == 0 { + // Attempt a solve; we exhaustively iterate through our goal tree and attempt to find any connection to the seed tree + paths := [][]node{} + for goalMapNode := range rrt.maps.goalMap { + seedReached := mp.attemptExtension(ctx, goalMapNode, rrt.maps.startMap, false) + if seedReached.error != nil { + rrt.solutionChan <- &rrtPlanReturn{planerr: seedReached.error, maps: rrt.maps} + return + } + if seedReached.node == nil { + continue + } + reachedDelta := mp.planOpts.DistanceFunc(&ik.Segment{StartPosition: seedReached.node.Pose(), EndPosition: goalMapNode.Pose()}) + if reachedDelta <= mp.algOpts.poseSolveDist { + // If we've reached the goal, extract the path from the RRT trees and return + path := extractPath(rrt.maps.startMap, rrt.maps.goalMap, &nodePair{a: seedReached.node, b: goalMapNode}, false) + paths = append(paths, path) + } + } + if len(paths) > 0 { + var bestPath []node + bestCost := math.Inf(1) + for _, goodPath := range paths { + currCost := sumCosts(goodPath) + if currCost < bestCost { + bestCost = currCost + bestPath = goodPath } - candidateNodes[cand.dist] = [2]node{cand.treeNode, cand.newNode} } + correctedPath, err := rectifyTPspacePath(bestPath, mp.frame) + if err != nil { + rrt.solutionChan <- &rrtPlanReturn{planerr: err, maps: rrt.maps} + return + } + rrt.solutionChan <- &rrtPlanReturn{steps: correctedPath, maps: rrt.maps} + return } } - mp.extendMap(ctx, candidateNodes, rrt, tpFrame) + + // Get random cartesian configuration + randPosNode, err = mp.sample(midptNode, iter+1) + if err != nil { + rrt.solutionChan <- &rrtPlanReturn{planerr: err, maps: rrt.maps} + return + } } rrt.solutionChan <- &rrtPlanReturn{maps: rrt.maps, planerr: errors.New("tpspace RRT unable to create valid path")} } -// getExtensionCandidate will return either nil, or the best node on a PTG to reach the desired random node and its RRT tree parent. +// getExtensionCandidate will return either nil, or the best node on a valid PTG to reach the desired random node and its RRT tree parent. func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( ctx context.Context, randPosNode node, ptgNum int, curPtg tpspace.PTG, - rrt *rrtParallelPlannerShared, -) *candidate { - nm := &neighborManager{nCPU: mp.planOpts.NumThreads} + rrt rrtMap, + nearest node, + invert bool, +) (*candidate, error) { + nm := &neighborManager{nCPU: mp.planOpts.NumThreads / len(mp.tpFrame.PTGs())} nm.parallelNeighbors = 10 var successNode node // Get the distance function that will find the nearest RRT map node in TP-space of *this* PTG ptgDistOpt := mp.algOpts.distOptions[curPtg] + if invert { + ptgDistOpt = mp.algOpts.invertDistOptions[curPtg] + } - // Get nearest neighbor to rand config in tree using this PTG - nearest := nm.nearestNeighbor(ctx, ptgDistOpt, randPosNode, rrt.maps.startMap) if nearest == nil { - return nil + // Get nearest neighbor to rand config in tree using this PTG + nearest = nm.nearestNeighbor(ctx, ptgDistOpt, randPosNode, rrt) + if nearest == nil { + return nil, errNoNeighbors + } } + // TODO: We could potentially improve solving by first getting the rough distance to the randPosNode to any point in the rrt tree, + // then dynamically expanding or contracting the limits of IK to be some fraction of that distance. // Get cartesian distance from NN to rand - relPose := spatialmath.Compose(spatialmath.PoseInverse(nearest.Pose()), randPosNode.Pose()) - relPosePt := relPose.Point() + var targetFunc ik.StateMetric + if invert { + sqMet := ik.NewSquaredNormMetric(randPosNode.Pose()) + targetFunc = func(pose *ik.State) float64 { + return sqMet(&ik.State{Position: spatialmath.PoseBetweenInverse(pose.Position, nearest.Pose())}) + } + } else { + relPose := spatialmath.PoseBetween(nearest.Pose(), randPosNode.Pose()) + targetFunc = ik.NewSquaredNormMetric(relPose) + } + solutionChan := make(chan *ik.Solution, 1) + mp.mu.Lock() + rseed := mp.randseed.Int() + mp.mu.Unlock() + err := curPtg.Solve(context.Background(), solutionChan, mp.algOpts.ikSeed, targetFunc, rseed) - // Convert cartesian distance to tp-space using inverse curPtg, yielding TP-space coordinates goalK and goalD - nodes := curPtg.CToTP(relPosePt.X, relPosePt.Y) - bestNode, bestDist := mp.closestNode(relPose, nodes, mp.algOpts.dupeNodeBuffer) - if bestNode == nil { - return nil + var bestNode *ik.Solution + select { + case bestNode = <-solutionChan: + default: } - goalK := bestNode.K - goalD := bestNode.Dist + if err != nil || bestNode == nil { + return nil, err + } + pose, err := curPtg.Transform(bestNode.Configuration) + if err != nil { + return nil, err + } + + bestDist := targetFunc(&ik.State{Position: pose}) + goalAlpha := bestNode.Configuration[0].Value + goalD := bestNode.Configuration[1].Value + // Check collisions along this traj and get the longest distance viable - trajK := curPtg.Trajectory(goalK) - var lastNode *tpspace.TrajNode - var lastPose spatialmath.Pose + trajK, err := curPtg.Trajectory(goalAlpha, goalD) + if err != nil { + return nil, err + } + finalTrajNode := trajK[len(trajK)-1] + + arcStartPose := nearest.Pose() + if invert { + arcStartPose = spatialmath.PoseBetweenInverse(finalTrajNode.Pose, arcStartPose) + } sinceLastCollideCheck := 0. lastDist := 0. - + var nodePose spatialmath.Pose // Check each point along the trajectory to confirm constraints are met - for _, trajPt := range trajK { - if trajPt.Dist > goalD { - // After we've passed randD, no need to keep checking, just add to RRT tree - break + for i := 0; i < len(trajK); i++ { + trajPt := trajK[i] + if invert { + // Start at known-good map point and extend + // For the goal tree this means iterating backwards + trajPt = trajK[(len(trajK)-1)-i] } - sinceLastCollideCheck += (trajPt.Dist - lastDist) - trajState := &State{Position: spatialmath.Compose(nearest.Pose(), trajPt.Pose), Frame: mp.frame} + + sinceLastCollideCheck += math.Abs(trajPt.Dist - lastDist) + trajState := &ik.State{Position: spatialmath.Compose(arcStartPose, trajPt.Pose), Frame: mp.frame} + nodePose = trajState.Position // This will get rewritten later for inverted trees if sinceLastCollideCheck > mp.planOpts.Resolution { ok, _ := mp.planOpts.CheckStateConstraints(trajState) if !ok { - return nil + return nil, errInvalidCandidate } sinceLastCollideCheck = 0. } - lastPose = trajState.Position - lastNode = trajPt - lastDist = lastNode.Dist + lastDist = trajPt.Dist } + + isLastNode := math.Abs(finalTrajNode.Dist-curPtg.MaxDistance()) < 0.1 + // add the last node in trajectory successNode = &basicNode{ - referenceframe.FloatsToInputs([]float64{float64(ptgNum), float64(goalK), lastNode.Dist}), - nearest.Cost() + lastNode.Dist, - lastPose, + q: referenceframe.FloatsToInputs([]float64{float64(ptgNum), goalAlpha, finalTrajNode.Dist}), + cost: finalTrajNode.Dist, + pose: nodePose, + corner: false, } - cand := &candidate{dist: bestDist, treeNode: nearest, newNode: successNode} - + cand := &candidate{dist: bestDist, treeNode: nearest, newNode: successNode, lastInTraj: isLastNode} // check if this successNode is too close to nodes already in the tree, and if so, do not add. // Get nearest neighbor to new node that's already in the tree - nearest = nm.nearestNeighbor(ctx, mp.planOpts, successNode, rrt.maps.startMap) + nearest = nm.nearestNeighbor(ctx, mp.planOpts, successNode, rrt) if nearest != nil { - dist := mp.planOpts.DistanceFunc(&Segment{StartPosition: successNode.Pose(), EndPosition: nearest.Pose()}) + dist := mp.planOpts.DistanceFunc(&ik.Segment{StartPosition: successNode.Pose(), EndPosition: nearest.Pose()}) // Ensure successNode is sufficiently far from the nearest node already existing in the tree // If too close, don't add a new node if dist < defaultIdenticalNodeDistance { cand = nil } } - return cand + return cand, nil } -// extendMap grows the rrt map to the best candidate node if it is valid to do so. +// attemptExtension will attempt to extend the rrt map towards the goal node, and will return the candidate added to the map that is +// closest to that goal. +func (mp *tpSpaceRRTMotionPlanner) attemptExtension( + ctx context.Context, + goalNode node, + rrt rrtMap, + invert bool, +) *nodeAndError { + var reseedCandidate *candidate + var seedNode node + maxReseeds := 1 // Will be updated as necessary + lastIteration := false + candChan := make(chan *candidate, len(mp.tpFrame.PTGs())) + defer close(candChan) + for i := 0; i <= maxReseeds; i++ { + select { + case <-ctx.Done(): + return &nodeAndError{nil, ctx.Err()} + default: + } + candidates := []*candidate{} + + for ptgNum, curPtg := range mp.tpFrame.PTGs() { + // Find the best traj point for each traj family, and store for later comparison + ptgNumPar, curPtgPar := ptgNum, curPtg + utils.PanicCapturingGo(func() { + cand, err := mp.getExtensionCandidate(ctx, goalNode, ptgNumPar, curPtgPar, rrt, seedNode, invert) + if err != nil && !errors.Is(err, errNoNeighbors) && !errors.Is(err, errInvalidCandidate) { + candChan <- nil + return + } + if cand != nil { + if cand.err == nil { + candChan <- cand + return + } + } + candChan <- nil + }) + } + + for i := 0; i < len(mp.tpFrame.PTGs()); i++ { + select { + case <-ctx.Done(): + return &nodeAndError{nil, ctx.Err()} + case cand := <-candChan: + if cand != nil { + candidates = append(candidates, cand) + } + } + } + var err error + reseedCandidate, err = mp.extendMap(ctx, candidates, rrt, invert) + if err != nil && !errors.Is(err, errNoCandidates) { + return &nodeAndError{nil, err} + } + if reseedCandidate == nil { + return &nodeAndError{nil, nil} + } + dist := mp.planOpts.DistanceFunc(&ik.Segment{StartPosition: reseedCandidate.newNode.Pose(), EndPosition: goalNode.Pose()}) + if dist < mp.algOpts.poseSolveDist || lastIteration { + // Reached the goal position, or otherwise failed to fully extend to the end of a trajectory + return &nodeAndError{reseedCandidate.newNode, nil} + } + if i == 0 { + dist = math.Sqrt(dist) + // TP-space distance is NOT the same thing as cartesian distance, but they track sufficiently well that this is valid to do. + maxReseeds = int(math.Min(float64(defaultMaxReseeds), math.Ceil(dist/reseedCandidate.newNode.Q()[2].Value)+2)) + } + // If our most recent traj was not a full-length extension, try to extend one more time and then return our best node. + // This helps prevent the planner from doing a 15-point turn to adjust orientation, which is very difficult to accurately execute. + if !reseedCandidate.lastInTraj { + lastIteration = true + } + + seedNode = reseedCandidate.newNode + } + return &nodeAndError{reseedCandidate.newNode, nil} +} + +// extendMap grows the rrt map to the best candidate node, returning the added candidate. func (mp *tpSpaceRRTMotionPlanner) extendMap( ctx context.Context, - candidateNodes map[float64][2]node, - rrt *rrtParallelPlannerShared, - tpFrame tpspace.PTGProvider, -) { + candidates []*candidate, + rrt rrtMap, + invert bool, +) (*candidate, error) { + if len(candidates) == 0 { + return nil, errNoCandidates + } var addedNode node // If we found any valid nodes that we can extend to, find the very best one and add that to the tree - if len(candidateNodes) > 0 { - bestDist := math.Inf(1) - var bestNode [2]node - for k, v := range candidateNodes { - if k < bestDist { - bestNode = v - bestDist = k - } + bestDist := math.Inf(1) + var bestCand *candidate + for _, cand := range candidates { + if cand.dist < bestDist { + bestCand = cand + bestDist = cand.dist } + } + treeNode := bestCand.treeNode // The node already in the tree to which we are parenting + newNode := bestCand.newNode // The node we are adding because it was the best extending PTG - ptgNum := int(bestNode[1].Q()[0].Value) - randK := uint(bestNode[1].Q()[1].Value) - - trajK := tpFrame.PTGs()[ptgNum].Trajectory(randK) + ptgNum := int(newNode.Q()[0].Value) + randAlpha := newNode.Q()[1].Value + randDist := newNode.Q()[2].Value - lastDist := 0. - sinceLastNode := 0. + trajK, err := mp.tpFrame.PTGs()[ptgNum].Trajectory(randAlpha, randDist) + if err != nil { + return nil, err + } - for _, trajPt := range trajK { + arcStartPose := treeNode.Pose() + if invert { + arcStartPose = spatialmath.PoseBetweenInverse(trajK[len(trajK)-1].Pose, arcStartPose) + } + lastDist := 0. + sinceLastNode := 0. + + var trajState *ik.State + if mp.algOpts.addIntermediate { + for i := 0; i < len(trajK); i++ { + trajPt := trajK[i] + if invert { + trajPt = trajK[(len(trajK)-1)-i] + } if ctx.Err() != nil { - return + return nil, ctx.Err() } - if trajPt.Dist > bestNode[1].Q()[2].Value { - // After we've passed goalD, no need to keep checking, just add to RRT tree - break + trajState = &ik.State{Position: spatialmath.Compose(arcStartPose, trajPt.Pose)} + if mp.algOpts.pathdebug { + if !invert { + mp.logger.Debugf("$FWDTREE,%f,%f\n", trajState.Position.Point().X, trajState.Position.Point().Y) + } else { + mp.logger.Debugf("$REVTREE,%f,%f\n", trajState.Position.Point().X, trajState.Position.Point().Y) + } } - trajState := &State{Position: spatialmath.Compose(bestNode[0].Pose(), trajPt.Pose)} sinceLastNode += (trajPt.Dist - lastDist) // Optionally add sub-nodes along the way. Will make the final path a bit better - if mp.algOpts.addIntermediate && sinceLastNode > mp.algOpts.addNodeEvery { + // Intermediate nodes currently disabled on the goal map because they do not invert nicely + if sinceLastNode > mp.algOpts.addNodeEvery && !invert { // add the last node in trajectory addedNode = &basicNode{ - referenceframe.FloatsToInputs([]float64{float64(ptgNum), float64(randK), trajPt.Dist}), - bestNode[0].Cost() + trajPt.Dist, - trajState.Position, + q: referenceframe.FloatsToInputs([]float64{float64(ptgNum), randAlpha, trajPt.Dist}), + cost: trajPt.Dist, + pose: trajState.Position, + corner: false, } - rrt.maps.startMap[addedNode] = bestNode[0] + rrt[addedNode] = treeNode sinceLastNode = 0. } lastDist = trajPt.Dist } - rrt.maps.startMap[bestNode[1]] = bestNode[0] + if mp.algOpts.pathdebug { + mp.logger.Debugf("$WPI,%f,%f\n", trajState.Position.Point().X, trajState.Position.Point().Y) + } } + rrt[newNode] = treeNode + return bestCand, nil } -func (mp *tpSpaceRRTMotionPlanner) setupTPSpaceOptions(tpFrame tpspace.PTGProvider) { +func (mp *tpSpaceRRTMotionPlanner) setupTPSpaceOptions() { tpOpt := &tpspaceOptions{ - rrtOptions: newRRTOptions(), - goalCheck: defaultGoalCheck, - autoBB: defaultAutoBB, + goalCheck: defaultGoalCheck, + autoBB: defaultAutoBB, - addIntermediate: defaultAddInt, - addNodeEvery: defaultAddNodeEvery, + addIntermediate: defaultAddInt, + addNodeEvery: defaultAddNodeEvery, + attemptSolveEvery: defaultAttemptSolveEvery, + smoothScaleFactor: defaultSmoothScaleFactor, - dupeNodeBuffer: defaultDuplicateNodeBuffer, + poseSolveDist: defaultIdenticalNodeDistance, - distOptions: map[tpspace.PTG]*plannerOptions{}, + distOptions: map[tpspace.PTG]*plannerOptions{}, + invertDistOptions: map[tpspace.PTG]*plannerOptions{}, } - for _, curPtg := range tpFrame.PTGs() { - tpOpt.distOptions[curPtg] = mp.make2DTPSpaceDistanceOptions(curPtg, tpOpt.dupeNodeBuffer) + for _, curPtg := range mp.tpFrame.PTGs() { + tpOpt.distOptions[curPtg] = mp.make2DTPSpaceDistanceOptions(curPtg, false) + tpOpt.invertDistOptions[curPtg] = mp.make2DTPSpaceDistanceOptions(curPtg, true) } mp.algOpts = tpOpt } -type tpspaceOptions struct { - *rrtOptions - goalCheck int // Check if goal is reachable every this many iters +// make2DTPSpaceDistanceOptions will create a plannerOptions object with a custom DistanceFunc constructed such that +// distances can be computed in TP space using the given PTG. +func (mp *tpSpaceRRTMotionPlanner) make2DTPSpaceDistanceOptions(ptg tpspace.PTG, invert bool) *plannerOptions { + opts := newBasicPlannerOptions(mp.frame) + mp.mu.Lock() + //nolint: gosec + randSeed := rand.New(rand.NewSource(mp.randseed.Int63() + mp.randseed.Int63())) + mp.mu.Unlock() + + segMetric := func(seg *ik.Segment) float64 { + // When running NearestNeighbor: + // StartPosition is the seed/query + // EndPosition is the pose already in the RRT tree + if seg.StartPosition == nil || seg.EndPosition == nil { + return math.Inf(1) + } + var targetFunc ik.StateMetric + if invert { + sqMet := ik.NewSquaredNormMetric(seg.StartPosition) + targetFunc = func(pose *ik.State) float64 { + return sqMet(&ik.State{Position: spatialmath.PoseBetweenInverse(pose.Position, seg.EndPosition)}) + } + } else { + relPose := spatialmath.PoseBetween(seg.EndPosition, seg.StartPosition) + targetFunc = ik.NewSquaredNormMetric(relPose) + } + solutionChan := make(chan *ik.Solution, 1) + err := ptg.Solve(context.Background(), solutionChan, mp.algOpts.ikSeed, targetFunc, randSeed.Int()) - // TODO: base this on frame limits? - autoBB float64 // Automatic bounding box on driveable area as a multiple of start-goal distance + var closeNode *ik.Solution + select { + case closeNode = <-solutionChan: + default: + } + if err != nil || closeNode == nil { + return math.Inf(1) + } + pose, err := ptg.Transform(closeNode.Configuration) + if err != nil { + return math.Inf(1) + } + return targetFunc(&ik.State{Position: pose}) + } + opts.DistanceFunc = segMetric + return opts +} - addIntermediate bool // whether to add intermediate waypoints. - // Add a subnode every this many mm along a valid trajectory. Large values run faster, small gives better paths - // Meaningless if the above is false. - addNodeEvery float64 +func (mp *tpSpaceRRTMotionPlanner) smoothPath(ctx context.Context, path []node) []node { + toIter := int(math.Min(float64(len(path)*len(path))/2, float64(mp.planOpts.SmoothIter))) + currCost := sumCosts(path) - // Don't add new RRT tree nodes if there is an existing node within this distance - dupeNodeBuffer float64 + maxCost := math.Inf(-1) + for _, wp := range path { + if wp.Cost() > maxCost { + maxCost = wp.Cost() + } + } + newFrame, err := tpspace.NewPTGFrameFromPTGFrame(mp.frame, maxCost*mp.algOpts.smoothScaleFactor) + if err != nil { + return path + } + smoothPlannerMP, err := newTPSpaceMotionPlanner(newFrame, mp.randseed, mp.logger, mp.planOpts) + if err != nil { + return path + } + smoothPlanner := smoothPlannerMP.(*tpSpaceRRTMotionPlanner) + for i := 0; i < toIter; i++ { + select { + case <-ctx.Done(): + return path + default: + } + // get start node of first edge. Cannot be either the last or second-to-last node. + // Intn will return an int in the half-open interval half-open interval [0,n) + firstEdge := mp.randseed.Intn(len(path) - 2) + secondEdge := firstEdge + 2 + mp.randseed.Intn((len(path)-2)-firstEdge) - // Cached functions for calculating TP-space distances for each PTG - distOptions map[tpspace.PTG]*plannerOptions -} + newInputSteps, err := mp.attemptSmooth(ctx, path, firstEdge, secondEdge, smoothPlanner) + if err != nil || newInputSteps == nil { + continue + } + newCost := sumCosts(newInputSteps) + if newCost >= currCost { + continue + } + // Re-connect to the final goal + if newInputSteps[len(newInputSteps)-1] != path[len(path)-1] { + newInputSteps = append(newInputSteps, path[len(path)-1]) + } -// closestNode will look through a set of nodes and return the one that is closest to a given pose -// A minimum distance may be passed beyond which nodes are disregarded. -func (mp *tpSpaceRRTMotionPlanner) closestNode(pose spatialmath.Pose, nodes []*tpspace.TrajNode, min float64) (*tpspace.TrajNode, float64) { - var bestNode *tpspace.TrajNode - bestDist := math.Inf(1) - for _, tNode := range nodes { - if tNode.Dist < min { + goalInputSteps, err := mp.attemptSmooth(ctx, newInputSteps, len(newInputSteps)-3, len(newInputSteps)-1, smoothPlanner) + if err != nil || goalInputSteps == nil { continue } - dist := mp.planOpts.DistanceFunc(&Segment{StartPosition: pose, EndPosition: tNode.Pose}) - if dist < bestDist { - bestNode = tNode - bestDist = dist + goalInputSteps = append(goalInputSteps, path[len(path)-1]) + path = goalInputSteps + currCost = sumCosts(path) + } + return path +} + +// attemptSmooth attempts to connect two given points in a path. The points must not be adjacent. +// Strategy is to subdivide the seed-side trajectories to give a greater probability of solving. +func (mp *tpSpaceRRTMotionPlanner) attemptSmooth( + ctx context.Context, + path []node, + firstEdge, secondEdge int, + smoother *tpSpaceRRTMotionPlanner, +) ([]node, error) { + startMap := map[node]node{} + var parent node + parentPose := spatialmath.NewZeroPose() + + for j := 0; j <= firstEdge; j++ { + pathNode := path[j] + startMap[pathNode] = parent + for _, adj := range []float64{0.25, 0.5, 0.75} { + fullQ := pathNode.Q() + newQ := []referenceframe.Input{fullQ[0], fullQ[1], {fullQ[2].Value * adj}} + trajK, err := smoother.tpFrame.PTGs()[int(math.Round(newQ[0].Value))].Trajectory(newQ[1].Value, newQ[2].Value) + if err != nil { + continue + } + + intNode := &basicNode{ + q: newQ, + cost: pathNode.Cost() - (pathNode.Q()[2].Value * (1 - adj)), + pose: spatialmath.Compose(parentPose, trajK[len(trajK)-1].Pose), + corner: false, + } + startMap[intNode] = parent } + parent = pathNode + parentPose = parent.Pose() + } + // TODO: everything below this point can become an invocation of `smoother.planRunner` + reached := smoother.attemptExtension(ctx, path[secondEdge], startMap, false) + if reached.error != nil || reached.node == nil { + return nil, errors.New("could not extend to smoothing destination") + } + + reachedDelta := mp.planOpts.DistanceFunc(&ik.Segment{StartPosition: reached.Pose(), EndPosition: path[secondEdge].Pose()}) + // If we tried the goal and have a close-enough XY location, check if the node is good enough to be a final goal + if reachedDelta > mp.algOpts.poseSolveDist { + return nil, errors.New("could not precisely reach smoothing destination") } - return bestNode, bestDist + + newInputSteps := extractPath(startMap, nil, &nodePair{a: reached.node, b: nil}, false) + + if secondEdge < len(path)-1 { + newInputSteps = append(newInputSteps, path[secondEdge+1:]...) + } + return rectifyTPspacePath(newInputSteps, mp.frame) } -// make2DTPSpaceDistanceOptions will create a plannerOptions object with a custom DistanceFunc constructed such that -// distances can be computed in TP space using the given PTG. -func (mp *tpSpaceRRTMotionPlanner) make2DTPSpaceDistanceOptions(ptg tpspace.PTG, min float64) *plannerOptions { - opts := newBasicPlannerOptions() +func (mp *tpSpaceRRTMotionPlanner) sample(rSeed node, iter int) (node, error) { + dist := rSeed.Cost() + if dist == 0 { + dist = 1.0 + } + rDist := dist * (mp.algOpts.autoBB + float64(iter)/10.) + randPosX := float64(mp.randseed.Intn(int(rDist))) + randPosY := float64(mp.randseed.Intn(int(rDist))) + randPosTheta := math.Pi * (mp.randseed.Float64() - 0.5) + randPos := spatialmath.NewPose( + r3.Vector{rSeed.Pose().Point().X + (randPosX - rDist/2.), rSeed.Pose().Point().Y + (randPosY - rDist/2.), 0}, + &spatialmath.OrientationVector{OZ: 1, Theta: randPosTheta}, + ) + return &basicNode{pose: randPos}, nil +} - segMet := func(seg *Segment) float64 { - if seg.StartPosition == nil || seg.EndPosition == nil { - return math.Inf(1) +// rectifyTPspacePath is needed because of how trees are currently stored. As trees grow from the start or goal, the Pose stored in the node +// is the distal pose away from the root of the tree, which in the case of the goal tree is in fact the 0-distance point of the traj. +// When this becomes a single path, poses should reflect the transformation at the end of each traj. Here we go through and recompute +// each pose in order to ensure correctness. +// TODO: if trees are stored as segments rather than nodes, then this becomes simpler/unnecessary. Related to RSDK-4139. +func rectifyTPspacePath(path []node, frame referenceframe.Frame) ([]node, error) { + correctedPath := []node{} + runningPose := spatialmath.NewZeroPose() + for _, wp := range path { + wpPose, err := frame.Transform(wp.Q()) + if err != nil { + return nil, err } - relPose := spatialmath.Compose(spatialmath.PoseInverse(seg.StartPosition), seg.EndPosition) - relPosePt := relPose.Point() - nodes := ptg.CToTP(relPosePt.X, relPosePt.Y) - closeNode, _ := mp.closestNode(relPose, nodes, min) - if closeNode == nil { - return math.Inf(1) + runningPose = spatialmath.Compose(runningPose, wpPose) + + thisNode := &basicNode{ + q: wp.Q(), + cost: wp.Cost(), + pose: runningPose, + corner: wp.Corner(), } - return closeNode.Dist + correctedPath = append(correctedPath, thisNode) } - opts.DistanceFunc = segMet - return opts + return correctedPath, nil } diff --git a/motionplan/tpSpaceRRT_test.go b/motionplan/tpSpaceRRT_test.go index 839408ae8e0..0d86e7d0909 100644 --- a/motionplan/tpSpaceRRT_test.go +++ b/motionplan/tpSpaceRRT_test.go @@ -4,6 +4,7 @@ package motionplan import ( "context" + "math" "math/rand" "testing" @@ -11,20 +12,28 @@ import ( "github.com/golang/geo/r3" "go.viam.com/test" + "go.viam.com/rdk/motionplan/ik" + "go.viam.com/rdk/motionplan/tpspace" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" ) +var printPath = false + const testTurnRad = 0.3 func TestPtgRrt(t *testing.T) { + t.Parallel() logger := golog.NewTestLogger(t) roverGeom, err := spatialmath.NewBox(spatialmath.NewZeroPose(), r3.Vector{10, 10, 10}, "") test.That(t, err, test.ShouldBeNil) geometries := []spatialmath.Geometry{roverGeom} - ackermanFrame, err := referenceframe.NewPTGFrameFromTurningRadius( + ctx := context.Background() + + ackermanFrame, err := tpspace.NewPTGFrameFromTurningRadius( "ackframe", + logger, 300., testTurnRad, 0, @@ -32,29 +41,72 @@ func TestPtgRrt(t *testing.T) { ) test.That(t, err, test.ShouldBeNil) - goalPos := spatialmath.NewPose(r3.Vector{X: 50, Y: 10, Z: 0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 180}) + goalPos := spatialmath.NewPose(r3.Vector{X: 200, Y: 7000, Z: 0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 90}) - opt := newBasicPlannerOptions() - opt.SetGoalMetric(NewPositionOnlyMetric(goalPos)) - opt.DistanceFunc = SquaredNormNoOrientSegmentMetric - opt.GoalThreshold = 10. + opt := newBasicPlannerOptions(ackermanFrame) + opt.DistanceFunc = ik.NewSquaredNormSegmentMetric(30.) mp, err := newTPSpaceMotionPlanner(ackermanFrame, rand.New(rand.NewSource(42)), logger, opt) test.That(t, err, test.ShouldBeNil) tp, ok := mp.(*tpSpaceRRTMotionPlanner) + tp.algOpts.pathdebug = printPath + if tp.algOpts.pathdebug { + tp.logger.Debug("$type,X,Y") + tp.logger.Debugf("$SG,%f,%f\n", 0., 0.) + tp.logger.Debugf("$SG,%f,%f\n", goalPos.Point().X, goalPos.Point().Y) + } test.That(t, ok, test.ShouldBeTrue) - - plan, err := tp.plan(context.Background(), goalPos, nil) + plan, err := tp.plan(ctx, goalPos, nil) test.That(t, err, test.ShouldBeNil) test.That(t, len(plan), test.ShouldBeGreaterThanOrEqualTo, 2) + + allPtgs := ackermanFrame.(tpspace.PTGProvider).PTGs() + lastPose := spatialmath.NewZeroPose() + + if tp.algOpts.pathdebug { + for _, mynode := range plan { + trajPts, _ := allPtgs[int(mynode.Q()[0].Value)].Trajectory(mynode.Q()[1].Value, mynode.Q()[2].Value) + for i, pt := range trajPts { + intPose := spatialmath.Compose(lastPose, pt.Pose) + if i == 0 { + tp.logger.Debugf("$WP,%f,%f\n", intPose.Point().X, intPose.Point().Y) + } + tp.logger.Debugf("$FINALPATH,%f,%f\n", intPose.Point().X, intPose.Point().Y) + if i == len(trajPts)-1 { + lastPose = intPose + break + } + } + } + } + plan = tp.smoothPath(ctx, plan) + if tp.algOpts.pathdebug { + lastPose = spatialmath.NewZeroPose() + for _, mynode := range plan { + trajPts, _ := allPtgs[int(mynode.Q()[0].Value)].Trajectory(mynode.Q()[1].Value, mynode.Q()[2].Value) + for i, pt := range trajPts { + intPose := spatialmath.Compose(lastPose, pt.Pose) + if i == 0 { + tp.logger.Debugf("$SMOOTHWP,%f,%f\n", intPose.Point().X, intPose.Point().Y) + } + tp.logger.Debugf("$SMOOTHPATH,%f,%f\n", intPose.Point().X, intPose.Point().Y) + if pt.Dist >= mynode.Q()[2].Value { + lastPose = intPose + break + } + } + } + } } func TestPtgWithObstacle(t *testing.T) { + t.Parallel() logger := golog.NewTestLogger(t) roverGeom, err := spatialmath.NewBox(spatialmath.NewZeroPose(), r3.Vector{10, 10, 10}, "") test.That(t, err, test.ShouldBeNil) geometries := []spatialmath.Geometry{roverGeom} - ackermanFrame, err := referenceframe.NewPTGFrameFromTurningRadius( + ackermanFrame, err := tpspace.NewPTGFrameFromTurningRadius( "ackframe", + logger, 300., testTurnRad, 0, @@ -64,23 +116,22 @@ func TestPtgWithObstacle(t *testing.T) { ctx := context.Background() - goalPos := spatialmath.NewPoseFromPoint(r3.Vector{X: 2000, Y: 0, Z: 0}) + goalPos := spatialmath.NewPoseFromPoint(r3.Vector{X: 6500, Y: 0, Z: 0}) fs := referenceframe.NewEmptyFrameSystem("test") fs.AddFrame(ackermanFrame, fs.World()) - opt := newBasicPlannerOptions() - opt.SetGoalMetric(NewPositionOnlyMetric(goalPos)) - opt.DistanceFunc = SquaredNormNoOrientSegmentMetric - opt.GoalThreshold = 10. + opt := newBasicPlannerOptions(ackermanFrame) + opt.DistanceFunc = ik.NewSquaredNormSegmentMetric(30.) + opt.GoalThreshold = 5 // obstacles - obstacle1, err := spatialmath.NewBox(spatialmath.NewPoseFromPoint(r3.Vector{2500, -500, 0}), r3.Vector{180, 1800, 1}, "") + obstacle1, err := spatialmath.NewBox(spatialmath.NewPoseFromPoint(r3.Vector{3300, -500, 0}), r3.Vector{180, 1800, 1}, "") test.That(t, err, test.ShouldBeNil) - obstacle2, err := spatialmath.NewBox(spatialmath.NewPoseFromPoint(r3.Vector{2500, 2000, 0}), r3.Vector{180, 1800, 1}, "") + obstacle2, err := spatialmath.NewBox(spatialmath.NewPoseFromPoint(r3.Vector{3300, 1800, 0}), r3.Vector{180, 1800, 1}, "") test.That(t, err, test.ShouldBeNil) - obstacle3, err := spatialmath.NewBox(spatialmath.NewPoseFromPoint(r3.Vector{2500, -1400, 0}), r3.Vector{50000, 10, 1}, "") + obstacle3, err := spatialmath.NewBox(spatialmath.NewPoseFromPoint(r3.Vector{2500, -1400, 0}), r3.Vector{50000, 30, 1}, "") test.That(t, err, test.ShouldBeNil) - obstacle4, err := spatialmath.NewBox(spatialmath.NewPoseFromPoint(r3.Vector{2500, 2400, 0}), r3.Vector{50000, 10, 1}, "") + obstacle4, err := spatialmath.NewBox(spatialmath.NewPoseFromPoint(r3.Vector{2500, 2400, 0}), r3.Vector{50000, 30, 1}, "") test.That(t, err, test.ShouldBeNil) geoms := []spatialmath.Geometry{obstacle1, obstacle2, obstacle3, obstacle4} @@ -102,8 +153,153 @@ func TestPtgWithObstacle(t *testing.T) { mp, err := newTPSpaceMotionPlanner(ackermanFrame, rand.New(rand.NewSource(42)), logger, opt) test.That(t, err, test.ShouldBeNil) tp, _ := mp.(*tpSpaceRRTMotionPlanner) - + tp.algOpts.pathdebug = printPath + if tp.algOpts.pathdebug { + tp.logger.Debug("$type,X,Y") + for _, geom := range geoms { + pts := geom.ToPoints(1.) + for _, pt := range pts { + if math.Abs(pt.Z) < 0.1 { + tp.logger.Debugf("$OBS,%f,%f\n", pt.X, pt.Y) + } + } + } + tp.logger.Debugf("$SG,%f,%f\n", 0., 0.) + tp.logger.Debugf("$SG,%f,%f\n", goalPos.Point().X, goalPos.Point().Y) + } plan, err := tp.plan(ctx, goalPos, nil) + test.That(t, err, test.ShouldBeNil) test.That(t, len(plan), test.ShouldBeGreaterThan, 2) + + allPtgs := ackermanFrame.(tpspace.PTGProvider).PTGs() + lastPose := spatialmath.NewZeroPose() + + if tp.algOpts.pathdebug { + for _, mynode := range plan { + trajPts, _ := allPtgs[int(mynode.Q()[0].Value)].Trajectory(mynode.Q()[1].Value, mynode.Q()[2].Value) + for i, pt := range trajPts { + intPose := spatialmath.Compose(lastPose, pt.Pose) + if i == 0 { + tp.logger.Debugf("$WP,%f,%f\n", intPose.Point().X, intPose.Point().Y) + } + tp.logger.Debugf("$FINALPATH,%f,%f\n", intPose.Point().X, intPose.Point().Y) + if i == len(trajPts)-1 { + lastPose = intPose + break + } + } + } + } + plan = tp.smoothPath(ctx, plan) + if tp.algOpts.pathdebug { + lastPose = spatialmath.NewZeroPose() + for _, mynode := range plan { + trajPts, _ := allPtgs[int(mynode.Q()[0].Value)].Trajectory(mynode.Q()[1].Value, mynode.Q()[2].Value) + for i, pt := range trajPts { + intPose := spatialmath.Compose(lastPose, pt.Pose) + if i == 0 { + tp.logger.Debugf("$SMOOTHWP,%f,%f\n", intPose.Point().X, intPose.Point().Y) + } + tp.logger.Debugf("$SMOOTHPATH,%f,%f\n", intPose.Point().X, intPose.Point().Y) + if pt.Dist >= mynode.Q()[2].Value { + lastPose = intPose + break + } + } + } + } +} + +func TestIKPtgRrt(t *testing.T) { + t.Parallel() + logger := golog.NewTestLogger(t) + roverGeom, err := spatialmath.NewBox(spatialmath.NewZeroPose(), r3.Vector{10, 10, 10}, "") + test.That(t, err, test.ShouldBeNil) + geometries := []spatialmath.Geometry{roverGeom} + + ackermanFrame, err := tpspace.NewPTGFrameFromTurningRadius( + "ackframe", + logger, + 300., + testTurnRad, + 0, + geometries, + ) + test.That(t, err, test.ShouldBeNil) + + goalPos := spatialmath.NewPose(r3.Vector{X: 50, Y: 10, Z: 0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 180}) + + opt := newBasicPlannerOptions(ackermanFrame) + opt.SetGoalMetric(ik.NewPositionOnlyMetric(goalPos)) + opt.DistanceFunc = ik.SquaredNormNoOrientSegmentMetric + opt.GoalThreshold = 10. + mp, err := newTPSpaceMotionPlanner(ackermanFrame, rand.New(rand.NewSource(42)), logger, opt) + test.That(t, err, test.ShouldBeNil) + tp, ok := mp.(*tpSpaceRRTMotionPlanner) + test.That(t, ok, test.ShouldBeTrue) + + plan, err := tp.plan(context.Background(), goalPos, nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, len(plan), test.ShouldBeGreaterThanOrEqualTo, 2) +} + +func TestTPsmoothing(t *testing.T) { + // TODO: this doesn't smooth properly yet. This should be made to smooth better. + t.Parallel() + logger := golog.NewTestLogger(t) + roverGeom, err := spatialmath.NewBox(spatialmath.NewZeroPose(), r3.Vector{10, 10, 10}, "") + test.That(t, err, test.ShouldBeNil) + geometries := []spatialmath.Geometry{roverGeom} + + ctx := context.Background() + + ackermanFrame, err := tpspace.NewPTGFrameFromTurningRadius( + "ackframe", + logger, + 300., + testTurnRad, + 0, + geometries, + ) + test.That(t, err, test.ShouldBeNil) + + opt := newBasicPlannerOptions(ackermanFrame) + opt.DistanceFunc = ik.NewSquaredNormSegmentMetric(30.) + mp, err := newTPSpaceMotionPlanner(ackermanFrame, rand.New(rand.NewSource(42)), logger, opt) + test.That(t, err, test.ShouldBeNil) + tp, _ := mp.(*tpSpaceRRTMotionPlanner) + + // plan which is known to be able to use some smoothing + planInputs := [][]referenceframe.Input{ + {{0}, {0}, {0}}, + {{3}, {-0.20713797715976653}, {848.2300164692441}}, + {{5}, {0.0314906475636095}, {848.2300108402619}}, + {{5}, {0.0016660735709435135}, {848.2300146893297}}, + {{0}, {0.00021343061342569985}, {408}}, + {{5}, {1.9088870836327245}, {737.7547597081078}}, + {{2}, {-1.3118738553451883}, {848.2300164692441}}, + {{0}, {-3.1070696573964987}, {848.2300164692441}}, + {{0}, {-2.5547017183037877}, {306}}, + {{4}, {-2.31209484211255}, {408}}, + {{0}, {1.1943809502464207}, {571.4368241014894}}, + {{0}, {0.724950779684863}, {848.2300164692441}}, + {{0}, {-1.2295409308605127}, {848.2294213788913}}, + {{5}, {2.677652944060827}, {848.230013198154}}, + {{0}, {2.7618396954635545}, {848.2300164692441}}, + {{0}, {0}, {0}}, + } + plan := []node{} + for _, inp := range planInputs { + thisNode := &basicNode{ + q: inp, + cost: inp[2].Value, + } + plan = append(plan, thisNode) + } + plan, err = rectifyTPspacePath(plan, tp.frame) + test.That(t, err, test.ShouldBeNil) + + plan = tp.smoothPath(ctx, plan) + test.That(t, plan, test.ShouldNotBeNil) } diff --git a/motionplan/tpspace/ptg.go b/motionplan/tpspace/ptg.go index 2ef1e86479a..4f40fb0ea8f 100644 --- a/motionplan/tpspace/ptg.go +++ b/motionplan/tpspace/ptg.go @@ -4,26 +4,26 @@ package tpspace import ( "math" - "github.com/golang/geo/r3" - + "go.viam.com/rdk/motionplan/ik" + "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" ) +const floatEpsilon = 0.0001 // If floats are closer than this consider them equal + // PTG is a Parameterized Trajectory Generator, which defines how to map back and forth from cartesian space to TP-space // PTG coordinates are specified in polar coordinates (alpha, d) // One of these is needed for each sort of motion that can be done. type PTG interface { - // CToTP Converts an (x, y) cartesian coord to a (k, d) TP-space coord - // d is the distance along a trajectory, k is a discretized uint index corresponding to an alpha value in [-pi, pi] - // See `index2alpha` for more - // Also returns a bool representing whether the xy is a within-traj match vs an extrapolation - CToTP(x, y float64) []*TrajNode + // Solve will return the (alpha, dist) TP-space coordinates whose corresponding relative pose minimizes the given function + ik.InverseKinematics + PrecomputePTG - // RefDistance returns the maximum distance that a single precomputed trajectory may travel - RefDistance() float64 + // MaxDistance returns the maximum distance that a single trajectory may travel + MaxDistance() float64 - // Returns the set of trajectory nodes for alpha index K - Trajectory(uint) []*TrajNode + // Returns the set of trajectory nodes along the given trajectory, out to the requested distance + Trajectory(alpha, dist float64) ([]*TrajNode, error) } // PTGProvider is something able to provide a set of PTGs associsated with it. For example, a frame which precomputes @@ -36,7 +36,8 @@ type PTGProvider interface { // PrecomputePTG is a precomputable PTG. type PrecomputePTG interface { // PTGVelocities returns the linear and angular velocity at a specific point along a trajectory - PTGVelocities(alpha, t, x, y, phi float64) (float64, float64, error) + PTGVelocities(alpha, dist float64) (float64, float64, error) + Transform([]referenceframe.Input) (spatialmath.Pose, error) } // TrajNode is a snapshot of a single point in time along a PTG trajectory, including the distance along that trajectory, @@ -46,18 +47,12 @@ type TrajNode struct { Pose spatialmath.Pose // for 2d, we only use x, y, and OV theta Time float64 // elapsed time on trajectory Dist float64 // distance travelled down trajectory - K uint // alpha k-value at this node + Alpha float64 // alpha k-value at this node LinVelMMPS float64 // linvel in millimeters per second at this node AngVelRPS float64 // angvel in radians per second at this node - - ptX float64 - ptY float64 } // discretized path to alpha. -// The inverse of this, which may be useful, looks like this: -// alpha = wrapTo2Pi(alpha) -// k := int(math.Round(0.5 * (float64(numPaths)*(1.0+alpha/math.Pi) - 1.0))). func index2alpha(k, numPaths uint) float64 { if k >= numPaths { return math.NaN() @@ -73,6 +68,60 @@ func wrapTo2Pi(theta float64) float64 { return theta - 2*math.Pi*math.Floor(theta/(2*math.Pi)) } -func xythetaToPose(x, y, theta float64) spatialmath.Pose { - return spatialmath.NewPose(r3.Vector{x, y, 0}, &spatialmath.OrientationVector{OZ: 1, Theta: theta}) +// ComputePTG will compute all nodes of simPTG at the requested alpha, out to the requested distance, at the specified diffT resolution. +func ComputePTG(simPTG PrecomputePTG, alpha, dist, diffT float64) ([]*TrajNode, error) { + // Initialize trajectory with an all-zero node + alphaTraj := []*TrajNode{{Pose: spatialmath.NewZeroPose()}} + + var err error + var t, v, w float64 + distTravelled := math.Copysign(math.Abs(v)*diffT, dist) + + // Step through each time point for this alpha + for math.Abs(distTravelled) < math.Abs(dist) { + t += diffT + nextNode, err := computePTGNode(simPTG, alpha, distTravelled, t) + if err != nil { + return nil, err + } + v = nextNode.LinVelMMPS + w = nextNode.AngVelRPS + + // Update velocities of last node because the computed velocities at this node are what should be set after passing the last node. + // Reasoning: if the distance passed in is 0, then we want the first node to return velocity 0. However, if we want a nonzero + // distance such that we return two nodes, then the first node, which has zero translation, should set a nonzero velocity so that + // the next node, which has a nonzero translation, is arrived at when it ought to be. + alphaTraj[len(alphaTraj)-1].LinVelMMPS = v + alphaTraj[len(alphaTraj)-1].AngVelRPS = w + + alphaTraj = append(alphaTraj, nextNode) + distTravelled += math.Copysign(math.Max(diffT, math.Abs(v)*diffT), dist) + } + + // Add final node + alphaTraj[len(alphaTraj)-1].LinVelMMPS = v + alphaTraj[len(alphaTraj)-1].AngVelRPS = w + pose, err := simPTG.Transform([]referenceframe.Input{{alpha}, {dist}}) + if err != nil { + return nil, err + } + tNode := &TrajNode{pose, t, dist, alpha, v, w} + alphaTraj = append(alphaTraj, tNode) + return alphaTraj, nil +} + +// computePTGNode will return the TrajNode of the requested PTG, at the specified alpha and dist. The provided time is used +// to fill in the time field. +func computePTGNode(simPTG PrecomputePTG, alpha, dist, atT float64) (*TrajNode, error) { + v, w, err := simPTG.PTGVelocities(alpha, dist) + if err != nil { + return nil, err + } + + // ptgIK caches these, so this should be fast. If cacheing is removed or a different simPTG used, this could be slow. + pose, err := simPTG.Transform([]referenceframe.Input{{alpha}, {dist}}) + if err != nil { + return nil, err + } + return &TrajNode{pose, atT, dist, alpha, v, w}, nil } diff --git a/motionplan/tpspace/ptgC.go b/motionplan/tpspace/ptgC.go index 4d0861b6181..f51f42a44b6 100644 --- a/motionplan/tpspace/ptgC.go +++ b/motionplan/tpspace/ptgC.go @@ -1,32 +1,80 @@ package tpspace import ( + "fmt" "math" + + "github.com/golang/geo/r3" + + "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/spatialmath" ) // ptgDiffDriveC defines a PTG family composed of circular trajectories with an alpha-dependent radius. type ptgDiffDriveC struct { maxMMPS float64 // millimeters per second velocity to target maxRPS float64 // radians per second of rotation when driving at maxMMPS and turning at max turning radius - k float64 // k = +1 for forwards, -1 for backwards } // NewCirclePTG creates a new PrecomputePTG of type ptgDiffDriveC. -func NewCirclePTG(maxMMPS, maxRPS, k float64) PrecomputePTG { +func NewCirclePTG(maxMMPS, maxRPS float64) PrecomputePTG { return &ptgDiffDriveC{ maxMMPS: maxMMPS, maxRPS: maxRPS, - k: k, } } // For this particular driver, turns alpha into a linear + angular velocity. Linear is just max * fwd/back. // Note that this will NOT work as-is for 0-radius turning. Robots capable of turning in place will need to be special-cased // because they will have zero linear velocity through their turns, not max. -func (ptg *ptgDiffDriveC) PTGVelocities(alpha, t, x, y, phi float64) (float64, float64, error) { +func (ptg *ptgDiffDriveC) PTGVelocities(alpha, dist float64) (float64, float64, error) { // (v,w) - v := ptg.maxMMPS * ptg.k - // Use a linear mapping: (Old was: w = tan( alpha/2 ) * W_MAX * sign(K)) - w := (alpha / math.Pi) * ptg.maxRPS * ptg.k + if dist == 0 { + return 0, 0, nil + } + k := math.Copysign(1.0, dist) + v := ptg.maxMMPS * k + w := (alpha / math.Pi) * ptg.maxRPS * k return v, w, nil } + +// Transform will return the pose for the given inputs. The first input is [-pi, pi]. This corresponds to the radius of the curve, +// where 0 is straight ahead, pi is turning at min turning radius to the right, and a value between 0 and pi represents turning at a radius +// of (input/pi)*minradius. A negative value denotes turning left. The second input is the distance traveled along this arc. +func (ptg *ptgDiffDriveC) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { + turnRad := ptg.maxMMPS / ptg.maxRPS + + if len(inputs) != 2 { + return nil, fmt.Errorf("ptgC takes 2 inputs, but received %d", len(inputs)) + } + alpha := inputs[0].Value + dist := inputs[1].Value + + // Check for OOB within FP error + if math.Pi-math.Abs(alpha) > math.Pi+floatEpsilon { + return nil, fmt.Errorf("ptgC input 0 is limited to [-pi, pi] but received %f", inputs[0]) + } + + if alpha > math.Pi { + alpha = math.Pi + } + if alpha < -1*math.Pi { + alpha = -1 * math.Pi + } + + pt := r3.Vector{0, dist, 0} // Straight line, +Y is "forwards" + angleRads := 0. + if alpha != 0 { + arcRadius := math.Pi * turnRad / math.Abs(alpha) // radius of arc + angleRads = dist / arcRadius // number of radians to travel along arc + pt = r3.Vector{arcRadius * (1 - math.Cos(angleRads)), arcRadius * math.Sin(angleRads), 0} + if alpha > 0 { + // positive alpha = positive rotation = left turn = negative X + pt.X *= -1 + angleRads *= -1 + } + } + pose := spatialmath.NewPose(pt, &spatialmath.OrientationVector{OZ: 1, Theta: -angleRads}) + + return pose, nil +} diff --git a/motionplan/tpspace/ptgCC.go b/motionplan/tpspace/ptgCC.go index 02cb13998cc..a437e0ed337 100644 --- a/motionplan/tpspace/ptgCC.go +++ b/motionplan/tpspace/ptgCC.go @@ -2,6 +2,9 @@ package tpspace import ( "math" + + "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/spatialmath" ) // ptgDiffDriveCC defines a PTG family combined of two stages; first reversing while turning at radius, then moving forwards while turning @@ -10,22 +13,26 @@ import ( type ptgDiffDriveCC struct { maxMMPS float64 // millimeters per second velocity to target maxRPS float64 // radians per second of rotation when driving at maxMMPS and turning at max turning radius - k float64 // k = +1 for forwards, -1 for backwards + + circle *ptgDiffDriveC } // NewCCPTG creates a new PrecomputePTG of type ptgDiffDriveCC. -func NewCCPTG(maxMMPS, maxRPS, k float64) PrecomputePTG { +func NewCCPTG(maxMMPS, maxRPS float64) PrecomputePTG { + circle := NewCirclePTG(maxMMPS, maxRPS).(*ptgDiffDriveC) + return &ptgDiffDriveCC{ maxMMPS: maxMMPS, maxRPS: maxRPS, - k: k, + circle: circle, } } // For this particular driver, turns alpha into a linear + angular velocity. Linear is just max * fwd/back. // Note that this will NOT work as-is for 0-radius turning. Robots capable of turning in place will need to be special-cased // because they will have zero linear velocity through their turns, not max. -func (ptg *ptgDiffDriveCC) PTGVelocities(alpha, t, x, y, phi float64) (float64, float64, error) { +func (ptg *ptgDiffDriveCC) PTGVelocities(alpha, dist float64) (float64, float64, error) { + k := math.Copysign(1.0, dist) r := ptg.maxMMPS / ptg.maxRPS u := math.Abs(alpha) * 0.5 @@ -33,23 +40,45 @@ func (ptg *ptgDiffDriveCC) PTGVelocities(alpha, t, x, y, phi float64) (float64, v := 0. w := 0. - if t < u*r/ptg.maxMMPS { + if dist < u*r { // l- v = -ptg.maxMMPS w = ptg.maxRPS - } else if t < (u+math.Pi*0.5)*r/ptg.maxMMPS { + } else { // l+ v = ptg.maxMMPS w = ptg.maxRPS } - // Turn in the opposite direction?? + // Turn in the opposite direction if alpha < 0 { w *= -1 } - v *= ptg.k - w *= ptg.k + v *= k + w *= k return v, w, nil } + +func (ptg *ptgDiffDriveCC) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { + alpha := inputs[0].Value + dist := inputs[1].Value + r := ptg.maxMMPS / ptg.maxRPS + reverseDistance := math.Abs(alpha) * 0.5 * r + flip := math.Copysign(1., alpha) // left or right + direction := math.Copysign(1., dist) // forwards or backwards + + revPose, err := ptg.circle.Transform([]referenceframe.Input{{-1 * flip * math.Pi}, {-1. * direction * math.Min(dist, reverseDistance)}}) + if err != nil { + return nil, err + } + if dist < reverseDistance { + return revPose, nil + } + fwdPose, err := ptg.circle.Transform([]referenceframe.Input{{flip * math.Pi}, {direction * (dist - reverseDistance)}}) + if err != nil { + return nil, err + } + return spatialmath.Compose(revPose, fwdPose), nil +} diff --git a/motionplan/tpspace/ptgCCS.go b/motionplan/tpspace/ptgCCS.go index 1d3ffc5c6bc..0675ca3d830 100644 --- a/motionplan/tpspace/ptgCCS.go +++ b/motionplan/tpspace/ptgCCS.go @@ -2,6 +2,9 @@ package tpspace import ( "math" + + "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/spatialmath" ) // ptgDiffDriveCCS defines a PTG family combining the CC and CS trajectories, essentially executing the CC trajectory @@ -9,34 +12,38 @@ import ( type ptgDiffDriveCCS struct { maxMMPS float64 // millimeters per second velocity to target maxRPS float64 // radians per second of rotation when driving at maxMMPS and turning at max turning radius - k float64 // k = +1 for forwards, -1 for backwards + r float64 + circle *ptgDiffDriveC } // NewCCSPTG creates a new PrecomputePTG of type ptgDiffDriveCCS. -func NewCCSPTG(maxMMPS, maxRPS, k float64) PrecomputePTG { +func NewCCSPTG(maxMMPS, maxRPS float64) PrecomputePTG { + r := maxMMPS / maxRPS + circle := NewCirclePTG(maxMMPS, maxRPS).(*ptgDiffDriveC) + return &ptgDiffDriveCCS{ maxMMPS: maxMMPS, maxRPS: maxRPS, - k: k, + r: r, + circle: circle, } } // For this particular driver, turns alpha into a linear + angular velocity. Linear is just max * fwd/back. // Note that this will NOT work as-is for 0-radius turning. Robots capable of turning in place will need to be special-cased // because they will have zero linear velocity through their turns, not max. -func (ptg *ptgDiffDriveCCS) PTGVelocities(alpha, t, x, y, phi float64) (float64, float64, error) { +func (ptg *ptgDiffDriveCCS) PTGVelocities(alpha, dist float64) (float64, float64, error) { u := math.Abs(alpha) * 0.5 - - r := ptg.maxMMPS / ptg.maxRPS + k := math.Copysign(1.0, dist) v := ptg.maxMMPS w := 0. - if t < u*r/ptg.maxMMPS { + if dist < u*ptg.r { // l- v = -ptg.maxMMPS w = ptg.maxRPS - } else if t < (u+math.Pi/2)*r/ptg.maxMMPS { + } else if dist < (u+math.Pi/2)*ptg.r { // l+ pi/2 v = ptg.maxMMPS w = ptg.maxRPS @@ -47,8 +54,46 @@ func (ptg *ptgDiffDriveCCS) PTGVelocities(alpha, t, x, y, phi float64) (float64, w *= -1 } - v *= ptg.k - w *= ptg.k + v *= k + w *= k return v, w, nil } + +func (ptg *ptgDiffDriveCCS) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { + alpha := inputs[0].Value + dist := inputs[1].Value + + arcConstant := math.Abs(alpha) * 0.5 + reverseDistance := arcConstant * ptg.r + fwdArcDistance := (arcConstant + math.Pi/2) * ptg.r + flip := math.Copysign(1., alpha) // left or right + direction := math.Copysign(1., dist) // forwards or backwards + + revPose, err := ptg.circle.Transform([]referenceframe.Input{{-1 * flip * math.Pi}, {-1. * direction * math.Min(dist, reverseDistance)}}) + if err != nil { + return nil, err + } + if dist < reverseDistance { + return revPose, nil + } + fwdPose, err := ptg.circle.Transform( + []referenceframe.Input{ + {flip * math.Pi}, + {direction * (math.Min(dist, fwdArcDistance) - reverseDistance)}, + }, + ) + if err != nil { + return nil, err + } + arcPose := spatialmath.Compose(revPose, fwdPose) + if dist < reverseDistance+fwdArcDistance { + return arcPose, nil + } + + finalPose, err := ptg.circle.Transform([]referenceframe.Input{{0}, {direction * (dist - (fwdArcDistance + reverseDistance))}}) + if err != nil { + return nil, err + } + return spatialmath.Compose(arcPose, finalPose), nil +} diff --git a/motionplan/tpspace/ptgCS.go b/motionplan/tpspace/ptgCS.go index 01d7e1c80f0..0e9eaa9da37 100644 --- a/motionplan/tpspace/ptgCS.go +++ b/motionplan/tpspace/ptgCS.go @@ -2,6 +2,13 @@ package tpspace import ( "math" + + "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/spatialmath" +) + +const ( + turnStraightConst = 1.2 // turn at max for this many radians, then go straight, depending on alpha ) // ptgDiffDriveCS defines a PTG family combined of two stages; first driving forwards while turning at radius, going straight. @@ -9,32 +16,35 @@ import ( type ptgDiffDriveCS struct { maxMMPS float64 // millimeters per second velocity to target maxRPS float64 // radians per second of rotation when driving at maxMMPS and turning at max turning radius - k float64 // k = +1 for forwards, -1 for backwards + + r float64 // turning radius + turnStraight float64 } // NewCSPTG creates a new PrecomputePTG of type ptgDiffDriveCS. -func NewCSPTG(maxMMPS, maxRPS, k float64) PrecomputePTG { +func NewCSPTG(maxMMPS, maxRPS float64) PrecomputePTG { + r := maxMMPS / maxRPS + turnStraight := turnStraightConst * r return &ptgDiffDriveCS{ - maxMMPS: maxMMPS, - maxRPS: maxRPS, - k: k, + maxMMPS: maxMMPS, + maxRPS: maxRPS, + r: r, + turnStraight: turnStraight, } } // For this particular driver, turns alpha into a linear + angular velocity. Linear is just max * fwd/back. // Note that this will NOT work as-is for 0-radius turning. Robots capable of turning in place will need to be special-cased // because they will have zero linear velocity through their turns, not max. -func (ptg *ptgDiffDriveCS) PTGVelocities(alpha, t, x, y, phi float64) (float64, float64, error) { - r := ptg.maxMMPS / ptg.maxRPS - +func (ptg *ptgDiffDriveCS) PTGVelocities(alpha, dist float64) (float64, float64, error) { // Magic number; rotate this much before going straight - // Bigger value = more rotation - turnStraight := 1.2 * math.Sqrt(math.Abs(alpha)) * r / ptg.maxMMPS + turnDist := math.Sqrt(math.Abs(alpha)) * ptg.turnStraight + k := math.Copysign(1.0, dist) v := ptg.maxMMPS w := 0. - if t < turnStraight { + if dist < turnDist { // l+ v = ptg.maxMMPS w = ptg.maxRPS * math.Min(1.0, 1.0-math.Exp(-1*alpha*alpha)) @@ -45,7 +55,35 @@ func (ptg *ptgDiffDriveCS) PTGVelocities(alpha, t, x, y, phi float64) (float64, w *= -1 } - v *= ptg.k - w *= ptg.k + v *= k + w *= k return v, w, nil } + +func (ptg *ptgDiffDriveCS) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { + alpha := inputs[0].Value + dist := inputs[1].Value + + actualRPS := ptg.maxRPS * math.Min(1.0, 1.0-math.Exp(-1*alpha*alpha)) + circle := NewCirclePTG(ptg.maxMMPS, actualRPS).(*ptgDiffDriveC) + + arcDistance := ptg.turnStraight * math.Sqrt(math.Abs(alpha)) + flip := math.Copysign(1., alpha) // left or right + direction := math.Copysign(1., dist) // forwards or backwards + var err error + arcPose := spatialmath.NewZeroPose() + if alpha != 0 { + arcPose, err = circle.Transform([]referenceframe.Input{{flip * math.Pi}, {direction * math.Min(dist, arcDistance)}}) + if err != nil { + return nil, err + } + } + if dist < arcDistance { + return arcPose, nil + } + fwdPose, err := circle.Transform([]referenceframe.Input{{0}, {direction * (dist - arcDistance)}}) + if err != nil { + return nil, err + } + return spatialmath.Compose(arcPose, fwdPose), nil +} diff --git a/motionplan/tpspace/ptgGridSim.go b/motionplan/tpspace/ptgGridSim.go index e3eac18f576..cef65c82a75 100644 --- a/motionplan/tpspace/ptgGridSim.go +++ b/motionplan/tpspace/ptgGridSim.go @@ -1,60 +1,52 @@ package tpspace import ( + "context" "math" - "go.viam.com/rdk/spatialmath" + "go.viam.com/rdk/motionplan/ik" + "go.viam.com/rdk/referenceframe" ) const ( defaultMaxTime = 15. defaultDiffT = 0.005 - defaultMinDist = 3. - defaultAlphaCnt uint = 121 - - defaultSearchRadius = 10. - - defaultMaxHeadingChange = 1.95 * math.Pi + defaultAlphaCnt uint = 91 ) // ptgGridSim will take a PrecomputePTG, and simulate out a number of trajectories through some requested time/distance for speed of lookup // later. It will store the trajectories in a grid data structure allowing relatively fast lookups. type ptgGridSim struct { + PrecomputePTG refDist float64 alphaCnt uint maxTime float64 // secs of robot execution to simulate diffT float64 // discretize trajectory simulation to this time granularity - minDist float64 // Save traj points at this arc distance granularity - - simPTG PrecomputePTG precomputeTraj [][]*TrajNode - // Discretized x[y][]node maps for rapid NN lookups - trajNodeGrid map[int]map[int][]*TrajNode - searchRad float64 // Distance around a query point to search for precompute in the cached grid + // If true, then CToTP calls will *only* check the furthest end of each precomputed trajectory. + // This is useful when used in conjunction with IK + endsOnly bool } // NewPTGGridSim creates a new PTG by simulating a PrecomputePTG for some distance, then cacheing the results in a grid for fast lookup. -func NewPTGGridSim(simPTG PrecomputePTG, arcs uint, simDist float64) (PTG, error) { +func NewPTGGridSim(simPTG PrecomputePTG, arcs uint, simDist float64, endsOnly bool) (PTG, error) { if arcs == 0 { arcs = defaultAlphaCnt } ptg := &ptgGridSim{ - refDist: simDist, - alphaCnt: arcs, - maxTime: defaultMaxTime, - diffT: defaultDiffT, - minDist: defaultMinDist, - searchRad: defaultSearchRadius, - - trajNodeGrid: map[int]map[int][]*TrajNode{}, + refDist: simDist, + alphaCnt: arcs, + maxTime: defaultMaxTime, + diffT: defaultDiffT, + endsOnly: endsOnly, } - ptg.simPTG = simPTG + ptg.PrecomputePTG = simPTG - precomp, err := ptg.simulateTrajectories(ptg.simPTG) + precomp, err := ptg.simulateTrajectories() if err != nil { return nil, err } @@ -63,41 +55,38 @@ func NewPTGGridSim(simPTG PrecomputePTG, arcs uint, simDist float64) (PTG, error return ptg, nil } -func (ptg *ptgGridSim) CToTP(x, y float64) []*TrajNode { - nearbyNodes := []*TrajNode{} - - // First, try to do a quick grid-based lookup - // TODO: an octree should be faster - for tx := int(math.Round(x - ptg.searchRad)); tx < int(math.Round(x+ptg.searchRad)); tx++ { - if ptg.trajNodeGrid[tx] != nil { - for ty := int(math.Round(y - ptg.searchRad)); ty < int(math.Round(y+ptg.searchRad)); ty++ { - nearbyNodes = append(nearbyNodes, ptg.trajNodeGrid[tx][ty]...) - } - } - } - - if len(nearbyNodes) > 0 { - return nearbyNodes - } - +func (ptg *ptgGridSim) Solve( + ctx context.Context, + solutionChan chan<- *ik.Solution, + seed []referenceframe.Input, + solveMetric ik.StateMetric, + rseed int, +) error { // Try to find a closest point to the paths: bestDist := math.Inf(1) var bestNode *TrajNode - for k := 0; k < int(ptg.alphaCnt); k++ { - nMax := len(ptg.precomputeTraj[k]) - 1 - for n := 0; n <= nMax; n++ { - distToPoint := math.Pow(ptg.precomputeTraj[k][n].ptX-x, 2) + math.Pow(ptg.precomputeTraj[k][n].ptY-y, 2) - if distToPoint < bestDist { - bestDist = distToPoint + if !ptg.endsOnly { + for k := 0; k < int(ptg.alphaCnt); k++ { + nMax := len(ptg.precomputeTraj[k]) - 1 + for n := 0; n <= nMax; n++ { + distToPoint := solveMetric(&ik.State{Position: ptg.precomputeTraj[k][n].Pose}) + if distToPoint < bestDist { + bestDist = distToPoint - bestNode = ptg.precomputeTraj[k][n] + bestNode = ptg.precomputeTraj[k][n] + } } } - } - if bestNode != nil { - return []*TrajNode{bestNode} + if bestNode != nil { + solutionChan <- &ik.Solution{ + Configuration: []referenceframe.Input{{bestNode.Alpha}, {bestNode.Dist}}, + Score: bestDist, + Exact: false, + } + return nil + } } // Given a point (x,y), compute the "k_closest" whose extrapolation @@ -105,9 +94,7 @@ func (ptg *ptgGridSim) CToTP(x, y float64) []*TrajNode { // which can be normalized by "1/refDistance" to get TP-Space distances. for k := 0; k < int(ptg.alphaCnt); k++ { n := len(ptg.precomputeTraj[k]) - 1 - - distToPoint := math.Pow(ptg.precomputeTraj[k][n].Dist, 2) + - math.Pow(ptg.precomputeTraj[k][n].ptX-x, 2) + math.Pow(ptg.precomputeTraj[k][n].ptY-y, 2) + distToPoint := solveMetric(&ik.State{Position: ptg.precomputeTraj[k][n].Pose}) if distToPoint < bestDist { bestDist = distToPoint @@ -115,113 +102,33 @@ func (ptg *ptgGridSim) CToTP(x, y float64) []*TrajNode { } } - return []*TrajNode{bestNode} + solutionChan <- &ik.Solution{ + Configuration: []referenceframe.Input{{bestNode.Alpha}, {bestNode.Dist}}, + Score: bestDist, + Exact: false, + } + return nil } -func (ptg *ptgGridSim) RefDistance() float64 { +func (ptg *ptgGridSim) MaxDistance() float64 { return ptg.refDist } -func (ptg *ptgGridSim) Trajectory(k uint) []*TrajNode { - if int(k) >= len(ptg.precomputeTraj) { - return nil - } - return ptg.precomputeTraj[k] +func (ptg *ptgGridSim) Trajectory(alpha, dist float64) ([]*TrajNode, error) { + return ComputePTG(ptg, alpha, dist, defaultDiffT) } -func (ptg *ptgGridSim) simulateTrajectories(simPtg PrecomputePTG) ([][]*TrajNode, error) { - xMin := 500.0 - xMax := -500.0 - yMin := 500.0 - yMax := -500.0 - +func (ptg *ptgGridSim) simulateTrajectories() ([][]*TrajNode, error) { // C-space path structure allTraj := make([][]*TrajNode, 0, ptg.alphaCnt) for k := uint(0); k < ptg.alphaCnt; k++ { alpha := index2alpha(k, ptg.alphaCnt) - // Initialize trajectory with an all-zero node - alphaTraj := []*TrajNode{{Pose: spatialmath.NewZeroPose()}} - - var err error - var w float64 - var v float64 - var x float64 - var y float64 - var phi float64 - var t float64 - var dist float64 - - // Last saved waypoints - var wpX float64 - var wpY float64 - var wpPhi float64 - - accumulatedHeadingChange := 0. - - lastVs := [2]float64{0, 0} - lastWs := [2]float64{0, 0} - - // Step through each time point for this alpha - for t < ptg.maxTime && dist < ptg.refDist && accumulatedHeadingChange < defaultMaxHeadingChange { - v, w, err = simPtg.PTGVelocities(alpha, t, x, y, phi) - if err != nil { - return nil, err - } - lastVs[1] = lastVs[0] - lastWs[1] = lastWs[0] - lastVs[0] = v - lastWs[0] = w - - // finite difference eq - x += math.Cos(phi) * v * ptg.diffT - y += math.Sin(phi) * v * ptg.diffT - phi += w * ptg.diffT - accumulatedHeadingChange += w * ptg.diffT - - dist += v * ptg.diffT - t += ptg.diffT - - wpDist1 := math.Sqrt(math.Pow(wpX-x, 2) + math.Pow(wpY-y, 2)) - wpDist2 := math.Abs(wpPhi - phi) - wpDist := math.Max(wpDist1, wpDist2) - - if wpDist > ptg.minDist { - // If our waypoint is farther along than our minimum, update - - // Update velocities of last node because reasons - alphaTraj[len(alphaTraj)-1].LinVelMMPS = v - alphaTraj[len(alphaTraj)-1].AngVelRPS = w - - pose := xythetaToPose(x, y, phi) - alphaTraj = append(alphaTraj, &TrajNode{pose, t, dist, k, v, w, pose.Point().X, pose.Point().Y}) - wpX = x - wpY = y - wpPhi = phi - } - - // For the grid! - xMin = math.Min(xMin, x) - xMax = math.Max(xMax, x) - yMin = math.Min(yMin, y) - yMax = math.Max(yMax, y) - } - - // Add final node - alphaTraj[len(alphaTraj)-1].LinVelMMPS = v - alphaTraj[len(alphaTraj)-1].AngVelRPS = w - pose := xythetaToPose(x, y, phi) - tNode := &TrajNode{pose, t, dist, k, v, w, pose.Point().X, pose.Point().Y} - - // Discretize into a grid for faster lookups later - if _, ok := ptg.trajNodeGrid[int(math.Round(x))]; !ok { - ptg.trajNodeGrid[int(math.Round(x))] = map[int][]*TrajNode{} + alphaTraj, err := ComputePTG(ptg, alpha, ptg.refDist, ptg.diffT) + if err != nil { + return nil, err } - ptg.trajNodeGrid[int(math.Round(x))][int(math.Round(y))] = append(ptg.trajNodeGrid[int(math.Round(x))][int(math.Round(y))], tNode) - - alphaTraj = append(alphaTraj, tNode) - allTraj = append(allTraj, alphaTraj) } diff --git a/motionplan/tpspace/ptgGridSim_test.go b/motionplan/tpspace/ptgGridSim_test.go index 4f22427460b..6551103e68d 100644 --- a/motionplan/tpspace/ptgGridSim_test.go +++ b/motionplan/tpspace/ptgGridSim_test.go @@ -6,26 +6,26 @@ import ( "go.viam.com/test" ) -var defaultPTGs = []func(float64, float64, float64) PrecomputePTG{ - NewCirclePTG, - NewCCPTG, - NewCCSPTG, - NewCSPTG, - NewAlphaPTG, -} - var ( - defaultMps = 0.3 + defaultMMps = 300. turnRadMeters = 0.3 ) func TestSim(t *testing.T) { + simDist := 2500. + alphaCnt := uint(121) for _, ptg := range defaultPTGs { - radPS := defaultMps / turnRadMeters + radPS := defaultMMps / (turnRadMeters * 1000) - ptgGen := ptg(defaultMps, radPS, 1.) + ptgGen := ptg(defaultMMps, radPS) test.That(t, ptgGen, test.ShouldNotBeNil) - _, err := NewPTGGridSim(ptgGen, defaultAlphaCnt, 1000.) + grid, err := NewPTGGridSim(ptgGen, alphaCnt, simDist, false) test.That(t, err, test.ShouldBeNil) + + for i := uint(0); i < alphaCnt; i++ { + traj, err := grid.Trajectory(index2alpha(i, alphaCnt), simDist) + test.That(t, err, test.ShouldBeNil) + test.That(t, traj, test.ShouldNotBeNil) + } } } diff --git a/motionplan/tpspace/ptgGroupFrame.go b/motionplan/tpspace/ptgGroupFrame.go new file mode 100644 index 00000000000..61720ac379e --- /dev/null +++ b/motionplan/tpspace/ptgGroupFrame.go @@ -0,0 +1,208 @@ +package tpspace + +import ( + "errors" + "fmt" + "math" + + "github.com/edaniels/golog" + pb "go.viam.com/api/component/arm/v1" + + "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/spatialmath" +) + +const ( + ptgIndex int = iota + trajectoryAlphaWithinPTG + distanceAlongTrajectoryIndex +) + +// If refDist is not explicitly set, default to pi radians times this adjustment value. +const refDistHalfCircles = 0.9 + +type ptgFactory func(float64, float64) PrecomputePTG + +var defaultPTGs = []ptgFactory{ + NewCirclePTG, + NewCCPTG, + NewCCSPTG, + NewCSPTG, + NewSideSPTG, + NewSideSOverturnPTG, +} + +type ptgGroupFrame struct { + name string + limits []referenceframe.Limit + geometries []spatialmath.Geometry + ptgs []PTG + velocityMMps float64 + turnRadMillimeters float64 + logger golog.Logger +} + +// NewPTGFrameFromTurningRadius will create a new Frame which is also a PTGProvider. It will precompute the default set of +// trajectories out to a given distance, or a default distance if the given distance is <= 0. +func NewPTGFrameFromTurningRadius( + name string, + logger golog.Logger, + velocityMMps, turnRadMeters, refDist float64, + geoms []spatialmath.Geometry, +) (referenceframe.Frame, error) { + if velocityMMps <= 0 { + return nil, fmt.Errorf("cannot create ptg frame, movement velocity %f must be >0", velocityMMps) + } + if turnRadMeters <= 0 { + return nil, fmt.Errorf("cannot create ptg frame, turning radius %f must be >0", turnRadMeters) + } + if refDist < 0 { + return nil, fmt.Errorf("cannot create ptg frame, refDist %f must be >=0", refDist) + } + + turnRadMillimeters := turnRadMeters * 1000 + + if refDist == 0 { + // Default to a distance of just over one half of a circle turning at max radius + refDist = turnRadMillimeters * math.Pi * refDistHalfCircles + logger.Debugf("refDist was zero, calculating default %f", refDist) + } + + // Get max angular velocity in radians per second + maxRPS := velocityMMps / turnRadMillimeters + pf := &ptgGroupFrame{name: name} + err := pf.initPTGs(logger, velocityMMps, maxRPS, refDist) + if err != nil { + return nil, err + } + + pf.geometries = geoms + pf.velocityMMps = velocityMMps + pf.turnRadMillimeters = turnRadMillimeters + + pf.limits = []referenceframe.Limit{ + {Min: 0, Max: float64(len(pf.ptgs) - 1)}, + {Min: -math.Pi, Max: math.Pi}, + {Min: 0, Max: refDist}, + } + + return pf, nil +} + +// NewPTGFrameFromPTGFrame will create a new Frame from a preexisting ptgGroupFrame, allowing the adjustment of `refDist` while keeping +// other params the same. This may be expanded to allow altering turning radius, geometries, etc. +func NewPTGFrameFromPTGFrame(frame referenceframe.Frame, refDist float64) (referenceframe.Frame, error) { + ptgFrame, ok := frame.(*ptgGroupFrame) + if !ok { + return nil, errors.New("cannot create ptg framem given frame is not a ptgGroupFrame") + } + if refDist < 0 { + return nil, fmt.Errorf("cannot create ptg frame, refDist %f must be >=0", refDist) + } + + if refDist <= 0 { + refDist = ptgFrame.turnRadMillimeters * math.Pi * refDistHalfCircles + ptgFrame.logger.Debugf("refDist was zero, calculating default %f", refDist) + } + + // Get max angular velocity in radians per second + maxRPS := ptgFrame.velocityMMps / ptgFrame.turnRadMillimeters + pf := &ptgGroupFrame{name: ptgFrame.name} + err := pf.initPTGs(ptgFrame.logger, ptgFrame.velocityMMps, maxRPS, refDist) + if err != nil { + return nil, err + } + + pf.geometries = ptgFrame.geometries + + pf.limits = []referenceframe.Limit{ + {Min: 0, Max: float64(len(pf.ptgs) - 1)}, + {Min: -math.Pi, Max: math.Pi}, + {Min: 0, Max: refDist}, + } + + return pf, nil +} + +func (pf *ptgGroupFrame) DoF() []referenceframe.Limit { + return pf.limits +} + +func (pf *ptgGroupFrame) Name() string { + return pf.name +} + +// TODO: Define some sort of config struct for a PTG frame. +func (pf *ptgGroupFrame) MarshalJSON() ([]byte, error) { + return nil, nil +} + +// Inputs are: [0] index of PTG to use, [1] index of the trajectory within that PTG, and [2] distance to travel along that trajectory. +func (pf *ptgGroupFrame) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { + if len(inputs) != len(pf.DoF()) { + return nil, referenceframe.NewIncorrectInputLengthError(len(inputs), len(pf.DoF())) + } + alpha := inputs[trajectoryAlphaWithinPTG].Value + dist := inputs[distanceAlongTrajectoryIndex].Value + + ptgIdx := int(math.Round(inputs[ptgIndex].Value)) + + traj, err := pf.ptgs[ptgIdx].Trajectory(alpha, dist) + if err != nil { + return nil, err + } + + return traj[len(traj)-1].Pose, nil +} + +func (pf *ptgGroupFrame) InputFromProtobuf(jp *pb.JointPositions) []referenceframe.Input { + n := make([]referenceframe.Input, len(jp.Values)) + for idx, d := range jp.Values { + n[idx] = referenceframe.Input{d} + } + return n +} + +func (pf *ptgGroupFrame) ProtobufFromInput(input []referenceframe.Input) *pb.JointPositions { + n := make([]float64, len(input)) + for idx, a := range input { + n[idx] = a.Value + } + return &pb.JointPositions{Values: n} +} + +func (pf *ptgGroupFrame) Geometries(inputs []referenceframe.Input) (*referenceframe.GeometriesInFrame, error) { + if len(pf.geometries) == 0 { + return referenceframe.NewGeometriesInFrame(pf.Name(), nil), nil + } + + transformedPose, err := pf.Transform(inputs) + if err != nil { + return nil, err + } + geoms := make([]spatialmath.Geometry, 0, len(pf.geometries)) + for _, geom := range pf.geometries { + geoms = append(geoms, geom.Transform(transformedPose)) + } + return referenceframe.NewGeometriesInFrame(pf.name, geoms), nil +} + +func (pf *ptgGroupFrame) PTGs() []PTG { + return pf.ptgs +} + +func (pf *ptgGroupFrame) initPTGs(logger golog.Logger, maxMps, maxRPS, simDist float64) error { + ptgs := []PTG{} + for _, ptg := range defaultPTGs { + ptgGen := ptg(maxMps, maxRPS) + if ptgGen != nil { + newptg, err := NewPTGIK(ptgGen, logger, simDist, 2) + if err != nil { + return err + } + ptgs = append(ptgs, newptg) + } + } + pf.ptgs = ptgs + return nil +} diff --git a/motionplan/tpspace/ptgIK.go b/motionplan/tpspace/ptgIK.go new file mode 100644 index 00000000000..23c5d17c941 --- /dev/null +++ b/motionplan/tpspace/ptgIK.go @@ -0,0 +1,149 @@ +package tpspace + +import ( + "context" + "errors" + "sync" + + "github.com/edaniels/golog" + + "go.viam.com/rdk/motionplan/ik" + "go.viam.com/rdk/referenceframe" +) + +const ( + defaultResolutionSeconds = 0.01 // seconds. Return trajectories updating velocities at this resolution. + + defaultZeroDist = 1e-3 // Sometimes nlopt will minimize trajectories to zero. Ensure min traj dist is at least this +) + +type ptgIK struct { + PrecomputePTG + refDist float64 + ptgFrame referenceframe.Frame + fastGradDescent *ik.NloptIK + + gridSim PTG + + mu sync.RWMutex + trajCache map[float64][]*TrajNode +} + +// NewPTGIK creates a new ptgIK, which creates a frame using the provided PrecomputePTG, and wraps it providing functions to fill the PTG +// interface, allowing inverse kinematics queries to be run against it. +func NewPTGIK(simPTG PrecomputePTG, logger golog.Logger, refDist float64, randSeed int) (PTG, error) { + if refDist <= 0 { + return nil, errors.New("refDist must be greater than zero") + } + + ptgFrame := newPTGIKFrame(simPTG, refDist) + + nlopt, err := ik.CreateNloptIKSolver(ptgFrame, logger, 1, false) + if err != nil { + return nil, err + } + + // create an ends-only grid sim for quick end-of-trajectory calculations + gridSim, err := NewPTGGridSim(simPTG, 0, refDist, true) + if err != nil { + return nil, err + } + + ptg := &ptgIK{ + PrecomputePTG: simPTG, + refDist: refDist, + ptgFrame: ptgFrame, + fastGradDescent: nlopt, + gridSim: gridSim, + trajCache: map[float64][]*TrajNode{}, + } + + return ptg, nil +} + +func (ptg *ptgIK) Solve( + ctx context.Context, + solutionChan chan<- *ik.Solution, + seed []referenceframe.Input, + solveMetric ik.StateMetric, + nloptSeed int, +) error { + internalSolutionGen := make(chan *ik.Solution, 1) + defer close(internalSolutionGen) + var solved *ik.Solution + + // Spawn the IK solver to generate a solution + err := ptg.fastGradDescent.Solve(ctx, internalSolutionGen, seed, solveMetric, nloptSeed) + // We should have zero or one solutions + + select { + case solved = <-internalSolutionGen: + default: + } + if err != nil || solved == nil || solved.Configuration[1].Value < defaultZeroDist { + // nlopt did not return a valid solution or otherwise errored. Fall back fully to the grid check. + return ptg.gridSim.Solve(ctx, solutionChan, seed, solveMetric, nloptSeed) + } + + if !solved.Exact { + // nlopt returned something but was unable to complete the solve. See if the grid check produces something better. + err = ptg.gridSim.Solve(ctx, internalSolutionGen, seed, solveMetric, nloptSeed) + if err == nil { + var gridSolved *ik.Solution + select { + case gridSolved = <-internalSolutionGen: + default: + } + // Check if the grid has a better solution + if gridSolved != nil { + if gridSolved.Score < solved.Score { + solved = gridSolved + } + } + } + } + + solutionChan <- solved + return nil +} + +func (ptg *ptgIK) MaxDistance() float64 { + return ptg.refDist +} + +func (ptg *ptgIK) Trajectory(alpha, dist float64) ([]*TrajNode, error) { + ptg.mu.RLock() + precomp := ptg.trajCache[alpha] + ptg.mu.RUnlock() + if precomp != nil { + if precomp[len(precomp)-1].Dist >= dist { + // Caching here provides a ~33% speedup to a solve call + subTraj := []*TrajNode{} + for _, wp := range precomp { + if wp.Dist < dist { + subTraj = append(subTraj, wp) + } else { + break + } + } + time := 0. + if len(subTraj) > 0 { + time = subTraj[len(subTraj)-1].Time + } + lastNode, err := computePTGNode(ptg, alpha, dist, time) + if err != nil { + return nil, err + } + subTraj = append(subTraj, lastNode) + return subTraj, nil + } + } + traj, err := ComputePTG(ptg, alpha, dist, defaultResolutionSeconds) + if err != nil { + return nil, err + } + ptg.mu.Lock() + ptg.trajCache[alpha] = traj + ptg.mu.Unlock() + return traj, nil +} diff --git a/motionplan/tpspace/ptgIKFrame.go b/motionplan/tpspace/ptgIKFrame.go new file mode 100644 index 00000000000..57f6ac99c36 --- /dev/null +++ b/motionplan/tpspace/ptgIKFrame.go @@ -0,0 +1,60 @@ +package tpspace + +import ( + "errors" + "math" + + pb "go.viam.com/api/component/arm/v1" + + "go.viam.com/rdk/referenceframe" +) + +// ptgFrame wraps a tpspace.PrecomputePTG so that it fills the Frame interface and can be used by IK. +type ptgIKFrame struct { + PrecomputePTG + limits []referenceframe.Limit +} + +// NewPTGIKFrame will create a new frame intended to be passed to an Inverse Kinematics solver, allowing IK to solve for parameters +// for the passed in PTG. +func newPTGIKFrame(ptg PrecomputePTG, dist float64) referenceframe.Frame { + pf := &ptgIKFrame{PrecomputePTG: ptg} + + pf.limits = []referenceframe.Limit{ + {Min: -math.Pi, Max: math.Pi}, + {Min: 0, Max: dist}, + } + return pf +} + +func (pf *ptgIKFrame) DoF() []referenceframe.Limit { + return pf.limits +} + +func (pf *ptgIKFrame) Name() string { + return "" +} + +func (pf *ptgIKFrame) MarshalJSON() ([]byte, error) { + return nil, errors.New("marshal json not implemented for ptg IK frame") +} + +func (pf *ptgIKFrame) InputFromProtobuf(jp *pb.JointPositions) []referenceframe.Input { + n := make([]referenceframe.Input, len(jp.Values)) + for idx, d := range jp.Values { + n[idx] = referenceframe.Input{d} + } + return n +} + +func (pf *ptgIKFrame) ProtobufFromInput(input []referenceframe.Input) *pb.JointPositions { + n := make([]float64, len(input)) + for idx, a := range input { + n[idx] = a.Value + } + return &pb.JointPositions{Values: n} +} + +func (pf *ptgIKFrame) Geometries(inputs []referenceframe.Input) (*referenceframe.GeometriesInFrame, error) { + return nil, errors.New("geometries not implemented for ptg IK frame") +} diff --git a/motionplan/tpspace/ptgSideS.go b/motionplan/tpspace/ptgSideS.go new file mode 100644 index 00000000000..5c8ab838895 --- /dev/null +++ b/motionplan/tpspace/ptgSideS.go @@ -0,0 +1,106 @@ +package tpspace + +import ( + "math" + + "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/spatialmath" +) + +const defaultCountersteer = 1.5 + +// ptgDiffDriveSideS defines a PTG family which makes a forwards turn, then a counter turn the other direction, and goes straight. +// This has the effect of translating to one side or the other without orientation change. +type ptgDiffDriveSideS struct { + maxMMPS float64 // millimeters per second velocity to target + maxRPS float64 // radians per second of rotation when driving at maxMMPS and turning at max turning radius + r float64 // turning radius + countersteer float64 // scale the length of the second arc by this much + circle *ptgDiffDriveC +} + +// NewSideSPTG creates a new PrecomputePTG of type ptgDiffDriveSideS. +func NewSideSPTG(maxMMPS, maxRPS float64) PrecomputePTG { + r := maxMMPS / maxRPS + circle := NewCirclePTG(maxMMPS, maxRPS).(*ptgDiffDriveC) + + return &ptgDiffDriveSideS{ + maxMMPS: maxMMPS, + maxRPS: maxRPS, + r: r, + countersteer: 1.0, + circle: circle, + } +} + +// NewSideSOverturnPTG creates a new PrecomputePTG of type ptgDiffDriveSideS which overturns. +// It turns X amount in one direction, then countersteers X*countersteerFactor in the other direction. +func NewSideSOverturnPTG(maxMMPS, maxRPS float64) PrecomputePTG { + r := maxMMPS / maxRPS + circle := NewCirclePTG(maxMMPS, maxRPS).(*ptgDiffDriveC) + + return &ptgDiffDriveSideS{ + maxMMPS: maxMMPS, + maxRPS: maxRPS, + r: r, + countersteer: defaultCountersteer, + circle: circle, + } +} + +// For this particular driver, turns alpha into a linear + angular velocity. Linear is just max * fwd/back. +// Note that this will NOT work as-is for 0-radius turning. Robots capable of turning in place will need to be special-cased +// because they will have zero linear velocity through their turns, not max. +func (ptg *ptgDiffDriveSideS) PTGVelocities(alpha, dist float64) (float64, float64, error) { + arcLength := math.Abs(alpha) * 0.5 * ptg.r + v := ptg.maxMMPS + w := 0. + flip := math.Copysign(1., alpha) // left or right + + if dist < arcLength { + // l- + v = ptg.maxMMPS + w = ptg.maxRPS * flip + } else if dist < arcLength+arcLength*ptg.countersteer { + v = ptg.maxMMPS + w = ptg.maxRPS * -1 * flip + } + + return v, w, nil +} + +func (ptg *ptgDiffDriveSideS) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { + alpha := inputs[0].Value + dist := inputs[1].Value + + flip := math.Copysign(1., alpha) // left or right + direction := math.Copysign(1., dist) // forwards or backwards + arcLength := math.Abs(alpha) * 0.5 * ptg.r // + + revPose, err := ptg.circle.Transform([]referenceframe.Input{{flip * math.Pi}, {direction * math.Min(dist, arcLength)}}) + if err != nil { + return nil, err + } + if dist < arcLength { + return revPose, nil + } + fwdPose, err := ptg.circle.Transform( + []referenceframe.Input{ + {-1 * flip * math.Pi}, + {direction * (math.Min(dist, arcLength+arcLength*ptg.countersteer) - arcLength)}, + }, + ) + if err != nil { + return nil, err + } + arcPose := spatialmath.Compose(revPose, fwdPose) + if dist < arcLength+arcLength*ptg.countersteer { + return arcPose, nil + } + + finalPose, err := ptg.circle.Transform([]referenceframe.Input{{0}, {direction * (dist - (arcLength + arcLength*ptg.countersteer))}}) + if err != nil { + return nil, err + } + return spatialmath.Compose(arcPose, finalPose), nil +} diff --git a/motionplan/tpspace/ptg_test.go b/motionplan/tpspace/ptg_test.go new file mode 100644 index 00000000000..081d8339ade --- /dev/null +++ b/motionplan/tpspace/ptg_test.go @@ -0,0 +1,22 @@ +package tpspace + +import ( + "math" + "testing" + + "go.viam.com/test" +) + +func TestAlphaIdx(t *testing.T) { + for i := uint(0); i < defaultAlphaCnt; i++ { + alpha := index2alpha(i, defaultAlphaCnt) + i2 := alpha2index(alpha, defaultAlphaCnt) + test.That(t, i, test.ShouldEqual, i2) + } +} + +func alpha2index(alpha float64, numPaths uint) uint { + alpha = wrapTo2Pi(alpha+math.Pi) - math.Pi + idx := uint(math.Round(0.5 * (float64(numPaths)*(1.0+alpha/math.Pi) - 1.0))) + return idx +} diff --git a/motionplan/tpspace/simPtgAlpha.go b/motionplan/tpspace/simPtgAlpha.go deleted file mode 100644 index e5899d02818..00000000000 --- a/motionplan/tpspace/simPtgAlpha.go +++ /dev/null @@ -1,38 +0,0 @@ -package tpspace - -import ( - "math" -) - -// Pi / 4 (45 degrees), used as a default alpha constant -// This controls how tightly our parabolas arc -// 57 degrees is also sometimes used by the reference. -const quarterPi = 0.78539816339 - -// simPtgAlpha defines a PTG family which follows a parabolic path. -type simPTGAlpha struct { - maxMMPS float64 // millimeters per second velocity to target - maxRPS float64 // radians per second of rotation when driving at maxMMPS and turning at max turning radius -} - -// NewAlphaPTG creates a new PrecomputePTG of type simPtgAlpha. -// K is unused for alpha PTGs *for now* but we may add in the future. -func NewAlphaPTG(maxMMPS, maxRPS, k float64) PrecomputePTG { - return &simPTGAlpha{ - maxMMPS: maxMMPS, - maxRPS: maxRPS, - } -} - -func (ptg *simPTGAlpha) PTGVelocities(alpha, t, x, y, phi float64) (float64, float64, error) { - // In order to know what to set our angvel at, we need to know how far into the path we are - atA := wrapTo2Pi(alpha - phi) - if atA > math.Pi { - atA -= 2 * math.Pi - } - - v := ptg.maxMMPS * math.Exp(-1.*math.Pow(atA/quarterPi, 2)) - w := ptg.maxRPS * (-0.5 + (1. / (1. + math.Exp(-atA/quarterPi)))) - - return v, w, nil -} diff --git a/motionplan/utils.go b/motionplan/utils.go index 0767eb3a33d..28dabbbac90 100644 --- a/motionplan/utils.go +++ b/motionplan/utils.go @@ -5,24 +5,42 @@ import ( "fmt" "math" + "go.viam.com/rdk/motionplan/ik" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" "go.viam.com/rdk/utils" ) -// FrameStepsFromRobotPath is a helper function which will extract the waypoints of a single frame from the map output of a robot path. -func FrameStepsFromRobotPath(frameName string, path []map[string][]referenceframe.Input) ([][]referenceframe.Input, error) { - solution := make([][]referenceframe.Input, 0, len(path)) - for _, step := range path { +// Plan describes a motion plan. +type Plan []map[string][]referenceframe.Input + +// GetFrameSteps is a helper function which will extract the waypoints of a single frame from the map output of a robot path. +func (plan Plan) GetFrameSteps(frameName string) ([][]referenceframe.Input, error) { + solution := make([][]referenceframe.Input, 0, len(plan)) + for _, step := range plan { frameStep, ok := step[frameName] if !ok { - return nil, fmt.Errorf("frame named %s not found in solved motion path", frameName) + return nil, fmt.Errorf("frame named %s not found in solved motion plan", frameName) } solution = append(solution, frameStep) } return solution, nil } +// String returns a human-readable version of the Plan, suitable for debugging. +func (plan Plan) String() string { + var str string + for _, step := range plan { + str += "\n" + for component, input := range step { + if len(input) > 0 { + str += fmt.Sprintf("%s: %v\t", component, input) + } + } + } + return str +} + // PathStepCount will determine the number of steps which should be used to get from the seed to the goal. // The returned value is guaranteed to be at least 1. // stepSize represents both the max mm movement per step, and max R4AA degrees per step. @@ -40,12 +58,12 @@ func PathStepCount(seedPos, goalPos spatialmath.Pose, stepSize float64) int { } // EvaluatePlan assigns a numeric score to a plan that corresponds to the cumulative distance between input waypoints in the plan. -func EvaluatePlan(plan [][]referenceframe.Input, distFunc SegmentMetric) (totalCost float64) { +func EvaluatePlan(plan [][]referenceframe.Input, distFunc ik.SegmentMetric) (totalCost float64) { if len(plan) < 2 { return math.Inf(1) } for i := 0; i < len(plan)-1; i++ { - cost := distFunc(&Segment{StartConfiguration: plan[i], EndConfiguration: plan[i+1]}) + cost := distFunc(&ik.Segment{StartConfiguration: plan[i], EndConfiguration: plan[i+1]}) totalCost += cost } return totalCost @@ -137,7 +155,7 @@ func (r *resultPromise) result(ctx context.Context) ([][]referenceframe.Input, e if planReturn.err() != nil { return nil, planReturn.err() } - return planReturn.toInputs(), nil + return nodesToInputs(planReturn.steps), nil default: } } diff --git a/operation/manager.go b/operation/manager.go index aa1c32421f3..ba29317c64f 100644 --- a/operation/manager.go +++ b/operation/manager.go @@ -10,22 +10,41 @@ import ( "go.viam.com/utils" ) -// SingleOperationManager ensures only 1 operation is happening a time +type anOp struct { + // cancelAndWaitFunc waits until the `SingleOperationManager.currentOp` is empty. This will + // interrupt any existing operations as necessary. + cancelAndWaitFunc func() + // Cancels the context of what's currently running an operation. + interruptFunc context.CancelFunc +} + +// SingleOperationManager ensures only 1 operation is happening at a time. // An operation can be nested, so if there is already an operation in progress, -// it can have sub-operations without an issue. +// it can have sub-operations. type SingleOperationManager struct { - mu sync.Mutex - currentOp *anOp + mu sync.Mutex + opDoneCond *sync.Cond + currentOp *anOp } -// CancelRunning cancel's a current operation unless it's mine. +// NewSingleOperationManager creates a new SingleOperationManager. Use this to appropriately +// initialize the members. +func NewSingleOperationManager() *SingleOperationManager { + ret := &SingleOperationManager{} + ret.opDoneCond = sync.NewCond(&ret.mu) + return ret +} + +// CancelRunning cancels a current operation unless it's mine. func (sm *SingleOperationManager) CancelRunning(ctx context.Context) { if ctx.Value(somCtxKeySingleOp) != nil { return } sm.mu.Lock() defer sm.mu.Unlock() - sm.cancelInLock(ctx) + if sm.currentOp != nil { + sm.currentOp.cancelAndWaitFunc() + } } // OpRunning returns if there is a current operation. @@ -41,32 +60,47 @@ const somCtxKeySingleOp = somCtxKey(iota) // New creates a new operation, cancels previous, returns a new context and function to call when done. func (sm *SingleOperationManager) New(ctx context.Context) (context.Context, func()) { - // handle nested ops + // Handle nested ops. Note an operation set on a context by one `SingleOperationManager` can be + // observed on a different instance of a `SingleOperationManager`. if ctx.Value(somCtxKeySingleOp) != nil { return ctx, func() {} } sm.mu.Lock() - // first cancel any old operation - sm.cancelInLock(ctx) + // Cancel any existing operation. This blocks until the operation is completed. + if sm.currentOp != nil { + sm.currentOp.cancelAndWaitFunc() + } theOp := &anOp{} ctx = context.WithValue(ctx, somCtxKeySingleOp, theOp) - theOp.ctx, theOp.cancelFunc = context.WithCancel(ctx) + var newUserCtx context.Context + newUserCtx, theOp.interruptFunc = context.WithCancel(ctx) + theOp.cancelAndWaitFunc = func() { + // Precondition: Caller must be holding `sm.mu`. + // + // If there are two threads competing to win a race, it's not sufficient to return once the + // condition variable is signaled. We must re-check that a new operation didn't beat us to + // getting the next operation slot. + // + // Ironically, "winning the race" in this scenario just means the "loser" is going to + // immediately interrupt the winner. A future optimization could avoid this unnecessary + // starting/stopping. + for sm.currentOp != nil { + sm.currentOp.interruptFunc() + sm.opDoneCond.Wait() + } + } sm.currentOp = theOp sm.mu.Unlock() - return theOp.ctx, func() { - if !theOp.closed { - theOp.closed = true - } + return newUserCtx, func() { sm.mu.Lock() - if theOp == sm.currentOp { - sm.currentOp = nil - } + sm.opDoneCond.Broadcast() + sm.currentOp = nil sm.mu.Unlock() } } @@ -93,17 +127,11 @@ func (sm *SingleOperationManager) WaitTillNotPowered(ctx context.Context, pollTi ) (err error) { // Defers a function that will stop and clean up if the context errors defer func(ctx context.Context) { - var errStop error if errors.Is(ctx.Err(), context.Canceled) { - sm.mu.Lock() - oldOp := sm.currentOp == ctx.Value(somCtxKeySingleOp) - sm.mu.Unlock() - - if oldOp || sm.currentOp == nil { - errStop = stop(ctx, map[string]interface{}{}) - } + err = multierr.Combine(ctx.Err(), stop(ctx, map[string]interface{}{})) + } else { + err = ctx.Err() } - err = multierr.Combine(ctx.Err(), errStop) }(ctx) return sm.WaitForSuccess( ctx, @@ -138,22 +166,3 @@ func (sm *SingleOperationManager) WaitForSuccess( } } } - -func (sm *SingleOperationManager) cancelInLock(ctx context.Context) { - myOp := ctx.Value(somCtxKeySingleOp) - op := sm.currentOp - - if op == nil || myOp == op { - return - } - - op.cancelFunc() - - sm.currentOp = nil -} - -type anOp struct { - ctx context.Context - cancelFunc context.CancelFunc - closed bool -} diff --git a/operation/manager_test.go b/operation/manager_test.go index 959ca2f6730..bad61e6c292 100644 --- a/operation/manager_test.go +++ b/operation/manager_test.go @@ -12,7 +12,7 @@ import ( ) func TestNestedOperatioDoesNotCancelParent(t *testing.T) { - som := SingleOperationManager{} + som := NewSingleOperationManager() ctx := context.Background() test.That(t, som.NewTimedWaitOp(ctx, time.Millisecond), test.ShouldBeTrue) @@ -24,7 +24,7 @@ func TestNestedOperatioDoesNotCancelParent(t *testing.T) { } func TestCallOnDifferentContext(t *testing.T) { - som := SingleOperationManager{} + som := NewSingleOperationManager() ctx := context.Background() test.That(t, som.NewTimedWaitOp(ctx, time.Millisecond), test.ShouldBeTrue) @@ -51,7 +51,7 @@ func TestCallOnDifferentContext(t *testing.T) { } func TestWaitForSuccess(t *testing.T) { - som := SingleOperationManager{} + som := NewSingleOperationManager() ctx := context.Background() count := int64(0) @@ -70,7 +70,7 @@ func TestWaitForSuccess(t *testing.T) { } func TestWaitForError(t *testing.T) { - som := SingleOperationManager{} + som := NewSingleOperationManager() count := int64(0) err := som.WaitForSuccess( @@ -88,7 +88,7 @@ func TestWaitForError(t *testing.T) { } func TestDontCancel(t *testing.T) { - som := SingleOperationManager{} + som := NewSingleOperationManager() ctx, done := som.New(context.Background()) defer done() @@ -97,26 +97,43 @@ func TestDontCancel(t *testing.T) { } func TestCancelRace(t *testing.T) { - som := SingleOperationManager{} - ctx, done := som.New(context.Background()) - defer done() - - var wg sync.WaitGroup - - wg.Add(1) - go func() { - _, done := som.New(context.Background()) - wg.Done() - defer done() - }() + // First set up the "worker" context and register an operation on + // the `SingleOperationManager` instance. + som := NewSingleOperationManager() + workerError := make(chan error, 1) + workerCtx, somCleanupFunc := som.New(context.Background()) + + // Spin up a separate go-routine for the worker to listen for cancelation. Canceling an + // operation blocks until the operation completes. This goroutine is responsible for running + // `somCleanupFunc` to signal that canceling has completed. + workerFunc := func() { + defer somCleanupFunc() + + select { + case <-workerCtx.Done(): + workerError <- nil + case <-time.After(5 * time.Second): + workerError <- errors.New("Failed to be signaled via a cancel") + } - som.CancelRunning(ctx) - wg.Wait() - test.That(t, ctx.Err(), test.ShouldNotBeNil) + close(workerError) + } + go workerFunc() + + // Set up a "test" context to cancel the worker. + testCtx, testCleanupFunc := context.WithTimeout(context.Background(), time.Second) + defer testCleanupFunc() + som.CancelRunning(testCtx) + // If `workerCtx.Done` was observed to be closed, the worker thread will pass a `nil` error back. + test.That(t, <-workerError, test.ShouldBeNil) + // When `SingleOperationManager` cancels an operation, the operation's context should be in a + // "context canceled" error state. + test.That(t, workerCtx.Err(), test.ShouldNotBeNil) + test.That(t, workerCtx.Err(), test.ShouldEqual, context.Canceled) } func TestStopCalled(t *testing.T) { - som := SingleOperationManager{} + som := NewSingleOperationManager() ctx, done := som.New(context.Background()) defer done() mock := &mock{stopCount: 0} @@ -137,7 +154,7 @@ func TestStopCalled(t *testing.T) { } func TestErrorContainsStopAndCancel(t *testing.T) { - som := SingleOperationManager{} + som := NewSingleOperationManager() ctx, cancel := context.WithCancel(context.Background()) defer cancel() mock := &mock{stopCount: 0} @@ -155,24 +172,6 @@ func TestErrorContainsStopAndCancel(t *testing.T) { test.That(t, errRet.Error(), test.ShouldEqual, "context canceled; Stop failed") } -func TestStopNotCalledOnOldContext(t *testing.T) { - som := SingleOperationManager{} - ctx, done := som.New(context.Background()) - defer done() - mock := &mock{stopCount: 0} - var wg sync.WaitGroup - - wg.Add(1) - go func() { - som.WaitTillNotPowered(ctx, time.Second, mock, mock.stop) - wg.Done() - }() - som.New(context.Background()) - wg.Wait() - test.That(t, ctx.Err(), test.ShouldNotBeNil) - test.That(t, mock.stopCount, test.ShouldEqual, 0) -} - type mock struct { stopCount int } diff --git a/pointcloud/basic_octree.go b/pointcloud/basic_octree.go index 56db75274f1..bf965abbeaa 100644 --- a/pointcloud/basic_octree.go +++ b/pointcloud/basic_octree.go @@ -20,8 +20,8 @@ const ( maxRecursionDepth = 1000 nodeRegionOverlap = 1e-6 // TODO (RSDK-3767): pass these in a different way. - confidenceThreshold = 60 // value between 0-100, threshold sets the confidence level required for a point to be considered a collision - buffer = 60.0 // max distance from base to point for it to be considered a collision in mm + confidenceThreshold = 50 // value between 0-100, threshold sets the confidence level required for a point to be considered a collision + buffer = 150.0 // max distance from base to point for it to be considered a collision in mm ) // NodeType represents the possible types of nodes in an octree. diff --git a/pointcloud/pointcloud_file.go b/pointcloud/pointcloud_file.go index e2480a207ae..b75c591178a 100644 --- a/pointcloud/pointcloud_file.go +++ b/pointcloud/pointcloud_file.go @@ -8,6 +8,7 @@ import ( "image/color" "io" "math" + "os" "path/filepath" "strconv" "strings" @@ -40,6 +41,12 @@ func NewFromFile(fn string, logger golog.Logger) (PointCloud, error) { switch filepath.Ext(fn) { case ".las": return NewFromLASFile(fn, logger) + case ".pcd": + f, err := os.Open(filepath.Clean(fn)) + if err != nil { + return nil, err + } + return ReadPCD(f) default: return nil, errors.Errorf("do not know how to read file %q", fn) } diff --git a/pointcloud/pointcloud_file_test.go b/pointcloud/pointcloud_file_test.go index f9fbdbb30c6..f3c2290fd18 100644 --- a/pointcloud/pointcloud_file_test.go +++ b/pointcloud/pointcloud_file_test.go @@ -39,6 +39,22 @@ func TestNewFromFile(t *testing.T) { nextCloud, err := NewFromFile(temp.Name(), logger) test.That(t, err, test.ShouldBeNil) test.That(t, nextCloud, test.ShouldResemble, cloud) + + cloud, err = NewFromFile(artifact.MustNewPath("pointcloud/test.pcd"), logger) + test.That(t, err, test.ShouldBeNil) + numPoints = cloud.Size() + test.That(t, numPoints, test.ShouldEqual, 293363) + + tempPCD, err := os.CreateTemp(t.TempDir(), "*.pcd") + test.That(t, err, test.ShouldBeNil) + defer os.Remove(tempPCD.Name()) + + err = ToPCD(cloud, tempPCD, PCDAscii) + test.That(t, err, test.ShouldBeNil) + + nextCloud, err = NewFromFile(tempPCD.Name(), logger) + test.That(t, err, test.ShouldBeNil) + test.That(t, nextCloud, test.ShouldResemble, cloud) } func TestPCD(t *testing.T) { diff --git a/referenceframe/errors.go b/referenceframe/errors.go index 16418afb161..2454e24f09f 100644 --- a/referenceframe/errors.go +++ b/referenceframe/errors.go @@ -2,6 +2,9 @@ package referenceframe import "github.com/pkg/errors" +// ErrAtLeastOneEndEffector is an error indicating that at least one end effector is required. +var ErrAtLeastOneEndEffector = errors.New("need at least one end effector") + // ErrCircularReference is an error indicating that a circular path exists somewhere between the end effector and the world. var ErrCircularReference = errors.New("infinite loop finding path from end effector to world") @@ -20,6 +23,13 @@ var ErrMarshalingHighDOFFrame = errors.New("cannot marshal frame with >1 DOF, us // ErrNoWorldConnection describes the error when a frame system is built but nothing is connected to the world node. var ErrNoWorldConnection = errors.New("there are no robot parts that connect to a 'world' node. Root node must be named 'world'") +// ErrNilJointPositions denotes an error when the joint positions are nil. +var ErrNilJointPositions = errors.New("joint positions are nil, check that you are" + + " passing non-empty joint positions when writing your driver") + +// ErrNilModelFrame denotes an error when the kinematics in form of model frames are nil. +var ErrNilModelFrame = errors.New("the model frame is nil, check that you are passing non-empty kinematics when writing your driver") + // NewParentFrameMissingError returns an error for when a part has named a parent whose part is missing from the collection of Parts // that are becoming a FrameSystem object. func NewParentFrameMissingError(partName, parentName string) error { @@ -56,3 +66,20 @@ func NewUnsupportedJointTypeError(jointType string) error { func NewDuplicateGeometryNameError(name string) error { return errors.Errorf("cannot specify multiple geometries with the same name: %s", name) } + +// NewFrameNotInListOfTransformsError returns an error indicating that a frame of the given name +// is missing from the provided list of transforms. +func NewFrameNotInListOfTransformsError(frameName string) error { + return errors.Errorf("frame named '%s' not in the list of transforms", frameName) +} + +// NewParentFrameNotInMapOfParentsError returns an error indicating that a parent from of the given name +// is missing from the provided map of parents. +func NewParentFrameNotInMapOfParentsError(parentFrameName string) error { + return errors.Errorf("parent frame named '%s' not in the map of parents", parentFrameName) +} + +// NewReservedWordError returns an error indicating that the provided name for the config is reserved. +func NewReservedWordError(configType, reservedWord string) error { + return errors.Errorf("reserved word: cannot name a %s '%s'", configType, reservedWord) +} diff --git a/referenceframe/frame.go b/referenceframe/frame.go index d0393581dc2..3461796410e 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -29,16 +29,20 @@ type Limit struct { Max float64 } -// RestrictedRandomFrameInputs will produce a list of valid, in-bounds inputs for the frame, restricting the range to -// `lim` percent of the limits. -func RestrictedRandomFrameInputs(m Frame, rSeed *rand.Rand, lim float64) []Input { +// RestrictedRandomFrameInputs will produce a list of valid, in-bounds inputs for the frame. +// The range of selection is restricted to `restrictionPercent` percent of the limits, and the +// selection frame is centered at reference. +func RestrictedRandomFrameInputs(m Frame, rSeed *rand.Rand, restrictionPercent float64, reference []Input) ([]Input, error) { if rSeed == nil { //nolint:gosec rSeed = rand.New(rand.NewSource(1)) } dof := m.DoF() + if len(reference) != len(dof) { + return nil, NewIncorrectInputLengthError(len(reference), len(dof)) + } pos := make([]Input, 0, len(dof)) - for _, limit := range dof { + for i, limit := range dof { l, u := limit.Min, limit.Max // Default to [-999,999] as range if limits are infinite @@ -49,10 +53,13 @@ func RestrictedRandomFrameInputs(m Frame, rSeed *rand.Rand, lim float64) []Input u = 999 } - span := u - l - pos = append(pos, Input{lim*span*rSeed.Float64() + l + (span * (1 - lim) / 2)}) + frameSpan := u - l + minVal := math.Max(l, reference[i].Value-restrictionPercent*frameSpan/2) + maxVal := math.Min(u, reference[i].Value+restrictionPercent*frameSpan/2) + samplingSpan := maxVal - minVal + pos = append(pos, Input{samplingSpan*rSeed.Float64() + minVal}) } - return pos + return pos, nil } // RandomFrameInputs will produce a list of valid, in-bounds inputs for the referenceframe. diff --git a/referenceframe/frame_system.go b/referenceframe/frame_system.go index e019803ad7b..78e3b4606ff 100644 --- a/referenceframe/frame_system.go +++ b/referenceframe/frame_system.go @@ -410,7 +410,7 @@ func (sfs *simpleFrameSystem) transformFromParent(inputMap map[string][]Input, s } // transform from source to world, world to target parent - return NewPoseInFrame(dst.Name(), spatial.Compose(spatial.PoseInverse(dstToWorld), srcToWorld)), nil + return NewPoseInFrame(dst.Name(), spatial.PoseBetween(dstToWorld, srcToWorld)), nil } // compose the quaternions from the input frame to the world referenceframe. diff --git a/referenceframe/frame_test.go b/referenceframe/frame_test.go index 17e9d0f15f2..a3f1d247a54 100644 --- a/referenceframe/frame_test.go +++ b/referenceframe/frame_test.go @@ -77,10 +77,14 @@ func TestPrismaticFrame(t *testing.T) { randomInputs := RandomFrameInputs(frame, nil) test.That(t, len(randomInputs), test.ShouldEqual, len(frame.DoF())) - restrictRandomInputs := RestrictedRandomFrameInputs(frame, nil, 0.001) - test.That(t, len(restrictRandomInputs), test.ShouldEqual, len(frame.DoF())) - test.That(t, restrictRandomInputs[0].Value, test.ShouldBeLessThan, 0.03) - test.That(t, restrictRandomInputs[0].Value, test.ShouldBeGreaterThan, -0.03) + + for i := 0; i < 10; i++ { + restrictRandomInputs, err := RestrictedRandomFrameInputs(frame, nil, 0.001, FloatsToInputs([]float64{-10})) + test.That(t, err, test.ShouldBeNil) + test.That(t, len(restrictRandomInputs), test.ShouldEqual, len(frame.DoF())) + test.That(t, restrictRandomInputs[0].Value, test.ShouldBeLessThan, -9.07) + test.That(t, restrictRandomInputs[0].Value, test.ShouldBeGreaterThan, -10.03) + } } func TestRevoluteFrame(t *testing.T) { @@ -212,7 +216,9 @@ func TestRandomFrameInputs(t *testing.T) { limitedFrame, _ := NewTranslationalFrame("", r3.Vector{X: 1}, Limit{-2, 2}) for i := 0; i < 100; i++ { - _, err := limitedFrame.Transform(RestrictedRandomFrameInputs(frame, seed, .2)) + r, err := RestrictedRandomFrameInputs(frame, seed, .2, FloatsToInputs([]float64{0})) + test.That(t, err, test.ShouldBeNil) + _, err = limitedFrame.Transform(r) test.That(t, err, test.ShouldBeNil) } } diff --git a/referenceframe/model.go b/referenceframe/model.go index b8970225200..2fead319413 100644 --- a/referenceframe/model.go +++ b/referenceframe/model.go @@ -237,11 +237,17 @@ func floatsToString(inputs []Input) string { func sortTransforms(unsorted map[string]Frame, parentMap map[string]string, start, finish string) ([]Frame, error) { seen := map[string]bool{} - nextTransform := unsorted[start] + nextTransform, ok := unsorted[start] + if !ok { + return nil, NewFrameNotInListOfTransformsError(start) + } orderedTransforms := []Frame{nextTransform} seen[start] = true for { - parent := parentMap[nextTransform.Name()] + parent, ok := parentMap[nextTransform.Name()] + if !ok { + return nil, NewParentFrameNotInMapOfParentsError(nextTransform.Name()) + } if seen[parent] { return nil, ErrCircularReference } @@ -250,7 +256,10 @@ func sortTransforms(unsorted map[string]Frame, parentMap map[string]string, star break } seen[parent] = true - nextTransform = unsorted[parent] + nextTransform, ok = unsorted[parent] + if !ok { + return nil, NewFrameNotInListOfTransformsError(parent) + } orderedTransforms = append(orderedTransforms, nextTransform) } @@ -280,11 +289,13 @@ func ModelFromPath(modelPath, name string) (Model, error) { } // New2DMobileModelFrame builds the kinematic model associated with the kinematicWheeledBase -// This model is intended to be used with a mobile base and has 3DOF corresponding to a state of x, y, and theta -// where x and y are the positional coordinates the base is located about and theta is the rotation about the z axis. +// This model is intended to be used with a mobile base and has either 2DOF corresponding to a state of x, y +// or has 3DOF corresponding to a state of x, y, and theta, where x and y are the positional coordinates +// the base is located about and theta is the rotation about the z axis. func New2DMobileModelFrame(name string, limits []Limit, collisionGeometry spatialmath.Geometry) (Model, error) { - if len(limits) != 2 { - return nil, errors.Errorf("Must have 2DOF state (x, y) to create 2DMobildModelFrame, have %d dof", len(limits)) + if len(limits) != 2 && len(limits) != 3 { + return nil, + errors.Errorf("Must have 2DOF state (x, y) or 3DOF state (x, y, theta) to create 2DMobileModelFrame, have %d dof", len(limits)) } // build the model - SLAM convention is that the XY plane is the ground plane @@ -296,16 +307,20 @@ func New2DMobileModelFrame(name string, limits []Limit, collisionGeometry spatia if err != nil { return nil, err } - orientationLimit := Limit{Min: -2 * math.Pi, Max: 2 * math.Pi} - theta, err := NewRotationalFrame("theta", *spatialmath.NewR4AA(), orientationLimit) - if err != nil { - return nil, err - } geometry, err := NewStaticFrameWithGeometry("geometry", spatialmath.NewZeroPose(), collisionGeometry) if err != nil { return nil, err } + model := NewSimpleModel(name) - model.OrdTransforms = []Frame{x, y, theta, geometry} + if len(limits) == 3 { + theta, err := NewRotationalFrame("theta", *spatialmath.NewR4AA(), limits[2]) + if err != nil { + return nil, err + } + model.OrdTransforms = []Frame{x, y, theta, geometry} + } else { + model.OrdTransforms = []Frame{x, y, geometry} + } return model, nil } diff --git a/referenceframe/model_json.go b/referenceframe/model_json.go index e1736f59a82..a508c3bbdcd 100644 --- a/referenceframe/model_json.go +++ b/referenceframe/model_json.go @@ -34,12 +34,12 @@ func (cfg *ModelConfig) ParseConfig(modelName string) (Model, error) { case "SVA", "": for _, link := range cfg.Links { if link.ID == World { - return nil, errors.New("reserved word: cannot name a link 'world'") + return nil, NewReservedWordError("link", "world") } } for _, joint := range cfg.Joints { if joint.ID == World { - return nil, errors.New("reserved word: cannot name a joint 'world'") + return nil, NewReservedWordError("joint", "world") } } @@ -100,7 +100,7 @@ func (cfg *ModelConfig) ParseConfig(modelName string) (Model, error) { return nil, errors.New("more than one end effector not supported") } if len(parents) < 1 { - return nil, errors.New("need at least one end effector") + return nil, ErrAtLeastOneEndEffector } var eename string // TODO(pl): is there a better way to do all this? Annoying to iterate over a map three times. Maybe if we diff --git a/referenceframe/model_json_test.go b/referenceframe/model_json_test.go index 457f748cce7..9fc005ef652 100644 --- a/referenceframe/model_json_test.go +++ b/referenceframe/model_json_test.go @@ -27,6 +27,15 @@ func TestParseJSONFile(t *testing.T) { "referenceframe/testjson/worldjoint.json", "referenceframe/testjson/worldlink.json", "referenceframe/testjson/worldDH.json", + "referenceframe/testjson/missinglink.json", + } + + badFilesErrors := []error{ + ErrCircularReference, + NewReservedWordError("link", "world"), + NewReservedWordError("joint", "world"), + ErrAtLeastOneEndEffector, + NewFrameNotInListOfTransformsError("base"), } for _, f := range goodFiles { @@ -47,10 +56,11 @@ func TestParseJSONFile(t *testing.T) { }) } - for _, f := range badFiles { + for i, f := range badFiles { t.Run(f, func(tt *testing.T) { _, err := ParseModelJSONFile(utils.ResolveFile(f), "") test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldEqual, badFilesErrors[i].Error()) }) } } diff --git a/referenceframe/model_test.go b/referenceframe/model_test.go index 8550770d3cf..ef91a142a10 100644 --- a/referenceframe/model_test.go +++ b/referenceframe/model_test.go @@ -117,7 +117,7 @@ func TestModelGeometries(t *testing.T) { } func Test2DMobileModelFrame(t *testing.T) { - expLimit := []Limit{{-10, 10}, {-10, 10}} + expLimit := []Limit{{-10, 10}, {-10, 10}, {-2 * math.Pi, 2 * math.Pi}} sphere, err := spatial.NewSphere(spatial.NewZeroPose(), 10, "") test.That(t, err, test.ShouldBeNil) frame, err := New2DMobileModelFrame("test", expLimit, sphere) diff --git a/referenceframe/ptgFrame.go b/referenceframe/ptgFrame.go deleted file mode 100644 index a54582830de..00000000000 --- a/referenceframe/ptgFrame.go +++ /dev/null @@ -1,159 +0,0 @@ -package referenceframe - -import ( - "fmt" - "math" - - pb "go.viam.com/api/component/arm/v1" - - "go.viam.com/rdk/motionplan/tpspace" - "go.viam.com/rdk/spatialmath" -) - -const ( - defaultSimDistMM = 1100. - defaultAlphaCnt uint = 121 -) - -const ( - ptgIndex int = iota - trajectoryIndexWithinPTG - distanceAlongTrajectoryIndex -) - -type ptgFactory func(float64, float64, float64) tpspace.PrecomputePTG - -var defaultPTGs = []ptgFactory{ - tpspace.NewCirclePTG, - tpspace.NewCCPTG, - tpspace.NewCCSPTG, - tpspace.NewCSPTG, - tpspace.NewAlphaPTG, -} - -type ptgGridSimFrame struct { - name string - limits []Limit - geometries []spatialmath.Geometry - ptgs []tpspace.PTG -} - -// NewPTGFrameFromTurningRadius will create a new Frame which is also a tpspace.PTGProvider. It will precompute the default set of -// trajectories out to a given distance, or a default distance if the given distance is <= 0. -func NewPTGFrameFromTurningRadius(name string, velocityMMps, turnRadMeters, simDist float64, geoms []spatialmath.Geometry) (Frame, error) { - if velocityMMps <= 0 { - return nil, fmt.Errorf("cannot create ptg frame, movement velocity %f must be >0", velocityMMps) - } - if turnRadMeters <= 0 { - return nil, fmt.Errorf("cannot create ptg frame, turning radius %f must be >0", turnRadMeters) - } - - if simDist <= 0 { - simDist = defaultSimDistMM - } - - // Get max angular velocity in radians per second - maxRPS := velocityMMps / (1000. * turnRadMeters) - pf := &ptgGridSimFrame{name: name} - err := pf.initPTGs(velocityMMps, maxRPS, simDist) - if err != nil { - return nil, err - } - - pf.geometries = geoms - - pf.limits = []Limit{ - {Min: 0, Max: float64(len(pf.ptgs) - 1)}, - {Min: 0, Max: float64(defaultAlphaCnt)}, - {Min: 0, Max: simDist}, - } - - return pf, nil -} - -func (pf *ptgGridSimFrame) DoF() []Limit { - return pf.limits -} - -func (pf *ptgGridSimFrame) Name() string { - return pf.name -} - -// TODO: Define some sort of config struct for a PTG frame. -func (pf *ptgGridSimFrame) MarshalJSON() ([]byte, error) { - return nil, nil -} - -// Inputs are: [0] index of PTG to use, [1] index of the trajectory within that PTG, and [2] distance to travel along that trajectory. -func (pf *ptgGridSimFrame) Transform(inputs []Input) (spatialmath.Pose, error) { - ptgIdx := int(math.Round(inputs[ptgIndex].Value)) - trajIdx := uint(math.Round(inputs[trajectoryIndexWithinPTG].Value)) - traj := pf.ptgs[ptgIdx].Trajectory(trajIdx) - lastPose := spatialmath.NewZeroPose() - for _, trajNode := range traj { - // Walk the trajectory until we pass the specified distance - if trajNode.Dist > inputs[distanceAlongTrajectoryIndex].Value { - lastPose = trajNode.Pose - } else { - break - } - } - - return lastPose, nil -} - -func (pf *ptgGridSimFrame) InputFromProtobuf(jp *pb.JointPositions) []Input { - n := make([]Input, len(jp.Values)) - for idx, d := range jp.Values { - n[idx] = Input{d} - } - return n -} - -func (pf *ptgGridSimFrame) ProtobufFromInput(input []Input) *pb.JointPositions { - n := make([]float64, len(input)) - for idx, a := range input { - n[idx] = a.Value - } - return &pb.JointPositions{Values: n} -} - -func (pf *ptgGridSimFrame) Geometries(inputs []Input) (*GeometriesInFrame, error) { - if len(pf.geometries) == 0 { - return NewGeometriesInFrame(pf.Name(), nil), nil - } - - transformedPose, err := pf.Transform(inputs) - if err != nil { - return nil, err - } - geoms := make([]spatialmath.Geometry, 0, len(pf.geometries)) - for _, geom := range pf.geometries { - geoms = append(geoms, geom.Transform(transformedPose)) - } - return NewGeometriesInFrame(pf.name, geoms), nil -} - -func (pf *ptgGridSimFrame) PTGs() []tpspace.PTG { - return pf.ptgs -} - -func (pf *ptgGridSimFrame) initPTGs(maxMps, maxRPS, simDist float64) error { - ptgs := []tpspace.PTG{} - for _, ptg := range defaultPTGs { - for _, k := range []float64{1., -1.} { - // Positive K calculates trajectories forwards, negative k calculates trajectories backwards - ptgGen := ptg(maxMps, maxRPS, k) - if ptgGen != nil { - // irreversible trajectories, e.g. alpha, will return nil for negative k - newptg, err := tpspace.NewPTGGridSim(ptgGen, defaultAlphaCnt, simDist) - if err != nil { - return err - } - ptgs = append(ptgs, newptg) - } - } - } - pf.ptgs = ptgs - return nil -} diff --git a/referenceframe/testjson/missinglink.json b/referenceframe/testjson/missinglink.json new file mode 100644 index 00000000000..ecb57c5f381 --- /dev/null +++ b/referenceframe/testjson/missinglink.json @@ -0,0 +1,208 @@ +{ + "name": "missinglink", + "links": [ + { + "id": "base_top", + "parent": "waist", + "translation": { + "x": 0, + "y": 0, + "z": 267 + }, + "geometry": { + "r": 50, + "l": 320, + "translation": { + "x": 0, + "y": 0, + "z": 160 + } + } + }, + { + "id": "upper_arm", + "parent": "shoulder", + "translation": { + "x": 53.5, + "y": 0, + "z": 284.5 + }, + "geometry": { + "x": 110, + "y": 190, + "z": 370, + "translation": { + "x": 0, + "y": 0, + "z": 135 + } + } + }, + { + "id": "upper_forearm", + "parent": "elbow", + "translation": { + "x": 77.5, + "y": 0, + "z": -172.5 + }, + "geometry": { + "x": 100, + "y": 190, + "z": 250, + "translation": { + "x": 49.49, + "y": 0, + "z": -49.49 + }, + "orientation": { + "type": "ov_degrees", + "value": { + "x": 0.707106, + "y": 0, + "z": -0.707106, + "th": 0 + } + } + } + }, + { + "id": "lower_forearm", + "parent": "forearm_rot", + "translation": { + "x": 0, + "y": 0, + "z": -170 + }, + "geometry": { + "r": 45, + "l": 285, + "translation": { + "x": 0, + "y": -27.5, + "z": -104.8 + }, + "orientation": { + "type": "ov_degrees", + "value": { + "th": -90, + "x": 0, + "y": 0.2537568, + "z": 0.9672615 + } + } + } + }, + { + "id": "wrist_link", + "parent": "wrist", + "translation": { + "x": 76, + "y": 0, + "z": -97 + }, + "geometry": { + "x": 150, + "y": 100, + "z": 135, + "translation": { + "x": 75, + "y": 10, + "z": -67.5 + } + } + }, + { + "id": "gripper_mount", + "parent": "gripper_rot", + "translation": { + "x": 0, + "y": 0, + "z": 0 + }, + "orientation": { + "type": "ov_degrees", + "value": { + "x": 0, + "y": 0, + "z": -1, + "th": 0 + } + } + } + ], + "joints": [ + { + "id": "waist", + "type": "revolute", + "parent": "base", + "axis": { + "x": 0, + "y": 0, + "z": 1 + }, + "max": 359, + "min": -359 + }, + { + "id": "shoulder", + "type": "revolute", + "parent": "base_top", + "axis": { + "x": 0, + "y": 1, + "z": 0 + }, + "max": 120, + "min": -118 + }, + { + "id": "elbow", + "type": "revolute", + "parent": "upper_arm", + "axis": { + "x": 0, + "y": 1, + "z": 0 + }, + "max": 10, + "min": -225 + }, + { + "id": "forearm_rot", + "type": "revolute", + "parent": "upper_forearm", + "axis": { + "x": 0, + "y": 0, + "z": -1 + }, + "max": 359, + "min": -359 + }, + { + "id": "wrist", + "type": "revolute", + "parent": "lower_forearm", + "axis": { + "x": 0, + "y": 1, + "z": 0 + }, + "max": 179, + "min": -97 + }, + { + "id": "gripper_rot", + "type": "revolute", + "parent": "wrist_link", + "axis": { + "x": 0, + "y": 0, + "z": -1 + }, + "max": 359, + "min": -359 + } + ] +} diff --git a/resource/errors.go b/resource/errors.go index 801c3e0cf03..cefad7358b5 100644 --- a/resource/errors.go +++ b/resource/errors.go @@ -68,6 +68,13 @@ func (e *mustRebuildError) Error() string { return fmt.Sprintf("cannot reconfigure %q; must rebuild", e.name) } +// NewBuildTimeoutError is used when a resource times out during construction or reconfiguration. +func NewBuildTimeoutError(name Name) error { + return fmt.Errorf( + "resource %s timed out during reconfigure. The default timeout is %v; update %s env variable to override", + name, utils.DefaultResourceConfigurationTimeout, utils.ResourceConfigurationTimeoutEnvVar) +} + // DependencyNotFoundError is used when a resource is not found in a dependencies. func DependencyNotFoundError(name Name) error { return errors.Errorf("%q missing from dependencies", name) diff --git a/resource/resource.go b/resource/resource.go index 4461d130a6b..d3179ff94f1 100644 --- a/resource/resource.go +++ b/resource/resource.go @@ -1,18 +1,18 @@ /* Package resource contains types that help identify and classify resources (components/services) of a robot. -The three most imporant types in this package are: API (which represents an API for a resource), Model (which represents a specific +The three most important types in this package are: API (which represents an API for a resource), Model (which represents a specific implementation of an API), and Name (which represents a specific instantiation of a resource.) -Both API and Model have a "triplet" format which begins with a namespace. API has "namespace:type:subtype" with "type" in this -case being either "service" or "component." Model has "namespace:modelfamily:modelname" with "modelfamily" being somewhat arbitrary, +Both API and Model have a "triplet" format that begins with a namespace. API has "namespace:type:subtype" with "type" in this +case being either "service" or "component." Model has "namespace:modelfamily:modelname" with "modelfamily" being somewhat arbitrary and useful mostly for organization/grouping. Note that each "tier" contains the tier to the left it. Such that ModelFamily contains Namespace, and Model itself contains ModelFamily. -An example resource (say, a motor) may use the motor API, and thus have the API "rdk:component:motor" and have a model such as -"rdk:builtin:gpio". Each individual instance of that motor will have an arbitrary name (defined in the robot's configuration) and that -is represented by a Name type, which also includes the API and (optionally) the remote it belongs to. Thus, the Name contains -everything (API, remote info, and unique name) to locate and cast a resource to the correct interface when requested by a client. While -Model is typically only needed during resource instantiation. +An example resource (say, a motor) may use the motor API and thus have the API "rdk:component:motor" and have a model such as +"rdk:builtin:gpio". Each instance of that motor will have an arbitrary name (defined in the robot's configuration) +represented by a Name type, which also includes the API and (optionally) the remote it belongs to. Thus, the Name contains +everything (API, remote info, and unique name) to locate and cast a resource to the correct interface when requested by a client. +Model on the other hand is typically only needed during resource instantiation. */ package resource @@ -140,7 +140,7 @@ type Actuator interface { type Shaped interface { // Geometries returns the list of geometries associated with the resource, in any order. The poses of the geometries reflect their // current location relative to the frame of the resource. - Geometries(context.Context) ([]spatialmath.Geometry, error) + Geometries(context.Context, map[string]interface{}) ([]spatialmath.Geometry, error) } // ErrDoUnimplemented is returned if the DoCommand methods is not implemented. diff --git a/resource/resource_registry.go b/resource/resource_registry.go index fa8ce08d644..2cea1b70571 100644 --- a/resource/resource_registry.go +++ b/resource/resource_registry.go @@ -4,6 +4,7 @@ import ( "context" "fmt" "reflect" + "strings" "sync" "github.com/edaniels/golog" @@ -74,7 +75,40 @@ type DependencyNotReadyError struct { } func (e *DependencyNotReadyError) Error() string { - return fmt.Sprintf("dependency %q is not ready yet; reason=%q", e.Name, e.Reason) + return fmt.Sprintf("dependency %q is not ready yet; reason=%s", e.Name, e.Reason) +} + +// PrettyPrint returns a formatted string representing a `DependencyNotReadyError` error. This can be useful as a +// `DependencyNotReadyError` often wraps a series of lower level `DependencyNotReadyError` errors. +func (e *DependencyNotReadyError) PrettyPrint() string { + var leafError error + indent := "" + ret := strings.Builder{} + // Iterate through each `Reason`, incrementing the indent at each level. + for curError := e; curError != nil; indent = fmt.Sprintf("%v%v", indent, " ") { + // Give the top-level error different language. + if curError == e { + ret.WriteString(fmt.Sprintf("Dependency %q is not ready yet\n", curError.Name)) + } else { + ret.WriteString(indent) + ret.WriteString(fmt.Sprintf("- Because %q is not ready yet\n", curError.Name)) + } + + // If the `Reason` is also of type `DependencyNotReadyError`, we keep going with the + // "because X is not ready" language. The leaf error will be framed separately. + var errArt *DependencyNotReadyError + if errors.As(curError.Reason, &errArt) { + curError = errArt + } else { + leafError = curError.Reason + curError = nil + } + } + + ret.WriteString(indent) + ret.WriteString(fmt.Sprintf("- Because %q", leafError)) + + return ret.String() } // IsDependencyNotReadyError returns if the given error is any kind of dependency not found error. diff --git a/resource/resource_registry_test.go b/resource/resource_registry_test.go index 229336ec619..ab6576b2719 100644 --- a/resource/resource_registry_test.go +++ b/resource/resource_registry_test.go @@ -3,6 +3,7 @@ package resource_test import ( "context" "errors" + "strings" "testing" "github.com/edaniels/golog" @@ -310,3 +311,17 @@ func TestTransformAttributeMap(t *testing.T) { }, }) } + +func TestDependencyNotReadyError(t *testing.T) { + toe := &resource.DependencyNotReadyError{"toe", errors.New("turf toe")} + foot := &resource.DependencyNotReadyError{"foot", toe} + leg := &resource.DependencyNotReadyError{"leg", foot} + human := &resource.DependencyNotReadyError{"human", leg} + + test.That(t, strings.Count(human.Error(), "\\"), test.ShouldEqual, 0) + test.That(t, human.PrettyPrint(), test.ShouldEqual, `Dependency "human" is not ready yet + - Because "leg" is not ready yet + - Because "foot" is not ready yet + - Because "toe" is not ready yet + - Because "turf toe"`) +} diff --git a/resource/response_metadata.go b/resource/response_metadata.go new file mode 100644 index 00000000000..e7c5fd67b2d --- /dev/null +++ b/resource/response_metadata.go @@ -0,0 +1,27 @@ +package resource + +import ( + "time" + + commonpb "go.viam.com/api/common/v1" + "google.golang.org/protobuf/types/known/timestamppb" +) + +// ResponseMetadata contains extra info associated with a Resource's standard response. +type ResponseMetadata struct { + CapturedAt time.Time +} + +// AsProto turns the ResponseMetadata struct into a protobuf message. +func (rm ResponseMetadata) AsProto() *commonpb.ResponseMetadata { + metadata := &commonpb.ResponseMetadata{} + metadata.CapturedAt = timestamppb.New(rm.CapturedAt) + return metadata +} + +// ResponseMetadataFromProto turns the protobuf message into a ResponseMetadata struct. +func ResponseMetadataFromProto(proto *commonpb.ResponseMetadata) ResponseMetadata { + metadata := ResponseMetadata{} + metadata.CapturedAt = proto.CapturedAt.AsTime() + return metadata +} diff --git a/resource/response_metadata_test.go b/resource/response_metadata_test.go new file mode 100644 index 00000000000..831ffbe8bc5 --- /dev/null +++ b/resource/response_metadata_test.go @@ -0,0 +1,24 @@ +package resource + +import ( + "testing" + "time" + + commonpb "go.viam.com/api/common/v1" + "go.viam.com/test" + "google.golang.org/protobuf/types/known/timestamppb" +) + +func TestResponseToProto(t *testing.T) { + ts := time.UnixMilli(12345) + metadata := ResponseMetadata{CapturedAt: ts} + proto := metadata.AsProto() + test.That(t, proto.CapturedAt.AsTime(), test.ShouldEqual, ts) +} + +func TestResponseFromProto(t *testing.T) { + ts := ×tamppb.Timestamp{Seconds: 12, Nanos: 345000000} + proto := &commonpb.ResponseMetadata{CapturedAt: ts} + metadata := ResponseMetadataFromProto(proto) + test.That(t, metadata.CapturedAt, test.ShouldEqual, time.UnixMilli(12345)) +} diff --git a/rimage/depth_map.go b/rimage/depth_map.go index 7fa0aa6957b..57d07cce132 100644 --- a/rimage/depth_map.go +++ b/rimage/depth_map.go @@ -7,6 +7,7 @@ import ( "image/png" "io" "math" + "strconv" "github.com/pkg/errors" "gonum.org/v1/gonum/mat" @@ -251,6 +252,10 @@ func (dm *DepthMap) ToPrettyPicture(hardMin, hardMax Depth) *Image { // Rotate rotates a copy of this depth map clockwise by the given amount. func (dm *DepthMap) Rotate(amount int) *DepthMap { + if amount == 0 { + return dm + } + if amount == 180 { return dm.Rotate180() } @@ -259,12 +264,12 @@ func (dm *DepthMap) Rotate(amount int) *DepthMap { return dm.Rotate90(true) } - if amount == -90 { + if amount == -90 || amount == 270 { return dm.Rotate90(false) } // made this a panic - panic("vision.DepthMap can only rotate 90, -90, or 180 degrees right now") + panic("vision.DepthMap can only rotate 90, -90, or 180 degrees, not " + strconv.Itoa(amount)) } // Rotate90 rotates a copy of this depth map either by 90 degrees clockwise or counterclockwise. diff --git a/rimage/transform/distorter.go b/rimage/transform/distorter.go index dc39a6fef22..cc41fbb9a48 100644 --- a/rimage/transform/distorter.go +++ b/rimage/transform/distorter.go @@ -31,6 +31,6 @@ func NewDistorter(distortionType DistortionType, parameters []float64) (Distorte case BrownConradyDistortionType: return NewBrownConrady(parameters) default: - return nil, errors.Errorf("do no know how to parse %q distortion model", distortionType) + return nil, errors.Errorf("do not know how to parse %q distortion model", distortionType) } } diff --git a/rimage/transform/parallel_projection.go b/rimage/transform/parallel_projection.go index 5f713b52a9a..77b59ad5372 100644 --- a/rimage/transform/parallel_projection.go +++ b/rimage/transform/parallel_projection.go @@ -6,12 +6,10 @@ import ( "math" "github.com/golang/geo/r3" - "github.com/montanaflynn/stats" "github.com/pkg/errors" "go.viam.com/rdk/pointcloud" "go.viam.com/rdk/rimage" - "go.viam.com/rdk/spatialmath" ) // ParallelProjection to pointclouds are done in a naive way that don't take any camera parameters into account. @@ -99,212 +97,3 @@ func (pp *ParallelProjection) PointCloudToRGBD(cloud pointcloud.PointCloud) (*ri func (pp *ParallelProjection) ImagePointTo3DPoint(pt image.Point, d rimage.Depth) (r3.Vector, error) { return r3.Vector{X: float64(pt.X), Y: float64(pt.Y), Z: float64(d)}, nil } - -// ParallelProjectionOntoXZWithRobotMarker allows the creation of a 2D projection of a pointcloud and robot -// position onto the XZ plane. -type ParallelProjectionOntoXZWithRobotMarker struct { - robotPose *spatialmath.Pose -} - -const ( - sigmaLevel = 7 // level of precision for stdev calculation (determined through experimentation) - imageHeight = 1080 // image height - imageWidth = 1080 // image width - missThreshold = 0.49 // probability limit below which the associated point is assumed to be free space - hitThreshold = 0.55 // probability limit above which the associated point is assumed to indicate an obstacle - pointRadius = 1 // radius of pointcloud point - robotMarkerRadius = 5 // radius of robot marker point -) - -// PointCloudToRGBD creates an image of a pointcloud in the XZ plane, scaling the points to a standard image -// size. It will also add a red marker to the map to represent the location of the robot. The returned depthMap -// is unused and so will always be nil. -func (ppRM *ParallelProjectionOntoXZWithRobotMarker) PointCloudToRGBD(cloud pointcloud.PointCloud, -) (*rimage.Image, *rimage.DepthMap, error) { - meta := cloud.MetaData() - - if cloud.Size() == 0 { - return nil, nil, errors.New("projection point cloud is empty") - } - - meanStdevX, meanStdevZ, err := calculatePointCloudMeanAndStdevXZ(cloud) - if err != nil { - return nil, nil, err - } - - maxX := math.Min(meanStdevX.mean+float64(sigmaLevel)*meanStdevZ.stdev, meta.MaxX) - minX := math.Max(meanStdevX.mean-float64(sigmaLevel)*meanStdevZ.stdev, meta.MinX) - maxZ := math.Min(meanStdevZ.mean+float64(sigmaLevel)*meanStdevZ.stdev, meta.MaxZ) - minZ := math.Max(meanStdevZ.mean-float64(sigmaLevel)*meanStdevZ.stdev, meta.MinZ) - - // Change the max and min values to ensure the robot marker can be represented in the output image - var robotMarker spatialmath.Pose - if ppRM.robotPose != nil { - robotMarker = *ppRM.robotPose - maxX = math.Max(maxX, robotMarker.Point().X) - minX = math.Min(minX, robotMarker.Point().X) - maxZ = math.Max(maxZ, robotMarker.Point().Z) - minZ = math.Min(minZ, robotMarker.Point().Z) - } - - // Calculate the scale factors - scaleFactor := calculateScaleFactor(maxX-minX, maxZ-minZ) - - // Add points in the pointcloud to a new image - var pointColor rimage.Color - im := rimage.NewImage(imageWidth, imageHeight) - cloud.Iterate(0, 0, func(pt r3.Vector, data pointcloud.Data) bool { - x := int(math.Round((pt.X - minX) * scaleFactor)) - y := int(math.Round((pt.Z - minZ) * scaleFactor)) - - // Adds a point to an image using the value to define the color. If no value is available, - // the default color of white is used. - if x >= 0 && x < imageWidth && y >= 0 && y < imageHeight { - pointColor, err = getColorFromProbabilityValue(data) - if err != nil { - return false - } - im.Circle(image.Point{X: x, Y: y}, pointRadius, pointColor) - } - return true - }) - - if err != nil { - return nil, nil, err - } - - // Add a red robot marker to the image - if ppRM.robotPose != nil { - x := int(math.Round((robotMarker.Point().X - minX) * scaleFactor)) - y := int(math.Round((robotMarker.Point().Z - minZ) * scaleFactor)) - robotMarkerColor := rimage.NewColor(255, 0, 0) - im.Circle(image.Point{X: x, Y: y}, robotMarkerRadius, robotMarkerColor) - } - return im, nil, nil -} - -// RGBDToPointCloud is unimplemented and will produce an error. -func (ppRM *ParallelProjectionOntoXZWithRobotMarker) RGBDToPointCloud( - img *rimage.Image, - dm *rimage.DepthMap, - crop ...image.Rectangle, -) (pointcloud.PointCloud, error) { - return nil, errors.New("converting an RGB image to Pointcloud is currently unimplemented for this projection") -} - -// ImagePointTo3DPoint is unimplemented and will produce an error. -func (ppRM *ParallelProjectionOntoXZWithRobotMarker) ImagePointTo3DPoint(pt image.Point, d rimage.Depth) (r3.Vector, error) { - return r3.Vector{}, errors.New("converting an image point to a 3D point is currently unimplemented for this projection") -} - -// getColorFromProbabilityValue returns an RGB color value based on the probability value and defined hit and miss -// thresholds -// TODO (RSDK-1705): Once probability values are available, a temporary algorithm is being used based on Cartographer's method -// of painting images. Currently this function will return a shade of green if the probability is above the hit threshold and -// a shade of blue if it is below the miss threshold. These shades will be more distinct the further from the threshold they are. -func getColorFromProbabilityValue(d pointcloud.Data) (rimage.Color, error) { - var r, g, b uint8 - - if d == nil { - return rimage.NewColor(0, 0, 0), errors.New("data received was null") - } - - if !d.HasValue() { - return rimage.NewColor(255, 255, 255), nil - } - - if d.Value() > 100 || d.Value() < 0 { - return rimage.NewColor(0, 0, 0), - errors.Errorf("received a value of %v which is outside the range (0 - 100) representing probabilities", d.Value()) - } - - prob := float64(d.Value()) / 100. - - switch { - case prob < missThreshold: - b = uint8(255 * ((missThreshold - prob) / (hitThreshold - 0))) - case prob > hitThreshold: - g = uint8(255 * ((prob - hitThreshold) / (1 - missThreshold))) - default: - b = uint8(255 * ((missThreshold - prob) / (hitThreshold - 0))) - g = uint8(255 * ((prob - hitThreshold) / (1 - missThreshold))) - } - - return rimage.NewColor(r, g, b), nil -} - -// NewParallelProjectionOntoXZWithRobotMarker creates a new ParallelProjectionOntoXZWithRobotMarker with the given -// robot pose. -func NewParallelProjectionOntoXZWithRobotMarker(rp *spatialmath.Pose) ParallelProjectionOntoXZWithRobotMarker { - return ParallelProjectionOntoXZWithRobotMarker{robotPose: rp} -} - -// Struct containing the mean and stdev. -type meanStdev struct { - mean float64 - stdev float64 -} - -// Calculates the mean and standard deviation of the X and Z coordinates stored in the point cloud. -func calculatePointCloudMeanAndStdevXZ(cloud pointcloud.PointCloud) (meanStdev, meanStdev, error) { - var X, Z []float64 - var x, z meanStdev - - cloud.Iterate(0, 0, func(pt r3.Vector, data pointcloud.Data) bool { - X = append(X, pt.X) - Z = append(Z, pt.Z) - return true - }) - - meanX, err := safeMath(stats.Mean(X)) - if err != nil { - return x, z, errors.Wrap(err, "unable to calculate mean of X values on given point cloud") - } - x.mean = meanX - - stdevX, err := safeMath(stats.StandardDeviation(X)) - if err != nil { - return x, z, errors.Wrap(err, "unable to calculate stdev of Z values on given point cloud") - } - x.stdev = stdevX - - meanZ, err := safeMath(stats.Mean(Z)) - if err != nil { - return x, z, errors.Wrap(err, "unable to calculate mean of Z values on given point cloud") - } - z.mean = meanZ - - stdevZ, err := safeMath(stats.StandardDeviation(Z)) - if err != nil { - return x, z, errors.Wrap(err, "unable to calculate stdev of Z values on given point cloud") - } - z.stdev = stdevZ - - return x, z, nil -} - -// Calculates the scaling factor needed to fit the projected pointcloud to the desired image size, cropping it -// if needed based on the mean and standard deviation of the X and Z coordinates. -func calculateScaleFactor(xRange, zRange float64) float64 { - var scaleFactor float64 - if xRange != 0 || zRange != 0 { - widthScaleFactor := float64(imageWidth-1) / xRange - heightScaleFactor := float64(imageHeight-1) / zRange - scaleFactor = math.Min(widthScaleFactor, heightScaleFactor) - } - return scaleFactor -} - -// Errors out if overflow has occurred in the given variable or if it is NaN. -func safeMath(v float64, err error) (float64, error) { - if err != nil { - return 0, err - } - switch { - case math.IsInf(v, 0): - return 0, errors.New("overflow detected") - case math.IsNaN(v): - return 0, errors.New("NaN detected") - } - return v, nil -} diff --git a/rimage/transform/parallel_projection_test.go b/rimage/transform/parallel_projection_test.go deleted file mode 100644 index 4b1ff10fdbe..00000000000 --- a/rimage/transform/parallel_projection_test.go +++ /dev/null @@ -1,225 +0,0 @@ -package transform - -import ( - "fmt" - "image" - "math" - "os" - "testing" - - "github.com/golang/geo/r3" - "go.viam.com/test" - "go.viam.com/utils/artifact" - - pc "go.viam.com/rdk/pointcloud" - "go.viam.com/rdk/rimage" - "go.viam.com/rdk/spatialmath" -) - -func TestParallelProjectionOntoXZWithRobotMarker(t *testing.T) { - t.Run("Project an empty pointcloud", func(t *testing.T) { - p := spatialmath.NewPose(r3.Vector{X: 0, Y: 0, Z: 0}, spatialmath.NewOrientationVector()) - ppRM := NewParallelProjectionOntoXZWithRobotMarker(&p) - - pointcloud := pc.New() - - im, unusedDepthMap, err := ppRM.PointCloudToRGBD(pointcloud) - test.That(t, err.Error(), test.ShouldContainSubstring, "projection point cloud is empty") - test.That(t, im, test.ShouldBeNil) - test.That(t, unusedDepthMap, test.ShouldBeNil) - }) - - t.Run("Project a pointcloud with NaN positional value", func(t *testing.T) { - p := spatialmath.NewPose(r3.Vector{X: 0, Y: 0, Z: 0}, spatialmath.NewOrientationVector()) - ppRM := NewParallelProjectionOntoXZWithRobotMarker(&p) - - pointcloud := pc.New() - - p1 := r3.Vector{X: math.NaN(), Y: 0, Z: 0} - err := pointcloud.Set(p1, pc.NewBasicData()) - test.That(t, err, test.ShouldBeNil) - - im, unusedDepthMap, err := ppRM.PointCloudToRGBD(pointcloud) - test.That(t, err.Error(), test.ShouldContainSubstring, "NaN detected") - test.That(t, im, test.ShouldBeNil) - test.That(t, unusedDepthMap, test.ShouldBeNil) - }) - - t.Run("Project a single point pointcloud with no data", func(t *testing.T) { - pose := spatialmath.NewPose(r3.Vector{X: 0, Y: 0, Z: 0}, spatialmath.NewOrientationVector()) - ppRM := NewParallelProjectionOntoXZWithRobotMarker(&pose) - - pointcloud := pc.New() - p1 := r3.Vector{X: 5, Y: 8, Z: 2} - err := pointcloud.Set(p1, pc.NewBasicData()) - test.That(t, err, test.ShouldBeNil) - - im, unusedDepthMap, err := ppRM.PointCloudToRGBD(pointcloud) - test.That(t, err, test.ShouldBeNil) - test.That(t, im.Width(), test.ShouldEqual, imageWidth) - test.That(t, im.Height(), test.ShouldEqual, imageHeight) - test.That(t, unusedDepthMap, test.ShouldBeNil) - - minX := math.Min(pose.Point().X, p1.X) - maxX := math.Max(pose.Point().X, p1.X) - minZ := math.Min(pose.Point().Z, p1.Z) - maxZ := math.Max(pose.Point().Z, p1.Z) - - scaleFactor := math.Min((imageWidth-1)/(maxX-minX), (imageHeight-1)/(maxZ-minZ)) - - robotMarkerExpectedPos := image.Point{ - X: int(math.Round((pose.Point().X - minX) * scaleFactor)), - Y: int(math.Round((pose.Point().Z - minZ) * scaleFactor)), - } - - colorAtPos := im.GetXY(robotMarkerExpectedPos.X, robotMarkerExpectedPos.Y) - expectedRobotMarkerColor := rimage.NewColor(255, 0, 0) - test.That(t, colorAtPos, test.ShouldResemble, expectedRobotMarkerColor) - - pointExpectedPos := image.Point{ - X: int(math.Round((p1.X - minX) * scaleFactor)), - Y: int(math.Round((p1.Z - minZ) * scaleFactor)), - } - - colorAtPoint := im.GetXY(pointExpectedPos.X, pointExpectedPos.Y) - expectedPointColor := rimage.NewColor(255, 255, 255) - test.That(t, err, test.ShouldBeNil) - test.That(t, colorAtPoint, test.ShouldResemble, expectedPointColor) - }) - - t.Run("Project a point with out of range data", func(t *testing.T) { - p := spatialmath.NewPose(r3.Vector{X: 0, Y: 0, Z: 0}, spatialmath.NewOrientationVector()) - ppRM := NewParallelProjectionOntoXZWithRobotMarker(&p) - - pointcloud := pc.New() - err := pointcloud.Set(r3.Vector{X: 0, Y: 0, Z: 0}, pc.NewValueData(200)) - test.That(t, err, test.ShouldBeNil) - - im, unusedDepthMap, err := ppRM.PointCloudToRGBD(pointcloud) - test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, - fmt.Sprintf("received a value of %v which is outside the range (0 - 100) representing probabilities", 200)) - test.That(t, im, test.ShouldBeNil) - test.That(t, unusedDepthMap, test.ShouldBeNil) - }) - - t.Run("Project a two point pointcloud with data with image pixel checks", func(t *testing.T) { - pose := spatialmath.NewPose(r3.Vector{X: 0, Y: 0, Z: 0}, spatialmath.NewOrientationVector()) - ppRM := NewParallelProjectionOntoXZWithRobotMarker(&pose) - - pointcloud := pc.New() - d := pc.NewBasicData() - p1 := r3.Vector{X: -2, Y: 10, Z: -3} - err := pointcloud.Set(p1, d) - test.That(t, err, test.ShouldBeNil) - p2 := r3.Vector{X: 10, Y: 10, Z: 10} - err = pointcloud.Set(p2, d) - test.That(t, err, test.ShouldBeNil) - - im, unusedDepthMap, err := ppRM.PointCloudToRGBD(pointcloud) - test.That(t, err, test.ShouldBeNil) - test.That(t, im.Width(), test.ShouldEqual, imageWidth) - test.That(t, im.Height(), test.ShouldEqual, imageHeight) - test.That(t, unusedDepthMap, test.ShouldBeNil) - - minX := math.Min(math.Min(pose.Point().X, p1.X), p2.X) - maxX := math.Max(math.Max(pose.Point().X, p1.X), p2.X) - minZ := math.Min(math.Min(pose.Point().Z, p1.Z), p2.Z) - maxZ := math.Max(math.Max(pose.Point().Z, p1.Z), p2.Z) - - scaleFactor := math.Min((imageWidth-1)/(maxX-minX), (imageHeight-1)/(maxZ-minZ)) - - robotMarkerExpectedPos := image.Point{ - X: int(math.Round((pose.Point().X - minX) * scaleFactor)), - Y: int(math.Round((pose.Point().Z - minZ) * scaleFactor)), - } - - colorAtPos := im.GetXY(robotMarkerExpectedPos.X, robotMarkerExpectedPos.Y) - expectedRobotMarkerColor := rimage.NewColor(255, 0, 0) - test.That(t, colorAtPos, test.ShouldResemble, expectedRobotMarkerColor) - - point1ExpectedPos := image.Point{ - X: int(math.Round((p1.X - minX) / scaleFactor)), - Y: int(math.Round((p1.Z - minZ) / scaleFactor)), - } - - colorAtPoint1 := im.GetXY(point1ExpectedPos.X, point1ExpectedPos.Y) - expectedPoint1Color, err := getColorFromProbabilityValue(d) - test.That(t, err, test.ShouldBeNil) - test.That(t, colorAtPoint1, test.ShouldResemble, expectedPoint1Color) - - point2ExpectedPos := image.Point{ - X: int(math.Round((p2.X - minX) / scaleFactor)), - Y: int(math.Round((p2.Z - minZ) / scaleFactor)), - } - - colorAtPoint2 := im.GetXY(point2ExpectedPos.X, point2ExpectedPos.Y) - expectedPoint2Color, err := getColorFromProbabilityValue(d) - test.That(t, err, test.ShouldBeNil) - test.That(t, colorAtPoint2, test.ShouldResemble, expectedPoint2Color) - }) - - t.Run("Project an imported pointcloud", func(t *testing.T) { - p := spatialmath.NewPose(r3.Vector{X: 0, Y: 0, Z: 0}, spatialmath.NewOrientationVector()) - ppRM := NewParallelProjectionOntoXZWithRobotMarker(&p) - - pcdFile, err := os.Open(artifact.MustPath("pointcloud/test_short.pcd")) - test.That(t, err, test.ShouldBeNil) - - PC, err := pc.ReadPCD(pcdFile) - test.That(t, err, test.ShouldBeNil) - - im, unusedDepthMap, err := ppRM.PointCloudToRGBD(PC) - test.That(t, err, test.ShouldBeNil) - test.That(t, im.Width(), test.ShouldEqual, imageWidth) - test.That(t, im.Height(), test.ShouldEqual, imageHeight) - test.That(t, unusedDepthMap, test.ShouldBeNil) - }) - - t.Run("Test that projecting two offset pointclouds will produce same image", func(t *testing.T) { - // Image 1 - pose := spatialmath.NewPose(r3.Vector{X: 0, Y: 0, Z: 0}, spatialmath.NewOrientationVector()) - ppRM := NewParallelProjectionOntoXZWithRobotMarker(&pose) - - pointcloud := pc.New() - d := pc.NewBasicData() - p1 := r3.Vector{X: -2, Y: 10, Z: -3} - err := pointcloud.Set(p1, d) - test.That(t, err, test.ShouldBeNil) - p2 := r3.Vector{X: 10, Y: 10, Z: 10} - err = pointcloud.Set(p2, d) - test.That(t, err, test.ShouldBeNil) - - im, unusedDepthMap, err := ppRM.PointCloudToRGBD(pointcloud) - test.That(t, err, test.ShouldBeNil) - test.That(t, im.Width(), test.ShouldEqual, imageWidth) - test.That(t, im.Height(), test.ShouldEqual, imageHeight) - test.That(t, unusedDepthMap, test.ShouldBeNil) - - // Image 2 - offset := r3.Vector{X: 7, Y: -19, Z: 2} - - pose1 := spatialmath.NewPose( - r3.Vector{X: pose.Point().X + offset.X, Y: pose.Point().Y + offset.Y, Z: pose.Point().Z + offset.Z}, - spatialmath.NewOrientationVector(), - ) - ppRM1 := NewParallelProjectionOntoXZWithRobotMarker(&pose1) - - pointcloud = pc.New() - d = pc.NewBasicData() - p1 = r3.Vector{X: p1.X + offset.X, Y: p1.Y + offset.Y, Z: p1.Z + offset.Z} - err = pointcloud.Set(p1, d) - test.That(t, err, test.ShouldBeNil) - p2 = r3.Vector{X: p2.X + offset.X, Y: p2.Y + offset.Y, Z: p2.Z + offset.Z} - err = pointcloud.Set(p2, d) - test.That(t, err, test.ShouldBeNil) - - im1, unusedDepthMap1, err := ppRM1.PointCloudToRGBD(pointcloud) - test.That(t, err, test.ShouldBeNil) - test.That(t, im1.Width(), test.ShouldEqual, imageWidth) - test.That(t, im1.Height(), test.ShouldEqual, imageHeight) - test.That(t, unusedDepthMap1, test.ShouldBeNil) - - test.That(t, im, test.ShouldResemble, im1) - }) -} diff --git a/robot/client/client.go b/robot/client/client.go index e3ed3615c0f..4312f2a796e 100644 --- a/robot/client/client.go +++ b/robot/client/client.go @@ -490,11 +490,18 @@ func (rc *RobotClient) checkConnection(ctx context.Context, checkEvery, reconnec if rc.changeChan != nil { rc.changeChan <- true } + + var notifyParentFn func() if rc.notifyParent != nil { rc.Logger().Debugf("connection was lost for remote %q", rc.address) - rc.notifyParent() + // RSDK-3670: This callback may ultimately acquire the `robotClient.mu` + // mutex. Execute the function after releasing the mutex. + notifyParentFn = rc.notifyParent } rc.mu.Unlock() + if notifyParentFn != nil { + notifyParentFn() + } } } } diff --git a/robot/impl/local_robot.go b/robot/impl/local_robot.go index 4acecb26bbe..afca591484d 100644 --- a/robot/impl/local_robot.go +++ b/robot/impl/local_robot.go @@ -8,6 +8,7 @@ import ( "context" "strings" "sync" + "sync/atomic" "time" "github.com/edaniels/golog" @@ -51,13 +52,14 @@ type localRobot struct { cloudConnSvc cloud.ConnectionService logger golog.Logger activeBackgroundWorkers sync.WaitGroup + reconfigureWorkers sync.WaitGroup cancelBackgroundWorkers func() closeContext context.Context triggerConfig chan struct{} configTicker *time.Ticker revealSensitiveConfigDiffs bool - lastWeakDependentsRound int64 + lastWeakDependentsRound atomic.Int64 // internal services that are in the graph but we also hold onto webSvc web.Service @@ -113,9 +115,6 @@ func (r *localRobot) PackageManager() packages.Manager { // Close attempts to cleanly close down all constituent parts of the robot. func (r *localRobot) Close(ctx context.Context) error { - r.mu.Lock() - defer r.mu.Unlock() - // we will stop and close web ourselves since modules need it to be // removed properly and in the right order, so grab it before its removed // from the graph/closed automatically. @@ -438,15 +437,8 @@ func newWithResources( return nil, err } - // Once web service is started, start module manager and add initially - // specified modules. + // Once web service is started, start module manager r.manager.startModuleManager(r.webSvc.ModuleAddress(), cfg.UntrustedEnv, logger) - for _, mod := range cfg.Modules { - if err := r.manager.moduleManager.Add(ctx, mod); err != nil { - r.logger.Errorw("error adding module", "module", mod.Name, "error", err) - continue - } - } r.activeBackgroundWorkers.Add(1) r.configTicker = time.NewTicker(5 * time.Second) @@ -520,7 +512,7 @@ func (r *localRobot) getDependencies( var needUpdate bool for _, dep := range r.manager.resources.GetAllParentsOf(rName) { if node, ok := r.manager.resources.Node(dep); ok { - if r.lastWeakDependentsRound <= node.UpdatedAt() { + if r.lastWeakDependentsRound.Load() <= node.UpdatedAt() { needUpdate = true } } @@ -620,7 +612,7 @@ func (r *localRobot) newResource( resName := conf.ResourceName() resInfo, ok := resource.LookupRegistration(resName.API, conf.Model) if !ok { - return nil, errors.Errorf("unknown resource type: %s and/or model: %s", resName.API, conf.Model) + return nil, errors.Errorf("unknown resource type: API %q with model %q not registered", resName.API, conf.Model) } deps, err := r.getDependencies(ctx, resName, gNode) @@ -652,7 +644,7 @@ func (r *localRobot) newResource( func (r *localRobot) updateWeakDependents(ctx context.Context) { // track that we are current in resources up to the latest update time. This will // be used to determine if this method should be called while completing a config. - r.lastWeakDependentsRound = r.manager.resources.LastUpdatedTime() + r.lastWeakDependentsRound.Store(r.manager.resources.LastUpdatedTime()) allResources := map[resource.Name]resource.Resource{} internalResources := map[resource.Name]resource.Resource{} @@ -677,34 +669,57 @@ func (r *localRobot) updateWeakDependents(ctx context.Context) { } } + timeout := utils.GetResourceConfigurationTimeout(r.logger) // NOTE(erd): this is intentionally hard coded since these services are treated specially with // how they request dependencies or consume the robot's config. We should make an effort to // formalize these as servcices that while internal, obey the reconfigure lifecycle. // For example, the framesystem should depend on all input enabled components while the web // service depends on all resources. // For now, we pass all resources and empty configs. - for resName, res := range internalResources { - switch resName { - case web.InternalServiceName: - if err := res.Reconfigure(ctx, allResources, resource.Config{}); err != nil { - r.Logger().Errorw("failed to reconfigure internal service", "service", resName, "error", err) - } - case framesystem.InternalServiceName: - fsCfg, err := r.FrameSystemConfig(ctx) - if err != nil { - r.Logger().Errorw("failed to reconfigure internal service", "service", resName, "error", err) - continue - } - if err := res.Reconfigure(ctx, components, resource.Config{ConvertedAttributes: fsCfg}); err != nil { - r.Logger().Errorw("failed to reconfigure internal service", "service", resName, "error", err) + processInternalResources := func(resName resource.Name, res resource.Resource, resChan chan struct{}) { + ctxWithTimeout, timeoutCancel := context.WithTimeout(ctx, timeout) + defer timeoutCancel() + r.reconfigureWorkers.Add(1) + goutils.PanicCapturingGo(func() { + defer func() { + resChan <- struct{}{} + r.reconfigureWorkers.Done() + }() + switch resName { + case web.InternalServiceName: + if err := res.Reconfigure(ctxWithTimeout, allResources, resource.Config{}); err != nil { + r.Logger().Errorw("failed to reconfigure internal service", "service", resName, "error", err) + } + case framesystem.InternalServiceName: + fsCfg, err := r.FrameSystemConfig(ctxWithTimeout) + if err != nil { + r.Logger().Errorw("failed to reconfigure internal service", "service", resName, "error", err) + break + } + if err := res.Reconfigure(ctxWithTimeout, components, resource.Config{ConvertedAttributes: fsCfg}); err != nil { + r.Logger().Errorw("failed to reconfigure internal service", "service", resName, "error", err) + } + case packages.InternalServiceName, cloud.InternalServiceName: + default: + r.logger.Warnw("do not know how to reconfigure internal service", "service", resName) } - case packages.InternalServiceName, cloud.InternalServiceName: - default: - r.logger.Warnw("do not know how to reconfigure internal service", "service", resName) + }) + + select { + case <-resChan: + case <-ctxWithTimeout.Done(): + r.logger.Warn(resource.NewBuildTimeoutError(resName)) } } - updateResourceWeakDependents := func(conf resource.Config) { + for resName, res := range internalResources { + resChan := make(chan struct{}, 1) + resName := resName + res := res + processInternalResources(resName, res, resChan) + } + + updateResourceWeakDependents := func(ctx context.Context, conf resource.Config) { resName := conf.ResourceName() resNode, ok := r.manager.resources.Node(resName) if !ok { @@ -726,12 +741,26 @@ func (r *localRobot) updateWeakDependents(ctx context.Context) { r.Logger().Errorw("failed to reconfigure resource with weak dependencies", "resource", resName, "error", err) } } - conf := r.Config() - for _, conf := range conf.Components { - updateResourceWeakDependents(conf) - } - for _, conf := range conf.Services { - updateResourceWeakDependents(conf) + + cfg := r.Config() + for _, conf := range append(cfg.Components, cfg.Services...) { + conf := conf + ctxWithTimeout, timeoutCancel := context.WithTimeout(ctx, timeout) + defer timeoutCancel() + resChan := make(chan struct{}, 1) + r.reconfigureWorkers.Add(1) + goutils.PanicCapturingGo(func() { + defer func() { + resChan <- struct{}{} + r.reconfigureWorkers.Done() + }() + updateResourceWeakDependents(ctxWithTimeout, conf) + }) + select { + case <-resChan: + case <-ctxWithTimeout.Done(): + r.logger.Warn(resource.NewBuildTimeoutError(conf.ResourceName())) + } } } @@ -954,6 +983,16 @@ func dialRobotClient( func (r *localRobot) Reconfigure(ctx context.Context, newConfig *config.Config) { var allErrs error + // Sync Packages before reconfiguring rest of robot and resolving references to any packages + // in the config. + // TODO(RSDK-1849): Make this non-blocking so other resources that do not require packages can run before package sync finishes. + // TODO(RSDK-2710) this should really use Reconfigure for the package and should allow itself to check + // if anything has changed. + err := r.packageManager.Sync(ctx, newConfig.Packages) + if err != nil { + allErrs = multierr.Combine(allErrs, err) + } + // Add default services and process their dependencies. Dependencies may // already come from config validation so we check that here. seen := make(map[resource.API]int) @@ -1000,23 +1039,8 @@ func (r *localRobot) Reconfigure(ctx context.Context, newConfig *config.Config) } } - // Sync Packages before reconfiguring rest of robot and resolving references to any packages - // in the config. - // TODO(RSDK-1849): Make this non-blocking so other resources that do not require packages can run before package sync finishes. - // TODO(RSDK-2710) this should really use Reconfigure for the package and should allow itself to check - // if anything has changed. - err := r.packageManager.Sync(ctx, newConfig.Packages) - if err != nil { - allErrs = multierr.Combine(allErrs, err) - } - - err = r.replacePackageReferencesWithPaths(newConfig) - if err != nil { - allErrs = multierr.Combine(allErrs, err) - } - // Now that we have the new config and all references are resolved, diff it - // with the current generated config to see what has changed. + // with the current generated config to see what has changed diff, err := config.DiffConfigs(*r.Config(), *newConfig, r.revealSensitiveConfigDiffs) if err != nil { r.logger.Errorw("error diffing the configs", "error", err) @@ -1028,6 +1052,19 @@ func (r *localRobot) Reconfigure(ctx context.Context, newConfig *config.Config) // Set mostRecentConfig if resources were not equal. r.mostRecentCfg = *newConfig + // We need to pre-add the new modules so that resource validation can check against the new models + // TODO(RSDK-4383) These lines are taken from uppdateResources() and should be refactored as part of this bugfix + for _, mod := range diff.Added.Modules { + if err := mod.Validate(""); err != nil { + r.manager.logger.Errorw("module config validation error; skipping", "module", mod.Name, "error", err) + continue + } + if err := r.manager.moduleManager.Add(ctx, mod); err != nil { + r.manager.logger.Errorw("error adding module", "module", mod.Name, "error", err) + continue + } + } + // If something was added or modified, go through components and services in // diff.Added and diff.Modified, call Validate on all those that are modularized, // and store implicit dependencies. @@ -1093,41 +1130,6 @@ func (r *localRobot) Reconfigure(ctx context.Context, newConfig *config.Config) } } -func walkConvertedAttributes[T any](pacMan packages.ManagerSyncer, convertedAttributes T, allErrs error) (T, error) { - // Replace all package references with the actual path containing the package - // on the robot. - var asIfc interface{} = convertedAttributes - if walker, ok := asIfc.(utils.Walker); ok { - newAttrs, err := walker.Walk(packages.NewPackagePathVisitor(pacMan)) - if err != nil { - allErrs = multierr.Combine(allErrs, err) - return convertedAttributes, allErrs - } - newAttrsTyped, err := utils.AssertType[T](newAttrs) - if err != nil { - var zero T - return zero, err - } - convertedAttributes = newAttrsTyped - } - return convertedAttributes, allErrs -} - -func (r *localRobot) replacePackageReferencesWithPaths(cfg *config.Config) error { - var allErrs error - for i, s := range cfg.Services { - s.ConvertedAttributes, allErrs = walkConvertedAttributes(r.packageManager, s.ConvertedAttributes, allErrs) - cfg.Services[i] = s - } - - for i, c := range cfg.Components { - c.ConvertedAttributes, allErrs = walkConvertedAttributes(r.packageManager, c.ConvertedAttributes, allErrs) - cfg.Components[i] = c - } - - return allErrs -} - // checkMaxInstance checks to see if the local robot has reached the maximum number of a specific resource type that are local. func (r *localRobot) checkMaxInstance(api resource.API, max int) error { maxInstance := 0 diff --git a/robot/impl/local_robot_test.go b/robot/impl/local_robot_test.go index 971e0c3bd9c..06450398fb3 100644 --- a/robot/impl/local_robot_test.go +++ b/robot/impl/local_robot_test.go @@ -63,6 +63,7 @@ import ( "go.viam.com/rdk/services/mlmodel" "go.viam.com/rdk/services/mlmodel/tflitecpu" "go.viam.com/rdk/services/motion" + motionBuiltin "go.viam.com/rdk/services/motion/builtin" "go.viam.com/rdk/services/navigation" _ "go.viam.com/rdk/services/register" "go.viam.com/rdk/services/sensors" @@ -1782,7 +1783,7 @@ func TestResourceStartsOnReconfigure(t *testing.T) { test.ShouldBeError, resource.NewNotAvailableError( base.Named("fake0"), - errors.New("resource build error: unknown resource type: rdk:component:base and/or model: rdk:builtin:random"), + errors.New(`resource build error: unknown resource type: API "rdk:component:base" with model "rdk:builtin:random" not registered`), ), ) test.That(t, noBase, test.ShouldBeNil) @@ -1860,11 +1861,13 @@ func TestConfigPackages(t *testing.T) { Name: "some-name-1", Package: "package-1", Version: "v1", + Type: "ml_model", }, { Name: "some-name-2", - Package: "package-1", + Package: "package-2", Version: "v2", + Type: "ml_model", }, }, Cloud: &config.Cloud{ @@ -1878,11 +1881,11 @@ func TestConfigPackages(t *testing.T) { path1, err := r.PackageManager().PackagePath("some-name-1") test.That(t, err, test.ShouldBeNil) - test.That(t, path1, test.ShouldEqual, path.Join(packageDir, "some-name-1")) + test.That(t, path1, test.ShouldEqual, path.Join(packageDir, ".data", "ml_model", "package-1-v1")) path2, err := r.PackageManager().PackagePath("some-name-2") test.That(t, err, test.ShouldBeNil) - test.That(t, path2, test.ShouldEqual, path.Join(packageDir, "some-name-2")) + test.That(t, path2, test.ShouldEqual, path.Join(packageDir, ".data", "ml_model", "package-2-v2")) } func TestConfigPackageReferenceReplacement(t *testing.T) { @@ -1894,7 +1897,7 @@ func TestConfigPackageReferenceReplacement(t *testing.T) { defer utils.UncheckedErrorFunc(fakePackageServer.Shutdown) packageDir := t.TempDir() - labelPath := "${packages.package-2}/labels.txt" + labelPath := "${packages.orgID/some-name-2}/labels.txt" robotConfig := &config.Config{ Packages: []config.PackageConfig{ @@ -1904,10 +1907,22 @@ func TestConfigPackageReferenceReplacement(t *testing.T) { Version: "v1", }, { - Name: "some-name-2", + Name: "orgID/some-name-2", Package: "package-2", Version: "latest", }, + { + Name: "my-module", + Package: "orgID/my-module", + Type: config.PackageTypeModule, + Version: "1.2", + }, + { + Name: "my-ml-model", + Package: "orgID/my-ml-model", + Type: config.PackageTypeMlModel, + Version: "latest", + }, }, PackagePath: packageDir, Services: []resource.Config{ @@ -1916,11 +1931,27 @@ func TestConfigPackageReferenceReplacement(t *testing.T) { API: mlmodel.API, Model: resource.DefaultModelFamily.WithModel("tflite_cpu"), ConvertedAttributes: &tflitecpu.TFLiteConfig{ - ModelPath: "${packages.package-1}/model.tflite", + ModelPath: "${packages.some-name-1}/model.tflite", LabelPath: labelPath, NumThreads: 1, }, }, + { + Name: "my-ml-model", + API: mlmodel.API, + Model: resource.DefaultModelFamily.WithModel("tflite_cpu"), + ConvertedAttributes: &tflitecpu.TFLiteConfig{ + ModelPath: "${packages.ml_models.my-ml-model}/model.tflite", + LabelPath: labelPath, + NumThreads: 2, + }, + }, + }, + Modules: []config.Module{ + { + Name: "my-module", + ExePath: "${packages.modules.my-module}/exec.sh", + }, }, } @@ -2112,12 +2143,16 @@ func TestConfigMethod(t *testing.T) { actualCfg.Components = nil expectedCfg.Components = nil - // Manually inspect remote resource as Equals should be used + // Manually inspect remote resource and process as Equals should be used // (alreadyValidated will have been set to true). test.That(t, len(actualCfg.Remotes), test.ShouldEqual, 1) test.That(t, actualCfg.Remotes[0].Equals(expectedCfg.Remotes[0]), test.ShouldBeTrue) actualCfg.Remotes = nil expectedCfg.Remotes = nil + test.That(t, len(actualCfg.Processes), test.ShouldEqual, 1) + test.That(t, actualCfg.Processes[0].Equals(expectedCfg.Processes[0]), test.ShouldBeTrue) + actualCfg.Processes = nil + expectedCfg.Processes = nil test.That(t, actualCfg, test.ShouldResemble, &expectedCfg) } @@ -2375,16 +2410,18 @@ func TestCheckMaxInstanceValid(t *testing.T) { cfg := &config.Config{ Services: []resource.Config{ { - Name: "fake1", - Model: resource.DefaultServiceModel, - API: motion.API, - DependsOn: []string{framesystem.InternalServiceName.String()}, + Name: "fake1", + Model: resource.DefaultServiceModel, + API: motion.API, + DependsOn: []string{framesystem.InternalServiceName.String()}, + ConvertedAttributes: &motionBuiltin.Config{}, }, { - Name: "fake2", - Model: resource.DefaultServiceModel, - API: motion.API, - DependsOn: []string{framesystem.InternalServiceName.String()}, + Name: "fake2", + Model: resource.DefaultServiceModel, + API: motion.API, + DependsOn: []string{framesystem.InternalServiceName.String()}, + ConvertedAttributes: &motionBuiltin.Config{}, }, }, Components: []resource.Config{ @@ -2826,9 +2863,12 @@ func TestOrphanedResources(t *testing.T) { test.That(t, err, test.ShouldBeError, resource.NewNotFoundError(generic.Named("h"))) - // Also assert that generic helper resource was deregistered. + // Also assert that testmodule's resources were deregistered. _, ok := resource.LookupRegistration(generic.API, helperModel) test.That(t, ok, test.ShouldBeFalse) + testMotorModel := resource.NewModel("rdk", "test", "motor") + _, ok = resource.LookupRegistration(motor.API, testMotorModel) + test.That(t, ok, test.ShouldBeFalse) }) } @@ -3100,3 +3140,132 @@ func TestResourcelessModuleRemove(t *testing.T) { test.ShouldEqual, 1) }) } + +func TestCrashedModuleReconfigure(t *testing.T) { + ctx := context.Background() + logger, logs := golog.NewObservedTestLogger(t) + + testPath, err := rtestutils.BuildTempModule(t, "module/testmodule") + test.That(t, err, test.ShouldBeNil) + + // Lower resource configuration timeout to avoid waiting for 60 seconds + // for manager.Add to time out waiting for module to start listening. + defer func() { + test.That(t, os.Unsetenv(rutils.ResourceConfigurationTimeoutEnvVar), + test.ShouldBeNil) + }() + test.That(t, os.Setenv(rutils.ResourceConfigurationTimeoutEnvVar, "500ms"), + test.ShouldBeNil) + + // Manually define model, as importing it can cause double registration. + helperModel := resource.NewModel("rdk", "test", "helper") + + cfg := &config.Config{ + Modules: []config.Module{ + { + Name: "mod", + ExePath: testPath, + }, + }, + Components: []resource.Config{ + { + Name: "h", + Model: helperModel, + API: generic.API, + }, + }, + } + r, err := robotimpl.New(ctx, cfg, logger) + test.That(t, err, test.ShouldBeNil) + defer func() { + test.That(t, r.Close(context.Background()), test.ShouldBeNil) + }() + + _, err = r.ResourceByName(generic.Named("h")) + test.That(t, err, test.ShouldBeNil) + + // Reconfigure module to a malformed module (does not start listening). + // Assert that "h" is removed after reconfiguration error. + cfg.Modules[0].ExePath = rutils.ResolveFile("module/testmodule/fakemodule.sh") + r.Reconfigure(ctx, cfg) + + testutils.WaitForAssertion(t, func(tb testing.TB) { + test.That(t, logs.FilterMessage("error reconfiguring module").Len(), test.ShouldEqual, 1) + }) + + _, err = r.ResourceByName(generic.Named("h")) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err, test.ShouldBeError, + resource.NewNotFoundError(generic.Named("h"))) + + // Reconfigure module back to testmodule. Assert that 'h' is eventually + // added back to the resource manager (the module recovers). + cfg.Modules[0].ExePath = testPath + r.Reconfigure(ctx, cfg) + + testutils.WaitForAssertion(t, func(tb testing.TB) { + _, err = r.ResourceByName(generic.Named("h")) + test.That(tb, err, test.ShouldBeNil) + }) +} + +func TestImplicitDepsAcrossModules(t *testing.T) { + ctx := context.Background() + logger, _ := golog.NewObservedTestLogger(t) + + // Precompile modules to avoid timeout issues when building takes too long. + complexPath, err := rtestutils.BuildTempModule(t, "examples/customresources/demos/complexmodule") + test.That(t, err, test.ShouldBeNil) + testPath, err := rtestutils.BuildTempModule(t, "module/testmodule") + test.That(t, err, test.ShouldBeNil) + + // Manually define models, as importing them can cause double registration. + myBaseModel := resource.NewModel("acme", "demo", "mybase") + testMotorModel := resource.NewModel("rdk", "test", "motor") + + cfg := &config.Config{ + Modules: []config.Module{ + { + Name: "complex-module", + ExePath: complexPath, + }, + { + Name: "test-module", + ExePath: testPath, + }, + }, + Components: []resource.Config{ + { + Name: "b", + Model: myBaseModel, + API: base.API, + Attributes: rutils.AttributeMap{ + "motorL": "m1", + "motorR": "m2", + }, + }, + { + Name: "m1", + Model: testMotorModel, + API: motor.API, + }, + { + Name: "m2", + Model: testMotorModel, + API: motor.API, + }, + }, + } + r, err := robotimpl.New(ctx, cfg, logger) + test.That(t, err, test.ShouldBeNil) + defer func() { + test.That(t, r.Close(context.Background()), test.ShouldBeNil) + }() + + _, err = r.ResourceByName(base.Named("b")) + test.That(t, err, test.ShouldBeNil) + _, err = r.ResourceByName(motor.Named("m1")) + test.That(t, err, test.ShouldBeNil) + _, err = r.ResourceByName(motor.Named("m2")) + test.That(t, err, test.ShouldBeNil) +} diff --git a/robot/impl/resource_manager.go b/robot/impl/resource_manager.go index 67d3e0a2019..74ab8233941 100644 --- a/robot/impl/resource_manager.go +++ b/robot/impl/resource_manager.go @@ -13,6 +13,7 @@ import ( "github.com/jhump/protoreflect/desc" "github.com/pkg/errors" "go.uber.org/multierr" + goutils "go.viam.com/utils" "go.viam.com/utils/pexec" "go.viam.com/utils/rpc" @@ -527,58 +528,83 @@ func (manager *resourceManager) completeConfig( } resourceNames := manager.resources.ReverseTopologicalSort() + timeout := rutils.GetResourceConfigurationTimeout(manager.logger) for _, resName := range resourceNames { - gNode, ok := manager.resources.Node(resName) - if !ok || !gNode.NeedsReconfigure() { - continue - } - if !(resName.API.IsComponent() || resName.API.IsService()) { - continue - } - var verb string - if gNode.IsUninitialized() { - verb = "configuring" - } else { - verb = "reconfiguring" - } - manager.logger.Debugw(fmt.Sprintf("now %s resource", verb), "resource", resName) - conf := gNode.Config() - - // this is done in config validation but partial start rules require us to check again - if _, err := conf.Validate("", resName.API.Type.Name); err != nil { - manager.logger.Errorw("resource config validation error", "resource", conf.ResourceName(), "model", conf.Model, "error", err) - gNode.SetLastError(errors.Wrap(err, "config validation error found in resource: "+conf.ResourceName().String())) - continue - } - if manager.moduleManager.Provides(conf) { - if _, err := manager.moduleManager.ValidateConfig(ctx, conf); err != nil { - manager.logger.Errorw("modular resource config validation error", "resource", conf.ResourceName(), "model", conf.Model, "error", err) - gNode.SetLastError(errors.Wrap(err, "config validation error found in modular resource: "+conf.ResourceName().String())) - continue + resChan := make(chan struct{}, 1) + resName := resName + ctxWithTimeout, timeoutCancel := context.WithTimeout(ctx, timeout) + defer timeoutCancel() + robot.reconfigureWorkers.Add(1) + goutils.PanicCapturingGo(func() { + defer func() { + resChan <- struct{}{} + robot.reconfigureWorkers.Done() + }() + gNode, ok := manager.resources.Node(resName) + if !ok || !gNode.NeedsReconfigure() { + return } - } + if !(resName.API.IsComponent() || resName.API.IsService()) { + return + } + var verb string + if gNode.IsUninitialized() { + verb = "configuring" + } else { + verb = "reconfiguring" + } + manager.logger.Debugw(fmt.Sprintf("now %s resource", verb), "resource", resName) + conf := gNode.Config() - switch { - case resName.API.IsComponent(), resName.API.IsService(): - newRes, newlyBuilt, err := manager.processResource(ctx, conf, gNode, robot) - if newlyBuilt || err != nil { - if err := manager.markChildrenForUpdate(resName); err != nil { - manager.logger.Errorw( - "failed to mark children of resource for update", - "resource", resName, - "reason", err) + // this is done in config validation but partial start rules require us to check again + if _, err := conf.Validate("", resName.API.Type.Name); err != nil { + manager.logger.Errorw("resource config validation error", "resource", conf.ResourceName(), "model", conf.Model, "error", err) + gNode.SetLastError(errors.Wrap(err, "config validation error found in resource: "+conf.ResourceName().String())) + return + } + if manager.moduleManager.Provides(conf) { + if _, err := manager.moduleManager.ValidateConfig(ctxWithTimeout, conf); err != nil { + manager.logger.Errorw("modular resource config validation error", "resource", conf.ResourceName(), "model", conf.Model, "error", err) + gNode.SetLastError(errors.Wrap(err, "config validation error found in modular resource: "+conf.ResourceName().String())) + return } } - if err != nil { - manager.logger.Errorw("error building resource", "resource", conf.ResourceName(), "model", conf.Model, "error", err) - gNode.SetLastError(errors.Wrap(err, "resource build error")) - continue + + switch { + case resName.API.IsComponent(), resName.API.IsService(): + newRes, newlyBuilt, err := manager.processResource(ctxWithTimeout, conf, gNode, robot) + if newlyBuilt || err != nil { + if err := manager.markChildrenForUpdate(resName); err != nil { + manager.logger.Errorw( + "failed to mark children of resource for update", + "resource", resName, + "reason", err) + } + } + if err != nil { + manager.logger.Errorw("error building resource", "resource", conf.ResourceName(), "model", conf.Model, "error", err) + gNode.SetLastError(errors.Wrap(err, "resource build error")) + return + } + // if the ctxWithTimeout has an error then that means we've timed out. This means + // that resource generation is running async, and we don't currently have good + // validation around how this might affect the resource graph. So, we avoid updating + // the graph to be safe. + if ctxWithTimeout.Err() != nil { + manager.logger.Errorw("error building resource", "resource", conf.ResourceName(), "model", conf.Model, "error", ctxWithTimeout.Err()) + } else { + gNode.SwapResource(newRes, conf.Model) + } + default: + err := errors.New("config is not for a component or service") + manager.logger.Errorw(err.Error(), "resource", resName) + gNode.SetLastError(err) } - gNode.SwapResource(newRes, conf.Model) - default: - err := errors.New("config is not for a component or service") - manager.logger.Errorw(err.Error(), "resource", resName) - gNode.SetLastError(err) + }) + select { + case <-resChan: + case <-ctxWithTimeout.Done(): + robot.logger.Warn(resource.NewBuildTimeoutError(resName)) } } } @@ -849,6 +875,11 @@ func (manager *resourceManager) updateResources( break } + // this is done in config validation but partial start rules require us to check again + if err := p.Validate(""); err != nil { + manager.logger.Errorw("process config validation error; skipping", "process", p.Name, "error", err) + continue + } _, err := manager.processManager.AddProcessFromConfig(ctx, p) if err != nil { manager.logger.Errorw("error while adding process; skipping", "process", p.ID, "error", err) @@ -873,6 +904,11 @@ func (manager *resourceManager) updateResources( // Remove processConfig from map in case re-addition fails. delete(manager.processConfigs, p.ID) + // this is done in config validation but partial start rules require us to check again + if err := p.Validate(""); err != nil { + manager.logger.Errorw("process config validation error; skipping", "process", p.Name, "error", err) + continue + } _, err := manager.processManager.AddProcessFromConfig(ctx, p) if err != nil { manager.logger.Errorw("error while changing process; skipping", "process", p.ID, "error", err) @@ -883,12 +919,22 @@ func (manager *resourceManager) updateResources( // modules are not added into the resource tree as they belong to the module manager for _, mod := range conf.Added.Modules { + // this is done in config validation but partial start rules require us to check again + if err := mod.Validate(""); err != nil { + manager.logger.Errorw("module config validation error; skipping", "module", mod.Name, "error", err) + continue + } if err := manager.moduleManager.Add(ctx, mod); err != nil { manager.logger.Errorw("error adding module", "module", mod.Name, "error", err) continue } } for _, mod := range conf.Modified.Modules { + // this is done in config validation but partial start rules require us to check again + if err := mod.Validate(""); err != nil { + manager.logger.Errorw("module config validation error; skipping", "module", mod.Name, "error", err) + continue + } orphanedResourceNames, err := manager.moduleManager.Reconfigure(ctx, mod) if err != nil { manager.logger.Errorw("error reconfiguring module", "module", mod.Name, "error", err) diff --git a/robot/impl/resource_manager_modular_test.go b/robot/impl/resource_manager_modular_test.go index b35984ec65c..82803723968 100644 --- a/robot/impl/resource_manager_modular_test.go +++ b/robot/impl/resource_manager_modular_test.go @@ -18,6 +18,7 @@ import ( "go.viam.com/rdk/resource" "go.viam.com/rdk/robot/framesystem" "go.viam.com/rdk/services/motion" + motionBuiltin "go.viam.com/rdk/services/motion/builtin" "go.viam.com/rdk/utils" ) @@ -201,7 +202,7 @@ func TestModularResources(t *testing.T) { Name: "builtin", API: motion.API, Model: resource.DefaultServiceModel, - ConvertedAttributes: &fake.Config{}, + ConvertedAttributes: &motionBuiltin.Config{}, DependsOn: []string{framesystem.InternalServiceName.String()}, } _, err = cfg3.Validate("test", resource.APITypeServiceName) diff --git a/robot/impl/robot_reconfigure_test.go b/robot/impl/robot_reconfigure_test.go index f74fe739417..e2cee5c4c5b 100644 --- a/robot/impl/robot_reconfigure_test.go +++ b/robot/impl/robot_reconfigure_test.go @@ -8,6 +8,7 @@ import ( "net" "os" "path/filepath" + "strings" "sync/atomic" "testing" "time" @@ -16,6 +17,7 @@ import ( "github.com/edaniels/golog" "github.com/google/uuid" "github.com/pkg/errors" + "go.uber.org/zap/zaptest/observer" "go.viam.com/test" "go.viam.com/utils" "go.viam.com/utils/pexec" @@ -25,12 +27,14 @@ import ( "go.viam.com/rdk/components/arm/fake" "go.viam.com/rdk/components/audioinput" "go.viam.com/rdk/components/base" + "go.viam.com/rdk/components/base/wheeled" "go.viam.com/rdk/components/board" "go.viam.com/rdk/components/camera" "go.viam.com/rdk/components/encoder" "go.viam.com/rdk/components/generic" "go.viam.com/rdk/components/gripper" "go.viam.com/rdk/components/motor" + fakemotor "go.viam.com/rdk/components/motor/fake" "go.viam.com/rdk/components/movementsensor" "go.viam.com/rdk/components/sensor" "go.viam.com/rdk/components/servo" @@ -45,6 +49,7 @@ import ( _ "go.viam.com/rdk/services/sensors/builtin" rdktestutils "go.viam.com/rdk/testutils" "go.viam.com/rdk/testutils/robottestutils" + rutils "go.viam.com/rdk/utils" ) var ( @@ -3495,6 +3500,120 @@ func TestReconfigureRename(t *testing.T) { test.That(t, res2.(*mockFake).closeCount, test.ShouldEqual, 0) } +func TestResourceConstructTimeout(t *testing.T) { + cfg := &config.Config{} + ctx := context.Background() + logger, logs := golog.NewObservedTestLogger(t) + fakeModel := resource.DefaultModelFamily.WithModel("fake") + + timeOutErrorCount := func() int { + return logs.Filter(func(o observer.LoggedEntry) bool { + for k, v := range o.ContextMap() { + if k == "error" && strings.Contains(fmt.Sprint(v), "timed out during reconfigure") { + return true + } + } + return false + }).Len() + } + + r, err := New(ctx, cfg, logger) + defer func() { + test.That(t, r.Close(context.Background()), test.ShouldBeNil) + }() + test.That(t, err, test.ShouldBeNil) + + // test no error logging with default config + testutils.WaitForAssertion(t, func(tb testing.TB) { + test.That(tb, timeOutErrorCount(), test.ShouldEqual, 0) + }) + + // create new config with resource that conceivably could time out + newCfg := &config.Config{ + Components: []resource.Config{ + { + Name: "fakewheel", + API: base.API, + Model: wheeled.Model, + ConvertedAttributes: &wheeled.Config{ + Right: []string{"left", "right"}, + Left: []string{"left", "right"}, + WheelCircumferenceMM: 1, + WidthMM: 2, + }, + DependsOn: []string{"left", "right"}, + }, + { + Name: "left", + API: motor.API, + Model: fakeModel, + ConvertedAttributes: &fakemotor.Config{}, + }, + { + Name: "right", + API: motor.API, + Model: fakeModel, + ConvertedAttributes: &fakemotor.Config{}, + }, + }, + } + + r.Reconfigure(ctx, newCfg) + // test no error logging with default timeout window and wheeled base + testutils.WaitForAssertion(t, func(tb testing.TB) { + test.That(tb, timeOutErrorCount(), test.ShouldEqual, 0) + }) + + // create new cfg with wheeled base modified to trigger Reconfigure, set timeout + // to the shortest possible window to ensure timeout + defer func() { + test.That(t, os.Unsetenv(rutils.ResourceConfigurationTimeoutEnvVar), + test.ShouldBeNil) + }() + test.That(t, os.Setenv(rutils.ResourceConfigurationTimeoutEnvVar, "1ns"), + test.ShouldBeNil) + + newestCfg := &config.Config{ + Components: []resource.Config{ + { + Name: "fakewheel", + API: base.API, + Model: wheeled.Model, + ConvertedAttributes: &wheeled.Config{ + Right: []string{"right"}, + Left: []string{"left"}, + WheelCircumferenceMM: 1, + WidthMM: 2, + }, + DependsOn: []string{"left", "right"}, + }, + { + Name: "left", + API: motor.API, + Model: fakeModel, + ConvertedAttributes: &fakemotor.Config{}, + }, + { + Name: "right", + API: motor.API, + Model: fakeModel, + ConvertedAttributes: &fakemotor.Config{}, + }, + }, + } + + r.Reconfigure(ctx, newestCfg) + // test that an error is logged when using arbitrarily short timeout window + testutils.WaitForAssertion(t, func(tb testing.TB) { + test.That(tb, timeOutErrorCount(), test.ShouldEqual, 1) + }) + + rr, ok := r.(*localRobot) + test.That(t, ok, test.ShouldBeTrue) + + rr.reconfigureWorkers.Wait() +} + type mockFake struct { resource.Named createdAt int diff --git a/robot/packages/cloud_package_manager.go b/robot/packages/cloud_package_manager.go index e6a3825df9e..923a0609db4 100644 --- a/robot/packages/cloud_package_manager.go +++ b/robot/packages/cloud_package_manager.go @@ -12,8 +12,8 @@ import ( "net/http" "net/url" "os" - "path" "path/filepath" + "runtime" "strings" "sync" "time" @@ -89,7 +89,7 @@ func NewCloudManager(client pb.PackageServiceClient, packagesDir string, logger }, nil } -// PackagePath returns the package if it exists and already download. If it does not exist it returns a ErrPackageMissing error. +// PackagePath returns the package if it exists and is already downloaded. If it does not exist it returns a ErrPackageMissing error. func (m *cloudManager) PackagePath(name PackageName) (string, error) { m.mu.RLock() defer m.mu.RUnlock() @@ -99,23 +99,7 @@ func (m *cloudManager) PackagePath(name PackageName) (string, error) { return "", ErrPackageMissing } - return m.localNamedPath(p.thePackage), nil -} - -func (m *cloudManager) RefPath(refPath string) (string, error) { - ref := config.GetPackageReference(refPath) - - // If no reference just return original path. - if ref == nil { - return refPath, nil - } - - packagePath, err := m.PackagePath(PackageName(ref.Package)) - if err != nil { - return "", err - } - - return path.Join(packagePath, path.Clean(ref.PathInPackage)), nil + return p.thePackage.LocalDataDirectory(m.packagesDir), nil } // Close manager. @@ -139,10 +123,13 @@ func (m *cloudManager) Sync(ctx context.Context, packages []config.PackageConfig newManagedPackages := make(map[PackageName]*managedPackage, len(packages)) for idx, p := range packages { - select { - case <-ctx.Done(): - return multierr.Append(outErr, ctx.Err()) - default: + if err := ctx.Err(); err != nil { + return multierr.Append(outErr, err) + } + + if err := p.Validate(""); err != nil { + m.logger.Errorw("package config validation error; skipping", "package", p.Name, "error", err) + continue } start := time.Now() @@ -152,10 +139,8 @@ func (m *cloudManager) Sync(ctx context.Context, packages []config.PackageConfig existing, ok := m.managedPackages[PackageName(p.Name)] if ok { if existing.thePackage.Package == p.Package && existing.thePackage.Version == p.Version { - m.logger.Debug(" Package already managed, skipping") - + m.logger.Debug("Package already managed, skipping") newManagedPackages[PackageName(p.Name)] = existing - delete(m.managedPackages, PackageName(p.Name)) continue } // anything left over in the m.managedPackages will be cleaned up later. @@ -163,17 +148,34 @@ func (m *cloudManager) Sync(ctx context.Context, packages []config.PackageConfig // Lookup the packages http url includeURL := true - resp, err := m.client.GetPackage(ctx, &pb.GetPackageRequest{Id: p.Package, Version: p.Version, IncludeUrl: &includeURL}) + + var platform *string + if p.Type == config.PackageTypeModule { + platformVal := fmt.Sprintf("%s/%s", runtime.GOOS, runtime.GOARCH) + platform = &platformVal + } + + packageType, err := config.PackageTypeToProto(p.Type) + if err != nil { + m.logger.Warnw("failed to get package type", "package", p.Name, "error", err) + } + resp, err := m.client.GetPackage(ctx, &pb.GetPackageRequest{ + Id: p.Package, + Version: p.Version, + Type: packageType, + Platform: platform, + IncludeUrl: &includeURL, + }) if err != nil { m.logger.Errorf("Failed fetching package details for package %s:%s, %s", p.Package, p.Version, err) outErr = multierr.Append(outErr, errors.Wrapf(err, "failed loading package url for %s:%s", p.Package, p.Version)) continue } - m.logger.Debugf(" Downloading from %s", sanitizeURLForLogs(resp.Package.Url)) + m.logger.Debugf("Downloading from %s", sanitizeURLForLogs(resp.Package.Url)) - // load package from a http endpoint - err = m.loadFile(ctx, resp.Package.Url, p) + // download package from a http endpoint + err = m.downloadPackage(ctx, resp.Package.Url, p) if err != nil { m.logger.Errorf("Failed downloading package %s:%s from %s, %s", p.Package, p.Version, sanitizeURLForLogs(resp.Package.Url), err) outErr = multierr.Append(outErr, errors.Wrapf(err, "failed downloading package %s:%s from %s", @@ -181,17 +183,14 @@ func (m *cloudManager) Sync(ctx context.Context, packages []config.PackageConfig continue } - err = linkFile(m.localDataPath(p), m.localNamedPath(p)) - if err != nil { - m.logger.Errorf("Failed linking package %s:%s, %s", p.Package, p.Version, err) - outErr = multierr.Append(outErr, err) - continue + if p.Type == config.PackageTypeMlModel { + outErr = multierr.Append(outErr, m.mLModelSymlinkCreation(p)) } // add to managed packages newManagedPackages[PackageName(p.Name)] = &managedPackage{thePackage: p, modtime: time.Now()} - m.logger.Debugf(" Sync complete after %dms", time.Since(start).Milliseconds()) + m.logger.Debugf("Sync complete after %v", time.Since(start)) } // swap for new managed packags. @@ -211,55 +210,106 @@ func (m *cloudManager) Cleanup(ctx context.Context) error { var allErrors error - // packageDir will contain either symlinks to the packages or the .data directory. - files, err := os.ReadDir(m.packagesDir) + expectedPackageDirectories := map[string]bool{} + for _, pkg := range m.managedPackages { + expectedPackageDirectories[pkg.thePackage.LocalDataDirectory(m.packagesDir)] = true + } + + topLevelFiles, err := os.ReadDir(m.packagesDataDir) if err != nil { return err } + // A packageTypeDir is a directory that contains all of the packages for the specified type. ex: .data/ml_model + for _, packageTypeDir := range topLevelFiles { + packageTypeDirName, err := safeJoin(m.packagesDataDir, packageTypeDir.Name()) + if err != nil { + allErrors = multierr.Append(allErrors, err) + continue + } - // keep track of known packages by their hashed name from the package id and version. - knownPackages := make(map[string]bool) - - // first remove all symlinks to the packages themself. - for _, f := range files { - if f.Type()&os.ModeSymlink == os.ModeSymlink { - // if managed skip removing package - if p, ok := m.managedPackages[PackageName(f.Name())]; ok { - knownPackages[hashName(p.thePackage)] = true + // There should be no non-dir files in the packages/.data dir. Delete any that exist + if packageTypeDir.Type()&os.ModeDir != os.ModeDir { + allErrors = multierr.Append(allErrors, os.Remove(packageTypeDirName)) + continue + } + // read all of the packages in the directory and delete those that aren't in expectedPackageDirectories + packageDirs, err := os.ReadDir(packageTypeDirName) + if err != nil { + allErrors = multierr.Append(allErrors, err) + continue + } + for _, packageDir := range packageDirs { + packageDirName, err := safeJoin(packageTypeDirName, packageDir.Name()) + if err != nil { + allErrors = multierr.Append(allErrors, err) continue } - - m.logger.Infof("Cleaning up unused package link %s", f.Name()) - - // Remove logical symlink to package - if err := os.Remove(path.Join(m.packagesDir, f.Name())); err != nil { - allErrors = multierr.Append(allErrors, err) + _, expectedToExist := expectedPackageDirectories[packageDirName] + if !expectedToExist { + m.logger.Debug("Removing old package", packageDirName) + allErrors = multierr.Append(allErrors, os.RemoveAll(packageDirName)) } } + // re-read the directory, if there is nothing left in it, delete the directory + packageDirs, err = os.ReadDir(packageTypeDirName) + if err != nil { + allErrors = multierr.Append(allErrors, err) + continue + } + if len(packageDirs) == 0 { + allErrors = multierr.Append(allErrors, os.RemoveAll(packageTypeDirName)) + } } - // remove any packages in the .data dir that aren't known to the manager. - // packageDir will contain either symlinks to the packages or the .data directory. - files, err = os.ReadDir(m.packagesDataDir) + allErrors = multierr.Append(allErrors, m.mlModelSymlinkCleanup()) + return allErrors +} + +// symlink packages/package-name to packages/.data/ml_model/orgid-package-name-ver for backwards compatablility +// TODO(RSDK-4386) Preserved for backwards compatibility. Could be removed or extended to other types in the future. +func (m *cloudManager) mLModelSymlinkCreation(p config.PackageConfig) error { + symlinkPath, err := safeJoin(m.packagesDir, p.Name) if err != nil { return err } - // remove any remaining files in the .data dir that should not be there. + if err := linkFile(p.LocalDataDirectory(m.packagesDir), symlinkPath); err != nil { + return errors.Wrapf(err, "failed linking ml_model package %s:%s, %s", p.Package, p.Version, err) + } + return nil +} + +// cleanup all symlinks in packages/ directory +// TODO(RSDK-4386) Preserved for backwards compatibility. Could be removed or extended to other types in the future. +func (m *cloudManager) mlModelSymlinkCleanup() error { + var allErrors error + files, err := os.ReadDir(m.packagesDir) + if err != nil { + return err + } + + // The only symlinks in this directory are those created for MLModels for _, f := range files { + if f.Type()&os.ModeSymlink != os.ModeSymlink { + continue + } // if managed skip removing package - if _, ok := knownPackages[f.Name()]; ok { + if _, ok := m.managedPackages[PackageName(f.Name())]; ok { continue } - m.logger.Debugf("Cleaning up unused package %s", f.Name()) + m.logger.Infof("Cleaning up unused package link %s", f.Name()) + symlinkPath, err := safeJoin(m.packagesDir, f.Name()) + if err != nil { + allErrors = multierr.Append(allErrors, err) + continue + } // Remove logical symlink to package - if err := os.RemoveAll(path.Join(m.packagesDataDir, f.Name())); err != nil { + if err := os.Remove(symlinkPath); err != nil { allErrors = multierr.Append(allErrors, err) } } - return allErrors } @@ -272,23 +322,34 @@ func sanitizeURLForLogs(u string) string { return parsed.String() } -func (m *cloudManager) loadFile(ctx context.Context, url string, p config.PackageConfig) error { +func (m *cloudManager) downloadPackage(ctx context.Context, url string, p config.PackageConfig) error { // TODO(): validate integrity of directory. - if dirExists(m.localDataPath(p)) { - m.logger.Debug(" Package already downloaded, skipping.") + if dirExists(p.LocalDataDirectory(m.packagesDir)) { + m.logger.Debug("Package already downloaded, skipping.") return nil } + // Create the parent directory for the package type if it doesn't exist + if err := os.MkdirAll(p.LocalDataParentDirectory(m.packagesDir), 0o700); err != nil { + return err + } + // Force redownload of package archive. if err := m.cleanup(p); err != nil { m.logger.Debug(err) } - if err := os.Remove(m.localNamedPath(p)); err != nil { - m.logger.Debug(err) + + if p.Type == config.PackageTypeMlModel { + symlinkPath, err := safeJoin(m.packagesDir, p.Name) + if err == nil { + if err := os.Remove(symlinkPath); err != nil { + utils.UncheckedError(err) + } + } } // Download from GCS - _, contentType, err := m.downloadFileFromGCSURL(ctx, url, p) + _, contentType, err := m.downloadFileFromGCSURL(ctx, url, p.LocalDownloadPath(m.packagesDir)) if err != nil { return err } @@ -299,29 +360,29 @@ func (m *cloudManager) loadFile(ctx context.Context, url string, p config.Packag } // unpack to temp directory to ensure we do an atomic rename once finished. - tmpDataPath, err := os.MkdirTemp(m.packagesDataDir, "*.tmp") + tmpDataPath, err := os.MkdirTemp(p.LocalDataParentDirectory(m.packagesDir), "*.tmp") if err != nil { return errors.Wrap(err, "failed to create temp data dir path") } defer func() { // cleanup archive file. - if err := os.Remove(m.localDownloadPath(p)); err != nil { + if err := os.Remove(p.LocalDownloadPath(m.packagesDir)); err != nil { m.logger.Debug(err) } - if err := os.Remove(tmpDataPath); err != nil { + if err := os.RemoveAll(tmpDataPath); err != nil { m.logger.Debug(err) } }() // unzip archive. - err = m.unpackFile(ctx, m.localDownloadPath(p), tmpDataPath) + err = m.unpackFile(ctx, p.LocalDownloadPath(m.packagesDir), tmpDataPath) if err != nil { utils.UncheckedError(m.cleanup(p)) return err } - err = os.Rename(tmpDataPath, m.localDataPath(p)) + err = os.Rename(tmpDataPath, p.LocalDataDirectory(m.packagesDir)) if err != nil { utils.UncheckedError(m.cleanup(p)) return err @@ -332,14 +393,12 @@ func (m *cloudManager) loadFile(ctx context.Context, url string, p config.Packag func (m *cloudManager) cleanup(p config.PackageConfig) error { return multierr.Combine( - os.RemoveAll(m.localDataPath(p)), - os.Remove(m.localDownloadPath(p)), + os.RemoveAll(p.LocalDataDirectory(m.packagesDir)), + os.Remove(p.LocalDownloadPath(m.packagesDir)), ) } -func (m *cloudManager) downloadFileFromGCSURL(ctx context.Context, url string, p config.PackageConfig) (string, string, error) { - downloadPath := m.localDownloadPath(p) - +func (m *cloudManager) downloadFileFromGCSURL(ctx context.Context, url, downloadPath string) (string, string, error) { getReq, err := http.NewRequestWithContext(ctx, http.MethodGet, url, nil) if err != nil { return "", "", err @@ -411,10 +470,8 @@ func (m *cloudManager) unpackFile(ctx context.Context, fromFile, toDir string) e tarReader := tar.NewReader(archive) for { - select { - case <-ctx.Done(): - return errors.New("interrupted") - default: + if err := ctx.Err(); err != nil { + return err } header, err := tarReader.Next() @@ -446,8 +503,15 @@ func (m *cloudManager) unpackFile(ctx context.Context, fromFile, toDir string) e } case tar.TypeReg: + // This is required because it is possible create tarballs without a directory entry + // but whose files names start with a new directory prefix + // Ex: tar -czf package.tar.gz ./bin/module.exe + parent := filepath.Dir(path) + if err := os.MkdirAll(parent, info.Mode()); err != nil { + return errors.Wrapf(err, "failed to create directory %q", parent) + } //nolint:gosec // path sanitized with safeJoin - outFile, err := os.Create(path) + outFile, err := os.OpenFile(path, os.O_RDWR|os.O_CREATE|os.O_TRUNC, 0o600|info.Mode().Perm()) if err != nil { return errors.Wrapf(err, "failed to create file %s", path) } @@ -473,10 +537,8 @@ func (m *cloudManager) unpackFile(ctx context.Context, fromFile, toDir string) e // Now we make another pass creating the links for i := range links { - select { - case <-ctx.Done(): - return errors.New("interrupted") - default: + if err := ctx.Err(); err != nil { + return err } if err := linkFile(links[i].Name, links[i].Path); err != nil { return errors.Wrapf(err, "failed to create link %s", links[i].Path) @@ -484,10 +546,8 @@ func (m *cloudManager) unpackFile(ctx context.Context, fromFile, toDir string) e } for i := range symlinks { - select { - case <-ctx.Done(): - return errors.New("interrupted") - default: + if err := ctx.Err(); err != nil { + return err } if err := linkFile(symlinks[i].Name, symlinks[i].Path); err != nil { return errors.Wrapf(err, "failed to create link %s", links[i].Path) @@ -497,18 +557,6 @@ func (m *cloudManager) unpackFile(ctx context.Context, fromFile, toDir string) e return nil } -func (m *cloudManager) localDownloadPath(p config.PackageConfig) string { - return filepath.Join(m.packagesDataDir, fmt.Sprintf("%s.download", hashName(p))) -} - -func (m *cloudManager) localDataPath(p config.PackageConfig) string { - return filepath.Join(m.packagesDataDir, hashName(p)) -} - -func (m *cloudManager) localNamedPath(p config.PackageConfig) string { - return filepath.Join(m.packagesDir, p.Name) -} - func getGoogleHash(headers http.Header, hashType string) string { hashes := headers.Values("x-goog-hash") hashesMap := make(map[string]string, len(hashes)) @@ -527,11 +575,6 @@ func crc32Hash() hash.Hash32 { return crc32.New(crc32.MakeTable(crc32.Castagnoli)) } -func hashName(f config.PackageConfig) string { - // replace / to avoid a directory path in the name. This will happen with `org/package` format. - return fmt.Sprintf("%s-%s", strings.ReplaceAll(f.Package, "/", "-"), f.Version) -} - // safeJoin performs a filepath.Join of 'parent' and 'subdir' but returns an error // if the resulting path points outside of 'parent'. func safeJoin(parent, subdir string) (string, error) { diff --git a/robot/packages/cloud_package_manager_test.go b/robot/packages/cloud_package_manager_test.go index aae0f1d76c3..49adf7dda6f 100644 --- a/robot/packages/cloud_package_manager_test.go +++ b/robot/packages/cloud_package_manager_test.go @@ -3,15 +3,15 @@ package packages import ( "context" "errors" - "fmt" "os" - "path" + "path/filepath" "testing" "github.com/edaniels/golog" pb "go.viam.com/api/app/packages/v1" "go.viam.com/test" "go.viam.com/utils" + "golang.org/x/exp/slices" "go.viam.com/rdk/config" putils "go.viam.com/rdk/robot/packages/testutils" @@ -48,7 +48,7 @@ func TestCloud(t *testing.T) { packageDir, pm := newPackageManager(t, client, fakeServer, logger) defer utils.UncheckedErrorFunc(func() error { return pm.Close(context.Background()) }) - input := []config.PackageConfig{{Name: "some-name", Package: "org1/test-model", Version: "v1"}} + input := []config.PackageConfig{{Name: "some-name", Package: "org1/test-model", Version: "v1", Type: "ml_model"}} err = pm.Sync(ctx, input) test.That(t, err, test.ShouldNotBeNil) test.That(t, err.Error(), test.ShouldContainSubstring, "failed loading package url") @@ -62,8 +62,8 @@ func TestCloud(t *testing.T) { defer utils.UncheckedErrorFunc(func() error { return pm.Close(context.Background()) }) input := []config.PackageConfig{ - {Name: "some-name", Package: "org1/test-model", Version: "v1"}, - {Name: "some-name-2", Package: "org1/test-model", Version: "v2"}, + {Name: "some-name", Package: "org1/test-model", Version: "v1", Type: "ml_model"}, + {Name: "some-name-2", Package: "org1/test-model", Version: "v2", Type: "ml_model"}, } fakeServer.StorePackage(input...) @@ -79,8 +79,8 @@ func TestCloud(t *testing.T) { defer utils.UncheckedErrorFunc(func() error { return pm.Close(context.Background()) }) input := []config.PackageConfig{ - {Name: "some-name", Package: "org1/test-model", Version: "v1"}, - {Name: "some-name-2", Package: "org1/test-model", Version: "v2"}, + {Name: "some-name", Package: "org1/test-model", Version: "v1", Type: "ml_model"}, + {Name: "some-name-2", Package: "org1/test-model", Version: "v2", Type: "ml_model"}, } fakeServer.StorePackage(input[1]) // only store second @@ -97,8 +97,8 @@ func TestCloud(t *testing.T) { defer utils.UncheckedErrorFunc(func() error { return pm.Close(context.Background()) }) input := []config.PackageConfig{ - {Name: "some-name-1", Package: "org1/test-model", Version: "v1"}, - {Name: "some-name-2", Package: "org1/test-model", Version: "v2"}, + {Name: "some-name-1", Package: "org1/test-model", Version: "v1", Type: "ml_model"}, + {Name: "some-name-2", Package: "org1/test-model", Version: "v2", Type: "ml_model"}, } fakeServer.StorePackage(input...) @@ -128,8 +128,8 @@ func TestCloud(t *testing.T) { defer utils.UncheckedErrorFunc(func() error { return pm.Close(context.Background()) }) input := []config.PackageConfig{ - {Name: "some-name", Package: "org1/test-model", Version: "v1"}, - {Name: "some-name-2", Package: "org1/test-model", Version: "v2"}, + {Name: "some-name", Package: "org1/test-model", Version: "v1", Type: "ml_model"}, + {Name: "some-name-2", Package: "org1/test-model", Version: "v2", Type: "ml_model"}, } fakeServer.StorePackage(input...) @@ -159,7 +159,7 @@ func TestCloud(t *testing.T) { defer utils.UncheckedErrorFunc(func() error { return pm.Close(context.Background()) }) input := []config.PackageConfig{ - {Name: "some-name-1", Package: "org1/test-model", Version: "v1"}, + {Name: "some-name-1", Package: "org1/test-model", Version: "v1", Type: "ml_model"}, } fakeServer.StorePackage(input...) @@ -187,7 +187,7 @@ func TestCloud(t *testing.T) { fakeServer.SetInvalidChecksum(true) input := []config.PackageConfig{ - {Name: "some-name-1", Package: "org1/test-model", Version: "v1"}, + {Name: "some-name-1", Package: "org1/test-model", Version: "v1", Type: "ml_model"}, } fakeServer.StorePackage(input...) @@ -195,6 +195,9 @@ func TestCloud(t *testing.T) { test.That(t, err, test.ShouldNotBeNil) test.That(t, err.Error(), test.ShouldContainSubstring, "download did not match expected hash") + err = pm.Cleanup(ctx) + test.That(t, err, test.ShouldBeNil) + validatePackageDir(t, packageDir, []config.PackageConfig{}) }) @@ -205,7 +208,7 @@ func TestCloud(t *testing.T) { fakeServer.SetInvalidHTTPRes(true) input := []config.PackageConfig{ - {Name: "some-name-1", Package: "org1/test-model", Version: "v1"}, + {Name: "some-name-1", Package: "org1/test-model", Version: "v1", Type: "ml_model"}, } fakeServer.StorePackage(input...) @@ -213,6 +216,9 @@ func TestCloud(t *testing.T) { test.That(t, err, test.ShouldNotBeNil) test.That(t, err.Error(), test.ShouldContainSubstring, "invalid status code 500") + err = pm.Cleanup(ctx) + test.That(t, err, test.ShouldBeNil) + validatePackageDir(t, packageDir, []config.PackageConfig{}) }) @@ -223,7 +229,7 @@ func TestCloud(t *testing.T) { fakeServer.SetInvalidTar(true) input := []config.PackageConfig{ - {Name: "some-name-1", Package: "org1/test-model", Version: "v1"}, + {Name: "some-name-1", Package: "org1/test-model", Version: "v1", Type: "ml_model"}, } fakeServer.StorePackage(input...) @@ -231,26 +237,32 @@ func TestCloud(t *testing.T) { test.That(t, err, test.ShouldNotBeNil) test.That(t, err.Error(), test.ShouldContainSubstring, "unexpected EOF") + err = pm.Cleanup(ctx) + test.That(t, err, test.ShouldBeNil) + validatePackageDir(t, packageDir, []config.PackageConfig{}) }) } func validatePackageDir(t *testing.T, dir string, input []config.PackageConfig) { - t.Helper() + // t.Helper() // create maps to make lookups easier. - byPackageHash := make(map[string]*config.PackageConfig) + bySanitizedName := make(map[string]*config.PackageConfig) byLogicalName := make(map[string]*config.PackageConfig) + byType := make(map[string][]string) for _, pI := range input { p := pI - byPackageHash[hashName(p)] = &p + bySanitizedName[p.SanitizedName()] = &p byLogicalName[p.Name] = &p + pType := string(p.Type) + byType[pType] = append(byType[pType], p.SanitizedName()) } // check all known packages exist and are linked to the correct package dir. for _, p := range input { - logicalPath := path.Join(dir, p.Name) - dataPath := path.Join(dir, fmt.Sprintf(".data/%s", hashName(p))) + logicalPath := filepath.Join(dir, p.Name) + dataPath := filepath.Join(dir, ".data", string(p.Type), p.SanitizedName()) info, err := os.Stat(logicalPath) test.That(t, err, test.ShouldBeNil) @@ -275,7 +287,7 @@ func validatePackageDir(t *testing.T, dir string, input []config.PackageConfig) test.That(t, err, test.ShouldBeNil) for _, f := range files { - if isSymLink(t, path.Join(dir, f.Name())) { + if isSymLink(t, filepath.Join(dir, f.Name())) { if _, ok := byLogicalName[f.Name()]; !ok { t.Fatalf("found unknown symlink in package dir %s", f.Name()) } @@ -290,14 +302,21 @@ func validatePackageDir(t *testing.T, dir string, input []config.PackageConfig) t.Fatalf("found unknown file in package dir %s", f.Name()) } - files, err = os.ReadDir(path.Join(dir, ".data")) + typeFolders, err := os.ReadDir(filepath.Join(dir, ".data")) test.That(t, err, test.ShouldBeNil) - for _, f := range files { - if _, ok := byPackageHash[f.Name()]; ok { - continue + for _, typeFile := range typeFolders { + expectedPackages, ok := byType[typeFile.Name()] + if !ok { + t.Errorf("found unknown file in package data dir %s", typeFile.Name()) + } + foundFiles, err := os.ReadDir(filepath.Join(dir, ".data", typeFile.Name())) + test.That(t, err, test.ShouldBeNil) + for _, packageFile := range foundFiles { + if !slices.Contains(expectedPackages, packageFile.Name()) { + t.Errorf("found unknown file in package %s dir %s", typeFile.Name(), packageFile.Name()) + } } - t.Errorf("found unknown file in package data dir %s", f.Name()) } } @@ -316,7 +335,7 @@ func TestPackageRefs(t *testing.T) { packageDir, pm := newPackageManager(t, client, fakeServer, logger) defer utils.UncheckedErrorFunc(func() error { return pm.Close(context.Background()) }) - input := []config.PackageConfig{{Name: "some-name", Package: "org1/test-model", Version: "v1"}} + input := []config.PackageConfig{{Name: "some-name", Package: "org1/test-model", Version: "v1", Type: "ml_model"}} fakeServer.StorePackage(input...) err = pm.Sync(ctx, input) @@ -326,7 +345,7 @@ func TestPackageRefs(t *testing.T) { t.Run("valid package", func(t *testing.T) { pPath, err := pm.PackagePath("some-name") test.That(t, err, test.ShouldBeNil) - test.That(t, pPath, test.ShouldEqual, path.Join(packageDir, "some-name")) + test.That(t, pPath, test.ShouldEqual, input[0].LocalDataDirectory(packageDir)) putils.ValidateContentsOfPPackage(t, pPath) }) @@ -340,50 +359,6 @@ func TestPackageRefs(t *testing.T) { test.That(t, err, test.ShouldEqual, ErrPackageMissing) }) }) - - t.Run("RefPath", func(t *testing.T) { - t.Run("empty path", func(t *testing.T) { - pPath, err := pm.RefPath("") - test.That(t, err, test.ShouldBeNil) - test.That(t, pPath, test.ShouldEqual, "") - }) - - t.Run("non-ref absolute path", func(t *testing.T) { - pPath, err := pm.RefPath("/some/absolute/path") - test.That(t, err, test.ShouldBeNil) - test.That(t, pPath, test.ShouldEqual, "/some/absolute/path") - }) - - t.Run("non-ref relative path", func(t *testing.T) { - pPath, err := pm.RefPath("some/absolute/path") - test.That(t, err, test.ShouldBeNil) - test.That(t, pPath, test.ShouldEqual, "some/absolute/path") - }) - - t.Run("non-ref relative path with backtrack", func(t *testing.T) { - pPath, err := pm.RefPath("some/../absolute/path") - test.That(t, err, test.ShouldBeNil) - test.That(t, pPath, test.ShouldEqual, "some/../absolute/path") - }) - - t.Run("valid ref, empty package path", func(t *testing.T) { - pPath, err := pm.RefPath("${packages.some-name}") - test.That(t, err, test.ShouldBeNil) - test.That(t, pPath, test.ShouldEqual, path.Join(packageDir, "some-name")) - }) - - t.Run("valid ref, with package path", func(t *testing.T) { - pPath, err := pm.RefPath("${packages.some-name}/some/path") - test.That(t, err, test.ShouldBeNil) - test.That(t, pPath, test.ShouldEqual, path.Join(packageDir, "some-name", "some/path")) - }) - - t.Run("valid ref, ensure no escape from package path", func(t *testing.T) { - pPath, err := pm.RefPath("${packages.some-name}/../../../some-other-package/some/path") - test.That(t, err, test.ShouldBeNil) - test.That(t, pPath, test.ShouldEqual, path.Join(packageDir, "some-name", "some-other-package/some/path")) - }) - }) } func isSymLink(t *testing.T, file string) bool { diff --git a/robot/packages/noop_package_manager.go b/robot/packages/noop_package_manager.go index 6f1fc8e2f6d..fc9bfe77567 100644 --- a/robot/packages/noop_package_manager.go +++ b/robot/packages/noop_package_manager.go @@ -2,7 +2,6 @@ package packages import ( "context" - "path" "go.viam.com/rdk/config" "go.viam.com/rdk/resource" @@ -30,22 +29,6 @@ func (m *noopManager) PackagePath(name PackageName) (string, error) { return string(name), nil } -func (m *noopManager) RefPath(refPath string) (string, error) { - ref := config.GetPackageReference(refPath) - - // If no reference just return original path. - if ref == nil { - return refPath, nil - } - - packagePath, err := m.PackagePath(PackageName(ref.Package)) - if err != nil { - return "", err - } - - return path.Join(packagePath, path.Clean(ref.PathInPackage)), nil -} - // Close manager. func (m *noopManager) Close(ctx context.Context) error { return nil diff --git a/robot/packages/package_manager.go b/robot/packages/package_manager.go index d89379806d1..b81901b8aef 100644 --- a/robot/packages/package_manager.go +++ b/robot/packages/package_manager.go @@ -35,12 +35,6 @@ type Manager interface { // PackagePath returns the package if it exists and is already downloaded. If it does not exist it returns a ErrPackageMissing error. PackagePath(name PackageName) (string, error) - - // RefPath returns the absolute path of the package reference for a given path with a package reference. - // - If not the original path is not a package reference the original path is returned without an error. - // - If the path contains a package reference and the package does not exist a ErrPackageMissing will be returned. - // - Any syntax errors in the package reference will produce an ErrInvalidPackageRef. - RefPath(name string) (string, error) } // ManagerSyncer provides a managed interface for both reading package paths and syncing packages from the RDK config. diff --git a/robot/packages/package_path_visitor.go b/robot/packages/package_path_visitor.go deleted file mode 100644 index cdcd55a29ab..00000000000 --- a/robot/packages/package_path_visitor.go +++ /dev/null @@ -1,44 +0,0 @@ -package packages - -import ( - "reflect" -) - -// PackagePathVisitor is a visitor that replaces strings containing references to package names -// with the path containing the package files on the robot. -type PackagePathVisitor struct { - packageManager Manager -} - -// NewPackagePathVisitor creates a new PackagePathVisitor. -func NewPackagePathVisitor(packageManager Manager) *PackagePathVisitor { - return &PackagePathVisitor{ - packageManager: packageManager, - } -} - -// Visit implements config.Visitor. -func (v *PackagePathVisitor) Visit(data interface{}) (interface{}, error) { - t := reflect.TypeOf(data) - - var s string - switch { - case t.Kind() == reflect.String: - s = data.(string) - case t.Kind() == reflect.Ptr && t.Elem().Kind() == reflect.String: - s = *data.(*string) - default: - return data, nil - } - - withReplacedRefs, err := v.packageManager.RefPath(s) - if err != nil { - return nil, err - } - - // If the input was a pointer, return a pointer. - if t.Kind() == reflect.Ptr { - return &withReplacedRefs, nil - } - return withReplacedRefs, nil -} diff --git a/robot/packages/package_path_visitor_test.go b/robot/packages/package_path_visitor_test.go deleted file mode 100644 index b1a8e860b54..00000000000 --- a/robot/packages/package_path_visitor_test.go +++ /dev/null @@ -1,71 +0,0 @@ -package packages - -import ( - "reflect" - "testing" - - "go.viam.com/test" -) - -func TestPackagePathVisitor(t *testing.T) { - testStringNoRef := "some/path/file_name.txt" - testStringRef := "${packages.custom_package}/file_name.txt" - testStringRefReplaced := "custom_package/file_name.txt" - testInt := 17 - - testCases := []struct { - desc string - input interface{} - expected interface{} - }{ - { - "visit string with package reference", - testStringRef, - testStringRefReplaced, - }, - { - "visit string without package reference", - testStringNoRef, - testStringNoRef, - }, - { - "visit pointer to string with package reference", - &testStringRef, - &testStringRefReplaced, - }, - { - "visit pointer to string without package reference", - &testStringNoRef, - &testStringNoRef, - }, - { - "visit non-string type", - testInt, - testInt, - }, - { - "visit pointer to non-string type", - &testInt, - &testInt, - }, - } - - for _, tc := range testCases { - t.Run(tc.desc, func(t *testing.T) { - v := NewPackagePathVisitor(NewNoopManager()) - actual, err := v.Visit(tc.input) - test.That(t, err, test.ShouldBeNil) - - if reflect.TypeOf(tc.input).Kind() == reflect.Ptr { - if reflect.TypeOf(actual).Kind() != reflect.Ptr { - t.Fatal("input was pointer, but output was not") - } - - tc.expected = reflect.Indirect(reflect.ValueOf(tc.expected)).Interface() - actual = reflect.Indirect(reflect.ValueOf(actual)).Interface() - } - - test.That(t, actual, test.ShouldEqual, tc.expected) - }) - } -} diff --git a/robot/packages/testutils/fake_package_server.go b/robot/packages/testutils/fake_package_server.go index aaa548613aa..8589f36069d 100644 --- a/robot/packages/testutils/fake_package_server.go +++ b/robot/packages/testutils/fake_package_server.go @@ -331,15 +331,16 @@ func ValidateContentsOfPPackage(t *testing.T, dir string) { isLink bool linkTarget string isDir bool + perms os.FileMode } expected := []content{ - {path: "some-link.txt", isLink: true, linkTarget: "sub-dir/sub-file.txt"}, - {path: "some-text.txt", checksum: "p/E54w=="}, - {path: "some-text2.txt", checksum: "p/E54w=="}, - {path: "sub-dir", isDir: true}, - {path: "sub-dir/sub-file.txt", checksum: "p/E54w=="}, - {path: "sub-dir-link", isLink: true, linkTarget: "sub-dir"}, + {path: "some-link.txt", isLink: true, linkTarget: "sub-dir/sub-file.txt", perms: 0o777}, + {path: "some-text.txt", checksum: "p/E54w==", perms: 0o644}, + {path: "some-text2.txt", checksum: "p/E54w==", perms: 0o644}, + {path: "sub-dir", isDir: true, perms: 0o755}, + {path: "sub-dir/sub-file.txt", checksum: "p/E54w==", perms: 0o644}, + {path: "sub-dir-link", isLink: true, linkTarget: "sub-dir", perms: 0o777}, } out := make([]content, 0, len(expected)) @@ -371,6 +372,7 @@ func ValidateContentsOfPPackage(t *testing.T, dir string) { isDir: info.IsDir(), isLink: isSymLink, linkTarget: symTarget, + perms: info.Mode().Perm(), }) return nil diff --git a/robot/session_test.go b/robot/session_test.go index 12db74afff7..97f67f91f27 100644 --- a/robot/session_test.go +++ b/robot/session_test.go @@ -872,8 +872,8 @@ func (dm *dummyMotor) ResetZeroPosition(ctx context.Context, offset float64, ext return nil } -func (dm *dummyMotor) Properties(ctx context.Context, extra map[string]interface{}) (map[motor.Feature]bool, error) { - return map[motor.Feature]bool{}, nil +func (dm *dummyMotor) Properties(ctx context.Context, extra map[string]interface{}) (motor.Properties, error) { + return motor.Properties{}, nil } func (dm *dummyMotor) IsPowered(ctx context.Context, extra map[string]interface{}) (bool, float64, error) { @@ -923,7 +923,7 @@ func (db *dummyBase) Properties(ctx context.Context, extra map[string]interface{ return base.Properties{}, nil } -func (db *dummyBase) Geometries(ctx context.Context) ([]spatialmath.Geometry, error) { +func (db *dummyBase) Geometries(ctx context.Context, extra map[string]interface{}) ([]spatialmath.Geometry, error) { return []spatialmath.Geometry{}, nil } diff --git a/robot/web/options/options.go b/robot/web/options/options.go index 891871d13d4..881a909ce04 100644 --- a/robot/web/options/options.go +++ b/robot/web/options/options.go @@ -67,6 +67,8 @@ type Options struct { WebRTCOnPeerAdded func(pc *webrtc.PeerConnection) WebRTCOnPeerRemoved func(pc *webrtc.PeerConnection) + + DisableMulticastDNS bool } // New returns a default set of options which will have the diff --git a/robot/web/web.go b/robot/web/web.go index 101aab10180..4cce690fd2a 100644 --- a/robot/web/web.go +++ b/robot/web/web.go @@ -454,7 +454,10 @@ func (svc *webService) Reconfigure(ctx context.Context, deps resource.Dependenci if err := svc.updateResources(deps); err != nil { return err } - return svc.addNewStreams(ctx) + if !svc.isRunning { + return nil + } + return svc.addNewStreams(svc.cancelCtx) } func (svc *webService) updateResources(resources map[resource.Name]resource.Resource) error { @@ -924,6 +927,10 @@ func (svc *webService) initRPCOptions(listenerTCPAddr *net.TCPAddr, options webo OnPeerRemoved: options.WebRTCOnPeerRemoved, }), } + if options.DisableMulticastDNS { + rpcOpts = append(rpcOpts, rpc.WithDisableMulticastDNS()) + } + var unaryInterceptors []googlegrpc.UnaryServerInterceptor unaryInterceptors = append(unaryInterceptors, ensureTimeoutUnaryInterceptor) diff --git a/services/datamanager/builtin/builtin.go b/services/datamanager/builtin/builtin.go index 9ed6bf2cc31..ccd68f7bb25 100644 --- a/services/datamanager/builtin/builtin.go +++ b/services/datamanager/builtin/builtin.go @@ -48,7 +48,7 @@ func init() { }, // NOTE(erd): this would be better as a weak dependencies returned through a more // typed validate or different system. - WeakDependencies: []internal.ResourceMatcher{internal.ComponentDependencyWildcardMatcher}, + WeakDependencies: []internal.ResourceMatcher{internal.ComponentDependencyWildcardMatcher, internal.SLAMDependencyWildcardMatcher}, }) } @@ -88,7 +88,7 @@ type builtIn struct { logger golog.Logger captureDir string captureDisabled bool - collectors map[componentMethodMetadata]*collectorAndConfig + collectors map[resourceMethodMetadata]*collectorAndConfig lock sync.Mutex backgroundWorkers sync.WaitGroup waitAfterLastModifiedMillis int @@ -118,7 +118,7 @@ func NewBuiltIn( Named: conf.ResourceName().AsNamed(), logger: logger, captureDir: viamCaptureDotDir, - collectors: make(map[componentMethodMetadata]*collectorAndConfig), + collectors: make(map[resourceMethodMetadata]*collectorAndConfig), syncIntervalMins: 0, additionalSyncPaths: []string{}, tags: []string{}, @@ -179,8 +179,8 @@ type collectorAndConfig struct { // Identifier for a particular collector: component name, component model, component type, // method parameters, and method name. -type componentMethodMetadata struct { - ComponentName string +type resourceMethodMetadata struct { + ResourceName string MethodParams string MethodMetadata data.MethodMetadata } @@ -196,7 +196,7 @@ func getDurationFromHz(captureFrequencyHz float32) time.Duration { // Initialize a collector for the component/method or update it if it has previously been created. // Return the component/method metadata which is used as a key in the collectors map. func (svc *builtIn) initializeOrUpdateCollector( - md componentMethodMetadata, + md resourceMethodMetadata, config *datamanager.DataCaptureConfig, ) ( *collectorAndConfig, error, @@ -366,11 +366,11 @@ func (svc *builtIn) Reconfigure( // Service is disabled, so close all collectors and clear the map so we can instantiate new ones if we enable this service. if svc.captureDisabled { svc.closeCollectors() - svc.collectors = make(map[componentMethodMetadata]*collectorAndConfig) + svc.collectors = make(map[resourceMethodMetadata]*collectorAndConfig) } // Initialize or add collectors based on changes to the component configurations. - newCollectors := make(map[componentMethodMetadata]*collectorAndConfig) + newCollectors := make(map[resourceMethodMetadata]*collectorAndConfig) if !svc.captureDisabled { for _, resConf := range svcConfig.ResourceConfigs { if resConf.Resource == nil { @@ -384,8 +384,8 @@ func (svc *builtIn) Reconfigure( MethodName: resConf.Method, } - componentMethodMetadata := componentMethodMetadata{ - ComponentName: resConf.Name.ShortName(), + componentMethodMetadata := resourceMethodMetadata{ + ResourceName: resConf.Name.ShortName(), MethodMetadata: methodMetadata, MethodParams: fmt.Sprintf("%v", resConf.AdditionalParams), } diff --git a/services/datamanager/builtin/sync_test.go b/services/datamanager/builtin/sync_test.go index f90c08a0faa..0a595a8468e 100644 --- a/services/datamanager/builtin/sync_test.go +++ b/services/datamanager/builtin/sync_test.go @@ -338,25 +338,22 @@ func TestArbitraryFileUpload(t *testing.T) { name: "scheduled sync of arbitrary files should work", manualSync: false, scheduleSyncDisabled: false, - serviceFail: false, }, { name: "manual sync of arbitrary files should work", manualSync: true, scheduleSyncDisabled: true, - serviceFail: false, }, { name: "running manual and scheduled sync concurrently should work and not lead to duplicate uploads", manualSync: true, scheduleSyncDisabled: false, - serviceFail: false, }, { name: "if an error response is received from the backend, local files should not be deleted", manualSync: false, scheduleSyncDisabled: false, - serviceFail: false, + serviceFail: true, }, } @@ -372,11 +369,13 @@ func TestArbitraryFileUpload(t *testing.T) { dmsvc, r := newTestDataManager(t) dmsvc.SetWaitAfterLastModifiedMillis(0) defer dmsvc.Close(context.Background()) + f := atomic.Bool{} + f.Store(tc.serviceFail) mockClient := mockDataSyncServiceClient{ succesfulDCRequests: make(chan *v1.DataCaptureUploadRequest, 100), failedDCRequests: make(chan *v1.DataCaptureUploadRequest, 100), fileUploads: make(chan *mockFileUploadClient, 100), - fail: &atomic.Bool{}, + fail: &f, } dmsvc.SetSyncerConstructor(getTestSyncerConstructorMock(mockClient)) cfg, deps := setupConfig(t, disabledTabularCollectorConfigPath) @@ -416,10 +415,12 @@ func TestArbitraryFileUpload(t *testing.T) { var fileUploads []*mockFileUploadClient var urs []*v1.FileUploadRequest // Get the successful requests - wait := time.After(time.Second * 5) + wait := time.After(time.Second * 3) select { case <-wait: - t.Fatalf("timed out waiting for sync request") + if !tc.serviceFail { + t.Fatalf("timed out waiting for sync request") + } case r := <-mockClient.fileUploads: fileUploads = append(fileUploads, r) select { @@ -430,12 +431,8 @@ func TestArbitraryFileUpload(t *testing.T) { } } - // Validate error and URs. - remainingFiles := getAllFilePaths(additionalPathsDir) - if tc.serviceFail { - // Error case, file should not be deleted. - test.That(t, len(remainingFiles), test.ShouldEqual, 1) - } else { + waitUntilNoFiles(additionalPathsDir) + if !tc.serviceFail { // Validate first metadata message. test.That(t, len(fileUploads), test.ShouldEqual, 1) test.That(t, len(urs), test.ShouldBeGreaterThan, 0) @@ -455,8 +452,144 @@ func TestArbitraryFileUpload(t *testing.T) { test.That(t, actData, test.ShouldResemble, fileContents) // Validate file no longer exists. - waitUntilNoFiles(additionalPathsDir) test.That(t, len(getAllFileInfos(additionalPathsDir)), test.ShouldEqual, 0) + test.That(t, dmsvc.Close(context.Background()), test.ShouldBeNil) + } else { + // Validate no files were successfully uploaded. + test.That(t, len(fileUploads), test.ShouldEqual, 0) + // Validate file still exists. + test.That(t, len(getAllFileInfos(additionalPathsDir)), test.ShouldEqual, 1) + } + }) + } +} + +func TestStreamingDCUpload(t *testing.T) { + tests := []struct { + name string + serviceFail bool + }{ + { + name: "A data capture file greater than MaxUnaryFileSize should be successfully uploaded" + + "via the streaming rpc.", + serviceFail: false, + }, + { + name: "if an error response is received from the backend, local files should not be deleted", + serviceFail: true, + }, + } + + for _, tc := range tests { + t.Run(tc.name, func(t *testing.T) { + // Set up server. + mockClock := clk.NewMock() + clock = mockClock + tmpDir := t.TempDir() + + // Set up data manager. + dmsvc, r := newTestDataManager(t) + defer dmsvc.Close(context.Background()) + var cfg *Config + var deps []string + captureInterval := time.Millisecond * 10 + cfg, deps = setupConfig(t, enabledBinaryCollectorConfigPath) + + // Set up service config with just capture enabled. + cfg.CaptureDisabled = false + cfg.ScheduledSyncDisabled = true + cfg.SyncIntervalMins = syncIntervalMins + cfg.CaptureDir = tmpDir + + resources := resourcesFromDeps(t, r, deps) + err := dmsvc.Reconfigure(context.Background(), resources, resource.Config{ + ConvertedAttributes: cfg, + }) + test.That(t, err, test.ShouldBeNil) + + // Capture an image, then close. + mockClock.Add(captureInterval) + waitForCaptureFilesToExceedNFiles(tmpDir, 0) + err = dmsvc.Close(context.Background()) + test.That(t, err, test.ShouldBeNil) + + // Get all captured data. + _, capturedData, err := getCapturedData(tmpDir) + test.That(t, err, test.ShouldBeNil) + + // Turn dmsvc back on with capture disabled. + newDMSvc, r := newTestDataManager(t) + defer newDMSvc.Close(context.Background()) + f := atomic.Bool{} + f.Store(tc.serviceFail) + mockClient := mockDataSyncServiceClient{ + streamingDCUploads: make(chan *mockStreamingDCClient, 10), + fail: &f, + } + // Set max unary file size to 1 byte, so it uses the streaming rpc. + datasync.MaxUnaryFileSize = 1 + newDMSvc.SetSyncerConstructor(getTestSyncerConstructorMock(mockClient)) + cfg.CaptureDisabled = true + cfg.ScheduledSyncDisabled = true + resources = resourcesFromDeps(t, r, deps) + err = newDMSvc.Reconfigure(context.Background(), resources, resource.Config{ + ConvertedAttributes: cfg, + }) + test.That(t, err, test.ShouldBeNil) + + // Call sync. + err = newDMSvc.Sync(context.Background(), nil) + test.That(t, err, test.ShouldBeNil) + + // Wait for upload requests. + var uploads []*mockStreamingDCClient + var urs []*v1.StreamingDataCaptureUploadRequest + // Get the successful requests + wait := time.After(time.Second * 3) + select { + case <-wait: + if !tc.serviceFail { + t.Fatalf("timed out waiting for sync request") + } + case r := <-mockClient.streamingDCUploads: + uploads = append(uploads, r) + select { + case <-wait: + t.Fatalf("timed out waiting for sync request") + case <-r.closed: + urs = append(urs, r.reqs...) + } + } + waitUntilNoFiles(tmpDir) + + // Validate error and URs. + remainingFiles := getAllFilePaths(tmpDir) + if tc.serviceFail { + // Validate no files were successfully uploaded. + test.That(t, len(uploads), test.ShouldEqual, 0) + // Error case, file should not be deleted. + test.That(t, len(remainingFiles), test.ShouldEqual, 1) + } else { + // Validate first metadata message. + test.That(t, len(uploads), test.ShouldEqual, 1) + test.That(t, len(urs), test.ShouldBeGreaterThan, 0) + actMD := urs[0].GetMetadata() + test.That(t, actMD, test.ShouldNotBeNil) + test.That(t, actMD.GetUploadMetadata(), test.ShouldNotBeNil) + test.That(t, actMD.GetSensorMetadata(), test.ShouldNotBeNil) + test.That(t, actMD.GetUploadMetadata().Type, test.ShouldEqual, v1.DataType_DATA_TYPE_BINARY_SENSOR) + test.That(t, actMD.GetUploadMetadata().PartId, test.ShouldNotBeBlank) + + // Validate ensuing data messages. + dataRequests := urs[1:] + var actData []byte + for _, d := range dataRequests { + actData = append(actData, d.GetData()...) + } + test.That(t, actData, test.ShouldResemble, capturedData[0].GetBinary()) + + // Validate file no longer exists. + test.That(t, len(getAllFileInfos(tmpDir)), test.ShouldEqual, 0) } test.That(t, dmsvc.Close(context.Background()), test.ShouldBeNil) }) @@ -661,6 +794,7 @@ type mockDataSyncServiceClient struct { succesfulDCRequests chan *v1.DataCaptureUploadRequest failedDCRequests chan *v1.DataCaptureUploadRequest fileUploads chan *mockFileUploadClient + streamingDCUploads chan *mockStreamingDCClient fail *atomic.Bool } @@ -690,7 +824,26 @@ func (c mockDataSyncServiceClient) FileUpload(ctx context.Context, opts ...grpc. return nil, errors.New("oh no error") } ret := &mockFileUploadClient{closed: make(chan struct{})} - c.fileUploads <- ret + select { + case <-ctx.Done(): + return nil, ctx.Err() + case c.fileUploads <- ret: + } + return ret, nil +} + +func (c mockDataSyncServiceClient) StreamingDataCaptureUpload(ctx context.Context, + opts ...grpc.CallOption, +) (v1.DataSyncService_StreamingDataCaptureUploadClient, error) { + if c.fail.Load() { + return nil, errors.New("oh no error") + } + ret := &mockStreamingDCClient{closed: make(chan struct{})} + select { + case <-ctx.Done(): + return nil, ctx.Err() + case c.streamingDCUploads <- ret: + } return ret, nil } @@ -711,6 +864,28 @@ func (m *mockFileUploadClient) CloseAndRecv() (*v1.FileUploadResponse, error) { } func (m *mockFileUploadClient) CloseSend() error { + m.closed <- struct{}{} + return nil +} + +type mockStreamingDCClient struct { + reqs []*v1.StreamingDataCaptureUploadRequest + closed chan struct{} + grpc.ClientStream +} + +func (m *mockStreamingDCClient) Send(req *v1.StreamingDataCaptureUploadRequest) error { + m.reqs = append(m.reqs, req) + return nil +} + +func (m *mockStreamingDCClient) CloseAndRecv() (*v1.StreamingDataCaptureUploadResponse, error) { + m.closed <- struct{}{} + return &v1.StreamingDataCaptureUploadResponse{}, nil +} + +func (m *mockStreamingDCClient) CloseSend() error { + m.closed <- struct{}{} return nil } diff --git a/services/datamanager/datacapture/data_capture_file.go b/services/datamanager/datacapture/data_capture_file.go index 86588b94a50..0752ea7aed7 100644 --- a/services/datamanager/datacapture/data_capture_file.go +++ b/services/datamanager/datacapture/data_capture_file.go @@ -22,14 +22,13 @@ import ( // TODO Data-343: Reorganize this into a more standard interface/package, and add tests. -// TODO: this is all way too complicated i think. Just keep track of read/write offsets - // FileExt defines the file extension for Viam data capture files. const ( InProgressFileExt = ".prog" FileExt = ".capture" readImage = "ReadImage" nextPointCloud = "NextPointCloud" + pointCloudMap = "PointCloudMap" ) // File is the data structure containing data captured by collectors. It is backed by a file on disk containing @@ -240,7 +239,7 @@ func getFileTimestampName() string { // TODO DATA-246: Implement this in some more robust, programmatic way. func getDataType(methodName string) v1.DataType { switch methodName { - case nextPointCloud, readImage: + case nextPointCloud, readImage, pointCloudMap: return v1.DataType_DATA_TYPE_BINARY_SENSOR default: return v1.DataType_DATA_TYPE_TABULAR_SENSOR diff --git a/services/datamanager/datasync/upload_arbitrary_file.go b/services/datamanager/datasync/upload_arbitrary_file.go index 7f85664e4e8..263902f17be 100644 --- a/services/datamanager/datasync/upload_arbitrary_file.go +++ b/services/datamanager/datasync/upload_arbitrary_file.go @@ -41,8 +41,7 @@ func uploadArbitraryFile(ctx context.Context, client v1.DataSyncServiceClient, f return errors.Wrapf(err, "error syncing %s", f.Name()) } - _, err = stream.CloseAndRecv() - if err != nil { + if _, err := stream.CloseAndRecv(); err != nil { return errors.Wrapf(err, "received error response while syncing %s", f.Name()) } @@ -50,8 +49,6 @@ func uploadArbitraryFile(ctx context.Context, client v1.DataSyncServiceClient, f } func sendFileUploadRequests(ctx context.Context, stream v1.DataSyncService_FileUploadClient, f *os.File) error { - //nolint:errcheck - defer stream.CloseSend() // Loop until there is no more content to be read from file. for { select { diff --git a/services/datamanager/datasync/upload_data_capture_file.go b/services/datamanager/datasync/upload_data_capture_file.go index d23d4a486c5..6ca9c52b82b 100644 --- a/services/datamanager/datasync/upload_data_capture_file.go +++ b/services/datamanager/datasync/upload_data_capture_file.go @@ -3,16 +3,22 @@ package datasync import ( "context" + "github.com/docker/go-units" + "github.com/pkg/errors" v1 "go.viam.com/api/app/datasync/v1" "go.viam.com/rdk/services/datamanager/datacapture" ) +// MaxUnaryFileSize is the max number of bytes to send using the unary DataCaptureUpload, as opposed to the +// StreamingDataCaptureUpload. +var MaxUnaryFileSize = int64(units.MB) + func uploadDataCaptureFile(ctx context.Context, client v1.DataSyncServiceClient, f *datacapture.File, partID string) error { md := f.ReadMetadata() sensorData, err := datacapture.SensorDataFromFile(f) if err != nil { - return err + return errors.Wrap(err, "error reading sensor data from file") } // Do not attempt to upload a file without any sensor readings. @@ -20,22 +26,91 @@ func uploadDataCaptureFile(ctx context.Context, client v1.DataSyncServiceClient, return nil } - ur := &v1.DataCaptureUploadRequest{ - Metadata: &v1.UploadMetadata{ - PartId: partID, - ComponentType: md.GetComponentType(), - ComponentName: md.GetComponentName(), - MethodName: md.GetMethodName(), - Type: md.GetType(), - MethodParameters: md.GetMethodParameters(), - FileExtension: md.GetFileExtension(), - Tags: md.GetTags(), - }, - SensorContents: sensorData, + uploadMD := &v1.UploadMetadata{ + PartId: partID, + ComponentType: md.GetComponentType(), + ComponentName: md.GetComponentName(), + MethodName: md.GetMethodName(), + Type: md.GetType(), + MethodParameters: md.GetMethodParameters(), + FileExtension: md.GetFileExtension(), + Tags: md.GetTags(), } - _, err = client.DataCaptureUpload(ctx, ur) - if err != nil { - return err + + // If it's a large binary file, we need to upload it in chunks. + if md.GetType() == v1.DataType_DATA_TYPE_BINARY_SENSOR && f.Size() > MaxUnaryFileSize { + if len(sensorData) > 1 { + return errors.New("binary sensor data file with more than one sensor reading is not supported") + } + + c, err := client.StreamingDataCaptureUpload(ctx) + if err != nil { + return errors.Wrap(err, "error creating upload client") + } + + toUpload := sensorData[0] + + // First send metadata. + streamMD := &v1.StreamingDataCaptureUploadRequest_Metadata{ + Metadata: &v1.DataCaptureUploadMetadata{ + UploadMetadata: uploadMD, + SensorMetadata: toUpload.GetMetadata(), + }, + } + if err := c.Send(&v1.StreamingDataCaptureUploadRequest{UploadPacket: streamMD}); err != nil { + return err + } + + // Then call the function to send the rest. + if err := sendStreamingDCRequests(ctx, c, toUpload.GetBinary()); err != nil { + return errors.Wrap(err, "error sending streaming data capture requests") + } + + if _, err := c.CloseAndRecv(); err != nil { + return errors.Wrap(err, "error receiving upload response") + } + } else { + ur := &v1.DataCaptureUploadRequest{ + Metadata: uploadMD, + SensorContents: sensorData, + } + _, err = client.DataCaptureUpload(ctx, ur) + if err != nil { + return err + } + } + + return nil +} + +func sendStreamingDCRequests(ctx context.Context, stream v1.DataSyncService_StreamingDataCaptureUploadClient, + contents []byte, +) error { + // Loop until there is no more content to send. + for i := 0; i < len(contents); i += UploadChunkSize { + select { + case <-ctx.Done(): + return ctx.Err() + default: + // Get the next chunk from contents. + end := i + UploadChunkSize + if end > len(contents) { + end = len(contents) + } + chunk := contents[i:end] + + // Build request with contents. + uploadReq := &v1.StreamingDataCaptureUploadRequest{ + UploadPacket: &v1.StreamingDataCaptureUploadRequest_Data{ + Data: chunk, + }, + } + + // Send request + if err := stream.Send(uploadReq); err != nil { + return err + } + } } return nil diff --git a/services/mlmodel/client.go b/services/mlmodel/client.go index 19674f59c67..2e4a96dca2f 100644 --- a/services/mlmodel/client.go +++ b/services/mlmodel/client.go @@ -2,12 +2,16 @@ package mlmodel import ( "context" + "unsafe" "github.com/edaniels/golog" + "github.com/pkg/errors" pb "go.viam.com/api/service/mlmodel/v1" "go.viam.com/utils/rpc" "google.golang.org/protobuf/types/known/structpb" + "gorgonia.org/tensor" + "go.viam.com/rdk/ml" "go.viam.com/rdk/resource" ) @@ -41,20 +45,155 @@ func NewClientFromConn( return c, nil } -func (c *client) Infer(ctx context.Context, input map[string]interface{}) (map[string]interface{}, error) { +func (c *client) Infer(ctx context.Context, tensors ml.Tensors, input map[string]interface{}) (ml.Tensors, map[string]interface{}, error) { + if input != nil && tensors != nil { + return nil, nil, errors.New("cannot have both input tensors and input map fed to Infer") + } inProto, err := structpb.NewStruct(input) if err != nil { - return nil, err + return nil, nil, err + } + tensorProto, err := TensorsToProto(tensors) + if err != nil { + return nil, nil, err + } + + if input == nil { + inProto = nil + } + if tensors == nil { + tensorProto = nil } resp, err := c.client.Infer(ctx, &pb.InferRequest{ - Name: c.name, - InputData: inProto, + Name: c.name, + InputTensors: tensorProto, + InputData: inProto, }) if err != nil { - return nil, err + return nil, nil, err + } + tensorResp, err := ProtoToTensors(resp.OutputTensors) + if err != nil { + return nil, nil, err + } + var mapResp map[string]interface{} + if resp.OutputData != nil { + mapResp = resp.OutputData.AsMap() + } + return tensorResp, mapResp, nil +} + +// ProtoToTensors takes pb.FlatTensors and turns it into a Tensors map. +func ProtoToTensors(pbft *pb.FlatTensors) (ml.Tensors, error) { + if pbft == nil { + return nil, errors.New("protobuf FlatTensors is nil") + } + tensors := ml.Tensors{} + for name, ftproto := range pbft.Tensors { + t, err := createNewTensor(ftproto) + if err != nil { + return nil, err + } + tensors[name] = t } + return tensors, nil +} + +// createNewTensor turns a proto FlatTensor into a *tensor.Dense. +func createNewTensor(pft *pb.FlatTensor) (*tensor.Dense, error) { + shape := make([]int, 0, len(pft.Shape)) + for _, s := range pft.Shape { + shape = append(shape, int(s)) + } + pt := pft.Tensor + switch t := pt.(type) { + case *pb.FlatTensor_Int8Tensor: + data := t.Int8Tensor + if data == nil { + return nil, errors.New("tensor of type Int8Tensor is nil") + } + dataSlice := data.GetData() + unsafeInt8Slice := *(*[]int8)(unsafe.Pointer(&dataSlice)) //nolint:gosec + int8Slice := make([]int8, 0, len(dataSlice)) + int8Slice = append(int8Slice, unsafeInt8Slice...) + return tensor.New(tensor.WithShape(shape...), tensor.WithBacking(int8Slice)), nil + case *pb.FlatTensor_Uint8Tensor: + data := t.Uint8Tensor + if data == nil { + return nil, errors.New("tensor of type Uint8Tensor is nil") + } + return tensor.New(tensor.WithShape(shape...), tensor.WithBacking(data.GetData())), nil + case *pb.FlatTensor_Int16Tensor: + data := t.Int16Tensor + if data == nil { + return nil, errors.New("tensor of type Int16Tensor is nil") + } + int16Data := uint32ToInt16(data.GetData()) + return tensor.New(tensor.WithShape(shape...), tensor.WithBacking(int16Data)), nil + case *pb.FlatTensor_Uint16Tensor: + data := t.Uint16Tensor + if data == nil { + return nil, errors.New("tensor of type Uint16Tensor is nil") + } + uint16Data := uint32ToUint16(data.GetData()) + return tensor.New(tensor.WithShape(shape...), tensor.WithBacking(uint16Data)), nil + case *pb.FlatTensor_Int32Tensor: + data := t.Int32Tensor + if data == nil { + return nil, errors.New("tensor of type Int32Tensor is nil") + } + return tensor.New(tensor.WithShape(shape...), tensor.WithBacking(data.GetData())), nil + case *pb.FlatTensor_Uint32Tensor: + data := t.Uint32Tensor + if data == nil { + return nil, errors.New("tensor of type Uint32Tensor is nil") + } + return tensor.New(tensor.WithShape(shape...), tensor.WithBacking(data.GetData())), nil + case *pb.FlatTensor_Int64Tensor: + data := t.Int64Tensor + if data == nil { + return nil, errors.New("tensor of type Int64Tensor is nil") + } + return tensor.New(tensor.WithShape(shape...), tensor.WithBacking(data.GetData())), nil + case *pb.FlatTensor_Uint64Tensor: + data := t.Uint64Tensor + if data == nil { + return nil, errors.New("tensor of type Uint64Tensor is nil") + } + return tensor.New(tensor.WithShape(shape...), tensor.WithBacking(data.GetData())), nil + case *pb.FlatTensor_FloatTensor: + data := t.FloatTensor + if data == nil { + return nil, errors.New("tensor of type FloatTensor is nil") + } + return tensor.New(tensor.WithShape(shape...), tensor.WithBacking(data.GetData())), nil + case *pb.FlatTensor_DoubleTensor: + data := t.DoubleTensor + if data == nil { + return nil, errors.New("tensor of type DoubleTensor is nil") + } + return tensor.New(tensor.WithShape(shape...), tensor.WithBacking(data.GetData())), nil + default: + return nil, errors.Errorf("don't know how to create tensor.Dense from proto type %T", pt) + } +} - return resp.OutputData.AsMap(), nil +func uint32ToInt16(uint32Slice []uint32) []int16 { + int16Slice := make([]int16, len(uint32Slice)) + + for i, value := range uint32Slice { + int16Slice[i] = int16(value) + } + return int16Slice +} + +func uint32ToUint16(uint32Slice []uint32) []uint16 { + uint16Slice := make([]uint16, len(uint32Slice)) + + for i, value := range uint32Slice { + uint16Slice[i] = uint16(value) + } + return uint16Slice } func (c *client) Metadata(ctx context.Context) (MLMetadata, error) { diff --git a/services/mlmodel/client_test.go b/services/mlmodel/client_test.go index 60b2de9fa3a..9732764e1e4 100644 --- a/services/mlmodel/client_test.go +++ b/services/mlmodel/client_test.go @@ -6,11 +6,12 @@ import ( "testing" "github.com/edaniels/golog" - "github.com/mitchellh/mapstructure" "go.viam.com/test" "go.viam.com/utils/rpc" + "gorgonia.org/tensor" viamgrpc "go.viam.com/rdk/grpc" + "go.viam.com/rdk/ml" "go.viam.com/rdk/resource" "go.viam.com/rdk/services/mlmodel" "go.viam.com/rdk/testutils/inject" @@ -40,9 +41,8 @@ func TestClient(t *testing.T) { test.That(t, err, test.ShouldBeNil) test.That(t, ok, test.ShouldBeTrue) test.That(t, resourceAPI.RegisterRPCService(context.Background(), rpcServer, svc), test.ShouldBeNil) - inputData := map[string]interface{}{ - "image": []uint8{10, 10, 255, 0, 0, 255, 255, 0, 100}, - } + inputTensors := ml.Tensors{} + inputTensors["image"] = tensor.New(tensor.WithShape(3, 3), tensor.WithBacking([]uint8{10, 10, 255, 0, 0, 255, 255, 0, 100})) go rpcServer.Serve(listener1) defer rpcServer.Stop() @@ -56,32 +56,45 @@ func TestClient(t *testing.T) { }) // working - t.Run("ml model client", func(t *testing.T) { + t.Run("ml model client infer", func(t *testing.T) { conn, err := viamgrpc.Dial(context.Background(), listener1.Addr().String(), logger) test.That(t, err, test.ShouldBeNil) client, err := mlmodel.NewClientFromConn(context.Background(), conn, "", mlmodel.Named(testMLModelServiceName), logger) test.That(t, err, test.ShouldBeNil) // Infer Command - result, err := client.Infer(context.Background(), inputData) + result, _, err := client.Infer(context.Background(), inputTensors, nil) test.That(t, err, test.ShouldBeNil) test.That(t, len(result), test.ShouldEqual, 4) - // decode the map[string]interface{} into a struct - temp := struct { - NDetections []int32 `mapstructure:"n_detections"` - ConfidenceScores [][]float32 `mapstructure:"confidence_scores"` - Labels [][]int32 `mapstructure:"labels"` - Locations [][][]float32 `mapstructure:"locations"` - }{} - err = mapstructure.Decode(result, &temp) test.That(t, err, test.ShouldBeNil) - test.That(t, temp.NDetections[0], test.ShouldEqual, 3) - test.That(t, len(temp.ConfidenceScores[0]), test.ShouldEqual, 3) - test.That(t, len(temp.Labels[0]), test.ShouldEqual, 3) - test.That(t, temp.Locations[0][0], test.ShouldResemble, []float32{0.1, 0.4, 0.22, 0.4}) + detections, err := result["n_detections"].At(0) + test.That(t, err, test.ShouldBeNil) + test.That(t, detections, test.ShouldEqual, 3) + confidenceScores, err := result["confidence_scores"].Slice(tensor.S(0, 1), nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, confidenceScores.Size(), test.ShouldEqual, 3) + labels, err := result["labels"].Slice(tensor.S(0, 1), nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, labels.Size(), test.ShouldEqual, 3) + location0, err := result["locations"].At(0, 0, 0) + test.That(t, err, test.ShouldBeNil) + test.That(t, location0, test.ShouldEqual, 0.1) + locations, err := result["locations"].Slice(tensor.S(0, 1), tensor.S(0, 1), nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, locations.Size(), test.ShouldEqual, 4) + test.That(t, locations.Data().([]float32), test.ShouldResemble, []float32{0.1, 0.4, 0.22, 0.4}) // nil data should work too - result, err = client.Infer(context.Background(), nil) + result, _, err = client.Infer(context.Background(), nil, nil) test.That(t, err, test.ShouldBeNil) test.That(t, len(result), test.ShouldEqual, 4) + // close the client + test.That(t, client.Close(context.Background()), test.ShouldBeNil) + test.That(t, conn.Close(), test.ShouldBeNil) + }) + t.Run("ml model client metadata", func(t *testing.T) { + conn, err := viamgrpc.Dial(context.Background(), listener1.Addr().String(), logger) + test.That(t, err, test.ShouldBeNil) + client, err := mlmodel.NewClientFromConn(context.Background(), conn, "", mlmodel.Named(testMLModelServiceName), logger) + test.That(t, err, test.ShouldBeNil) // Metadata Command meta, err := client.Metadata(context.Background()) test.That(t, err, test.ShouldBeNil) diff --git a/services/mlmodel/mlmodel.go b/services/mlmodel/mlmodel.go index 144812dc77d..f3487ca902d 100644 --- a/services/mlmodel/mlmodel.go +++ b/services/mlmodel/mlmodel.go @@ -4,10 +4,14 @@ package mlmodel import ( "context" + "unsafe" + "github.com/pkg/errors" servicepb "go.viam.com/api/service/mlmodel/v1" vprotoutils "go.viam.com/utils/protoutils" + "gorgonia.org/tensor" + "go.viam.com/rdk/ml" "go.viam.com/rdk/resource" "go.viam.com/rdk/robot" ) @@ -26,10 +30,127 @@ func init() { // the struct that will decode that map[string]interface{} correctly. type Service interface { resource.Resource - Infer(ctx context.Context, input map[string]interface{}) (map[string]interface{}, error) + Infer(ctx context.Context, tensors ml.Tensors, input map[string]interface{}) (ml.Tensors, map[string]interface{}, error) Metadata(ctx context.Context) (MLMetadata, error) } +// TensorsToProto turns the ml.Tensors map into a protobuf message of FlatTensors. +func TensorsToProto(ts ml.Tensors) (*servicepb.FlatTensors, error) { + pbts := &servicepb.FlatTensors{ + Tensors: make(map[string]*servicepb.FlatTensor), + } + for name, t := range ts { + tp, err := tensorToProto(t) + if err != nil { + return nil, errors.Wrap(err, "failed to convert tensor to proto message") + } + pbts.Tensors[name] = tp + } + return pbts, nil +} + +func tensorToProto(t *tensor.Dense) (*servicepb.FlatTensor, error) { + ftpb := &servicepb.FlatTensor{} + shape := t.Shape() + for _, s := range shape { + ftpb.Shape = append(ftpb.Shape, uint64(s)) + } + // switch on data type of the underlying array + data := t.Data() + switch dataSlice := data.(type) { + case []int8: + unsafeByteSlice := *(*[]byte)(unsafe.Pointer(&dataSlice)) //nolint:gosec + data := &servicepb.FlatTensorDataInt8{} + data.Data = append(data.Data, unsafeByteSlice...) + ftpb.Tensor = &servicepb.FlatTensor_Int8Tensor{Int8Tensor: data} + case []uint8: + ftpb.Tensor = &servicepb.FlatTensor_Uint8Tensor{ + Uint8Tensor: &servicepb.FlatTensorDataUInt8{ + Data: dataSlice, + }, + } + case []int16: + ftpb.Tensor = &servicepb.FlatTensor_Int16Tensor{ + Int16Tensor: &servicepb.FlatTensorDataInt16{ + Data: int16ToUint32(dataSlice), + }, + } + case []uint16: + ftpb.Tensor = &servicepb.FlatTensor_Uint16Tensor{ + Uint16Tensor: &servicepb.FlatTensorDataUInt16{ + Data: uint16ToUint32(dataSlice), + }, + } + case []int32: + ftpb.Tensor = &servicepb.FlatTensor_Int32Tensor{ + Int32Tensor: &servicepb.FlatTensorDataInt32{ + Data: dataSlice, + }, + } + case []uint32: + ftpb.Tensor = &servicepb.FlatTensor_Uint32Tensor{ + Uint32Tensor: &servicepb.FlatTensorDataUInt32{ + Data: dataSlice, + }, + } + case []int64: + ftpb.Tensor = &servicepb.FlatTensor_Int64Tensor{ + Int64Tensor: &servicepb.FlatTensorDataInt64{ + Data: dataSlice, + }, + } + case []uint64: + ftpb.Tensor = &servicepb.FlatTensor_Uint64Tensor{ + Uint64Tensor: &servicepb.FlatTensorDataUInt64{ + Data: dataSlice, + }, + } + case []int: + unsafeInt64Slice := *(*[]int64)(unsafe.Pointer(&dataSlice)) //nolint:gosec + data := &servicepb.FlatTensorDataInt64{} + data.Data = append(data.Data, unsafeInt64Slice...) + ftpb.Tensor = &servicepb.FlatTensor_Int64Tensor{Int64Tensor: data} + case []uint: + unsafeUint64Slice := *(*[]uint64)(unsafe.Pointer(&dataSlice)) //nolint:gosec + data := &servicepb.FlatTensorDataUInt64{} + data.Data = append(data.Data, unsafeUint64Slice...) + ftpb.Tensor = &servicepb.FlatTensor_Uint64Tensor{Uint64Tensor: data} + case []float32: + ftpb.Tensor = &servicepb.FlatTensor_FloatTensor{ + FloatTensor: &servicepb.FlatTensorDataFloat{ + Data: dataSlice, + }, + } + case []float64: + ftpb.Tensor = &servicepb.FlatTensor_DoubleTensor{ + DoubleTensor: &servicepb.FlatTensorDataDouble{ + Data: dataSlice, + }, + } + default: + return nil, errors.Errorf("cannot turn underlying tensor data of type %T into proto message", dataSlice) + } + return ftpb, nil +} + +func int16ToUint32(int16Slice []int16) []uint32 { + uint32Slice := make([]uint32, len(int16Slice)) + + for i, value := range int16Slice { + uint32Slice[i] = uint32(value) + } + return uint32Slice +} + +func uint16ToUint32(uint16Slice []uint16) []uint32 { + uint32Slice := make([]uint32, len(uint16Slice)) + + for i, value := range uint16Slice { + uint32Slice[i] = uint32(value) + } + return uint32Slice +} + // MLMetadata contains the metadata of the model file, such as the name of the model, what // kind of model it is, and the expected tensor/array shape and types of the inputs and outputs of the model. type MLMetadata struct { diff --git a/services/mlmodel/mlmodel_test.go b/services/mlmodel/mlmodel_test.go new file mode 100644 index 00000000000..fb60f304b94 --- /dev/null +++ b/services/mlmodel/mlmodel_test.go @@ -0,0 +1,40 @@ +package mlmodel + +import ( + "testing" + + "go.viam.com/test" + "gorgonia.org/tensor" +) + +func TestTensorRoundTrip(t *testing.T) { + testCases := []struct { + name string + tensor *tensor.Dense + }{ + {"int8", tensor.New(tensor.WithShape(2, 3), tensor.WithBacking([]int8{-1, 2, 3, 4, -5, 6}))}, + {"uint8", tensor.New(tensor.WithShape(2, 3), tensor.WithBacking([]uint8{1, 2, 3, 4, 5, 6}))}, + {"int16", tensor.New(tensor.WithShape(2, 3), tensor.WithBacking([]int16{-1, 2, 3, 4, -5, 6}))}, + {"uint16", tensor.New(tensor.WithShape(2, 3), tensor.WithBacking([]uint16{1, 2, 3, 4, 5, 6}))}, + {"int32", tensor.New(tensor.WithShape(2, 3), tensor.WithBacking([]int32{-1, 2, 3, 4, -5, 6}))}, + {"uint32", tensor.New(tensor.WithShape(2, 3), tensor.WithBacking([]uint32{1, 2, 3, 4, 5, 6}))}, + {"int64", tensor.New(tensor.WithShape(2, 3), tensor.WithBacking([]int64{-1, 2, 3, 4, -5, 6}))}, + {"uint64", tensor.New(tensor.WithShape(2, 3), tensor.WithBacking([]uint64{1, 2, 3, 4, 5, 6}))}, + {"float32", tensor.New(tensor.WithShape(2, 3), tensor.WithBacking([]float32{1.1, 2.0, 3.0, 4.0, 5.0, -6.0}))}, + {"float64", tensor.New(tensor.WithShape(2, 3), tensor.WithBacking([]float64{-1.1, 2.0, -3.0, 4.5, 5.0, 6.0}))}, + } + + for _, tensor := range testCases { + t.Run(tensor.name, func(t *testing.T) { + resp, err := tensorToProto(tensor.tensor) + test.That(t, err, test.ShouldBeNil) + test.That(t, resp.Shape, test.ShouldHaveLength, 2) + test.That(t, resp.Shape[0], test.ShouldEqual, 2) + test.That(t, resp.Shape[1], test.ShouldEqual, 3) + back, err := createNewTensor(resp) + test.That(t, err, test.ShouldBeNil) + test.That(t, back.Shape(), test.ShouldResemble, tensor.tensor.Shape()) + test.That(t, back.Data(), test.ShouldResemble, tensor.tensor.Data()) + }) + } +} diff --git a/services/mlmodel/server.go b/services/mlmodel/server.go index c09a60ad23d..184a3ddac79 100644 --- a/services/mlmodel/server.go +++ b/services/mlmodel/server.go @@ -4,10 +4,12 @@ import ( "context" "encoding/base64" + "github.com/pkg/errors" pb "go.viam.com/api/service/mlmodel/v1" vprotoutils "go.viam.com/utils/protoutils" "google.golang.org/protobuf/types/known/structpb" + "go.viam.com/rdk/ml" "go.viam.com/rdk/resource" ) @@ -28,24 +30,45 @@ func (server *serviceServer) Infer(ctx context.Context, req *pb.InferRequest) (* if err != nil { return nil, err } - id, err := asMap(req.InputData) + if req.InputData != nil && req.InputTensors != nil { + return nil, errors.New("both input_data and input_tensors fields in the Infer request are not nil." + + "Server can only process one or the other") + } + var id map[string]interface{} + if req.InputData != nil { + id, err = asMap(req.InputData) + if err != nil { + return nil, err + } + } + var it ml.Tensors + if req.InputTensors != nil { + it, err = ProtoToTensors(req.InputTensors) + if err != nil { + return nil, err + } + } + ot, od, err := svc.Infer(ctx, it, id) if err != nil { return nil, err } - od, err := svc.Infer(ctx, id) + outputData, err := vprotoutils.StructToStructPb(od) if err != nil { return nil, err } - outputData, err := vprotoutils.StructToStructPb(od) + outputTensors, err := TensorsToProto(ot) if err != nil { return nil, err } - return &pb.InferResponse{OutputData: outputData}, nil + return &pb.InferResponse{OutputData: outputData, OutputTensors: outputTensors}, nil } // AsMap converts x to a general-purpose Go map. // The map values are converted by calling Value.AsInterface. func asMap(x *structpb.Struct) (map[string]interface{}, error) { + if x == nil { + return nil, errors.New("input pb.Struct is nil") + } f := x.GetFields() vs := make(map[string]interface{}, len(f)) for k, in := range f { diff --git a/services/mlmodel/server_test.go b/services/mlmodel/server_test.go index 3d7f887054c..e8bd1f66a73 100644 --- a/services/mlmodel/server_test.go +++ b/services/mlmodel/server_test.go @@ -4,12 +4,12 @@ import ( "context" "testing" - "github.com/mitchellh/mapstructure" "github.com/pkg/errors" pb "go.viam.com/api/service/mlmodel/v1" "go.viam.com/test" - vprotoutils "go.viam.com/utils/protoutils" + "gorgonia.org/tensor" + "go.viam.com/rdk/ml" "go.viam.com/rdk/resource" "go.viam.com/rdk/services/mlmodel" "go.viam.com/rdk/testutils/inject" @@ -117,29 +117,41 @@ var injectedMetadataFunc = func(ctx context.Context) (mlmodel.MLMetadata, error) return md, nil } -var injectedInferFunc = func(ctx context.Context, input map[string]interface{}) (map[string]interface{}, error) { +var injectedInferFunc = func( + ctx context.Context, + tensors ml.Tensors, + input map[string]interface{}, +) (ml.Tensors, map[string]interface{}, error) { // this is a possible form of what a detection tensor with 3 detection in 1 image would look like - outputMap := make(map[string]interface{}) - outputMap["n_detections"] = []int32{3} - outputMap["confidence_scores"] = [][]float32{{0.9084375, 0.7359375, 0.33984375}} - outputMap["labels"] = [][]int32{{0, 0, 4}} - outputMap["locations"] = [][][]float32{{ - {0.1, 0.4, 0.22, 0.4}, - {0.02, 0.22, 0.77, 0.90}, - {0.40, 0.50, 0.40, 0.50}, - }} - return outputMap, nil + outputMap := ml.Tensors{} + outputMap["n_detections"] = tensor.New( + tensor.WithShape(1), + tensor.WithBacking([]int32{3}), + ) + outputMap["confidence_scores"] = tensor.New( + tensor.WithShape(1, 3), + tensor.WithBacking([]float32{0.9084375, 0.7359375, 0.33984375}), + ) + outputMap["labels"] = tensor.New( + tensor.WithShape(1, 3), + tensor.WithBacking([]int32{0, 0, 4}), + ) + outputMap["locations"] = tensor.New( + tensor.WithShape(1, 3, 4), + tensor.WithBacking([]float32{0.1, 0.4, 0.22, 0.4, 0.02, 0.22, 0.77, 0.90, 0.40, 0.50, 0.40, 0.50}), + ) + return outputMap, nil, nil } func TestServerInfer(t *testing.T) { - inputData := map[string]interface{}{ - "image": [][]uint8{{10, 10, 255, 0, 0, 255, 255, 0, 100}}, - } - inputProto, err := vprotoutils.StructToStructPb(inputData) + // input tensors to proto + inputTensors := ml.Tensors{} + inputTensors["image"] = tensor.New(tensor.WithShape(3, 3), tensor.WithBacking([]uint8{10, 10, 255, 0, 0, 255, 255, 0, 100})) + tensorsProto, err := mlmodel.TensorsToProto(inputTensors) test.That(t, err, test.ShouldBeNil) inferRequest := &pb.InferRequest{ - Name: testMLModelServiceName, - InputData: inputProto, + Name: testMLModelServiceName, + InputTensors: tensorsProto, } mockSrv := inject.NewMLModelService(testMLModelServiceName) @@ -152,19 +164,26 @@ func TestServerInfer(t *testing.T) { test.That(t, err, test.ShouldBeNil) resp, err := server.Infer(context.Background(), inferRequest) test.That(t, err, test.ShouldBeNil) - outMap := resp.OutputData.AsMap() - test.That(t, len(outMap), test.ShouldEqual, 4) - // decode the map[string]interface{} into a struct - temp := struct { - NDetections []int32 `mapstructure:"n_detections"` - ConfidenceScores [][]float32 `mapstructure:"confidence_scores"` - Labels [][]int32 `mapstructure:"labels"` - Locations [][][]float32 `mapstructure:"locations"` - }{} - err = mapstructure.Decode(outMap, &temp) - test.That(t, err, test.ShouldBeNil) - test.That(t, temp.NDetections[0], test.ShouldEqual, 3) - test.That(t, len(temp.ConfidenceScores[0]), test.ShouldEqual, 3) - test.That(t, len(temp.Labels[0]), test.ShouldEqual, 3) - test.That(t, temp.Locations[0][0], test.ShouldResemble, []float32{0.1, 0.4, 0.22, 0.4}) + test.That(t, len(resp.OutputTensors.Tensors), test.ShouldEqual, 4) + protoTensors := resp.OutputTensors.Tensors + // n detections + test.That(t, protoTensors["n_detections"].GetShape(), test.ShouldResemble, []uint64{1}) + nDetections := protoTensors["n_detections"].GetInt32Tensor() + test.That(t, nDetections, test.ShouldNotBeNil) + test.That(t, nDetections.Data[0], test.ShouldEqual, 3) + // confidence scores + test.That(t, protoTensors["confidence_scores"].GetShape(), test.ShouldResemble, []uint64{1, 3}) + confScores := protoTensors["confidence_scores"].GetFloatTensor() + test.That(t, confScores, test.ShouldNotBeNil) + test.That(t, confScores.Data, test.ShouldHaveLength, 3) + // labels + test.That(t, protoTensors["labels"].GetShape(), test.ShouldResemble, []uint64{1, 3}) + labels := protoTensors["labels"].GetInt32Tensor() + test.That(t, labels, test.ShouldNotBeNil) + test.That(t, labels.Data, test.ShouldHaveLength, 3) + // locations + test.That(t, protoTensors["locations"].GetShape(), test.ShouldResemble, []uint64{1, 3, 4}) + locations := protoTensors["locations"].GetFloatTensor() + test.That(t, locations, test.ShouldNotBeNil) + test.That(t, locations.Data[0:4], test.ShouldResemble, []float32{0.1, 0.4, 0.22, 0.4}) } diff --git a/services/mlmodel/tflitecpu/tflite_cpu.go b/services/mlmodel/tflitecpu/tflite_cpu.go index 8808a4f5e54..d1812fffa4d 100644 --- a/services/mlmodel/tflitecpu/tflite_cpu.go +++ b/services/mlmodel/tflitecpu/tflite_cpu.go @@ -12,11 +12,11 @@ import ( "github.com/pkg/errors" "go.opencensus.io/trace" + "go.viam.com/rdk/ml" inf "go.viam.com/rdk/ml/inference" "go.viam.com/rdk/ml/inference/tflite_metadata" "go.viam.com/rdk/resource" "go.viam.com/rdk/services/mlmodel" - "go.viam.com/rdk/utils" ) var sModel = resource.DefaultModelFamily.WithModel("tflite_cpu") @@ -48,23 +48,6 @@ type TFLiteConfig struct { LabelPath string `json:"label_path"` } -// Walk implements the Walker interface and correctly replaces model and label paths. -func (cfg *TFLiteConfig) Walk(visitor utils.Visitor) (interface{}, error) { - modelPath, err := visitor.Visit(cfg.ModelPath) - if err != nil { - return nil, err - } - cfg.ModelPath = modelPath.(string) - - labelPath, err := visitor.Visit(cfg.LabelPath) - if err != nil { - return nil, err - } - cfg.LabelPath = labelPath.(string) - - return cfg, nil -} - // Model is a struct that implements the TensorflowLite CPU implementation of the MLMS. // It includes the configured parameters, model struct, and associated metadata. type Model struct { @@ -126,47 +109,35 @@ func NewTFLiteCPUModel(ctx context.Context, params *TFLiteConfig, name resource. // Infer takes the input map and uses the inference package to // return the result from the tflite cpu model as a map. -func (m *Model) Infer(ctx context.Context, input map[string]interface{}) (map[string]interface{}, error) { +func (m *Model) Infer(ctx context.Context, tensors ml.Tensors, input map[string]interface{}) (ml.Tensors, map[string]interface{}, error) { _, span := trace.StartSpan(ctx, "service::mlmodel::tflite_cpu::Infer") defer span.End() - - outMap := make(map[string]interface{}) - doInfer := func(input interface{}) (map[string]interface{}, error) { - outTensors, err := m.model.Infer(input) - if err != nil { - return nil, errors.Wrapf(err, "couldn't infer from model %q", m.Name()) - } - - // Fill in the output map with the names from metadata if u have them - // Otherwise, do output1, output2, etc. - n := int(math.Min(float64(len(m.metadata.Outputs)), float64(len(m.model.Info.OutputTensorTypes)))) - for i := 0; i < n; i++ { - if m.metadata.Outputs[i].Name != "" { - outMap[m.metadata.Outputs[i].Name] = outTensors[i] + if input != nil { + return nil, nil, errors.New("input maps for tflite_cpu.Infer is no longer supported. Use tensor inputs") + } + outTensors, err := m.model.Infer(tensors) + if err != nil { + return nil, nil, errors.Wrapf(err, "couldn't infer from model %q", m.Name()) + } + // Fill in the output map with the names from metadata if u have them + // if at any point this fails, just use the default name. + results := ml.Tensors{} + for defaultName, tensor := range outTensors { + outName := defaultName + // tensors are usually added in the same order as the metadata was added. The number + // at the end of the tensor name (after the colon) is essentially an ordinal. + parts := strings.Split(defaultName, ":") // number after colon associates it with metadata + if len(parts) > 1 { + nameInt, err := strconv.Atoi(parts[len(parts)-1]) + if err == nil && len(m.metadata.Outputs) > nameInt && m.metadata.Outputs[nameInt].Name != "" { + outName = m.metadata.Outputs[nameInt].Name } else { - outMap["output"+strconv.Itoa(i)] = outTensors[i] + outName = strings.Join(parts[0:len(parts)-1], ":") // just use default name, add colons back } } - return outMap, nil + results[outName] = tensor } - - // If there's only one thing in the input map, use it. - if len(input) == 1 { - for _, in := range input { - return doInfer(in) - } - } - // If you have more than 1 thing - if m.metadata != nil { - // Use metadata if possible to grab input name - return doInfer(input[m.metadata.Inputs[0].Name]) - } - // Try to use "input" - if in, ok := input["input"]; ok { - return doInfer(in) - } - - return nil, errors.New("input map has multiple elements and none are named 'input'") + return results, nil, nil } // Metadata reads the metadata from your tflite cpu model into the metadata struct diff --git a/services/mlmodel/tflitecpu/tflite_cpu_test.go b/services/mlmodel/tflitecpu/tflite_cpu_test.go index 04f21f32bf1..107543be853 100644 --- a/services/mlmodel/tflitecpu/tflite_cpu_test.go +++ b/services/mlmodel/tflitecpu/tflite_cpu_test.go @@ -3,7 +3,6 @@ package tflitecpu import ( "context" "net" - "reflect" "testing" "github.com/edaniels/golog" @@ -11,11 +10,12 @@ import ( "go.viam.com/test" "go.viam.com/utils/artifact" "go.viam.com/utils/rpc" + "gorgonia.org/tensor" viamgrpc "go.viam.com/rdk/grpc" + "go.viam.com/rdk/ml" "go.viam.com/rdk/resource" "go.viam.com/rdk/rimage" - "go.viam.com/rdk/robot/packages" "go.viam.com/rdk/services/mlmodel" ) @@ -65,18 +65,31 @@ func TestTFLiteCPUDetector(t *testing.T) { resized := resize.Resize(uint(got.metadata.Inputs[0].Shape[1]), uint(got.metadata.Inputs[0].Shape[2]), pic, resize.Bilinear) imgBytes := rimage.ImageToUInt8Buffer(resized) test.That(t, imgBytes, test.ShouldNotBeNil) - inputMap := make(map[string]interface{}) - inputMap["image"] = imgBytes - - gotOutput, err := got.Infer(ctx, inputMap) + inputMap := ml.Tensors{} + inputMap["image"] = tensor.New( + tensor.WithShape(got.metadata.Inputs[0].Shape[1], got.metadata.Inputs[0].Shape[2], 3), + tensor.WithBacking(imgBytes), + ) + gotOutput, _, err := got.Infer(ctx, inputMap, nil) test.That(t, err, test.ShouldBeNil) test.That(t, gotOutput, test.ShouldNotBeNil) - test.That(t, gotOutput["number of detections"], test.ShouldResemble, []float32{25}) - test.That(t, len(gotOutput["score"].([]float32)), test.ShouldResemble, 25) - test.That(t, len(gotOutput["location"].([]float32)), test.ShouldResemble, 100) - test.That(t, len(gotOutput["category"].([]float32)), test.ShouldResemble, 25) - test.That(t, gotOutput["category"].([]float32)[0], test.ShouldEqual, 17) // 17 is dog + test.That(t, len(gotOutput), test.ShouldEqual, 4) + // n detections + test.That(t, gotOutput["number of detections"], test.ShouldNotBeNil) + test.That(t, gotOutput["number of detections"].Data(), test.ShouldResemble, []float32{25}) + // score + test.That(t, gotOutput["score"], test.ShouldNotBeNil) + test.That(t, gotOutput["score"].Shape(), test.ShouldResemble, tensor.Shape{1, 25}) + // category + test.That(t, gotOutput["category"], test.ShouldNotBeNil) + test.That(t, gotOutput["category"].Shape(), test.ShouldResemble, tensor.Shape{1, 25}) + result, err := gotOutput["category"].At(0, 0) + test.That(t, err, test.ShouldBeNil) + test.That(t, result, test.ShouldEqual, 17) // 17 is dog + // location + test.That(t, gotOutput["location"], test.ShouldNotBeNil) + test.That(t, gotOutput["location"].Shape(), test.ShouldResemble, tensor.Shape{1, 25, 4}) } func TestTFLiteCPUClassifier(t *testing.T) { @@ -113,17 +126,27 @@ func TestTFLiteCPUClassifier(t *testing.T) { resized := resize.Resize(uint(got.metadata.Inputs[0].Shape[1]), uint(got.metadata.Inputs[0].Shape[2]), pic, resize.Bilinear) imgBytes := rimage.ImageToUInt8Buffer(resized) test.That(t, imgBytes, test.ShouldNotBeNil) - inputMap := make(map[string]interface{}) - inputMap["image"] = imgBytes + inputMap := ml.Tensors{} + inputMap["images"] = tensor.New( + tensor.WithShape(got.metadata.Inputs[0].Shape[1], got.metadata.Inputs[0].Shape[2], 3), + tensor.WithBacking(imgBytes), + ) - gotOutput, err := got.Infer(ctx, inputMap) + gotOutput, _, err := got.Infer(ctx, inputMap, nil) test.That(t, err, test.ShouldBeNil) test.That(t, gotOutput, test.ShouldNotBeNil) - test.That(t, gotOutput["probability"].([]uint8), test.ShouldNotBeNil) - test.That(t, gotOutput["probability"].([]uint8)[290], test.ShouldEqual, 0) - test.That(t, gotOutput["probability"].([]uint8)[291], test.ShouldBeGreaterThan, 200) // 291 is lion - test.That(t, gotOutput["probability"].([]uint8)[292], test.ShouldEqual, 0) + test.That(t, len(gotOutput), test.ShouldEqual, 1) + test.That(t, gotOutput["probability"], test.ShouldNotBeNil) + result, err := gotOutput["probability"].At(0, 290) + test.That(t, err, test.ShouldBeNil) + test.That(t, result, test.ShouldEqual, 0) + result, err = gotOutput["probability"].At(0, 291) + test.That(t, err, test.ShouldBeNil) + test.That(t, result, test.ShouldBeGreaterThan, 200) // 291 is lion + result, err = gotOutput["probability"].At(0, 292) + test.That(t, err, test.ShouldBeNil) + test.That(t, result, test.ShouldEqual, 0) } func TestTFLiteCPUTextModel(t *testing.T) { @@ -151,17 +174,20 @@ func TestTFLiteCPUTextModel(t *testing.T) { test.That(t, got.metadata, test.ShouldNotBeNil) // Test that the Infer() works even on a text classifier - inputMap := make(map[string]interface{}) - inputMap["text"] = makeExampleSlice(got.model.Info.InputHeight) - test.That(t, len(inputMap["text"].([]int32)), test.ShouldEqual, 384) - gotOutput, err := got.Infer(ctx, inputMap) + zeros := make([]int32, got.model.Info.InputHeight) + inputMap := ml.Tensors{} + inputMap["input_ids"] = makeExampleTensor(got.model.Info.InputHeight) + test.That(t, inputMap["input_ids"].Shape(), test.ShouldResemble, tensor.Shape{384}) + inputMap["input_mask"] = tensor.New(tensor.WithShape(384), tensor.WithBacking(zeros)) + inputMap["segment_ids"] = tensor.New(tensor.WithShape(384), tensor.WithBacking(zeros)) + gotOutput, _, err := got.Infer(ctx, inputMap, nil) test.That(t, err, test.ShouldBeNil) test.That(t, gotOutput, test.ShouldNotBeNil) test.That(t, len(gotOutput), test.ShouldEqual, 2) - test.That(t, gotOutput["output0"], test.ShouldNotBeNil) - test.That(t, gotOutput["output1"], test.ShouldNotBeNil) - test.That(t, len(gotOutput["output0"].([]float32)), test.ShouldEqual, 384) - test.That(t, len(gotOutput["output1"].([]float32)), test.ShouldEqual, 384) + test.That(t, gotOutput["end_logits"], test.ShouldNotBeNil) + test.That(t, gotOutput["start_logits"], test.ShouldNotBeNil) + test.That(t, gotOutput["end_logits"].Shape(), test.ShouldResemble, tensor.Shape{1, 384}) + test.That(t, gotOutput["start_logits"].Shape(), test.ShouldResemble, tensor.Shape{1, 384}) } func TestTFLiteCPUClient(t *testing.T) { @@ -198,8 +224,11 @@ func TestTFLiteCPUClient(t *testing.T) { resized := resize.Resize(320, 320, pic, resize.Bilinear) imgBytes := rimage.ImageToUInt8Buffer(resized) test.That(t, imgBytes, test.ShouldNotBeNil) - inputMap := make(map[string]interface{}) - inputMap["image"] = imgBytes + inputMap := ml.Tensors{} + inputMap["image"] = tensor.New( + tensor.WithShape(320, 320, 3), + tensor.WithBacking(imgBytes), + ) conn, err := viamgrpc.Dial(context.Background(), listener1.Addr().String(), logger) test.That(t, err, test.ShouldBeNil) @@ -221,83 +250,31 @@ func TestTFLiteCPUClient(t *testing.T) { test.That(t, gotMD.Outputs[1].AssociatedFiles[0].Name, test.ShouldResemble, "labelmap.txt") // Test call to Infer - gotOutput, err := client.Infer(context.Background(), inputMap) + gotOutput, _, err := client.Infer(context.Background(), inputMap, nil) test.That(t, err, test.ShouldBeNil) test.That(t, gotOutput, test.ShouldNotBeNil) test.That(t, len(gotOutput), test.ShouldEqual, 4) - locs := reflect.ValueOf(gotOutput["location"]) - test.That(t, locs.Len(), test.ShouldEqual, 100) - scores := reflect.ValueOf(gotOutput["score"]) - test.That(t, scores.Len(), test.ShouldEqual, 25) - nDets := reflect.ValueOf(gotOutput["number of detections"]) - test.That(t, nDets.Len(), test.ShouldEqual, 1) - test.That(t, nDets.Index(0).Interface().(float64), test.ShouldResemble, float64(25)) - test.That(t, reflect.TypeOf(gotOutput["category"]).Kind(), test.ShouldResemble, reflect.Slice) - cats := reflect.ValueOf(gotOutput["category"]) - test.That(t, cats.Len(), test.ShouldEqual, 25) - test.That(t, cats.Index(0).Interface().(float64), test.ShouldResemble, float64(17)) // 17 is dog + // n detections + test.That(t, gotOutput["number of detections"], test.ShouldNotBeNil) + test.That(t, gotOutput["number of detections"].Data(), test.ShouldResemble, []float32{25}) + // score + test.That(t, gotOutput["score"], test.ShouldNotBeNil) + test.That(t, gotOutput["score"].Shape(), test.ShouldResemble, tensor.Shape{1, 25}) + // category + test.That(t, gotOutput["category"], test.ShouldNotBeNil) + test.That(t, gotOutput["category"].Shape(), test.ShouldResemble, tensor.Shape{1, 25}) + result, err := gotOutput["category"].At(0, 0) + test.That(t, err, test.ShouldBeNil) + test.That(t, result, test.ShouldEqual, 17) // 17 is dog + // location + test.That(t, gotOutput["location"], test.ShouldNotBeNil) + test.That(t, gotOutput["location"].Shape(), test.ShouldResemble, tensor.Shape{1, 25, 4}) } -func makeExampleSlice(length int) []int32 { +func makeExampleTensor(length int) *tensor.Dense { out := make([]int32, 0, length) for i := 0; i < length; i++ { out = append(out, int32(i)) } - return out -} - -func TestTFLiteConfigWalker(t *testing.T) { - makeVisionAttributes := func(modelPath, labelPath string) *TFLiteConfig { - return &TFLiteConfig{ - ModelPath: modelPath, - LabelPath: labelPath, - NumThreads: 1, - } - } - - labelPath := "/other/path/on/robot/textFile.txt" - visionAttrs := makeVisionAttributes("/some/path/on/robot/model.tflite", labelPath) - - labelPathWithRefs := "${packages.test_model}/textFile.txt" - visionAttrsWithRefs := makeVisionAttributes("${packages.test_model}/model.tflite", labelPathWithRefs) - - labelPathOneRef := "${packages.test_model}/textFile.txt" - visionAttrsOneRef := makeVisionAttributes("/some/path/on/robot/model.tflite", labelPathOneRef) - - packageManager := packages.NewNoopManager() - testAttributesWalker := func(t *testing.T, attrs *TFLiteConfig, expectedModelPath, expectedLabelPath string) { - newAttrs, err := attrs.Walk(packages.NewPackagePathVisitor(packageManager)) - test.That(t, err, test.ShouldBeNil) - - test.That(t, newAttrs.(*TFLiteConfig).ModelPath, test.ShouldEqual, expectedModelPath) - test.That(t, newAttrs.(*TFLiteConfig).LabelPath, test.ShouldEqual, expectedLabelPath) - test.That(t, newAttrs.(*TFLiteConfig).NumThreads, test.ShouldEqual, 1) - } - - testAttributesWalker(t, visionAttrs, "/some/path/on/robot/model.tflite", "/other/path/on/robot/textFile.txt") - testAttributesWalker(t, visionAttrsWithRefs, "test_model/model.tflite", "test_model/textFile.txt") - testAttributesWalker(t, visionAttrsOneRef, "/some/path/on/robot/model.tflite", "test_model/textFile.txt") -} - -func TestLabelPathWalkFail(t *testing.T) { - labelPath := "/blah/blah/mylabels.txt" - var oldLabelPath *string - - packageManager := packages.NewNoopManager() - visitor := packages.NewPackagePathVisitor(packageManager) - - outNew, err := visitor.Visit(labelPath) - test.That(t, err, test.ShouldBeNil) - test.That(t, outNew, test.ShouldResemble, labelPath) - func() { - defer func() { - if r := recover(); r == nil { - // If we're here, the old approach "Visit(nil)" did not panic - test.That(t, 3, test.ShouldResemble, 5) - } - }() - - // This should cause a panic - visitor.Visit(oldLabelPath) - }() + return tensor.New(tensor.WithShape(length), tensor.WithBacking(out)) } diff --git a/services/motion/builtin/builtin.go b/services/motion/builtin/builtin.go index a94f5eb5a3b..4368f354d38 100644 --- a/services/motion/builtin/builtin.go +++ b/services/motion/builtin/builtin.go @@ -4,18 +4,19 @@ package builtin import ( "bytes" "context" - "errors" "fmt" + "math" "sync" "github.com/edaniels/golog" "github.com/golang/geo/r3" geo "github.com/kellydunn/golang-geo" + "github.com/pkg/errors" + "go.uber.org/zap" + "go.uber.org/zap/zapcore" servicepb "go.viam.com/api/service/motion/v1" - "go.viam.com/rdk/components/arm" "go.viam.com/rdk/components/base" - "go.viam.com/rdk/components/base/fake" "go.viam.com/rdk/components/base/kinematicbase" "go.viam.com/rdk/components/movementsensor" "go.viam.com/rdk/internal" @@ -44,16 +45,25 @@ func init() { } const ( - builtinOpLabel = "motion-service" - defaultLinearVelocityMillisPerSec = 300 // mm per second; used for bases only - defaultAngularVelocityDegsPerSec = 60 // degrees per second; used for bases only + builtinOpLabel = "motion-service" + maxTravelDistanceMM = 5e6 // this is equivalent to 5km ) +// inputEnabledActuator is an actuator that interacts with the frame system. +// This allows us to figure out where the actuator currently is and then +// move it. Input units are always in meters or radians. +type inputEnabledActuator interface { + resource.Actuator + referenceframe.InputEnabled +} + // ErrNotImplemented is thrown when an unreleased function is called. var ErrNotImplemented = errors.New("function coming soon but not yet implemented") // Config describes how to configure the service; currently only used for specifying dependency on framesystem service. -type Config struct{} +type Config struct { + LogFilePath string `json:"log_file_path"` +} // Validate here adds a dependency on the internal framesystem service. func (c *Config) Validate(path string) ([]string, error) { @@ -73,6 +83,30 @@ func NewBuiltIn(ctx context.Context, deps resource.Dependencies, conf resource.C return ms, nil } +func newFilePathLoggerConfig(filepath string) zap.Config { + return zap.Config{ + Level: zap.NewAtomicLevelAt(zap.DebugLevel), + Encoding: "console", + EncoderConfig: zapcore.EncoderConfig{ + TimeKey: "ts", + LevelKey: "level", + NameKey: "logger", + CallerKey: "caller", + FunctionKey: zapcore.OmitKey, + MessageKey: "msg", + StacktraceKey: "stacktrace", + LineEnding: zapcore.DefaultLineEnding, + EncodeLevel: zapcore.CapitalColorLevelEncoder, + EncodeTime: zapcore.ISO8601TimeEncoder, + EncodeDuration: zapcore.StringDurationEncoder, + EncodeCaller: zapcore.ShortCallerEncoder, + }, + DisableStacktrace: true, + OutputPaths: []string{filepath, "stdout"}, + ErrorOutputPaths: []string{filepath, "stderr"}, + } +} + // Reconfigure updates the motion service when the config has changed. func (ms *builtIn) Reconfigure( ctx context.Context, @@ -82,35 +116,47 @@ func (ms *builtIn) Reconfigure( ms.lock.Lock() defer ms.lock.Unlock() - localizers := make(map[resource.Name]motion.Localizer) + config, err := resource.NativeConfig[*Config](conf) + if err != nil { + return err + } + if config.LogFilePath != "" { + logger, err := newFilePathLoggerConfig(config.LogFilePath).Build() + if err != nil { + return err + } + ms.logger = logger.Sugar().Named("motion") + } + movementSensors := make(map[resource.Name]movementsensor.MovementSensor) + slamServices := make(map[resource.Name]slam.Service) components := make(map[resource.Name]resource.Resource) for name, dep := range deps { switch dep := dep.(type) { case framesystem.Service: ms.fsService = dep - case slam.Service, movementsensor.MovementSensor: - localizer, err := motion.NewLocalizer(ctx, dep) - if err != nil { - return err - } - localizers[name] = localizer + case movementsensor.MovementSensor: + movementSensors[name] = dep + case slam.Service: + slamServices[name] = dep default: components[name] = dep } } + ms.movementSensors = movementSensors + ms.slamServices = slamServices ms.components = components - ms.localizers = localizers return nil } type builtIn struct { resource.Named resource.TriviallyCloseable - fsService framesystem.Service - localizers map[resource.Name]motion.Localizer - components map[resource.Name]resource.Resource - logger golog.Logger - lock sync.Mutex + fsService framesystem.Service + movementSensors map[resource.Name]movementsensor.MovementSensor + slamServices map[resource.Name]slam.Service + components map[resource.Name]resource.Resource + logger golog.Logger + lock sync.Mutex } // Move takes a goal location and will plan and execute a movement to move a component specified by its name to that destination. @@ -155,20 +201,35 @@ func (ms *builtIn) Move( goalPose, _ := tf.(*referenceframe.PoseInFrame) // the goal is to move the component to goalPose which is specified in coordinates of goalFrameName - output, err := motionplan.PlanMotion(ctx, ms.logger, goalPose, movingFrame, fsInputs, frameSys, worldState, constraints, extra) + steps, err := motionplan.PlanMotion(ctx, &motionplan.PlanRequest{ + Logger: ms.logger, + Goal: goalPose, + Frame: movingFrame, + StartConfiguration: fsInputs, + FrameSystem: frameSys, + WorldState: worldState, + ConstraintSpecs: constraints, + Options: extra, + }) if err != nil { return false, err } // move all the components - for _, step := range output { + for _, step := range steps { // TODO(erh): what order? parallel? for name, inputs := range step { if len(inputs) == 0 { continue } - err := resources[name].GoToInputs(ctx, inputs) - if err != nil { + r := resources[name] + if err := r.GoToInputs(ctx, inputs); err != nil { + // If there is an error on GoToInputs, stop the component if possible before returning the error + if actuator, ok := r.(inputEnabledActuator); ok { + if stopErr := actuator.Stop(ctx, nil); stopErr != nil { + return false, errors.Wrap(err, stopErr.Error()) + } + } return false, err } } @@ -186,17 +247,24 @@ func (ms *builtIn) MoveOnMap( extra map[string]interface{}, ) (bool, error) { operation.CancelOtherWithLabel(ctx, builtinOpLabel) + kinematicsOptions := kinematicbase.NewKinematicBaseOptions() // make call to motionplan - plan, kb, err := ms.planMoveOnMap(ctx, componentName, destination, slamName, extra) + plan, kb, err := ms.planMoveOnMap(ctx, componentName, destination, slamName, kinematicsOptions, extra) if err != nil { return false, fmt.Errorf("error making plan for MoveOnMap: %w", err) } // execute the plan for i := 1; i < len(plan); i++ { - if err := kb.GoToInputs(ctx, plan[i]); err != nil { - return false, err + if inputEnabledKb, ok := kb.(inputEnabledActuator); ok { + if err := inputEnabledKb.GoToInputs(ctx, plan[i]); err != nil { + // If there is an error on GoToInputs, stop the component if possible before returning the error + if stopErr := kb.Stop(ctx, nil); stopErr != nil { + return false, errors.Wrap(err, stopErr.Error()) + } + return false, err + } } } return true, nil @@ -211,239 +279,69 @@ func (ms *builtIn) MoveOnGlobe( heading float64, movementSensorName resource.Name, obstacles []*spatialmath.GeoObstacle, - linearVelocityMillisPerSec float64, - angularVelocityDegsPerSec float64, + motionCfg *motion.MotionConfiguration, extra map[string]interface{}, ) (bool, error) { operation.CancelOtherWithLabel(ctx, builtinOpLabel) - // get the localizer from the componentName - localizer, ok := ms.localizers[movementSensorName] - if !ok { - return false, resource.DependencyNotFoundError(movementSensorName) + // ensure arguments are well behaved + if motionCfg == nil { + motionCfg = &motion.MotionConfiguration{} } - - currentPosition, dstPIF, err := ms.getRelativePositionAndDestination(ctx, localizer, componentName, movementSensorName, *destination) - if err != nil { - return false, err + if obstacles == nil { + obstacles = []*spatialmath.GeoObstacle{} } - - plan, kb, err := ms.planMoveOnGlobe(ctx, - componentName, - currentPosition, - dstPIF, - localizer, - obstacles, - linearVelocityMillisPerSec, - angularVelocityDegsPerSec, - extra, - ) - if err != nil { - return false, err + if destination == nil { + return false, errors.New("destination cannot be nil") } - // execute the plan - for _, step := range plan { - for _, inputs := range step { - if ctx.Err() != nil { - return false, ctx.Err() - } - if len(inputs) == 0 { - continue - } - if err := kb.GoToInputs(ctx, inputs); err != nil { - return false, err - } - } - } - - return true, nil -} - -// planMoveOnGlobe returns the plan for MoveOnGlobe to execute. -func (ms *builtIn) planMoveOnGlobe( - ctx context.Context, - componentName resource.Name, - currentPosition r3.Vector, - dstPIF *referenceframe.PoseInFrame, - localizer motion.Localizer, - obstacles []*spatialmath.GeoObstacle, - linearVelocityMillisPerSec float64, - angularVelocityDegsPerSec float64, - extra map[string]interface{}, -) ([]map[string][]referenceframe.Input, kinematicbase.KinematicBase, error) { - // create a new empty framesystem which we add our base to - fs := referenceframe.NewEmptyFrameSystem("") - - // convert GeoObstacles into GeometriesInFrame with respect to the base's starting point - geoms := spatialmath.GeoObstaclesToGeometries(obstacles, currentPosition) - - gif := referenceframe.NewGeometriesInFrame(referenceframe.World, geoms) - wrldst, err := referenceframe.NewWorldState([]*referenceframe.GeometriesInFrame{gif}, nil) - if err != nil { - return nil, nil, err - } - - // construct limits - straightlineDistance := dstPIF.Pose().Point().Norm() - limits := []referenceframe.Limit{ - {Min: -straightlineDistance * 3, Max: straightlineDistance * 3}, - {Min: -straightlineDistance * 3, Max: straightlineDistance * 3}, - } - ms.logger.Debugf("base limits: %v", limits) - - // create a KinematicBase from the componentName - baseComponent, ok := ms.components[componentName] - if !ok { - return nil, nil, fmt.Errorf("only Base components are supported for MoveOnGlobe: could not find an Base named %v", componentName) - } - b, ok := baseComponent.(base.Base) - if !ok { - return nil, nil, fmt.Errorf("cannot move base of type %T because it is not a Base", baseComponent) - } - var kb kinematicbase.KinematicBase - if fake, ok := b.(*fake.Base); ok { - kb, err = kinematicbase.WrapWithFakeKinematics(ctx, fake, localizer, limits) - } else { - kb, err = kinematicbase.WrapWithKinematics(ctx, b, localizer, limits, - linearVelocityMillisPerSec, angularVelocityDegsPerSec) - } - if err != nil { - return nil, nil, err - } - - inputMap := map[string][]referenceframe.Input{componentName.Name: make([]referenceframe.Input, 3)} - - // Add the kinematic wheeled base to the framesystem - if err := fs.AddFrame(kb.Kinematics(), fs.World()); err != nil { - return nil, nil, err - } - - // make call to motionplan - plan, err := motionplan.PlanMotion(ctx, ms.logger, dstPIF, kb.Kinematics(), inputMap, fs, wrldst, nil, extra) - if err != nil { - return nil, nil, err - } - return plan, kb, nil -} - -// getRelativePositionAndDestination returns the position of the base relative to the localizer only if there is -// information about their spatial relationship within the framesystem and the resulting pose in frame -// as the destination relative to the base to plan for. -func (ms *builtIn) getRelativePositionAndDestination( - ctx context.Context, - localizer motion.Localizer, - componentName resource.Name, - movementSensorName resource.Name, - destination geo.Point, -) (r3.Vector, *referenceframe.PoseInFrame, error) { - var currentPosition r3.Vector - - // get localizer current pose in frame - currentPIF, err := localizer.CurrentPosition(ctx) - if err != nil { - return currentPosition, nil, err - } - - currentPosition = currentPIF.Pose().Point() - ms.logger.Debugf("current position: %v", currentPosition) - - // get position of localizer relative to base - robotFS, err := ms.fsService.FrameSystem(ctx, nil) - if err != nil { - return currentPosition, nil, err - } - - localizerFrame := robotFS.Frame(movementSensorName.ShortName()) - if localizerFrame != nil { - // build maps of relevant components and inputs from initial inputs - fsInputs, _, err := ms.fsService.CurrentInputs(ctx) - if err != nil { - return currentPosition, nil, err - } - - // transform currentPIF by the movementsensor translation specified for its frame - destinationFrameName := componentName.Name - tf, err := robotFS.Transform(fsInputs, ¤tPIF, destinationFrameName) - if err != nil { - return currentPosition, nil, err - } - currentPosition = tf.(*referenceframe.PoseInFrame).Pose().Point() - ms.logger.Debugf("corrected current position: %v", currentPosition) - } - - // convert destination into spatialmath.Pose with respect to lat = 0 = lng - dstPose := spatialmath.GeoPointToPose(&destination) - ms.logger.Debugf("destination as geo point and pose: %v, %v", destination, dstPose.Point()) - - // convert the destination to be relative to the currentPosition - relativeDestinationPt := r3.Vector{ - X: dstPose.Point().X - currentPosition.X, - Y: dstPose.Point().Y - currentPosition.Y, - Z: 0, - } - ms.logger.Debugf("destination pose relative to current position: %v", relativeDestinationPt) - - relativeDstPose := spatialmath.NewPoseFromPoint(relativeDestinationPt) - dstPIF := referenceframe.NewPoseInFrame(referenceframe.World, relativeDstPose) - - return currentPosition, dstPIF, nil -} - -// MoveSingleComponent will pass through a move command to a component with a MoveToPosition method that takes a pose. Arms are the only -// component that supports this. This method will transform the destination pose, given in an arbitrary frame, into the pose of the arm. -// The arm will then move its most distal link to that pose. If you instead wish to move any other component than the arm end to that pose, -// then you must manually adjust the given destination by the transform from the arm end to the intended component. -// Because this uses an arm's MoveToPosition method when issuing commands, it does not support obstacle avoidance. -func (ms *builtIn) MoveSingleComponent( - ctx context.Context, - componentName resource.Name, - destination *referenceframe.PoseInFrame, - worldState *referenceframe.WorldState, - extra map[string]interface{}, -) (bool, error) { - operation.CancelOtherWithLabel(ctx, builtinOpLabel) - - // Get the arm and all initial inputs - fsInputs, _, err := ms.fsService.CurrentInputs(ctx) + moveRequest, err := ms.newMoveOnGlobeRequest(ctx, componentName, destination, movementSensorName, obstacles, motionCfg, extra) if err != nil { return false, err } - ms.logger.Debugf("frame system inputs: %v", fsInputs) - - armResource, ok := ms.components[componentName] - if !ok { - return false, fmt.Errorf("could not find a resource named %v", componentName.ShortName()) - } - movableArm, ok := armResource.(arm.Arm) - if !ok { - return false, fmt.Errorf( - "could not cast resource named %v to an arm. MoveSingleComponent only supports moving arms for now", - componentName, - ) - } - - // get destination pose in frame of movable component - goalPose := destination.Pose() - if destination.Parent() != componentName.ShortName() { - ms.logger.Debugf("goal given in frame of %q", destination.Parent()) - frameSys, err := ms.fsService.FrameSystem(ctx, worldState.Transforms()) - if err != nil { + // start a loop that plans every iteration and exits when something is read from the success channel + for { + ma := newMoveAttempt(ctx, moveRequest) + if err := ma.start(); err != nil { return false, err } - // re-evaluate goalPose to be in the frame we're going to move in - tf, err := frameSys.Transform(fsInputs, destination, componentName.ShortName()+"_origin") - if err != nil { + // this ensures that if the context is cancelled we always return early at the top of the loop + if err := ctx.Err(); err != nil { + ma.cancel() return false, err } - goalPoseInFrame, _ := tf.(*referenceframe.PoseInFrame) - goalPose = goalPoseInFrame.Pose() - ms.logger.Debugf("converted goal pose %q", spatialmath.PoseToProtobuf(goalPose)) + + select { + // if context was cancelled by the calling function, error out + case <-ctx.Done(): + ma.cancel() + return false, ctx.Err() + + // once execution responds: return the result to the caller + case resp := <-ma.responseChan: + ms.logger.Debugf("execution complete: %#v", resp) + ma.cancel() + return resp.success, resp.err + + // if the position poller hit an error return it, otherwise replan + case resp := <-moveRequest.position.responseChan: + ms.logger.Debugf("position response: %#v", resp) + ma.cancel() + if resp.err != nil { + return false, resp.err + } + + // if the obstacle poller hit an error return it, otherwise replan + case resp := <-moveRequest.obstacle.responseChan: + ms.logger.Debugf("obstacle response: %#v", resp) + ma.cancel() + if resp.err != nil { + return false, resp.err + } + } } - err = movableArm.MoveToPosition(ctx, goalPose, extra) - return err == nil, err } func (ms *builtIn) GetPose( @@ -460,7 +358,7 @@ func (ms *builtIn) GetPose( ctx, referenceframe.NewPoseInFrame( componentName.ShortName(), - spatialmath.NewPoseFromPoint(r3.Vector{0, 0, 0}), + spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 0, Z: 0}), ), destinationFrame, supplementalTransforms, @@ -473,25 +371,21 @@ func (ms *builtIn) planMoveOnMap( componentName resource.Name, destination spatialmath.Pose, slamName resource.Name, + kinematicsOptions kinematicbase.Options, extra map[string]interface{}, ) ([][]referenceframe.Input, kinematicbase.KinematicBase, error) { // get the SLAM Service from the slamName - localizer, ok := ms.localizers[slamName] + slamSvc, ok := ms.slamServices[slamName] if !ok { return nil, nil, resource.DependencyNotFoundError(slamName) } - // assert localizer as a slam service and get map limits - slamSvc, ok := localizer.(slam.Service) - if !ok { - return nil, nil, fmt.Errorf("cannot assert localizer of type %T as slam service", localizer) - } - // gets the extents of the SLAM map - limits, err := slam.GetLimits(ctx, slamSvc) + limits, err := slam.Limits(ctx, slamSvc) if err != nil { return nil, nil, err } + limits = append(limits, referenceframe.Limit{Min: -2 * math.Pi, Max: 2 * math.Pi}) // create a KinematicBase from the componentName component, ok := ms.components[componentName] @@ -502,19 +396,24 @@ func (ms *builtIn) planMoveOnMap( if !ok { return nil, nil, fmt.Errorf("cannot move component of type %T because it is not a Base", component) } - var kb kinematicbase.KinematicBase - if fake, ok := b.(*fake.Base); ok { - kb, err = kinematicbase.WrapWithFakeKinematics(ctx, fake, localizer, limits) - } else { - kb, err = kinematicbase.WrapWithKinematics(ctx, b, localizer, limits, - defaultLinearVelocityMillisPerSec, defaultAngularVelocityDegsPerSec) + + if extra != nil { + if profile, ok := extra["motion_profile"]; ok { + motionProfile, ok := profile.(string) + if !ok { + return nil, nil, errors.New("could not interpret motion_profile field as string") + } + kinematicsOptions.PositionOnlyMode = motionProfile == motionplan.PositionOnlyMotionProfile + } } + + kb, err := kinematicbase.WrapWithKinematics(ctx, b, ms.logger, motion.NewSLAMLocalizer(slamSvc), limits, kinematicsOptions) if err != nil { return nil, nil, err } // get point cloud data in the form of bytes from pcd - pointCloudData, err := slam.GetPointCloudMapFull(ctx, slamSvc) + pointCloudData, err := slam.PointCloudMapFull(ctx, slamSvc) if err != nil { return nil, nil, err } @@ -524,16 +423,14 @@ func (ms *builtIn) planMoveOnMap( return nil, nil, err } - if extra == nil { - extra = make(map[string]interface{}) - } - extra["planning_alg"] = "rrtstar" - // get current position inputs, err := kb.CurrentInputs(ctx) if err != nil { return nil, nil, err } + if kinematicsOptions.PositionOnlyMode { + inputs = inputs[:2] + } ms.logger.Debugf("base position: %v", inputs) dst := referenceframe.NewPoseInFrame(referenceframe.World, spatialmath.NewPoseFromPoint(destination.Point())) @@ -554,10 +451,19 @@ func (ms *builtIn) planMoveOnMap( seedMap := map[string][]referenceframe.Input{f.Name(): inputs} ms.logger.Debugf("goal position: %v", dst.Pose().Point()) - solutionMap, err := motionplan.PlanMotion(ctx, ms.logger, dst, f, seedMap, fs, worldState, nil, extra) + plan, err := motionplan.PlanMotion(ctx, &motionplan.PlanRequest{ + Logger: ms.logger, + Goal: dst, + Frame: f, + StartConfiguration: seedMap, + FrameSystem: fs, + WorldState: worldState, + ConstraintSpecs: nil, + Options: extra, + }) if err != nil { return nil, nil, err } - plan, err := motionplan.FrameStepsFromRobotPath(f.Name(), solutionMap) - return plan, kb, err + steps, err := plan.GetFrameSteps(f.Name()) + return steps, kb, err } diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index fd57c20d5de..3690ab47eb1 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -10,26 +10,33 @@ import ( "github.com/edaniels/golog" "github.com/golang/geo/r3" geo "github.com/kellydunn/golang-geo" - // register. + "github.com/pkg/errors" + // registers all components. commonpb "go.viam.com/api/common/v1" "go.viam.com/test" "go.viam.com/utils" "go.viam.com/utils/artifact" "go.viam.com/rdk/components/arm" + armFake "go.viam.com/rdk/components/arm/fake" + ur "go.viam.com/rdk/components/arm/universalrobots" "go.viam.com/rdk/components/base" - "go.viam.com/rdk/components/base/fake" + baseFake "go.viam.com/rdk/components/base/fake" + "go.viam.com/rdk/components/base/kinematicbase" "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" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" + "go.viam.com/rdk/robot/framesystem" robotimpl "go.viam.com/rdk/robot/impl" "go.viam.com/rdk/services/motion" "go.viam.com/rdk/services/slam" "go.viam.com/rdk/spatialmath" "go.viam.com/rdk/testutils/inject" + rdkutils "go.viam.com/rdk/utils" ) func setupMotionServiceFromConfig(t *testing.T, configFilename string) (motion.Service, func()) { @@ -47,6 +54,181 @@ func setupMotionServiceFromConfig(t *testing.T, configFilename string) (motion.S } } +func getPointCloudMap(path string) (func() ([]byte, error), error) { + const chunkSizeBytes = 1 * 1024 * 1024 + file, err := os.Open(path) + if err != nil { + return nil, err + } + chunk := make([]byte, chunkSizeBytes) + f := func() ([]byte, error) { + bytesRead, err := file.Read(chunk) + if err != nil { + defer utils.UncheckedErrorFunc(file.Close) + return nil, err + } + return chunk[:bytesRead], err + } + return f, nil +} + +func createInjectedMovementSensor(name string, gpsPoint *geo.Point) *inject.MovementSensor { + injectedMovementSensor := inject.NewMovementSensor(name) + injectedMovementSensor.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { + return gpsPoint, 0, nil + } + injectedMovementSensor.CompassHeadingFunc = func(ctx context.Context, extra map[string]interface{}) (float64, error) { + return 0, nil + } + injectedMovementSensor.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { + return &movementsensor.Properties{CompassHeadingSupported: true}, nil + } + + return injectedMovementSensor +} + +func createInjectedSlam(name, pcdPath string) *inject.SLAMService { + injectSlam := inject.NewSLAMService(name) + injectSlam.PointCloudMapFunc = func(ctx context.Context) (func() ([]byte, error), error) { + return getPointCloudMap(filepath.Clean(artifact.MustPath(pcdPath))) + } + injectSlam.PositionFunc = func(ctx context.Context) (spatialmath.Pose, string, error) { + return spatialmath.NewZeroPose(), "", nil + } + return injectSlam +} + +func createBaseLink(t *testing.T, baseName string) *referenceframe.LinkInFrame { + basePose := spatialmath.NewPoseFromPoint(r3.Vector{0, 0, 0}) + baseSphere, err := spatialmath.NewSphere(basePose, 10, "base-sphere") + test.That(t, err, test.ShouldBeNil) + baseLink := referenceframe.NewLinkInFrame( + referenceframe.World, + spatialmath.NewZeroPose(), + baseName, + baseSphere, + ) + return baseLink +} + +func createFrameSystemService( + ctx context.Context, + deps resource.Dependencies, + fsParts []*referenceframe.FrameSystemPart, + logger golog.Logger, +) (framesystem.Service, error) { + fsSvc, err := framesystem.New(ctx, deps, logger) + if err != nil { + return nil, err + } + conf := resource.Config{ + ConvertedAttributes: &framesystem.Config{Parts: fsParts}, + } + if err := fsSvc.Reconfigure(ctx, deps, conf); err != nil { + return nil, err + } + deps[fsSvc.Name()] = fsSvc + + return fsSvc, nil +} + +func createMoveOnGlobeEnvironment(ctx context.Context, t *testing.T, origin, destination *geo.Point) ( + *inject.MovementSensor, framesystem.Service, base.Base, motion.Service, +) { + logger := golog.NewTestLogger(t) + + // create fake base + baseCfg := resource.Config{ + Name: "test-base", + API: base.API, + Frame: &referenceframe.LinkConfig{Geometry: &spatialmath.GeometryConfig{R: 20}}, + } + fakeBase, err := baseFake.NewBase(ctx, nil, baseCfg, logger) + test.That(t, err, test.ShouldBeNil) + + // create base link + baseLink := createBaseLink(t, "test-base") + + // create injected MovementSensor + staticMovementSensor := createInjectedMovementSensor("test-gps", origin) + + // create MovementSensor link + movementSensorLink := referenceframe.NewLinkInFrame( + baseLink.Name(), + spatialmath.NewPoseFromPoint(r3.Vector{-10, 0, 0}), + "test-gps", + nil, + ) + + // create a fake kinematic base + localizer := motion.NewMovementSensorLocalizer(staticMovementSensor, origin, spatialmath.NewZeroPose()) + straightlineDistance := spatialmath.GeoPointToPose(destination, origin).Point().Norm() + limits := []referenceframe.Limit{ + {Min: -straightlineDistance * 3, Max: straightlineDistance * 3}, + {Min: -straightlineDistance * 3, Max: straightlineDistance * 3}, + {Min: -2 * math.Pi, Max: 2 * math.Pi}, + } + kinematicsOptions := kinematicbase.NewKinematicBaseOptions() + kinematicsOptions.PlanDeviationThresholdMM = 1 // can afford to do this for tests + kb, err := kinematicbase.WrapWithFakeKinematics(ctx, fakeBase.(*baseFake.Base), localizer, limits, kinematicsOptions) + test.That(t, err, test.ShouldBeNil) + + // create injected MovementSensor + dynamicMovementSensor := inject.NewMovementSensor("test-gps") + dynamicMovementSensor.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { + input, err := kb.CurrentInputs(ctx) + test.That(t, err, test.ShouldBeNil) + heading := rdkutils.RadToDeg(math.Atan2(input[0].Value, input[1].Value)) + distance := math.Sqrt(input[1].Value*input[1].Value + input[0].Value*input[0].Value) + pt := origin.PointAtDistanceAndBearing(distance*1e-6, heading) + return pt, 0, nil + } + dynamicMovementSensor.CompassHeadingFunc = func(ctx context.Context, extra map[string]interface{}) (float64, error) { + return 0, nil + } + dynamicMovementSensor.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { + return &movementsensor.Properties{CompassHeadingSupported: true}, nil + } + + // create the frame system + fsParts := []*referenceframe.FrameSystemPart{ + {FrameConfig: movementSensorLink}, + {FrameConfig: baseLink}, + } + deps := resource.Dependencies{ + fakeBase.Name(): kb, + dynamicMovementSensor.Name(): dynamicMovementSensor, + } + + fsSvc, err := createFrameSystemService(ctx, deps, fsParts, logger) + test.That(t, err, test.ShouldBeNil) + + conf := resource.Config{ConvertedAttributes: &Config{}} + ms, err := NewBuiltIn(ctx, deps, conf, logger) + test.That(t, err, test.ShouldBeNil) + + return dynamicMovementSensor, fsSvc, fakeBase, ms +} + +func createMoveOnMapEnvironment(ctx context.Context, t *testing.T, pcdPath string) motion.Service { + injectSlam := createInjectedSlam("test_slam", pcdPath) + + cfg := resource.Config{ + Name: "test_base", + API: base.API, + Frame: &referenceframe.LinkConfig{Geometry: &spatialmath.GeometryConfig{R: 100}}, + } + logger := golog.NewTestLogger(t) + fakeBase, err := baseFake.NewBase(ctx, nil, cfg, logger) + test.That(t, err, test.ShouldBeNil) + + deps := resource.Dependencies{injectSlam.Name(): injectSlam, fakeBase.Name(): fakeBase} + conf := resource.Config{ConvertedAttributes: &Config{}} + ms, err := NewBuiltIn(ctx, deps, conf, logger) + test.That(t, err, test.ShouldBeNil) + return ms +} + func TestMoveFailures(t *testing.T) { var err error ms, teardown := setupMotionServiceFromConfig(t, "../data/arm_gantry.json") @@ -172,115 +354,79 @@ func TestMoveWithObstacles(t *testing.T) { }) } -func TestMoveSingleComponent(t *testing.T) { - ms, teardown := setupMotionServiceFromConfig(t, "../data/moving_arm.json") - defer teardown() +func TestMoveOnMapLongDistance(t *testing.T) { + t.Parallel() + ctx := context.Background() + // goal position is scaled to be in mm + goal := spatialmath.NewPoseFromPoint(r3.Vector{X: -32.508 * 1000, Y: -2.092 * 1000}) - grabPose := spatialmath.NewPoseFromPoint(r3.Vector{-25, 30, 0}) - t.Run("succeeds when all frame info in config", func(t *testing.T) { - _, err := ms.MoveSingleComponent( + t.Run("test cbirrt planning on office map", func(t *testing.T) { + t.Parallel() + ms := createMoveOnMapEnvironment(ctx, t, "slam/example_cartographer_outputs/viam-office-02-22-3/pointcloud/pointcloud_4.pcd") + extra := make(map[string]interface{}) + extra["planning_alg"] = "cbirrt" + + path, _, err := ms.(*builtIn).planMoveOnMap( context.Background(), - arm.Named("pieceArm"), - referenceframe.NewPoseInFrame("c", grabPose), - nil, - map[string]interface{}{}, + base.Named("test_base"), + goal, + slam.Named("test_slam"), + kinematicbase.NewKinematicBaseOptions(), + extra, ) - // Gripper is not an arm and cannot move test.That(t, err, test.ShouldBeNil) - }) - t.Run("fails due to gripper not being an arm", func(t *testing.T) { - _, err := ms.MoveSingleComponent( - context.Background(), - gripper.Named("pieceGripper"), - referenceframe.NewPoseInFrame("c", grabPose), - nil, - map[string]interface{}{}, - ) - // Gripper is not an arm and cannot move - test.That(t, err, test.ShouldNotBeNil) + test.That(t, len(path), test.ShouldBeGreaterThan, 2) }) - t.Run("succeeds with supplemental info in world state", func(t *testing.T) { - worldState, err := referenceframe.NewWorldState( - nil, - []*referenceframe.LinkInFrame{referenceframe.NewLinkInFrame("c", spatialmath.NewZeroPose(), "testFrame2", nil)}, - ) - test.That(t, err, test.ShouldBeNil) - _, err = ms.MoveSingleComponent( + t.Run("test rrtstar planning on office map", func(t *testing.T) { + t.Parallel() + ms := createMoveOnMapEnvironment(ctx, t, "slam/example_cartographer_outputs/viam-office-02-22-3/pointcloud/pointcloud_4.pcd") + extra := make(map[string]interface{}) + extra["planning_alg"] = "rrtstar" + + path, _, err := ms.(*builtIn).planMoveOnMap( context.Background(), - arm.Named("pieceArm"), - referenceframe.NewPoseInFrame("testFrame2", grabPose), - worldState, - map[string]interface{}{}, + base.Named("test_base"), + goal, + slam.Named("test_slam"), + kinematicbase.NewKinematicBaseOptions(), + extra, ) test.That(t, err, test.ShouldBeNil) + test.That(t, len(path), test.ShouldBeGreaterThan, 2) }) } func TestMoveOnMap(t *testing.T) { + t.Skip() // RSDK-4279 t.Parallel() ctx := context.Background() - logger := golog.NewTestLogger(t) - injectSlam := inject.NewSLAMService("test_slam") - - const chunkSizeBytes = 1 * 1024 * 1024 - - injectSlam.GetPointCloudMapFunc = func(ctx context.Context) (func() ([]byte, error), error) { - path := filepath.Clean(artifact.MustPath("pointcloud/octagonspace.pcd")) - file, err := os.Open(path) - if err != nil { - return nil, err - } - chunk := make([]byte, chunkSizeBytes) - f := func() ([]byte, error) { - bytesRead, err := file.Read(chunk) - if err != nil { - defer utils.UncheckedErrorFunc(file.Close) - return nil, err - } - return chunk[:bytesRead], err - } - return f, nil - } - - cfg := resource.Config{ - Name: "test_base", - API: base.API, - Frame: &referenceframe.LinkConfig{Geometry: &spatialmath.GeometryConfig{R: 100}}, - } - - fakeBase, err := fake.NewBase(ctx, nil, cfg, logger) - test.That(t, err, test.ShouldBeNil) - - ms, err := NewBuiltIn( - ctx, - resource.Dependencies{injectSlam.Name(): injectSlam, fakeBase.Name(): fakeBase}, - resource.Config{ - ConvertedAttributes: &Config{}, - }, - logger, - ) - test.That(t, err, test.ShouldBeNil) - // goal x-position of 1.32m is scaled to be in mm goal := spatialmath.NewPoseFromPoint(r3.Vector{X: 1.32 * 1000, Y: 0}) t.Run("check that path is planned around obstacle", func(t *testing.T) { t.Parallel() + ms := createMoveOnMapEnvironment(ctx, t, "pointcloud/octagonspace.pcd") + extra := make(map[string]interface{}) + extra["motion_profile"] = "orientation" path, _, err := ms.(*builtIn).planMoveOnMap( context.Background(), base.Named("test_base"), goal, slam.Named("test_slam"), - nil, + kinematicbase.NewKinematicBaseOptions(), + extra, ) test.That(t, err, test.ShouldBeNil) // path of length 2 indicates a path that goes straight through central obstacle test.That(t, len(path), test.ShouldBeGreaterThan, 2) + // every waypoint should have the form [x,y,theta] + test.That(t, len(path[0]), test.ShouldEqual, 3) }) t.Run("ensure success of movement around obstacle", func(t *testing.T) { t.Parallel() + ms := createMoveOnMapEnvironment(ctx, t, "pointcloud/octagonspace.pcd") success, err := ms.MoveOnMap( context.Background(), base.Named("test_base"), @@ -294,6 +440,7 @@ func TestMoveOnMap(t *testing.T) { t.Run("check that straight line path executes", func(t *testing.T) { t.Parallel() + ms := createMoveOnMapEnvironment(ctx, t, "pointcloud/octagonspace.pcd") easyGoal := spatialmath.NewPoseFromPoint(r3.Vector{X: 0.277 * 1000, Y: 0.593 * 1000}) success, err := ms.MoveOnMap( context.Background(), @@ -305,182 +452,207 @@ func TestMoveOnMap(t *testing.T) { test.That(t, err, test.ShouldBeNil) test.That(t, success, test.ShouldBeTrue) }) + + t.Run("check that position-only mode returns 2D plan", func(t *testing.T) { + t.Parallel() + ms := createMoveOnMapEnvironment(ctx, t, "pointcloud/octagonspace.pcd") + extra := make(map[string]interface{}) + extra["motion_profile"] = "position_only" + path, _, err := ms.(*builtIn).planMoveOnMap( + context.Background(), + base.Named("test_base"), + goal, + slam.Named("test_slam"), + kinematicbase.NewKinematicBaseOptions(), + extra, + ) + test.That(t, err, test.ShouldBeNil) + // every waypoint should have the form [x,y] + test.That(t, len(path[0]), test.ShouldEqual, 2) + }) + + t.Run("check that position-only mode executes", func(t *testing.T) { + t.Parallel() + ms := createMoveOnMapEnvironment(ctx, t, "pointcloud/octagonspace.pcd") + extra := make(map[string]interface{}) + extra["motion_profile"] = "position_only" + success, err := ms.MoveOnMap( + context.Background(), + base.Named("test_base"), + goal, + slam.Named("test_slam"), + extra, + ) + test.That(t, err, test.ShouldBeNil) + test.That(t, success, test.ShouldBeTrue) + }) } -func TestInjectedMoveOnGlobe(t *testing.T) { - t.Parallel() +func TestMoveOnMapTimeout(t *testing.T) { ctx := context.Background() logger := golog.NewTestLogger(t) + cfg, err := config.Read(ctx, "../data/real_wheeled_base.json", logger) + test.That(t, err, test.ShouldBeNil) + myRobot, err := robotimpl.New(ctx, cfg, logger) + test.That(t, err, test.ShouldBeNil) + defer func() { + test.That(t, myRobot.Close(context.Background()), test.ShouldBeNil) + }() - // create motion config - motionCfg := make(map[string]interface{}) - motionCfg["motion_profile"] = "position_only" - motionCfg["timeout"] = 5. + injectSlam := createInjectedSlam("test_slam", "pointcloud/octagonspace.pcd") - // create fake base - baseCfg := resource.Config{ - Name: "test-base", - API: base.API, - Frame: &referenceframe.LinkConfig{Geometry: &spatialmath.GeometryConfig{R: 20}}, - } - fakeBase, err := fake.NewBase(ctx, nil, baseCfg, logger) + realBase, err := base.FromRobot(myRobot, "test_base") test.That(t, err, test.ShouldBeNil) - // create base frame - basePose := spatialmath.NewPoseFromPoint(r3.Vector{0, 0, 0}) - baseSphere, err := spatialmath.NewSphere(basePose, 10, "base-sphere") + deps := resource.Dependencies{ + injectSlam.Name(): injectSlam, + realBase.Name(): realBase, + } + conf := resource.Config{ConvertedAttributes: &Config{}} + ms, err := NewBuiltIn(ctx, deps, conf, logger) test.That(t, err, test.ShouldBeNil) - baseFrame, err := referenceframe.NewStaticFrameWithGeometry( - "test-base", - basePose, - baseSphere, + + easyGoal := spatialmath.NewPoseFromPoint(r3.Vector{X: 1001, Y: 1001}) + success, err := ms.MoveOnMap( + context.Background(), + base.Named("test_base"), + easyGoal, + slam.Named("test_slam"), + nil, ) - test.That(t, err, test.ShouldBeNil) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, success, test.ShouldBeFalse) +} - // create injected MovementSensor - injectedMovementSensor := inject.NewMovementSensor("test-gps") - injectedMovementSensor.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { - return geo.NewPoint(0, 0), 0, nil - } +func TestMoveOnGlobe(t *testing.T) { + t.Parallel() + ctx := context.Background() - // create MovementSensor frame - movementSensorFrame, err := referenceframe.NewStaticFrame( - "test-gps", - spatialmath.NewPoseFromPoint(r3.Vector{-10, 0, 0}), - ) - test.That(t, err, test.ShouldBeNil) + gpsPoint := geo.NewPoint(-70, 40) - // create a framesystem - newFS := referenceframe.NewEmptyFrameSystem("test-FS") - worldFrame, err := referenceframe.NewStaticFrame("world", spatialmath.NewPoseFromPoint(r3.Vector{0, 0, 0})) - test.That(t, err, test.ShouldBeNil) - newFS.AddFrame(baseFrame, worldFrame) - newFS.AddFrame(movementSensorFrame, baseFrame) - - // need to create an injected framesystem service - injectedFS := inject.NewFrameSystemService("fake-FS") - injectedFS.FrameSystemFunc = func(ctx context.Context, - additionalTransforms []*referenceframe.LinkInFrame, - ) (referenceframe.FrameSystem, error) { - return newFS, nil - } - injectedFS.CurrentInputsFunc = func(ctx context.Context) (map[string][]referenceframe.Input, - map[string]referenceframe.InputEnabled, - error, - ) { - return referenceframe.StartPositions(newFS), nil, nil - } + // create motion config + extra := make(map[string]interface{}) + extra["motion_profile"] = "position_only" + extra["timeout"] = 5. - // create the motion service - ms, err := NewBuiltIn( - ctx, - resource.Dependencies{ - fakeBase.Name(): fakeBase, - injectedMovementSensor.Name(): injectedMovementSensor, - injectedFS.Name(): injectedFS, - }, - resource.Config{ - ConvertedAttributes: &Config{}, - }, - logger, - ) - test.That(t, err, test.ShouldBeNil) + dst := geo.NewPoint(gpsPoint.Lat(), gpsPoint.Lng()+1e-5) + expectedDst := r3.Vector{380, 0, 0} + epsilonMM := 15. - gp, _, err := injectedMovementSensor.Position(ctx, nil) - test.That(t, err, test.ShouldBeNil) - destGP := geo.NewPoint(gp.Lat(), gp.Lng()+0.0000009) + motionCfg := &motion.MotionConfiguration{PositionPollingFreqHz: 4, ObstaclePollingFreqHz: 1, PlanDeviationMM: epsilonMM} t.Run("ensure success to a nearby geo point", func(t *testing.T) { t.Parallel() + injectedMovementSensor, _, fakeBase, ms := createMoveOnGlobeEnvironment(ctx, t, gpsPoint, dst) - success, err := ms.MoveOnGlobe( - context.Background(), + moveRequest, err := ms.(*builtIn).newMoveOnGlobeRequest( + ctx, fakeBase.Name(), - destGP, - math.NaN(), + dst, injectedMovementSensor.Name(), - nil, - math.NaN(), - math.NaN(), + []*spatialmath.GeoObstacle{}, motionCfg, + extra, ) test.That(t, err, test.ShouldBeNil) - test.That(t, success, test.ShouldBeTrue) - }) - - t.Run("relative position and distance are calculated properly", func(t *testing.T) { - t.Parallel() + plan, err := moveRequest.plan(ctx) + test.That(t, err, test.ShouldBeNil) + waypoints, err := plan.GetFrameSteps(fakeBase.Name().Name) + test.That(t, err, test.ShouldBeNil) + test.That(t, len(waypoints), test.ShouldEqual, 2) + test.That(t, waypoints[1][0].Value, test.ShouldAlmostEqual, expectedDst.X, epsilonMM) + test.That(t, waypoints[1][1].Value, test.ShouldAlmostEqual, expectedDst.Y, epsilonMM) - localizer, ok := ms.(*builtIn).localizers[injectedMovementSensor.Name()] - test.That(t, ok, test.ShouldBeTrue) - currentPosition, dstPIF, err := ms.(*builtIn).getRelativePositionAndDestination(context.Background(), - localizer, + success, err := ms.MoveOnGlobe( + ctx, fakeBase.Name(), + dst, + 0, injectedMovementSensor.Name(), - *destGP, + []*spatialmath.GeoObstacle{}, + motionCfg, + extra, ) test.That(t, err, test.ShouldBeNil) - test.That(t, currentPosition, test.ShouldResemble, r3.Vector{-10, 0, 0}) - test.That(t, spatialmath.R3VectorAlmostEqual(dstPIF.Pose().Point(), r3.Vector{110, 0, 0}, 0.1), test.ShouldBeTrue) - test.That(t, dstPIF.Parent(), test.ShouldEqual, referenceframe.World) + test.That(t, success, test.ShouldBeTrue) }) t.Run("go around an obstacle", func(t *testing.T) { t.Parallel() + injectedMovementSensor, _, fakeBase, ms := createMoveOnGlobeEnvironment(ctx, t, gpsPoint, dst) boxPose := spatialmath.NewPoseFromPoint(r3.Vector{50, 0, 0}) boxDims := r3.Vector{5, 50, 10} geometries, err := spatialmath.NewBox(boxPose, boxDims, "wall") test.That(t, err, test.ShouldBeNil) - geoObstacle := spatialmath.NewGeoObstacle(geo.NewPoint(0, 0), []spatialmath.Geometry{geometries}) - - localizer, err := motion.NewLocalizer(context.Background(), injectedMovementSensor) - test.That(t, err, test.ShouldBeNil) + geoObstacle := spatialmath.NewGeoObstacle(gpsPoint, []spatialmath.Geometry{geometries}) - currentPosition, dstPIF, err := ms.(*builtIn).getRelativePositionAndDestination(context.Background(), - localizer, + moveRequest, err := ms.(*builtIn).newMoveOnGlobeRequest( + ctx, fakeBase.Name(), + dst, injectedMovementSensor.Name(), - *destGP, + []*spatialmath.GeoObstacle{geoObstacle}, + motionCfg, + extra, ) test.That(t, err, test.ShouldBeNil) + plan, err := moveRequest.plan(ctx) + test.That(t, err, test.ShouldBeNil) + waypoints, err := plan.GetFrameSteps(fakeBase.Name().Name) + test.That(t, err, test.ShouldBeNil) + test.That(t, len(waypoints), test.ShouldBeGreaterThan, 2) + test.That(t, waypoints[len(waypoints)-1][0].Value, test.ShouldAlmostEqual, expectedDst.X, epsilonMM) + test.That(t, waypoints[len(waypoints)-1][1].Value, test.ShouldAlmostEqual, expectedDst.Y, epsilonMM) - plan, _, err := ms.(*builtIn).planMoveOnGlobe(context.Background(), + success, err := ms.MoveOnGlobe( + ctx, fakeBase.Name(), - currentPosition, - dstPIF, - localizer, + dst, + 0, + injectedMovementSensor.Name(), []*spatialmath.GeoObstacle{geoObstacle}, - defaultLinearVelocityMillisPerSec, - defaultAngularVelocityDegsPerSec, motionCfg, + extra, ) - test.That(t, len(plan), test.ShouldBeGreaterThan, 2) test.That(t, err, test.ShouldBeNil) + test.That(t, success, test.ShouldBeTrue) }) t.Run("fail because of obstacle", func(t *testing.T) { t.Parallel() + injectedMovementSensor, _, fakeBase, ms := createMoveOnGlobeEnvironment(ctx, t, gpsPoint, dst) boxPose := spatialmath.NewPoseFromPoint(r3.Vector{50, 0, 0}) - boxDims := r3.Vector{2, 666, 10} + boxDims := r3.Vector{2, 6660, 10} geometries, err := spatialmath.NewBox(boxPose, boxDims, "wall") test.That(t, err, test.ShouldBeNil) - geoObstacle := spatialmath.NewGeoObstacle(geo.NewPoint(0, 0), []spatialmath.Geometry{geometries}) + geoObstacle := spatialmath.NewGeoObstacle(gpsPoint, []spatialmath.Geometry{geometries}) - success, err := ms.MoveOnGlobe( - context.Background(), + moveRequest, err := ms.(*builtIn).newMoveOnGlobeRequest( + ctx, fakeBase.Name(), - destGP, - math.NaN(), + dst, injectedMovementSensor.Name(), []*spatialmath.GeoObstacle{geoObstacle}, - math.NaN(), - math.NaN(), - motionCfg, + &motion.MotionConfiguration{}, + extra, ) + test.That(t, err, test.ShouldBeNil) + plan, err := moveRequest.plan(ctx) test.That(t, err, test.ShouldNotBeNil) - test.That(t, success, test.ShouldBeFalse) + test.That(t, len(plan), test.ShouldEqual, 0) + }) + + t.Run("check offset constructed correctly", func(t *testing.T) { + t.Parallel() + _, fsSvc, _, _ := createMoveOnGlobeEnvironment(ctx, t, gpsPoint, dst) + baseOrigin := referenceframe.NewPoseInFrame("test-base", spatialmath.NewZeroPose()) + movementSensorToBase, err := fsSvc.TransformPose(ctx, baseOrigin, "test-gps", nil) + if err != nil { + movementSensorToBase = baseOrigin + } + test.That(t, movementSensorToBase.Pose().Point(), test.ShouldResemble, r3.Vector{10, 0, 0}) }) } @@ -556,3 +728,199 @@ func TestGetPose(t *testing.T) { test.That(t, err, test.ShouldBeError, referenceframe.NewParentFrameMissingError("testFrame", "noParent")) test.That(t, pose, test.ShouldBeNil) } + +func TestStoppableMoveFunctions(t *testing.T) { + ctx := context.Background() + logger := golog.NewTestLogger(t) + failToReachGoalError := errors.New("failed to reach goal") + calledStopFunc := false + testIfStoppable := func(t *testing.T, success bool, err error) { + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err, test.ShouldEqual, failToReachGoalError) + test.That(t, success, test.ShouldBeFalse) + test.That(t, calledStopFunc, test.ShouldBeTrue) + } + + t.Run("successfully stop arms", func(t *testing.T) { + armName := "test-arm" + injectArmName := arm.Named(armName) + goal := referenceframe.NewPoseInFrame( + armName, + spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: -10, Z: -10}), + ) + + // Create an injected Arm + armCfg := resource.Config{ + Name: armName, + API: arm.API, + Model: resource.DefaultModelFamily.WithModel("ur5e"), + ConvertedAttributes: &armFake.Config{ + ArmModel: "ur5e", + }, + Frame: &referenceframe.LinkConfig{ + Parent: "world", + }, + } + + fakeArm, err := armFake.NewArm(ctx, nil, armCfg, logger) + test.That(t, err, test.ShouldBeNil) + + injectArm := &inject.Arm{ + Arm: fakeArm, + } + injectArm.StopFunc = func(ctx context.Context, extra map[string]interface{}) error { + calledStopFunc = true + return nil + } + injectArm.GoToInputsFunc = func(ctx context.Context, goal []referenceframe.Input) error { + return failToReachGoalError + } + injectArm.ModelFrameFunc = func() referenceframe.Model { + model, _ := ur.MakeModelFrame("ur5e") + return model + } + injectArm.MoveToPositionFunc = func(ctx context.Context, to spatialmath.Pose, extra map[string]interface{}) error { + return failToReachGoalError + } + + // create arm link + armLink := referenceframe.NewLinkInFrame( + referenceframe.World, + spatialmath.NewZeroPose(), + armName, + nil, + ) + + // Create a motion service + fsParts := []*referenceframe.FrameSystemPart{ + { + FrameConfig: armLink, + ModelFrame: injectArm.ModelFrameFunc(), + }, + } + deps := resource.Dependencies{ + injectArmName: injectArm, + } + + _, err = createFrameSystemService(ctx, deps, fsParts, logger) + test.That(t, err, test.ShouldBeNil) + + conf := resource.Config{ConvertedAttributes: &Config{}} + ms, err := NewBuiltIn(ctx, deps, conf, logger) + test.That(t, err, test.ShouldBeNil) + + t.Run("stop during Move(...) call", func(t *testing.T) { + calledStopFunc = false + success, err := ms.Move(ctx, injectArmName, goal, nil, nil, nil) + testIfStoppable(t, success, err) + }) + }) + + t.Run("successfully stop kinematic bases", func(t *testing.T) { + // Create an injected Base + baseName := "test-base" + + geometry, err := (&spatialmath.GeometryConfig{R: 20}).ParseConfig() + test.That(t, err, test.ShouldBeNil) + + injectBase := inject.NewBase(baseName) + injectBase.GeometriesFunc = func(ctx context.Context) ([]spatialmath.Geometry, error) { + return []spatialmath.Geometry{geometry}, nil + } + injectBase.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (base.Properties, error) { + return base.Properties{ + TurningRadiusMeters: 0, + WidthMeters: 600 * 0.001, + }, nil + } + injectBase.StopFunc = func(ctx context.Context, extra map[string]interface{}) error { + calledStopFunc = true + return nil + } + injectBase.SpinFunc = func(ctx context.Context, angleDeg, degsPerSec float64, extra map[string]interface{}) error { + return failToReachGoalError + } + injectBase.MoveStraightFunc = func(ctx context.Context, distanceMm int, mmPerSec float64, extra map[string]interface{}) error { + return failToReachGoalError + } + injectBase.SetVelocityFunc = func(ctx context.Context, linear, angular r3.Vector, extra map[string]interface{}) error { + return failToReachGoalError + } + + // Create a base link + baseLink := createBaseLink(t, baseName) + + t.Run("stop during MoveOnGlobe(...) call", func(t *testing.T) { + calledStopFunc = false + gpsPoint := geo.NewPoint(-70, 40) + + // Create an injected MovementSensor + movementSensorName := "test-gps" + injectMovementSensor := createInjectedMovementSensor(movementSensorName, gpsPoint) + + // Create a MovementSensor link + movementSensorLink := referenceframe.NewLinkInFrame( + baseLink.Name(), + spatialmath.NewPoseFromPoint(r3.Vector{-10, 0, 0}), + movementSensorName, + nil, + ) + + // Create a motion service + fsParts := []*referenceframe.FrameSystemPart{ + {FrameConfig: movementSensorLink}, + {FrameConfig: baseLink}, + } + deps := resource.Dependencies{ + injectBase.Name(): injectBase, + injectMovementSensor.Name(): injectMovementSensor, + } + + _, err := createFrameSystemService(ctx, deps, fsParts, logger) + test.That(t, err, test.ShouldBeNil) + + conf := resource.Config{ConvertedAttributes: &Config{}} + ms, err := NewBuiltIn(ctx, deps, conf, logger) + test.That(t, err, test.ShouldBeNil) + + goal := geo.NewPoint(gpsPoint.Lat()+1e-4, gpsPoint.Lng()+1e-4) + motionCfg := motion.MotionConfiguration{ + PlanDeviationMM: 10000, + LinearMPerSec: 10, + PositionPollingFreqHz: 4, + ObstaclePollingFreqHz: 1, + } + success, err := ms.MoveOnGlobe( + ctx, injectBase.Name(), goal, 0, injectMovementSensor.Name(), + nil, &motionCfg, nil, + ) + testIfStoppable(t, success, err) + }) + + t.Run("stop during MoveOnMap(...) call", func(t *testing.T) { + calledStopFunc = false + slamName := "test-slam" + + // Create an injected SLAM + injectSlam := createInjectedSlam(slamName, "pointcloud/octagonspace.pcd") + + // Create a motion service + deps := resource.Dependencies{ + injectBase.Name(): injectBase, + injectSlam.Name(): injectSlam, + } + + ms, err := NewBuiltIn( + ctx, + deps, + resource.Config{ConvertedAttributes: &Config{}}, + logger, + ) + test.That(t, err, test.ShouldBeNil) + + goal := spatialmath.NewPoseFromPoint(r3.Vector{X: 1.32 * 1000, Y: 0}) + success, err := ms.MoveOnMap(ctx, injectBase.Name(), goal, injectSlam.Name(), nil) + testIfStoppable(t, success, err) + }) + }) +} diff --git a/services/motion/builtin/move_attempt.go b/services/motion/builtin/move_attempt.go new file mode 100644 index 00000000000..2e4f8acbd91 --- /dev/null +++ b/services/motion/builtin/move_attempt.go @@ -0,0 +1,82 @@ +package builtin + +import ( + "context" + "sync" + + goutils "go.viam.com/utils" + + "go.viam.com/rdk/utils" +) + +// moveResponse is a struct that is used to communicate the outcome of a moveAttempt. +type moveResponse struct { + err error + success bool +} + +// moveAttempt is a struct whose lifetime lasts the duration of an attempt to complete a moveRequest +// it contains a context in which the move call executes and tracks the goroutines that it spawns. +type moveAttempt struct { + ctx context.Context + cancelFn context.CancelFunc + backgroundWorkers *sync.WaitGroup + + request *moveRequest + responseChan chan moveResponse +} + +// newMoveAttempt instantiates a moveAttempt which can later be started. +// The caller of this function is expected to also call the cancel function to clean up after instantiation. +func newMoveAttempt(ctx context.Context, request *moveRequest) *moveAttempt { + cancelCtx, cancelFn := context.WithCancel(ctx) + var backgroundWorkers sync.WaitGroup + + return &moveAttempt{ + ctx: cancelCtx, + cancelFn: cancelFn, + backgroundWorkers: &backgroundWorkers, + + request: request, + responseChan: make(chan moveResponse), + } +} + +// start begins a new moveAttempt by using its moveRequest to create a plan, spawn relevant replanners, and finally execute the motion. +// the caller of this function should monitor the moveAttempt's responseChan as well as the replanners' responseChan to get insight +// into the status of the moveAttempt. +func (ma *moveAttempt) start() error { + plan, err := ma.request.plan(ma.ctx) + if err != nil { + return err + } + + ma.backgroundWorkers.Add(1) + goutils.ManagedGo(func() { + ma.request.position.startPolling(ma.ctx) + }, ma.backgroundWorkers.Done) + + ma.backgroundWorkers.Add(1) + goutils.ManagedGo(func() { + ma.request.obstacle.startPolling(ma.ctx) + }, ma.backgroundWorkers.Done) + + // spawn function to execute the plan on the robot + ma.backgroundWorkers.Add(1) + goutils.ManagedGo(func() { + if resp := ma.request.execute(ma.ctx, plan); resp.success || resp.err != nil { + ma.responseChan <- resp + } + }, ma.backgroundWorkers.Done) + return nil +} + +// cancel cleans up a moveAttempt +// it cancels the processes spawned by it, drains all the channels that could have been written to and waits on processes to return. +func (ma *moveAttempt) cancel() { + ma.cancelFn() + utils.FlushChan(ma.request.position.responseChan) + utils.FlushChan(ma.request.obstacle.responseChan) + utils.FlushChan(ma.responseChan) + ma.backgroundWorkers.Wait() +} diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go new file mode 100644 index 00000000000..81213b334a6 --- /dev/null +++ b/services/motion/builtin/move_request.go @@ -0,0 +1,217 @@ +package builtin + +import ( + "context" + "fmt" + "math" + "time" + + geo "github.com/kellydunn/golang-geo" + "github.com/pkg/errors" + + "go.viam.com/rdk/components/base" + "go.viam.com/rdk/components/base/kinematicbase" + "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/spatialmath" +) + +// moveRequest is a structure that contains all the information necessary for to make a move call. +type moveRequest struct { + config *motion.MotionConfiguration + + planRequest *motionplan.PlanRequest + kinematicBase kinematicbase.KinematicBase + position, obstacle *replanner +} + +// plan creates a plan using the currentInputs of the robot and the moveRequest's planRequest. +func (mr *moveRequest) plan(ctx context.Context) (motionplan.Plan, error) { + inputs, err := mr.kinematicBase.CurrentInputs(ctx) + if err != nil { + return nil, err + } + // TODO: this is really hacky and we should figure out a better place to store this information + if len(mr.kinematicBase.Kinematics().DoF()) == 2 { + inputs = inputs[:2] + } + mr.planRequest.StartConfiguration = map[string][]referenceframe.Input{mr.kinematicBase.Kinematics().Name(): inputs} + return motionplan.PlanMotion(ctx, mr.planRequest) +} + +func (mr *moveRequest) execute(ctx context.Context, plan motionplan.Plan) moveResponse { + waypoints, err := plan.GetFrameSteps(mr.kinematicBase.Kinematics().Name()) + if err != nil { + return moveResponse{err: err} + } + + // Iterate through the list of waypoints and issue a command to move to each + for i := 1; i < len(waypoints); i++ { + select { + case <-ctx.Done(): + return moveResponse{} + default: + mr.planRequest.Logger.Info(waypoints[i]) + if err := mr.kinematicBase.GoToInputs(ctx, waypoints[i]); err != nil { + // If there is an error on GoToInputs, stop the component if possible before returning the error + if stopErr := mr.kinematicBase.Stop(ctx, nil); stopErr != nil { + return moveResponse{err: errors.Wrap(err, stopErr.Error())} + } + // If the error was simply a cancellation of context return without erroring out + if errors.Is(err, context.Canceled) { + return moveResponse{} + } + return moveResponse{err: err} + } + } + } + + // the plan has been fully executed so check to see if the GeoPoint we are at is close enough to the goal. + errorState, err := mr.kinematicBase.ErrorState(ctx, waypoints, len(waypoints)-1) + if err != nil { + return moveResponse{err: err} + } + if errorState.Point().Norm() <= mr.config.PlanDeviationMM { + return moveResponse{success: true} + } + return moveResponse{err: errors.New("reached end of plan but not at goal")} +} + +// newMoveOnGlobeRequest instantiates a moveRequest intended to be used in the context of a MoveOnGlobe call. +func (ms *builtIn) newMoveOnGlobeRequest( + ctx context.Context, + componentName resource.Name, + destination *geo.Point, + movementSensorName resource.Name, + obstacles []*spatialmath.GeoObstacle, + motionCfg *motion.MotionConfiguration, + extra map[string]interface{}, +) (*moveRequest, error) { + // build kinematic options + kinematicsOptions := kinematicbase.NewKinematicBaseOptions() + if motionCfg.LinearMPerSec != 0 { + kinematicsOptions.LinearVelocityMMPerSec = motionCfg.LinearMPerSec * 1000 + } + if motionCfg.AngularDegsPerSec != 0 { + kinematicsOptions.AngularVelocityDegsPerSec = motionCfg.AngularDegsPerSec + } + if motionCfg.PlanDeviationMM != 0 { + kinematicsOptions.PlanDeviationThresholdMM = motionCfg.PlanDeviationMM + } + kinematicsOptions.GoalRadiusMM = motionCfg.PlanDeviationMM + kinematicsOptions.HeadingThresholdDegrees = 8 + + // build the localizer from the movement sensor + movementSensor, ok := ms.movementSensors[movementSensorName] + if !ok { + return nil, resource.DependencyNotFoundError(movementSensorName) + } + origin, _, err := movementSensor.Position(ctx, nil) + if err != nil { + return nil, err + } + + // add an offset between the movement sensor and the base if it is applicable + baseOrigin := referenceframe.NewPoseInFrame(componentName.ShortName(), 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 + movementSensorToBase = baseOrigin + } + localizer := motion.NewMovementSensorLocalizer(movementSensor, origin, movementSensorToBase.Pose()) + + // convert destination into spatialmath.Pose with respect to where the localizer was initialized + goal := spatialmath.GeoPointToPose(destination, origin) + + // convert GeoObstacles into GeometriesInFrame with respect to the base's starting point + geoms := spatialmath.GeoObstaclesToGeometries(obstacles, origin) + + gif := referenceframe.NewGeometriesInFrame(referenceframe.World, geoms) + worldState, err := referenceframe.NewWorldState([]*referenceframe.GeometriesInFrame{gif}, nil) + if err != nil { + return nil, err + } + + // construct limits + straightlineDistance := goal.Point().Norm() + if straightlineDistance > maxTravelDistanceMM { + return nil, fmt.Errorf("cannot move more than %d kilometers", int(maxTravelDistanceMM*1e-6)) + } + limits := []referenceframe.Limit{ + {Min: -straightlineDistance * 3, Max: straightlineDistance * 3}, + {Min: -straightlineDistance * 3, Max: straightlineDistance * 3}, + {Min: -2 * math.Pi, Max: 2 * math.Pi}, + } + ms.logger.Debugf("base limits: %v", limits) + + if extra != nil { + if profile, ok := extra["motion_profile"]; ok { + motionProfile, ok := profile.(string) + if !ok { + return nil, errors.New("could not interpret motion_profile field as string") + } + kinematicsOptions.PositionOnlyMode = motionProfile == motionplan.PositionOnlyMotionProfile + } + } + + // create a KinematicBase from the componentName + baseComponent, ok := ms.components[componentName] + if !ok { + return nil, resource.NewNotFoundError(componentName) + } + b, ok := baseComponent.(base.Base) + if !ok { + return nil, fmt.Errorf("cannot move component of type %T because it is not a Base", baseComponent) + } + + kb, err := kinematicbase.WrapWithKinematics(ctx, b, ms.logger, localizer, limits, kinematicsOptions) + if err != nil { + return nil, err + } + + // create a new empty framesystem which we add the kinematic base to + fs := referenceframe.NewEmptyFrameSystem("") + kbf := kb.Kinematics() + if err := fs.AddFrame(kbf, fs.World()); err != nil { + return nil, err + } + + // TODO(RSDK-3407): this does not adequately account for geometries right now since it is a transformation after the fact. + // This is probably acceptable for the time being, but long term the construction of the frame system for the kinematic base should + // be moved under the purview of the kinematic base wrapper instead of being done here. + offsetFrame, err := referenceframe.NewStaticFrame("offset", movementSensorToBase.Pose()) + if err != nil { + return nil, err + } + if err := fs.AddFrame(offsetFrame, kbf); err != nil { + return nil, err + } + + return &moveRequest{ + config: motionCfg, + planRequest: &motionplan.PlanRequest{ + Logger: ms.logger, + Goal: referenceframe.NewPoseInFrame(referenceframe.World, goal), + Frame: offsetFrame, + FrameSystem: fs, + StartConfiguration: referenceframe.StartPositions(fs), + WorldState: worldState, + Options: extra, + }, + kinematicBase: kb, + position: newReplanner( + time.Duration(1000/motionCfg.PositionPollingFreqHz)*time.Millisecond, + func(ctx context.Context) replanResponse { + return replanResponse{} + }, + ), + obstacle: newReplanner( + time.Duration(1000/motionCfg.ObstaclePollingFreqHz)*time.Millisecond, + func(ctx context.Context) replanResponse { + return replanResponse{} + }, + ), + }, nil +} diff --git a/services/motion/builtin/replanner.go b/services/motion/builtin/replanner.go new file mode 100644 index 00000000000..81c19340e94 --- /dev/null +++ b/services/motion/builtin/replanner.go @@ -0,0 +1,52 @@ +package builtin + +import ( + "context" + "time" +) + +// replanResponse is the struct returned by the replanner. +type replanResponse struct { + err error + replan bool +} + +// replanner bundles everything needed to execute a function at a given interval and return. +type replanner struct { + period time.Duration + fnToPoll func(ctx context.Context) replanResponse + responseChan chan replanResponse +} + +// newReplanner is a constructor. +func newReplanner(period time.Duration, fnToPoll func(context.Context) replanResponse) *replanner { + return &replanner{ + period: period, + fnToPoll: fnToPoll, + responseChan: make(chan replanResponse), + } +} + +// startPolling executes the replanner's configured function at its configured period +// The caller of this function should read from the replanner's responseChan to know when a replan is requested. +func (r *replanner) startPolling(ctx context.Context) { + ticker := time.NewTicker(r.period) + defer ticker.Stop() + for { + // this ensures that if the context is cancelled we always return early at the top of the loop + if ctx.Err() != nil { + return + } + + select { + case <-ctx.Done(): + return + case <-ticker.C: + response := r.fnToPoll(ctx) + if response.err != nil || response.replan { + r.responseChan <- response + return + } + } + } +} diff --git a/services/motion/builtin/verify_main_test.go b/services/motion/builtin/verify_main_test.go new file mode 100644 index 00000000000..c8e5bbecc77 --- /dev/null +++ b/services/motion/builtin/verify_main_test.go @@ -0,0 +1,12 @@ +package builtin + +import ( + "testing" + + testutilsext "go.viam.com/utils/testutils/ext" +) + +// TestMain is used to control the execution of all tests run within this package (including _test packages). +func TestMain(m *testing.M) { + testutilsext.VerifyTestMain(m) +} diff --git a/services/motion/client.go b/services/motion/client.go index 16c3903ed2c..3aff1c766ff 100644 --- a/services/motion/client.go +++ b/services/motion/client.go @@ -1,4 +1,3 @@ -// Package motion contains a gRPC based motion client package motion import ( @@ -108,8 +107,7 @@ func (c *client) MoveOnGlobe( heading float64, movementSensorName resource.Name, obstacles []*spatialmath.GeoObstacle, - linearVelocity float64, - angularVelocity float64, + motionCfg *MotionConfiguration, extra map[string]interface{}, ) (bool, error) { ext, err := vprotoutils.StructToStructPb(extra) @@ -122,11 +120,12 @@ func (c *client) MoveOnGlobe( } req := &pb.MoveOnGlobeRequest{ - Name: c.name, - ComponentName: protoutils.ResourceNameToProto(componentName), - Destination: &commonpb.GeoPoint{Latitude: destination.Lat(), Longitude: destination.Lng()}, - MovementSensorName: protoutils.ResourceNameToProto(movementSensorName), - Extra: ext, + Name: c.name, + ComponentName: protoutils.ResourceNameToProto(componentName), + Destination: &commonpb.GeoPoint{Latitude: destination.Lat(), Longitude: destination.Lng()}, + MovementSensorName: protoutils.ResourceNameToProto(movementSensorName), + MotionConfiguration: &pb.MotionConfiguration{}, + Extra: ext, } // Optionals @@ -135,57 +134,42 @@ func (c *client) MoveOnGlobe( } if len(obstacles) > 0 { obstaclesProto := make([]*commonpb.GeoObstacle, 0, len(obstacles)) - for _, eachObst := range obstacles { - convObst, err := spatialmath.GeoObstacleToProtobuf(eachObst) - if err != nil { - return false, err - } - obstaclesProto = append(obstaclesProto, convObst) + for _, obstacle := range obstacles { + obstaclesProto = append(obstaclesProto, spatialmath.GeoObstacleToProtobuf(obstacle)) } req.Obstacles = obstaclesProto } - reqLinear := float32(linearVelocity) - if !math.IsNaN(linearVelocity) { - req.LinearMetersPerSec = &reqLinear + + if !math.IsNaN(motionCfg.LinearMPerSec) && motionCfg.LinearMPerSec != 0 { + req.MotionConfiguration.LinearMPerSec = &motionCfg.LinearMPerSec } - reqAngular := float32(angularVelocity) - if !math.IsNaN(angularVelocity) { - req.AngularDegPerSec = &reqAngular + if !math.IsNaN(motionCfg.AngularDegsPerSec) && motionCfg.AngularDegsPerSec != 0 { + req.MotionConfiguration.AngularDegsPerSec = &motionCfg.AngularDegsPerSec } - - resp, err := c.client.MoveOnGlobe(ctx, req) - if err != nil { - return false, err + if !math.IsNaN(motionCfg.ObstaclePollingFreqHz) && motionCfg.ObstaclePollingFreqHz > 0 { + req.MotionConfiguration.ObstaclePollingFrequencyHz = &motionCfg.ObstaclePollingFreqHz } - - return resp.Success, nil -} - -func (c *client) MoveSingleComponent( - ctx context.Context, - componentName resource.Name, - destination *referenceframe.PoseInFrame, - worldState *referenceframe.WorldState, - extra map[string]interface{}, -) (bool, error) { - ext, err := vprotoutils.StructToStructPb(extra) - if err != nil { - return false, err + if !math.IsNaN(motionCfg.PositionPollingFreqHz) && motionCfg.PositionPollingFreqHz > 0 { + req.MotionConfiguration.PositionPollingFrequencyHz = &motionCfg.PositionPollingFreqHz } - worldStateMsg, err := worldState.ToProtobuf() - if err != nil { - return false, err + if !math.IsNaN(motionCfg.PlanDeviationMM) && motionCfg.PlanDeviationMM >= 0 { + planDeviationM := 1e-3 * motionCfg.PlanDeviationMM + req.MotionConfiguration.PlanDeviationM = &planDeviationM } - resp, err := c.client.MoveSingleComponent(ctx, &pb.MoveSingleComponentRequest{ - Name: c.name, - ComponentName: protoutils.ResourceNameToProto(componentName), - Destination: referenceframe.PoseInFrameToProtobuf(destination), - WorldState: worldStateMsg, - Extra: ext, - }) + + if len(motionCfg.VisionServices) > 0 { + svcs := []*commonpb.ResourceName{} + for _, name := range motionCfg.VisionServices { + svcs = append(svcs, protoutils.ResourceNameToProto(name)) + } + req.MotionConfiguration.VisionServices = svcs + } + + resp, err := c.client.MoveOnGlobe(ctx, req) if err != nil { return false, err } + return resp.Success, nil } diff --git a/services/motion/client_test.go b/services/motion/client_test.go index 6e97d26db25..391836e2c3e 100644 --- a/services/motion/client_test.go +++ b/services/motion/client_test.go @@ -100,8 +100,7 @@ func TestClient(t *testing.T) { heading float64, movementSensorName resource.Name, obstacles []*spatialmath.GeoObstacle, - linearVelocity float64, - angularVelocity float64, + motionCfg *motion.MotionConfiguration, extra map[string]interface{}, ) (bool, error) { return false, errors.New("Not yet implemented") @@ -126,7 +125,7 @@ func TestClient(t *testing.T) { test.That(t, result, test.ShouldEqual, success) // MoveOnGlobe - globeResult, err := client.MoveOnGlobe(ctx, baseName, globeDest, math.NaN(), gpsName, nil, math.NaN(), math.NaN(), nil) + globeResult, err := client.MoveOnGlobe(ctx, baseName, globeDest, math.NaN(), gpsName, nil, &motion.MotionConfiguration{}, nil) test.That(t, err, test.ShouldNotBeNil) test.That(t, err.Error(), test.ShouldContainSubstring, notYetImplementedErr.Error()) test.That(t, globeResult, test.ShouldEqual, false) @@ -196,8 +195,7 @@ func TestClient(t *testing.T) { heading float64, movementSensorName resource.Name, obstacles []*spatialmath.GeoObstacle, - linearVelocity float64, - angularVelocity float64, + motionCfg *motion.MotionConfiguration, extra map[string]interface{}, ) (bool, error) { return false, passedErr @@ -219,7 +217,7 @@ func TestClient(t *testing.T) { test.That(t, resp, test.ShouldEqual, false) // MoveOnGlobe - resp, err = client2.MoveOnGlobe(ctx, baseName, globeDest, math.NaN(), gpsName, nil, math.NaN(), math.NaN(), nil) + resp, err = client2.MoveOnGlobe(ctx, baseName, globeDest, math.NaN(), gpsName, nil, &motion.MotionConfiguration{}, nil) test.That(t, err, test.ShouldNotBeNil) test.That(t, err.Error(), test.ShouldContainSubstring, passedErr.Error()) test.That(t, resp, test.ShouldEqual, false) diff --git a/services/motion/data/wheeled_base.json b/services/motion/data/fake_wheeled_base.json similarity index 100% rename from services/motion/data/wheeled_base.json rename to services/motion/data/fake_wheeled_base.json diff --git a/services/motion/data/real_wheeled_base.json b/services/motion/data/real_wheeled_base.json new file mode 100644 index 00000000000..f0ad6ba8ff5 --- /dev/null +++ b/services/motion/data/real_wheeled_base.json @@ -0,0 +1,90 @@ +{ + "components": [ + { + "name": "test_base", + "type": "base", + "model": "wheeled", + "attributes": { + "wheel_circumference_mm": 217, + "left": [ + "fake-left" + ], + "right": [ + "fake-right" + ], + "width_mm": 260, + "spin_slip_factor": 1.76 + }, + "depends_on": [], + "frame": { + "parent": "world", + "translation": { + "x": 0, + "y": 0, + "z": 0 + }, + "orientation": { + "type": "ov_degrees", + "value": { + "x": 0, + "y": 0, + "z": 1, + "th": 0 + } + }, + "geometry": { + "r": 20, + "translation": { + "x": 0, + "y": 0, + "z": 0 + } + } + } + }, + { + "name": "fake-left", + "type": "motor", + "model": "fake", + "attributes": { + "pins": { + "dir": "", + "pwm": "" + }, + "board": "", + "max_rpm": 1 + }, + "depends_on": [] + }, + { + "name": "fake-right", + "type": "motor", + "model": "fake", + "attributes": { + "pins": { + "dir": "", + "pwm": "" + }, + "board": "", + "max_rpm": 1 + }, + "depends_on": [] + }, + { + "name": "fake-board", + "type": "board", + "model": "fake", + "attributes": { + "fail_new": false + }, + "depends_on": [] + } + ], + "services": [ + { + "model": "fake", + "name": "test_slam", + "type": "slam" + } + ] +} diff --git a/services/motion/localizer.go b/services/motion/localizer.go index bdc24e0b8b4..f3a5527de80 100644 --- a/services/motion/localizer.go +++ b/services/motion/localizer.go @@ -2,30 +2,20 @@ package motion import ( "context" - "fmt" + "math" + + geo "github.com/kellydunn/golang-geo" + "github.com/pkg/errors" "go.viam.com/rdk/components/movementsensor" "go.viam.com/rdk/referenceframe" - "go.viam.com/rdk/resource" "go.viam.com/rdk/services/slam" "go.viam.com/rdk/spatialmath" ) // Localizer is an interface which both slam and movementsensor can satisfy when wrapped respectively. type Localizer interface { - CurrentPosition(context.Context) (referenceframe.PoseInFrame, error) -} - -// NewLocalizer constructs either a slamLocalizer or movementSensorLocalizer from the given resource. -func NewLocalizer(ctx context.Context, res resource.Resource) (Localizer, error) { - switch res := res.(type) { - case slam.Service: - return &slamLocalizer{Service: res}, nil - case movementsensor.MovementSensor: - return &movementSensorLocalizer{MovementSensor: res}, nil - default: - return nil, fmt.Errorf("cannot localize on resource of type %T", res) - } + CurrentPosition(context.Context) (*referenceframe.PoseInFrame, error) } // slamLocalizer is a struct which only wraps an existing slam service. @@ -33,28 +23,63 @@ type slamLocalizer struct { slam.Service } +// NewSLAMLocalizer creates a new Localizer that relies on a slam service to report Pose. +func NewSLAMLocalizer(slam slam.Service) Localizer { + return &slamLocalizer{Service: slam} +} + // CurrentPosition returns slam's current position. -func (s slamLocalizer) CurrentPosition(ctx context.Context) (referenceframe.PoseInFrame, error) { - var pif referenceframe.PoseInFrame - pose, _, err := s.GetPosition(ctx) +func (s *slamLocalizer) CurrentPosition(ctx context.Context) (*referenceframe.PoseInFrame, error) { + pose, _, err := s.Position(ctx) if err != nil { - return pif, err + return nil, err } - return *referenceframe.NewPoseInFrame(referenceframe.World, pose), err + return referenceframe.NewPoseInFrame(referenceframe.World, pose), err } // movementSensorLocalizer is a struct which only wraps an existing movementsensor. type movementSensorLocalizer struct { movementsensor.MovementSensor + origin *geo.Point + calibration spatialmath.Pose +} + +// NewMovementSensorLocalizer creates a Localizer from a MovementSensor. +// An origin point must be specified and the localizer will return Poses relative to this point. +// A calibration pose can also be specified, which will adjust the location after it is calculated relative to the origin. +func NewMovementSensorLocalizer(ms movementsensor.MovementSensor, origin *geo.Point, calibration spatialmath.Pose) Localizer { + return &movementSensorLocalizer{MovementSensor: ms, origin: origin, calibration: calibration} } // CurrentPosition returns a movementsensor's current position. -func (m movementSensorLocalizer) CurrentPosition(ctx context.Context) (referenceframe.PoseInFrame, error) { - var pif referenceframe.PoseInFrame +func (m *movementSensorLocalizer) CurrentPosition(ctx context.Context) (*referenceframe.PoseInFrame, error) { gp, _, err := m.Position(ctx, nil) if err != nil { - return pif, err + return nil, err + } + var o spatialmath.Orientation + properties, err := m.Properties(ctx, nil) + if err != nil { + return nil, err } - pose := spatialmath.GeoPointToPose(gp) - return *referenceframe.NewPoseInFrame(m.Name().Name, pose), nil + switch { + case properties.CompassHeadingSupported: + heading, err := m.CompassHeading(ctx, nil) + if err != nil { + return nil, err + } + o = &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: heading} + case properties.OrientationSupported: + o, err = m.Orientation(ctx, nil) + if err != nil { + return nil, err + } + default: + return nil, errors.New("could not get orientation from Localizer") + } + + pose := spatialmath.NewPose(spatialmath.GeoPointToPose(gp, m.origin).Point(), o) + alignEast := spatialmath.NewPoseFromOrientation(&spatialmath.OrientationVector{OZ: 1, Theta: -math.Pi / 2}) + correction := spatialmath.Compose(m.calibration, alignEast) + return referenceframe.NewPoseInFrame(m.Name().Name, spatialmath.Compose(pose, correction)), nil } diff --git a/services/motion/motion.go b/services/motion/motion.go index 17c7a120532..2fd4c2e6b36 100644 --- a/services/motion/motion.go +++ b/services/motion/motion.go @@ -1,4 +1,4 @@ -// Package motion implements an motion service. +// Package motion is the service that allows you to plan and execute movements. package motion import ( @@ -47,15 +47,7 @@ type Service interface { heading float64, movementSensorName resource.Name, obstacles []*spatialmath.GeoObstacle, - linearVelocity float64, - angularVelocity float64, - extra map[string]interface{}, - ) (bool, error) - MoveSingleComponent( - ctx context.Context, - componentName resource.Name, - destination *referenceframe.PoseInFrame, - worldState *referenceframe.WorldState, + motionConfig *MotionConfiguration, extra map[string]interface{}, ) (bool, error) GetPose( @@ -67,6 +59,18 @@ type Service interface { ) (*referenceframe.PoseInFrame, error) } +// MotionConfiguration specifies how to configure a call +// +//nolint:revive +type MotionConfiguration struct { + VisionServices []resource.Name + PositionPollingFreqHz float64 + ObstaclePollingFreqHz float64 + PlanDeviationMM float64 + LinearMPerSec float64 + AngularDegsPerSec float64 +} + // SubtypeName is the name of the type of service. const SubtypeName = "motion" diff --git a/services/motion/register/register.go b/services/motion/register/register.go index c7d7839fb2f..8f2b9b324a3 100644 --- a/services/motion/register/register.go +++ b/services/motion/register/register.go @@ -1,4 +1,4 @@ -// Package register registers all relevant motion services and also API specific functions +// Package register registers all relevant motion services and API specific functions. package register import ( diff --git a/services/motion/server.go b/services/motion/server.go index 76aded55538..defe8e08afe 100644 --- a/services/motion/server.go +++ b/services/motion/server.go @@ -1,4 +1,3 @@ -// Package motion contains a gRPC based motion service server package motion import ( @@ -86,14 +85,7 @@ func (server *serviceServer) MoveOnGlobe(ctx context.Context, req *pb.MoveOnGlob } obstacles = append(obstacles, convObst) } - linear := math.NaN() - if req.LinearMetersPerSec != nil { - linear = float64(req.GetLinearMetersPerSec()) - } - angular := math.NaN() - if req.AngularDegPerSec != nil { - angular = float64(req.GetAngularDegPerSec()) - } + motionCfg := setupMotionConfiguration(req.MotionConfiguration) success, err := svc.MoveOnGlobe( ctx, @@ -102,33 +94,51 @@ func (server *serviceServer) MoveOnGlobe(ctx context.Context, req *pb.MoveOnGlob heading, protoutils.ResourceNameFromProto(req.GetMovementSensorName()), obstacles, - linear, - angular, + &motionCfg, req.Extra.AsMap(), ) return &pb.MoveOnGlobeResponse{Success: success}, err } -func (server *serviceServer) MoveSingleComponent( - ctx context.Context, - req *pb.MoveSingleComponentRequest, -) (*pb.MoveSingleComponentResponse, error) { - svc, err := server.coll.Resource(req.Name) - if err != nil { - return nil, err +func setupMotionConfiguration(motionCfg *pb.MotionConfiguration) MotionConfiguration { + visionSvc := []resource.Name{} + planDeviationM := 0. + positionPollingHz := 0. + obstaclePollingHz := 0. + linearMPerSec := 0. + angularDegsPerSec := 0. + + if motionCfg != nil { + if motionCfg.VisionServices != nil { + for _, name := range motionCfg.GetVisionServices() { + visionSvc = append(visionSvc, protoutils.ResourceNameFromProto(name)) + } + } + if motionCfg.PositionPollingFrequencyHz != nil { + positionPollingHz = motionCfg.GetPositionPollingFrequencyHz() + } + if motionCfg.ObstaclePollingFrequencyHz != nil { + obstaclePollingHz = motionCfg.GetObstaclePollingFrequencyHz() + } + if motionCfg.PlanDeviationM != nil { + planDeviationM = motionCfg.GetPlanDeviationM() + } + if motionCfg.LinearMPerSec != nil { + linearMPerSec = motionCfg.GetLinearMPerSec() + } + if motionCfg.AngularDegsPerSec != nil { + angularDegsPerSec = motionCfg.GetAngularDegsPerSec() + } } - worldState, err := referenceframe.WorldStateFromProtobuf(req.GetWorldState()) - if err != nil { - return nil, err + + return MotionConfiguration{ + VisionServices: visionSvc, + PositionPollingFreqHz: positionPollingHz, + ObstaclePollingFreqHz: obstaclePollingHz, + PlanDeviationMM: 1e3 * planDeviationM, + LinearMPerSec: linearMPerSec, + AngularDegsPerSec: angularDegsPerSec, } - success, err := svc.MoveSingleComponent( - ctx, - protoutils.ResourceNameFromProto(req.GetComponentName()), - referenceframe.ProtobufToPoseInFrame(req.GetDestination()), - worldState, - req.Extra.AsMap(), - ) - return &pb.MoveSingleComponentResponse{Success: success}, err } func (server *serviceServer) GetPose(ctx context.Context, req *pb.GetPoseRequest) (*pb.GetPoseResponse, error) { diff --git a/services/motion/server_test.go b/services/motion/server_test.go index 3a6feffbf33..bb97f67a8ba 100644 --- a/services/motion/server_test.go +++ b/services/motion/server_test.go @@ -126,8 +126,7 @@ func TestServerMoveOnGlobe(t *testing.T) { heading float64, movementSensorName resource.Name, obstacles []*spatialmath.GeoObstacle, - linearVelocity float64, - angularVelocity float64, + motionCfg *motion.MotionConfiguration, extra map[string]interface{}, ) (bool, error) { return false, notYetImplementedErr diff --git a/services/navigation/builtin/builtin.go b/services/navigation/builtin/builtin.go index 8e925ecd360..983242f6106 100644 --- a/services/navigation/builtin/builtin.go +++ b/services/navigation/builtin/builtin.go @@ -1,15 +1,14 @@ -// Package builtin contains the default navigation service, along with a gRPC server and client +// Package builtin implements a navigation service. package builtin import ( "context" "encoding/json" - "fmt" "math" "sync" - "time" "github.com/edaniels/golog" + "github.com/golang/geo/r3" geo "github.com/kellydunn/golang-geo" "github.com/pkg/errors" "go.mongodb.org/mongo-driver/bson/primitive" @@ -20,13 +19,25 @@ import ( "go.viam.com/rdk/resource" "go.viam.com/rdk/services/motion" "go.viam.com/rdk/services/navigation" + "go.viam.com/rdk/services/vision" "go.viam.com/rdk/spatialmath" rdkutils "go.viam.com/rdk/utils" ) const ( - metersPerSecDefault = 0.5 - degPerSecDefault = 45 + defaultLinearVelocityMPerSec = 0.5 + defaultAngularVelocityDegsPerSec = 45 + + // how far off the path must the robot be to trigger replanning. + defaultPlanDeviationM = 1e9 + + // the allowable quality change between the new plan and the remainder + // of the original plan. + defaultReplanCostFactor = 1. + + // frequency measured in hertz. + defaultObstaclePollingFrequencyHz = 2. + defaultPositionPollingFrequencyHz = 2. ) func init() { @@ -55,10 +66,17 @@ type Config struct { BaseName string `json:"base"` MovementSensorName string `json:"movement_sensor"` MotionServiceName string `json:"motion_service"` + VisionServices []string `json:"vision_services"` + // DegPerSec and MetersPerSec are targets and not hard limits on speed - DegPerSec float64 `json:"degs_per_sec"` - MetersPerSec float64 `json:"meters_per_sec"` - Obstacles []*spatialmath.GeoObstacleConfig `json:"obstacles,omitempty"` + DegPerSec float64 `json:"degs_per_sec,omitempty"` + MetersPerSec float64 `json:"meters_per_sec,omitempty"` + + Obstacles []*spatialmath.GeoObstacleConfig `json:"obstacles,omitempty"` + PositionPollingFrequencyHz float64 `json:"position_polling_frequency_hz,omitempty"` + ObstaclePollingFrequencyHz float64 `json:"obstacle_polling_frequency_hz,omitempty"` + PlanDeviationM float64 `json:"plan_deviation_m,omitempty"` + ReplanCostFactor float64 `json:"replan_cost_factor,omitempty"` } // Validate creates the list of implicit dependencies. @@ -80,12 +98,37 @@ func (conf *Config) Validate(path string) ([]string, error) { } deps = append(deps, resource.NewName(motion.API, conf.MotionServiceName).String()) + for _, v := range conf.VisionServices { + deps = append(deps, resource.NewName(vision.API, v).String()) + } + // get default speeds from config if set, else defaults from nav services const if conf.MetersPerSec == 0 { - conf.MetersPerSec = metersPerSecDefault + conf.MetersPerSec = defaultLinearVelocityMPerSec } if conf.DegPerSec == 0 { - conf.DegPerSec = degPerSecDefault + conf.DegPerSec = defaultAngularVelocityDegsPerSec + } + if conf.PositionPollingFrequencyHz == 0 { + conf.PositionPollingFrequencyHz = defaultPositionPollingFrequencyHz + } + if conf.ObstaclePollingFrequencyHz == 0 { + conf.ObstaclePollingFrequencyHz = defaultObstaclePollingFrequencyHz + } + if conf.PlanDeviationM == 0 { + conf.PlanDeviationM = defaultPlanDeviationM + } + if conf.ReplanCostFactor == 0 { + conf.ReplanCostFactor = defaultReplanCostFactor + } + + // ensure obstacles have no translation + for _, obs := range conf.Obstacles { + for _, geoms := range obs.Geometries { + if !geoms.TranslationOffset.ApproxEqual(r3.Vector{}) { + return nil, errors.New("geometries specified through the navigation are not allowed to have a translation") + } + } } return deps, nil @@ -93,12 +136,9 @@ func (conf *Config) Validate(path string) ([]string, error) { // NewBuiltIn returns a new navigation service for the given robot. func NewBuiltIn(ctx context.Context, deps resource.Dependencies, conf resource.Config, logger golog.Logger) (navigation.Service, error) { - cancelCtx, cancelFunc := context.WithCancel(context.Background()) navSvc := &builtIn{ - Named: conf.ResourceName().AsNamed(), - logger: logger, - cancelCtx: cancelCtx, - cancelFunc: cancelFunc, + Named: conf.ResourceName().AsNamed(), + logger: logger, } if err := navSvc.Reconfigure(ctx, deps, conf); err != nil { return nil, err @@ -108,6 +148,7 @@ func NewBuiltIn(ctx context.Context, deps resource.Dependencies, conf resource.C type builtIn struct { resource.Named + actionMu sync.RWMutex mu sync.RWMutex store navigation.NavStore storeType string @@ -118,17 +159,24 @@ type builtIn struct { motion motion.Service obstacles []*spatialmath.GeoObstacle - metersPerSec float64 - degPerSec float64 - logger golog.Logger - cancelCtx context.Context - cancelFunc func() - activeBackgroundWorkers sync.WaitGroup + motionCfg *motion.MotionConfiguration + replanCostFactor float64 + + logger golog.Logger + wholeServiceCancelFunc func() + currentWaypointCancelFunc func() + waypointInProgress *navigation.Waypoint + activeBackgroundWorkers sync.WaitGroup } func (svc *builtIn) Reconfigure(ctx context.Context, deps resource.Dependencies, conf resource.Config) error { - svc.mu.RLock() - defer svc.mu.RUnlock() + svc.actionMu.Lock() + defer svc.actionMu.Unlock() + + if svc.wholeServiceCancelFunc != nil { + svc.wholeServiceCancelFunc() + } + svc.activeBackgroundWorkers.Wait() svcConfig, err := resource.NativeConfig[*Config](conf) if err != nil { @@ -142,11 +190,22 @@ func (svc *builtIn) Reconfigure(ctx context.Context, deps resource.Dependencies, if err != nil { return err } - motionSrv, err := motion.FromDependencies(deps, svcConfig.MotionServiceName) + motionSvc, err := motion.FromDependencies(deps, svcConfig.MotionServiceName) if err != nil { return err } + var visionServices []resource.Name + for _, svc := range svcConfig.VisionServices { + visionSvc, err := vision.FromDependencies(deps, svc) + if err != nil { + return err + } + visionServices = append(visionServices, visionSvc.Name()) + } + + svc.mu.Lock() + defer svc.mu.Unlock() var newStore navigation.NavStore if svc.storeType != string(svcConfig.Store.Type) { switch svcConfig.Store.Type { @@ -171,14 +230,22 @@ func (svc *builtIn) Reconfigure(ctx context.Context, deps resource.Dependencies, return err } + svc.mode = navigation.ModeManual svc.store = newStore svc.storeType = string(svcConfig.Store.Type) svc.base = base1 svc.movementSensor = movementSensor - svc.motion = motionSrv + svc.motion = motionSvc svc.obstacles = newObstacles - svc.metersPerSec = svcConfig.MetersPerSec - svc.degPerSec = svcConfig.DegPerSec + svc.replanCostFactor = svcConfig.ReplanCostFactor + svc.motionCfg = &motion.MotionConfiguration{ + VisionServices: visionServices, + LinearMPerSec: svcConfig.MetersPerSec, + AngularDegsPerSec: svcConfig.DegPerSec, + PlanDeviationMM: 1e3 * svcConfig.PlanDeviationM, + PositionPollingFreqHz: svcConfig.PositionPollingFrequencyHz, + ObstaclePollingFreqHz: svcConfig.ObstaclePollingFrequencyHz, + } return nil } @@ -190,138 +257,50 @@ func (svc *builtIn) Mode(ctx context.Context, extra map[string]interface{}) (nav } func (svc *builtIn) SetMode(ctx context.Context, mode navigation.Mode, extra map[string]interface{}) error { - svc.mu.Lock() - defer svc.mu.Unlock() + svc.actionMu.Lock() + defer svc.actionMu.Unlock() + + svc.mu.RLock() if svc.mode == mode { + svc.mu.RUnlock() return nil } + svc.mu.RUnlock() // switch modes - svc.cancelFunc() + if svc.wholeServiceCancelFunc != nil { + svc.wholeServiceCancelFunc() + } svc.activeBackgroundWorkers.Wait() - cancelCtx, cancelFunc := context.WithCancel(context.Background()) - svc.cancelCtx = cancelCtx - svc.cancelFunc = cancelFunc - svc.mode = navigation.ModeManual - if mode == navigation.ModeWaypoint { - if extra != nil && extra["experimental"] == true { - if err := svc.startWaypointExperimental(extra); err != nil { - return err - } - } else if err := svc.startWaypoint(extra); err != nil { - return err - } - svc.mode = mode + svc.mu.Lock() + defer svc.mu.Unlock() + cancelCtx, cancelFunc := context.WithCancel(context.Background()) + svc.wholeServiceCancelFunc = cancelFunc + svc.mode = mode + if svc.mode == navigation.ModeWaypoint { + svc.startWaypoint(cancelCtx, extra) } return nil } -func (svc *builtIn) computeCurrentBearing(ctx context.Context, path []*geo.Point) (float64, error) { - props, err := svc.movementSensor.Properties(ctx, nil) - if err != nil { - return 0, err - } - if props.CompassHeadingSupported { - return svc.movementSensor.CompassHeading(ctx, nil) - } - pathLen := len(path) - return fixAngle(path[pathLen-2].BearingTo(path[pathLen-1])), nil -} - -func (svc *builtIn) startWaypoint(extra map[string]interface{}) error { - svc.activeBackgroundWorkers.Add(1) - utils.PanicCapturingGo(func() { - defer svc.activeBackgroundWorkers.Done() - - path := []*geo.Point{} - for { - if !utils.SelectContextOrWait(svc.cancelCtx, 500*time.Millisecond) { - return - } - currentLoc, _, err := svc.movementSensor.Position(svc.cancelCtx, extra) - if err != nil { - svc.logger.Errorw("failed to get gps location", "error", err) - continue - } - - if len(path) <= 1 || currentLoc.GreatCircleDistance(path[len(path)-1]) > .0001 { - // gps often updates less frequently - path = append(path, currentLoc) - if len(path) > 2 { - path = path[len(path)-2:] - } - } - - navOnce := func(ctx context.Context) error { - if len(path) <= 1 { - return errors.New("not enough gps data") - } - - currentBearing, err := svc.computeCurrentBearing(ctx, path) - if err != nil { - return err - } - - bearingToGoal, distanceToGoal, err := svc.waypointDirectionAndDistanceToGo(ctx, currentLoc) - if err != nil { - return err - } - - if distanceToGoal < .005 { - svc.logger.Debug("i made it") - return svc.waypointReached(ctx) - } - - bearingDelta := computeBearing(bearingToGoal, currentBearing) - steeringDir := -bearingDelta / 180.0 - - svc.logger.Debugf("currentBearing: %0.0f bearingToGoal: %0.0f distanceToGoal: %0.3f bearingDelta: %0.1f steeringDir: %0.2f", - currentBearing, bearingToGoal, distanceToGoal, bearingDelta, steeringDir) - - // TODO(erh->erd): maybe need an arc/stroke abstraction? - // - Remember that we added -1*bearingDelta instead of steeringDir - // - Test both naval/land to prove it works - if err := svc.base.Spin(ctx, -1*bearingDelta, svc.degPerSec, nil); err != nil { - return fmt.Errorf("error turning: %w", err) - } - - distanceMm := distanceToGoal * 1000 * 1000 - distanceMm = math.Min(distanceMm, 10*1000) - - // TODO: handle swap from mm to meters - if err := svc.base.MoveStraight(ctx, int(distanceMm), (svc.metersPerSec * 1000), nil); err != nil { - return fmt.Errorf("error moving %w", err) - } - - return nil - } - - if err := navOnce(svc.cancelCtx); err != nil { - svc.logger.Infof("error navigating: %s", err) - } - } - }) - return nil -} - -func (svc *builtIn) waypointDirectionAndDistanceToGo(ctx context.Context, currentLoc *geo.Point) (float64, float64, error) { - wp, err := svc.nextWaypoint(ctx) - if err != nil { - return 0, 0, err - } - - goal := wp.ToPoint() - - return fixAngle(currentLoc.BearingTo(goal)), currentLoc.GreatCircleDistance(goal), nil -} +func (svc *builtIn) Location(ctx context.Context, extra map[string]interface{}) (*spatialmath.GeoPose, error) { + svc.mu.RLock() + defer svc.mu.RUnlock() -func (svc *builtIn) Location(ctx context.Context, extra map[string]interface{}) (*geo.Point, error) { if svc.movementSensor == nil { return nil, errors.New("no way to get location") } loc, _, err := svc.movementSensor.Position(ctx, extra) - return loc, err + if err != nil { + return nil, err + } + compassHeading, err := svc.movementSensor.CompassHeading(ctx, extra) + if err != nil { + return nil, err + } + geoPose := spatialmath.NewGeoPose(loc, compassHeading) + return geoPose, err } func (svc *builtIn) Waypoints(ctx context.Context, extra map[string]interface{}) ([]navigation.Waypoint, error) { @@ -340,75 +319,64 @@ func (svc *builtIn) AddWaypoint(ctx context.Context, point *geo.Point, extra map } func (svc *builtIn) RemoveWaypoint(ctx context.Context, id primitive.ObjectID, extra map[string]interface{}) error { + svc.mu.Lock() + defer svc.mu.Unlock() + if svc.waypointInProgress != nil && svc.waypointInProgress.ID == id { + if svc.currentWaypointCancelFunc != nil { + svc.currentWaypointCancelFunc() + } + svc.waypointInProgress = nil + } return svc.store.RemoveWaypoint(ctx, id) } -func (svc *builtIn) nextWaypoint(ctx context.Context) (navigation.Waypoint, error) { - return svc.store.NextWaypoint(ctx) -} - func (svc *builtIn) waypointReached(ctx context.Context) error { - wp, err := svc.nextWaypoint(ctx) - if err != nil { - return fmt.Errorf("can't mark waypoint reached: %w", err) + if ctx.Err() != nil { + return ctx.Err() } - return svc.store.WaypointVisited(ctx, wp.ID) -} -func (svc *builtIn) Close(ctx context.Context) error { - svc.cancelFunc() - svc.activeBackgroundWorkers.Wait() - return svc.store.Close(ctx) -} + svc.mu.RLock() + wp := svc.waypointInProgress + svc.mu.RUnlock() -func fixAngle(a float64) float64 { - for a < 0 { - a += 360 - } - for a > 360 { - a -= 360 + if wp == nil { + return errors.New("can't mark waypoint reached since there is none in progress") } - return a + return svc.store.WaypointVisited(ctx, wp.ID) } -func computeBearing(a, b float64) float64 { - a = fixAngle(a) - b = fixAngle(b) - - t := b - a - if t < -180 { - t += 360 - } +func (svc *builtIn) Close(ctx context.Context) error { + svc.actionMu.Lock() + defer svc.actionMu.Unlock() - if t > 180 { - t -= 360 + if svc.wholeServiceCancelFunc != nil { + svc.wholeServiceCancelFunc() } + svc.activeBackgroundWorkers.Wait() - return t + return svc.store.Close(ctx) } -func (svc *builtIn) startWaypointExperimental(extra map[string]interface{}) error { +func (svc *builtIn) startWaypoint(ctx context.Context, extra map[string]interface{}) { + if extra == nil { + extra = map[string]interface{}{"motion_profile": "position_only"} + } else if _, ok := extra["motion_profile"]; !ok { + extra["motion_profile"] = "position_only" + } + svc.activeBackgroundWorkers.Add(1) utils.PanicCapturingGo(func() { defer svc.activeBackgroundWorkers.Done() navOnce := func(ctx context.Context, wp navigation.Waypoint) error { - if extra == nil { - extra = map[string]interface{}{"motion_profile": "position_only"} - } else if _, ok := extra["motion_profile"]; !ok { - extra["motion_profile"] = "position_only" - } - - goal := wp.ToPoint() _, err := svc.motion.MoveOnGlobe( ctx, svc.base.Name(), - goal, + wp.ToPoint(), math.NaN(), svc.movementSensor.Name(), svc.obstacles, - svc.metersPerSec*1000, - svc.degPerSec, + svc.motionCfg, extra, ) if err != nil { @@ -418,13 +386,51 @@ func (svc *builtIn) startWaypointExperimental(extra map[string]interface{}) erro return svc.waypointReached(ctx) } - // loop until no waypoints remaining - for wp, err := svc.nextWaypoint(svc.cancelCtx); err == nil; wp, err = svc.nextWaypoint(svc.cancelCtx) { + // do not exit loop - even if there are no waypoints remaining + for { + if ctx.Err() != nil { + return + } + + wp, err := svc.store.NextWaypoint(ctx) + if err != nil { + continue + } + svc.mu.Lock() + svc.waypointInProgress = &wp + cancelCtx, cancelFunc := context.WithCancel(ctx) + svc.currentWaypointCancelFunc = cancelFunc + svc.mu.Unlock() + svc.logger.Infof("navigating to waypoint: %+v", wp) - if err := navOnce(svc.cancelCtx, wp); err != nil { + if err := navOnce(cancelCtx, wp); err != nil { + if svc.waypointIsDeleted() { + svc.logger.Infof("skipping waypoint %+v since it was deleted", wp) + continue + } + svc.logger.Infof("skipping waypoint %+v due to error while navigating towards it: %s", wp, err) + if err := svc.waypointReached(ctx); err != nil { + if svc.waypointIsDeleted() { + svc.logger.Infof("skipping waypoint %+v since it was deleted", wp) + continue + } + svc.logger.Infof("can't mark waypoint %+v as reached, exiting navigation due to error: %s", wp, err) + return + } } } }) - return nil +} + +func (svc *builtIn) waypointIsDeleted() bool { + svc.mu.RLock() + defer svc.mu.RUnlock() + return svc.waypointInProgress == nil +} + +func (svc *builtIn) GetObstacles(ctx context.Context, extra map[string]interface{}) ([]*spatialmath.GeoObstacle, error) { + svc.mu.RLock() + defer svc.mu.RUnlock() + return svc.obstacles, nil } diff --git a/services/navigation/builtin/builtin_test.go b/services/navigation/builtin/builtin_test.go index 3a04deace49..dac31388209 100644 --- a/services/navigation/builtin/builtin_test.go +++ b/services/navigation/builtin/builtin_test.go @@ -1,14 +1,16 @@ -// Package builtin contains the default navigation service, along with a gRPC server and client package builtin import ( "context" "errors" "testing" + "time" "github.com/edaniels/golog" + "github.com/golang/geo/r3" geo "github.com/kellydunn/golang-geo" "go.viam.com/test" + "go.viam.com/utils/testutils" "go.viam.com/rdk/components/base" fakebase "go.viam.com/rdk/components/base/fake" @@ -23,6 +25,8 @@ import ( "go.viam.com/rdk/services/navigation" "go.viam.com/rdk/services/slam" fakeslam "go.viam.com/rdk/services/slam/fake" + _ "go.viam.com/rdk/services/vision" + _ "go.viam.com/rdk/services/vision/colordetector" "go.viam.com/rdk/spatialmath" "go.viam.com/rdk/testutils/inject" ) @@ -43,6 +47,42 @@ func setupNavigationServiceFromConfig(t *testing.T, configFilename string) (navi } } +func currentInputsShouldEqual(ctx context.Context, t *testing.T, kinematicBase kinematicbase.KinematicBase, pt *geo.Point) { + t.Helper() + inputs, err := kinematicBase.CurrentInputs(ctx) + test.That(t, err, test.ShouldBeNil) + actualPt := geo.NewPoint(inputs[0].Value, inputs[1].Value) + test.That(t, actualPt.Lat(), test.ShouldEqual, pt.Lat()) + test.That(t, actualPt.Lng(), test.ShouldEqual, pt.Lng()) +} + +func blockTillCallCount(t *testing.T, callCount int, callChan chan struct{}, timeout time.Duration) { + t.Helper() + waitForCallsTimeOutCtx, cancelFn := context.WithTimeout(context.Background(), timeout) + defer cancelFn() + for i := 0; i < callCount; i++ { + select { + case <-callChan: + case <-waitForCallsTimeOutCtx.Done(): + t.Log("timed out waiting for test to finish") + t.FailNow() + } + } +} + +func deleteAllWaypoints(ctx context.Context, svc navigation.Service) error { + waypoints, err := svc.(*builtIn).store.Waypoints(ctx) + if err != nil { + return err + } + for _, wp := range waypoints { + if err := svc.RemoveWaypoint(ctx, wp.ID, nil); err != nil { + return err + } + } + return nil +} + func TestNavSetup(t *testing.T) { ns, teardown := setupNavigationServiceFromConfig(t, "../data/nav_cfg.json") defer teardown() @@ -50,17 +90,22 @@ func TestNavSetup(t *testing.T) { navMode, err := ns.Mode(ctx, nil) test.That(t, err, test.ShouldBeNil) - test.That(t, navMode, test.ShouldEqual, 0) + test.That(t, navMode, test.ShouldEqual, navigation.ModeManual) - err = ns.SetMode(ctx, 1, nil) + err = ns.SetMode(ctx, navigation.ModeWaypoint, nil) test.That(t, err, test.ShouldBeNil) navMode, err = ns.Mode(ctx, nil) test.That(t, err, test.ShouldBeNil) - test.That(t, navMode, test.ShouldEqual, 1) + test.That(t, navMode, test.ShouldEqual, navigation.ModeWaypoint) + + // Prevent race + err = ns.SetMode(ctx, navigation.ModeManual, nil) + test.That(t, err, test.ShouldBeNil) - loc, err := ns.Location(ctx, nil) + geoPose, err := ns.Location(ctx, nil) test.That(t, err, test.ShouldBeNil) - test.That(t, loc, test.ShouldResemble, geo.NewPoint(40.7, -73.98)) + expectedGeoPose := spatialmath.NewGeoPose(geo.NewPoint(40.7, -73.98), 25.) + test.That(t, geoPose, test.ShouldResemble, expectedGeoPose) wayPt, err := ns.Waypoints(ctx, nil) test.That(t, err, test.ShouldBeNil) @@ -80,6 +125,12 @@ func TestNavSetup(t *testing.T) { wayPt, err = ns.Waypoints(ctx, nil) test.That(t, err, test.ShouldBeNil) test.That(t, len(wayPt), test.ShouldEqual, 0) + + obs, err := ns.GetObstacles(ctx, nil) + test.That(t, len(obs), test.ShouldEqual, 1) + test.That(t, err, test.ShouldBeNil) + + test.That(t, len(ns.(*builtIn).motionCfg.VisionServices), test.ShouldEqual, 1) } func TestStartWaypoint(t *testing.T) { @@ -97,17 +148,20 @@ func TestStartWaypoint(t *testing.T) { test.That(t, err, test.ShouldBeNil) fakeSlam := fakeslam.NewSLAM(slam.Named("foo"), logger) - limits, err := fakeSlam.GetLimits(ctx) + limits, err := fakeSlam.Limits(ctx) test.That(t, err, test.ShouldBeNil) - localizer, err := motion.NewLocalizer(ctx, fakeSlam) + localizer := motion.NewSLAMLocalizer(fakeSlam) test.That(t, err, test.ShouldBeNil) // cast fakeBase fake, ok := fakeBase.(*fakebase.Base) test.That(t, ok, test.ShouldBeTrue) - kinematicBase, err := kinematicbase.WrapWithFakeKinematics(ctx, fake, localizer, limits) + options := kinematicbase.NewKinematicBaseOptions() + options.PositionOnlyMode = false + + kinematicBase, err := kinematicbase.WrapWithFakeKinematics(ctx, fake, localizer, limits, options) test.That(t, err, test.ShouldBeNil) injectMovementSensor := inject.NewMovementSensor("test_movement") @@ -116,21 +170,6 @@ func TestStartWaypoint(t *testing.T) { return geo.NewPoint(inputs[0].Value, inputs[1].Value), 0, err } - injectMS.MoveOnGlobeFunc = func( - ctx context.Context, - componentName resource.Name, - destination *geo.Point, - heading float64, - movementSensorName resource.Name, - obstacles []*spatialmath.GeoObstacle, - linearVelocityMillisPerSec float64, - angularVelocityDegsPerSec float64, - extra map[string]interface{}, - ) (bool, error) { - err := kinematicBase.GoToInputs(ctx, referenceframe.FloatsToInputs([]float64{destination.Lat(), destination.Lng(), 0})) - return true, err - } - ns, err := NewBuiltIn( ctx, resource.Dependencies{injectMS.Name(): injectMS, fakeBase.Name(): fakeBase, injectMovementSensor.Name(): injectMovementSensor}, @@ -149,57 +188,332 @@ func TestStartWaypoint(t *testing.T) { logger, ) test.That(t, err, test.ShouldBeNil) + defer func() { + test.That(t, ns.Close(context.Background()), test.ShouldBeNil) + }() - pt := geo.NewPoint(1, 0) - err = ns.AddWaypoint(ctx, pt, nil) - test.That(t, err, test.ShouldBeNil) + t.Run("Reach waypoints successfully", func(t *testing.T) { + callChan := make(chan struct{}, 2) + injectMS.MoveOnGlobeFunc = func( + ctx context.Context, + componentName resource.Name, + destination *geo.Point, + heading float64, + movementSensorName resource.Name, + obstacles []*spatialmath.GeoObstacle, + motionCfg *motion.MotionConfiguration, + extra map[string]interface{}, + ) (bool, error) { + err := kinematicBase.GoToInputs(ctx, referenceframe.FloatsToInputs([]float64{destination.Lat(), destination.Lng()})) + callChan <- struct{}{} + return true, err + } + pt := geo.NewPoint(1, 0) + err = ns.AddWaypoint(ctx, pt, nil) + test.That(t, err, test.ShouldBeNil) - pt = geo.NewPoint(3, 1) - err = ns.AddWaypoint(ctx, pt, nil) - test.That(t, err, test.ShouldBeNil) + pt = geo.NewPoint(3, 1) + err = ns.AddWaypoint(ctx, pt, nil) + test.That(t, err, test.ShouldBeNil) - err = ns.SetMode(ctx, navigation.ModeWaypoint, map[string]interface{}{"experimental": true}) - test.That(t, err, test.ShouldBeNil) - ns.(*builtIn).activeBackgroundWorkers.Wait() + ns.(*builtIn).mode = navigation.ModeManual + err = ns.SetMode(ctx, navigation.ModeWaypoint, nil) + test.That(t, err, test.ShouldBeNil) + blockTillCallCount(t, 2, callChan, time.Second*5) + ns.(*builtIn).wholeServiceCancelFunc() + ns.(*builtIn).activeBackgroundWorkers.Wait() - inputs, err := kinematicBase.CurrentInputs(ctx) - test.That(t, err, test.ShouldBeNil) - actualPt := geo.NewPoint(inputs[0].Value, inputs[1].Value) - test.That(t, actualPt.Lat(), test.ShouldEqual, pt.Lat()) - test.That(t, actualPt.Lng(), test.ShouldEqual, pt.Lng()) + currentInputsShouldEqual(ctx, t, kinematicBase, pt) + }) + + t.Run("Extra defaults to motion_profile", func(t *testing.T) { + callChan := make(chan struct{}, 1) + // setup injected MoveOnGlobe to test what extra defaults to from startWaypointExperimental function + injectMS.MoveOnGlobeFunc = func( + ctx context.Context, + componentName resource.Name, + destination *geo.Point, + heading float64, + movementSensorName resource.Name, + obstacles []*spatialmath.GeoObstacle, + motionCfg *motion.MotionConfiguration, + extra map[string]interface{}, + ) (bool, error) { + callChan <- struct{}{} + if extra != nil && extra["motion_profile"] != nil { + return true, nil + } + return false, errors.New("no motion_profile exist") + } + + // construct new point to navigate to + pt := geo.NewPoint(0, 0) + err = ns.AddWaypoint(ctx, pt, nil) + test.That(t, err, test.ShouldBeNil) - // setup injected MoveOnGlobe to test what extra defaults to from startWaypointExperimental function - injectMS.MoveOnGlobeFunc = func( - ctx context.Context, - componentName resource.Name, - destination *geo.Point, - heading float64, - movementSensorName resource.Name, - obstacles []*spatialmath.GeoObstacle, - linearVelocity float64, - angularVelocity float64, - extra map[string]interface{}, - ) (bool, error) { - if extra != nil && extra["motion_profile"] != nil { - return true, nil + cancelCtx, fn := context.WithCancel(ctx) + ns.(*builtIn).startWaypoint(cancelCtx, map[string]interface{}{}) + blockTillCallCount(t, 1, callChan, time.Second*5) + fn() + ns.(*builtIn).activeBackgroundWorkers.Wait() + + // go to same point again + err = ns.AddWaypoint(ctx, pt, nil) + test.That(t, err, test.ShouldBeNil) + + cancelCtx, fn = context.WithCancel(ctx) + ns.(*builtIn).startWaypoint(cancelCtx, nil) + blockTillCallCount(t, 1, callChan, time.Second*5) + fn() + ns.(*builtIn).activeBackgroundWorkers.Wait() + }) + + t.Run("Test MoveOnGlobe cancellation and errors", func(t *testing.T) { + eventChannel, statusChannel := make(chan string), make(chan string, 1) + cancelledContextMsg := "context cancelled" + hitAnErrorMsg := "hit an error" + arrivedAtWaypointMsg := "arrived at destination" + invalidStateMsg := "bad message passed to event channel" + + injectMS.MoveOnGlobeFunc = func( + ctx context.Context, + componentName resource.Name, + destination *geo.Point, + heading float64, + movementSensorName resource.Name, + obstacles []*spatialmath.GeoObstacle, + motionCfg *motion.MotionConfiguration, + extra map[string]interface{}, + ) (bool, error) { + if ctx.Err() != nil { + statusChannel <- cancelledContextMsg + return false, ctx.Err() + } + select { + case <-ctx.Done(): + statusChannel <- cancelledContextMsg + return false, ctx.Err() + case msg := <-eventChannel: + var err error + if msg == arrivedAtWaypointMsg { + err = kinematicBase.GoToInputs( + ctx, + referenceframe.FloatsToInputs([]float64{destination.Lat(), destination.Lng()}), + ) + } + + statusChannel <- msg + switch { + case msg == hitAnErrorMsg: + return false, errors.New(hitAnErrorMsg) + case msg == arrivedAtWaypointMsg: + return true, err + default: + // should be unreachable + return false, errors.New(invalidStateMsg) + } + } } - return false, errors.New("no motion_profile exist") + + pt1, pt2, pt3 := geo.NewPoint(1, 2), geo.NewPoint(2, 3), geo.NewPoint(3, 4) + points := []*geo.Point{pt1, pt2, pt3} + t.Run("MoveOnGlobe error results in skipping the current waypoint", func(t *testing.T) { + // Set manual mode to ensure waypoint loop from prior test exits + err = ns.SetMode(ctx, navigation.ModeManual, map[string]interface{}{"experimental": true}) + test.That(t, err, test.ShouldBeNil) + ctx, cancelFunc := context.WithCancel(ctx) + defer ns.(*builtIn).activeBackgroundWorkers.Wait() + defer cancelFunc() + err = deleteAllWaypoints(ctx, ns) + for _, pt := range points { + err = ns.AddWaypoint(ctx, pt, nil) + test.That(t, err, test.ShouldBeNil) + } + + ns.(*builtIn).startWaypoint(ctx, map[string]interface{}{"experimental": true}) + + // Get the ID of the first waypoint + wp1, err := ns.(*builtIn).store.NextWaypoint(ctx) + test.That(t, err, test.ShouldBeNil) + + // Reach the first waypoint + eventChannel <- arrivedAtWaypointMsg + test.That(t, <-statusChannel, test.ShouldEqual, arrivedAtWaypointMsg) + currentInputsShouldEqual(ctx, t, kinematicBase, pt1) + + // Ensure we aren't querying before the nav service has a chance to mark the previous waypoint visited. + wp2, err := ns.(*builtIn).store.NextWaypoint(ctx) + test.That(t, err, test.ShouldBeNil) + for wp2.ID == wp1.ID { + wp2, err = ns.(*builtIn).store.NextWaypoint(ctx) + test.That(t, err, test.ShouldBeNil) + } + + // Skip the second waypoint due to an error + eventChannel <- hitAnErrorMsg + test.That(t, <-statusChannel, test.ShouldEqual, hitAnErrorMsg) + currentInputsShouldEqual(ctx, t, kinematicBase, pt1) + + // Ensure we aren't querying before the nav service has a chance to mark the previous waypoint visited. + wp3, err := ns.(*builtIn).store.NextWaypoint(ctx) + test.That(t, err, test.ShouldBeNil) + for wp3.ID == wp2.ID { + wp3, err = ns.(*builtIn).store.NextWaypoint(ctx) + test.That(t, err, test.ShouldBeNil) + } + + // Reach the third waypoint + eventChannel <- arrivedAtWaypointMsg + test.That(t, <-statusChannel, test.ShouldEqual, arrivedAtWaypointMsg) + currentInputsShouldEqual(ctx, t, kinematicBase, pt3) + }) + t.Run("Calling SetMode cancels current and future MoveOnGlobe calls", func(t *testing.T) { + // Set manual mode to ensure waypoint loop from prior test exits + err = deleteAllWaypoints(ctx, ns) + test.That(t, err, test.ShouldBeNil) + for _, pt := range points { + err = ns.AddWaypoint(ctx, pt, nil) + test.That(t, err, test.ShouldBeNil) + } + + // start navigation - set ModeManual first to ensure navigation starts up + err = ns.SetMode(ctx, navigation.ModeWaypoint, map[string]interface{}{"experimental": true}) + + // Reach the first waypoint + eventChannel <- arrivedAtWaypointMsg + test.That(t, <-statusChannel, test.ShouldEqual, arrivedAtWaypointMsg) + currentInputsShouldEqual(ctx, t, kinematicBase, pt1) + + // Change the mode to manual --> stops navigation to waypoints + err = ns.SetMode(ctx, navigation.ModeManual, map[string]interface{}{"experimental": true}) + test.That(t, err, test.ShouldBeNil) + select { + case msg := <-statusChannel: + test.That(t, msg, test.ShouldEqual, cancelledContextMsg) + case <-time.After(5 * time.Second): + ns.(*builtIn).activeBackgroundWorkers.Wait() + } + currentInputsShouldEqual(ctx, t, kinematicBase, pt1) + }) + + t.Run("Calling RemoveWaypoint on the waypoint in progress cancels current MoveOnGlobe call", func(t *testing.T) { + // Set manual mode to ensure waypoint loop from prior test exits + err = ns.SetMode(ctx, navigation.ModeManual, map[string]interface{}{"experimental": true}) + test.That(t, err, test.ShouldBeNil) + err = deleteAllWaypoints(ctx, ns) + for _, pt := range points { + err = ns.AddWaypoint(ctx, pt, nil) + test.That(t, err, test.ShouldBeNil) + } + + // start navigation - set ModeManual first to ensure navigation starts up + err = ns.SetMode(ctx, navigation.ModeWaypoint, map[string]interface{}{"experimental": true}) + + // Get the ID of the first waypoint + wp1, err := ns.(*builtIn).store.NextWaypoint(ctx) + test.That(t, err, test.ShouldBeNil) + + // Reach the first waypoint + eventChannel <- arrivedAtWaypointMsg + test.That(t, <-statusChannel, test.ShouldEqual, arrivedAtWaypointMsg) + currentInputsShouldEqual(ctx, t, kinematicBase, pt1) + + // Remove the second waypoint, which is in progress. Ensure we aren't querying before the nav service has a chance to mark + // the previous waypoint visited. + wp2, err := ns.(*builtIn).store.NextWaypoint(ctx) + test.That(t, err, test.ShouldBeNil) + for wp2.ID == wp1.ID { + wp2, err = ns.(*builtIn).store.NextWaypoint(ctx) + test.That(t, err, test.ShouldBeNil) + } + + // ensure we actually start the wp2 waypoint before removing it + testutils.WaitForAssertion(t, func(tb testing.TB) { + tb.Helper() + ns.(*builtIn).mu.RLock() + svcWp := ns.(*builtIn).waypointInProgress + ns.(*builtIn).mu.RUnlock() + test.That(tb, wp2.ID, test.ShouldEqual, svcWp.ID) + }) + + err = ns.RemoveWaypoint(ctx, wp2.ID, nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, <-statusChannel, test.ShouldEqual, cancelledContextMsg) + currentInputsShouldEqual(ctx, t, kinematicBase, pt1) + + // Reach the third waypoint + eventChannel <- arrivedAtWaypointMsg + test.That(t, <-statusChannel, test.ShouldEqual, arrivedAtWaypointMsg) + currentInputsShouldEqual(ctx, t, kinematicBase, pt3) + }) + + t.Run("Calling RemoveWaypoint on a waypoint that is not in progress does not cancel MoveOnGlobe", func(t *testing.T) { + // Set manual mode to ensure waypoint loop from prior test exits + err = ns.SetMode(ctx, navigation.ModeManual, map[string]interface{}{"experimental": true}) + test.That(t, err, test.ShouldBeNil) + err = deleteAllWaypoints(ctx, ns) + var wp3 navigation.Waypoint + for i, pt := range points { + if i < 3 { + err = ns.AddWaypoint(ctx, pt, nil) + test.That(t, err, test.ShouldBeNil) + } else { + wp3, err = ns.(*builtIn).store.AddWaypoint(ctx, pt) + test.That(t, err, test.ShouldBeNil) + } + } + + // start navigation - set ModeManual first to ensure navigation starts up + err = ns.SetMode(ctx, navigation.ModeWaypoint, map[string]interface{}{"experimental": true}) + + // Reach the first waypoint + eventChannel <- arrivedAtWaypointMsg + test.That(t, <-statusChannel, test.ShouldEqual, arrivedAtWaypointMsg) + currentInputsShouldEqual(ctx, t, kinematicBase, pt1) + + // Remove the third waypoint, which is not in progress yet + err = ns.RemoveWaypoint(ctx, wp3.ID, nil) + test.That(t, err, test.ShouldBeNil) + + // Reach the second waypoint + eventChannel <- arrivedAtWaypointMsg + test.That(t, <-statusChannel, test.ShouldEqual, arrivedAtWaypointMsg) + currentInputsShouldEqual(ctx, t, kinematicBase, pt2) + }) + }) +} + +func TestValidateGeometry(t *testing.T) { + cfg := Config{ + BaseName: "base", + MovementSensorName: "localizer", } - // construct new point to navigate to - pt = geo.NewPoint(0, 0) - err = ns.AddWaypoint(ctx, pt, nil) - test.That(t, err, test.ShouldBeNil) + createBox := func(translation r3.Vector) Config { + boxPose := spatialmath.NewPoseFromPoint(translation) + geometries, err := spatialmath.NewBox(boxPose, r3.Vector{10, 10, 10}, "") + test.That(t, err, test.ShouldBeNil) - err = ns.(*builtIn).startWaypointExperimental(map[string]interface{}{}) - test.That(t, err, test.ShouldBeNil) - ns.(*builtIn).activeBackgroundWorkers.Wait() + geoObstacle := spatialmath.NewGeoObstacle(geo.NewPoint(0, 0), []spatialmath.Geometry{geometries}) + geoObstacleCfg, err := spatialmath.NewGeoObstacleConfig(geoObstacle) + test.That(t, err, test.ShouldBeNil) - // go to same point again - err = ns.AddWaypoint(ctx, pt, nil) - test.That(t, err, test.ShouldBeNil) + cfg.Obstacles = []*spatialmath.GeoObstacleConfig{geoObstacleCfg} - err = ns.(*builtIn).startWaypointExperimental(nil) - test.That(t, err, test.ShouldBeNil) - ns.(*builtIn).activeBackgroundWorkers.Wait() + return cfg + } + + t.Run("fail case", func(t *testing.T) { + cfg = createBox(r3.Vector{10, 10, 10}) + _, err := cfg.Validate("") + expectedErr := "geometries specified through the navigation are not allowed to have a translation" + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldEqual, expectedErr) + }) + + t.Run("success case", func(t *testing.T) { + cfg = createBox(r3.Vector{}) + _, err := cfg.Validate("") + test.That(t, err, test.ShouldBeNil) + }) } diff --git a/services/navigation/client.go b/services/navigation/client.go index ae36cacdd39..70186ca0a29 100644 --- a/services/navigation/client.go +++ b/services/navigation/client.go @@ -14,6 +14,7 @@ import ( rprotoutils "go.viam.com/rdk/protoutils" "go.viam.com/rdk/resource" + "go.viam.com/rdk/spatialmath" ) // client implements NavigationServiceClient. @@ -87,7 +88,7 @@ func (c *client) SetMode(ctx context.Context, mode Mode, extra map[string]interf return nil } -func (c *client) Location(ctx context.Context, extra map[string]interface{}) (*geo.Point, error) { +func (c *client) Location(ctx context.Context, extra map[string]interface{}) (*spatialmath.GeoPose, error) { ext, err := protoutils.StructToStructPb(extra) if err != nil { return nil, err @@ -96,9 +97,11 @@ func (c *client) Location(ctx context.Context, extra map[string]interface{}) (*g if err != nil { return nil, err } - loc := resp.GetLocation() - result := geo.NewPoint(loc.GetLatitude(), loc.GetLongitude()) - return result, nil + geoPose := spatialmath.NewGeoPose( + geo.NewPoint(resp.GetLocation().GetLatitude(), resp.GetLocation().GetLongitude()), + resp.GetCompassHeading(), + ) + return geoPose, nil } func (c *client) Waypoints(ctx context.Context, extra map[string]interface{}) ([]Waypoint, error) { @@ -161,6 +164,24 @@ func (c *client) RemoveWaypoint(ctx context.Context, id primitive.ObjectID, extr return nil } +func (c *client) GetObstacles(ctx context.Context, extra map[string]interface{}) ([]*spatialmath.GeoObstacle, error) { + req := &pb.GetObstaclesRequest{} + resp, err := c.client.GetObstacles(ctx, req) + if err != nil { + return nil, err + } + protoObs := resp.GetObstacles() + geos := []*spatialmath.GeoObstacle{} + for _, o := range protoObs { + obstacle, err := spatialmath.GeoObstacleFromProtobuf(o) + if err != nil { + return nil, err + } + geos = append(geos, obstacle) + } + return geos, nil +} + func (c *client) DoCommand(ctx context.Context, cmd map[string]interface{}) (map[string]interface{}, error) { return rprotoutils.DoFromResourceClient(ctx, c.client, c.name, cmd) } diff --git a/services/navigation/client_test.go b/services/navigation/client_test.go index 635730f0d26..c645ba5f3f5 100644 --- a/services/navigation/client_test.go +++ b/services/navigation/client_test.go @@ -18,6 +18,7 @@ import ( viamgrpc "go.viam.com/rdk/grpc" "go.viam.com/rdk/resource" "go.viam.com/rdk/services/navigation" + "go.viam.com/rdk/spatialmath" "go.viam.com/rdk/testutils" "go.viam.com/rdk/testutils/inject" ) @@ -57,9 +58,12 @@ func TestClient(t *testing.T) { return nil } expectedLoc := geo.NewPoint(80, 1) - workingNavigationService.LocationFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, error) { + expectedCompassHeading := 90. + expectedGeoPose := spatialmath.NewGeoPose(expectedLoc, expectedCompassHeading) + workingNavigationService.LocationFunc = func(ctx context.Context, extra map[string]interface{}) (*spatialmath.GeoPose, error) { extraOptions = extra - return expectedLoc, nil + + return expectedGeoPose, nil } waypoints := []navigation.Waypoint{ { @@ -94,7 +98,7 @@ func TestClient(t *testing.T) { receivedFailingMode = mode return errors.New("failure to set mode") } - failingNavigationService.LocationFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, error) { + failingNavigationService.LocationFunc = func(ctx context.Context, extra map[string]interface{}) (*spatialmath.GeoPose, error) { return nil, errors.New("failure to retrieve location") } failingNavigationService.WaypointsFunc = func(ctx context.Context, extra map[string]interface{}) ([]navigation.Waypoint, error) { @@ -187,9 +191,9 @@ func TestClient(t *testing.T) { // test location extra := map[string]interface{}{"foo": "Location"} - loc, err := workingDialedClient.Location(context.Background(), extra) + geoPose, err := workingDialedClient.Location(context.Background(), extra) test.That(t, err, test.ShouldBeNil) - test.That(t, loc, test.ShouldResemble, expectedLoc) + test.That(t, geoPose, test.ShouldResemble, expectedGeoPose) test.That(t, extraOptions, test.ShouldResemble, extra) // test remove waypoint @@ -256,9 +260,9 @@ func TestClient(t *testing.T) { test.That(t, err, test.ShouldNotBeNil) // test location - loc, err := dialedClient.Location(context.Background(), map[string]interface{}{}) + geoPose, err := dialedClient.Location(context.Background(), map[string]interface{}{}) test.That(t, err, test.ShouldNotBeNil) - test.That(t, loc, test.ShouldBeNil) + test.That(t, geoPose, test.ShouldBeNil) // test remove waypoint wptID := primitive.NewObjectID() diff --git a/services/navigation/data/nav_cfg.json b/services/navigation/data/nav_cfg.json index 05dedcc4256..be697363bb3 100644 --- a/services/navigation/data/nav_cfg.json +++ b/services/navigation/data/nav_cfg.json @@ -11,12 +11,27 @@ "model": "fake" }], "services": - [{ + [ + { + "name": "blue_square", + "type": "vision", + "model": "color_detector", + "attributes": { + "segment_size_px": 100, + "detect_color": "#1C4599", + "hue_tolerance_pct": 0.07, + "value_cutoff_pct": 0.15 + } + }, + { "name":"test_navigation", "type": "navigation", "attributes":{ "base":"test_base", "movement_sensor":"test_movement", + "vision_services": [ + "blue_square" + ], "obstacles": [{ "geometries": @@ -33,12 +48,7 @@ }, "x":10, "y":10, - "z":10, - "translation":{ - "x":1, - "y":1, - "z":1 - } + "z":10 }], "location":{ "latitude": 1, diff --git a/services/navigation/navigation.go b/services/navigation/navigation.go index 41156e8a88b..e5f9c1e57e8 100644 --- a/services/navigation/navigation.go +++ b/services/navigation/navigation.go @@ -1,4 +1,4 @@ -// Package navigation contains a navigation service, along with a gRPC server and client +// Package navigation is the service that allows you to navigate along waypoints. package navigation import ( @@ -10,6 +10,7 @@ import ( "go.viam.com/rdk/resource" "go.viam.com/rdk/robot" + "go.viam.com/rdk/spatialmath" ) func init() { @@ -36,12 +37,14 @@ type Service interface { Mode(ctx context.Context, extra map[string]interface{}) (Mode, error) SetMode(ctx context.Context, mode Mode, extra map[string]interface{}) error - Location(ctx context.Context, extra map[string]interface{}) (*geo.Point, error) + Location(ctx context.Context, extra map[string]interface{}) (*spatialmath.GeoPose, error) // Waypoint Waypoints(ctx context.Context, extra map[string]interface{}) ([]Waypoint, error) AddWaypoint(ctx context.Context, point *geo.Point, extra map[string]interface{}) error RemoveWaypoint(ctx context.Context, id primitive.ObjectID, extra map[string]interface{}) error + + GetObstacles(ctx context.Context, extra map[string]interface{}) ([]*spatialmath.GeoObstacle, error) } // SubtypeName is the name of the type of service. diff --git a/services/navigation/register/register.go b/services/navigation/register/register.go index c64531b0187..7f649c38756 100644 --- a/services/navigation/register/register.go +++ b/services/navigation/register/register.go @@ -1,4 +1,4 @@ -// Package register registers all relevant navigation models and also API specific functions +// Package register registers all relevant navigation models and API specific functions. package register import ( diff --git a/services/navigation/server.go b/services/navigation/server.go index 46781bde02a..2d22062156b 100644 --- a/services/navigation/server.go +++ b/services/navigation/server.go @@ -11,6 +11,7 @@ import ( "go.viam.com/rdk/protoutils" "go.viam.com/rdk/resource" + "go.viam.com/rdk/spatialmath" ) // serviceServer implements the contract from navigation.proto. @@ -25,9 +26,7 @@ func NewRPCServiceServer(coll resource.APIResourceCollection[Service]) interface return &serviceServer{coll: coll} } -func (server *serviceServer) GetMode(ctx context.Context, req *pb.GetModeRequest) ( - *pb.GetModeResponse, error, -) { +func (server *serviceServer) GetMode(ctx context.Context, req *pb.GetModeRequest) (*pb.GetModeResponse, error) { svc, err := server.coll.Resource(req.Name) if err != nil { return nil, err @@ -48,9 +47,7 @@ func (server *serviceServer) GetMode(ctx context.Context, req *pb.GetModeRequest }, nil } -func (server *serviceServer) SetMode(ctx context.Context, req *pb.SetModeRequest) ( - *pb.SetModeResponse, error, -) { +func (server *serviceServer) SetMode(ctx context.Context, req *pb.SetModeRequest) (*pb.SetModeResponse, error) { svc, err := server.coll.Resource(req.Name) if err != nil { return nil, err @@ -72,25 +69,23 @@ func (server *serviceServer) SetMode(ctx context.Context, req *pb.SetModeRequest return &pb.SetModeResponse{}, nil } -func (server *serviceServer) GetLocation(ctx context.Context, req *pb.GetLocationRequest) ( - *pb.GetLocationResponse, error, -) { +func (server *serviceServer) GetLocation(ctx context.Context, req *pb.GetLocationRequest) (*pb.GetLocationResponse, error) { svc, err := server.coll.Resource(req.Name) if err != nil { return nil, err } - loc, err := svc.Location(ctx, req.Extra.AsMap()) + geoPose, err := svc.Location(ctx, req.Extra.AsMap()) if err != nil { return nil, err } + return &pb.GetLocationResponse{ - Location: &commonpb.GeoPoint{Latitude: loc.Lat(), Longitude: loc.Lng()}, + Location: &commonpb.GeoPoint{Latitude: geoPose.Location().Lat(), Longitude: geoPose.Location().Lng()}, + CompassHeading: geoPose.Heading(), }, nil } -func (server *serviceServer) GetWaypoints(ctx context.Context, req *pb.GetWaypointsRequest) ( - *pb.GetWaypointsResponse, error, -) { +func (server *serviceServer) GetWaypoints(ctx context.Context, req *pb.GetWaypointsRequest) (*pb.GetWaypointsResponse, error) { svc, err := server.coll.Resource(req.Name) if err != nil { return nil, err @@ -111,9 +106,7 @@ func (server *serviceServer) GetWaypoints(ctx context.Context, req *pb.GetWaypoi }, nil } -func (server *serviceServer) AddWaypoint(ctx context.Context, req *pb.AddWaypointRequest) ( - *pb.AddWaypointResponse, error, -) { +func (server *serviceServer) AddWaypoint(ctx context.Context, req *pb.AddWaypointRequest) (*pb.AddWaypointResponse, error) { svc, err := server.coll.Resource(req.Name) if err != nil { return nil, err @@ -125,9 +118,7 @@ func (server *serviceServer) AddWaypoint(ctx context.Context, req *pb.AddWaypoin return &pb.AddWaypointResponse{}, nil } -func (server *serviceServer) RemoveWaypoint(ctx context.Context, req *pb.RemoveWaypointRequest) ( - *pb.RemoveWaypointResponse, error, -) { +func (server *serviceServer) RemoveWaypoint(ctx context.Context, req *pb.RemoveWaypointRequest) (*pb.RemoveWaypointResponse, error) { svc, err := server.coll.Resource(req.Name) if err != nil { return nil, err @@ -142,8 +133,25 @@ func (server *serviceServer) RemoveWaypoint(ctx context.Context, req *pb.RemoveW return &pb.RemoveWaypointResponse{}, nil } +func (server *serviceServer) GetObstacles(ctx context.Context, req *pb.GetObstaclesRequest) (*pb.GetObstaclesResponse, error) { + svc, err := server.coll.Resource(req.Name) + if err != nil { + return nil, err + } + obstacles, err := svc.GetObstacles(ctx, req.Extra.AsMap()) + if err != nil { + return nil, err + } + protoObs := []*commonpb.GeoObstacle{} + for _, obstacle := range obstacles { + protoObs = append(protoObs, spatialmath.GeoObstacleToProtobuf(obstacle)) + } + return &pb.GetObstaclesResponse{Obstacles: protoObs}, nil +} + // DoCommand receives arbitrary commands. -func (server *serviceServer) DoCommand(ctx context.Context, +func (server *serviceServer) DoCommand( + ctx context.Context, req *commonpb.DoCommandRequest, ) (*commonpb.DoCommandResponse, error) { svc, err := server.coll.Resource(req.Name) diff --git a/services/navigation/server_test.go b/services/navigation/server_test.go index f8365b77fa3..50aa501e7ba 100644 --- a/services/navigation/server_test.go +++ b/services/navigation/server_test.go @@ -15,6 +15,7 @@ import ( "go.viam.com/rdk/resource" "go.viam.com/rdk/services/navigation" + "go.viam.com/rdk/spatialmath" "go.viam.com/rdk/testutils" "go.viam.com/rdk/testutils/inject" ) @@ -175,9 +176,11 @@ func TestServer(t *testing.T) { t.Run("working location function", func(t *testing.T) { loc := geo.NewPoint(90, 1) - injectSvc.LocationFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, error) { + expectedCompassHeading := 90. + expectedGeoPose := spatialmath.NewGeoPose(loc, expectedCompassHeading) + injectSvc.LocationFunc = func(ctx context.Context, extra map[string]interface{}) (*spatialmath.GeoPose, error) { extraOptions = extra - return loc, nil + return expectedGeoPose, nil } extra := map[string]interface{}{"foo": "Location"} ext, err := protoutils.StructToStructPb(extra) @@ -190,10 +193,11 @@ func TestServer(t *testing.T) { test.That(t, protoLoc.GetLatitude(), test.ShouldEqual, loc.Lat()) test.That(t, protoLoc.GetLongitude(), test.ShouldEqual, loc.Lng()) test.That(t, extraOptions, test.ShouldResemble, extra) + test.That(t, resp.GetCompassHeading(), test.ShouldEqual, 90.) }) t.Run("failing location function", func(t *testing.T) { - injectSvc.LocationFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, error) { + injectSvc.LocationFunc = func(ctx context.Context, extra map[string]interface{}) (*spatialmath.GeoPose, error) { return nil, errors.New("location retrieval failed") } req := &pb.GetLocationRequest{Name: testSvcName1.ShortName()} diff --git a/services/navigation/store.go b/services/navigation/store.go index 8a0b4f739d7..d1cccc7d7a6 100644 --- a/services/navigation/store.go +++ b/services/navigation/store.go @@ -1,4 +1,3 @@ -// Package navigation implements the navigation service. package navigation import ( @@ -81,6 +80,9 @@ type MemoryNavigationStore struct { // Waypoints returns a copy of all of the waypoints in the MemoryNavigationStore. func (store *MemoryNavigationStore) Waypoints(ctx context.Context) ([]Waypoint, error) { + if ctx.Err() != nil { + return nil, ctx.Err() + } store.mu.RLock() defer store.mu.RUnlock() wps := make([]Waypoint, 0, len(store.waypoints)) @@ -96,6 +98,9 @@ func (store *MemoryNavigationStore) Waypoints(ctx context.Context) ([]Waypoint, // AddWaypoint adds a waypoint to the MemoryNavigationStore. func (store *MemoryNavigationStore) AddWaypoint(ctx context.Context, point *geo.Point) (Waypoint, error) { + if ctx.Err() != nil { + return Waypoint{}, ctx.Err() + } store.mu.Lock() defer store.mu.Unlock() newPoint := Waypoint{ @@ -109,6 +114,9 @@ func (store *MemoryNavigationStore) AddWaypoint(ctx context.Context, point *geo. // RemoveWaypoint removes a waypoint from the MemoryNavigationStore. func (store *MemoryNavigationStore) RemoveWaypoint(ctx context.Context, id primitive.ObjectID) error { + if ctx.Err() != nil { + return ctx.Err() + } store.mu.Lock() defer store.mu.Unlock() newWps := make([]*Waypoint, 0, len(store.waypoints)-1) @@ -124,6 +132,9 @@ func (store *MemoryNavigationStore) RemoveWaypoint(ctx context.Context, id primi // NextWaypoint gets the next waypoint that has not been visited. func (store *MemoryNavigationStore) NextWaypoint(ctx context.Context) (Waypoint, error) { + if ctx.Err() != nil { + return Waypoint{}, ctx.Err() + } store.mu.RLock() defer store.mu.RUnlock() for _, wp := range store.waypoints { @@ -136,6 +147,9 @@ func (store *MemoryNavigationStore) NextWaypoint(ctx context.Context) (Waypoint, // WaypointVisited sets that a waypoint has been visited. func (store *MemoryNavigationStore) WaypointVisited(ctx context.Context, id primitive.ObjectID) error { + if ctx.Err() != nil { + return ctx.Err() + } store.mu.Lock() defer store.mu.Unlock() for _, wp := range store.waypoints { diff --git a/services/slam/client.go b/services/slam/client.go index 2e766245358..dc018e9b15b 100644 --- a/services/slam/client.go +++ b/services/slam/client.go @@ -44,9 +44,9 @@ func NewClientFromConn( return c, nil } -// GetPosition creates a request, calls the slam service GetPosition, and parses the response into a Pose with a component reference string. -func (c *client) GetPosition(ctx context.Context) (spatialmath.Pose, string, error) { - ctx, span := trace.StartSpan(ctx, "slam::client::GetPosition") +// Position creates a request, calls the slam service Position, and parses the response into a Pose with a component reference string. +func (c *client) Position(ctx context.Context) (spatialmath.Pose, string, error) { + ctx, span := trace.StartSpan(ctx, "slam::client::Position") defer span.End() req := &pb.GetPositionRequest{ @@ -64,28 +64,28 @@ func (c *client) GetPosition(ctx context.Context) (spatialmath.Pose, string, err return spatialmath.NewPoseFromProtobuf(p), componentReference, nil } -// GetPointCloudMap creates a request, calls the slam service GetPointCloudMap and returns a callback +// PointCloudMap creates a request, calls the slam service PointCloudMap and returns a callback // function which will return the next chunk of the current pointcloud map when called. -func (c *client) GetPointCloudMap(ctx context.Context) (func() ([]byte, error), error) { - ctx, span := trace.StartSpan(ctx, "slam::client::GetPointCloudMap") +func (c *client) PointCloudMap(ctx context.Context) (func() ([]byte, error), error) { + ctx, span := trace.StartSpan(ctx, "slam::client::PointCloudMap") defer span.End() - return grpchelper.GetPointCloudMapCallback(ctx, c.name, c.client) + return grpchelper.PointCloudMapCallback(ctx, c.name, c.client) } -// GetInternalState creates a request, calls the slam service GetInternalState and returns a callback +// InternalState creates a request, calls the slam service InternalState and returns a callback // function which will return the next chunk of the current internal state of the slam algo when called. -func (c *client) GetInternalState(ctx context.Context) (func() ([]byte, error), error) { - ctx, span := trace.StartSpan(ctx, "slam::client::GetInternalState") +func (c *client) InternalState(ctx context.Context) (func() ([]byte, error), error) { + ctx, span := trace.StartSpan(ctx, "slam::client::InternalState") defer span.End() - return grpchelper.GetInternalStateCallback(ctx, c.name, c.client) + return grpchelper.InternalStateCallback(ctx, c.name, c.client) } -// GetLatestMapInfo creates a request, calls the slam service GetLatestMapInfo, and +// LatestMapInfo creates a request, calls the slam service LatestMapInfo, and // returns the timestamp of the last update to the map. -func (c *client) GetLatestMapInfo(ctx context.Context) (time.Time, error) { - ctx, span := trace.StartSpan(ctx, "slam::client::GetLatestMapInfo") +func (c *client) LatestMapInfo(ctx context.Context) (time.Time, error) { + ctx, span := trace.StartSpan(ctx, "slam::client::LatestMapInfo") defer span.End() req := &pb.GetLatestMapInfoRequest{ diff --git a/services/slam/client_test.go b/services/slam/client_test.go index 5795b19360b..065298d3675 100644 --- a/services/slam/client_test.go +++ b/services/slam/client_test.go @@ -57,11 +57,11 @@ func TestClientWorkingService(t *testing.T) { workingSLAMService := &inject.SLAMService{} - workingSLAMService.GetPositionFunc = func(ctx context.Context) (spatial.Pose, string, error) { + workingSLAMService.PositionFunc = func(ctx context.Context) (spatial.Pose, string, error) { return poseSucc, componentRefSucc, nil } - workingSLAMService.GetPointCloudMapFunc = func(ctx context.Context) (func() ([]byte, error), error) { + workingSLAMService.PointCloudMapFunc = func(ctx context.Context) (func() ([]byte, error), error) { reader := bytes.NewReader(pcd) clientBuffer := make([]byte, chunkSizePointCloud) f := func() ([]byte, error) { @@ -74,7 +74,7 @@ func TestClientWorkingService(t *testing.T) { return f, nil } - workingSLAMService.GetInternalStateFunc = func(ctx context.Context) (func() ([]byte, error), error) { + workingSLAMService.InternalStateFunc = func(ctx context.Context) (func() ([]byte, error), error) { reader := bytes.NewReader(internalStateSucc) clientBuffer := make([]byte, chunkSizeInternalState) f := func() ([]byte, error) { @@ -88,7 +88,7 @@ func TestClientWorkingService(t *testing.T) { return f, nil } - workingSLAMService.GetLatestMapInfoFunc = func(ctx context.Context) (time.Time, error) { + workingSLAMService.LatestMapInfoFunc = func(ctx context.Context) (time.Time, error) { return timestampSucc, nil } @@ -118,27 +118,27 @@ func TestClientWorkingService(t *testing.T) { workingSLAMClient, err := slam.NewClientFromConn(context.Background(), conn, "", slam.Named(nameSucc), logger) test.That(t, err, test.ShouldBeNil) - // test get position - pose, componentRef, err := workingSLAMClient.GetPosition(context.Background()) + // test position + pose, componentRef, err := workingSLAMClient.Position(context.Background()) test.That(t, err, test.ShouldBeNil) test.That(t, spatial.PoseAlmostEqual(poseSucc, pose), test.ShouldBeTrue) test.That(t, componentRef, test.ShouldEqual, componentRefSucc) - // test get point cloud map - fullBytesPCD, err := slam.GetPointCloudMapFull(context.Background(), workingSLAMClient) + // test point cloud map + fullBytesPCD, err := slam.PointCloudMapFull(context.Background(), workingSLAMClient) test.That(t, err, test.ShouldBeNil) // comparing raw bytes to ensure order is correct test.That(t, fullBytesPCD, test.ShouldResemble, pcd) // comparing pointclouds to ensure PCDs are correct testhelper.TestComparePointCloudsFromPCDs(t, fullBytesPCD, pcd) - // test get internal state - fullBytesInternalState, err := slam.GetInternalStateFull(context.Background(), workingSLAMClient) + // test internal state + fullBytesInternalState, err := slam.InternalStateFull(context.Background(), workingSLAMClient) test.That(t, err, test.ShouldBeNil) test.That(t, fullBytesInternalState, test.ShouldResemble, internalStateSucc) - // test get latest map info - timestamp, err := workingSLAMClient.GetLatestMapInfo(context.Background()) + // test latest map info + timestamp, err := workingSLAMClient.LatestMapInfo(context.Background()) test.That(t, err, test.ShouldBeNil) test.That(t, timestamp, test.ShouldResemble, timestampSucc) @@ -151,27 +151,27 @@ func TestClientWorkingService(t *testing.T) { workingDialedClient, err := slam.NewClientFromConn(context.Background(), conn, "", slam.Named(nameSucc), logger) test.That(t, err, test.ShouldBeNil) - // test get position - pose, componentRef, err := workingDialedClient.GetPosition(context.Background()) + // test position + pose, componentRef, err := workingDialedClient.Position(context.Background()) test.That(t, err, test.ShouldBeNil) test.That(t, spatial.PoseAlmostEqual(poseSucc, pose), test.ShouldBeTrue) test.That(t, componentRef, test.ShouldEqual, componentRefSucc) - // test get point cloud map - fullBytesPCD, err := slam.GetPointCloudMapFull(context.Background(), workingDialedClient) + // test point cloud map + fullBytesPCD, err := slam.PointCloudMapFull(context.Background(), workingDialedClient) test.That(t, err, test.ShouldBeNil) // comparing raw bytes to ensure order is correct test.That(t, fullBytesPCD, test.ShouldResemble, pcd) // comparing pointclouds to ensure PCDs are correct testhelper.TestComparePointCloudsFromPCDs(t, fullBytesPCD, pcd) - // test get internal state - fullBytesInternalState, err := slam.GetInternalStateFull(context.Background(), workingDialedClient) + // test internal state + fullBytesInternalState, err := slam.InternalStateFull(context.Background(), workingDialedClient) test.That(t, err, test.ShouldBeNil) test.That(t, fullBytesInternalState, test.ShouldResemble, internalStateSucc) - // test get latest map info - timestamp, err := workingDialedClient.GetLatestMapInfo(context.Background()) + // test latest map info + timestamp, err := workingDialedClient.LatestMapInfo(context.Background()) test.That(t, err, test.ShouldBeNil) test.That(t, timestamp, test.ShouldResemble, timestampSucc) @@ -192,27 +192,27 @@ func TestClientWorkingService(t *testing.T) { dialedClient, err := resourceAPI.RPCClient(context.Background(), conn, "", slam.Named(nameSucc), logger) test.That(t, err, test.ShouldBeNil) - // test get position - pose, componentRef, err := dialedClient.GetPosition(context.Background()) + // test position + pose, componentRef, err := dialedClient.Position(context.Background()) test.That(t, err, test.ShouldBeNil) test.That(t, spatial.PoseAlmostEqual(poseSucc, pose), test.ShouldBeTrue) test.That(t, componentRef, test.ShouldEqual, componentRefSucc) - // test get point cloud map - fullBytesPCD, err := slam.GetPointCloudMapFull(context.Background(), dialedClient) + // test point cloud map + fullBytesPCD, err := slam.PointCloudMapFull(context.Background(), dialedClient) test.That(t, err, test.ShouldBeNil) // comparing raw bytes to ensure order is correct test.That(t, fullBytesPCD, test.ShouldResemble, pcd) // comparing pointclouds to ensure PCDs are correct testhelper.TestComparePointCloudsFromPCDs(t, fullBytesPCD, pcd) - // test get internal state - fullBytesInternalState, err := slam.GetInternalStateFull(context.Background(), dialedClient) + // test internal state + fullBytesInternalState, err := slam.InternalStateFull(context.Background(), dialedClient) test.That(t, err, test.ShouldBeNil) test.That(t, fullBytesInternalState, test.ShouldResemble, internalStateSucc) - // test get latest map info - timestamp, err := dialedClient.GetLatestMapInfo(context.Background()) + // test latest map info + timestamp, err := dialedClient.LatestMapInfo(context.Background()) test.That(t, err, test.ShouldBeNil) test.That(t, timestamp, test.ShouldResemble, timestampSucc) @@ -235,19 +235,19 @@ func TestFailingClient(t *testing.T) { failingSLAMService := &inject.SLAMService{} - failingSLAMService.GetPositionFunc = func(ctx context.Context) (spatial.Pose, string, error) { + failingSLAMService.PositionFunc = func(ctx context.Context) (spatial.Pose, string, error) { return nil, "", errors.New("failure to get position") } - failingSLAMService.GetPointCloudMapFunc = func(ctx context.Context) (func() ([]byte, error), error) { + failingSLAMService.PointCloudMapFunc = func(ctx context.Context) (func() ([]byte, error), error) { return nil, errors.New("failure during get pointcloud map") } - failingSLAMService.GetInternalStateFunc = func(ctx context.Context) (func() ([]byte, error), error) { + failingSLAMService.InternalStateFunc = func(ctx context.Context) (func() ([]byte, error), error) { return nil, errors.New("failure during get internal state") } - failingSLAMService.GetLatestMapInfoFunc = func(ctx context.Context) (time.Time, error) { + failingSLAMService.LatestMapInfoFunc = func(ctx context.Context) (time.Time, error) { return time.Time{}, errors.New("failure to get latest map info") } @@ -273,43 +273,43 @@ func TestFailingClient(t *testing.T) { ctx := context.Background() cancelCtx, cancelFunc := context.WithCancel(ctx) cancelFunc() - _, err = failingSLAMClient.GetPointCloudMap(cancelCtx) + _, err = failingSLAMClient.PointCloudMap(cancelCtx) test.That(t, err.Error(), test.ShouldContainSubstring, "context cancel") - _, err = failingSLAMClient.GetInternalState(cancelCtx) + _, err = failingSLAMClient.InternalState(cancelCtx) test.That(t, err.Error(), test.ShouldContainSubstring, "context cancel") - // test get position - pose, componentRef, err := failingSLAMClient.GetPosition(context.Background()) + // test position + pose, componentRef, err := failingSLAMClient.Position(context.Background()) test.That(t, err.Error(), test.ShouldContainSubstring, "failure to get position") test.That(t, pose, test.ShouldBeNil) test.That(t, componentRef, test.ShouldBeEmpty) - // test get pointcloud map - fullBytesPCD, err := slam.GetPointCloudMapFull(context.Background(), failingSLAMClient) + // test pointcloud map + fullBytesPCD, err := slam.PointCloudMapFull(context.Background(), failingSLAMClient) test.That(t, err.Error(), test.ShouldContainSubstring, "failure during get pointcloud map") test.That(t, fullBytesPCD, test.ShouldBeNil) - // test get internal state - fullBytesInternalState, err := slam.GetInternalStateFull(context.Background(), failingSLAMClient) + // test internal state + fullBytesInternalState, err := slam.InternalStateFull(context.Background(), failingSLAMClient) test.That(t, err.Error(), test.ShouldContainSubstring, "failure during get internal state") test.That(t, fullBytesInternalState, test.ShouldBeNil) - // test get latest map info - timestamp, err := failingSLAMClient.GetLatestMapInfo(context.Background()) + // test latest map info + timestamp, err := failingSLAMClient.LatestMapInfo(context.Background()) test.That(t, err.Error(), test.ShouldContainSubstring, "failure to get latest map info") test.That(t, timestamp, test.ShouldResemble, time.Time{}) test.That(t, conn.Close(), test.ShouldBeNil) }) - failingSLAMService.GetPointCloudMapFunc = func(ctx context.Context) (func() ([]byte, error), error) { + failingSLAMService.PointCloudMapFunc = func(ctx context.Context) (func() ([]byte, error), error) { f := func() ([]byte, error) { return nil, errors.New("failure during callback") } return f, nil } - failingSLAMService.GetInternalStateFunc = func(ctx context.Context) (func() ([]byte, error), error) { + failingSLAMService.InternalStateFunc = func(ctx context.Context) (func() ([]byte, error), error) { f := func() ([]byte, error) { return nil, errors.New("failure during callback") } @@ -323,13 +323,13 @@ func TestFailingClient(t *testing.T) { failingSLAMClient, err := slam.NewClientFromConn(context.Background(), conn, "", slam.Named(nameFail), logger) test.That(t, err, test.ShouldBeNil) - // test get pointcloud map - fullBytesPCD, err := slam.GetPointCloudMapFull(context.Background(), failingSLAMClient) + // test pointcloud map + fullBytesPCD, err := slam.PointCloudMapFull(context.Background(), failingSLAMClient) test.That(t, err.Error(), test.ShouldContainSubstring, "failure during callback") test.That(t, fullBytesPCD, test.ShouldBeNil) - // test get internal state - fullBytesInternalState, err := slam.GetInternalStateFull(context.Background(), failingSLAMClient) + // test internal state + fullBytesInternalState, err := slam.InternalStateFull(context.Background(), failingSLAMClient) test.That(t, err.Error(), test.ShouldContainSubstring, "failure during callback") test.That(t, fullBytesInternalState, test.ShouldBeNil) diff --git a/services/slam/collector.go b/services/slam/collector.go new file mode 100644 index 00000000000..c2685ba7154 --- /dev/null +++ b/services/slam/collector.go @@ -0,0 +1,74 @@ +package slam + +import ( + "context" + + pb "go.viam.com/api/service/slam/v1" + "google.golang.org/protobuf/types/known/anypb" + + "go.viam.com/rdk/data" + "go.viam.com/rdk/spatialmath" +) + +type method int64 + +const ( + position method = iota + pointCloudMap +) + +func (m method) String() string { + if m == position { + return "Position" + } + if m == pointCloudMap { + return "PointCloudMap" + } + return "Unknown" +} + +func newPositionCollector(resource interface{}, params data.CollectorParams) (data.Collector, error) { + slam, err := assertSLAM(resource) + if err != nil { + return nil, err + } + + cFunc := data.CaptureFunc(func(ctx context.Context, _ map[string]*anypb.Any) (interface{}, error) { + pose, componentRef, err := slam.Position(ctx) + if err != nil { + return nil, data.FailedToReadErr(params.ComponentName, position.String(), err) + } + return &pb.GetPositionResponse{Pose: spatialmath.PoseToProtobuf(pose), ComponentReference: componentRef}, nil + }) + return data.NewCollector(cFunc, params) +} + +func newPointCloudMapCollector(resource interface{}, params data.CollectorParams) (data.Collector, error) { + slam, err := assertSLAM(resource) + if err != nil { + return nil, err + } + + cFunc := data.CaptureFunc(func(ctx context.Context, _ map[string]*anypb.Any) (interface{}, error) { + f, err := slam.PointCloudMap(ctx) + if err != nil { + return nil, data.FailedToReadErr(params.ComponentName, pointCloudMap.String(), err) + } + + pcd, err := HelperConcatenateChunksToFull(f) + if err != nil { + return nil, data.FailedToReadErr(params.ComponentName, pointCloudMap.String(), err) + } + + return pcd, nil + }) + return data.NewCollector(cFunc, params) +} + +func assertSLAM(resource interface{}) (Service, error) { + slamService, ok := resource.(Service) + if !ok { + return nil, data.InvalidInterfaceErr(API) + } + return slamService, nil +} diff --git a/services/slam/fake/data_loader.go b/services/slam/fake/data_loader.go index 3c548b46873..426f897513b 100644 --- a/services/slam/fake/data_loader.go +++ b/services/slam/fake/data_loader.go @@ -47,7 +47,7 @@ const ( positionTemplate = "%s/position/position_%d.json" ) -func fakeGetPointCloudMap(_ context.Context, datasetDir string, slamSvc *SLAM) (func() ([]byte, error), error) { +func fakePointCloudMap(_ context.Context, datasetDir string, slamSvc *SLAM) (func() ([]byte, error), error) { path := filepath.Clean(artifact.MustPath(fmt.Sprintf(pcdTemplate, datasetDir, slamSvc.getCount()))) slamSvc.logger.Debug("Reading " + path) file, err := os.Open(path) @@ -66,7 +66,7 @@ func fakeGetPointCloudMap(_ context.Context, datasetDir string, slamSvc *SLAM) ( return f, nil } -func fakeGetInternalState(_ context.Context, datasetDir string, slamSvc *SLAM) (func() ([]byte, error), error) { +func fakeInternalState(_ context.Context, datasetDir string, slamSvc *SLAM) (func() ([]byte, error), error) { path := filepath.Clean(artifact.MustPath(fmt.Sprintf(internalStateTemplate, datasetDir, slamSvc.getCount()))) slamSvc.logger.Debug("Reading " + path) file, err := os.Open(path) @@ -85,7 +85,7 @@ func fakeGetInternalState(_ context.Context, datasetDir string, slamSvc *SLAM) ( return f, nil } -func fakeGetPosition(_ context.Context, datasetDir string, slamSvc *SLAM) (spatialmath.Pose, string, error) { +func fakePosition(_ context.Context, datasetDir string, slamSvc *SLAM) (spatialmath.Pose, string, error) { path := filepath.Clean(artifact.MustPath(fmt.Sprintf(positionTemplate, datasetDir, slamSvc.getCount()))) slamSvc.logger.Debug("Reading " + path) data, err := os.ReadFile(path) diff --git a/services/slam/fake/slam.go b/services/slam/fake/slam.go index 659b5ec8540..dd4fc116964 100644 --- a/services/slam/fake/slam.go +++ b/services/slam/fake/slam.go @@ -64,35 +64,36 @@ func (slamSvc *SLAM) getCount() int { return slamSvc.dataCount } -// GetPosition returns a Pose and a component reference string of the robot's current location according to SLAM. -func (slamSvc *SLAM) GetPosition(ctx context.Context) (spatialmath.Pose, string, error) { - ctx, span := trace.StartSpan(ctx, "slam::fake::GetPosition") +// Position returns a Pose and a component reference string of the robot's current location according to SLAM. +func (slamSvc *SLAM) Position(ctx context.Context) (spatialmath.Pose, string, error) { + ctx, span := trace.StartSpan(ctx, "slam::fake::Position") defer span.End() - return fakeGetPosition(ctx, datasetDirectory, slamSvc) + return fakePosition(ctx, datasetDirectory, slamSvc) } -// GetPointCloudMap returns a callback function which will return the next chunk of the current pointcloud +// PointCloudMap returns a callback function which will return the next chunk of the current pointcloud // map. -func (slamSvc *SLAM) GetPointCloudMap(ctx context.Context) (func() ([]byte, error), error) { - ctx, span := trace.StartSpan(ctx, "slam::fake::GetPointCloudMap") +func (slamSvc *SLAM) PointCloudMap(ctx context.Context) (func() ([]byte, error), error) { + ctx, span := trace.StartSpan(ctx, "slam::fake::PointCloudMap") defer span.End() slamSvc.incrementDataCount() - slamSvc.mapTimestamp = time.Now().UTC() - return fakeGetPointCloudMap(ctx, datasetDirectory, slamSvc) + return fakePointCloudMap(ctx, datasetDirectory, slamSvc) } -// GetInternalState returns a callback function which will return the next chunk of the current internal +// InternalState returns a callback function which will return the next chunk of the current internal // state of the slam algo. -func (slamSvc *SLAM) GetInternalState(ctx context.Context) (func() ([]byte, error), error) { - ctx, span := trace.StartSpan(ctx, "slam::fake::GetInternalState") +func (slamSvc *SLAM) InternalState(ctx context.Context) (func() ([]byte, error), error) { + ctx, span := trace.StartSpan(ctx, "slam::fake::InternalState") defer span.End() - return fakeGetInternalState(ctx, datasetDirectory, slamSvc) + return fakeInternalState(ctx, datasetDirectory, slamSvc) } -// GetLatestMapInfo returns a message indicating details regarding the latest map returned to the system. -func (slamSvc *SLAM) GetLatestMapInfo(ctx context.Context) (time.Time, error) { - _, span := trace.StartSpan(ctx, "slam::fake::GetLatestMapInfo") +// LatestMapInfo returns information used to determine whether the slam mode is localizing. +// Fake Slam is always in mapping mode, so it always returns a new timestamp. +func (slamSvc *SLAM) LatestMapInfo(ctx context.Context) (time.Time, error) { + _, span := trace.StartSpan(ctx, "slam::fake::LatestMapInfo") defer span.End() + slamSvc.mapTimestamp = time.Now().UTC() return slamSvc.mapTimestamp, nil } @@ -102,9 +103,9 @@ func (slamSvc *SLAM) incrementDataCount() { slamSvc.dataCount = ((slamSvc.dataCount + 1) % maxDataCount) } -// GetLimits returns the bounds of the slam map as a list of referenceframe.Limits. -func (slamSvc *SLAM) GetLimits(ctx context.Context) ([]referenceframe.Limit, error) { - data, err := slam.GetPointCloudMapFull(ctx, slamSvc) +// Limits returns the bounds of the slam map as a list of referenceframe.Limits. +func (slamSvc *SLAM) Limits(ctx context.Context) ([]referenceframe.Limit, error) { + data, err := slam.PointCloudMapFull(ctx, slamSvc) if err != nil { return nil, err } diff --git a/services/slam/fake/slam_test.go b/services/slam/fake/slam_test.go index 9ae2aaa33c7..eab3cdf36a1 100644 --- a/services/slam/fake/slam_test.go +++ b/services/slam/fake/slam_test.go @@ -20,11 +20,11 @@ import ( "go.viam.com/rdk/spatialmath" ) -func TestFakeSLAMGetPosition(t *testing.T) { +func TestFakeSLAMPosition(t *testing.T) { expectedComponentReference := "" slamSvc := NewSLAM(slam.Named("test"), golog.NewTestLogger(t)) - p, componentReference, err := slamSvc.GetPosition(context.Background()) + p, componentReference, err := slamSvc.Position(context.Background()) test.That(t, err, test.ShouldBeNil) test.That(t, componentReference, test.ShouldEqual, expectedComponentReference) @@ -36,26 +36,21 @@ func TestFakeSLAMGetPosition(t *testing.T) { &spatialmath.Quaternion{Real: 0.9999997195238413, Imag: 0, Jmag: 0, Kmag: 0.0007489674483818071}) test.That(t, spatialmath.PoseAlmostEqual(p, expectedPose), test.ShouldBeTrue) - p2, componentReference, err := slamSvc.GetPosition(context.Background()) + p2, componentReference, err := slamSvc.Position(context.Background()) test.That(t, err, test.ShouldBeNil) test.That(t, componentReference, test.ShouldEqual, expectedComponentReference) test.That(t, p, test.ShouldResemble, p2) } -func TestFakeSLAMGetLatestMapInfo(t *testing.T) { +func TestFakeSLAMLatestMapInfo(t *testing.T) { slamSvc := NewSLAM(slam.Named("test"), golog.NewTestLogger(t)) - timestamp1, err := slamSvc.GetLatestMapInfo(context.Background()) + timestamp1, err := slamSvc.LatestMapInfo(context.Background()) test.That(t, err, test.ShouldBeNil) - timestamp2, err := slamSvc.GetLatestMapInfo(context.Background()) - test.That(t, err, test.ShouldBeNil) - test.That(t, timestamp1, test.ShouldResemble, timestamp2) - _, err = slamSvc.GetPointCloudMap(context.Background()) - test.That(t, err, test.ShouldBeNil) - timestamp3, err := slamSvc.GetLatestMapInfo(context.Background()) + timestamp2, err := slamSvc.LatestMapInfo(context.Background()) test.That(t, err, test.ShouldBeNil) - test.That(t, timestamp3.After(timestamp2), test.ShouldBeTrue) + test.That(t, timestamp2.After(timestamp1), test.ShouldBeTrue) } func TestFakeSLAMStateful(t *testing.T) { @@ -67,11 +62,11 @@ func TestFakeSLAMStateful(t *testing.T) { // maxDataCount lowered under test to reduce test runtime maxDataCount = 5 slamSvc := &SLAM{Named: slam.Named("test").AsNamed(), logger: golog.NewTestLogger(t)} - verifyGetPointCloudMapStateful(t, slamSvc) + verifyPointCloudMapStateful(t, slamSvc) }) } -func TestFakeSLAMGetInternalState(t *testing.T) { +func TestFakeSLAMInternalState(t *testing.T) { testName := "Returns a callback function which, returns the current fake internal state in chunks" t.Run(testName, func(t *testing.T) { slamSvc := NewSLAM(slam.Named("test"), golog.NewTestLogger(t)) @@ -80,23 +75,23 @@ func TestFakeSLAMGetInternalState(t *testing.T) { expectedData, err := os.ReadFile(path) test.That(t, err, test.ShouldBeNil) - data := getDataFromStream(t, slamSvc.GetInternalState) + data := getDataFromStream(t, slamSvc.InternalState) test.That(t, len(data), test.ShouldBeGreaterThan, 0) test.That(t, data, test.ShouldResemble, expectedData) - data2 := getDataFromStream(t, slamSvc.GetInternalState) + data2 := getDataFromStream(t, slamSvc.InternalState) test.That(t, len(data2), test.ShouldBeGreaterThan, 0) test.That(t, data, test.ShouldResemble, data2) test.That(t, data2, test.ShouldResemble, expectedData) }) } -func TestFakeSLAMGetPointMap(t *testing.T) { +func TestFakeSLAMPointMap(t *testing.T) { testName := "Returns a callback function which, returns the current fake pointcloud map state in chunks and advances the dataset" t.Run(testName, func(t *testing.T) { slamSvc := NewSLAM(slam.Named("test"), golog.NewTestLogger(t)) - data := getDataFromStream(t, slamSvc.GetPointCloudMap) + data := getDataFromStream(t, slamSvc.PointCloudMap) test.That(t, len(data), test.ShouldBeGreaterThan, 0) path := filepath.Clean(artifact.MustPath(fmt.Sprintf(pcdTemplate, datasetDirectory, slamSvc.getCount()))) @@ -105,7 +100,7 @@ func TestFakeSLAMGetPointMap(t *testing.T) { test.That(t, data, test.ShouldResemble, expectedData) - data2 := getDataFromStream(t, slamSvc.GetPointCloudMap) + data2 := getDataFromStream(t, slamSvc.PointCloudMap) test.That(t, len(data2), test.ShouldBeGreaterThan, 0) path2 := filepath.Clean(artifact.MustPath(fmt.Sprintf(pcdTemplate, datasetDirectory, slamSvc.getCount()))) @@ -135,7 +130,7 @@ func reverse[T any](slice []T) []T { return slice } -func verifyGetPointCloudMapStateful(t *testing.T, slamSvc *SLAM) { +func verifyPointCloudMapStateful(t *testing.T, slamSvc *SLAM) { testDataCount := maxDataCount getPointCloudMapResults := []float64{} getPositionResults := []spatialmath.Pose{} @@ -143,7 +138,7 @@ func verifyGetPointCloudMapStateful(t *testing.T, slamSvc *SLAM) { // Call GetPointCloudMap twice for every testData artifact for i := 0; i < testDataCount*2; i++ { - f, err := slamSvc.GetPointCloudMap(context.Background()) + f, err := slamSvc.PointCloudMap(context.Background()) test.That(t, err, test.ShouldBeNil) test.That(t, f, test.ShouldNotBeNil) pcd, err := helperConcatenateChunksToFull(f) @@ -154,11 +149,11 @@ func verifyGetPointCloudMapStateful(t *testing.T, slamSvc *SLAM) { getPointCloudMapResults = append(getPointCloudMapResults, pc.MetaData().MaxX) test.That(t, err, test.ShouldBeNil) - p, _, err := slamSvc.GetPosition(context.Background()) + p, _, err := slamSvc.Position(context.Background()) test.That(t, err, test.ShouldBeNil) getPositionResults = append(getPositionResults, p) - f, err = slamSvc.GetInternalState(context.Background()) + f, err = slamSvc.InternalState(context.Background()) test.That(t, err, test.ShouldBeNil) test.That(t, f, test.ShouldNotBeNil) internalState, err := helperConcatenateChunksToFull(f) diff --git a/services/slam/grpchelper/grpchelper.go b/services/slam/grpchelper/grpchelper.go index bb78286528f..7b795cd73e8 100644 --- a/services/slam/grpchelper/grpchelper.go +++ b/services/slam/grpchelper/grpchelper.go @@ -8,9 +8,9 @@ import ( pb "go.viam.com/api/service/slam/v1" ) -// GetPointCloudMapCallback helps a client request the point cloud stream from a SLAM server, +// PointCloudMapCallback helps a client request the point cloud stream from a SLAM server, // returning a callback function for accessing the stream data. -func GetPointCloudMapCallback(ctx context.Context, name string, slamClient pb.SLAMServiceClient) (func() ([]byte, error), error) { +func PointCloudMapCallback(ctx context.Context, name string, slamClient pb.SLAMServiceClient) (func() ([]byte, error), error) { req := &pb.GetPointCloudMapRequest{Name: name} // If the target gRPC server returns an error status, this call doesn't return an error. @@ -33,9 +33,9 @@ func GetPointCloudMapCallback(ctx context.Context, name string, slamClient pb.SL return f, nil } -// GetInternalStateCallback helps a client request the internal state stream from a SLAM server, +// InternalStateCallback helps a client request the internal state stream from a SLAM server, // returning a callback function for accessing the stream data. -func GetInternalStateCallback(ctx context.Context, name string, slamClient pb.SLAMServiceClient) (func() ([]byte, error), error) { +func InternalStateCallback(ctx context.Context, name string, slamClient pb.SLAMServiceClient) (func() ([]byte, error), error) { req := &pb.GetInternalStateRequest{Name: name} // If the target gRPC server returns an error status, this call doesn't return an error. diff --git a/services/slam/server.go b/services/slam/server.go index b81200c1760..f2fb8182d54 100644 --- a/services/slam/server.go +++ b/services/slam/server.go @@ -39,7 +39,7 @@ func (server *serviceServer) GetPosition(ctx context.Context, req *pb.GetPositio return nil, err } - p, componentReference, err := svc.GetPosition(ctx) + p, componentReference, err := svc.Position(ctx) if err != nil { return nil, err } @@ -65,9 +65,9 @@ func (server *serviceServer) GetPointCloudMap(req *pb.GetPointCloudMapRequest, return err } - f, err := svc.GetPointCloudMap(ctx) + f, err := svc.PointCloudMap(ctx) if err != nil { - return errors.Wrap(err, "getting callback function from GetPointCloudMap encountered an issue") + return errors.Wrap(err, "getting callback function from PointCloudMap encountered an issue") } // In the future, channel buffer could be used here to optimize for latency @@ -103,7 +103,7 @@ func (server *serviceServer) GetInternalState(req *pb.GetInternalStateRequest, return err } - f, err := svc.GetInternalState(ctx) + f, err := svc.InternalState(ctx) if err != nil { return err } @@ -139,7 +139,7 @@ func (server *serviceServer) GetLatestMapInfo(ctx context.Context, req *pb.GetLa return nil, err } - mapTimestamp, err := svc.GetLatestMapInfo(ctx) + mapTimestamp, err := svc.LatestMapInfo(ctx) if err != nil { return nil, err } diff --git a/services/slam/server_test.go b/services/slam/server_test.go index ea1ee53e6ae..38baa88f0d8 100644 --- a/services/slam/server_test.go +++ b/services/slam/server_test.go @@ -78,7 +78,7 @@ func TestWorkingServer(t *testing.T) { poseSucc := spatial.NewPose(r3.Vector{X: 1, Y: 2, Z: 3}, &spatial.OrientationVector{Theta: math.Pi / 2, OX: 0, OY: 0, OZ: -1}) componentRefSucc := "cam" - injectSvc.GetPositionFunc = func(ctx context.Context) (spatial.Pose, string, error) { + injectSvc.PositionFunc = func(ctx context.Context) (spatial.Pose, string, error) { return poseSucc, componentRefSucc, nil } @@ -92,7 +92,7 @@ func TestWorkingServer(t *testing.T) { }) t.Run("working GetPointCloudMap", func(t *testing.T) { - injectSvc.GetPointCloudMapFunc = func(ctx context.Context) (func() ([]byte, error), error) { + injectSvc.PointCloudMapFunc = func(ctx context.Context) (func() ([]byte, error), error) { reader := bytes.NewReader(pcd) serverBuffer := make([]byte, chunkSizeServer) f := func() ([]byte, error) { @@ -121,7 +121,7 @@ func TestWorkingServer(t *testing.T) { t.Run("working GetInternalState", func(t *testing.T) { internalStateSucc := []byte{0, 1, 2, 3, 4} chunkSizeInternalState := 2 - injectSvc.GetInternalStateFunc = func(ctx context.Context) (func() ([]byte, error), error) { + injectSvc.InternalStateFunc = func(ctx context.Context) (func() ([]byte, error), error) { reader := bytes.NewReader(internalStateSucc) f := func() ([]byte, error) { serverBuffer := make([]byte, chunkSizeInternalState) @@ -146,7 +146,7 @@ func TestWorkingServer(t *testing.T) { t.Run("working GetLatestMapInfo", func(t *testing.T) { timestamp := time.Now().UTC() - injectSvc.GetLatestMapInfoFunc = func(ctx context.Context) (time.Time, error) { + injectSvc.LatestMapInfoFunc = func(ctx context.Context) (time.Time, error) { return timestamp, nil } @@ -170,11 +170,11 @@ func TestWorkingServer(t *testing.T) { poseSucc := spatial.NewPose(r3.Vector{X: 1, Y: 2, Z: 3}, &spatial.OrientationVector{Theta: math.Pi / 2, OX: 0, OY: 0, OZ: -1}) componentRefSucc := "cam" - injectSvc.GetPositionFunc = func(ctx context.Context) (spatial.Pose, string, error) { + injectSvc.PositionFunc = func(ctx context.Context) (spatial.Pose, string, error) { return poseSucc, componentRefSucc, nil } - injectSvc.GetPointCloudMapFunc = func(ctx context.Context) (func() ([]byte, error), error) { + injectSvc.PointCloudMapFunc = func(ctx context.Context) (func() ([]byte, error), error) { reader := bytes.NewReader(pcd) serverBuffer := make([]byte, chunkSizeServer) f := func() ([]byte, error) { @@ -231,7 +231,7 @@ func TestFailingServer(t *testing.T) { slamServer := slam.NewRPCServiceServer(injectAPISvc).(pb.SLAMServiceServer) t.Run("failing GetPosition", func(t *testing.T) { - injectSvc.GetPositionFunc = func(ctx context.Context) (spatial.Pose, string, error) { + injectSvc.PositionFunc = func(ctx context.Context) (spatial.Pose, string, error) { return nil, "", errors.New("failure to get position") } @@ -244,8 +244,8 @@ func TestFailingServer(t *testing.T) { }) t.Run("failing GetPointCloudMap", func(t *testing.T) { - // GetPointCloudMapFunc failure - injectSvc.GetPointCloudMapFunc = func(ctx context.Context) (func() ([]byte, error), error) { + // PointCloudMapFunc failure + injectSvc.PointCloudMapFunc = func(ctx context.Context) (func() ([]byte, error), error) { return nil, errors.New("failure to get pointcloud map") } @@ -256,7 +256,7 @@ func TestFailingServer(t *testing.T) { test.That(t, err.Error(), test.ShouldContainSubstring, "failure to get pointcloud map") // Callback failure - injectSvc.GetPointCloudMapFunc = func(ctx context.Context) (func() ([]byte, error), error) { + injectSvc.PointCloudMapFunc = func(ctx context.Context) (func() ([]byte, error), error) { f := func() ([]byte, error) { return []byte{}, errors.New("callback error") } @@ -269,8 +269,8 @@ func TestFailingServer(t *testing.T) { }) t.Run("failing GetInternalState", func(t *testing.T) { - // GetInternalStateFunc error - injectSvc.GetInternalStateFunc = func(ctx context.Context) (func() ([]byte, error), error) { + // InternalStateFunc error + injectSvc.InternalStateFunc = func(ctx context.Context) (func() ([]byte, error), error) { return nil, errors.New("failure to get internal state") } @@ -280,7 +280,7 @@ func TestFailingServer(t *testing.T) { test.That(t, err.Error(), test.ShouldContainSubstring, "failure to get internal state") // Callback failure - injectSvc.GetInternalStateFunc = func(ctx context.Context) (func() ([]byte, error), error) { + injectSvc.InternalStateFunc = func(ctx context.Context) (func() ([]byte, error), error) { f := func() ([]byte, error) { return []byte{}, errors.New("callback error") } @@ -292,7 +292,7 @@ func TestFailingServer(t *testing.T) { }) t.Run("failing GetLatestMapInfo", func(t *testing.T) { - injectSvc.GetLatestMapInfoFunc = func(ctx context.Context) (time.Time, error) { + injectSvc.LatestMapInfoFunc = func(ctx context.Context) (time.Time, error) { return time.Time{}, errors.New("failure to get latest map info") } reqInfo := &pb.GetLatestMapInfoRequest{Name: testSlamServiceName} diff --git a/services/slam/slam.go b/services/slam/slam.go index 8c818c5c0fc..e363ec6d40d 100644 --- a/services/slam/slam.go +++ b/services/slam/slam.go @@ -12,6 +12,7 @@ import ( "go.opencensus.io/trace" pb "go.viam.com/api/service/slam/v1" + "go.viam.com/rdk/data" "go.viam.com/rdk/pointcloud" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" @@ -27,6 +28,14 @@ func init() { RPCServiceDesc: &pb.SLAMService_ServiceDesc, RPCClient: NewClientFromConn, }) + data.RegisterCollector(data.MethodMetadata{ + API: API, + MethodName: position.String(), + }, newPositionCollector) + data.RegisterCollector(data.MethodMetadata{ + API: API, + MethodName: pointCloudMap.String(), + }, newPointCloudMapCollector) } // SubtypeName is the name of the type of service. @@ -48,10 +57,10 @@ func FromRobot(r robot.Robot, name string) (Service, error) { // Service describes the functions that are available to the service. type Service interface { resource.Resource - GetPosition(ctx context.Context) (spatialmath.Pose, string, error) - GetPointCloudMap(ctx context.Context) (func() ([]byte, error), error) - GetInternalState(ctx context.Context) (func() ([]byte, error), error) - GetLatestMapInfo(ctx context.Context) (time.Time, error) + Position(ctx context.Context) (spatialmath.Pose, string, error) + PointCloudMap(ctx context.Context) (func() ([]byte, error), error) + InternalState(ctx context.Context) (func() ([]byte, error), error) + LatestMapInfo(ctx context.Context) (time.Time, error) } // HelperConcatenateChunksToFull concatenates the chunks from a streamed grpc endpoint. @@ -70,32 +79,32 @@ func HelperConcatenateChunksToFull(f func() ([]byte, error)) ([]byte, error) { } } -// GetPointCloudMapFull concatenates the streaming responses from GetPointCloudMap into a full point cloud. -func GetPointCloudMapFull(ctx context.Context, slamSvc Service) ([]byte, error) { - ctx, span := trace.StartSpan(ctx, "slam::GetPointCloudMapFull") +// PointCloudMapFull concatenates the streaming responses from PointCloudMap into a full point cloud. +func PointCloudMapFull(ctx context.Context, slamSvc Service) ([]byte, error) { + ctx, span := trace.StartSpan(ctx, "slam::PointCloudMapFull") defer span.End() - callback, err := slamSvc.GetPointCloudMap(ctx) + callback, err := slamSvc.PointCloudMap(ctx) if err != nil { return nil, err } return HelperConcatenateChunksToFull(callback) } -// GetInternalStateFull concatenates the streaming responses from GetInternalState into +// InternalStateFull concatenates the streaming responses from InternalState into // the internal serialized state of the slam algorithm. -func GetInternalStateFull(ctx context.Context, slamSvc Service) ([]byte, error) { - ctx, span := trace.StartSpan(ctx, "slam::GetInternalStateFull") +func InternalStateFull(ctx context.Context, slamSvc Service) ([]byte, error) { + ctx, span := trace.StartSpan(ctx, "slam::InternalStateFull") defer span.End() - callback, err := slamSvc.GetInternalState(ctx) + callback, err := slamSvc.InternalState(ctx) if err != nil { return nil, err } return HelperConcatenateChunksToFull(callback) } -// GetLimits returns the bounds of the slam map as a list of referenceframe.Limits. -func GetLimits(ctx context.Context, svc Service) ([]referenceframe.Limit, error) { - data, err := GetPointCloudMapFull(ctx, svc) +// Limits returns the bounds of the slam map as a list of referenceframe.Limits. +func Limits(ctx context.Context, svc Service) ([]referenceframe.Limit, error) { + data, err := PointCloudMapFull(ctx, svc) if err != nil { return nil, err } diff --git a/services/vision/client.go b/services/vision/client.go index ac84cda89ea..ac2c53f9e43 100644 --- a/services/vision/client.go +++ b/services/vision/client.go @@ -222,7 +222,11 @@ func protoToObjects(pco []*commonpb.PointCloudObject) ([]*vision.Object, error) } return "" }() - objects[i], err = vision.NewObjectWithLabel(pc, label) + if len(o.Geometries.Geometries) >= 1 { + objects[i], err = vision.NewObjectWithLabel(pc, label, o.Geometries.GetGeometries()[0]) + } else { + objects[i], err = vision.NewObjectWithLabel(pc, label, nil) + } if err != nil { return nil, err } diff --git a/services/vision/mlvision/classifier.go b/services/vision/mlvision/classifier.go index 506e6723fcd..550d7c17482 100644 --- a/services/vision/mlvision/classifier.go +++ b/services/vision/mlvision/classifier.go @@ -4,24 +4,27 @@ import ( "context" "image" "strconv" + "strings" + "sync" "github.com/nfnt/resize" "github.com/pkg/errors" - "go.uber.org/multierr" + "gorgonia.org/tensor" + "go.viam.com/rdk/ml" "go.viam.com/rdk/rimage" "go.viam.com/rdk/services/mlmodel" "go.viam.com/rdk/vision/classification" ) -func attemptToBuildClassifier(mlm mlmodel.Service) (classification.Classifier, error) { +func attemptToBuildClassifier(mlm mlmodel.Service, nameMap *sync.Map) (classification.Classifier, error) { md, err := mlm.Metadata(context.Background()) if err != nil { return nil, errors.New("could not get any metadata") } // Set up input type, height, width, and labels - var inHeight, inWidth uint + var inHeight, inWidth int if len(md.Inputs) < 1 { return nil, errors.New("no input tensors received") } @@ -31,37 +34,74 @@ func attemptToBuildClassifier(mlm mlmodel.Service) (classification.Classifier, e return nil, errors.Errorf("invalid length of shape array (expected 4, got %d)", shapeLen) } if shape := md.Inputs[0].Shape; getIndex(shape, 3) == 1 { - inHeight, inWidth = uint(shape[2]), uint(shape[3]) + inHeight, inWidth = shape[2], shape[3] } else { - inHeight, inWidth = uint(shape[1]), uint(shape[2]) + inHeight, inWidth = shape[1], shape[2] } return func(ctx context.Context, img image.Image) (classification.Classifications, error) { - resized := resize.Resize(inWidth, inHeight, img, resize.Bilinear) - inMap := make(map[string]interface{}) + origW, origH := img.Bounds().Dx(), img.Bounds().Dy() + resizeW := inWidth + if resizeW == -1 { + resizeW = origW + } + resizeH := inHeight + if resizeH == -1 { + resizeH = origH + } + resized := img + if (origW != resizeW) || (origH != resizeH) { + resized = resize.Resize(uint(resizeW), uint(resizeH), img, resize.Bilinear) + } + inMap := ml.Tensors{} switch inType { case UInt8: - inMap["image"] = rimage.ImageToUInt8Buffer(resized) + inMap["image"] = tensor.New( + tensor.WithShape(1, resized.Bounds().Dy(), resized.Bounds().Dx(), 3), + tensor.WithBacking(rimage.ImageToUInt8Buffer(resized)), + ) case Float32: - inMap["image"] = rimage.ImageToFloatBuffer(resized) + inMap["image"] = tensor.New( + tensor.WithShape(1, resized.Bounds().Dy(), resized.Bounds().Dx(), 3), + tensor.WithBacking(rimage.ImageToFloatBuffer(resized)), + ) default: return nil, errors.New("invalid input type. try uint8 or float32") } - outMap, err := mlm.Infer(ctx, inMap) + outMap, _, err := mlm.Infer(ctx, inMap, nil) if err != nil { return nil, err } - var err2 error - - probs, err := unpack(outMap, "probability") - if err != nil || len(probs) == 0 { - probs, err2 = unpack(outMap, DefaultOutTensorName+"0") - if err2 != nil { - return nil, multierr.Combine(err, err2) + // check if output tensor name that classifier is looking for is already present + // in the nameMap. If not, find the probability name, and cache it in the nameMap + pName, ok := nameMap.Load("probability") + if !ok { + _, ok := outMap["probability"] + if !ok { + if len(outMap) == 1 { + for name := range outMap { // only 1 element in map, assume its probabilities + nameMap.Store("probability", name) + pName = name + } + } + } else { + nameMap.Store("probability", "probability") + pName = "probability" } } - + probabilityName, ok := pName.(string) + if !ok { + return nil, errors.Errorf("name map did not store a string of the tensor name, but an object of type %T instead", pName) + } + data, ok := outMap[probabilityName] + if !ok { + return nil, errors.Errorf("no tensor named 'probability' among output tensors [%s]", strings.Join(tensorNames(outMap), ", ")) + } + probs, err := convertToFloat64Slice(data.Data()) + if err != nil { + return nil, err + } confs := checkClassificationScores(probs) if labels != nil && len(labels) != len(confs) { return nil, errors.New("length of output expected to be length of label list (but is not)") @@ -92,7 +132,7 @@ func checkIfClassifierWorks(ctx context.Context, cf classification.Classifier) e _, err := cf(ctx, img) if err != nil { - return errors.New("Cannot use model as a classifier") + return errors.Wrap(err, "Cannot use model as a classifier") } return nil } diff --git a/services/vision/mlvision/detector.go b/services/vision/mlvision/detector.go index fc4b21f60d9..d6a137d8b4d 100644 --- a/services/vision/mlvision/detector.go +++ b/services/vision/mlvision/detector.go @@ -5,25 +5,28 @@ import ( "image" "math" "strconv" + "strings" + "sync" "github.com/nfnt/resize" "github.com/pkg/errors" - "go.uber.org/multierr" + "gorgonia.org/tensor" + "go.viam.com/rdk/ml" "go.viam.com/rdk/rimage" "go.viam.com/rdk/services/mlmodel" "go.viam.com/rdk/utils" "go.viam.com/rdk/vision/objectdetection" ) -func attemptToBuildDetector(mlm mlmodel.Service) (objectdetection.Detector, error) { +func attemptToBuildDetector(mlm mlmodel.Service, nameMap *sync.Map) (objectdetection.Detector, error) { md, err := mlm.Metadata(context.Background()) if err != nil { return nil, errors.New("could not get any metadata") } // Set up input type, height, width, and labels - var inHeight, inWidth uint + var inHeight, inWidth int if len(md.Inputs) < 1 { return nil, errors.New("no input tensors received") } @@ -39,50 +42,61 @@ func attemptToBuildDetector(mlm mlmodel.Service) (objectdetection.Detector, erro } if shape := md.Inputs[0].Shape; getIndex(shape, 3) == 1 { - inHeight, inWidth = uint(shape[2]), uint(shape[3]) + inHeight, inWidth = shape[2], shape[3] } else { - inHeight, inWidth = uint(shape[1]), uint(shape[2]) + inHeight, inWidth = shape[1], shape[2] } return func(ctx context.Context, img image.Image) ([]objectdetection.Detection, error) { origW, origH := img.Bounds().Dx(), img.Bounds().Dy() - resized := resize.Resize(inWidth, inHeight, img, resize.Bilinear) - inMap := make(map[string]interface{}) + resizeW := inWidth + if resizeW == -1 { + resizeW = origW + } + resizeH := inHeight + if resizeH == -1 { + resizeH = origH + } + resized := img + if (origW != resizeW) || (origH != resizeH) { + resized = resize.Resize(uint(resizeW), uint(resizeH), img, resize.Bilinear) + } + inMap := ml.Tensors{} switch inType { case UInt8: - inMap["image"] = rimage.ImageToUInt8Buffer(resized) + inMap["image"] = tensor.New( + tensor.WithShape(1, resized.Bounds().Dy(), resized.Bounds().Dx(), 3), + tensor.WithBacking(rimage.ImageToUInt8Buffer(resized)), + ) case Float32: - inMap["image"] = rimage.ImageToFloatBuffer(resized) + inMap["image"] = tensor.New( + tensor.WithShape(1, resized.Bounds().Dy(), resized.Bounds().Dx(), 3), + tensor.WithBacking(rimage.ImageToFloatBuffer(resized)), + ) default: return nil, errors.New("invalid input type. try uint8 or float32") } - outMap, err := mlm.Infer(ctx, inMap) + outMap, _, err := mlm.Infer(ctx, inMap, nil) if err != nil { return nil, err } - var err2 error - - locations, err := unpack(outMap, "location") - if err != nil || len(locations) == 0 { - locations, err2 = unpack(outMap, DefaultOutTensorName+"0") - if err2 != nil { - return nil, multierr.Combine(err, err2) - } + // use the nameMap to find the tensor names, or guess and cache the names + locationName, categoryName, scoreName, err := findDetectionTensorNames(outMap, nameMap) + if err != nil { + return nil, err } - categories, err := unpack(outMap, "category") - if err != nil || len(categories) == 0 { - categories, err2 = unpack(outMap, DefaultOutTensorName+"1") - if err2 != nil { - return nil, multierr.Combine(err, err2) - } + locations, err := convertToFloat64Slice(outMap[locationName].Data()) + if err != nil { + return nil, err } - scores, err := unpack(outMap, "score") - if err != nil || len(scores) == 0 { - scores, err2 = unpack(outMap, DefaultOutTensorName+"2") - if err2 != nil { - return nil, multierr.Combine(err, err2) - } + categories, err := convertToFloat64Slice(outMap[categoryName].Data()) + if err != nil { + return nil, err + } + scores, err := convertToFloat64Slice(outMap[scoreName].Data()) + if err != nil { + return nil, err } // Now reshape outMap into Detections @@ -107,6 +121,176 @@ func attemptToBuildDetector(mlm mlmodel.Service) (objectdetection.Detector, erro }, nil } +// findDetectionTensors finds the tensors that are necessary for object detection +// the returned tensor order is location, category, score. It caches results. +func findDetectionTensorNames(outMap ml.Tensors, nameMap *sync.Map) (string, string, string, error) { + // first try the nameMap + loc, okLoc := nameMap.Load("location") + cat, okCat := nameMap.Load("category") + score, okScores := nameMap.Load("score") + if okLoc && okCat && okScores { // names are known + locString, ok := loc.(string) + if !ok { + return "", "", "", errors.Errorf("name map was not storing string, but a type %T", loc) + } + catString, ok := cat.(string) + if !ok { + return "", "", "", errors.Errorf("name map was not storing string, but a type %T", cat) + } + scoreString, ok := score.(string) + if !ok { + return "", "", "", errors.Errorf("name map was not storing string, but a type %T", score) + } + return locString, catString, scoreString, nil + } + // next, if nameMap is not set, just see if the outMap has expected names + _, okLoc = outMap["location"] + _, okCat = outMap["category"] + _, okScores = outMap["score"] + if okLoc && okCat && okScores { // names are as expected + nameMap.Store("location", "location") + nameMap.Store("category", "category") + nameMap.Store("score", "score") + return "location", "category", "score", nil + } + // last, do a hack-y thing to try to guess the tensor names for the detection output tensors + locationName, categoryName, scoreName, err := guessDetectionTensorNames(outMap) + if err != nil { + return "", "", "", err + } + nameMap.Store("location", locationName) + nameMap.Store("category", categoryName) + nameMap.Store("score", scoreName) + return locationName, categoryName, scoreName, nil +} + +// guessDetectionTensors is a hack-y function meant to find the correct detection tensors if the tensors +// were not given the expected names, or have no metadata. This function should succeed +// for models built with the viam platform. +func guessDetectionTensorNames(outMap ml.Tensors) (string, string, string, error) { + foundTensor := map[string]bool{} + mappedNames := map[string]string{} + outNames := tensorNames(outMap) + _, okLoc := outMap["location"] + if okLoc { + foundTensor["location"] = true + mappedNames["location"] = "location" + } + _, okCat := outMap["category"] + if okCat { + foundTensor["category"] = true + mappedNames["category"] = "category" + } + _, okScores := outMap["score"] + if okScores { + foundTensor["score"] = true + mappedNames["score"] = "score" + } + // first find how many detections there were + // this will be used to find the other tensors + nDetections := 0 + for name, t := range outMap { + if _, alreadyFound := foundTensor[name]; alreadyFound { + continue + } + if t.Dims() == 1 { // usually n-detections has its own tensor + val, err := t.At(0) + if err != nil { + return "", "", "", err + } + val64, err := convertToFloat64Slice(val) + if err != nil { + return "", "", "", err + } + nDetections = int(val64[0]) + foundTensor[name] = true + break + } + } + if !okLoc { // guess the name of the location tensor + // location tensor should have 3 dimensions usually + for name, t := range outMap { + if _, alreadyFound := foundTensor[name]; alreadyFound { + continue + } + if t.Dims() == 3 { + mappedNames["location"] = name + foundTensor[name] = true + break + } + } + if _, ok := mappedNames["location"]; !ok { + return "", "", "", errors.Errorf("could not find an output tensor named 'location' among [%s]", strings.Join(outNames, ", ")) + } + } + if !okCat { // guess the name of the category tensor + // a category usually has a whole number in its elements, so either look for + // int data types in the tensor, or sum the elements and make sure they dont have any decimals + for name, t := range outMap { + if _, alreadyFound := foundTensor[name]; alreadyFound { + continue + } + dt := t.Dtype() + if t.Dims() == 2 { + if dt == tensor.Int || dt == tensor.Int32 || dt == tensor.Int64 || + dt == tensor.Uint32 || dt == tensor.Uint64 || dt == tensor.Int8 || dt == tensor.Uint8 { + mappedNames["category"] = name + foundTensor[name] = true + break + } + // check if fully whole number + var whole tensor.Tensor + var err error + if nDetections == 0 { + whole, err = tensor.Sum(t) + if err != nil { + return "", "", "", err + } + } else { + s, err := t.Slice(nil, tensor.S(0, nDetections)) + if err != nil { + return "", "", "", err + } + whole, err = tensor.Sum(s) + if err != nil { + return "", "", "", err + } + } + val, err := convertToFloat64Slice(whole.Data()) + if err != nil { + return "", "", "", err + } + if math.Mod(val[0], 1) == 0 { + mappedNames["category"] = name + foundTensor[name] = true + break + } + } + } + if _, ok := mappedNames["category"]; !ok { + return "", "", "", errors.Errorf("could not find an output tensor named 'category' among [%s]", strings.Join(outNames, ", ")) + } + } + if !okScores { // guess the name of the scores tensor + // a score usually has a float data type + for name, t := range outMap { + if _, alreadyFound := foundTensor[name]; alreadyFound { + continue + } + dt := t.Dtype() + if t.Dims() == 2 && (dt == tensor.Float32 || dt == tensor.Float64) { + mappedNames["score"] = name + foundTensor[name] = true + break + } + } + if _, ok := mappedNames["score"]; !ok { + return "", "", "", errors.Errorf("could not find an output tensor named 'score' among [%s]", strings.Join(outNames, ", ")) + } + } + return mappedNames["location"], mappedNames["category"], mappedNames["score"], nil +} + // In the case that the model provided is not a detector, attemptToBuildDetector will return a // detector function that function fails because the expected keys are not in the outputTensor. // use checkIfDetectorWorks to get sample output tensors on gray image so we know if the functions @@ -121,7 +305,7 @@ func checkIfDetectorWorks(ctx context.Context, df objectdetection.Detector) erro _, err := df(ctx, img) if err != nil { - return errors.New("Cannot use model as a detector") + return errors.Wrap(err, "Cannot use model as a detector") } return nil } diff --git a/services/vision/mlvision/ml_model.go b/services/vision/mlvision/ml_model.go index 77786f83e82..06ae9178b89 100644 --- a/services/vision/mlvision/ml_model.go +++ b/services/vision/mlvision/ml_model.go @@ -9,12 +9,15 @@ import ( "os" "path/filepath" "strings" + "sync" "github.com/edaniels/golog" "github.com/montanaflynn/stats" "github.com/pkg/errors" "go.opencensus.io/trace" + "golang.org/x/exp/constraints" + "go.viam.com/rdk/ml" "go.viam.com/rdk/resource" "go.viam.com/rdk/robot" "go.viam.com/rdk/services/mlmodel" @@ -52,10 +55,17 @@ func init() { // MLModelConfig specifies the parameters needed to turn an ML model into a vision Model. type MLModelConfig struct { - resource.TriviallyValidateConfig ModelName string `json:"mlmodel_name"` } +// Validate will add the ModelName as an implicit dependency to the robot. +func (conf *MLModelConfig) Validate(path string) ([]string, error) { + if conf.ModelName == "" { + return nil, errors.New("mlmodel_name cannot be empty") + } + return []string{conf.ModelName}, nil +} + func registerMLModelVisionService( ctx context.Context, name resource.Name, @@ -71,7 +81,11 @@ func registerMLModelVisionService( return nil, err } - classifierFunc, err := attemptToBuildClassifier(mlm) + // the nameMap that associates the tensor names as they are found in the model, to + // what the vision service expects. This might not be necessary any more once we + // get the vision service to have rename maps in its configs. + nameMap := &sync.Map{} + classifierFunc, err := attemptToBuildClassifier(mlm, nameMap) if err != nil { logger.Debugw("unable to use ml model as a classifier, will attempt to evaluate as"+ "detector and segmenter", "model", params.ModelName, "error", err) @@ -86,7 +100,7 @@ func registerMLModelVisionService( } } - detectorFunc, err := attemptToBuildDetector(mlm) + detectorFunc, err := attemptToBuildDetector(mlm, nameMap) if err != nil { logger.Debugw("unable to use ml model as a detector, will attempt to evaluate as 3D segmenter", "model", params.ModelName, "error", err) @@ -101,7 +115,7 @@ func registerMLModelVisionService( } } - segmenter3DFunc, err := attemptToBuild3DSegmenter(mlm) + segmenter3DFunc, err := attemptToBuild3DSegmenter(mlm, nameMap) if err != nil { logger.Debugw("unable to use ml model as 3D segmenter", "model", params.ModelName, "error", err) } else { @@ -111,28 +125,6 @@ func registerMLModelVisionService( return vision.NewService(name, r, nil, classifierFunc, detectorFunc, segmenter3DFunc) } -// Unpack output based on expected type and force it into a []float64. -func unpack(inMap map[string]interface{}, name string) ([]float64, error) { - var out []float64 - me := inMap[name] - if me == nil { - return nil, errors.Errorf("no such tensor named %q to unpack", name) - } - switch v := me.(type) { - case []uint8: - out = make([]float64, 0, len(v)) - for _, t := range v { - out = append(out, float64(t)) - } - case []float32: - out = make([]float64, 0, len(v)) - for _, t := range v { - out = append(out, float64(t)) - } - } - return out, nil -} - // getLabelsFromMetadata returns a slice of strings--the intended labels. func getLabelsFromMetadata(md mlmodel.MLMetadata) []string { if len(md.Outputs) < 1 { @@ -232,3 +224,81 @@ func checkClassificationScores(in []float64) []float64 { } return in // no need to sigmoid } + +// Number interface for converting between numbers. +type number interface { + constraints.Integer | constraints.Float +} + +// convertNumberSlice converts any number slice into another number slice. +func convertNumberSlice[T1, T2 number](t1 []T1) []T2 { + t2 := make([]T2, len(t1)) + for i := range t1 { + t2[i] = T2(t1[i]) + } + return t2 +} + +func convertToFloat64Slice(slice interface{}) ([]float64, error) { + switch v := slice.(type) { + case []float64: + return v, nil + case float64: + return []float64{v}, nil + case []float32: + return convertNumberSlice[float32, float64](v), nil + case float32: + return convertNumberSlice[float32, float64]([]float32{v}), nil + case []int: + return convertNumberSlice[int, float64](v), nil + case int: + return convertNumberSlice[int, float64]([]int{v}), nil + case []uint: + return convertNumberSlice[uint, float64](v), nil + case uint: + return convertNumberSlice[uint, float64]([]uint{v}), nil + case []int8: + return convertNumberSlice[int8, float64](v), nil + case int8: + return convertNumberSlice[int8, float64]([]int8{v}), nil + case []int16: + return convertNumberSlice[int16, float64](v), nil + case int16: + return convertNumberSlice[int16, float64]([]int16{v}), nil + case []int32: + return convertNumberSlice[int32, float64](v), nil + case int32: + return convertNumberSlice[int32, float64]([]int32{v}), nil + case []int64: + return convertNumberSlice[int64, float64](v), nil + case int64: + return convertNumberSlice[int64, float64]([]int64{v}), nil + case []uint8: + return convertNumberSlice[uint8, float64](v), nil + case uint8: + return convertNumberSlice[uint8, float64]([]uint8{v}), nil + case []uint16: + return convertNumberSlice[uint16, float64](v), nil + case uint16: + return convertNumberSlice[uint16, float64]([]uint16{v}), nil + case []uint32: + return convertNumberSlice[uint32, float64](v), nil + case uint32: + return convertNumberSlice[uint32, float64]([]uint32{v}), nil + case []uint64: + return convertNumberSlice[uint64, float64](v), nil + case uint64: + return convertNumberSlice[uint64, float64]([]uint64{v}), nil + default: + return nil, errors.Errorf("dont know how to convert slice of %T into a []float64", slice) + } +} + +// tensorNames returns all the names of the tensors. +func tensorNames(t ml.Tensors) []string { + names := []string{} + for name := range t { + names = append(names, name) + } + return names +} diff --git a/services/vision/mlvision/ml_model_test.go b/services/vision/mlvision/ml_model_test.go index 67c4c29446e..c37721e76de 100644 --- a/services/vision/mlvision/ml_model_test.go +++ b/services/vision/mlvision/ml_model_test.go @@ -89,14 +89,15 @@ func TestAddingIncorrectModelTypeToModel(t *testing.T) { mlm, err := getTestMlModel(modelLocDetector) test.That(t, err, test.ShouldBeNil) - classifier, err := attemptToBuildClassifier(mlm) + nameMap := &sync.Map{} + classifier, err := attemptToBuildClassifier(mlm, nameMap) test.That(t, err, test.ShouldBeNil) test.That(t, classifier, test.ShouldNotBeNil) err = checkIfClassifierWorks(ctx, classifier) test.That(t, err, test.ShouldNotBeNil) - detector, err := attemptToBuildDetector(mlm) + detector, err := attemptToBuildDetector(mlm, nameMap) test.That(t, err, test.ShouldBeNil) test.That(t, detector, test.ShouldNotBeNil) @@ -108,7 +109,8 @@ func TestAddingIncorrectModelTypeToModel(t *testing.T) { mlm, err = getTestMlModel(modelLocClassifier) test.That(t, err, test.ShouldBeNil) - classifier, err = attemptToBuildClassifier(mlm) + nameMap = &sync.Map{} + classifier, err = attemptToBuildClassifier(mlm, nameMap) test.That(t, err, test.ShouldBeNil) test.That(t, classifier, test.ShouldNotBeNil) @@ -118,7 +120,7 @@ func TestAddingIncorrectModelTypeToModel(t *testing.T) { mlm, err = getTestMlModel(modelLocClassifier) test.That(t, err, test.ShouldBeNil) - detector, err = attemptToBuildDetector(mlm) + detector, err = attemptToBuildDetector(mlm, nameMap) test.That(t, err, test.ShouldBeNil) test.That(t, detector, test.ShouldNotBeNil) @@ -159,7 +161,8 @@ func TestNewMLDetector(t *testing.T) { test.That(t, check.Outputs[1].Name, test.ShouldResemble, "category") test.That(t, check.Outputs[0].Extra["labels"], test.ShouldNotBeNil) - gotDetector, err := attemptToBuildDetector(out) + nameMap := &sync.Map{} + gotDetector, err := attemptToBuildDetector(out, nameMap) test.That(t, err, test.ShouldBeNil) test.That(t, gotDetector, test.ShouldNotBeNil) @@ -183,7 +186,8 @@ func TestNewMLDetector(t *testing.T) { outNL, err := tflitecpu.NewTFLiteCPUModel(ctx, &noLabelCfg, mlmodel.Named("myOtherMLDet")) test.That(t, err, test.ShouldBeNil) test.That(t, outNL, test.ShouldNotBeNil) - gotDetectorNL, err := attemptToBuildDetector(outNL) + nameMap = &sync.Map{} + gotDetectorNL, err := attemptToBuildDetector(outNL, nameMap) test.That(t, err, test.ShouldBeNil) test.That(t, gotDetectorNL, test.ShouldNotBeNil) gotDetectionsNL, err := gotDetectorNL(ctx, pic) @@ -232,7 +236,8 @@ func TestNewMLClassifier(t *testing.T) { test.That(t, check.Outputs[0].Name, test.ShouldResemble, "probability") test.That(t, check.Outputs[0].Extra["labels"], test.ShouldNotBeNil) - gotClassifier, err := attemptToBuildClassifier(out) + nameMap := &sync.Map{} + gotClassifier, err := attemptToBuildClassifier(out, nameMap) test.That(t, err, test.ShouldBeNil) test.That(t, gotClassifier, test.ShouldNotBeNil) @@ -250,7 +255,8 @@ func TestNewMLClassifier(t *testing.T) { outNL, err := tflitecpu.NewTFLiteCPUModel(ctx, &noLabelCfg, mlmodel.Named("myOtherMLClassif")) test.That(t, err, test.ShouldBeNil) test.That(t, outNL, test.ShouldNotBeNil) - gotClassifierNL, err := attemptToBuildClassifier(outNL) + nameMap = &sync.Map{} + gotClassifierNL, err := attemptToBuildClassifier(outNL, nameMap) test.That(t, err, test.ShouldBeNil) test.That(t, gotClassifierNL, test.ShouldNotBeNil) gotClassificationsNL, err := gotClassifierNL(ctx, pic) @@ -290,8 +296,10 @@ func TestMoreMLDetectors(t *testing.T) { // Even without metadata we should find test.That(t, check.Inputs[0].Shape, test.ShouldResemble, []int{1, 320, 320, 3}) test.That(t, check.Inputs[0].DataType, test.ShouldResemble, "float32") + test.That(t, len(check.Outputs), test.ShouldEqual, 4) - gotDetector, err := attemptToBuildDetector(outModel) + nameMap := &sync.Map{} + gotDetector, err := attemptToBuildDetector(outModel, nameMap) test.That(t, err, test.ShouldBeNil) test.That(t, gotDetector, test.ShouldNotBeNil) @@ -321,7 +329,8 @@ func TestMoreMLClassifiers(t *testing.T) { test.That(t, check, test.ShouldNotBeNil) test.That(t, err, test.ShouldBeNil) - gotClassifier, err := attemptToBuildClassifier(outModel) + nameMap := &sync.Map{} + gotClassifier, err := attemptToBuildClassifier(outModel, nameMap) test.That(t, err, test.ShouldBeNil) test.That(t, gotClassifier, test.ShouldNotBeNil) @@ -348,7 +357,8 @@ func TestMoreMLClassifiers(t *testing.T) { test.That(t, check, test.ShouldNotBeNil) test.That(t, err, test.ShouldBeNil) - gotClassifier, err = attemptToBuildClassifier(outModel) + nameMap = &sync.Map{} + gotClassifier, err = attemptToBuildClassifier(outModel, nameMap) test.That(t, err, test.ShouldBeNil) test.That(t, gotClassifier, test.ShouldNotBeNil) gotClassifications, err = gotClassifier(ctx, pic) @@ -421,7 +431,8 @@ func TestOneClassifierOnManyCameras(t *testing.T) { out, err := tflitecpu.NewTFLiteCPUModel(ctx, &cfg, mlmodel.Named("testClassifier")) test.That(t, err, test.ShouldBeNil) test.That(t, out, test.ShouldNotBeNil) - outClassifier, err := attemptToBuildClassifier(out) + nameMap := &sync.Map{} + outClassifier, err := attemptToBuildClassifier(out, nameMap) test.That(t, err, test.ShouldBeNil) test.That(t, outClassifier, test.ShouldNotBeNil) valuePanda, valueLion := classifyTwoImages(picPanda, picLion, outClassifier) @@ -439,10 +450,12 @@ func TestMultipleClassifiersOneModel(t *testing.T) { out, err := tflitecpu.NewTFLiteCPUModel(ctx, &cfg, mlmodel.Named("testClassifier")) test.That(t, err, test.ShouldBeNil) - Classifier1, err := attemptToBuildClassifier(out) + nameMap := &sync.Map{} + Classifier1, err := attemptToBuildClassifier(out, nameMap) test.That(t, err, test.ShouldBeNil) - Classifier2, err := attemptToBuildClassifier(out) + nameMap = &sync.Map{} + Classifier2, err := attemptToBuildClassifier(out, nameMap) test.That(t, err, test.ShouldBeNil) picPanda, err := rimage.NewImageFromFile(artifact.MustPath("vision/tflite/redpanda.jpeg")) diff --git a/services/vision/mlvision/segmenter3d.go b/services/vision/mlvision/segmenter3d.go index 797cbc8e2c3..42c6f581dc3 100644 --- a/services/vision/mlvision/segmenter3d.go +++ b/services/vision/mlvision/segmenter3d.go @@ -2,12 +2,13 @@ package mlvision import ( "errors" + "sync" "go.viam.com/rdk/services/mlmodel" "go.viam.com/rdk/vision/segmentation" ) // TODO: RSDK-2665, build 3D segmenter from ML models. -func attemptToBuild3DSegmenter(mlm mlmodel.Service) (segmentation.Segmenter, error) { +func attemptToBuild3DSegmenter(mlm mlmodel.Service, nameMap *sync.Map) (segmentation.Segmenter, error) { return nil, errors.New("vision 3D segmenters from ML models are currently not supported") } diff --git a/services/vision/obstaclesdepth/obstacles_depth.go b/services/vision/obstaclesdepth/obstacles_depth.go new file mode 100644 index 00000000000..1b12062b2a3 --- /dev/null +++ b/services/vision/obstaclesdepth/obstacles_depth.go @@ -0,0 +1,349 @@ +// Package obstaclesdepth uses an underlying depth camera to fulfill GetObjectPointClouds, +// using the method outlined in (Manduchi, Roberto, et al. "Obstacle detection and terrain classification +// for autonomous off-road navigation." Autonomous robots 18 (2005): 81-102.) +package obstaclesdepth + +import ( + "context" + "image" + "math" + "sort" + "strconv" + "sync" + + "github.com/edaniels/golog" + "github.com/golang/geo/r3" + "github.com/muesli/clusters" + "github.com/muesli/kmeans" + "github.com/pkg/errors" + "github.com/viamrobotics/gostream" + "go.opencensus.io/trace" + goutils "go.viam.com/utils" + + "go.viam.com/rdk/components/camera" + "go.viam.com/rdk/pointcloud" + "go.viam.com/rdk/resource" + "go.viam.com/rdk/rimage" + "go.viam.com/rdk/rimage/transform" + "go.viam.com/rdk/robot" + svision "go.viam.com/rdk/services/vision" + "go.viam.com/rdk/spatialmath" + "go.viam.com/rdk/utils" + vision "go.viam.com/rdk/vision" +) + +var model = resource.DefaultModelFamily.WithModel("obstacles_depth") + +// ObsDepthConfig specifies the parameters to be used for the obstacle depth service. +type ObsDepthConfig struct { + Hmin float64 `json:"h_min_m"` + Hmax float64 `json:"h_max_m"` + ThetaMax float64 `json:"theta_max_deg"` + ReturnPCDs bool `json:"return_pcds"` + WithGeometries *bool `json:"with_geometries"` +} + +// obsDepth is the underlying struct actually used by the service. +type obsDepth struct { + dm *rimage.DepthMap + obstaclePts []image.Point + hMin float64 + hMax float64 + sinTheta float64 + intrinsics *transform.PinholeCameraIntrinsics + returnPCDs bool + withGeoms bool + k int + depthStream gostream.VideoStream +} + +const ( + // the first 3 consts are parameters from Manduchi et al. + defaultHmin = 0.0 + defaultHmax = 1.0 + defaultThetamax = 45 + + defaultK = 10 // default number of obstacle segments to create + sampleN = 4 // we sample 1 in every sampleN depth points +) + +func init() { + resource.RegisterService(svision.API, model, resource.Registration[svision.Service, *ObsDepthConfig]{ + DeprecatedRobotConstructor: func(ctx context.Context, r any, c resource.Config, logger golog.Logger) (svision.Service, error) { + attrs, err := resource.NativeConfig[*ObsDepthConfig](c) + if err != nil { + return nil, err + } + actualR, err := utils.AssertType[robot.Robot](r) + if err != nil { + return nil, err + } + return registerObstaclesDepth(ctx, c.ResourceName(), attrs, actualR, logger) + }, + }) +} + +// Validate ensures all parts of the config are valid. +func (config *ObsDepthConfig) Validate(path string) ([]string, error) { + deps := []string{} + if config.Hmin >= config.Hmax && !(config.Hmin == 0 && config.Hmax == 0) { + return nil, errors.New("Hmin should be less than Hmax") + } + if config.Hmin < 0 { + return nil, errors.New("Hmin should be greater than or equal to 0") + } + if config.Hmax < 0 { + return nil, errors.New("Hmax should be greater than or equal to 0") + } + if config.ThetaMax < 0 || config.ThetaMax > 360 { + return nil, errors.New("ThetaMax should be in degrees between 0 and 360") + } + + return deps, nil +} + +func registerObstaclesDepth( + ctx context.Context, + name resource.Name, + conf *ObsDepthConfig, + r robot.Robot, + logger golog.Logger, +) (svision.Service, error) { + _, span := trace.StartSpan(ctx, "service::vision::registerObstacleDepth") + defer span.End() + if conf == nil { + return nil, errors.New("config for obstacles_depth cannot be nil") + } + + // Use defaults if needed + if conf.Hmax == 0 { + conf.Hmax = defaultHmax + } + if conf.ThetaMax == 0 { + conf.ThetaMax = defaultThetamax + } + if conf.WithGeometries == nil { + wg := true + conf.WithGeometries = &wg + } + + sinTheta := math.Sin(conf.ThetaMax * math.Pi / 180) // sin(radians(theta)) + myObsDep := obsDepth{ + hMin: 1000 * conf.Hmin, hMax: 1000 * conf.Hmax, sinTheta: sinTheta, + returnPCDs: conf.ReturnPCDs, k: defaultK, withGeoms: *conf.WithGeometries, + } + + segmenter := myObsDep.buildObsDepth(logger) // does the thing + return svision.NewService(name, r, nil, nil, nil, segmenter) +} + +// BuildObsDepth will check for intrinsics and determine how to build based on that. +func (o *obsDepth) buildObsDepth(logger golog.Logger) func(ctx context.Context, src camera.VideoSource) ([]*vision.Object, error) { + return func(ctx context.Context, src camera.VideoSource) ([]*vision.Object, error) { + props, err := src.Properties(ctx) + if err != nil { + logger.Warnw("could not find camera properties. obstacles depth started without camera's intrinsic parameters", "error", err) + return o.obsDepthNoIntrinsics(ctx, src) + } + if props.IntrinsicParams == nil { + logger.Warn("obstacles depth started but camera did not have intrinsic parameters") + return o.obsDepthNoIntrinsics(ctx, src) + } + o.intrinsics = props.IntrinsicParams + if o.withGeoms { + return o.obsDepthWithIntrinsics(ctx, src) + } + return o.obsDepthNoIntrinsics(ctx, src) + } +} + +// buildObsDepthNoIntrinsics will return the median depth in the depth map as a Geometry point. +func (o *obsDepth) obsDepthNoIntrinsics(ctx context.Context, src camera.VideoSource) ([]*vision.Object, error) { + pic, release, err := camera.ReadImage(ctx, src) + if err != nil { + return nil, errors.Errorf("could not get image from %s", src) + } + defer release() + + dm, err := rimage.ConvertImageToDepthMap(ctx, pic) + if err != nil { + return nil, errors.New("could not convert image to depth map") + } + depData := dm.Data() + if len(depData) == 0 { + return nil, errors.New("could not get info from depth map") + } + // Sort the depth data [smallest...largest] + sort.Slice(depData, func(i, j int) bool { + return depData[i] < depData[j] + }) + med := int(0.5 * float64(len(depData))) + pt := spatialmath.NewPoint(r3.Vector{X: 0, Y: 0, Z: float64(depData[med])}, "") + toReturn := make([]*vision.Object, 1) + toReturn[0] = &vision.Object{Geometry: pt} + return toReturn, nil +} + +// buildObsDepthWithIntrinsics will use the methodology in Manduchi et al. to find obstacle points +// before clustering and projecting those points into 3D obstacles. +func (o *obsDepth) obsDepthWithIntrinsics(ctx context.Context, src camera.VideoSource) ([]*vision.Object, error) { + // Check if we have intrinsics here. If not, don't even try + if o.intrinsics == nil { + return nil, errors.New("tried to build obstacles depth with intrinsics but no instrinsics found") + } + if o.depthStream == nil { + depthStream, err := src.Stream(ctx) + if err != nil { + return nil, errors.Errorf("could not get stream from %s", src) + } + o.depthStream = depthStream + } + + pic, release, err := o.depthStream.Next(ctx) + if err != nil { + return nil, errors.Errorf("could not get image from stream %s", o.depthStream) + } + defer release() + dm, err := rimage.ConvertImageToDepthMap(ctx, pic) + if err != nil { + return nil, errors.New("could not convert image to depth map") + } + w, h := dm.Width(), dm.Height() + o.dm = dm + + var wg sync.WaitGroup + obstaclePointChan := make(chan image.Point) + + for i := 0; i < w; i += sampleN { + wg.Add(1) + go func(i int) { + defer wg.Done() + for j := 0; j < h; j++ { + candidate := image.Pt(i, j) + obs: // for every sub-sampled point, figure out if it is an obstacle + for l := 0; l < w; l += sampleN { // continue with the sub-sampling + for m := 0; m < h; m++ { + compareTo := image.Pt(l, m) + if candidate == compareTo { + continue + } + if o.isCompatible(candidate, compareTo) { + obstaclePointChan <- candidate + break obs + } + } + } + } + }(i) + } + + goutils.ManagedGo(func() { + wg.Wait() + close(obstaclePointChan) + }, nil) + + obstaclePoints := make([]image.Point, 0, w*h/sampleN) + for op := range obstaclePointChan { + obstaclePoints = append(obstaclePoints, op) + } + o.obstaclePts = obstaclePoints + + // Cluster the points in 3D + boxes, outClusters, err := o.performKMeans3D(o.k) + if err != nil { + return nil, err + } + + // Packaging the return depending on if they want PCDs + n := int(math.Min(float64(len(outClusters)), float64(len(boxes)))) // should be same len but for safety + toReturn := make([]*vision.Object, n) + for i := 0; i < n; i++ { // for each cluster/box make an object + if o.returnPCDs { + pcdToReturn := pointcloud.NewWithPrealloc(len(outClusters[i].Observations)) + basicData := pointcloud.NewBasicData() + for _, pt := range outClusters[i].Observations { + if len(pt.Coordinates()) >= 3 { + vec := r3.Vector{X: pt.Coordinates()[0], Y: pt.Coordinates()[1], Z: pt.Coordinates()[2]} + err = pcdToReturn.Set(vec, basicData) + if err != nil { + return nil, err + } + } + } + toReturn[i] = &vision.Object{PointCloud: pcdToReturn, Geometry: boxes[i]} + } else { + toReturn[i] = &vision.Object{Geometry: boxes[i]} + } + } + return toReturn, nil +} + +// isCompatible will check compatibility between 2 points. +// as defined by Manduchi et al. +func (o *obsDepth) isCompatible(p1, p2 image.Point) bool { + xdist, ydist := math.Abs(float64(p1.X-p2.X)), math.Abs(float64(p1.Y-p2.Y)) + zdist := math.Abs(float64(o.dm.Get(p1)) - float64(o.dm.Get(p2))) + dist := math.Sqrt((xdist * xdist) + (ydist * ydist) + (zdist * zdist)) + + if ydist < o.hMin || ydist > o.hMax { + return false + } + if ydist/dist < o.sinTheta { + return false + } + return true +} + +// performKMeans3D will do k-means clustering on projected obstacle points. +func (o *obsDepth) performKMeans3D(k int) ([]spatialmath.Geometry, clusters.Clusters, error) { + var observations3D clusters.Observations + for _, pt := range o.obstaclePts { + outX, outY, outZ := o.intrinsics.PixelToPoint(float64(pt.X), float64(pt.Y), float64(o.dm.GetDepth(pt.X, pt.Y))) + observations3D = append(observations3D, clusters.Coordinates{outX, outY, outZ}) + } + km := kmeans.New() + clusters, err := km.Partition(observations3D, k) + if err != nil { + return nil, nil, err + } + boxes := make([]spatialmath.Geometry, 0, len(clusters)) + + for i, c := range clusters { + xmax, ymax, zmax := math.Inf(-1), math.Inf(-1), math.Inf(-1) + xmin, ymin, zmin := math.Inf(1), math.Inf(1), math.Inf(1) + + for _, pt := range c.Observations { + x, y, z := pt.Coordinates().Coordinates()[0], pt.Coordinates().Coordinates()[1], pt.Coordinates().Coordinates()[2] + + if x < xmin { + xmin = x + } + if x > xmax { + xmax = x + } + if y < ymin { + ymin = y + } + if y > ymax { + ymax = y + } + if z < zmin { + zmin = z + } + if z > zmax { + zmax = z + } + } + // Make a box from those bounds and add it in + xdiff, ydiff, zdiff := xmax-xmin, ymax-ymin, zmax-zmin + xc, yc, zc := (xmin+xmax)/2, (ymin+ymax)/2, (zmin+zmax)/2 + pose := spatialmath.NewPoseFromPoint(r3.Vector{xc, yc, zc}) + + box, err := spatialmath.NewBox(pose, r3.Vector{xdiff, ydiff, zdiff}, strconv.Itoa(i)) + if err != nil { + return nil, nil, err + } + boxes = append(boxes, box) + } + return boxes, clusters, err +} diff --git a/services/vision/obstaclesdepth/obstacles_depth_test.go b/services/vision/obstaclesdepth/obstacles_depth_test.go new file mode 100644 index 00000000000..d993567a61f --- /dev/null +++ b/services/vision/obstaclesdepth/obstacles_depth_test.go @@ -0,0 +1,199 @@ +package obstaclesdepth + +import ( + "context" + "image" + "testing" + + "github.com/edaniels/golog" + "github.com/golang/geo/r3" + "go.viam.com/test" + "go.viam.com/utils/artifact" + + "go.viam.com/rdk/components/camera" + "go.viam.com/rdk/resource" + "go.viam.com/rdk/rimage" + "go.viam.com/rdk/rimage/transform" + "go.viam.com/rdk/services/vision" + "go.viam.com/rdk/spatialmath" + "go.viam.com/rdk/testutils/inject" +) + +// testReader creates and serves a fake depth image for testing. +type testReader struct{} + +func (r testReader) Read(ctx context.Context) (image.Image, func(), error) { + d := rimage.NewEmptyDepthMap(50, 50) + for i := 0; i < 40; i++ { + for j := 5; j < 45; j++ { + d.Set(i, j, rimage.Depth(400)) + } + } + return d, nil, nil +} + +func (r testReader) Close(ctx context.Context) error { + return nil +} + +// fullReader grabs and serves a fake depth image for testing. +type fullReader struct{} + +func (r fullReader) Read(ctx context.Context) (image.Image, func(), error) { + // We want this to return a valid depth image of known size (640 x 480) + pic, err := rimage.NewDepthMapFromFile(context.Background(), artifact.MustPath("vision/exampleDepth.png")) + return pic, nil, err +} + +func (r fullReader) Close(ctx context.Context) error { + return nil +} + +func TestObstacleDepth(t *testing.T) { + no := false + noIntrinsicsCfg := ObsDepthConfig{ + Hmin: defaultHmin, + Hmax: defaultHmax, + ThetaMax: defaultThetamax, + ReturnPCDs: false, + WithGeometries: &no, + } + someIntrinsics := transform.PinholeCameraIntrinsics{Fx: 604.5, Fy: 609.6, Ppx: 324.6, Ppy: 238.9, Width: 640, Height: 480} + withIntrinsicsCfg := ObsDepthConfig{ + Hmin: defaultHmin, + Hmax: defaultHmax, + ThetaMax: defaultThetamax, + ReturnPCDs: true, + } + + ctx := context.Background() + testLogger := golog.NewLogger("test") + r := &inject.Robot{ResourceNamesFunc: func() []resource.Name { + return []resource.Name{camera.Named("testCam"), camera.Named("noIntrinsicsCam")} + }} + tr := testReader{} + syst := transform.PinholeCameraModel{&someIntrinsics, nil} + myCamSrcIntrinsics, err := camera.NewVideoSourceFromReader(ctx, tr, &syst, camera.DepthStream) + test.That(t, err, test.ShouldBeNil) + test.That(t, myCamSrcIntrinsics, test.ShouldNotBeNil) + myCamSrcNoIntrinsics, err := camera.NewVideoSourceFromReader(ctx, tr, nil, camera.DepthStream) + test.That(t, err, test.ShouldBeNil) + test.That(t, myCamSrcNoIntrinsics, test.ShouldNotBeNil) + myIntrinsicsCam := camera.FromVideoSource(resource.Name{Name: "testCam"}, myCamSrcIntrinsics) + noIntrinsicsCam := camera.FromVideoSource(resource.Name{Name: "noIntrinsicsCam"}, myCamSrcNoIntrinsics) + r.ResourceByNameFunc = func(n resource.Name) (resource.Resource, error) { + switch n.Name { + case "testCam": + return myIntrinsicsCam, nil + case "noIntrinsicsCam": + return noIntrinsicsCam, nil + default: + return nil, resource.NewNotFoundError(n) + } + } + name := vision.Named("test") + srv, err := registerObstaclesDepth(ctx, name, &noIntrinsicsCfg, r, testLogger) + test.That(t, err, test.ShouldBeNil) + test.That(t, srv.Name(), test.ShouldResemble, name) + + // Not a detector or classifier + img, err := rimage.NewImageFromFile(artifact.MustPath("vision/objectdetection/detection_test.jpg")) + test.That(t, err, test.ShouldBeNil) + test.That(t, img, test.ShouldNotBeNil) + _, err = srv.Detections(ctx, img, nil) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, "does not implement") + _, err = srv.Classifications(ctx, img, 1, nil) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, "does not implement") + + t.Run("no intrinsics version", func(t *testing.T) { + // Test that it is a segmenter + obs, err := srv.GetObjectPointClouds(ctx, "noIntrinsicsCam", nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, obs, test.ShouldNotBeNil) + test.That(t, len(obs), test.ShouldEqual, 1) + test.That(t, obs[0].PointCloud, test.ShouldBeNil) + poseShouldBe := spatialmath.NewPose(r3.Vector{0, 0, 400}, nil) + test.That(t, obs[0].Geometry.Pose(), test.ShouldResemble, poseShouldBe) + }) + t.Run("intrinsics version", func(t *testing.T) { + // Now with intrinsics (and pointclouds)! + srv2, err := registerObstaclesDepth(ctx, name, &withIntrinsicsCfg, r, testLogger) + test.That(t, err, test.ShouldBeNil) + test.That(t, srv2, test.ShouldNotBeNil) + obs, err := srv2.GetObjectPointClouds(ctx, "testCam", nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, obs, test.ShouldNotBeNil) + test.That(t, len(obs), test.ShouldEqual, defaultK) + for _, o := range obs { + test.That(t, o.PointCloud, test.ShouldNotBeNil) + test.That(t, o.Geometry, test.ShouldNotBeNil) + } + }) +} + +func BenchmarkObstacleDepthIntrinsics(b *testing.B) { + someIntrinsics := transform.PinholeCameraIntrinsics{Fx: 604.5, Fy: 609.6, Ppx: 324.6, Ppy: 238.9, Width: 640, Height: 480} + withIntrinsicsCfg := ObsDepthConfig{ + Hmin: defaultHmin, + Hmax: defaultHmax, + ThetaMax: defaultThetamax, + ReturnPCDs: true, + } + + ctx := context.Background() + testLogger := golog.NewLogger("test") + r := &inject.Robot{ResourceNamesFunc: func() []resource.Name { + return []resource.Name{camera.Named("testCam")} + }} + tr := fullReader{} + syst := transform.PinholeCameraModel{&someIntrinsics, nil} + myCamSrc, _ := camera.NewVideoSourceFromReader(ctx, tr, &syst, camera.DepthStream) + myCam := camera.FromVideoSource(resource.Name{Name: "testCam"}, myCamSrc) + r.ResourceByNameFunc = func(n resource.Name) (resource.Resource, error) { + switch n.Name { + case "testCam": + return myCam, nil + default: + return nil, resource.NewNotFoundError(n) + } + } + name := vision.Named("test") + srv, _ := registerObstaclesDepth(ctx, name, &withIntrinsicsCfg, r, testLogger) + + for i := 0; i < b.N; i++ { + srv.GetObjectPointClouds(ctx, "testCam", nil) + } +} + +func BenchmarkObstacleDepthNoIntrinsics(b *testing.B) { + no := false + noIntrinsicsCfg := ObsDepthConfig{ + ReturnPCDs: false, + WithGeometries: &no, + } + + ctx := context.Background() + testLogger := golog.NewLogger("test") + r := &inject.Robot{ResourceNamesFunc: func() []resource.Name { + return []resource.Name{camera.Named("testCam")} + }} + tr := fullReader{} + myCamSrc, _ := camera.NewVideoSourceFromReader(ctx, tr, nil, camera.DepthStream) + myCam := camera.FromVideoSource(resource.Name{Name: "testCam"}, myCamSrc) + r.ResourceByNameFunc = func(n resource.Name) (resource.Resource, error) { + switch n.Name { + case "testCam": + return myCam, nil + default: + return nil, resource.NewNotFoundError(n) + } + } + name := vision.Named("test") + srv, _ := registerObstaclesDepth(ctx, name, &noIntrinsicsCfg, r, testLogger) + + for i := 0; i < b.N; i++ { + srv.GetObjectPointClouds(ctx, "testCam", nil) + } +} diff --git a/services/vision/obstacledistance/obstacle_distance.go b/services/vision/obstaclesdistance/obstacles_distance.go similarity index 96% rename from services/vision/obstacledistance/obstacle_distance.go rename to services/vision/obstaclesdistance/obstacles_distance.go index 5f6380891e1..5a2f19aaea3 100644 --- a/services/vision/obstacledistance/obstacle_distance.go +++ b/services/vision/obstaclesdistance/obstacles_distance.go @@ -1,6 +1,6 @@ -// Package obstacledistance uses an underlying camera to fulfill vision service methods, specifically +// Package obstaclesdistance uses an underlying camera to fulfill vision service methods, specifically // GetObjectPointClouds, which performs several queries of NextPointCloud and returns a median point. -package obstacledistance +package obstaclesdistance import ( "context" diff --git a/services/vision/obstacledistance/obstacle_distance_test.go b/services/vision/obstaclesdistance/obstacles_distance_test.go similarity index 99% rename from services/vision/obstacledistance/obstacle_distance_test.go rename to services/vision/obstaclesdistance/obstacles_distance_test.go index b5af8b998bd..64429b67a21 100644 --- a/services/vision/obstacledistance/obstacle_distance_test.go +++ b/services/vision/obstaclesdistance/obstacles_distance_test.go @@ -1,4 +1,4 @@ -package obstacledistance +package obstaclesdistance import ( "context" diff --git a/services/vision/radiusclustering/radius_clustering.go b/services/vision/obstaclespointcloud/obstacles_pointcloud.go similarity index 53% rename from services/vision/radiusclustering/radius_clustering.go rename to services/vision/obstaclespointcloud/obstacles_pointcloud.go index 0521fcad50e..826be301b3d 100644 --- a/services/vision/radiusclustering/radius_clustering.go +++ b/services/vision/obstaclespointcloud/obstacles_pointcloud.go @@ -1,6 +1,6 @@ -// Package radiusclustering uses the 3D radius clustering algorithm as defined in the +// Package obstaclespointcloud uses the 3D radius clustering algorithm as defined in the // RDK vision/segmentation package as vision model. -package radiusclustering +package obstaclespointcloud import ( "context" @@ -16,12 +16,12 @@ import ( "go.viam.com/rdk/vision/segmentation" ) -var model = resource.DefaultModelFamily.WithModel("radius_clustering_segmenter") +var model = resource.DefaultModelFamily.WithModel("obstacles_pointcloud") func init() { - resource.RegisterService(vision.API, model, resource.Registration[vision.Service, *segmentation.RadiusClusteringConfig]{ + resource.RegisterService(vision.API, model, resource.Registration[vision.Service, *segmentation.ErCCLConfig]{ DeprecatedRobotConstructor: func(ctx context.Context, r any, c resource.Config, logger golog.Logger) (vision.Service, error) { - attrs, err := resource.NativeConfig[*segmentation.RadiusClusteringConfig](c) + attrs, err := resource.NativeConfig[*segmentation.ErCCLConfig](c) if err != nil { return nil, err } @@ -29,27 +29,27 @@ func init() { if err != nil { return nil, err } - return registerRCSegmenter(ctx, c.ResourceName(), attrs, actualR) + return registerOPSegmenter(ctx, c.ResourceName(), attrs, actualR) }, }) } -// registerRCSegmenter creates a new 3D radius clustering segmenter from the config. -func registerRCSegmenter( +// registerOPSegmenter creates a new 3D radius clustering segmenter from the config. +func registerOPSegmenter( ctx context.Context, name resource.Name, - conf *segmentation.RadiusClusteringConfig, + conf *segmentation.ErCCLConfig, r robot.Robot, ) (vision.Service, error) { - _, span := trace.StartSpan(ctx, "service::vision::registerRadiusClustering") + _, span := trace.StartSpan(ctx, "service::vision::registerObstaclesPointcloud") defer span.End() if conf == nil { - return nil, errors.New("config for radius clustering segmenter cannot be nil") + return nil, errors.New("config for obstacles pointcloud segmenter cannot be nil") } err := conf.CheckValid() if err != nil { - return nil, errors.Wrap(err, "radius clustering segmenter config error") + return nil, errors.Wrap(err, "obstacles pointcloud segmenter config error") } - segmenter := segmentation.Segmenter(conf.RadiusClustering) + segmenter := segmentation.Segmenter(conf.ErCCLAlgorithm) return vision.NewService(name, r, nil, nil, nil, segmenter) } diff --git a/services/vision/radiusclustering/radius_clustering_test.go b/services/vision/obstaclespointcloud/obstacles_pointcloud_test.go similarity index 79% rename from services/vision/radiusclustering/radius_clustering_test.go rename to services/vision/obstaclespointcloud/obstacles_pointcloud_test.go index da8c7b2e4c3..6c79420d887 100644 --- a/services/vision/radiusclustering/radius_clustering_test.go +++ b/services/vision/obstaclespointcloud/obstacles_pointcloud_test.go @@ -1,10 +1,11 @@ -package radiusclustering +package obstaclespointcloud import ( "context" "image/color" "testing" + "github.com/golang/geo/r3" "github.com/pkg/errors" "go.viam.com/test" @@ -33,25 +34,28 @@ func TestRadiusClusteringSegmentation(t *testing.T) { return nil, resource.NewNotFoundError(n) } } - params := &segmentation.RadiusClusteringConfig{ - MinPtsInPlane: 100, - MinPtsInSegment: 3, - ClusteringRadiusMm: 5., - MeanKFiltering: 10., + params := &segmentation.ErCCLConfig{ + MinPtsInPlane: 100, + MaxDistFromPlane: 10, + MinPtsInSegment: 3, + AngleTolerance: 20, + NormalVec: r3.Vector{0, 0, 1}, + ClusteringRadius: 5, + ClusteringStrictness: 3, } // bad registration, no parameters name := vision.Named("test_rcs") - _, err := registerRCSegmenter(context.Background(), name, nil, r) + _, err := registerOPSegmenter(context.Background(), name, nil, r) test.That(t, err, test.ShouldNotBeNil) test.That(t, err.Error(), test.ShouldContainSubstring, "cannot be nil") // bad registration, parameters out of bounds - params.ClusteringRadiusMm = -3.0 - _, err = registerRCSegmenter(context.Background(), name, params, r) + params.ClusteringRadius = -3 + _, err = registerOPSegmenter(context.Background(), name, params, r) test.That(t, err, test.ShouldNotBeNil) test.That(t, err.Error(), test.ShouldContainSubstring, "segmenter config error") // successful registration - params.ClusteringRadiusMm = 5.0 - seg, err := registerRCSegmenter(context.Background(), name, params, r) + params.ClusteringRadius = 1 + seg, err := registerOPSegmenter(context.Background(), name, params, r) test.That(t, err, test.ShouldBeNil) test.That(t, seg.Name(), test.ShouldResemble, name) @@ -78,13 +82,13 @@ func TestRadiusClusteringSegmentation(t *testing.T) { err = cloud.Set(pc.NewVector(1, 1, 4), pc.NewColoredData(color.NRGBA{255, 0, 0, 255})) test.That(t, err, test.ShouldBeNil) // cluster 2 - err = cloud.Set(pc.NewVector(1, 1, 101), pc.NewColoredData(color.NRGBA{255, 0, 0, 255})) + err = cloud.Set(pc.NewVector(2, 2, 101), pc.NewColoredData(color.NRGBA{255, 0, 0, 255})) test.That(t, err, test.ShouldBeNil) - err = cloud.Set(pc.NewVector(1, 1, 102), pc.NewColoredData(color.NRGBA{255, 0, 0, 255})) + err = cloud.Set(pc.NewVector(2, 2, 102), pc.NewColoredData(color.NRGBA{255, 0, 0, 255})) test.That(t, err, test.ShouldBeNil) - err = cloud.Set(pc.NewVector(1, 1, 103), pc.NewColoredData(color.NRGBA{255, 0, 0, 255})) + err = cloud.Set(pc.NewVector(2, 2, 103), pc.NewColoredData(color.NRGBA{255, 0, 0, 255})) test.That(t, err, test.ShouldBeNil) - err = cloud.Set(pc.NewVector(1, 1, 104), pc.NewColoredData(color.NRGBA{255, 0, 0, 255})) + err = cloud.Set(pc.NewVector(2, 2, 104), pc.NewColoredData(color.NRGBA{255, 0, 0, 255})) test.That(t, err, test.ShouldBeNil) return cloud, nil } diff --git a/services/vision/register/register.go b/services/vision/register/register.go index ca21ca29eac..a012afa25b0 100644 --- a/services/vision/register/register.go +++ b/services/vision/register/register.go @@ -6,6 +6,7 @@ import ( _ "go.viam.com/rdk/services/vision/colordetector" _ "go.viam.com/rdk/services/vision/detectionstosegments" _ "go.viam.com/rdk/services/vision/mlvision" - _ "go.viam.com/rdk/services/vision/obstacledistance" - _ "go.viam.com/rdk/services/vision/radiusclustering" + _ "go.viam.com/rdk/services/vision/obstaclesdepth" + _ "go.viam.com/rdk/services/vision/obstaclesdistance" + _ "go.viam.com/rdk/services/vision/obstaclespointcloud" ) diff --git a/services/vision/server.go b/services/vision/server.go index 72df70c12d9..9c58cd643a8 100644 --- a/services/vision/server.go +++ b/services/vision/server.go @@ -198,6 +198,9 @@ func segmentsToProto(frame string, segs []*vision.Object) ([]*commonpb.PointClou protoSegs := make([]*commonpb.PointCloudObject, 0, len(segs)) for _, seg := range segs { var buf bytes.Buffer + if seg.PointCloud == nil { + seg.PointCloud = pointcloud.New() + } err := pointcloud.ToPCD(seg, &buf, pointcloud.PCDBinary) if err != nil { return nil, err diff --git a/spatialmath/geo_obstacle.go b/spatialmath/geo_obstacle.go index 7c4a08c6c5c..94b017c8583 100644 --- a/spatialmath/geo_obstacle.go +++ b/spatialmath/geo_obstacle.go @@ -31,7 +31,7 @@ func (gob *GeoObstacle) Geometries() []Geometry { } // GeoObstacleToProtobuf converts the GeoObstacle struct into an equivalent Protobuf message. -func GeoObstacleToProtobuf(geoObst *GeoObstacle) (*commonpb.GeoObstacle, error) { +func GeoObstacleToProtobuf(geoObst *GeoObstacle) *commonpb.GeoObstacle { var convGeoms []*commonpb.Geometry for _, geometry := range geoObst.geometries { convGeoms = append(convGeoms, geometry.ToProtobuf()) @@ -39,7 +39,7 @@ func GeoObstacleToProtobuf(geoObst *GeoObstacle) (*commonpb.GeoObstacle, error) return &commonpb.GeoObstacle{ Location: &commonpb.GeoPoint{Latitude: geoObst.location.Lat(), Longitude: geoObst.location.Lng()}, Geometries: convGeoms, - }, nil + } } // GeoObstacleFromProtobuf takes a Protobuf representation of a GeoObstacle and converts back into a Go struct. @@ -112,52 +112,71 @@ func GeoObstaclesFromConfig(config *GeoObstacleConfig) ([]*GeoObstacle, error) { return gobs, nil } -// GetCartesianDistance calculates the latitude and longitide displacement between p and q in kilometers. +// GetCartesianDistance calculates the latitude and longitide displacement between p and q in millimeters. +// Note that this is an approximation since we are trying to project a point on a sphere onto a plane. +// The closer these points are the more accurate the approximation is. func GetCartesianDistance(p, q *geo.Point) (float64, float64) { mod := geo.NewPoint(p.Lat(), q.Lng()) - // Calculates the Haversine distance between two points in kilometers - latDist := p.GreatCircleDistance(mod) - lngDist := q.GreatCircleDistance(mod) - return latDist, lngDist + // Calculate the Haversine distance between two points in kilometers, convert to mm + distAlongLat := 1e6 * p.GreatCircleDistance(mod) + distAlongLng := 1e6 * q.GreatCircleDistance(mod) + return distAlongLat, distAlongLng } -// GeoPointToPose converts p into a spatialmath pose relative to lng = 0 = lat. -func GeoPointToPose(p *geo.Point) Pose { - origin := geo.NewPoint(0, 0) - latDist, lngDist := GetCartesianDistance(origin, p) - azimuth := origin.BearingTo(p) +// GeoPointToPose converts p into a Pose +// Because the function we use to project a point on a spheroid to a plane is nonlinear, we linearize it about a specified origin point. +func GeoPointToPose(point, origin *geo.Point) Pose { + latDist, lngDist := GetCartesianDistance(origin, point) + azimuth := origin.BearingTo(point) switch { case azimuth >= 0 && azimuth <= 90: - // multiply to convert km to mm - return NewPoseFromPoint(r3.Vector{latDist * 1e6, lngDist * 1e6, 0}) + return NewPoseFromPoint(r3.Vector{latDist, lngDist, 0}) case azimuth > 90 && azimuth <= 180: - return NewPoseFromPoint(r3.Vector{latDist * 1e6, -lngDist * 1e6, 0}) + return NewPoseFromPoint(r3.Vector{latDist, -lngDist, 0}) case azimuth >= -90 && azimuth < 0: - return NewPoseFromPoint(r3.Vector{-latDist * 1e6, lngDist * 1e6, 0}) + return NewPoseFromPoint(r3.Vector{-latDist, lngDist, 0}) default: - return NewPoseFromPoint(r3.Vector{-latDist * 1e6, -lngDist * 1e6, 0}) + return NewPoseFromPoint(r3.Vector{-latDist, -lngDist, 0}) } } // GeoObstaclesToGeometries converts a list of GeoObstacles into a list of Geometries. -func GeoObstaclesToGeometries(obstacles []*GeoObstacle, worldOrigin r3.Vector) []Geometry { +func GeoObstaclesToGeometries(obstacles []*GeoObstacle, origin *geo.Point) []Geometry { // we note that there are two transformations to be accounted for // when converting a GeoObstacle. Namely, the obstacle's pose needs to // transformed by the specified in GPS coordinates. geoms := []Geometry{} for _, v := range obstacles { - obstacleOrigin := GeoPointToPose(v.location) - relativeDestinationPt := r3.Vector{ - X: obstacleOrigin.Point().X - worldOrigin.X, - Y: obstacleOrigin.Point().Y - worldOrigin.Y, - Z: 0, - } - relativeDstPose := NewPoseFromPoint(relativeDestinationPt) + relativePose := GeoPointToPose(v.location, origin) for _, geom := range v.geometries { - geo := geom.Transform(relativeDstPose) + geo := geom.Transform(relativePose) geoms = append(geoms, geo) } } return geoms } + +// GeoPose is a struct to store to location and heading in a geospatial environment. +type GeoPose struct { + location *geo.Point + heading float64 +} + +// NewGeoPose constructs a GeoPose from a geo.Point and float64. +func NewGeoPose(loc *geo.Point, heading float64) *GeoPose { + return &GeoPose{ + location: loc, + heading: heading, + } +} + +// Location returns the locating coordinates of the GeoPose. +func (gpo *GeoPose) Location() *geo.Point { + return gpo.location +} + +// Heading returns a number from [0-360) where 0 is north. +func (gpo *GeoPose) Heading() float64 { + return gpo.heading +} diff --git a/spatialmath/geo_obstacle_test.go b/spatialmath/geo_obstacle_test.go index 176fe5a372c..93290e7e1c3 100644 --- a/spatialmath/geo_obstacle_test.go +++ b/spatialmath/geo_obstacle_test.go @@ -1,6 +1,7 @@ package spatialmath import ( + "fmt" "testing" "github.com/golang/geo/r3" @@ -9,6 +10,30 @@ import ( "go.viam.com/test" ) +func TestGeoPose(t *testing.T) { + origin := geo.NewPoint(0, 0) + testCases := []struct { + *geo.Point + r3.Vector + }{ + {geo.NewPoint(9e-9, 9e-9), r3.Vector{1, 1, 0}}, + {geo.NewPoint(0, 9e-9), r3.Vector{1, 0, 0}}, + {geo.NewPoint(-9e-9, 9e-9), r3.Vector{1, -1, 0}}, + {geo.NewPoint(9e-9, 0), r3.Vector{0, 1, 0}}, + {geo.NewPoint(0, 0), r3.Vector{0, 0, 0}}, + {geo.NewPoint(-9e-9, -9e-9), r3.Vector{-1, -1, 0}}, + {geo.NewPoint(0, -9e-9), r3.Vector{-1, 0, 0}}, + {geo.NewPoint(9e-9, -9e-9), r3.Vector{-1, 1, 0}}, + } + + for i, tc := range testCases { + t.Run(fmt.Sprint(i), func(t *testing.T) { + pose := GeoPointToPose(tc.Point, origin) + test.That(t, R3VectorAlmostEqual(pose.Point(), tc.Vector, 0.1), test.ShouldBeTrue) + }) + } +} + func TestGeoObstacles(t *testing.T) { testLatitude := 39.58836 testLongitude := -105.64464 @@ -23,7 +48,7 @@ func TestGeoObstacles(t *testing.T) { test.That(t, testGeoms, test.ShouldResemble, testGeoObst.Geometries()) t.Run("Conversion from GeoObstacle to Protobuf", func(t *testing.T) { - convGeoObstProto, err := GeoObstacleToProtobuf(testGeoObst) + convGeoObstProto := GeoObstacleToProtobuf(testGeoObst) test.That(t, err, test.ShouldBeNil) test.That(t, testPoint.Lat(), test.ShouldEqual, convGeoObstProto.GetLocation().GetLatitude()) test.That(t, testPoint.Lng(), test.ShouldEqual, convGeoObstProto.GetLocation().GetLongitude()) @@ -68,33 +93,3 @@ func TestGeoObstacles(t *testing.T) { test.That(t, conv[0].geometries, test.ShouldResemble, testGeoObst.geometries) }) } - -func TestConvertGeoPointToPose(t *testing.T) { - gp := geo.NewPoint(0, 0) - pose := GeoPointToPose(gp) - test.That(t, R3VectorAlmostEqual(pose.Point(), r3.Vector{0, 0, 0}, 0.1), test.ShouldBeTrue) - - gp = geo.NewPoint(0.0000009, 0.0000009) - pose = GeoPointToPose(gp) - test.That(t, R3VectorAlmostEqual(pose.Point(), r3.Vector{100, 100, 0}, 0.1), test.ShouldBeTrue) - - gp = geo.NewPoint(0, 0.0000009) - pose = GeoPointToPose(gp) - test.That(t, R3VectorAlmostEqual(pose.Point(), r3.Vector{100, 0, 0}, 0.1), test.ShouldBeTrue) - - gp = geo.NewPoint(-0.0000009, 0.0000009) - pose = GeoPointToPose(gp) - test.That(t, R3VectorAlmostEqual(pose.Point(), r3.Vector{100, -100, 0}, 0.1), test.ShouldBeTrue) - - gp = geo.NewPoint(-0.0000009, 0) - pose = GeoPointToPose(gp) - test.That(t, R3VectorAlmostEqual(pose.Point(), r3.Vector{0, -100, 0}, 0.1), test.ShouldBeTrue) - - gp = geo.NewPoint(-0.0000009, -0.0000009) - pose = GeoPointToPose(gp) - test.That(t, R3VectorAlmostEqual(pose.Point(), r3.Vector{-100, -100, 0}, 0.1), test.ShouldBeTrue) - - gp = geo.NewPoint(0, -0.0000009) - pose = GeoPointToPose(gp) - test.That(t, R3VectorAlmostEqual(pose.Point(), r3.Vector{-100, 0, 0}, 0.1), test.ShouldBeTrue) -} diff --git a/spatialmath/pose.go b/spatialmath/pose.go index cd786eecba7..ecf4ccead05 100644 --- a/spatialmath/pose.go +++ b/spatialmath/pose.go @@ -97,11 +97,36 @@ func Compose(a, b Pose) Pose { } // PoseBetween returns the difference between two dualQuaternions, that is, the dq which if multiplied by one will give the other. +// Example: if PoseBetween(a, b) = c, then Compose(a, c) = b. func PoseBetween(a, b Pose) Pose { - return &dualQuaternion{dualquat.Mul(dualQuaternionFromPose(b).Number, dualquat.ConjQuat(dualQuaternionFromPose(a).Number))} + invA := &dualQuaternion{dualquat.ConjQuat(dualQuaternionFromPose(a).Number)} + result := &dualQuaternion{invA.Transformation(dualQuaternionFromPose(b).Number)} + // Normalization + if vecLen := 1 / quat.Abs(result.Real); vecLen != 1 { + result.Real.Real *= vecLen + result.Real.Imag *= vecLen + result.Real.Jmag *= vecLen + result.Real.Kmag *= vecLen + } + return result +} + +// PoseBetweenInverse returns an origin pose which when composed with the first parameter, yields the second. +// Example: if PoseBetweenInverse(a, b) = c, then Compose(c, a) = b +// PoseBetweenInverse(a, b) is equivalent to Compose(b, PoseInverse(a)). +func PoseBetweenInverse(a, b Pose) Pose { + result := &dualQuaternion{dualQuaternionFromPose(b).Transformation(dualquat.ConjQuat(dualQuaternionFromPose(a).Number))} + // Normalization + if vecLen := 1 / quat.Abs(result.Real); vecLen != 1 { + result.Real.Real *= vecLen + result.Real.Imag *= vecLen + result.Real.Jmag *= vecLen + result.Real.Kmag *= vecLen + } + return result } -// PoseDelta returns the difference between two dualQuaternion. +// PoseDelta returns the difference between two dualQuaternion. Useful for measuring distances, NOT to be used for spatial transformations. // We use quaternion/angle axis for this because distances are well-defined. func PoseDelta(a, b Pose) Pose { return &distancePose{ diff --git a/spatialmath/pose_test.go b/spatialmath/pose_test.go index 7a9a51bf829..ccb172a620a 100644 --- a/spatialmath/pose_test.go +++ b/spatialmath/pose_test.go @@ -42,6 +42,15 @@ func TestBasicPoseConstruction(t *testing.T) { ptCompare(t, delta.Point(), r3.Vector{1.0, 1.0, 1.0}) ovCompare(t, delta.Orientation().OrientationVectorRadians(), NewOrientationVector()) + p2 = NewPoseFromPoint(r3.Vector{2, 3, 4}) + + pb := PoseBetween(p1, p2) + test.That(t, PoseAlmostEqual(Compose(p1, pb), p2), test.ShouldBeTrue) + pbi := PoseBetweenInverse(p1, p2) + test.That(t, PoseAlmostEqual(Compose(pbi, p1), p2), test.ShouldBeTrue) + pbi2 := Compose(p2, PoseInverse(p1)) + test.That(t, PoseAlmostEqual(pbi, pbi2), test.ShouldBeTrue) + p = NewPoseFromOrientation(&R4AA{0, 4, 5, 6}) test.That(t, p.Orientation().OrientationVectorRadians(), test.ShouldResemble, &OrientationVector{0, 0, 0, 1}) } diff --git a/spatialmath/quat_test.go b/spatialmath/quat_test.go index 314340e5e61..41a3eaeb3eb 100644 --- a/spatialmath/quat_test.go +++ b/spatialmath/quat_test.go @@ -2,7 +2,6 @@ package spatialmath import ( "math" - "runtime" "testing" "github.com/golang/geo/r3" @@ -33,30 +32,51 @@ func TestAngleAxisConversion2(t *testing.T) { } func TestEulerAnglesConversion(t *testing.T) { - // TODO RSDK-2010 (rh) handle edge cases properly while - // maintaining quadrant in euler conversions - if runtime.GOARCH == "arm64" { - t.Skip() + for _, testcase := range []struct { + name string + expectedEA EulerAngles + q quat.Number + }{ + { + "vanilla 1: roll pitch and yaw are not near edge cases", + EulerAngles{math.Pi / 4.0, math.Pi / 4.0, 3.0 * math.Pi / 4.0}, + quat.Number{Real: 0.46193978734586505, Imag: -0.19134171618254486, Jmag: 0.4619397662556434, Kmag: 0.7325378046916491}, + }, + { + "vanilla 2: roll pitch and yaw are not near edge cases", + EulerAngles{-math.Pi / 4.0, -math.Pi / 4.0, math.Pi / 4.0}, + quat.Number{Real: 0.8446231850190303, Imag: -0.19134170056642805, Jmag: -0.461939798632522, Kmag: 0.19134170056642805}, + }, + { + "gimbal lock: pitch is Ï€/2", + EulerAngles{-3 * math.Pi / 4.0, math.Pi / 2.0, 0}, + quat.Number{Real: 0.2705980500730985, Imag: -0.6532814824381882, Jmag: 0.27059805007309856, Kmag: 0.6532814824381883}, + }, + { + "heading only", + EulerAngles{0, 0, math.Pi / 3}, + quat.Number{Real: 0.8660254042574935, Imag: 0, Jmag: 0, Kmag: 0.5}, + }, + } { + t.Run(testcase.name, func(t *testing.T) { + endEa := QuatToEulerAngles(testcase.q) + q2 := endEa.Quaternion() + + t.Run("roll", func(t *testing.T) { + test.That(t, testcase.expectedEA.Roll, test.ShouldAlmostEqual, endEa.Roll, 1e-6) + }) + t.Run("pitch", func(t *testing.T) { + test.That(t, testcase.expectedEA.Pitch, test.ShouldAlmostEqual, endEa.Pitch, 1e-6) + }) + t.Run("yaw", func(t *testing.T) { + test.That(t, testcase.expectedEA.Yaw, test.ShouldAlmostEqual, endEa.Yaw, 1e-6) + }) + t.Run("quat", func(t *testing.T) { + quatCompare(t, testcase.q, q2) + }) + }, + ) } - - // gimbal lock edge case: pitch is Ï€ / 2 - expectedEA := EulerAngles{math.Pi / 4.0, math.Pi / 2.0, math.Pi} - q := quat.Number{Real: 0.2705980500730985, Imag: -0.6532814824381882, Jmag: 0.27059805007309856, Kmag: 0.6532814824381883} - endEa := QuatToEulerAngles(q) - test.That(t, expectedEA.Roll, test.ShouldAlmostEqual, endEa.Roll) - test.That(t, expectedEA.Pitch, test.ShouldAlmostEqual, endEa.Pitch) - test.That(t, expectedEA.Yaw, test.ShouldAlmostEqual, endEa.Yaw) - q2 := endEa.Quaternion() - quatCompare(t, q, q2) - - expectedEA = EulerAngles{math.Pi / 4.0, math.Pi / 4.0, 3.0 * math.Pi / 4.0} - q = quat.Number{Real: 0.4619397662556435, Imag: -0.19134171618254486, Jmag: 0.4619397662556434, Kmag: 0.7325378163287418} - endEa = QuatToEulerAngles(q) - test.That(t, expectedEA.Roll, test.ShouldAlmostEqual, endEa.Roll) - test.That(t, expectedEA.Pitch, test.ShouldAlmostEqual, endEa.Pitch) - test.That(t, expectedEA.Yaw, test.ShouldAlmostEqual, endEa.Yaw) - q2 = endEa.Quaternion() - quatCompare(t, q, q2) } func TestMatrixConversion(t *testing.T) { diff --git a/spatialmath/quaternion.go b/spatialmath/quaternion.go index db515f6befc..5d72c18f70d 100644 --- a/spatialmath/quaternion.go +++ b/spatialmath/quaternion.go @@ -51,7 +51,15 @@ func QuatToEulerAngles(q quat.Number) *EulerAngles { // yaw (z-axis rotation) sinyCosp := 2.0 * (q.Real*q.Kmag + q.Imag*q.Jmag) + if math.Abs(sinyCosp) < 1e-12 { // && runtime.GOARCH == "arm64" + sinyCosp = 0 + } + cosyCosp := 1.0 - 2.0*(q.Jmag*q.Jmag+q.Kmag*q.Kmag) + if math.Abs(cosyCosp) < 1e-12 { + cosyCosp = 0 + } + angles.Yaw = math.Atan2(sinyCosp, cosyCosp) // for a pitch that is Ï€ / 2, we experience gimbal lock diff --git a/testutils/inject/app_service_client.go b/testutils/inject/app_service_client.go new file mode 100644 index 00000000000..e70da5b365c --- /dev/null +++ b/testutils/inject/app_service_client.go @@ -0,0 +1,37 @@ +package inject + +import ( + "context" + + apppb "go.viam.com/api/app/v1" + "google.golang.org/grpc" +) + +// AppServiceClient represents a fake instance of an app service client. +type AppServiceClient struct { + apppb.AppServiceClient + ListOrganizationsFunc func(ctx context.Context, in *apppb.ListOrganizationsRequest, + opts ...grpc.CallOption) (*apppb.ListOrganizationsResponse, error) + CreateKeyFunc func(ctx context.Context, in *apppb.CreateKeyRequest, + opts ...grpc.CallOption) (*apppb.CreateKeyResponse, error) +} + +// ListOrganizations calls the injected ListOrganizationsFunc or the real version. +func (asc *AppServiceClient) ListOrganizations(ctx context.Context, in *apppb.ListOrganizationsRequest, + opts ...grpc.CallOption, +) (*apppb.ListOrganizationsResponse, error) { + if asc.ListOrganizationsFunc == nil { + return asc.AppServiceClient.ListOrganizations(ctx, in, opts...) + } + return asc.ListOrganizationsFunc(ctx, in, opts...) +} + +// CreateKey calls the injected CreateKeyFunc or the real version. +func (asc *AppServiceClient) CreateKey(ctx context.Context, in *apppb.CreateKeyRequest, + opts ...grpc.CallOption, +) (*apppb.CreateKeyResponse, error) { + if asc.CreateKeyFunc == nil { + return asc.AppServiceClient.CreateKey(ctx, in, opts...) + } + return asc.CreateKeyFunc(ctx, in, opts...) +} diff --git a/testutils/inject/arm.go b/testutils/inject/arm.go index 7762c500686..c686c730811 100644 --- a/testutils/inject/arm.go +++ b/testutils/inject/arm.go @@ -24,6 +24,8 @@ type Arm struct { IsMovingFunc func(context.Context) (bool, error) CloseFunc func(ctx context.Context) error ModelFrameFunc func() referenceframe.Model + CurrentInputsFunc func(ctx context.Context) ([]referenceframe.Input, error) + GoToInputsFunc func(ctx context.Context, goal []referenceframe.Input) error } // NewArm returns a new injected arm. @@ -114,3 +116,19 @@ func (a *Arm) ModelFrame() referenceframe.Model { } return a.ModelFrameFunc() } + +// CurrentInputs calls the injected CurrentInputs or the real version. +func (a *Arm) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) { + if a.CurrentInputsFunc == nil { + return a.Arm.CurrentInputs(ctx) + } + return a.CurrentInputsFunc(ctx) +} + +// GoToInputs calls the injected GoToInputs or the real version. +func (a *Arm) GoToInputs(ctx context.Context, goal []referenceframe.Input) error { + if a.GoToInputsFunc == nil { + return a.Arm.GoToInputs(ctx, goal) + } + return a.GoToInputsFunc(ctx, goal) +} diff --git a/testutils/inject/base.go b/testutils/inject/base.go index 6e657ac23ac..a4671065b95 100644 --- a/testutils/inject/base.go +++ b/testutils/inject/base.go @@ -112,9 +112,9 @@ func (b *Base) Properties(ctx context.Context, extra map[string]interface{}) (ba } // Geometries returns the base's geometries. -func (b *Base) Geometries(ctx context.Context) ([]spatialmath.Geometry, error) { +func (b *Base) Geometries(ctx context.Context, extra map[string]interface{}) ([]spatialmath.Geometry, error) { if b.GeometriesFunc == nil { - return b.Base.Geometries(ctx) + return b.Base.Geometries(ctx, extra) } return b.GeometriesFunc(ctx) } diff --git a/testutils/inject/camera.go b/testutils/inject/camera.go index 3b0107bf5ce..317abe53011 100644 --- a/testutils/inject/camera.go +++ b/testutils/inject/camera.go @@ -17,6 +17,7 @@ type Camera struct { camera.Camera name resource.Name DoFunc func(ctx context.Context, cmd map[string]interface{}) (map[string]interface{}, error) + ImagesFunc func(ctx context.Context) ([]camera.NamedImage, resource.ResponseMetadata, error) StreamFunc func( ctx context.Context, errHandlers ...gostream.ErrorHandler, @@ -75,6 +76,14 @@ func (c *Camera) Properties(ctx context.Context) (camera.Properties, error) { return c.PropertiesFunc(ctx) } +// Images calls the injected Images or the real version. +func (c *Camera) Images(ctx context.Context) ([]camera.NamedImage, resource.ResponseMetadata, error) { + if c.ImagesFunc == nil { + return c.Camera.Images(ctx) + } + return c.ImagesFunc(ctx) +} + // Close calls the injected Close or the real version. func (c *Camera) Close(ctx context.Context) error { if c.CloseFunc == nil { diff --git a/testutils/inject/data_service_client.go b/testutils/inject/data_service_client.go new file mode 100644 index 00000000000..bba53dee407 --- /dev/null +++ b/testutils/inject/data_service_client.go @@ -0,0 +1,27 @@ +package inject + +import ( + "context" + + datapb "go.viam.com/api/app/data/v1" + "google.golang.org/grpc" +) + +// DataServiceClient represents a fake instance of a data service client. +type DataServiceClient struct { + datapb.DataServiceClient + TabularDataByFilterFunc func( + ctx context.Context, + in *datapb.TabularDataByFilterRequest, + opts ...grpc.CallOption, + ) (*datapb.TabularDataByFilterResponse, error) +} + +// TabularDataByFilter calls the injected TabularDataByFilter or the real version. +func (client *DataServiceClient) TabularDataByFilter(ctx context.Context, in *datapb.TabularDataByFilterRequest, opts ...grpc.CallOption, +) (*datapb.TabularDataByFilterResponse, error) { + if client.TabularDataByFilterFunc == nil { + return client.DataServiceClient.TabularDataByFilter(ctx, in, opts...) + } + return client.TabularDataByFilterFunc(ctx, in, opts...) +} diff --git a/testutils/inject/encoder.go b/testutils/inject/encoder.go index ee93bb4467a..117b4dade6b 100644 --- a/testutils/inject/encoder.go +++ b/testutils/inject/encoder.go @@ -17,7 +17,7 @@ type Encoder struct { positionType encoder.PositionType, extra map[string]interface{}, ) (float64, encoder.PositionType, error) - PropertiesFunc func(ctx context.Context, extra map[string]interface{}) (map[encoder.Feature]bool, error) + PropertiesFunc func(ctx context.Context, extra map[string]interface{}) (encoder.Properties, error) } // NewEncoder returns a new injected Encoder. @@ -51,7 +51,7 @@ func (e *Encoder) Position( } // Properties calls the injected Properties or the real version. -func (e *Encoder) Properties(ctx context.Context, extra map[string]interface{}) (map[encoder.Feature]bool, error) { +func (e *Encoder) Properties(ctx context.Context, extra map[string]interface{}) (encoder.Properties, error) { if e.PropertiesFunc == nil { return e.Encoder.Properties(ctx, extra) } diff --git a/testutils/inject/mlmodel_service.go b/testutils/inject/mlmodel_service.go index d8cfe0fb2b5..5807bd2c3d3 100644 --- a/testutils/inject/mlmodel_service.go +++ b/testutils/inject/mlmodel_service.go @@ -3,6 +3,7 @@ package inject import ( "context" + "go.viam.com/rdk/ml" "go.viam.com/rdk/resource" "go.viam.com/rdk/services/mlmodel" ) @@ -11,7 +12,7 @@ import ( type MLModelService struct { mlmodel.Service name resource.Name - InferFunc func(ctx context.Context, input map[string]interface{}) (map[string]interface{}, error) + InferFunc func(ctx context.Context, tensors ml.Tensors, input map[string]interface{}) (ml.Tensors, map[string]interface{}, error) MetadataFunc func(ctx context.Context) (mlmodel.MLMetadata, error) CloseFunc func(ctx context.Context) error } @@ -27,11 +28,15 @@ func (s *MLModelService) Name() resource.Name { } // Infer calls the injected Infer or the real variant. -func (s *MLModelService) Infer(ctx context.Context, input map[string]interface{}) (map[string]interface{}, error) { +func (s *MLModelService) Infer( + ctx context.Context, + tensors ml.Tensors, + input map[string]interface{}, +) (ml.Tensors, map[string]interface{}, error) { if s.InferFunc == nil { - return s.Service.Infer(ctx, input) + return s.Service.Infer(ctx, tensors, input) } - return s.InferFunc(ctx, input) + return s.InferFunc(ctx, tensors, input) } // Metadata calls the injected Metadata or the real variant. diff --git a/testutils/inject/motion_service.go b/testutils/inject/motion_service.go index f1bd6afdd00..72d2e56550c 100644 --- a/testutils/inject/motion_service.go +++ b/testutils/inject/motion_service.go @@ -39,15 +39,7 @@ type MotionService struct { heading float64, movementSensorName resource.Name, obstacles []*spatialmath.GeoObstacle, - linearVelocity float64, - angularVelocity float64, - extra map[string]interface{}, - ) (bool, error) - MoveSingleComponentFunc func( - ctx context.Context, - componentName resource.Name, - grabPose *referenceframe.PoseInFrame, - worldState *referenceframe.WorldState, + motionCfg *motion.MotionConfiguration, extra map[string]interface{}, ) (bool, error) GetPoseFunc func( @@ -109,28 +101,13 @@ func (mgs *MotionService) MoveOnGlobe( heading float64, movementSensorName resource.Name, obstacles []*spatialmath.GeoObstacle, - linearVel float64, - angularVel float64, + motionCfg *motion.MotionConfiguration, extra map[string]interface{}, ) (bool, error) { if mgs.MoveOnGlobeFunc == nil { - return mgs.Service.MoveOnGlobe(ctx, componentName, destination, heading, movementSensorName, obstacles, linearVel, angularVel, extra) - } - return mgs.MoveOnGlobeFunc(ctx, componentName, destination, heading, movementSensorName, obstacles, linearVel, angularVel, extra) -} - -// MoveSingleComponent calls the injected MoveSingleComponent or the real variant. It uses the same function as Move. -func (mgs *MotionService) MoveSingleComponent( - ctx context.Context, - componentName resource.Name, - destination *referenceframe.PoseInFrame, - worldState *referenceframe.WorldState, - extra map[string]interface{}, -) (bool, error) { - if mgs.MoveFunc == nil { - return mgs.Service.MoveSingleComponent(ctx, componentName, destination, worldState, extra) + return mgs.Service.MoveOnGlobe(ctx, componentName, destination, heading, movementSensorName, obstacles, motionCfg, extra) } - return mgs.MoveSingleComponentFunc(ctx, componentName, destination, worldState, extra) + return mgs.MoveOnGlobeFunc(ctx, componentName, destination, heading, movementSensorName, obstacles, motionCfg, extra) } // GetPose calls the injected GetPose or the real variant. diff --git a/testutils/inject/motor.go b/testutils/inject/motor.go index c2c2e7bd207..8fd90e73428 100644 --- a/testutils/inject/motor.go +++ b/testutils/inject/motor.go @@ -17,7 +17,7 @@ type Motor struct { GoToFunc func(ctx context.Context, rpm, position float64, extra map[string]interface{}) error ResetZeroPositionFunc func(ctx context.Context, offset float64, extra map[string]interface{}) error PositionFunc func(ctx context.Context, extra map[string]interface{}) (float64, error) - PropertiesFunc func(ctx context.Context, extra map[string]interface{}) (map[motor.Feature]bool, error) + PropertiesFunc func(ctx context.Context, extra map[string]interface{}) (motor.Properties, error) StopFunc func(ctx context.Context, extra map[string]interface{}) error IsPoweredFunc func(ctx context.Context, extra map[string]interface{}) (bool, float64, error) IsMovingFunc func(context.Context) (bool, error) @@ -74,7 +74,7 @@ func (m *Motor) Position(ctx context.Context, extra map[string]interface{}) (flo } // Properties calls the injected Properties or the real version. -func (m *Motor) Properties(ctx context.Context, extra map[string]interface{}) (map[motor.Feature]bool, error) { +func (m *Motor) Properties(ctx context.Context, extra map[string]interface{}) (motor.Properties, error) { if m.PropertiesFunc == nil { return m.Motor.Properties(ctx, extra) } diff --git a/testutils/inject/navigation_service.go b/testutils/inject/navigation_service.go index b13198357c7..ed12856b961 100644 --- a/testutils/inject/navigation_service.go +++ b/testutils/inject/navigation_service.go @@ -8,6 +8,7 @@ import ( "go.viam.com/rdk/resource" "go.viam.com/rdk/services/navigation" + "go.viam.com/rdk/spatialmath" ) // NavigationService represents a fake instance of a navigation service. @@ -17,7 +18,7 @@ type NavigationService struct { ModeFunc func(ctx context.Context, extra map[string]interface{}) (navigation.Mode, error) SetModeFunc func(ctx context.Context, mode navigation.Mode, extra map[string]interface{}) error - LocationFunc func(ctx context.Context, extra map[string]interface{}) (*geo.Point, error) + LocationFunc func(ctx context.Context, extra map[string]interface{}) (*spatialmath.GeoPose, error) WaypointsFunc func(ctx context.Context, extra map[string]interface{}) ([]navigation.Waypoint, error) AddWaypointFunc func(ctx context.Context, point *geo.Point, extra map[string]interface{}) error @@ -54,7 +55,7 @@ func (ns *NavigationService) SetMode(ctx context.Context, mode navigation.Mode, } // Location calls the injected LocationFunc or the real version. -func (ns *NavigationService) Location(ctx context.Context, extra map[string]interface{}) (*geo.Point, error) { +func (ns *NavigationService) Location(ctx context.Context, extra map[string]interface{}) (*spatialmath.GeoPose, error) { if ns.LocationFunc == nil { return ns.Service.Location(ctx, extra) } diff --git a/testutils/inject/powersensor.go b/testutils/inject/powersensor.go new file mode 100644 index 00000000000..55960ff7848 --- /dev/null +++ b/testutils/inject/powersensor.go @@ -0,0 +1,60 @@ +package inject + +import ( + "context" + + "go.viam.com/rdk/components/powersensor" + "go.viam.com/rdk/resource" +) + +// A PowerSensor reports information about voltage, current and power. +type PowerSensor struct { + powersensor.PowerSensor + name resource.Name + VoltageFunc func(ctx context.Context, extra map[string]interface{}) (float64, bool, error) + CurrentFunc func(ctx context.Context, extra map[string]interface{}) (float64, bool, error) + PowerFunc func(ctx context.Context, extra map[string]interface{}) (float64, error) + DoFunc func(ctx context.Context, cmd map[string]interface{}) (map[string]interface{}, error) +} + +// NewPowerSensor returns a new injected movement sensor. +func NewPowerSensor(name string) *PowerSensor { + return &PowerSensor{name: powersensor.Named(name)} +} + +// Name returns the name of the resource. +func (i *PowerSensor) Name() resource.Name { + return i.name +} + +// DoCommand calls the injected DoCommand or the real version. +func (i *PowerSensor) DoCommand(ctx context.Context, cmd map[string]interface{}) (map[string]interface{}, error) { + if i.DoFunc == nil { + return i.PowerSensor.DoCommand(ctx, cmd) + } + return i.DoFunc(ctx, cmd) +} + +// Voltage func or passthrough. +func (i *PowerSensor) Voltage(ctx context.Context, cmd map[string]interface{}) (float64, bool, error) { + if i.VoltageFunc == nil { + return i.PowerSensor.Voltage(ctx, cmd) + } + return i.VoltageFunc(ctx, cmd) +} + +// Current func or passthrough. +func (i *PowerSensor) Current(ctx context.Context, cmd map[string]interface{}) (float64, bool, error) { + if i.CurrentFunc == nil { + return i.PowerSensor.Current(ctx, cmd) + } + return i.CurrentFunc(ctx, cmd) +} + +// Power func or passthrough. +func (i *PowerSensor) Power(ctx context.Context, cmd map[string]interface{}) (float64, error) { + if i.PowerFunc == nil { + return i.PowerSensor.Power(ctx, cmd) + } + return i.PowerFunc(ctx, cmd) +} diff --git a/testutils/inject/slam_service.go b/testutils/inject/slam_service.go index ca2a017a6f0..73dff4711ce 100644 --- a/testutils/inject/slam_service.go +++ b/testutils/inject/slam_service.go @@ -12,13 +12,13 @@ import ( // SLAMService represents a fake instance of a slam service. type SLAMService struct { slam.Service - name resource.Name - GetPositionFunc func(ctx context.Context) (spatialmath.Pose, string, error) - GetPointCloudMapFunc func(ctx context.Context) (func() ([]byte, error), error) - GetInternalStateFunc func(ctx context.Context) (func() ([]byte, error), error) - GetLatestMapInfoFunc func(ctx context.Context) (time.Time, error) - DoCommandFunc func(ctx context.Context, cmd map[string]interface{}) (map[string]interface{}, error) - CloseFunc func(ctx context.Context) error + name resource.Name + PositionFunc func(ctx context.Context) (spatialmath.Pose, string, error) + PointCloudMapFunc func(ctx context.Context) (func() ([]byte, error), error) + InternalStateFunc func(ctx context.Context) (func() ([]byte, error), error) + LatestMapInfoFunc func(ctx context.Context) (time.Time, error) + DoCommandFunc func(ctx context.Context, cmd map[string]interface{}) (map[string]interface{}, error) + CloseFunc func(ctx context.Context) error } // NewSLAMService returns a new injected SLAM service. @@ -31,36 +31,36 @@ func (slamSvc *SLAMService) Name() resource.Name { return slamSvc.name } -// GetPosition calls the injected GetPositionFunc or the real version. -func (slamSvc *SLAMService) GetPosition(ctx context.Context) (spatialmath.Pose, string, error) { - if slamSvc.GetPositionFunc == nil { - return slamSvc.Service.GetPosition(ctx) +// Position calls the injected PositionFunc or the real version. +func (slamSvc *SLAMService) Position(ctx context.Context) (spatialmath.Pose, string, error) { + if slamSvc.PositionFunc == nil { + return slamSvc.Service.Position(ctx) } - return slamSvc.GetPositionFunc(ctx) + return slamSvc.PositionFunc(ctx) } -// GetPointCloudMap calls the injected GetPointCloudMap or the real version. -func (slamSvc *SLAMService) GetPointCloudMap(ctx context.Context) (func() ([]byte, error), error) { - if slamSvc.GetPointCloudMapFunc == nil { - return slamSvc.Service.GetPointCloudMap(ctx) +// PointCloudMap calls the injected PointCloudMap or the real version. +func (slamSvc *SLAMService) PointCloudMap(ctx context.Context) (func() ([]byte, error), error) { + if slamSvc.PointCloudMapFunc == nil { + return slamSvc.Service.PointCloudMap(ctx) } - return slamSvc.GetPointCloudMapFunc(ctx) + return slamSvc.PointCloudMapFunc(ctx) } -// GetInternalState calls the injected GetInternalState or the real version. -func (slamSvc *SLAMService) GetInternalState(ctx context.Context) (func() ([]byte, error), error) { - if slamSvc.GetInternalStateFunc == nil { - return slamSvc.Service.GetInternalState(ctx) +// InternalState calls the injected InternalState or the real version. +func (slamSvc *SLAMService) InternalState(ctx context.Context) (func() ([]byte, error), error) { + if slamSvc.InternalStateFunc == nil { + return slamSvc.Service.InternalState(ctx) } - return slamSvc.GetInternalStateFunc(ctx) + return slamSvc.InternalStateFunc(ctx) } -// GetLatestMapInfo calls the injected GetLatestMapInfoFunc or the real version. -func (slamSvc *SLAMService) GetLatestMapInfo(ctx context.Context) (time.Time, error) { - if slamSvc.GetLatestMapInfoFunc == nil { - return slamSvc.Service.GetLatestMapInfo(ctx) +// LatestMapInfo calls the injected LatestMapInfoFunc or the real version. +func (slamSvc *SLAMService) LatestMapInfo(ctx context.Context) (time.Time, error) { + if slamSvc.LatestMapInfoFunc == nil { + return slamSvc.Service.LatestMapInfo(ctx) } - return slamSvc.GetLatestMapInfoFunc(ctx) + return slamSvc.LatestMapInfoFunc(ctx) } // DoCommand calls the injected DoCommand or the real variant. diff --git a/utils/channel.go b/utils/channel.go new file mode 100644 index 00000000000..da77c44948e --- /dev/null +++ b/utils/channel.go @@ -0,0 +1,12 @@ +package utils + +// FlushChan is a function that takes a generic chanel and completely empties it. +func FlushChan[T any](c chan T) { + for gotSomething := true; gotSomething; { + select { + case <-c: + default: + gotSomething = false + } + } +} diff --git a/utils/env.go b/utils/env.go new file mode 100644 index 00000000000..82c165ebc3e --- /dev/null +++ b/utils/env.go @@ -0,0 +1,36 @@ +package utils + +import ( + "os" + "time" + + "github.com/edaniels/golog" +) + +const ( + // DefaultResourceConfigurationTimeout is the default resource configuration + // timeout. + DefaultResourceConfigurationTimeout = time.Minute + + // ResourceConfigurationTimeoutEnvVar is the environment variable that can + // be set to override defaultResourceConfigurationTimeout as the duration + // that resources and modules are allowed to (re)configure and startup + // respectively. + ResourceConfigurationTimeoutEnvVar = "VIAM_RESOURCE_CONFIGURATION_TIMEOUT" +) + +// GetResourceConfigurationTimeout calculates the resource configuration +// timeout (env variable value if set, defaultResourceConfigurationTimeout +// otherwise). +func GetResourceConfigurationTimeout(logger golog.Logger) time.Duration { + if timeoutVal := os.Getenv(ResourceConfigurationTimeoutEnvVar); timeoutVal != "" { + timeout, err := time.ParseDuration(timeoutVal) + if err != nil { + logger.Warn("Failed to parse %s env var, falling back to default %v timeout", + ResourceConfigurationTimeoutEnvVar, DefaultResourceConfigurationTimeout) + return DefaultResourceConfigurationTimeout + } + return timeout + } + return DefaultResourceConfigurationTimeout +} diff --git a/utils/parallel.go b/utils/parallel.go index a39ea7ef2c9..476de357488 100644 --- a/utils/parallel.go +++ b/utils/parallel.go @@ -124,7 +124,7 @@ func ParallelForEachPixel(size image.Point, f func(x, y int)) { // SimpleFunc is for RunInParallel. type SimpleFunc func(ctx context.Context) error -// RunInParallel runs all functions in parallel, return is elapsed time and n error. +// RunInParallel runs all functions in parallel, return is elapsed time and an error. func RunInParallel(ctx context.Context, fs []SimpleFunc) (time.Duration, error) { start := time.Now() ctx, cancel := context.WithCancel(ctx) @@ -164,3 +164,50 @@ func RunInParallel(ctx context.Context, fs []SimpleFunc) (time.Duration, error) wg.Wait() return time.Since(start), bigError } + +// FloatFunc is for GetInParallel. +type FloatFunc func(ctx context.Context) (float64, error) + +// GetInParallel runs all functions in parallel, return is elapsed time, a list of floats, and an error. +func GetInParallel(ctx context.Context, fs []FloatFunc) (time.Duration, []float64, error) { + start := time.Now() + ctx, cancel := context.WithCancel(ctx) + + var wg sync.WaitGroup + + var bigError error + var bigErrorMutex sync.Mutex + storeError := func(err error) { + bigErrorMutex.Lock() + defer bigErrorMutex.Unlock() + if bigError == nil || !errors.Is(err, context.Canceled) { + bigError = multierr.Combine(bigError, err) + } + } + + results := make([]float64, len(fs)) + + helper := func(f FloatFunc, i int) { + defer func() { + if thePanic := recover(); thePanic != nil { + storeError(fmt.Errorf("got panic getting something in parallel: %v", thePanic)) + cancel() + } + wg.Done() + }() + value, err := f(ctx) + if err != nil { + storeError(err) + cancel() + } + results[i] = value + } + + for i, f := range fs { + wg.Add(1) + go helper(f, i) + } + + wg.Wait() + return time.Since(start), results, bigError +} diff --git a/vision/object.go b/vision/object.go index 12b7b8fa3f8..a56bbc583ca 100644 --- a/vision/object.go +++ b/vision/object.go @@ -4,6 +4,8 @@ import ( "errors" "math" + commonpb "go.viam.com/api/common/v1" + pc "go.viam.com/rdk/pointcloud" "go.viam.com/rdk/spatialmath" ) @@ -17,19 +19,29 @@ type Object struct { // NewObject creates a new vision.Object from a point cloud with an empty label. func NewObject(cloud pc.PointCloud) (*Object, error) { - return NewObjectWithLabel(cloud, "") + return NewObjectWithLabel(cloud, "", nil) } // NewObjectWithLabel creates a new vision.Object from a point cloud with the given label. -func NewObjectWithLabel(cloud pc.PointCloud, label string) (*Object, error) { +func NewObjectWithLabel(cloud pc.PointCloud, label string, geometry *commonpb.Geometry) (*Object, error) { if cloud == nil { return NewEmptyObject(), nil } - box, err := pc.BoundingBoxFromPointCloudWithLabel(cloud, label) + if geometry == nil { + box, err := pc.BoundingBoxFromPointCloudWithLabel(cloud, label) + if err != nil { + return nil, err + } + return &Object{cloud, box}, nil + } + if label != "" { // will override geometry proto label with given label (unless empty) + geometry.Label = label + } + geom, err := spatialmath.NewGeometryFromProto(geometry) if err != nil { return nil, err } - return &Object{cloud, box}, nil + return &Object{PointCloud: cloud, Geometry: geom}, nil } // NewEmptyObject creates a new empty point cloud with metadata. diff --git a/vision/object_test.go b/vision/object_test.go index 6b6a0e75229..03e9a711244 100644 --- a/vision/object_test.go +++ b/vision/object_test.go @@ -36,6 +36,42 @@ func TestObjectCreation(t *testing.T) { test.That(t, obj.Geometry.AlmostEqual(expectedBox), test.ShouldBeTrue) } +func TestObjectCreationWithLabel(t *testing.T) { + // test that object created "withlabel" maintains label + + geomLabel := "blah" + providedLabel := "notBlah" + + // create point cloud + pc := pointcloud.New() + err := pc.Set(pointcloud.NewVector(0, 0, 200), nil) + test.That(t, err, test.ShouldBeNil) + + // create labelled and unlabelled Geometries + geom := spatialmath.NewPoint(r3.Vector{0, 0, 200}, geomLabel) + geom2 := spatialmath.NewPoint(r3.Vector{0, 0, 200}, "") + pbGeomWithLabel := geom.ToProtobuf() + pbGeomNoLabel := geom2.ToProtobuf() + + // Test that a providedLabel will overwrite the geometry label + obj, err := NewObjectWithLabel(pc, "", pbGeomWithLabel) + test.That(t, err, test.ShouldBeNil) + test.That(t, obj.Geometry.Label(), test.ShouldResemble, geomLabel) + + obj, err = NewObjectWithLabel(pc, providedLabel, pbGeomWithLabel) + test.That(t, err, test.ShouldBeNil) + test.That(t, obj.Geometry.Label(), test.ShouldResemble, providedLabel) + + // Test that with no geometry label, the providedLabel persists + obj2, err := NewObjectWithLabel(pc, "", pbGeomNoLabel) + test.That(t, err, test.ShouldBeNil) + test.That(t, obj2.Geometry.Label(), test.ShouldResemble, "") + + obj2, err = NewObjectWithLabel(pc, providedLabel, pbGeomNoLabel) + test.That(t, err, test.ShouldBeNil) + test.That(t, obj2.Geometry.Label(), test.ShouldResemble, providedLabel) +} + func TestObjectDistance(t *testing.T) { pc := pointcloud.New() err := pc.Set(pointcloud.NewVector(0, 0, 0), nil) diff --git a/vision/odometry/cmd/estimate_motion.go b/vision/odometry/cmd/estimate_motion.go deleted file mode 100644 index e74923798cf..00000000000 --- a/vision/odometry/cmd/estimate_motion.go +++ /dev/null @@ -1,137 +0,0 @@ -// Package main is a motion estimation via visual odometry tool. -package main - -import ( - "bytes" - "encoding/base64" - "html/template" - "image" - "image/jpeg" - "log" - "net/http" - "os" - - "github.com/edaniels/golog" - - "go.viam.com/rdk/rimage" - "go.viam.com/rdk/vision/keypoints" - "go.viam.com/rdk/vision/odometry" -) - -var ( - logger = golog.NewLogger("visual-odometry") - imageTemplate = ` - - -` -) - -func main() { - image1Path := os.Args[1] - image2Path := os.Args[2] - configPath := os.Args[3] - // get orb points for each image - imOrb1, imOrb2, err := RunOrbPointFinding(image1Path, image2Path, configPath) - if err != nil { - logger.Fatal(err.Error()) - } - // get matched lines - _, matchedLines, err := RunMotionEstimation(image1Path, image2Path, configPath) - if err != nil { - logger.Error(err.Error()) - } - http.HandleFunc("/orb/", func(w http.ResponseWriter, r *http.Request) { - writeImageWithTemplate(w, imOrb1, "img") - writeImageWithTemplate(w, imOrb2, "img") - writeImageWithTemplate(w, matchedLines, "img") - }) - http.Handle("/", http.FileServer(http.Dir("."))) - logger.Info("Listening on 8080...") - logger.Info("Images can be visualized at http://localhost:8080/orb/") - err = http.ListenAndServe(":8080", nil) //nolint:gosec - if err != nil { - log.Fatal("ListenAndServe:", err) - } -} - -// RunOrbPointFinding gets the orb points for each image. -func RunOrbPointFinding(image1Path, image2Path, configPath string) (image.Image, image.Image, error) { - // load images - img1, err := rimage.NewImageFromFile(image1Path) - if err != nil { - return nil, nil, err - } - img2, err := rimage.NewImageFromFile(image2Path) - if err != nil { - return nil, nil, err - } - im1 := rimage.MakeGray(rimage.ConvertImage(img1)) - im2 := rimage.MakeGray(rimage.ConvertImage(img2)) - // load cfg - cfg, err := odometry.LoadMotionEstimationConfig(configPath) - if err != nil { - return nil, nil, err - } - sampleMethod := cfg.KeyPointCfg.BRIEFConf.Sampling - sampleN := cfg.KeyPointCfg.BRIEFConf.N - samplePatchSize := cfg.KeyPointCfg.BRIEFConf.PatchSize - samplePoints := keypoints.GenerateSamplePairs(sampleMethod, sampleN, samplePatchSize) - _, kps1, err := keypoints.ComputeORBKeypoints(im1, samplePoints, cfg.KeyPointCfg) - if err != nil { - return nil, nil, err - } - _, kps2, err := keypoints.ComputeORBKeypoints(im2, samplePoints, cfg.KeyPointCfg) - if err != nil { - return nil, nil, err - } - orbPts1 := keypoints.PlotKeypoints(im1, kps1) - orbPts2 := keypoints.PlotKeypoints(im2, kps2) - return orbPts1, orbPts2, nil -} - -// RunMotionEstimation runs motion estimation between the two frames in artifacts. -func RunMotionEstimation(image1Path, image2Path, configPath string) (*odometry.Motion3D, image.Image, error) { - // load images - img1, err := rimage.NewImageFromFile(image1Path) - if err != nil { - return nil, nil, err - } - img2, err := rimage.NewImageFromFile(image2Path) - if err != nil { - return nil, nil, err - } - im1 := rimage.ConvertImage(img1) - im2 := rimage.ConvertImage(img2) - // load cfg - cfg, err := odometry.LoadMotionEstimationConfig(configPath) - if err != nil { - return nil, nil, err - } - // Estimate motion - motion, matchedLines, err := odometry.EstimateMotionFrom2Frames(im1, im2, cfg, logger) - if err != nil { - return nil, matchedLines, err - } - logger.Info(motion.Rotation) - logger.Info(motion.Translation) - - return motion, matchedLines, nil -} - -// writeImageWithTemplate encodes an image 'img' in jpeg format and writes it into ResponseWriter using a template. -func writeImageWithTemplate(w http.ResponseWriter, img image.Image, templ string) { - buffer := new(bytes.Buffer) - if err := jpeg.Encode(buffer, img, nil); err != nil { - log.Fatalln("unable to encode image.") - } - - str := base64.StdEncoding.EncodeToString(buffer.Bytes()) - if tmpl, err := template.New("image").Parse(imageTemplate); err != nil { - log.Println("unable to parse image template.") - } else { - data := map[string]interface{}{templ: str} - if err = tmpl.Execute(w, data); err != nil { - log.Println("unable to execute template.") - } - } -} diff --git a/vision/odometry/cmd/estimate_motion_test.go b/vision/odometry/cmd/estimate_motion_test.go deleted file mode 100644 index f480be7f877..00000000000 --- a/vision/odometry/cmd/estimate_motion_test.go +++ /dev/null @@ -1,23 +0,0 @@ -package main - -import ( - "testing" - - "go.viam.com/test" - "go.viam.com/utils/artifact" -) - -func TestRun(t *testing.T) { - // TODO(RSDK-586): Re-enable after testing new field changes during hack-week. - t.Skip() - - path1 := artifact.MustPath("vision/odometry/000001.png") - path2 := artifact.MustPath("vision/odometry/000002.png") - cfgPath := artifact.MustPath("vision/odometry/vo_config.json") - // motion - motion, _, err := RunMotionEstimation(path1, path2, cfgPath) - test.That(t, err, test.ShouldBeNil) - test.That(t, motion.Translation.At(0, 0), test.ShouldBeGreaterThan, 1.0) - test.That(t, motion.Translation.At(1, 0), test.ShouldBeGreaterThan, 0.0) - test.That(t, motion.Translation.At(2, 0), test.ShouldBeLessThan, -0.8) -} diff --git a/vision/odometry/motionestimation.go b/vision/odometry/motionestimation.go deleted file mode 100644 index 2ab4417c17d..00000000000 --- a/vision/odometry/motionestimation.go +++ /dev/null @@ -1,123 +0,0 @@ -package odometry - -import ( - "encoding/json" - "image" - "os" - - "github.com/edaniels/golog" - "github.com/golang/geo/r2" - "go.viam.com/utils" - "gonum.org/v1/gonum/mat" - - "go.viam.com/rdk/rimage" - "go.viam.com/rdk/rimage/transform" - "go.viam.com/rdk/vision/keypoints" -) - -// MotionEstimationConfig contains the parameters needed for motion estimation between two video frames. -type MotionEstimationConfig struct { - KeyPointCfg *keypoints.ORBConfig `json:"kps"` - MatchingCfg *keypoints.MatchingConfig `json:"matching"` - CamIntrinsics *transform.PinholeCameraIntrinsics `json:"intrinsic_parameters"` - ScaleEstimatorCfg *ScaleEstimatorConfig `json:"scale_estimator"` - CamHeightGround float64 `json:"cam_height_ground_m"` -} - -// LoadMotionEstimationConfig loads a motion estimation configuration from a json file. -func LoadMotionEstimationConfig(path string) (*MotionEstimationConfig, error) { - var config MotionEstimationConfig - configFile, err := os.Open(path) //nolint:gosec - defer utils.UncheckedErrorFunc(configFile.Close) - if err != nil { - return nil, err - } - jsonParser := json.NewDecoder(configFile) - err = jsonParser.Decode(&config) - if err != nil { - return nil, err - } - return &config, nil -} - -// Motion3D contains the estimated 3D rotation and translation from 2 frames. -type Motion3D struct { - Rotation *mat.Dense - Translation *mat.Dense -} - -// NewMotion3DFromRotationTranslation returns a new pointer to Motion3D from a rotation and a translation matrix. -func NewMotion3DFromRotationTranslation(rotation, translation *mat.Dense) *Motion3D { - return &Motion3D{ - Rotation: rotation, - Translation: translation, - } -} - -// EstimateMotionFrom2Frames estimates the 3D motion of the camera between frame img1 and frame img2. -func EstimateMotionFrom2Frames(img1, img2 *rimage.Image, cfg *MotionEstimationConfig, logger golog.Logger, -) (*Motion3D, image.Image, error) { - // Convert both images to gray - im1 := rimage.MakeGray(img1) - im2 := rimage.MakeGray(img2) - sampleMethod := cfg.KeyPointCfg.BRIEFConf.Sampling - sampleN := cfg.KeyPointCfg.BRIEFConf.N - samplePatchSize := cfg.KeyPointCfg.BRIEFConf.PatchSize - samplePoints := keypoints.GenerateSamplePairs(sampleMethod, sampleN, samplePatchSize) - // compute keypoints - orb1, kps1, err := keypoints.ComputeORBKeypoints(im1, samplePoints, cfg.KeyPointCfg) - if err != nil { - return nil, nil, err - } - orb2, kps2, err := keypoints.ComputeORBKeypoints(im2, samplePoints, cfg.KeyPointCfg) - if err != nil { - return nil, nil, err - } - // match descriptors - matches := keypoints.MatchDescriptors(orb1, orb2, cfg.MatchingCfg, logger) - // get 2 sets of matching keypoints - matchedKps1, matchedKps2, err := keypoints.GetMatchingKeyPoints(matches, kps1, kps2) - if err != nil { - return nil, nil, err - } - matchedOrbPts1 := keypoints.PlotKeypoints(im1, matchedKps1) - matchedOrbPts2 := keypoints.PlotKeypoints(im2, matchedKps2) - matchedLines := keypoints.PlotMatchedLines(matchedOrbPts1, matchedOrbPts2, matchedKps1, matchedKps2, true) - // get intrinsics matrix - k := cfg.CamIntrinsics.GetCameraMatrix() - - // Estimate camera pose - matchedKps1Float := convertImagePointSliceToFloatPointSlice(matchedKps1) - matchedKps2Float := convertImagePointSliceToFloatPointSlice(matchedKps2) - pose, err := transform.EstimateNewPose(matchedKps1Float, matchedKps2Float, k) - if err != nil { - return nil, matchedLines, err - } - - // Rescale motion - estimatedCamHeight, err := EstimateCameraHeight(matchedKps1Float, matchedKps2Float, pose, cfg.ScaleEstimatorCfg, cfg.CamIntrinsics) - if err != nil { - return nil, matchedLines, err - } - scale := cfg.CamHeightGround / estimatedCamHeight - - var rescaledTranslation mat.Dense - rescaledTranslation.Scale(scale, pose.Translation) - - return &Motion3D{ - Rotation: pose.Rotation, - Translation: &rescaledTranslation, - }, matchedLines, nil -} - -// convertImagePointSliceToFloatPointSlice is a helper to convert slice of image.Point to a slice of r2.Point. -func convertImagePointSliceToFloatPointSlice(pts []image.Point) []r2.Point { - ptsOut := make([]r2.Point, len(pts)) - for i, pt := range pts { - ptsOut[i] = r2.Point{ - X: float64(pt.X), - Y: float64(pt.Y), - } - } - return ptsOut -} diff --git a/vision/odometry/motionestimation_test.go b/vision/odometry/motionestimation_test.go deleted file mode 100644 index 38e2c09fa10..00000000000 --- a/vision/odometry/motionestimation_test.go +++ /dev/null @@ -1,49 +0,0 @@ -package odometry - -import ( - "testing" - - "github.com/edaniels/golog" - "go.viam.com/test" - "go.viam.com/utils/artifact" - "gonum.org/v1/gonum/mat" - - "go.viam.com/rdk/rimage" -) - -func TestNewMotion3DFromRotationTranslation(t *testing.T) { - // rotation = Id - rot := mat.NewDense(3, 3, nil) - rot.Set(0, 0, 1) - rot.Set(1, 1, 1) - rot.Set(2, 2, 1) - - // Translation = 1m in z direction - tr := mat.NewDense(3, 1, []float64{0, 0, 1}) - - motion := NewMotion3DFromRotationTranslation(rot, tr) - test.That(t, motion, test.ShouldNotBeNil) - test.That(t, motion.Rotation, test.ShouldResemble, rot) - test.That(t, motion.Translation, test.ShouldResemble, tr) -} - -func TestEstimateMotionFrom2Frames(t *testing.T) { - // TODO(RSDK-586): Re-enable after testing new field changes during hack-week. - t.Skip() - - logger := golog.NewTestLogger(t) - // load cfg - cfg, err := LoadMotionEstimationConfig(artifact.MustPath("vision/odometry/vo_config.json")) - test.That(t, err, test.ShouldBeNil) - // load images - im1, err := rimage.NewImageFromFile(artifact.MustPath("vision/odometry/000001.png")) - test.That(t, err, test.ShouldBeNil) - im2, err := rimage.NewImageFromFile(artifact.MustPath("vision/odometry/000002.png")) - test.That(t, err, test.ShouldBeNil) - // Estimate motion - motion, matchedLines, err := EstimateMotionFrom2Frames(im1, im2, cfg, logger) - test.That(t, err, test.ShouldBeNil) - test.That(t, matchedLines, test.ShouldNotBeNil) - test.That(t, motion.Translation.At(2, 0), test.ShouldBeLessThan, -0.8) - test.That(t, motion.Translation.At(1, 0), test.ShouldBeLessThan, 0.2) -} diff --git a/vision/odometry/scale_estimator.go b/vision/odometry/scale_estimator.go deleted file mode 100644 index a415b9d6ddc..00000000000 --- a/vision/odometry/scale_estimator.go +++ /dev/null @@ -1,202 +0,0 @@ -// Package odometry implements functions for visual odometry -package odometry - -import ( - "errors" - "math" - - "github.com/golang/geo/r2" - "github.com/golang/geo/r3" - "gonum.org/v1/gonum/mat" - - "go.viam.com/rdk/rimage/transform" - "go.viam.com/rdk/vision/delaunay" -) - -// ScaleEstimatorConfig contains the parameters that are necessary for scale estimation. -type ScaleEstimatorConfig struct { - ThresholdNormalAngle float64 `json:"th_normal_angle_rads"` - ThresholdPlaneInlier float64 `json:"th_plane_inlier"` -} - -// estimatePitchFromCameraPose gets a rough estimation of the camera pitch (angle of camera axis with ground plane -// in radians). -func estimatePitchFromCameraPose(pose *transform.CamPose) float64 { - pitch := math.Asin(pose.Translation.At(1, 0)) - return pitch -} - -// estimatePlaneFrom3Points estimate a plane equation from 3 points. -func estimatePlaneFrom3Points(p0, p1, p2 r3.Vector) (r3.Vector, float64) { - o1 := p1.Sub(p0) - o2 := p2.Sub(p0) - normal := o1.Cross(o2) - offset := -normal.Dot(p0) - return normal, offset -} - -// distToPlane returns the distance of a point to a plane. -func distToPlane(pt, normal r3.Vector, offset float64) float64 { - dot := pt.Dot(normal) - num := math.Abs(dot + offset) - denom := normal.Norm() - return num / denom -} - -// getPlaneInliers returns the indices of 3D points in pts3d that are distant from at most threshold to plane. -func getPlaneInliers(pts3d []r3.Vector, normal r3.Vector, offset, threshold float64) []int { - inliers := make([]int, 0, len(pts3d)) - for i, pt := range pts3d { - dist := distToPlane(pt, normal, offset) - if dist < threshold { - inliers = append(inliers, i) - } - } - return inliers -} - -func getCameraHeightFromGroundPoint(pt r3.Vector, pitch float64) float64 { - return pt.Y*math.Cos(pitch) - pt.Z*math.Sin(pitch) -} - -// getAverageHeightGroundPoints returns the average height of 3d ground points wrt to the gr. -// pitch angle should be expressed in radians. -func getAverageHeightGroundPoints(groundPoints []r3.Vector, pitch float64) float64 { - if len(groundPoints) < 1 { - panic("no ground points to get height from") - } - height := 0. - for _, pt := range groundPoints { - height += getCameraHeightFromGroundPoint(pt, pitch) - // height += pt.Y - } - return height / float64(len(groundPoints)) -} - -// remap3dFeatures remaps the y and z coordinates so that the y coordinate is the up-down coordinate and the -// z coordinate is the in-out coordinate, given a 3D feature vector. -func remap3dFeatures(f3d []r3.Vector, pitch float64) []r3.Vector { - remappedF3d := make([]r3.Vector, len(f3d)) - for i, pt := range f3d { - y := pt.Y*math.Cos(pitch) - pt.Z*math.Sin(pitch) - z := pt.Y*math.Sin(pitch) + pt.Z*math.Cos(pitch) - remappedF3d[i] = r3.Vector{pt.X, y, z} - } - return remappedF3d -} - -// getSelected3DFeatures returns the 3D features whose ids are selected. -func getSelected3DFeatures(f3d []r3.Vector, ids []int) []r3.Vector { - f3dSelected := make([]r3.Vector, 0, len(f3d)) - for _, id := range ids { - f3dSelected = append(f3dSelected, f3d[id]) - } - return f3dSelected -} - -// GetTriangulated3DPointsFrom2DKeypoints gets the triangulated 3D point cloud from the matched 2D keypoints, the -// second camera pose and the intrinsic camera matrix. -func GetTriangulated3DPointsFrom2DKeypoints(pts1, pts2 []r2.Point, pose *transform.CamPose, - intrinsics *transform.PinholeCameraIntrinsics, -) ([]r3.Vector, error) { - intrinsicK := intrinsics.GetCameraMatrix() - // homogenize 2d keypoints in image coordinates - pts1H := transform.Convert2DPointsToHomogeneousPoints(pts1) - pts2H := transform.Convert2DPointsToHomogeneousPoints(pts2) - // Create projection matrix: intrinsicK@Pose - projectionMatrix := mat.NewDense(3, 4, nil) - projectionMatrix.Mul(intrinsicK, pose.PoseMat) - // get triangulated 3d points in camera1 reference through projection matrix - pts3d, err := transform.GetLinearTriangulatedPoints(projectionMatrix, pts1H, pts2H) - if err != nil { - return nil, err - } - return pts3d, nil -} - -// getGroundInlierPoints takes a list of 3D points, a list of 3D triangles, and two thresholds, and returns the indices -// of the points that are inliers of the ground plane. -func getGroundInlierPoints(p3d []r3.Vector, triangles3D [][]r3.Vector, thresholdNormalAngle, - thresholdPlaneInlier float64, -) ([]int, error) { - inliersGround := make([]int, 0, len(p3d)) - maxInliers := 0 - groundFound := false - for _, triangle := range triangles3D { - normal, offset := estimatePlaneFrom3Points(triangle[0], triangle[1], triangle[2]) - angularDiff := math.Abs(normal.Dot(r3.Vector{0, 1, 0})) / normal.Norm() - // if current normal vector is almost collinear with Y unit vector - if angularDiff > thresholdNormalAngle { - inliers := getPlaneInliers(p3d, normal, offset, thresholdPlaneInlier) - if len(inliers) > maxInliers { - maxInliers = len(inliers) - inliersGround = make([]int, len(inliers)) - copy(inliersGround, inliers) - groundFound = true - } - } - } - if groundFound { - return inliersGround, nil - } - err := errors.New("ground plane not found") - return nil, err -} - -// GetPointsOnGroundPlane gets the ids of matched keypoints that belong to the ground plane. -func GetPointsOnGroundPlane(pts1, pts2 []r2.Point, pose *transform.CamPose, - thresholdNormalAngle, thresholdPlaneInlier float64, - intrinsics *transform.PinholeCameraIntrinsics, -) ([]int, []r3.Vector, error) { - // get 3D points - f3d, err := GetTriangulated3DPointsFrom2DKeypoints(pts1, pts2, pose, intrinsics) - if err != nil { - return nil, nil, err - } - // get camera pitch - pitch := estimatePitchFromCameraPose(pose) - // remap 3d features - p3d := remap3dFeatures(f3d, pitch) - // get 2d Delaunay triangulation; 2D Delaunay triangulation can be obtained from either pts1 or pts2, as there is a - // bijection between the 2 sets - pts2dDelaunay := make([]delaunay.Point, len(pts1)) - for i, pt := range pts1 { - pts2dDelaunay[i] = delaunay.Point{pt.X, pt.Y} - } - tri, err := delaunay.Triangulate(pts2dDelaunay) - if err != nil { - return nil, nil, err - } - triangleMap := tri.GetTrianglesPointsMap() - // get 3D triangles - triangles3D := make([][]r3.Vector, len(triangleMap)) - for k, triangle := range triangleMap { - p0 := p3d[triangle[0]] - p1 := p3d[triangle[1]] - p2 := p3d[triangle[2]] - triangles3D[k] = []r3.Vector{p0, p1, p2} - } - // get plane equation for every 3D triangle and get the one which normal is quasi collinear with (0, -1, 0) and - // with most inliers - inliersGround, err := getGroundInlierPoints(p3d, triangles3D, thresholdNormalAngle, thresholdPlaneInlier) - if err != nil { - return nil, nil, err - } - // if found ground plane, get ground plane 3d points in original reference - pointsGround := getSelected3DFeatures(p3d, inliersGround) - return inliersGround, pointsGround, nil -} - -// EstimateCameraHeight estimates the camera height wrt to ground plane. -func EstimateCameraHeight(pts1, pts2 []r2.Point, pose *transform.CamPose, - cfg *ScaleEstimatorConfig, intrinsics *transform.PinholeCameraIntrinsics, -) (float64, error) { - _, pointsGround, err := GetPointsOnGroundPlane(pts1, pts2, pose, cfg.ThresholdNormalAngle, cfg.ThresholdPlaneInlier, intrinsics) - if err != nil { - return 0, err - } - // get average height of camera from the points in estimated ground plane - pitch := estimatePitchFromCameraPose(pose) - height := getAverageHeightGroundPoints(pointsGround, pitch) - return height, nil -} diff --git a/vision/odometry/scale_estimator_test.go b/vision/odometry/scale_estimator_test.go deleted file mode 100644 index d1cec31e7e7..00000000000 --- a/vision/odometry/scale_estimator_test.go +++ /dev/null @@ -1,158 +0,0 @@ -package odometry - -import ( - "encoding/json" - "fmt" - "io" - "math" - "math/rand" - "os" - "testing" - - "github.com/edaniels/golog" - "github.com/golang/geo/r2" - "github.com/golang/geo/r3" - "go.viam.com/test" - "go.viam.com/utils/artifact" - "gonum.org/v1/gonum/mat" - - "go.viam.com/rdk/rimage/transform" -) - -func generatePointZEqualsZeroPlane(n int) []r3.Vector { - points := make([]r3.Vector, n) - for i := 0; i < n; i++ { - x := rand.Float64() - y := rand.Float64() - points[i] = r3.Vector{x, y, 0} - } - return points -} - -type poseGroundTruth struct { - Pts1 [][]float64 `json:"pts1"` - Pts2 [][]float64 `json:"pts2"` - R [][]float64 `json:"rot"` - T [][]float64 `json:"translation"` - K [][]float64 `json:"cam_mat"` - F [][]float64 `json:"fundamental_matrix"` -} - -func convert2DSliceToVectorSlice(points [][]float64) []r2.Point { - vecs := make([]r2.Point, len(points)) - for i, pt := range points { - vecs[i] = r2.Point{ - X: pt[0], - Y: pt[1], - } - } - return vecs -} - -func convert2DSliceToDense(data [][]float64) *mat.Dense { - m := len(data) - n := len(data[0]) - out := mat.NewDense(m, n, nil) - for i, row := range data { - out.SetRow(i, row) - } - return out -} - -func readJSONGroundTruth(logger golog.Logger) *poseGroundTruth { - // Open jsonFile - jsonFile, err := os.Open(artifact.MustPath("rimage/matched_kps.json")) - if err != nil { - return nil - } - logger.Info("Ground Truth json file successfully loaded") - defer jsonFile.Close() - // read our opened jsonFile as a byte array. - byteValue, _ := io.ReadAll(jsonFile) - - // initialize poseGroundTruth - var gt poseGroundTruth - - // unmarshal byteArray - json.Unmarshal(byteValue, >) - return > -} - -func TestPlaneFrom3PointsDistance(t *testing.T) { - points := []r3.Vector{{0, 0, 0}, {0, 1, 0}, {1, 0, 0}} - normal, offset := estimatePlaneFrom3Points(points[0], points[1], points[2]) - test.That(t, normal.X, test.ShouldEqual, 0) - test.That(t, normal.Y, test.ShouldEqual, 0) - test.That(t, normal.Z, test.ShouldEqual, -1) - test.That(t, offset, test.ShouldAlmostEqual, 0) - pt := r3.Vector{1, 1, 1} - dist := distToPlane(pt, normal, offset) - test.That(t, dist, test.ShouldEqual, 1) -} - -func TestGetPlaneInliers(t *testing.T) { - points := generatePointZEqualsZeroPlane(1000) - normal := r3.Vector{0, 0, 1} - offset := 0.0 - inliers := getPlaneInliers(points, normal, offset, 0.0001) - test.That(t, len(inliers), test.ShouldEqual, 1000) - // test parallel plane with distance > threshold - offset = 3.0 - inliersOffset3 := getPlaneInliers(points, normal, offset, 0.0001) - test.That(t, len(inliersOffset3), test.ShouldEqual, 0) - // test parallel plane with distance < threshold - offset = 0.1 - inliersSmallOffset := getPlaneInliers(points, normal, offset, 0.25) - test.That(t, len(inliersSmallOffset), test.ShouldEqual, 1000) -} - -func TestEstimatePitch(t *testing.T) { - // get pose from kitti odometry dataset - poseData := []float64{ - 9.999996e-01, -9.035185e-04, -2.101169e-04, 1.289128e-03, - 9.037964e-04, 9.999987e-01, 1.325646e-03, -1.821616e-02, - 2.089193e-04, -1.325834e-03, 9.999991e-01, 1.310643e+00, - } - // get our camera pose structure - poseMat := mat.NewDense(3, 4, poseData) - pose := transform.NewCamPoseFromMat(poseMat) - // estimate pitch - pitch := estimatePitchFromCameraPose(pose) - pitchDegrees := pitch * 180 / math.Pi - // test pitch value is similar to the KITTI GT - test.That(t, pitchDegrees, test.ShouldAlmostEqual, -1.0437668176234958) - - // test camera height - with small pitch, height should be close to Y coordinate (<3-5 cm) - pt := r3.Vector{10, 1.73, 1.5} - height := getCameraHeightFromGroundPoint(pt, pitch) - test.That(t, math.Abs(height-pt.Y), test.ShouldBeLessThan, 0.03) -} - -func TestEstimateCameraHeight(t *testing.T) { - logger := golog.NewTestLogger(t) - gt := readJSONGroundTruth(logger) - pts1 := convert2DSliceToVectorSlice(gt.Pts1) - pts2 := convert2DSliceToVectorSlice(gt.Pts2) - K := convert2DSliceToDense(gt.K) - T := convert2DSliceToDense(gt.T) - R := convert2DSliceToDense(gt.R) - intrinsics := transform.PinholeCameraIntrinsics{ - Fx: K.At(0, 0), - Fy: K.At(1, 1), - Ppx: K.At(0, 2), - Ppy: K.At(1, 2), - } - poseMat := mat.NewDense(3, 4, nil) - poseMat.Augment(R, T) - pose := transform.NewCamPoseFromMat(poseMat) - cfg := &ScaleEstimatorConfig{ - ThresholdNormalAngle: 0.97, - ThresholdPlaneInlier: 0.005, - } - height, err := EstimateCameraHeight(pts1, pts2, pose, cfg, &intrinsics) - test.That(t, err, test.ShouldBeNil) - heightMessage := fmt.Sprintf("Estimated Height: %f", height) - logger.Info(heightMessage) - test.That(t, height, test.ShouldBeLessThan, 0) - test.That(t, height, test.ShouldAlmostEqual, -0.722854, 0.0001) -} diff --git a/vision/segmentation/detections_to_objects.go b/vision/segmentation/detections_to_objects.go index 70a9341a931..fd71eac1f47 100644 --- a/vision/segmentation/detections_to_objects.go +++ b/vision/segmentation/detections_to_objects.go @@ -89,7 +89,7 @@ func DetectionSegmenter(detector objectdetection.Detector, meanK int, sigma, con if pc.Size() == 0 { continue } - obj, err := vision.NewObjectWithLabel(pc, d.Label()) + obj, err := vision.NewObjectWithLabel(pc, d.Label(), nil) if err != nil { return nil, err } diff --git a/vision/segmentation/er_ccl_clustering.go b/vision/segmentation/er_ccl_clustering.go new file mode 100644 index 00000000000..004994cf7be --- /dev/null +++ b/vision/segmentation/er_ccl_clustering.go @@ -0,0 +1,319 @@ +package segmentation + +import ( + "context" + "math" + + "github.com/golang/geo/r3" + "github.com/mitchellh/mapstructure" + "github.com/pkg/errors" + + "go.viam.com/rdk/components/camera" + pc "go.viam.com/rdk/pointcloud" + "go.viam.com/rdk/resource" + "go.viam.com/rdk/utils" + "go.viam.com/rdk/vision" +) + +// MaxCCLIterations is a value to stop the CCL algo from going on for too long. +const MaxCCLIterations = 300000 + +// ErCCLConfig specifies the necessary parameters to apply the +// connected components based clustering algo. +type ErCCLConfig struct { + resource.TriviallyValidateConfig + MinPtsInPlane int `json:"min_points_in_plane"` + MinPtsInSegment int `json:"min_points_in_segment"` + MaxDistFromPlane float64 `json:"max_dist_from_plane_mm"` + NormalVec r3.Vector `json:"ground_plane_normal_vec"` + AngleTolerance float64 `json:"ground_angle_tolerance_degs"` + ClusteringRadius int `json:"clustering_radius"` + ClusteringStrictness float64 `json:"clustering_strictness"` +} + +type node struct { + i, j int + label int + minHeight, maxHeight float64 + // could be implemented without i,j + // label -1 means no cluster, otherwise labeled according to index +} + +// CheckValid checks to see in the input values are valid. +func (erCCL *ErCCLConfig) CheckValid() error { + // min_points_in_plane + if erCCL.MinPtsInPlane == 0 { + erCCL.MinPtsInPlane = 500 + } + if erCCL.MinPtsInPlane <= 0 { + return errors.Errorf("min_points_in_plane must be greater than 0, got %v", erCCL.MinPtsInPlane) + } + // min_points_in_segment + if erCCL.MinPtsInSegment < 0 { + return errors.Errorf("min_points_in_segment must be greater than or equal to 0, got %v", erCCL.MinPtsInSegment) + } + // max_dist_from_plane_mm + if erCCL.MaxDistFromPlane == 0 { + erCCL.MaxDistFromPlane = 100 + } + if erCCL.MaxDistFromPlane <= 0 { + return errors.Errorf("max_dist_from_plane must be greater than 0, got %v", erCCL.MaxDistFromPlane) + } + // ground_plane_normal_vec + // going to have to add that the ground plane's normal vec has to be {0, 1, 0} or {0, 0, 1} + if !erCCL.NormalVec.IsUnit() { + return errors.Errorf("ground_plane_normal_vec should be a unit vector, got %v", erCCL.NormalVec) + } + if erCCL.NormalVec.Norm2() == 0 { + erCCL.NormalVec = r3.Vector{X: 0, Y: 0, Z: 1} + } + // ground_angle_tolerance_degs + if erCCL.AngleTolerance == 0.0 { + erCCL.AngleTolerance = 30.0 + } + if erCCL.AngleTolerance > 180 || erCCL.AngleTolerance < 0 { + return errors.Errorf("max_angle_of_plane must between 0 & 180 (inclusive), got %v", erCCL.AngleTolerance) + } + // clustering_radius + if erCCL.ClusteringRadius == 0 { + erCCL.ClusteringRadius = 1 + } + if erCCL.ClusteringRadius < 0 { + return errors.Errorf("radius must be greater than 0, got %v", erCCL.ClusteringRadius) + } + // clustering_strictness + if erCCL.ClusteringStrictness == 0 { + erCCL.ClusteringStrictness = 5 + } + if erCCL.ClusteringStrictness < 0 { + return errors.Errorf("clustering_strictness must be greater than 0, got %v", erCCL.ClusteringStrictness) + } + return nil +} + +// ConvertAttributes changes the AttributeMap input into an ErCCLConfig. +func (erCCL *ErCCLConfig) ConvertAttributes(am utils.AttributeMap) error { + decoder, err := mapstructure.NewDecoder(&mapstructure.DecoderConfig{TagName: "json", Result: erCCL}) + if err != nil { + return err + } + err = decoder.Decode(am) + if err == nil { + err = erCCL.CheckValid() + } + return err +} + +// NewERCCLClustering returns a Segmenter that removes the ground plane and returns a segmentation +// of the objects in a point cloud using a connected components clustering algo described in the paper +// "A Fast Spatial Clustering Method for Sparse LiDAR Point Clouds Using GPU Programming" by Tian et al. 2020. +func NewERCCLClustering(params utils.AttributeMap) (Segmenter, error) { + // convert attributes to appropriate struct + if params == nil { + return nil, errors.New("config for ER-CCL segmentation cannot be nil") + } + cfg := &ErCCLConfig{} + err := cfg.ConvertAttributes(params) + if err != nil { + return nil, err + } + return cfg.ErCCLAlgorithm, nil +} + +// ErCCLAlgorithm applies the connected components clustering algorithm directly on a given point cloud. +func (erCCL *ErCCLConfig) ErCCLAlgorithm(ctx context.Context, src camera.VideoSource) ([]*vision.Object, error) { + // get next point cloud + cloud, err := src.NextPointCloud(ctx) + if err != nil { + return nil, err + } + // run ransac, get pointcloud without ground plane + ps := NewPointCloudGroundPlaneSegmentation(cloud, erCCL.MaxDistFromPlane, erCCL.MinPtsInPlane, erCCL.AngleTolerance, erCCL.NormalVec) + // if there are found planes, remove them, and keep all the non-plane points + _, nonPlane, err := ps.FindGroundPlane(ctx) + if err != nil { + return nil, err + } + + // need to figure out coordinate system + // if height is not y, then height is going to be z + heightIsY := erCCL.NormalVec.Y != 0 + + // calculating s value, want 200 x 200 graph + resolution := math.Ceil((nonPlane.MetaData().MaxX - nonPlane.MetaData().MinX) / 200) + if heightIsY { + resolution = math.Ceil((math.Ceil((nonPlane.MetaData().MaxZ-nonPlane.MetaData().MinZ)/200) + resolution) / 2) + } else { + resolution = math.Ceil((math.Ceil((nonPlane.MetaData().MaxY-nonPlane.MetaData().MinY)/200) + resolution) / 2) + } + + // create obstacle flag map, return that 2d slice of nodes + labelMap := pcProjection(nonPlane, resolution, heightIsY) + + // actually run erCCLL + // iterate through every box, searching down and right r distance + // run calculations to meet similarity threshold + // if similar enough update to initial label value (will also be smallest) + // iterate through pointcloud + + err = LabelMapUpdate(labelMap, erCCL.ClusteringRadius, 0.9, erCCL.ClusteringStrictness, resolution) + if err != nil { + return nil, err + } + + // look up label value of point by looking at 2d array and seeing what label inside that struct + // set this label + var iterateErr error + segments := NewSegments() + nonPlane.Iterate(0, 0, func(p r3.Vector, d pc.Data) bool { + i := int(math.Ceil((p.X - nonPlane.MetaData().MinX) / resolution)) + j := int(math.Ceil((p.Z - nonPlane.MetaData().MinZ) / resolution)) + if !heightIsY { + j = int(math.Ceil((p.Y - nonPlane.MetaData().MinY) / resolution)) + } + err := segments.AssignCluster(p, d, labelMap[i][j].label) + if err != nil { + iterateErr = err + return false + } + return true + }) + if iterateErr != nil { + return nil, iterateErr + } + // prune smaller clusters. Default minimum number of points determined by size of original point cloud. + minPtsInSegment := int(math.Max(float64(nonPlane.Size())/200.0, 10.0)) + if erCCL.MinPtsInSegment != 0 { + minPtsInSegment = erCCL.MinPtsInSegment + } + validClouds := pc.PrunePointClouds(segments.PointClouds(), minPtsInSegment) + // wrap + objects, err := NewSegmentsFromSlice(validClouds, "") + if err != nil { + return nil, err + } + return objects.Objects, nil + // this seems a bit wasteful to make segments then make more segments after filtering, but rolling with it for now + // TODO: RSDK-4613 +} + +// LabelMapUpdate updates the label map until it converges or errors. +func LabelMapUpdate(labelMap [][]node, r int, alpha, beta, s float64) error { + i := 0 + continueRunning := true + for continueRunning { + // 0.9 is alpha + continueRunning := minimumSearch(labelMap, r, 0.9, beta, s) + if !continueRunning { + break + } + + if i > MaxCCLIterations { // arbitrary cutoff for iterations + return errors.New("could not converge, change parameters") + } + i++ + } + return nil +} + +func pcProjection(cloud pc.PointCloud, s float64, heightIsY bool) [][]node { + h := int(math.Ceil((cloud.MetaData().MaxX-cloud.MetaData().MinX)/s)) + 1 + w := int(math.Ceil((cloud.MetaData().MaxZ-cloud.MetaData().MinZ)/s)) + 1 + if !heightIsY { + w = int(math.Ceil((cloud.MetaData().MaxY-cloud.MetaData().MinY)/s)) + 1 + } + retVal := make([][]node, h) + for i := range retVal { + retVal[i] = make([]node, w) + for j, curNode := range retVal[i] { + curNode.label = -1 + curNode.minHeight = 0 + curNode.maxHeight = 0 + curNode.i = i + curNode.j = j + retVal[i][j] = curNode + } + } + cloud.Iterate(0, 0, func(p r3.Vector, d pc.Data) bool { + i := int(math.Ceil((p.X - cloud.MetaData().MinX) / s)) + j := int(math.Ceil((p.Z - cloud.MetaData().MinZ) / s)) + var curNode node + if heightIsY { + curNode = retVal[i][j] + curNode.maxHeight = math.Max(curNode.maxHeight, p.Y) + curNode.minHeight = math.Min(curNode.minHeight, p.Y) + } else { + j = int(math.Ceil((p.Y - cloud.MetaData().MinY) / s)) + curNode = retVal[i][j] + curNode.maxHeight = math.Max(curNode.maxHeight, p.Z) + curNode.minHeight = math.Min(curNode.minHeight, p.Z) + } + curNode.label = i*w + j + retVal[i][j] = curNode + return true + }) + return retVal +} + +// minimumSearch updates the label map 'once' meaning it searches from every cell once. +func minimumSearch(labelMap [][]node, r int, alpha, beta, s float64) bool { + mapChanged := false + + for i, curNodeSlice := range labelMap { + for j, curNode := range curNodeSlice { + if curNode.label == -1 { + // skip if no points at cell + continue + } + minLabel := curNode.label + neighbors := make([]node, 0) + // finding neighbors + finding min label value + for x := 0; x < r; x++ { + newI := i + x + if newI >= len(labelMap) { + break + } + for y := 0; y < r; y++ { + newJ := j + y + if newJ >= len(curNodeSlice) { + break + } + if newI == i && newJ == j { + continue // might be able to remove this because original should be in neighbors list + } + neighborNode := labelMap[newI][newJ] + if similarEnough(curNode, neighborNode, r, alpha, beta, s) { + neighbors = append(neighbors, neighborNode) + minLabel = int(math.Min(float64(minLabel), float64(neighborNode.label))) + } + } + } + if minLabel != curNode.label { + mapChanged = true + labelMap[curNode.i][curNode.j].label = minLabel + } + for _, neighbor := range neighbors { + if neighbor.label != minLabel { + mapChanged = true + labelMap[neighbor.i][neighbor.j].label = minLabel + } + } + } + } + return mapChanged +} + +// similarEnough takes in two nodes and tries to see if they meet some similarity threshold +// there are three components, first calculate distance between nodes, then height difference between points +// use these values to then calculate a score for similarity and if it exceeds a threshold calculated from the +// search radius and clustering strictness value. +func similarEnough(curNode, neighbor node, r int, alpha, beta, s float64) bool { + // trying to avoid math.pow since these are ints and math.pow is slow + if neighbor.label == -1 { + return false + } + d := s * math.Sqrt(float64(((curNode.i-neighbor.i)*(curNode.i-neighbor.i) + (curNode.j-neighbor.j)*(curNode.j-neighbor.j)))) + h := math.Abs(curNode.maxHeight-neighbor.maxHeight) + math.Abs(curNode.minHeight-neighbor.minHeight) + ecc := alpha*math.Exp(-d) + (1-alpha)*math.Exp(-h) + return ecc >= beta*math.Exp(float64(-r)) +} diff --git a/vision/segmentation/er_ccl_clustering_test.go b/vision/segmentation/er_ccl_clustering_test.go new file mode 100644 index 00000000000..87cae5264d9 --- /dev/null +++ b/vision/segmentation/er_ccl_clustering_test.go @@ -0,0 +1,90 @@ +package segmentation_test + +import ( + "context" + "testing" + + "github.com/edaniels/golog" + "github.com/golang/geo/r3" + "go.viam.com/test" + "go.viam.com/utils/artifact" + + pc "go.viam.com/rdk/pointcloud" + "go.viam.com/rdk/testutils/inject" + "go.viam.com/rdk/utils" + "go.viam.com/rdk/vision" + "go.viam.com/rdk/vision/segmentation" +) + +func TestERCCL(t *testing.T) { + t.Parallel() + logger := golog.NewTestLogger(t) + injectCamera := &inject.Camera{} + injectCamera.NextPointCloudFunc = func(ctx context.Context) (pc.PointCloud, error) { + return pc.NewFromLASFile(artifact.MustPath("pointcloud/test.las"), logger) + } + + objConfig := utils.AttributeMap{ + // lidar config + // "min_points_in_plane": 1500, + // "max_dist_from_plane_mm": 100.0, + // "min_points_in_segment": 150, + // "ground_angle_tolerance_degs": 20, + // "ground_plane_normal_vec": r3.Vector{0, 0, 1}, + // "clustering_radius": 5, + // "clustering_granularity": 2, + + // realsense config + "min_points_in_plane": 1500, + "max_dist_from_plane_mm": 10.0, + "min_points_in_segment": 250, + "ground_angle_tolerance_degs": 20, + "ground_plane_normal_vec": r3.Vector{0, -1, 0}, + "clustering_radius": 5, + "clustering_granularity": 3, + } + + segmenter, err := segmentation.NewERCCLClustering(objConfig) + test.That(t, err, test.ShouldBeNil) + _, err = segmenter(context.Background(), injectCamera) + test.That(t, err, test.ShouldBeNil) +} + +func BenchmarkERCCL(b *testing.B) { + injectCamera := &inject.Camera{} + injectCamera.NextPointCloudFunc = func(ctx context.Context) (pc.PointCloud, error) { + return pc.NewFromLASFile(artifact.MustPath("pointcloud/test.las"), nil) + } + var pts []*vision.Object + var err error + // do segmentation + objConfig := utils.AttributeMap{ + // lidar config + // "min_points_in_plane": 1500, + // "max_dist_from_plane_mm": 100.0, + // "min_points_in_segment": 150, + // "ground_angle_tolerance_degs": 20, + // "ground_plane_normal_vec": r3.Vector{0, 0, 1}, + // "clustering_radius": 5, + // "clustering_strictness": 2, + + // realsense config + "min_points_in_plane": 1500, + "max_dist_from_plane_mm": 10.0, + "min_points_in_segment": 250, + "ground_angle_tolerance_degs": 20, + "ground_plane_normal_vec": r3.Vector{0, -1, 0}, + "clustering_radius": 5, + "clustering_strictness": 3, + } + + segmenter, err := segmentation.NewERCCLClustering(objConfig) + + for i := 0; i < b.N; i++ { + pts, err = segmenter(context.Background(), injectCamera) + } + // to prevent vars from being optimized away + if pts == nil || err != nil { + panic("segmenter didn't work") + } +} diff --git a/vision/segmentation/plane_segmentation.go b/vision/segmentation/plane_segmentation.go index c9acd053150..d63b6f07572 100644 --- a/vision/segmentation/plane_segmentation.go +++ b/vision/segmentation/plane_segmentation.go @@ -66,6 +66,59 @@ func pointCloudSplit(cloud pc.PointCloud, inMap map[r3.Vector]bool) (pc.PointClo return mapCloud, nonMapCloud, nil } +// SegmentPlaneWRTGround segments the biggest 'ground' plane in the 3D Pointcloud. +// nIterations is the number of iteration for ransac +// nIter to choose? nIter = log(1-p)/log(1-(1-e)^s), where p is prob of success, e is outlier ratio, s is subset size (3 for plane). +// dstThreshold is the float64 value for the maximum allowed distance to the found plane for a point to belong to it +// This function returns a Plane struct, as well as the remaining points in a pointcloud +// It also returns the equation of the found plane: [0]x + [1]y + [2]z + [3] = 0. +// angleThrehold is the maximum acceptable angle between the groundVec and angle of the plane, +// if the plane is at a larger angle than maxAngle, it will not be considered for segmentation, if set to 0 then not considered +// normalVec is the normal vector of the plane representing the ground. +func SegmentPlaneWRTGround(ctx context.Context, cloud pc.PointCloud, nIterations int, angleThreshold, + dstThreshold float64, normalVec r3.Vector, +) (pc.Plane, pc.PointCloud, error) { + if cloud.Size() <= 3 { // if point cloud does not have even 3 points, return original cloud with no planes + return pc.NewEmptyPlane(), cloud, nil + } + //nolint:gosec + r := rand.New(rand.NewSource(1)) + pts := GetPointCloudPositions(cloud) + nPoints := cloud.Size() + + // First get all equations + equations := make([][4]float64, 0, nIterations) + for i := 0; i < nIterations; i++ { + // sample 3 Points from the slice of 3D Points + n1, n2, n3 := utils.SampleRandomIntRange(1, nPoints-1, r), + utils.SampleRandomIntRange(1, nPoints-1, r), + utils.SampleRandomIntRange(1, nPoints-1, r) + p1, p2, p3 := pts[n1], pts[n2], pts[n3] + + // get 2 vectors that are going to define the plane + v1 := p2.Sub(p1) + v2 := p3.Sub(p1) + // cross product to get the normal unit vector to the plane (v1, v2) + cross := v1.Cross(v2) + planeVec := cross.Normalize() + // find current plane equation denoted as: + // cross[0]*x + cross[1]*y + cross[2]*z + d = 0 + // to find d, we just need to pick a point and deduce d from the plane equation (vec orth to p1, p2, p3) + d := -planeVec.Dot(p2) + + currentEquation := [4]float64{planeVec.X, planeVec.Y, planeVec.Z, d} + + if angleThreshold != 0 { + if math.Acos(normalVec.Dot(planeVec)) <= angleThreshold*math.Pi/180.0 { + equations = append(equations, currentEquation) + } + } else { + equations = append(equations, currentEquation) + } + } + return findBestEq(ctx, cloud, len(equations), equations, pts, dstThreshold) +} + // SegmentPlane segments the biggest plane in the 3D Pointcloud. // nIterations is the number of iteration for ransac // nIter to choose? nIter = log(1-p)/log(1-(1-e)^s), where p is prob of success, e is outlier ratio, s is subset size (3 for plane). @@ -106,6 +159,12 @@ func SegmentPlane(ctx context.Context, cloud pc.PointCloud, nIterations int, thr equations = append(equations, currentEquation) } + return findBestEq(ctx, cloud, nIterations, equations, pts, threshold) +} + +func findBestEq(ctx context.Context, cloud pc.PointCloud, nIterations int, equations [][4]float64, + pts []r3.Vector, threshold float64, +) (pc.Plane, pc.PointCloud, error) { // Then find the best equation in parallel. It ends up being faster to loop // by equations (iterations) and then points due to what I (erd) think is // memory locality exploitation. @@ -156,6 +215,8 @@ func SegmentPlane(ctx context.Context, cloud pc.PointCloud, nIterations int, thr } bestEquation = bestResults[bestIdx].equation + nPoints := cloud.Size() + planeCloud := pc.NewWithPrealloc(bestInliers) nonPlaneCloud := pc.NewWithPrealloc(nPoints - bestInliers) planeCloudCenter := r3.Vector{} @@ -188,27 +249,48 @@ func SegmentPlane(ctx context.Context, cloud pc.PointCloud, nIterations int, thr // PlaneSegmentation is an interface used to find geometric planes in a 3D space. type PlaneSegmentation interface { FindPlanes(ctx context.Context) ([]pc.Plane, pc.PointCloud, error) + FindGroundPlane(ctx context.Context) (pc.Plane, pc.PointCloud, error) } type pointCloudPlaneSegmentation struct { - cloud pc.PointCloud - threshold float64 - minPoints int - nIterations int + cloud pc.PointCloud + distanceThreshold float64 + minPoints int + nIterations int + angleThreshold float64 + normalVec r3.Vector } // NewPointCloudPlaneSegmentation initializes the plane segmentation with the necessary parameters to find the planes // threshold is the float64 value for the maximum allowed distance to the found plane for a point to belong to it. // minPoints is the minimum number of points necessary to be considered a plane. func NewPointCloudPlaneSegmentation(cloud pc.PointCloud, threshold float64, minPoints int) PlaneSegmentation { - return &pointCloudPlaneSegmentation{cloud, threshold, minPoints, 2000} + return &pointCloudPlaneSegmentation{ + cloud: cloud, + distanceThreshold: threshold, + minPoints: minPoints, + nIterations: 2000, + angleThreshold: 0, + normalVec: r3.Vector{X: 0, Y: 0, Z: 1}, + } +} + +// NewPointCloudGroundPlaneSegmentation initializes the plane segmentation with the necessary parameters to find +// ground like planes, meaning they are less than angleThreshold away from the plane corresponding to normaLVec +// distanceThreshold is the float64 value for the maximum allowed distance to the found plane for a +// point to belong to it. +// minPoints is the minimum number of points necessary to be considered a plane. +func NewPointCloudGroundPlaneSegmentation(cloud pc.PointCloud, distanceThreshold float64, minPoints int, + angleThreshold float64, normalVec r3.Vector, +) PlaneSegmentation { + return &pointCloudPlaneSegmentation{cloud, distanceThreshold, minPoints, 2000, angleThreshold, normalVec} } // FindPlanes takes in a point cloud and outputs an array of the planes and a point cloud of the leftover points. func (pcps *pointCloudPlaneSegmentation) FindPlanes(ctx context.Context) ([]pc.Plane, pc.PointCloud, error) { planes := make([]pc.Plane, 0) var err error - plane, nonPlaneCloud, err := SegmentPlane(ctx, pcps.cloud, pcps.nIterations, pcps.threshold) + plane, nonPlaneCloud, err := SegmentPlane(ctx, pcps.cloud, pcps.nIterations, pcps.distanceThreshold) if err != nil { return nil, nil, err } @@ -223,7 +305,7 @@ func (pcps *pointCloudPlaneSegmentation) FindPlanes(ctx context.Context) ([]pc.P var lastNonPlaneCloud pc.PointCloud for { lastNonPlaneCloud = nonPlaneCloud - smallerPlane, smallerNonPlaneCloud, err := SegmentPlane(ctx, nonPlaneCloud, pcps.nIterations, pcps.threshold) + smallerPlane, smallerNonPlaneCloud, err := SegmentPlane(ctx, nonPlaneCloud, pcps.nIterations, pcps.distanceThreshold) if err != nil { return nil, nil, err } @@ -243,6 +325,24 @@ func (pcps *pointCloudPlaneSegmentation) FindPlanes(ctx context.Context) ([]pc.P return planes, nonPlaneCloud, nil } +// FindGroundPlane takes in a point cloud and outputs an array of a ground like plane and a point cloud of the leftover points. +func (pcps *pointCloudPlaneSegmentation) FindGroundPlane(ctx context.Context) (pc.Plane, pc.PointCloud, error) { + var err error + plane, nonPlaneCloud, err := SegmentPlaneWRTGround(ctx, pcps.cloud, pcps.nIterations, + pcps.angleThreshold, pcps.distanceThreshold, pcps.normalVec) + if err != nil { + return nil, nil, err + } + planeCloud, err := plane.PointCloud() + if err != nil { + return nil, nil, err + } + if planeCloud.Size() <= pcps.minPoints { + return nil, pcps.cloud, nil + } + return plane, nonPlaneCloud, nil +} + // VoxelGridPlaneConfig contains the parameters needed to create a Plane from a VoxelGrid. type VoxelGridPlaneConfig struct { WeightThresh float64 `json:"weight_threshold"` @@ -288,6 +388,11 @@ func (vgps *voxelGridPlaneSegmentation) FindPlanes(ctx context.Context) ([]pc.Pl return planes, nonPlaneCloud, nil } +// FindGroundPlane is yet to be implemented. +func (vgps *voxelGridPlaneSegmentation) FindGroundPlane(ctx context.Context) (pc.Plane, pc.PointCloud, error) { + return nil, nil, errors.New("function not yet implemented") +} + // SplitPointCloudByPlane divides the point cloud in two point clouds, given the equation of a plane. // one point cloud will have all the points above the plane and the other with all the points below the plane. // Points exactly on the plane are not included! diff --git a/vision/segmentation/plane_segmentation_test.go b/vision/segmentation/plane_segmentation_test.go index c9d204f2fa7..b62a3ff686c 100644 --- a/vision/segmentation/plane_segmentation_test.go +++ b/vision/segmentation/plane_segmentation_test.go @@ -24,7 +24,7 @@ func init() { func TestPlaneConfig(t *testing.T) { cfg := VoxelGridPlaneConfig{} // invalid weight threshold - cfg.WeightThresh = -1. + cfg.WeightThresh = -2. err := cfg.CheckValid() test.That(t, err.Error(), test.ShouldContainSubstring, "weight_threshold cannot be less than 0") // invalid angle threshold @@ -48,8 +48,40 @@ func TestPlaneConfig(t *testing.T) { test.That(t, err, test.ShouldBeNil) } +func TestSegmentPlaneWRTGround(t *testing.T) { + // get depth map + d, err := rimage.NewDepthMapFromFile( + context.Background(), + artifact.MustPath("vision/segmentation/pointcloudsegmentation/align-test-1615172036.png")) + test.That(t, err, test.ShouldBeNil) + + // Pixel to Meter + sensorParams, err := transform.NewDepthColorIntrinsicsExtrinsicsFromJSONFile(intel515ParamsPath) + test.That(t, err, test.ShouldBeNil) + depthIntrinsics := &sensorParams.DepthCamera + cloud := depthadapter.ToPointCloud(d, depthIntrinsics) + test.That(t, err, test.ShouldBeNil) + // Segment Plane + nIter := 3000 + groundNormVec := r3.Vector{0, 1, 0} + angleThresh := 30.0 + plane, _, err := SegmentPlaneWRTGround(context.Background(), cloud, nIter, angleThresh, 0.5, groundNormVec) + eq := plane.Equation() + test.That(t, err, test.ShouldBeNil) + + p1 := r3.Vector{-eq[3] / eq[0], 0, 0} + p2 := r3.Vector{0, -eq[3] / eq[1], 0} + p3 := r3.Vector{0, 0, -eq[3] / eq[2]} + + v1 := p2.Sub(p1).Normalize() + v2 := p3.Sub(p1).Normalize() + + planeNormVec := v1.Cross(v2) + planeNormVec = planeNormVec.Normalize() + test.That(t, math.Acos(planeNormVec.Dot(groundNormVec)), test.ShouldBeLessThanOrEqualTo, angleThresh*math.Pi/180) +} + func TestSegmentPlane(t *testing.T) { - t.Parallel() // Intel Sensor Extrinsic data from manufacturer // Intel sensor depth 1024x768 to RGB 1280x720 // Translation Vector : [-0.000828434,0.0139185,-0.0033418] diff --git a/vision/segmentation/radius_clustering.go b/vision/segmentation/radius_clustering.go index dbf43666d25..92f08cd8ba6 100644 --- a/vision/segmentation/radius_clustering.go +++ b/vision/segmentation/radius_clustering.go @@ -14,14 +14,18 @@ import ( "go.viam.com/rdk/vision" ) -// RadiusClusteringConfig specifies the necessary parameters for 3D object finding. +// RadiusClusteringConfig specifies the necessary parameters to apply the +// radius based clustering algo. type RadiusClusteringConfig struct { resource.TriviallyValidateConfig - MinPtsInPlane int `json:"min_points_in_plane"` - MinPtsInSegment int `json:"min_points_in_segment"` - ClusteringRadiusMm float64 `json:"clustering_radius_mm"` - MeanKFiltering int `json:"mean_k_filtering"` - Label string `json:"label,omitempty"` + MinPtsInPlane int `json:"min_points_in_plane"` + MaxDistFromPlane float64 `json:"max_dist_from_plane_mm"` + NormalVec r3.Vector `json:"ground_plane_normal_vec"` + AngleTolerance float64 `json:"ground_angle_tolerance_degs"` + MinPtsInSegment int `json:"min_points_in_segment"` + ClusteringRadiusMm float64 `json:"clustering_radius_mm"` + MeanKFiltering int `json:"mean_k_filtering"` + Label string `json:"label,omitempty"` } // CheckValid checks to see in the input values are valid. @@ -35,6 +39,21 @@ func (rcc *RadiusClusteringConfig) CheckValid() error { if rcc.ClusteringRadiusMm <= 0 { return errors.Errorf("clustering_radius_mm must be greater than 0, got %v", rcc.ClusteringRadiusMm) } + if rcc.MaxDistFromPlane == 0 { + rcc.MaxDistFromPlane = 100 + } + if rcc.MaxDistFromPlane <= 0 { + return errors.Errorf("max_dist_from_plane must be greater than 0, got %v", rcc.MaxDistFromPlane) + } + if rcc.AngleTolerance > 180 || rcc.AngleTolerance < 0 { + return errors.Errorf("max_angle_of_plane must between 0 & 180 (inclusive), got %v", rcc.AngleTolerance) + } + if rcc.NormalVec.Norm2() == 0 { + rcc.NormalVec = r3.Vector{X: 0, Y: 0, Z: 1} + } + if !rcc.NormalVec.IsUnit() { + return errors.Errorf("ground_plane_normal_vec should be a unit vector, got %v", rcc.NormalVec) + } return nil } @@ -74,9 +93,9 @@ func (rcc *RadiusClusteringConfig) RadiusClustering(ctx context.Context, src cam if err != nil { return nil, err } - ps := NewPointCloudPlaneSegmentation(cloud, 10, rcc.MinPtsInPlane) + ps := NewPointCloudGroundPlaneSegmentation(cloud, rcc.MaxDistFromPlane, rcc.MinPtsInPlane, rcc.AngleTolerance, rcc.NormalVec) // if there are found planes, remove them, and keep all the non-plane points - _, nonPlane, err := ps.FindPlanes(ctx) + _, nonPlane, err := ps.FindGroundPlane(ctx) if err != nil { return nil, err } diff --git a/vision/segmentation/radius_clustering_test.go b/vision/segmentation/radius_clustering_test.go index b0c00d583f6..9177603790b 100644 --- a/vision/segmentation/radius_clustering_test.go +++ b/vision/segmentation/radius_clustering_test.go @@ -28,9 +28,25 @@ func TestRadiusClusteringValidate(t *testing.T) { cfg.MinPtsInSegment = 5 err = cfg.CheckValid() test.That(t, err.Error(), test.ShouldContainSubstring, "clustering_radius_mm must be greater than 0") - // valid + // invalid angle from plane cfg.ClusteringRadiusMm = 5 + cfg.AngleTolerance = 190 + err = cfg.CheckValid() + test.That(t, err.Error(), test.ShouldContainSubstring, "max_angle_of_plane must between 0 & 180 (inclusive)") + // valid + cfg.AngleTolerance = 180 cfg.MeanKFiltering = 5 + cfg.MaxDistFromPlane = 4 + err = cfg.CheckValid() + test.That(t, err, test.ShouldBeNil) + + // cfg succeeds even without MaxDistFromPlane, AngleTolerance, NormalVec + cfg = segmentation.RadiusClusteringConfig{ + MinPtsInPlane: 10, + MinPtsInSegment: 10, + ClusteringRadiusMm: 10, + MeanKFiltering: 10, + } err = cfg.CheckValid() test.That(t, err, test.ShouldBeNil) } @@ -47,6 +63,7 @@ func TestPixelSegmentation(t *testing.T) { expectedLabel := "test_label" objConfig := utils.AttributeMap{ "min_points_in_plane": 50000, + "max_dist_from_plane": 10, "min_points_in_segment": 500, "clustering_radius_mm": 10.0, "mean_k_filtering": 50.0, @@ -59,19 +76,31 @@ func TestPixelSegmentation(t *testing.T) { segments, err := segmenter(context.Background(), injectCamera) test.That(t, err, test.ShouldBeNil) testSegmentation(t, segments, expectedLabel) +} + +func TestPixelSegmentationNoFiltering(t *testing.T) { + t.Parallel() + logger := golog.NewTestLogger(t) + injectCamera := &inject.Camera{} + injectCamera.NextPointCloudFunc = func(ctx context.Context) (pc.PointCloud, error) { + return pc.NewFromLASFile(artifact.MustPath("pointcloud/test.las"), logger) + } // do segmentation with no mean k filtering - objConfig = utils.AttributeMap{ - "min_points_in_plane": 50000, - "min_points_in_segment": 500, - "clustering_radius_mm": 10.0, - "mean_k_filtering": -1., + expectedLabel := "test_label" + objConfig := utils.AttributeMap{ + "min_points_in_plane": 9000, + "max_dist_from_plane": 110.0, + "min_points_in_segment": 350, + "max_angle_of_plane": 30, + "clustering_radius_mm": 600.0, + "mean_k_filtering": 0, "extra_uneeded_param": 4444, "another_extra_one": "hey", "label": expectedLabel, } - segmenter, err = segmentation.NewRadiusClustering(objConfig) + segmenter, err := segmentation.NewRadiusClustering(objConfig) test.That(t, err, test.ShouldBeNil) - segments, err = segmenter(context.Background(), injectCamera) + segments, err := segmenter(context.Background(), injectCamera) test.That(t, err, test.ShouldBeNil) testSegmentation(t, segments, expectedLabel) } @@ -92,3 +121,29 @@ func testSegmentation(t *testing.T, segments []*vision.Object, expectedLabel str test.That(t, box.Label(), test.ShouldEqual, expectedLabel) } } + +func BenchmarkRadiusClustering(b *testing.B) { + injectCamera := &inject.Camera{} + injectCamera.NextPointCloudFunc = func(ctx context.Context) (pc.PointCloud, error) { + return pc.NewFromLASFile(artifact.MustPath("pointcloud/test.las"), nil) + } + var pts []*vision.Object + var err error + // do segmentation + objConfig := utils.AttributeMap{ + "min_points_in_plane": 9000, + "max_dist_from_plane": 110.0, + "min_points_in_segment": 350, + "max_angle_of_plane": 30, + "clustering_radius_mm": 600.0, + "mean_k_filtering": 0, + } + segmenter, _ := segmentation.NewRadiusClustering(objConfig) + for i := 0; i < b.N; i++ { + pts, err = segmenter(context.Background(), injectCamera) + } + // to prevent vars from being optimized away + if pts == nil || err != nil { + panic("segmenter didn't work") + } +} diff --git a/vision/segmentation/radius_clustering_voxel.go b/vision/segmentation/radius_clustering_voxel.go index 6e8d1ec54c3..75c3624ade5 100644 --- a/vision/segmentation/radius_clustering_voxel.go +++ b/vision/segmentation/radius_clustering_voxel.go @@ -19,6 +19,7 @@ type RadiusClusteringVoxelConfig struct { VoxelSize float64 `json:"voxel_size"` Lambda float64 `json:"lambda"` // clustering parameter for making voxel planes MinPtsInPlane int `json:"min_points_in_plane"` + MaxDistFromPlane float64 `json:"max_dist_from_plane"` MinPtsInSegment int `json:"min_points_in_segment"` ClusteringRadiusMm float64 `json:"clustering_radius_mm"` WeightThresh float64 `json:"weight_threshold"` @@ -38,6 +39,7 @@ func (rcc *RadiusClusteringVoxelConfig) CheckValid() error { } radiusClustering := RadiusClusteringConfig{ MinPtsInPlane: rcc.MinPtsInPlane, + MaxDistFromPlane: rcc.MaxDistFromPlane, MinPtsInSegment: rcc.MinPtsInSegment, ClusteringRadiusMm: rcc.ClusteringRadiusMm, MeanKFiltering: 50.0, diff --git a/vision/segmentation/radius_clustering_voxel_test.go b/vision/segmentation/radius_clustering_voxel_test.go index c50046a5165..2104b5dcd29 100644 --- a/vision/segmentation/radius_clustering_voxel_test.go +++ b/vision/segmentation/radius_clustering_voxel_test.go @@ -35,6 +35,7 @@ func TestClusteringVoxelConfig(t *testing.T) { cfg.AngleThresh = 40 cfg.CosineThresh = .1 cfg.DistanceThresh = 44 + cfg.MaxDistFromPlane = 10 err = cfg.CheckValid() test.That(t, err.Error(), test.ShouldContainSubstring, "weight_threshold cannot be less than 0") // valid @@ -56,6 +57,7 @@ func TestVoxelSegmentMeans(t *testing.T) { "voxel_size": 1.0, "lambda": 0.1, "min_points_in_plane": 100, + "max_dist_from_plane": 10, "min_points_in_segment": 25, "clustering_radius_mm": 7.5, "weight_threshold": 0.9, diff --git a/vision/segmentation/segments.go b/vision/segmentation/segments.go index 3d5bf5a6f48..804497e9777 100644 --- a/vision/segmentation/segments.go +++ b/vision/segmentation/segments.go @@ -28,7 +28,7 @@ func NewSegments() *Segments { func NewSegmentsFromSlice(clouds []pc.PointCloud, label string) (*Segments, error) { segments := NewSegments() for i, cloud := range clouds { - seg, err := vision.NewObjectWithLabel(cloud, label) + seg, err := vision.NewObjectWithLabel(cloud, label, nil) if err != nil { return nil, err } diff --git a/web/frontend/.eslintrc.cjs b/web/frontend/.eslintrc.cjs index 6cbd9cda11d..3c056e1e763 100644 --- a/web/frontend/.eslintrc.cjs +++ b/web/frontend/.eslintrc.cjs @@ -115,7 +115,6 @@ module.exports = { 'object-curly-spacing': ['error', 'always'], 'object-property-newline': ['error', { allowAllPropertiesOnSameLine: true }], 'no-continue': 'off', - 'no-duplicate-imports': 'off', 'no-extra-parens': 'off', 'no-magic-numbers': 'off', 'no-multiple-empty-lines': ['error', { max: 1 }], @@ -156,6 +155,7 @@ module.exports = { 'no-caller': 'error', 'no-param-reassign': 'error', 'no-return-await': 'error', + 'no-undef-init': 'off', radix: 'error', 'require-await': 'error', strict: 'error', @@ -183,13 +183,14 @@ module.exports = { 'unicorn/filename-case': 'off', 'unicorn/no-null': 'off', 'unicorn/consistent-destructuring': 'off', + 'unicorn/no-array-for-each': 'off', + 'unicorn/no-useless-undefined': 'off', // Tailwind 'tailwindcss/no-custom-classname': 'off', // Typescript '@typescript-eslint/indent': ['error', 2], - '@typescript-eslint/no-duplicate-imports': ['error'], '@typescript-eslint/no-unused-vars': ['error', { argsIgnorePattern: '^_' }], '@typescript-eslint/no-explicit-any': 'error', '@typescript-eslint/no-non-null-assertion': 'off', diff --git a/web/frontend/package-lock.json b/web/frontend/package-lock.json index bcad875307c..7454e28351a 100644 --- a/web/frontend/package-lock.json +++ b/web/frontend/package-lock.json @@ -1,59 +1,70 @@ { "name": "@viamrobotics/remote-control", - "version": "2.0.1", + "version": "2.0.18", "lockfileVersion": 3, "requires": true, "packages": { "": { "name": "@viamrobotics/remote-control", - "version": "2.0.1", + "version": "2.0.18", "license": "Apache-2.0", "devDependencies": { "@improbable-eng/grpc-web": "0.15.0", "@mdi/js": "7.2.96", - "@sveltejs/vite-plugin-svelte": "^2.4.2", - "@threlte/core": "^6.0.0-next.8", - "@threlte/extras": "^5.0.0-next.13", + "@sveltejs/vite-plugin-svelte": "^2.4.4", + "@threlte/core": "^6.0.3", + "@threlte/extras": "^5.1.0", "@types/google-protobuf": "3.15.6", - "@types/three": "0.152.1", - "@typescript-eslint/eslint-plugin": "^5.60.1", - "@typescript-eslint/parser": "^5.60.1", - "@viamrobotics/prime": "0.2.19", + "@types/three": "0.155.0", + "@typescript-eslint/eslint-plugin": "^6.3.0", + "@typescript-eslint/parser": "^6.3.0", + "@viamrobotics/prime": "0.5.2", "@viamrobotics/rpc": "0.1.37", - "@viamrobotics/sdk": "0.2.2", - "@viamrobotics/typescript-config": "^0.0.3", - "cypress": "12.16.0", - "eslint": "8.43.0", + "@viamrobotics/sdk": "0.3.2-rc.0", + "@viamrobotics/three": "^0.0.2", + "@viamrobotics/typescript-config": "^0.0.4", + "cypress": "12.17.3", + "eslint": "8.46.0", "eslint-import-resolver-custom-alias": "1.3.2", - "eslint-plugin-import": "2.27.5", + "eslint-plugin-import": "2.28.0", "eslint-plugin-promise": "6.1.1", - "eslint-plugin-svelte": "^2.31.1", + "eslint-plugin-svelte": "^2.32.4", "eslint-plugin-tailwindcss": "3.13.0", - "eslint-plugin-unicorn": "47.0.0", + "eslint-plugin-unicorn": "48.0.1", "google-protobuf": "3.21.2", "jshashes": "1.0.8", - "maplibre-gl": "^3.1.0", - "postcss": "8.4.24", - "svelte": "^4.0.0", - "svelte-check": "^3.4.4", - "tailwindcss": "3.3.2", - "three": "0.152.2", + "maplibre-gl": "^3.3.0", + "postcss": "8.4.27", + "svelte": "^4.1.2", + "svelte-check": "^3.4.6", + "svelte-inview": "^4.0.1", + "tailwindcss": "3.3.3", + "three": "0.155.0", "three-inspect": "0.3.4", - "trzy": "0.0.49", - "typescript": "5.1.3", - "vite": "4.3.9", - "vite-plugin-css-injected-by-js": "3.1.1", - "vitest": "0.32.2" + "trzy": "0.3.11", + "typescript": "5.1.6", + "vite": "4.4.9", + "vite-plugin-css-injected-by-js": "3.3.0", + "vitest": "0.34.1" }, "peerDependencies": { - "@improbable-eng/grpc-web": "~0.15.*", - "@viamrobotics/prime": "~0.2.*", - "@viamrobotics/rpc": "~0.1.*", - "@viamrobotics/sdk": "0.2.0-pre.1", - "google-protobuf": "~3.*.*", - "tailwindcss": "~3.3.*", - "three": "~0.152.*", - "trzy": "0.0.49" + "@improbable-eng/grpc-web": ">=0.15", + "@viamrobotics/prime": ">=0.5", + "@viamrobotics/rpc": ">=0.1", + "@viamrobotics/sdk": "0.3.2-rc.0", + "google-protobuf": ">=3", + "tailwindcss": ">=3.3", + "three": ">=0.155", + "trzy": "0.3.11" + } + }, + "node_modules/@aashutoshrathi/word-wrap": { + "version": "1.2.6", + "resolved": "https://registry.npmjs.org/@aashutoshrathi/word-wrap/-/word-wrap-1.2.6.tgz", + "integrity": "sha512-1Yjs2SvM8TflER/OD3cOjhWWOZb58A2t7wpE2S9XfBYTiIl+XFhQG2bjy4Pu1I+EAlCNUzRDYDdFwFYUKvXcIA==", + "dev": true, + "engines": { + "node": ">=0.10.0" } }, "node_modules/@alloc/quick-lru": { @@ -94,9 +105,9 @@ } }, "node_modules/@babel/helper-validator-identifier": { - "version": "7.19.1", - "resolved": "https://registry.npmjs.org/@babel/helper-validator-identifier/-/helper-validator-identifier-7.19.1.tgz", - "integrity": "sha512-awrNfaMtnHUr653GgGEs++LlAvW6w+DcPrOliSMXWCKo597CwL5Acf/wWdNkf/tfEQE3mjkeD1YOVZOUV/od1w==", + "version": "7.22.5", + "resolved": "https://registry.npmjs.org/@babel/helper-validator-identifier/-/helper-validator-identifier-7.22.5.tgz", + "integrity": "sha512-aJXu+6lErq8ltp+JhkJUfk1MTGyuA4v7f3pA+BJ5HLfNC6nAQ0Cpi9uOquUj8Hehg0aUiHzWQbOVJGao6ztBAQ==", "dev": true, "engines": { "node": ">=6.9.0" @@ -198,9 +209,9 @@ } }, "node_modules/@cypress/request": { - "version": "2.88.10", - "resolved": "https://registry.npmjs.org/@cypress/request/-/request-2.88.10.tgz", - "integrity": "sha512-Zp7F+R93N0yZyG34GutyTNr+okam7s/Fzc1+i3kcqOP8vk6OuajuE9qZJ6Rs+10/1JFtXFYMdyarnU1rZuJesg==", + "version": "2.88.12", + "resolved": "https://registry.npmjs.org/@cypress/request/-/request-2.88.12.tgz", + "integrity": "sha512-tOn+0mDZxASFM+cuAP9szGUGPI1HwWVSvdzm7V4cCsPdFTx6qMj29CwaQmRAMIEhORIUBFBsYROYJcveK4uOjA==", "dev": true, "dependencies": { "aws-sign2": "~0.7.0", @@ -216,9 +227,9 @@ "json-stringify-safe": "~5.0.1", "mime-types": "~2.1.19", "performance-now": "^2.1.0", - "qs": "~6.5.2", + "qs": "~6.10.3", "safe-buffer": "^5.1.2", - "tough-cookie": "~2.5.0", + "tough-cookie": "^4.1.3", "tunnel-agent": "^0.6.0", "uuid": "^8.3.2" }, @@ -245,10 +256,58 @@ "ms": "^2.1.1" } }, + "node_modules/@esbuild/android-arm": { + "version": "0.18.20", + "resolved": "https://registry.npmjs.org/@esbuild/android-arm/-/android-arm-0.18.20.tgz", + "integrity": "sha512-fyi7TDI/ijKKNZTUJAQqiG5T7YjJXgnzkURqmGj13C6dCqckZBLdl4h7bkhHt/t0WP+zO9/zwroDvANaOqO5Sw==", + "cpu": [ + "arm" + ], + "dev": true, + "optional": true, + "os": [ + "android" + ], + "engines": { + "node": ">=12" + } + }, + "node_modules/@esbuild/android-arm64": { + "version": "0.18.20", + "resolved": "https://registry.npmjs.org/@esbuild/android-arm64/-/android-arm64-0.18.20.tgz", + "integrity": "sha512-Nz4rJcchGDtENV0eMKUNa6L12zz2zBDXuhj/Vjh18zGqB44Bi7MBMSXjgunJgjRhCmKOjnPuZp4Mb6OKqtMHLQ==", + "cpu": [ + "arm64" + ], + "dev": true, + "optional": true, + "os": [ + "android" + ], + "engines": { + "node": ">=12" + } + }, + "node_modules/@esbuild/android-x64": { + "version": "0.18.20", + "resolved": "https://registry.npmjs.org/@esbuild/android-x64/-/android-x64-0.18.20.tgz", + "integrity": "sha512-8GDdlePJA8D6zlZYJV/jnrRAi6rOiNaCC/JclcXpB+KIuvfBN4owLtgzY2bsxnx666XjJx2kDPUmnTtR8qKQUg==", + "cpu": [ + "x64" + ], + "dev": true, + "optional": true, + "os": [ + "android" + ], + "engines": { + "node": ">=12" + } + }, "node_modules/@esbuild/darwin-arm64": { - "version": "0.17.15", - "resolved": "https://registry.npmjs.org/@esbuild/darwin-arm64/-/darwin-arm64-0.17.15.tgz", - "integrity": "sha512-7siLjBc88Z4+6qkMDxPT2juf2e8SJxmsbNVKFY2ifWCDT72v5YJz9arlvBw5oB4W/e61H1+HDB/jnu8nNg0rLA==", + "version": "0.18.20", + "resolved": "https://registry.npmjs.org/@esbuild/darwin-arm64/-/darwin-arm64-0.18.20.tgz", + "integrity": "sha512-bxRHW5kHU38zS2lPTPOyuyTm+S+eobPUnTNkdJEfAddYgEcll4xkT8DB9d2008DtTbl7uJag2HuE5NZAZgnNEA==", "cpu": [ "arm64" ], @@ -261,6 +320,294 @@ "node": ">=12" } }, + "node_modules/@esbuild/darwin-x64": { + "version": "0.18.20", + "resolved": "https://registry.npmjs.org/@esbuild/darwin-x64/-/darwin-x64-0.18.20.tgz", + "integrity": "sha512-pc5gxlMDxzm513qPGbCbDukOdsGtKhfxD1zJKXjCCcU7ju50O7MeAZ8c4krSJcOIJGFR+qx21yMMVYwiQvyTyQ==", + "cpu": [ + "x64" + ], + "dev": true, + "optional": true, + "os": [ + "darwin" + ], + "engines": { + "node": ">=12" + } + }, + "node_modules/@esbuild/freebsd-arm64": { + "version": "0.18.20", + "resolved": "https://registry.npmjs.org/@esbuild/freebsd-arm64/-/freebsd-arm64-0.18.20.tgz", + "integrity": "sha512-yqDQHy4QHevpMAaxhhIwYPMv1NECwOvIpGCZkECn8w2WFHXjEwrBn3CeNIYsibZ/iZEUemj++M26W3cNR5h+Tw==", + "cpu": [ + "arm64" + ], + "dev": true, + "optional": true, + "os": [ + "freebsd" + ], + "engines": { + "node": ">=12" + } + }, + "node_modules/@esbuild/freebsd-x64": { + "version": "0.18.20", + "resolved": "https://registry.npmjs.org/@esbuild/freebsd-x64/-/freebsd-x64-0.18.20.tgz", + "integrity": "sha512-tgWRPPuQsd3RmBZwarGVHZQvtzfEBOreNuxEMKFcd5DaDn2PbBxfwLcj4+aenoh7ctXcbXmOQIn8HI6mCSw5MQ==", + "cpu": [ + "x64" + ], + "dev": true, + "optional": true, + "os": [ + "freebsd" + ], + "engines": { + "node": ">=12" + } + }, + "node_modules/@esbuild/linux-arm": { + "version": "0.18.20", + "resolved": "https://registry.npmjs.org/@esbuild/linux-arm/-/linux-arm-0.18.20.tgz", + "integrity": "sha512-/5bHkMWnq1EgKr1V+Ybz3s1hWXok7mDFUMQ4cG10AfW3wL02PSZi5kFpYKrptDsgb2WAJIvRcDm+qIvXf/apvg==", + "cpu": [ + "arm" + ], + "dev": true, + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": ">=12" + } + }, + "node_modules/@esbuild/linux-arm64": { + "version": "0.18.20", + "resolved": "https://registry.npmjs.org/@esbuild/linux-arm64/-/linux-arm64-0.18.20.tgz", + "integrity": "sha512-2YbscF+UL7SQAVIpnWvYwM+3LskyDmPhe31pE7/aoTMFKKzIc9lLbyGUpmmb8a8AixOL61sQ/mFh3jEjHYFvdA==", + "cpu": [ + "arm64" + ], + "dev": true, + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": ">=12" + } + }, + "node_modules/@esbuild/linux-ia32": { + "version": "0.18.20", + "resolved": "https://registry.npmjs.org/@esbuild/linux-ia32/-/linux-ia32-0.18.20.tgz", + "integrity": "sha512-P4etWwq6IsReT0E1KHU40bOnzMHoH73aXp96Fs8TIT6z9Hu8G6+0SHSw9i2isWrD2nbx2qo5yUqACgdfVGx7TA==", + "cpu": [ + "ia32" + ], + "dev": true, + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": ">=12" + } + }, + "node_modules/@esbuild/linux-loong64": { + "version": "0.18.20", + "resolved": "https://registry.npmjs.org/@esbuild/linux-loong64/-/linux-loong64-0.18.20.tgz", + "integrity": "sha512-nXW8nqBTrOpDLPgPY9uV+/1DjxoQ7DoB2N8eocyq8I9XuqJ7BiAMDMf9n1xZM9TgW0J8zrquIb/A7s3BJv7rjg==", + "cpu": [ + "loong64" + ], + "dev": true, + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": ">=12" + } + }, + "node_modules/@esbuild/linux-mips64el": { + "version": "0.18.20", + "resolved": "https://registry.npmjs.org/@esbuild/linux-mips64el/-/linux-mips64el-0.18.20.tgz", + "integrity": "sha512-d5NeaXZcHp8PzYy5VnXV3VSd2D328Zb+9dEq5HE6bw6+N86JVPExrA6O68OPwobntbNJ0pzCpUFZTo3w0GyetQ==", + "cpu": [ + "mips64el" + ], + "dev": true, + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": ">=12" + } + }, + "node_modules/@esbuild/linux-ppc64": { + "version": "0.18.20", + "resolved": "https://registry.npmjs.org/@esbuild/linux-ppc64/-/linux-ppc64-0.18.20.tgz", + "integrity": "sha512-WHPyeScRNcmANnLQkq6AfyXRFr5D6N2sKgkFo2FqguP44Nw2eyDlbTdZwd9GYk98DZG9QItIiTlFLHJHjxP3FA==", + "cpu": [ + "ppc64" + ], + "dev": true, + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": ">=12" + } + }, + "node_modules/@esbuild/linux-riscv64": { + "version": "0.18.20", + "resolved": "https://registry.npmjs.org/@esbuild/linux-riscv64/-/linux-riscv64-0.18.20.tgz", + "integrity": "sha512-WSxo6h5ecI5XH34KC7w5veNnKkju3zBRLEQNY7mv5mtBmrP/MjNBCAlsM2u5hDBlS3NGcTQpoBvRzqBcRtpq1A==", + "cpu": [ + "riscv64" + ], + "dev": true, + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": ">=12" + } + }, + "node_modules/@esbuild/linux-s390x": { + "version": "0.18.20", + "resolved": "https://registry.npmjs.org/@esbuild/linux-s390x/-/linux-s390x-0.18.20.tgz", + "integrity": "sha512-+8231GMs3mAEth6Ja1iK0a1sQ3ohfcpzpRLH8uuc5/KVDFneH6jtAJLFGafpzpMRO6DzJ6AvXKze9LfFMrIHVQ==", + "cpu": [ + "s390x" + ], + "dev": true, + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": ">=12" + } + }, + "node_modules/@esbuild/linux-x64": { + "version": "0.18.20", + "resolved": "https://registry.npmjs.org/@esbuild/linux-x64/-/linux-x64-0.18.20.tgz", + "integrity": "sha512-UYqiqemphJcNsFEskc73jQ7B9jgwjWrSayxawS6UVFZGWrAAtkzjxSqnoclCXxWtfwLdzU+vTpcNYhpn43uP1w==", + "cpu": [ + "x64" + ], + "dev": true, + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": ">=12" + } + }, + "node_modules/@esbuild/netbsd-x64": { + "version": "0.18.20", + "resolved": "https://registry.npmjs.org/@esbuild/netbsd-x64/-/netbsd-x64-0.18.20.tgz", + "integrity": "sha512-iO1c++VP6xUBUmltHZoMtCUdPlnPGdBom6IrO4gyKPFFVBKioIImVooR5I83nTew5UOYrk3gIJhbZh8X44y06A==", + "cpu": [ + "x64" + ], + "dev": true, + "optional": true, + "os": [ + "netbsd" + ], + "engines": { + "node": ">=12" + } + }, + "node_modules/@esbuild/openbsd-x64": { + "version": "0.18.20", + "resolved": "https://registry.npmjs.org/@esbuild/openbsd-x64/-/openbsd-x64-0.18.20.tgz", + "integrity": "sha512-e5e4YSsuQfX4cxcygw/UCPIEP6wbIL+se3sxPdCiMbFLBWu0eiZOJ7WoD+ptCLrmjZBK1Wk7I6D/I3NglUGOxg==", + "cpu": [ + "x64" + ], + "dev": true, + "optional": true, + "os": [ + "openbsd" + ], + "engines": { + "node": ">=12" + } + }, + "node_modules/@esbuild/sunos-x64": { + "version": "0.18.20", + "resolved": "https://registry.npmjs.org/@esbuild/sunos-x64/-/sunos-x64-0.18.20.tgz", + "integrity": "sha512-kDbFRFp0YpTQVVrqUd5FTYmWo45zGaXe0X8E1G/LKFC0v8x0vWrhOWSLITcCn63lmZIxfOMXtCfti/RxN/0wnQ==", + "cpu": [ + "x64" + ], + "dev": true, + "optional": true, + "os": [ + "sunos" + ], + "engines": { + "node": ">=12" + } + }, + "node_modules/@esbuild/win32-arm64": { + "version": "0.18.20", + "resolved": "https://registry.npmjs.org/@esbuild/win32-arm64/-/win32-arm64-0.18.20.tgz", + "integrity": "sha512-ddYFR6ItYgoaq4v4JmQQaAI5s7npztfV4Ag6NrhiaW0RrnOXqBkgwZLofVTlq1daVTQNhtI5oieTvkRPfZrePg==", + "cpu": [ + "arm64" + ], + "dev": true, + "optional": true, + "os": [ + "win32" + ], + "engines": { + "node": ">=12" + } + }, + "node_modules/@esbuild/win32-ia32": { + "version": "0.18.20", + "resolved": "https://registry.npmjs.org/@esbuild/win32-ia32/-/win32-ia32-0.18.20.tgz", + "integrity": "sha512-Wv7QBi3ID/rROT08SABTS7eV4hX26sVduqDOTe1MvGMjNd3EjOz4b7zeexIR62GTIEKrfJXKL9LFxTYgkyeu7g==", + "cpu": [ + "ia32" + ], + "dev": true, + "optional": true, + "os": [ + "win32" + ], + "engines": { + "node": ">=12" + } + }, + "node_modules/@esbuild/win32-x64": { + "version": "0.18.20", + "resolved": "https://registry.npmjs.org/@esbuild/win32-x64/-/win32-x64-0.18.20.tgz", + "integrity": "sha512-kTdfRcSiDfQca/y9QIkng02avJ+NCaQvrMejlsB3RRv5sE9rRoeBPISaZpKxHELzRxZyLvNts1P27W3wV+8geQ==", + "cpu": [ + "x64" + ], + "dev": true, + "optional": true, + "os": [ + "win32" + ], + "engines": { + "node": ">=12" + } + }, "node_modules/@eslint-community/eslint-utils": { "version": "4.4.0", "resolved": "https://registry.npmjs.org/@eslint-community/eslint-utils/-/eslint-utils-4.4.0.tgz", @@ -277,23 +624,23 @@ } }, "node_modules/@eslint-community/regexpp": { - "version": "4.5.0", - "resolved": "https://registry.npmjs.org/@eslint-community/regexpp/-/regexpp-4.5.0.tgz", - "integrity": "sha512-vITaYzIcNmjn5tF5uxcZ/ft7/RXGrMUIS9HalWckEOF6ESiwXKoMzAQf2UW0aVd6rnOeExTJVd5hmWXucBKGXQ==", + "version": "4.6.2", + "resolved": "https://registry.npmjs.org/@eslint-community/regexpp/-/regexpp-4.6.2.tgz", + "integrity": "sha512-pPTNuaAG3QMH+buKyBIGJs3g/S5y0caxw0ygM3YyE6yJFySwiGGSzA+mM3KJ8QQvzeLh3blwgSonkFjgQdxzMw==", "dev": true, "engines": { "node": "^12.0.0 || ^14.0.0 || >=16.0.0" } }, "node_modules/@eslint/eslintrc": { - "version": "2.0.3", - "resolved": "https://registry.npmjs.org/@eslint/eslintrc/-/eslintrc-2.0.3.tgz", - "integrity": "sha512-+5gy6OQfk+xx3q0d6jGZZC3f3KzAkXc/IanVxd1is/VIIziRqqt3ongQz0FiTUXqTk0c7aDB3OaFuKnuSoJicQ==", + "version": "2.1.1", + "resolved": "https://registry.npmjs.org/@eslint/eslintrc/-/eslintrc-2.1.1.tgz", + "integrity": "sha512-9t7ZA7NGGK8ckelF0PQCfcxIUzs1Md5rrO6U/c+FIQNanea5UZC0wqKXH4vHBccmu4ZJgZ2idtPeW7+Q2npOEA==", "dev": true, "dependencies": { "ajv": "^6.12.4", "debug": "^4.3.2", - "espree": "^9.5.2", + "espree": "^9.6.0", "globals": "^13.19.0", "ignore": "^5.2.0", "import-fresh": "^3.2.1", @@ -309,9 +656,9 @@ } }, "node_modules/@eslint/js": { - "version": "8.43.0", - "resolved": "https://registry.npmjs.org/@eslint/js/-/js-8.43.0.tgz", - "integrity": "sha512-s2UHCoiXfxMvmfzqoN+vrQ84ahUSYde9qNO1MdxmoEhyHWsfmwOpFlwYV+ePJEVc7gFnATGUi376WowX1N7tFg==", + "version": "8.46.0", + "resolved": "https://registry.npmjs.org/@eslint/js/-/js-8.46.0.tgz", + "integrity": "sha512-a8TLtmPi8xzPkCbp/OGFUo5yhRkHM2Ko9kOWP4znJr0WAhWyThaw3PnwX4vOTWOAMsV2uRt32PPDcEz63esSaA==", "dev": true, "engines": { "node": "^12.22.0 || ^14.17.0 || >=16.0.0" @@ -362,6 +709,18 @@ "google-protobuf": "^3.14.0" } }, + "node_modules/@jest/schemas": { + "version": "29.6.0", + "resolved": "https://registry.npmjs.org/@jest/schemas/-/schemas-29.6.0.tgz", + "integrity": "sha512-rxLjXyJBTL4LQeJW3aKo0M/+GkCOXsO+8i9Iu7eDb6KwtP65ayoDsitrdPBtujxQ88k4wI2FNYfa6TOGwSn6cQ==", + "dev": true, + "dependencies": { + "@sinclair/typebox": "^0.27.8" + }, + "engines": { + "node": "^14.15.0 || ^16.10.0 || >=18.0.0" + } + }, "node_modules/@jridgewell/gen-mapping": { "version": "0.3.2", "resolved": "https://registry.npmjs.org/@jridgewell/gen-mapping/-/gen-mapping-0.3.2.tgz", @@ -493,15 +852,13 @@ } }, "node_modules/@maplibre/maplibre-gl-style-spec": { - "version": "19.2.1", - "resolved": "https://registry.npmjs.org/@maplibre/maplibre-gl-style-spec/-/maplibre-gl-style-spec-19.2.1.tgz", - "integrity": "sha512-ZVT5QlkVhlxlPav+ca0NO3Moc7EzbHDO2FXW4ic3Q0Vm+TDUw9I8A2EBws7xUUQZf7HQB3kQ+3Jsh5mFLRD4GQ==", + "version": "19.3.0", + "resolved": "https://registry.npmjs.org/@maplibre/maplibre-gl-style-spec/-/maplibre-gl-style-spec-19.3.0.tgz", + "integrity": "sha512-ZbhX9CTV+Z7vHwkRIasDOwTSzr76e8Q6a55RMsAibjyX6+P0ZNL1qAKNzOjjBDP3+aEfNMl7hHo5knuY6pTAUQ==", "dev": true, "dependencies": { "@mapbox/jsonlint-lines-primitives": "~2.0.2", - "@mapbox/point-geometry": "^0.1.0", "@mapbox/unitbezier": "^0.0.1", - "@types/mapbox__point-geometry": "^0.1.2", "json-stringify-pretty-compact": "^3.0.0", "minimist": "^1.2.8", "rw": "^1.3.3", @@ -554,18 +911,24 @@ "node": ">= 8" } }, + "node_modules/@sinclair/typebox": { + "version": "0.27.8", + "resolved": "https://registry.npmjs.org/@sinclair/typebox/-/typebox-0.27.8.tgz", + "integrity": "sha512-+Fj43pSMwJs4KRrH/938Uf+uAELIgVBmQzg/q1YG10djyfA3TnrU8N8XzqCh/okZdszqBQTZf96idMfE5lnwTA==", + "dev": true + }, "node_modules/@sveltejs/vite-plugin-svelte": { - "version": "2.4.2", - "resolved": "https://registry.npmjs.org/@sveltejs/vite-plugin-svelte/-/vite-plugin-svelte-2.4.2.tgz", - "integrity": "sha512-ePfcC48ftMKhkT0OFGdOyycYKnnkT6i/buzey+vHRTR/JpQvuPzzhf1PtKqCDQfJRgoPSN2vscXs6gLigx/zGw==", + "version": "2.4.4", + "resolved": "https://registry.npmjs.org/@sveltejs/vite-plugin-svelte/-/vite-plugin-svelte-2.4.4.tgz", + "integrity": "sha512-Q5z7+iIjs3sw/Jquxaa9KSY5/MShboNjvsxnQYRMdREx/SBDmEYTjeXenpMBh6k0IQ3tMKESCiwKq3/TeAQ8Og==", "dev": true, "dependencies": { "@sveltejs/vite-plugin-svelte-inspector": "^1.0.3", "debug": "^4.3.4", "deepmerge": "^4.3.1", "kleur": "^4.1.5", - "magic-string": "^0.30.0", - "svelte-hmr": "^0.15.2", + "magic-string": "^0.30.2", + "svelte-hmr": "^0.15.3", "vitefu": "^0.2.4" }, "engines": { @@ -594,19 +957,27 @@ } }, "node_modules/@threlte/core": { - "version": "6.0.0-next.8", - "resolved": "https://registry.npmjs.org/@threlte/core/-/core-6.0.0-next.8.tgz", - "integrity": "sha512-EBjy6tBoJXtUnog7Y/qle8foIE4whYbKwkbYIC1AVYqxiQRB13Pz+RgKUoq+vnzMq/yFRFZRV5bJ+BEwSX4rIQ==", - "dev": true + "version": "6.0.3", + "resolved": "https://registry.npmjs.org/@threlte/core/-/core-6.0.3.tgz", + "integrity": "sha512-5fTK+iQGeXpqU3HVKp6CkWIXa1KlUxarvChQU+GhU1ScgmqFn791S0fQsFammrufqNLEWzpG8IG0q1iE7fpQkQ==", + "dev": true, + "peerDependencies": { + "svelte": ">=4", + "three": ">=0.133" + } }, "node_modules/@threlte/extras": { - "version": "5.0.0-next.13", - "resolved": "https://registry.npmjs.org/@threlte/extras/-/extras-5.0.0-next.13.tgz", - "integrity": "sha512-MpjYAO5MHW3lCG9JDQbzEh925od0u+X9xlA6hsVEOlNNOSBvccJVEGRYLC87A88t8AABhLbnF9pDvX/bwvAZOA==", + "version": "5.1.0", + "resolved": "https://registry.npmjs.org/@threlte/extras/-/extras-5.1.0.tgz", + "integrity": "sha512-Cn7VdVt1/X8j7aeKCkoiRVcysEujPexR1mnzSckAzvMkMdm26J21mTKCV3T62aGahYrvXusDw/fHUdgD/jCajg==", "dev": true, "dependencies": { - "lodash": "^4.17.21", - "troika-three-text": "^0.46.4" + "lodash-es": "^4.17.21", + "troika-three-text": "^0.47.2" + }, + "peerDependencies": { + "svelte": ">=4", + "three": ">=0.133" } }, "node_modules/@tweenjs/tween.js": { @@ -678,9 +1049,9 @@ } }, "node_modules/@types/node": { - "version": "14.18.32", - "resolved": "https://registry.npmjs.org/@types/node/-/node-14.18.32.tgz", - "integrity": "sha512-Y6S38pFr04yb13qqHf8uk1nHE3lXgQ30WZbv1mLliV9pt0NjvqdWttLcrOYLnXbOafknVYRHZGoMSpR9UwfYow==", + "version": "16.18.40", + "resolved": "https://registry.npmjs.org/@types/node/-/node-16.18.40.tgz", + "integrity": "sha512-+yno3ItTEwGxXiS/75Q/aHaa5srkpnJaH+kdkTVJ3DtJEwv92itpKbxU+FjPoh2m/5G9zmUQfrL4A4C13c+iGA==", "dev": true }, "node_modules/@types/normalize-package-data": { @@ -725,17 +1096,27 @@ "integrity": "sha512-9w+a7bR8PeB0dCT/HBULU2fMqf6BAzvKbxFboYhmDtDkKPiyXYbjoe2auwsXlEFI7CFNMF1dCv3dFH5Poy9R1w==", "dev": true }, + "node_modules/@types/supercluster": { + "version": "7.1.0", + "resolved": "https://registry.npmjs.org/@types/supercluster/-/supercluster-7.1.0.tgz", + "integrity": "sha512-6JapQ2GmEkH66r23BK49I+u6zczVDGTtiJEVvKDYZVSm/vepWaJuTq6BXzJ6I4agG5s8vA1KM7m/gXWDg03O4Q==", + "dev": true, + "dependencies": { + "@types/geojson": "*" + } + }, "node_modules/@types/three": { - "version": "0.152.1", - "resolved": "https://registry.npmjs.org/@types/three/-/three-0.152.1.tgz", - "integrity": "sha512-PMOCQnx9JRmq+2OUGTPoY9h1hTWD2L7/nmuW/SyNq1Vbq3Lwt3MNdl3wYSa4DvLTGv62NmIXD9jYdAOwohwJyw==", + "version": "0.155.0", + "resolved": "https://registry.npmjs.org/@types/three/-/three-0.155.0.tgz", + "integrity": "sha512-IzdbqXsGsbG0flvq9D5L9pZRwySQQps2bGcizLYEsfvK3dM+B0sqKR6S+xAOXbouXemfDmHttrcQjVOM46YnAw==", "dev": true, "dependencies": { "@tweenjs/tween.js": "~18.6.4", "@types/stats.js": "*", "@types/webxr": "*", "fflate": "~0.6.9", - "lil-gui": "~0.17.0" + "lil-gui": "~0.17.0", + "meshoptimizer": "~0.18.1" } }, "node_modules/@types/webxr": { @@ -755,32 +1136,34 @@ } }, "node_modules/@typescript-eslint/eslint-plugin": { - "version": "5.60.1", - "resolved": "https://registry.npmjs.org/@typescript-eslint/eslint-plugin/-/eslint-plugin-5.60.1.tgz", - "integrity": "sha512-KSWsVvsJsLJv3c4e73y/Bzt7OpqMCADUO846bHcuWYSYM19bldbAeDv7dYyV0jwkbMfJ2XdlzwjhXtuD7OY6bw==", + "version": "6.3.0", + "resolved": "https://registry.npmjs.org/@typescript-eslint/eslint-plugin/-/eslint-plugin-6.3.0.tgz", + "integrity": "sha512-IZYjYZ0ifGSLZbwMqIip/nOamFiWJ9AH+T/GYNZBWkVcyNQOFGtSMoWV7RvY4poYCMZ/4lHzNl796WOSNxmk8A==", "dev": true, "dependencies": { - "@eslint-community/regexpp": "^4.4.0", - "@typescript-eslint/scope-manager": "5.60.1", - "@typescript-eslint/type-utils": "5.60.1", - "@typescript-eslint/utils": "5.60.1", + "@eslint-community/regexpp": "^4.5.1", + "@typescript-eslint/scope-manager": "6.3.0", + "@typescript-eslint/type-utils": "6.3.0", + "@typescript-eslint/utils": "6.3.0", + "@typescript-eslint/visitor-keys": "6.3.0", "debug": "^4.3.4", - "grapheme-splitter": "^1.0.4", - "ignore": "^5.2.0", + "graphemer": "^1.4.0", + "ignore": "^5.2.4", + "natural-compare": "^1.4.0", "natural-compare-lite": "^1.4.0", - "semver": "^7.3.7", - "tsutils": "^3.21.0" + "semver": "^7.5.4", + "ts-api-utils": "^1.0.1" }, "engines": { - "node": "^12.22.0 || ^14.17.0 || >=16.0.0" + "node": "^16.0.0 || >=18.0.0" }, "funding": { "type": "opencollective", "url": "https://opencollective.com/typescript-eslint" }, "peerDependencies": { - "@typescript-eslint/parser": "^5.0.0", - "eslint": "^6.0.0 || ^7.0.0 || ^8.0.0" + "@typescript-eslint/parser": "^6.0.0 || ^6.0.0-alpha", + "eslint": "^7.0.0 || ^8.0.0" }, "peerDependenciesMeta": { "typescript": { @@ -789,25 +1172,26 @@ } }, "node_modules/@typescript-eslint/parser": { - "version": "5.60.1", - "resolved": "https://registry.npmjs.org/@typescript-eslint/parser/-/parser-5.60.1.tgz", - "integrity": "sha512-pHWlc3alg2oSMGwsU/Is8hbm3XFbcrb6P5wIxcQW9NsYBfnrubl/GhVVD/Jm/t8HXhA2WncoIRfBtnCgRGV96Q==", + "version": "6.3.0", + "resolved": "https://registry.npmjs.org/@typescript-eslint/parser/-/parser-6.3.0.tgz", + "integrity": "sha512-ibP+y2Gr6p0qsUkhs7InMdXrwldjxZw66wpcQq9/PzAroM45wdwyu81T+7RibNCh8oc0AgrsyCwJByncY0Ongg==", "dev": true, "dependencies": { - "@typescript-eslint/scope-manager": "5.60.1", - "@typescript-eslint/types": "5.60.1", - "@typescript-eslint/typescript-estree": "5.60.1", + "@typescript-eslint/scope-manager": "6.3.0", + "@typescript-eslint/types": "6.3.0", + "@typescript-eslint/typescript-estree": "6.3.0", + "@typescript-eslint/visitor-keys": "6.3.0", "debug": "^4.3.4" }, "engines": { - "node": "^12.22.0 || ^14.17.0 || >=16.0.0" + "node": "^16.0.0 || >=18.0.0" }, "funding": { "type": "opencollective", "url": "https://opencollective.com/typescript-eslint" }, "peerDependencies": { - "eslint": "^6.0.0 || ^7.0.0 || ^8.0.0" + "eslint": "^7.0.0 || ^8.0.0" }, "peerDependenciesMeta": { "typescript": { @@ -816,16 +1200,16 @@ } }, "node_modules/@typescript-eslint/scope-manager": { - "version": "5.60.1", - "resolved": "https://registry.npmjs.org/@typescript-eslint/scope-manager/-/scope-manager-5.60.1.tgz", - "integrity": "sha512-Dn/LnN7fEoRD+KspEOV0xDMynEmR3iSHdgNsarlXNLGGtcUok8L4N71dxUgt3YvlO8si7E+BJ5Fe3wb5yUw7DQ==", + "version": "6.3.0", + "resolved": "https://registry.npmjs.org/@typescript-eslint/scope-manager/-/scope-manager-6.3.0.tgz", + "integrity": "sha512-WlNFgBEuGu74ahrXzgefiz/QlVb+qg8KDTpknKwR7hMH+lQygWyx0CQFoUmMn1zDkQjTBBIn75IxtWss77iBIQ==", "dev": true, "dependencies": { - "@typescript-eslint/types": "5.60.1", - "@typescript-eslint/visitor-keys": "5.60.1" + "@typescript-eslint/types": "6.3.0", + "@typescript-eslint/visitor-keys": "6.3.0" }, "engines": { - "node": "^12.22.0 || ^14.17.0 || >=16.0.0" + "node": "^16.0.0 || >=18.0.0" }, "funding": { "type": "opencollective", @@ -833,25 +1217,25 @@ } }, "node_modules/@typescript-eslint/type-utils": { - "version": "5.60.1", - "resolved": "https://registry.npmjs.org/@typescript-eslint/type-utils/-/type-utils-5.60.1.tgz", - "integrity": "sha512-vN6UztYqIu05nu7JqwQGzQKUJctzs3/Hg7E2Yx8rz9J+4LgtIDFWjjl1gm3pycH0P3mHAcEUBd23LVgfrsTR8A==", + "version": "6.3.0", + "resolved": "https://registry.npmjs.org/@typescript-eslint/type-utils/-/type-utils-6.3.0.tgz", + "integrity": "sha512-7Oj+1ox1T2Yc8PKpBvOKWhoI/4rWFd1j7FA/rPE0lbBPXTKjdbtC+7Ev0SeBjEKkIhKWVeZSP+mR7y1Db1CdfQ==", "dev": true, "dependencies": { - "@typescript-eslint/typescript-estree": "5.60.1", - "@typescript-eslint/utils": "5.60.1", + "@typescript-eslint/typescript-estree": "6.3.0", + "@typescript-eslint/utils": "6.3.0", "debug": "^4.3.4", - "tsutils": "^3.21.0" + "ts-api-utils": "^1.0.1" }, "engines": { - "node": "^12.22.0 || ^14.17.0 || >=16.0.0" + "node": "^16.0.0 || >=18.0.0" }, "funding": { "type": "opencollective", "url": "https://opencollective.com/typescript-eslint" }, "peerDependencies": { - "eslint": "*" + "eslint": "^7.0.0 || ^8.0.0" }, "peerDependenciesMeta": { "typescript": { @@ -860,12 +1244,12 @@ } }, "node_modules/@typescript-eslint/types": { - "version": "5.60.1", - "resolved": "https://registry.npmjs.org/@typescript-eslint/types/-/types-5.60.1.tgz", - "integrity": "sha512-zDcDx5fccU8BA0IDZc71bAtYIcG9PowaOwaD8rjYbqwK7dpe/UMQl3inJ4UtUK42nOCT41jTSCwg76E62JpMcg==", + "version": "6.3.0", + "resolved": "https://registry.npmjs.org/@typescript-eslint/types/-/types-6.3.0.tgz", + "integrity": "sha512-K6TZOvfVyc7MO9j60MkRNWyFSf86IbOatTKGrpTQnzarDZPYPVy0oe3myTMq7VjhfsUAbNUW8I5s+2lZvtx1gg==", "dev": true, "engines": { - "node": "^12.22.0 || ^14.17.0 || >=16.0.0" + "node": "^16.0.0 || >=18.0.0" }, "funding": { "type": "opencollective", @@ -873,21 +1257,21 @@ } }, "node_modules/@typescript-eslint/typescript-estree": { - "version": "5.60.1", - "resolved": "https://registry.npmjs.org/@typescript-eslint/typescript-estree/-/typescript-estree-5.60.1.tgz", - "integrity": "sha512-hkX70J9+2M2ZT6fhti5Q2FoU9zb+GeZK2SLP1WZlvUDqdMbEKhexZODD1WodNRyO8eS+4nScvT0dts8IdaBzfw==", + "version": "6.3.0", + "resolved": "https://registry.npmjs.org/@typescript-eslint/typescript-estree/-/typescript-estree-6.3.0.tgz", + "integrity": "sha512-Xh4NVDaC4eYKY4O3QGPuQNp5NxBAlEvNQYOqJquR2MePNxO11E5K3t5x4M4Mx53IZvtpW+mBxIT0s274fLUocg==", "dev": true, "dependencies": { - "@typescript-eslint/types": "5.60.1", - "@typescript-eslint/visitor-keys": "5.60.1", + "@typescript-eslint/types": "6.3.0", + "@typescript-eslint/visitor-keys": "6.3.0", "debug": "^4.3.4", "globby": "^11.1.0", "is-glob": "^4.0.3", - "semver": "^7.3.7", - "tsutils": "^3.21.0" + "semver": "^7.5.4", + "ts-api-utils": "^1.0.1" }, "engines": { - "node": "^12.22.0 || ^14.17.0 || >=16.0.0" + "node": "^16.0.0 || >=18.0.0" }, "funding": { "type": "opencollective", @@ -900,42 +1284,41 @@ } }, "node_modules/@typescript-eslint/utils": { - "version": "5.60.1", - "resolved": "https://registry.npmjs.org/@typescript-eslint/utils/-/utils-5.60.1.tgz", - "integrity": "sha512-tiJ7FFdFQOWssFa3gqb94Ilexyw0JVxj6vBzaSpfN/8IhoKkDuSAenUKvsSHw2A/TMpJb26izIszTXaqygkvpQ==", + "version": "6.3.0", + "resolved": "https://registry.npmjs.org/@typescript-eslint/utils/-/utils-6.3.0.tgz", + "integrity": "sha512-hLLg3BZE07XHnpzglNBG8P/IXq/ZVXraEbgY7FM0Cnc1ehM8RMdn9mat3LubJ3KBeYXXPxV1nugWbQPjGeJk6Q==", "dev": true, "dependencies": { - "@eslint-community/eslint-utils": "^4.2.0", - "@types/json-schema": "^7.0.9", - "@types/semver": "^7.3.12", - "@typescript-eslint/scope-manager": "5.60.1", - "@typescript-eslint/types": "5.60.1", - "@typescript-eslint/typescript-estree": "5.60.1", - "eslint-scope": "^5.1.1", - "semver": "^7.3.7" + "@eslint-community/eslint-utils": "^4.4.0", + "@types/json-schema": "^7.0.12", + "@types/semver": "^7.5.0", + "@typescript-eslint/scope-manager": "6.3.0", + "@typescript-eslint/types": "6.3.0", + "@typescript-eslint/typescript-estree": "6.3.0", + "semver": "^7.5.4" }, "engines": { - "node": "^12.22.0 || ^14.17.0 || >=16.0.0" + "node": "^16.0.0 || >=18.0.0" }, "funding": { "type": "opencollective", "url": "https://opencollective.com/typescript-eslint" }, "peerDependencies": { - "eslint": "^6.0.0 || ^7.0.0 || ^8.0.0" + "eslint": "^7.0.0 || ^8.0.0" } }, "node_modules/@typescript-eslint/visitor-keys": { - "version": "5.60.1", - "resolved": "https://registry.npmjs.org/@typescript-eslint/visitor-keys/-/visitor-keys-5.60.1.tgz", - "integrity": "sha512-xEYIxKcultP6E/RMKqube11pGjXH1DCo60mQoWhVYyKfLkwbIVVjYxmOenNMxILx0TjCujPTjjnTIVzm09TXIw==", + "version": "6.3.0", + "resolved": "https://registry.npmjs.org/@typescript-eslint/visitor-keys/-/visitor-keys-6.3.0.tgz", + "integrity": "sha512-kEhRRj7HnvaSjux1J9+7dBen15CdWmDnwrpyiHsFX6Qx2iW5LOBUgNefOFeh2PjWPlNwN8TOn6+4eBU3J/gupw==", "dev": true, "dependencies": { - "@typescript-eslint/types": "5.60.1", - "eslint-visitor-keys": "^3.3.0" + "@typescript-eslint/types": "6.3.0", + "eslint-visitor-keys": "^3.4.1" }, "engines": { - "node": "^12.22.0 || ^14.17.0 || >=16.0.0" + "node": "^16.0.0 || >=18.0.0" }, "funding": { "type": "opencollective", @@ -943,9 +1326,9 @@ } }, "node_modules/@viamrobotics/prime": { - "version": "0.2.19", - "resolved": "https://registry.npmjs.org/@viamrobotics/prime/-/prime-0.2.19.tgz", - "integrity": "sha512-YCikn/y62VjaLwaH7qt0Vlj52GyIjCrXF8/4rtE4260xG2IPsZMdVdgGueiORg1R3hiixfx+sZL8q+tbERQvZA==", + "version": "0.5.2", + "resolved": "https://registry.npmjs.org/@viamrobotics/prime/-/prime-0.5.2.tgz", + "integrity": "sha512-Wf250FCtsxZGUZX+RYi8xhmhMexu+ZS3eMYrqkyqvGknDgw/Nb5UVT9vWBaLEDXUDpIMsbzy9IiNk1UwRiI/hA==", "dev": true }, "node_modules/@viamrobotics/rpc": { @@ -971,32 +1354,41 @@ } }, "node_modules/@viamrobotics/sdk": { - "version": "0.2.2", - "resolved": "https://registry.npmjs.org/@viamrobotics/sdk/-/sdk-0.2.2.tgz", - "integrity": "sha512-U/1xq43t6QnOn90LlR3dlv8fJJHvXM/cGjJPG1AUD7FPV9gS+QRxjs9rQ6zZRg51EhAGa/jmqX2e/+br+9FZAw==", + "version": "0.3.2-rc.0", + "resolved": "https://registry.npmjs.org/@viamrobotics/sdk/-/sdk-0.3.2-rc.0.tgz", + "integrity": "sha512-5MbP2K44aewwyvaGvYVJ/O+xWW+1PKBhRMifwT0BUyGUMq5BL5hi8Wxmnb3LZB6/X/j/tdh2Jk1PpLWpJFUwaQ==", "dev": true, "dependencies": { "@viamrobotics/rpc": "^0.1.37", "exponential-backoff": "^3.1.1" } }, + "node_modules/@viamrobotics/three": { + "version": "0.0.2", + "resolved": "https://registry.npmjs.org/@viamrobotics/three/-/three-0.0.2.tgz", + "integrity": "sha512-pRRwtVfY1kcaymwSs+o3Lpc9RBvWOGwhb5RPlbYgYK994N3oHSWwWG1PpBeuBGkwqM6hLCcTRVp8+SLQOHy+Ag==", + "dev": true, + "peerDependencies": { + "three": "*" + } + }, "node_modules/@viamrobotics/typescript-config": { - "version": "0.0.3", - "resolved": "https://registry.npmjs.org/@viamrobotics/typescript-config/-/typescript-config-0.0.3.tgz", - "integrity": "sha512-SEsauLS3OYbrK3k+eMMgouh2BQyi4Nk6yUCbn0aPNHPzSD+MszT+mKsdeAscrcnnjfWFR9cFQ6aadr0d753+3w==", + "version": "0.0.4", + "resolved": "https://registry.npmjs.org/@viamrobotics/typescript-config/-/typescript-config-0.0.4.tgz", + "integrity": "sha512-WJE6gpiu7KdvMe6nXEbyqmVTOzRUrj0BufISTtPLHCviGsehOyTWT/BuY7Eu91PVa187ML44QXMdFe2xlUlNyA==", "dev": true, "peerDependencies": { "typescript": ">=5 <6" } }, "node_modules/@vitest/expect": { - "version": "0.32.2", - "resolved": "https://registry.npmjs.org/@vitest/expect/-/expect-0.32.2.tgz", - "integrity": "sha512-6q5yzweLnyEv5Zz1fqK5u5E83LU+gOMVBDuxBl2d2Jfx1BAp5M+rZgc5mlyqdnxquyoiOXpXmFNkcGcfFnFH3Q==", + "version": "0.34.1", + "resolved": "https://registry.npmjs.org/@vitest/expect/-/expect-0.34.1.tgz", + "integrity": "sha512-q2CD8+XIsQ+tHwypnoCk8Mnv5e6afLFvinVGCq3/BOT4kQdVQmY6rRfyKkwcg635lbliLPqbunXZr+L1ssUWiQ==", "dev": true, "dependencies": { - "@vitest/spy": "0.32.2", - "@vitest/utils": "0.32.2", + "@vitest/spy": "0.34.1", + "@vitest/utils": "0.34.1", "chai": "^4.3.7" }, "funding": { @@ -1004,15 +1396,14 @@ } }, "node_modules/@vitest/runner": { - "version": "0.32.2", - "resolved": "https://registry.npmjs.org/@vitest/runner/-/runner-0.32.2.tgz", - "integrity": "sha512-06vEL0C1pomOEktGoLjzZw+1Fb+7RBRhmw/06WkDrd1akkT9i12su0ku+R/0QM69dfkIL/rAIDTG+CSuQVDcKw==", + "version": "0.34.1", + "resolved": "https://registry.npmjs.org/@vitest/runner/-/runner-0.34.1.tgz", + "integrity": "sha512-YfQMpYzDsYB7yqgmlxZ06NI4LurHWfrH7Wy3Pvf/z/vwUSgq1zLAb1lWcItCzQG+NVox+VvzlKQrYEXb47645g==", "dev": true, "dependencies": { - "@vitest/utils": "0.32.2", - "concordance": "^5.0.4", + "@vitest/utils": "0.34.1", "p-limit": "^4.0.0", - "pathe": "^1.1.0" + "pathe": "^1.1.1" }, "funding": { "url": "https://opencollective.com/vitest" @@ -1046,40 +1437,40 @@ } }, "node_modules/@vitest/snapshot": { - "version": "0.32.2", - "resolved": "https://registry.npmjs.org/@vitest/snapshot/-/snapshot-0.32.2.tgz", - "integrity": "sha512-JwhpeH/PPc7GJX38vEfCy9LtRzf9F4er7i4OsAJyV7sjPwjj+AIR8cUgpMTWK4S3TiamzopcTyLsZDMuldoi5A==", + "version": "0.34.1", + "resolved": "https://registry.npmjs.org/@vitest/snapshot/-/snapshot-0.34.1.tgz", + "integrity": "sha512-0O9LfLU0114OqdF8lENlrLsnn024Tb1CsS9UwG0YMWY2oGTQfPtkW+B/7ieyv0X9R2Oijhi3caB1xgGgEgclSQ==", "dev": true, "dependencies": { - "magic-string": "^0.30.0", - "pathe": "^1.1.0", - "pretty-format": "^27.5.1" + "magic-string": "^0.30.1", + "pathe": "^1.1.1", + "pretty-format": "^29.5.0" }, "funding": { "url": "https://opencollective.com/vitest" } }, "node_modules/@vitest/spy": { - "version": "0.32.2", - "resolved": "https://registry.npmjs.org/@vitest/spy/-/spy-0.32.2.tgz", - "integrity": "sha512-Q/ZNILJ4ca/VzQbRM8ur3Si5Sardsh1HofatG9wsJY1RfEaw0XKP8IVax2lI1qnrk9YPuG9LA2LkZ0EI/3d4ug==", + "version": "0.34.1", + "resolved": "https://registry.npmjs.org/@vitest/spy/-/spy-0.34.1.tgz", + "integrity": "sha512-UT4WcI3EAPUNO8n6y9QoEqynGGEPmmRxC+cLzneFFXpmacivjHZsNbiKD88KUScv5DCHVDgdBsLD7O7s1enFcQ==", "dev": true, "dependencies": { - "tinyspy": "^2.1.0" + "tinyspy": "^2.1.1" }, "funding": { "url": "https://opencollective.com/vitest" } }, "node_modules/@vitest/utils": { - "version": "0.32.2", - "resolved": "https://registry.npmjs.org/@vitest/utils/-/utils-0.32.2.tgz", - "integrity": "sha512-lnJ0T5i03j0IJaeW73hxe2AuVnZ/y1BhhCOuIcl9LIzXnbpXJT9Lrt6brwKHXLOiA7MZ6N5hSJjt0xE1dGNCzQ==", + "version": "0.34.1", + "resolved": "https://registry.npmjs.org/@vitest/utils/-/utils-0.34.1.tgz", + "integrity": "sha512-/ql9dsFi4iuEbiNcjNHQWXBum7aL8pyhxvfnD9gNtbjR9fUKAjxhj4AA3yfLXg6gJpMGGecvtF8Au2G9y3q47Q==", "dev": true, "dependencies": { "diff-sequences": "^29.4.3", "loupe": "^2.3.6", - "pretty-format": "^27.5.1" + "pretty-format": "^29.5.0" }, "funding": { "url": "https://opencollective.com/vitest" @@ -1311,7 +1702,26 @@ "integrity": "sha512-HGyxoOTYUyCM6stUe6EJgnd4EoewAI7zMdfqO+kGjnlZmBDz/cR5pf8r/cR4Wq60sL/p0IkcjUEEPwS3GFrIyw==", "dev": true, "engines": { - "node": ">=8" + "node": ">=8" + } + }, + "node_modules/array.prototype.findlastindex": { + "version": "1.2.2", + "resolved": "https://registry.npmjs.org/array.prototype.findlastindex/-/array.prototype.findlastindex-1.2.2.tgz", + "integrity": "sha512-tb5thFFlUcp7NdNF6/MpDk/1r/4awWG1FIz3YqDf+/zJSTezBb+/5WViH41obXULHVpDzoiCLpJ/ZO9YbJMsdw==", + "dev": true, + "dependencies": { + "call-bind": "^1.0.2", + "define-properties": "^1.1.4", + "es-abstract": "^1.20.4", + "es-shim-unscopables": "^1.0.0", + "get-intrinsic": "^1.1.3" + }, + "engines": { + "node": ">= 0.4" + }, + "funding": { + "url": "https://github.com/sponsors/ljharb" } }, "node_modules/array.prototype.flat": { @@ -1438,9 +1848,9 @@ } }, "node_modules/aws4": { - "version": "1.11.0", - "resolved": "https://registry.npmjs.org/aws4/-/aws4-1.11.0.tgz", - "integrity": "sha512-xh1Rl34h6Fi1DC2WWKfxUTVqRsNnr6LsKz2+hfwDxQJWmrx8+c7ylaqBMcHfl1U1r2dsifOvKX3LQuLNZ+XSvA==", + "version": "1.12.0", + "resolved": "https://registry.npmjs.org/aws4/-/aws4-1.12.0.tgz", + "integrity": "sha512-NmWvPnx0F1SfrQbYwOi7OeaNGokp9XhzNioJ/CSBs8Qa4vxug81mhJEAVZwxXuBmYB5KDRfMq/F3RR0BIU7sWg==", "dev": true }, "node_modules/axobject-query": { @@ -1517,12 +1927,6 @@ "integrity": "sha512-XpNj6GDQzdfW+r2Wnn7xiSAd7TM3jzkxGXBGTtWKuSXv1xUV+azxAm8jdWZN06QTQk+2N2XB9jRDkvbmQmcRtg==", "dev": true }, - "node_modules/blueimp-md5": { - "version": "2.19.0", - "resolved": "https://registry.npmjs.org/blueimp-md5/-/blueimp-md5-2.19.0.tgz", - "integrity": "sha512-DRQrD6gJyy8FbiE4s+bDoXS9hiW3Vbx5uCdwvcCf3zLHL+Iv7LtGHLpr+GZV8rHG8tK766FGYBwRbu8pELTt+w==", - "dev": true - }, "node_modules/brace-expansion": { "version": "1.1.11", "resolved": "https://registry.npmjs.org/brace-expansion/-/brace-expansion-1.1.11.tgz", @@ -1942,25 +2346,6 @@ "integrity": "sha512-/Srv4dswyQNBfohGpz9o6Yb3Gz3SrUDqBH5rTuhGR7ahtlbYKnVxw2bCFMRljaA7EXHaXZ8wsHdodFvbkhKmqg==", "dev": true }, - "node_modules/concordance": { - "version": "5.0.4", - "resolved": "https://registry.npmjs.org/concordance/-/concordance-5.0.4.tgz", - "integrity": "sha512-OAcsnTEYu1ARJqWVGwf4zh4JDfHZEaSNlNccFmt8YjB2l/n19/PF2viLINHc57vO4FKIAFl2FWASIGZZWZ2Kxw==", - "dev": true, - "dependencies": { - "date-time": "^3.1.0", - "esutils": "^2.0.3", - "fast-diff": "^1.2.0", - "js-string-escape": "^1.0.1", - "lodash": "^4.17.15", - "md5-hex": "^3.0.1", - "semver": "^7.3.2", - "well-known-symbols": "^2.0.0" - }, - "engines": { - "node": ">=10.18.0 <11 || >=12.14.0 <13 || >=14" - } - }, "node_modules/core-util-is": { "version": "1.0.2", "resolved": "https://registry.npmjs.org/core-util-is/-/core-util-is-1.0.2.tgz", @@ -2007,15 +2392,15 @@ } }, "node_modules/cypress": { - "version": "12.16.0", - "resolved": "https://registry.npmjs.org/cypress/-/cypress-12.16.0.tgz", - "integrity": "sha512-mwv1YNe48hm0LVaPgofEhGCtLwNIQEjmj2dJXnAkY1b4n/NE9OtgPph4TyS+tOtYp5CKtRmDvBzWseUXQTjbTg==", + "version": "12.17.3", + "resolved": "https://registry.npmjs.org/cypress/-/cypress-12.17.3.tgz", + "integrity": "sha512-/R4+xdIDjUSLYkiQfwJd630S81KIgicmQOLXotFxVXkl+eTeVO+3bHXxdi5KBh/OgC33HWN33kHX+0tQR/ZWpg==", "dev": true, "hasInstallScript": true, "dependencies": { - "@cypress/request": "^2.88.10", + "@cypress/request": "^2.88.11", "@cypress/xvfb": "^1.2.4", - "@types/node": "^14.14.31", + "@types/node": "^16.18.39", "@types/sinonjs__fake-timers": "8.1.1", "@types/sizzle": "^2.3.2", "arch": "^2.2.0", @@ -2050,7 +2435,7 @@ "pretty-bytes": "^5.6.0", "proxy-from-env": "1.0.0", "request-progress": "^3.0.0", - "semver": "^7.3.2", + "semver": "^7.5.3", "supports-color": "^8.1.1", "tmp": "~0.2.1", "untildify": "^4.0.0", @@ -2075,18 +2460,6 @@ "node": ">=0.10" } }, - "node_modules/date-time": { - "version": "3.1.0", - "resolved": "https://registry.npmjs.org/date-time/-/date-time-3.1.0.tgz", - "integrity": "sha512-uqCUKXE5q1PNBXjPqvwhwJf9SwMoAHBgWJ6DcrnS5o+W2JOiIILl0JEdVD8SGujrNS02GGxgwAg2PN2zONgtjg==", - "dev": true, - "dependencies": { - "time-zone": "^1.0.0" - }, - "engines": { - "node": ">=6" - } - }, "node_modules/dayjs": { "version": "1.11.5", "resolved": "https://registry.npmjs.org/dayjs/-/dayjs-1.11.5.tgz", @@ -2372,9 +2745,9 @@ "dev": true }, "node_modules/esbuild": { - "version": "0.17.15", - "resolved": "https://registry.npmjs.org/esbuild/-/esbuild-0.17.15.tgz", - "integrity": "sha512-LBUV2VsUIc/iD9ME75qhT4aJj0r75abCVS0jakhFzOtR7TQsqQA5w0tZ+KTKnwl3kXE0MhskNdHDh/I5aCR1Zw==", + "version": "0.18.20", + "resolved": "https://registry.npmjs.org/esbuild/-/esbuild-0.18.20.tgz", + "integrity": "sha512-ceqxoedUrcayh7Y7ZX6NdbbDzGROiyVBgC4PriJThBKSVPWnnFHZAkfI1lJT8QFkOwH4qOS2SJkS4wvpGl8BpA==", "dev": true, "hasInstallScript": true, "bin": { @@ -2384,28 +2757,28 @@ "node": ">=12" }, "optionalDependencies": { - "@esbuild/android-arm": "0.17.15", - "@esbuild/android-arm64": "0.17.15", - "@esbuild/android-x64": "0.17.15", - "@esbuild/darwin-arm64": "0.17.15", - "@esbuild/darwin-x64": "0.17.15", - "@esbuild/freebsd-arm64": "0.17.15", - "@esbuild/freebsd-x64": "0.17.15", - "@esbuild/linux-arm": "0.17.15", - "@esbuild/linux-arm64": "0.17.15", - "@esbuild/linux-ia32": "0.17.15", - "@esbuild/linux-loong64": "0.17.15", - "@esbuild/linux-mips64el": "0.17.15", - "@esbuild/linux-ppc64": "0.17.15", - "@esbuild/linux-riscv64": "0.17.15", - "@esbuild/linux-s390x": "0.17.15", - "@esbuild/linux-x64": "0.17.15", - "@esbuild/netbsd-x64": "0.17.15", - "@esbuild/openbsd-x64": "0.17.15", - "@esbuild/sunos-x64": "0.17.15", - "@esbuild/win32-arm64": "0.17.15", - "@esbuild/win32-ia32": "0.17.15", - "@esbuild/win32-x64": "0.17.15" + "@esbuild/android-arm": "0.18.20", + "@esbuild/android-arm64": "0.18.20", + "@esbuild/android-x64": "0.18.20", + "@esbuild/darwin-arm64": "0.18.20", + "@esbuild/darwin-x64": "0.18.20", + "@esbuild/freebsd-arm64": "0.18.20", + "@esbuild/freebsd-x64": "0.18.20", + "@esbuild/linux-arm": "0.18.20", + "@esbuild/linux-arm64": "0.18.20", + "@esbuild/linux-ia32": "0.18.20", + "@esbuild/linux-loong64": "0.18.20", + "@esbuild/linux-mips64el": "0.18.20", + "@esbuild/linux-ppc64": "0.18.20", + "@esbuild/linux-riscv64": "0.18.20", + "@esbuild/linux-s390x": "0.18.20", + "@esbuild/linux-x64": "0.18.20", + "@esbuild/netbsd-x64": "0.18.20", + "@esbuild/openbsd-x64": "0.18.20", + "@esbuild/sunos-x64": "0.18.20", + "@esbuild/win32-arm64": "0.18.20", + "@esbuild/win32-ia32": "0.18.20", + "@esbuild/win32-x64": "0.18.20" } }, "node_modules/escape-string-regexp": { @@ -2421,27 +2794,27 @@ } }, "node_modules/eslint": { - "version": "8.43.0", - "resolved": "https://registry.npmjs.org/eslint/-/eslint-8.43.0.tgz", - "integrity": "sha512-aaCpf2JqqKesMFGgmRPessmVKjcGXqdlAYLLC3THM8t5nBRZRQ+st5WM/hoJXkdioEXLLbXgclUpM0TXo5HX5Q==", + "version": "8.46.0", + "resolved": "https://registry.npmjs.org/eslint/-/eslint-8.46.0.tgz", + "integrity": "sha512-cIO74PvbW0qU8e0mIvk5IV3ToWdCq5FYG6gWPHHkx6gNdjlbAYvtfHmlCMXxjcoVaIdwy/IAt3+mDkZkfvb2Dg==", "dev": true, "dependencies": { "@eslint-community/eslint-utils": "^4.2.0", - "@eslint-community/regexpp": "^4.4.0", - "@eslint/eslintrc": "^2.0.3", - "@eslint/js": "8.43.0", + "@eslint-community/regexpp": "^4.6.1", + "@eslint/eslintrc": "^2.1.1", + "@eslint/js": "^8.46.0", "@humanwhocodes/config-array": "^0.11.10", "@humanwhocodes/module-importer": "^1.0.1", "@nodelib/fs.walk": "^1.2.8", - "ajv": "^6.10.0", + "ajv": "^6.12.4", "chalk": "^4.0.0", "cross-spawn": "^7.0.2", "debug": "^4.3.2", "doctrine": "^3.0.0", "escape-string-regexp": "^4.0.0", - "eslint-scope": "^7.2.0", - "eslint-visitor-keys": "^3.4.1", - "espree": "^9.5.2", + "eslint-scope": "^7.2.2", + "eslint-visitor-keys": "^3.4.2", + "espree": "^9.6.1", "esquery": "^1.4.2", "esutils": "^2.0.2", "fast-deep-equal": "^3.1.3", @@ -2451,7 +2824,6 @@ "globals": "^13.19.0", "graphemer": "^1.4.0", "ignore": "^5.2.0", - "import-fresh": "^3.0.0", "imurmurhash": "^0.1.4", "is-glob": "^4.0.0", "is-path-inside": "^3.0.3", @@ -2461,9 +2833,8 @@ "lodash.merge": "^4.6.2", "minimatch": "^3.1.2", "natural-compare": "^1.4.0", - "optionator": "^0.9.1", + "optionator": "^0.9.3", "strip-ansi": "^6.0.1", - "strip-json-comments": "^3.1.0", "text-table": "^0.2.0" }, "bin": { @@ -2510,9 +2881,9 @@ } }, "node_modules/eslint-module-utils": { - "version": "2.7.4", - "resolved": "https://registry.npmjs.org/eslint-module-utils/-/eslint-module-utils-2.7.4.tgz", - "integrity": "sha512-j4GT+rqzCoRKHwURX7pddtIPGySnX9Si/cgMI5ztrcqOPtk5dDEeZ34CQVPphnqkJytlc97Vuk05Um2mJ3gEQA==", + "version": "2.8.0", + "resolved": "https://registry.npmjs.org/eslint-module-utils/-/eslint-module-utils-2.8.0.tgz", + "integrity": "sha512-aWajIYfsqCKRDgUfjEXNN/JlrzauMuSEy5sbd7WXbtW3EH6A6MpwEh42c7qD+MqQo9QMJ6fWLAeIJynx0g6OAw==", "dev": true, "dependencies": { "debug": "^3.2.7" @@ -2536,26 +2907,29 @@ } }, "node_modules/eslint-plugin-import": { - "version": "2.27.5", - "resolved": "https://registry.npmjs.org/eslint-plugin-import/-/eslint-plugin-import-2.27.5.tgz", - "integrity": "sha512-LmEt3GVofgiGuiE+ORpnvP+kAm3h6MLZJ4Q5HCyHADofsb4VzXFsRiWj3c0OFiV+3DWFh0qg3v9gcPlfc3zRow==", + "version": "2.28.0", + "resolved": "https://registry.npmjs.org/eslint-plugin-import/-/eslint-plugin-import-2.28.0.tgz", + "integrity": "sha512-B8s/n+ZluN7sxj9eUf7/pRFERX0r5bnFA2dCaLHy2ZeaQEAz0k+ZZkFWRFHJAqxfxQDx6KLv9LeIki7cFdwW+Q==", "dev": true, "dependencies": { "array-includes": "^3.1.6", + "array.prototype.findlastindex": "^1.2.2", "array.prototype.flat": "^1.3.1", "array.prototype.flatmap": "^1.3.1", "debug": "^3.2.7", "doctrine": "^2.1.0", "eslint-import-resolver-node": "^0.3.7", - "eslint-module-utils": "^2.7.4", + "eslint-module-utils": "^2.8.0", "has": "^1.0.3", - "is-core-module": "^2.11.0", + "is-core-module": "^2.12.1", "is-glob": "^4.0.3", "minimatch": "^3.1.2", + "object.fromentries": "^2.0.6", + "object.groupby": "^1.0.0", "object.values": "^1.1.6", - "resolve": "^1.22.1", - "semver": "^6.3.0", - "tsconfig-paths": "^3.14.1" + "resolve": "^1.22.3", + "semver": "^6.3.1", + "tsconfig-paths": "^3.14.2" }, "engines": { "node": ">=4" @@ -2586,9 +2960,9 @@ } }, "node_modules/eslint-plugin-import/node_modules/semver": { - "version": "6.3.0", - "resolved": "https://registry.npmjs.org/semver/-/semver-6.3.0.tgz", - "integrity": "sha512-b39TBaTSfV6yBrapU89p5fKekE2m/NwnDocOVruQFS1/veMgdzuPcnOM34M6CwxW8jH/lxEa5rBoDeUwu5HHTw==", + "version": "6.3.1", + "resolved": "https://registry.npmjs.org/semver/-/semver-6.3.1.tgz", + "integrity": "sha512-BR7VvDCVHO+q2xBEWskxS6DJE1qRnb7DxzUrogb71CWoSficBxYsiAGd+Kl0mmq/MprG9yArRkyrQxTO6XjMzA==", "dev": true, "bin": { "semver": "bin/semver.js" @@ -2607,22 +2981,22 @@ } }, "node_modules/eslint-plugin-svelte": { - "version": "2.31.1", - "resolved": "https://registry.npmjs.org/eslint-plugin-svelte/-/eslint-plugin-svelte-2.31.1.tgz", - "integrity": "sha512-08v+DqzHiwIVEbi+266D7+BDhayp9OSqCwa/lHaZlZOlFY0vZLYs/h7SkkUPzA5fTVt8OUJBtvCxFiWEYOvvGg==", + "version": "2.32.4", + "resolved": "https://registry.npmjs.org/eslint-plugin-svelte/-/eslint-plugin-svelte-2.32.4.tgz", + "integrity": "sha512-VJ12i2Iogug1jvhwxSlognnfGj76P5gks/V4pUD4SCSVQOp14u47MNP0zAG8AQR3LT0Fi1iUvIFnY4l9z5Rwbg==", "dev": true, "dependencies": { "@eslint-community/eslint-utils": "^4.2.0", "@jridgewell/sourcemap-codec": "^1.4.14", "debug": "^4.3.1", "esutils": "^2.0.3", - "known-css-properties": "^0.27.0", + "known-css-properties": "^0.28.0", "postcss": "^8.4.5", "postcss-load-config": "^3.1.4", "postcss-safe-parser": "^6.0.0", "postcss-selector-parser": "^6.0.11", "semver": "^7.5.3", - "svelte-eslint-parser": "^0.31.0" + "svelte-eslint-parser": "^0.32.2" }, "engines": { "node": "^14.17.0 || >=16.0.0" @@ -2632,7 +3006,7 @@ }, "peerDependencies": { "eslint": "^7.0.0 || ^8.0.0-0", - "svelte": "^3.37.0 || ^4.0.0-0" + "svelte": "^3.37.0 || ^4.0.0" }, "peerDependenciesMeta": { "svelte": { @@ -2695,12 +3069,12 @@ } }, "node_modules/eslint-plugin-unicorn": { - "version": "47.0.0", - "resolved": "https://registry.npmjs.org/eslint-plugin-unicorn/-/eslint-plugin-unicorn-47.0.0.tgz", - "integrity": "sha512-ivB3bKk7fDIeWOUmmMm9o3Ax9zbMz1Bsza/R2qm46ufw4T6VBFBaJIR1uN3pCKSmSXm8/9Nri8V+iUut1NhQGA==", + "version": "48.0.1", + "resolved": "https://registry.npmjs.org/eslint-plugin-unicorn/-/eslint-plugin-unicorn-48.0.1.tgz", + "integrity": "sha512-FW+4r20myG/DqFcCSzoumaddKBicIPeFnTrifon2mWIzlfyvzwyqZjqVP7m4Cqr/ZYisS2aiLghkUWaPg6vtCw==", "dev": true, "dependencies": { - "@babel/helper-validator-identifier": "^7.19.1", + "@babel/helper-validator-identifier": "^7.22.5", "@eslint-community/eslint-utils": "^4.4.0", "ci-info": "^3.8.0", "clean-regexp": "^1.0.0", @@ -2711,10 +3085,9 @@ "lodash": "^4.17.21", "pluralize": "^8.0.0", "read-pkg-up": "^7.0.1", - "regexp-tree": "^0.1.24", + "regexp-tree": "^0.1.27", "regjsparser": "^0.10.0", - "safe-regex": "^2.1.1", - "semver": "^7.3.8", + "semver": "^7.5.4", "strip-indent": "^3.0.0" }, "engines": { @@ -2724,27 +3097,18 @@ "url": "https://github.com/sindresorhus/eslint-plugin-unicorn?sponsor=1" }, "peerDependencies": { - "eslint": ">=8.38.0" + "eslint": ">=8.44.0" } }, "node_modules/eslint-scope": { - "version": "5.1.1", - "resolved": "https://registry.npmjs.org/eslint-scope/-/eslint-scope-5.1.1.tgz", - "integrity": "sha512-2NxwbF/hZ0KpepYN0cNbo+FN6XoK7GaHlQhgx/hIZl6Va0bF45RQOOwhLIy8lQDbuCiadSLCBnH2CFYquit5bw==", + "version": "7.2.2", + "resolved": "https://registry.npmjs.org/eslint-scope/-/eslint-scope-7.2.2.tgz", + "integrity": "sha512-dOt21O7lTMhDM+X9mB4GX+DZrZtCUJPL/wlcTqxyrx5IvO0IYtILdtrQGQp+8n5S0gwSVmOf9NQrjMOgfQZlIg==", "dev": true, "dependencies": { "esrecurse": "^4.3.0", - "estraverse": "^4.1.1" + "estraverse": "^5.2.0" }, - "engines": { - "node": ">=8.0.0" - } - }, - "node_modules/eslint-visitor-keys": { - "version": "3.4.1", - "resolved": "https://registry.npmjs.org/eslint-visitor-keys/-/eslint-visitor-keys-3.4.1.tgz", - "integrity": "sha512-pZnmmLwYzf+kWaM/Qgrvpen51upAktaaiI01nsJD/Yr3lMOdNtq0cxkrrg16w64VtisN6okbs7Q8AfGqj4c9fA==", - "dev": true, "engines": { "node": "^12.22.0 || ^14.17.0 || >=16.0.0" }, @@ -2752,15 +3116,11 @@ "url": "https://opencollective.com/eslint" } }, - "node_modules/eslint/node_modules/eslint-scope": { - "version": "7.2.0", - "resolved": "https://registry.npmjs.org/eslint-scope/-/eslint-scope-7.2.0.tgz", - "integrity": "sha512-DYj5deGlHBfMt15J7rdtyKNq/Nqlv5KfU4iodrQ019XESsRnwXH9KAE0y3cwtUHDo2ob7CypAnCqefh6vioWRw==", + "node_modules/eslint-visitor-keys": { + "version": "3.4.2", + "resolved": "https://registry.npmjs.org/eslint-visitor-keys/-/eslint-visitor-keys-3.4.2.tgz", + "integrity": "sha512-8drBzUEyZ2llkpCA67iYrgEssKDUu68V8ChqqOfFupIaG/LCVPUT+CoGJpT77zJprs4T/W7p07LP7zAIMuweVw==", "dev": true, - "dependencies": { - "esrecurse": "^4.3.0", - "estraverse": "^5.2.0" - }, "engines": { "node": "^12.22.0 || ^14.17.0 || >=16.0.0" }, @@ -2768,22 +3128,13 @@ "url": "https://opencollective.com/eslint" } }, - "node_modules/eslint/node_modules/estraverse": { - "version": "5.3.0", - "resolved": "https://registry.npmjs.org/estraverse/-/estraverse-5.3.0.tgz", - "integrity": "sha512-MMdARuVEQziNTeJD8DgMqmhwR11BRQ/cBP+pLtYdSTnf3MIO8fFeiINEbX36ZdNlfU/7A9f3gUw49B3oQsvwBA==", - "dev": true, - "engines": { - "node": ">=4.0" - } - }, "node_modules/espree": { - "version": "9.5.2", - "resolved": "https://registry.npmjs.org/espree/-/espree-9.5.2.tgz", - "integrity": "sha512-7OASN1Wma5fum5SrNhFMAMJxOUAbhyfQ8dQ//PJaJbNw0URTPWqIghHWt1MmAANKhHZIYOHruW4Kw4ruUWOdGw==", + "version": "9.6.1", + "resolved": "https://registry.npmjs.org/espree/-/espree-9.6.1.tgz", + "integrity": "sha512-oruZaFkjorTpF32kDSI5/75ViwGeZginGGy2NoOSg3Q9bnwlnmDm4HLnkl0RE3n+njDXR037aY1+x58Z/zFdwQ==", "dev": true, "dependencies": { - "acorn": "^8.8.0", + "acorn": "^8.9.0", "acorn-jsx": "^5.3.2", "eslint-visitor-keys": "^3.4.1" }, @@ -2806,15 +3157,6 @@ "node": ">=0.10" } }, - "node_modules/esquery/node_modules/estraverse": { - "version": "5.3.0", - "resolved": "https://registry.npmjs.org/estraverse/-/estraverse-5.3.0.tgz", - "integrity": "sha512-MMdARuVEQziNTeJD8DgMqmhwR11BRQ/cBP+pLtYdSTnf3MIO8fFeiINEbX36ZdNlfU/7A9f3gUw49B3oQsvwBA==", - "dev": true, - "engines": { - "node": ">=4.0" - } - }, "node_modules/esrecurse": { "version": "4.3.0", "resolved": "https://registry.npmjs.org/esrecurse/-/esrecurse-4.3.0.tgz", @@ -2827,7 +3169,7 @@ "node": ">=4.0" } }, - "node_modules/esrecurse/node_modules/estraverse": { + "node_modules/estraverse": { "version": "5.3.0", "resolved": "https://registry.npmjs.org/estraverse/-/estraverse-5.3.0.tgz", "integrity": "sha512-MMdARuVEQziNTeJD8DgMqmhwR11BRQ/cBP+pLtYdSTnf3MIO8fFeiINEbX36ZdNlfU/7A9f3gUw49B3oQsvwBA==", @@ -2836,15 +3178,6 @@ "node": ">=4.0" } }, - "node_modules/estraverse": { - "version": "4.3.0", - "resolved": "https://registry.npmjs.org/estraverse/-/estraverse-4.3.0.tgz", - "integrity": "sha512-39nnKffWz8xN1BU/2c79n9nB9HDzo0niYUqx6xyqUnyoAnQyyWpOTdZEeiCch8BBu515t4wp9ZmgVfVhn9EBpw==", - "dev": true, - "engines": { - "node": ">=4.0" - } - }, "node_modules/estree-walker": { "version": "3.0.3", "resolved": "https://registry.npmjs.org/estree-walker/-/estree-walker-3.0.3.tgz", @@ -2963,12 +3296,6 @@ "integrity": "sha512-f3qQ9oQy9j2AhBe/H9VC91wLmKBCCU/gDOnKNAYG5hswO7BLKj09Hc5HYNz9cGI++xlpDCIgDaitVs03ATR84Q==", "dev": true }, - "node_modules/fast-diff": { - "version": "1.3.0", - "resolved": "https://registry.npmjs.org/fast-diff/-/fast-diff-1.3.0.tgz", - "integrity": "sha512-VxPP4NqbUjj6MaAOafWeUn2cXWLcCtljklUtZf0Ind4XQ+QPtmA0b18zZy0jIQx+ExRVCR/ZQpBmik5lXshNsw==", - "dev": true - }, "node_modules/fast-glob": { "version": "3.2.12", "resolved": "https://registry.npmjs.org/fast-glob/-/fast-glob-3.2.12.tgz", @@ -3232,13 +3559,14 @@ } }, "node_modules/get-intrinsic": { - "version": "1.2.0", - "resolved": "https://registry.npmjs.org/get-intrinsic/-/get-intrinsic-1.2.0.tgz", - "integrity": "sha512-L049y6nFOuom5wGyRc3/gdTLO94dySVKRACj1RmJZBQXlbTMhtNIgkWkUHq+jYmZvKf14EW1EoJnnjbmoHij0Q==", + "version": "1.2.1", + "resolved": "https://registry.npmjs.org/get-intrinsic/-/get-intrinsic-1.2.1.tgz", + "integrity": "sha512-2DcsyfABl+gVHEfCOaTrWgyt+tb6MSEGmKq+kI5HwLbIYgjgmMcV8KQ41uaKz1xxUcn9tJtgFbQUEVcEbd0FYw==", "dev": true, "dependencies": { "function-bind": "^1.1.1", "has": "^1.0.3", + "has-proto": "^1.0.1", "has-symbols": "^1.0.3" }, "funding": { @@ -3462,12 +3790,6 @@ "integrity": "sha512-9ByhssR2fPVsNZj478qUUbKfmL0+t5BDVyjShtyZZLiK7ZDAArFFfopyOTj0M05wE2tJPisA4iTnnXl2YoPvOA==", "dev": true }, - "node_modules/grapheme-splitter": { - "version": "1.0.4", - "resolved": "https://registry.npmjs.org/grapheme-splitter/-/grapheme-splitter-1.0.4.tgz", - "integrity": "sha512-bzh50DW9kTPM00T8y4o8vQg89Di9oLJVLW/KaOGIXJWP/iqCN6WKYkbNOF04vFLJhwcpYUh9ydh/+5vpOqV4YQ==", - "dev": true - }, "node_modules/graphemer": { "version": "1.4.0", "resolved": "https://registry.npmjs.org/graphemer/-/graphemer-1.4.0.tgz", @@ -3605,9 +3927,9 @@ ] }, "node_modules/ignore": { - "version": "5.2.0", - "resolved": "https://registry.npmjs.org/ignore/-/ignore-5.2.0.tgz", - "integrity": "sha512-CmxgYGiEPCLhfLnpPp1MoRmifwEIOgjcHXxOBjv7mY96c+eWScsOP9c112ZyLdWHi0FxHjI+4uVhKYp/gcdRmQ==", + "version": "5.2.4", + "resolved": "https://registry.npmjs.org/ignore/-/ignore-5.2.4.tgz", + "integrity": "sha512-MAb38BcSbH0eHNBxn7ql2NH/kX33OkB3lZ1BNdh7ENeRChHTYsTvWrMubiIAMNS2llXEEgZ1MUOBtXChP3kaFQ==", "dev": true, "engines": { "node": ">= 4" @@ -3794,9 +4116,9 @@ } }, "node_modules/is-core-module": { - "version": "2.11.0", - "resolved": "https://registry.npmjs.org/is-core-module/-/is-core-module-2.11.0.tgz", - "integrity": "sha512-RRjxlvLDkD1YJwDbroBHMb+cukurkDWNyHx7D3oNB5x9rb5ogcksMC5wHCadcXoo67gVr/+3GFySh3134zi6rw==", + "version": "2.13.0", + "resolved": "https://registry.npmjs.org/is-core-module/-/is-core-module-2.13.0.tgz", + "integrity": "sha512-Z7dk6Qo8pOCp3l4tsX2C5ZVas4V+UxwQodwZhLopL91TX8UyyHEXafPcyoeeWuLrwzHcr3igO78wNLwHJHsMCQ==", "dev": true, "dependencies": { "has": "^1.0.3" @@ -4090,15 +4412,6 @@ "jiti": "bin/jiti.js" } }, - "node_modules/js-string-escape": { - "version": "1.0.1", - "resolved": "https://registry.npmjs.org/js-string-escape/-/js-string-escape-1.0.1.tgz", - "integrity": "sha512-Smw4xcfIQ5LVjAOuJCvN/zIodzA/BBSsluuoSykP+lUvScIi4U6RJLfwHet5cxFnCswUjISV8oAXaqaJDY3chg==", - "dev": true, - "engines": { - "node": ">= 0.8" - } - }, "node_modules/js-tokens": { "version": "4.0.0", "resolved": "https://registry.npmjs.org/js-tokens/-/js-tokens-4.0.0.tgz", @@ -4253,9 +4566,9 @@ } }, "node_modules/known-css-properties": { - "version": "0.27.0", - "resolved": "https://registry.npmjs.org/known-css-properties/-/known-css-properties-0.27.0.tgz", - "integrity": "sha512-uMCj6+hZYDoffuvAJjFAPz56E9uoowFHmTkqRtRq5WyC5Q6Cu/fTZKNQpX/RbzChBYLLl3lo8CjFZBAZXq9qFg==", + "version": "0.28.0", + "resolved": "https://registry.npmjs.org/known-css-properties/-/known-css-properties-0.28.0.tgz", + "integrity": "sha512-9pSL5XB4J+ifHP0e0jmmC98OGC1nL8/JjS+fi6mnTlIf//yt/MfVLtKg7S6nCtj/8KTcWX7nRlY0XywoYY1ISQ==", "dev": true }, "node_modules/lazy-ass": { @@ -4367,6 +4680,12 @@ "integrity": "sha512-v2kDEe57lecTulaDIuNTPy3Ry4gLGJ6Z1O3vE1krgXZNrsQ+LFTGHVxVjcXPs17LhbZVGedAJv8XZ1tvj5FvSg==", "dev": true }, + "node_modules/lodash-es": { + "version": "4.17.21", + "resolved": "https://registry.npmjs.org/lodash-es/-/lodash-es-4.17.21.tgz", + "integrity": "sha512-mKnC+QJ9pWVzv+C4/U3rRsHapFfHvQFoFB92e52xeyGMcX6/OlIl78je1u8vePzYZSkkogMPJ2yjxxsb89cxyw==", + "dev": true + }, "node_modules/lodash.merge": { "version": "4.6.2", "resolved": "https://registry.npmjs.org/lodash.merge/-/lodash.merge-4.6.2.tgz", @@ -4466,21 +4785,27 @@ } }, "node_modules/magic-string": { - "version": "0.30.0", - "resolved": "https://registry.npmjs.org/magic-string/-/magic-string-0.30.0.tgz", - "integrity": "sha512-LA+31JYDJLs82r2ScLrlz1GjSgu66ZV518eyWT+S8VhyQn/JL0u9MeBOvQMGYiPk1DBiSN9DDMOcXvigJZaViQ==", + "version": "0.30.2", + "resolved": "https://registry.npmjs.org/magic-string/-/magic-string-0.30.2.tgz", + "integrity": "sha512-lNZdu7pewtq/ZvWUp9Wpf/x7WzMTsR26TWV03BRZrXFsv+BI6dy8RAiKgm1uM/kyR0rCfUcqvOlXKG66KhIGug==", "dev": true, "dependencies": { - "@jridgewell/sourcemap-codec": "^1.4.13" + "@jridgewell/sourcemap-codec": "^1.4.15" }, "engines": { "node": ">=12" } }, + "node_modules/magic-string/node_modules/@jridgewell/sourcemap-codec": { + "version": "1.4.15", + "resolved": "https://registry.npmjs.org/@jridgewell/sourcemap-codec/-/sourcemap-codec-1.4.15.tgz", + "integrity": "sha512-eF2rxCRulEKXHTRiDrDy6erMYWqNw4LPdQ8UQA4huuxaQsVeRPFl2oM8oDGxMFhJUWZf9McpLtJasDDZb/Bpeg==", + "dev": true + }, "node_modules/maplibre-gl": { - "version": "3.1.0", - "resolved": "https://registry.npmjs.org/maplibre-gl/-/maplibre-gl-3.1.0.tgz", - "integrity": "sha512-KFarVUUszCEucPwnGsFJtPMQBg/F6lg+SPDmTztKUD/n0YShETjIOdNmm5jpxacEX3+dq50MzlqDr6VH+RtDDA==", + "version": "3.3.0", + "resolved": "https://registry.npmjs.org/maplibre-gl/-/maplibre-gl-3.3.0.tgz", + "integrity": "sha512-LDia3b8u2S8qtl50n8TYJM0IPLzfc01KDc71LNuydvDiEXAGBI5togty+juVtUipRZZjs4dAW6xhgrabc6lIgw==", "dev": true, "dependencies": { "@mapbox/geojson-rewind": "^0.5.2", @@ -4490,11 +4815,12 @@ "@mapbox/unitbezier": "^0.0.1", "@mapbox/vector-tile": "^1.3.1", "@mapbox/whoots-js": "^3.1.0", - "@maplibre/maplibre-gl-style-spec": "^19.2.1", + "@maplibre/maplibre-gl-style-spec": "^19.3.0", "@types/geojson": "^7946.0.10", "@types/mapbox__point-geometry": "^0.1.2", "@types/mapbox__vector-tile": "^1.3.0", "@types/pbf": "^3.0.2", + "@types/supercluster": "^7.1.0", "earcut": "^2.2.4", "geojson-vt": "^3.2.1", "gl-matrix": "^3.4.3", @@ -4516,18 +4842,6 @@ "url": "https://github.com/maplibre/maplibre-gl-js?sponsor=1" } }, - "node_modules/md5-hex": { - "version": "3.0.1", - "resolved": "https://registry.npmjs.org/md5-hex/-/md5-hex-3.0.1.tgz", - "integrity": "sha512-BUiRtTtV39LIJwinWBjqVsU9xhdnz7/i889V859IBFpuqGAj6LuOvHv5XLbgZ2R7ptJoJaEcxkv88/h25T7Ciw==", - "dev": true, - "dependencies": { - "blueimp-md5": "^2.10.0" - }, - "engines": { - "node": ">=8" - } - }, "node_modules/mdn-data": { "version": "2.0.30", "resolved": "https://registry.npmjs.org/mdn-data/-/mdn-data-2.0.30.tgz", @@ -4549,6 +4863,12 @@ "node": ">= 8" } }, + "node_modules/meshoptimizer": { + "version": "0.18.1", + "resolved": "https://registry.npmjs.org/meshoptimizer/-/meshoptimizer-0.18.1.tgz", + "integrity": "sha512-ZhoIoL7TNV4s5B6+rx5mC//fw8/POGyNxS/DZyCJeiZ12ScLfVwRE/GfsxwiTkMYYD5DmK2/JXnEVXqL4rF+Sw==", + "dev": true + }, "node_modules/micromatch": { "version": "4.0.5", "resolved": "https://registry.npmjs.org/micromatch/-/micromatch-4.0.5.tgz", @@ -4804,6 +5124,35 @@ "url": "https://github.com/sponsors/ljharb" } }, + "node_modules/object.fromentries": { + "version": "2.0.6", + "resolved": "https://registry.npmjs.org/object.fromentries/-/object.fromentries-2.0.6.tgz", + "integrity": "sha512-VciD13dswC4j1Xt5394WR4MzmAQmlgN72phd/riNp9vtD7tp4QQWJ0R4wvclXcafgcYK8veHRed2W6XeGBvcfg==", + "dev": true, + "dependencies": { + "call-bind": "^1.0.2", + "define-properties": "^1.1.4", + "es-abstract": "^1.20.4" + }, + "engines": { + "node": ">= 0.4" + }, + "funding": { + "url": "https://github.com/sponsors/ljharb" + } + }, + "node_modules/object.groupby": { + "version": "1.0.0", + "resolved": "https://registry.npmjs.org/object.groupby/-/object.groupby-1.0.0.tgz", + "integrity": "sha512-70MWG6NfRH9GnbZOikuhPPYzpUpof9iW2J9E4dW7FXTqPNb6rllE6u39SKwwiNh8lCwX3DDb5OgcKGiEBrTTyw==", + "dev": true, + "dependencies": { + "call-bind": "^1.0.2", + "define-properties": "^1.2.0", + "es-abstract": "^1.21.2", + "get-intrinsic": "^1.2.1" + } + }, "node_modules/object.values": { "version": "1.1.6", "resolved": "https://registry.npmjs.org/object.values/-/object.values-1.1.6.tgz", @@ -4846,17 +5195,17 @@ } }, "node_modules/optionator": { - "version": "0.9.1", - "resolved": "https://registry.npmjs.org/optionator/-/optionator-0.9.1.tgz", - "integrity": "sha512-74RlY5FCnhq4jRxVUPKDaRwrVNXMqsGsiW6AJw4XK8hmtm10wC0ypZBLw5IIp85NZMr91+qd1RvvENwg7jjRFw==", + "version": "0.9.3", + "resolved": "https://registry.npmjs.org/optionator/-/optionator-0.9.3.tgz", + "integrity": "sha512-JjCoypp+jKn1ttEFExxhetCKeJt9zhAgAve5FXHixTvFDW/5aEktX9bufBKLRRMdU7bNtpLfcGu94B3cdEJgjg==", "dev": true, "dependencies": { + "@aashutoshrathi/word-wrap": "^1.2.3", "deep-is": "^0.1.3", "fast-levenshtein": "^2.0.6", "levn": "^0.4.1", "prelude-ls": "^1.2.1", - "type-check": "^0.4.0", - "word-wrap": "^1.2.3" + "type-check": "^0.4.0" }, "engines": { "node": ">= 0.8.0" @@ -5102,9 +5451,9 @@ } }, "node_modules/postcss": { - "version": "8.4.24", - "resolved": "https://registry.npmjs.org/postcss/-/postcss-8.4.24.tgz", - "integrity": "sha512-M0RzbcI0sO/XJNucsGjvWU9ERWxb/ytp1w6dKtxTKgixdtQDq4rmx/g8W1hnaheq9jgwL/oyEdH5Bc4WwJKMqg==", + "version": "8.4.27", + "resolved": "https://registry.npmjs.org/postcss/-/postcss-8.4.27.tgz", + "integrity": "sha512-gY/ACJtJPSmUFPDCHtX78+01fHa64FaU4zaaWfuh1MhGJISufJAH4cun6k/8fwsHYeK4UQmENQK+tRLCFJE8JQ==", "dev": true, "funding": [ { @@ -5298,17 +5647,17 @@ } }, "node_modules/pretty-format": { - "version": "27.5.1", - "resolved": "https://registry.npmjs.org/pretty-format/-/pretty-format-27.5.1.tgz", - "integrity": "sha512-Qb1gy5OrP5+zDf2Bvnzdl3jsTf1qXVMazbvCoKhtKqVs4/YK4ozX4gKQJJVyNe+cajNPn0KoC0MC3FUmaHWEmQ==", + "version": "29.6.2", + "resolved": "https://registry.npmjs.org/pretty-format/-/pretty-format-29.6.2.tgz", + "integrity": "sha512-1q0oC8eRveTg5nnBEWMXAU2qpv65Gnuf2eCQzSjxpWFkPaPARwqZZDGuNE0zPAZfTCHzIk3A8dIjwlQKKLphyg==", "dev": true, "dependencies": { - "ansi-regex": "^5.0.1", + "@jest/schemas": "^29.6.0", "ansi-styles": "^5.0.0", - "react-is": "^17.0.1" + "react-is": "^18.0.0" }, "engines": { - "node": "^10.13.0 || ^12.13.0 || ^14.15.0 || >=15.0.0" + "node": "^14.15.0 || ^16.10.0 || >=18.0.0" } }, "node_modules/pretty-format/node_modules/ansi-styles": { @@ -5352,23 +5701,35 @@ } }, "node_modules/punycode": { - "version": "2.1.1", - "resolved": "https://registry.npmjs.org/punycode/-/punycode-2.1.1.tgz", - "integrity": "sha512-XRsRjdf+j5ml+y/6GKHPZbrF/8p2Yga0JPtdqTIY2Xe5ohJPD9saDJJLPvp9+NSBprVvevdXZybnj2cv8OEd0A==", + "version": "2.3.0", + "resolved": "https://registry.npmjs.org/punycode/-/punycode-2.3.0.tgz", + "integrity": "sha512-rRV+zQD8tVFys26lAGR9WUuS4iUAngJScM+ZRSKtvl5tKeZ2t5bvdNFdNHBW9FWR4guGHlgmsZ1G7BSm2wTbuA==", "dev": true, "engines": { "node": ">=6" } }, "node_modules/qs": { - "version": "6.5.3", - "resolved": "https://registry.npmjs.org/qs/-/qs-6.5.3.tgz", - "integrity": "sha512-qxXIEh4pCGfHICj1mAJQ2/2XVZkjCDTcEgfoSQxc/fYivUZxTkk7L3bDBJSoNrEzXI17oUO5Dp07ktqE5KzczA==", + "version": "6.10.4", + "resolved": "https://registry.npmjs.org/qs/-/qs-6.10.4.tgz", + "integrity": "sha512-OQiU+C+Ds5qiH91qh/mg0w+8nwQuLjM4F4M/PbmhDOoYehPh+Fb0bDjtR1sOvy7YKxvj28Y/M0PhP5uVX0kB+g==", "dev": true, + "dependencies": { + "side-channel": "^1.0.4" + }, "engines": { "node": ">=0.6" + }, + "funding": { + "url": "https://github.com/sponsors/ljharb" } }, + "node_modules/querystringify": { + "version": "2.2.0", + "resolved": "https://registry.npmjs.org/querystringify/-/querystringify-2.2.0.tgz", + "integrity": "sha512-FIqgj2EUvTa7R50u0rGsyTftzjYmv/a3hO345bZNrqabNqjtgiDMgmo4mkUjd+nzU5oF3dClKqFIPUKybUyqoQ==", + "dev": true + }, "node_modules/queue-microtask": { "version": "1.2.3", "resolved": "https://registry.npmjs.org/queue-microtask/-/queue-microtask-1.2.3.tgz", @@ -5396,9 +5757,9 @@ "dev": true }, "node_modules/react-is": { - "version": "17.0.2", - "resolved": "https://registry.npmjs.org/react-is/-/react-is-17.0.2.tgz", - "integrity": "sha512-w2GsyukL62IJnlaff/nRegPQR94C/XXamvMWmSHRJ4y7Ts/4ocGRmTHvOs8PSE6pB3dWOrD/nueuU5sduBsQ4w==", + "version": "18.2.0", + "resolved": "https://registry.npmjs.org/react-is/-/react-is-18.2.0.tgz", + "integrity": "sha512-xWGDIW6x921xtzPkhiULtthJHoJvBbF3q26fzloPCK0hsvxtPVelvftw3zjbHWSkR2km9Z+4uxbDDK/6Zw9B8w==", "dev": true }, "node_modules/read-cache": { @@ -5525,9 +5886,9 @@ } }, "node_modules/regexp-tree": { - "version": "0.1.24", - "resolved": "https://registry.npmjs.org/regexp-tree/-/regexp-tree-0.1.24.tgz", - "integrity": "sha512-s2aEVuLhvnVJW6s/iPgEGK6R+/xngd2jNQ+xy4bXNDKxZKJH6jpPHY6kVeVv1IeLCHgswRj+Kl3ELaDjG6V1iw==", + "version": "0.1.27", + "resolved": "https://registry.npmjs.org/regexp-tree/-/regexp-tree-0.1.27.tgz", + "integrity": "sha512-iETxpjK6YoRWJG5o6hXLwvjYAoW+FEZn9os0PD/b6AP6xQwsa/Y7lCVgIixBbUPMfhu+i2LtdeAqVTgGlQarfA==", "dev": true, "bin": { "regexp-tree": "bin/regexp-tree" @@ -5589,13 +5950,19 @@ "node": ">=0.10.0" } }, + "node_modules/requires-port": { + "version": "1.0.0", + "resolved": "https://registry.npmjs.org/requires-port/-/requires-port-1.0.0.tgz", + "integrity": "sha512-KigOCHcocU3XODJxsu8i/j8T9tzT4adHiecwORRQ0ZZFcp7ahwXuRU1m+yuO90C5ZUyGeGfocHDI14M3L3yDAQ==", + "dev": true + }, "node_modules/resolve": { - "version": "1.22.2", - "resolved": "https://registry.npmjs.org/resolve/-/resolve-1.22.2.tgz", - "integrity": "sha512-Sb+mjNHOULsBv818T40qSPeRiuWLyaGMa5ewydRLFimneixmVy2zdivRl+AF6jaYPC8ERxGDmFSiqui6SfPd+g==", + "version": "1.22.4", + "resolved": "https://registry.npmjs.org/resolve/-/resolve-1.22.4.tgz", + "integrity": "sha512-PXNdCiPqDqeUou+w1C2eTQbNfxKSuMxqTCuvlmmMsk1NWHL5fRrhY6Pl0qEYYc6+QqGClco1Qj8XnjPego4wfg==", "dev": true, "dependencies": { - "is-core-module": "^2.11.0", + "is-core-module": "^2.13.0", "path-parse": "^1.0.7", "supports-preserve-symlinks-flag": "^1.0.0" }, @@ -5669,9 +6036,9 @@ } }, "node_modules/rollup": { - "version": "3.21.5", - "resolved": "https://registry.npmjs.org/rollup/-/rollup-3.21.5.tgz", - "integrity": "sha512-a4NTKS4u9PusbUJcfF4IMxuqjFzjm6ifj76P54a7cKnvVzJaG12BLVR+hgU2YDGHzyMMQNxLAZWuALsn8q2oQg==", + "version": "3.28.0", + "resolved": "https://registry.npmjs.org/rollup/-/rollup-3.28.0.tgz", + "integrity": "sha512-d7zhvo1OUY2SXSM6pfNjgD5+d0Nz87CUp4mt8l/GgVP3oBsPwzNvSzyu1me6BSG9JIgWNTVcafIXBIyM8yQ3yw==", "dev": true, "bin": { "rollup": "dist/bin/rollup" @@ -5754,15 +6121,6 @@ } ] }, - "node_modules/safe-regex": { - "version": "2.1.1", - "resolved": "https://registry.npmjs.org/safe-regex/-/safe-regex-2.1.1.tgz", - "integrity": "sha512-rx+x8AMzKb5Q5lQ95Zoi6ZbJqwCLkqi3XuJXp5P3rT8OEc6sZCJG5AE5dU3lsgRr/F4Bs31jSlVN+j5KrsGu9A==", - "dev": true, - "dependencies": { - "regexp-tree": "~0.1.1" - } - }, "node_modules/safe-regex-test": { "version": "1.0.0", "resolved": "https://registry.npmjs.org/safe-regex-test/-/safe-regex-test-1.0.0.tgz", @@ -5827,9 +6185,9 @@ } }, "node_modules/semver": { - "version": "7.5.3", - "resolved": "https://registry.npmjs.org/semver/-/semver-7.5.3.tgz", - "integrity": "sha512-QBlUtyVk/5EeHbi7X0fw6liDZc7BBmEaSYn01fMU1OUYbf6GPsbTtd8WmnqbI20SeycoHSeiybkE/q1Q+qlThQ==", + "version": "7.5.4", + "resolved": "https://registry.npmjs.org/semver/-/semver-7.5.4.tgz", + "integrity": "sha512-1bCSESV6Pv+i21Hvpxp3Dx+pSD8lIPt8uVjRrxAUt/nbswYc+tK6Y2btiULjd4+fnq15PX+nqQDC7Oft7WkwcA==", "dev": true, "dependencies": { "lru-cache": "^6.0.0" @@ -5903,6 +6261,12 @@ "integrity": "sha512-wnD2ZE+l+SPC/uoS0vXeE9L1+0wuaMqKlfz9AMUo38JsyLSBWSFcHR1Rri62LZc12vLr1gb3jl7iwQhgwpAbGQ==", "dev": true }, + "node_modules/simplex-noise": { + "version": "4.0.1", + "resolved": "https://registry.npmjs.org/simplex-noise/-/simplex-noise-4.0.1.tgz", + "integrity": "sha512-zl/+bdSqW7HJOQ0oDbxrNYaF4F5ik0i7M6YOYmEoIJNtg16NpvWaTTM1Y7oV/7T0jFljawLgYPS81Uu2rsfo1A==", + "dev": true + }, "node_modules/slash": { "version": "3.0.0", "resolved": "https://registry.npmjs.org/slash/-/slash-3.0.0.tgz", @@ -6109,9 +6473,9 @@ "dev": true }, "node_modules/std-env": { - "version": "3.3.2", - "resolved": "https://registry.npmjs.org/std-env/-/std-env-3.3.2.tgz", - "integrity": "sha512-uUZI65yrV2Qva5gqE0+A7uVAvO40iPo6jGhs7s8keRfHCmtg+uB2X6EiLGCI9IgL1J17xGhvoOqSz79lzICPTA==", + "version": "3.3.3", + "resolved": "https://registry.npmjs.org/std-env/-/std-env-3.3.3.tgz", + "integrity": "sha512-Rz6yejtVyWnVjC1RFvNmYL10kgjC49EOghxWn0RFqlCHGFpQx+Xe7yW3I4ceK1SGrWIGMjD5Kbue8W/udkbMJg==", "dev": true }, "node_modules/string-width": { @@ -6327,16 +6691,16 @@ } }, "node_modules/svelte": { - "version": "4.0.0", - "resolved": "https://registry.npmjs.org/svelte/-/svelte-4.0.0.tgz", - "integrity": "sha512-+yCYu3AEUu9n91dnQNGIbnVp8EmNQtuF/YImW4+FTXRHard7NMo+yTsWzggPAbj3fUEJ1FBJLkql/jkp6YB5pg==", + "version": "4.1.2", + "resolved": "https://registry.npmjs.org/svelte/-/svelte-4.1.2.tgz", + "integrity": "sha512-/evA8U6CgOHe5ZD1C1W3va9iJG7mWflcCdghBORJaAhD2JzrVERJty/2gl0pIPrJYBGZwZycH6onYf+64XXF9g==", "dev": true, "dependencies": { "@ampproject/remapping": "^2.2.1", "@jridgewell/sourcemap-codec": "^1.4.15", "@jridgewell/trace-mapping": "^0.3.18", - "acorn": "^8.8.2", - "aria-query": "^5.2.1", + "acorn": "^8.9.0", + "aria-query": "^5.3.0", "axobject-query": "^3.2.1", "code-red": "^1.0.3", "css-tree": "^2.3.1", @@ -6351,9 +6715,9 @@ } }, "node_modules/svelte-check": { - "version": "3.4.4", - "resolved": "https://registry.npmjs.org/svelte-check/-/svelte-check-3.4.4.tgz", - "integrity": "sha512-Uys9+R65cj8TmP8f5UpS7B2xKpNLYNxEWJsA5ZoKcWq/uwvABFF7xS6iPQGLoa7hxz0DS6xU60YFpmq06E4JxA==", + "version": "3.4.6", + "resolved": "https://registry.npmjs.org/svelte-check/-/svelte-check-3.4.6.tgz", + "integrity": "sha512-OBlY8866Zh1zHQTkBMPS6psPi7o2umTUyj6JWm4SacnIHXpWFm658pG32m3dKvKFL49V4ntAkfFHKo4ztH07og==", "dev": true, "dependencies": { "@jridgewell/trace-mapping": "^0.3.17", @@ -6362,7 +6726,7 @@ "import-fresh": "^3.2.1", "picocolors": "^1.0.0", "sade": "^1.7.4", - "svelte-preprocess": "^5.0.3", + "svelte-preprocess": "^5.0.4", "typescript": "^5.0.3" }, "bin": { @@ -6373,15 +6737,15 @@ } }, "node_modules/svelte-eslint-parser": { - "version": "0.31.0", - "resolved": "https://registry.npmjs.org/svelte-eslint-parser/-/svelte-eslint-parser-0.31.0.tgz", - "integrity": "sha512-/31RpBf/e3YjoFphjsyo3JRyN1r4UalGAGafXrZ6EJK4h4COOO0rbfBoen5byGsXnIJKsrlC1lkEd2Vzpq2IDg==", + "version": "0.32.2", + "resolved": "https://registry.npmjs.org/svelte-eslint-parser/-/svelte-eslint-parser-0.32.2.tgz", + "integrity": "sha512-Ok9D3A4b23iLQsONrjqtXtYDu5ZZ/826Blaw2LeFZVTg1pwofKDG4mz3/GYTax8fQ0plRGHI6j+d9VQYy5Lo/A==", "dev": true, "dependencies": { "eslint-scope": "^7.0.0", "eslint-visitor-keys": "^3.0.0", "espree": "^9.0.0", - "postcss": "^8.4.23", + "postcss": "^8.4.25", "postcss-scss": "^4.0.6" }, "engines": { @@ -6391,7 +6755,7 @@ "url": "https://github.com/sponsors/ota-meshi" }, "peerDependencies": { - "svelte": "^3.37.0 || ^4.0.0-0" + "svelte": "^3.37.0 || ^4.0.0" }, "peerDependenciesMeta": { "svelte": { @@ -6399,41 +6763,25 @@ } } }, - "node_modules/svelte-eslint-parser/node_modules/eslint-scope": { - "version": "7.2.0", - "resolved": "https://registry.npmjs.org/eslint-scope/-/eslint-scope-7.2.0.tgz", - "integrity": "sha512-DYj5deGlHBfMt15J7rdtyKNq/Nqlv5KfU4iodrQ019XESsRnwXH9KAE0y3cwtUHDo2ob7CypAnCqefh6vioWRw==", + "node_modules/svelte-hmr": { + "version": "0.15.3", + "resolved": "https://registry.npmjs.org/svelte-hmr/-/svelte-hmr-0.15.3.tgz", + "integrity": "sha512-41snaPswvSf8TJUhlkoJBekRrABDXDMdpNpT2tfHIv4JuhgvHqLMhEPGtaQn0BmbNSTkuz2Ed20DF2eHw0SmBQ==", "dev": true, - "dependencies": { - "esrecurse": "^4.3.0", - "estraverse": "^5.2.0" - }, "engines": { - "node": "^12.22.0 || ^14.17.0 || >=16.0.0" + "node": "^12.20 || ^14.13.1 || >= 16" }, - "funding": { - "url": "https://opencollective.com/eslint" - } - }, - "node_modules/svelte-eslint-parser/node_modules/estraverse": { - "version": "5.3.0", - "resolved": "https://registry.npmjs.org/estraverse/-/estraverse-5.3.0.tgz", - "integrity": "sha512-MMdARuVEQziNTeJD8DgMqmhwR11BRQ/cBP+pLtYdSTnf3MIO8fFeiINEbX36ZdNlfU/7A9f3gUw49B3oQsvwBA==", - "dev": true, - "engines": { - "node": ">=4.0" + "peerDependencies": { + "svelte": "^3.19.0 || ^4.0.0" } }, - "node_modules/svelte-hmr": { - "version": "0.15.2", - "resolved": "https://registry.npmjs.org/svelte-hmr/-/svelte-hmr-0.15.2.tgz", - "integrity": "sha512-q/bAruCvFLwvNbeE1x3n37TYFb3mTBJ6TrCq6p2CoFbSTNhDE9oAtEfpy+wmc9So8AG0Tja+X0/mJzX9tSfvIg==", + "node_modules/svelte-inview": { + "version": "4.0.1", + "resolved": "https://registry.npmjs.org/svelte-inview/-/svelte-inview-4.0.1.tgz", + "integrity": "sha512-8NuT/DKFiZAccDw1Z16cIsdZ7K6/BGxrUfDaFaWTCEdn3YqMj1TUAkfmQ08FQ1+INl1G+TQS+ImXIBiiISgXog==", "dev": true, - "engines": { - "node": "^12.20 || ^14.13.1 || >= 16" - }, "peerDependencies": { - "svelte": "^3.19.0 || ^4.0.0-next.0" + "svelte": "^3.0.0 || ^4.0.0" } }, "node_modules/svelte-preprocess": { @@ -6517,9 +6865,9 @@ "dev": true }, "node_modules/tailwindcss": { - "version": "3.3.2", - "resolved": "https://registry.npmjs.org/tailwindcss/-/tailwindcss-3.3.2.tgz", - "integrity": "sha512-9jPkMiIBXvPc2KywkraqsUfbfj+dHDb+JPWtSJa9MLFdrPyazI7q6WX2sUrm7R9eVR7qqv3Pas7EvQFzxKnI6w==", + "version": "3.3.3", + "resolved": "https://registry.npmjs.org/tailwindcss/-/tailwindcss-3.3.3.tgz", + "integrity": "sha512-A0KgSkef7eE4Mf+nKJ83i75TMyq8HqY3qmFIJSWy8bNt0v1lG7jUcpGpoTFxAwYcWOphcTBLPPJg+bDfhDf52w==", "dev": true, "dependencies": { "@alloc/quick-lru": "^5.2.0", @@ -6542,7 +6890,6 @@ "postcss-load-config": "^4.0.1", "postcss-nested": "^6.0.1", "postcss-selector-parser": "^6.0.11", - "postcss-value-parser": "^4.2.0", "resolve": "^1.22.2", "sucrase": "^3.32.0" }, @@ -6610,9 +6957,9 @@ } }, "node_modules/three": { - "version": "0.152.2", - "resolved": "https://registry.npmjs.org/three/-/three-0.152.2.tgz", - "integrity": "sha512-Ff9zIpSfkkqcBcpdiFo2f35vA9ZucO+N8TNacJOqaEE6DrB0eufItVMib8bK8Pcju/ZNT6a7blE1GhTpkdsILw==", + "version": "0.155.0", + "resolved": "https://registry.npmjs.org/three/-/three-0.155.0.tgz", + "integrity": "sha512-sNgCYmDijnIqkD/bMfk+1pHg3YzsxW7V2ChpuP6HCQ8NiZr3RufsXQr8M3SSUMjW4hG+sUk7YbyuY0DncaDTJQ==", "dev": true }, "node_modules/three-inspect": { @@ -6624,6 +6971,15 @@ "three": "*" } }, + "node_modules/three-mesh-bvh": { + "version": "0.6.3", + "resolved": "https://registry.npmjs.org/three-mesh-bvh/-/three-mesh-bvh-0.6.3.tgz", + "integrity": "sha512-xjuGLSI9nBATIsWcT/DnnNma5xXYyvBiXfUbhGLAFqItOlOKYF5JWsUOX+cuSAnSWovEoHzd5Emx23qKiByrlw==", + "dev": true, + "peerDependencies": { + "three": ">= 0.151.0" + } + }, "node_modules/throttleit": { "version": "1.0.0", "resolved": "https://registry.npmjs.org/throttleit/-/throttleit-1.0.0.tgz", @@ -6636,15 +6992,6 @@ "integrity": "sha512-w89qg7PI8wAdvX60bMDP+bFoD5Dvhm9oLheFp5O4a2QF0cSBGsBX4qZmadPMvVqlLJBBci+WqGGOAPvcDeNSVg==", "dev": true }, - "node_modules/time-zone": { - "version": "1.0.0", - "resolved": "https://registry.npmjs.org/time-zone/-/time-zone-1.0.0.tgz", - "integrity": "sha512-TIsDdtKo6+XrPtiTm1ssmMngN1sAhyKnTO2kunQWqNPWIVvCm15Wmw4SWInwTVgJ5u/Tr04+8Ei9TNcw4x4ONA==", - "dev": true, - "engines": { - "node": ">=4" - } - }, "node_modules/tinybench": { "version": "2.5.0", "resolved": "https://registry.npmjs.org/tinybench/-/tinybench-2.5.0.tgz", @@ -6652,9 +6999,9 @@ "dev": true }, "node_modules/tinypool": { - "version": "0.5.0", - "resolved": "https://registry.npmjs.org/tinypool/-/tinypool-0.5.0.tgz", - "integrity": "sha512-paHQtnrlS1QZYKF/GnLoOM/DN9fqaGOFbCbxzAhwniySnzl9Ebk8w73/dd34DAhe/obUbPAOldTyYXQZxnPBPQ==", + "version": "0.7.0", + "resolved": "https://registry.npmjs.org/tinypool/-/tinypool-0.7.0.tgz", + "integrity": "sha512-zSYNUlYSMhJ6Zdou4cJwo/p7w5nmAH17GRfU/ui3ctvjXFErXXkruT4MWW6poDeXgCaIBlGLrfU6TbTXxyGMww==", "dev": true, "engines": { "node": ">=14.0.0" @@ -6700,57 +7047,84 @@ } }, "node_modules/tough-cookie": { - "version": "2.5.0", - "resolved": "https://registry.npmjs.org/tough-cookie/-/tough-cookie-2.5.0.tgz", - "integrity": "sha512-nlLsUzgm1kfLXSXfRZMc1KLAugd4hqJHDTvc2hDIwS3mZAfMEuMbc03SujMF+GEcpaX/qboeycw6iO8JwVv2+g==", + "version": "4.1.3", + "resolved": "https://registry.npmjs.org/tough-cookie/-/tough-cookie-4.1.3.tgz", + "integrity": "sha512-aX/y5pVRkfRnfmuX+OdbSdXvPe6ieKX/G2s7e98f4poJHnqH3281gDPm/metm6E/WRamfx7WC4HUqkWHfQHprw==", "dev": true, "dependencies": { - "psl": "^1.1.28", - "punycode": "^2.1.1" + "psl": "^1.1.33", + "punycode": "^2.1.1", + "universalify": "^0.2.0", + "url-parse": "^1.5.3" }, "engines": { - "node": ">=0.8" + "node": ">=6" + } + }, + "node_modules/tough-cookie/node_modules/universalify": { + "version": "0.2.0", + "resolved": "https://registry.npmjs.org/universalify/-/universalify-0.2.0.tgz", + "integrity": "sha512-CJ1QgKmNg3CwvAv/kOFmtnEN05f0D/cn9QntgNOQlQF9dgvVTHj3t+8JPdjqawCHk7V/KA+fbUqzZ9XWhcqPUg==", + "dev": true, + "engines": { + "node": ">= 4.0.0" } }, "node_modules/troika-three-text": { - "version": "0.46.4", - "resolved": "https://registry.npmjs.org/troika-three-text/-/troika-three-text-0.46.4.tgz", - "integrity": "sha512-Qsv0HhUKTZgSmAJs5wvO7YlBoJSP9TGPLmrg+K9pbQq4lseQdcevbno/WI38bwJBZ/qS56hvfqEzY0zUEFzDIw==", + "version": "0.47.2", + "resolved": "https://registry.npmjs.org/troika-three-text/-/troika-three-text-0.47.2.tgz", + "integrity": "sha512-qylT0F+U7xGs+/PEf3ujBdJMYWbn0Qci0kLqI5BJG2kW1wdg4T1XSxneypnF05DxFqJhEzuaOR9S2SjiyknMng==", "dev": true, "dependencies": { "bidi-js": "^1.0.2", - "troika-three-utils": "^0.46.0", - "troika-worker-utils": "^0.46.0", + "troika-three-utils": "^0.47.2", + "troika-worker-utils": "^0.47.2", "webgl-sdf-generator": "1.1.1" }, "peerDependencies": { - "three": ">=0.103.0" + "three": ">=0.125.0" } }, "node_modules/troika-three-utils": { - "version": "0.46.0", - "resolved": "https://registry.npmjs.org/troika-three-utils/-/troika-three-utils-0.46.0.tgz", - "integrity": "sha512-llHyrXAcwzr0bpg80GxsIp73N7FuImm4WCrKDJkAqcAsWmE5pfP9+Qzw+oMWK1P/AdHQ79eOrOl9NjyW4aOw0w==", + "version": "0.47.2", + "resolved": "https://registry.npmjs.org/troika-three-utils/-/troika-three-utils-0.47.2.tgz", + "integrity": "sha512-/28plhCxfKtH7MSxEGx8e3b/OXU5A0xlwl+Sbdp0H8FXUHKZDoksduEKmjQayXYtxAyuUiCRunYIv/8Vi7aiyg==", "dev": true, "peerDependencies": { - "three": ">=0.103.0" + "three": ">=0.125.0" } }, "node_modules/troika-worker-utils": { - "version": "0.46.0", - "resolved": "https://registry.npmjs.org/troika-worker-utils/-/troika-worker-utils-0.46.0.tgz", - "integrity": "sha512-bzOx5f2ZBxkFhXtIvDJlLn2AI3bzCkGVbCndl/2dL5QZrwHEKl45OEIilCxYQQWJG1rEbOD9O80tMjoYjw19OA==", + "version": "0.47.2", + "resolved": "https://registry.npmjs.org/troika-worker-utils/-/troika-worker-utils-0.47.2.tgz", + "integrity": "sha512-mzss4MeyzUkYBppn4x5cdAqrhBHFEuVmMMgLMTyFV23x6GvQMyo+/R5E5Lsbrt7WSt5RfvewjcwD1DChRTA9lA==", "dev": true }, "node_modules/trzy": { - "version": "0.0.49", - "resolved": "https://registry.npmjs.org/trzy/-/trzy-0.0.49.tgz", - "integrity": "sha512-W8CCYYIjsrmFbPXILB4G6S499/OeassKnohWVjr+aV4jGo0gTh/RegbAzkiy1e78Dvx1LxDvVWAid0r75wQCUQ==", + "version": "0.3.11", + "resolved": "https://registry.npmjs.org/trzy/-/trzy-0.3.11.tgz", + "integrity": "sha512-pqDPW/DBpxA9hgZvDRYdouEsVrLMtL0LfhkBGnOH0QXyHeP/noW/ppODUOf4P7DLh9TWNWiA4as/bAsfLPmnWg==", "dev": true, + "dependencies": { + "simplex-noise": "^4.0.1", + "three-mesh-bvh": "^0.6.3" + }, "peerDependencies": { "three": "*" } }, + "node_modules/ts-api-utils": { + "version": "1.0.1", + "resolved": "https://registry.npmjs.org/ts-api-utils/-/ts-api-utils-1.0.1.tgz", + "integrity": "sha512-lC/RGlPmwdrIBFTX59wwNzqh7aR2otPNPR/5brHZm/XKFYKsfqxihXUe9pU3JI+3vGkl+vyCoNNnPhJn3aLK1A==", + "dev": true, + "engines": { + "node": ">=16.13.0" + }, + "peerDependencies": { + "typescript": ">=4.2.0" + } + }, "node_modules/ts-interface-checker": { "version": "0.1.13", "resolved": "https://registry.npmjs.org/ts-interface-checker/-/ts-interface-checker-0.1.13.tgz", @@ -6758,13 +7132,13 @@ "dev": true }, "node_modules/tsconfig-paths": { - "version": "3.14.1", - "resolved": "https://registry.npmjs.org/tsconfig-paths/-/tsconfig-paths-3.14.1.tgz", - "integrity": "sha512-fxDhWnFSLt3VuTwtvJt5fpwxBHg5AdKWMsgcPOOIilyjymcYVZoCQF8fvFRezCNfblEXmi+PcM1eYHeOAgXCOQ==", + "version": "3.14.2", + "resolved": "https://registry.npmjs.org/tsconfig-paths/-/tsconfig-paths-3.14.2.tgz", + "integrity": "sha512-o/9iXgCYc5L/JxCHPe3Hvh8Q/2xm5Z+p18PESBU6Ff33695QnCHBEjcytY2q19ua7Mbl/DavtBOLq+oG0RCL+g==", "dev": true, "dependencies": { "@types/json5": "^0.0.29", - "json5": "^1.0.1", + "json5": "^1.0.2", "minimist": "^1.2.6", "strip-bom": "^3.0.0" } @@ -6775,27 +7149,6 @@ "integrity": "sha512-d6xOpEDfsi2CZVlPQzGeux8XMwLT9hssAsaPYExaQMuYskwb+x1x7J371tWlbBdWHroy99KnVB6qIkUbs5X3UQ==", "dev": true }, - "node_modules/tsutils": { - "version": "3.21.0", - "resolved": "https://registry.npmjs.org/tsutils/-/tsutils-3.21.0.tgz", - "integrity": "sha512-mHKK3iUXL+3UF6xL5k0PEhKRUBKPBCv/+RkEOpjRWxxx27KKRBmmA60A9pgOUvMi8GKhRMPEmjBRPzs2W7O1OA==", - "dev": true, - "dependencies": { - "tslib": "^1.8.1" - }, - "engines": { - "node": ">= 6" - }, - "peerDependencies": { - "typescript": ">=2.8.0 || >= 3.2.0-dev || >= 3.3.0-dev || >= 3.4.0-dev || >= 3.5.0-dev || >= 3.6.0-dev || >= 3.6.0-beta || >= 3.7.0-dev || >= 3.7.0-beta" - } - }, - "node_modules/tsutils/node_modules/tslib": { - "version": "1.14.1", - "resolved": "https://registry.npmjs.org/tslib/-/tslib-1.14.1.tgz", - "integrity": "sha512-Xni35NKzjgMrwevysHTCArtLDpPvye8zV/0E4EyYn43P7/7qvQwPh9BGkHewbMulVntbigmcT7rdX3BNo9wRJg==", - "dev": true - }, "node_modules/tunnel-agent": { "version": "0.6.0", "resolved": "https://registry.npmjs.org/tunnel-agent/-/tunnel-agent-0.6.0.tgz", @@ -6862,9 +7215,9 @@ } }, "node_modules/typescript": { - "version": "5.1.3", - "resolved": "https://registry.npmjs.org/typescript/-/typescript-5.1.3.tgz", - "integrity": "sha512-XH627E9vkeqhlZFQuL+UsyAXEnibT0kWR2FWONlr4sTjvxyJYnyefgrkyECLzM5NenmKzRAy2rR/OlYLA1HkZw==", + "version": "5.1.6", + "resolved": "https://registry.npmjs.org/typescript/-/typescript-5.1.6.tgz", + "integrity": "sha512-zaWCozRZ6DLEWAWFrVDz1H6FVXzUSfTy5FUMWsQlU8Ym5JP9eO4xkTIROFCQvhQf61z6O/G6ugw3SgAnvvm+HA==", "dev": true, "bin": { "tsc": "bin/tsc", @@ -6890,9 +7243,9 @@ "dev": true }, "node_modules/ufo": { - "version": "1.1.2", - "resolved": "https://registry.npmjs.org/ufo/-/ufo-1.1.2.tgz", - "integrity": "sha512-TrY6DsjTQQgyS3E3dBaOXf0TpPD8u9FVrVYmKVegJuFw51n/YB9XPt+U6ydzFG5ZIN7+DIjPbNmXoBj9esYhgQ==", + "version": "1.2.0", + "resolved": "https://registry.npmjs.org/ufo/-/ufo-1.2.0.tgz", + "integrity": "sha512-RsPyTbqORDNDxqAdQPQBpgqhWle1VcTSou/FraClYlHf6TZnQcGslpLcAphNR+sQW4q5lLWLbOsRlh9j24baQg==", "dev": true }, "node_modules/unbox-primitive": { @@ -6952,6 +7305,16 @@ "punycode": "^2.1.0" } }, + "node_modules/url-parse": { + "version": "1.5.10", + "resolved": "https://registry.npmjs.org/url-parse/-/url-parse-1.5.10.tgz", + "integrity": "sha512-WypcfiRhfeUP9vvF0j6rw0J3hrWrw6iZv3+22h6iRMJ/8z1Tj6XfLP4DsUix5MhMPnXpiHDoKyoZ/bdCkwBCiQ==", + "dev": true, + "dependencies": { + "querystringify": "^2.1.1", + "requires-port": "^1.0.0" + } + }, "node_modules/util-deprecate": { "version": "1.0.2", "resolved": "https://registry.npmjs.org/util-deprecate/-/util-deprecate-1.0.2.tgz", @@ -6992,14 +7355,14 @@ } }, "node_modules/vite": { - "version": "4.3.9", - "resolved": "https://registry.npmjs.org/vite/-/vite-4.3.9.tgz", - "integrity": "sha512-qsTNZjO9NoJNW7KnOrgYwczm0WctJ8m/yqYAMAK9Lxt4SoySUfS5S8ia9K7JHpa3KEeMfyF8LoJ3c5NeBJy6pg==", + "version": "4.4.9", + "resolved": "https://registry.npmjs.org/vite/-/vite-4.4.9.tgz", + "integrity": "sha512-2mbUn2LlUmNASWwSCNSJ/EG2HuSRTnVNaydp6vMCm5VIqJsjMfbIWtbH2kDuwUVW5mMUKKZvGPX/rqeqVvv1XA==", "dev": true, "dependencies": { - "esbuild": "^0.17.5", - "postcss": "^8.4.23", - "rollup": "^3.21.0" + "esbuild": "^0.18.10", + "postcss": "^8.4.27", + "rollup": "^3.27.1" }, "bin": { "vite": "bin/vite.js" @@ -7007,12 +7370,16 @@ "engines": { "node": "^14.18.0 || >=16.0.0" }, + "funding": { + "url": "https://github.com/vitejs/vite?sponsor=1" + }, "optionalDependencies": { "fsevents": "~2.3.2" }, "peerDependencies": { "@types/node": ">= 14", "less": "*", + "lightningcss": "^1.21.0", "sass": "*", "stylus": "*", "sugarss": "*", @@ -7025,6 +7392,9 @@ "less": { "optional": true }, + "lightningcss": { + "optional": true + }, "sass": { "optional": true }, @@ -7040,15 +7410,15 @@ } }, "node_modules/vite-node": { - "version": "0.32.2", - "resolved": "https://registry.npmjs.org/vite-node/-/vite-node-0.32.2.tgz", - "integrity": "sha512-dTQ1DCLwl2aEseov7cfQ+kDMNJpM1ebpyMMMwWzBvLbis8Nla/6c9WQcqpPssTwS6Rp/+U6KwlIj8Eapw4bLdA==", + "version": "0.34.1", + "resolved": "https://registry.npmjs.org/vite-node/-/vite-node-0.34.1.tgz", + "integrity": "sha512-odAZAL9xFMuAg8aWd7nSPT+hU8u2r9gU3LRm9QKjxBEF2rRdWpMuqkrkjvyVQEdNFiBctqr2Gg4uJYizm5Le6w==", "dev": true, "dependencies": { "cac": "^6.7.14", "debug": "^4.3.4", - "mlly": "^1.2.0", - "pathe": "^1.1.0", + "mlly": "^1.4.0", + "pathe": "^1.1.1", "picocolors": "^1.0.0", "vite": "^3.0.0 || ^4.0.0" }, @@ -7063,9 +7433,9 @@ } }, "node_modules/vite-plugin-css-injected-by-js": { - "version": "3.1.1", - "resolved": "https://registry.npmjs.org/vite-plugin-css-injected-by-js/-/vite-plugin-css-injected-by-js-3.1.1.tgz", - "integrity": "sha512-mwrFvEEy0TuH8Ul0cb2HgjmNboQ/JnEFy+kHCWqAJph3ikMOiIuyYVdx0JO4nEIWJyzSnc4TTdmoTulsikvJEg==", + "version": "3.3.0", + "resolved": "https://registry.npmjs.org/vite-plugin-css-injected-by-js/-/vite-plugin-css-injected-by-js-3.3.0.tgz", + "integrity": "sha512-xG+jyHNCmUqi/TXp6q88wTJGeAOrNLSyUUTp4qEQ9QZLGcHWQQsCsSSKa59rPMQr8sOzfzmWDd8enGqfH/dBew==", "dev": true, "peerDependencies": { "vite": ">2.0.0-0" @@ -7086,35 +7456,34 @@ } }, "node_modules/vitest": { - "version": "0.32.2", - "resolved": "https://registry.npmjs.org/vitest/-/vitest-0.32.2.tgz", - "integrity": "sha512-hU8GNNuQfwuQmqTLfiKcqEhZY72Zxb7nnN07koCUNmntNxbKQnVbeIS6sqUgR3eXSlbOpit8+/gr1KpqoMgWCQ==", + "version": "0.34.1", + "resolved": "https://registry.npmjs.org/vitest/-/vitest-0.34.1.tgz", + "integrity": "sha512-G1PzuBEq9A75XSU88yO5G4vPT20UovbC/2osB2KEuV/FisSIIsw7m5y2xMdB7RsAGHAfg2lPmp2qKr3KWliVlQ==", "dev": true, "dependencies": { "@types/chai": "^4.3.5", "@types/chai-subset": "^1.3.3", "@types/node": "*", - "@vitest/expect": "0.32.2", - "@vitest/runner": "0.32.2", - "@vitest/snapshot": "0.32.2", - "@vitest/spy": "0.32.2", - "@vitest/utils": "0.32.2", - "acorn": "^8.8.2", + "@vitest/expect": "0.34.1", + "@vitest/runner": "0.34.1", + "@vitest/snapshot": "0.34.1", + "@vitest/spy": "0.34.1", + "@vitest/utils": "0.34.1", + "acorn": "^8.9.0", "acorn-walk": "^8.2.0", "cac": "^6.7.14", "chai": "^4.3.7", - "concordance": "^5.0.4", "debug": "^4.3.4", "local-pkg": "^0.4.3", - "magic-string": "^0.30.0", - "pathe": "^1.1.0", + "magic-string": "^0.30.1", + "pathe": "^1.1.1", "picocolors": "^1.0.0", - "std-env": "^3.3.2", + "std-env": "^3.3.3", "strip-literal": "^1.0.1", "tinybench": "^2.5.0", - "tinypool": "^0.5.0", + "tinypool": "^0.7.0", "vite": "^3.0.0 || ^4.0.0", - "vite-node": "0.32.2", + "vite-node": "0.34.1", "why-is-node-running": "^2.2.2" }, "bin": { @@ -7180,15 +7549,6 @@ "integrity": "sha512-9Z0JcMTFxeE+b2x1LJTdnaT8rT8aEp7MVxkNwoycNmJWwPdzoXzMh0BjJSh/AEFP+KPYZUli814h8bJZFIZ2jA==", "dev": true }, - "node_modules/well-known-symbols": { - "version": "2.0.0", - "resolved": "https://registry.npmjs.org/well-known-symbols/-/well-known-symbols-2.0.0.tgz", - "integrity": "sha512-ZMjC3ho+KXo0BfJb7JgtQ5IBuvnShdlACNkKkdsqBmYw3bPAaJfPeYUo6tLUaT5tG/Gkh7xkpBhKRQ9e7pyg9Q==", - "dev": true, - "engines": { - "node": ">=6" - } - }, "node_modules/which": { "version": "2.0.2", "resolved": "https://registry.npmjs.org/which/-/which-2.0.2.tgz", @@ -7256,15 +7616,6 @@ "node": ">=8" } }, - "node_modules/word-wrap": { - "version": "1.2.3", - "resolved": "https://registry.npmjs.org/word-wrap/-/word-wrap-1.2.3.tgz", - "integrity": "sha512-Hz/mrNwitNRh/HUAtM/VT/5VH+ygD6DV7mYKZAtHOrbs8U7lvPS6xf7EJKMF0uW1KJCl0H701g3ZGus+muE5vQ==", - "dev": true, - "engines": { - "node": ">=0.10.0" - } - }, "node_modules/wrap-ansi": { "version": "7.0.0", "resolved": "https://registry.npmjs.org/wrap-ansi/-/wrap-ansi-7.0.0.tgz", diff --git a/web/frontend/package.json b/web/frontend/package.json index 63effade15c..20f9d1a847d 100644 --- a/web/frontend/package.json +++ b/web/frontend/package.json @@ -1,6 +1,6 @@ { "name": "@viamrobotics/remote-control", - "version": "2.0.1", + "version": "2.0.18", "license": "Apache-2.0", "type": "module", "files": [ @@ -14,51 +14,53 @@ } }, "peerDependencies": { - "@improbable-eng/grpc-web": "~0.15.*", - "@viamrobotics/prime": "~0.2.*", - "@viamrobotics/rpc": "~0.1.*", - "@viamrobotics/sdk": "0.2.0-pre.1", - "google-protobuf": "~3.*.*", - "tailwindcss": "~3.3.*", - "three": "~0.152.*", - "trzy": "0.0.49" + "@improbable-eng/grpc-web": ">=0.15", + "@viamrobotics/prime": ">=0.5", + "@viamrobotics/rpc": ">=0.1", + "@viamrobotics/sdk": "0.3.2-rc.0", + "google-protobuf": ">=3", + "tailwindcss": ">=3.3", + "three": ">=0.155", + "trzy": "0.3.11" }, "devDependencies": { "@improbable-eng/grpc-web": "0.15.0", "@mdi/js": "7.2.96", - "@sveltejs/vite-plugin-svelte": "^2.4.2", - "@threlte/core": "^6.0.0-next.8", - "@threlte/extras": "^5.0.0-next.13", + "@sveltejs/vite-plugin-svelte": "^2.4.4", + "@threlte/core": "^6.0.3", + "@threlte/extras": "^5.1.0", "@types/google-protobuf": "3.15.6", - "@types/three": "0.152.1", - "@typescript-eslint/eslint-plugin": "^5.60.1", - "@typescript-eslint/parser": "^5.60.1", - "@viamrobotics/prime": "0.2.19", + "@types/three": "0.155.0", + "@typescript-eslint/eslint-plugin": "^6.3.0", + "@typescript-eslint/parser": "^6.3.0", + "@viamrobotics/prime": "0.5.2", "@viamrobotics/rpc": "0.1.37", - "@viamrobotics/sdk": "0.2.2", - "@viamrobotics/typescript-config": "^0.0.3", - "cypress": "12.16.0", - "eslint": "8.43.0", + "@viamrobotics/sdk": "0.3.2-rc.0", + "@viamrobotics/three": "^0.0.2", + "@viamrobotics/typescript-config": "^0.0.4", + "cypress": "12.17.3", + "eslint": "8.46.0", "eslint-import-resolver-custom-alias": "1.3.2", - "eslint-plugin-import": "2.27.5", + "eslint-plugin-import": "2.28.0", "eslint-plugin-promise": "6.1.1", - "eslint-plugin-svelte": "^2.31.1", + "eslint-plugin-svelte": "^2.32.4", "eslint-plugin-tailwindcss": "3.13.0", - "eslint-plugin-unicorn": "47.0.0", + "eslint-plugin-unicorn": "48.0.1", "google-protobuf": "3.21.2", "jshashes": "1.0.8", - "maplibre-gl": "^3.1.0", - "postcss": "8.4.24", - "svelte": "^4.0.0", - "svelte-check": "^3.4.4", - "tailwindcss": "3.3.2", - "three": "0.152.2", + "maplibre-gl": "^3.3.0", + "postcss": "8.4.27", + "svelte": "^4.1.2", + "svelte-check": "^3.4.6", + "svelte-inview": "^4.0.1", + "tailwindcss": "3.3.3", + "three": "0.155.0", "three-inspect": "0.3.4", - "trzy": "0.0.49", - "typescript": "5.1.3", - "vite": "4.3.9", - "vite-plugin-css-injected-by-js": "3.1.1", - "vitest": "0.32.2" + "trzy": "0.3.11", + "typescript": "5.1.6", + "vite": "4.4.9", + "vite-plugin-css-injected-by-js": "3.3.0", + "vitest": "0.34.1" }, "scripts": { "copy-prime-assets": "node ./scripts/copy-prime-assets.js", diff --git a/web/frontend/src/api/motion.ts b/web/frontend/src/api/motion.ts index b65c0151f3a..e3d3a5a2f0a 100644 --- a/web/frontend/src/api/motion.ts +++ b/web/frontend/src/api/motion.ts @@ -1,6 +1,6 @@ import { type Client, commonApi, motionApi, robotApi } from '@viamrobotics/sdk'; import { Struct } from 'google-protobuf/google/protobuf/struct_pb'; -import { getSLAMPosition } from './slam'; +import { getPosition } from './slam'; import { rcLogConditionally } from '@/lib/log'; export const moveOnMap = async (robotClient: Client, name: string, componentName: string, x: number, y: number) => { @@ -12,7 +12,7 @@ export const moveOnMap = async (robotClient: Client, name: string, componentName request.setName('builtin'); // set pose in frame - const lastPose = await getSLAMPosition(robotClient, name); + const lastPose = await getPosition(robotClient, name); const destination = new commonApi.Pose(); destination.setX(x * 1000); diff --git a/web/frontend/src/api/navigation.ts b/web/frontend/src/api/navigation.ts index 26f8d05f5c5..86c496e00bc 100644 --- a/web/frontend/src/api/navigation.ts +++ b/web/frontend/src/api/navigation.ts @@ -1,127 +1,79 @@ -import type { Client } from '@viamrobotics/sdk'; -import { commonApi, navigationApi } from '@viamrobotics/sdk'; -import { rcLogConditionally } from '@/lib/log'; - -export type NavigationModes = - | typeof navigationApi.Mode.MODE_MANUAL - | typeof navigationApi.Mode.MODE_UNSPECIFIED - | typeof navigationApi.Mode.MODE_WAYPOINT - -export type LngLat = { lng: number, lat: number } -export type Waypoint = LngLat & { id: string } - -export const setMode = async (robotClient: Client, name: string, mode: NavigationModes) => { - const request = new navigationApi.SetModeRequest(); - request.setName(name); - request.setMode(mode); - - rcLogConditionally(request); - - const response = await new Promise((resolve, reject) => { - robotClient.navigationService.setMode(request, (error, res) => { - if (error) { - reject(error); - } else { - resolve(res); - } - }); - }); - - return response?.toObject(); -}; - -export const setWaypoint = async (robotClient: Client, lat: number, lng: number, name: string) => { - const request = new navigationApi.AddWaypointRequest(); - const point = new commonApi.GeoPoint(); - - point.setLatitude(lat); - point.setLongitude(lng); - request.setName(name); - request.setLocation(point); - - rcLogConditionally(request); - - const response = await new Promise((resolve, reject) => { - robotClient.navigationService.addWaypoint(request, (error, res) => { - if (error) { - reject(error); - } else { - resolve(res); - } - }); - }); - - return response?.toObject(); -}; - -const formatWaypoints = (list: navigationApi.Waypoint[]) => { +/* eslint-disable no-underscore-dangle */ + +import * as THREE from 'three'; +import { NavigationClient, type Waypoint } from '@viamrobotics/sdk'; +import { ViamObject3D } from '@viamrobotics/three'; +import type { + BoxGeometry, CapsuleGeometry, Obstacle, SphereGeometry, +} from './types/navigation'; +import { notify } from '@viamrobotics/prime'; +export * from './types/navigation'; + +export const formatWaypoints = (list: Waypoint[]) => { return list.map((item) => { - const location = item.getLocation(); + const { location } = item; return { - id: item.getId(), - lng: location?.getLongitude() ?? 0, - lat: location?.getLatitude() ?? 0, + id: item.id, + lng: location?.longitude ?? 0, + lat: location?.latitude ?? 0, }; }); }; -export const getWaypoints = async (robotClient: Client, name: string): Promise => { - const req = new navigationApi.GetWaypointsRequest(); - req.setName(name); - - rcLogConditionally(req); - - const response = await new Promise<{ getWaypointsList(): navigationApi.Waypoint[] } | null>((resolve, reject) => { - robotClient.navigationService.getWaypoints(req, (error, res) => { - if (error) { - reject(error); - } else { - resolve(res); - } - }); - }); - - return formatWaypoints(response?.getWaypointsList() ?? []); -}; - -export const removeWaypoint = async (robotClient: Client, name: string, id: string) => { - const request = new navigationApi.RemoveWaypointRequest(); - request.setName(name); - request.setId(id); +export const getObstacles = async (navClient: NavigationClient): Promise => { + const list = await navClient.getObstacles(); - rcLogConditionally(request); + return list.map((obstacle, index) => { + const { location } = obstacle; - const response = await new Promise((resolve, reject) => { - robotClient.navigationService.removeWaypoint(request, (error, res) => { - if (error) { - reject(error); - } else { - resolve(res); - } - }); - }); - - return response?.toObject(); -}; - -export const getLocation = async (robotClient: Client, name: string) => { - const request = new navigationApi.GetLocationRequest(); - request.setName(name); - - rcLogConditionally(request); - - const response = await new Promise((resolve, reject) => { - robotClient.navigationService.getLocation(request, (error, res) => { - if (error) { - reject(error); - } else { - resolve(res); - } - }); + return { + name: `Obstacle ${index + 1}`, + location: { + lng: location?.longitude ?? 0, + lat: location?.latitude ?? 0, + }, + geometries: obstacle.geometriesList.map((geometry) => { + const { center } = geometry; + const pose = new ViamObject3D(); + const th = THREE.MathUtils.degToRad(center?.theta ?? 0); + pose.orientationVector.set(center?.oX, center?.oY, center?.oZ, th); + + if (geometry.box) { + const { dimsMm } = geometry.box; + + return { + type: 'box', + length: (dimsMm?.x ?? 0) / 1000, + width: (dimsMm?.y ?? 0) / 1000, + height: (dimsMm?.z ?? 0) / 1000, + pose, + } satisfies BoxGeometry; + + } else if (geometry.sphere) { + + return { + type: 'sphere', + radius: (geometry.sphere.radiusMm ?? 0) / 1000, + pose, + } satisfies SphereGeometry; + + } else if (geometry.capsule) { + const { capsule } = geometry; + + return { + type: 'capsule', + radius: (capsule?.radiusMm ?? 0) / 1000, + length: (capsule?.lengthMm ?? 0) / 1000, + pose, + } satisfies CapsuleGeometry; + + } + + notify.danger('An unsupported geometry was encountered in an obstacle', JSON.stringify(geometry)); + throw new Error( + `An unsupported geometry was encountered in an obstacle: ${JSON.stringify(geometry)}` + ); + }), + } satisfies Obstacle; }); - - return { - lat: response?.getLocation()?.getLatitude() ?? 0, - lng: response?.getLocation()?.getLongitude() ?? 0, - }; }; diff --git a/web/frontend/src/api/robot.ts b/web/frontend/src/api/robot.ts index 35ff4c60063..86cbfb48490 100644 --- a/web/frontend/src/api/robot.ts +++ b/web/frontend/src/api/robot.ts @@ -1,5 +1,4 @@ -import type { Client } from '@viamrobotics/sdk'; -import { robotApi } from '@viamrobotics/sdk'; +import { type Client, robotApi } from '@viamrobotics/sdk'; export const getOperations = async (robotClient: Client) => { const request = new robotApi.GetOperationsRequest(); diff --git a/web/frontend/src/api/slam.ts b/web/frontend/src/api/slam.ts index 18c7bf1c641..5b712468783 100644 --- a/web/frontend/src/api/slam.ts +++ b/web/frontend/src/api/slam.ts @@ -1,64 +1,6 @@ import { type Client, slamApi } from '@viamrobotics/sdk'; -import { rcLogConditionally } from '@/lib/log'; -const concatArrayU8 = (arrays: Uint8Array[]) => { - const totalLength = arrays.reduce((acc, value) => acc + value.length, 0); - const result = new Uint8Array(totalLength); - - let length = 0; - - for (const array of arrays) { - result.set(array, length); - length += array.length; - } - - return result; -}; - -export const getPointCloudMap = (robotClient: Client, name: string) => { - const request = new slamApi.GetPointCloudMapRequest(); - request.setName(name); - rcLogConditionally(request); - - const chunks: Uint8Array[] = []; - const stream = robotClient.slamService.getPointCloudMap(request); - - stream.on('data', (response) => { - const chunk = response.getPointCloudPcdChunk_asU8(); - chunks.push(chunk); - }); - - return new Promise((resolve, reject) => { - stream.on('status', (status) => { - if (status.code !== 0) { - const error = { - message: status.details, - code: status.code, - metadata: status.metadata, - }; - reject(error); - } - }); - - stream.on('end', (end) => { - if (end === undefined) { - const error = { message: 'Stream ended without status code' }; - reject(error); - } else if (end.code !== 0) { - const error = { - message: end.details, - code: end.code, - metadata: end.metadata, - }; - reject(error); - } - const arr = concatArrayU8(chunks); - resolve(arr); - }); - }); -}; - -export const getSLAMPosition = async (robotClient: Client, name: string) => { +export const getPosition = async (robotClient: Client, name: string) => { const request = new slamApi.GetPositionRequest(); request.setName(name); diff --git a/web/frontend/src/api/types/navigation.ts b/web/frontend/src/api/types/navigation.ts new file mode 100644 index 00000000000..9518d360c72 --- /dev/null +++ b/web/frontend/src/api/types/navigation.ts @@ -0,0 +1,40 @@ +import type { navigationApi } from '@viamrobotics/sdk'; +import type { ViamObject3D } from '@viamrobotics/three'; + +export type NavigationModes = + | typeof navigationApi.Mode.MODE_MANUAL + | typeof navigationApi.Mode.MODE_UNSPECIFIED + | typeof navigationApi.Mode.MODE_WAYPOINT + +export type LngLat = { lng: number, lat: number } +export type Waypoint = LngLat & { id: string } + +type BaseGeometry = { + pose: ViamObject3D +} + +export type CapsuleGeometry = BaseGeometry & { + type: 'capsule'; + radius: number; + length: number; +} + +export type SphereGeometry = BaseGeometry & { + type: 'sphere'; + radius: number; +} + +export type BoxGeometry = BaseGeometry & { + type: 'box'; + length: number; + width: number; + height: number; +} + +export type Geometry = BoxGeometry | SphereGeometry | CapsuleGeometry + +export interface Obstacle { + name: string; + location: LngLat, + geometries: Geometry[]; +} diff --git a/web/frontend/src/components/arm/index.svelte b/web/frontend/src/components/arm/index.svelte index b402ab02d76..3adcf449c81 100644 --- a/web/frontend/src/components/arm/index.svelte +++ b/web/frontend/src/components/arm/index.svelte @@ -1,7 +1,6 @@ + +
+ + + {#if geometry.type === 'box'} + + {:else if geometry.type === 'capsule'} + + {:else if geometry.type === 'sphere'} + + {/if} +
diff --git a/web/frontend/src/components/navigation/components/input/lnglat.svelte b/web/frontend/src/components/navigation/components/input/lnglat.svelte new file mode 100644 index 00000000000..55729252ce5 --- /dev/null +++ b/web/frontend/src/components/navigation/components/input/lnglat.svelte @@ -0,0 +1,56 @@ + + +
+ + + +
diff --git a/web/frontend/src/components/navigation/components/input/orientation.svelte b/web/frontend/src/components/navigation/components/input/orientation.svelte new file mode 100644 index 00000000000..35d9658d00e --- /dev/null +++ b/web/frontend/src/components/navigation/components/input/orientation.svelte @@ -0,0 +1,32 @@ + + +{#if view === '2D'} + +{:else if view === '3D'} + +{/if} diff --git a/web/frontend/src/components/navigation/components/map.svelte b/web/frontend/src/components/navigation/components/map.svelte index a70ba47cc9d..c3bde8399e0 100644 --- a/web/frontend/src/components/navigation/components/map.svelte +++ b/web/frontend/src/components/navigation/components/map.svelte @@ -1,55 +1,98 @@