From b7e0686e72414291a496b369dae1a30eb5acd7e7 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 9 Jun 2023 15:32:11 -0700 Subject: [PATCH 01/39] Humble sync 6 June 9: 1.1.7 (#3616) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Option allowing to use simple lookupTransform API (#3412) * Option allowing to use simple lookupTransform API ignoring time shifts between source and base frame during the movement * Refine comments * Fix wrong warning message format (#3416) * Fix wrong warning message format (Closes #3415) * fix code formatting * nav2_dwb_controller: add forward_prune_distance parameter (#3374) Until now, the prune_distance was used as distance threshold to shorten the upcoming path when shorten_transformed_plan was enabled. However, the prune and shortening mechanisms are de-correlated mechanisms. One could wish to use a different shortening distance for upcoming points, than the prune distance used for passed points. For this reason, a new parameter "forward_prune_distance" was added. * Fix service_name for server_name in cancel assisted teleop node * Fix mask coordinates calculation in worldToMask (#3418) * Remove goal checker default from follow path node * Correct CostmapFilters copyrights (#3423) * Correct the parameter description for AMCL (#3451) Signed-off-by: Trung Kien * Add default service name to BTServiceNode (#3448) * Add default service name to BtServiceNode * docstring * fix initialization-list order * Update nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp Co-authored-by: Steve Macenski --------- Co-authored-by: Steve Macenski * Fix Typos (#3452) * removing galactic from table as EOL (#3460) * Support for Dev Containers and Codespaces (#3457) * Alias image tag over current branch name * Duplicate build and push steps for dev tag * Alias image tag over current branch name * Modify build and push steps for dev tag * Build and push dev tag first to not cache from stale stages as otherwise caching from multple regestry images seems error prone * Revert "Build and push dev tag first" as otherwise the build failer durring the dev tag could then still block build of the main tag This reverts commit 12dd5b1a4e3f37847e6333b9e9ae9ef480a80623. * Cache from multple reference images while giving layers from the main tag priority this assumes that cache-from prioritizes firstly listed references https://github.com/moby/buildkit/blob/0ad8d61575be009ce6478edf1d85716849c8ff1a/solver/llbsolver/bridge.go#L92 * Cache tests in dev image as well colcon cache can then skip tests for uneffected packages * Add devcontainer.json * Ignore doc for image builds * Add more extensions * Change workspaceFolder to root src path to avoid auto generating .vscode folder in repo created by ms-iot.vscode-ros extension upon configuring ros packages with c_cpp_properties.json * Enable features for github-cli * Add docs about codespaces and have it opened when starting codespaces * Update update_ci_image.yaml to fix duplicate step ids and add workflow file to push paths * Patch CI actions and Dockerfiles (#3468) * Unset default value for FAIL_ON_TEST_FAILURE as unsetting it via --build-arg seems unreliable https://github.com/docker/compose/issues/3608 * Use build arg default for failing on test failers * Update from deprecated set-output commands https://github.blog/changelog/2022-10-11-github-actions-deprecating-save-state-and-set-output-commands/ * Use Codespaces prebuilds (#3470) * Add commands to devcontainer * Set builtin bash to be safe https://gist.github.com/mohanpedala/1e2ff5661761d3abd0385e8223e16425 https://vaneyckt.io/posts/safer_bash_scripts_with_set_euxo_pipefail/ * Setup workspace on create * Revert use of set -u for bash don't raise error due to variables otherwise colcon setup.sh chokes from using an unbounded path variable * Add safe.directory for git config otherwise colcon cache errors out because of issues with git due to complex user mapping magic that vscode does with devcontainers https://stackoverflow.com/questions/72978485/git-submodule-update-failed-with-fatal-detected-dubious-ownership-in-repositor also used by Moveit2: https://github.com/ros-planning/moveit2/pull/1994 https://github.blog/2022-04-12-git-security-vulnerability-announced/ * Set env using remoteEnv instead of inlining them in scripts * Revert to using the main tag now that the tester stage has been replicated with the new devcontainer script commands instead * formating * Scrap `-dev` image tag and use codspaces prebuilds instead * Build incrementally from update content command by copying the build workspace steps from circleci config * Adapt the build workspace steps for bash * Fix for different ceres isinf() API (#3471) * Fixing name of security launch file * Clean up pending service client request on interrupt/timeout (#3479) Signed-off-by: Øystein Sture * Added str cast to parse int (#3486) Co-authored-by: antoniomarangi * Add flag to not send request in BTServiceNode (#3431) * Add flag to not send request in BTServiceNode * rename goal to request * Fail if should not send goal * Update nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp Co-authored-by: Steve Macenski * Update nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp Co-authored-by: Steve Macenski * . * fix linter * fix CI --------- Co-authored-by: Steve Macenski * Prepare test results to only use junit/xunit schema (#3441) * Set ctest arg to output junit To try and help CircleCI to parse the output files https://stackoverflow.com/a/70774733/2577586 * Replace the original Test.xml by outputting the junit to the same filename Context: https://github.com/colcon/colcon-cmake/blob/8f1b92a190b2ad4289ecf837c3200d540c13fdd9/colcon_cmake/task/cmake/test.py#L133 * Fix default formatting to a list WARNING:colcon.colcon_defaults.argument_parser.defaults:Default value 'ctest-args' for parser 'test' should be a list, not: --output-junit Test.xml * Revert junit file name https://circleci.com/docs/collect-test-data/#ctest-for-c-cxx-tests * Fine and rename ctest summary Test.xml * Fix find path * Simplify extention renaming * Copy ctest junit file into test_results so that they can be stored by CI * Revert ctest config modifications * Prepare Test Results by removing Test.xml generated by ctest to simplify fix for circleci * Reorder storage of test result artifacts before Test.xml files are removed so that they can still be archived and viewed for later * Use find command * Container retention via version tagging (#3491) * Use github action expression syntax to alias over github repository name * Tag by version instead of by timestamp * Avoid pushing untagged image to GHCR by setting provenance to false now that provenance is enabled by default as of v4 of docker/build-push-action - https://github.com/docker/build-push-action/pull/781 - https://github.com/docker/build-push-action/issues/778 * Use checkout action to set version output (#3492) Otherwise there is no source code to use to set the version output. Fixes: #3491 * Change directory to inside checked out repo (#3493) or relative path under $GITHUB_WORKSPACE that actions/checkout places the repository * Write and read from correct output mapping (#3494) * Revert "Change directory to inside checked out repo (#3493)" This reverts commit 332c1fb07bd787bab8a8eeea5fc896a944bb54d8. * Add `version` to outputs for check step and use output from `check` id * Use output from check_ci_files job * updating world in simple commander for TB3 package change (#3495) * Ensure version output is always set (#3503) even when github.event_name != 'push' by moving run step to same job as build-push action. Also set context path provided by checkout action to avoid future nonintuitive behavoir using default Git context, even when the checkout action appears to be being used. - https://github.com/docker/build-push-action#git-context - https://github.com/docker/build-push-action#path-context * Add labels to pushed image versions (#3505) using Pre-Defined Annotation Keys as defined by The OpenContainers Annotations Spec - https://specs.opencontainers.org/image-spec/annotations/#pre-defined-annotation-keys - https://docs.github.com/en/actions/using-workflows/events-that-trigger-workflows - https://docs.github.com/en/rest/repos/repos?apiVersion=2022-11-28#get-a-repository * Typo README.md (#3506) * [Velocity smoother] Set zeros command if timeout (#3512) * Set zeros command if timeout * Fix lint * Fix gtest increase time to allow deceleration * Always update last_cmd_ * Revert test modif * remove test logs * Fix paste error * Update velocity_smoother.cpp * Update velocity_smoother.cpp * Improve Dev Container ergonomics (#3482) * Install and enable bash autocompletion by using apt durring on create command and by copying skelton .bashrc file that sources it by default * Edit apt for autocomplete by disabling docker-clean from containerized ubuntu * Add ROS2 Ament Task Provider extension Provides tasks and problem matchers for ROS2 projects using ament https://marketplace.visualstudio.com/items?itemName=althack.ament-task-provider * Source underlay for extentions to allow them to find the path to ros binaries such as ament_cpplint needed for althack.ament-task-provider * Target new dever stage in Dockerfile * Reduce need for internet after image build by installing developer dependencies earlier * Edit apt caching before apt updating * Source underlay systemwide this is a hacky workarround to ensure VS Code can run ShellExecution tasks with the ros envorment included in PATH otherwise, postponing this to the on-create-command results in vscode extentions not finding system installed ros commands this also works for all user shells regardless of how devontainers could change the user * Postpone bashrc setup to postCreateCommand once the dev container has been assigned to a user for the first time * Cleanup onCreateCommand as we don't use ros_entrypoint.sh for development and so it doesn't really need to be updated * Quite down the logs when building devcontainer * Formatting * Add refrence ccp properties config file generated from the vscode ROS extention but with the hardcoded paths in includePath deleted * Update version of cppStandard for ROS Rolling * Update workspaceFolder to use new .vscode folder * Mount ccache directory to volume to speed up rebuilding devcontainer whenever onCreateCommand is triggered because of modifications to .devcontainer/ files * Avoid use of containerEnv to express ccache direcotry as doing so is not possable, for more info: - https://stackoverflow.com/a/75759647/2577586 - https://github.com/microsoft/vscode-remote-release/issues/7147#issuecomment-1237779733 Just target a path in the temp direcotry instead * Stage auto generated includePath * Remove workspace install from include path except for autogenerated headers from message packages * Avoid hardcoded path to sorce folder * Avoid hardcoded path to install folder but this is still rather fragile as the reletive path between workspaceFolder and the colcon workspace isn't fixed * Sort list of paths * Remove cpp properties configuration as it seems it's existance prevents autoupdating the includePaths property unless user manually runs the vscode command `>ROS:Update C++ Properties` https://github.com/ms-iot/vscode-ros/blob/47d8f14f4ec0498cd9e8381e6fcc5f47abb340f2/src/extension.ts#L71 and even when this command is invoked it blows aways any customizated properties anyhow issue about wrong cppStandard tracked here: https://github.com/ms-iot/vscode-ros/issues/818 * Fix typo to move docker-clean from loaded config path * fix data race: addFilter() and resizeMap() can be executed concurrently (#3518) Co-authored-by: Dirk Braunschweiger * fix data race: prohibit resizeMap() during plugin/filter initialization (#3522) Co-authored-by: Dirk Braunschweiger * Mount overlay workspace into Dev Container via volume (#3524) * Add volume for overlay to avoid rebuilding it from scratch whenever the dev container is rebuilt this saves startup time locally when fiddling with the configs * Append devcontainerId to volume name to avoid conflicts with other devcontainers note that devcontainerId is stable across rebuilds - https://containers.dev/implementors/json_reference/#variables-in-devcontainerjson * Call updateContentCommand from onCreateCommand to deduplicate scripts and keep setup DRY given the addition of a mounted overlay volume which could include a prebuilt colcon workspace well before the dev container is created/rebuilt * Comment out colcon clean from setup to avoid unintentional removal of built packages from the persistent overlay workspace volume. Users can uncomment the line locally or simply remove the overlay workspace volume if they want to rebuild packages from scratch. * Format json * Add headless and use_rviz LaunchConfigurations to demo launch files (#3527) * Add headless and use_rviz LaunchConfigurations in nav2_simple_commander demo launch files for whether to start rviz or gzclient to simplify their use in headless environments * Fix headless logic to match tb3_simulation_launch.py for launch arg consistency * Fix State-Lattice planner crashes due to FP precision loss (#3531) * Fix State-Lattice planner crashes due to FP precision loss * Move testcase comment * Add PoseProgressChecker (#3530) * add rotation progress checker * clean include * add stopped goal checker reset test * add rotation progress checker tests * uncrustify * better name: PoseProgressChecker instead of RotationProgressChecker * camelCase * uncrustify * rename in tests * more rename * simplify parentheses * faster and better tests --------- Co-authored-by: Guillaume Doisy * [velocity_smoother] Fix accel and deccel inverted for negative speeds (#3529) * fix inverted accel / deccel * handle speed through 0.0 * add applyConstraints tests * fold logic * same logic in findEtaConstraint * lint * Update nav2_velocity_smoother/src/velocity_smoother.cpp * Update nav2_velocity_smoother/src/velocity_smoother.cpp * findEtaConstraint tests * space * lint * typos * comment typos --------- Co-authored-by: Guillaume Doisy Co-authored-by: Steve Macenski * Enable Visualizations for Dev Container (#3523) * Add visualizer stage to install demo dependencies * Install foxglove * Install gzweb * Add hack for resolvable mesh URIs located by the aws SDL model files - https://github.com/aws-robotics/aws-robomaker-small-warehouse-world/pull/24 * Revert hack and use fork that fixes issues with deploy.sh - https://github.com/osrf/gzweb/pull/248 * Update target stage to visualizer * Comment out gzclient and rviz for debugging * Add hack for resolvable mesh URIs as migrating the python3 scripts still hasn't resolved the issue * Reorder stages for readability by keeping sequential builder and tester stages adjacent while keeping tester stage the default exported target * fix typo * Install gdb for launching ros launch files using the ROS VS Code extension - https://github.com/ms-iot/vscode-ros/issues/588 * Add vscode tasks file * Add Start Gzweb task * Add Start Foxglove tasks for bridge and studio * Add Start Foxglove compound task using dependsOn * Set default problemMatcher to empty to avoid nagging the user to select one as currently none really support our use case * Source overlay before running foxglove_bridge to ensure nav2 message types are defined by inlining all args into command and sourcing workspace setup * Formatting * Generalize and simplify hack * Generalize gazebo model discovery * Patch gzserver to run headless using xvfb to avoid host/platform specific x11 quirks exposed by vscode automatic x11 forwarding This is needed to provide gazebo a virtual frame buffer as it still need one after all these years. This also avoids the need modifying launch files to call xvfb-run - https://github.com/microsoft/vscode-remote-release/issues/8031 - https://github.com/gazebosim/gazebo-classic/issues/1602 * Set isBackground for start tasks * Add stop tasks * Add restart foxglove task * Switch to shell for commanding pkill to gracefully return 0 when process is not running allowing sequence of dependsOn tasks to run such as for the restart tasks * Add icons to tasks for readability * Add restart gzweb task * Add global start, stop, and restart tasks for all background visualization tasks * Formatting * Hide tasks users need not run manually to avoid cluttering up the run task quick pick * Shorten label for background tasks so they succinctly show from the running task list * Show global start and stop visualizations tasks as they may be too helpful to hide * Revert "Comment out gzclient and rviz for debugging" This reverts commit 0addae2a1ee70c5771055c5dd8fa050af438b896. * Add --ipc=host to runArgs to enable shared memory transport - https://community.rti.com/kb/communicate-between-two-docker-containers-using-rti-connext-dds-and-shared-memory * Add --pid=host to runArgs to simplify discovery - https://community.rti.com/kb/communicate-between-two-docker-containers-using-rti-connext-dds-and-shared-memory * Add to runArgs to simplify debugging - https://code.visualstudio.com/docs/devcontainers/create-dev-container#_use-docker-compose * Add comments * Comment out runArgs unintended side effects or cross talk between containers by default also avoids interfering with vscode's X11 forwarding * [nav2_planner] Fix costmap thread reset on cleanup (#3548) * remove costmap thread reset on cleanup * Init costmap thread in on_configure method * Move costmap_thread init in on_configure method * Add IsBatteryChargingCondition (#3553) * Add IsBatteryChargingCondition * Minor fixes in battery charging and add testing * Fix format * Added isBatteryChargingCondition BT node to params * Impl noise filtering layer in the costmap_2d (#2567) Signed-off-by: ryzhikovas * Improve Dev Container Web App Visualization (#3551) * Add Caddyfile to reverse proxy websockets in an attempt to avoid authentication tokens in headers when forwarding ports from codespaces via web interface - https://docs.github.com/en/codespaces/developing-in-codespaces/forwarding-ports-in-your-codespace#using-command-line-tools-and-rest-clients-to-access-ports - https://caddyserver.com/docs/quick-starts/reverse-proxy - https://caddy.community/t/caddy-v2-how-to-proxy-websoket-v2ray-websocket-tls/7040/13 * Update caddy related tasks * Rename Gzweb task to Gzweb Bridge to make room for more gzweb tasks * Add Gzweb Client Task * Add Caddyfile to reverse proxy websockets now for Gzweb * Specify config file to avoid crosstalk between caddy stop commands * Fix reverse proxy for websockets by correcting matcher using headers as websocket request header value is lowercase for gzweb and foxglove * Comment out log output files for debugging * Simplify tasks by removing client tasks * Stop tasks by using terminate via the workbench.action.tasks.terminate command * Move Caddyfile * Add Web Server tasks * Move Caddyfile * Update log output file path * Update root path * Update reverse_proxy for both gzweb and foxglove by using the path argument for respective matchers - https://caddyserver.com/docs/caddyfile/matchers#path-matchers * Use snippets to keep Caddyfile DRY - https://caddyserver.com/docs/caddyfile/concepts#snippets * Use rewrite to catch trailing slash as file_server defaults do not correct reverse_proxy. This make typing the websocket URL more forgiving - https://caddyserver.com/docs/caddyfile/patterns#trailing-slashes * Improve websocket snippet to keep Caddyfile DRY * Use header_regexp for case-insensitive matching given web port forwarding from Codespaces is odd and rewrites the value of this header field to lowercases even when local browser request is sent as `Upgrade` * Add helper index page to web server to link to web apps for reverse proxy * Limit templates to fix gzweb by adding matcher for only root index otherwise gzweb's own index.html gets overwritten * Add comments to Cadyfile to document tricky configuration * Stage working redirect * Simplify index.html * Add helper redirect to simplify foxglove to set the respective queries values to automate websocket setup, and ensure the websocket schema matches the https request * Avoid hardcoding port number * Clean up comments * Use header to compute redirect to take into account requesting forwarding or more codespace port forwarding shenanigans * Use shorthand placeholders - https://caddyserver.com/docs/caddyfile/concepts#placeholders * Formatting * Keep trailing slash to stay consistent with caddy file_server directive that serves a 308 Permanent Redirect for both foxglove and gzweb paths anyway * Refactor matcher logic to account for requests either from host ports from local dev containers or forwarded requests from codespace web port forwarding * Split snippet into globals for composability * Update comments * Add Placeholders for debugging * Use tables to center * Use github markdown - https://github.com/sindresorhus/github-markdown-css * Simplify vars * Rename vars * Revert "Rename vars" as dotted var names do not work in Caddyfile This reverts commit 3e2d1b3fe30f8a4ffb5134fc2f6f5cffd574bcdc. * Add System Monitor to debug CPU load and memory issues * Update headings * Update layout * Update layout * Add Foxglove layout for Nav2 * Symlink assets folder for web server * Fetch Foxglove layout using layoutUrl a new parameter to load layout json data from URL - https://github.com/orgs/foxglove/discussions/217 * Cleanup * Use fork to fetch Foxglove layout using layoutUrl until this PR is merged: - https://github.com/foxglove/studio/pull/5987 * Update Caddyfile to handle relative root by using local srv folder * Inject mobile view html tags using the caddy replace module - https://caddyserver.com/docs/modules/http.handlers.replace_response - https://github.com/caddyserver/replace-response * Simplify Caddyfile * Use snippet for apps * Simplify Caddyfile * Simplify Caddyfile * Build caddy using custom modules * Remove unused symlinks * Add comments * Use environment and defined variables for config to avoid hard coded paths * Add FoxgloveUrl to vars for reuse in templates * Fix trailing slash for DataSourceUrl * Use exec to run gzserver with xvfb to prevent ros launch from orphaning process and ensure gzserver receives SIGTERM signal given gzserver often hangs after only SIGINT - https://unix.stackexchange.com/a/196053/213124 * Update redirect for foxglove to redirect from path /foxglove/autolayout * Add redirect for foxglove to redirect from path /foxglove/autoconnect but does not use LayoutUrl as to not change from cached layout * Use web app manifest to set display as standalone - https://web.dev/add-manifest/ - https://developer.mozilla.org/en-US/docs/Web/Manifest * Template manifest files to embed host info into app name * Add manifests for other web apps * Add shortcuts for Foxglove - https://developer.mozilla.org/en-US/docs/Web/Manifest/shortcuts - https://web.dev/app-shortcuts/ * Format * Update comments * Revert use of fork * Remove debug directive * Improve usability of PWAs in Dev Containers (#3576) * Add WIP icons * Add WIP icons for gzweb * Add WIP icons for glances * Set cross origin to use credentials ensuring auth cookie is included in request header when requesting for web app manifest file thus avoiding CORS policy violations in browser when accessing forwarded codespaces ports from the web > The request for the manifest is made without credentials (even if it's on the same domain), thus if the manifest requires credentials, you must include `crossorigin="use-credentials"` in the manifest tag. - https://web.dev/add-manifest/ - https://stackoverflow.com/a/57184506/2577586 * Use ReqHost variable in templates to account for X-Forwarded-Host value in header * Delete duplicate manifest * Set id property in app manifests so we can address them independently from their start_url - https://developer.chrome.com/blog/pwa-manifest-id/ * Ensure apps are uniquely identifies by adding trailing slash to id and thus different URI directories * Refactor root landing page into nav2 app by moving page file into nav2 sub folder adding root redirect pointing to /nav2/ and updating html, markdown, manifest files respectively * Fix https detection for Caddy reverse proxies by also checking X-Forwarded-Proto in request header * Remove unnecessary files * Prune smaller images * Prune duplicate icon * Clean up html tags * Update manifest icons * Rename icons * Revert "Prune duplicate icon" This reverts commit 571040173ca83716dfd2f6d5db4b351389a557a8. * Add back favicon for shortcut * Add self index for completeness and bookmarking * Simplify icon linking * Delete binary files * Fix hyperlink path * Include image files using gitattributes to track these binary files via git LFS * Add icons using git lfs * Standardized all icon paths * Use external links for icons to avoid the need for using git LFS although this is a bit of a hack * Stage any and maskable icons * Use any and masked icons * Set colors to match maskable icon colors * Update icon * Use lossless compression without removing background - https://shortpixel.com/online-image-compression * Use WebP instead of PNG for smaller file sizes - https://en.wikipedia.org/wiki/WebP * Move icons into icons folder * Use _SRV environment variables for service paths * Download media files from github during docker image build to avoid adding always online dependencies when creating or starting dev containers * Delete media icons from git repo now that we download media from anonymized URLs on github - https://docs.github.com/en/get-started/writing-on-github/working-with-advanced-formatting/attaching-files * Add comments * Enable file browsing for non app paths for remote debugging of media and asset files * Consolidate assets into single folder * Add links for file browser paths to Server Diagnostics * Delete unused symlink * Update landing page to match manifest by including same shortcuts and start url * Patch gzweb to disable modelList avoiding 404s for thumbnails as they are hardcoded into js * Update comments * Simplify Caddyfile by reverting to symlinking but add ROOT_SRV env for custom overriding * Loop over nav2 srv folders when symlinking to generalize over folder names * Add matcher for file browsing root directory while still redirecting to nav2 app by default * Use placeholders for root variable to consolidate env default fallback settings e.g `:/srv` * Promote file browser in Nav2 app shortcuts * Fix and update SRV envs * Postpone symlinking for Nav2 web app to when post-create-command script then runs given full repo is not copied into builder stage in Dockerfile. While this could be postponed to update-content-command leaving it here avoids blowing user changes after the container has been created or modified. * Add guard to check if srv folder exists * Add refresh rate shortcuts to glances * Add file browser shortcut to nav2 * Set scope for nav2 PWA to root to allow for opening child apps inside nav2 app * Display child apps in fullscreen mode by default as users can still open them in standalone via nav2 app given the nav2 app's scope is the parent root path * Update shortcuts and landing page * Document PWA scope and installation order when using Nav2 PWA scoped as root * Revert setting scope for nav2 PWA to root path as adding file browser shortcut to nav2 PWA is not worth the trouble of having to explain installation order caveats and URL launch behavior. File browser shortcut is still accessible from inside nav2 pwa launcher but merely displays in browser preview given root / is out of scope for /nav2/ * Update server diagnostics for troubleshooting * Verify checksum of archive before extraction incase anonymized URL changes expected archive * Fix the condition in ackerman motion model constraints (#3581) * Fix the condition in ackerman motion model constraints * Fix ackerman motion model tests * Fix another ackerman motion model test * Fix broken symlink for gzweb (#3585) to load world models * Fix broken link to contributing guidelines (#3587) The original URL (https://navigation.ros.org/contribute/index.html) seems not to exist, returning an HTTP 404. Hence, I've replaced the link with a page that seems most relevant. * Adding Our Sponsors - May 2023 (#3593) * adding our sponsors - may 2023 * adding blurb * adding links * adding links * adding links * adding Open Nav * Add CostmapFilterInfoServer as a component (#3596) * Resolve #3532: reset i (#3597) * [MPPI] empty path_follow_critic proper fix (#3599) * [MPPI] empty path_follow_critic proper fix * fix linting issue --------- Co-authored-by: Guillaume Doisy Co-authored-by: Steve Macenski * bumping humble to 1.1.7 for release --------- Signed-off-by: Trung Kien Signed-off-by: Øystein Sture Signed-off-by: ryzhikovas Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Co-authored-by: Jose Luis Blanco-Claraco Co-authored-by: DylanDeCoeyer-Quimesis <102609575+DylanDeCoeyer-Quimesis@users.noreply.github.com> Co-authored-by: Trung Kien Co-authored-by: HovorunB <87417416+HovorunB@users.noreply.github.com> Co-authored-by: Tony Najjar Co-authored-by: Ruffin Co-authored-by: Øystein Sture Co-authored-by: mrmara <48493979+mrmara@users.noreply.github.com> Co-authored-by: antoniomarangi Co-authored-by: Tony Najjar Co-authored-by: Griswald Brooks Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: Dirk Braunschweiger <1677757+dirkmb@users.noreply.github.com> Co-authored-by: Dirk Braunschweiger Co-authored-by: Guillaume Doisy Co-authored-by: Guillaume Doisy Co-authored-by: Alberto Tudela Co-authored-by: ryzhikovas Co-authored-by: Alexandr Buyval Co-authored-by: Hyung-Taik Choi Co-authored-by: Filipe Cerveira --- .circleci/config.yml | 9 +- .devcontainer/caddy/Caddyfile | 132 ++ .../caddy/srv/assets/foxglove/manifest.json | 40 + .../srv/assets/foxglove/nav2_layout.json | 463 +++++++ .../caddy/srv/assets/glances/manifest.json | 40 + .../caddy/srv/assets/gzweb/manifest.json | 23 + .../caddy/srv/assets/nav2/manifest.json | 23 + .../caddy/srv/nav2/github-markdown.css | 1102 +++++++++++++++++ .devcontainer/caddy/srv/nav2/index.html | 36 + .devcontainer/caddy/srv/nav2/index.md | 51 + .devcontainer/devcontainer.json | 61 + .devcontainer/on-create-command.sh | 12 + .devcontainer/post-create-command.sh | 18 + .devcontainer/update-content-command.sh | 60 + .dockerignore | 7 +- .github/workflows/update_ci_image.yaml | 48 +- .vscode/tasks.json | 179 +++ Dockerfile | 95 +- README.md | 83 +- doc/development/codespaces.md | 23 + doc/sponsors_may_2023.png | Bin 0 -> 160698 bytes nav2_amcl/package.xml | 2 +- nav2_amcl/src/amcl_node.cpp | 4 +- nav2_behavior_tree/CMakeLists.txt | 3 + .../nav2_behavior_tree/bt_action_node.hpp | 3 + .../nav2_behavior_tree/bt_service_node.hpp | 22 +- .../is_battery_charging_condition.hpp | 81 ++ nav2_behavior_tree/nav2_tree_nodes.xml | 8 +- nav2_behavior_tree/package.xml | 2 +- .../is_battery_charging_condition.cpp | 66 + .../test/plugins/condition/CMakeLists.txt | 4 + .../condition/test_is_battery_charging.cpp | 130 ++ nav2_behaviors/package.xml | 2 +- nav2_bringup/package.xml | 2 +- .../params/nav2_multirobot_params_1.yaml | 1 + .../params/nav2_multirobot_params_2.yaml | 1 + nav2_bringup/params/nav2_params.yaml | 97 +- nav2_bt_navigator/package.xml | 2 +- nav2_bt_navigator/src/bt_navigator.cpp | 3 +- .../collision_monitor_node.hpp | 5 +- .../nav2_collision_monitor/pointcloud.hpp | 5 +- .../include/nav2_collision_monitor/range.hpp | 5 +- .../include/nav2_collision_monitor/scan.hpp | 5 +- .../include/nav2_collision_monitor/source.hpp | 8 +- nav2_collision_monitor/package.xml | 2 +- .../params/collision_monitor_params.yaml | 1 + .../src/collision_monitor_node.cpp | 18 +- nav2_collision_monitor/src/pointcloud.cpp | 31 +- nav2_collision_monitor/src/range.cpp | 31 +- nav2_collision_monitor/src/scan.cpp | 31 +- nav2_collision_monitor/src/source.cpp | 6 +- nav2_collision_monitor/test/sources_test.cpp | 94 +- .../nav2_common/launch/rewritten_yaml.py | 2 +- nav2_common/package.xml | 2 +- nav2_constrained_smoother/CMakeLists.txt | 4 + .../smoother_cost_function.hpp | 2 +- .../nav2_constrained_smoother/utils.hpp | 12 +- nav2_constrained_smoother/package.xml | 2 +- nav2_controller/CMakeLists.txt | 7 +- .../plugins/pose_progress_checker.hpp | 67 + .../plugins/simple_progress_checker.hpp | 8 +- nav2_controller/package.xml | 2 +- nav2_controller/plugins.xml | 5 + .../plugins/pose_progress_checker.cpp | 97 ++ .../plugins/simple_progress_checker.cpp | 12 +- nav2_controller/plugins/test/CMakeLists.txt | 2 +- nav2_controller/plugins/test/goal_checker.cpp | 10 + .../plugins/test/progress_checker.cpp | 127 +- nav2_controller/src/controller_server.cpp | 2 + nav2_core/package.xml | 2 +- nav2_costmap_2d/CMakeLists.txt | 1 + nav2_costmap_2d/costmap_plugins.xml | 3 + .../costmap_filters/costmap_filter.hpp | 5 +- .../include/nav2_costmap_2d/denoise/image.hpp | 207 ++++ .../denoise/image_processing.hpp | 1051 ++++++++++++++++ .../include/nav2_costmap_2d/denoise_layer.hpp | 131 ++ .../nav2_costmap_2d/layered_costmap.hpp | 6 +- nav2_costmap_2d/package.xml | 2 +- .../costmap_filters/costmap_filter.cpp | 19 +- nav2_costmap_2d/plugins/denoise_layer.cpp | 211 ++++ nav2_costmap_2d/plugins/voxel_layer.cpp | 7 +- nav2_costmap_2d/src/costmap_2d_ros.cpp | 12 + nav2_costmap_2d/src/layered_costmap.cpp | 6 + nav2_costmap_2d/test/unit/CMakeLists.txt | 10 + .../test/unit/costmap_filter_test.cpp | 105 ++ .../test/unit/denoise_layer_test.cpp | 519 ++++++++ .../test/unit/image_processing_test.cpp | 514 ++++++++ nav2_costmap_2d/test/unit/image_test.cpp | 110 ++ .../test/unit/image_tests_helper.hpp | 124 ++ nav2_dwb_controller/costmap_queue/package.xml | 2 +- .../include/dwb_core/dwb_local_planner.hpp | 1 + nav2_dwb_controller/dwb_core/package.xml | 2 +- .../dwb_core/src/dwb_local_planner.cpp | 14 +- nav2_dwb_controller/dwb_critics/package.xml | 2 +- nav2_dwb_controller/dwb_msgs/package.xml | 2 +- nav2_dwb_controller/dwb_plugins/package.xml | 2 +- .../nav2_dwb_controller/package.xml | 2 +- nav2_dwb_controller/nav_2d_msgs/package.xml | 2 +- nav2_dwb_controller/nav_2d_utils/package.xml | 2 +- nav2_lifecycle_manager/package.xml | 2 +- nav2_map_server/CMakeLists.txt | 1 + .../costmap_filter_info_server.hpp | 4 +- nav2_map_server/package.xml | 2 +- .../costmap_filter_info_server.cpp | 11 +- nav2_mppi_controller/README.md | 22 +- .../critics/path_align_critic.hpp | 2 +- .../nav2_mppi_controller/motion_models.hpp | 2 +- nav2_mppi_controller/package.xml | 2 +- .../src/critics/path_follow_critic.cpp | 5 +- .../test/motion_model_tests.cpp | 4 +- nav2_msgs/package.xml | 2 +- nav2_navfn_planner/package.xml | 2 +- nav2_planner/package.xml | 2 +- nav2_planner/src/planner_server.cpp | 3 + .../package.xml | 2 +- nav2_rotation_shim_controller/package.xml | 2 +- nav2_rviz_plugins/package.xml | 2 +- nav2_simple_commander/README.md | 2 +- .../launch/assisted_teleop_example_launch.py | 23 +- .../launch/follow_path_example_launch.py | 23 +- .../launch/inspection_demo_launch.py | 23 +- .../nav_through_poses_example_launch.py | 23 +- .../launch/nav_to_pose_example_launch.py | 23 +- .../launch/picking_demo_launch.py | 23 +- .../launch/recoveries_example_launch.py | 23 +- .../launch/security_demo_launch.py | 23 +- nav2_simple_commander/launch/warehouse.world | 8 +- .../waypoint_follower_example_launch.py | 23 +- nav2_simple_commander/package.xml | 2 +- nav2_smac_planner/README.md | 2 +- nav2_smac_planner/package.xml | 2 +- nav2_smac_planner/src/node_lattice.cpp | 6 +- nav2_smac_planner/test/test_nodelattice.cpp | 67 + nav2_smoother/package.xml | 2 +- nav2_system_tests/package.xml | 2 +- nav2_theta_star_planner/package.xml | 2 +- nav2_theta_star_planner/src/theta_star.cpp | 6 +- .../include/nav2_util/service_client.hpp | 4 + nav2_util/package.xml | 2 +- nav2_velocity_smoother/package.xml | 2 +- .../src/velocity_smoother.cpp | 52 +- .../test/test_velocity_smoother.cpp | 353 +++++- nav2_voxel_grid/package.xml | 2 +- nav2_waypoint_follower/package.xml | 2 +- navigation2/package.xml | 2 +- tools/distro.Dockerfile | 2 +- tools/source.Dockerfile | 6 +- 147 files changed, 7112 insertions(+), 314 deletions(-) create mode 100644 .devcontainer/caddy/Caddyfile create mode 100644 .devcontainer/caddy/srv/assets/foxglove/manifest.json create mode 100644 .devcontainer/caddy/srv/assets/foxglove/nav2_layout.json create mode 100644 .devcontainer/caddy/srv/assets/glances/manifest.json create mode 100644 .devcontainer/caddy/srv/assets/gzweb/manifest.json create mode 100644 .devcontainer/caddy/srv/assets/nav2/manifest.json create mode 100644 .devcontainer/caddy/srv/nav2/github-markdown.css create mode 100644 .devcontainer/caddy/srv/nav2/index.html create mode 100644 .devcontainer/caddy/srv/nav2/index.md create mode 100644 .devcontainer/devcontainer.json create mode 100755 .devcontainer/on-create-command.sh create mode 100755 .devcontainer/post-create-command.sh create mode 100755 .devcontainer/update-content-command.sh create mode 100644 .vscode/tasks.json create mode 100644 doc/development/codespaces.md create mode 100644 doc/sponsors_may_2023.png create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp create mode 100644 nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp create mode 100644 nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp create mode 100644 nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp create mode 100644 nav2_controller/plugins/pose_progress_checker.cpp create mode 100644 nav2_costmap_2d/include/nav2_costmap_2d/denoise/image.hpp create mode 100644 nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp create mode 100644 nav2_costmap_2d/include/nav2_costmap_2d/denoise_layer.hpp create mode 100644 nav2_costmap_2d/plugins/denoise_layer.cpp create mode 100644 nav2_costmap_2d/test/unit/costmap_filter_test.cpp create mode 100644 nav2_costmap_2d/test/unit/denoise_layer_test.cpp create mode 100644 nav2_costmap_2d/test/unit/image_processing_test.cpp create mode 100644 nav2_costmap_2d/test/unit/image_test.cpp create mode 100644 nav2_costmap_2d/test/unit/image_tests_helper.hpp diff --git a/.circleci/config.yml b/.circleci/config.yml index 6ed07d76e98..822daab9575 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -301,10 +301,15 @@ _commands: when: always - store_artifacts: path: << parameters.workspace >>/log/test - - store_test_results: - path: << parameters.workspace >>/test_results - store_artifacts: path: << parameters.workspace >>/test_results + - run: + name: Prepare Test Results + working_directory: << parameters.workspace >> + command: find test_results -name "Test.xml" -type f -delete + when: always + - store_test_results: + path: << parameters.workspace >>/test_results _steps: pre_checkout: &pre_checkout diff --git a/.devcontainer/caddy/Caddyfile b/.devcontainer/caddy/Caddyfile new file mode 100644 index 00000000000..1150579a47a --- /dev/null +++ b/.devcontainer/caddy/Caddyfile @@ -0,0 +1,132 @@ +# Snippet for global matchers and variables +# to logically expression request conditions +# E.g. for conditionally changing redirects +(globals) { + # Use gzip compression for all responses + encode gzip + + # Matcher for http request scheme. E.g. "http" or "https" + @http_scheme { + expression {http.request.scheme}=="https" || {header.X-Forwarded-Scheme}=="https" || {header.X-Forwarded-Proto}=="https" + } + # If any http scheme is "https", then use "wss" + vars @http_scheme WsScheme "wss" + # Else default to "ws" + vars WsScheme "ws" + + # Matcher for forwarded request headers + @host_forwarded { + header X-Forwarded-Host * + } + # If http headers exists, then use them + vars @host_forwarded ReqHost {header.X-Forwarded-Host} + # Else default to host in request + vars ReqHost {http.request.hostport} + + # Matcher for websocket connection upgrade requests + @websockets { + # Avoid case sensitivity issues when matching field values + # E.g. when values are rewritten by Codespace port forwarding + header_regexp Connection (?i)(Upgrade) + header Upgrade websocket + } +} + +# Snippet for redirect with given URL queries values +# to simplify remote development with web apps +# E.g auto redirect websocket URL to match request scheme +(redirect) { + # Configure redirect to match request scheme + vars LayoutUrl "/assets/foxglove/nav2_layout.json" + vars DataSourceUrl "{vars.WsScheme}://{vars.ReqHost}{args.0}/" + redir /autoconnect "{args.0}/?ds=foxglove-websocket&ds.url={vars.DataSourceUrl}" + redir /autolayout "{args.0}/?ds=foxglove-websocket&ds.url={vars.DataSourceUrl}&layoutUrl={vars.LayoutUrl}" +} + +# Snippet for dummy imports +(dummy) { +} + +# Snippet for enabling mobile web app features +# to improve user experience on small screen devices +# E.g. for enabling fullscreen mode on iOS and Android +(mobile) { + # Match for directory redirects to index.html + route / { + # Inject link to manifest just after tag + # https://developer.mozilla.org/docs/Web/Manifest + replace `` `` + } + # Redirect relative handle_path'ed manifest.json to /manifests directory + redir /manifest.json /assets{http.request.orig_uri.path.dir}manifest.json +} + +# Snippet for hosted web app using websockets +# to serve static files and reverse proxying connections +# E.g. for serving GzWeb and Foxglove web apps +(app) { + # handle and strip path prefix from redirect + handle_path {args.0}/* { + # Set root directory for static files + root * {http.vars.root}{args.0} + # Enable mobile web app features + import mobile + # Reverse proxy websockets to backend address + reverse_proxy @websockets {args.1} + # Import custom snippets + import {args.2} {args.0} + } +} + +# Listen for http requests on port 8080 +# regardless of hostname or domain address +# E.g. whatever Codespaces assigns to host +:8080 { + # Include global matchers and variables + import globals + root * {$ROOT_SRV:/srv} + file_server browse + + # Handle root content + # I.e. assets internal to workspace + handle /* { + # Template manifest.json files + templates */manifest.json { + mime application/json + } + } + + # Handle nav2 web app + # I.e. main landing page + handle_path /nav2/* { + root * {http.vars.root}/nav2 + import mobile + # Render markdown files as html + templates + } + + # Matcher for requests without browse query + @no_browse { + path / + not query browse=true + } + # Redirect to nav2 web app by default + redir @no_browse /nav2/ + + # Import app snippets for web apps + import app "/gzweb" "localhost:9090" "dummy" + import app "/foxglove" "localhost:8765" "redirect" + + # Handle glances web app + redir /glances /glances/ + handle_path /glances/* { + import mobile + # Reverse proxy to glances backend + reverse_proxy * "localhost:61208" + } + + # For debugging + # log { + # output file /var/log/caddy/server.log + # } +} diff --git a/.devcontainer/caddy/srv/assets/foxglove/manifest.json b/.devcontainer/caddy/srv/assets/foxglove/manifest.json new file mode 100644 index 00000000000..0b9b8b84d7e --- /dev/null +++ b/.devcontainer/caddy/srv/assets/foxglove/manifest.json @@ -0,0 +1,40 @@ +{ + "name": "Foxglove: {{placeholder "http.vars.ReqHost"}}", + "short_name": "Foxglove: {{placeholder "http.vars.ReqHost"}}", + "icons": [ + { + "src": "/media/icons/foxglove/any_icon_x512.webp", + "sizes": "512x512", + "type": "image/webp", + "purpose": "any" + }, + { + "src": "/media/icons/foxglove/maskable_icon_x512.webp", + "sizes": "512x512", + "type": "image/webp", + "purpose": "maskable" + } + ], + "id": "/foxglove/", + "start_url": "/foxglove/autoconnect", + "theme_color": "#6F3BE8", + "background_color": "#6F3BE8", + "display": "fullscreen", + "shortcuts" : [ + { + "name": "Auto Connect", + "url": "/foxglove/autoconnect", + "description": "Auto connect to default data source" + }, + { + "name": "Auto Layout", + "url": "/foxglove/autolayout", + "description": "Auto connect using default layout" + }, + { + "name": "Manual Connect", + "url": "/foxglove/", + "description": "Manually connect to data source" + } + ] +} diff --git a/.devcontainer/caddy/srv/assets/foxglove/nav2_layout.json b/.devcontainer/caddy/srv/assets/foxglove/nav2_layout.json new file mode 100644 index 00000000000..f92821fac49 --- /dev/null +++ b/.devcontainer/caddy/srv/assets/foxglove/nav2_layout.json @@ -0,0 +1,463 @@ +{ + "configById": { + "3D!18i6zy7": { + "layers": { + "845139cb-26bc-40b3-8161-8ab60af4baf5": { + "visible": true, + "frameLocked": true, + "label": "Grid", + "instanceId": "845139cb-26bc-40b3-8161-8ab60af4baf5", + "layerId": "foxglove.Grid", + "size": 10, + "divisions": 10, + "lineWidth": 1, + "color": "#A0A0A4ff", + "position": [ + 0, + 0, + 0 + ], + "rotation": [ + 0, + 0, + 0 + ], + "order": 1 + } + }, + "cameraState": { + "perspective": true, + "distance": 21.05263157877764, + "phi": 38.925517117715195, + "thetaOffset": -138.92710744521386, + "targetOffset": [ + -2.6847696124888896, + 0.2191229688744439, + 3.6086809432821955e-16 + ], + "target": [ + 0, + 0, + 0 + ], + "targetOrientation": [ + 0, + 0, + 0, + 1 + ], + "fovy": 45, + "near": 0.5, + "far": 5000 + }, + "followMode": "follow-pose", + "scene": { + "transforms": { + "showLabel": false, + "editable": false, + "labelSize": 0.049999999999999975, + "enablePreloading": false, + "lineWidth": 2 + } + }, + "transforms": { + "frame:camera_link": { + "visible": false + }, + "frame:camera_depth_frame": { + "visible": false + }, + "frame:camera_depth_optical_frame": { + "visible": false + }, + "frame:camera_rgb_frame": { + "visible": false + }, + "frame:camera_rgb_optical_frame": { + "visible": false + }, + "frame:imu_link": { + "visible": false + }, + "frame:caster_back_right_link": { + "visible": false + }, + "frame:caster_back_left_link": { + "visible": false + }, + "frame:odom": { + "visible": true + }, + "frame:base_footprint": { + "visible": true + }, + "frame:wheel_left_link": { + "visible": false + }, + "frame:wheel_right_link": { + "visible": false + }, + "frame:base_link": { + "visible": false + }, + "frame:base_scan": { + "visible": false + }, + "frame:map": { + "visible": true + } + }, + "topics": { + "/scan": { + "visible": true, + "colorField": "intensity", + "colorMode": "flat", + "colorMap": "turbo", + "pointSize": 5, + "flatColor": "#ff0000" + }, + "/global_costmap/costmap": { + "visible": true, + "maxColor": "#E800174d", + "unknownColor": "#5700ff4d", + "minColor": "#ffffff4d", + "invalidColor": "#ff00ff4d", + "colorMode": "costmap", + "alpha": 0.3 + }, + "/global_costmap/obstacle_layer": { + "visible": false + }, + "/global_costmap/voxel_marked_cloud": { + "visible": false + }, + "/goal_pose": { + "visible": false + }, + "/local_costmap/costmap": { + "visible": false + }, + "/local_costmap/voxel_layer": { + "visible": false + }, + "/local_costmap/clearing_endpoints": { + "visible": false, + "colorField": "x", + "colorMode": "colormap", + "colorMap": "turbo" + }, + "/map": { + "visible": true, + "minColor": "#ffffff", + "maxColor": "#000000", + "unknownColor": "#708986ff", + "frameLocked": false, + "colorMode": "map" + }, + "/amcl_pose": { + "visible": false + }, + "/local_plan": { + "visible": true, + "lineWidth": 0.01, + "gradient": [ + "#c8ff00c7", + "#00c8ffba" + ] + }, + "/plan": { + "visible": false, + "gradient": [ + "rgba(124, 107, 255, 1)", + "#ff6b6b" + ], + "lineWidth": 1 + }, + "/plan_smoothed": { + "visible": false + }, + "/received_global_plan": { + "visible": true, + "gradient": [ + "#ff0000c7", + "#6b70ffc2" + ], + "lineWidth": 0.02, + "type": "line", + "arrowScale": [ + 0.02, + 0.0015, + 0.0015 + ] + }, + "/transformed_global_plan": { + "visible": false + }, + "/robot_description": { + "visible": false + }, + "/cost_cloud": { + "visible": false + }, + "/initialpose": { + "visible": false + } + }, + "publish": { + "type": "pose_estimate", + "poseTopic": "/move_base_simple/goal", + "pointTopic": "", + "poseEstimateTopic": "/initialpose", + "poseEstimateXDeviation": 0.5, + "poseEstimateYDeviation": 0.5, + "poseEstimateThetaDeviation": 0.26179939 + }, + "followTf": "map" + }, + "DiagnosticSummary!3bo4e39": { + "minLevel": 0, + "pinnedIds": [], + "hardwareIdFilter": "", + "topicToRender": "/diagnostics", + "sortByLevel": true + }, + "RosOut!1iib9dq": { + "searchTerms": [], + "minLogLevel": 1 + }, + "3D!2agiaqk": { + "layers": { + "845139cb-26bc-40b3-8161-8ab60af4baf5": { + "visible": false, + "frameLocked": true, + "label": "Grid", + "instanceId": "845139cb-26bc-40b3-8161-8ab60af4baf5", + "layerId": "foxglove.Grid", + "size": 10, + "divisions": 10, + "lineWidth": 1, + "color": "#A0A0A4ff", + "position": [ + 0, + 0, + 0 + ], + "rotation": [ + 0, + 0, + 0 + ], + "order": 1 + } + }, + "cameraState": { + "perspective": true, + "distance": 4.078136514883917, + "phi": 56.068374260572, + "thetaOffset": 92.50000000000723, + "targetOffset": [ + 0.03816360663426963, + 0.15755079607173259, + 7.341598429161142e-18 + ], + "target": [ + 0, + 0, + 0 + ], + "targetOrientation": [ + 0, + 0, + 0, + 1 + ], + "fovy": 45, + "near": 0.5, + "far": 5000 + }, + "followMode": "follow-pose", + "scene": { + "transforms": { + "showLabel": true, + "editable": false, + "labelSize": 0.049999999999999975, + "enablePreloading": false, + "lineWidth": 2 + } + }, + "transforms": { + "frame:camera_link": { + "visible": false + }, + "frame:camera_depth_frame": { + "visible": false + }, + "frame:camera_depth_optical_frame": { + "visible": false + }, + "frame:camera_rgb_frame": { + "visible": false + }, + "frame:camera_rgb_optical_frame": { + "visible": false + }, + "frame:imu_link": { + "visible": false + }, + "frame:caster_back_right_link": { + "visible": false + }, + "frame:caster_back_left_link": { + "visible": false + }, + "frame:odom": { + "visible": true + }, + "frame:base_footprint": { + "visible": true + }, + "frame:wheel_left_link": { + "visible": false + }, + "frame:wheel_right_link": { + "visible": false + }, + "frame:base_link": { + "visible": false + }, + "frame:base_scan": { + "visible": false + }, + "frame:map": { + "visible": true + } + }, + "topics": { + "/scan": { + "visible": true, + "colorField": "intensity", + "colorMode": "flat", + "colorMap": "turbo", + "pointSize": 5, + "flatColor": "#ff0000" + }, + "/global_costmap/costmap": { + "visible": true, + "maxColor": "#E800174d", + "unknownColor": "#5700ff4d", + "minColor": "#ffffff4d", + "invalidColor": "#ff00ff4d", + "colorMode": "costmap", + "alpha": 0.3 + }, + "/global_costmap/obstacle_layer": { + "visible": false + }, + "/global_costmap/voxel_marked_cloud": { + "visible": false + }, + "/goal_pose": { + "visible": false + }, + "/local_costmap/costmap": { + "visible": false + }, + "/local_costmap/voxel_layer": { + "visible": false + }, + "/local_costmap/clearing_endpoints": { + "visible": false, + "colorField": "x", + "colorMode": "colormap", + "colorMap": "turbo" + }, + "/map": { + "visible": true, + "minColor": "#ffffff", + "maxColor": "#000000", + "unknownColor": "#708986ff", + "frameLocked": false, + "colorMode": "map" + }, + "/amcl_pose": { + "visible": false + }, + "/local_plan": { + "visible": true, + "lineWidth": 0.01, + "gradient": [ + "#c8ff00c7", + "#00c8ffba" + ] + }, + "/plan": { + "visible": false, + "gradient": [ + "rgba(124, 107, 255, 1)", + "#ff6b6b" + ], + "lineWidth": 1 + }, + "/plan_smoothed": { + "visible": false + }, + "/received_global_plan": { + "visible": true, + "gradient": [ + "#ff0000c7", + "#6b70ffc2" + ], + "lineWidth": 0.02, + "type": "line", + "arrowScale": [ + 0.02, + 0.0015, + 0.0015 + ] + }, + "/transformed_global_plan": { + "visible": false + }, + "/robot_description": { + "visible": true + }, + "/cost_cloud": { + "visible": false + }, + "/initialpose": { + "visible": false + } + }, + "publish": { + "type": "pose_estimate", + "poseTopic": "/move_base_simple/goal", + "pointTopic": "", + "poseEstimateTopic": "/initialpose", + "poseEstimateXDeviation": 0.5, + "poseEstimateYDeviation": 0.5, + "poseEstimateThetaDeviation": 0.26179939 + }, + "followTf": "base_link" + } + }, + "globalVariables": {}, + "userNodes": {}, + "playbackConfig": { + "speed": 1 + }, + "layout": { + "first": "3D!18i6zy7", + "second": { + "first": "DiagnosticSummary!3bo4e39", + "second": { + "first": "RosOut!1iib9dq", + "second": "3D!2agiaqk", + "direction": "column" + }, + "direction": "column", + "splitPercentage": 28.227360308285164 + }, + "direction": "row", + "splitPercentage": 74.87855655794587 + } + } \ No newline at end of file diff --git a/.devcontainer/caddy/srv/assets/glances/manifest.json b/.devcontainer/caddy/srv/assets/glances/manifest.json new file mode 100644 index 00000000000..78d27cb0bd6 --- /dev/null +++ b/.devcontainer/caddy/srv/assets/glances/manifest.json @@ -0,0 +1,40 @@ +{ + "name": "Glances: {{placeholder "http.vars.ReqHost"}}", + "short_name": "Glances: {{placeholder "http.vars.ReqHost"}}", + "icons": [ + { + "src": "/media/icons/glances/any_icon_x512.webp", + "sizes": "512x512", + "type": "image/webp", + "purpose": "any" + }, + { + "src": "/media/icons/glances/maskable_icon_x512.webp", + "sizes": "512x512", + "type": "image/webp", + "purpose": "maskable" + } + ], + "id": "/glances/", + "start_url": "/glances/", + "theme_color": "#2C363F", + "background_color": "#2C363F", + "display": "fullscreen", + "shortcuts" : [ + { + "name": "Refresh 1sec", + "url": "/glances/1", + "description": "Refresh page every 1 second" + }, + { + "name": "Refresh 5sec", + "url": "/glances/5", + "description": "Refresh page every 5 seconds" + }, + { + "name": "Refresh 10sec", + "url": "/glances/10", + "description": "Refresh page every 10 seconds" + } + ] +} diff --git a/.devcontainer/caddy/srv/assets/gzweb/manifest.json b/.devcontainer/caddy/srv/assets/gzweb/manifest.json new file mode 100644 index 00000000000..0596075f158 --- /dev/null +++ b/.devcontainer/caddy/srv/assets/gzweb/manifest.json @@ -0,0 +1,23 @@ +{ + "name": "Gzweb: {{placeholder "http.vars.ReqHost"}}", + "short_name": "Gzweb: {{placeholder "http.vars.ReqHost"}}", + "icons": [ + { + "src": "/media/icons/gzweb/any_icon_x512.webp", + "sizes": "512x512", + "type": "image/webp", + "purpose": "any" + }, + { + "src": "/media/icons/gzweb/maskable_icon_x512.webp", + "sizes": "512x512", + "type": "image/webp", + "purpose": "maskable" + } + ], + "id": "/gzweb/", + "start_url": "/gzweb/", + "theme_color": "#ffffff", + "background_color": "#ffffff", + "display": "fullscreen" +} diff --git a/.devcontainer/caddy/srv/assets/nav2/manifest.json b/.devcontainer/caddy/srv/assets/nav2/manifest.json new file mode 100644 index 00000000000..f482a47e803 --- /dev/null +++ b/.devcontainer/caddy/srv/assets/nav2/manifest.json @@ -0,0 +1,23 @@ +{ + "name": "Nav2: {{placeholder "http.vars.ReqHost"}}", + "short_name": "Nav2: {{placeholder "http.vars.ReqHost"}}", + "icons": [ + { + "src": "/media/icons/nav2/any_icon_x512.webp", + "sizes": "512x512", + "type": "image/webp", + "purpose": "any" + }, + { + "src": "/media/icons/nav2/maskable_icon_x512.webp", + "sizes": "512x512", + "type": "image/webp", + "purpose": "maskable" + } + ], + "id": "/nav2/", + "start_url": "/nav2/", + "theme_color": "#ffffff", + "background_color": "#ffffff", + "display": "standalone" +} diff --git a/.devcontainer/caddy/srv/nav2/github-markdown.css b/.devcontainer/caddy/srv/nav2/github-markdown.css new file mode 100644 index 00000000000..049cae6b291 --- /dev/null +++ b/.devcontainer/caddy/srv/nav2/github-markdown.css @@ -0,0 +1,1102 @@ +@media (prefers-color-scheme: dark) { + .markdown-body { + color-scheme: dark; + --color-prettylights-syntax-comment: #8b949e; + --color-prettylights-syntax-constant: #79c0ff; + --color-prettylights-syntax-entity: #d2a8ff; + --color-prettylights-syntax-storage-modifier-import: #c9d1d9; + --color-prettylights-syntax-entity-tag: #7ee787; + --color-prettylights-syntax-keyword: #ff7b72; + --color-prettylights-syntax-string: #a5d6ff; + --color-prettylights-syntax-variable: #ffa657; + --color-prettylights-syntax-brackethighlighter-unmatched: #f85149; + --color-prettylights-syntax-invalid-illegal-text: #f0f6fc; + --color-prettylights-syntax-invalid-illegal-bg: #8e1519; + --color-prettylights-syntax-carriage-return-text: #f0f6fc; + --color-prettylights-syntax-carriage-return-bg: #b62324; + --color-prettylights-syntax-string-regexp: #7ee787; + --color-prettylights-syntax-markup-list: #f2cc60; + --color-prettylights-syntax-markup-heading: #1f6feb; + --color-prettylights-syntax-markup-italic: #c9d1d9; + --color-prettylights-syntax-markup-bold: #c9d1d9; + --color-prettylights-syntax-markup-deleted-text: #ffdcd7; + --color-prettylights-syntax-markup-deleted-bg: #67060c; + --color-prettylights-syntax-markup-inserted-text: #aff5b4; + --color-prettylights-syntax-markup-inserted-bg: #033a16; + --color-prettylights-syntax-markup-changed-text: #ffdfb6; + --color-prettylights-syntax-markup-changed-bg: #5a1e02; + --color-prettylights-syntax-markup-ignored-text: #c9d1d9; + --color-prettylights-syntax-markup-ignored-bg: #1158c7; + --color-prettylights-syntax-meta-diff-range: #d2a8ff; + --color-prettylights-syntax-brackethighlighter-angle: #8b949e; + --color-prettylights-syntax-sublimelinter-gutter-mark: #484f58; + --color-prettylights-syntax-constant-other-reference-link: #a5d6ff; + --color-fg-default: #c9d1d9; + --color-fg-muted: #8b949e; + --color-fg-subtle: #6e7681; + --color-canvas-default: #0d1117; + --color-canvas-subtle: #161b22; + --color-border-default: #30363d; + --color-border-muted: #21262d; + --color-neutral-muted: rgba(110,118,129,0.4); + --color-accent-fg: #58a6ff; + --color-accent-emphasis: #1f6feb; + --color-attention-subtle: rgba(187,128,9,0.15); + --color-danger-fg: #f85149; + } + } + + @media (prefers-color-scheme: light) { + .markdown-body { + color-scheme: light; + --color-prettylights-syntax-comment: #6e7781; + --color-prettylights-syntax-constant: #0550ae; + --color-prettylights-syntax-entity: #8250df; + --color-prettylights-syntax-storage-modifier-import: #24292f; + --color-prettylights-syntax-entity-tag: #116329; + --color-prettylights-syntax-keyword: #cf222e; + --color-prettylights-syntax-string: #0a3069; + --color-prettylights-syntax-variable: #953800; + --color-prettylights-syntax-brackethighlighter-unmatched: #82071e; + --color-prettylights-syntax-invalid-illegal-text: #f6f8fa; + --color-prettylights-syntax-invalid-illegal-bg: #82071e; + --color-prettylights-syntax-carriage-return-text: #f6f8fa; + --color-prettylights-syntax-carriage-return-bg: #cf222e; + --color-prettylights-syntax-string-regexp: #116329; + --color-prettylights-syntax-markup-list: #3b2300; + --color-prettylights-syntax-markup-heading: #0550ae; + --color-prettylights-syntax-markup-italic: #24292f; + --color-prettylights-syntax-markup-bold: #24292f; + --color-prettylights-syntax-markup-deleted-text: #82071e; + --color-prettylights-syntax-markup-deleted-bg: #ffebe9; + --color-prettylights-syntax-markup-inserted-text: #116329; + --color-prettylights-syntax-markup-inserted-bg: #dafbe1; + --color-prettylights-syntax-markup-changed-text: #953800; + --color-prettylights-syntax-markup-changed-bg: #ffd8b5; + --color-prettylights-syntax-markup-ignored-text: #eaeef2; + --color-prettylights-syntax-markup-ignored-bg: #0550ae; + --color-prettylights-syntax-meta-diff-range: #8250df; + --color-prettylights-syntax-brackethighlighter-angle: #57606a; + --color-prettylights-syntax-sublimelinter-gutter-mark: #8c959f; + --color-prettylights-syntax-constant-other-reference-link: #0a3069; + --color-fg-default: #24292f; + --color-fg-muted: #57606a; + --color-fg-subtle: #6e7781; + --color-canvas-default: #ffffff; + --color-canvas-subtle: #f6f8fa; + --color-border-default: #d0d7de; + --color-border-muted: hsla(210,18%,87%,1); + --color-neutral-muted: rgba(175,184,193,0.2); + --color-accent-fg: #0969da; + --color-accent-emphasis: #0969da; + --color-attention-subtle: #fff8c5; + --color-danger-fg: #cf222e; + } + } + + .markdown-body { + -ms-text-size-adjust: 100%; + -webkit-text-size-adjust: 100%; + margin: 0; + color: var(--color-fg-default); + background-color: var(--color-canvas-default); + font-family: -apple-system,BlinkMacSystemFont,"Segoe UI","Noto Sans",Helvetica,Arial,sans-serif,"Apple Color Emoji","Segoe UI Emoji"; + font-size: 16px; + line-height: 1.5; + word-wrap: break-word; + } + + .markdown-body .octicon { + display: inline-block; + fill: currentColor; + vertical-align: text-bottom; + } + + .markdown-body h1:hover .anchor .octicon-link:before, + .markdown-body h2:hover .anchor .octicon-link:before, + .markdown-body h3:hover .anchor .octicon-link:before, + .markdown-body h4:hover .anchor .octicon-link:before, + .markdown-body h5:hover .anchor .octicon-link:before, + .markdown-body h6:hover .anchor .octicon-link:before { + width: 16px; + height: 16px; + content: ' '; + display: inline-block; + background-color: currentColor; + -webkit-mask-image: url("data:image/svg+xml,"); + mask-image: url("data:image/svg+xml,"); + } + + .markdown-body details, + .markdown-body figcaption, + .markdown-body figure { + display: block; + } + + .markdown-body summary { + display: list-item; + } + + .markdown-body [hidden] { + display: none !important; + } + + .markdown-body a { + background-color: transparent; + color: var(--color-accent-fg); + text-decoration: none; + } + + .markdown-body abbr[title] { + border-bottom: none; + text-decoration: underline dotted; + } + + .markdown-body b, + .markdown-body strong { + font-weight: var(--base-text-weight-semibold, 600); + } + + .markdown-body dfn { + font-style: italic; + } + + .markdown-body h1 { + margin: .67em 0; + font-weight: var(--base-text-weight-semibold, 600); + padding-bottom: .3em; + font-size: 2em; + border-bottom: 1px solid var(--color-border-muted); + } + + .markdown-body mark { + background-color: var(--color-attention-subtle); + color: var(--color-fg-default); + } + + .markdown-body small { + font-size: 90%; + } + + .markdown-body sub, + .markdown-body sup { + font-size: 75%; + line-height: 0; + position: relative; + vertical-align: baseline; + } + + .markdown-body sub { + bottom: -0.25em; + } + + .markdown-body sup { + top: -0.5em; + } + + .markdown-body img { + border-style: none; + max-width: 100%; + box-sizing: content-box; + background-color: var(--color-canvas-default); + } + + .markdown-body code, + .markdown-body kbd, + .markdown-body pre, + .markdown-body samp { + font-family: monospace; + font-size: 1em; + } + + .markdown-body figure { + margin: 1em 40px; + } + + .markdown-body hr { + box-sizing: content-box; + overflow: hidden; + background: transparent; + border-bottom: 1px solid var(--color-border-muted); + height: .25em; + padding: 0; + margin: 24px 0; + background-color: var(--color-border-default); + border: 0; + } + + .markdown-body input { + font: inherit; + margin: 0; + overflow: visible; + font-family: inherit; + font-size: inherit; + line-height: inherit; + } + + .markdown-body [type=button], + .markdown-body [type=reset], + .markdown-body [type=submit] { + -webkit-appearance: button; + } + + .markdown-body [type=checkbox], + .markdown-body [type=radio] { + box-sizing: border-box; + padding: 0; + } + + .markdown-body [type=number]::-webkit-inner-spin-button, + .markdown-body [type=number]::-webkit-outer-spin-button { + height: auto; + } + + .markdown-body [type=search]::-webkit-search-cancel-button, + .markdown-body [type=search]::-webkit-search-decoration { + -webkit-appearance: none; + } + + .markdown-body ::-webkit-input-placeholder { + color: inherit; + opacity: .54; + } + + .markdown-body ::-webkit-file-upload-button { + -webkit-appearance: button; + font: inherit; + } + + .markdown-body a:hover { + text-decoration: underline; + } + + .markdown-body ::placeholder { + color: var(--color-fg-subtle); + opacity: 1; + } + + .markdown-body hr::before { + display: table; + content: ""; + } + + .markdown-body hr::after { + display: table; + clear: both; + content: ""; + } + + .markdown-body table { + border-spacing: 0; + border-collapse: collapse; + display: block; + width: max-content; + max-width: 100%; + overflow: auto; + } + + .markdown-body td, + .markdown-body th { + padding: 0; + } + + .markdown-body details summary { + cursor: pointer; + } + + .markdown-body details:not([open])>*:not(summary) { + display: none !important; + } + + .markdown-body a:focus, + .markdown-body [role=button]:focus, + .markdown-body input[type=radio]:focus, + .markdown-body input[type=checkbox]:focus { + outline: 2px solid var(--color-accent-fg); + outline-offset: -2px; + box-shadow: none; + } + + .markdown-body a:focus:not(:focus-visible), + .markdown-body [role=button]:focus:not(:focus-visible), + .markdown-body input[type=radio]:focus:not(:focus-visible), + .markdown-body input[type=checkbox]:focus:not(:focus-visible) { + outline: solid 1px transparent; + } + + .markdown-body a:focus-visible, + .markdown-body [role=button]:focus-visible, + .markdown-body input[type=radio]:focus-visible, + .markdown-body input[type=checkbox]:focus-visible { + outline: 2px solid var(--color-accent-fg); + outline-offset: -2px; + box-shadow: none; + } + + .markdown-body a:not([class]):focus, + .markdown-body a:not([class]):focus-visible, + .markdown-body input[type=radio]:focus, + .markdown-body input[type=radio]:focus-visible, + .markdown-body input[type=checkbox]:focus, + .markdown-body input[type=checkbox]:focus-visible { + outline-offset: 0; + } + + .markdown-body kbd { + display: inline-block; + padding: 3px 5px; + font: 11px ui-monospace,SFMono-Regular,SF Mono,Menlo,Consolas,Liberation Mono,monospace; + line-height: 10px; + color: var(--color-fg-default); + vertical-align: middle; + background-color: var(--color-canvas-subtle); + border: solid 1px var(--color-neutral-muted); + border-bottom-color: var(--color-neutral-muted); + border-radius: 6px; + box-shadow: inset 0 -1px 0 var(--color-neutral-muted); + } + + .markdown-body h1, + .markdown-body h2, + .markdown-body h3, + .markdown-body h4, + .markdown-body h5, + .markdown-body h6 { + margin-top: 24px; + margin-bottom: 16px; + font-weight: var(--base-text-weight-semibold, 600); + line-height: 1.25; + } + + .markdown-body h2 { + font-weight: var(--base-text-weight-semibold, 600); + padding-bottom: .3em; + font-size: 1.5em; + border-bottom: 1px solid var(--color-border-muted); + } + + .markdown-body h3 { + font-weight: var(--base-text-weight-semibold, 600); + font-size: 1.25em; + } + + .markdown-body h4 { + font-weight: var(--base-text-weight-semibold, 600); + font-size: 1em; + } + + .markdown-body h5 { + font-weight: var(--base-text-weight-semibold, 600); + font-size: .875em; + } + + .markdown-body h6 { + font-weight: var(--base-text-weight-semibold, 600); + font-size: .85em; + color: var(--color-fg-muted); + } + + .markdown-body p { + margin-top: 0; + margin-bottom: 10px; + } + + .markdown-body blockquote { + margin: 0; + padding: 0 1em; + color: var(--color-fg-muted); + border-left: .25em solid var(--color-border-default); + } + + .markdown-body ul, + .markdown-body ol { + margin-top: 0; + margin-bottom: 0; + padding-left: 2em; + } + + .markdown-body ol ol, + .markdown-body ul ol { + list-style-type: lower-roman; + } + + .markdown-body ul ul ol, + .markdown-body ul ol ol, + .markdown-body ol ul ol, + .markdown-body ol ol ol { + list-style-type: lower-alpha; + } + + .markdown-body dd { + margin-left: 0; + } + + .markdown-body tt, + .markdown-body code, + .markdown-body samp { + font-family: ui-monospace,SFMono-Regular,SF Mono,Menlo,Consolas,Liberation Mono,monospace; + font-size: 12px; + } + + .markdown-body pre { + margin-top: 0; + margin-bottom: 0; + font-family: ui-monospace,SFMono-Regular,SF Mono,Menlo,Consolas,Liberation Mono,monospace; + font-size: 12px; + word-wrap: normal; + } + + .markdown-body .octicon { + display: inline-block; + overflow: visible !important; + vertical-align: text-bottom; + fill: currentColor; + } + + .markdown-body input::-webkit-outer-spin-button, + .markdown-body input::-webkit-inner-spin-button { + margin: 0; + -webkit-appearance: none; + appearance: none; + } + + .markdown-body::before { + display: table; + content: ""; + } + + .markdown-body::after { + display: table; + clear: both; + content: ""; + } + + .markdown-body>*:first-child { + margin-top: 0 !important; + } + + .markdown-body>*:last-child { + margin-bottom: 0 !important; + } + + .markdown-body a:not([href]) { + color: inherit; + text-decoration: none; + } + + .markdown-body .absent { + color: var(--color-danger-fg); + } + + .markdown-body .anchor { + float: left; + padding-right: 4px; + margin-left: -20px; + line-height: 1; + } + + .markdown-body .anchor:focus { + outline: none; + } + + .markdown-body p, + .markdown-body blockquote, + .markdown-body ul, + .markdown-body ol, + .markdown-body dl, + .markdown-body table, + .markdown-body pre, + .markdown-body details { + margin-top: 0; + margin-bottom: 16px; + } + + .markdown-body blockquote>:first-child { + margin-top: 0; + } + + .markdown-body blockquote>:last-child { + margin-bottom: 0; + } + + .markdown-body h1 .octicon-link, + .markdown-body h2 .octicon-link, + .markdown-body h3 .octicon-link, + .markdown-body h4 .octicon-link, + .markdown-body h5 .octicon-link, + .markdown-body h6 .octicon-link { + color: var(--color-fg-default); + vertical-align: middle; + visibility: hidden; + } + + .markdown-body h1:hover .anchor, + .markdown-body h2:hover .anchor, + .markdown-body h3:hover .anchor, + .markdown-body h4:hover .anchor, + .markdown-body h5:hover .anchor, + .markdown-body h6:hover .anchor { + text-decoration: none; + } + + .markdown-body h1:hover .anchor .octicon-link, + .markdown-body h2:hover .anchor .octicon-link, + .markdown-body h3:hover .anchor .octicon-link, + .markdown-body h4:hover .anchor .octicon-link, + .markdown-body h5:hover .anchor .octicon-link, + .markdown-body h6:hover .anchor .octicon-link { + visibility: visible; + } + + .markdown-body h1 tt, + .markdown-body h1 code, + .markdown-body h2 tt, + .markdown-body h2 code, + .markdown-body h3 tt, + .markdown-body h3 code, + .markdown-body h4 tt, + .markdown-body h4 code, + .markdown-body h5 tt, + .markdown-body h5 code, + .markdown-body h6 tt, + .markdown-body h6 code { + padding: 0 .2em; + font-size: inherit; + } + + .markdown-body summary h1, + .markdown-body summary h2, + .markdown-body summary h3, + .markdown-body summary h4, + .markdown-body summary h5, + .markdown-body summary h6 { + display: inline-block; + } + + .markdown-body summary h1 .anchor, + .markdown-body summary h2 .anchor, + .markdown-body summary h3 .anchor, + .markdown-body summary h4 .anchor, + .markdown-body summary h5 .anchor, + .markdown-body summary h6 .anchor { + margin-left: -40px; + } + + .markdown-body summary h1, + .markdown-body summary h2 { + padding-bottom: 0; + border-bottom: 0; + } + + .markdown-body ul.no-list, + .markdown-body ol.no-list { + padding: 0; + list-style-type: none; + } + + .markdown-body ol[type=a] { + list-style-type: lower-alpha; + } + + .markdown-body ol[type=A] { + list-style-type: upper-alpha; + } + + .markdown-body ol[type=i] { + list-style-type: lower-roman; + } + + .markdown-body ol[type=I] { + list-style-type: upper-roman; + } + + .markdown-body ol[type="1"] { + list-style-type: decimal; + } + + .markdown-body div>ol:not([type]) { + list-style-type: decimal; + } + + .markdown-body ul ul, + .markdown-body ul ol, + .markdown-body ol ol, + .markdown-body ol ul { + margin-top: 0; + margin-bottom: 0; + } + + .markdown-body li>p { + margin-top: 16px; + } + + .markdown-body li+li { + margin-top: .25em; + } + + .markdown-body dl { + padding: 0; + } + + .markdown-body dl dt { + padding: 0; + margin-top: 16px; + font-size: 1em; + font-style: italic; + font-weight: var(--base-text-weight-semibold, 600); + } + + .markdown-body dl dd { + padding: 0 16px; + margin-bottom: 16px; + } + + .markdown-body table th { + font-weight: var(--base-text-weight-semibold, 600); + } + + .markdown-body table th, + .markdown-body table td { + padding: 6px 13px; + border: 1px solid var(--color-border-default); + } + + .markdown-body table tr { + background-color: var(--color-canvas-default); + border-top: 1px solid var(--color-border-muted); + } + + .markdown-body table tr:nth-child(2n) { + background-color: var(--color-canvas-subtle); + } + + .markdown-body table img { + background-color: transparent; + } + + .markdown-body img[align=right] { + padding-left: 20px; + } + + .markdown-body img[align=left] { + padding-right: 20px; + } + + .markdown-body .emoji { + max-width: none; + vertical-align: text-top; + background-color: transparent; + } + + .markdown-body span.frame { + display: block; + overflow: hidden; + } + + .markdown-body span.frame>span { + display: block; + float: left; + width: auto; + padding: 7px; + margin: 13px 0 0; + overflow: hidden; + border: 1px solid var(--color-border-default); + } + + .markdown-body span.frame span img { + display: block; + float: left; + } + + .markdown-body span.frame span span { + display: block; + padding: 5px 0 0; + clear: both; + color: var(--color-fg-default); + } + + .markdown-body span.align-center { + display: block; + overflow: hidden; + clear: both; + } + + .markdown-body span.align-center>span { + display: block; + margin: 13px auto 0; + overflow: hidden; + text-align: center; + } + + .markdown-body span.align-center span img { + margin: 0 auto; + text-align: center; + } + + .markdown-body span.align-right { + display: block; + overflow: hidden; + clear: both; + } + + .markdown-body span.align-right>span { + display: block; + margin: 13px 0 0; + overflow: hidden; + text-align: right; + } + + .markdown-body span.align-right span img { + margin: 0; + text-align: right; + } + + .markdown-body span.float-left { + display: block; + float: left; + margin-right: 13px; + overflow: hidden; + } + + .markdown-body span.float-left span { + margin: 13px 0 0; + } + + .markdown-body span.float-right { + display: block; + float: right; + margin-left: 13px; + overflow: hidden; + } + + .markdown-body span.float-right>span { + display: block; + margin: 13px auto 0; + overflow: hidden; + text-align: right; + } + + .markdown-body code, + .markdown-body tt { + padding: .2em .4em; + margin: 0; + font-size: 85%; + white-space: break-spaces; + background-color: var(--color-neutral-muted); + border-radius: 6px; + } + + .markdown-body code br, + .markdown-body tt br { + display: none; + } + + .markdown-body del code { + text-decoration: inherit; + } + + .markdown-body samp { + font-size: 85%; + } + + .markdown-body pre code { + font-size: 100%; + } + + .markdown-body pre>code { + padding: 0; + margin: 0; + word-break: normal; + white-space: pre; + background: transparent; + border: 0; + } + + .markdown-body .highlight { + margin-bottom: 16px; + } + + .markdown-body .highlight pre { + margin-bottom: 0; + word-break: normal; + } + + .markdown-body .highlight pre, + .markdown-body pre { + padding: 16px; + overflow: auto; + font-size: 85%; + line-height: 1.45; + background-color: var(--color-canvas-subtle); + border-radius: 6px; + } + + .markdown-body pre code, + .markdown-body pre tt { + display: inline; + max-width: auto; + padding: 0; + margin: 0; + overflow: visible; + line-height: inherit; + word-wrap: normal; + background-color: transparent; + border: 0; + } + + .markdown-body .csv-data td, + .markdown-body .csv-data th { + padding: 5px; + overflow: hidden; + font-size: 12px; + line-height: 1; + text-align: left; + white-space: nowrap; + } + + .markdown-body .csv-data .blob-num { + padding: 10px 8px 9px; + text-align: right; + background: var(--color-canvas-default); + border: 0; + } + + .markdown-body .csv-data tr { + border-top: 0; + } + + .markdown-body .csv-data th { + font-weight: var(--base-text-weight-semibold, 600); + background: var(--color-canvas-subtle); + border-top: 0; + } + + .markdown-body [data-footnote-ref]::before { + content: "["; + } + + .markdown-body [data-footnote-ref]::after { + content: "]"; + } + + .markdown-body .footnotes { + font-size: 12px; + color: var(--color-fg-muted); + border-top: 1px solid var(--color-border-default); + } + + .markdown-body .footnotes ol { + padding-left: 16px; + } + + .markdown-body .footnotes ol ul { + display: inline-block; + padding-left: 16px; + margin-top: 16px; + } + + .markdown-body .footnotes li { + position: relative; + } + + .markdown-body .footnotes li:target::before { + position: absolute; + top: -8px; + right: -8px; + bottom: -8px; + left: -24px; + pointer-events: none; + content: ""; + border: 2px solid var(--color-accent-emphasis); + border-radius: 6px; + } + + .markdown-body .footnotes li:target { + color: var(--color-fg-default); + } + + .markdown-body .footnotes .data-footnote-backref g-emoji { + font-family: monospace; + } + + .markdown-body .pl-c { + color: var(--color-prettylights-syntax-comment); + } + + .markdown-body .pl-c1, + .markdown-body .pl-s .pl-v { + color: var(--color-prettylights-syntax-constant); + } + + .markdown-body .pl-e, + .markdown-body .pl-en { + color: var(--color-prettylights-syntax-entity); + } + + .markdown-body .pl-smi, + .markdown-body .pl-s .pl-s1 { + color: var(--color-prettylights-syntax-storage-modifier-import); + } + + .markdown-body .pl-ent { + color: var(--color-prettylights-syntax-entity-tag); + } + + .markdown-body .pl-k { + color: var(--color-prettylights-syntax-keyword); + } + + .markdown-body .pl-s, + .markdown-body .pl-pds, + .markdown-body .pl-s .pl-pse .pl-s1, + .markdown-body .pl-sr, + .markdown-body .pl-sr .pl-cce, + .markdown-body .pl-sr .pl-sre, + .markdown-body .pl-sr .pl-sra { + color: var(--color-prettylights-syntax-string); + } + + .markdown-body .pl-v, + .markdown-body .pl-smw { + color: var(--color-prettylights-syntax-variable); + } + + .markdown-body .pl-bu { + color: var(--color-prettylights-syntax-brackethighlighter-unmatched); + } + + .markdown-body .pl-ii { + color: var(--color-prettylights-syntax-invalid-illegal-text); + background-color: var(--color-prettylights-syntax-invalid-illegal-bg); + } + + .markdown-body .pl-c2 { + color: var(--color-prettylights-syntax-carriage-return-text); + background-color: var(--color-prettylights-syntax-carriage-return-bg); + } + + .markdown-body .pl-sr .pl-cce { + font-weight: bold; + color: var(--color-prettylights-syntax-string-regexp); + } + + .markdown-body .pl-ml { + color: var(--color-prettylights-syntax-markup-list); + } + + .markdown-body .pl-mh, + .markdown-body .pl-mh .pl-en, + .markdown-body .pl-ms { + font-weight: bold; + color: var(--color-prettylights-syntax-markup-heading); + } + + .markdown-body .pl-mi { + font-style: italic; + color: var(--color-prettylights-syntax-markup-italic); + } + + .markdown-body .pl-mb { + font-weight: bold; + color: var(--color-prettylights-syntax-markup-bold); + } + + .markdown-body .pl-md { + color: var(--color-prettylights-syntax-markup-deleted-text); + background-color: var(--color-prettylights-syntax-markup-deleted-bg); + } + + .markdown-body .pl-mi1 { + color: var(--color-prettylights-syntax-markup-inserted-text); + background-color: var(--color-prettylights-syntax-markup-inserted-bg); + } + + .markdown-body .pl-mc { + color: var(--color-prettylights-syntax-markup-changed-text); + background-color: var(--color-prettylights-syntax-markup-changed-bg); + } + + .markdown-body .pl-mi2 { + color: var(--color-prettylights-syntax-markup-ignored-text); + background-color: var(--color-prettylights-syntax-markup-ignored-bg); + } + + .markdown-body .pl-mdr { + font-weight: bold; + color: var(--color-prettylights-syntax-meta-diff-range); + } + + .markdown-body .pl-ba { + color: var(--color-prettylights-syntax-brackethighlighter-angle); + } + + .markdown-body .pl-sg { + color: var(--color-prettylights-syntax-sublimelinter-gutter-mark); + } + + .markdown-body .pl-corl { + text-decoration: underline; + color: var(--color-prettylights-syntax-constant-other-reference-link); + } + + .markdown-body g-emoji { + display: inline-block; + min-width: 1ch; + font-family: "Apple Color Emoji","Segoe UI Emoji","Segoe UI Symbol"; + font-size: 1em; + font-style: normal !important; + font-weight: var(--base-text-weight-normal, 400); + line-height: 1; + vertical-align: -0.075em; + } + + .markdown-body g-emoji img { + width: 1em; + height: 1em; + } + + .markdown-body .task-list-item { + list-style-type: none; + } + + .markdown-body .task-list-item label { + font-weight: var(--base-text-weight-normal, 400); + } + + .markdown-body .task-list-item.enabled label { + cursor: pointer; + } + + .markdown-body .task-list-item+.task-list-item { + margin-top: 4px; + } + + .markdown-body .task-list-item .handle { + display: none; + } + + .markdown-body .task-list-item-checkbox { + margin: 0 .2em .25em -1.4em; + vertical-align: middle; + } + + .markdown-body .contains-task-list:dir(rtl) .task-list-item-checkbox { + margin: 0 -1.6em .25em .2em; + } + + .markdown-body .contains-task-list { + position: relative; + } + + .markdown-body .contains-task-list:hover .task-list-item-convert-container, + .markdown-body .contains-task-list:focus-within .task-list-item-convert-container { + display: block; + width: auto; + height: 24px; + overflow: visible; + clip: auto; + } + + .markdown-body ::-webkit-calendar-picker-indicator { + filter: invert(50%); + } \ No newline at end of file diff --git a/.devcontainer/caddy/srv/nav2/index.html b/.devcontainer/caddy/srv/nav2/index.html new file mode 100644 index 00000000000..4bebe27f4c7 --- /dev/null +++ b/.devcontainer/caddy/srv/nav2/index.html @@ -0,0 +1,36 @@ +{{$pathParts := splitList "/" .OriginalReq.URL.Path}} +{{$markdownFilename := default "index" (slice $pathParts 2 | join "/")}} +{{$markdownFilePath := printf "/%s.md" $markdownFilename}} +{{if not (fileExists $markdownFilePath)}}{{httpError 404}}{{end}} +{{$markdownFile := (include $markdownFilePath | splitFrontMatter)}} +{{$title := default $markdownFilename $markdownFile.Meta.title}} + + + + + + {{$title}} + + + + + + + +
{{markdown $markdownFile.Body}}
+ + diff --git a/.devcontainer/caddy/srv/nav2/index.md b/.devcontainer/caddy/srv/nav2/index.md new file mode 100644 index 00000000000..60e2db1bfb6 --- /dev/null +++ b/.devcontainer/caddy/srv/nav2/index.md @@ -0,0 +1,51 @@ +{ + "title": "Nav2 App" +} +## Progressive Web Apps + +| PWAs | Shortcuts | +|-|-| +| [](/foxglove/autoconnect)
**Foxglove** | [**Auto Connect**](/foxglove/autoconnect)
[Auto Layout](/foxglove/autolayout)
[Manual](/foxglove/) | +| [](/gzweb/)
**Gzweb** | [**Auto Connect**](/gzweb/) | +| [](/glances/)
**Glances** | [**System Monitor**](/glances/)
[Refresh 1sec](/glances/1)
[Refresh 10sec](/glances/10) | +| [](/nav2/)
**Nav2** | [**App Launcher**](/nav2/)
[File Browser](/?browse=true) | + +## External Resources + +For more related documentation: + +- [Nav2 Documentation](https://navigation.ros.org) + - [Development Guides](https://navigation.ros.org/development_guides) + - [Dev Containers](https://navigation.ros.org/development_guides/devcontainer_docs) + +## Session Info + +Useful information about host server and remote client: + +|Key | Value | +|-|-| +| Host | `{{.Host}}` | +| Remote IP | `{{placeholder "http.request.remote.host"}}` | +| Date | `{{now}}` | + +### Server Diagnostics + +
+Websocket Debug + +For troubleshooting websocket connections: + +|Key | Value | +|-|-| +| `header.X-Forwarded-Host` | `{{placeholder "http.request.header.X-Forwarded-Host"}}` | +| `http.request.hostport` | `{{placeholder "http.request.hostport"}}` | +| `http.vars.ReqHost` | `{{placeholder "http.vars.ReqHost"}}` | + +|Key | Value | +|-|-| +| `http.request.scheme` | `{{placeholder "http.request.scheme"}}` | +| `header.X-Forwarded-Scheme` | `{{placeholder "http.request.header.X-Forwarded-Scheme"}}` | +| `header.X-Forwarded-Proto` | `{{placeholder "http.request.header.X-Forwarded-Proto"}}` | +| `http.vars.WsScheme` | `{{placeholder "http.vars.WsScheme"}}` | + +
diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json new file mode 100644 index 00000000000..09aaa5d31d4 --- /dev/null +++ b/.devcontainer/devcontainer.json @@ -0,0 +1,61 @@ +{ + "name": "Nav2", + "build": { + "dockerfile": "../Dockerfile", + "context": "..", + "target": "visualizer", + "cacheFrom": "ghcr.io/ros-planning/navigation2:main" + }, + "runArgs": [ + // "--cap-add=SYS_PTRACE", // enable debugging, e.g. gdb + // "--ipc=host", // shared memory transport with host, e.g. rviz GUIs + // "--network=host", // network access to host interfaces, e.g. eth0 + // "--pid=host", // DDS discovery with host, without --network=host + // "--privileged", // device access to host peripherals, e.g. USB + // "--security-opt=seccomp=unconfined", // enable debugging, e.g. gdb + ], + "workspaceFolder": "/opt/overlay_ws/src/navigation2", + "workspaceMount": "source=${localWorkspaceFolder},target=${containerWorkspaceFolder},type=bind", + "onCreateCommand": ".devcontainer/on-create-command.sh", + "updateContentCommand": ".devcontainer/update-content-command.sh", + "postCreateCommand": ".devcontainer/post-create-command.sh", + "remoteEnv": { + "OVERLAY_MIXINS": "release ccache lld", + "CCACHE_DIR": "/tmp/.ccache" + }, + "mounts": [ + { + "source": "ccache-${devcontainerId}", + "target": "/tmp/.ccache", + "type": "volume" + }, + { + "source": "overlay-${devcontainerId}", + "target": "/opt/overlay_ws", + "type": "volume" + } + ], + "features": { + // "ghcr.io/devcontainers/features/desktop-lite:1": {}, + "ghcr.io/devcontainers/features/github-cli:1": {} + }, + "customizations": { + "codespaces": { + "openFiles": [ + "doc/development/codespaces.md" + ] + }, + "vscode": { + "settings": {}, + "extensions": [ + "althack.ament-task-provider", + "eamodio.gitlens", + "esbenp.prettier-vscode", + "GitHub.copilot", + "ms-iot.vscode-ros", + "streetsidesoftware.code-spell-checker", + "twxs.cmake" + ] + } + } +} diff --git a/.devcontainer/on-create-command.sh b/.devcontainer/on-create-command.sh new file mode 100755 index 00000000000..f86d22bc8d0 --- /dev/null +++ b/.devcontainer/on-create-command.sh @@ -0,0 +1,12 @@ +#!/bin/bash + +# Immediately catch all errors +set -eo pipefail + +# Uncomment for debugging +# set -x +# env + +git config --global --add safe.directory "*" + +.devcontainer/update-content-command.sh diff --git a/.devcontainer/post-create-command.sh b/.devcontainer/post-create-command.sh new file mode 100755 index 00000000000..9f64a2c6ab6 --- /dev/null +++ b/.devcontainer/post-create-command.sh @@ -0,0 +1,18 @@ +#!/bin/bash + +# Immediately catch all errors +set -eo pipefail + +# Uncomment for debugging +# set -x +# env + +# Enable autocomplete for user +cp /etc/skel/.bashrc ~/ + +# Check if srv folder exists +if [ -d "$ROOT_SRV" ]; then + # Setup Nav2 web app + for dir in $OVERLAY_WS/src/navigation2/.devcontainer/caddy/srv/*; \ + do if [ -d "$dir" ]; then ln -s "$dir" $ROOT_SRV; fi done +fi diff --git a/.devcontainer/update-content-command.sh b/.devcontainer/update-content-command.sh new file mode 100755 index 00000000000..2bdbf5cc1a3 --- /dev/null +++ b/.devcontainer/update-content-command.sh @@ -0,0 +1,60 @@ +#!/bin/bash + +# Immediately catch all errors +set -eo pipefail + +# Uncomment for debugging +# set -x +# env + +cd $OVERLAY_WS + +colcon cache lock + +BUILD_UNFINISHED=$( + colcon list \ + --names-only \ + --packages-skip-build-finished \ + | xargs) +echo BUILD_UNFINISHED: $BUILD_UNFINISHED + +BUILD_FAILED=$( + colcon list \ + --names-only \ + --packages-select-build-failed \ + | xargs) +echo BUILD_FAILED: $BUILD_FAILED + +BUILD_INVALID=$( + colcon list \ + --names-only \ + --packages-select-cache-invalid \ + --packages-select-cache-key build \ + | xargs) +echo BUILD_INVALID: $BUILD_INVALID + +BUILD_PACKAGES="" +if [ -n "$BUILD_UNFINISHED" ] || \ + [ -n "$BUILD_FAILED" ] || \ + [ -n "$BUILD_INVALID" ] +then + BUILD_PACKAGES=$( + colcon list \ + --names-only \ + --packages-above \ + $BUILD_UNFINISHED \ + $BUILD_FAILED \ + $BUILD_INVALID \ + | xargs) +fi +echo BUILD_PACKAGES: $BUILD_PACKAGES + +# colcon clean packages --yes \ +# --packages-select ${BUILD_PACKAGES} \ +# --base-select install + +. $UNDERLAY_WS/install/setup.sh +colcon build \ + --symlink-install \ + --mixin $OVERLAY_MIXINS \ + --packages-select ${BUILD_PACKAGES} diff --git a/.dockerignore b/.dockerignore index 5e31fef9d5b..19c4cc591c8 100644 --- a/.dockerignore +++ b/.dockerignore @@ -2,10 +2,11 @@ # Repo .circleci/ -.github/ +.devcontainer/ +.dockerignore .git/ .github/ -.dockerignore .gitignore -**Dockerfile **.Dockerfile +**Dockerfile +doc/ diff --git a/.github/workflows/update_ci_image.yaml b/.github/workflows/update_ci_image.yaml index 9358ea96295..9067e923742 100644 --- a/.github/workflows/update_ci_image.yaml +++ b/.github/workflows/update_ci_image.yaml @@ -12,6 +12,7 @@ on: - '**/package.xml' - '**/*.repos' - 'Dockerfile' + - '.github/workflows/update_ci_image.yaml' jobs: check_ci_files: @@ -25,8 +26,8 @@ jobs: id: check if: github.event_name == 'push' run: | - echo "::set-output name=trigger::true" - echo "::set-output name=no_cache::false" + echo "trigger=true" >> $GITHUB_OUTPUT + echo "no_cache=false" >> $GITHUB_OUTPUT check_ci_image: name: Check CI Image if: github.event_name == 'schedule' @@ -36,7 +37,7 @@ jobs: trigger: ${{ steps.check.outputs.trigger }} no_cache: ${{ steps.check.outputs.no_cache }} container: - image: ghcr.io/ros-planning/navigation2:main + image: ghcr.io/${{ github.repository }}:${{ github.ref_name }} steps: - name: "Check apt updates" id: check @@ -51,9 +52,9 @@ jobs: cat upgrade.log cat upgrade.log \ | grep "^0 upgraded, 0 newly installed, 0 to remove and 0 not upgraded.$" \ - && echo "::set-output name=trigger::false" \ - || echo "::set-output name=trigger::true" - echo "::set-output name=no_cache::true" + && echo "trigger=false" >> $GITHUB_OUTPUT \ + || echo "trigger=true" >> $GITHUB_OUTPUT + echo "no_cache=true" >> $GITHUB_OUTPUT rebuild_ci_image: name: Rebuild CI Image if: always() @@ -62,6 +63,7 @@ jobs: - check_ci_image runs-on: ubuntu-latest steps: + - uses: actions/checkout@v3 - name: Set up Docker Buildx uses: docker/setup-buildx-action@v2 - name: Login to Docker Hub @@ -73,8 +75,11 @@ jobs: - name: Set build config id: config run: | - timestamp=$(date --utc +%Y%m%d%H%M%S) - echo "::set-output name=timestamp::${timestamp}" + created=$(date -u +'%Y-%m-%dT%H:%M:%SZ') + echo "created=${created}" >> $GITHUB_OUTPUT + + version=$(grep -oP '(?<=).*?(?=)' navigation2/package.xml) + echo "version=${version}" >> $GITHUB_OUTPUT no_cache=false if [ "${{needs.check_ci_files.outputs.no_cache}}" == 'true' ] || \ @@ -82,7 +87,7 @@ jobs: then no_cache=true fi - echo "::set-output name=no_cache::${no_cache}" + echo "no_cache=${no_cache}" >> $GITHUB_OUTPUT trigger=false if [ "${{needs.check_ci_files.outputs.trigger}}" == 'true' ] || \ @@ -90,21 +95,36 @@ jobs: then trigger=true fi - echo "::set-output name=trigger::${trigger}" - - name: Build and push + echo "trigger=${trigger}" >> $GITHUB_OUTPUT + - name: Build and push ${{ github.ref_name }} if: steps.config.outputs.trigger == 'true' id: docker_build uses: docker/build-push-action@v3 with: + context: . pull: true push: true + provenance: false no-cache: ${{ steps.config.outputs.no_cache }} - cache-from: type=registry,ref=ghcr.io/ros-planning/navigation2:main + cache-from: type=registry,ref=ghcr.io/${{ github.repository }}:${{ github.ref_name }} cache-to: type=inline target: builder tags: | - ghcr.io/ros-planning/navigation2:main - ghcr.io/ros-planning/navigation2:main-${{ steps.config.outputs.timestamp }} + ghcr.io/${{ github.repository }}:${{ github.ref_name }} + ghcr.io/${{ github.repository }}:${{ github.ref_name }}-${{ steps.config.outputs.version }} + labels: | + org.opencontainers.image.authors=${{ github.event.repository.owner.html_url }} + org.opencontainers.image.created=${{ steps.config.outputs.created }} + org.opencontainers.image.description=${{ github.event.repository.description }} + org.opencontainers.image.documentation=${{ github.event.repository.homepage }} + org.opencontainers.image.licenses=${{ github.event.repository.license.spdx_id }} + org.opencontainers.image.ref.name=${{ github.ref }} + org.opencontainers.image.revision=${{ github.sha }} + org.opencontainers.image.source=${{ github.event.repository.clone_url }} + org.opencontainers.image.title=${{ github.event.repository.name }} + org.opencontainers.image.url=${{ github.event.repository.html_url }} + org.opencontainers.image.vendor=${{ github.event.repository.owner.login }} + org.opencontainers.image.version=${{ steps.config.outputs.version }} - name: Image digest if: steps.config.outputs.trigger == 'true' run: echo ${{ steps.docker_build.outputs.digest }} diff --git a/.vscode/tasks.json b/.vscode/tasks.json new file mode 100644 index 00000000000..39bdc832813 --- /dev/null +++ b/.vscode/tasks.json @@ -0,0 +1,179 @@ +{ + // See https://go.microsoft.com/fwlink/?LinkId=733558 + // for the documentation about the tasks.json format + "version": "2.0.0", + "tasks": [ + { + "label": "Web Server", + "icon": { + "id": "debug-start" + }, + "type": "process", + "command": "caddy", + "args": [ + "run", + "--watch" + ], + "options": { + "cwd": "${workspaceFolder}/.devcontainer/caddy" + }, + "isBackground": true, + "hide": true, + "problemMatcher": [] + }, + { + "label": "Stop Web Server", + "icon": { + "id": "debug-stop" + }, + "type": "shell", + "command": "echo ${input:Terminate Web Server}", + "hide": true, + "problemMatcher": [] + }, + { + "label": "System Monitor", + "icon": { + "id": "debug-start" + }, + "type": "process", + "command": "glances", + "args": [ + "-w" + ], + "isBackground": true, + "hide": true, + "problemMatcher": [] + }, + { + "label": "Stop System Monitor", + "icon": { + "id": "debug-stop" + }, + "type": "shell", + "command": "echo ${input:Terminate System Monitor}", + "hide": true, + "problemMatcher": [] + }, + { + "label": "Gzweb Bridge", + "icon": { + "id": "debug-start" + }, + "type": "process", + "command": "npm", + "args": [ + "start" + ], + "options": { + "cwd": "${env:GZWEB_WS}", + "env": { + "npm_config_port": "9090" + } + }, + "hide": true, + "isBackground": true, + "problemMatcher": [] + }, + { + "label": "Stop Gzweb Bridge", + "icon": { + "id": "debug-stop" + }, + "type": "shell", + "command": "echo ${input:Terminate Gzweb Bridge}", + "hide": true, + "problemMatcher": [] + }, + { + "label": "Foxglove Bridge", + "icon": { + "id": "debug-start" + }, + "type": "shell", + "command": "source ./install/setup.bash && ros2 run foxglove_bridge foxglove_bridge", + "options": { + "cwd": "${env:OVERLAY_WS}", + }, + "isBackground": true, + "hide": true, + "problemMatcher": [] + }, + { + "label": "Stop Foxglove Bridge", + "icon": { + "id": "debug-stop" + }, + "type": "shell", + "command": "echo ${input:Terminate Foxglove Bridge}", + "hide": true, + "problemMatcher": [] + }, + { + "label": "Start Visualizations", + "icon": { + "id": "debug-start" + }, + "dependsOn": [ + "Web Server", + "System Monitor", + "Gzweb Bridge", + "Foxglove Bridge" + ], + // "hide": true, + "problemMatcher": [] + }, + { + "label": "Stop Visualizations", + "icon": { + "id": "debug-stop" + }, + "dependsOn": [ + "Stop Web Server", + "Stop System Monitor", + "Stop Gzweb Bridge", + "Stop Foxglove Bridge" + ], + // "hide": true, + "problemMatcher": [] + }, + { + "label": "Restart Visualizations", + "icon": { + "id": "debug-restart" + }, + "dependsOn": [ + "Stop Visualizations", + "Start Visualizations" + ], + "dependsOrder": "sequence", + "problemMatcher": [] + } + ], + "inputs": [ + { + "id": "Terminate Web Server", + "type": "command", + "command": "workbench.action.tasks.terminate", + "args": "Web Server" + }, + { + "id": "Terminate System Monitor", + "type": "command", + "command": "workbench.action.tasks.terminate", + "args": "System Monitor" + }, + { + "id": "Terminate Gzweb Bridge", + "type": "command", + "command": "workbench.action.tasks.terminate", + "args": "Gzweb Bridge" + }, + { + "id": "Terminate Foxglove Bridge", + "type": "command", + "command": "workbench.action.tasks.terminate", + "args": "Foxglove Bridge" + }, + ] +} diff --git a/Dockerfile b/Dockerfile index ba26fc246f1..69c89230193 100644 --- a/Dockerfile +++ b/Dockerfile @@ -125,10 +125,103 @@ RUN sed --in-place \ # test overlay build ARG RUN_TESTS -ARG FAIL_ON_TEST_FAILURE=True +ARG FAIL_ON_TEST_FAILURE RUN if [ -n "$RUN_TESTS" ]; then \ . install/setup.sh && \ colcon test && \ colcon test-result \ || ([ -z "$FAIL_ON_TEST_FAILURE" ] || exit 1) \ fi + +# multi-stage for developing +FROM builder AS dever + +# edit apt for caching +RUN mv /etc/apt/apt.conf.d/docker-clean /etc/apt/ + +# install developer dependencies +RUN apt-get update && \ + apt-get install -y \ + bash-completion \ + gdb \ + wget && \ + pip3 install \ + bottle \ + glances + +# source underlay for shell +RUN echo 'source "$UNDERLAY_WS/install/setup.bash"' >> /etc/bash.bashrc + +# multi-stage for caddy +FROM caddy:builder AS caddyer + +# build custom modules +RUN xcaddy build \ + --with github.com/caddyserver/replace-response + +# multi-stage for visualizing +FROM dever AS visualizer + +ENV ROOT_SRV /srv +RUN mkdir -p $ROOT_SRV + +# install demo dependencies +RUN apt-get update && apt-get install -y \ + ros-$ROS_DISTRO-aws-robomaker-small-warehouse-world \ + ros-$ROS_DISTRO-rviz2 \ + ros-$ROS_DISTRO-turtlebot3-simulations + +# install gzweb dependacies +RUN apt-get install -y --no-install-recommends \ + imagemagick \ + libboost-all-dev \ + libgazebo-dev \ + libgts-dev \ + libjansson-dev \ + libtinyxml-dev \ + nodejs \ + npm \ + psmisc \ + xvfb + +# clone gzweb +ENV GZWEB_WS /opt/gzweb +RUN git clone https://github.com/osrf/gzweb.git $GZWEB_WS + +# setup gzweb +RUN cd $GZWEB_WS && . /usr/share/gazebo/setup.sh && \ + GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:$(find /opt/ros/$ROS_DISTRO/share \ + -mindepth 1 -maxdepth 2 -type d -name "models" | paste -s -d: -) && \ + sed -i "s|var modelList =|var modelList = []; var oldModelList =|g" gz3d/src/gzgui.js && \ + xvfb-run -s "-screen 0 1280x1024x24" ./deploy.sh -m local && \ + ln -s $GZWEB_WS/http/client/assets http/client/assets/models && \ + ln -s $GZWEB_WS/http/client $ROOT_SRV/gzweb + +# patch gzsever +RUN GZSERVER=$(which gzserver) && \ + mv $GZSERVER $GZSERVER.orig && \ + echo '#!/bin/bash' > $GZSERVER && \ + echo 'exec xvfb-run -s "-screen 0 1280x1024x24" gzserver.orig "$@"' >> $GZSERVER && \ + chmod +x $GZSERVER + +# install foxglove dependacies +RUN apt-get install -y --no-install-recommends \ + ros-$ROS_DISTRO-foxglove-bridge + +# setup foxglove +# Use custom fork until PR is merged: +# https://github.com/foxglove/studio/pull/5987 +# COPY --from=ghcr.io/foxglove/studio /src $ROOT_SRV/foxglove +COPY --from=ghcr.io/ruffsl/foxglove_studio@sha256:8a2f2be0a95f24b76b0d7aa536f1c34f3e224022eed607cbf7a164928488332e /src $ROOT_SRV/foxglove + +# install web server +COPY --from=caddyer /usr/bin/caddy /usr/bin/caddy + +# download media files +RUN mkdir -p $ROOT_SRV/media && cd /tmp && \ + export ICONS="icons.tar.gz" && wget https://github.com/ros-planning/navigation2/files/11506823/$ICONS && \ + echo "cae5e2a5230f87b004c8232b579781edb4a72a7431405381403c6f9e9f5f7d41 $ICONS" | sha256sum -c && \ + tar xvz -C $ROOT_SRV/media -f $ICONS && rm $ICONS + +# multi-stage for exporting +FROM tester AS exporter diff --git a/README.md b/README.md index 0acfb8fe5c8..488eb280427 100644 --- a/README.md +++ b/README.md @@ -16,10 +16,27 @@ For detailed instructions on how to: - [Navigation Plugins](https://navigation.ros.org/plugins/index.html) - [Migration Guides](https://navigation.ros.org/migration/index.html) - [Container Images for Building Nav2](https://github.com/orgs/ros-planning/packages/container/package/navigation2) -- [Contribute](https://navigation.ros.org/contribute/index.html) +- [Contribute](https://navigation.ros.org/development_guides/involvement_docs/index.html) Please visit our [documentation site](https://navigation.ros.org/). [Please visit our community Slack here](https://join.slack.com/t/navigation2/shared_invite/zt-hu52lnnq-cKYjuhTY~sEMbZXL8p9tOw) (if this link does not work, please contact maintainers to reactivate). +## Our Sponsors + +Please thank our amazing sponsors for their generous support of Nav2 on behalf of the community to allow the project to continue to be professionally maintained, developed, and supported for the long-haul! [Open Navigation LLC](https://www.opennav.org/) provides project leadership, maintenance, development, and support services to the Nav2 & ROS community. + +

+ +

+ +### [Dexory](https://www.dexory.com/) develops robotics and AI logistics solutions to drive better business decisions using a digital twin of warehouses to provide inventory insights. + +### [Polymath Robotics](https://www.polymathrobotics.com/) creates safety-critical navigation systems for industrial vehicles that are radically simple to enable and deploy. + +### [Stereolabs](https://www.stereolabs.com/) produces the high-quality ZED stereo cameras with a complete vision pipeline from neural depth to SLAM, 3D object tracking, AI and more. + +### Confidential is just happy to support Nav2's mission! + + ## Citation If you use the navigation framework, an algorithm from this repository, or ideas from it @@ -54,39 +71,39 @@ please cite this work in your papers! ## Build Status -| Service | Foxy | Galactic | Humble | Main | +| Service | Foxy | Humble | Main | +| :---: | :---: | :---: | :---: | +| ROS Build Farm | [![Build Status](http://build.ros2.org/job/Fdev__navigation2__ubuntu_focal_amd64/badge/icon)](http://build.ros2.org/job/Fdev__navigation2__ubuntu_focal_amd64/) | [![Build Status](https://build.ros2.org/job/Hdev__navigation2__ubuntu_jammy_amd64/badge/icon)](https://build.ros2.org/job/Hdev__navigation2__ubuntu_jammy_amd64/) | N/A | +| Circle CI | N/A | N/A | [![Build Status](https://circleci.com/gh/ros-planning/navigation2/tree/main.svg?style=svg)](https://circleci.com/gh/ros-planning/navigation2/tree/main) | + + +| Package | Foxy Source | Foxy Debian | Humble Source | Humble Debian | | :---: | :---: | :---: | :---: | :---: | -| ROS Build Farm | [![Build Status](http://build.ros2.org/job/Fdev__navigation2__ubuntu_focal_amd64/badge/icon)](http://build.ros2.org/job/Fdev__navigation2__ubuntu_focal_amd64/) | [![Build Status](http://build.ros2.org/job/Gdev__navigation2__ubuntu_focal_amd64/badge/icon)](http://build.ros2.org/job/Gdev__navigation2__ubuntu_focal_amd64/) | [![Build Status](https://build.ros2.org/job/Hdev__navigation2__ubuntu_jammy_amd64/badge/icon)](https://build.ros2.org/job/Hdev__navigation2__ubuntu_jammy_amd64/) | N/A | -| Circle CI | N/A | N/A | N/A | [![Build Status](https://circleci.com/gh/ros-planning/navigation2/tree/main.svg?style=svg)](https://circleci.com/gh/ros-planning/navigation2/tree/main) | - - -| Package | Foxy Source | Foxy Debian | Galactic Source | Galactic Debian | Humble Source | Humble Debian | -| :---: | :---: | :---: | :---: | :---: | :---: | :---: | -| Navigation2 | [![Build Status](http://build.ros2.org/job/Fsrc_uF__navigation2__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__navigation2__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__navigation2__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__navigation2__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__navigation2__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__navigation2__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__navigation2__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__navigation2__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__navigation2__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__navigation2__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__navigation2__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__navigation2__ubuntu_jammy_amd64__binary/) | -| nav2_amcl | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_amcl__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_amcl__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_amcl__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_amcl__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_amcl__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_amcl__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_amcl__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_amcl__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_amcl__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_amcl__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_amcl__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_amcl__ubuntu_jammy_amd64__binary/) | -| nav2_behavior_tree | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_behavior_tree__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_behavior_tree__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_behavior_tree__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_behavior_tree__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_behavior_tree__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_behavior_tree__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_behavior_tree__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_behavior_tree__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_behavior_tree__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_behavior_tree__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_behavior_tree__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_behavior_tree__ubuntu_jammy_amd64__binary/) | -| nav2_{recoveries, behaviors} | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_recoveries__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_recoveries__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_recoveries__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_recoveries__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_recoveries__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_recoveries__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_recoveries__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_recoveries__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_behaviors__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_behaviors__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_behaviors__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_behaviors__ubuntu_jammy_amd64__binary/) | -| nav2_bringup | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_bringup__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_bringup__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_bringup__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_bringup__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_bringup__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_bringup__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_bringup__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_bringup__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_bringup__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_bringup__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_bringup__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_bringup__ubuntu_jammy_amd64__binary/) | -| nav2_bt_navigator | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_bt_navigator__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_bt_navigator__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_bt_navigator__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_bt_navigator__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_bt_navigator__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_bt_navigator__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_bt_navigator__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_bt_navigator__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_bt_navigator__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_bt_navigator__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_bt_navigator__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_bt_navigator__ubuntu_jammy_amd64__binary/) | -| nav2_common | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_common__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_common__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_common__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_common__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_common__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_common__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_common__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_common__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_common__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_common__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_common__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_common__ubuntu_jammy_amd64__binary/) | +| Navigation2 | [![Build Status](http://build.ros2.org/job/Fsrc_uF__navigation2__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__navigation2__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__navigation2__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__navigation2__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__navigation2__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__navigation2__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__navigation2__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__navigation2__ubuntu_jammy_amd64__binary/) | +| nav2_amcl | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_amcl__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_amcl__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_amcl__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_amcl__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_amcl__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_amcl__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_amcl__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_amcl__ubuntu_jammy_amd64__binary/) | +| nav2_behavior_tree | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_behavior_tree__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_behavior_tree__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_behavior_tree__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_behavior_tree__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_behavior_tree__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_behavior_tree__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_behavior_tree__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_behavior_tree__ubuntu_jammy_amd64__binary/) | +| nav2_{recoveries, behaviors} | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_recoveries__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_recoveries__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_recoveries__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_recoveries__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_behaviors__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_behaviors__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_behaviors__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_behaviors__ubuntu_jammy_amd64__binary/) | +| nav2_bringup | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_bringup__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_bringup__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_bringup__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_bringup__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_bringup__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_bringup__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_bringup__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_bringup__ubuntu_jammy_amd64__binary/) | +| nav2_bt_navigator | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_bt_navigator__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_bt_navigator__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_bt_navigator__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_bt_navigator__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_bt_navigator__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_bt_navigator__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_bt_navigator__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_bt_navigator__ubuntu_jammy_amd64__binary/) | +| nav2_common | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_common__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_common__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_common__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_common__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_common__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_common__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_common__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_common__ubuntu_jammy_amd64__binary/) | | nav2_constrained_smoother | N/A | N/A | N/A | N/A | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_constrained_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_constrained_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_constrained_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_constrained_smoother__ubuntu_jammy_amd64__binary/) | -| nav2_controller | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_controller__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_controller__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_controller__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_controller__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_controller__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_controller__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_controller__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_controller__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_controller__ubuntu_jammy_amd64__binary/) | -| nav2_core | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_core__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_core__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_core__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_core__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_core__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_core__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_core__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_core__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_core__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_core__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_core__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_core__ubuntu_jammy_amd64__binary/) | -| nav2_costmap_2d | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_costmap_2d__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_costmap_2d__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_costmap_2d__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_costmap_2d__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_costmap_2d__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_costmap_2d__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_costmap_2d__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_costmap_2d__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_costmap_2d__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_costmap_2d__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_costmap_2d__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_costmap_2d__ubuntu_jammy_amd64__binary/) | -| nav2_dwb_controller | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_dwb_controller__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_dwb_controller__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_dwb_controller__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_dwb_controller__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_dwb_controller__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_dwb_controller__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_dwb_controller__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_dwb_controller__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_dwb_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_dwb_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_dwb_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_dwb_controller__ubuntu_jammy_amd64__binary/) | -| nav2_lifecycle_manager | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_lifecycle_manager__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_lifecycle_manager__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_lifecycle_manager__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_lifecycle_manager__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_lifecycle_manager__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_lifecycle_manager__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_lifecycle_manager__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_lifecycle_manager__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_lifecycle_manager__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_lifecycle_manager__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_lifecycle_manager__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_lifecycle_manager__ubuntu_jammy_amd64__binary/) | -| nav2_map_server | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_map_server__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_map_server__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_map_server__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_map_server__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_map_server__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_map_server__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_map_server__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_map_server__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_map_server__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_map_server__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_map_server__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_map_server__ubuntu_jammy_amd64__binary/) | -| nav2_msgs | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_msgs__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_msgs__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_msgs__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_msgs__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_msgs__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_msgs__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_msgs__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_msgs__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_msgs__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_msgs__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_msgs__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_msgs__ubuntu_jammy_amd64__binary/) | -| nav2_navfn_planner | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_navfn_planner__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_navfn_planner__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_navfn_planner__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_navfn_planner__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_navfn_planner__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_navfn_planner__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_navfn_planner__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_navfn_planner__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_navfn_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_navfn_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_navfn_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_navfn_planner__ubuntu_jammy_amd64__binary/) | -| nav2_planner | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_planner__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_planner__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_planner__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_planner__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_planner__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_planner__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_planner__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_planner__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_planner__ubuntu_jammy_amd64__binary/) | -| nav2_regulated_pure_pursuit | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_regulated_pure_pursuit_controller__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_regulated_pure_pursuit_controller__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_regulated_pure_pursuit_controller__ubuntu_focal_amd64__binary/badge/icon)](https://build.ros2.org/job/Fbin_uF64__nav2_regulated_pure_pursuit_controller__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_regulated_pure_pursuit_controller__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_regulated_pure_pursuit_controller__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_regulated_pure_pursuit_controller__ubuntu_focal_amd64__binary/badge/icon)](https://build.ros2.org/job/Gbin_uF64__nav2_regulated_pure_pursuit_controller__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_regulated_pure_pursuit_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_regulated_pure_pursuit_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_regulated_pure_pursuit_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_regulated_pure_pursuit_controller__ubuntu_jammy_amd64__binary/) | +| nav2_controller | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_controller__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_controller__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_controller__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_controller__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_controller__ubuntu_jammy_amd64__binary/) | +| nav2_core | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_core__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_core__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_core__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_core__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_core__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_core__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_core__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_core__ubuntu_jammy_amd64__binary/) | +| nav2_costmap_2d | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_costmap_2d__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_costmap_2d__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_costmap_2d__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_costmap_2d__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_costmap_2d__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_costmap_2d__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_costmap_2d__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_costmap_2d__ubuntu_jammy_amd64__binary/) | +| nav2_dwb_controller | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_dwb_controller__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_dwb_controller__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_dwb_controller__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_dwb_controller__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_dwb_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_dwb_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_dwb_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_dwb_controller__ubuntu_jammy_amd64__binary/) | +| nav2_lifecycle_manager | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_lifecycle_manager__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_lifecycle_manager__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_lifecycle_manager__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_lifecycle_manager__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_lifecycle_manager__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_lifecycle_manager__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_lifecycle_manager__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_lifecycle_manager__ubuntu_jammy_amd64__binary/) | +| nav2_map_server | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_map_server__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_map_server__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_map_server__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_map_server__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_map_server__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_map_server__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_map_server__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_map_server__ubuntu_jammy_amd64__binary/) | +| nav2_msgs | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_msgs__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_msgs__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_msgs__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_msgs__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_msgs__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_msgs__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_msgs__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_msgs__ubuntu_jammy_amd64__binary/) | +| nav2_navfn_planner | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_navfn_planner__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_navfn_planner__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_navfn_planner__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_navfn_planner__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_navfn_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_navfn_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_navfn_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_navfn_planner__ubuntu_jammy_amd64__binary/) | +| nav2_planner | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_planner__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_planner__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_planner__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_planner__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_planner__ubuntu_jammy_amd64__binary/) | +| nav2_regulated_pure_pursuit | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_regulated_pure_pursuit_controller__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_regulated_pure_pursuit_controller__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_regulated_pure_pursuit_controller__ubuntu_focal_amd64__binary/badge/icon)](https://build.ros2.org/job/Fbin_uF64__nav2_regulated_pure_pursuit_controller__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_regulated_pure_pursuit_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_regulated_pure_pursuit_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_regulated_pure_pursuit_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_regulated_pure_pursuit_controller__ubuntu_jammy_amd64__binary/) | | nav2_rotation_shim_controller | N/A | N/A | N/A | N/A | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_rotation_shim_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_rotation_shim_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_rotation_shim_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_rotation_shim_controller__ubuntu_jammy_amd64__binary/) | -| nav2_rviz_plugins | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_rviz_plugins__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_rviz_plugins__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_rviz_plugins__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_rviz_plugins__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_rviz_plugins__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_rviz_plugins__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_rviz_plugins__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_rviz_plugins__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_rviz_plugins__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_rviz_plugins__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_rviz_plugins__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_rviz_plugins__ubuntu_jammy_amd64__binary/) | -| nav2_simple_commander | N/A | N/A | [![Build Status](https://build.ros2.org/job/Gsrc_uF__nav2_simple_commander__ubuntu_focal__source/badge/icon)](https://build.ros2.org/job/Gsrc_uF__nav2_simple_commander__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/job/Gbin_uF64__nav2_simple_commander__ubuntu_focal_amd64__binary/badge/icon)](https://build.ros2.org/job/Gbin_uF64__nav2_simple_commander__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_simple_commander__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_simple_commander__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_simple_commander__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_simple_commander__ubuntu_jammy_amd64__binary/) | -| nav2_smac_planner | [![Build Status](http://build.ros2.org/job/Fsrc_uF__smac_planner__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__smac_planner__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__smac_planner__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__smac_planner__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_smac_planner__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_smac_planner__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_smac_planner__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_smac_planner__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_smac_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_smac_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_smac_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_smac_planner__ubuntu_jammy_amd64__binary/) | +| nav2_rviz_plugins | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_rviz_plugins__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_rviz_plugins__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_rviz_plugins__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_rviz_plugins__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_rviz_plugins__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_rviz_plugins__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_rviz_plugins__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_rviz_plugins__ubuntu_jammy_amd64__binary/) | +| nav2_simple_commander | N/A | N/A | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_simple_commander__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_simple_commander__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_simple_commander__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_simple_commander__ubuntu_jammy_amd64__binary/) | +| nav2_smac_planner | [![Build Status](http://build.ros2.org/job/Fsrc_uF__smac_planner__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__smac_planner__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__smac_planner__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__smac_planner__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_smac_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_smac_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_smac_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_smac_planner__ubuntu_jammy_amd64__binary/) | | nav2_smoother | N/A | N/A | N/A | N/A | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_smoother__ubuntu_jammy_amd64__binary/) | -| nav2_system_tests | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_system_tests__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_system_tests__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_system_tests__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_system_tests__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_system_tests__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_system_tests__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_system_tests__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_system_tests__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_system_tests__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_system_tests__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_system_tests__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_system_tests__ubuntu_jammy_amd64__binary/) | -| nav2_theta_star_planner | N/A | N/A | [![Build Status](https://build.ros2.org/job/Gsrc_uF__nav2_theta_star_planner__ubuntu_focal__source/badge/icon)](https://build.ros2.org/job/Gsrc_uF__nav2_theta_star_planner__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/job/Gbin_uF64__nav2_theta_star_planner__ubuntu_focal_amd64__binary/badge/icon)](https://build.ros2.org/job/Gbin_uF64__nav2_theta_star_planner__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_theta_star_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_theta_star_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_theta_star_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_theta_star_planner__ubuntu_jammy_amd64__binary/) | -| nav2_util | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_util__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_util__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_util__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_util__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_util__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_util__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_util__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_util__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_util__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_util__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_util__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_util__ubuntu_jammy_amd64__binary/) | -| nav2_voxel_grid | [![Build Status](https://build.ros2.org/job/Fsrc_uF__nav2_voxel_grid__ubuntu_focal__source/badge/icon)](https://build.ros2.org/job/Fsrc_uF__nav2_voxel_grid__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/job/Fbin_uF64__nav2_voxel_grid__ubuntu_focal_amd64__binary/badge/icon)](https://build.ros2.org/job/Fbin_uF64__nav2_voxel_grid__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Gsrc_uF__nav2_voxel_grid__ubuntu_focal__source/badge/icon)](https://build.ros2.org/job/Gsrc_uF__nav2_voxel_grid__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/job/Gbin_uF64__nav2_voxel_grid__ubuntu_focal_amd64__binary/badge/icon)](https://build.ros2.org/job/Gbin_uF64__nav2_voxel_grid__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_voxel_grid__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_voxel_grid__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_voxel_grid__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_voxel_grid__ubuntu_jammy_amd64__binary/) | -| nav2_waypoint_follower | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_waypoint_follower__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_waypoint_follower__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_waypoint_follower__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_waypoint_follower__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_waypoint_follower__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_waypoint_follower__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_waypoint_follower__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_waypoint_follower__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_waypoint_follower__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_waypoint_follower__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_waypoint_follower__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_waypoint_follower__ubuntu_jammy_amd64__binary/) | +| nav2_system_tests | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_system_tests__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_system_tests__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_system_tests__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_system_tests__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_system_tests__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_system_tests__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_system_tests__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_system_tests__ubuntu_jammy_amd64__binary/) | +| nav2_theta_star_planner | N/A | N/A | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_theta_star_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_theta_star_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_theta_star_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_theta_star_planner__ubuntu_jammy_amd64__binary/) | +| nav2_util | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_util__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_util__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_util__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_util__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_util__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_util__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_util__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_util__ubuntu_jammy_amd64__binary/) | +| nav2_voxel_grid | [![Build Status](https://build.ros2.org/job/Fsrc_uF__nav2_voxel_grid__ubuntu_focal__source/badge/icon)](https://build.ros2.org/job/Fsrc_uF__nav2_voxel_grid__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/job/Fbin_uF64__nav2_voxel_grid__ubuntu_focal_amd64__binary/badge/icon)](https://build.ros2.org/job/Fbin_uF64__nav2_voxel_grid__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_voxel_grid__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_voxel_grid__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_voxel_grid__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_voxel_grid__ubuntu_jammy_amd64__binary/) | +| nav2_waypoint_follower | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_waypoint_follower__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_waypoint_follower__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_waypoint_follower__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_waypoint_follower__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_waypoint_follower__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_waypoint_follower__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_waypoint_follower__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_waypoint_follower__ubuntu_jammy_amd64__binary/) | diff --git a/doc/development/codespaces.md b/doc/development/codespaces.md new file mode 100644 index 00000000000..841db7532db --- /dev/null +++ b/doc/development/codespaces.md @@ -0,0 +1,23 @@ +# Codespaces + +TODO: welcome and introduction + +# Overview + +TODO: document devcontainer +TODO: reference extensions +TODO: use of dockercompose and services + +# Terminal + +TODO: link to vscode terminal + +# Graphics and Simulations + +TODO: vnc options +TODO: foxglove example +TODO: gazebo example with gzweb + +# References + +TODO: links to more info \ No newline at end of file diff --git a/doc/sponsors_may_2023.png b/doc/sponsors_may_2023.png new file mode 100644 index 0000000000000000000000000000000000000000..ce928b75e848e148cf3d7634e5db350e172ec9fd GIT binary patch literal 160698 zcmeFZRa}%`)IClp24#YTbcmGFA>A!4Ez;ec1|lV0LwDC84TI7(baxEh-OT@>-}im5 z|Mx#{{5)LDJm;K!_Fj9fwI|@cyae_`!iOj*DA-bxqDm+z_k2-M?vUL_1AqC!HoAy{ z;)^0BDx~77yES9#Y0ew~>qdUc=T z^$)T%vR8wWF1sI>SbuvZwR_kceYA|5*u1RqE|Fh@RH*lOjumPkSLwl*{P)lI*%j%& z&;LAu|MzIgd@=s-IZqG;`~RMlKBEx+-%}>{lBNMhj;q_?}_aB{Z!Qd zeH48CND%#hpL|vR-xvPxX8gY`{ok!X`TrR-+T>409)4p-`~2wm>ytSPGJ10h3qN+Q z8^w@kH5`GYOg@?+l2qrXgL?2%lj^+6lLEwScB8qud551${aX|=BSd{cp8f%iu+_p2 zO~8O1#b=^Ow&9S37w<^!aMgWwzn}W~X~*xk-0Yj9*Z3yP0^(jJy?V!W-n4rw4XJgw zdgt%F4v)^K@X2?ZC}q;--0ZjIKN>rK zK3h1D*gtP_DU(9%MognXHk(u%_c}t?+L3mK@9|$IP}MZkl^m*j&DDpr@Avm~3@GU; zMn|egMZ{b$7?_JX3a?FZ4YEG)BzXSl*}@Y3MaSuJ)$!SSf(dhaMYMfkwF^Gn>GDrd{g)?({XWFpBpr<3YnB#nmOc&^Na)=) z{f@DP`#yPnVINHJT7!ae)M0aX?P2D#?0VT`8Ym{4N5$SJf0D5 z;{O58@|hkP)E-TxMXnKITC5(cd_+CT9>w4$VJHryW z!GV69G9s;8P?7FTM4eRGRlDu6=JbAGv(UW1qAE?8^yzasOem3rtX*>YuCZamAMbsp zQ)~&Bm-W)_0)NuV9t05RK@T>S%nDb8;HR{Q#~;< z#&@+8m`clmAu!%+Ay{6`wY#>%x|g4;F3Nq;Q&rQgLGEE^c^iWhS~1VaEq9D|(O1lK z^_W*zRo%C@$5bA2m~ExuKk)KG3^#=E_AxmNyqU5qD4y8ZpOm>5>hhk{c*e`OrvAI# z*sOSxs)^H9o&paQa+AMY$3~)T_B(+#@Av8yQig^a5zn8+JhTv9Ou%G( z&70HTWBv48h`LX1cvnTlrg0m|r?F9cF*6(zb(wHXpwIdVdgw^$Lho-?Y=OJI4|86! ze(xwt!L1MOoQ2`hoyQL?JgK}{UW{3hPHNltx!>aMFmc8cc7H`XSdGp{~W@aVcf2&F5*Tk zdxcCRb-&jm(={`BWDF??uJ2E;-9k38int}o2b6OavEy^L*ISCWG zi6aPN|E}dS^u|0Rld=YxrCx-nxEb>@K1E%D7;NXPF_)Ll)puO`=8UyY;p$@CZM=Mx zqN%19z`e^Rh%)o#xRHca6+;s%fo<-$c>@Ueqsp8dTbTtSer|bk5ftU@`Y7o^huDj`%Z%+2&j&PS{$tJ zM~wGT*|4c|Qb(jlQcwH2mwv^>jazD09S@%uz!Y}{)K$BCTeQL@pPjyif0~T};VQL* z3Mts(cRIH-Bglr`FbyhldrY{TAtM2KQZ}cM2}s zR#Y_v!+14ObVqMB8rEGautLhTL^a&9Vo%vu#E-$&&Ww?Eb~EGI;9foA*0Vnh!q^E=LEdOt1S7-xD2Se(>G6NT#3Jb#oDrlzDxj_#0tj;1g8 z9swe6qKmXsRi@i;IVjAcmPk{ z#>E#gcQN~iPRz9tv#-|&Cm5$|+*WHOU?p>{G|!*M8mK8A{<|uSw;}O+@e;R_hJ&b+ zUC3T0ml0aA(OgJ+?)UIb2|_(w=X9@g^Cn}P%7uBE-7I^v{hK4mzAisRD>NqV+E6Sq zYacu5ELoXXmCdcyB&lL|?8Oz8-%Ld)_gm*^pE~0cEKG6P%>6nn)cfcPiqG=0>e_EM z+&le_Y9gnw$oxOxN@qJKIFr^dl0FL$kMFx6mL<_ET}`pzyQc|t8rdi`u6A7h^Q{Go zz!j@s31+y={Gl>bdz;#Y89w5%mgwbgW4QcrANwOZn(6$VJN;2A%{^-*5IjRNljUlw zc9_hm*Oy2omI18@jywIi;sxC3rs)t`*F7##tK4WrOuzPalnG?qwWWnR!vTreAurS2 zLCqWXr^O+)p(t zs4c-i8CHM!&ZAImCUK2#c1(D-1@C$6VHumhBbl!_yC$4iaBOrmA}zfAgf%Iia4h_$ z;gg;+Zr_kts}ZO}Na3AEdcL1!6-QlqP6{s_V~ekHO7AzGg{r6@J`9p3y=wPW9U5P#nK#K=4TAYALtk1d>Jme6D^-t1yykcSN_V(v5sJiq@o?t&Zbb3fAU zTk(Xsy;baV*)Se~TxfFTUbtQ^17j-=3oa^~EgG7sdC*bJ3FNkHT$POF?JfAd8 zk0GQ8dUWxLVw*lW4!)C?rU&IV8Md~BSY%hTu;X;faGr6RfN-5$iZNlnR5|j%h7{S< z#5jITZulWB-Pz=DB!Gj;rZr-#i*@EvyxsJ2wfM~YEUs&@spa`|)l1wt6#`T82|-9u zB2CaVZi|<+kvoR_p^R*A;%=?FZgX5(-HQp8kOhuQoVkup#wRwoD6OCw1U)B+6t7?9 z`F}uV-oo2?=eXEgF!t6p@$w$|-vcETe2N(M-a{PE+u=k^i8xd!4#ry}jXXL$DW-SWGgNR30JE8|9-J zJSG9#mBFRzP)VN~syqF`YIQrcU)yPE zeV^bzuVc~m#8p+)SjSAV9-*14WQceD!aytF)~eWN*^3a z_{myzCay(2lT~n&m%kWL4bX}>6wLJ(%6SO#dZUH##`cCF}T z?H)Y***5h;j{Mo^89!!j_?Y}|`R|}tdy}o_i&z8bt zF)_(VD}`jfmSGa{QhWIKj2NE?`V^6lQD+-MCan$Wn?d>`+&i6ih)vle@Jy?Y4;Pv_ zuKUb)PeXb~98wUH4({K*l^UO*#G-Y*gXBBkYKr*SbjQTfLZvLk+OmIfIskC0Okk?d z?=l2hgjsR#{M=k|tZUv!g5|o1(FqOsM`-dCM@dkc&C=@$w|aFY&9c)y>|=e0@rz9J zOA>bn{OzkpDS-s<1jLJb>{66g87F4slFDe8%|!}-Cb_+T{~<5|^Ub|~$0c46U9YC@ zlo3$Nw6hs{#)pZFhz=#%E)lv#4VtfgY4Y8!{9z^j3ij62%rI3pbqg^od$B-oqy~Tu z*!F2Z17wn|5ll7cbj-*4qcGHk7r!{kPN$~pKh2peSXT)%OWqTA+OzZu zs%Y=!{}C)dzrQ!Ufluxm6R!l8%4KpLoq$T9ckn@jlZ>0)0=7BJ)PPN$AmTuG%zAhL zOO90kW;cWz6{EVt?^@A!pQw`LjiVy#2ZZ2!QDKegEkn#P71<#D_d45QH z%9+3FVJXLvg2>;+yz#aE@WWs(ZBu9X1<@D)(2avjAGl%B7cUX9a5ytX-H)33h-nLs z-FN;a^Bj*J`t|9FmCQPRg2EEMe1D)7WH6{~*l6~%tm@YsbnMdIS)P1$`Qq;VRHM8} zy3HFtuYm(635q%DH)Z1ABvpvcSOP5_h(>kCA*ppo0Z1YS+GD=Vl7l^|4pE(xm=Ezh z76f>6D9-;vj1gVT-CO!vmKN?fQ@Pp%GC(W!@{11kmw#`}7u2J%3p*Vp=@nvHg-VzK zO-|~J$v60VP-ww^+Hl+6*%#JWPf$#xc6cB`#NTK*ND#iyY;fDd-nxm2jZEb#Ow}{V zvwkQZAA5#redT={UN70Eya#$Q$F3rj%i}v{);^QVQ=GSnJ}q_cOHfwU`oHaCV1iZ z54iWJ%vi2@8?(a4h`HS9U$3x|AUPiYT@DnL?GOI_>#r1`W^_yo6F+s-nFXCbiXhem z`Qw3CF0~We>=~8?9cCCq=Y$dG*R*eu|M>ByLpa_Ut^6$%xkl7 za2r{>92u(z_1dKdQEUH@flq(C(0smf<-NT*p7;BY`2PR}i`sVW<_W2l$x{9G^fK*4 zjbAl3lJ+l_Xhk=*KZuD^1B(l?3_z?5XeW(eCB|+Sw6K=igz2%Cq>dX!b83~mjx;Yx zuiBg+S&XpXr?HcA6I%}P=u6UW_#W)?(ITBxYea0s7Ls<^mSjRXM}7ZJT>RM_ScZ8; z#2C03Z(Rn8+ZE>HLPsXL^s6ydU0;Q;DZ~xs3qY}ikU%YvQSSefeO-pfl^Hb@IED(=kMo`o82h8_T zE-bDk$=-4g>35P;;w_R@Y!sur_3^IJ1oq z_9rw-s;xnuNVlb*HKq2S4%`2U+DBQ8L`%yc^s0}en?(B z22@371nZb_jD*{Xnfhg?@X{kehcwMji#%LVQM}36-u;}!(j+daZYQfXjBP z{*wQXs_HusvRLcpx-fz__FZU|F-qhn{y!)3V@u`?2zfhD{tGS`2(G$3B85SR8QAWD ze`ua3|4u&=;QU4QQ+KF3wmyRra2_fmWa{w zPz#AAA7K=DI0gRg`qh4FU8hJysj3XPQGgpmB5)~qixpzw9MU`vi5qx}k8C~ur2KGs zfcP|+H^$JZBWlI&Nfkd`!hI9}FpS_|FX@mu`Fb-Sy9`oiyE-FL0&-eQC_2vH0@}Nl;0+5{NDWx_cR9z!17Tvcw0?m%i@WPVR4zEnS=`3Drf2txuWdbHzQ5ez zD^I_+^;(Q2Wv%kD06t<;XCp@#w2@rwl>Qd&h){Ch6&0rqc8l!ujZGKyAek<@p43hn zqvdA|+Ld|$ITm4Q=B2D-^s(j%j$Wr7H- z@<;D5eMimb(d((OvY2+!%2{l;eQ5(qkP9h{rF@F2G-?yGbTf3+c^ZO}t zQd&X5NYHmg_|5~lkbuyooTbue$G;Cqy8M3svR2LxzNS$(4{7sCR}Vw=na*gP($oH9 zfN21dr#^iU7_pINrqAbd*-T6Re7Y`~?I9}Ly@H~M9q{G_RcLEHsoh<#ag(a2Y@%Lj zky=|7WEyaph=7h&J0Xek$q3Ai$i}cC%|M}T#|vG&ufhFN!_6()H4-K6U#$b51&9gI zXnA#4jP2f8!H@b;-5^YVTkOQNzy-c(HZ5u9H%%z~ME{irKXD;)_41zhk1C1m2j7}Z zm?`f4bPAg$j~e?>ME%F+Q#I>#eEX6_19Ge0GUYZk^21u-8^ac(T-((5e$HM*ME6zX zNon2CdOw6CkjEL* z8G(2nP$eNAe$$us)P-if=FWEDe|I|xl?`+vqN22>@r8!go1MfGIFTmAxi`8yglW3{psO4Os4qg(zA zEq1h}yOKnm@a!5uj%I@nhu{!I`1i2Kiqq{X=mk!eA#0N1ifi%R3i)nOB>Q=-4UwoBP5cs?%$F@G-nms$u%-eb!MlfHAN zXRQ;wdKGT=cemlFza*);dV2M$jtzN7+njVfh^%HD%F@#{>!n!IDl02Xb9`JIUXrETDF!U;@%amy^nF zSieL$H^x%)=-#;^;M^}mp^=tca53+wGEJHESsy%`(4NQlzX4EKZd_up^8j_D9gwm` z{5|evmw#HQtd07?u3(R7!27%Gh-!j|DqZcwjgv1gD5bAoXsxCNLH_pf?Bb6?s)tu z>&`9IOn(PRM}exf(XaR$%`1}D@8Baa|Kxxo30ZBZtH%oo45$4EW5c={F16INH4Wb4JCFVL7gUl8Mh>xZmblK0Mp`n`ypMfq_M&8u zOMFVSXii0231;0b)p??%yB+1EZo`wTPPPa|Y%bB!~>FMb31=KWEtaZz# zAR;|Q=luX+#g5FT7@6v)L=9VBiq!#~K$0V71vE8f4#?%6LQkfiAT*|b02l!F+kR)w z;1jn^fzNwUc7+%{xcF_;9(O;rag@1bf#^~8+i6Yc12%O4A`OT_fp*MN8bfNt5A%?G;(v|jU-u{@z2IWy& zvt=|v5OJ}xRe5^tPxblzNeUfW`~TclAt1HAfs`+-mshczK(DI)0vNSh*cCKoz4s1K zFj+NX*qC;O)2_k6KU$`mA%oDsb8C-ykDBUo$RBW3I@^Lk%6x_wy>kp3^%y6vY@5oR zE5Uhv0Xll`&o)LLb`E#?Uv&b0399!isPcDlexS=u;BMH1pFMn-#Ho&{p zoqxHOum**=c@&L3S3ot%9nJzN|9qrt==$%23p{)dw>_AFNcuzcS;KrpwDKD4=Dq`{ zo~old&;!y@{KX%Dz_pv5&8kW>L^mte{c5ty@5)YFB3aLe0tG74Egw7_TCmW4fOnFB zt|!`skOiC%XfV=HAi4P~fZ}CwOpi1aF=b*Y9%9iIY7+}K?ibdVvo+nK;^SPQ8Gn7u z>9uyLsrI#Ia&j_a@IV=#b&( zgyaC-V)33__u~L_$~oGeH@bgD@%JP_@5FC=WNhvIHbds(S!|Tev|Z)c{DfsgK42ez zPgu>>eW+bIK$pb*FC}KHn5d#ZB??tfdkf{nCA|HHPxQVtUny9Ak8gT4L~by2R`zRy zozq;f3vOL!G)ysBe#TSl`Zo5)QHlLS!#dYcWQ})Wef>epxddn^nx{=v?mg#IP6z>g zspn?~ejy2zkPG<=wc$|?4T8tV0r}o_k}4hx7s@Mmi($(BBz#s2*8u)Tk1Wci>#DLN zPrl+)W^b1rzq(=K0BUgBTEAg zCkQPwGS$H%q0R@MNOGw*cV@lEf*IZ)UV3bR4dh_0^UZgw%N%O}B!#^XYvnAlc}^iO zAVGizl&{8qu3%_EIpW}9) zE~F8;8{Ibmz<@xhL4nNg-^KM35trBaJ=&tcQk-DCcEcZ&g(-8rNc0vE7={-22?rpGa*4BE?2KyWy9mimA^= zM$wD@^4)oPkNqUPAnd>a@!YEuJG(}Ojq!w3-;(_2bbb2qlze&hS=*Q8%SHaIu32Rf zUsB*P1Kkhd$dNg9N!Iw7BdBJ1X53g#jotuBfcSXpOJiaLuss180Bq5gkKrRiVm9kt zJ_ip2TN6a7p(&Bd<+tpvT6|XLN+!?I@Eq*)Nf7g6PO%V4#37qwAWTauTX#pCU4ia| z-A>El6&sDnWIkx8!68znKh1S-mn18!@#eJcxFb4H zR8!;f9^in3#!_$+)>=lrturz+3~IXsUZDlqpp5l~=l<6Kxxb+j z^sj+ywFcrW|7%f)WI(&Yyq3P<`(MbtkR9|MY9e8Wa1r6YPjX7;F`$1F zL)6`2F*n*dRLeCk0CL{ciFQXPeMA!N8u(bG>H%Tf)TwS4^Q*@K?wTq>`s?{}v{4st zD<4UKO{dSXdTZBt)Yn7Z@MkYK6eP}<9xNO~+Fo(TWvsXzk11I3&3$S9gNDcDBjQpL zfW;x%wSbnf0Mbt-qtz5tOKa*4ZsR%X0Msv#Af+xz%mG10|_n4*It zrDJ!WOb1q}Gd8&|cmd`j%}ZECqpbL$uTMFruKOyc-%a8#N+Khw%$Ldl0SA~QAicS0 zY+tWC)XJWyqTGhX-$cl)qGdFb_B82dGE7}fexTOA5y&1OU^qcSNneaO0NFT#MzS(! zWnb0AqwzNs)(0!*#q5ZeB>ybiGw5q4j}%$ zYU|XBQ+UmD#rKRK3@Y#he4aBtQeq=N0+A`sUUm@*1=?$ABD6IKGpLKhHQ9Kl;5i~V zVY~X+>WCJ#$Sb<^hbwqNOC17938UY04hlMZQ zZw0^{fPLoirMdG3GS68iLdzq?5wddIs7Uoi2Pd^miJ+#HwymxwfGsRP7N(!jgg0(` zT?rf6i%PU1Id}MXc?ZRz3$KPxrR9aX2lXug%izerY0d+-fCi6SvM}l0(Lh24lEZJw4;s;`Lg? z#KaqFo9u~=%0jQDpTWYw+cm(kza9E$bof>tOI>OjrR7v)Q^99%rl!XotbFwca5o|5 z?jSMMc{Zn1dI*7%a*?#^qoCI^+Mp^gq;+U>3A{Y7U$0H37%i)|_fCVR8yi{yIuMVS zC!h19yHFtn|JBAhhUaqKRvt_%EBFDVTETx`){x8>FF+JHukcH_`pW=ahoPhe0BY0) zWJ|zZxt@Jysz2PGFmN!RH~c~M#CKGL4Tkk2Ev9D%{tuU<(@*ECMd zHgk5-b%%OCTt+?m{q_VP7x_^agLImW+TggwX`84dC7&UX4J{ki2U}Rmj?a#`lPdk0 z&hJtKY;g`K$m~d zi_f(_1Cc$b?L`0jXPM9W1)r5&GpVAkVukTgfP6j!pSD@D(vHon$k3-fSO0~`Bq{6! zwnm_a9Vh^F-DQ}4!h2hfC>!7BA7rEmyP-6APtid=04e%LG4y&)yOGEh`0V-`@MlIz zl6NGc&96@`Q-yiF`mt{GRUNm-B4S!IPG6zwDB zWcuv1Yjd=0+W3{z`r8fRF_8hC@gF?Vxqymbi?)pVEAfjOt?Jhuz$ON!Q%q5o=ud-# ztDMD;)CKJR5R1k-{#i_GF*+R85g75vmtKxpb4z^}$u%xUm$0G9d0Ss8i_TI^Uy@sE z3ooI!K#u^DJm}6OLE4<083)R(u zE%(pJE?zSoyl2oVw0?1B066#KtOG+TP^02_+40`7z`%K~{wB1+Z>q88HetRY4r03*ggP=C!5;v&o!0~Mq4k^&PowVfNo(5zhol?wz^e1i%bM+Wk zr+mWq;64$5XFRWrKx9n5sb*L*#=1?9&aa?EeEi~bof?8$^HW%yqMDk9Pv@fg;YMH| zegxRGIT{N@3eYYukb;6YG6!?3_%Wglm!7+ddteVT4ayCW&q1Tt>26J#!>~3u6~=oR zGi{QjeKOpqZ@BwK!41Xhc(gn}2IhLdq+C z?^KP541Lk&d!_A0)0k>_SnL2m$avscXO>dCqtge``c$z)WqZFCzL@jltptcPO2f*;-w+}p=I=C?ChH@9J# z=S(d31}t{wz26mYYFEj+b?%jbJ~VKbxpsh95aep094E%AGr$H=1iorRXT@oQ$8@_! z#R7u&Boy=zrwrQleDEKjA@@2`^2)8a6%`TwBi-b=vD%)mf#GZEAQoQa_AwA@Kg z0tOJC0s>p|fSoimyT$3|uGh(D@@XV!Yb&BVnR)BnpqWV4#m8Q?e7|MUrKWOWqtQDR zAw1XKES5gGHZJOL$=FlmCd@J3d??$^)UwaL-d#^!SC_|nQmh3Wk$_y5Mn=%jlAy@{ z!&n!gWM8j%zK>d-u!**3k}g+};h`qZQdU=pJ*6T*uh)HzT?bOAL6BGz+h$Dy)~ z=zglu3h-mbM5`w^T!{kPKxcFCiust%nCDJ*Phm`an+!&z;_g!*4Y94BuptiVxgJjAD(N@qOccx&*?IzX)S#eGLy@aVW z|$)0~m)bL#i2ZQuwxCAmlX&VA_9&I)jxgYYShwqq*;rUy-K%aF^s zNhOa<0Jp5^un*<0(B^83xih)jU@w(9)s(tNU-ovE5n5Q&J;l1 z?T#beVW)89qXI~~58~9lNjA2!i-9pk&b;N>Dxew6KJXLyKKZvfGDHMDh-G~LZ(h}o zOY0kQv{;3sz<=DV&72l65{6eaZCjXIE$b=K55Sr2of^CbWEh*qTu9MXZbkaZp41^J z{{yq+^%Ufj`{rPDLHT9RxXBLC?%Fo1>)l@-w#S;L3JHH|lPw>Co)X6gJE0MsWx^^#!^P+GTyDW+u7fG$r&J=)OUeliLcM_&5A+F0O!!nHr(Sq& zu^zA(G|1Sr%|4{4s(*RO6iFTj^d+u;czmr$h(#iHipK=AF5B>N!dQV~wF6dgz-y4AlcUzZ>G*Vf#}0iMY) z#SIgh33Ks+iZ7M+KIH^$^f<%~1W)i1V)dqb9Iku_LYX_d=sXF}oHutcfmsE?9z5Z$ zadU4(Q*o-xPV3V_EZ$YzxPlOhT!1-=z=2K^Xh?4za_KS(JNe?HM8Ie`7>@#cnpTIk z;|{6RTA|e)3MCdWo;@QKRVN3}fhdsjz4vbA<8jSYJ$a#mg&-jSs|2zB!DGfOYqOtU z76BCuAaXC8-jKzfWo4~~z^lP9P@%^x`NP|ValU{;<)`h&-;%STQ5gMh)|83+t`%rK zcF*r%W(shZLD~iEFGvV3q^&XFVi-&J<29z0CwH! za(u;iBn1z2?4)YInQw%&gW(A4IS~bDJVT5yX(T{vU^VQk9!HHJKvk6n9h_fQv)FFE zqhT=}RBXJS7gJSr5Y1{N+fmMs9^wG}(^|T5scY+>mMGb9$y#OGjug_Dk*4~M@kQr% zGTluf9@h>bSg^S2HZ(TiuDSKPYQ?<&5E-p}wk3x6=@r{)8&zZ7avJCelaO_WISKr@ z*#Sd$^MoXNwm@G?dCU>vPyA0vfVYLF47E-jN(6{j?0$MGp`i7u1~n>nBW!}UPhjyd zi=>~kF)%P6n8O{FIb$r@9d6C(CYjq!%a>lsu=9v$J<8=U;mJE_`sSJ^SwsqeOMefd z16|=hNEq$rjBbw)4NwW!hAA~VF8vJ|pH}6IP-|VP-^6sZsN$$A)(ErS!s02+w z`9Bi~{m{5Q3>pRA&X!d#N%}(KDw-@q#bh8wVic3-7+UKNVDt?g5He7^Bl2@o^<^=7 zqLsAf2#q7DIk^JcKKdBjh5SKg~StA$$h{<4w6x+w^o#LKM-v!Kh^C|tIh#l zZIYk$j2HO-XMH4j91nIxb;jiLdt5$tOd!@4qZ9OG-FBCOlzqWgIRDFQZuur-bx;7; z9VBVM?d9?H9%4#^YnItHPn6CYtoF|5xejARmDU;B0swJNK{jtj)Xj==mu7Rbgi*Jq4%ns*wkK`()(hF9eQ$uB1;H7!D>68`B58f=1VSn2VfGjl!HeyWN;Hn zn#dBK50HLCfyg7w6Qdx@(3SU`*!d+o2=5I*#jj%oarzP(D$E`}Fy7(y@EP$h0%n6- zlFeq&rn{&2blf}JKidzXC+!ga6|Ud81JooMHBL0s|CD&;+?~sB=dB7UjZRt~)pMr2 z7kZ1Ay0@avhDj~3S>Db%QRV?OT^$g-85r2jVJ*--`jyFEHGJ%iZq>1h&_YUmeprLh z&2Qi`ycI;FrgQ<-`PzmCc6L$T5qhxh&{jCHIi7Sgy+_jzAQBK!)ph>_Bx9YA06U-c z3@$XW$}cg$hp+d%>e)q{>&sgC^RfGn1(ju}^07lNK(eV}Z7p}{T+g$rk2tu;=qF~R zb*ioM(z125Ra9Qc0&JDgpWQQ`qwY>Xk%Qr~m+VfcS^@Qm!ZV9RxL$fgNX-X=nEya6 zMBeas^AaW?HbaxV-rQ1`X^B}Eav zzsnABlBy#UA=HK8UFmO!^H=sMGoFI6prIGm@U=s9d;5^&%C+UqQ?|Y_5fnMr!4RJ- zK>XgYnEQ|^d5MotS6^zo>#3c2A$IVPbl~KYnF_J}fNg?F8z!)$yg{nXwl%W(;#Q&e zg5DsRI$h7i<*mbAJy7NW4F7vkgO6$hWx7b;;n;nsue(HX=)7(gby7-VcNypfev|!` z0bo_L)nE@r_Maa22gpbGlfpr1^nm<1h^v0Ke{%KYv)~%(v$uwT+t3qr15EVsK4#X* z{;Z8Vek9kY2v)7~*_)!1*Y3b7n%Hpl?VyBAQdP-*1q>)k$Z7mG#h6jka5=xAyd7b{ z&cHx3?TU_3Bj$Bz93yiV$QT2>WX_nFS>dz&dn3a?fWjA&2=zT+ub?gf4Os0wd&yep zJE~qdy>8{s{hD2w8!)o0 z>4Ql4%_2Afra7!f?FdbrY*!Mo211`B80UpMZ#_`?lxGmQ|v zc9Y^(KMotr3opPJkia_aXTcZo~=mPt-=>$n4yJ z(TF#-RE~`U4!|wHe-~}P@^(|N!NiLeubUFDQAKn;pmArU=rp1PI;dQD6iQ@XLt=)- z&qLIJv2mYACWYTe3w}BCG%Y>CW&dIb%x_G%Crfyj&|P!`KY&q13ECbQD7YODg5UX^ zO2f_Gec6%({88I4h+D&xtCs5-NuLh35{aXN@@r=}9uG-2__X;)O<1MsQTtkr36sFW z^=vf|;q5n*F?$#thSj9nKb~BSpMmr3_%=CN0*p%-!O8R!wp|7KA@;QRC2`e;{3(OO z(D-O|l^M6QLEGN%VCu!vPr_$J;mfV*a7l(FX&*QmE&@c;!En+he}=^7OXrR|IrIH5 zAh8g$y>*B0OG?ac*iS2&D1j;q=9mbc#vbq<`g9_JxmtS8-U^6z5k7a}BNlsGlsP-g z_)%Bk-18_<>{~Vp%~UOn$r7jo$Go3egVo=h#xo3RY_XeG0;cDg_bMQFQS``;+%Aj} z!*?x1Hg87)AnSxA$BOlQa^e_H3TRDi-=`#R7TEC5<1d0sZ$ox3@IvC*^H^qGJKxrv zqM3Vfbv9fVZ^?Cz{PAAJV!wlty*f(@DW|Q;liFeHk};@0PP)guj-~WikTnhPl-HF* zh(*EGm$y!8k5Jv~n`b&^l&70HpKZVhR~APXQn%A0ZtI<`9~P%K_cyt|4xo#sg5lIo z0bsI7H?ICTar3PuSkIaZCFei0U)Xo*G@BWpG>b||jUq7t8648EBcld$Ezl%HGxhbs zUkn?26Y~zotQd&j!1EIwtscmk_dpb&eS8fbODizWcuotZAfH!oZoi`~2*Ql~Y!2o? zZ!)&1h~(XzR5>*pw;ewn;A?oj+2aKTJ)z3Z*f}A7vmtErYtiA)o-MS8tY9)p_7TuA znm*C?I;OF5M=STwZhD0TGq+SF@EcMB68V(akZ$53Bxq{^*l$577UTno(HspRw}Yz; zRG+ExiaQ+Zxv~Q;hi-(L=Q2eY^JM=J;Dm6S0Cm=La1bk?mhU9ljr>sqBrF zSKvn$^fo0r=<+6jhwDvM{m0!jhhAOxtMaAVi=GC@?HJ_)!eBW%Flh^%d|tP@Qlz1n z5p|#4I!UFPk2SS`h`~2FfKYUK8gZ9bd_X8Rbxj23(_Zu7qSmzGzOA%fi^;#@WO+t9 z6rxR<>(ycb0PCcJUyzhkUovpB;*W|80(uRMbO-%BXDB?c@#DJF1o2J15A4^sE`pmf zHg(|X1^Q1%H*@yhz)O^*f0@QuQI(w;A-7fES~u=a6y%tE#5e}*^c1tnc)(AX`uY8b zr$?{dpMAx&1LLU&JX_5QjDymI9;XBJD>O>sDaOCoYC=lPntM$;-qL`jEA!Bd`h72aCGFAk3j`~_TmN%Js$%Sl#dNg9SR0tK-}N$ zK4JGF#{s^a9fSOZMC=HkXUn&)qoK`Digxkfi0-aORu}IZZ$AF@<_MTLCzpHn_7({v1bSBVF$D0mtsr{OeHh38@|)CiF-i7P|;yTUHeaV8zO{l z7<{Zq?xlMkF;e<*v-1$QRcp{5gr%npS+3hLk1^o9I?*FmZoLn0-TnZw45|I&YahOD z?ZTYNw-5xWu)_!=Z2wdn4EM0SOQ&Zn&|bGId|6}$F{~)SrC#FChO#(>kr;;Z`4F({ zn$U@`US5PiDQQ0{M`Rd(MS(Du?bGpO7reS^0CRm)^KLZEDe<#3HMsm*-D^cLmO*MDmNW=F3Z80hRa zq^4)Zlj0m3b~ey|%vcfz8l^|d@Fh|{uN16Es!%CNfrx@vdzLO>vlh3{F$f;Fw%t-8 z@(aYjNMk?g4O!{pchN&GHkg7a=6pyv>|lmd>xH8aCu_3{K>vQtCE>eEPP=q&;r+FZ zyEH;5GU$7^`R?sZN6atcJN=gA_d+kfXFT%`EDeeHSw010);M3(l6}i^rEllYM~}(-$QSz~BY6VRq~i@Bxa0?q;@_G>H^Bhv_ly6 z(*}GcV9INu&KzJZ2mFgxN4Ue{Rr9P!kty$d=Sfd;RdmsDlKCco|EUpdQ`ZffvJ-FM zwtg_KT0ICoJ8KxDlO$b9?kfOzWCAibvlotFjZJ=k9EIci-=KCk%HV_UUdCx~IVT@& z@Kl7{PM&2I;M_W`eaB$NWRJQf(8a{S0vkxJOowhvN7I7w zWNM@YB#6zt|AT*QiGiUcL7;_GB`BDL>z`z8D$pCc3%j$Dt(uu@{_Hll`5yCl z%41DjEV7ZD#_E1*JRs3n-6w?~(r%j*l?4BkGTdZRZiILq4Qq6dKQu??BOSNF^uN*| zl*kJb9hGRdTpb-(b8wxT zQY_Ch&u{uDJG}HfgVxpHPp=%VID%wUv4A;YAs^_IXNC*)SBpBAB}as8S_jDV8p$AF z!N1!Wpuh77E%n|%o{8maJN`GZmcIDi3AOmoZ#@B@pR5~e8Yd3A@KnW$ii(=^RvZ}@ zWiIv2dt%~3hyx04nX>9kPuSEf&TGrUy*-;-uklhfBQ@jZRAomp7%@sXpNe<=ngeDY zTk9F6SQ?0>St&gZfH|%C|D)T5D!gbFtdyD6E>hf^(d*iVnqO;mwo#Vg$_B zsVj1^O6pvgv%_9l9rZ|pC)sZ6SIel-ow)_5lBn(<_gBb8x67{&ouxcv#V=FQpM5!A zKWO}V4Y=9cA1|GPkqtp0J(mzsDeLL)g@4n}-yG~v#eXI zZK037>UR!~hk~Es31>;QmL|U9%36w=p6j_qiu|nGMYribjX|gXqs`|LopN57R|CMw zE4rPPUeFfIqA)D7;1#-{rm0%Iu^E(fiBSDU+-}hV<78xL>_KhWLOc$N3BKjHZPZt_ zz_zn3(C830p?E3Yf9j#s=bzGe$&;}UD_~wg{+J>F?RsnFnm`$_Mu7kU$fZ#pY9*bL z<`ubS;@(v_a;E|y!;h{B1pRzh5I-&| z(Fz8?(s$Y{cj%aC`CWc~H{`9jpzX29O7D;l{khw6*ZBFl?@%t?BrvNn>P<=@l~;Ah zwOtOZK=vf&(z+Y9p2{J0R^GO^DbnWl^5YiP8+IV*7^)o_lfUY^8ngR+$l8dEQh8&| zgic47+pd#z!)V#-6{TvN*I07Z930P4>4;}+YT=+r^iax>`RzWM#y)Jf)jBsORU zL^SQQh|=W0J&EC-a%A@1IC!f9E;(woB7Cn2h(KSk*xY9w_A+t8TB*B4L{p)``2Y%g zzkbkfKwEWm_+h|16<+X3>mxaA(4pLYdr)&)OqoMZZ<^*o@J5eX>u6{EO2KS;&uven z?K7C_doSwHs%l=i>f~pe92L^S4%G(9F7dk-&aIpMxG(m-!Jr~QswtYnNJP|#giy(0 z&64H^TU_2VviBh$NBnj*`x5pt*| z&?^IBcI6}142Fp=00+hg1Nbor|+P{<37115x#DS3N&9A2@kuO#3@a>k%5!wYktC4d@tOW zNO2;jgjJ1H+~1u-I%rKm$kGHjb-2JHM3L)p{8-=bp&Pm_qFuJ`TpaovO<%ppL-LmT zoo%9c4WB5hD?7~%@HW>`g?)}lx}UfKoqiiU4iFCBRHPC@*?1^7`1l+OsWY)35+oV_ zG=dnI2lq(mnZ;6`OSRnr$4g7>%N{JcJyu?aZ6^GwkaD1USB}P(FVty%e5F2r#(4gX z3)VVR4;mL?IK_a@Zuu&AvY!0?x~f$g`n<Z7*6-nRQBh(k$K`>i zmZJ!L9=C=8D@and>lg34eY}==+)~C>&TttK?t7L(&$oN^B=To6kp`Q&0`EfmGw^|XVv0)3x=?a$JXh z-MlFtl~G1n%WeBB`SI`+h^{ll*i`uqWCM3#L_t&$&v@wZ2mKl8VJNljp5a;}#<_H* zC5(T)#;QYeBV}Uf4m1Q!y}G+SMSW`!hR?*O)#oR&WKcIBbVyY;m^Fd zAd6kLEt96BEr^+&llYmpG=<k-r~of~qPNbGZzN zRkTD}oBI_Rm(&OtFzwUv(((y_r&5K!Pz&;|LHm&TtrDye9Y10uPY9I%go6CI)&s17 zdmkRP-<6UJeHmUHB&J{+=;2I^%qD;nYHJFC5BlINYNtkFoKs%)_W+Dmx)3r&L{jLz zCL6|2r~;i3!X}UEct`Tva(-1EgemeX#Z6U!{Z;p;Ekk`uI@%>3h+kK(l~Jq577L4=pt^pgho8nS@%wG7owkj#8#fA``+@%!>_M%%B@|xFuDLn~ zu^>Xz)0+Ow@T~Gn$eQD3eLiIOZSjHZfaBYXh@n?`Hut=ZAS{~VTsPOP_2hRP`s6f` zbB_Jq5Yen|b98?AI;tiYNkB^QPuu7<)UVUMBMM3ceUH&oU_{b6Ij5GHCzNO2$%H+WDeP=cfTWLp=Ps|Pt7h&?+BcnLdVCGFe+lsun z)VW|GIY(9Y7q_rm2ujC^^slB6!hI;#45Za|6*ofo+3!a76L`R+Go{vD;Zw%o-)1L~6PDc<_~nGVI&8Z%nrz{)qi*Yz>{V zIfR`>$Y(|4r}{9*3im{@Hcalr-odU&UC&o7Cl41S6VE?5t)@CM<ggLcMtTKPoA{oc|y zClOSvwm0mb-2dKvrpBGucXF68&=gw;71m7rq&CcEShDYn26>#qSuHSnB~BQeYtI(~ zsBwpC+|P7dEmUAILs(EomAGEB%a2KrT|&i0J<*X&U|i%saD`-9GShgj+WOI`pl=^Z zOu{RtdIcw-p)Es`@L@mofO(7nvn*EfeEqJ zV&W!=-o3iz-giI(w3;gMUaMXd==j(c%*ZZ7g3J~OT^}&bN@~3jzX)b+7%$?s_jMJ2 z3UFSNord^J<26F70rC`=&916!lJwafq)mh0i)b$h{O;0)!A8wRn)q?q*_EqT&7&Cf zza(hbk2|($DC61-G$+_=$fLScJDWmtQ;|Lax*tp z>@Rj#OQcjB<(!8H2dApf(?u44p5Dx)Sm<)cFC1saUwQh0`l<<%CkhMgt!VLA70H=Z zH9Z7Mjib*O(h_0p&^Qw^qoS_v(FrNek(hr{>7mDABek{snMOr~BwbViuXfQ}>aqS_+_sXQq{W+~BMF=c2wW0GLIyjjVu{$5%UfbGVFMP^Gd%jHijw#h6LD8ih`f`9 zbCjtvj6(I8x>j(G$abo!?)s5jt9sd7z)iu;kz;#>Xc$SgxwrG#TvHkNG1NUW?brD| zC(2uIUw5|13ka>6jcH$TaLg4DC=s7tmBFp6q$H@_(~?^@!{p_fpVM2pd0;asFb@bhzdnS|z;5Pxbiyo1f)cJMitB z{o>C*(f_=;?_oMFw@m!7+~L;-7d;DI(imw9UYmvM;!E^0+QK#S@9qL487J4#qWD*m z^otF5=Nt#PRl_{jStn~`2HJn1Aa~s{6uA6eTlATVeqTD{=N|l(8L1~SN~*dpn}>r= zye%L1J|#5eW>|0T%j3=<3Eca4-RA5>43$rllUQ^1o&=2^-F@T~J7(2&2^r&yoG_J$ zNOOkakYuLBDNf8n2CbV$NfmeS9-(Q3|ihp5A)7?}WbEhD_QnCX?@4 z!q8NTEqM-S2F19K(oj(1xY7*5U3ug(Y%V6bgT|dnD(^9U7eAWxLO)Y|^5tbz{?LZ0JkVmRSyE1$U;y3Vk{Yuq zbA~R-P*MY_EdFuh#~$aZCmXe>-Lu9I%Pr3i#a@T0vyo!sC3=^C{M-cj)~5lS56H+niX%SUEEUbQ$@%J3W~SJ@Ov6Kp7y`bcMvc+J_xTYbp( zh}{QMS}>#)kYK5?RK^_l$! zDjQP$)mYrfh$zePn`Zq+*cWV>T(am!Z zEk$S@=$0{TX)7B;)EZybwuy;e^ zQay>E9V#55K~*;@e8_2u7vJ|0Qm@;+l$ZCsz43A3uI2dzEAXflugsA;E%Q};C%O#R zRZk3YT@7Uts+Kv;C7GG^0!@{QJj9r!_FRW#@aB{?Dkxu%9`RTGTIV^fkfvHJCtgCN zp_k;*vFhAgScDi4&42AJScdH%kiGUcx*93z$o@D@M^3WhtNnf=K|!FsdVI*|o{sDn z))&nVb(aoB+u&9#;oS@^RveE<>ec)A^H{28jth*9oF-3GGs&VDFXvA<2z8jhxtD@0 zk8W2r^E2G!qz?I%HFn{4+*?;JIgf1y(GE3|hWp{|cRdz1u}~>Z#|&=dev*G9oq1^9 zSbQw=_{jgA-|bi)?Yi?PCstQc;>gk!kHQA8ERW0fL-c8@RB6xoF)q4tH&XFOu@a}I z<6Vih))c)!6)8Qta}cd&_EEj-P=4Qw-Zw!~%ZnU7`~z(1RtCY>SUJp&Z2B2e;Ar2t zUCzeHTahLdkWC$_`1rFqri4aw_t|1WHiOMkkUK-u&D#+XX&3g^L^MU&`D1N9UbrT2 zUgnr=#zY>W>05n3MjNjtmLO5hDfn9ac*B`*Ve zK2m}sd0f=|RvcRC+YBYF*7hs!b-3(*GNncRvoypy*RMoMig?Q77+a+|eg4So*SW$F ze}ZbsmoueDswAZA`LL}2oCrgkV}qMKn!YmVGOoO`_UM$okt4d;`O6Pvm&BN0p5RXo z%|-~>ui&b9Y~GI@*c5x~H}J_;r(~;AV6v7r@>^T>i*26*gN{BQ$uhYfy2t?Q?GtJ1#hd}j7Uq`T&QzVAhlaYXYgqD}V6I5pAAeA`<9bAeOe!JY z0RwqqyEd`zU@ZT9#Kp>^*FlYi%0^E?VH85HQi7aeT;#%<{88$F9@YNiw{P@vz7tks z_BRqcZ0l~a@@$CYVNzrn(no664j>_wBK`SbVY=`sm(5=Xo@_&=>})t?%Y4UQ=ly># zHe1?0o7rM?QfLS7HE+#Oak}@fwK*++HIsV9DM9Kwe9aEh<@h;L8H@9>)%mp2EjZsZ zx7;VBnZ5Mfq}QnMyVm^cU!3W7=qU&YunKCp%qIy)$NlKn5@;z5_nOOlA;{GQ1XfkY zzIxei#=h?ofq}_FZ(ZvMIdu^MUNl5yq^EcO90@Uxi)FEsYBh;vF44gCyB@J>WR)HAW`$3Z^pEfx)2F<55jh#m0b?u3J8#@JtgR` z3euVoUDt9-9^77`ziM(!M*h)ajcnUSYG-1zAM-WZc;*TZ3_GCinLIrUMlz%C@M0{p z&?YDxPYw$Zx`VWY_wT1c&Oc}fRAuxyx>UF`V7|)rygTZZ(3<6Il;59+VK>3|;*^fi z=3F6#t0hWS_dF2o5#BJSuL$MD9h9R+Sz!jP!y4BhlwM`>$Zx|Ik9Avz61@%`NDQP} zed!M(O)6f)nV+Y@{kF*yT7M8PP%=YdIp($d=|PsJ;q{M(d&n!QY_f0a63cm8Lc|#C zq?&ork!q2DCmLjkd8B-A*L%(Hcztn9A{t_XyVPHreU&T)4ezQ+fgqI#L>88^5B60v zoQg%oi++|_&HC3Wq^6^*E)E^T-wPiZ$O_-7F*Y__c*Y{fWz01Gp~QmTv~PrV#eOHC z)aAOMK>s_%oOdq3H$Qc>q|=LafB!rXSgo$F>OCO)$7 zud_z7XYMfPbRe%_wCno5AjOR2z3}fx!tH;7snbMiLnO2i1vzOP-~7f^6G1c-%?@Q{ zNv!0`q0Xc}tgCm30bnbz`#+r z|G=KOYZ%&dAm`m0_Zax5*U-9@@KdV{uIH(aWuXolznDk{+f!E?40EF4?-k1&zZzya zS*rHy61=@L!#{0*Lsuhl!jWMqPuY@buQwPSFh7~JZ~n@}l_5`~S1WOw&5SE?Z@&H} zxxN3nZpqh;C)aoUFMB8b0gMkqJMf0BcxSO5VVDkdZ-}=!{h;!QIzc^_8CYc|gf@K(1@6aW9F5e&G#4F10wI$nTjzp+`zFk{J+;=F{Q9by@ zoHJRXOuhOFN}N#JH5>bEy7@r;V;+e2e;yBp5Y$_Z=iFMPkI-Li^C zLX!Tod{13YR~Zu{mpfN$2nt7gEDKuzsVUz}aYTt7Um(C`*6{e~iGy`6n95;@uUVfn zoECY~L^%>Ki(zQEz=Md*#zZqLHo7^eazqx`p45Gr>_LN?65-(0O_Q0wjx2)*en8HR zy3Kt(#fnfK-$iQvdJP`y2^^};W8!25x=3cddQuiAmD&~t-fZQ0xnM#-(z)5Vy*(I6 zXP16Qn}+TCbafJ~sizNzUCcNghQTTC!22$x(Qkn!-A_zQ6kXjwGkMC|!8C&q&z?1$PD4ndq;PYPq;FS}G2W z`1lkQq=7%U;T0v>!^TEyGOtcY5(9LxU^4WwecpS&w?*!mf<6+TETF|4S3;9-DUyXkyw3WLjW;uFe28&?6*m8xS=FGuL z_hy9@5|ZejNb?cqlXdEp`|~ew2ZvVqw3$Z@m2bWLz1de|GYA4HJA5#QEt^I}clP34 z8P)3ucB=I#W}11pSNI6y-pVFby?yR|@N zw}cs16M<66%awH>g68b{2J!`+?U0A`v2OAY+3c4_z53v!mT|m9LCPlhb1kG4Y)v=! z`w=_amcNa4K&YuD{5$`y;AbfAt$X}RQJ^1*%MN!*)S|(2HFGzyFQoL> zWFt++tt-)l=er3Kiyb#0B=4XHRCNu)hH}5we)!4$deKC{ZGKB1nRr15-SN6mt27C$ zq#D2cIpG$lch1%^Us9>Q!<)IM)@-Z9`J|O975fSj!ymx?6sYcrnw=YnE#1bRYIW3n zUye^K^bcO)&QhfNe2e7S6sNbBgUS2U*zD;3hzi^G@pJEu1ye(Y#OF9|>AW0FBIb_C zUy4`_J0)@G0!#M~5;D5vDB*7Trsr?}=x|;sf4zGUr}8vSn`>U4F$jLqPV!*fq!Yp`dWKucWWq`H`kwfg`ru$Jxa&;$e?fsYK&cnq_9jz zsjL6Q=9N1vv~=+K)jaPCG-D~!8+O!Euv>d}Bn0{^#d}A-(o$E1jA;uz+=<>O_~G4B zFj>8y91)&ZD}{<$8U>dU>XDTgEVp275;ilDa7^JP?Y9@nrsL0+V$q%3?}UsAZ`Rjb zgz%XR*ey&lg>WeYqF1we%*b3c=X4E8fb$QA!H+|8aBlsAB|!)|?eaf4E$*^1orCBAySEq~VT{D$STl(A7xz)l4EvB#EY{YlN(#sut(}KY`=0Kbnsq^KZ zz%+y@nIWmD3bg;pro`@k?c!-mzR7o&7Z~OQ+{`9Z;esEcs3|fHLRUHEX(FCQuRE{t zx5s=7W~{quC%8#k@k_^@?maV-#lNAC7C+kRhluI5_i!$XZE64e%3E`G%|QrQgq~tA zu{yk6ED+LB3Yg9Asp%3RO* zP0q5N>KF4j*UgbWqWuQ=uSB%(|ERin?9060Zm%27PhIB{-)32C^gBw^rsiU|X5LLj z6s?U3o*L+GZW#FlJ05-a+a#45TF#Wf< zBoUI&6(xtVB?K)7d)Xe~{U{}In48;UkzVr*na#59qfPPJ;6w&41eh*1>xD-C2<~7J zR?@3WJ%solUDq*nZJwV&d6TGRS#yPyj5<9c8BFBC4u%Rs>!x@X6sFDO7!{7U|H{-uI>Xt&F{uHSSG++WkQIHZ*~XsxS$O(PtSC~F60q&coyVR* z-IpdNo_&%ooaP{qmv*oSL(pBqIXeT^GqFk6kk9xb5@uViw@JS1ypt75Ao(n$?t?NQMJs0~wxA%5NYCBH_RM4QN01GNkbAleir`!>wqJL;fcL zW@13Gzo28>o{{AB(L`2;xZnj|0VI6^vV_0@`C3{$GC)+>6=x2u< z)Eng*^mxenr1F$-kJIx_<$-A%5_K;?}i5$`EJplK3<^V|}x`bbM%+N|#rqNiqhAw?VEdyeS zo!5U%I|vy$=v4mPCV6XBgn`MKUou14Y5CV|(t9Oocs6D+5PqSz$iLfw-KzrC8PtWP zWZ~@_eC~sgea{=x9>Zc4DIbv5tJ287nf;uiD)9M4P=NI^ssmI_Z3kIEixjwhg+i<< z>*gV*Bqbh*(WZURt|mG^lm!y!>CHb$2fYiW8uBX^N&U}qb=Kd1NjX(9=hG*2QCZE7 zY=N9-Yb>TPcVf4cMs^k(WhSt)?+W(Ss}}8AHi}dm8pNNr>UTQHFS`P%>mcLHLLpB`8Bm0^7inss$X_9o7BPeAi?xGy|zfA>P zyvlvV?{>3ejbz$(SNTyQO7#vYPR0x0FSy@}B^J6&+-;v*EhdJd!NI`;dJ~dmQ>|R{ z_M%>TcBR8g(nq8HMyQ{F8kPHdoxz6U&{4>WNbXQQ>zv31xI{Po?=H~_<|uoJci(yV ztjl_+GcWuX{`=X?LOF?!1*co5dQ93LPqMtN2A|`ivT4B`6kCkO!S8avmvVEoFY{!n zgEJ{YO+YO7<=nvP_weCC^VJ4YV^_%BNP+(bplrNe(FIJXg=ca40Ob8SyOhe3&*Pnw zic^YEd*zzGC>u_8@|bkA-x|-YYxkkHUc&9g(8TidHVTY)CST#)njw`;%z8G~5c}h3 ze8CLrvN?XGiqMmz?TMb(_*UYl#rqY|^p(dGt-guZX(!|E&hx3eJsm2Q&xf z#T?DCmMg!(GZ%noJ~B%nf=)j)I<)zkfB8t+o zAHn!yWpwu!_Z+U?Sz~1Ph4YvQO2g}fS{+5j7GB3MMWHbXTyzhznF@R#{e2QT#E@mo zuf4Cc>fb>xO}_*Z39tkcX^J@k&0#@f!d>x4o57eXInQT_N{%O?A{w>+i@cAi4aM&yKbkg5$~Tmgy=|9nj!JAAA|cI z74iW&M)AeceCA#MZLuUjIE-KJ|A9kM5d*6?l|9kqj3Q+(Eqt5P9KCh@_coA8X3&v6 zH2AWa;cA+kr*E-k+f7QG{B`Z|Aw|k^gdZH^MP~k{`+|N;KOe+)M-vLS6w>wTa{-8+ z3p}cfuQ2KJ?Z9eh2zKj3Vh3!4B+vZspIjQDIPpaO2VN@&NM~36gN-4Emln$BC<9eQ zT;V@twba7TbQ}?fp&eDo^~RJxd9i553}{L*=|zYgmJ>Lc@-&F`*OyyS!+lx8Nf0RY z8)LT-^jDsRbbanNT*RGe#O(V2e>y9)*um<)Uh5F=)6vWS6}Q$51 z$)gw(MvSAeAu;cx&4>^&H%0f~D^Wn+XEc*e?M7Po=ydfuDZemYMfcc zC_bY*#n}E+Ux&Y6Q}@`nR_wC_PJbSRNMS2qzQ(Ah-?`}#ai?(3f$z)exL2xlU>|6U zKhxhAoO}uG(5>@i4mZsSNqh?D?0@x)?^~hN8pjPZuP>%=pyv3DsXJY+4sh@vj#Df{ z5id|mt$_bHzRa6JB2jd`gkXaXU#hZ^>}i|HyV#od{Hko}!5ssg`6x9Hp1uMFB!0|A zi6$k8F;OnWCa*`C>2Sxp#jE zXr2jtUjAeHkb3_iG?RmyIjtUBj(Mou2^GvqdwN%k2>s2V^{UJAF9W;g+24OYS=fqu{|p7{K@D}~S5mR~P+$JH0upv+ z`)v~D{}4vbBqW}kP|^PT1%yDU(s4I*=+QKDp5KK+*m%mN=mS4ZBu&%N%}B{IvqpvM z1cj<6yYpR<#B!VE-kxog&%ccqz%}2-O6%@VZ|seC))OyNKSn|!%DG-OQM%0Mx_G`* zNB#kO{Oq6hjpZ!>T%?)`e4c>Lb$ z__Tn-x#YFCXs(B=oz-`j!|DRExtI9+8s>24|HliUJjqcO5pBMhWcKEt=9Y6rZ=LK` z!np-B@xNcf5!yaaP{c=J(9MsH^7ds;TVel4pwI37MYAnNU0Oz7wUlkniFaw$5u9N6 z$vrV#-8iFya&HsXgM2$_Z(rw=P--nR2_(i_E2@{G3wa|#B2v4eNnwTlTaA0SBwWNt z3aIXyxLf4@_%i6+^yyu`yVZw(St#~4>BGTzR0K~ZCMf@q#d&F-jgo^2&G*q&__j}0 z>>>P;Y=#h@xT+3a6P=iJT}D23J>?$$j)kr+lcf6(UyJ^D?p!Fz%!qmN?gg1nyfxn! zegGv%j-V_fbX_FqD%Ct4ehFOhDe|z(FZQhj7Hf0C)R-}oe~$-fK#}2EY{u>DnnGY_ z-?**C-K|*~u5DJ?NbwjfgsUfghg%pDtS8T)SQ{<%pB4*kpBvjx%9|PqM=;yK4{D%0 z;}b|#1`SbQ_oB#+t$&Cvi zOOFi;`xF{J)VHi<&S9egg42_*bWX&zQaiyntfW!u+jE565}0s#&?X){P-a zp0;>(?mgSY^Er(h@RME}A)lGL#gh4m7TkXL3?nT{I=Q|!<>PC&TS$8fbhJ|-kbiLU zJl!FDXgT93zG6;c3R=Go#qqzP9sT7z!B!SvLXp)VO;`xgc=AT&X7&$+_G#iiH>JvizlrEIG zPP#<*g-NenZYmB!1*w_ItZ#;zI;m7NxYy?cvnxq3!ASJ4=ga+lxk3~a%|~R>GX0sl23h&_ zPAeJ`rm<0aj!1lno3;0cs7krF^UGgulFfoO1|bg1k{@_4H&Bl9j)N5flKPb(?WlPA zUF&^hoRFgjN9;1m^%%{Zb)j$($*Bsv+HP6hje(g-W*)Jjv5l_@iOXt&?;OhN)&yAh zd$5%;S7dhnpkoTPAuf0ZuWoE!VGm;XLef(i^#V6><+x+Gcu zw9Mk+)2^0TP0x>h-j}C`$@s^@`aYdOQv2bu(_=U{m1DV5cQP%KBd<0S{@!_&^*ujB zM7n?D9C#`jr~JNnrODH9ZtzJlF%K{$^S?OF-OpQrOiBi>2`-G7&nsVi!Tx~jkqNo# zzf1KfgmBM4{lLGl(rk8vk+L~-qTlZ76Jy(c~} zFbKLF@r!J9J3ewJ`>lDn;-@NMTBNogc66&8Wz2Lpq)+4$KjX>WE!|pe<^gS1bRnXS z&l(uBGk=G*VJCm@p<$@qfRa9Nh`UvKGzXFoUL?zU@s(<@Rs0w1t{QeHhie@U`x!VA zyKJ_``YBh~IPAtMW$tBBB0eWnQR1lZ5gx+n9`7A+8e8!cAa_F`)VnAlUC+ee~+O4eN8RUIs` zI}4zF@lSupSJ;1OAS5T7Q1w9x_|bM_1upOv%w?7n zz@cc??PByydqh9E$8Mfrpxc|{DK`}?OWEr)L_**EH zIiC+6Tn|h!O{o#$c3vj2x4{F+@&a0K zq%+qh7#ie|{;m^{bEG{_&O1Qr-9-55K00mPsa_IRq^el{YD>8oFx8mVPqCCcBve=gt26xM(4hxY1Cbq{T7^xiDQnw=bwhO*S&wcL zULfV++$-P&|HIX_VszpB0dH=)bfr0+2yWk-EDgUn|DZN!ch}wVDmcJ`p_GMKu~Oj& zMu_?3inid)%%BzM-e{t%wpHgA%u&QhuJqH)6W7m293_nJ_ zk6v9S{zX!ijhY&|8d-{l^qQZQ;pqHBL$az>pk=g_wXg4WLK*pA%%22rTPX}Sx971w z-&DK>oJ^8zIO(r6q+JB)I(-CIroO>eNN^uC?y=yqyep`FS7S4unj+~1eH55-(`d_-7AUN|aBT-AW-oGj)K*q)orlaUQrpAZhRf3so1 z6cB)F*0;`gV+7Z>jl)1O^Mu8i!?V*W!h>ZyBmIu_P*E<$;^9O1*3o z`bO<>=GG`R z3;`JPy4E$AEMYNrEK>KbKUD%;Z4Vcm_yFoQ-ZvTgJGAcMZEMGeM#_l>Z(Aooorthh z%;Vbdq#%y(p)5EYPW>G=o=M5O3k&C1{aj+PnH;OcUGH5XpQCczu`&$DjF6qq(|=0$ zYmEiE5^-|HnmHWeV#0MfNKwpzhb`OC zsxSkrO;{X>-8!}ED&S6%AbGsj_MEuFoL<#}(r!J(1UE|jxt-O@%NB9*PMZTQ6MYfK zy~U~g1RoCnh=>SVts)?e31t|GhZSB5!W#wL+I(1^!1Mlx2$(_60{0nmzYlG4J;<1& z2Sjd^r#YAe(Sl&cx^yHORYnN;ApGPg#C>aDJyCx6kFD zyl+cd^MH|Dt|AL{9>sOE#XFC#dr=(FgIoD#bNh8<&I1wdvs2l0Onzk4dX&E} zwij%^G4rI$33KM@XQNx7R&@-MFhwqH9}2-@S;DmzOntHbiFdb1VlJ!Nm;6jk$P=y= zRHxLOau;4FczBB6oWB)F_P-Zq)^(v!qp7@$WJbwc|Dtd2Mbw+j38>-GIj1| z+!*qjP?a~I%YrJr?JL;`BOD4xZ91z51s0h0*zfbKv5x+-hOgbn7Q0)G<$%OjFbK-4# zsybrZSHjD|r4CW;)9EXHE7E_8iz%q`M4KrP<{y53+F%fo!J-3BD6P-SsDb#Uf`qKx@!m6?hszLeQ@RPEtZ2Ve$%F2(SUxV%Y->eeXYM?qJ}kM>`DQ`0zM41 zAaLC!(W|+Gy+K}{ib<|AkB6->Xc_1vWV=XM)|v60b8@Y=_%r;gu87uSM~k<}p=Wx1 zrJMcal>?_n^X?RLt}edo^O&f+#=!PcdfhEGTkg;%!|a=W zHhbZ^!*P=U9}z77#~|%p-ku{m;IrgFZ^p^ICzEaJXQjsgRHp))jI5bU#t`VMh9;!tJl-I#nTYQ1XS~&^0^-`=!uD9Nyzlr z?#4DY5x>&Jm{?%?p(C%3qLMC3BGPpFlh5bv@in*~U{-`@saw{3n7q9gY*y9--`BPh z#Q*`<;{ehuXZ{cDL_!o231;ECR z6r4LlY#+V$OkZ%O9wF+P@FB{#TkRM1wjt{wdeu`gcnks8aHO)fl!ghEHmhar+wcM~ z#$=1_901Mnc$^n1Q!P4OI>klvo!RPt6-!em*jGB+OI(qxWVcI}!X=)u4J2a>=gzYk z%b|L3icYW9Lp#P>zbgitynioh{s+Yzim+|)B{E%Fd|2%I2En<8*ye$!sWS098m#JO zXoSigE|;Lu_CZb|CLbSBMmh1$X$E=9l4C7ra*&T-a%ITs|Iq)E8a}-H8Cy(9-}S@3 z=V_f11b7Ahie=CcGn<51z-MIvCd;9M%b)aboVA_-`y}3e?0nUP4`eSNv7&5)j+T>m zURj-gxp^hztFj6^ug>#vM*XEN2Ga z`1k%LKBoQ~(p8HT^Pv8ke1F~qa%$ZRN7~4Ujd5Ui_U@h6i@S)|SD$h%pU)1v);xMY zeiLDb{bsHV%MCvF+IIo|bjNrIPjFHd0*0&VqKC$B`_f}AyKEsLcY%Rkr+%--f7Z|d zeV-!v6hkb|i@M@&Ws0{LAXfLbzu1-bT?aJ+lzD&mua zfZ^$8On77;9!f~Ohs_7{DTrjoF1^pJqR$EX$|80XfM4*-cPtpq`d0D*#pe^BWnOmC z>#4VfCaoRdenT;~rh;yV=o|*Sy@e7muxYrg2=w(>iW+fl9K)R>Jory zys$z{R=2`xzJs5PRu02F8dl5WWIjqL)qQ)_5^69Fhh8Y3oY(8+6>=`6ez*(On5eXR zN634&lAXy)@?9mt%T{_{D#+@a0MTk+VSG@mZxm~GSL*y?{Ud$+;cx!1fc_KIs5-8eO83+|1t-W1;C^PuEufQWi(KR zz|B^U83gvHH8;eIs?P%JNVmy@0?r@MJ$OULe^|U(ak5LGu%&!k4J8Q%pY*_=9{M{Hm2`Rd5`n+^Zaj_pevE$A~A^(A1KRt8a=JPIIu9L>`=54LDaG!N(XR-I<#}1@*t&pUiTTf01V>jPovLhB zIzW$)>@Iox#ojqRGxeAe@=k1IfYBHG%jV(IPdo{a`TP+okJH+z*XEq$p4p$=U3?vS zQl+98N9Y}8Fy#Zg$=ezf=`KEuT>SuGi5oM<+^}0Q)QX(d-zx13PAaFwEZMud%UlMq zsoh%T!_|?@TEm18Kfoy?CAp+|eu{;<>m?{#wF2W?PU0T=_&%~2JGzU7K@cr&BasTN z8~+2%ab#`w?jSWZ#S(X|)f^d!xIo1Y7BSqLNAJhzbpR-ME9SUfka;vUv3EvxfQKiR zbLZ%7VnbU5AAr5_QVj64A6;mCGccjr#yVU%QFAXr<01}Ds(v;Rma&GB>%on;c5?a7 zwW=h&dwvgQz>Nng^yujteixzEiT2v;_TpC2B*m$*hs)X}{EJCpMNz8Ozavcr1P;~}?AB7)Ht_)gll$|dEjrmA>w8w0*LJ^%iT6I1g{ zC$<7f@NrDt?C>n4`UI5V=AomrzrcdS?YB9J!_{5Xrg`FdOdvPZo)MIkU9BFy7@9%! zlk8K>0RuMGlF_MI+7HZ(J%0sv`MT8b9R>!v)Xf+nfeY%n>}JaJpoG$L@N0OY(_)$K z0B;AZp6AjDNy^>7&&4qrYXvkPgp^c3+nMnea`k9dkJ?FnETWDzSnWJ(7HEt&=gxW8rnh@2+2%dJ5U=E?j&FYybHfx`> zeaJ@M`g{Nsg7!qT?(Bd7$z`yi&1POMsoalcs|2%RYtQ62Lp!Th8YGlsw~!9JijDeJ zs_TZu*kT~BfcFmEgY)~>mx%s86?TYdto;qt1d#t?;;U4A*~ZG(rc=78Y4ej00cl`! zK)7Neb6l!aIqeVL4nYr8=|GNmdabH_+J`H3pEjAz9LK&)EZ2hw8Bkjvt6h~&ANsd% zbpG$OnqdU}AYY-@u6o(er4y`WmT&v{{d^D;;Iz8k&`@t2_2rk}%>rI+bHDf2wG6pb z?B1!j+Jz$@*OX`Xh1F})cWn~fFSg8SBZ~xH|IJp&u!emKxFV<9qh9$T_UwT$YYU&~ z`5m|y;3ZF+q_Fucb_LY(kAV2TriEdQ$;Go96y={xI*|_cb0NwGXZKUw* z$CyX)Xx4QK!T8;B?AF2fn{$NTQ|45V$&44i-h~`|*CAJO6#ogd?$Kp9;($PPPaRGN*iQI8bQ*XMXb`o7N6*6pl3G=o!3Sg)>ThNb z2Z2~T^m+rEV1ba={rDqSW+f2}JOG1-)pk+ox(0GkS*m?a#t)<%AdiZ}K{EdiA zeWaC|DZ^mBpv%O0mA4SeL@)K*l2D?9J2MO7@QDNlEO<!sllL;barY$hFUgRz)NJcL?n0OE)^qj&jVBCnFqw@p!hL z@5~{! zbl^5X%Au<(wiV01RTKZZ%_HfyZ{G)BB~;~#od)G zN*8H4;uJ6;(yLNz%1ZOpb+a++JCMo8wceq-Oq7h&bE}N4$oA^|W={K!LOoNAAI7F3 z)6hUc21*U|txft(LJbSpy8X_}12vqpmzv7_EHCd| z(3}-pavqL$U9Dwk3XMt{Yjq^Dr>3I@&*Y068kHcv>~^}xY8QGD#qDO-2oc>R5g`4KF*Biqa15#@6`3`4SR_V!4&d1ye(LBBA&?ze-UV$NJTR zd}S{g7nY2kcE&h9FE@m)mz?<+{4xM3>)##;9PRBdKC)fNbfi~xCO^HiZC_%x%;Wqc z32Kt?llOdU)T#3Nfk&#TBzgnme1=mSWfcu7IS09T#Cl- ze0?HAe03?AoSoNrrc;JNwVJiqjtKuF_qsAM=O#-EhesY-l9+Wo2C7%Y&yPeAu)uSJz-DcTHHeNUR{AEW z$>7xWytIT>6$wN{kyl`f3fm}Pvd?Hk8AG1>>;scw* zPJ;D{u5$PFyU+zrVYokpZZ>Eqn`)tCB|E&A;zeZl%gOmFo6$#Xe-Zm+up7Zsj5eQ9 z|DQlxz}{vdLlqq^)&j8>nioGgbfxk?HqtEHeO{}4pC6P709ap{CZZ*mC)_q!;g>vu zp9~MCyJ6}WHjNT9LtW@I0=CtH(@&vYnr6ywJU1J=qFE z#MJ*|@2%gW;I{W+&_^*rDU}ov6cA9lK@gCZmVP9pLAn_fl@93!rCV~S0hMl~d!(D8 zJKlxoIp=uZKjC|Q_(A8IftfvfuejH}VhaIYhGVkqe%rm@&8cfdyVzwqI_8yMrqz90 zKL>1LiTP5a<+U03`kzExA+7-PbkqGop<8}I+t3%xS#3x)b>szeZYLnO)y49pWqgh&cezoSYK}WwJ|Lega zyzG%Ct(rcY5;-&>^dLNe4zGw=OOQG&kh+ZdvqGEw^gA>A#m{ViRWMQw$4M#^O-2>G z5f8YyoV(5jwF0X*Zj;BRAaG;&3e3js8nRNEYX+Qe=v{pPMLsTEP!8re@F{qyQ>Wg* z;1T4|s^QOb z7>h9i4#Q@P#0e>NJ5uOh(Evb=ipyPx;>xZz_rZhQ^p|wi^3cK1<)TDSR4(1FRhnL! z%X`X}%m*|Hl)4Gfu*Z%tTo<{>$V8<)&0opm={MEX=eRMNdp1i%yFkwUsnxfwzll{J zxf?c}_bm9Jc$}61rLMPfhY{eSaOB3J|0E$+RY{(%uh@qcj|1H3!kzNzPqUJabrEBR z(5uls6Xa7uQt?EU>%>a|8rO8bi-VD7=l=o4at8y=Kyg5)M07l-P3L0DcZlSOh>oAq zCeM3bp7?KS6v|+7)ej&<=459~PkNCP3JrD5MaE*Uwbh+hAv^7`PM5DI@Jy^8>UiCz zb~Yh#0Cz5|4SY;4B3`*o@pTv~Ie>$JAWAKV>|iN#V9;i?_qu*U8-MdWr0D1Qp-KRf zEkkx2bBTIkjc0ja&R;NRApYg&EMc;C1Z9Wi?c)Mt{`p@8+0$J%F7*PCjyr~8GH#76 z0;oPJT$;x$^E!ZihonSe<<>8&XKj^xy~}aJnFnEt${zy4isOLRfE2CM-t=}og8cRq zD|5U?y4Uv3CEtGy2 zgd|SKA|=M2y=}Y9+x<6ynBVhPoFajpYG+U{Uc~Z?&Hbc-U+&E3M4U|EkS`C2xSCpj z7SlOOcigkzKAZ|=+vF}m4qFMVP=rtPR2(<4Y-hKmahio_pI(6K3H7F4L7*>mpKe?+ zb`muZO${U15DY;)JHdl^Axrp`vCT-L{T+~xqru^aVn2rwxErSWRGSBfi$ z4f2(2sYb)fJs`vMEmu*nYbDPX~DBYfrLs6RY6Lq!~!iS&WgF!u5A*eh#>JFBvyt`Z6(l02~KQ zCnYGX0rUdhiIB(iWW=LOQFBlrL#)#W+jG_W++Wgoc>__XG4^Iuv*BO}uVv8PeZ2qu>R{&<%PUNep>fe`NVs4Krx2&?l;j& zZ*^fN|9kmr&@oz6X-f#}fyo|Ldl(pH^LQ84+9*uMFP13akWTo4s~9>#{=@nQ9N9Ke zTLLtOV%uE`(E?LWSP}C_JM3JA%%*aFgH>VzsU2dzkn&LUVEBa(#l62&=)-~kh8pO{ zyLR>^au*K7W!1AsBxy;$eEG8Rt$=}%C+P3>vbz;B5E23PlszHGkjk>^Q;&P2Ymw$b zzR|(EmF?R(D|%)N&sJZ6i)xN?i~T|NXMnZQ^Q6Fb5quoJeeB9U>0fLcsgD@}Du$Om zvtAnvcfQ6n46`hbPa5CPF;A7Dy6K=#;b_$lD6}@LaE>QA%rUX+659S6ShmyAjX_UZEyM{J38Mo(U^@LBGU_u3LpcH#feGHig4hsL`a zbIaTO*@Q4i6bkBJsSCBv_%_~r!cUg|ST+6|(BFOC4n2V{F1zVMsKWzP9`HU#uN5g+ zvd9npUAEXh{uGu#)3Rp&y_J##1$X=8;=#zvY-YVa@th zy1QaNkVN$KLz_=eeI&O#JsT~dWw&|KJ=U8;55^iSARlii^Hq}KoseEKk`b~6?R(KDPsPf#f_@?~s59d1MV1o845Mj> zMzh3>_lEtOOn0DDIhBQ9jq(aMlyzUJ|EW0fTk7LgAc_t0`WP7zVdZk67nTB6s1sT!-E5-hFgK29Q(L40v(D+en zzIG1swZW!kyIu8X{oKI&D`c7ty!C^^E&ZQ&sA6Q{c%Y91p@Ybgqr`^x(m)uy0#SXz zOK;CQgI`DDa>?@EBFTRSrA-bW`Q8m!PA;s}>@~#C9c&p;{ywz_XQu{@Su3;9Pvy2Z z_W3Ma))IXSDY4g+MZPeW%RtD_Nn|X5)RUj57An3%HaFK!iWAB3 z?!&N|0w^!pxsq}Df5ayN?D&D#p+5|ukZhqZo4YY3Bt)%qP2-&U=NB-d(;ru}@r!M? z@f45i50X+2cI8YZ7}x|2Bt;KtnP^+B=kgdH?QB4=_UrzNw*lDU`6H1R*9?(9A zP9zR9ewzNCdMo-qL5a*E@GQW3v|@Pg#Ra2Zw=z+y;84nSw)LcUzelrYdJ+mg!F^C+Gln;J>bUpj&AoJI zK2tJp|IN30k*Q{@;Y!eiV&8L_mDkdJU>+};Tc_;k1-+j2bd6Wr^3lT8QVq72s=94Ckjv+5+} z2dZvgHwqeC>p6j9$%Enw^o15Zvk9p2aKy+H_BcO>63dFyFVXy=yDzMVA8EKe|B|kQ zJlb5?hCBXdM-5!sVqcLLdq7BR(q}LYBUTi9tfR75HlZBB6vsNqH&6z68<@xV0WH%a zA{zuLS|}-cSoazFqW;Uby{BZ6&qvqwn9ff^cHQ5dGMdO|EhL(ae znZtMd+tBnwIuSIXtuQ;N!-y&n#+fl`yS>aD7;v)k(Qo(>AHaLOX>ph(RiGSs z6LOdfGkKn5caut*0ga%dAJqX&HU+~0{W;F3eqFSG`raRBy)JDXE-;8~z(TBP=E~}w zWJr{P_8p&TUv+@g&YefbFdyjtF>5to!%5ei-_3WfyRlSp;baeh((qI`O;pB^3|FgKWK~E4B3fN2p zLId`;U8^Gm^G&xz$~|Z!S2rUmp^>D`k{gbX%xq-b##t{hJ0l~^V#uxZ4D3fP1Oa~r zX4}9T?9Rc5X9@*+=zWU7P#Gg4`zE<`dl94{zv0J(8V0)TUlfkVXA_7XhP6PGS_y`` zzX_T=5lieFgJs9I+U^BH+MPy!CWlBeePOa&y1>-Q@S8^fF9f2l;vte>5T{7#THs%|66sYdOcPL>p;I_w^6n{@fk;FV?WWm?N5JI`!uDNF6jUDM(@J9stEKV?76cog;AgKyUmS#-x z=BA0QT`sua_`^S{Hvz=}13Jup_y+{t(|ynFbU1ff;Bf1W^4KY5W4fSehfF-%`T2e7xkp@Q(bYT$YRPTQ+kGVoLSdU ze<-V_+&3srgtz^AXKaUzEA?Z|?Ist>op2;#eVxDNlz!-n`>Ou=Fk!q0Fg_ zvQT(t99yDM@ERFY)g}{n^5u)}?x|azLF2lzziYM1ccgQcMtJu|j!L!++-3;ao64Te zuEvAPd!Xv#nr#M7FANSoTG;A z$oZTn1k>{+krzvzVaXV%w-5_GprO&JwobQdo$w9KI#MnbD@yP*6m6`Nm|J9}u~?`N zaI1N>sq`djk3N?g_f%0e#b#Tr^3H8O+tGVhy(n}{v&Qij`_d&%op{*pY}Ix$B=WgQ zyxIAPb)a}P$os^pf7qmCI73&QtX~#|G!rCA=9rstJ-d(vaZ^Zy@rF>ga`=GpYPair zRlUaQU;LA^jO4I+#&pV9@o{lv8(g-7Wum@4nMd0)M7tL$&dX(}B}K=@S#PwlGc+dn zUVXMNl~cHS-cT%K$kfHr< zaN{pu4GOWqG4aQWX(BNDo8tMlZ52=ImTpb_Ns;^Xs$G zpYD_X)z$KMBw8Z-BT8xATVADdsJDEn-r$Wj%N^UioBuu2L^mH(T^SL}xml4preQMM z)61&S_@EhSseIVB!n$8hGZ~oLy&}4TWm7g(he%{_g4EWQoyu(UdW9q0hg%Z(=-WgthO7MPl+6 z>tHjlyN^$1$&Q?BF;eSLHd78KryMSWHBgtt7fSya)mdAC7nSL&)#I*MkE%ZO@WV!? z+N5%XgoK=O+w}dk37-5Re{xW&9%^xH_?{0pyd#|CMx>aA13Bi&o3ubD7v<4yfgpUG z8&X~gwGGTXlL$JhC-M~`LRh(M=P-QBy{3{*@w2{HpEfU%$~fR9%0pl0^XJkb9Ylq2 z&%nP$5s{}{m?du4Qz|nOs8;4Wa{TMT<;$a027$j1oytR2Lc+(#N26vYDeW_Il91jh zyz5Q&tA6_-%gPj+)Yo<7zc7g9Y_l#&tDAHPpX-+23v(BT7M|)VbjK%&E6(OPnDC^0 z(UwwnZH`)uDD(KqVmy}89#s;H`V5Z3X2AE)vtyNPq31Lfmai;F#Td5v||A$fNCLci7ehEQXT zESK|SNb=m>@Fmye_a8MbpomdkZ8vv|yUP^>fBv*WZ%3znA-C!+JU*F@HuYINcoSD9 z20NE|>b;+~UCo?6=6_p#^+3G`72?idg+0YKbhbB0ANVy|*5!nqpV? z*;)MZ`6XGHWs^c3s7VF45!GBu`b>N&NiZFMw_y@@DCvLaC4XBt5xAo< z+}Ldci-((gX590^*nu3ga-WFRMp%`k%C^y{$=-hJTUe!(u5yBa1L{@i_pH5$ECpqL z-rWRe%n5-VF0HVR*dw))WG$pm#Vonf6D@^s{6?P5gE5!u2`IN+pZR$%yYXpjZ!Go9 z(d*8=<-1|N&Wwb&K7Xkr~{?Fo%+3B$lj?yF%HD=RvL zF^O|$R1K91%F;jwm5ZJoe-jTvbI zd<&VaKE0_sRj@8p7yLx8qWet0t$-lX>-6hjsg+JZjCR)E^U6Zy%q)sNlalqdJM6}@ z!`YmxK7I-mt{reUi2(sQIj6x6PyFzT^7?45>9~7)!?A7Mr$3jwsA96EiOQ58V;a|@ zZQHkNt@ju)Nx+>xbCbqyKd^Y{==S?pes!SIBeE7Vxn4!!5oj#tS=x`tgMyI@F8mMv@Z&_e3hD`y6^)fen79wI z9a>dHMC1>&N}AwW2>PA_02@k5%cD9-rR+X2t1OUIqq2owi_?(-)#hpoUb2Hs{s!k+k9mDNdYF9f>fAFzcPI*%3vszE{{;R(Vs;lm=TNQrFDw11&c zf4k%B+QS9S39twM!Ar`SpvlGU!(h3(EcZsyr-O*ylc|)H`wNTDR^j5|=>7+nlX`g3 z^Sw)k%181DI!3wl*CG~w2?Tc#u{@WMxLig&J!+OgpQrnzJt0>d$BQqrBiva^LSkjl zE)w1Ri6DZ9oixjIyqfLF7DCF5d&W-VcHhcmkHHTxa~;}cH&1!qTCxa$t?j1K)ulRWzfSnWd8s!uppp%1dE)EMPv6sJm#(ZM*4FO0@y}8J z7L7e@+XI)y*NlTH#?!f6{F2%_rWK?w?bOR(%bg=Hua;9#*uaqPE@iwHCxchoqjtrW z*vPG(`y6vV;&Vn%6bW{!j}+EA>cwIlNqA5#OQji0qv=wKnKh60myTw>MMZIanL853 z$}O%FtDgIo*&E2Ew8pg-kaqa%=hZXBgmY!+52*+*s*`aCZ(kysDjz7h%b3 zw{x)BwO02m!MOu$w^G{n`uTYPDZIxgnNz(QM7-#XqygQgs1JaO!#aovQ&LjiS9{+; z?#>h|=Tqa#)0U!F#t|W*@@XNVia&lNGeHWt(8RDYA+-KZ)fa6)%WQV}dYo~=lnZiA!GDRaum;hROqsqr9%xG^)bA@){7@5c}!~IFE(RQ!d1U%1cK=9o44y zSnAO}Q{Tp1f}HW973FJE0h(;!H#*^TK0U;ZcS>zAeeCjwsSZ&xW|Y&~+$jw>aCV4( zY}^nNfH)?1d~jbN;bcHx{LEZavU<3W>^J<70gV1o*5?eBa`OA(f@h#|?Fg@{OEGsj zGL0%#)=X*9p~-f)tRUS7UtW^Pw#3mi=S zlb(iD%aJ^S^bR$o`R@8Jug<;W>QOL;n%34WH&3OuH4h_|zK2$Fb?n~env(6JJ~(bA z`~zcTE}F_9!6+J-vh`$+Rf8X1sg#Qou&kA>K`>n$#SQYFHNhnRHa0-@N9360Dj z5AY3~X}m#+gI&Dg7lh!qTfGRImQ|Ln%4uF{yWIhy*ibUjA9$IvsM~q)`O1(Qx=@dN zZd?F;Jm@eu%6*kHk;i4_RZ~$$6qkG2!TN6#Bz`q1G$iDF-UTPS9Y!_L?Sx?Z)R~1< z*2U3OjlVuQ-L+U?)R|vX8>~` zTja39%yc;`wv$+~e>7OuHwm<+ib0f`gYRD%P;pv?(rz zm5aYR?$Yb_tF46{6^6ix4CCtHT)SOncuHr5d|3k*wSkFmwyea#iGm|c)3 z@a4GNH+-G^PLFIF3taFLx|$mv{7cv#oZ(ofJH-1_z34EGr%h>fsNy_PFToR@xV9pB zr|_t#Vi&XoSUaLy`5UUnjGZUcvtHuQq~@-0hlYhcibz0zXWfiE;lzw#s>sUbzAXqkkj3&CqB0NO7auXJH*P&YA8j`1~MM!lU10wNr2xR zAC8S(oaOD~w%yuY1Geyx%f;$MMOiPg+^@9inqL^WE4XI$Um#gmPse=7y*I}l*{z-s zrt8z{Y=u$PPA|^@KtoUJ2uRXT>Iq|LKY*{BG!`{>?YuA9<|k&^znBjs@@yZJ+hFJb z)khZ@ZB9(xvb=Z^4Wk(*lIP@qS~WNG@h15v2wWxQd;NJbE4{aBi;lqS)Og-lsv{ z)SDk(`b#ut28rT~A@ChJ@BdmH0~apnxOL7-iz<&v9l`m%eE$W(2qr5wP21fYKl0vW zOiIX8h62GfOF^`f6(Uka3hc_Rg0ZQLW}njY%7kXeC&}jhvYO3o7Ti^U=qHORwxW3; zs6pX$YSuWN+IJ-fH-5IVTF)^s*ql~-dAJ%{H>@{Y>R(ww_J5Sr~UMd&0(y}^KF&9ll8^D^*Vy$p2tO{t3WFWyB;l3pF~z;ILF3Z z2vc7|#Am*F9JEkijl^cX7@+=mu`yLp=G3jbjXl&b_t+v9q5O7`b;{gP2ai~b|88&TvHCI{I*AKN4iI*JTWc|5P)Tz(iAK?l~UxE z<%Ct$ag^%!1$N!K>3VfL5ENbOKw4Q-rR9aL8##}20`oPjKDZFC$z2*qKP)7fPyP+M&7?qNjeN7 z5>%&=jANhH7BHc?cTe5S8iWj+nu49>vsOpKv}Mxhil{YMA{x4eI3#a9xH5{7hXeL3%#l)$4PY@G{`8Fsy(5cz`Lv{^xNa z;HrNfHoe0l`RC!?tN-`Q|F;|eCma8-X-cq3YXl?Q5{Kn+bT^MNH8K`v;{n@-_5AD~ zcoHlhmLuEV;6*o0rvm%qXyjjKM(Z*)co@>_`?7F^8f9| z|5LJIOA8!5g@9|B17_TZ;DNJ?i-v{PdT##XFndp z)E@2tlbo7EXxL49gI~k2Jy@*wO=@9b;gt42e`{%|$S_yEgkB-Vn@!K1l7Jwz=JX^s z;ooRN=pEKUuG`^Gbab>t&kWf^c$W|wDP29iRK|1vd=`WS?xw4w<;qz~pVC^nS@oNO zz3?a{HU9e7>sZ@jSjYYFom#@^r~@Kw(zGh=IW0zmOE3NN`eFcl1eM6gFnVQ462>`c zKVrr_&GLKZH~)FPmlS(TgIdi0p;kpPn|eodf3Aj3jl0(Hy?UzWSOJJ5~Y#Q*G<5bCB-O?X%sn{juFWH`OV`B^g}*jCelJftN$f&=HD z&oc?aIylurIrsJS6dC>e(jCQZ>g?=1QflrO6ckx{;m_ZqdGNq;@ay;P?!xDv?scbd zvl20=#PQfI_N2?DUjOG)u`VUiOioQjGHIC@8>{8Kd}2Hv_w@U_3+(Ld;z1P0zrLS0 zCiv$)J=p&0QdnPKC*`!*T3gFd%2b&6@qy;2&D(PqV7&$w76NVuTfxCM7MlM32awJ* zyvNPGKieLwm?mvyXUERLk*AHSNJ~qLh={1JuFlOhKRr1PqTnYXC1oK0k4wPDwvG7B z$U9mc?X9B`r*7~`;D|<0F1yEvyJxcK`Hy^H5&r+Y%C|XcTH4u$08-eJSV2T}em*AY zsi(`<{7|k2l48td3xh_$n-;xpABO&gTTGhX-rm5VehS1pTeVO%L_=6YGooT*2Zx89 z(X6od?&LhSM&nhY$qjk9xPkWD%c6bx=%j=_yh`WmBwSp)m1Py>F3ubaqve~Of^yu} z)6bthdtc1`dlNR`=4UJ{`sMx2Ia0X9t}h!I8d?;sX0D-8$)r^s%VT~JQ?g{gI%>Dp z^;xem@bK`ElH0ns6Aqja!K7Ab+ZI)ztvyam98qEtJv@+q>Nb$K)Sr*o*rLOG_S>0m zjLkv1`u_d*US5LETMxOo_CQ%DOZk|Xm^vML3knM468J7(xpL*&wJMvrK%>X^?%kuK zli<;wKe_MMRRne5pIvyt#ZHD@@JSb}Nvax55EqVPLLRk`>`6 zm7J-VzJf;l8ZI%Nd~{b9dwo5>o|}P2#@f1&i`sj0xHXcQlF#wRjT>Jr(I)XLsW1XGE?^2e2Bb$PsS$$3DjQl%nS$5Dr1U^y+vUAN}trKP1| z^|q5(SXp9m>2(r|I67$Oot>B%Ws~R_-_3X~etwr7z9&!Qd2EU6f;$rh1raAlaLQ&Y z%k-v3FV)oo$oLOh`m(t#e;Xr@Wj9>x;xatM!PWv;5e=k;7Fe2h7p}U0div>}XiiSf z>y9|5?ZrMwcuR<@1c!y$V#ZN4QW3piV;$Eg@5JIin^IR-ul;zP*J(o^j=+VIiTM2c zj~`*#m0*#>>6JsIV@I!mjfXGT63)PB)RlxdJ%K+b4rQEWqNn%0PlBb?yEKr$yFAoW zp!a~EA9aO@frp#-bmtI5MMV|T`;u&~UZ*?Jqhrs(r+Bm1PbQX=mxm{UPNP(Pn}iWn zWse!6?y%~9`rAA-@rUCj)yHpw+PS*9WrM~i31J)mb(sI-!&Riw&&jUOA|PSiv@ADv z7gh5#7eF?^@ubS=zxew1Q(oT6_E=6hJ$-~8B|bij+kq`Pd3+EVzoDeGNQp_`aH)Bv z>;8t#Tn9hmgip28>O7VJw-xc>@hZ-bWAH8zk^3+rj7(j;y)K*G0F1vY&iRg_P zhP2Ko?)^ECN14;@f%;hFk<(Eb;?0{kuC7(Z^=!;K)ypHu{D=sZA?ukI)r`L_bxx6J z_gjcC+}}1-h$+(*iyRj8=GV7EgI38Nl?xZH5Wcat=CPfBq0;%(uW}bQ5NX_#_RI&r z`xC2Two0x{EQi6S|4SpI`N0A`29=zLmyU7XUgfo(o*%NAwLn)-ffT|1VycB?mb#{#L5xLX!6h7(N=GZCN@}R7VFnAR zvsr0>TN>_^VGuq<0*is~$B!S`UbPE@U9PIC0#yn##vy$5>eYE1+#_)JsmTWI%waC#lJFUCbY`pMj~5wYK8twu=ctVo88Ry>vR=R5zClEva*P;-R6LfyK_w&JJx=ea--%2mVX-NUgxg95m(P#f#lH3zfhA{r#!u#S1t% zQtAYCZj~q$ie;upx-1PK9hi8dP?YBvI%F8O89345rFLqJLjE?bZX^q8ekyMtk?U;4< zwzgI(Gvt4~9&ggmmZOkrQtulMU`;j6Y)D008wZcV-^b^Z?7zHrdCCv$Ezr`7*KR}A zKAmyOfq=ceeY$LX$krGc1;sc3yfaAVzpT%WVmao?YT{hOMcWGUU*MI$s`zI=|; zj{O9?Cr#$+)vK+8>Y$BN4FQ{v%@c#8)TybHFI*+|_L<`xz)HH`-D4=;*t<2<)%9k1 z&}wokGWhlGDnIyGHV%%Tux#bl-jS*#}9IYh6u zt?k()Z#5>aETyM+Iy)cqWhoDIbv;teJ3<_Y(ofhlQhEP=F)jEtGq2>7Ii zh6cb@NQ*H^X=#6df6Wq;lGxZvcq%V1p`gI0yj)a^bmZc7viNvXh(#)0*d(qdoeH|` z>>)R=KX`EK@Zfq*?vi(bt*vdgN?+E?Oa(Ba)yJjM*(%yNxMWFa!!wBxIsv_O?#+-l zXbJn2md-u3xahpUG2@?m{SM20yfKM%R#sMC_an!K{?~0$5Qk8a%i0mrD>b#YwxZFA z*GRAge43h?z;^I*b1$W&bZ&Up9kT|HAu ze4*IbSU^>VX69gRVq#*tCzK>V3x9~=v}Bs~tLDP`MSXa>r6xX5SEv8uJ!MbN90V7# zu^jCk9eHB(=KTC_)YMbMCGAVSnWmOH=;r3sg6zW)tl^?)!_e23X#V+Cx|4(m0p7vE z!I+rng~inE!<~*hxzI z7XQSfCXs&r+}qa|pOTP_jEs=b_HbwA+qZAL4y&Uzh|~3{#>r&RQpowH>dg*<1=6l^ zq>9gGo3ybRDSSycF)`8ovD;gsePrOGF3CM=>eTksn~F?nr(=KXGCud(Q#CTmn7*Fq zbc2*Mww?H%fEJH%pn>4J?hd;09)!5vNP_)mfX?J3PPv}&>mokkeiF+G<9lRMKvB;V zQW`GS7HMnbdPSF4vq1MlQ4uUW4@58@^L{IV6Cyg^S3mwTHCln*Y`_xkyTnlQU@RgtPn098ycLvZ8{Qmop3f?faRl`O)3g^&;*w8#{ZeYQ~Gw z(Hcwg`)&|OQ;JF|C`gWGhYfQ1etAffTLx{!_~>be)s$H!E)M{WMQ`7}Wn*gx&v?b3 zgnbeA=Z=O(p}s_di}`AAW)GYSOX=*~oNA@L`NA9e3<#S5`cyk@&bla;YYtU9VCH`& z}Y1vsjn(6{dN{Kk4GGEt$KO-jfw2eExa`oy+v2mzvn=4?l zthKeZtsuXE03s5XdJw&)eqJ~Xdnm69_zXia zC+)DmG(I9O)d2>CN8eQ+S)0^>0zR+5z$sVC;Dh_DhMK`W-d?4 zrh$Q7*!*yJg+L-^ZB12GB%naRewQv?f{?LEgzOQJFA#Ih!x`%}_-6v%2R@_-9p_6( zrvTV*y&QmJh4Z$Fg`h)w4Ew>sLAO}X*2;*gw2$q3v-*Yx**LBzJ@pS&hTdN$xIZ%O zLPJZNr&BZD)bxCK^p*}GAVt8&Ag~J})sE}Ba2#sp8V&q`b?~AA(?(N4yJm&=Rz_Ac9jHh+7o0334xC)W{ z<;#~LYQD|&CMh#B6T*0|E5@cU3!3ycY3vc=AZ};b#`)kVZ)xr{rPI#bBvzxBjV()F zL_JL}nPSinjPBM@o^zD!C)fNkS({J)f>4ys zI6PyoEJl9sYk?2aLJCj+pF`dU?qiA%f9RSBKd1bZ2Fx+*wCgL7{tduJicl zsKRzZ0UWGZVTUvkqk6GyJkO+!*r^9No17lRaBM%ztfr)sB_P59Fg!z_igLhc z1D|)s?U*w#&TFEaw<~3-3h|OIh=|nME%l>F1yt+YQLw$sh```EEFLB%i=E}6Gyh(? z$Gk2F?ysp@2U2KF_rbf}iWKAFsknHN9h{ln$}qc?{^ss3W~e9~VgZ$`-o``#J^ubO z-8y{pgI&!tHOv&!zkRqx*SGHt{Y=M0b0ovY3pO9F0G8X8lt~oa%Jl?GKPgl(5ftFC zI^I6Q1YAe2sH9|fZSaEEZrZPM4kKg5u&fw)y6kNF8@>limzqeT+3M$2H5|e3nlZ`*8JEL|}3Z zSiO^zLk!-6Dpm5)m82ffqAE}!2$(trI9=!ZvZ1F18@NsY&%VXEqYu@KGhWtfl$u?J zXh4D@27_H3g@z#SH3=;-gNoVS+609Bp@E?vwgy$j#l?qTsJEa&1HdR(lkTCgi*KjO zB?^RSTUgJZ93PHVI&`{ow%K60!6rZ$B^BG6B=q*2OblB&;6&TDYnPI!yc`ya^78Tk zLVtZ#d+uDGx0~zf$@zF5yDYV$*AQ!$6lbMYJH9}FYZ|PB=tdhrh-^myNu@!eK>E(1 znX!Sseok&~vRZ1(NCIb7xAXYk*DdaQ_jG$^+qK69u9J~Hkd3ircpMCPM?WM)>_Anh zQi1F#1`0z{{=`uY+x4g=5x z94kdCNkB=7Sz6L4x1J#rz|+WhLG|E4dgueW*D)V_jKPKZR9Cy>LJ2MrlcwW05A5C5 zvCs~~glS8xV^MV^90l?B&a62zqFf!U3{2<+l z0xJ^}pB6Y&D(zRmA67eLbziu6vD6YR&|`0FX9r>)yPhxy`~CN;SD(BRV(RmXoi<FF8UoQ5N@cz`a+dAD0lu5|70&Zt=gDK^^%cnFhD(f|_n?j8c1%bwpoadawdCCGB@ zmq!<-deo2Ta_0gThmDV|Q>yK{Dgq${1Ax~6K$=1=y%QKW`|U@Jx&Ho&J#+NH0%yD^ z(A?h`9~Z!ZC*n?=sTG012C*?lSBvDTr}bqId@U%5XLGW?dxtGawwwCrXu{Gbjmem~ z{&o$9NM@ZvqxGNE;u}j=!JScqwDk(UfxhlDHjD<9Y49H%2OV>(-}k zF>4b>uFA{HKWYn-&WRd$&?#_~^+mTuM4mA9{q|bx#?2{OTCbTr2St6+lJoPN#tUsA zOKfaxg+aB|&t24qzJH(CTb~-zG~sjFNC93Mj8P}sYV#Sg01^~nS{v=Ue_-Iw_I6)) zcS|4Zu%y(ZX&^kcx*hDtr(NLmhKsTpHA?whcFZ`|y~?^qzrOMe5ASmsu*?HS+XTD< zVEvZYJ5BUu~6D`7{6bK7U{so8xC~thytYV*HW(sguRQr=u53p^) z#EJRe{^Z5U011g{R1Im92G%jGbys%hma)zwuv(0MEZ_tncQ`G73d_kcuY8~DwcxxK;NNZ%JCLb}ba%rsoD&Ps8Urc-(0zhhO6T@+ zZq5>T2ydBG$?z;9dX+~y)$fqVPB7F0kKF?O{RxrlXt!NAlT!)~`x4XGr6tnA!Qre` zo36>=5U?&JB+gR@0j%k=lmfPnjXNs{i=o2p&YGS5{rELOD^{ig%Ee}9Cw5GN5X^oNaR(fw3Ef0S&R$Omnis!jb z@%YU_v2iSiMV0%CdQMxQ|^B3+$=qF--CF0Wd)Cd zFKRPa^jbMEw!i~Rk}3pyVez2WAzAT!q{SZu9-{QZ80&LJ)5#DyDrc)e-s!ojs+^ziU)If4gSFD^2s zZ9+op!RfwXUrV^)f$aQ9wu4vcN|k!C+2X{|;81<)E0M^LvI%(>jIkW1Q&T<1hk40j z{+f>wCjx>szi{xhYuw}e1DOrQ1ik-B81X1CS!35;7HC2OfT(n43~1`fHQxcCT<-O|gK>aiN$ zS*8Xnf9vQhKN85T0vUAL>-xMo+jbMjmltX2Cp8m?q1zYMq*@+BH6|Bi2ZV2zJ<&@L zL@<#H_i3W6Q%82uOG?>!xymwRG4^I?Ch~iNqqx_|xXm23q_U3~HI{e+w@JOt(`>wO z^~8h5%O-+=^>-Z_V8JI{^gP_Hf2HF3?2yA;i;Og8fTeHX=S`_cvE4oWH$Azzxoph_ zw9Up5$NTAArE`bHRMhoY2d~zz$UP!`zkT~TnBZ&Mqa_Bds!JdSIBL-@13y$uX61IbMppJoR7SU{4k zg!Xr46r0LsYl z@M~6!3J6Dh?3D5}x!0|LpU^$LNl9OtDoyaszb=i%VzkVRM<9aEgP3%%%4xICR+3ca zurNg`k_l;PxI5gxwYy=!Q}MDFqEz>_UvKrp$et^~Q!UNZIAnbHS@h-nw;*oP2Dt&(ynS>0fc3g@yb;Ln#FAHYSavVDK#{ zCUtsT#=jO8o^ZPLXDMY`fAA&JK+4J_77c&3dAL0^*Sl37nVk*vjJWy$W&7wT{}Sh$ z$82o)I5Mm@o0NzHiG5b10y3sJ!5YV(U$Bo4O3eC4#~b?Vx}GDQuh+ky)LKx=w#Z1= zY5dWhTwofMHwIc^_v7?E*PEH??nzM7+KYHkGL4NM^`}{frNwR%+ zMRg>lUp((qtrr?)BhJHqYPT?1PoY&w-yucOQrFeh29U2!I>)&{cm!$83nrb$k2sOr zP0VbXTPKSr(?&MTi1)bj6Pj}2^UmK zEdcrImj_>}r8AGHi-{4quN|*RS&2^er#S>A_?oiTNao;DyLo=RPF9zmqk~+P=)cug zeow{W#fvWAnN}D3rPby6ho#+HK|KTgPuN-PO@7_oSzXnt#9bah)OEf3a&eoot4M=$ zy|JN#Nf+zM%Y$N{sUknUT6dI7zD5!4+^&C9uJpNig_xVwf>K`RJ>>3NG$y3mkzGzQ z*ogkCYvD_BauB89S-N+1tP%<814(xP@<&raf)sp?YvW!*L?67oFz6b@XqgoNYLo61 zA0*NNY&AF<>&;miE5?W92X1xOo8kM;b;L7}00D0d1f8Ojqq8#)7gtnNloFU{a9g6j zgie;zF${oM4LjoYzqJZb96v=6o z6c^VKhbUK5D`GKfR=cYGe_WkaR26KrwKuJVh)9=8Nq0+$qBKZ%Nw;)KcZ*7wAl)Dc z2q>kJn{E)0lrCxh<#*1x`G+@NH~`sitTpE|*K50*qL`Na&ptFCR_TR>)yYpw#TJv0 zkdW-#>6+1wIE$(+TZSLu&0r1uSX_*Zj8=pxKcx`9+cQE{?$0T@11HQt_5lNt!;U^i z3j)yw7c9CaX;qb#@pWnWZd+5}M!$i5XJX^`S6TseZ9x6Wla5FtmZ9NQa=zLom(%SN zr|a{-JJaxg4WcIDg(CKt7Y)$>+Odb{DxMkM7;iP)8>Ec>&pfAsV}wuF8x*eXrI1Md2Fox zDK?Q!TAEVoBcZi9(azj3ZVw_7_x&ute1(*ua#sq9Kf;*8jggW+z2z!G>tIn=5VJ8} zo($i-CS2T?I^()N+FTzFxIAO2JGj~NF=tpzTzoya#nXA|>}Y%5U7gZkn;cnTXk?WA z?{aH9J%D0cjZ(l+*7jzd>qg!2{+U%EriiIr%S`#4yboi6g=WW!<4 zc=7Apo-I7Z`p*(A`&?eTZ>2j*ZAg)JQfx*?*lUl|*lXjNWUiny{M*BNq9e(l_ zluOLHMxwVXZbsQBt&4HrWgvSZ^nePfXHZ>fO5<|JHH^#UB3VbXL228m%Tb zel{Zz49dXCRiDAqj^3~0`0}<^49l6q!%S+dAEh&|>-+bx(9ldlClhE5p<#vY@N@#@ z$GD+iGnirc@?6!}*{x?AC7n?u00KOJ_TwHm_bX7G`1$#@&5$PV>2oaL>`&%0l}%!2 zqdGJMFXry>(#i^h4zU1p*)0KIn>KTPD9~Gf}s3!n{_{bCKp9}e)Nar&YcHTi~=YwA=pIbAY>|BT*iLx z&f+qa=Ku3BcLpw{BWPmerKF^$l&8>kQrrgFkIAc^C+>S0zEvpYs2ji`VTT`_9Ocl0 ziJQ$=(XY2b8gC1f85%JkK~7G`uIQAbjY$xT!^JngcrRza`&6bciuH^!NxJ-6Axkw& z?0DC9WnplnZ9Z*Jv}qRdcK&R0c=)a6YS*KShRY2aQp0NeH!zV! zPIFO7uf6}qQu5D~shAQ<(RM2RbS7I{8>|2HXRvH~r^K(x@A@@0v28!SuQd3$Q$nvd zlLc2I%`DCAw=E3~FT!G6GvE$lWccm;_L|+_?Um#0l67OTn{e&B8U2E$#cgXs$hUih zSRpMk=Kkfc$pVXtrDt)=cBZ#YXPccZR%+L`WAuk9O6^{h_@>oC(dh@m$HyE>osp;hfx0(3}X;z|63CS#)pFFOG= z1$%&Zz_=@fLRurj&i?e|1RWjyxj2)67@fajz3U$hIk_HrRUtU9#IG+7X5$y9eBrd$ z(9jrKxKalRgR8qEf^hz*F^y<&1VA|*ASR$vDCJ%QHx&9!=3&i=pCl4E;}wPu;4J3n zID&o+I(gX%W!UG@M{b+RH8nLZ!Mw0OO}Na@*cAKGEWh>i`~eJMh~ci@^FKdzgkKTA zqL7$xaTF}&f=OoMa&skh>730jJx^@IIolX1D7m+jvRJ_*W3 z1r-ZokCxfwmX*=UDk;q)kv~RC>)yR{ZT32joy;4!cDvmCy=h|D`mL${7I0Jl{KukK z%lvjzD{^OFY*x#~n$IKS2;WhQdVAI%A6qRAH*a#uj~ zdVKY+1kf0)`d=nGkd^u6265&31@fscUkod=cD?pJYx!HuS3ie=EN72#+k$bVtN8De z;~&Nbe~+cU{OKOp98-33k5DQX%7~1M)hzi6{+j#SciHv7@Y?=}7bbo`W#w7r-lNg; zo|RzQQA$_kke9^+0=8;cZu0_4(<01T6`ue3|qk! zmkX0q1=a)&EeIOTwGiL zvtvd)kV=b)38gm{)!F#-r`~oFAkGlcipJilZxVX4*2cBwb zCXod?s6ImU5%Roy_ioENXJCIH8z?X+8@!HL?dHCmVq%7(2fnGUu8xdkj>yZ&EwOlM zYhq<)=dd%u;Opk-SY1~aaHw_`-Qf!n8fpe6vfHE{C9{VqjemZ9l&MOTR_>u%=jomo z=j7xR%aisg(*1yjU3*+8>ipkHfD=H$ZzHj{32>J2j$M12}c%;h( zlU6Sk((<^>T{_Cr1oj-xQ#}!>cz-kyEMt*f7YtWmK#L#bBb5ix2s-nH4d=W+pp+cXCV%o>Kz!L#03#n@z+l@Sqx-M*XOKf zb&<={hjDvnfr_LevAsy z0jBq{V0~gZuZnt|+%6w>5v%}QF&^CIko>v1Y>4n zvj?J4wG$2L7ipvbRt7OCsURR#Zrp27+xDjNv2bwU>taq@i~D2hH@ZaR7&?RF043~W zv~Ey7z~pXcupZo9Xa}CGcin*9VeZS#Teqk=jS&1I#beX$+qb8WMex!^g@>P<9kK9}2>YJw-e+Zv zzBK+x;Lpy+mLcvZHg`!AT2)^UjOkUA_h0+aO$uq+Xy@%8RQM7H9m>mQ%batRn|?zU zBqZY?4$rpw`E^B;f%R0l!~+ZE_Iq=2Wn*J|TMSil;756oIG&TV zv;eh>BdjZaF0RqB!4S&d%gY4w7BWnJ*T>t!;X|u9s~qy_w}Xfyk*ogM*}stS$X(>_ zV9o-;edF99sWxR?(Qb~f-gy;KQE5d*4WXg=4Ixjv7dy#X(uUO`pXs?yUEY)F60g++V&6w@ffd%@ zBC)2REjb7uqqI+3ZW#Wg-ceP>wE&{ArRC~k(%HO=!SR9dw{I1O&E37ddYleowVC`f zwDe37m%f>kD&vSXbKVI#r@aKJu#7FLY(Z72+#{QDjT(1$PLugoZyEvD)cjQ8+Mwo7 zeV_)mUi=Md5~Rk(K-*sm!!Kgd_#pM8ZN#M~Ux8|R#IM=|k;<6qv9YpiKx{1gA;aS=K3*`RaPeZ~75ad0q@pZ5oa_sV z6{Bss_2H0g&o_U_LQc-c)Rd4)U;pzMz*ir`@RQ!Y*~ec)oat$3xNV;7WoH{b>@*E# zj1db9je+=d-W2JHfUV0D%ab)&U0<`~RbHcH8qj8B=mXRNt(kGAJVbS~684%%d1sp9 zsD&0+Hs$lSl}0C}U#L(CxY_FHkekQC=fYp+jcSSdPSU&4TTjrB`r05M$LqF#=(z*H zI2R4h3Ng`5U-!rygvnDA7J#5?naNDS{W*n;s}oqj@e(& z`Ld3c=;4Gao13qss+Szqx@LQA_p?J+v9>2F#lpg2?g7F_GS(H@2mhY8Sp9s7)mye?}?DPOGqH#Cm@=qASKm%dHU-cu0&KsL{V|^;sV16 zmSTKLj}NQ|@UC|+A2KoB>;qRi{PYV6BZ9oq3c$R@#l;C$ucN@>R-e;XJnuTCH1`h< zOjQpz=-Jqs-L`btyM*A(3RWSz^Y@g*7cjJ$I=gcK4RUk8gP;6RrQP1r(t3S;oq~)k zEkzr;_9+oTAras}JU?^g4vhSu(9js`>dF^x^s?UAh(vGOUEl@__X;Iot6*Sx8Op@; zqrJUR0Ztc3mwoE>KdUsPckkZ0LryIw{L1BbayXZYI2$+i6$LRh!ClG?n%locy61;& z@)B!U(r-lM8C$+gPrvXsaf<)>n_|4;{560l!fKhdS{ZVgs5`B`m(qrbaQEdsjk0BBE$d?w3g5onuDwgoVfX zLd8x;%kf8ZllwYL1bpy&m`jWgSTBAqec!xYn2DOb;N^3CI9dBs71zaOuc3;Q)D?M8 zXM>J3rA|ZRR-K#M*jT=_G%VaT1IH67EmA#g$B;}>-+VMt`D;IfrPk_&^EFNKUe3E2 zeZBJ94ma<2SJu62H%^c<9WJZKRKH`>G@D2z+Hbx-JK?&|ev83CU!km1<|pnF$so=h z*ei40GpNi~R@QZ9!wfFk(`1kNg)Vklp~onG$kdMe=)Tv?IEP_XgvX-Z|G$85j0x=C zt+gIy1_}_C6#3Z#z%=UWAOUTFMA_ZlJ@Le7RvFa(%KCZ>(vkp$qwQG{SdxFt48ss> zfF*z$iH*-L32QK*!1uW`m{4m^HpkUK9y0#$!m$|4-l>TR&Qiq7%=Gl=h|NI^1Ua=L z^oe1A8KljOj2su*Fi7inIWgOl@4tH5+@{@>$ccu8)rf*g3FS8l3Gd!hQzHou$u@je4pYJh3G&TXG{)+b z%Xc1L`+xoQE4~uB8@?OPl}iiK&5y#W1k*0*C(#DVye$6%mOJbFIEUrHT`VJZiBEhGpGK&N`pE(Gg#+z8CqJ(YHOzxh2gJp|Dn*t#AC7N zYWsD#Z@y`tcz8|fXxkJez9qW5M`j|QmY0y8oII<6mo|-x#BB7U6>Z)vn17wd<2*L} zeIm<%i!en#^n4^QnUo4o!H1rVytXnWf$MIq^uHD8pmVYS5Mmd#8?Q=TI)1OsU zwfYHik#hWY6VG3$g!K4z=gH{pypPr-L0dE5=GqJmcx)0Zmhdc$8) zPR{w&t2Xn&63zJ*U)2{cl&il0aw4c;i(L$6v)1`(UD43c5S*So6pY~GC4c>{kI0sU ze;Wn$#~L~wjbaKnM&>S%O%WC?h0!rF*JmYoB+SH6q({M&A-GO@HKAXDE2m=#CM`ZV zp_~teq{z>oKjU@15xjXqZkwbD&pW8+OOm3ad*8F^S~7{hz91zd`?>oHIWqF-^@SjH zMvcZe!V&``1gzuBUr&&~9JhFIW!uq^8FVh=lHMZTsFE@U29nR72{1940UVqweG1+P ztl80#kx#X*xVX3q^6~^{wl=G4YnxD-E^cgZ@VE8N5~3iVUznjwF%L}MtfRFR;pq$`3Y>Wc<6k}2Wv+s>$_HyT7N-q@ zYHij1>^GB-@00`5=_5V;#1!Y9=+(m{jsk_u4+=wH$8Vc*_UG7L`ab(6lK}^sr;d(> zIm1lf-WEd6MC)<8$;xt6k|z80C*#@1@LfkKeNnfQmf|=2g$ZIL1BPqy-!87vBmg88 zb8CAdh{0o;xTRY^NKV zoT9+H=$a(bBM)~H;+(u~+(UoeYOxq+PqtFOA-nY9gGUoVo#Fp?gXk7jSA#*&2irx9 zKZp<#Hvn6&4rJ{q6^lL&2?+tv&X^Q6Spfe)H5?P!HtV_UkHZ;BSPkhq}pLb6Q)EiTW)!IEL#xs zi4TC*fO>fN;0Eq-g4r-M#GgM-(~EKl>pgkv%+k!v6|9T10*sS$fZtC#U<=pP)C3KF z0s;RRVM+QM$T$xdoYSU;1~AZ~-`@%ShlxXcf{V%flioTf57KO=t`SoL?nXe*q@5l@ zDm1iz=MiF-?D0BJw8dMol9I%Hg(#SZ@ygj5u1(s9Oe}=zBO@|U)Mp_=|aH8pq&3JRvAXj@uZGBe+a zR*+Z5?CYnSf88vr6o*UG_u#N5U)X8}(k_Y_S7*yCX$!tNzDE3TLUvA#+N5t%g-4?M z)D@Ebp`aA@sLXG~`_LWRNaSzuiTmB~X(iebhcm)1JJ;Ls`n8)q-eKUakg@Ztt?8IQ z<0O5CMrP*yi_NGme-A%@=D5BL>-+dg3vJCw#dN;eH%c@g2czI(ed#-N_v=c7_2NTB2+g@pKtJOx@@7k z#z*kVzk%{w0_$aV>vc^*s|IT}2ub~!T!gOZ25+iAi#m%nzsB(oJ$(sM1GDUN<)n@f z9Ey~As$aqxBHsMG;VrSZ^i24uT*gWf1<`j7Xk%v>@W47XI~;3 zq4MD8e{W1@!^BhsViLcqhzrD5;4+xFtA;LQ+Jzh1Dz@?*G*F0twy!9@`}lDgAgwh* zcRYVgGcz-VWX>Gu9wtn@?&mPReEAZ1vnoU7>5kssO<1xMn~o-U**P|l_uONj_%8{_ zbQmYIJCXp!)XtNWm%t<{%ffjd-3uEjIW5f;e|uC;S=q5aT`1Wx5!j4RpFVXme8d*} zcO>{O@{jT%8cpGlCUg1U&u9xjgHD2;S+D9hxmL5+WpC|iC z+@_;mSl>Exj(fu5>x)K2q^_+k^+L5!B_RSGF1TFT5m}t|ZKRx@3N5QcQDT;3Dq^88EpQ?A5``Ssm zwu9eJWO%#TsyH#fL(%Kq++5bJ3vmR}v!OwbghNH;#h99=p`f3nwl>*iJ*N$o=&L76 zx)fd3T8_`pdR)#N8pa$@kdAz_jh=)n)}*fpKYag|ik0@^sS>rgle=5MhG(OSv1Qb2--zGQ<79ycMNVZ*E$Y1 z_C4K40$4K7pG#CFL&+WISq~L z9T_>f>5h((vHrQq&c509;bEKK&$CpZs<_Q)xgi!j@{rQ>^_gTGnMf)*ns72X+SGvf z?IV9TC#N7v2%a4s9zt#NuSCI192p+pLB-UM&&uLtQ&g8B!*BS?t2PSA!sYgs?y;!w z9y1%0P^CjwVuq-g-^fgQy2GcdNi28xuu_HbC2|8#kC6drmX2tln6%77Tm-W(15+A# z{6iZv#9X^4za@MUuLiBlt2a4{g#D}QNI!#}eTwe(e3Q8QvF&i(*ZQ3DxoUd3DYCWf z0yIm@s%Q8#XC9tXL0@s5S9p&zu;@H8-xJ>zP^_s&Z{H8x=QnSnHcg#-SIO2slxx{ zvE%=h$FA>|GOqMz^f)@Ok6{Dd`{Tz~^Kq$veWE!5-IN-nU~@V@J3lZ-$p$4=PgQj| zoRC#?;Q)k4@XRZ;sshP;Lv<9=1qtw4^b8Dc`SCo7R7L_PHg%1>+MMJGz43|U-76Cl zd3SpM{+^zoM`Ug8p@D8SwY4vK@@3NyC+-r$nmA}+3bD`&uN9z zcfUyd$PVBC_38e{iVBY7=f(IQT3V#94JAF&MIcfgAn}!2Vm1=A@_1S$X=?wg_NP%_ za@ zO^u$&2M$`=)1>uC3r|l^ZS98GeP1!Ri_b|(0xxkePeJU^)t(f5A!qRmJ;wGasmyH5 zZLZ?;1IrV*%76TiIxpDq^JnMKoxKrA_Nl9?YAP$kWHNH@mxXozV1AaV8K-g(z<+&% z8qVWOh>oMguOL5a-nul}->b{?Y&spRZv);7Tg_*R>Un6HLOaXr3e)!TyX#WA2H~8%qP2*vp?lpv&_)B$nEG@pH z`n7Aux*apdQaQvqJX69#!U^H+wl&6JRe1Yt*RyASr~4Rbfx^n;r8*yr^GfsO)1fal zu*bTfJa9*`^oUO7w5NRg=a1;V=bvA$yx-ou{@btr5#X0{p)MM$%zN30mORh^76JoPCwM}C-RY( z$8CRp7Y!3vtJ#aseS7-Q>*9o~J?6Hgu&^*pOZcEP^A14{mou5ZzP_!DJl(^GH!@J! zLi<7zrqGeQn^?{F7#YK45FmPa%lRHFDk)jq2#&s5oo$e)aET^;?VDep%`a?%-H zg<(m!*m9EHzx%xX$S%;0Rfjn7fd>?Qng3VxDP;&cJyuY_i+VYs3`d))s_OJieVI2i z3ro-RblQC<(`Q##S2J5n-*F}CRe_<}J38_m+()7g{C0A3!u_jzK7W0c^!9CVMA7$< zWV_E;-2D7q9je8;KkicPA2tPZ!Vgqn6tPeeA^l#U(BH*&L14>Af)Dv zO9&+^4GSq9p#On5vM*$1YcWJ?1~RzD;$%NyVc-1uqnnJBR9j1nEG-#aZ`wm`X$@J~ zDe-&1%+KAZZ1W9l>UU-WO7rrMK9l-RxLc7ag*= z8zYH@5r!ESiWL@|gL!>~j~nXH{KXd-Kq|@%$jsy7#ylraRW{ZZYA}BDCaAL`7%So{ zSwsZ>&09AESUyU<7K$ZEA8h2aIdpZ|wZ#eGV=s5NpEnl`tE~7>c8ws;H_w;udY^9& zf@%M)*vxI}*J3UA10+T2>>MmO1Hk^vR|P|;v9a;e`)>+a+Aq2L%6n^PpKQpuCDmpL zCEB0vjeqR1SL{Dw9_)RtByKhML5W7(?Kuzk-H^utaw}i2F3S}aSxIYiGUuj7OBxLF zjw}(gNF+kk%+6RQ+Y5cZ^}9zWYG7t$mM~twijti7Cvre+r5f}f}m(ROTZH^I$s>Hlne`CqwNR7ShH!uQ43@OsT^cFX(s zMc_()e&}Bs@_9f10j5zt-WT#k4wfE!#aCA3AKz@%-+2D}yX-3s{x{ddt1)Ch2xV1Y z8_(tz;H_6swEEaEvsT0QFk4m-k4%f4=ESBI@RLNAA4;uRs%Vjs}jn_9x_>Vxe|tZSx1w%*5VGAak_9>JzOT@RC{=I zWS4IAls;NiR$mk&30EwS1Jx=Zb&7$QO~L_572O{m50_bp)Q#SOKtE=yE^eDbcN)Zp@->8k{ z-@k8B3QBmh28%2K^bTHGrA$$(pwk1fTWAFU_huItGxOUNQ&LgEBolv`{|@-83r}BP zUt42ANZtb2SF?UK(@X6i%2>!}GxpEi8~UmOARv@2!G3%GTC%d%plq$3W!fO{{`N(Y zaB$C1-U;>6)1!>Cn1aY9IT@KTes1^F6nP9=f2;@MY(UM=^aK|lp9{F`g=doRrd}>w zVD!2@q7dgl+e#CdF#SwaOa1$Je7Tj~@QcfGMQgcXliQz_KELf*zl$}MsTvEzRv$ZY zqJLE^2p>V2V!q_Wlz|)=pP=8W3>P00MLDdmm|yA3%*~hY(7eefIFczjx6qN&jWNUQGYR?-P^}DnO?EN@N&-v#gBQOk^z0@l(+E? zFVpy*+}G7>Ih(MObyXPrk?q41_C{vdCN|5;4`6- z5ie9GchBsOGJ`rzO%Cwvsy~L)n=-cF)$sjI_x4?hvD$A6orK1g?FGVDgZLfwsd#)X zzJ?8Eny1yHL~^P6zn{bP~t*d>vz=4r(ZO9S{xM};8}6}5Nfy~WVT8z$uK_a_hWI>QserB z>|^{M1XQ{Ke}` zF5ic)a^%R?23m6%mT5J_iYXioCjJg=>r%Q_-=2u#NHFKK;NZV}C>c6mH+z_+nOAP( zktUf#Ap{2xpEC9cR?^L}_xT$3lkjV#3OQx*{PJT`XcQ~Xq{f=-voN_><&mC5QE_>b zbF0ZLu}{+i9ywm90^xKOi0S_k8{+Tc=HK`%U&8NbPaG^x6E;)rHxv zHK`Vcwg#Edc5R8eVi2*^{e1rH z92{Y_slT zA@VsZQ^dRfTNkk*%}9J9iHgd=@X!y+hJ7a%Egy3;b9TeVXzdFZ&J*m^gr<3tH{PuN zq3=B})~Z@}9$KPM$d$B*-Ktt6cz*GF&lFs#=eB0%Eq?Q2#1WH&JE%yIf7e1I172T| z?acqXMA?6i5V72kNvZnTza>&>crfBT!i7kEY{kq}*5q36arn9PTXkzSIi0@m;oXnL zjbG=`F6nf&iobTWxJb$#<(P^~htOJK-YU^*Bu~39t)TF`w{$}k>mbIq8lCR-%7A#D zhb=2=c3ehas};>kcS1K|(gV_4OEQrpAuqTY7@WT%<;C{+%U-@f?NUukPszy54!9d5 zhpVwh6)a=e;$`~25Lb-8Yd^*|hl`@s;(p3f%Vmo60ICyXL3<#PjerHddPl)a%gG=! zn~M1lBlV~izjw4fz24i^V_wIns;;Km{OMjPxL{Rr)VifoLC2r7((U@nKuk*0$xDTq z8GGy5J*it+o;D?VO+`huwbrX*#0o0H76glCN?Cq(`c56_ZmHyob+&E~gbGISmo~EU zmeVG9wlzp!4JJ~w)(ZCW@xefSi>RN21x$y#K~dhS$^ay|lkk@N1s%~{cFqQkI=k5YbMgUQL zeb`+TXa>mcFfcH9V9OKYo&))>?sR7;Rso@}qJlyt>iA1So?EaIy1o-;NritTSJcfO zKL6JjCsidSEXSn9Fs1=U0v(1oz<39mAL+(9%EpF<^MB>3V>ma+5p1-yH87ZCH67mh z!y`{w(EE;t+`9Ydaac$Q3lr1NpFJhT#a4gTAoas?=SF+zeU&(jkl<5H&bim|zCrC+ zxRehW#L)MAw*}9lvDurmaCKy4Tq=`0l*BF771-?1(4=^HoGB9eSEx zd}+Mz-{0SF!07-E5O<3h`bLW%bWJ4vSvy~nH$A>>uw0sX^#(^gL0ystT@-g8-{WG9 z;P^TJh%*YWp5FQK=IH21;4t_Evny|C#X0-{wtEuzCY77ZSVvFty#KHsVTsN6rsZQ1 zS^ECxD$^8CA9#@1ezqE8h;(D4Ot&VK>RK?fu|OChq5AN5`b>Asw6ocWRm#278!{`_ zX`==WibX|i5s`ff<3I9PLPDbn5E<2X(ik(cL-8u+h~qMm#cwX8Ua23ljM-XC-N&yW z;{FZgT%}&dOxhwnO1_&BE|LYs`sdE0cr{ou7RE#M{C6c1YhJyQkxt>Anw#2p>El8) zS`U2FV;e4&9Gd9<#VRMyt~8vteI+GtiSf!!F(=G0wpNz-&zL0&a=Axi6<6~k2fOYY zEn#n!_8!eX)L zR7M_$KGYAPFlasJVrKOYN3DBub${|j$I$u#d8@^hyetOt&_VAj_Ola#FR!Bzmijn8 zPQ2#?#Iq+0sz*O-ZusY)Q@(0PU90tr@{y{czhM{m;{H!c&ljiNt_m1s;aE26_EVX1 z$S(6y6Hd}@RgDK%`*uYOTjz^~6Skz{cKql5Ig?UE2ZyXBN*NYB*jWGLY@Zc?KLHaL zL*XK3f-xwxK}8($0-Erc+P~p&1s$1fYhd;L0m^_O=kl1+a?#=q8DMO zd2#&`gn$7vs7iT3&&=EibJs%01g7Ruv%v>5?@J9S_*w{Ym95Rq-XaQ-G(=-cWNXUP z+OIv`-5W5CqSmwl4GD=nWh`*tHo4R!Gft=OND0P9KBxP~0B3y9EJs1YOLoxGwo!k5 zp?nHCB~G8!j(3TWt z`P)7Xr|sFKxc(ZjO-&0UEXS%2$%wkM8N0fenOJD61nKCPiPlJYU8NplVKFjVXx^3b z_KL}y3@0$yjYvDcVL>9GtLsgu$|EQ%sp3=^+;NY9;C{znvYdh4g~)d-^Itqc+V~68 zU|`hDpS2Ce4E`60U}JXn$N-_7J=Z`k2|q#WQtgI5iL1zo7*5 zJ{aY97f0Z&zB=eq;D{qQ>CQIKE~)o2W9Hc%_XMaw$ZtD6DwnWDwvY^@k-5(aIIMHd zD2LziX$llmaYx6-$7J+dKJ+2PD*L~!}?3(zpkUI z;5&GMk^GKF0eKfRA8;uGUqV#W_+IwHER@sxDu}4q?gNu+UlZ_?&>WJQeN+=d(5?`-zc}-3`7)`YGX6 zO3#=L9+CvXw%(^#nc7v>QUM>GSzh;byhfLPaNo)5gEIM?+1u!>RJNxV>Y`$MdST=0`CrgdlsFunmF+$(_d@gQx??zBQ)+e-3Vo%JG+0!uB8iCgr zdJXn+<&6Y3g^{RhjbU+eCQAo>RL(c2JlbNhkk7B+zAT;7GFnCrBFnO4St8^}5sbMw zIsFMN;cSUoZJ7lS)b76=Du$=-GKl{dU)z3rXKGWf@xnD=JSFUN_%6MhG)F#l!~g(?qbT!G?Wm6wOE%d7}CRlzz$ z@61<-WxJnH=r*Zb1>_j#N!a%c;QjA@+&Kc`}rei#FM7)?BBd0 z*zj5Ji%ZeLjeC9gIx^bYdOqQKZ{G$6ekd+Z)rg|0{q!j&BO@p`5$02+m86BiX8mm_ zfPXR$B0F$fKyQtV$m{BcNhm+2bx`)r3JD1g9vzh_Dk`ZqDw5TAYi|XgJyqB>HKuH< zXt+QqBV4O6C&#$6+H8sr@6izI!Tzp?<28-j9GxquYya(D$!W;pDXX`WJdr@SdPuub$xb(hcY~&!w z-;4TPyLOIsr}_{5wi%yXe+%wxr($wA_n&t#lJRG$_FDaaKwWRRlK`NBks{ zRF*-L*MCsPX7V6`r{`wq{_mouB7LZp?~`0gtnqZcd#ChBQwHZ|YP3vxqAs_+bzLE@ zuZhOLV%p<6=j*Y>NvvVmElds{fY<;I1Op3LCm40`j#_&W!@U~{;)g6O zrcqW2X=&3iQ&$kL17)_^ybtsyGFvybqxE4%Vc|0zd&6!}MZiD>x$G;lL{mXw3j$aZ zXj6C2!OMW?s`)wZkergz4uHppSg%mBbDI8*4`vJz&0czGZVsFapY1one;^C>UuB-a= zDeT~ti;LSUH&>TeZjLU2t#-DXy?tzer-0CP6P;d$iD{!eDEz!;H(N5k9V9`(6BPaS z+U5bR!@qe`ABKZMgv?L z+ILCTVhfZqjpD3S+|iKm8>}KU-Q|<)>-g$8JyzB`x9DDHbmV%_g-1L$3279)cqBD; zHqQQmYEp%=`U?$JhAxOWeV=oasl=UY@C}1d0kq9ewmkmcN(;1e74-+O`Lo+maoIb* zL$&OzYn%6bxLE}sV-62L#A2_yop~J`Qh?L00P4p~orv5GtTgCB(VuXhIEfNzUEd&= zqVW#E@V5`-`@41%_cP~pe7Bu9XT;BIU$c|xYrQur?M;6@G@?}cikv-mY`+i*Mq4tU z1)zNXJ@e%eA6jiDy-HQ`4+_!*alfZuLY;($H%a{M2|l*uSpWT|_l7G*k_QbLLnB6; zqv+^))jiXel0M#rt4B%J-tClpOCpGTX#Te~>C$;2(pOFD%w}w@^^ujQBF=)DWh=rZ z3dAKd9+KZbF7^k_v9r?>_B0xtY1+1(Ms3V|7R6C%DdtQCQfxhr-V^&Y@EWrVQ9Q%c zL0j`d`*YTWkNiux3FAFb=`_pdrEvm*Hdo#F;}=am;250yo-Ies`S?LlR5=^pMVi6d z@0fvj^9I=|J`48B6dq8oY|k!iSC+6sHI`SX?%tZk^ZZ|tNd3hNfNs9WmM};IZ9{4I zYpe{tXKXO{nX}Va;iE^7GI{WWemw3)xWEl&XKTx#m`*ya1HqY0A!l=J_qwoY-A)1SuL`|fUTeEeKclqJ5WW`DpetgLK_SoOZM zk59Swd-2s(TS)o0{D(oAyk?Qbsi_^6#>&p%G?O~ey$Nk;2>|jK9wBIG2zmD|CZ;Uo zQ%u>rcYLHiH|DLatq9-Vp0@}V!=V+yYtv>Sgkk{13!kF0vc4ZDj@vVy$(}%@qFt92 z7f)52!Tjq92;P%xiwQaAIa~A#Pfad)5(jMTlMz$yuPwCSVlDp{tm1Vg^AE+bb9#kc zqj2MUELnTtTUQnYp7Ygc>$RKNofT9?&PTk5o;u{mmF#Nvirb5MFU|Lgj%sVi%MIGs zIYzzU;pt4T<3p?XR`K*tAB!e4Gnyv!lG>8PzQ+^TZ$pn75_XIF<$=T(KoI>d_g#;B zwg2;uHGl^o>S>^7VX=Xl6Vwnd-z&C3MpND+T5LxqVo+q3N+HXGebo zV8nUmxl|?0cWC_l*=Qir4u&eXE^i1$lKE4^WXU?a>8U(#%A%?&C%`2^m7HkP=@=Lg zhcp|UvGv;QGwv`E2!OG66Wvxw(Vd*0ek>?3#lKQT;eB+&4yJ28JrBBpjw)8iO@cV< z&kzCYV`7Ag*-{#WAh1!4dhFSNv2aGG;^h}k&C6NX<}&FX`HT_u=9_tcs)(nx-b=Mw zKZ$rK)u6RedFndBDPz1KDL$Jebn{!l&?@_`Z$1fCkVcY!v78L>%SX0VX+%IlL|g8W#>q(k%` z5+_1}f{K~kM}te>q0s~)g$AOjFjx}@JGmTCDR6lL(NOmu;qWQ&9a$w7Jo4Rtt6F)a z`@ibwRs5du9p{RL^}W566t*~^W9>wtmZnBm)3NKJ+kI423-}f> ziIV2)%H_|_#57ssU7wOrRUNqaTxr1DeP>~hxY_7~i2Dkw-g^9;cSkr3^{G)DQ2l#% z9MM>mclgZlE;Z+Up&N4QdF9`!B~(pKM_bx!Y1Vl#k$16Ay)`n;OD-p0FAv<7SS5iZ zMM_}1HOEz7VDN|HCTDToRP_t&k$pTn&>xInv83?hL~O#!mO7&tyB#lwz$?yu@SIt0 zOBxb-ImmVnxp)-PzJwQ$hU3BS_(lbNL8CscaZ^$6TRwlx(R8t-??0YVn1+;#;c*yz z+7RJxpyL+#nLdvAgpezVrKP46RIl*wWwEcR;+g< z41ui3%?%Resw1MHINzEUu+6tEWC2qhf>vOvvr?K~L*7zISlFP^<)%ivvT%#kLJD!_ z;;#T6K0bBu47?v@gyPe*{L5n&>{_6%nKR$q2g+Y7z+mLhL*PQrZaCj9K#*t3Yg8Dp zfoGvGJNQCPZKlc;s|EECtW5D|Fk+~(u5N5_G$uS+=WX&ApXK6@J(}!VZ__z_$3E{7 zq6I$In|%}=qX1Gp*x^u5y&^}PG5T&Z&s%uq;Ck(k6cumW2!gcl`=lhshfI*tvVs#G zq=dDNfc1?H0G~?|pBGzOV`E3gzmS*Ld-B8uc`rfo!H0IUmj#`j&7eELd=XB8Sju&@ z?{^}Yt*tj;v#8atRX~=iC#k~!C9dJD6Jv5OrJeWPU?apC37eefWTO6Y9sk7g*3 z+h2lzYzH5LeB~z~jKU&!Pe(eEAY`u(DJVkQ$Xc+=)Z7Ym>=N z%zL9-luQRs_b4_RvSLP!ADH0ed>jO5f5AJ;}HOF zG8_8SVNl@;rZ2kaC%2;w{v9VyWuCd_tlOfuZVkR*RLI~xm5Q51xl$r_2iB&z*_Wfl zm3P(f-%2l?3>OVmROqBXHIEf1idSRTvi41&W6q9vjD#5(o0d<^xx!*3jlh7*R4lMDUoF6Rl+C3!Clo)vM z4LbPzL(^nW1L-aOSNIVws)Qdupf-N$J7NBjt=~!{&2U-u(aE_w?4d;3xcJMvD^a&3 zNE%Ogznm@K>Vyk3q`T_v__i{eJ@A>nbN8{9Wy|35nIrYF2DX^|9Uj=(j zW?O26;+54kEhGftc>l@|%@@;&zMuhOX2J>=7`U=qAAWY&MeJvNNKCx$ z&UJsX#I)7zSCNP+hq~QhswA`ReI3_=&yo4hpT{RwS0mr>i|h+g3RIP6xn%|81l|i5 zT;tHoFJdNq1cH@ET3W<^1B>9KZ?GOvhF+2-CjR{SGrpHDr5#K|zWa2`2h>rT)2e6x^{=hP8Mwg~aaf-+#c0Y*jvfxu;-p8!A=o zJ`E1yjd8iPH67Dz2|YB4k@8Y>!$GETa;mdwbbQOr60gQWE@tPr?-6?{i*zFzOQe3R zQMt6QjEIK}13`Al`WlPY`9xQvu)t9zIHUOGHM0QX{dXTeew&i8cE!U-hO^Si23zW? z3BkQ8Ej+KQUJ3EK)1djatL;fI(yaxu7nTsDE3!6vb*>PkdkIAh5 z!!b^xj8yzrx5q_lCS88fJgk^%BdL)h|9qmX9W?$98%Y5b6X>X031q!Lg3rmK^$>GF zAw7R>Y&(g*R@BiG-O)2X-$<|->KOKNAi);yNR44>72{L^uuAMO&{X&x{z>`#`7?ay z6_-Mbfah^=Z~%2Z@G|X;KRosiejD1x#4sFyxf<$$4(QEB3Se$D)Ys>JSRTUc)A2GZ z%UnsRtEb0q8d76c24Mzvb9bkO5E=p;2mJy%k?4>GZbWr?c{#iv=re)>0+vklm);1X zMkpr%0V10;uEVdDl|yjr{z0Qhw?E<&E^BI%y2Ejx8TJ1CB)I%b-3s7B3eMPl?d@HI zG@X5lFE4SBm)F{A7MQLB_;F$(f8)B zkx??TvPkGFu!yl@=5^YQe5K?yfyDK^0u_Xwu5LKC0Kaf$c{#XcVaQi>cK!=iODL$Q z1vde))b}_h84Y^TOe065)WU0t8icO{nwqTGG*lq)7vvs2vGnJ^zy87jA3690w|=xx zT)(bwXmEayf9?~)6JO+=kGwJgn}qj#kvCdM1VwN7-s)U>#*PwlWqIA}bb{xPr6>3j zZnn2u+FOpa&T(>cTdySst{!g2XJ#HbTr1$ScuP)hwDogVj*vQJFlxP2b)*rzEpWE72@nr6ud2#psOF5b?iZmYy-5*`uXa{Uuik- zM^Ei1P@oWk`L)_ZsQ%@}KH@E!?g;lNVX#>v-sJqI*A; zU)1WA05Hku>j>uxx~C(SDCt&PD!+_wt znL2o1nhQ@rq}bEb!_LA2ZjwD%S%()snzB9YcbRK6(A6bX&z^c$UA>>98gOedFievuUn`B62100Ue^?<6&)xhBLpMv~f( zRJ|SgE+Id1zbLG2Y$8=z_KkENKGeaDI^$;J{rhY4d@_j(fN^z|8| zI-8@9tAN~q@L=DO=hJM4=~iFzf|WuF7~Sthx(0$aj>k6@*&fQ&6DXgU!+j z&EA7N0gQzt%dlhnNhE<8wRsdenT zAB*iff{sFIJ{idj$HZ>yOQoiD0*WQthCh85l3zya1yIXiB3R6^dxDtm8@_<}CJKh^ z-Cg(O`Mtf}`Q||X$C8qGr`pRjSL}w`5w^UA(mf_>EsA04NiS$XP?3I>5&g)1=Imt79qNI zJIR2Xw&~={=-VAVy01URWCP7C)(2g+)%^zaM2nhzSJ^LZFy!9zn7qBI{DSzCxKEVA zzv)bPweeVufWR{-B_+w2i!oG#vR<;+eG+(P6=l>J^fnA93=WALMw_hAw zX+hVmJlg*6%2ilkC4gJ;?c2w8cKKkWW@pFIo(6yk@_}GcC|JY3i_ADV+WC;5&z{L# z$p;}@FuPmr*=Pf0BdRw!0bNfz_=>axHAouppMd1?%L&@LHZ}?gxuI(t*Qe3UM~-RZW1~+4jvvJE-o(RarkyX#*Bafv*qIR7Yf^DV5oqaJ^nfv#zgLT zzl9esCZ^=Y3l=h8MfI64Ur>XCv%t!X_H}_JXM_*>=zhPstlgX;Pk8&Ib+y|qF2&v5 z{r=mxgq{c6`q6=U4|K+K41~UZtE;L!JlJKV67il-7dIAlY1y4jf@kf4fq~bZ-L(gx zN*VkuvO#e@3#Tqs4#v;Jur*x*3m4_DqlY_E4%!=!Xy&X#%-PeM2U3PwI?3 zxa!g?{0kzQFq~wFSg{Qn^7FV(KQ6FGJTfnEnK7GUz9;a4_2)gLgael5Zhxe(xeaR&b*$do;1~P7upCA~es^ z2)E_Wgb_~dG$DC9oDW>?rp9VFos9Wv5IWly$+!x?x7M$4R@WPi+fAep7c61%32UsFz4 z7$&8>TaJKLcfi@yVbd3#Rsy&ueg2-F)!?^OyfF|Cz*l~Lz7O<5-U*gD1La^-Qxp6s z`{Q@i1EHYEgG@uye%Cr2Erh3m!8lZuQlHdzMCxEpwho=0ZnY8-5nZVl;GVJ%5PDy| zLOEUP@2Jy~V|NKuG}Me*-S+%o`iFlvl2UiWdv^@AjmvIF^){mWO}5Gzk(9ly{apXQ z6f@k=pc79)GLlqbjr;c>`PG9i5MoWX>yqn_kn$`q6ck>lY!i`Mktcwhp~1~R^ywLJmBF1jXF4YZ8y-aM1TD6A0lAu=E8RmmOWa0r zaxz|AU#MqvLy7lluhB6a>FDx0n_2@MU8%_hmyl4^-d~eo^bqgLs>|1~LHCagNcx{> z=-*|H&dy;iyT%}Lcu+h!wQT%b6wRJUDkQ?-H>4&dcRe^fVC*|^_F^c#^3|rxFqo^N z*L-K!Oiw;Ompr6i*)ReH{N+Wchq9SxVIg0#WZB0sXKhX`%%`U?meH(jD0~1Wq-jKo zg%l`MvJsRIN0`o?p%3S!5%B$p+MgDe0GRT$}15bnqL%3S~qv>*A0pP zAl5)rFS@m-`Db^C;kSA9^iF-@M{Z?Rx~$Ezvj%ijO2Cqr4--FY_K-dzgX2d0O=X9( zee-W|OCS}l%|I&IY&g%}%P^3HgC%0%JoDNaX-F?vTUcFUU?%3Jh{zCmSo`;{tjdCI zI^@1-=KhjJXF*Y681!iAIZQpWXGT0yWjFs(w5a;1%)5`uRO3*UkO(DRASuT^iLW91I*kw~^D(;MqNw zj^G67R#DjLQF=>@<q#sAI(z}_#)+} z@1yDc`XlcXc_HCjzF3ebA{H4|Uk{06AU8^B-5nTi;LqNG<3qFv7qR{&A$uf8z1s- z8;l#wcmv`)^Q`6zd+f7Q1=+WK@JLiea@L|DX`<8-LnaJ_NIJR!Ti!v+_anD9#?6}0 zmB(dOxjeT!?bNI2jSd{_4{IsM|363;{LVs`IPM>Wj}rP)p5E8d5%c*=Kl!xg2LQ;0 zTo+&r1zHT)^#XvV2p72tBK!<(cSY~G6XrLJBB~3sM%!<@@K2xwE#VM2zcZS6S@3o68Aa$voU6nz}EdGuuDaoDfRlY zH<42gNd0hpSONo|dJw2kt}$IBrIZoB;|*w;YcBm%-PYd>Nf@od2pfWkzUZYbQN!n4 zB7dWX7LIBJ2+uAw?MN+X`?8D?rY+BI2}x*Rpuo4#CVoMvJ5)()fpQkfAJLS z8+0Q4^e`QKS2cU7Hn-Q{l9eQ|YjwtCSvXqs686=68;j!1{QDOU_7PY@aSm?)gVD(r z(&BdB*jiQwF4U*r^!4oa!Z8VxLv(}cT;^?)I1g)is2Kh>Y+(B}7En~mP4 z{%Bm@N|UiKu-0+E`V;-gAnfE%2xG;J`F%Db&8RWobZ4tM<#=c)&~BuQ$C!8$;YA^6 z6lLw?T;aI$HnGm7!R#`PmQMXubT%IR4@tQFIXe6_^N!C{!gr|?TEZ@56#tYsAtSv+ zYAlkv2XU!c%Za}jK3G{nYY9SO>c4H@+JURfA>qUu-bdewus`~Ec(UopVH3bc%~kH^(m&wR-#hs2PH4m)z8vT&!@}+& zZ!XAkh5Y5wZE^PSpb1T_!b28&GxQpK&Gs=~p_S_>ER@;N&b-%?rV+HqG`-E1d$GO; zI!Gys^GoWG(&oUe;u@~>{;x(VSHD<8ZLP2^aj`k${=3^L62%+|@&OKhq=*x{>uAVe zytZ=_FvtKd1!WxDvv2f(#C{4DVS@_)DnHcMivdl@oDMa;Gth{Tuwia)9$`!hC2Otx`sIa%FBHw1 z9jN?cYZmqeibEhU<2eRSIZpX?ZY+?r=sK4LK+;TB7ai^&0Wm@^0a#z0R#0swao0Hm6A4{^c*B2L| z@_Igf)+F|eyePrzE>h{oA7e^P%mpff`29M&ygWT`vfRutHNocM?1#plzCPwQ(L~5H z8mp=Cdi0O6+Q-$+%je(r?njI(UlJbZQVI(JaBrRfa>KyO3xN7E;=i)|HcPBH#>Nvn zl=VWt+Frf0wNz~!| zX^-Fd+#2w!G|;~829xvDJIROSSwchj&59dY-V0Sqh_x(T%#e#Xg3O^E^`+g86oJX(=y6!!R>j!gT?W1;o*1 zK$k#?Q5Epxf%|K^1+GAGi3MI=qG4Pq;j0@l_4sW=A$a(qY7SV?+uw zo=v^^HK}7LhDJ+EEdn@tW5eMO+8|al(H>SBfdSZOUNl;gtf}x5(+szO{nggJa)6xq zA<>H$ieuv<5fLO=lEJW253CFti;38t6pUJ7geq?SVW_V00>bh1V2h8Hn+2u(CWYKn z8}0c88JV?#2z)IF)YV12yp-AVO+Wjr?h1rP;6deD&!|;*Z*PCjzLANE0>nou>gk!8 znLWamd1s)bqjR4nzOTrB-|osAWGoqTDuj<*{9)A-D;w~iDoeFVj-MM-q^z_^E%5RJ zf^C0Klw|OGrxzzw!M2>1S?-t`-*WMyVQ;?Gek2U0r=oj1UkmXTCC=iD9J? zS|ahZoA9uQnxJj7=LZf61am3QU<8~q&bG#BwwcV~A$8lx`@v^W>p1(<-Y!mX?U(wy z_8OA3SSp8{h`_R<63Dz6>g#*n-SX_>?4sEptVWT!R^K9sJZy*YNr20E+5`DT6{y`XkS;%LcJu;SBel@ixF-L5ah@Wiy*Y&xB5_c?kt60TQgt_7 zzDx0Z>)wx9$yq|TGd~JtM>H-Tq?uH<1-D@|zJK=pl@^*kC$*Cgr3402-YZJ9m3G>b z+B_H4R=l{r*G-b8#@0P?Iq;=9D*T8>PS4@D+T+!gms~%erjD_h=Bi8;3+M_}f;jow|=l z%5rzjkgTK{Y+JOnsq$^i72j-!(?xt72KNP)*^q6bF){Xl+M^+)Q5!1&HA-+dvHJYWC`M;;!kn(FG)e^mKPc6e@h zQE0GD-w*wmOM1f17l-*nN0+&{@afvx1`S=>Ln9+e2;yp#^*=_f3GF9EN6!GD>b*5D z1mPF#+F~9j3y%Oq>+0$Popcl)9?s#GIR%p}4;!2Nl&cSwA%UI{hN<)!UyZmRIYx)q z(}~JfRRjSkB9-k9+CBg^7S9&HxjH!XbHexlNDa@=pL3j-S6O*Dhnpb4ZO_N?@L6Z; z?Tx)Z-=Z|iKHsbMG38sH2c}&xK7uHz$@e@Wt%{&}bbfjT2;n{Jnn(h@-2`^|3pCaZ z&T5-ww!`2DQxKaLPRqmrjS|}~?(Po85Uw>s5`C#r zgToQ(D(2Ah$SBR2+pK&W9$48<*_BpyEi_?`q(*;|&!9;Up?h!l`K$p8m8)kbaMP0z zP54ar&Qo1VkBvzObd3JxNj(RLr^BZAws{BQ``AM61-QH~88i^e@j4j6NBXPlm;C52 zUvWfVi^v}#&9X5vpcVYf8WPd}zI(7QUcddlii^tFvX^lxLF>4g*4<_Oa=sv-HrCBs zjw5Uc2yf%l&TnsQ{u|Mo58fkuywiB=bD0Ln5fl9$yyu7W6~lt?w8N^q=}M0GkN1lD z?aN{#>UbiDp69`Sb6`ME z41=uO*RfHn9-`=%yCk@k6*ZHnG2o38p!>JF>>(3#wi!AhLRcXShY`pZ)xV!4i0Qc_*T$cUb9E&uZqk=iC+N|Vmti!z@J z)*z+lQpVq#M_^~aP%o009z0L#XRj#sR)=4wNZIJ`H)$c9F@L%n+Cr54*`!v%Vv`5x|(kh-We#9du zD>eHiyg&^ZwohzjSOoEZU}RK_3)#)p2Qlc8Q_rjpr`Oa}w;N&%j-l+gGNl9B$KVsk zw6+>o#u_EjYshUpJPHPIe1xfqev{2N+kGX&vF(?Mbw8K}Bn!`)q>^>4Z@uMI^&RZh z&5{l7&a=p2LPr`GV2JSXxxmNn-pPs1>YFb}pwk|cd%cHAV$uE0*-@XlB8r+f2#}iB&&{zm{UPHOr)Je*MsCui`C@z!ZCL&L(ox=v zt*y6~V-#`8UpBndU825X4Xly0MI%tMA8-*vxeLyCbJ`exp8_jf0|*;*1A~?l?6DlT zZ}(n^U7;<&f;9UbEU8u##oQQK?1=1O;H;Izz4|&%egmrzM5qB&sL;`V^TWL`lDO@I zJ!Ra=>`@D)F8HWDK0hIdewni)%8y7@%er;2PXvJ$O@RTCM<*XvAnbBs?PGC%+1>lz zZeBA(b`@d;`{IzKOE>rI&Bl4mk<7QXDiBBT97SC1j&CEzb zOww?sbY$RzuU;?aL9|~`TExlAJKWcIe6-jBim5HuTc%>c7hhyL4gJ0z?Z6Bwk($~c zO0m-vW6PcFESaF6pKNTc^&bd6GwFP(i6E-8`?ob0 zM}Nme(+Df8QW3t1_(b^Afa^foaKoDM1j6s(6sj`}Gl0bNtHn~H;7QjG@UMYVG?I{n z_-5I#6NKQFaqHfIKSIE3Ce3Y8)F`9^5-2}i&_+X=O9hjPNTE9|6dfgvH%XF~2beax zj!w^_3;NXhhyjk9*8J}S!jr&jTvJ^$jgq`r?LPPcjT)c1`TC0A(G|td+=YDuyhG)# z7Xe_K*cTqkkd};osXy@qW>gMWC9TtBp3)Fy^&cK<^dZhCiJ3f{p(BU>7qjU0tPh0} z-UkdKgbUo~DD6pQL}QpG=^uDEIlCf|#%~HA(h7-w=KD$~%MlWhZ2<9$ie`f{A=o0T zKPkohvfFwjNsy#J4sY#<3vMc?3-Lckvmr3_oU(eh!*|6oiC(4@7i8Ql)c4xP&HXEh zn-S7ndGtmj@bVslEzR} ziTi3N8~oGtGUSGND}rx}`S36n8~YyUiOuHcVKrPN$DX^FRRgF>)WPq_h@g8%dtaW1 zViUcU->yn;YI^bZEty)DF>FEA3WtoIZXQ1W?yh@CA~shRfBt;zY+0CHSo7o?8*k26 zpEOjl>=^&S`RUWBD5zqhg%5(cF;3i|y{+?Ozq7Sbt&8;ZGQ>VaA$@1=GG)Dk=1Gu* zZr<{&Z^RDw)J=aZ(V%3A=iWUfm+#fbZRe-3*Ub9jRAHm6?#22|uKSSv`Q5d*I!YRU zqkmpeCcv_G1%QtLuiQfOOD8EbO$>rF_eujnbieu-h9T*V%nCD)ht{oqYpwQ8I7wPDa@)t9{{7rL>rfn+kFr(Y~NK0cDF?~&i%;3Xsc zc$Mg9RMX%2<;~smpD_qf4(PYZe5l17PrS8#fH6xs@=SLt} zCe*px7*1NUfGgJ##g)~{t*k~PQzqkETZN!K;XF{x9=JxSLRBMGDeu%yG~vd+y=06a zI)5PLcJG0CN?F8&muSKpCd@A}a-SL+944Pj87Ky<2y>NtmZ~>K4kc=308Nk%I5kiG zJR8FE{VN2D9+!w5`=1@`H2M!7TzJ(Bak88UF)M`Oj{d#~#ev}dB{H(q)JL&e!^6C# zl$5=^FG8_ZqYs|ujx-yu@BL%I^~c9mQ6U}^6?XU9LuRjDHLSehKcc!gJRsOUWQ4iS z*2(Fqg~i(6zilKtNZ?-#JG-7h(?;8fQ=90uEjtbv0y>Y4w1T!Rlx%D$M7)T2co;A+ zZn7RlWJp*3L#E--24G+yfg6O86edi_|Io^mI;fJG={T1&da@1_+|oOqy1H7+d*X|G z5Ypr6j=_ZvpcvfDq<8Py*@q8_OBlxc)L7XiPgTj;N=%zi#`^StWD60RNonW?RR)lt z_inV%7PPTyXsCQ){I&@JXI6j4r~?jkfEW2`&))nl3r)_>)|xk2)Lbd*V_u1%*H`OI zm(PZf?8p@g0m=JvZ-T$T@1%l*dc6mIjdFt$hBV%Ajwc2At={7fhaU`mGAuYkaI!!{ z{N#;6_Ty}M`9GKhES(DU81`ZWHovfl&7M{7OGgf)t-VW4MW^!#sQt0GiZ5pu-G4(8 zQ;?JZG55lCa9RBP*N*Sw68kB8%&ZC!Xs6ycHMQ8p*5pdz|J2a*f|nh8379(<`;M4u zcko=Bq_?J*+2DH>H>AJhSHsW6`m1Y177P*UJ6Q9LLUElN2KbMgZ>i-CIu7aQA`Hoc0?Vdn z1`Vqga~8AZTmjw7m9{@U<6UJN8=_F$jAfZCFx_ zCHkw=o_#Z8x9ja*Eh5ZyLQl2r3dQd=C z3k&t+=cmB%q`&`CQdRZVty@yZ&#BDO)}~WaW$RofbGj?K(qKGVc}*vdUpD9g&!DNg z#=fgphhf~^KxuveHDd_#u0>`a(aR|wIR9weRCN;6SX}GvwNj4Dl<<8U>Al+-1;u*V zGi&m)jx&`>49TY*Xu|rt8%bQ0d3mn$%6ISWs5TLZnS+^xSzexXqb0U{;nHN1BZ!>p z>d5E;&dJCVIP%;HL1e_NL*aov^;m^gc zX3F=em1-*R1h^MZ`_!X`T32Rm2$MRZp{94Io?1*~Q{A2d{_6#AS_AUgQF8ZK9ExY? z7^@~qlx{w^a*grv;FfN!73O4oO6SLF5Yge=Qg8=e*{l*lUYFS_X)Gq$AP z@3=h4d=(U1@~Bvqql$Pd^VV&n3R`jl?XzLT+RBUerJV{Rman(S^F)H4_%ty8?+5N=ryEw5zvNU{H0A||g(h4Y$J+n48jML-!(c-sw*LizL4 z=zkRCPl;Ma)v{VY%l;#OkD3(q^!c~HM*;_vh=_m*bhK03y_Tf97KJ0gffNuV?8 z3?meFk&JW`C;;fQHek1V-|D>yLA>ih*cnMlH7vYZ0EbkUQ9tAgycg=CLLA2S;qM4O zerQsn3a51?t;l{=MMYF@Zc>Kn(`_Q!z;2w7+RDm_uV2k7tvRZRjSokV`y=b$^1As6 zZdh14c>f(t8GfUA^+|)dgXRPz5%3ksVU5k^AbH?f>vMFvz^o-oXU*0E>NjS9{yZzI zdM3l}9-x?n9IlhpwJs~bE}VA95%svz>LWyyZWt7cDg6u}Myji;)w;|@v{2kgxaMN+ zPiLH&79LgDh)=_z5GCxqpB%$yUViXp9h59O8>j!Q?y>xi%P*dJ{d6i&BskH+ZR)ds zTxuo0TKC@GUIhnk1&{L;C^J5qzqbm%+o?f`NL0QB1=4)Rv4kO%L%J zlAUrZ@YY}DW$uKhBB=9UNuO!{2Exz|Dg}uy>tGE^6ite{nzNwO#F&c!a&uY-~527DAUNOe4 z)1%Az6yhT0o?BpDKursDEk;q>%0xUV12X6YhcLyMZB4?8=Rx;mG#a=ypv@HscE8V=45 zGQ0mQC7;qs$=uYX+4HLh8N-?41BD$NSr}`RWoB_P zuewERe-C!GkxG2*aQ=ydAQ~}&LZV2HCzcMiPgl(uCK~)PF+`60$k!;>=t&GGK@VW5 zckQvU3Bq0s?e!BTkAmk>G_=Nu#)bwUP0}xRzq5Qz3f}ApGSW|1p6FSfO_g0m(5e-F z_D;a+*o0&}TH06bIaMiEZVnu#7|8FGt!QgfLxrD1;@pYug+OXS*5tw~j)HMcETZ(^FR ztr-5hO_S!8R*!?y)J!tI>^B%2227(4w#E|7f6|brKuAD9PDa+Q@ZJ2LwBJ4?FV}l- z(q_$KkzGyem$;elKIG)!sHx&bzubox3-S9VE+8#Aj~_$EHmc_bJ3byCy^P^ z1t|p_zX`RhLOK%I7l>%X{)=l?Qo2=;nVQ;ITN|6rOm_0UdjgihE(+zWCkWaWcQ-c= zXkIL@P8Iif@QC&so!`Y|2M)=VHE=fau2;EftGDSBc4uQeAT6liYM$JzJ@wgABBm=< zyWh?9k~RPh|J7%B;1{Q7KM+Dg zA*TnTaJg*-rz$TL5sd=3YEU;3cnAjR7Ulzt+`K#iuFpHmc7DZY9@;InG%~@dzY1w4 z(pv`|-*^xq)72?XuGjI5 z=oTELY;z;m+^OJ&kj$1Ka{&Q)=0&p~4IZBt=-?#%53Ls!C1E|frve+e<)Z#SJKDfl zMge?oPCsinR`85jE_Q?}MxNkc9uw}s4hK;plPHwF4z=UflqMl<_K4e#PC@z00)7hF zZ%$GwM9rhLCq~2$;f_)Td8N6zP8Rmow$7GTR==T%absgRMg|@>fW=U}qCRoo@-G+n z4G#ly;Ww|Oe0K?t)}eT58A7Wgq$8ZJ;MKpdwNv1mH@UV=&P0Cqj<w0-fU4t>h-5Jk8UyV-Gi7H!36Bdn-`)Vqu{u8-|I^#xCT=qS!@(piT5V zy%VFFT%?o2R6lK@|L8vYx3Igp#{{b*Jwci;oxpTtOiTBh%*Fa3p#y`BHIf)jIQ(m3 zN=Dg}SFC^e%>6gnWaO7WkI`ch1+4Tkr=#0j78W%}CNm4^gQq83Ubg6v{v-d473^NZ zwpx@PxJOUX*A%j@MM!7mJD}d2#k5N;(SV6nymcWhSd6f$gcxmqzSSeivl0sjqJ|VK z$?(GN>lq9B+8!d50ZdpL?>GIrrzrk*6-h$=vV^$jqkfn+GXlF480m=CZ;rHz4cCS4E%=^AS?aK@7 zJMm~`B3%^Q+d&bxcukv+338%@JP0a_HY}8sPD}rV4i65-L`NIyDFCqIVeeq;Y-0nc*UzO5;?0BY?HB$2H8`m42JlJ? zji1wyjbun}fz)2=EM7Cn?4tSezpK>NMfhvB8-~xv#!uiTz{zup%Ki`;_c1q5M8JP) z`2ombs41k;wNX0J<`p^R<=6ZPXILP}#smf#Rn@+>bDZ=)e^M5;$h`Dgz}^|WF>C)* z6hQ=}Z?d$RaUKRowfCtC|3)kf} zBB$1o@WHFn?IePqgH|?(H+ zs$<;jZez!fj|L7)!Vu@|mXxV#%h2#bnhV!dqvT{r94-~|kqNkRdi>U=mvy$Yd0j^0 z@Mwd!0$eyBKf22MH;@m$oD1Hpyi@nZ5c?`(*hc-GtEk%%rQB5~?QDypv1w?@U0Moh z7Lg>pu7wN>RzCB_M2-6oZgFT+OY(9htj`OdRBdx^HNBo&Nc5kAkGO z8b|TA*sq=6IzNn?Dlm|AvUydwSwx59y|oR=6+RA;4jSjlACOTad;Tl$N(WKQnq8yI zMu@xcC)Pi3>F_sf-IrQRsV)AsxuB7c((&7*bhNb+XiXn)ZZ43 zm{XtGej@#Ir?8Ep)%sw#_!9mRi~S*w>$CId$tjG&oUw@{t*nv9 zXBKF}vur_JyvB|AD;zvTXvoh-m?5?h!d(QBKu|Sm8TF^^BcO}mc(Sc*GPruOnH7-j z?d{;NQd1wm1rhi@=|?nsOI=-GUo4OTgYOH@AIWvOQ^0v4r;FGr8tt{1d}y{7YU~>Q zXa@_+qOnRSWUuovfl$~VM;TO-;5&wt`RDiVPt?{1U-bE7Yi^y+s%4E%(fwGjx?ZhM z$;r%K2K^ont|E!PeiP!qBT!RbUR_h=qp&SRc0!XoLjBK<^#WVuq$;C1v?}^{7W^z8 zQl9J_eIq?Man&kgFYA6!mTYRs5k@q+gBPxtu0@P&yxH?x#OqwKEZRdQ06Zk{kn)g8 z@JP%>rV80nAU-v{Ia5^^HiP3XgNfqNXFp%BsIb081`&QKw6tA3=J6?`3_gMUVZ&L{ zI9%vLE}loTlRc~OkTYcJL;&t|emF!&jrdr*#?qA0KxklY9=v$w%jMxgM!^LW!}Bgl zY#B7vNTv+K(l4U%m-KfC11T`QBAy)FIv;>S1?1b=*RQnj@ipuHk`Sm2s(*+^EE-gj zKk*77tEE7~myiI(w3V|L-c2Oknwm~}54?Z9%Mc%P#evPDi`0sPC`MXd5bDo*~Pb2 z7NNQxBvNg=#xo-jlIPsqo7%X-`TLGMn@Oj!i*u>}K5PDvNOHEk9gB$d9lJ$(D5^&C zSbexU&uTUX;};$KYu5yr3(+P2MNGQPWZY=lpi|2qvLZcViN;1|%l@WI9q6a~Ci9Nx zVFYkMzDN+NqTXy+<+~dYjauXWn+-%Z6eJtX(CN)nNgN|BAM$qtxGYmr9IE?1-#KjL zl`WR@#X($_?gDoeHIK0F?I9&4SF`Q1hxYHjnP;DDEyvunf=uOKKi_F(B;72;mRR4Ra4y1OzOky0AD=A=Q!e9Rq4?L4n~AT~sKMp7 zEZ}?!@SiOyf|e_}%ec-Nq#$3&zMHE;B5tOD6+HfwwLVuo*~@D;rTxyPI=)qz2fKk} zb3PCgP-NWbSN*;H(62}f9RE#$c+*+wCSb280~Xio&PY z;&1+Fvi+mlmI1eYqSVX^v5I9#nGASJHU^HQVk#)R^j6V?S)c7Jb#?A6rzi#8&|S~3 zi(G9GvTKM|AI?Ht#V!r52gB^KV~|rbJ@M6Hulx1)@873FeL2^%I>W-Y-{?>Z&&nzL z*qBK9{2k#r!@qhVvLC$D$2kjOZt+R@YoX`)dcHMS)@}CL8a_M=fC3uu6UxX@z>M*e zU3-BAZTw~IOYYmZ?X$IJ4EfBp;-bQ}CrTc5DLvdCF0QE9X+Pe(9l{8>(AC0JtcRcl z*B@i+9bg&AlUR)JHhQkS$Sa`_dkJSG*G6-lX1qt6O5)V^LyR^=PtOyOSqODPB%S}Z zn0XI9`A=p;-Om@t#S>1y0NSncATSMBC%48U?2Xs`G}~Aeu3CVm593Q?XPR#++y22w z5{rT3MHR;-S9ss85{Dl@3UEZbA3|cJrjug->038Fnn?U?Bc{Hug;l@*&bpjD=qy2h zCcpBUqwvSyY=Rfv~Jak_l@kPfK4w?+*%cA2Tn z&C!0b$&T3xK7qTfxQ}eus7bCnq&ZU8rujp4#%jP&u%cXvxXd{T&h%gYE z3@(t&6(myol)+IyR6wudiL##Uo#i$1p&<&oS^+|fcC-*@$1rA7fAI>`P!?7g)&MD! z39V$^-}#Oen5qSO3SVS~Npbo7k-x{mo3%AzoO9dp?%WWoy`;2JUZSBK2Rv>bmTzls z4eNB*Qb0J^2z@9@N{p$_zMo1hb*UDg}_R4Jt&deI{x1clZ0?dv7jYr zWJ$3%w-!}^7pG7@RE5$ zpWVH_aY0Xg9X$%LPHCwodWH-hZf=NjuCDgd*Y5@C{?ZcJdDpEHC#QdZXAB=c>}@Z5 zt;rlOx-_jkZ?F9y4l%%TWE2E`79eEbJXBUDak@d~+wpyz;q~d@8<`I+Bc*?}hD&2( zwe<44S=nVj^=xX)`O3R=YGk$(x)>#=GD`YY7rok9;ZHAWXgD72h_q~D9(?6dU8P>{ zSEI<^uK=rh;T`}J5(Y4aW0)2JEU_9&8z3Ugx_|h|*d#{#0^XuAC#Cn?k4iq3J};wi z0Iqw}$x+m4HaR7J^!OkwyMJJy%&bK)z`h=EPjj>}3r_hLFs-|p;=qrE%Za)~N$`jq z0I%f!dQ#i~k`^ukul$op(e@WFE&~q<({eNe(2-kn4Nsk%(P|ik+*un%*i}mV>Q$TRGK4%1>=MQkyWqYW;5ECoV=2JU8*J0xlhdY2ez!(znDH#$(jbRO9EM zf`e@er^evAI5ApT%vfTEu^8+15wq_&DE^PFM0i%OGh2;a@TmQNsT)=hzxsN_OCiqp z=jV507CWSS*Sn0NTb~51Zxazwq+JdgjM`06{D=q}lRbYQR6jAV!5LE6x%)}(_o4y@ z!5^s&XSUJ2hw#P#jQ3sR4%>Oq4Sb~ffLh*gdnB>)>;K^^b>y>VG4$g1Ra9^ZY=UVx zI5{D2x$aa{aaELA7O&_N&+XesyT8;Sh$WJQX`$~^b#>yw1311+$zk?MDX4M5->kQwoo7LZq$F)d*-P}9@W zlTp%AYJpf;oSWs9LwrYsas&}WM7=ByX13(DcDFlsYP-AHrX?hb95x_A0~B+hp`)TC z9Uq%8YqeA~Gh>`A$EJwc_~qtiEh2&@-zM`yfh8x0a2=ZFV$#!_UbI`ht*pEP2xFb; z0`pE}>PGFfIRo?1)*hV9*8W2#R1N&>)3dPfShbwaw!kv3auBKFC*XXg1_V45H?H*g zf$O`)VqVJD=K1TGSANI4w+{LLW5k|a35H^h;K5OBN{$G*p?m5*yCwh=+*9YrT-vFy z%5;RZ27`5D;aW2K(W~r;6Zx`NaghYqT=+!G;~KzEOX%@5coqn&16c~ zbKskVykzCfU08+Re8@QOTV|i4FV3L4zJE0OLC##3Z1-fy5kz&#Jb`BjB&d9$ggF00 zcA@-(3=u1AC;yP2?0v3y&l|ChPMV)xG@Z-OG1vD(vqR!tuaP)&(#Wf&XhZ717`)4E z)%eSDdZU^@ErQkC*SCGfNA-AWyt=Y=d)b?BRfgmI>Mf#Fj&&Mb`O~blSC<34#O~PZ z$}Wm)auderC1MWrB{t*Ak3wCn|HX&(ul5&H5i#jqp77iuNQXm>r6r@&=Scdb=~sVI zWlkSe*-w=Of&a(eTSis6x8K4`R6-F%x%M>Y74w?coW6^GOn2Xhw$m8Os4rpP zpTDVZ=4N<9$Bog9wiGq?FwEcAGvdEW@Qzixo===NC2eU{!ze%>-+)<0C2hIu`g3y~ zXq4VR44J`K_pE*Rp7aGc7^G$!yy;g_*&7~-)hHX(S_&6sG&CIch;LKoZt@7w{7|Fb zc*=Ehrk6o*;!|Joi@xVhNY7|VhRe>Ef_MDhFYM;t$E9H{X9#gOp0p|y_>*RCj>P7v zvGSX7cX&vL zt)Eps9W$B8xz6P=dg014Xci)V)a2-8#^mtF@-4aKeo*MmnvViPIqcUTI&8jUWnqdH zVXLYW61noSon-ADwg4WYQU*xe_5!dYT3IPT)Nw&}@~3PzCN|d6YH6!6lgDz#?DcE0 z1w~NK1Y#k0mVsWSfc0V{?v zc-LcV1qFpik<8E~-@iW;@bF0D@nf_!pS-*+Fm8mEqmGNvd5AYK?k)H_9}7Dfn^VUZ zf+G+cq9Y^q@I#&|I%POr4FFvMVy=VnBIEUu9Ipb5V=?t#uRSe3e7NHA^w~2|cKu2z zc!2Crj*N`dj>mI+@~IJ*f{)!6sR#?*Brd@oK|J+)1h;PofB!x-I0(|#pm+(&_BR~5 zX@*iedVAyf-hM0oVg@@8K+`24K+np^0L?WJQ_#{xv1%(SD_k6%l}Wi4Fup!A86tE> z=KCdr*a5i!GZ!<3oyWlr)xz5H{M_6dJ-y64%Y?MF+>Dg_EVNgDE6Qe5Yp9wpXOFYJ zs-CmpPlBcPcTUiuxfR!u z4`>Ay1x$Wt zSv$g=lah4-QtK#0AJ_H`u`)%rL!kURyrKGrCJQcRV6~LFrOdB6c1(o&-d>-K{Rq#| z=YDMTqR5c#FK-7o?>{KO9Q_qCXK|}#7>4q~+B&M=e=JzCyP3yI?h@NRBH*S*GeI5$ z9y8G2r}BWYk>n`2C61mhXpdWzy5&AW4)8(VHhh|*Xi>sEx+2!tG*xRy4*N^`ugeX0 zjV8Xx%gIsl8iA)>zlF6Dal&rdFo0ZW~$c z9l5%7PSr>8zv!sAK{lM;s{~ALDfzAtx|K{Mq4Rax__^Pr`Ls7uIfRq%x$AtWF^(&= z2*j?iUEn&(Umns($*zw&mg0GUpFS+=*d(xlUbAK~{h`wBCPiRDQgFs+IYG@Ivz}~J0WzTDTXIH@L=5Ub|TSYeDwig~Cn##`TZW`=$K!&boZ=q|^djDqp zo9eBt+GLM6#-5FB!LO$elKv`{sORy5rraifQ+C4V91rfL(#gN!h(4W%Xks43Q8@aHv^13u7I6ySG;H)=g}8hmRHr~9^hjAJfEH{2%mreXfa^+ZXhLve%c#@Jv=sc z1lt?{0x>W$_GeI+&hhj=FDosbDS1bn9_X8znhMJi%|;!iCQo*PL@$EBeFLBfG)~D* z`AFCtM<^xru`!PDnG`nmk4JCCn)fosUm9-oz;=E#T(hfCWjKkngFf zh9?2QyjV~Fna1;u#}c{wxtxy>OczAF_6cqyY42Z?eC-;qwaWR7^s`R~2~4@8wfe}s z(|a*JEs}d*LmSPSYk}$+>^R)e>lb6W^w4ff%fRExh)+LVYZp+Zdp0yOBqS&Qs``CM z7{Bs~Y?5^h^w;vv-|oO1iVDp{@s-7lkKH;8m}yt^qd3z09bpva{`I{uBZ>_ptkd_V z;f}g;b8w^&^A?Uz|Nh1KX;1ZrZ+(1pvgHSzruQiv=7}YC>uLuQMhdB?Y!qQFJ{h9=No-) zeGA7U!uvC;tF5W&^W??I(9k1xu18?|Sx{gDy6twmCi`9o`-i|}+{N9)#k>LBvl}?x z9;ne*-CzDYu8qypBc*m*;;^nNB~@BiUT!j5t1`QyA(VomttpjVuE3 z2XeB=Xsxk{2?JzN`si)v6YTiGsrcz>VSR)2`lU2-Od|=2M!=H(Wc>1ueET!g7ybI> z{z?y^II5~@kM2Kmgoh_BHH}il)&%r~6LYCBE*_i922rJ zy}jMG)vlt}_4tvnJ=xxuKp(nn5^nCHSR|&^gPE%)zzxJ(KrwJqfnt$0xA}f3L#a+> zZw0&q#wRA?_^gsrQa*KKSibUCM?|Fo+V1P)Q|q{mrgcY#keE16t1R%zz4L=f%P|Um zyziUy^C92AH!){Z)i~ZvP!d0fHP1Pf!b_VSfbn4WqfrT@T?Baq<@Z{tJzi>Rf{eNk z>6PPE1eegFn9sq%fw0R#({e*8D2~$5&~Shhx?_w1M+K-z!-nnP@x_NLcfr3F9JadT zlUX0f63ga(YHAAZQ+!LR@kt!cV@F43Mlh^4?eE``mF>>XG1nw5_660_pt7K?+Yf!D zo^g?!&2u*OcDndQNBh>)Kw(!A7|2?dbs+TI?L*%cZt#MeIKq4P*Gh`7@>Cwrti#D}DL@YFTS#wDeu z1xo91DJ!Ww;^iR~wq68$#8SHs)Cyl|X!v_X3{FjHE9<~G-RW|(M!3peE{?r_WZ<)` ze)alY6lii@?Pf7u-hNToB@2rxKr%a9{oQ)Ts`$*x%Ga$KXrqsXi`e2sm!C(OdcZ$7iGun3;v%v<2_3^nxQ;VS<4UecV;#r4lIx*V?Lbss4eNi2E6zeiJ4h z9$s9G?GT;|FghqN9_BM4_AWD;l>arX=%Tv0dcF5CED?tU2hTm5qybTy$S@}LN3Lln z6$o1=r(;;ICLkbi9N>gwSX^3)dS)6!zs1Fg=rvrUYB>w}_D!?Il(4Z7)2`^D7OCxz z;xE}Ix*ULh>nSoaWto_muC9~+ctKhWtf7Da^2e6KH8s_~j;BXSK1s$$ecjW8zy|8* zkedCaDCzy>$xOn4v0>p_mqrn&=#S4k8nGDF_U_5|HQc**_5K4;BTBq@e*aGQVdPqe zdBAI1bq5E43LdY}yqc+v!}PMabuK&Ua{D5d$NU83Mg+BKoH78N{FH^|E+X>gfm+Is zC(V66mCm)VW(M{3M{+b=Ue8_t_hPo({(R(X^xv)20OCs#N0Z+t`$cKJm^hg19PAtH z5-%o=bT_KH3vcUQF=2BEii8<*w6O1r{p+n-oSJ4Q9Do@vFkm9zAknDutmDW`$%~Wf z4v8&@2HMk6!h)269>06nOV?dNOY)H%m!5-VGj^kI7PC}%q^aU+&FXh7krbp0ON5+G z9TUUva47lSA1^#;o4=?z#ibSk9~_ZAy@uf|@Qwm$&fgoPoDx^PV6;CT$#~!N(7C+4 zVU>xMwMFea(>9@}4+mDwqUamU;vtY_iyus{AW4lSp86x|a>C5TNAgq8(4y8DoED-> zsc)iyeZ3kxJ8L?c^)G_5ys~KJt}#OH6m}o&ZR|xnHnn~8vIW=}7GSKHFA|&~KJPJf z>UH@IVmwR~MTq~+x)#GARSX?!!IKry`&?>X-rmg}VNA*|Ud+*~`6d{v=v+w-mw0xDMa({@p6;umbM8x>`zp>n;_9rdtU|a*T%DDIvMM+6$&Qf3* z;UY%=#=yYla8u{x1?IV!s;WBueL8gCPaPfY`@|6>Tu+^W=ua>=KvG;&;}sfu^K;^~ zaP`EQcMa%%&Dmf`8^gx1fq}A}-7O6be%J&B`u4*&!=#+X+n+>Qn_G(#dNcMn2QfOr zKtc@mj8#LX3F-!orBTF32L}aQ&Ba@mx@4Yd{l;7%qr|y2RWTU5N2;o!F+MY01<>!y zYaI!^gpv<#75*%!{ZLeMl?!X)>SaSDoV6hpRfEv9kc=y7#B48*AP&W88z0{g3(cUa z*U_U*P9k+S9LQpV<)E~*<*CRb_BUaXLHN*qW+0KkhWuSXDV2SNE3;DoY)rudt)N7Y z)x>V#&t$Un7bH6oMom4%hg?t6`L)Fj(I(#e?Ce|&d|V6+uvg#MJlrrOD9}ScDbJ3`OQ|sv z`W^W=_0vP1-KUi6t^iY6SC_*F!eAvG$!_#I(MNTA-V1}Wr?n>oMnIsfGH8%49weT~ zK0z(&{7LSKp9IjeU{LMK`Y-kMD`wmx&=o5QLeFo5*n|vNxwf^LKo*AM;3X*I$okQR zJ5K11gOtv-giC3=O_1Lee%G}PWZ7T8#AXJJqX7BjF&q1(ub!2AweK!+m_@5J-|reo z2NN^Y1o%|?`L%&t&peiIZBzVpG4PGg)+~7rV2R6{bl5k>g)*qEr6T|86>JGeNfFhTzX9n5l%N*=KZ06Xz}k1gz>vQG-8 z8jjZ)mXU#hv7X*rlQ-FEX{=047_`dSx}v~Wl6}>;($ytP6vB(liSR@Wxx%^3C=Czi zc^;o*ZiaVguI^B#b|QPLYW}O8N7}K7LOjRr=Cn(8#8R#@hz-N&x<4Vw@o1PZEi03o zh3)`^XXLsf^aGnL5tRXEPu>g#p>_gfe%02vpcGX32W6qWZz+BYiNL@Of0)K|^PUZC zw0Rs*l=3iNr(J>h2Jk=Z%jxxqNwMdGfBhOvm){=A`VPBNz`BGLCIt6~6QKL2hGsb! zUz@f*pWvn!bU`{PsF!`pQsiPdQ>(QRz)D>L)0nb{)7rYt@u z;v9lRJDi?(@7CD?X)esjF!9}vqNJqUnl-Mhmywt6VUR!bZ~--uGD}^Ch;F}M?`NRg z$E6Z#`+`=4=z~urB_&n+<=WNu`g(Cm$<)TEAPW9FEu5t)Kj*Gkf{1P(#RiA$H8~$2 z3Fia7bMTykrE-}d@iMdVd(h{$Xe(Z`eZ)iib52{r1zgJ_LPH1bDretXVmj7rcSdi%zieh?J9c~PELGt zL0dE~X$eVuYN&sjAm;>*HZg)rX zLaZE1G5@$tN=_af9*(Z#8|R$CQ@qz4Kpf3$X>V^2&cw|sggu(Jwzg&FZ4!R8ke_ws zo?_m>35SFlOv3(DQ}flU4^O_PXJ=;SWWg@7rRO&jAb3J}JtCKtmX45B3sYAN{!THL zSI4KJYS;25a#|MJlG_BIZb&>(I@wrUT=;w>LVHDGVsK^4DqiW(_{d?Rm>OPFztP## zGjl%5nh^=JR@??heV8L1@bGMRM%2kX(>p->CJ1>bjTJ~G@**jz-O-%Vus9ko+70`l z_GXhl$2qt)KRq5ouW{=Iwgcbk;pR=O8w(5+7^9Fi?XBKxlM>?^S2Jlpk3K>dTUl9Z z&GVBer5@^g$>kx=s6bg$aT~h_sCxBpKJ1$UKYBFp)u6}ZIUL{)Hr?8dvC`@8c2apF z*&4{7psZ>zSfc*SY8!&$oU<3Y+|KUy2Saui?i_#OdA@|_k))))xRjvy;nfA3g)Zci z=31r8D=P+}cNU)iAK`PNz@T-m-fQ&(&M06Sg5c+DWyK-7J4?+2()3MqoSxpT+1^uFgL|l74B(rSsO$} zR_Hh1fFMV7ytgrKv$NO^1M5GX(oJMy)cn>^iMfbVCZN$u2fpzWyp-PmBAaSrU z?j`J*3cLJ&|8iPP@fn>Ti~gjA=Y;1=7iiPyIj4C$OU*SxyoTu~&_Nn*CyA;7$#p1G znS}1!j~|$ftD*hZlwYq6WkMryG<;R$gKrk>u6+Og9o|8`MnV|a3g76&p%bRLP_mP| zMTV;>|H}Or;@MkL(ji<;N$>2mTu>zhEze{)mFbqS3z3(anveZ`m-77o_IIQ}RysPaPF zb#lbMUkx%q7x(ey_ndBqQ$y%r_@xG0dy*#U#`ThzVz_t!yr)D%6H< zKLcPznNH3bc=CeDNyRCPKq7kV^>J9W4W6H6x-^KUamOq1dwaIIvzPTB@;!Ac-|KB= zkK3z_C_ZSTV10Xf_=o98TmmV|h$e>G9`A)28Y>})v~~k}y<4E`vWJ{UA|2r@=Kfti z3i5~5KYSq_2vRQ;6mX-jtMc&hII_)wvJt4^a|9gx6*ax~1q|uZQ&RE|5aKj>1Xhe=KO-Fgqn7>D#J0eS{q{QqJho-z7^)u>UAiV04+p`sPAsu zrD4^6B)5Hv?yvC#BGBt!AZ^`6^@haM+;jO)^5#m2Ay z%Kl*gFwzUs`Yv9Z10ehbTP}CV2vX8hyYaDeQqyo) zg0xD{fM_msNy-Pv!Qjv^O7Vqr9$=SwnnVCBTp3aL8%&{fYOKBWdM{Jp`#0taA92D| zT?8b=Ao5m9bWKcMO>J|sia4B%Lq=-R*OYBN zS%$cpsya=pY}`&*PeBUh5JLx=PoP#KDkpE`h9S?^ zM!$OSXO)9vxrI*AnA?z0TF|9!@$amMsGPedkoSz#*f$atUYqlSC|EVW&6JKd4O7Oq z#5j??7Fa?2QDb_qDfa)!+g$b=FChsr-&jzDV*{%ohH{yhi7-*ONc8y-&8~+gksy07 z1Z7n(d!n2!Uw~PBkAZ>j-AFe0`4Sz4z49~YZ1lCTu;?POg&g-cRK+ z_AOKjVId*?fbp`gLd-EYHuja9NE5v{G8oI(LkVQ<_EUbVSAGs{C}*CYNfpdq0|FJ% zenP(u(o?q~_jM(b2j0NK(yKFO7*g~BxzszbwAIi6@8ta84>^yLOee@o{;1j3#}G?? z%&`Z)I*9>+)S^8`x3JE|Xs`unK2*{tqAg&su+Rdd+DrQxNXum>B|Y}>cy@oO-ivTP z>tbQS?eueIa66R9uyH}?jfzQ5>|ag)O`HeskSQe|0tJX1Y+8l)=S?5itJ~{g5TjNa zra_n@ikZ0u$3y2PP|%7Kazf*^G@CK8Vj=VN8PF>^0S^ZgdG*PXrYGfB67r;ARx$J} zRF5E%JbGiQz^l7#M~4L&lQLNbYTb!8uY+|Js-*-exe(^C-{)B0z;7w+gv- zYqkt`WCq41^}}eCn(Se(e}GCb-Gc|-gU2@=#Hj}c5-fdvK7rR)vRC(16W2XglI*Vh zyV`a3w>Bj~m9W3N;6t}_=e~g9yde* zPFe$9%dH=b%CNxVe%KTHxb%FZe8dI-&{Zd}XeL^Vt6}E>0p9KF{jxGTv_?DMc|r1S zX0#UeEi9Ay^Y!(CZ;XBFWZP?Ml8EX=rfT zg`x`?sk+TH-|)#@{%RitR9ZJT*vX)Zf^!jrC(L3#@%*L--`E(60c+J+5+U#<13ZR6 zun4vtORo^NFJKY+&&Hd&6ef%^OcsL$K`e|L8cmRmal z;ps22UV8468~BOjvpV8rZ8!_y&@t_cLs3A5WktZJSa10ec40|LjomsCF>bD66Ie8o z-08hk!ay%3DB`A2K%neset1(ya=pD{l>Qww|MK(MjZ$7}X`Mn{rHvrqyJiZ1sZV#v zLk5ek9~>JS<2zTc*us+2+3{{;UOgX9(dzlt~3rfV-gL)|V z^Ho$hDXtc=i2$!)Yd2QVb?vlEG2cgmcp3`7Eia7GTuosXIv(qzzjr}4EvTr*D>*i{ z$cvT>SHz!!$8}=pN0`+%XsOj%owvXer|G+QRlaF%XMeABP{ORplJ;J5u6AXYR&Ph% z@iE*ljY1P8-pTp8OQUitw3@s1aBkF@b$rTht&a%Hxpy>jehUq?*=SR872hj0?+lM% zd-XcpnM4H;{CV=RVfd{Pn4tBKrtE%9s~7Hy9<}U=<*zavTps5%mb?Y(Ogp7zFqvi_ z8Xg*Wp**UhdUdeBk85bKjHm3ctsObpofD6P&8xX<&X>O(&j?93W7MsdS(!R=rVG2m z><7E9;XP>gUU=RtY?V-9tZCF(T~6;@lgq{UfKY{i@yVOdZQtB_<}Za;7Ar*SC~kky z<$KlJy48Qxy{`B0G&*LldvwdPi@_mJGNAP0=yzUi?SlWqUzg8rN|1_Ong>YC)~@u% za#Bjj+7^#~^p+#zB2br_ify7_WvO2%dN+8KQA5>mAe8$46JHDv2YE=K-`i1Z^+4Q)Dyk)+t zMKUIHtfZV$A5WLTBWxh!LS~_#@RHNUfxN=~PbFu%LVzd*(L6oY{cC-QxcArke%|vO zQ3yKC^EhWJ8y#Pg_Qj!eb#psze11Wwif{v8?xL%~IXO8XxtzK|Usf={Ml!I*R(DqT z%fB54!Y2oRq9=EE+v($vPDS^K8yYbBpgsTH@|9CCYO<%tVlYi^K-UMNbo3doTw!PRMY8_4SnZvX8{d|KowcGs&vhNH^4G;wH-FvK| zt0Eu6Mr&fzUZLVs1Fp{lZci zc#`zWEOd^}4`Gq!I6tU24kcbaRpHP}wY|Rzzc<~xrDl@BS5?RVyp!kbW*n&Q$DjVu z?rGkt)>V1)#-eWhA&J$;1w6Tv$<(wIp`D4(oZW40Pi5q|i&9dB3y%%@Nz?eh9ey)t z@7i9tww-g-9;Tq+b_nPu_2QD0vE!+?;~49!^1rjmm(v%PTY0?ZnnkAjJ5VOE9$Sw` z$f&dGZV#W_5;dP*vqAP*)?GMrE*ce_<&*fYJfT=QbBYV4cRDlg1VV%aVNtH6q}2J* zIj;UWTJQ0~)x~G0f(-z8(O5+CRTFlGUq8YiQJ0dGJpVg=$ghy|G@Ox&s>rD8uV()H z^QaO~^WPKn5CK-IN&M~KL2*YUms|bG(X|tN#v|GD5nA`ZoY0JZy+Bn^b5lTubfWuv{UGZB;wW_mNX*gAsVs)y^n~92w+Bo zqg?~WpNNyT5L)wz;t&_7d2axhJ@$vy)9U>F>psgyakZQ2GgTkXxBzVAUjDoF?!{iU z+4!TP#PrvemUUozTtPtRN#s!qTou98}Cj6+vOaz6`qBNo=7^qsgn*Zc zgF}fTuKal#BcCkO@LhZ8!tZa&s2PP%^!jff%K}5Xl4=itXeJH9sv;H%|z1kvfZgdrSlv!lQmYgw}XVxOswQ6C@8ACG>T37fl_q}*F8b}Dic#y%*wqH zTZlU=Pwp)Qo$9)7OJ=@EO>s^Z_d@N}F|1Rg8cvw;GatTt{qCKhaZil&O!vZ#gl0KB zTrCT>s~aF93eqZsl*A9V9r^)NRLy>=G_&5=B+Q~wm{L+w;ur^bOd%X150A^F*bTzw zr}5&{B2&Hf1!}PZgu2;#NqTRGUWy)6JRMkZ+FgO@K@<@7)jYOxf97OGa9R{Op4{r& zf;dUstI%?AIOJ#zioffIju86Lq&rxA<18nKWC=T=(JX6vwU6KZ495IK;q-bHlam|` zxf~GKF%xc*i#Z_s=F8|;hwmwP-@{7aChIPKJJ=Dy#&)RhcsdJA2)QuGsGI%CzS?i@ z;ut_Y^*-bE3<(JVwK%R*Obty~jHU^LHwBfB}x9?%T*Q)>P?X{+mf5c+`sZItX} zwFuTHjEi{>cIe{TK{he6CYnF;8}gy*&uXAz0CxW2!w3E??l^%3nmvBk@~!l9)3Nce zJjHn|*?1YU2C^G8EsC*6620GavuX#EM z=oI28s4weqvxU%*Xt&cZ2V~g%*GUNo-tdILR=lYN?RRKEkIT0$ z80+TE&`F`}ax{|y-2$mx&is;+SOL4A(hZwm3_p(O767C?SQINBtHF9)aOjb z6D!Kk7t-l8kN)XDxB?%P@SkW$EA^#3({UYhC;9Ic+owQ(!f`y z9FD1Vh&a3#@IJq&bHCXEP1D9pGrc`zzmk$tT1V2Y1o&i(abmi0kBr^DSS=t-X8+)e)4Wq^p~otfQoUzBctAuYZ1}N@Fn& z)4e}`{&Zj5d0?63&qr%@I6XCmcJ=CUYl=b%L=Sv=c{A`)oa(suEdJ)@i1<)Z%vG7T@Y|Wx&{Y}Oa_hnRUu^mnKkK8eRt?PxU$(*eFcG_3X)H+ zpu`)`R{f!B$p7zU*a^q1N^2)CLk^vl_4odJ*Wt0H^>o(;E#L6)a1fLE0WM%>HgJXk zTYqC|$u<$rtntF)Obk80YR=EL7V7Sp2aslC(KoR$=# z?lF6$2!s%kj^@d^rMq9o@S^>3R9WXR)L?o}`VoF>w>H00#FG**djuyYU?yNC1c+N) z*}Lh3JwZnd12VYm50#QWO-a>5@aa#I^!LX-$M*+(rUv4Ob*4CE|HG}>JZ4tJS1Hc7 z11VF>o(F&bDq7;0Jg_pHaoz2P+KZ`G=~adGgEIaEun?)eIC-aRSnp?pf)6YlZ2A(! zVPyI3p9dQ$wCqWwFwx|Xf0Li|@x`ib<(DsCGBPsYNC@!r`~UWsddB}eoEfg2G1wMV zyc439UA18(kB*@!EPlC0yzWa}(NaA2opty5?x=vn=E>jcUIUU-Yy{#40~6Ek^Vc0g zGmNtrZ2&Osd}5uol({@6HlGqc#>e&+L&za?uy-J+OH4J@x}Abg`!li|Yg=2eJLFI? zkpLNU)^%5o=5h&T`3BGZxOB#w_HRHpQkRN!y4DBS8j73NwK|eB2@}?7$OE4VnU?YD z1LWY%UjWBfuBr;61B^qL1iS`PWn@~_5FiJqoF>abUK`*=%5VL~%q)9q8FlCbsLx<2 zB2w7eim6!=b_Nt}vjw*MVD|KR9e!WV($Z30eUwfv3fj(2jEDfxxc%Ju0}6)20OME= zH;bthf4S8Gm~f_M=j@VPW8n#V#Oi|_KeD4DE#~laud^Aom=-|}q(;A#;@Cw;z&~>w zcw+cs#8<=6kU(%cIXYU5c`Weg(ExZ|Z)Ci%noS2`S7Ks%ZYO3~&iFHw89G%W4)_YM z*;qnMJZ})95}w>>9`om2l8+xZzzx58_3CK01~d)iGBPq1HR7R z2R-lKo^9p33x|OpTw})y3>J>fpc=8@jEnZG(oorLpJRj-J(&o`ciLt$5d{S-=7t6a znF`^ze#hPlDW-8Q1=@2-X$j%=5f7Hqq#bc|me#%&tOVUMkKgA#Is%|0ezCsH^BMKT zAP|4c|9u=nFi>SKJe;%Od!eZrRn6JRg}1=7#8YnpC)fRaU%f)E_HD@o+}@=FqFSgw z0c?`ih?hp_!1tk6SJH?H9gF2_4FFGjKE5B6HB79Mu&WO%Ctvm#rcg{;t*!~Ui{SPl zQtjJXWYCIZ6=+?D4%cO-+$wqMt}oO@-f&%c-;Kz!a^H?+)~PIZoZUztT!owp>fA7I zadUMA&ynCm1-b4h?q&{7{JmXTW@fXv`5dp()XwiLTDgzmF6g=zEChr#d31&`v8jcz zYK@f2Mh+Nud}mRwHFGLR(R+4}pw73Zamd zm=w;rLNpJ{_B>o~~_rSLF7|-l`20 z!iH}B9Ud>tVqzt6o9CO*LNnab|nYcXmOB@7hPhX`U`raaEb&o++PVmq5e?!b?c27v5dolbraMM6& ze{q`OY^ReA=o;V2$)U8eT^(2gGLCh$YoBQeIgc5%H*r&$A5v470ANZz7XEzSvxldP zX#u<@4k~g~G6x_J%O#_pIH)?9h=u!-Bm;m(yCRCsjVHI`SL$8wh2iV#+u3c+(1H-) z;Ze)(GME1Y&=;F#amH(#H|yAoCAfZCySu%j0E>!R#6(0zoiyT#a+!@Oii}`@r;~2m z@teOYw=V}HS5}W-Buj;ljHvPOEHSC%7iA5XTZJxn>$>fB6TxM(=r2@VA2FYFaLm%F zI$+T(b}o{XDVVwV$}1@zN+(CoZSmqVHR4I=+VCZ`|6!jA_c?6G0g;P1es>c1VuZ)A zGb|6TdzPB{jC=a;zQo4BZ~vaJ8=nzTh)NOyJ;1c}`s+L{!H=oq)bXXOYc;GQSmfCj zm6Q-3axgQ?S{yJyC$S2mHWpY$KqxN*Gn|_F{0uZCT}h)!GNqNSB^ z1+yrPiXJfnToU%V5MuUpaI)nS5QvJ^(accGnSc~AA(gNME`FwMbx` z(*lu8PrRjUJWEctxlBZ^N=1hZ8+X3P?`|B*i}6h56;_=pyUT=?I+3+=tN2g_Z zydamov%g-r&cJ9$9z4~Vaax%!n{lp^>{QFE9;jCOO$Tu&>#ebRiDn5glj=kD+G_LF zM`}DLnMutZqksNPmaNaMog9bIUgA;n?O36XA|hDyTHm$4=DTGG82CCveKiTgxmd5V zvO`~LcN6>?=4OP7>+uW)hUCr^R1xfjJbzPSly<{o-5>wSo%@Xb79mutCcKh=6lA%Tgw`3$Z z5~J#DSPf~o>j}0w=qM)$nFyfFmz2|z@~5PyZ;cf+&dj{BFlf_4Rf^ynD?}&U#&f?E zPE5tuF~V4t98Ei5z#O;hm1yR~$Iqt6yFbKvxrw|2>XMeV$8;-AcO^kB54sNX$x>@# z^aTpYojKTA&GPheQE%S8V~Qa2vjHZ>Sul+RGzPo1K2mTVp@qqSt7gGrf-0z)Una=@ z+EuK+Qv9;W{*R{B`Vu;>U%+^!-Fl8z+44Dgpjl%R8MLtWHa7S*VGK%-T~mg#)Gfyg z4F~oX!0Kzk^{d2a^Y!AwLesv44}7KTqj@XzK17EmzdWz(D2dE%Y=8m#{j&O^$7lJ> zJ3G#qAKYh*e`lfPVA~i1sbY73^v|rz*7>WN(>(`k`{OPdbG^O-xF=op{i~r0lp<^UR%M&QGwXZq5OM$xDY2M#Mn!PN#3Y>@ z%{(rT;r6$`V0h@5gt`p~?zoDwI@L5P=<|96MJH*g+^0|fIu?pSfk z;pcH3l(!5}X!ie1I&_A!)YsRiaj^rv=I-6sP<_HB%zcsidAvPBu1TCq$gyvu9x%ia z(~~k+%k(U4w0KH$9(G_?$ zii&-sx*q5K($vElN{t&&80hr376z8=@ogR+AT%j{LUin8t79%xx*d-TQE!2CeY|Mp zUfS7*7DaPHgm<9L_Z$ zxn{1@F;OWyURYu}YB5=QE?px+#olK)_BX_!)s4fcV^70IXoXF?`l!?Jl}tFR=GoHY zya&6)&fWXkd^QJDZ#j+Rc7CjhI8Oy9Heys_UkoLBM6gby&`bbInx&uYF6RLkiRyHV zV@3cIb+;Jq{F5sZUSF|2@Z#MHV^o&1c9|dY7<6l^O`=;(G{nD?(%%Vjund+)u=HfPogM^MAVKzF zOta42&FXc9+lcYh5C2tACg(#K6Gy2WbyA!8C}98s|L~7euUbHahuue&dVB zd8wVCv|eN1yjIewo`cs}NSGDQtW(>!F4KGcRgqC|^dg7d_tUZZc-%k3RSE&&Ib>WW z4DJgPrRM7#?Z3||b_a$yP&WfXK=N;;{T=}^28a+di4qu4ULWr)5wmC#P*bz z8)I&1$_9j4NiWP7hsXh*<4rkUX)gS)My{?w4!xm1MHA?mfNqCcp zB~sYoVli`YS}M@~y{+kJtwhbqel*wH2|$GQ*GFLYEb4re{NIZOu1l!41yJJ32yr!W2~24tOkaTsfNC$zFx+>I)0? zyG%@-y}fzBXgO|ApbjW^0B{xS#*L2OQ{B#Y(2!=0mUOwgbs}bC?CVqM2doGgna*6D zD%gyJSlh^FuR3)OC$_`Rn^vS2_SW!j-@Y497^kyfd|VwrJLR-1P%~TM{55I`uEj-= zNA=nBU60yx`CeR6YsiRa*Mc%pzFzRDAG75>=pX0~of&>K6Evj0;tzyo`XFF$JY|Qg5CcKZ8G!h< zZmRH*koom>rp>Cbu&^mf@nPuP2M0NYgoL)PsR2z29yx&4dQZ`e3=OYeyXJZ@ZhTDr z`Sj_6MIb3B9Uosg;wpW7FhXE5SRCW}^{-UIm9ED-)~9HU>bWsRMZ4ZNiJNwfro79K z;OfH!%zP|AQCmk5I%?QLn5uGM%cFM1@xSzd@MC%q!%ywLr!;8P{ZlRLRnCd%hUu{T zKKCiWv&iAoP*kagTomdv%-{Xhd&0kWSsZV$6(qVkk>dinOV{mB$pPie$=;wlL<$k> zMZcqGU4Mwx|2+!m{~iUvG0pvgR=OgUQ4^&aad^}rKJT?*$u_^Tm@4Ohxn_~mk^!Sq z(i6xHkno}EuBjeeF&joR*+$gu_6Wk@q^cgi8|%)_0CWBcR9J?SeVik8?&x z<`!d1AQm52Y&oj=2UP{Ym6|HI0WSLjn{N2MZU(_9vb6JEy!RIUjkmv zCK+b+#8$cPJkxbJ?0+|s3)0MlxG7V#|9oso-+vpYN1jA5Y3R$*;w{2UEDzHUMHSt2 z7&&U>#KdJl_J<}WoJn1Nlfh$cBuCI`=Z%NQC6F7=zEG>sdcqnz>e)X(JEJ1akMfg{ zla&<$RvGNIQy=dL7d18KL8}C1dQ&Qq=#yi9hoZO)I1TR&f!z%g-+3UC9Wc73Ktq8; zB{aqtEp8DZDYVGJ!2zdg3yLv_$xEy|=@6Zv@`a*ilGbDT1*gHjSqC?FT0q=5jf+Yvn~!@Oz& z4>6$Da6r~zz5+eZ^&cN#`ch-miv=RU$-&QC~Ha@P4}WwWK`S|V4wfBpK^D|%TC z_ORA$DCiv22a2cYhy`L=qdb%sZes-j@tG!CLjFm(VzL8aAiDQ)eT?duLZaXYOf zUSMboI4S&SIk+6oFztn1G~L2C&z33zEJ6E>;Xnk+WN02QASHNDL?$9mO|;DEThyF;H13&I4Wx^_X|6Y@1kL1Vgjs0+qHt& z3-D*OuFp&-vhwn^HYqt+eW1H;pebNE0D z4MZDL-FI}FTU$HRO>dKuGR9$4h{rvtDHpev_K|^IW5xW=2f1I6YR4)Yvvs=K;c(Zf z;3mSvJ;}yux@oxcQtT+Esld?42$+5Tl>X}!ckbMCMjdXTvo+WJJv_XGu+!{ZK)dZH zvYGeqWk7ts2@#U3ixd6hD(;&%Z%nwNK0f%NKxq{Pk*>prcvggdC60GB^R_Or9uXU! za6l(3T-mIdSN|SR6-j8XZvFdf#7|Vq`%mhD-%z~`(DFYhAv~N9QI-Eca2Nt1Mu;jb z|MN!@Za|JQ`Og=M*HLffKVM{ggriUPpD$Fz|Mwr$Gh)I8SsKFiCkmF8-jrvd_wLg69n5zJu0yKGQ0P^r__F&w+832RTCb+P|X#f6&zifgKnwC(pLP!lCTyk~XZYHj& zsR2o`C941RFF_UfkS742YrC?W3}bDj;^9?W>!AOQcBfO(0}LO_w(R^S2?c_0g@a zuA=4*FnZeM$CHL93{G3zw{Q0xuKnjl>~}pc;y-=*#QkoB%VO#%tM1Hv~2Xf`bQt^WV7{eaK}uOnwjXnA!P zl_)YG8CQN`q4T^qadu7)In?lgylvH<0t@o#Q1HLV9Nfkk%>TKgs2l%3w;gWV|J*dx zz5UO-Mje3vT-X1;_@Bq&zgOe`{j=c=(f2*-gn#~cRFrzwDu?*B`x%aZf;1{5dL})F z|FP4+qk+UjMZ$!P+wecx|2_%-jSK&clK)Ku|5s@sftZY}l9m|YYC*&I%UzMs1^|dy z!7+q)=T1^J9?YG;F_QeuNW?8#Hj7soBMIVIN)=IpGeOnp8%GWV4 zI&~5qGN~^xuFgvR|&TbhP7zlK-DMk2W86W=1=gvWdVmIaG<>}>Pf?dQz zPF&DTXsslG?Zo;|WkLWBE%4$!Dqc?_7#(^5ic~>M5L?ITY0o~@2h)L0H&ns{x&^9M z6lV0K0$l>_>$tcWa-lJYwlD^<)`dSHM)U7` zSGbV?<6{by!y07cGo>1VzFkBt%64>Fzor zAW|aTCEd+SD+p>%hrbR&)QH(xlv=lSlv{9{mi@3r<^bB;0Qm@6;k@t3VR zNk~X^Qq7Si9~OKk1;u3!vb;dl9m!tv93poA*7ATLQNSy*uf)wqco~|yySRHXZ z`NQL3R4 zTW0~7o|Nmc5~Z#eE_ir&I*~z@m6dx3b&xM|01b)`O{U2clsPKp?|VPi!(L1I1amZ_ zOT1O#*GI8}brAAyyoa#wVMbKi2&ss8Y0H+!Ja_@FPXLt#qJy<<43v_Q z<6@)G05ogZVuJDK#su66R3xEPvgiq+qzSRHvHg90zH7Vel5; z*

;>>IZwKtJIw_I614L20F`i1f~*DmDC}q=Oq14I3QheyNPU8{GgcRY?$kFcWk_$FI0DV6X;b4~d9!CO!;7Z@E}zcHl#PzzAN};_#lVGf6i&?=4H|yfYet zzK@VI!Y>{ki_XUHS#yO@od0fV>ZDr7fHjbmDGr`=`Jg@|(``{(Tju4+TB4ibzx~DJ z7Do2v?rQ;^U)X%7PjGGl-}X({-h1a;6Tg!*gC7ZF|V~iP~nY> zHTzc_?ug3s(7V3IW-TmJpy=8jyj}n-0b9laHI|zjiuQu1*0N)rprSIY5E&@k4 z24V#Ig@z}<>`N6}K}V_^V;jt|vhB*`Kqul~Y6ji0m4tk3TwI>ms<1!N%g(x|rolp`e*1Q3I<~Ge?SjD@JY%KR6bcC#|hS4aCMO&iwfVY7R^O+AbvocY-46<8X z=cDDA{z{6agd;JO+^I*aH+ul9_zOJX=2SJ6T}W)TW<9Eo4$vi**_IVJfJPXgu)y_Q zg~RIW|MY;Iu3w)y?J@Mx!V~{6Mp;widQqc=?FtdxI|=*jr*c0TU`74MGxeT?Xye|h z5n5UcAMl{Jk-5a%%ZnlRAxN=wR9xqpFc%zjYK)VdN@pO{h^<+n_UNXw%1IE;ZkfY> z6a8~GD6bwl1bt{xLtI>3OHTK+4(jvKtlu%YvEDDGUZRD@@er1__6T^YRdb?Y)KbOB zI(F|eYfU&*xK<3t9k3TdAH4mrbj!&K3yrmJ#k%cdYf?6JSI(t<`*v3l4QqCM-Sb8d zK)_gptQ~)U=<5^pI?cD-lm4h+5OseZaJ97kPr$3i5&`3{Ed3E)eS@#X5BcG$2!=(0 zq|2o~fmeKic%nSnseBe*SzpWt4<6ub5T%JnADB102BzI@jl^0KLQepXh-v{%h?}t0 zlwtQEcM%gi7d|Z@PIxI(pdiqrG`F&10(Ah0iYTL^x~CzcY{ifwD^|BD(i_Vll!4&q z=-}W0PW-Q5w`>#@0Un#3b1~ydGVhHR5}9Kz%&@-&NUqo#2w`c4*1^lDco7tRZ5_jK zUOuE&DsCwjB8tnTTCiVrV#KrxyS@zES7e236i}6g>wz8^kF<;_Y*SRsqF7%ibqo=s z-{YsE^LZV!0yJVs<18)(TFD)NX{2z$siLz?jl*RE3|5c7AZz|_SMUnyOE;v3KxLZ( z=nlDq-y0h=^4#=hQ`|aWT%KakkKCGTe!<2R>4~>6F?qn3d1V~5PcM0RR5?5j`7VwE zGPB{h=`H4s2e@0)nCfN^C~;YAvY)?L377|sj4?hgZko!)Lby8b#I`!%w~5r!q;9yN z1wqAroRh;=t<_6GODn-~)C!AVgvpdgq1Q82Y!F;{8!l4g_0_ov?L&awwLM0zYWeI( zFwLT(nwy&wj|S3Ad@VL62F6f>T215B7T90T*iC@F(mt*9r>mEl|JuuiT~G)DBHKt- zaN-b)IH7=ShOq>+iGn`3$cM@2T2p8YhoWTw2R@;YurL*2)Q~6^F=q<3^h>v{E{kVq z*pEZdL+U&H`mN%*>?C?$xo|xFKd%nZ6uAR>qao_@x+hFb{lGEHu-b2F^vBeFlFKFr zIk&+YoQz`XkP-ALXh#K&w6=7ynBs5=}zWhD;8Dp^%K;NTUwip+yZuMQ0yUL+2w~eR-fwHa_`X)7>DOF_q;-3lI#f%H{wX>hSeZVFGlXKVfKn zc5G9G_VQ*AP%EKS7*0-3Av)xeaUtl;1b(%(u3B12#cKF`E>(pl3JMW2Px7GEg*23f z@@9{Sh)7#2^Jjpg-m-vsb(_4fx6je2UVLQDBuJBGu;KFXFUwW_d=R@I)Q=X>!*v%I zGwb{d6r!bD(eI_Dbn3eyMTQiO4Gs8rQp$P7#BK@!tbO#h9(4CFr)?ulBF0A8YduGp zL1GBNQCG1DdO|pja+>}j->h}W?Z2Sp0|{Kn&9sm8$W|>lwqe6*j_tuaA5DOc6xaIy zgLkkwnN4l}j@%90NCS0WTUc5$Mn|~9=+`uY1x26yR4U}9MqDL^`ERZ-fBkx^KcrI# z4nsaTtwALKBpkG>XAlXlX+$s!yj3m2bT>)8R{jaz_RJ?QUNpkmQbuqi3X=kA;ph}r z1UHQ`P^QkMK^F8<-HgI`yrgr0J|6iHf=TZ{??f6R1H8nAeDM<}99H^XNDElxhA`y0 zC+6xuV|_tJ=ItI>!ujx-8`8hLlB19AhOZ^zrZWh4%Ln*XgvuYn<=VSlWz&N9`R@Qt zAbk+*2?_99fgtfPI(1qcvxzb(MR?p8nHoUjwp~vKGhsNw--f~pY;SGh!wYPFN6$XdE9GAx`2#osUtkKq^RN%MD{wVc(Fi@~;xP3|Ct^e6 zYYW)m&2z#p{(bGgO|OYUqXJEUklVLmi9@ka3*7`oMHtb5kW-}Zm|I(itmdm$rY*nc zEA_CL%{cXKu#GQ@zosw%W6yeAJADo1W~YoB~Zx^ zWS&o+@T;ILyxqT!jPGV+#V;XvZ7yrV?(cg@1ti5cAGjuzn^)H)C@dWR9tOomscip! z_39Op`vW~@Icm0CE+<&JgFstF&p$psenkU%D^qg`{DdsNrvvj#i!=Z5@#7lchaoQ_ zz}AJ4LsL^zuBY3cDtg}{BQw`8fHScC!jS}9$wb}dDAfWFs1iP~o-ytBr4SSnGM%n* zZVh^oGzf_&%+034dNO`jk_wyj;tv=z`$)4guR*m|V|e%8y+A9n&51X)@@3{TpO85* z3dQ9*qO^o_*=i5dCb@<1YuK{~?e4&nVKD(b2=XZbDoL<<1^@CWh( z$qHQk?UFk9A9fDFKk_FaKalLe)!+W#FLo2|S+!PFQnd_YrGByd-f65gV%~Y?_PA-I z>#p8J+nOD&s$ZPl3kNy=0p^RA*6?TCw1Q&g4;d!;EDshk5b{a5>OP8Aq_(^97UT7^ z*n(tn<-!?LC%NxIK zWY*ACPPAb0Qfs#HnR1$nw~9H7!yJj-;TQ5#TecV6FQiFCjBMt6kw0dO`tnI1YqmU6 zQdRL-w0t#I^R+?tUpw8u!V(qFaXHvoX|>MgMFXX21toR^V(#7LV0vOWw2(3CE$GBW z#b5hXy#L0t^uNLmD&7$x$t2QdTK(&88Tp_79KU{!N|x5Br_cCYVO~p&{g!Cm3 zBMTEPDb-8q;H=&>BWuMmV?SCh>FfvU>H5|pkz*r!Ia?tezf<L;yjy-iNgbNVq#o|=-Zp42n*umllKwDv-D)D-Z2g@EsyrO9P1yr z2lWMuBq86R-v-F-`E&PGcDqDZpPH!HxHFfglt#w~fdQ`8Z(yG5*%c_7*LZ5aJGH`f zC=kRH#KaPXwo>R&kk3#zw8CVe3fDrxaAt44Q~K26PQH*Ni?4^vx7WgA`@`hDp3m-I zh)GBeetV6)*`NJL9K}sfJxvZTF6t;gBL(MQyRwA%F*F(2W( zUcMs~LYL38{XO&8&xT@?#O*lwB=P)fG1f1&HWxBFpIYc?{}E2nO*62OT>Pq)JM-5p z^#hTQgqj_xt?thYGT%7Mmwi<9x~}$ho%4IX$IGQlCS8biu|Etf<6iC>K%*1gE6b4VEA^PF42y_s zGQZl3JIJuLl_S#9bLwz-e*Amn(KV?iyJN2l+OqrAq|`GG?~k3~hf9*iO6bMJ&dpYb z{1KR4R5_QIW5A+|3mNReq<{Vb_URg9vj0KG8zg`PBq2J#Fb(MrK7x#IkW1JGq6Ll!2mf za!^ebF4HJ}nYwp`6~}RUkE1JDx*l&TY4znMdxT^Ge)fxJ_kWq_{>!j8wcalE*kc*7 zliITXt6*vF{VeRNT8p1Wa@pb%68=C(!~CE(Z(2G!LZ}7;QwJwSx;gI!SM41WpZVa@ ztu#A#*WRzD<7VKbX{TSm)H}W7zI+DW@Xnt<2rZ_dZ^Ezg)df|r7328kb48At)hz5yg9<@1-_BxI#TU@t6Kg0^4h-!_0Lkv z@EI1~&+n|w8cyhGU9KSo?d3iTWh}1n+0I=1q@8{&OrXbqfbPqI8*5F@L$zl1Xg{Y{ zzbl^aq7hFWH1TPW7Xs5-0;2 z*VtJS1EgsB1V{Oc!w7 zi8S9_{A*RJf~9`G^tF|a#ku@fd;X7O?%u;wDQzR;P@?vH%sxhUuu;+YNlE{g0(G{jTarnaU>_5Pf7 zjV{If2{~NnDZ2T}fg%z87UlZ}i3i3{8m~V)@OW6zvtCVQD@aVd_SI&fAj$vjgMV(m ze(TobC|Xzggeh|2LDmiQxeL48cG05EeQ*Q6M|Xj!iud)!u-w}uGiqHEWWm&%4$ph5 zU!yf(qZ}P_aZ5Gva#c`xuW|=xH<_WmysUF`2MhD|hNYin6K4j3`R#^le7#jmx&W|2 zUQbM~ffC($uoqW;+NOO-KFh%Oe8n%iZq|GQk51j5{NFgTz*l7BnSv3~}nx zxJT#G=imCbQK#A!$501uL4gbT-q}BsS()S+fh}>kvS#A<&>C3m<`p8p**&eyNhm33 zj=vHj{`T=i=KBey*`aUZab(Rzao`K-Sn}X1>zHdsiOzJ-keG8Z-bWZ$Q+Qs{e z_GMbs`lS4WjF{I#(}UA%iymPD>5n-}_Yk-|Zfbkan&*^~0@pBXu!fDN+5(5)d-72i zR)q|?&Ipyn-DZ&hp@M`2){z*Gz_VZ-`QdmM^$NzP0TmwqZYQ-ACxz@(NcOY*@6x_p zUfN+$>0}utWq^I?ACmDHUbw{OFc~&Gr}NLhGR4Row|;*N4u0TSueiKado+{%lJ2c= zV~D@gJN@;!?!|4P4khM4O;!%a)YH1$T?F?EgU$4l*xC%1dn-vFhhw+9Q3L-R`|F zOiMc*Yht1awheDur1g0~>?*&LSC}VtaGOBh=?U1*&(wWUp|xupFgV#f=O)iHo}UZ4 z7a-cCVxMWm=av4#di1;5O22vqUwpy&!Au z(+H)rRZVV zFNIOjLFX)SZ&rs!JicCSVft<;-k|97cy>oq!=N|M0XIN)@x$Q6U%T?Jg4&GZLs*wu z0z`94;&Cua9@9I89D0$2)wIZAC4^VCL|U>_dJkgVPF8(w=gM{xY_%k-lLqp(=iW8XPv97t3kO0>>P-_#167@ zOzn25Cl>{GS!-y&zRbxoK20%Udk8*QRaI5?+*1|%ga-|Jk}hgw*nycQ?|uu_47$hxF?mE$&makGaxF3fI<-cW6%6z9TYnCT*tBCuuhC0dO5a8@|!!`W&n8afe0o z7Q;NrOFC!+%;HqfYqHLFRkV=N3B`@?^2k7`7Il|7NJ`OF&Qh2n)f$ej``j0$)Vg$A z=*4{z8JU5Ef8EzNHj+)J?l{QuQZ9!7gV;NB63BlcM(b*(_@i+lHARt^5kD(FZFH zFI9HP`Iuwz@|sXwHLe~}Ag9;5h34SmLks?PN`!EqPe0`bH2?H1{RvIhJ7WE#*wyfWZ`I%YH$h~i86VrMRG`*B z?Q7*i9~~8r)4-e)ju^7|$|WhhA8M37G?`Ojlm`JOT?vQPG!Ck2<9{RbkkuEYi6q#_ z^uui`NrJMHE>9(1`_ru8HH##Z z%vpJz2(+sTum|%pCOEn5zaKsOq~5o*a2pYB)rP)Gz;{A40oe^?80cT>zJLENU=b;; zf5w(!vV2e!<(`W+#?i^{EWlxxW+uIvh8;K4+}}g+{X^$g^q*ySg{c-6wL-sSqxFS_ z(2$VValgo)2evY1uVSaO>Gcd$eiw-Q*Y)7=>X9S_To$xjk%OW-*bTC4!MitRU;o-p zYUj}TV&nI|aFr<|FHfylKQJt8XzJ|5?zvZN{MnGJ`} z-=9kZ6h;p3WvW+fcAwd|&kBYYwl`LjI;YJqEditT2p^w_^Jvmz{=TEq4SmBR^yz%f z51b!%n5WyZLk0AX7x(G=$IMvdl}j%$E@tF|_}&!!!HF`d?J6xMVb|{g=i*kTzw^%- zM^9Fd>mN0Kq%kQwtf6s`mO4ZbCSE;+J@8_JAj3=OC4;N{zKBJTsHf^wbu%?)XR*Ay znrs6wtG%r)dIkm#hx47)5X)oq&z;w-mtk~XZ_i6!t2}NwtXMCUu{Kv4dEsgMqyi)3$FOyt$^RjBfonBmBLC?}<=>5z6 zp)hTD!KU*4of~secr{$nvQg6$R%OEt-QwcNjs?K>GBa0tMdju;AbgJ_i(@wD506#+LSKJB zxN`vFZN5CQ@O|F$nOjLnw_6;im4vmg<+9j6J#yF9$~aM*`tkhBV%~JPJ+8Wl(->g~ z<9#*88%h$-j2u*@O4q~#t618~bNcEkgbHt6Jps;3mECV9iX0>E?L2l0iFuM}M~Z5S zgp=!F7*u!O-Fw?syfAy-s3mp z&NDDHU0i3JJK<_eWO9zSrK{JHpqG>$H{X04ZDh9Mt}xxjre^cmgt=jK^axm7c>JdO z?-j)~2ao5kuf}@YhmuIks1A>(WnN)Kd!|?0F_SZMUBYEyHRaT;Yyz|OCY`9^>ho%A zHJGwI2dt5`jm-`BPpmwMxt%KCIAY}u?tF%ZCS%`vU+Lix=gJ`@6usidgL!Np}(ecr>iS&wqOWr9gqdA=Cdj~y?M z6^k<-gvVjZ7PQ|ixnN*`eS4B=$JAfzGi=CC$p%AC3GMGAa1g<~q-Wg;cnMRng{ZBt zEpTolt`(G{_4zPSt~qQdUkNdL#;;xx>nYhG;x|<2zU`Ra4+NLO!ddtF%Dd4)UjW{& zsHmvX{;zK~;P@xq>>=&u8Md+>>>0G1v@l-rFugx znW*(S(XR2vP|Qrk7$CoZ=ZOzdoqS{d@u3I7f+@`8oylgZShf7hR#8(`n~vwI3&Twf z+Att?k-r~%!VxW7!L9c2E;%P`+|snfL(d54KLH8$t3>nd_ZK~AFX!UVSZ|LV$W_eD zM^eXW)U9ra#_LiDckh^V^{*E=(itlH_Arwemj7A(D%@D>yOq%}`#x&3SZCE~1N8i< zDJkE&C!^o!6b>)^;F^AT+`QoNeDUmgUe0E&pY+x1<8W0l#J_SDU0eDpk2o(VAD3>( zcU-G;|0hz+7L>5}NQEGm&NX6TY8GuzgH!napyBEFw#%`2V6uO z;d3}CN&FVLN`h3RVxO<7wiepo`=Q!OLzfx;4W~Nd^|NyO z?b)700hs?GPgG04yI>Zakb9tVALEhk2Og*6UeUH*rXMseadQZ!M)NvrTuFNS(IKhQ zBA3YX(_s6b9(PoBd&geIwzRbD>{#w7I=)bpmX@%o@7-I}?SW%68i9l3=e&}86-WQ> zUFG~a4w>t6YJv({ajq(xiN3QPhN~ZW>Bqw*sBuqDPK@;R?T*jjq!?;Jz{URJoS$DB~-q;ooGf-9Pt#s4iyi{sXFN z0M~=xo`~`3c;&n-NzoPcj&NWbCdX?C9oqAfqr7@F@pmp$vA9qLr6L|dqPyfu(JEV2 zWgISKR1Q3uG7jL(4g`AQJH44ERVEUh0B-|>8_s;w`p3_(0`!(jwSG;?jq!Io1@SOy z(M`9=*!k}EX!Esd8?8krou5}iS_l#Wb`Y_XFQ1FWxQNJX zkz&2@flFBH2R}bQm`NL-EpI6+I|vL31_nlsM?i$*srOoNU6@=~<6^e$HgZ>q#o3(G z2J+G&0+}|ZxR^*>an-edie22J!AvmO#K9Uv2-G)u)h~c-3{*e|%T!r8IZiNC9vXZ= zzGY-+Ncj-`=Jjjg?F;7Y1Nlz8>dRgW}5Y9ZQe7K_Lcyq9SG-c$0!c0?xUZ^?qa8fSsyU7xHR?(XiM zoSX#ywzbs&5G^45MuC$67KV|DiI9M|m(zaW=$+)!++6+Kc9VkxLfvT=UwUFfa&C*p zgiv_~g(-lCJe=265<3C!G=^v8L>SdS&-h~+;_tcC;Vu-_tbC8MSu{&H<>`SGgm>B2 zWy(Sx2LDZgcpjQ!pzmdN_ehMaAM|&?_&^&6J3Bi!H#ER40I>>P899lGPkkC^Ez3bI zgZzR#RhaPm#Pj6aYcd}y6VrwQW>!08Z}##k+}l^A27mU|?hon6I&RrZf-|hAp-L~- zwB~8x8}P;+%2(%u-YXD6(9qD(hPQuU9CU5eExLIf{M-mqEkF6`X&Z-0h6C7FiSQzKbu!baRrq@StHF~PYr z_O-b&jPC<}hhpb_0s=xpVL?GzIk}+jUg8goia@RbcmzhGnWE$m(Rp}ypoj9{?@YfZ z+gv^o!bNMNzWSP7Nq23z^UVI_zr%-L)RjstOHbxsLR0a;4;Ch-JGXCxrMrUB4g|&2 zAa-@3Z5n#8qoSfPP~DS%Ns}b}!q4k`Y}R7U^r7NVuX0bmv|oxOsdZ>3lWA~a*A^yM zLiblJn5I}ZYzpx-0vFiX(P52wQkgqGXGv!?FR-1EK%>GV;3M+5h;}0RaC)mE+?1C* zqIH-#lGC*z^>bpWrJo#;ROMy0qE+uAWW^mzpOzgbB z5}Wdel~;MgQakYIrurGvlu~-SQA|`DABVVkpXhE#0PxDm8Hl0gMGlh!LQavN!wIxC z;o-yJmp2}nKx1_DNz+v+(AC(qA^S94F!t=zRigFV=Q7<(5PlvuXe@s3q}dBHGWGCb z7Tbp<@HEuGPz!7%VAg{eZm`btcf{!@(^l5CYLur&-w)K~ZeQHb5-@Sa>^)ku+{h0% zyg1?BFyppX5cslsomB^HxIoV2baC!1nNne`Rlv^7ECcnN{%?(NVyU^uUp+`_Mkl`1 zHwdi)a9%IoGJ$5x;uF)*D6k}8StvH&^pa@$ZB%)z&9aD@1#l(C) zJ<$T12^l}MfUlO;W&_C`uiN;xqsk}6XuF@YR~f=xghSa@E$sH^;;OWzG3>@8kM(zK zIEcESn`g}Ie&3VONj&*Hkv8nF4_NYrMnZA_JF?VoU7nkh`Gryri_R;BWfQ;cdjmRs z21HuCv0=voVLV({msCFJ!J9w*e9~2!`gE6aCjQyy3q3#E-IB19+}?BVv1i2>M3kq) zC^OUPYmC&)gXLn-6n*m!E;PdJzbl~<#smQ@F9cx-oziQXbEI^B)!R+_t4^%y8h=W) zW+wkZpztt}InJ-X`Plj-ai7AcS{`kqI+>$M+^;c`Ub7A~XpR{+HlV`@5625{2cMty zawfcF>}C(Y%FR@SVWHAmxs}ykj^%)#)lZSTtSaN}GFHW`jwte~(&FOPKx$Ltw%6Cs zK?ZzZB-%_}N_DQaBPhYX7t-vL*)}DyPgjZF-@BKmlqs2!;cnJqcbxzIi+I_Qui-(*UoK1yW-{U@ZE|XAE{AFJ`1OxsinDh-mlnl*AlQG z*wMj3K@QfBi(fwz5fh8yb>`f+b~lcV!BYKkd%S+l%#d^J*U7vr;==DGTX1I@-3QAf zGTv*8(RAps)q@|beH6`ws%reGv@49KlFI;-&=FqUyxe;_4#+0yY3<-paj(lF5OGvA7Z>mRcmoZoUP?=A(qLph zHZsCo>$ahPXVL$sv}b-=0OO)qX({^$@#kK3LH&zqojTXD+k<-?+`M!U%$b;iozhGP zdwRC!TOWW#7S8lV_FuG4;fWjeHEBbi2glFiQo5bPHk{~{^dgAWhkNX8Bf9=Ziq$M8 z04kQ0)YjhKT2LXNsyYeVA4))a=!q0Kkswk$()aH@G?8$U<5}-e*3O*CYGl-6uopuW z4bIo@1=Ri95pVJkjrmCRPt8BRfkwhn73l0^5JmyLHqnc0Mkf91g@AoP?a#L;E5mm1 zd(Y~>_<4yC@Y@BoMromE50NdLG%u9w-89wLV=R=RtcHG7P~JVl5UO=KlaY~$BLB@e zIH-VuLn|OCpgQ@r`}^RLt?h-Ya_U7o%SL05I=m-q=o_wO=!4cH@cWTIQ!igWjn7ve z93BQDX}q^LuyY}Dlrz%LcK#M~?qs#Ds>`wsUwxk$i?Fby<99!QKORx(=8+M0M#e&b zWn#0pdovhFcpRW{jU86U#)$Q?pFMr|p32%roNR#DZ&Ttd23a4Sjt}M|Vw?Qcnx;e7 ze1wQ^Kej@IPWsV%R?hezJ|p_`g=0cd*Kt(yr!lW+`_$Aq#B>uDR;ao}bbgFWH$u+9 zCEaKcN-SoNcKKNTCS3uw5{>&o8DVKZ6TJ|UK3QwyCg?FyQJf&Q!4mXIHcK~KH`1yy zck5Nro_pDJOp0lL>|}!_?brEkP=gW8C<{SwuJ?VWwzxk^`^Uj>`{!v+!>rp8@RrxY z>}0Mx%Qik>sRTmz@$vDZa&9OiIfFAZ^?p~G{wWiX-!~!8V#F7tRgdjtp{ED?I$_w{ z%8xex$S5fhfbK3gcVl6Jxcu1j=S+C$5^rS|k&V>x#Th-wM9+CbkE+KamcFNE>Heaj@E;nm! zSZqhJQrX64A0Rp4*CNHz!1F&Via=6cy~h>By)(3XWoU2^IKb>T&sP)o6v=|Yvwj%z zq=tqMq7COiHix+*$XoUl_gm~Y7oR%GUy}|qh21)FG)z;s zxlNQU&~%+}^~<|CK8L-pPMhk0{N}eC*`;`A9y%XJ*I8TtFI3`OMGI+&7NLq2)M~|) z41YwVWMfmRTUeex{WUlk+NgyY$z)z9i#D7t5ggA^YAli09itM*DelBQv{G^waVObh zlUSG2E@4rF#>T&bAU@MH^C&leW;oFy_rT*k+#GqFWULlJt30?VS3XwS4#>;OqU7Wp z>+16F{Zv0=UamqGoCzg-c6KH3q9Bid{5Jk26u{7|jf?)JNVjX0{ZCB(uSbO&9%E0@ z&e64}Zn3)OxAxoMGi&!|kUl-&6DlU=X)oQ9Vo-B0UE4+)UI#b`LW-J~olP2lE)NyL zm-Rw85`h$mJw^ZDIq~f(x8?!J4DSU?v>Mytb>1MU80pC#i9^gtu@cBy1q1|GJ$anU z2j?BTO!-O+#a$NXcNz>%ZwOarYkLqA@S2}L5oZ!dRaO;m(e)iX%SUmS zW*$Ul#bzXse50nMWEG$UdZ^+N@;L+wN6x5B3*uy_y-a_D;=kwPlm!PR& zpa}obqc7A`H65GJMxER(G(t2IY4x7?^m1<_R(rl`RJPAx9fr*5E|`RNx)-2|BE(Dv zapCO&i5Bv12LxSY84D`Jex)Yw!SpT=FnO|65GEVcd;M?x%k&G|?vG z8?_3+1i`88d+4-A__%gxzn&7t#yZUCs1p!AF)9(T8^5L!8dRy8yOk3rvEX8^Z;J^;h`aa7^d0U3MVax>vzBloaC!V`{1_i$C)Ny{>pAn>oU)WDaB)Tz=aTyl3 z@B8>HM$MHQf5htkoSnC_ybQxf1mdoV1J1U7(w7i+^as09)@I{MkA#Bqzl4Z?XdtAo z@20(h@g^XZk+K`&u4!Pj(G9+RQ!fy|=dV4Wv3gm6y*B_sT#CXAT?PdO45Gp9zv+MYvTWG;n!7Sea9GPtk?~o@;WG_whgEmhXnF)6 z7cncWzVdH-Jz1OkH{r#e{LI*eS;hBwEND2{fkK;S?4M*WT>b!j1}*4#flV3%gOT=| zN6yULgyq zNEC|^*n12N4wkFX1~h9IC_p6QA?E)xUfSK$V;)r0tW_x>T_#m~;LOYZiA_D&ph4S+ z9V-UsgB@?(%ty<%i6PjZ7T*^n4;=0w#At5heYrtC5LqzDZ}Tv^aTKHlFsl=BpZUIG z{|$>lfMS2L_d+n5b@(p|xzQ0{hTv&Y%3vPK8~{}A-X*Vyk(N`M{$)J}o~B^@ZdI;Q z{V0r}sN)f;3Z}rvjiwpXfR=>vSd$&5rP~2S%_>AZ=D(ig@yd(;l(D#fPV=`E(Q|R1 z@>KT|>wZ~!x*oK)BkoVWy~lre@-6_iXK-*3qJnd=-yMtFjsq*V9fW}Qq?iQ?SbCpt=?2(%L0%0D; zP;UL@?th^@LgYZl8jf)+etUWzKe0)&O3Jz_itWU2L{eyIUhi%tMw_ouhxrU0%E9 zVJgRRS-MqDM+B>`Ruzn$OMMmZ@VypD_DPD`W>aIc!J;FVmQt9wc9Ir5fz4!WKodok zDpa+TMm;nFkBF{9{+%qiBkd$K8|3yC{2&3&R1payFY) znU85d9gf)dRqM~kyZHxWy5|`>(=lW0Gv@|xdF67r*Kju(Cq5F6&rbVwYFpSf{M2Db zq{me`?Zgj9(yw)-P)F3Wb>&a59LmPbOs~~7n1@*maK<~IdsAV`^Ji>i<|UC9#G|V! zq5_^bld(OxCP+yaZCOu*6ImIGIwc~E{Iwa`$*QCzVpwRigfldo55}{G&7S+Tf`K{E zVH}9ZBtfkoymWpjOE>JK-r6-BTYGeMKx}zlYBPQxRy?J|7U@vNr8{P*Ev#Qai*~_G zVEMsJT*r+#5xPM9=wjnV^*!g#!_`}Ubtl|N8{T}|l2$%0N$*ESb`~9OifP7Aqo2;7 z){Fk3o8jzKWv}}@?K>TG`MwQm#0@*;1-$( zQuFcK?qy~;Q76SgKdC3QaoiHB}(2}QjZ*dQG^m6^)a~(3gcp)a=D4?$D zSwB37h5g;Od?kg5xwPoV-Yh=X;z_SiT$3>-Yrn!)t=0ZTl~8*5oo0*;mt$(M+W<}S zx24`(m??Az9UUM!FqVRl3(ZX#;fhjHPXn!c?w!mH3=ac99n-xe`8{sV#C$|>zePVT zp0!;eIDO9a2Y16c*&~M1YQYg6^hX3t&Fd*&hF-I}r+1TlXvdC8!98jZ@;LMN)ukTR z!Wii7g=5!Ak28|FEoyUK-s8n0Al*|P*~=YvR|qMJtIQ?P?GoP57uA`6=TzHDQYY-; z0BcaCvb2;`0rYl$K2*E7AjXuHU=d~^O_mwh3pJXgGi zY^Eh6@K^$Ml_;$ubpru>!vAknT@I8j9j z*$k83(g{}m0oE`g7m3%S6=|(K3{qG0(44)`8#u>Mh@HoSJm5c?qyhYs+z56ngc@G zBFQiRj+$NS_#deIAk9v55Z~p+{W)Snik(oaM0M)$#oyb9R@$?DNCPvp-!RA`-T%AYspz9lk5V9n=jAzAsU%p$Vk_RJxz zMC1%BKzeYneB9&P9xOwpnvoGj%>YY>%X{Gxmju1C9k@!`*5NYN#erRoO2Ojo)`J%gx`UB07bOKNi&%Kz*}`D`Q)D z;SmGO^!9ddPtUFCV%_7Tgo{zucaQmDL)Ln%o;sKCYJcaCZ62V%r_f2lI1@8BkI!gi zY83r<#A2^i>bMJaV8)R2$4TeW2N(a?%EcbUip$P7~O z?ABN-ipEbj6e=0WKNIqLU1+r=0kssH-61C4AqMwmTEssz)8|BqyB9RQvPC$IEF45M zCBO8~7_vPp)vPRC$F|!(@96dnMdz}3=x5wfFr2e~v`E1H*bESt)YMdXuvZ82-lEe@sNj_|zVS^(49)47Fx5k#ESWFxcZ#zk{jsCO_Ba>l6f?R`WP=EX~S z5g9eZf(xZ--QJzs<0gw){q}|qWMpJWyX@8b{;)MWtGs2s5LB~*hj%EADtFGV=(wLG zmr7{2fDqOG?-wS4K*oLCo6T8uu(T5f#2H58*z@pI(>RvURaBHrOHetWh?`s#3jHnB zm~@pG)Q&QsM^M-Bb~lvQP9ERO8;;>J#@IL>Xr1xc_$hdUqe3O^X*?T$bfXSPfMFh3 z@KP@t4ldHH?pveQ?Y~OgSkrCh>q%10A<<^9M|jam?iICh>aP4IR7>L$UPTa^dzufj zaER3eYK&0)s=4CQ^ zyZk@C>CP2nPg+fpIr>fIJ2UH{XG~$C2W_(t%*Tb+e@1%x2zGaV_Blq<*&DthYt)U%A_zLD3fP#;KTOy;z-e2`E*+VeoW$)AHq$tz%F2r3R+Y@|dFQD<{J7XmO@lt?g&h`wrQ<<@ zcGuYrGy_h>qo>TCB8kTT28BXDOMLKcQ$T2O#D)@p$X%Zch6;e_nY0j zcEqb(^S-vLGBD157fa`X(>cuVf9Pc!AT7wD7q_voaUNkU5i4`xET;(UqWF6kV-dgN z_QN-3|rFp|nT`bVYZ+H!}ed z*|xBqRCyHoBPgI!+yOlQqo7OJtHNgy#whGOh3P~vTn!4HqMRIGZVVS2XtU!XS^duB z2sow18*nOA9JuqGoczI($Ry1NDY_xU=ve6bo)z?J)GExtt~g8tDSu&-RYr6$eiq>k ztyN)CaFaMD0x?pW>ws7+C(HBa^B}1rIoo&wkKak_O1dLtYh!g|8N@@L{XSNP1TB-X zWVk+nISd0PX=Sw=i*`WdWZASjKakHxK#A7ljeneZ32WSo+UO3l~AfBF5wB#=-ezDQu-XY2%jR z97U`W8tWEPJo8yeG3*y5uq$20&4v^+J9_mnv@_A6lmTHQNWfh6`ob0DDqje$gGh@v zyErAF^kZIbE;MP+LudQ4$h&NAZf$Q4!aX9QzFbsgTe```6 zH05*KZ#~%;d4(YXZScu$@B*~XK}bjlzmyDYG0ch@u~nOzqBwJ;=M=ADCg7yJHxLpM z0&{aHR=#{`Y$A@9e?@sD@A(m79C>ilnB;=#1$Yq}gC1H=PVV|g4M=dnHUcKP zn`TARK5|P}a77tg&yq#{7bTAuuQTUzABJC}AXH zIbUFN-s5-u9Cs@LDmrIpXD`ih2E8t#)f?VK$o>n52bU0zDp_Ew3uOj|WXB!f2S8p9 zUDPbrKJP2;|!K(%3 z!C`X_59QORnnO8?@GxP7I>?xkU5ZR6D?sKA{0-Pe0v0Ka{k}2+!yn7e?d7!T_llEu zO%n{OzYYv%Y^U>zPELe8U5#x{b))Cl#xvD3r^nB<`y*=e3C~QH-~4zYiZlIjHSc(p zm7C>88bLt&j6WgUIhb%f3(Qci2$5lVE2$Uuw|UXjLG_!88r8q}jMgth?N zYobbL-SJ>;Q@+1Of@2rsWXZN(MQHc4KYsid=rUPZ9K@??5t#e|HY!LG4R^o*vTLZi?E#C1 zMj{YdAnC=frsnQ z!#BM>!96ZrlZ=J%8(AS}ZtE&I17*(MehJ!hYtEQ(0eiVF*pEY|2;us=voUs_oB z;dp2YIX$k=3rWkDpn9r?Ku8yVO;uXD2%IXw(o)-fU=PFl10^o0*Z|;4&2i>q93J(VuCjwY=@bE_TzCpP^osX0e#W{NoFDj zCT6CpH-32QhdhOXB+1j#1vaxOh|}(znvJa>V0uSK4nC_dUuHr$fpr6(53FonyjXw; z1o(uHzdvk0cAFOv!OzalKsL*-`i;YC5m`=AQ0T*aDzL+-1w|puySo42ft8h2c2-tJ z>XJv{eBl@1|M6O~Ky6s8~}ehWvNnNS)cudQ~ZkP)IbOstT`~QM0tYr{@)z z?`dmm%gF^FMRXY2auFR?8{YC3=9fGLmK*pxaR70zySp3SF#wIcAhf{B3N=$sZEbCN z`2jFc`s>=++1PNdF}?+*W#zXC2MY+`A;YW=bc!-16Ederc^&9EIitSTGK96HC_b~l zpNXgQ`n6Qe9_a4?TnwMp{;1z7BRK;tGf+uWQ$}TPhb4%6;Qkkr4qY{(7MjjXB&wR` z`%11>nfJN6b+rv=6#Z^uMDt5aqpA5k8R@WyC@&@TFz`3AKpe|b?n}5G($a|YGyYx1 zu&Sz$JornGlEjlOW2X{MO0k8MAoq%pY`nV0X5ZPL>T&;64X-F8L1Kv_ z6O-RYSFKx@X_s#I7{epag~u~Z;LQc1xBY!>4r406GcgZ$S2Zm?{k+qRQlfgVi$q8% zVHi|18WnQSxM;wdQo&>Sp&y^iUQiA&w>@=GE(df&%?5HI=s;G$y#a_FLq-lVI_)k? zbWUX@4<{!lH+S@r!~r}uAXI=JNEKK*z_SO>NM|S`KvV$C{>DQD&m?gvC@8>N2Z)m} zrvg6KgK6|#t7AC43QaMwvDulK@DVR*aMnhO{EpE0YH~P}b%r3M02i!WE3d4Kb2qCE z);8O}BD}Y5Nh!DSeBf35ZADx8)bGe3UeH7S121q!>6nIOHtgV?(OO!IqB$=LobwOZ;T5*cg;TB~^Sw1&yhtBzdpTcZ& z!iD{Jzzp2-qiBT}WDhN#_X1d#8KMdjdq>UvZCc<;34eF2;t9?YdED8Hem@T-8OJoh zejmiW`_~yKa~j;;Li$IwN5U3)^IGxN{Dbmp415=HGY&E6*m;3IQniUh5 z>JCCfk_Lr@Nn0v+k^qtbnug%7LK`>@fIUL943`2#xqtlld{hErAxTLh0)A0^iR_A* z%Jh#UBqTuBWqENC+#~48$!E#`MY@Y09v;H84haeZBV`2)BqCSCe??K=`CYYfNA}4< zuYm1nID}xg2e&KAX+lRZK*I&AsWX-{MCWsCtjrWIOgEN(ff9L1N&PTX>T6SVFG=5j z{`PI$R8!ptX29SX62O;plbxl8g^%wWV&izdPHl}c=^ZFt*@SnR2zol0tdcphc&6n! z^R|i6f=<-O+sKcqw;*>}xMe9Mz&42_Q(F3mIIRyqs;gaL8T&=*g;IK^3-n!tJ7^G< zWj^&{?P76TWoKZ!!{t2{nip@lVX7`>y@2;LMc_YhTNK5?hWZY4#$-qiv*C601%kz( zhL%>&xUtB4L>0)dG{55Ud%RSH)(vll;tw3Or-9)LWp{U@!2CvI>Zl49>HngCxZDOc zBQPl39(fDUz;fsoY=a>LJl5o4ga&^JkWrw4ug;rc(uTDdyk$5VjOJ{OaffvXxpvJj z-6z2}E2{7`i;W*Xykjq*5h|V9`ovOOmkmZGKn-o7i(1ceiNgP9G3&J*kU!$Fy*X#t6))$8L-Psh zSjJZyGr!SChwG%IrO9YP{V!*DEE=VY4&+&Ky?Daa83R2{RuRH<2(O}{Np1~KcQrIJ>Z>v+?&|APsvu;4 zDCEd80@v0sit!^6EX$0SZ-Q*ojsy+d{1mis;#=MQCU#$!u)w1#iR&9d@M0=>i>i)p z)Qy0jM$N@9*zqN~_`d@0qN9_WYKV=yfGa}q?E3mkrp0a;uHyPRx}d~?so3GpYerhy zfX69P&pg-W+@K!9w@H+La_g}A8f?n2_ptdMU}D3M?;g6;DOrq3s{A90>@aIxeMyJV zm2<$msc^CU1xsjI5koIXr;2v5!6_wuK{yjZE%Kt1+k7~=_FPIK?e61TUf`~nC5hYDiuYev!i3RYP~j$X-Z8) z1FCLDDUsz0Eb^pK{2$AN^kq~En-oAYtF$1V9nN-}02L0LWi_5pzS zLZt7%Zo*jcJKuRYq8I!YdfPr)K842v`_`0qGm3Z+I~gZrN<-7&%!%<@ky&- z%Ed!Y|KCH-$?^q)BWya0zm1$5(j#HEqc5qpG&0@hw-tz8k?b~0y-2T;i{O0Gc>BUd zbH#$)69Zm#(twxO_4#)k&z?!W+Bn)Z)62KPoIf~lBKssOBa@6rgVwEl3d%!B_He-N zAG{^)@e$?rv@~VV17s#IBjYn^Qgu|hbiWTQ$*ZiWSgH$ukN?Pl60Jc??b*r{4F82P zY4Qa^7#{W3&H^0f9&{R*1+`A`Jjh=JlZTxuF9OwyjG&wU&k-t2 zgI86Iy0x-FLhHDRL_#3y7a3k`;}$+UbfF2khxHu{G|Fr|=BQE+c0q(}r>6DV4(Qn- zZ`$DFu52SN=`?m}t2coSxAFNBY;E|EGp7neFWSK392*7=u&Bh&^h4^X6(@;QNC zIzH5Ix4WcrbQ{27>bsS67Xp9mZu!~S7poLCLc)g(p8^9f-yKBXo$K851H!mdDt08y z^$YhC_4W1rg-1=VU+loS;atul^88`1c&GA$NK=`SR9Miq-T4=tBF~I3ROiJ6D_XCI zrcZv9jGP=L*>_McEzHnvWlgLDWA5KN^C$=yNEau2kViMWa}mH&8qa%?O% z<5m3M9Ai`AXSB3rhF=7oHo!B!Xb*L% z3zP%zt8R%MN`^s7JQ&!(6nIclBf1t%h{odciyAtNkJzj3N|I~KxiF6Qm!x^}#FO51 z2*lMsK1@nZ#&wB6U4%T-`{@kjVZ1!la%WDWqNra2Ag^kU6Od`|ZfqoW)m%IQZGpv) z#hb7`Z6GEwdN(mPoa$LsvE6^4B`HWB3Jvx5!(?m;t^^p2KAmylW@2KR`FwPQSQ19u zcl*O*Z3!gDoq4qRQOrb;36s#X(q#2{_M&gjK~ekSc`Pv`TmFFdpLXBe8d-p%!L0(w zte)wZ1F{dd6UUY+jU~bW4MhHidTFQT1xgKA#vbYT01vvS zP3y=VMQ-9wEK&I+*(=WX2%2$Y|RrFjs z#R3(c<3oWDx8G=wN-mCy=stS+DSinMWthmow|ckK|17B-umi+59@3f3&SXw$5M@A4 zP%py%s5sN|BuY%^w{NlrkV)j>yWBl}7&Ul(5y(~Zj~Z5=B)#NbLSS_R1{Tal0LSpV4YfzJBFSl>mF{kvlMC?2U$p3Gw-EF^gKp?! z&H7YbI`QdyIIalFheB$qsu%Mb%NuXi8SxE1WxK3b0^$S_h{y$5S#>4`EBJS4RyhUfCP z?^0d2n_X}&9IYv^UU$AJqF=hQ_X|p`dT?*ra#8?1t!%8Z74B|y(Er2!(hY@IIPZC> zDa69Yf)~A6L_%OyAFU%)25lN5Rd3Q5Aiz^=X=P=c+-+MIz*2E%_p?wclalHo+UX<* z-A?php6xAstZ!~kUlY{q0XVS<>WS}dPNR+($_Q!ymX!+<2c}t%J4)`ifzs^=)Sj}K z+liq6EionY8c6x@W)}1JAs_8rUkXmX-<_igB}t8iHWBxX%k`vufS~ERjw6pnZ-K3F z0@WY?pdgCCa2gLyTM(nXy#I)6|GWRs(&=d!9qko`MNAl|ghY*i7GwEOB}ckllVl2^ z?>Cv5lJY^_wkwr#nsPauf{KcEe~z+Od>#T$fQovx6LU>32-_9}O49OTYdl+4xn`$i z7Uy$kC#57Q$;&AggH+&TOudc=0LB_afSV1(0%4#647YAN|MjP^3pJ@~^#P7PX!q!? z!w!bA#5onr(=$>e3IKb8Ms^2k__b8xQ}p>hs=Jkxo`9cF?LnxGg}~D&&CmveeNm=^ zP}D^jF6j|1X?J#6t=9#8d_guNp9t7c%>M%rzI8F8n`0zyW4ApJ5=~dwZvhPNb7S@2 zgIYG!m_oB5MzSeudwUC`EEY<3c6;y$Z+wORh(2+D%Gi{_)4|b^iJm@XsJX`DJU$~O zy&cen#C33_idPC2cZx&Q!I;n>#!7w6sZhGzw9lbfm!4?^(@X8H~hd;5OB$ zaCUYEGeZMy^d*TW;47b@TWtI)>X9^{dS=BW4HVp=43?29v+G7y8GZ_kENAw+AK_ZXBYb`SPzpse;5E)n$1fc+UW zz@0SU3YnTD&#>Gr358vEd@`q!kntOiVJ9JQ4d5V`k;9kAKrr*Z1Qp ztV95Ka^Y53BVK{VX8*DmU29ZQJ|f{0T(8XeaPV>kN7rsM8eow>&7-yV-4S>$r|XIm z_vH&S_}A$b!wwe>r+1pA$WrH1T5X;sj|(C21fXkphDhWrpl-ZPa(~dA5G+^;>F{LM zW{48EnlWGW>|UJk1hpeNEeQCaHO_|~Blzc)U%de{F<{oG3ETj95ITx(Q{~lOaV0NR zRO6^fX54FoQ0#z4jvkA>n;U=6_hD#S;PwXie}d$3LIz&y?xL^@D-G-EVFe#E0po43GNe}WTX9@4?I!B6ym&)v_t7;H=2w(YWkP=(h?)F#SO*6 z7$XsT`{ejPxCqW$6o(-{`6rYhtE7GSi`$NgbQ+L7WZxWqtu#Z4dbKloV!-k zkTx_}R1EwF{BU1#!aPUVr<-!yk82q;r0O7QZEcm^Kj;H#@c7d&v#HnYX1B4Xqrm)& zm0NPw<$j`W;B?gfh_wbdRQ=nEog%_=+z?K^Z)Bjk+R?ng5uS4A1v5F9buW< z(EQg^t!T;?c)t0*#^%*jkxWu}9qZ-{T=9UT%PwPeUVOMqkC%Ne>LLv8Tjt^h;fv`b z5~<*3vQO;Zq;pk#wu&VxUr1yw(tY0 zHipujoRID88{M}c4{?m--UpY&uG8#$*BAe!Sv6;$^Ki1BDArj|T?jNgab8j^s^hW$ zh~4&~E2O|Lxqq-;yFt72Vs@Z&1hq01P$1|>`c@WhLa0Q5fPJ56aW{`A=-;0~?vR}{ zZ}~B>L*qeqTMzsfyG}z689L&4_bV8Jc+F8PHpaaa!k!GjJA@@xwuww1ur+~+Vf0)q zTw6=$2iUnX5ebBNr8~4xuqq`lfGV`W{+nRfT?RuzUpLw7iy#76!sj)O80(~glcuex zSZF>{7->~{wk$@Nxmjd)hKmZnim@wK$pzatn?tt2K)BQBfskNWczAzh-g$+dp5El& zEvP{M5?>K)5b_ZN;u}5%(KB{8x;ISiZdi*^1j^%yr8&$%MtFAM0yh`T4|MxU_2-(u zK+3OmJ+v81Ee!5LPmIN*1-HSAT1l2e?8S>>osT&sfEmCR6N|LH*Zz)}|$jev~19q1SGt{BCYt z$wez?P?>AnySxiA3=kx!kB;CVf-1&Wm?gxXlYp{lEc)&#I$%o!(K5T%F(JKZx@tDSruTGl8-$_zh56mdEi~p=uU>5)Z0a|J1Ey|uFtp)g9_Q9T zmz9|b#jF1`$A#%HGjqXV;KZLPdH8{esFy)<1JQ}T^eF9a)Z33rLD!0rs-A=Wh?yjz zQW}-VacA#ubbIKIU1M|oEYDE^^G-vRDw~z{r>l{rA-LBULll5O-y-T^NxB3JfnjT;n4|)1 z6PF7$JeZfoVg7>ei&}coY8pAGbdm2Dkw)nIKmtwf>z{aeE$;3{(;*KoJ_a63i)aFu z^-R(H89}UuDNI*D(z0q^01l1{4GsgwrTR|Xy-Vm}$hJ)6VeGR{f6*-^=k725x|W}) zn#_Arj1bT*XWlOvx_hkf)bs2%I8(;(?HEF00m1)iD7Q88(Tslpg4u@(7YIct{Yuqn zmoG0%H;&=X0(#y@gS)wtcC12lDzvM$Gg&`yewCiw?Xw_-sWxdk_y~XH+b@+%5U7C- z-oJlDdveg0t&GH1x*${$nKlWouor0qE%@pmE7~y{gQ;vb0zqaFOku| zM)Ta)-z6aHhV&fyhD~(IeL|ZQ-}QE6p+=tt3Mrk}6UZ!Jy%2h42OQAxajgn1%gW33 zcRRz&DZS3iIPtQo3|cm^AMAC7FTQh3-0N;lrk&5C_n+pq6{BFCxY`ci5dL3KQs$Z2 z=!GMwNL3q9#x=2cp@;1?bbr;RlFNPbYS zsTl5XQ9q&lY`UD!{9NwAYJ?N#lUe^&mbp$N(`@0fT6*6=e?JZ$o{@n8pGp0~c2QB$ z#Rx)M`?Qi@@&0joN($`$U0C|Sz6$fi`1_Xmt%HLDpuu&3KrD2c(y6_6A)lm^sj)s3 z{x(TkwA)P1l4rHJ_-9c(%dTJL#vbfOc7IosO5-?Zj{iaa(JLqqCbP^{rHhaT zvrt%-mj~_6@{LtsTfx5rZ9Mc3&&P)WjmH0U`1cp;d!as#7XoPZKNAT{1FB%2O{Km%0(d|Q|avwYjkS6ciC5L}vUv`DIAM+`7G5eNj(EYea^ z*oHi@Z}fcQg4NnZ08Amv-a|)^g|GnR7*N1jU0z-We%j)qF>rX{0Rn-vya33p{P4d| z)t!UnJ5YMzRs336S@HEPBYmKjQS1_!Evj^5Y>O^nZuP6(m57&}(ABZl)QK}y%41^_ zdzVw3rg#eQ3Vb>YE&-7Pf~Su-Iq&EzwB5AMxMuY|Y_|rot-bm1I67nB0&?HVw|AT; zuD6DgJ{FWU{6s2^Bowl?PGMP$G$^EuYD2<122o{Tg2B)d47lxpvjb@rY>bN3>49VT z@7|E7d^(eMWpQ=&xE%?xEa;U&DG!z<@g3tp6Y1>@ZJGs-^CO^|`kL|C%mEFn9f&|6 z!??J*Y5~8#oQK7~(U3)ph#1x70E4*OW9DlQE0x?g?zdY(&%)AWzdko@{0QHImxqU7 zk8K?`c!0S+wP|xoTqIs09-82#;UXPC!_AbP4V_S%ltN?exm( z#`%@+6Op=W^*t@($wx3H0#j%x{R>{(kveW+hMDl{Ar~Bbtw`Ud#2d|NrTx=AU2m=u z>8O1%o1j0*+bKK#Qm`nl)QZ8Pfo$|7dZ*y2vMv*WpIlPkGYkw2U^F1XE_&Jrw$Z;b z>jR7__FW;KvO-u`7y#(}G07w1+3iE$Xihv!!B__;rvsp>aua=ml{;{!x)ixc4Q=70 zYM2@^2`W3TCb4WoAkKQnY}H6ZylW=pZ&byR0u`xozxVlhTe6F8JRilI$q%+)1W9zU z3F|EFhr0yv*Mg>vVIl!N53G3pH+IUegI=dmAs|5zNybH;k=@5vCw!&+@YC<^Wmqr^ zGqHm}ys+)FTu8}npH&yFw6uE!3WPw4v7Gp}Vv(sN(dU@|eTrYYt=6DD1|7>JpXHv5 zp>EQY;4hwYy5*sW_Kzdq-1#fasZ!0bf&U8h0;roQ1Oz6*2IYvE^Y2rr2(sU!$LvU|8;AWKjMCF@~ zEcj<&4-%xYp*+Qx-^}rAA?$njBJ@QqpJ??H6(>$OY2b-&rg?|&OgnRmi?!0 z+P^ZX&tU7BrA=M4`VeqK*ucoh&(9BHgL9=q(!ZYkHGKohYQ&msm+XH6Jin!f^=n-2 zJMLbWKakCCAxEORywa}f$;4^<*UiDjbpS&Up18d7awoue*a%i%qtrmD`dz`8Cl{mP zod@7Gyl-1^F#P(6(lGH^=HNkSWbrylBJ4eXP^J^mIX2Lj%uW+-ucmTP9;(C zA=UfigxH+(s-}}Bw?F5_%p@A;e(O!0@hng1ERh4JRpj;cP(T9c_XsriqN5KvT@1{2 z8l$&HFB6WnjmqiNVbGgK|DoXbB)bK#Yh@s>V)=W{{rdNi(wYSK9KO zNxUKM^U5(ptW(eJm+qLVI*TvPerd-Ju@_KNDSk;WYg0+bmZAZ_kTp_m{hbqieUv77A?jF7|uUV*@a)!*fO_w$yU&zXi zKv9>+BpY5lt_3s-FiA|{u!sRFEQAWk3Q%Kefqd^TLLtsRq#Zz0Kym_0?C=ZjeAzz4 z+a6koF2|gCaYgN(QrVFBb5UCtU!1F8j zs_1d5y6P?NNO?P`y;0%PEvL*+da+6MD@iB64hjGW;TE~UfoPKg(p@grO$#e+L9r6$ zn+4yT!_-f`9#5=1gHtdXK=9X$-6ZIA=6RHb8Q;s3LGn$5zsYUdMyg{ZRCcoqDYg&mr`pVx*{lxy z{-=}Ec?B@UW(+O83?HPI#buPkOC4aV77nF*6xh(v;H>(s;At^>)nbL3tmwkOFSgOv zt0>fr0jzJh+S-C!-@Jmm;O4UF{&l+8#HR_#@9yZ1*;-9}!j@V`qwmROme|O3ORDg` z{tpV?xuX&HWM3Y|=0{LufHF4#i$KPUl%(+RaIV=G$sTxwhX?xU6Tr!XIkIoy)*I&1 zO75xsZ8xmP8x+rfI8&hA61aW0^5J8rKT{s*`BueM$4=DgYg&x4(goi?BYGJYZ0H>} zYUSrnjw_gVP}k}T|J+C=9j>6^Ywzr|1p7$vd~)*UO`|5j{$L;2`ysGT%#jW!tF9;O zf<+9CS=d!$Mg^U@Hkoex8*@}Qh^I(hUe619dI$+W-?wLD`j3%#vbGD?^&9Sw*37PS z$W(xBr&e4LWmmjq4icM)<2T~Oz5BeSWb${*DS(Fwl${><(T=wm4tn}xB$5>*VbuUo zK1iIjDhq2bWFNe6cRz!xB1M5U(CZ~d7F<>$u)2CUJhlEBWx|F6?dAl=JwDP0KPH-t zYGXA$2no#qjS;zwnrch`%Em`x1oRWJiew4R>fXH9^aNNSXqxFS9yG zHd|0#kvNh}J&K_t%gpIiQ1O3J5^)(NClw5}MYTRrToqO#sq7QuX`ojFxC@3Kw;3Nl z@6Uxtr_^9nNAxpPP>iO$Sk zujc2TAi`hElJUf1ZLf6^Ghd=j zoT!Vq<7SV<=}8hlzqf^XxpEe3Fk6IKMacfH+~D~srMKZ3xm)6xx5YB`C&H$A8z3iY zX=ZgxX>0MELi4Oe?o0c9 zr`^9m@#Sy)GcXVNyx9dVDXGXCZ^=ssEZl%`S^S0uL68+zH(d&tciNS*ITXeJu zPY52XD}QI?5F^AIDLnp+9FN`ntA*a5em5>lH$t-8GnPJN97=o%Oefk3YT3>MVoRVj zsILAvR!o=6@9cnW_Vv*>7awaILdRB`(p>hDkuQf0_tMe*PS8pAaNpi@ipW|pB41uh zTs)juU46`7$?A|dw=%IvJlrta)wK?dVgA^Qqy`YqvV_h}J9ADhD4*k9)j>CdMMn4` ztYx{?j9NDK*zn!GS55S|9BkkIGc)7xG3RR5%g)2&I`7Yo-+^6JK*5Q#Kl?gM962Sq!<7iXMUI4chCO%LeX2c}qy{)o;uY25M z{z&n(%h#>2n}yQFc4(DZYOAP3AeDI-QWYzVd&oG#xcx>c-*2AF+X@nPmHz!K^-HUh zUfwY=^xvkWQAL8l@{PFdhT)8sDeH=HprfPvi1cOh zg-E~DdeF~sfxh0cl9{LFCjj#y`14cZs(pwhzde(tOro!4EdTjQ_&|*Y-X|#S4vvra zH`C)L4e32PtLnKbqY1Iv|6`UCJ=*@Xvccf=_+*JV;XGC#-S69QpUZ!`yc~f%CKSiL zB=T#xe+C-fdmr1|+Szg0FNLcf3lB`6ffq8&H@!0-`5_Y~_1z1{ma^UHSz@K}^&5C6 zCr)u=g#`&iP*Lx{q=a22|6f0nLp^>J@--bg+TM!eE_N9YXYQP+?D)pb%E>uoN76-Z z(!KU9>+Z{kIbD%+{p?>YuZo@n<4fS{Kr>)2t3ytk%i)o75bhv zmN#`LJ3Yz&*5e&bPvtD>v5Z}}OM@6!+d4uPJt3c4`So|&n^;}8_2c{YzA^5RsK-8> z<_>vat!>F^+Q)z>z7%`~gxo)0O%HdeM`NES>GU70Vzq$j8s`cS}=eT&%_| zoYjbIEI@RLxS5}5eJos{PN&bvzhL{%-y$?GiTn05KQd8Wm@%wo3@9HDakN+d!{ca5 z=rCl|C|<;Mk*LNG77vS!y>LH*t&>$CGC=BppIQTUfYT{Q4$A^h9|fk%M%Nb?2c@{W zd2e2yP}4~QvCrMwo;?%&ma7HRh|6xz9i!QwR%cGxUNd0 z$4a?<=*2p3Kd=-PRRt8C@v$)%1*R>3hIW;QT6hv!euQSrruo&!zpNCFwPJV<8sjI) zMTl~_`p@|BU50FWJvh_4ZVssreZnN+$*3wR*`03i8(31)Cf~ddy$TFdZ`{1;40e6+ zr?L{^?7WNYYkHRoG{7{MI!d0n&UcD*jE?jJCpf2q`^Y~I_;<$7KWxzx(@+@kSiIr% zYn#+*$&VJMM!ThR`}Wt-+*sU*(LkxmH@gGGd*`Pd6wOm@wxBQyCE4>h3A(qtF}mo% zr<@%Bq_+)d`#LUPGYaX|@|=g7a}AY19SfV-hK!}y`;+;b`FHNz0WbCUwL6r8o4256 z(tJhkVKc3}&d}+)x>Kh7ZdZedo|)wd3#33NAGPK7rofK z%A8V-Q)!D|SNTBTN^#^Y?^fKIx$|lg+Z*M|#YpA6{+bkIBYXfF_&i(5?C%?DYJTy% zUsz$sCiGPx(Y|*4&jX|T@>T5b`e@Iec#uTN;SVO12!4tk)jBq(_b^P;CFC|j=wkSR z7BKl2Nm=Sm)tY>i(RY(zIc(jWBDuslg#5!uu@N$8G)fkV5zrN+^ui?Pt zrm&wVeTj#^XWuAvi(9R)nU`V*6SbW>_~(Ze7_i~YuA&U zg|Il@ufGa2i_Mqm;6(dHUy|*zZOR+h>9bJN?^~3?iGca?SkJtm^%0?ZJ-U&x9O*pA zzev~O9z58Un(2-Ga$j>JEZvWAEJh-Z%gfCio%*ys0clH?dk}e?=@#a)yR*5q1(Xuk zs=X;=SC4CU;DP|?r-g;NxI%l#`G`-$^EH-p>+z*+BoR9A-Aux`J#X)c%(XSlQ`dMX zFgs&(+464Q>RcM^2oNXie2QuNV}=DE76)K_bn%mjqA!p|da)nbKeCEz-K=;oPoGfJ zl?o6;hO!AJj+#WuU453u7N%?$71{{cPQ7=lp}jrl<%9M5?o?m3mim}2K$R~Ff>G+25r0jOjdm{mG9Vwgo^~Yf1R3>4Ap}pKa9o$(#(CWF8U|HQNhPk+$^Nlr(1?vC)ehclns>C}>;x`N0CSL@odjqUobous9J;^HD$lM@NLp8$zl|M_DOQ!pKZ@u$gY132t526B7%?mCXvCEyBk z?$2>ACc4hFyH(UgI#23=TQ~MCyxO;&^B~n{cf-Ft80M_4Yh*BHz^u4et0gIHO%Fp0V z6f%`TX?)A6`pnG<4`UxICDymIgR<*ph^Xc1q{mR&&jiD&eGE47u1|Z#1){^Tk?+DV ztZf+2Q#UFy4{s%}Y<`?Q#=CX%sG_0*Hd@sgqhjN$h}qD(f&2usWEkAEw6&q%zYo&x zs}o+y(bFqHYy9_ok@cBjUEi5*8`%}AbbXDW;R z^NFbT<*0dUK`)R_TR?!0jW54+$R?c4cppd2XZt2yQRB<32#c~eSAp|D@@j_1`|-~o zzGzch9Wj7#o|V;aROa5OEZ77<1?1NYXL{3u)*YS*EUx`NLzaoKtbN!ByEP%V=y>+- zovYT*`UL)_d$wD43qMAbsnJH8g>%@^%kNZiBk`iCe7fY2tTj9CUDTXv?O!MQMS?_yxo+RO(? zr?T)bkEIQm!q8ptLLb+!50w3|U^BFZj7O3M)|7_cjp3~>jP|)3_^z8U%(4e{2YY~z@wq847b#yGi z;@cyDdFqCJ>RRSh379uMk6TZY7^vQ)8CeLACh{vN693}N6FE#Q)Cu0}x_TUPR~?C5 z1+Q z*tK$MJs#^f5`8CVL7AU3xZW%%eg7mOEqA!aRxE@>0E%_s0|SdBE!}l&2XP9i8fY$^ zv#(!k5MUb_(FK|aN0PfrYE@2q(bF@~16Zx6uMdV2V3y(6wzobtJxV#9 zpZ)NR(ib}aFQ@qv<4g;-?Y9af_t@@w%i1LNAqrpXn>!V)c>c`hHZ1+xA3r6ao^^eD(p{%7fJ}K6OsEO}dfcgJeK$~?*(r`gD zg-oEy`$ztQ?zVQNVoYjS5h81o9fO?#^D8UWHuJ48nsSH>m8H5+AJ5LtCTt<|9i;rS zmyh@MLO_MRvkpVU?P1%Y18q|wp7)$kItz_Kme0+JdM_BY{wRK;ss7b*b@XZ&TO9XK zM!93Y%j+%v%rT9}RT*M@?Gi&6d%?!>ubAAK0&j<%-Wqb4jdN@WE8XENKe9ca@|?2V07CP%w7_kAKRP*J*d$W0%zXn)$cQO}DSXk|}V{l`3x z7muPhFqEj31{H0_JMcUIk`ENVf2rBE59Y5Es*SnaY%nppzMK2Nl|d-+;fyIx7Yua?@^d6$R|Lt z224eZ^uxEiOA|gS4>RlM@srRZ?uH9qVG+ewz0oS&=j^gMLSEtgj>xnDni3{-O5D4@ z!TSUFW398B5TyTnS}9^QG_T(rslXKRK~AA7QQuMgbN4>EeJIzgi^?6yfdA8qTazJu>TZQrt>d~5C>Ux|J*%HOS^JQ`hgr5$^3-`uY40U6v~H&;>9Iaa zT-iz6@GBe_Mw1k@<<=W}!Q7Tbx12W1SAeqx%2G5D(tWeF{Jya;(yM^z0NjKhFc&MF zR}L64T^f>jtgDrLdWBZ+5b$A?4KePReO+y^crLIN{uVY?pETOlwJ=N=b zPhwwE+22WyW_TNu`=8zGUk4m9CVQ;8Z4?2obqgy{qhF*e!aQ%GBNj$0+zsB(`HksV zaxWUp8M~hAwHrYNL(;rD4=O?XRUc7xf4-!`xAPmA+^!!VKM#!Q!eEvs<2qvsBagF}4mIP_KgZGXzegddK z6C{#<@!|zg>8cndhlg>ZK=*@~$BqUz_PMx*2L~g?lM76gH3H z_J^pt`Y-?&u!YudT7oRlV3y@i)u>?9#QKAG5=z(MK4Kbn_3BBfFH5D>RK--@tzs&c z%Th^-BD=3z$0tWT`HOTa6bL{QghBQe?1{RAdn{CYP_g6T;|SHa`y`1go*szxo26nePT)$4*R%7I3MJ&ygB=Vc+E|-ZIQy06 z&wz#HJYxERI8q~50L~SNo0IpEm8wO}NhRQQ92CF%xA3Ya2$XaWH75^{OsAYNP0^%M zZsX02s>S@G_&%)7zl$<6zr61WL*pJE*;^i32c{5hMC*?1s(AJQtUW%$#f7y~5c&{w z-hy1Rkv!c|>cU@^){o5hpJ$GqIdq+lPs&pC(Fplpz3RDewT$;^$^HoXl-_&#iWPO` zmDM_Z_+ipsPYztqfZJGcZ;9S&n{c+4{_B}ZPtZ%HG)Me(VZ9#uC`@Eb?%Ak5_Y!#qhVG>b=Cd zS1L8qLoJL~t;FBB|9ur`B?=L{YxP+- z&(6s6&S2_Y>n9Z(l2z39E$3$E!A9C#k@u*_-q9$zedVO?4m4CJ5SL0B`koz1psKL*AY>wO=kug_?DD1j}8{ zyHbgLrh9pJC|?(9?K+0Q>a390O9uMe!T&bfo`ljx@ePOBz3ChhYm@C;_*YnZ|cZ7|AZj^LKFJiAfVx}UKgDf>~j>MjSv?T+jLUZAE(a?I?Q@^2!(h|P+dFPJrQ@`=FS2>*B1O6|B+0Da~Pg8eBpEqFnAYS%~21#Sb(dFlgZ(l(o zp8r!oV_dvV)}hWvcYgt0kM(z1s*O)6c4HJUCpb6;|BN2-LWD2S#pTH5ALh$*9kN&8 z3`LD9T&{vWZT|FYcH1;ThxBA0>fiRd@1zTU-7!MpZz#0;uGu?yv*8vdR1|#g{(1sY zgx?WODRnUZS$;4~BU0|}lajccFPxX(;pg!c z{gxP@XS#t(rMG`*M-TrPcB_cls7P4e${kt;7virI-=e_Ew<5&*tsvgs2y>9pwc-A3o!?#J7X3 zI%VZ>+^AEfkUUH(*B%x(#&4U*UV+HLd%f*Cb5+w*rNlxB$I)0T@<>ZLe!d`$VKPs5 zg6U9%aYy5(er`?Litv zf4vim!t*Q?t2b_@k?fS?tHyKhlPN|b+i0(_Y%-7dCcOgHd>x1#7Chej!B0Ngr2hL) zYBx#w4b0c-%8xQdCcZ!fy!;+K1Xp(@MmpU?;dLFA3|0)8dNgIlvxv50Vr$_lSaiT3 z;_7Z|-TL`o-x=XZQDytGa8g)Dl+RZq#E!#QT^jBB{KZI%RC4M3H2}k-A^#luURh^{*!F{{w!j(QILxWKbAJxVLvY zcVXN>E*>Nabnduc$K-uW`TlR|iYNf&Zk63+$P}5}JQKM(q}`iWU6|mv_lEmo zfVm9x^ud#WpwU|@5?KYHAol^-Pw(&)>%Zm)8dh_8>KwtDux&v{HAd!*7&%znzJ2>K zw~ay5oy2APqcO$bG3J@m6^#ZT^FJD}Fa9r5Ziruj9h?X+Cyjgo;CMt(0j}wQqii_H zVfRe{6O=>ux3>xBM7RY-*RK6<$gQxsetUfksPkfuD z?Gw>q-wj}NpD983|J8Qo|4?sz-0fDkEafI)>`V3~LS-u&`x1k}3@TfeG#6vKCfilI zB}@ny`4gV`ZCVwr#Y5rz^h^E$JG0zLQ1j`0twyxa5X8HvA$%n^=}W& z*w|eFo&=IvL&N>6<2BWM#J$t;(|r_ya@3>qcuH zJYGgQvKJcHnF4A+9>gL|st{0zu-gE!c|#3S?*^@N^~eDBR>%M4AtR4Cd1Kgk-{TYD zB#%E_(!~OBhJmA}WTEDjo$ zB`aeImDj|WVGNI?VUiEZyl}3gNiY4odb}p`Q(**Vs%7FVxIExh2*BkA_vm-@LD>ps zfyhW#KvD7WM+?wsO|dEXdL~D|4L9 z?0o@o2}z0j2FN05c8=VKk3zI#k%|*_JzrSOC)IabBA5R*TJ>!!V5Ysf&N#KQYclRAcT--|#N$*okikb^6+0Z}iBG-fH4F2>Plj`o1N=N)N4%5H&ZgZ!` z#EQFE5`@~?7Vn707JWr$i6S}o3{4X_!dcmkjK&UoO^KNb0t_5P+vlFfBbq`*$ZFXh zCc&Y2$e(uniFt#k&bU$JP>xpc&%4*0nCT|jMu+`}F9r1T_e#$W*ka41-k!O$?IM*X zA|~bmgZW_JW8l@JPno(qaQBAp@g-=JrX{wuk|8m%Pk@->MyT>S8;bCt>LmY=I#BR# z@7A4JCXlj%%RRqQnO&KEvp`KF41?KtF1vJeOaIQ~4bV`2CV3!xLw4d!kI-(pDu(ky>@f(r1s zK#qz&TVwGeuUMRIzg&MYspNRF!q!B3%%`3|i5M04ZI6%Z7cR~{@|W*!lvv$dri8rH z&SOeN)58A#ay6U1I59ju4RXz(t8so&_1nOs1d4Zr>E`QU+e*)mJxqLmmkSvkm$24TvR>|2ixHdF2m#iuLf6*p0m9 zEV;g`;??#0;|Vl_Av?FzJ&&GgY972!R7{3 zlg#vVJo9Dg=jYz!0ZPbukbf^!*T+UGmZ;96`}jy&{~Mm4d@^IBL;K@1=Jo;Cf&W2hZ0@R zfR?wa;Px`yfH>|EP~{7-)jaA?IIKb2KbGnNKMYJ;A&;pj((GzZH?aO+qpt(RmLhVs zxrAEpT^i*kiw+H40%YRdot^XO)!#(=B=5eFvfo)<7hP*h^%FmJ0h60pdxU1r6LXf( z_ezF0(;{zvfM(b?GrjYlly_;%ODlZ=&j8T*Np5b1i>m#=(6KB7x#!dwa9P;>Km*jA zaTUGlxK%@HTP}Rd?TZkfCdzSb9ZF3tKa+8Q|3w&1u~#@%&U;(j7XOhvx)K=F4%DrH zKISpeGx42%SY&4Vt_J3KL&5Cp8banB!^~rLOL@Y(Q4!k(kX`m_x}TDot%&$WFW#Y9 zF={{xGD80=^)<^rPZ#8GEAjZh)<2-~Q)ONWOd?@h*+j)jx`17(NmWly<+T6LLY%D? zpC-n6rhX{O)8a~uspE4^%Sj0m_05T5jcR&kX^~tyE81-Gk}~M8F5Lm z@WuZ9y!SWxU_ILUU||c zdL+DXZ7z0#EfAlf|O7~JyppWeiz{H`(Yi0o63Pf=g6w1f~nphl7 zhM=*3zyN#m3ld*ziV1Xf-Q1Q~00cj?7l?9Zek^x*p6R39@d4Mu9nY9fSH+0HL4sj^ z?a+)$f~&`SR(V6usEyOzxk8O5K$6I^@%It}{tIevJV)k8xx>}5lg2A*B*fUr@`VH~ zPuyvcBH1k?wKmHF!gfIcuw+Cz z68epK`K6&j!s8Giyv5e2j84ig@UYT_H_RiP-~KGiIRHQ7ofg**g^%Kh(HM{WwJMhGN!P;i z0&TrJSK19?8(<|PBOlib$ubB_L7fzE^sH6bM3E?%%c;6L^rko0K)@$w+w!zR)ml!A z9Cv$nKZpH~D;Lhk^J`b+)`Js?+caofb4|Eww%_$d>9DF9HlTro_;|p*?v|M%t3&tT z^tijJGA9wN19yY;dTrZjUi27hKJ~PYYm+82yryZ7fw@ZOnaz;!qXT zq?)~?M!nTPc-cAGR$Y@Wi0UDq_kd@@swgfCg2r4MM!Rzy4wGo$1X)z|Q9_!=U7#%n z$P-Ug`B~y0ABdDc5UA=9_h1QM8%4``=@duiDD&ILllM8-Q_GC$HeBnyVCo$8t@o6SzG<6-sqGnf^*`KeeFtrdoQpX;K|;>v3G*~sdlae zM?TynFRsy|Dl=%^MiS2uf*9lFA?$nFEX8Ck{);CjiC+bf0`oebJgUZQ#On`~6Rq(5 zRA=K#_%yZRrE->JETUE8rH_BMLhqTMd~2U+3#9$SmrZ;Gf`_lC#3JT_N7ZCXX0%R` zZgQ<|-_wb1$z!UXJQwI30?~Zr2MBenH8p0y-nr!O@@aE?^3I924D*mW1;E(^1|xk3 z0er`0Ld`M_8f&Zi(#>hFr0#ndZq>RWOug-w>iE7;hUCMsEro}ji z!Zmf|*ok|drE1m8vJvI#2mijd35ivsOco$(6_GRrua@{d%h1(lE(QfC2~adlw3n|_ zaZI&4YcWc%2F_jOA$ygz9in2tM%uMa%Jn~I#ojkeFf_J=oB7BN*`xYZ55{F&f)^vP zlGh7_xdWqvIXm|sts^W#;yuI_m&(Y*a5jsy4Xxcldn|#?^Dc?hIO}$^JDp0;roPEg z?-R6q`sh^o!#r5PUpvI`IpR&kc!9l^m7ZpJ6L>UZuEj(JLf?a{`<>xlSG^^a^M=Ces z$!)D9OhzQohW#6q`hwGP#w6|_d^f-P1uX%#T*Pd{(RLcN_EWaYmkN?e75s^k^U nav2_amcl - 1.1.6 + 1.1.7

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 2e9730ae56b..4afae2a122a 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -146,11 +146,11 @@ AmclNode::AmclNode(const rclcpp::NodeOptions & options) add_parameter( "max_particles", rclcpp::ParameterValue(2000), - "Minimum allowed number of particles"); + "Maximum allowed number of particles"); add_parameter( "min_particles", rclcpp::ParameterValue(500), - "Maximum allowed number of particles"); + "Minimum allowed number of particles"); add_parameter( "odom_frame_id", rclcpp::ParameterValue(std::string("odom")), diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 1767bdcf329..6522072368e 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -129,6 +129,9 @@ list(APPEND plugin_libs nav2_distance_traveled_condition_bt_node) add_library(nav2_initial_pose_received_condition_bt_node SHARED plugins/condition/initial_pose_received_condition.cpp) list(APPEND plugin_libs nav2_initial_pose_received_condition_bt_node) +add_library(nav2_is_battery_charging_condition_bt_node SHARED plugins/condition/is_battery_charging_condition.cpp) +list(APPEND plugin_libs nav2_is_battery_charging_condition_bt_node) + add_library(nav2_is_battery_low_condition_bt_node SHARED plugins/condition/is_battery_low_condition.cpp) list(APPEND plugin_libs nav2_is_battery_low_condition_bt_node) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index 90c7adda745..6141aa24eff 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp @@ -193,6 +193,9 @@ class BtActionNode : public BT::ActionNodeBase // user defined callback on_tick(); + if (!should_send_goal_) { + return BT::NodeStatus::FAILURE; + } send_new_goal(); } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index a5fe6e00211..e050ab30382 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp @@ -39,13 +39,16 @@ class BtServiceNode : public BT::ActionNodeBase public: /** * @brief A nav2_behavior_tree::BtServiceNode constructor - * @param service_node_name Service name this node creates a client for + * @param service_node_name BT node name * @param conf BT node configuration + * @param service_name Optional service name this node creates a client for instead of from input port */ BtServiceNode( const std::string & service_node_name, - const BT::NodeConfiguration & conf) - : BT::ActionNodeBase(service_node_name, conf), service_node_name_(service_node_name) + const BT::NodeConfiguration & conf, + const std::string & service_name = "") + : BT::ActionNodeBase(service_node_name, conf), service_name_(service_name), service_node_name_( + service_node_name) { node_ = config().blackboard->template get("node"); callback_group_ = node_->create_callback_group( @@ -128,7 +131,17 @@ class BtServiceNode : public BT::ActionNodeBase BT::NodeStatus tick() override { if (!request_sent_) { + // reset the flag to send the request or not, + // allowing the user the option to set it in on_tick + should_send_request_ = true; + + // user defined callback, may modify "should_send_request_". on_tick(); + + if (!should_send_request_) { + return BT::NodeStatus::FAILURE; + } + future_result_ = service_client_->async_send_request(request_).share(); sent_time_ = node_->now(); request_sent_ = true; @@ -240,6 +253,9 @@ class BtServiceNode : public BT::ActionNodeBase std::shared_future future_result_; bool request_sent_{false}; rclcpp::Time sent_time_; + + // Can be set in on_tick or on_wait_for_result to indicate if a request should be sent. + bool should_send_request_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp new file mode 100644 index 00000000000..37a53b41e12 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp @@ -0,0 +1,81 @@ +// Copyright (c) 2023 Alberto J. Tudela Roldán +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_BATTERY_CHARGING_CONDITION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_BATTERY_CHARGING_CONDITION_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/battery_state.hpp" +#include "behaviortree_cpp_v3/condition_node.h" + +namespace nav2_behavior_tree +{ + +/** + * @brief A BT::ConditionNode that listens to a battery topic and + * returns SUCCESS when battery is charging and FAILURE otherwise + */ +class IsBatteryChargingCondition : public BT::ConditionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::IsBatteryChargingCondition + * @param condition_name Name for the XML tag for this node + * @param conf BT node configuration + */ + IsBatteryChargingCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf); + + IsBatteryChargingCondition() = delete; + + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ + BT::NodeStatus tick() override; + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing node-specific ports + */ + static BT::PortsList providedPorts() + { + return { + BT::InputPort( + "battery_topic", std::string("/battery_status"), "Battery topic") + }; + } + +private: + /** + * @brief Callback function for battery topic + * @param msg Shared pointer to sensor_msgs::msg::BatteryState message + */ + void batteryCallback(sensor_msgs::msg::BatteryState::SharedPtr msg); + + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor callback_group_executor_; + rclcpp::Subscription::SharedPtr battery_sub_; + std::string battery_topic_; + bool is_battery_charging_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_BATTERY_CHARGING_CONDITION_HPP_ diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 49cfeffb4a3..3f199f3d99c 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -44,7 +44,7 @@ - Service name to cancel the assisted teleop behavior + Server name to cancel the assisted teleop behavior Server timeout @@ -109,7 +109,7 @@ Path to follow - Goal checker + Goal checker Server name Server timeout @@ -218,6 +218,10 @@ Bool if check based on voltage or total % + + Topic for battery info + + Distance to check if passed reference frame to check in diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index 68e00ae4d85..30442368aa0 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.1.6 + 1.1.7 TODO Michael Jeronimo Carlos Orduno diff --git a/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp b/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp new file mode 100644 index 00000000000..c9c05f68753 --- /dev/null +++ b/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp @@ -0,0 +1,66 @@ +// Copyright (c) 2023 Alberto J. Tudela Roldán +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp" + +namespace nav2_behavior_tree +{ + +IsBatteryChargingCondition::IsBatteryChargingCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf) +: BT::ConditionNode(condition_name, conf), + battery_topic_("/battery_status"), + is_battery_charging_(false) +{ + getInput("battery_topic", battery_topic_); + auto node = config().blackboard->get("node"); + callback_group_ = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + callback_group_executor_.add_callback_group(callback_group_, node->get_node_base_interface()); + + rclcpp::SubscriptionOptions sub_option; + sub_option.callback_group = callback_group_; + battery_sub_ = node->create_subscription( + battery_topic_, + rclcpp::SystemDefaultsQoS(), + std::bind(&IsBatteryChargingCondition::batteryCallback, this, std::placeholders::_1), + sub_option); +} + +BT::NodeStatus IsBatteryChargingCondition::tick() +{ + callback_group_executor_.spin_some(); + if (is_battery_charging_) { + return BT::NodeStatus::SUCCESS; + } + return BT::NodeStatus::FAILURE; +} + +void IsBatteryChargingCondition::batteryCallback(sensor_msgs::msg::BatteryState::SharedPtr msg) +{ + is_battery_charging_ = + (msg->power_supply_status == sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_CHARGING); +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("IsBatteryCharging"); +} diff --git a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt index 21fd0b29409..3e5ba4392bc 100644 --- a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt @@ -34,6 +34,10 @@ ament_add_gtest(test_condition_is_stuck test_is_stuck.cpp) target_link_libraries(test_condition_is_stuck nav2_is_stuck_condition_bt_node) ament_target_dependencies(test_condition_is_stuck ${dependencies}) +ament_add_gtest(test_condition_is_battery_charging test_is_battery_charging.cpp) +target_link_libraries(test_condition_is_battery_charging nav2_is_battery_charging_condition_bt_node) +ament_target_dependencies(test_condition_is_battery_charging ${dependencies}) + ament_add_gtest(test_condition_is_battery_low test_is_battery_low.cpp) target_link_libraries(test_condition_is_battery_low nav2_is_battery_low_condition_bt_node) ament_target_dependencies(test_condition_is_battery_low ${dependencies}) diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp new file mode 100644 index 00000000000..46af6d5c6e0 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp @@ -0,0 +1,130 @@ +// Copyright (c) 2023 Alberto J. Tudela Roldán +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "sensor_msgs/msg/battery_state.hpp" + +#include "utils/test_behavior_tree_fixture.hpp" +#include "nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp" + +class IsBatteryChargingConditionTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("test_is_battery_charging"); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set( + "node", + node_); + + factory_->registerNodeType("IsBatteryCharging"); + + battery_pub_ = node_->create_publisher( + "/battery_status", + rclcpp::SystemDefaultsQoS()); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + battery_pub_.reset(); + node_.reset(); + factory_.reset(); + } + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static rclcpp::Publisher::SharedPtr battery_pub_; +}; + +rclcpp::Node::SharedPtr IsBatteryChargingConditionTestFixture::node_ = nullptr; +BT::NodeConfiguration * IsBatteryChargingConditionTestFixture::config_ = nullptr; +std::shared_ptr IsBatteryChargingConditionTestFixture::factory_ = nullptr; +rclcpp::Publisher::SharedPtr +IsBatteryChargingConditionTestFixture::battery_pub_ = nullptr; + +TEST_F(IsBatteryChargingConditionTestFixture, test_behavior_power_supply_status) +{ + std::string xml_txt = + R"( + + + + + )"; + + auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard); + + sensor_msgs::msg::BatteryState battery_msg; + battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN; + battery_pub_->publish(battery_msg); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node_); + EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); + + battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_CHARGING; + battery_pub_->publish(battery_msg); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node_); + EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::SUCCESS); + + battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_DISCHARGING; + battery_pub_->publish(battery_msg); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node_); + EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); + + battery_msg.power_supply_status = + sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_NOT_CHARGING; + battery_pub_->publish(battery_msg); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node_); + EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); + + battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_FULL; + battery_pub_->publish(battery_msg); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node_); + EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + bool all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml index b4b941258e1..e3855fc2313 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -2,7 +2,7 @@ nav2_behaviors - 1.1.6 + 1.1.7 TODO Carlos Orduno Steve Macenski diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index 3d0ee04f323..89e9d77d870 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.1.6 + 1.1.7 Bringup scripts and configurations for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml index a7659a337f0..32d82f7fec2 100644 --- a/nav2_bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_1.yaml @@ -98,6 +98,7 @@ bt_navigator: - nav2_back_up_cancel_bt_node - nav2_assisted_teleop_cancel_bt_node - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node error_code_names: - compute_path_error_code - follow_path_error_code diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml index a75dcbc66b5..75b73385131 100644 --- a/nav2_bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_2.yaml @@ -98,6 +98,7 @@ bt_navigator: - nav2_back_up_cancel_bt_node - nav2_assisted_teleop_cancel_bt_node - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node error_code_names: - compute_path_error_code - follow_path_error_code diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index d66df5d5e0f..f8aad5616ec 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -52,52 +52,57 @@ bt_navigator: # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_are_error_codes_active_condition_bt_node + - nav2_would_a_controller_recovery_help_condition_bt_node + - nav2_would_a_planner_recovery_help_condition_bt_node + - nav2_would_a_smoother_recovery_help_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node bt_navigator_navigate_through_poses_rclcpp_node: ros__parameters: diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index f99cdcb03b0..7f5530c2123 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.1.6 + 1.1.7 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index e2b9d8fc00a..4f0f690d01e 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -79,7 +79,8 @@ BtNavigator::BtNavigator(const rclcpp::NodeOptions & options) "nav2_spin_cancel_bt_node", "nav2_assisted_teleop_cancel_bt_node", "nav2_back_up_cancel_bt_node", - "nav2_drive_on_heading_cancel_bt_node" + "nav2_drive_on_heading_cancel_bt_node", + "nav2_is_battery_charging_condition_bt_node" }; declare_parameter("plugin_lib_names", plugin_libs); diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index 87f498e401d..2dc29958568 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp @@ -127,13 +127,16 @@ class CollisionMonitor : public nav2_util::LifecycleNode * source->base time inerpolated transform. * @param transform_tolerance Transform tolerance * @param source_timeout Maximum time interval in which data is considered valid + * @param base_shift_correction Whether to correct source data towards to base frame movement, + * considering the difference between current time and latest source time * @return True if all sources were configured successfully or false in failure case */ bool configureSources( const std::string & base_frame_id, const std::string & odom_frame_id, const tf2::Duration & transform_tolerance, - const rclcpp::Duration & source_timeout); + const rclcpp::Duration & source_timeout, + const bool base_shift_correction); /** * @brief Main processing routine diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp index c316c065f72..d5af32c2772 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp @@ -41,6 +41,8 @@ class PointCloud : public Source * @param global_frame_id Global frame ID for correct transform calculation * @param transform_tolerance Transform tolerance * @param source_timeout Maximum time interval in which data is considered valid + * @param base_shift_correction Whether to correct source data towards to base frame movement, + * considering the difference between current time and latest source time */ PointCloud( const nav2_util::LifecycleNode::WeakPtr & node, @@ -49,7 +51,8 @@ class PointCloud : public Source const std::string & base_frame_id, const std::string & global_frame_id, const tf2::Duration & transform_tolerance, - const rclcpp::Duration & source_timeout); + const rclcpp::Duration & source_timeout, + const bool base_shift_correction); /** * @brief PointCloud destructor */ diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp index 0fbe47501a8..777b8e404ce 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp @@ -41,6 +41,8 @@ class Range : public Source * @param global_frame_id Global frame ID for correct transform calculation * @param transform_tolerance Transform tolerance * @param source_timeout Maximum time interval in which data is considered valid + * @param base_shift_correction Whether to correct source data towards to base frame movement, + * considering the difference between current time and latest source time */ Range( const nav2_util::LifecycleNode::WeakPtr & node, @@ -49,7 +51,8 @@ class Range : public Source const std::string & base_frame_id, const std::string & global_frame_id, const tf2::Duration & transform_tolerance, - const rclcpp::Duration & source_timeout); + const rclcpp::Duration & source_timeout, + const bool base_shift_correction); /** * @brief Range destructor */ diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp index 29747e8131f..c3f7a11f3fa 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp @@ -41,6 +41,8 @@ class Scan : public Source * @param global_frame_id Global frame ID for correct transform calculation * @param transform_tolerance Transform tolerance * @param source_timeout Maximum time interval in which data is considered valid + * @param base_shift_correction Whether to correct source data towards to base frame movement, + * considering the difference between current time and latest source time */ Scan( const nav2_util::LifecycleNode::WeakPtr & node, @@ -49,7 +51,8 @@ class Scan : public Source const std::string & base_frame_id, const std::string & global_frame_id, const tf2::Duration & transform_tolerance, - const rclcpp::Duration & source_timeout); + const rclcpp::Duration & source_timeout, + const bool base_shift_correction); /** * @brief Scan destructor */ diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp index a24859bb4a5..b7d58b67363 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp @@ -46,6 +46,8 @@ class Source * @param global_frame_id Global frame ID for correct transform calculation * @param transform_tolerance Transform tolerance * @param source_timeout Maximum time interval in which data is considered valid + * @param base_shift_correction Whether to correct source data towards to base frame movement, + * considering the difference between current time and latest source time */ Source( const nav2_util::LifecycleNode::WeakPtr & node, @@ -54,7 +56,8 @@ class Source const std::string & base_frame_id, const std::string & global_frame_id, const tf2::Duration & transform_tolerance, - const rclcpp::Duration & source_timeout); + const rclcpp::Duration & source_timeout, + const bool base_shift_correction); /** * @brief Source destructor */ @@ -125,6 +128,9 @@ class Source tf2::Duration transform_tolerance_; /// @brief Maximum time interval in which data is considered valid rclcpp::Duration source_timeout_; + /// @brief Whether to correct source data towards to base frame movement, + /// considering the difference between current time and latest source time + bool base_shift_correction_; }; // class Source } // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index 7efdefac4e9..67edd5606c1 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -2,7 +2,7 @@ nav2_collision_monitor - 1.1.6 + 1.1.7 Collision Monitor Alexey Merzlyakov Steve Macenski diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml index f0fb4ef5dde..1b0c36529e7 100644 --- a/nav2_collision_monitor/params/collision_monitor_params.yaml +++ b/nav2_collision_monitor/params/collision_monitor_params.yaml @@ -7,6 +7,7 @@ collision_monitor: cmd_vel_out_topic: "cmd_vel" transform_tolerance: 0.5 source_timeout: 5.0 + base_shift_correction: True stop_pub_timeout: 2.0 # Polygons represent zone around the robot for "stop" and "slowdown" action types, # and robot footprint for "approach" action type. diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 313d71fb0ac..939555bf3d8 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -205,6 +205,10 @@ bool CollisionMonitor::getParameters( node, "source_timeout", rclcpp::ParameterValue(2.0)); source_timeout = rclcpp::Duration::from_seconds(get_parameter("source_timeout").as_double()); + nav2_util::declare_parameter_if_not_declared( + node, "base_shift_correction", rclcpp::ParameterValue(true)); + const bool base_shift_correction = + get_parameter("base_shift_correction").as_bool(); nav2_util::declare_parameter_if_not_declared( node, "stop_pub_timeout", rclcpp::ParameterValue(1.0)); @@ -215,7 +219,10 @@ bool CollisionMonitor::getParameters( return false; } - if (!configureSources(base_frame_id, odom_frame_id, transform_tolerance, source_timeout)) { + if ( + !configureSources( + base_frame_id, odom_frame_id, transform_tolerance, source_timeout, base_shift_correction)) + { return false; } @@ -271,7 +278,8 @@ bool CollisionMonitor::configureSources( const std::string & base_frame_id, const std::string & odom_frame_id, const tf2::Duration & transform_tolerance, - const rclcpp::Duration & source_timeout) + const rclcpp::Duration & source_timeout, + const bool base_shift_correction) { try { auto node = shared_from_this(); @@ -289,7 +297,7 @@ bool CollisionMonitor::configureSources( if (source_type == "scan") { std::shared_ptr s = std::make_shared( node, source_name, tf_buffer_, base_frame_id, odom_frame_id, - transform_tolerance, source_timeout); + transform_tolerance, source_timeout, base_shift_correction); s->configure(); @@ -297,7 +305,7 @@ bool CollisionMonitor::configureSources( } else if (source_type == "pointcloud") { std::shared_ptr p = std::make_shared( node, source_name, tf_buffer_, base_frame_id, odom_frame_id, - transform_tolerance, source_timeout); + transform_tolerance, source_timeout, base_shift_correction); p->configure(); @@ -305,7 +313,7 @@ bool CollisionMonitor::configureSources( } else if (source_type == "range") { std::shared_ptr r = std::make_shared( node, source_name, tf_buffer_, base_frame_id, odom_frame_id, - transform_tolerance, source_timeout); + transform_tolerance, source_timeout, base_shift_correction); r->configure(); diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index df4e86b63ea..d4d2ea1adff 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -30,10 +30,11 @@ PointCloud::PointCloud( const std::string & base_frame_id, const std::string & global_frame_id, const tf2::Duration & transform_tolerance, - const rclcpp::Duration & source_timeout) + const rclcpp::Duration & source_timeout, + const bool base_shift_correction) : Source( node, source_name, tf_buffer, base_frame_id, global_frame_id, - transform_tolerance, source_timeout), + transform_tolerance, source_timeout, base_shift_correction), data_(nullptr) { RCLCPP_INFO(logger_, "[%s]: Creating PointCloud", source_name_.c_str()); @@ -75,11 +76,29 @@ void PointCloud::getData( return; } - // Obtaining the transform to get data from source frame and time where it was received - // to the base frame and current time tf2::Transform tf_transform; - if (!getTransform(data_->header.frame_id, data_->header.stamp, curr_time, tf_transform)) { - return; + if (base_shift_correction_) { + // Obtaining the transform to get data from source frame and time where it was received + // to the base frame and current time + if ( + !nav2_util::getTransform( + data_->header.frame_id, data_->header.stamp, + base_frame_id_, curr_time, global_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { + return; + } + } else { + // Obtaining the transform to get data from source frame to base frame without time shift + // considered. Less accurate but much more faster option not dependent on state estimation + // frames. + if ( + !nav2_util::getTransform( + data_->header.frame_id, base_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { + return; + } } sensor_msgs::PointCloud2ConstIterator iter_x(*data_, "x"); diff --git a/nav2_collision_monitor/src/range.cpp b/nav2_collision_monitor/src/range.cpp index 3ef51e2b69e..7c12a4bd525 100644 --- a/nav2_collision_monitor/src/range.cpp +++ b/nav2_collision_monitor/src/range.cpp @@ -30,10 +30,11 @@ Range::Range( const std::string & base_frame_id, const std::string & global_frame_id, const tf2::Duration & transform_tolerance, - const rclcpp::Duration & source_timeout) + const rclcpp::Duration & source_timeout, + const bool base_shift_correction) : Source( node, source_name, tf_buffer, base_frame_id, global_frame_id, - transform_tolerance, source_timeout), + transform_tolerance, source_timeout, base_shift_correction), data_(nullptr) { RCLCPP_INFO(logger_, "[%s]: Creating Range", source_name_.c_str()); @@ -84,11 +85,29 @@ void Range::getData( return; } - // Obtaining the transform to get data from source frame and time where it was received - // to the base frame and current time tf2::Transform tf_transform; - if (!getTransform(data_->header.frame_id, data_->header.stamp, curr_time, tf_transform)) { - return; + if (base_shift_correction_) { + // Obtaining the transform to get data from source frame and time where it was received + // to the base frame and current time + if ( + !nav2_util::getTransform( + data_->header.frame_id, data_->header.stamp, + base_frame_id_, curr_time, global_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { + return; + } + } else { + // Obtaining the transform to get data from source frame to base frame without time shift + // considered. Less accurate but much more faster option not dependent on state estimation + // frames. + if ( + !nav2_util::getTransform( + data_->header.frame_id, base_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { + return; + } } // Calculate poses and refill data array diff --git a/nav2_collision_monitor/src/scan.cpp b/nav2_collision_monitor/src/scan.cpp index d1f52b31f17..50f670cb131 100644 --- a/nav2_collision_monitor/src/scan.cpp +++ b/nav2_collision_monitor/src/scan.cpp @@ -27,10 +27,11 @@ Scan::Scan( const std::string & base_frame_id, const std::string & global_frame_id, const tf2::Duration & transform_tolerance, - const rclcpp::Duration & source_timeout) + const rclcpp::Duration & source_timeout, + const bool base_shift_correction) : Source( node, source_name, tf_buffer, base_frame_id, global_frame_id, - transform_tolerance, source_timeout), + transform_tolerance, source_timeout, base_shift_correction), data_(nullptr) { RCLCPP_INFO(logger_, "[%s]: Creating Scan", source_name_.c_str()); @@ -73,11 +74,29 @@ void Scan::getData( return; } - // Obtaining the transform to get data from source frame and time where it was received - // to the base frame and current time tf2::Transform tf_transform; - if (!getTransform(data_->header.frame_id, data_->header.stamp, curr_time, tf_transform)) { - return; + if (base_shift_correction_) { + // Obtaining the transform to get data from source frame and time where it was received + // to the base frame and current time + if ( + !nav2_util::getTransform( + data_->header.frame_id, data_->header.stamp, + base_frame_id_, curr_time, global_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { + return; + } + } else { + // Obtaining the transform to get data from source frame to base frame without time shift + // considered. Less accurate but much more faster option not dependent on state estimation + // frames. + if ( + !nav2_util::getTransform( + data_->header.frame_id, base_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { + return; + } } // Calculate poses and refill data array diff --git a/nav2_collision_monitor/src/source.cpp b/nav2_collision_monitor/src/source.cpp index bd1028518ce..2b736723687 100644 --- a/nav2_collision_monitor/src/source.cpp +++ b/nav2_collision_monitor/src/source.cpp @@ -33,10 +33,12 @@ Source::Source( const std::string & base_frame_id, const std::string & global_frame_id, const tf2::Duration & transform_tolerance, - const rclcpp::Duration & source_timeout) + const rclcpp::Duration & source_timeout, + const bool base_shift_correction) : node_(node), source_name_(source_name), tf_buffer_(tf_buffer), base_frame_id_(base_frame_id), global_frame_id_(global_frame_id), - transform_tolerance_(transform_tolerance), source_timeout_(source_timeout) + transform_tolerance_(transform_tolerance), source_timeout_(source_timeout), + base_shift_correction_(base_shift_correction) { } diff --git a/nav2_collision_monitor/test/sources_test.cpp b/nav2_collision_monitor/test/sources_test.cpp index 7101b611751..0bfcd1a062c 100644 --- a/nav2_collision_monitor/test/sources_test.cpp +++ b/nav2_collision_monitor/test/sources_test.cpp @@ -172,10 +172,11 @@ class ScanWrapper : public nav2_collision_monitor::Scan const std::string & base_frame_id, const std::string & global_frame_id, const tf2::Duration & transform_tolerance, - const rclcpp::Duration & data_timeout) + const rclcpp::Duration & data_timeout, + const bool base_shift_correction) : nav2_collision_monitor::Scan( node, source_name, tf_buffer, base_frame_id, global_frame_id, - transform_tolerance, data_timeout) + transform_tolerance, data_timeout, base_shift_correction) {} bool dataReceived() const @@ -194,10 +195,11 @@ class PointCloudWrapper : public nav2_collision_monitor::PointCloud const std::string & base_frame_id, const std::string & global_frame_id, const tf2::Duration & transform_tolerance, - const rclcpp::Duration & data_timeout) + const rclcpp::Duration & data_timeout, + const bool base_shift_correction) : nav2_collision_monitor::PointCloud( node, source_name, tf_buffer, base_frame_id, global_frame_id, - transform_tolerance, data_timeout) + transform_tolerance, data_timeout, base_shift_correction) {} bool dataReceived() const @@ -216,10 +218,11 @@ class RangeWrapper : public nav2_collision_monitor::Range const std::string & base_frame_id, const std::string & global_frame_id, const tf2::Duration & transform_tolerance, - const rclcpp::Duration & data_timeout) + const rclcpp::Duration & data_timeout, + const bool base_shift_correction) : nav2_collision_monitor::Range( node, source_name, tf_buffer, base_frame_id, global_frame_id, - transform_tolerance, data_timeout) + transform_tolerance, data_timeout, base_shift_correction) {} bool dataReceived() const @@ -235,6 +238,9 @@ class Tester : public ::testing::Test ~Tester(); protected: + // Data sources creation routine + void createSources(const bool base_shift_correction = true); + // Setting TF chains void sendTransforms(const rclcpp::Time & stamp); @@ -263,7 +269,22 @@ Tester::Tester() tf_buffer_ = std::make_shared(test_node_->get_clock()); tf_buffer_->setUsingDedicatedThread(true); // One-thread broadcasting-listening model tf_listener_ = std::make_shared(*tf_buffer_); +} + +Tester::~Tester() +{ + scan_.reset(); + pointcloud_.reset(); + range_.reset(); + test_node_.reset(); + + tf_listener_.reset(); + tf_buffer_.reset(); +} + +void Tester::createSources(const bool base_shift_correction) +{ // Create Scan object test_node_->declare_parameter( std::string(SCAN_NAME) + ".topic", rclcpp::ParameterValue(SCAN_TOPIC)); @@ -273,7 +294,7 @@ Tester::Tester() scan_ = std::make_shared( test_node_, SCAN_NAME, tf_buffer_, BASE_FRAME_ID, GLOBAL_FRAME_ID, - TRANSFORM_TOLERANCE, DATA_TIMEOUT); + TRANSFORM_TOLERANCE, DATA_TIMEOUT, base_shift_correction); scan_->configure(); // Create PointCloud object @@ -293,7 +314,7 @@ Tester::Tester() pointcloud_ = std::make_shared( test_node_, POINTCLOUD_NAME, tf_buffer_, BASE_FRAME_ID, GLOBAL_FRAME_ID, - TRANSFORM_TOLERANCE, DATA_TIMEOUT); + TRANSFORM_TOLERANCE, DATA_TIMEOUT, base_shift_correction); pointcloud_->configure(); // Create Range object @@ -308,22 +329,10 @@ Tester::Tester() range_ = std::make_shared( test_node_, RANGE_NAME, tf_buffer_, BASE_FRAME_ID, GLOBAL_FRAME_ID, - TRANSFORM_TOLERANCE, DATA_TIMEOUT); + TRANSFORM_TOLERANCE, DATA_TIMEOUT, base_shift_correction); range_->configure(); } -Tester::~Tester() -{ - scan_.reset(); - pointcloud_.reset(); - range_.reset(); - - test_node_.reset(); - - tf_listener_.reset(); - tf_buffer_.reset(); -} - void Tester::sendTransforms(const rclcpp::Time & stamp) { std::shared_ptr tf_broadcaster = @@ -453,6 +462,8 @@ TEST_F(Tester, testGetData) { rclcpp::Time curr_time = test_node_->now(); + createSources(); + sendTransforms(curr_time); // Publish data for sources @@ -485,6 +496,8 @@ TEST_F(Tester, testGetOutdatedData) { rclcpp::Time curr_time = test_node_->now(); + createSources(); + sendTransforms(curr_time); // Publish outdated data for sources @@ -515,6 +528,8 @@ TEST_F(Tester, testIncorrectFrameData) { rclcpp::Time curr_time = test_node_->now(); + createSources(); + // Send incorrect transform sendTransforms(curr_time - 1s); @@ -546,6 +561,8 @@ TEST_F(Tester, testIncorrectData) { rclcpp::Time curr_time = test_node_->now(); + createSources(); + sendTransforms(curr_time); // Publish data for sources @@ -567,6 +584,41 @@ TEST_F(Tester, testIncorrectData) ASSERT_EQ(data.size(), 0u); } +TEST_F(Tester, testIgnoreTimeShift) +{ + rclcpp::Time curr_time = test_node_->now(); + + createSources(false); + + // Send incorrect transform + sendTransforms(curr_time - 1s); + + // Publish data for sources + test_node_->publishScan(curr_time, 1.0); + test_node_->publishPointCloud(curr_time); + test_node_->publishRange(curr_time, 1.0); + + // Wait until all sources will receive the data + ASSERT_TRUE(waitScan(500ms)); + ASSERT_TRUE(waitPointCloud(500ms)); + ASSERT_TRUE(waitRange(500ms)); + + // Scan data should be consistent + std::vector data; + scan_->getData(curr_time, data); + checkScan(data); + + // Pointcloud data should be consistent + data.clear(); + pointcloud_->getData(curr_time, data); + checkPointCloud(data); + + // Range data should be consistent + data.clear(); + range_->getData(curr_time, data); + checkRange(data); +} + int main(int argc, char ** argv) { // Initialize the system diff --git a/nav2_common/nav2_common/launch/rewritten_yaml.py b/nav2_common/nav2_common/launch/rewritten_yaml.py index 63d3905d5e4..3b29fdda595 100644 --- a/nav2_common/nav2_common/launch/rewritten_yaml.py +++ b/nav2_common/nav2_common/launch/rewritten_yaml.py @@ -161,7 +161,7 @@ def pathify(self, d, p=None, paths=None, joinchar='.'): if isinstance(d, dict): for k in d: v = d[k] - self.pathify(v, pn + k, paths, joinchar=joinchar) + self.pathify(v, str(pn) + str(k), paths, joinchar=joinchar) elif isinstance(d, list): for idx, e in enumerate(d): self.pathify(e, pn + str(idx), paths, joinchar=joinchar) diff --git a/nav2_common/package.xml b/nav2_common/package.xml index 9056c712cf4..8ef6b4cf6fc 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.1.6 + 1.1.7 Common support functionality used throughout the navigation 2 stack Carl Delsey Apache-2.0 diff --git a/nav2_constrained_smoother/CMakeLists.txt b/nav2_constrained_smoother/CMakeLists.txt index 0d0cb497ae5..6bc4e7dc6ee 100644 --- a/nav2_constrained_smoother/CMakeLists.txt +++ b/nav2_constrained_smoother/CMakeLists.txt @@ -17,6 +17,10 @@ find_package(Ceres REQUIRED COMPONENTS SuiteSparse) set(CMAKE_CXX_STANDARD 17) +if(${CERES_VERSION} VERSION_LESS_EQUAL 2.0.0) + add_definitions(-DUSE_OLD_CERES_API) +endif() + nav2_package() set(library_name nav2_constrained_smoother) diff --git a/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother_cost_function.hpp b/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother_cost_function.hpp index c0c3919cccf..7253119721c 100644 --- a/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother_cost_function.hpp +++ b/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother_cost_function.hpp @@ -159,7 +159,7 @@ class SmootherCostFunction Eigen::Matrix center = arcCenter( pt_prev, pt, pt_next, next_to_last_length_ratio_ < 0); - if (ceres::isinf(center[0])) { + if (CERES_ISINF(center[0])) { return; } T turning_rad = (pt - center).norm(); diff --git a/nav2_constrained_smoother/include/nav2_constrained_smoother/utils.hpp b/nav2_constrained_smoother/include/nav2_constrained_smoother/utils.hpp index 682ed1c16b9..fb4e2aa1307 100644 --- a/nav2_constrained_smoother/include/nav2_constrained_smoother/utils.hpp +++ b/nav2_constrained_smoother/include/nav2_constrained_smoother/utils.hpp @@ -20,6 +20,16 @@ #define EPSILON 0.0001 +/** + * Compatibility with different ceres::isinf() and ceres::IsInfinite() API + * used in Ceres Solver 2.1.0+ and 2.0.0- versions respectively + */ +#if defined(USE_OLD_CERES_API) + #define CERES_ISINF(x) ceres::IsInfinite(x) +#else + #define CERES_ISINF(x) ceres::isinf(x) +#endif + namespace nav2_constrained_smoother { @@ -86,7 +96,7 @@ inline Eigen::Matrix tangentDir( bool is_cusp) { Eigen::Matrix center = arcCenter(pt_prev, pt, pt_next, is_cusp); - if (ceres::isinf(center[0])) { // straight line + if (CERES_ISINF(center[0])) { // straight line Eigen::Matrix d1 = pt - pt_prev; Eigen::Matrix d2 = pt_next - pt; diff --git a/nav2_constrained_smoother/package.xml b/nav2_constrained_smoother/package.xml index 837c67ec168..b20c946bf7a 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -2,7 +2,7 @@ nav2_constrained_smoother - 1.1.6 + 1.1.7 Ceres constrained smoother Matej Vargovcik Steve Macenski diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index e5ea991c34a..ded67fbf41a 100644 --- a/nav2_controller/CMakeLists.txt +++ b/nav2_controller/CMakeLists.txt @@ -50,6 +50,10 @@ set(dependencies add_library(simple_progress_checker SHARED plugins/simple_progress_checker.cpp) ament_target_dependencies(simple_progress_checker ${dependencies}) +add_library(pose_progress_checker SHARED plugins/pose_progress_checker.cpp) +target_link_libraries(pose_progress_checker simple_progress_checker) +ament_target_dependencies(pose_progress_checker ${dependencies}) + add_library(simple_goal_checker SHARED plugins/simple_goal_checker.cpp) ament_target_dependencies(simple_goal_checker ${dependencies}) @@ -79,7 +83,7 @@ target_link_libraries(${executable_name} ${library_name}) rclcpp_components_register_nodes(${library_name} "nav2_controller::ControllerServer") -install(TARGETS simple_progress_checker simple_goal_checker stopped_goal_checker ${library_name} +install(TARGETS simple_progress_checker pose_progress_checker simple_goal_checker stopped_goal_checker ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -102,6 +106,7 @@ endif() ament_export_include_directories(include) ament_export_libraries(simple_progress_checker + pose_progress_checker simple_goal_checker stopped_goal_checker ${library_name}) diff --git a/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp b/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp new file mode 100644 index 00000000000..3db219eaae0 --- /dev/null +++ b/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp @@ -0,0 +1,67 @@ +// Copyright (c) 2023 Dexory +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_CONTROLLER__PLUGINS__POSE_PROGRESS_CHECKER_HPP_ +#define NAV2_CONTROLLER__PLUGINS__POSE_PROGRESS_CHECKER_HPP_ + +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "nav2_controller/plugins/simple_progress_checker.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +namespace nav2_controller +{ +/** +* @class PoseProgressChecker +* @brief This plugin is used to check the position and the angle of the robot to make sure +* that it is actually progressing or rotating towards a goal. +*/ + +class PoseProgressChecker : public SimpleProgressChecker +{ +public: + void initialize( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name) override; + bool check(geometry_msgs::msg::PoseStamped & current_pose) override; + +protected: + /** + * @brief Calculates robots movement from baseline pose + * @param pose Current pose of the robot + * @return true, if movement is greater than radius_, or false + */ + bool isRobotMovedEnough(const geometry_msgs::msg::Pose2D & pose); + + static double poseAngleDistance( + const geometry_msgs::msg::Pose2D &, + const geometry_msgs::msg::Pose2D &); + + double required_movement_angle_; + + // Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + std::string plugin_name_; + + /** + * @brief Callback executed when a paramter change is detected + * @param parameters list of changed parameters + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); +}; +} // namespace nav2_controller + +#endif // NAV2_CONTROLLER__PLUGINS__POSE_PROGRESS_CHECKER_HPP_ diff --git a/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp b/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp index 656d67fb300..b86a7018e57 100644 --- a/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp @@ -46,12 +46,16 @@ class SimpleProgressChecker : public nav2_core::ProgressChecker * @param pose Current pose of the robot * @return true, if movement is greater than radius_, or false */ - bool is_robot_moved_enough(const geometry_msgs::msg::Pose2D & pose); + bool isRobotMovedEnough(const geometry_msgs::msg::Pose2D & pose); /** * @brief Resets baseline pose with the current pose of the robot * @param pose Current pose of the robot */ - void reset_baseline_pose(const geometry_msgs::msg::Pose2D & pose); + void resetBaselinePose(const geometry_msgs::msg::Pose2D & pose); + + static double pose_distance( + const geometry_msgs::msg::Pose2D &, + const geometry_msgs::msg::Pose2D &); rclcpp::Clock::SharedPtr clock_; diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 3c6d8f8626d..14bfa0e04e1 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.1.6 + 1.1.7 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_controller/plugins.xml b/nav2_controller/plugins.xml index ad27127c03d..ffbd0e5f1cb 100644 --- a/nav2_controller/plugins.xml +++ b/nav2_controller/plugins.xml @@ -4,6 +4,11 @@ Checks if distance between current and previous pose is above a threshold + + + Checks if distance and angle between current and previous pose is above a threshold + + Checks if current pose is within goal window for x,y and yaw diff --git a/nav2_controller/plugins/pose_progress_checker.cpp b/nav2_controller/plugins/pose_progress_checker.cpp new file mode 100644 index 00000000000..271e5635c07 --- /dev/null +++ b/nav2_controller/plugins/pose_progress_checker.cpp @@ -0,0 +1,97 @@ +// Copyright (c) 2023 Dexory +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "nav2_controller/plugins/pose_progress_checker.hpp" +#include +#include +#include +#include +#include "angles/angles.h" +#include "nav_2d_utils/conversions.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose2_d.hpp" +#include "nav2_util/node_utils.hpp" +#include "pluginlib/class_list_macros.hpp" + +using rcl_interfaces::msg::ParameterType; +using std::placeholders::_1; + +namespace nav2_controller +{ + +void PoseProgressChecker::initialize( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name) +{ + plugin_name_ = plugin_name; + SimpleProgressChecker::initialize(parent, plugin_name); + auto node = parent.lock(); + + nav2_util::declare_parameter_if_not_declared( + node, plugin_name + ".required_movement_angle", rclcpp::ParameterValue(0.5)); + node->get_parameter_or(plugin_name + ".required_movement_angle", required_movement_angle_, 0.5); + + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&PoseProgressChecker::dynamicParametersCallback, this, _1)); +} + +bool PoseProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose) +{ + // relies on short circuit evaluation to not call is_robot_moved_enough if + // baseline_pose is not set. + geometry_msgs::msg::Pose2D current_pose2d; + current_pose2d = nav_2d_utils::poseToPose2D(current_pose.pose); + + if (!baseline_pose_set_ || PoseProgressChecker::isRobotMovedEnough(current_pose2d)) { + resetBaselinePose(current_pose2d); + return true; + } + return clock_->now() - baseline_time_ <= time_allowance_; +} + +bool PoseProgressChecker::isRobotMovedEnough(const geometry_msgs::msg::Pose2D & pose) +{ + return pose_distance(pose, baseline_pose_) > radius_ || + poseAngleDistance(pose, baseline_pose_) > required_movement_angle_; +} + +double PoseProgressChecker::poseAngleDistance( + const geometry_msgs::msg::Pose2D & pose1, + const geometry_msgs::msg::Pose2D & pose2) +{ + return abs(angles::shortest_angular_distance(pose1.theta, pose2.theta)); +} + +rcl_interfaces::msg::SetParametersResult +PoseProgressChecker::dynamicParametersCallback(std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + for (auto parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); + + if (type == ParameterType::PARAMETER_DOUBLE) { + if (name == plugin_name_ + ".required_movement_angle") { + required_movement_angle_ = parameter.as_double(); + } + } + } + result.successful = true; + return result; +} + +} // namespace nav2_controller + +PLUGINLIB_EXPORT_CLASS(nav2_controller::PoseProgressChecker, nav2_core::ProgressChecker) diff --git a/nav2_controller/plugins/simple_progress_checker.cpp b/nav2_controller/plugins/simple_progress_checker.cpp index 1ced16dc5bd..5cfe2da8417 100644 --- a/nav2_controller/plugins/simple_progress_checker.cpp +++ b/nav2_controller/plugins/simple_progress_checker.cpp @@ -29,8 +29,6 @@ using std::placeholders::_1; namespace nav2_controller { -static double pose_distance(const geometry_msgs::msg::Pose2D &, const geometry_msgs::msg::Pose2D &); - void SimpleProgressChecker::initialize( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, const std::string & plugin_name) @@ -62,8 +60,8 @@ bool SimpleProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose geometry_msgs::msg::Pose2D current_pose2d; current_pose2d = nav_2d_utils::poseToPose2D(current_pose.pose); - if ((!baseline_pose_set_) || (is_robot_moved_enough(current_pose2d))) { - reset_baseline_pose(current_pose2d); + if ((!baseline_pose_set_) || (isRobotMovedEnough(current_pose2d))) { + resetBaselinePose(current_pose2d); return true; } return !((clock_->now() - baseline_time_) > time_allowance_); @@ -74,19 +72,19 @@ void SimpleProgressChecker::reset() baseline_pose_set_ = false; } -void SimpleProgressChecker::reset_baseline_pose(const geometry_msgs::msg::Pose2D & pose) +void SimpleProgressChecker::resetBaselinePose(const geometry_msgs::msg::Pose2D & pose) { baseline_pose_ = pose; baseline_time_ = clock_->now(); baseline_pose_set_ = true; } -bool SimpleProgressChecker::is_robot_moved_enough(const geometry_msgs::msg::Pose2D & pose) +bool SimpleProgressChecker::isRobotMovedEnough(const geometry_msgs::msg::Pose2D & pose) { return pose_distance(pose, baseline_pose_) > radius_; } -static double pose_distance( +double SimpleProgressChecker::pose_distance( const geometry_msgs::msg::Pose2D & pose1, const geometry_msgs::msg::Pose2D & pose2) { diff --git a/nav2_controller/plugins/test/CMakeLists.txt b/nav2_controller/plugins/test/CMakeLists.txt index d4f34d3918f..0226676ce39 100644 --- a/nav2_controller/plugins/test/CMakeLists.txt +++ b/nav2_controller/plugins/test/CMakeLists.txt @@ -1,4 +1,4 @@ ament_add_gtest(pctest progress_checker.cpp) -target_link_libraries(pctest simple_progress_checker) +target_link_libraries(pctest simple_progress_checker pose_progress_checker) ament_add_gtest(gctest goal_checker.cpp) target_link_libraries(gctest simple_goal_checker stopped_goal_checker) diff --git a/nav2_controller/plugins/test/goal_checker.cpp b/nav2_controller/plugins/test/goal_checker.cpp index 387bfcc735b..acfffcb9cd9 100644 --- a/nav2_controller/plugins/test/goal_checker.cpp +++ b/nav2_controller/plugins/test/goal_checker.cpp @@ -145,6 +145,16 @@ TEST(VelocityIterator, goal_checker_reset) EXPECT_TRUE(true); } +TEST(VelocityIterator, stopped_goal_checker_reset) +{ + auto x = std::make_shared("stopped_goal_checker"); + + nav2_core::GoalChecker * sgc = new StoppedGoalChecker; + sgc->reset(); + delete sgc; + EXPECT_TRUE(true); +} + TEST(VelocityIterator, two_checks) { auto x = std::make_shared("goal_checker"); diff --git a/nav2_controller/plugins/test/progress_checker.cpp b/nav2_controller/plugins/test/progress_checker.cpp index 418ff1032ae..0f425614475 100644 --- a/nav2_controller/plugins/test/progress_checker.cpp +++ b/nav2_controller/plugins/test/progress_checker.cpp @@ -37,10 +37,13 @@ #include "gtest/gtest.h" #include "nav2_controller/plugins/simple_progress_checker.hpp" +#include "nav2_controller/plugins/pose_progress_checker.hpp" #include "nav_2d_utils/conversions.hpp" #include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/geometry_utils.hpp" using nav2_controller::SimpleProgressChecker; +using nav2_controller::PoseProgressChecker; class TestLifecycleNode : public nav2_util::LifecycleNode { @@ -83,8 +86,8 @@ class TestLifecycleNode : public nav2_util::LifecycleNode void checkMacro( nav2_core::ProgressChecker & pc, - double x0, double y0, - double x1, double y1, + double x0, double y0, double theta0, + double x1, double y1, double theta1, int delay, bool expected_result) { @@ -92,10 +95,12 @@ void checkMacro( geometry_msgs::msg::PoseStamped pose0, pose1; pose0.pose.position.x = x0; pose0.pose.position.y = y0; + pose0.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta0); pose1.pose.position.x = x1; pose1.pose.position.y = y1; + pose1.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta1); EXPECT_TRUE(pc.check(pose0)); - rclcpp::sleep_for(std::chrono::seconds(delay)); + rclcpp::sleep_for(std::chrono::milliseconds(delay)); if (expected_result) { EXPECT_TRUE(pc.check(pose1)); } else { @@ -119,12 +124,116 @@ TEST(SimpleProgressChecker, unit_tests) SimpleProgressChecker pc; pc.initialize(x, "nav2_controller"); - checkMacro(pc, 0, 0, 0, 0, 1, true); - checkMacro(pc, 0, 0, 1, 0, 1, true); - checkMacro(pc, 0, 0, 0, 1, 1, true); - checkMacro(pc, 0, 0, 1, 0, 11, true); - checkMacro(pc, 0, 0, 0, 1, 11, true); - checkMacro(pc, 0, 0, 0, 0, 11, false); + + double time_allowance = 0.5; + int half_time_allowance_ms = static_cast(time_allowance * 0.5 * 1000); + int twice_time_allowance_ms = static_cast(time_allowance * 2.0 * 1000); + + auto rec_param = std::make_shared( + x->get_node_base_interface(), x->get_node_topics_interface(), + x->get_node_graph_interface(), + x->get_node_services_interface()); + + auto results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("nav2_controller.movement_time_allowance", time_allowance)}); + + rclcpp::spin_until_future_complete( + x->get_node_base_interface(), + results); + + EXPECT_EQ( + x->get_parameter("nav2_controller.movement_time_allowance").as_double(), + time_allowance); + + // BELOW time allowance (set to time_allowance) + // no movement + checkMacro(pc, 0, 0, 0, 0, 0, 0, half_time_allowance_ms, true); + // translation below required_movement_radius (default 0.5) + checkMacro(pc, 0, 0, 0, 0.25, 0, 0, half_time_allowance_ms, true); + checkMacro(pc, 0, 0, 0, 0, 0.25, 0, half_time_allowance_ms, true); + // translation above required_movement_radius (default 0.5) + checkMacro(pc, 0, 0, 0, 1, 0, 0, half_time_allowance_ms, true); + checkMacro(pc, 0, 0, 0, 0, 1, 0, half_time_allowance_ms, true); + + // ABOVE time allowance (set to time_allowance) + // no movement + checkMacro(pc, 0, 0, 0, 0, 0, 0, twice_time_allowance_ms, false); + // translation below required_movement_radius (default 0.5) + checkMacro(pc, 0, 0, 0, 0.25, 0, 0, twice_time_allowance_ms, false); + checkMacro(pc, 0, 0, 0, 0, 0.25, 0, twice_time_allowance_ms, false); + // translation above required_movement_radius (default 0.5) + checkMacro(pc, 0, 0, 0, 1, 0, 0, twice_time_allowance_ms, true); + checkMacro(pc, 0, 0, 0, 0, 1, 0, twice_time_allowance_ms, true); +} + +TEST(PoseProgressChecker, pose_progress_checker_reset) +{ + auto x = std::make_shared("pose_progress_checker"); + + PoseProgressChecker * rpc = new PoseProgressChecker; + rpc->reset(); + delete rpc; + EXPECT_TRUE(true); +} + +TEST(PoseProgressChecker, unit_tests) +{ + auto x = std::make_shared("pose_progress_checker"); + + PoseProgressChecker rpc; + rpc.initialize(x, "nav2_controller"); + + double time_allowance = 0.5; + int half_time_allowance_ms = static_cast(time_allowance * 0.5 * 1000); + int twice_time_allowance_ms = static_cast(time_allowance * 2.0 * 1000); + + auto rec_param = std::make_shared( + x->get_node_base_interface(), x->get_node_topics_interface(), + x->get_node_graph_interface(), + x->get_node_services_interface()); + + auto results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("nav2_controller.movement_time_allowance", time_allowance)}); + + rclcpp::spin_until_future_complete( + x->get_node_base_interface(), + results); + + EXPECT_EQ( + x->get_parameter("nav2_controller.movement_time_allowance").as_double(), + time_allowance); + + // BELOW time allowance (set to time_allowance) + // no movement + checkMacro(rpc, 0, 0, 0, 0, 0, 0, half_time_allowance_ms, true); + // translation below required_movement_radius (default 0.5) + checkMacro(rpc, 0, 0, 0, 0.25, 0, 0, half_time_allowance_ms, true); + checkMacro(rpc, 0, 0, 0, 0, 0.25, 0, half_time_allowance_ms, true); + // rotation below required_movement_angle (default 0.5) + checkMacro(rpc, 0, 0, 0, 0, 0, 0.25, half_time_allowance_ms, true); + checkMacro(rpc, 0, 0, 0, 0, 0, -0.25, half_time_allowance_ms, true); + // translation above required_movement_radius (default 0.5) + checkMacro(rpc, 0, 0, 0, 1, 0, 0, half_time_allowance_ms, true); + checkMacro(rpc, 0, 0, 0, 0, 1, 0, half_time_allowance_ms, true); + // rotation above required_movement_angle (default 0.5) + checkMacro(rpc, 0, 0, 0, 0, 0, 1, half_time_allowance_ms, true); + checkMacro(rpc, 0, 0, 0, 0, 0, -1, half_time_allowance_ms, true); + + // ABOVE time allowance (set to time_allowance) + // no movement + checkMacro(rpc, 0, 0, 0, 0, 0, 0, twice_time_allowance_ms, false); + // translation below required_movement_radius (default 0.5) + checkMacro(rpc, 0, 0, 0, 0.25, 0, 0, twice_time_allowance_ms, false); + checkMacro(rpc, 0, 0, 0, 0, 0.25, 0, twice_time_allowance_ms, false); + // rotation below required_movement_angle (default 0.5) + checkMacro(rpc, 0, 0, 0, 0, 0, 0.25, twice_time_allowance_ms, false); + checkMacro(rpc, 0, 0, 0, 0, 0, -0.25, twice_time_allowance_ms, false); + // translation above required_movement_radius (default 0.5) + checkMacro(rpc, 0, 0, 0, 1, 0, 0, twice_time_allowance_ms, true); + checkMacro(rpc, 0, 0, 0, 0, 1, 0, twice_time_allowance_ms, true); + // rotation above required_movement_angle (default 0.5) + checkMacro(rpc, 0, 0, 0, 0, 0, 1, twice_time_allowance_ms, true); + checkMacro(rpc, 0, 0, 0, 0, 0, -1, twice_time_allowance_ms, true); } int main(int argc, char ** argv) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 2a52e296dad..ff86f642557 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -123,6 +123,8 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) get_parameter("failure_tolerance", failure_tolerance_); costmap_ros_->configure(); + // Launch a thread to run the costmap node + costmap_thread_ = std::make_unique(costmap_ros_); try { progress_checker_type_ = nav2_util::get_plugin_type_param(node, progress_checker_id_); diff --git a/nav2_core/package.xml b/nav2_core/package.xml index 0016b0d840c..fa990f04dc6 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.1.6 + 1.1.7 A set of headers for plugins core to the Nav2 stack Steve Macenski Carl Delsey diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index 683cb04fdd2..f039cf5d42b 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -87,6 +87,7 @@ add_library(layers SHARED src/observation_buffer.cpp plugins/voxel_layer.cpp plugins/range_sensor_layer.cpp + plugins/denoise_layer.cpp ) ament_target_dependencies(layers ${dependencies} diff --git a/nav2_costmap_2d/costmap_plugins.xml b/nav2_costmap_2d/costmap_plugins.xml index e5867d51089..6748cd5fcae 100644 --- a/nav2_costmap_2d/costmap_plugins.xml +++ b/nav2_costmap_2d/costmap_plugins.xml @@ -15,6 +15,9 @@ A range-sensor (sonar, IR) based obstacle layer for costmap_2d + + Filters noise-induced freestanding obstacles or small obstacles groups + diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp index 35f53c6b96c..49aeab21e38 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp @@ -2,6 +2,7 @@ * * Software License Agreement (BSD License) * + * Copyright (c) 2008, 2013, Willow Garage, Inc. * Copyright (c) 2020 Samsung Research Russia * All rights reserved. * @@ -32,7 +33,9 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * - * Author: Alexey Merzlyakov + * Author: Eitan Marder-Eppstein + * David V. Lu!! + * Alexey Merzlyakov *********************************************************************/ #ifndef NAV2_COSTMAP_2D__COSTMAP_FILTERS__COSTMAP_FILTER_HPP_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image.hpp new file mode 100644 index 00000000000..db7ae8fce84 --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image.hpp @@ -0,0 +1,207 @@ +// Copyright (c) 2023 Andrey Ryzhikov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_COSTMAP_2D__DENOISE__IMAGE_HPP_ +#define NAV2_COSTMAP_2D__DENOISE__IMAGE_HPP_ + +#include +#include + +namespace nav2_costmap_2d +{ + +/** + * @brief Image with pixels of type T + * Сan own data, be a wrapper over some memory buffer, or refer to a fragment of another image + * Pixels of one row are stored continuity. But rows continuity is not guaranteed. + * The distance (number of elements of type T) from row(i) to row(i + 1) is equal to step() + * @tparam T type of pixel + */ +template +class Image +{ +public: + /// @brief Create empty image + Image() = default; + + /** + * @brief Create image referencing to a third-party buffer + * @param rows number of image rows + * @param columns number of image columns + * @param data existing memory buffer with size at least rows * columns + * @param step offset from row(i) to row(i + 1) in memory buffer (number of elements of type T). + * offset = columns if rows are stored continuity + */ + Image(size_t rows, size_t columns, T * data, size_t step); + + /** + * @brief Create image referencing to the other + * Share image data between new and old object. + * Changing data in a new object will affect the given one and vice versa + */ + Image(Image & other); + + /** + * @brief Create image from the other (move constructor) + */ + Image(Image && other) noexcept; + + /// @return number of image rows + size_t rows() const {return rows_;} + + /// @return number of image columns + size_t columns() const {return columns_;} + + /// @return true if image empty + bool empty() const {return rows_ == 0 || columns_ == 0;} + + /// @return offset (number of elements of type T) from row(i) to row(i + 1) + size_t step() const {return step_;} + + /** + * @return pointer to first pixel of row + * @warning If row >= rows(), the behavior is undefined + */ + T * row(size_t row); + + /// @overload + const T * row(size_t row) const; + + /** + * @brief Read (and modify, if need) each pixel sequentially + * @tparam Functor function object. + * Signature should be equivalent to the following: + * void fn(T& pixel) or void fn(const T& pixel) + * @param fn a function that will be applied to each pixel in the image. Can modify image data + */ + template + void forEach(Functor && fn); + + /** + * @brief Read each pixel sequentially + * @tparam Functor function object. + * Signature should be equivalent to the following: + * void fn(const T& pixel) + * @param fn a function that will be applied to each pixel in the image + */ + template + void forEach(Functor && fn) const; + /** + * @brief Convert each pixel to corresponding pixel of target using a custom function + * + * The source and target must be the same size. + * For calculation of new target value the operation can use source value and + * an optionally current target value. + * This function call operation(this[i, j], target[i, j]) for each pixel + * where target[i, j] is mutable + * @tparam TargetElement type of target pixel + * @tparam Converter function object. + * Signature should be equivalent to the following: + * void fn(const T& src, TargetElement& trg) + * @param target output image with TargetElement-type pixels + * @param operation the binary operation op is applied to pairs of pixels: + * first (const) from source and second (mutable) from target + * @throw std::logic_error if the source and target of different sizes + */ + template + void convert(Image & target, Converter && converter) const; + +private: + T * data_start_{}; + size_t rows_{}; + size_t columns_{}; + size_t step_{}; +}; + +template +Image::Image(size_t rows, size_t columns, T * data, size_t step) +: rows_{rows}, columns_{columns}, step_{step} +{ + data_start_ = data; +} + +template +Image::Image(Image & other) +: data_start_{other.data_start_}, + rows_{other.rows_}, columns_{other.columns_}, step_{other.step_} {} + +template +Image::Image(Image && other) noexcept +: data_start_{other.data_start_}, + rows_{other.rows_}, columns_{other.columns_}, step_{other.step_} {} + +template +T * Image::row(size_t row) +{ + return const_cast( static_cast &>(*this).row(row) ); +} + +template +const T * Image::row(size_t row) const +{ + return data_start_ + row * step_; +} + +template +template +void Image::forEach(Functor && fn) +{ + static_cast &>(*this).forEach( + [&](const T & pixel) { + fn(const_cast(pixel)); + }); +} + +template +template +void Image::forEach(Functor && fn) const +{ + const T * rowPtr = row(0); + + for (size_t row = 0; row < rows(); ++row) { + const T * rowEnd = rowPtr + columns(); + + for (const T * pixel = rowPtr; pixel != rowEnd; ++pixel) { + fn(*pixel); + } + rowPtr += step(); + } +} + +template +template +void Image::convert(Image & target, Converter && converter) const +{ + if (rows() != target.rows() || columns() != target.columns()) { + throw std::logic_error("Image::convert. The source and target images size are different"); + } + const T * source_row = row(0); + TargetElement * target_row = target.row(0); + + for (size_t row = 0; row < rows(); ++row) { + const T * rowInEnd = source_row + columns(); + const T * src = source_row; + TargetElement * trg = target_row; + + for (; src != rowInEnd; ++src, ++trg) { + converter(*src, *trg); + } + source_row += step(); + target_row += target.step(); + } +} + +} // namespace nav2_costmap_2d + +#endif // NAV2_COSTMAP_2D__DENOISE__IMAGE_HPP_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp new file mode 100644 index 00000000000..ee798948475 --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp @@ -0,0 +1,1051 @@ +// Copyright (c) 2023 Andrey Ryzhikov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_COSTMAP_2D__DENOISE__IMAGE_PROCESSING_HPP_ +#define NAV2_COSTMAP_2D__DENOISE__IMAGE_PROCESSING_HPP_ + +#include "image.hpp" +#include +#include +#include +#include +#include + +namespace nav2_costmap_2d +{ + +/** + * @enum nav2_costmap_2d::ConnectivityType + * @brief Describes the type of pixel connectivity (is the way in which + * pixels in image relate to their neighbors) + */ +enum class ConnectivityType : int +{ + /// neighbors pixels are connected horizontally and vertically + Way4 = 4, + /// neighbors pixels are connected horizontally, vertically and diagonally + Way8 = 8 +}; + +/** + * @brief A memory buffer that can grow to an upper-bounded capacity + */ +class MemoryBuffer +{ +public: + /// @brief Free memory allocated for the buffer + inline ~MemoryBuffer() {reset();} + /** + * @brief Return a pointer to an uninitialized array of count elements + * Delete the old block of memory and allocates a new one if the size of the old is too small. + * The returned pointer is valid until the next call to get() or destructor. + * @tparam T type of element + * @param count number of elements + * @throw std::bad_alloc or any other exception thrown by allocator + */ + template + T * get(std::size_t count); + +private: + inline void reset(); + inline void allocate(size_t bytes); + +private: + void * data_{}; + size_t size_{}; +}; + +// forward declarations +namespace imgproc_impl +{ +template +class EquivalenceLabelTrees; + +template +void morphologyOperation( + const Image & input, Image & output, + const Image & shape, AggregateFn aggregate); + +using ShapeBuffer3x3 = std::array; +inline Image createShape(ShapeBuffer3x3 & buffer, ConnectivityType connectivity); +} // namespace imgproc_impl + +/** + * @brief Perform morphological dilation + * @tparam Max function object + * @param input input image + * @param output output image + * @param connectivity selector for selecting structuring element (Way4-> cross, Way8-> rect) + * @param max_function takes as input std::initializer_list with three elements. + * Returns the greatest value in list + */ +template +inline void dilate( + const Image & input, Image & output, + ConnectivityType connectivity, Max && max_function) +{ + using namespace imgproc_impl; + ShapeBuffer3x3 shape_buffer; + Image shape = createShape(shape_buffer, connectivity); + morphologyOperation(input, output, shape, max_function); +} + +/** +* @brief Compute the connected components labeled image of binary image +* Implements the SAUF algorithm +* (Two Strategies to Speed up Connected Component Labeling Algorithms +* Kesheng Wu, Ekow Otoo, Kenji Suzuki). +* @tparam connectivity pixels connectivity type +* @tparam Label integer type of label +* @tparam IsBg functor with signature bool (uint8_t) +* @param image input image +* @param buffer memory block that will be used to store the result (labeled image) +* and the internal buffer for labels trees +* @param label_trees union-find data structure +* @param is_background returns true if the passed pixel value is background +* @throw LabelOverflow if all possible values of the Label type are used and +* it is impossible to create a new unique +* @return pair(labeled image, total number of labels) +* Labeled image has the same size as image. Label 0 represents the background label, +* labels [1, - 1] - separate components. +* Total number of labels == 0 for empty image. +* In other cases, label 0 is always counted, +* even if there is no background in the image. +* For example, for an image of one background pixel, the total number of labels == 2. +* Two labels (0, 1) have been counted, although label 0 is not used) +*/ +template +std::pair, Label> connectedComponents( + const Image & image, MemoryBuffer & buffer, + imgproc_impl::EquivalenceLabelTrees

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 3f199f3d99c..ce5979929cb 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -246,24 +246,6 @@ Server timeout - - Error code - - - - Error code - - - - Error code - - - - Error code - Error codes to check, user defined - - - diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index 30442368aa0..589030f1232 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.1.7 + 1.1.8 TODO Michael Jeronimo Carlos Orduno diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml index e3855fc2313..d7942a1893b 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -2,7 +2,7 @@ nav2_behaviors - 1.1.7 + 1.1.8 TODO Carlos Orduno Steve Macenski diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index 89e9d77d870..c239f0036dd 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.1.7 + 1.1.8 Bringup scripts and configurations for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml index 32d82f7fec2..58d12bc05d2 100644 --- a/nav2_bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_1.yaml @@ -99,9 +99,6 @@ bt_navigator: - nav2_assisted_teleop_cancel_bt_node - nav2_drive_on_heading_cancel_bt_node - nav2_is_battery_charging_condition_bt_node - error_code_names: - - compute_path_error_code - - follow_path_error_code bt_navigator_navigate_through_poses_rclcpp_node: ros__parameters: diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml index 75b73385131..f1704edf330 100644 --- a/nav2_bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_2.yaml @@ -99,9 +99,6 @@ bt_navigator: - nav2_assisted_teleop_cancel_bt_node - nav2_drive_on_heading_cancel_bt_node - nav2_is_battery_charging_condition_bt_node - error_code_names: - - compute_path_error_code - - follow_path_error_code bt_navigator_navigate_through_poses_rclcpp_node: ros__parameters: diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index f8aad5616ec..5f4b08f68a1 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -67,10 +67,6 @@ bt_navigator: - nav2_goal_updated_condition_bt_node - nav2_globally_updated_goal_condition_bt_node - nav2_is_path_valid_condition_bt_node - - nav2_are_error_codes_active_condition_bt_node - - nav2_would_a_controller_recovery_help_condition_bt_node - - nav2_would_a_planner_recovery_help_condition_bt_node - - nav2_would_a_smoother_recovery_help_condition_bt_node - nav2_initial_pose_received_condition_bt_node - nav2_reinitialize_global_localization_service_bt_node - nav2_rate_controller_bt_node diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index 7f5530c2123..c4ba88031d2 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.1.7 + 1.1.8 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index 67edd5606c1..d80a6ff0e0a 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -2,7 +2,7 @@ nav2_collision_monitor - 1.1.7 + 1.1.8 Collision Monitor Alexey Merzlyakov Steve Macenski diff --git a/nav2_common/package.xml b/nav2_common/package.xml index 8ef6b4cf6fc..e10aae9ddaa 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.1.7 + 1.1.8 Common support functionality used throughout the navigation 2 stack Carl Delsey Apache-2.0 diff --git a/nav2_constrained_smoother/package.xml b/nav2_constrained_smoother/package.xml index b20c946bf7a..762ae72db32 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -2,7 +2,7 @@ nav2_constrained_smoother - 1.1.7 + 1.1.8 Ceres constrained smoother Matej Vargovcik Steve Macenski diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 14bfa0e04e1..a9a686981a3 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.1.7 + 1.1.8 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_core/package.xml b/nav2_core/package.xml index fa990f04dc6..dfc71ee64f9 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.1.7 + 1.1.8 A set of headers for plugins core to the Nav2 stack Steve Macenski Carl Delsey diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index d72d96f6aba..1b571b2745a 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 1.1.7 + 1.1.8 This package provides an implementation of a 2D costmap that takes in sensor data from the world, builds a 2D or 3D occupancy grid of the data (depending diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index 6ddcc7eede1..bc4debf1be5 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 1.1.7 + 1.1.8 The costmap_queue package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml index 7d9e69754df..b08ab9a7308 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 1.1.7 + 1.1.8 TODO Carl Delsey BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index bb43e9bc45e..3f74ce58a1b 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 1.1.7 + 1.1.8 The dwb_critics package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml index c7da0e947ac..7778d8ebf63 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 1.1.7 + 1.1.8 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index 402d1ab93af..9abbd2499f3 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 1.1.7 + 1.1.8 Standard implementations of the GoalChecker and TrajectoryGenerators for dwb_core diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index 8107ba52cd4..da5861c10eb 100644 --- a/nav2_dwb_controller/nav2_dwb_controller/package.xml +++ b/nav2_dwb_controller/nav2_dwb_controller/package.xml @@ -2,7 +2,7 @@ nav2_dwb_controller - 1.1.7 + 1.1.8 ROS2 controller (DWB) metapackage diff --git a/nav2_dwb_controller/nav_2d_msgs/package.xml b/nav2_dwb_controller/nav_2d_msgs/package.xml index e53ab31e25f..09c4e745a44 100644 --- a/nav2_dwb_controller/nav_2d_msgs/package.xml +++ b/nav2_dwb_controller/nav_2d_msgs/package.xml @@ -2,7 +2,7 @@ nav_2d_msgs - 1.1.7 + 1.1.8 Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D. David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml index fb4ad305084..d541ce92b7f 100644 --- a/nav2_dwb_controller/nav_2d_utils/package.xml +++ b/nav2_dwb_controller/nav_2d_utils/package.xml @@ -2,7 +2,7 @@ nav_2d_utils - 1.1.7 + 1.1.8 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index 586762e8130..65572c482d5 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 1.1.7 + 1.1.8 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index 5eda211d959..35d39d5f576 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 1.1.7 + 1.1.8 Refactored map server for ROS2 Navigation diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index 315349f7e98..d1e5b68de56 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -2,7 +2,7 @@ nav2_mppi_controller - 1.1.7 + 1.1.8 nav2_mppi_controller Aleksei Budyakov Steve Macenski diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index cd9c0d4b156..f38fb405b19 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 1.1.7 + 1.1.8 Messages and service files for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index 60e1947097c..d957d1ee6eb 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 1.1.7 + 1.1.8 TODO Steve Macenski Carlos Orduno diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index f025a7d1592..4083e291b64 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 1.1.7 + 1.1.8 TODO Steve Macenski Apache-2.0 diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index 3b3fd9c12a9..5edc284ed25 100644 --- a/nav2_regulated_pure_pursuit_controller/package.xml +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -2,7 +2,7 @@ nav2_regulated_pure_pursuit_controller - 1.1.7 + 1.1.8 Regulated Pure Pursuit Controller Steve Macenski Shrijit Singh diff --git a/nav2_rotation_shim_controller/package.xml b/nav2_rotation_shim_controller/package.xml index 6f5be755619..6a44e9029b6 100644 --- a/nav2_rotation_shim_controller/package.xml +++ b/nav2_rotation_shim_controller/package.xml @@ -2,7 +2,7 @@ nav2_rotation_shim_controller - 1.1.7 + 1.1.8 Rotation Shim Controller Steve Macenski Apache-2.0 diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 71f2cb60f6e..5436f3d46f5 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 1.1.7 + 1.1.8 Navigation 2 plugins for rviz Michael Jeronimo Apache-2.0 diff --git a/nav2_simple_commander/package.xml b/nav2_simple_commander/package.xml index 766f106d1d3..c87e718cfcc 100644 --- a/nav2_simple_commander/package.xml +++ b/nav2_simple_commander/package.xml @@ -2,7 +2,7 @@ nav2_simple_commander - 1.1.7 + 1.1.8 An importable library for writing mobile robot applications in python3 steve Apache-2.0 diff --git a/nav2_smac_planner/package.xml b/nav2_smac_planner/package.xml index 65ace345922..524aa418db2 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -2,7 +2,7 @@ nav2_smac_planner - 1.1.7 + 1.1.8 Smac global planning plugin: A*, Hybrid-A*, State Lattice Steve Macenski Apache-2.0 diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml index 53cbad8d0a1..bc54f7b45c8 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -2,7 +2,7 @@ nav2_smoother - 1.1.7 + 1.1.8 Smoother action interface Matej Vargovcik Steve Macenski diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 2cb3f05e2df..fb121d0673d 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 1.1.7 + 1.1.8 TODO Carlos Orduno Apache-2.0 diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index ffe7674a230..c6554db0399 100644 --- a/nav2_theta_star_planner/package.xml +++ b/nav2_theta_star_planner/package.xml @@ -2,7 +2,7 @@ nav2_theta_star_planner - 1.1.7 + 1.1.8 Theta* Global Planning Plugin Steve Macenski Anshumaan Singh diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 2de5fa3b587..3b5e246ef73 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 1.1.7 + 1.1.8 TODO Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_velocity_smoother/package.xml b/nav2_velocity_smoother/package.xml index a8da7d2ef38..986ba2be89a 100644 --- a/nav2_velocity_smoother/package.xml +++ b/nav2_velocity_smoother/package.xml @@ -2,7 +2,7 @@ nav2_velocity_smoother - 1.1.7 + 1.1.8 Nav2's Output velocity smoother Steve Macenski Apache-2.0 diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index 0d9327bd3d3..f92a2166c19 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 1.1.7 + 1.1.8 voxel_grid provides an implementation of an efficient 3D voxel grid. The occupancy grid can support 3 different representations for the state of a cell: marked, free, or unknown. Due to the underlying implementation relying on bitwise and and or integer operations, the voxel grid only supports 16 different levels per voxel column. However, this limitation yields raytracing and cell marking performance in the grid comparable to standard 2D structures making it quite fast compared to most 3D structures. diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index 056a8eb4f2d..adc360bb394 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 1.1.7 + 1.1.8 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/navigation2/package.xml b/navigation2/package.xml index 3f0fd7edda4..c74a2ae1a5f 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 1.1.7 + 1.1.8 ROS2 Navigation Stack From ead48a9039c80cadb18dcfbb21fff0bdd496ef03 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Fri, 30 Jun 2023 10:32:52 -0700 Subject: [PATCH 07/39] Fixing build warning (#3667) (#3673) (cherry picked from commit 7d4b1992811ccc9d36566d29251fdc8eaee66efc) Co-authored-by: Steve Macenski --- nav2_smac_planner/test/test_nodelattice.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_smac_planner/test/test_nodelattice.cpp b/nav2_smac_planner/test/test_nodelattice.cpp index a534e8862f2..61479e6e1b8 100644 --- a/nav2_smac_planner/test/test_nodelattice.cpp +++ b/nav2_smac_planner/test/test_nodelattice.cpp @@ -347,7 +347,7 @@ TEST(NodeLatticeTest, test_node_lattice_custom_footprint) // Test that the node is valid though all motion primitives poses for custom footprint nav2_smac_planner::MotionPrimitivePtrs motion_primitives = nav2_smac_planner::NodeLattice::motion_table.getMotionPrimitives(&node); - EXPECT_GT(motion_primitives.size(), 0); + EXPECT_GT(motion_primitives.size(), 0u); for (unsigned int i = 0; i < motion_primitives.size(); i++) { EXPECT_EQ(node.isNodeValid(true, checker.get(), motion_primitives[i], false), true); EXPECT_EQ(node.isNodeValid(true, checker.get(), motion_primitives[i], true), true); From 7e8439cb6313bf85c0e118250fc2efeb0e842b02 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 26 Jul 2023 11:01:31 -0700 Subject: [PATCH 08/39] Fix the velocity smoother being stuck when the deadband is too high (#3690) (#3715) * Move last_cmd update before deadband * fix lint (cherry picked from commit cb34d0ce1d24c1c437f548834a31a2ee8c4d9889) Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> --- nav2_velocity_smoother/src/velocity_smoother.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index 04a32081a2a..aa5fbec7830 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -299,13 +299,14 @@ void VelocitySmoother::smootherTimer() current_.linear.y, command_->linear.y, max_accels_[1], max_decels_[1], eta); cmd_vel->angular.z = applyConstraints( current_.angular.z, command_->angular.z, max_accels_[2], max_decels_[2], eta); + last_cmd_ = *cmd_vel; // Apply deadband restrictions & publish cmd_vel->linear.x = fabs(cmd_vel->linear.x) < deadband_velocities_[0] ? 0.0 : cmd_vel->linear.x; cmd_vel->linear.y = fabs(cmd_vel->linear.y) < deadband_velocities_[1] ? 0.0 : cmd_vel->linear.y; cmd_vel->angular.z = fabs(cmd_vel->angular.z) < deadband_velocities_[2] ? 0.0 : cmd_vel->angular.z; - last_cmd_ = *cmd_vel; + smoothed_cmd_pub_->publish(std::move(cmd_vel)); } From 0cf04629e2d4ea862160781687fa77d7835557c8 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 4 Aug 2023 13:23:08 -0700 Subject: [PATCH 09/39] Humble sync 7 August 4 1.1.9 (#3739) * Fix map not showing on rviz when navigation is launched with namespace (#3620) * updating mppi's path angle critic for optional bidirectionality (#3624) * updating mppi's path angle critic for optional bidirectionality * Update README.md * fixing path angle critic's non-directional bias (#3632) * fixing path angle critic's non-directional bias * adding reformat * adapting goal critic for speed to goal (#3641) * adapting goal critic for speed to goal * retuning goal critic * add readme entries * Update critics_tests.cpp * Fix uninitialized value (#3651) * In NAV2, this warning is treated as an error Signed-off-by: Ryan Friedman * Fix rviz panel node arguments (#3655) Signed-off-by: Nick Lamprianidis * Reduce out-of-range log to DEBUG (#3656) * Adding nan twist rejection for velocity smoother and collision monitor (#3658) * adding nan twist rejection for velocity smoother and collision monitor * deref * MPPI: Support Exact Path Following For Feasible Plans (#3659) * alternative to path align critic for inversion control * fix default behavior (enforce_path_inversion: false) (#3643) Co-authored-by: Guillaume Doisy * adding dyaw option for path alignment to incentivize following the path's intent where necessary * add docs for use path orientations * fix typo --------- Co-authored-by: Guillaume Doisy Co-authored-by: Guillaume Doisy * Fix smoother server tests (#3663) * Fix smoother server tests * Update test_smoother_server.cpp * nav2_bt_navigator: log current location on navigate_to_pose action initialization (#3720) It is very useful to know the current location considered by the bt_navigator for debug purposes. * nav2_behaviors: export all available plugins (#3716) It allows external packages to include those headers and create child classes through inheritance. * changing costmap layers private to protected (#3722) * adding error warnings around incorrect inflation layer setups in MPPI and Smac which impact performance substantially (#3728) * adding error warnings around incorrect inflation layer setups in MPPI and Smac which impact performance substantially * fix test failures * Update RewrittenYaml to support list rewrites (#3727) * allowing leaf key rewrites that aren't dcits (#3730) * adding checks on config and dynamic parameters for proper velocity and acceleration limits (#3731) * Fix Goal updater QoS (#3719) * Fix GoalUpdater QoS * Fixes * bumping Humble to 1.1.9 for release * fix merge conflict resolution in collision monitor node --------- Signed-off-by: Ryan Friedman Signed-off-by: Nick Lamprianidis Co-authored-by: Filipe Cerveira Co-authored-by: Ryan Co-authored-by: Nick Lamprianidis Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: Guillaume Doisy Co-authored-by: Guillaume Doisy Co-authored-by: DylanDeCoeyer-Quimesis <102609575+DylanDeCoeyer-Quimesis@users.noreply.github.com> Co-authored-by: gyaanantia <78275262+gyaanantia@users.noreply.github.com> Co-authored-by: Tony Najjar --- nav2_amcl/package.xml | 2 +- nav2_behavior_tree/package.xml | 2 +- .../plugins/decorator/goal_updater_node.cpp | 15 +++- .../decorator/test_goal_updater_node.cpp | 89 +++++++++++++++---- .../plugins/assisted_teleop.hpp | 0 .../nav2_behaviors}/plugins/back_up.hpp | 0 .../plugins/drive_on_heading.hpp | 0 .../nav2_behaviors}/plugins/spin.hpp | 0 .../nav2_behaviors}/plugins/wait.hpp | 0 nav2_behaviors/package.xml | 2 +- nav2_behaviors/plugins/assisted_teleop.cpp | 2 +- nav2_behaviors/plugins/back_up.cpp | 2 +- nav2_behaviors/plugins/drive_on_heading.cpp | 2 +- nav2_behaviors/plugins/spin.cpp | 2 +- nav2_behaviors/plugins/wait.cpp | 2 +- nav2_bringup/launch/rviz_launch.py | 3 +- nav2_bringup/package.xml | 2 +- nav2_bt_navigator/package.xml | 2 +- .../src/navigators/navigate_to_pose.cpp | 9 +- .../collision_monitor_node.hpp | 1 + nav2_collision_monitor/package.xml | 2 +- .../src/collision_monitor_node.cpp | 6 ++ nav2_collision_monitor/src/range.cpp | 2 +- .../nav2_common/launch/rewritten_yaml.py | 12 ++- nav2_common/package.xml | 2 +- nav2_constrained_smoother/package.xml | 2 +- nav2_controller/package.xml | 2 +- nav2_core/package.xml | 2 +- .../nav2_costmap_2d/inflation_layer.hpp | 1 - .../nav2_costmap_2d/range_sensor_layer.hpp | 2 +- .../include/nav2_costmap_2d/static_layer.hpp | 2 +- .../include/nav2_costmap_2d/voxel_layer.hpp | 1 - nav2_costmap_2d/package.xml | 2 +- .../costmap_filters/costmap_filter.cpp | 2 +- nav2_dwb_controller/costmap_queue/package.xml | 2 +- nav2_dwb_controller/dwb_core/package.xml | 2 +- nav2_dwb_controller/dwb_critics/package.xml | 2 +- nav2_dwb_controller/dwb_msgs/package.xml | 2 +- nav2_dwb_controller/dwb_plugins/package.xml | 2 +- .../nav2_dwb_controller/package.xml | 2 +- nav2_dwb_controller/nav_2d_msgs/package.xml | 2 +- nav2_dwb_controller/nav_2d_utils/package.xml | 2 +- nav2_lifecycle_manager/package.xml | 2 +- nav2_map_server/package.xml | 2 +- nav2_mppi_controller/README.md | 35 +++++--- .../critics/goal_critic.hpp | 1 + .../critics/path_align_critic.hpp | 1 + .../critics/path_angle_critic.hpp | 2 + .../critics/path_follow_critic.hpp | 1 + .../tools/path_handler.hpp | 19 +++- .../nav2_mppi_controller/tools/utils.hpp | 66 +++++++++++++- nav2_mppi_controller/package.xml | 2 +- .../src/critics/constraint_critic.cpp | 5 +- .../src/critics/goal_angle_critic.cpp | 2 +- .../src/critics/goal_critic.cpp | 15 ++-- .../src/critics/obstacles_critic.cpp | 13 ++- .../src/critics/path_align_critic.cpp | 26 ++++-- .../src/critics/path_angle_critic.cpp | 41 +++++++-- .../src/critics/path_follow_critic.cpp | 2 +- .../src/critics/prefer_forward_critic.cpp | 2 +- nav2_mppi_controller/src/path_handler.cpp | 52 +++++++++-- nav2_mppi_controller/test/critics_tests.cpp | 4 +- .../test/path_handler_test.cpp | 72 +++++++++++++-- nav2_mppi_controller/test/utils_test.cpp | 72 ++++++++++++++- nav2_msgs/package.xml | 2 +- nav2_navfn_planner/package.xml | 2 +- nav2_planner/package.xml | 2 +- .../package.xml | 2 +- nav2_rotation_shim_controller/package.xml | 2 +- nav2_rviz_plugins/package.xml | 2 +- nav2_rviz_plugins/src/nav2_panel.cpp | 2 +- nav2_simple_commander/package.xml | 2 +- .../nav2_smac_planner/collision_checker.hpp | 7 +- nav2_smac_planner/package.xml | 2 +- nav2_smac_planner/src/collision_checker.cpp | 20 ++++- nav2_smac_planner/src/smac_planner_2d.cpp | 4 +- nav2_smac_planner/src/smac_planner_hybrid.cpp | 10 +-- .../src/smac_planner_lattice.cpp | 4 +- nav2_smac_planner/test/test_a_star.cpp | 14 +-- .../test/test_collision_checker.cpp | 24 +++-- nav2_smac_planner/test/test_node2d.cpp | 6 +- nav2_smac_planner/test/test_nodehybrid.cpp | 9 +- nav2_smac_planner/test/test_nodelattice.cpp | 18 +++- nav2_smac_planner/test/test_smoother.cpp | 2 +- nav2_smoother/package.xml | 2 +- nav2_smoother/test/test_smoother_server.cpp | 38 ++++---- nav2_system_tests/package.xml | 2 +- nav2_theta_star_planner/package.xml | 2 +- nav2_util/include/nav2_util/robot_utils.hpp | 9 ++ nav2_util/package.xml | 2 +- nav2_util/src/robot_utils.cpp | 30 +++++++ nav2_util/test/test_robot_utils.cpp | 26 ++++++ .../velocity_smoother.hpp | 1 + nav2_velocity_smoother/package.xml | 2 +- .../src/velocity_smoother.cpp | 37 +++++++- .../test/test_velocity_smoother.cpp | 34 +++++++ nav2_voxel_grid/package.xml | 2 +- nav2_waypoint_follower/package.xml | 2 +- navigation2/package.xml | 2 +- 99 files changed, 766 insertions(+), 191 deletions(-) rename nav2_behaviors/{ => include/nav2_behaviors}/plugins/assisted_teleop.hpp (100%) rename nav2_behaviors/{ => include/nav2_behaviors}/plugins/back_up.hpp (100%) rename nav2_behaviors/{ => include/nav2_behaviors}/plugins/drive_on_heading.hpp (100%) rename nav2_behaviors/{ => include/nav2_behaviors}/plugins/spin.hpp (100%) rename nav2_behaviors/{ => include/nav2_behaviors}/plugins/wait.hpp (100%) diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index 2975c39f80e..074f39ef540 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 1.1.8 + 1.1.9

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index 589030f1232..cb9333efbbd 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.1.8 + 1.1.9 TODO Michael Jeronimo Carlos Orduno diff --git a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp index febaa7de648..1f7226e6aac 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp @@ -46,7 +46,7 @@ GoalUpdater::GoalUpdater( sub_option.callback_group = callback_group_; goal_sub_ = node_->create_subscription( goal_updater_topic, - 10, + rclcpp::SystemDefaultsQoS(), std::bind(&GoalUpdater::callback_updated_goal, this, _1), sub_option); } @@ -59,8 +59,17 @@ inline BT::NodeStatus GoalUpdater::tick() callback_group_executor_.spin_some(); - if (rclcpp::Time(last_goal_received_.header.stamp) > rclcpp::Time(goal.header.stamp)) { - goal = last_goal_received_; + if (last_goal_received_.header.stamp != rclcpp::Time(0)) { + auto last_goal_received_time = rclcpp::Time(last_goal_received_.header.stamp); + auto goal_time = rclcpp::Time(goal.header.stamp); + if (last_goal_received_time > goal_time) { + goal = last_goal_received_; + } else { + RCLCPP_WARN( + node_->get_logger(), "The timestamp of the received goal (%f) is older than the " + "current goal (%f). Ignoring the received goal.", + last_goal_received_time.seconds(), goal_time.seconds()); + } } setOutput("output_goal", goal); diff --git a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp index 341192757fb..95bb3e48b55 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp @@ -95,6 +95,8 @@ TEST_F(GoalUpdaterTestFixture, test_tick) )"; tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + auto goal_updater_pub = + node_->create_publisher("goal_update", 10); // create new goal and set it on blackboard geometry_msgs::msg::PoseStamped goal; @@ -102,35 +104,92 @@ TEST_F(GoalUpdaterTestFixture, test_tick) goal.pose.position.x = 1.0; config_->blackboard->set("goal", goal); - // tick until node succeeds - while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { - tree_->rootNode()->executeTick(); - } - + // tick tree without publishing updated goal and get updated_goal + tree_->rootNode()->executeTick(); geometry_msgs::msg::PoseStamped updated_goal; config_->blackboard->get("updated_goal", updated_goal); +} - EXPECT_EQ(updated_goal, goal); +TEST_F(GoalUpdaterTestFixture, test_older_goal_update) +{ + // create tree + std::string xml_txt = + R"( + + + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + auto goal_updater_pub = + node_->create_publisher("goal_update", 10); + + // create new goal and set it on blackboard + geometry_msgs::msg::PoseStamped goal; + goal.header.stamp = node_->now(); + goal.pose.position.x = 1.0; + config_->blackboard->set("goal", goal); + // publish updated_goal older than goal geometry_msgs::msg::PoseStamped goal_to_update; - goal_to_update.header.stamp = node_->now(); + goal_to_update.header.stamp = rclcpp::Time(goal.header.stamp) - rclcpp::Duration(1, 0); goal_to_update.pose.position.x = 2.0; + goal_updater_pub->publish(goal_to_update); + tree_->rootNode()->executeTick(); + geometry_msgs::msg::PoseStamped updated_goal; + config_->blackboard->get("updated_goal", updated_goal); + + // expect to succeed and not update goal + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(updated_goal, goal); +} + +TEST_F(GoalUpdaterTestFixture, test_get_latest_goal_update) +{ + // create tree + std::string xml_txt = + R"( + + + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); auto goal_updater_pub = node_->create_publisher("goal_update", 10); - auto start = node_->now(); - while ((node_->now() - start).seconds() < 0.5) { - tree_->rootNode()->executeTick(); - goal_updater_pub->publish(goal_to_update); + // create new goal and set it on blackboard + geometry_msgs::msg::PoseStamped goal; + goal.header.stamp = node_->now(); + goal.pose.position.x = 1.0; + config_->blackboard->set("goal", goal); - rclcpp::spin_some(node_); - } + // publish updated_goal older than goal + geometry_msgs::msg::PoseStamped goal_to_update_1; + goal_to_update_1.header.stamp = node_->now(); + goal_to_update_1.pose.position.x = 2.0; + + geometry_msgs::msg::PoseStamped goal_to_update_2; + goal_to_update_2.header.stamp = node_->now(); + goal_to_update_2.pose.position.x = 3.0; + goal_updater_pub->publish(goal_to_update_1); + goal_updater_pub->publish(goal_to_update_2); + tree_->rootNode()->executeTick(); + geometry_msgs::msg::PoseStamped updated_goal; config_->blackboard->get("updated_goal", updated_goal); - EXPECT_NE(updated_goal, goal); - EXPECT_EQ(updated_goal, goal_to_update); + // expect to succeed + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + // expect to update goal with latest goal update + EXPECT_EQ(updated_goal, goal_to_update_2); } int main(int argc, char ** argv) diff --git a/nav2_behaviors/plugins/assisted_teleop.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/assisted_teleop.hpp similarity index 100% rename from nav2_behaviors/plugins/assisted_teleop.hpp rename to nav2_behaviors/include/nav2_behaviors/plugins/assisted_teleop.hpp diff --git a/nav2_behaviors/plugins/back_up.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/back_up.hpp similarity index 100% rename from nav2_behaviors/plugins/back_up.hpp rename to nav2_behaviors/include/nav2_behaviors/plugins/back_up.hpp diff --git a/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp similarity index 100% rename from nav2_behaviors/plugins/drive_on_heading.hpp rename to nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp diff --git a/nav2_behaviors/plugins/spin.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/spin.hpp similarity index 100% rename from nav2_behaviors/plugins/spin.hpp rename to nav2_behaviors/include/nav2_behaviors/plugins/spin.hpp diff --git a/nav2_behaviors/plugins/wait.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/wait.hpp similarity index 100% rename from nav2_behaviors/plugins/wait.hpp rename to nav2_behaviors/include/nav2_behaviors/plugins/wait.hpp diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml index d7942a1893b..6f6494b7557 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -2,7 +2,7 @@ nav2_behaviors - 1.1.8 + 1.1.9 TODO Carlos Orduno Steve Macenski diff --git a/nav2_behaviors/plugins/assisted_teleop.cpp b/nav2_behaviors/plugins/assisted_teleop.cpp index defdfaf5a70..2882773bc0e 100644 --- a/nav2_behaviors/plugins/assisted_teleop.cpp +++ b/nav2_behaviors/plugins/assisted_teleop.cpp @@ -14,7 +14,7 @@ #include -#include "assisted_teleop.hpp" +#include "nav2_behaviors/plugins/assisted_teleop.hpp" #include "nav2_util/node_utils.hpp" namespace nav2_behaviors diff --git a/nav2_behaviors/plugins/back_up.cpp b/nav2_behaviors/plugins/back_up.cpp index 90a69a686ed..61246d05098 100644 --- a/nav2_behaviors/plugins/back_up.cpp +++ b/nav2_behaviors/plugins/back_up.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "back_up.hpp" +#include "nav2_behaviors/plugins/back_up.hpp" namespace nav2_behaviors { diff --git a/nav2_behaviors/plugins/drive_on_heading.cpp b/nav2_behaviors/plugins/drive_on_heading.cpp index 53525b3bb6e..9b44f0bacb8 100644 --- a/nav2_behaviors/plugins/drive_on_heading.cpp +++ b/nav2_behaviors/plugins/drive_on_heading.cpp @@ -14,7 +14,7 @@ // limitations under the License. #include -#include "drive_on_heading.hpp" +#include "nav2_behaviors/plugins/drive_on_heading.hpp" #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS(nav2_behaviors::DriveOnHeading<>, nav2_core::Behavior) diff --git a/nav2_behaviors/plugins/spin.cpp b/nav2_behaviors/plugins/spin.cpp index a76fe38f525..b7df8b36541 100644 --- a/nav2_behaviors/plugins/spin.cpp +++ b/nav2_behaviors/plugins/spin.cpp @@ -18,7 +18,7 @@ #include #include -#include "spin.hpp" +#include "nav2_behaviors/plugins/spin.hpp" #include "tf2/utils.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "nav2_util/node_utils.hpp" diff --git a/nav2_behaviors/plugins/wait.cpp b/nav2_behaviors/plugins/wait.cpp index 236f7122018..5100ab01b0a 100644 --- a/nav2_behaviors/plugins/wait.cpp +++ b/nav2_behaviors/plugins/wait.cpp @@ -15,7 +15,7 @@ #include #include -#include "wait.hpp" +#include "nav2_behaviors/plugins/wait.hpp" namespace nav2_behaviors { diff --git a/nav2_bringup/launch/rviz_launch.py b/nav2_bringup/launch/rviz_launch.py index 9f123b81eb6..29194acad5b 100644 --- a/nav2_bringup/launch/rviz_launch.py +++ b/nav2_bringup/launch/rviz_launch.py @@ -71,7 +71,8 @@ def generate_launch_description(): namespace=namespace, arguments=['-d', namespaced_rviz_config_file], output='screen', - remappings=[('/tf', 'tf'), + remappings=[('/map', 'map'), + ('/tf', 'tf'), ('/tf_static', 'tf_static'), ('/goal_pose', 'goal_pose'), ('/clicked_point', 'clicked_point'), diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index c239f0036dd..6b018950c4d 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.1.8 + 1.1.9 Bringup scripts and configurations for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index c4ba88031d2..cdba8c07f13 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.1.8 + 1.1.9 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index 82727af5e2d..bc22714ef58 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -204,8 +204,15 @@ NavigateToPoseNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal) void NavigateToPoseNavigator::initializeGoalPose(ActionT::Goal::ConstSharedPtr goal) { + geometry_msgs::msg::PoseStamped current_pose; + nav2_util::getCurrentPose( + current_pose, *feedback_utils_.tf, + feedback_utils_.global_frame, feedback_utils_.robot_frame, + feedback_utils_.transform_tolerance); + RCLCPP_INFO( - logger_, "Begin navigating from current location to (%.2f, %.2f)", + logger_, "Begin navigating from current location (%.2f, %.2f) to (%.2f, %.2f)", + current_pose.pose.position.x, current_pose.pose.position.y, goal->pose.pose.position.x, goal->pose.pose.position.y); // Reset state for new action feedback diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index 2dc29958568..2aed9920d0a 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp @@ -27,6 +27,7 @@ #include "tf2_ros/transform_listener.h" #include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/robot_utils.hpp" #include "nav2_collision_monitor/types.hpp" #include "nav2_collision_monitor/polygon.hpp" diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index d80a6ff0e0a..eb750b4e607 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -2,7 +2,7 @@ nav2_collision_monitor - 1.1.8 + 1.1.9 Collision Monitor Alexey Merzlyakov Steve Macenski diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 939555bf3d8..69523e3c14e 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -148,6 +148,12 @@ CollisionMonitor::on_shutdown(const rclcpp_lifecycle::State & /*state*/) void CollisionMonitor::cmdVelInCallback(geometry_msgs::msg::Twist::ConstSharedPtr msg) { + // If message contains NaN or Inf, ignore + if (!nav2_util::validateTwist(*msg)) { + RCLCPP_ERROR(get_logger(), "Velocity message contains NaNs or Infs! Ignoring as invalid!"); + return; + } + process({msg->linear.x, msg->linear.y, msg->angular.z}); } diff --git a/nav2_collision_monitor/src/range.cpp b/nav2_collision_monitor/src/range.cpp index 7c12a4bd525..ae15bd488cb 100644 --- a/nav2_collision_monitor/src/range.cpp +++ b/nav2_collision_monitor/src/range.cpp @@ -78,7 +78,7 @@ void Range::getData( // Ignore data, if its range is out of scope of range sensor abilities if (data_->range < data_->min_range || data_->range > data_->max_range) { - RCLCPP_WARN( + RCLCPP_DEBUG( logger_, "[%s]: Data range %fm is out of {%f..%f} sensor span. Ignoring...", source_name_.c_str(), data_->range, data_->min_range, data_->max_range); diff --git a/nav2_common/nav2_common/launch/rewritten_yaml.py b/nav2_common/nav2_common/launch/rewritten_yaml.py index 3b29fdda595..2887fa5503f 100644 --- a/nav2_common/nav2_common/launch/rewritten_yaml.py +++ b/nav2_common/nav2_common/launch/rewritten_yaml.py @@ -128,17 +128,21 @@ def updateYamlPathVals(self, yaml, yaml_key_list, rewrite_val): yaml[key] = rewrite_val break key = yaml_key_list.pop(0) - yaml[key] = self.updateYamlPathVals(yaml.get(key, {}), yaml_key_list, rewrite_val) - + if isinstance(yaml, list): + yaml[int(key)] = self.updateYamlPathVals(yaml[int(key)], yaml_key_list, rewrite_val) + else: + yaml[key] = self.updateYamlPathVals(yaml.get(key, {}), yaml_key_list, rewrite_val) return yaml def substitute_keys(self, yaml, key_rewrites): if len(key_rewrites) != 0: - for key, val in yaml.items(): - if isinstance(val, dict) and key in key_rewrites: + for key in list(yaml.keys()): + val = yaml[key] + if key in key_rewrites: new_key = key_rewrites[key] yaml[new_key] = yaml[key] del yaml[key] + if isinstance(val, dict): self.substitute_keys(val, key_rewrites) def getYamlLeafKeys(self, yamlData): diff --git a/nav2_common/package.xml b/nav2_common/package.xml index e10aae9ddaa..d215166a495 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.1.8 + 1.1.9 Common support functionality used throughout the navigation 2 stack Carl Delsey Apache-2.0 diff --git a/nav2_constrained_smoother/package.xml b/nav2_constrained_smoother/package.xml index 762ae72db32..df524559579 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -2,7 +2,7 @@ nav2_constrained_smoother - 1.1.8 + 1.1.9 Ceres constrained smoother Matej Vargovcik Steve Macenski diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index a9a686981a3..d3ae1711be1 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.1.8 + 1.1.9 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_core/package.xml b/nav2_core/package.xml index dfc71ee64f9..c5a5fcae00e 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.1.8 + 1.1.9 A set of headers for plugins core to the Nav2 stack Steve Macenski Carl Delsey diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp index 5f68dcaed12..48d126577d6 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp @@ -178,7 +178,6 @@ class InflationLayer : public Layer */ void onFootprintChanged() override; -private: /** * @brief Lookup pre-computed distances * @param mx The x coordinate of the current cell diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp index 71cd4654a96..a4383f14771 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp @@ -128,7 +128,7 @@ class RangeSensorLayer : public CostmapLayer */ void bufferIncomingRangeMsg(const sensor_msgs::msg::Range::SharedPtr range_message); -private: +protected: /** * @brief Processes all sensors into the costmap buffered from callbacks */ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp index aec68fac1f5..dd1a65cef0c 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp @@ -123,7 +123,7 @@ class StaticLayer : public CostmapLayer */ virtual void matchSize(); -private: +protected: /** * @brief Get parameters of layer */ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp index 14baefcbdd9..888e401439f 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp @@ -133,7 +133,6 @@ class VoxelLayer : public ObstacleLayer */ virtual void resetMaps(); -private: /** * @brief Use raycasting between 2 points to clear freespace */ diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index 1b571b2745a..daf0c64fab0 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 1.1.8 + 1.1.9 This package provides an implementation of a 2D costmap that takes in sensor data from the world, builds a 2D or 3D occupancy grid of the data (depending diff --git a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp index ff6aa1d5d04..3ca44c2a78d 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp @@ -75,7 +75,7 @@ void CostmapFilter::onInitialize() // Get parameters node->get_parameter(name_ + "." + "enabled", enabled_); filter_info_topic_ = node->get_parameter(name_ + "." + "filter_info_topic").as_string(); - double transform_tolerance; + double transform_tolerance {}; node->get_parameter(name_ + "." + "transform_tolerance", transform_tolerance); transform_tolerance_ = tf2::durationFromSec(transform_tolerance); diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index bc4debf1be5..881883872c3 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 1.1.8 + 1.1.9 The costmap_queue package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml index b08ab9a7308..93228c13f51 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 1.1.8 + 1.1.9 TODO Carl Delsey BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index 3f74ce58a1b..025990ae337 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 1.1.8 + 1.1.9 The dwb_critics package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml index 7778d8ebf63..4ebe4e1404e 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 1.1.8 + 1.1.9 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index 9abbd2499f3..f26bfef2510 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 1.1.8 + 1.1.9 Standard implementations of the GoalChecker and TrajectoryGenerators for dwb_core diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index da5861c10eb..b3e77825da6 100644 --- a/nav2_dwb_controller/nav2_dwb_controller/package.xml +++ b/nav2_dwb_controller/nav2_dwb_controller/package.xml @@ -2,7 +2,7 @@ nav2_dwb_controller - 1.1.8 + 1.1.9 ROS2 controller (DWB) metapackage diff --git a/nav2_dwb_controller/nav_2d_msgs/package.xml b/nav2_dwb_controller/nav_2d_msgs/package.xml index 09c4e745a44..5561c30bdfa 100644 --- a/nav2_dwb_controller/nav_2d_msgs/package.xml +++ b/nav2_dwb_controller/nav_2d_msgs/package.xml @@ -2,7 +2,7 @@ nav_2d_msgs - 1.1.8 + 1.1.9 Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D. David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml index d541ce92b7f..417e2e7d62e 100644 --- a/nav2_dwb_controller/nav_2d_utils/package.xml +++ b/nav2_dwb_controller/nav_2d_utils/package.xml @@ -2,7 +2,7 @@ nav_2d_utils - 1.1.8 + 1.1.9 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index 65572c482d5..c1ec76b748c 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 1.1.8 + 1.1.9 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index 35d39d5f576..94ea2640c89 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 1.1.8 + 1.1.9 Refactored map server for ROS2 Navigation diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index f025075d17b..951ffb15624 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -66,6 +66,9 @@ This process is then repeated a number of times and returns a converged solution | max_robot_pose_search_dist | double | Default: Costmap half-size. Max integrated distance ahead of robot pose to search for nearest path point in case of path looping. | | prune_distance | double | Default: 1.5. Distance ahead of nearest point on path to robot to prune path to. | | transform_tolerance | double | Default: 0.1. Time tolerance for data transformations with TF. | + | enforce_path_inversion | double | Default: False. If true, it will prune paths containing cusping points for segments changing directions (e.g. path inversions) such that the controller will be forced to change directions at or very near the planner's requested inversion point. This is targeting Smac Planner users with feasible paths who need their robots to switch directions where specifically requested. | + | inversion_xy_tolerance | double | Default: 0.2. Cartesian proximity (m) to path inversion point to be considered "achieved" to pass on the rest of the path after path inversion. | + | inversion_yaw_tolerance | double | Default: 0.4. Angular proximity (radians) to path inversion point to be considered "achieved" to pass on the rest of the path after path inversion. 0.4 rad = 23 deg. | #### Ackermann Motion Model | Parameter | Type | Definition | @@ -83,14 +86,14 @@ This process is then repeated a number of times and returns a converged solution | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | | cost_weight | double | Default 3.0. Weight to apply to critic term. | | cost_power | int | Default 1. Power order to apply to term. | - | threshold_to_consider | double | Default 0.40. Minimal distance between robot and goal above which angle goal cost considered. | + | threshold_to_consider | double | Default 0.5. Minimal distance between robot and goal above which angle goal cost considered. | #### Goal Critic | Parameter | Type | Definition | | -------------------- | ------ | ----------------------------------------------------------------------------------------------------------- | | cost_weight | double | Default 5.0. Weight to apply to critic term. | | cost_power | int | Default 1. Power order to apply to term. | - | threshold_to_consider | double | Default 1.0. Distance between robot and goal above which goal cost starts being considered | + | threshold_to_consider | double | Default 1.4. Distance between robot and goal above which goal cost starts being considered | #### Obstacles Critic @@ -111,20 +114,22 @@ This process is then repeated a number of times and returns a converged solution | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | | cost_weight | double | Default 10.0. Weight to apply to critic term. | | cost_power | int | Default 1. Power order to apply to term. | - | threshold_to_consider | double | Default 0.4. Distance between robot and goal above which path align cost stops being considered | + | threshold_to_consider | double | Default 0.5. Distance between robot and goal above which path align cost stops being considered | | offset_from_furthest | double | Default 20. Checks that the candidate trajectories are sufficiently far along their way tracking the path to apply the alignment critic. This ensures that path alignment is only considered when actually tracking the path, preventing awkward initialization motions preventing the robot from leaving the path to achieve the appropriate heading. | | trajectory_point_step | double | Default 4. Step of trajectory points to evaluate for path distance to reduce compute time. Between 1-10 is typically reasonable. | | max_path_occupancy_ratio | double | Default 0.07 (7%). Maximum proportion of the path that can be occupied before this critic is not considered to allow the obstacle and path follow critics to avoid obstacles while following the path's intent in presence of dynamic objects in the scene. | - + | use_path_orientations | bool | Default false. Whether to consider path's orientations in path alignment, which can be useful when paired with feasible smac planners to incentivize directional changes only where/when the smac planner requests them. If you want the robot to deviate and invert directions where the controller sees fit, keep as false. If your plans do not contain orientation information (e.g. navfn), keep as false. | #### Path Angle Critic | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | | cost_weight | double | Default 2.0. Weight to apply to critic term. | | cost_power | int | Default 1. Power order to apply to term. | - | threshold_to_consider | double | Default 0.4. Distance between robot and goal above which path angle cost stops being considered | + | threshold_to_consider | double | Default 0.5. Distance between robot and goal above which path angle cost stops being considered | | offset_from_furthest | int | Default 4. Number of path points after furthest one any trajectory achieves to compute path angle relative to. | | max_angle_to_furthest | double | Default 1.2. Angular distance between robot and goal above which path angle cost starts being considered | + | forward_preference | bool | Default true. Whether or not your robot has a preference for which way is forward in motion. Different from if reversing is generally allowed, but if you robot contains *no* particular preference one way or another. | + #### Path Follow Critic | Parameter | Type | Definition | @@ -132,14 +137,14 @@ This process is then repeated a number of times and returns a converged solution | cost_weight | double | Default 5.0. Weight to apply to critic term. | | cost_power | int | Default 1. Power order to apply to term. | | offset_from_furthest | int | Default 6. Number of path points after furthest one any trajectory achieves to drive path tracking relative to. | - | threshold_to_consider | float | Default 0.4. Distance between robot and goal above which path follow cost stops being considered | + | threshold_to_consider | float | Default 1.4. Distance between robot and goal above which path follow cost stops being considered | #### Prefer Forward Critic | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | | cost_weight | double | Default 5.0. Weight to apply to critic term. | | cost_power | int | Default 1. Power order to apply to term. | - | threshold_to_consider | double | Default 0.4. Distance between robot and goal above which prefer forward cost stops being considered | + | threshold_to_consider | double | Default 0.5. Distance between robot and goal above which prefer forward cost stops being considered | #### Twirling Critic @@ -186,17 +191,17 @@ controller_server: enabled: true cost_power: 1 cost_weight: 5.0 - threshold_to_consider: 1.0 + threshold_to_consider: 1.4 GoalAngleCritic: enabled: true cost_power: 1 cost_weight: 3.0 - threshold_to_consider: 0.4 + threshold_to_consider: 0.5 PreferForwardCritic: enabled: true cost_power: 1 cost_weight: 5.0 - threshold_to_consider: 0.4 + threshold_to_consider: 0.5 ObstaclesCritic: enabled: true cost_power: 1 @@ -212,21 +217,23 @@ controller_server: cost_weight: 14.0 max_path_occupancy_ratio: 0.05 trajectory_point_step: 3 - threshold_to_consider: 0.40 + threshold_to_consider: 0.5 offset_from_furthest: 20 + use_path_orientations: false PathFollowCritic: enabled: true cost_power: 1 cost_weight: 5.0 offset_from_furthest: 5 - threshold_to_consider: 0.6 + threshold_to_consider: 1.4 PathAngleCritic: enabled: true cost_power: 1 cost_weight: 2.0 offset_from_furthest: 4 - threshold_to_consider: 0.40 + threshold_to_consider: 0.5 max_angle_to_furthest: 1.0 + forward_preference: true # TwirlingCritic: # enabled: true # twirling_cost_power: 1 @@ -255,7 +262,7 @@ If you don't require path following behavior (e.g. just want to follow a goal po As this is a predictive planner, there is some relationship between maximum speed, prediction times, and costmap size that users should keep in mind while tuning for their application. If a controller server costmap is set to 3.0m in size, that means that with the robot in the center, there is 1.5m of information on either side of the robot. When your prediction horizon (time_steps * model_dt) at maximum speed (vx_max) is larger than this, then your robot will be artificially limited in its maximum speeds and behavior by the costmap limitation. For example, if you predict forward 3 seconds (60 steps @ 0.05s per step) at 0.5m/s maximum speed, the **minimum** required costmap radius is 1.5m - or 3m total width. -The same applies to the Path Follow and Align offsets from furthest. In the same example if the furthest point we can consider is already at the edge of the costmap, then further offsets are thresholded because they're unusable. So its important while selecting these parameters to make sure that the theoretical offsets can exist on the costmap settings selected with the maximum prediction horizon and velocities desired. +The same applies to the Path Follow and Align offsets from furthest. In the same example if the furthest point we can consider is already at the edge of the costmap, then further offsets are thresholded because they're unusable. So its important while selecting these parameters to make sure that the theoretical offsets can exist on the costmap settings selected with the maximum prediction horizon and velocities desired. Setting the threshold for consideration in the path follower + goal critics as the same as your prediction horizon can make sure you have clean hand-offs between them, as the path follower will otherwise attempt to slow slightly once it reaches the final goal pose as its marker. The Path Follow critic cannot drive velocities greater than the projectable distance of that velocity on the available path on the rolling costmap. The Path Align critic `offset_from_furthest` represents the number of path points a trajectory passes through while tracking the path. If this is set either absurdly low (e.g. 5) it can trigger when a robot is simply trying to start path tracking causing some suboptimal behaviors and local minima while starting a task. If it is set absurdly high (e.g. 50) relative to the path resolution and costmap size, then the critic may never trigger or only do so when at full-speed. A balance here is wise. A selection of this value to be ~30% of the maximum velocity distance projected is good (e.g. if a planner produces points every 2.5cm, 60 can fit on the 1.5m local costmap radius. If the max speed is 0.5m/s with a 3s prediction time, then 20 points represents 33% of the maximum speed projected over the prediction horizon onto the path). When in doubt, `prediction_horizon_s * max_speed / path_resolution / 3.0` is a good baseline. diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp index 62d6bd10420..8fb8fb688ae 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp @@ -1,4 +1,5 @@ // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp index 7618afb9a4b..a5499955575 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp @@ -49,6 +49,7 @@ class PathAlignCritic : public CriticFunction int trajectory_point_step_{0}; float threshold_to_consider_{0}; float max_path_occupancy_ratio_{0}; + bool use_path_orientations_{false}; unsigned int power_{0}; float weight_{0}; }; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp index 92130dceb36..6bf412eb258 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp @@ -48,6 +48,8 @@ class PathAngleCritic : public CriticFunction float threshold_to_consider_{0}; size_t offset_from_furthest_{0}; + bool reversing_allowed_{true}; + bool forward_preference_{true}; unsigned int power_{0}; float weight_{0}; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp index 12317c7b614..c06fba5b896 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp @@ -1,4 +1,5 @@ // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp index 2fbf4edac7f..9f0803a23ae 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp @@ -1,4 +1,6 @@ // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Dexory +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -123,10 +125,18 @@ class PathHandler const geometry_msgs::msg::PoseStamped & global_pose); /** - * @brief Prune global path to only interesting portions + * @brief Prune a path to only interesting portions + * @param plan Plan to prune * @param end Final path iterator */ - void pruneGlobalPlan(const PathIterator end); + void prunePlan(nav_msgs::msg::Path & plan, const PathIterator end); + + /** + * @brief Check if the robot pose is within the set inversion tolerances + * @param robot_pose Robot's current pose to check + * @return bool If the robot pose is within the set inversion tolerances + */ + bool isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose); std::string name_; std::shared_ptr costmap_; @@ -134,11 +144,16 @@ class PathHandler ParametersHandler * parameters_handler_; nav_msgs::msg::Path global_plan_; + nav_msgs::msg::Path global_plan_up_to_inversion_; rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; double max_robot_pose_search_dist_{0}; double prune_distance_{0}; double transform_tolerance_{0}; + double inversion_xy_tolerance_{0.2}; + double inversion_yaw_tolerance{0.4}; + bool enforce_path_inversion_{false}; + unsigned int inversion_locale_{0u}; }; } // namespace mppi diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index 7319e318f0d..93c1aae1061 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -1,4 +1,5 @@ // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -416,15 +417,25 @@ inline void setPathCostsIfNotSet( * @param pose pose * @param point_x Point to find angle relative to X axis * @param point_y Point to find angle relative to Y axis + * @param forward_preference If reversing direction is valid * @return Angle between two points */ -inline double posePointAngle(const geometry_msgs::msg::Pose & pose, double point_x, double point_y) +inline double posePointAngle( + const geometry_msgs::msg::Pose & pose, double point_x, double point_y, bool forward_preference) { double pose_x = pose.position.x; double pose_y = pose.position.y; double pose_yaw = tf2::getYaw(pose.orientation); double yaw = atan2(point_y - pose_y, point_x - pose_x); + + // If no preference for forward, return smallest angle either in heading or 180 of heading + if (!forward_preference) { + return std::min( + abs(angles::shortest_angular_distance(yaw, pose_yaw)), + abs(angles::shortest_angular_distance(yaw, angles::normalize_angle(pose_yaw + M_PI)))); + } + return abs(angles::shortest_angular_distance(yaw, pose_yaw)); } @@ -599,6 +610,59 @@ inline void savitskyGolayFilter( control_sequence.wz(offset)}; } +/** + * @brief Find the iterator of the first pose at which there is an inversion on the path, + * @param path to check for inversion + * @return the first point after the inversion found in the path + */ +inline unsigned int findFirstPathInversion(nav_msgs::msg::Path & path) +{ + // At least 3 poses for a possible inversion + if (path.poses.size() < 3) { + return path.poses.size(); + } + + // Iterating through the path to determine the position of the path inversion + for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) { + // We have two vectors for the dot product OA and AB. Determining the vectors. + double oa_x = path.poses[idx].pose.position.x - + path.poses[idx - 1].pose.position.x; + double oa_y = path.poses[idx].pose.position.y - + path.poses[idx - 1].pose.position.y; + double ab_x = path.poses[idx + 1].pose.position.x - + path.poses[idx].pose.position.x; + double ab_y = path.poses[idx + 1].pose.position.y - + path.poses[idx].pose.position.y; + + // Checking for the existance of cusp, in the path, using the dot product. + double dot_product = (oa_x * ab_x) + (oa_y * ab_y); + if (dot_product < 0.0) { + return idx + 1; + } + } + + return path.poses.size(); +} + +/** + * @brief Find and remove poses after the first inversion in the path + * @param path to check for inversion + * @return The location of the inversion, return 0 if none exist + */ +inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path) +{ + nav_msgs::msg::Path cropped_path = path; + const unsigned int first_after_inversion = findFirstPathInversion(cropped_path); + if (first_after_inversion == path.poses.size()) { + return 0u; + } + + cropped_path.poses.erase( + cropped_path.poses.begin() + first_after_inversion, cropped_path.poses.end()); + path = cropped_path; + return first_after_inversion; +} + } // namespace mppi::utils #endif // NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_ diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index d1e5b68de56..017957da525 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -2,7 +2,7 @@ nav2_mppi_controller - 1.1.8 + 1.1.9 nav2_mppi_controller Aleksei Budyakov Steve Macenski diff --git a/nav2_mppi_controller/src/critics/constraint_critic.cpp b/nav2_mppi_controller/src/critics/constraint_critic.cpp index fc26f89d118..4dc2bcf1deb 100644 --- a/nav2_mppi_controller/src/critics/constraint_critic.cpp +++ b/nav2_mppi_controller/src/critics/constraint_critic.cpp @@ -28,15 +28,14 @@ void ConstraintCritic::initialize() logger_, "ConstraintCritic instantiated with %d power and %f weight.", power_, weight_); - float vx_max, vy_max, vx_min, vy_min; + float vx_max, vy_max, vx_min; getParentParam(vx_max, "vx_max", 0.5); getParentParam(vy_max, "vy_max", 0.0); getParentParam(vx_min, "vx_min", -0.35); - getParentParam(vy_min, "vy_min", 0.0); const float min_sgn = vx_min > 0.0 ? 1.0 : -1.0; max_vel_ = std::sqrt(vx_max * vx_max + vy_max * vy_max); - min_vel_ = min_sgn * std::sqrt(vx_min * vx_min + vy_min * vy_min); + min_vel_ = min_sgn * std::sqrt(vx_min * vx_min + vy_max * vy_max); } void ConstraintCritic::score(CriticData & data) diff --git a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp index c196871327c..62bfdf0676d 100644 --- a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp @@ -24,7 +24,7 @@ void GoalAngleCritic::initialize() getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 3.0); - getParam(threshold_to_consider_, "threshold_to_consider", 0.40); + getParam(threshold_to_consider_, "threshold_to_consider", 0.5); RCLCPP_INFO( logger_, diff --git a/nav2_mppi_controller/src/critics/goal_critic.cpp b/nav2_mppi_controller/src/critics/goal_critic.cpp index 859a9c3f5c3..e38a98ed6b0 100644 --- a/nav2_mppi_controller/src/critics/goal_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_critic.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -17,13 +18,15 @@ namespace mppi::critics { +using xt::evaluation_strategy::immediate; + void GoalCritic::initialize() { auto getParam = parameters_handler_->getParamGetter(name_); getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 5.0); - getParam(threshold_to_consider_, "threshold_to_consider", 1.0); + getParam(threshold_to_consider_, "threshold_to_consider", 1.4); RCLCPP_INFO( logger_, "GoalCritic instantiated with %d power and %f weight.", @@ -47,14 +50,14 @@ void GoalCritic::score(CriticData & data) const auto goal_x = data.path.x(goal_idx); const auto goal_y = data.path.y(goal_idx); - const auto last_x = xt::view(data.trajectories.x, xt::all(), -1); - const auto last_y = xt::view(data.trajectories.y, xt::all(), -1); + const auto traj_x = xt::view(data.trajectories.x, xt::all(), xt::all()); + const auto traj_y = xt::view(data.trajectories.y, xt::all(), xt::all()); auto dists = xt::sqrt( - xt::pow(last_x - goal_x, 2) + - xt::pow(last_y - goal_y, 2)); + xt::pow(traj_x - goal_x, 2) + + xt::pow(traj_y - goal_y, 2)); - data.costs += xt::pow(std::move(dists) * weight_, power_); + data.costs += xt::pow(xt::mean(dists, {1}) * weight_, power_); } } // namespace mppi::critics diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index 44e629ddc06..fa98d569739 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -31,6 +31,17 @@ void ObstaclesCritic::initialize() collision_checker_.setCostmap(costmap_); possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_); + + if (possibly_inscribed_cost_ < 1) { + RCLCPP_ERROR( + logger_, + "Inflation layer either not found or inflation is not set sufficiently for " + "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set" + " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See " + "github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields" + " for full instructions. This will substantially impact run-time performance."); + } + RCLCPP_INFO( logger_, "ObstaclesCritic instantiated with %d power and %f / %f weights. " @@ -184,7 +195,7 @@ CollisionCost ObstaclesCritic::costAtPose(float x, float y, float theta) collision_checker_.worldToMap(x, y, x_i, y_i); cost = collision_checker_.pointCost(x_i, y_i); - if (consider_footprint_ && cost >= possibly_inscribed_cost_) { + if (consider_footprint_ && (cost >= possibly_inscribed_cost_ || possibly_inscribed_cost_ < 1)) { cost = static_cast(collision_checker_.footprintCostAtPose( x, y, theta, costmap_ros_->getRobotFootprint())); collision_cost.using_footprint = true; diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index b95b96a04a6..2585193ade5 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -34,7 +34,8 @@ void PathAlignCritic::initialize() getParam(trajectory_point_step_, "trajectory_point_step", 4); getParam( threshold_to_consider_, - "threshold_to_consider", 0.40f); + "threshold_to_consider", 0.5); + getParam(use_path_orientations_, "use_path_orientations", false); RCLCPP_INFO( logger_, @@ -71,9 +72,11 @@ void PathAlignCritic::score(CriticData & data) const auto & T_x = data.trajectories.x; const auto & T_y = data.trajectories.y; + const auto & T_yaw = data.trajectories.yaws; const auto P_x = xt::view(data.path.x, xt::range(_, -1)); // path points const auto P_y = xt::view(data.path.y, xt::range(_, -1)); // path points + const auto P_yaw = xt::view(data.path.yaws, xt::range(_, -1)); // path points const size_t batch_size = T_x.shape(0); const size_t time_steps = T_x.shape(1); @@ -85,18 +88,27 @@ void PathAlignCritic::score(CriticData & data) return; } + float dist_sq = 0, dx = 0, dy = 0, dyaw = 0, summed_dist = 0; + double min_dist_sq = std::numeric_limits::max(); + size_t min_s = 0; + for (size_t t = 0; t < batch_size; ++t) { - float summed_dist = 0; + summed_dist = 0; for (size_t p = trajectory_point_step_; p < time_steps; p += trajectory_point_step_) { - double min_dist_sq = std::numeric_limits::max(); - size_t min_s = 0; + min_dist_sq = std::numeric_limits::max(); + min_s = 0; // Find closest path segment to the trajectory point for (size_t s = 0; s < path_segments_count - 1; s++) { xt::xtensor_fixed> P; - float dx = P_x(s) - T_x(t, p); - float dy = P_y(s) - T_y(t, p); - float dist_sq = dx * dx + dy * dy; + dx = P_x(s) - T_x(t, p); + dy = P_y(s) - T_y(t, p); + if (use_path_orientations_) { + dyaw = angles::shortest_angular_distance(P_yaw(s), T_yaw(t, p)); + dist_sq = dx * dx + dy * dy + dyaw * dyaw; + } else { + dist_sq = dx * dx + dy * dy; + } if (dist_sq < min_dist_sq) { min_dist_sq = dist_sq; min_s = s; diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index a588bbe2913..f09554bf189 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -21,22 +22,37 @@ namespace mppi::critics void PathAngleCritic::initialize() { + auto getParentParam = parameters_handler_->getParamGetter(parent_name_); + float vx_min; + getParentParam(vx_min, "vx_min", -0.35); + if (fabs(vx_min) < 1e-6) { // zero + reversing_allowed_ = false; + } else if (vx_min < 0.0) { // reversing possible + reversing_allowed_ = true; + } + auto getParam = parameters_handler_->getParamGetter(name_); getParam(offset_from_furthest_, "offset_from_furthest", 4); getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 2.0); getParam( threshold_to_consider_, - "threshold_to_consider", 0.40f); + "threshold_to_consider", 0.5); getParam( max_angle_to_furthest_, "max_angle_to_furthest", 1.2); + getParam( + forward_preference_, + "forward_preference", true); + if (!reversing_allowed_) { + forward_preference_ = true; + } RCLCPP_INFO( logger_, - "PathAngleCritic instantiated with %d power and %f weight.", - power_, weight_); + "PathAngleCritic instantiated with %d power and %f weight. Reversing %s", + power_, weight_, reversing_allowed_ ? "allowed." : "not allowed."); } void PathAngleCritic::score(CriticData & data) @@ -58,17 +74,28 @@ void PathAngleCritic::score(CriticData & data) const float goal_x = xt::view(data.path.x, offseted_idx); const float goal_y = xt::view(data.path.y, offseted_idx); - if (utils::posePointAngle(data.state.pose.pose, goal_x, goal_y) < max_angle_to_furthest_) { + if (utils::posePointAngle( + data.state.pose.pose, goal_x, goal_y, forward_preference_) < max_angle_to_furthest_) + { return; } - const auto yaws_between_points = xt::atan2( + auto yaws_between_points = xt::atan2( goal_y - data.trajectories.y, goal_x - data.trajectories.x); - const auto yaws = + + auto yaws = xt::abs(utils::shortest_angular_distance(data.trajectories.yaws, yaws_between_points)); - data.costs += xt::pow(xt::mean(yaws, {1}, immediate) * weight_, power_); + if (reversing_allowed_ && !forward_preference_) { + const auto yaws_between_points_corrected = xt::where( + yaws < M_PI_2, yaws_between_points, utils::normalize_angles(yaws_between_points + M_PI)); + const auto corrected_yaws = xt::abs( + utils::shortest_angular_distance(data.trajectories.yaws, yaws_between_points_corrected)); + data.costs += xt::pow(xt::mean(corrected_yaws, {1}, immediate) * weight_, power_); + } else { + data.costs += xt::pow(xt::mean(yaws, {1}, immediate) * weight_, power_); + } } } // namespace mppi::critics diff --git a/nav2_mppi_controller/src/critics/path_follow_critic.cpp b/nav2_mppi_controller/src/critics/path_follow_critic.cpp index e440bd0b3e8..01ecb91728e 100644 --- a/nav2_mppi_controller/src/critics/path_follow_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_follow_critic.cpp @@ -26,7 +26,7 @@ void PathFollowCritic::initialize() getParam( threshold_to_consider_, - "threshold_to_consider", 0.40f); + "threshold_to_consider", 1.4); getParam(offset_from_furthest_, "offset_from_furthest", 6); getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 5.0); diff --git a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp index 5cea014bbc2..18b6900177a 100644 --- a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp +++ b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp @@ -24,7 +24,7 @@ void PreferForwardCritic::initialize() getParam(weight_, "cost_weight", 5.0); getParam( threshold_to_consider_, - "threshold_to_consider", 0.40f); + "threshold_to_consider", 0.5); RCLCPP_INFO( logger_, "PreferForwardCritic instantiated with %d power and %f weight.", power_, weight_); diff --git a/nav2_mppi_controller/src/path_handler.cpp b/nav2_mppi_controller/src/path_handler.cpp index 5127917e287..cfefb3b6d38 100644 --- a/nav2_mppi_controller/src/path_handler.cpp +++ b/nav2_mppi_controller/src/path_handler.cpp @@ -1,4 +1,6 @@ // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Dexory +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -35,6 +37,12 @@ void PathHandler::initialize( getParam(max_robot_pose_search_dist_, "max_robot_pose_search_dist", getMaxCostmapDist()); getParam(prune_distance_, "prune_distance", 1.5); getParam(transform_tolerance_, "transform_tolerance", 0.1); + getParam(enforce_path_inversion_, "enforce_path_inversion", false); + if (enforce_path_inversion_) { + getParam(inversion_xy_tolerance_, "inversion_xy_tolerance", 0.2); + getParam(inversion_yaw_tolerance, "inversion_yaw_tolerance", 0.4); + inversion_locale_ = 0u; + } } std::pair @@ -43,12 +51,13 @@ PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( { using nav2_util::geometry_utils::euclidean_distance; - auto begin = global_plan_.poses.begin(); + auto begin = global_plan_up_to_inversion_.poses.begin(); // Limit the search for the closest pose up to max_robot_pose_search_dist on the path auto closest_pose_upper_bound = nav2_util::geometry_utils::first_after_integrated_distance( - global_plan_.poses.begin(), global_plan_.poses.end(), max_robot_pose_search_dist_); + global_plan_up_to_inversion_.poses.begin(), global_plan_up_to_inversion_.poses.end(), + max_robot_pose_search_dist_); // Find closest point to the robot auto closest_point = nav2_util::geometry_utils::min_by( @@ -63,7 +72,7 @@ PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( auto pruned_plan_end = nav2_util::geometry_utils::first_after_integrated_distance( - closest_point, global_plan_.poses.end(), prune_distance_); + closest_point, global_plan_up_to_inversion_.poses.end(), prune_distance_); unsigned int mx, my; // Find the furthest relevent pose on the path to consider within costmap @@ -95,12 +104,12 @@ PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( geometry_msgs::msg::PoseStamped PathHandler::transformToGlobalPlanFrame( const geometry_msgs::msg::PoseStamped & pose) { - if (global_plan_.poses.empty()) { + if (global_plan_up_to_inversion_.poses.empty()) { throw std::runtime_error("Received plan with zero length"); } geometry_msgs::msg::PoseStamped robot_pose; - if (!transformPose(global_plan_.header.frame_id, pose, robot_pose)) { + if (!transformPose(global_plan_up_to_inversion_.header.frame_id, pose, robot_pose)) { throw std::runtime_error( "Unable to transform robot pose into global plan's frame"); } @@ -116,7 +125,15 @@ nav_msgs::msg::Path PathHandler::transformPath( transformToGlobalPlanFrame(robot_pose); auto [transformed_plan, lower_bound] = getGlobalPlanConsideringBoundsInCostmapFrame(global_pose); - pruneGlobalPlan(lower_bound); + prunePlan(global_plan_up_to_inversion_, lower_bound); + + if (enforce_path_inversion_ && inversion_locale_ != 0u) { + if (isWithinInversionTolerances(global_pose)) { + prunePlan(global_plan_, global_plan_.poses.begin() + inversion_locale_); + global_plan_up_to_inversion_ = global_plan_; + inversion_locale_ = utils::removePosesAfterFirstInversion(global_plan_up_to_inversion_); + } + } if (transformed_plan.poses.empty()) { throw std::runtime_error("Resulting plan has 0 poses in it."); @@ -156,13 +173,32 @@ double PathHandler::getMaxCostmapDist() void PathHandler::setPath(const nav_msgs::msg::Path & plan) { global_plan_ = plan; + global_plan_up_to_inversion_ = global_plan_; + if (enforce_path_inversion_) { + inversion_locale_ = utils::removePosesAfterFirstInversion(global_plan_up_to_inversion_); + } } nav_msgs::msg::Path & PathHandler::getPath() {return global_plan_;} -void PathHandler::pruneGlobalPlan(const PathIterator end) +void PathHandler::prunePlan(nav_msgs::msg::Path & plan, const PathIterator end) { - global_plan_.poses.erase(global_plan_.poses.begin(), end); + plan.poses.erase(plan.poses.begin(), end); +} + +bool PathHandler::isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose) +{ + // Keep full path if we are within tolerance of the inversion pose + const auto last_pose = global_plan_up_to_inversion_.poses.back(); + double distance = std::hypot( + robot_pose.pose.position.x - last_pose.pose.position.x, + robot_pose.pose.position.y - last_pose.pose.position.y); + + double angle_distance = angles::shortest_angular_distance( + tf2::getYaw(robot_pose.pose.orientation), + tf2::getYaw(last_pose.pose.orientation)); + + return distance <= inversion_xy_tolerance_ && fabs(angle_distance) <= inversion_yaw_tolerance; } } // namespace mppi diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index e79e4fe2417..a1a42aa7764 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -417,9 +417,9 @@ TEST(CriticTests, PathFollowCritic) // Scoring testing // provide state poses and path close within positional tolerances - state.pose.pose.position.x = 1.0; + state.pose.pose.position.x = 2.0; path.reset(6); - path.x(5) = 0.85; + path.x(5) = 1.7; critic.score(data); EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); diff --git a/nav2_mppi_controller/test/path_handler_test.cpp b/nav2_mppi_controller/test/path_handler_test.cpp index 20e055392f3..e6a992f4824 100644 --- a/nav2_mppi_controller/test/path_handler_test.cpp +++ b/nav2_mppi_controller/test/path_handler_test.cpp @@ -38,9 +38,9 @@ class PathHandlerWrapper : public PathHandler PathHandlerWrapper() : PathHandler() {} - void pruneGlobalPlanWrapper(const PathIterator end) + void pruneGlobalPlanWrapper(nav_msgs::msg::Path & path, const PathIterator end) { - return pruneGlobalPlan(end); + return prunePlan(path, end); } double getMaxCostmapDistWrapper() @@ -66,6 +66,21 @@ class PathHandlerWrapper : public PathHandler { return transformToGlobalPlanFrame(pose); } + + void setGlobalPlanUpToInversion(const nav_msgs::msg::Path & path) + { + global_plan_up_to_inversion_ = path; + } + + bool isWithinInversionTolerancesWrapper(const geometry_msgs::msg::PoseStamped & robot_pose) + { + return isWithinInversionTolerances(robot_pose); + } + + nav_msgs::msg::Path & getInvertedPath() + { + return global_plan_up_to_inversion_; + } }; TEST(PathHandlerTests, GetAndPrunePath) @@ -82,7 +97,7 @@ TEST(PathHandlerTests, GetAndPrunePath) EXPECT_EQ(path.poses.size(), rtn_path.poses.size()); PathIterator it = rtn_path.poses.begin() + 5; - handler.pruneGlobalPlanWrapper(it); + handler.pruneGlobalPlanWrapper(rtn_path, it); auto rtn2_path = handler.getPath(); EXPECT_EQ(rtn2_path.poses.size(), 6u); } @@ -131,10 +146,10 @@ TEST(PathHandlerTests, TestBounds) handler.setPath(path); auto [transformed_plan, closest] = handler.getGlobalPlanConsideringBoundsInCostmapFrameWrapper(robot_pose); - auto & path_in = handler.getPath(); - EXPECT_EQ(closest - path_in.poses.begin(), 25); - handler.pruneGlobalPlanWrapper(closest); - auto & path_pruned = handler.getPath(); + auto & path_inverted = handler.getInvertedPath(); + EXPECT_EQ(closest - path_inverted.poses.begin(), 25); + handler.pruneGlobalPlanWrapper(path_inverted, closest); + auto & path_pruned = handler.getInvertedPath(); EXPECT_EQ(path_pruned.poses.size(), 75u); } @@ -189,3 +204,46 @@ TEST(PathHandlerTests, TestTransforms) auto final_path = handler.transformPath(robot_pose); EXPECT_EQ(final_path.poses.size(), path_out.poses.size()); } + +TEST(PathHandlerTests, TestInversionToleranceChecks) +{ + nav_msgs::msg::Path path; + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = static_cast(i); + path.poses.push_back(pose); + } + path.poses.back().pose.orientation.w = 1; + + PathHandlerWrapper handler; + handler.setGlobalPlanUpToInversion(path); + + // Not near (0,0) + geometry_msgs::msg::PoseStamped robot_pose; + EXPECT_FALSE(handler.isWithinInversionTolerancesWrapper(robot_pose)); + + // Exactly on top of it + robot_pose.pose.position.x = 9; + robot_pose.pose.orientation.w = 1.0; + EXPECT_TRUE(handler.isWithinInversionTolerancesWrapper(robot_pose)); + + // Laterally of it + robot_pose.pose.position.y = 9; + EXPECT_FALSE(handler.isWithinInversionTolerancesWrapper(robot_pose)); + + // On top but off angled + robot_pose.pose.position.y = 0; + robot_pose.pose.orientation.z = 0.8509035; + robot_pose.pose.orientation.w = 0.525322; + EXPECT_FALSE(handler.isWithinInversionTolerancesWrapper(robot_pose)); + + // On top but off angled within tolerances + robot_pose.pose.position.y = 0; + robot_pose.pose.orientation.w = 0.9961947; + robot_pose.pose.orientation.z = 0.0871558; + EXPECT_TRUE(handler.isWithinInversionTolerancesWrapper(robot_pose)); + + // Offset spatially + off angled but both within tolerances + robot_pose.pose.position.x = 9.10; + EXPECT_TRUE(handler.isWithinInversionTolerancesWrapper(robot_pose)); +} diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp index 37acd78e876..8e6c2e4e501 100644 --- a/nav2_mppi_controller/test/utils_test.cpp +++ b/nav2_mppi_controller/test/utils_test.cpp @@ -195,7 +195,14 @@ TEST(UtilsTests, AnglesTests) pose.position.y = 0.0; pose.orientation.w = 1.0; double point_x = 1.0, point_y = 0.0; - EXPECT_NEAR(posePointAngle(pose, point_x, point_y), 0.0, 1e-6); + bool forward_preference = true; + EXPECT_NEAR(posePointAngle(pose, point_x, point_y, forward_preference), 0.0, 1e-6); + forward_preference = false; + EXPECT_NEAR(posePointAngle(pose, point_x, point_y, forward_preference), 0.0, 1e-6); + point_x = -1.0; + EXPECT_NEAR(posePointAngle(pose, point_x, point_y, forward_preference), 0.0, 1e-6); + forward_preference = true; + EXPECT_NEAR(posePointAngle(pose, point_x, point_y, forward_preference), M_PI, 1e-6); } TEST(UtilsTests, FurthestAndClosestReachedPoint) @@ -362,3 +369,66 @@ TEST(UtilsTests, SmootherTest) EXPECT_LT(smoothed_val, original_val); } + +TEST(UtilsTests, FindPathInversionTest) +{ + // Straight path, no inversions to be found + nav_msgs::msg::Path path; + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = i; + path.poses.push_back(pose); + } + EXPECT_EQ(utils::findFirstPathInversion(path), 10u); + + // To short to process + path.poses.erase(path.poses.begin(), path.poses.begin() + 7); + EXPECT_EQ(utils::findFirstPathInversion(path), 3u); + + // Has inversion at index 10, so should return 11 for the first point afterwards + // 0 1 2 3 4 5 6 7 8 9 10 **9** 8 7 6 5 4 3 2 1 + path.poses.clear(); + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = i; + path.poses.push_back(pose); + } + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = 10 - i; + path.poses.push_back(pose); + } + EXPECT_EQ(utils::findFirstPathInversion(path), 11u); +} + +TEST(UtilsTests, RemovePosesAfterPathInversionTest) +{ + nav_msgs::msg::Path path; + // straight path + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = i; + path.poses.push_back(pose); + } + EXPECT_EQ(utils::removePosesAfterFirstInversion(path), 0u); + + // try empty path + path.poses.clear(); + EXPECT_EQ(utils::removePosesAfterFirstInversion(path), 0u); + + // cusping path + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = i; + path.poses.push_back(pose); + } + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = 10 - i; + path.poses.push_back(pose); + } + EXPECT_EQ(utils::removePosesAfterFirstInversion(path), 11u); + // Check to see if removed + EXPECT_EQ(path.poses.size(), 11u); + EXPECT_EQ(path.poses.back().pose.position.x, 10); +} diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index f38fb405b19..49b585af2a1 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 1.1.8 + 1.1.9 Messages and service files for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index d957d1ee6eb..2b5ca370092 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 1.1.8 + 1.1.9 TODO Steve Macenski Carlos Orduno diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index 4083e291b64..18873edabdd 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 1.1.8 + 1.1.9 TODO Steve Macenski Apache-2.0 diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index 5edc284ed25..23e7cc44b53 100644 --- a/nav2_regulated_pure_pursuit_controller/package.xml +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -2,7 +2,7 @@ nav2_regulated_pure_pursuit_controller - 1.1.8 + 1.1.9 Regulated Pure Pursuit Controller Steve Macenski Shrijit Singh diff --git a/nav2_rotation_shim_controller/package.xml b/nav2_rotation_shim_controller/package.xml index 6a44e9029b6..f97c3662249 100644 --- a/nav2_rotation_shim_controller/package.xml +++ b/nav2_rotation_shim_controller/package.xml @@ -2,7 +2,7 @@ nav2_rotation_shim_controller - 1.1.8 + 1.1.9 Rotation Shim Controller Steve Macenski Apache-2.0 diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 5436f3d46f5..20200ebeefd 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 1.1.8 + 1.1.9 Navigation 2 plugins for rviz Michael Jeronimo Apache-2.0 diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp index 085bf014b2d..b39fe348097 100644 --- a/nav2_rviz_plugins/src/nav2_panel.cpp +++ b/nav2_rviz_plugins/src/nav2_panel.cpp @@ -275,7 +275,7 @@ Nav2Panel::Nav2Panel(QWidget * parent) accumulated_nav_through_poses_->addTransition(accumulatedNTPTransition); auto options = rclcpp::NodeOptions().arguments( - {"--ros-args --remap __node:=navigation_dialog_action_client"}); + {"--ros-args", "--remap", "__node:=rviz_navigation_dialog_action_client", "--"}); client_node_ = std::make_shared("_", options); client_nav_ = std::make_shared( diff --git a/nav2_simple_commander/package.xml b/nav2_simple_commander/package.xml index c87e718cfcc..830d297706f 100644 --- a/nav2_simple_commander/package.xml +++ b/nav2_simple_commander/package.xml @@ -2,7 +2,7 @@ nav2_simple_commander - 1.1.8 + 1.1.9 An importable library for writing mobile robot applications in python3 steve Apache-2.0 diff --git a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp index e6520f4bd26..5933a6dc92f 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp @@ -14,6 +14,7 @@ #include #include "nav2_costmap_2d/footprint_collision_checker.hpp" #include "nav2_smac_planner/constants.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" #ifndef NAV2_SMAC_PLANNER__COLLISION_CHECKER_HPP_ #define NAV2_SMAC_PLANNER__COLLISION_CHECKER_HPP_ @@ -34,11 +35,13 @@ class GridCollisionChecker * for use when regular bin intervals are appropriate * @param costmap The costmap to collision check against * @param num_quantizations The number of quantizations to precompute footprint + * @param node Node to extract clock and logger from * orientations for to speed up collision checking */ GridCollisionChecker( nav2_costmap_2d::Costmap2D * costmap, - unsigned int num_quantizations); + unsigned int num_quantizations, + rclcpp_lifecycle::LifecycleNode::SharedPtr node); /** * @brief A constructor for nav2_smac_planner::GridCollisionChecker @@ -117,6 +120,8 @@ class GridCollisionChecker bool footprint_is_radius_; std::vector angles_; double possible_inscribed_cost_{-1}; + rclcpp::Logger logger_{rclcpp::get_logger("SmacPlannerCollisionChecker")}; + rclcpp::Clock::SharedPtr clock_; }; } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/package.xml b/nav2_smac_planner/package.xml index 524aa418db2..0ab37485fcf 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -2,7 +2,7 @@ nav2_smac_planner - 1.1.8 + 1.1.9 Smac global planning plugin: A*, Hybrid-A*, State Lattice Steve Macenski Apache-2.0 diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp index c0cbfb98fa7..20d288809da 100644 --- a/nav2_smac_planner/src/collision_checker.cpp +++ b/nav2_smac_planner/src/collision_checker.cpp @@ -19,9 +19,15 @@ namespace nav2_smac_planner GridCollisionChecker::GridCollisionChecker( nav2_costmap_2d::Costmap2D * costmap, - unsigned int num_quantizations) + unsigned int num_quantizations, + rclcpp_lifecycle::LifecycleNode::SharedPtr node) : FootprintCollisionChecker(costmap) { + if (node) { + clock_ = node->get_clock(); + logger_ = node->get_logger(); + } + // Convert number of regular bins into angles float bin_size = 2 * M_PI / static_cast(num_quantizations); angles_.reserve(num_quantizations); @@ -104,7 +110,17 @@ bool GridCollisionChecker::inCollision( static_cast(x), static_cast(y)); if (footprint_cost_ < possible_inscribed_cost_) { - return false; + if (possible_inscribed_cost_ > 0) { + return false; + } else { + RCLCPP_ERROR_THROTTLE( + logger_, *clock_, 1000, + "Inflation layer either not found or inflation is not set sufficiently for " + "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set" + " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See " + "github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields" + " for full instructions. This will substantially impact run-time performance."); + } } // If its inscribed, in collision, or unknown in the middle, diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 606f6861efd..333bf20c8bf 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -31,7 +31,7 @@ using std::placeholders::_1; SmacPlanner2D::SmacPlanner2D() : _a_star(nullptr), - _collision_checker(nullptr, 1), + _collision_checker(nullptr, 1, nullptr), _smoother(nullptr), _costmap(nullptr), _costmap_downsampler(nullptr) @@ -108,7 +108,7 @@ void SmacPlanner2D::configure( } // Initialize collision checker - _collision_checker = GridCollisionChecker(_costmap, 1 /*for 2D, most be 1*/); + _collision_checker = GridCollisionChecker(_costmap, 1 /*for 2D, most be 1*/, node); _collision_checker.setFootprint( costmap_ros->getRobotFootprint(), true /*for 2D, most use radius*/, diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index ee614c7b946..c4c3019afc6 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -32,7 +32,7 @@ using std::placeholders::_1; SmacPlannerHybrid::SmacPlannerHybrid() : _a_star(nullptr), - _collision_checker(nullptr, 1), + _collision_checker(nullptr, 1, nullptr), _smoother(nullptr), _costmap(nullptr), _costmap_downsampler(nullptr) @@ -182,7 +182,7 @@ void SmacPlannerHybrid::configure( } // Initialize collision checker - _collision_checker = GridCollisionChecker(_costmap, _angle_quantizations); + _collision_checker = GridCollisionChecker(_costmap, _angle_quantizations, node); _collision_checker.setFootprint( _costmap_ros->getRobotFootprint(), _costmap_ros->getUseRadius(), @@ -525,6 +525,8 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para _lookup_table_dim += 1.0; } + auto node = _node.lock(); + // Re-Initialize A* template if (reinit_a_star) { _a_star = std::make_unique>(_motion_model, _search_info); @@ -540,7 +542,6 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para // Re-Initialize costmap downsampler if (reinit_downsampler) { if (_downsample_costmap && _downsampling_factor > 1) { - auto node = _node.lock(); std::string topic_name = "downsampled_costmap"; _costmap_downsampler = std::make_unique(); _costmap_downsampler->on_configure( @@ -550,7 +551,7 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para // Re-Initialize collision checker if (reinit_collision_checker) { - _collision_checker = GridCollisionChecker(_costmap, _angle_quantizations); + _collision_checker = GridCollisionChecker(_costmap, _angle_quantizations, node); _collision_checker.setFootprint( _costmap_ros->getRobotFootprint(), _costmap_ros->getUseRadius(), @@ -559,7 +560,6 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para // Re-Initialize smoother if (reinit_smoother) { - auto node = _node.lock(); SmootherParams params; params.get(node, _name); _smoother = std::make_unique(params); diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index b7e22ee1f96..248ef1f218a 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -31,7 +31,7 @@ using rcl_interfaces::msg::ParameterType; SmacPlannerLattice::SmacPlannerLattice() : _a_star(nullptr), - _collision_checker(nullptr, 1), + _collision_checker(nullptr, 1, nullptr), _smoother(nullptr), _costmap(nullptr) { @@ -169,7 +169,7 @@ void SmacPlannerLattice::configure( // increments causing "wobbly" checks that could cause larger robots to virtually show collisions // in valid configurations. This approximation helps to bound orientation error for all checks // in exchange for slight inaccuracies in the collision headings in terminal search states. - _collision_checker = GridCollisionChecker(_costmap, 72u); + _collision_checker = GridCollisionChecker(_costmap, 72u, node); _collision_checker.setFootprint( costmap_ros->getRobotFootprint(), costmap_ros->getUseRadius(), diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index 72a6ba29683..05dfb0796f8 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -39,6 +39,7 @@ RclCppFixture g_rclcppfixture; TEST(AStarTest, test_a_star_2d) { + auto lnode = std::make_shared("test"); nav2_smac_planner::SearchInfo info; nav2_smac_planner::AStarAlgorithm a_star( nav2_smac_planner::MotionModel::TWOD, info); @@ -62,7 +63,7 @@ TEST(AStarTest, test_a_star_2d) // functional case testing std::unique_ptr checker = - std::make_unique(costmapA, 1); + std::make_unique(costmapA, 1, lnode); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); a_star.setCollisionChecker(checker.get()); a_star.setStart(20u, 20u, 0); @@ -122,6 +123,7 @@ TEST(AStarTest, test_a_star_2d) TEST(AStarTest, test_a_star_se2) { + auto lnode = std::make_shared("test"); nav2_smac_planner::SearchInfo info; info.change_penalty = 0.1; info.non_straight_penalty = 1.1; @@ -152,7 +154,7 @@ TEST(AStarTest, test_a_star_se2) } std::unique_ptr checker = - std::make_unique(costmapA, size_theta); + std::make_unique(costmapA, size_theta, lnode); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); // functional case testing @@ -178,6 +180,7 @@ TEST(AStarTest, test_a_star_se2) TEST(AStarTest, test_a_star_lattice) { + auto lnode = std::make_shared("test"); nav2_smac_planner::SearchInfo info; info.change_penalty = 0.05; info.non_straight_penalty = 1.05; @@ -213,7 +216,7 @@ TEST(AStarTest, test_a_star_lattice) } std::unique_ptr checker = - std::make_unique(costmapA, size_theta); + std::make_unique(costmapA, size_theta, lnode); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); // functional case testing @@ -225,7 +228,7 @@ TEST(AStarTest, test_a_star_lattice) // check path is the right size and collision free EXPECT_EQ(num_it, 21); - EXPECT_EQ(path.size(), 49u); + EXPECT_GT(path.size(), 47u); for (unsigned int i = 0; i != path.size(); i++) { EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); } @@ -239,6 +242,7 @@ TEST(AStarTest, test_a_star_lattice) TEST(AStarTest, test_se2_single_pose_path) { + auto lnode = std::make_shared("test"); nav2_smac_planner::SearchInfo info; info.change_penalty = 0.1; info.non_straight_penalty = 1.1; @@ -263,7 +267,7 @@ TEST(AStarTest, test_se2_single_pose_path) new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); std::unique_ptr checker = - std::make_unique(costmapA, size_theta); + std::make_unique(costmapA, size_theta, lnode); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); // functional case testing diff --git a/nav2_smac_planner/test/test_collision_checker.cpp b/nav2_smac_planner/test/test_collision_checker.cpp index 40e73c82ada..84f2d5b39dd 100644 --- a/nav2_smac_planner/test/test_collision_checker.cpp +++ b/nav2_smac_planner/test/test_collision_checker.cpp @@ -19,11 +19,21 @@ #include "gtest/gtest.h" #include "nav2_smac_planner/collision_checker.hpp" +#include "nav2_util/lifecycle_node.hpp" using namespace nav2_costmap_2d; // NOLINT +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + TEST(collision_footprint, test_basic) { + auto node = std::make_shared("testA"); nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0); geometry_msgs::msg::Point p1; @@ -41,7 +51,7 @@ TEST(collision_footprint, test_basic) nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; - nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72); + nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node); collision_checker.setFootprint(footprint, false /*use footprint*/, 0.0); collision_checker.inCollision(5.0, 5.0, 0.0, false); float cost = collision_checker.getCost(); @@ -51,9 +61,10 @@ TEST(collision_footprint, test_basic) TEST(collision_footprint, test_point_cost) { + auto node = std::make_shared("testB"); nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0); - nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72); + nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node); nav2_costmap_2d::Footprint footprint; collision_checker.setFootprint(footprint, true /*radius / pointcose*/, 0.0); @@ -65,9 +76,10 @@ TEST(collision_footprint, test_point_cost) TEST(collision_footprint, test_world_to_map) { + auto node = std::make_shared("testC"); nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0); - nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72); + nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node); nav2_costmap_2d::Footprint footprint; collision_checker.setFootprint(footprint, true /*radius / point cost*/, 0.0); @@ -90,6 +102,7 @@ TEST(collision_footprint, test_world_to_map) TEST(collision_footprint, test_footprint_at_pose_with_movement) { + auto node = std::make_shared("testD"); nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 254); for (unsigned int i = 40; i <= 60; ++i) { @@ -113,7 +126,7 @@ TEST(collision_footprint, test_footprint_at_pose_with_movement) nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; - nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72); + nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node); collision_checker.setFootprint(footprint, false /*use footprint*/, 0.0); collision_checker.inCollision(50, 50, 0.0, false); @@ -132,6 +145,7 @@ TEST(collision_footprint, test_footprint_at_pose_with_movement) TEST(collision_footprint, test_point_and_line_cost) { + auto node = std::make_shared("testE"); nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D( 100, 100, 0.10000, 0, 0.0, 128.0); @@ -153,7 +167,7 @@ TEST(collision_footprint, test_point_and_line_cost) nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; - nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72); + nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node); collision_checker.setFootprint(footprint, false /*use footprint*/, 0.0); collision_checker.inCollision(50, 50, 0.0, false); diff --git a/nav2_smac_planner/test/test_node2d.cpp b/nav2_smac_planner/test/test_node2d.cpp index f3af941c4bc..eede15fa840 100644 --- a/nav2_smac_planner/test/test_node2d.cpp +++ b/nav2_smac_planner/test/test_node2d.cpp @@ -34,9 +34,10 @@ RclCppFixture g_rclcppfixture; TEST(Node2DTest, test_node_2d) { + auto node = std::make_shared("test"); nav2_costmap_2d::Costmap2D costmapA(10, 10, 0.05, 0.0, 0.0, 0); std::unique_ptr checker = - std::make_unique(&costmapA, 72); + std::make_unique(&costmapA, 72, node); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); // test construction @@ -101,6 +102,7 @@ TEST(Node2DTest, test_node_2d) TEST(Node2DTest, test_node_2d_neighbors) { + auto lnode = std::make_shared("test"); nav2_smac_planner::SearchInfo info; unsigned int size_x = 10u; unsigned int size_y = 10u; @@ -122,7 +124,7 @@ TEST(Node2DTest, test_node_2d_neighbors) nav2_costmap_2d::Costmap2D costmapA(10, 10, 0.05, 0.0, 0.0, 0); std::unique_ptr checker = - std::make_unique(&costmapA, 72); + std::make_unique(&costmapA, 72, lnode); unsigned char cost = static_cast(1); nav2_smac_planner::Node2D * node = new nav2_smac_planner::Node2D(1); node->setCost(cost); diff --git a/nav2_smac_planner/test/test_nodehybrid.cpp b/nav2_smac_planner/test/test_nodehybrid.cpp index e304b97e42c..16d6278bad3 100644 --- a/nav2_smac_planner/test/test_nodehybrid.cpp +++ b/nav2_smac_planner/test/test_nodehybrid.cpp @@ -35,6 +35,7 @@ RclCppFixture g_rclcppfixture; TEST(NodeHybridTest, test_node_hybrid) { + auto node = std::make_shared("test"); nav2_smac_planner::SearchInfo info; info.change_penalty = 0.1; info.non_straight_penalty = 1.1; @@ -56,7 +57,7 @@ TEST(NodeHybridTest, test_node_hybrid) nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D( 10, 10, 0.05, 0.0, 0.0, 0); std::unique_ptr checker = - std::make_unique(costmapA, 72); + std::make_unique(costmapA, 72, node); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); // test construction @@ -135,6 +136,7 @@ TEST(NodeHybridTest, test_node_hybrid) TEST(NodeHybridTest, test_obstacle_heuristic) { + auto node = std::make_shared("test"); nav2_smac_planner::SearchInfo info; info.change_penalty = 0.1; info.non_straight_penalty = 1.1; @@ -169,7 +171,7 @@ TEST(NodeHybridTest, test_obstacle_heuristic) } } std::unique_ptr checker = - std::make_unique(costmapA, 72); + std::make_unique(costmapA, 72, node); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); nav2_smac_planner::NodeHybrid testA(0); @@ -245,6 +247,7 @@ TEST(NodeHybridTest, test_node_debin_neighbors) TEST(NodeHybridTest, test_node_reeds_neighbors) { + auto lnode = std::make_shared("test"); nav2_smac_planner::SearchInfo info; info.change_penalty = 1.2; info.non_straight_penalty = 1.4; @@ -284,7 +287,7 @@ TEST(NodeHybridTest, test_node_reeds_neighbors) nav2_costmap_2d::Costmap2D costmapA(100, 100, 0.05, 0.0, 0.0, 0); std::unique_ptr checker = - std::make_unique(&costmapA, 72); + std::make_unique(&costmapA, 72, lnode); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); nav2_smac_planner::NodeHybrid * node = new nav2_smac_planner::NodeHybrid(49); std::function neighborGetter = diff --git a/nav2_smac_planner/test/test_nodelattice.cpp b/nav2_smac_planner/test/test_nodelattice.cpp index 61479e6e1b8..16674d9dadc 100644 --- a/nav2_smac_planner/test/test_nodelattice.cpp +++ b/nav2_smac_planner/test/test_nodelattice.cpp @@ -22,9 +22,18 @@ #include "nav2_smac_planner/node_lattice.hpp" #include "gtest/gtest.h" #include "ament_index_cpp/get_package_share_directory.hpp" +#include "nav2_util/lifecycle_node.hpp" using json = nlohmann::json; +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + TEST(NodeLatticeTest, parser_test) { std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_smac_planner"); @@ -164,6 +173,7 @@ TEST(NodeLatticeTest, test_node_lattice_conversions) TEST(NodeLatticeTest, test_node_lattice) { + auto node = std::make_shared("test"); std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_smac_planner"); std::string filePath = pkg_share_dir + @@ -207,7 +217,7 @@ TEST(NodeLatticeTest, test_node_lattice) nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D( 10, 10, 0.05, 0.0, 0.0, 0); std::unique_ptr checker = - std::make_unique(costmapA, 72); + std::make_unique(costmapA, 72, node); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); // test node valid and cost @@ -241,6 +251,7 @@ TEST(NodeLatticeTest, test_node_lattice) TEST(NodeLatticeTest, test_get_neighbors) { + auto lnode = std::make_shared("test"); std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_smac_planner"); std::string filePath = pkg_share_dir + @@ -271,7 +282,7 @@ TEST(NodeLatticeTest, test_get_neighbors) nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D( 10, 10, 0.05, 0.0, 0.0, 0); std::unique_ptr checker = - std::make_unique(costmapA, 72); + std::make_unique(costmapA, 72, lnode); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); std::function neighborGetter = @@ -291,6 +302,7 @@ TEST(NodeLatticeTest, test_get_neighbors) TEST(NodeLatticeTest, test_node_lattice_custom_footprint) { + auto lnode = std::make_shared("test"); std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_smac_planner"); std::string filePath = pkg_share_dir + @@ -321,7 +333,7 @@ TEST(NodeLatticeTest, test_node_lattice_custom_footprint) nav2_costmap_2d::Costmap2D * costmap = new nav2_costmap_2d::Costmap2D( 40, 40, 0.05, 0.0, 0.0, 0); std::unique_ptr checker = - std::make_unique(costmap, 72); + std::make_unique(costmap, 72, lnode); // Make some custom asymmetrical footprint nav2_costmap_2d::Footprint footprint; diff --git a/nav2_smac_planner/test/test_smoother.cpp b/nav2_smac_planner/test/test_smoother.cpp index d7d27f1d20d..acf709c2824 100644 --- a/nav2_smac_planner/test/test_smoother.cpp +++ b/nav2_smac_planner/test/test_smoother.cpp @@ -95,7 +95,7 @@ TEST(SmootherTest, test_full_smoother) a_star.initialize( false, max_iterations, std::numeric_limits::max(), max_planning_time, 401, size_theta); std::unique_ptr checker = - std::make_unique(costmap, size_theta); + std::make_unique(costmap, size_theta, node); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); // Create A* search to smooth diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml index bc54f7b45c8..f361b621dbf 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -2,7 +2,7 @@ nav2_smoother - 1.1.8 + 1.1.9 Smoother action interface Matej Vargovcik Steve Macenski diff --git a/nav2_smoother/test/test_smoother_server.cpp b/nav2_smoother/test/test_smoother_server.cpp index 8dbc5416dbf..5bbbf399fa0 100644 --- a/nav2_smoother/test/test_smoother_server.cpp +++ b/nav2_smoother/test/test_smoother_server.cpp @@ -226,21 +226,16 @@ class DummySmootherServer : public nav2_smoother::SmootherServer }; // Define a test class to hold the context for the tests - -class SmootherConfigTest : public ::testing::Test -{ -}; - class SmootherTest : public ::testing::Test { -protected: - SmootherTest() {SetUp();} +public: + SmootherTest() {} ~SmootherTest() {} - void SetUp() + void SetUp() override { - node_lifecycle_ = - std::make_shared( + node_ = + std::make_shared( "LifecycleSmootherTestNode", rclcpp::NodeOptions()); smoother_server_ = std::make_shared(); @@ -255,10 +250,10 @@ class SmootherTest : public ::testing::Test smoother_server_->activate(); client_ = rclcpp_action::create_client( - node_lifecycle_->get_node_base_interface(), - node_lifecycle_->get_node_graph_interface(), - node_lifecycle_->get_node_logging_interface(), - node_lifecycle_->get_node_waitables_interface(), "smooth_path"); + node_->get_node_base_interface(), + node_->get_node_graph_interface(), + node_->get_node_logging_interface(), + node_->get_node_waitables_interface(), "smooth_path"); std::cout << "Setup complete." << std::endl; } @@ -267,6 +262,9 @@ class SmootherTest : public ::testing::Test smoother_server_->deactivate(); smoother_server_->cleanup(); smoother_server_->shutdown(); + smoother_server_.reset(); + client_.reset(); + node_.reset(); } bool sendGoal( @@ -294,7 +292,7 @@ class SmootherTest : public ::testing::Test auto future_goal = client_->async_send_goal(goal); - if (rclcpp::spin_until_future_complete(node_lifecycle_, future_goal) != + if (rclcpp::spin_until_future_complete(node_, future_goal) != rclcpp::FutureReturnCode::SUCCESS) { std::cout << "failed sending goal" << std::endl; @@ -318,12 +316,12 @@ class SmootherTest : public ::testing::Test std::cout << "Getting async result..." << std::endl; auto future_result = client_->async_get_result(goal_handle_); std::cout << "Waiting on future..." << std::endl; - rclcpp::spin_until_future_complete(node_lifecycle_, future_result); + rclcpp::spin_until_future_complete(node_, future_result); std::cout << "future received!" << std::endl; return future_result.get(); } - std::shared_ptr node_lifecycle_; + std::shared_ptr node_; std::shared_ptr smoother_server_; std::shared_ptr> client_; std::shared_ptr> goal_handle_; @@ -390,7 +388,7 @@ TEST_F(SmootherTest, testingCollisionCheckDisabled) SUCCEED(); } -TEST_F(SmootherConfigTest, testingConfigureSuccessWithValidSmootherPlugin) +TEST(SmootherConfigTest, testingConfigureSuccessWithValidSmootherPlugin) { auto smoother_server = std::make_shared(); smoother_server->set_parameter( @@ -405,7 +403,7 @@ TEST_F(SmootherConfigTest, testingConfigureSuccessWithValidSmootherPlugin) SUCCEED(); } -TEST_F(SmootherConfigTest, testingConfigureFailureWithInvalidSmootherPlugin) +TEST(SmootherConfigTest, testingConfigureFailureWithInvalidSmootherPlugin) { auto smoother_server = std::make_shared(); smoother_server->set_parameter( @@ -420,7 +418,7 @@ TEST_F(SmootherConfigTest, testingConfigureFailureWithInvalidSmootherPlugin) SUCCEED(); } -TEST_F(SmootherConfigTest, testingConfigureSuccessWithDefaultPlugin) +TEST(SmootherConfigTest, testingConfigureSuccessWithDefaultPlugin) { auto smoother_server = std::make_shared(); auto state = smoother_server->configure(); diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index fb121d0673d..bfdee60c710 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 1.1.8 + 1.1.9 TODO Carlos Orduno Apache-2.0 diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index c6554db0399..7f0ca4cafdb 100644 --- a/nav2_theta_star_planner/package.xml +++ b/nav2_theta_star_planner/package.xml @@ -2,7 +2,7 @@ nav2_theta_star_planner - 1.1.8 + 1.1.9 Theta* Global Planning Plugin Steve Macenski Anshumaan Singh diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp index baead03cbaa..e653e39a250 100644 --- a/nav2_util/include/nav2_util/robot_utils.hpp +++ b/nav2_util/include/nav2_util/robot_utils.hpp @@ -20,6 +20,8 @@ #include #include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "tf2/time.h" #include "tf2_ros/buffer.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "rclcpp/rclcpp.hpp" @@ -107,6 +109,13 @@ bool getTransform( const std::shared_ptr tf_buffer, tf2::Transform & tf2_transform); +/** + * @brief Validates a twist message contains no nans or infs + * @param msg Twist message to validate + * @return True if valid, false if contains unactionable values + */ +bool validateTwist(const geometry_msgs::msg::Twist & msg); + } // end namespace nav2_util #endif // NAV2_UTIL__ROBOT_UTILS_HPP_ diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 3b5e246ef73..861f323f241 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 1.1.8 + 1.1.9 TODO Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp index 37ab261230d..20791e82938 100644 --- a/nav2_util/src/robot_utils.cpp +++ b/nav2_util/src/robot_utils.cpp @@ -15,6 +15,7 @@ // limitations under the License. #include +#include #include #include "nav2_util/robot_utils.hpp" @@ -141,4 +142,33 @@ bool getTransform( return true; } +bool validateTwist(const geometry_msgs::msg::Twist & msg) +{ + if (std::isinf(msg.linear.x) || std::isnan(msg.linear.x)) { + return false; + } + + if (std::isinf(msg.linear.y) || std::isnan(msg.linear.y)) { + return false; + } + + if (std::isinf(msg.linear.z) || std::isnan(msg.linear.z)) { + return false; + } + + if (std::isinf(msg.angular.x) || std::isnan(msg.angular.x)) { + return false; + } + + if (std::isinf(msg.angular.y) || std::isnan(msg.angular.y)) { + return false; + } + + if (std::isinf(msg.angular.z) || std::isnan(msg.angular.z)) { + return false; + } + + return true; +} + } // end namespace nav2_util diff --git a/nav2_util/test/test_robot_utils.cpp b/nav2_util/test/test_robot_utils.cpp index bd277515ab0..d8922868fb6 100644 --- a/nav2_util/test/test_robot_utils.cpp +++ b/nav2_util/test/test_robot_utils.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include "rclcpp/rclcpp.hpp" #include "nav2_util/robot_utils.hpp" #include "tf2_ros/transform_listener.h" @@ -32,3 +33,28 @@ TEST(RobotUtils, LookupExceptionError) global_pose.header.frame_id = "base_link"; ASSERT_FALSE(nav2_util::transformPoseInTargetFrame(global_pose, global_pose, tf, "map", 0.1)); } + +TEST(RobotUtils, validateTwist) +{ + geometry_msgs::msg::Twist msg; + EXPECT_TRUE(nav2_util::validateTwist(msg)); + + msg.linear.x = NAN; + EXPECT_FALSE(nav2_util::validateTwist(msg)); + msg.linear.x = 1; + msg.linear.y = NAN; + EXPECT_FALSE(nav2_util::validateTwist(msg)); + msg.linear.y = 1; + msg.linear.z = NAN; + EXPECT_FALSE(nav2_util::validateTwist(msg)); + + msg.linear.z = 1; + msg.angular.x = NAN; + EXPECT_FALSE(nav2_util::validateTwist(msg)); + msg.angular.x = 1; + msg.angular.y = NAN; + EXPECT_FALSE(nav2_util::validateTwist(msg)); + msg.angular.y = 1; + msg.angular.z = NAN; + EXPECT_FALSE(nav2_util::validateTwist(msg)); +} diff --git a/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp b/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp index c3e7835c6b8..a059e74076c 100644 --- a/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp +++ b/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp @@ -25,6 +25,7 @@ #include "nav2_util/lifecycle_node.hpp" #include "nav2_util/node_utils.hpp" #include "nav2_util/odometry_utils.hpp" +#include "nav2_util/robot_utils.hpp" namespace nav2_velocity_smoother { diff --git a/nav2_velocity_smoother/package.xml b/nav2_velocity_smoother/package.xml index 986ba2be89a..d0009942e7b 100644 --- a/nav2_velocity_smoother/package.xml +++ b/nav2_velocity_smoother/package.xml @@ -2,7 +2,7 @@ nav2_velocity_smoother - 1.1.8 + 1.1.9 Nav2's Output velocity smoother Steve Macenski Apache-2.0 diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index aa5fbec7830..bc101fd7328 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -74,11 +74,18 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State &) node->get_parameter("max_accel", max_accels_); node->get_parameter("max_decel", max_decels_); - for (unsigned int i = 0; i != max_decels_.size(); i++) { + for (unsigned int i = 0; i != 3; i++) { if (max_decels_[i] > 0.0) { - RCLCPP_WARN( - get_logger(), - "Positive values set of deceleration! These should be negative to slow down!"); + throw std::runtime_error( + "Positive values set of deceleration! These should be negative to slow down!"); + } + if (max_accels_[i] < 0.0) { + throw std::runtime_error( + "Negative values set of acceleration! These should be positive to speed up!"); + } + if (min_velocities_[i] > max_velocities_[i]) { + throw std::runtime_error( + "Min velocities are higher than max velocities!"); } } @@ -174,6 +181,12 @@ VelocitySmoother::on_shutdown(const rclcpp_lifecycle::State &) void VelocitySmoother::inputCommandCallback(const geometry_msgs::msg::Twist::SharedPtr msg) { + // If message contains NaN or Inf, ignore + if (!nav2_util::validateTwist(*msg)) { + RCLCPP_ERROR(get_logger(), "Velocity message contains NaNs or Infs! Ignoring as invalid!"); + return; + } + command_ = msg; last_command_time_ = now(); } @@ -352,8 +365,24 @@ VelocitySmoother::dynamicParametersCallback(std::vector param } else if (name == "min_velocity") { min_velocities_ = parameter.as_double_array(); } else if (name == "max_accel") { + for (unsigned int i = 0; i != 3; i++) { + if (parameter.as_double_array()[i] < 0.0) { + RCLCPP_WARN( + get_logger(), + "Negative values set of acceleration! These should be positive to speed up!"); + result.successful = false; + } + } max_accels_ = parameter.as_double_array(); } else if (name == "max_decel") { + for (unsigned int i = 0; i != 3; i++) { + if (parameter.as_double_array()[i] > 0.0) { + RCLCPP_WARN( + get_logger(), + "Positive values set of deceleration! These should be negative to slow down!"); + result.successful = false; + } + } max_decels_ = parameter.as_double_array(); } else if (name == "deadband_velocity") { deadband_velocities_ = parameter.as_double_array(); diff --git a/nav2_velocity_smoother/test/test_velocity_smoother.cpp b/nav2_velocity_smoother/test/test_velocity_smoother.cpp index a1b2f5454af..c58e20b22c4 100644 --- a/nav2_velocity_smoother/test/test_velocity_smoother.cpp +++ b/nav2_velocity_smoother/test/test_velocity_smoother.cpp @@ -593,6 +593,28 @@ TEST(VelocitySmootherTest, testInvalidParams) EXPECT_THROW(smoother->configure(state), std::runtime_error); } +TEST(VelocitySmootherTest, testInvalidParamsAccelDecel) +{ + auto smoother = + std::make_shared(); + + std::vector bad_test_accel{-10.0, -10.0, -10.0}; + std::vector bad_test_decel{10.0, 10.0, 10.0}; + std::vector bad_test_min_vel{10.0, 10.0, 10.0}; + std::vector bad_test_max_vel{-10.0, -10.0, -10.0}; + + smoother->declare_parameter("max_velocity", rclcpp::ParameterValue(bad_test_max_vel)); + smoother->declare_parameter("min_velocity", rclcpp::ParameterValue(bad_test_min_vel)); + rclcpp_lifecycle::State state; + EXPECT_THROW(smoother->configure(state), std::runtime_error); + + smoother->set_parameter(rclcpp::Parameter("max_accel", rclcpp::ParameterValue(bad_test_accel))); + EXPECT_THROW(smoother->configure(state), std::runtime_error); + + smoother->set_parameter(rclcpp::Parameter("max_decel", rclcpp::ParameterValue(bad_test_decel))); + EXPECT_THROW(smoother->configure(state), std::runtime_error); +} + TEST(VelocitySmootherTest, testDynamicParameter) { auto smoother = @@ -613,6 +635,8 @@ TEST(VelocitySmootherTest, testDynamicParameter) std::vector min_accel{0.0, 0.0, 0.0}; std::vector deadband{0.0, 0.0, 0.0}; std::vector bad_test{0.0, 0.0}; + std::vector bad_test_accel{-10.0, -10.0, -10.0}; + std::vector bad_test_decel{10.0, 10.0, 10.0}; auto results = rec_param->set_parameters_atomically( {rclcpp::Parameter("smoothing_frequency", 100.0), @@ -662,6 +686,16 @@ TEST(VelocitySmootherTest, testDynamicParameter) rclcpp::spin_until_future_complete(smoother->get_node_base_interface(), results); EXPECT_FALSE(results.get().successful); + // Test invalid accel / decel + results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("max_accel", bad_test_accel)}); + rclcpp::spin_until_future_complete(smoother->get_node_base_interface(), results); + EXPECT_FALSE(results.get().successful); + results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("max_decel", bad_test_decel)}); + rclcpp::spin_until_future_complete(smoother->get_node_base_interface(), results); + EXPECT_FALSE(results.get().successful); + // test full state after major changes smoother->deactivate(state); smoother->cleanup(state); diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index f92a2166c19..d19fe4815b3 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 1.1.8 + 1.1.9 voxel_grid provides an implementation of an efficient 3D voxel grid. The occupancy grid can support 3 different representations for the state of a cell: marked, free, or unknown. Due to the underlying implementation relying on bitwise and and or integer operations, the voxel grid only supports 16 different levels per voxel column. However, this limitation yields raytracing and cell marking performance in the grid comparable to standard 2D structures making it quite fast compared to most 3D structures. diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index adc360bb394..c1a88efec7c 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 1.1.8 + 1.1.9 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/navigation2/package.xml b/navigation2/package.xml index c74a2ae1a5f..43315f5da6d 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 1.1.8 + 1.1.9 ROS2 Navigation Stack From 0ca14fe0db48d98ecbb71365073bb2ae811df240 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 31 Aug 2023 12:03:57 -0700 Subject: [PATCH 10/39] Fixing 3768: planner server lifecycle transition down (#3786) --- nav2_controller/src/controller_server.cpp | 23 +++++++- nav2_costmap_2d/src/costmap_2d_ros.cpp | 38 ++++++++----- .../lifecycle_manager.hpp | 15 +++++ .../src/lifecycle_manager.cpp | 31 ++++++++++ nav2_planner/src/planner_server.cpp | 32 ++++++++++- .../include/nav2_util/lifecycle_node.hpp | 20 +++++++ nav2_util/src/lifecycle_node.cpp | 56 +++++++++++++++++-- nav2_util/test/test_lifecycle_node.cpp | 36 ++++++++++++ 8 files changed, 226 insertions(+), 25 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 3dc7ef494e2..baa9775348c 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -19,6 +19,7 @@ #include #include +#include "lifecycle_msgs/msg/state.hpp" #include "nav2_core/exceptions.hpp" #include "nav_2d_utils/conversions.hpp" #include "nav_2d_utils/tf_help.hpp" @@ -245,7 +246,19 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) for (it = controllers_.begin(); it != controllers_.end(); ++it) { it->second->deactivate(); } - costmap_ros_->deactivate(); + + /* + * The costmap is also a lifecycle node, so it may have already fired on_deactivate + * via rcl preshutdown cb. Despite the rclcpp docs saying on_shutdown callbacks fire + * in the order added, the preshutdown callbacks clearly don't per se, due to using an + * unordered_set iteration. Once this issue is resolved, we can maybe make a stronger + * ordering assumption: https://github.com/ros2/rclcpp/issues/2096 + */ + if (costmap_ros_->get_current_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + costmap_ros_->deactivate(); + } publishZeroVelocity(); vel_publisher_->on_deactivate(); @@ -270,11 +283,17 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) controllers_.clear(); goal_checkers_.clear(); - costmap_ros_->cleanup(); + + if (costmap_ros_->get_current_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + costmap_ros_->cleanup(); + } // Release any allocated resources action_server_.reset(); odom_sub_.reset(); + costmap_thread_.reset(); vel_publisher_.reset(); speed_limit_sub_.reset(); diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 1fd02a084db..5fbd1beea8e 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -272,14 +272,18 @@ Costmap2DROS::on_deactivate(const rclcpp_lifecycle::State & /*state*/) RCLCPP_INFO(get_logger(), "Deactivating"); dyn_params_handler.reset(); - costmap_publisher_->on_deactivate(); - footprint_pub_->on_deactivate(); stop(); // Map thread stuff map_update_thread_shutdown_ = true; - map_update_thread_->join(); + + if (map_update_thread_->joinable()) { + map_update_thread_->join(); + } + + costmap_publisher_->on_deactivate(); + footprint_pub_->on_deactivate(); return nav2_util::CallbackReturn::SUCCESS; } @@ -529,18 +533,22 @@ void Costmap2DROS::stop() { stop_updates_ = true; - std::vector> * plugins = layered_costmap_->getPlugins(); - std::vector> * filters = layered_costmap_->getFilters(); - // unsubscribe from topics - for (std::vector>::iterator plugin = plugins->begin(); - plugin != plugins->end(); ++plugin) - { - (*plugin)->deactivate(); - } - for (std::vector>::iterator filter = filters->begin(); - filter != filters->end(); ++filter) - { - (*filter)->deactivate(); + // layered_costmap_ is set only if on_configure has been called + if (layered_costmap_) { + std::vector> * plugins = layered_costmap_->getPlugins(); + std::vector> * filters = layered_costmap_->getFilters(); + + // unsubscribe from topics + for (std::vector>::iterator plugin = plugins->begin(); + plugin != plugins->end(); ++plugin) + { + (*plugin)->deactivate(); + } + for (std::vector>::iterator filter = filters->begin(); + filter != filters->end(); ++filter) + { + (*filter)->deactivate(); + } } initialized_ = false; stopped_ = true; diff --git a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp index edd3454ffe6..51c771323eb 100644 --- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp +++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp @@ -114,6 +114,13 @@ class LifecycleManager : public rclcpp::Node */ bool resume(); + /** + * @brief Perform preshutdown activities before our Context is shutdown. + * Note that this is related to our Context's shutdown sequence, not the + * lifecycle node state machine or shutdown(). + */ + void onRclPreshutdown(); + // Support function for creating service clients /** * @brief Support function for creating service clients @@ -186,6 +193,14 @@ class LifecycleManager : public rclcpp::Node */ void CreateActiveDiagnostic(diagnostic_updater::DiagnosticStatusWrapper & stat); + /** + * Register our preshutdown callback for this Node's rcl Context. + * The callback fires before this Node's Context is shutdown. + * Note this is not directly related to the lifecycle state machine or the + * shutdown() instance function. + */ + void registerRclPreshutdownCallback(); + // Timer thread to look at bond connections rclcpp::TimerBase::SharedPtr init_timer_; rclcpp::TimerBase::SharedPtr bond_timer_; diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp index 68c58ea22ea..52cbeff3109 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager.cpp @@ -45,6 +45,8 @@ LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options) declare_parameter("bond_respawn_max_duration", 10.0); declare_parameter("attempt_respawn_reconnection", true); + registerRclPreshutdownCallback(); + node_names_ = get_parameter("node_names").as_string_array(); get_parameter("autostart", autostart_); double bond_timeout_s; @@ -378,6 +380,35 @@ LifecycleManager::destroyBondTimer() } } +void +LifecycleManager::onRclPreshutdown() +{ + RCLCPP_INFO( + get_logger(), "Running Nav2 LifecycleManager rcl preshutdown (%s)", + this->get_name()); + + destroyBondTimer(); + + /* + * Dropping the bond map is what we really need here, but we drop the others + * to prevent the bond map being used. Likewise, squash the service thread. + */ + service_thread_.reset(); + node_names_.clear(); + node_map_.clear(); + bond_map_.clear(); +} + +void +LifecycleManager::registerRclPreshutdownCallback() +{ + rclcpp::Context::SharedPtr context = get_node_base_interface()->get_context(); + + context->add_pre_shutdown_callback( + std::bind(&LifecycleManager::onRclPreshutdown, this) + ); +} + void LifecycleManager::checkBondConnections() { diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 67d3506d0ea..ba817d6617a 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -25,6 +25,7 @@ #include #include "builtin_interfaces/msg/duration.hpp" +#include "lifecycle_msgs/msg/state.hpp" #include "nav2_util/costmap.hpp" #include "nav2_util/node_utils.hpp" #include "nav2_util/geometry_utils.hpp" @@ -66,6 +67,10 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) PlannerServer::~PlannerServer() { + /* + * Backstop ensuring this state is destroyed, even if deactivate/cleanup are + * never called. + */ planners_.clear(); costmap_thread_.reset(); } @@ -194,7 +199,19 @@ PlannerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) action_server_pose_->deactivate(); action_server_poses_->deactivate(); plan_publisher_->on_deactivate(); - costmap_ros_->deactivate(); + + /* + * The costmap is also a lifecycle node, so it may have already fired on_deactivate + * via rcl preshutdown cb. Despite the rclcpp docs saying on_shutdown callbacks fire + * in the order added, the preshutdown callbacks clearly don't per se, due to using an + * unordered_set iteration. Once this issue is resolved, we can maybe make a stronger + * ordering assumption: https://github.com/ros2/rclcpp/issues/2096 + */ + if (costmap_ros_->get_current_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + costmap_ros_->deactivate(); + } PlannerMap::iterator it; for (it = planners_.begin(); it != planners_.end(); ++it) { @@ -218,13 +235,24 @@ PlannerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) action_server_poses_.reset(); plan_publisher_.reset(); tf_.reset(); - costmap_ros_->cleanup(); + + /* + * Double check whether something else transitioned it to INACTIVE + * already, e.g. the rcl preshutdown callback. + */ + if (costmap_ros_->get_current_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + costmap_ros_->cleanup(); + } PlannerMap::iterator it; for (it = planners_.begin(); it != planners_.end(); ++it) { it->second->cleanup(); } + planners_.clear(); + costmap_thread_.reset(); costmap_ = nullptr; return nav2_util::CallbackReturn::SUCCESS; } diff --git a/nav2_util/include/nav2_util/lifecycle_node.hpp b/nav2_util/include/nav2_util/lifecycle_node.hpp index d4ba1688499..6da348c5f1a 100644 --- a/nav2_util/include/nav2_util/lifecycle_node.hpp +++ b/nav2_util/include/nav2_util/lifecycle_node.hpp @@ -167,6 +167,13 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode return nav2_util::CallbackReturn::SUCCESS; } + /** + * @brief Perform preshutdown activities before our Context is shutdown. + * Note that this is related to our Context's shutdown sequence, not the + * lifecycle node state machine. + */ + virtual void on_rcl_preshutdown(); + /** * @brief Create bond connection to lifecycle manager */ @@ -183,6 +190,19 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode */ void printLifecycleNodeNotification(); + /** + * Register our preshutdown callback for this Node's rcl Context. + * The callback fires before this Node's Context is shutdown. + * Note this is not directly related to the lifecycle state machine. + */ + void register_rcl_preshutdown_callback(); + std::unique_ptr rcl_preshutdown_cb_handle_{nullptr}; + + /** + * Run some common cleanup steps shared between rcl preshutdown and destruction. + */ + void runCleanups(); + // Connection to tell that server is still up std::unique_ptr bond_{nullptr}; }; diff --git a/nav2_util/src/lifecycle_node.cpp b/nav2_util/src/lifecycle_node.cpp index 9868a38bbbe..e09ae54b5d6 100644 --- a/nav2_util/src/lifecycle_node.cpp +++ b/nav2_util/src/lifecycle_node.cpp @@ -36,17 +36,20 @@ LifecycleNode::LifecycleNode( bond::msg::Constants::DISABLE_HEARTBEAT_TIMEOUT_PARAM, true)); printLifecycleNodeNotification(); + + register_rcl_preshutdown_callback(); } LifecycleNode::~LifecycleNode() { RCLCPP_INFO(get_logger(), "Destroying"); - // In case this lifecycle node wasn't properly shut down, do it here - if (get_current_state().id() == - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - on_deactivate(get_current_state()); - on_cleanup(get_current_state()); + + runCleanups(); + + if (rcl_preshutdown_cb_handle_) { + rclcpp::Context::SharedPtr context = get_node_base_interface()->get_context(); + context->remove_pre_shutdown_callback(*(rcl_preshutdown_cb_handle_.get())); + rcl_preshutdown_cb_handle_.reset(); } } @@ -64,6 +67,47 @@ void LifecycleNode::createBond() bond_->start(); } +void LifecycleNode::runCleanups() +{ + /* + * In case this lifecycle node wasn't properly shut down, do it here. + * We will give the user some ability to clean up properly here, but it's + * best effort; i.e. we aren't trying to account for all possible states. + */ + if (get_current_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + this->deactivate(); + } + + if (get_current_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + this->cleanup(); + } +} + +void LifecycleNode::on_rcl_preshutdown() +{ + RCLCPP_INFO( + get_logger(), "Running Nav2 LifecycleNode rcl preshutdown (%s)", + this->get_name()); + + runCleanups(); + + destroyBond(); +} + +void LifecycleNode::register_rcl_preshutdown_callback() +{ + rclcpp::Context::SharedPtr context = get_node_base_interface()->get_context(); + + rcl_preshutdown_cb_handle_ = std::make_unique( + context->add_pre_shutdown_callback( + std::bind(&LifecycleNode::on_rcl_preshutdown, this)) + ); +} + void LifecycleNode::destroyBond() { RCLCPP_INFO(get_logger(), "Destroying bond (%s) to lifecycle manager.", this->get_name()); diff --git a/nav2_util/test/test_lifecycle_node.cpp b/nav2_util/test/test_lifecycle_node.cpp index fd204df47f2..07ef0177d72 100644 --- a/nav2_util/test/test_lifecycle_node.cpp +++ b/nav2_util/test/test_lifecycle_node.cpp @@ -47,3 +47,39 @@ TEST(LifecycleNode, MultipleRclcppNodesExitCleanly) std::this_thread::sleep_for(std::chrono::seconds(1)); SUCCEED(); } + +TEST(LifecycleNode, OnPreshutdownCbFires) +{ + // Ensure the on_rcl_preshutdown_cb fires + + class MyNodeType : public nav2_util::LifecycleNode + { +public: + MyNodeType( + const std::string & node_name) + : nav2_util::LifecycleNode(node_name) {} + + bool fired = false; + +protected: + void on_rcl_preshutdown() override + { + fired = true; + + nav2_util::LifecycleNode::on_rcl_preshutdown(); + } + }; + + auto node = std::make_shared("test_node"); + + ASSERT_EQ(node->fired, false); + + rclcpp::shutdown(); + + ASSERT_EQ(node->fired, true); + + // Fire dtor to ensure nothing insane happens, e.g. exceptions. + node.reset(); + + SUCCEED(); +} From a9634959959edad741eece74bddb765aaccc79b5 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 12 Sep 2023 09:38:53 -0700 Subject: [PATCH 11/39] Use ParameterFile (allow_substs) (#3706) (#3806) Signed-off-by: ymd-stella Co-authored-by: ymd-stella <7959916+ymd-stella@users.noreply.github.com> --- nav2_bringup/launch/bringup_launch.py | 13 ++++++++----- nav2_bringup/launch/localization_launch.py | 14 ++++++++------ nav2_bringup/launch/navigation_launch.py | 8 +++++--- nav2_bringup/launch/slam_launch.py | 13 ++++++++----- .../launch/collision_monitor_node.launch.py | 13 ++++++++----- 5 files changed, 37 insertions(+), 24 deletions(-) diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index 4377f38e20c..cec4f520855 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -24,6 +24,7 @@ from launch.substitutions import LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.actions import PushRosNamespace +from launch_ros.descriptions import ParameterFile from nav2_common.launch import RewrittenYaml @@ -58,11 +59,13 @@ def generate_launch_description(): 'use_sim_time': use_sim_time, 'yaml_filename': map_yaml_file} - configured_params = RewrittenYaml( - source_file=params_file, - root_key=namespace, - param_rewrites=param_substitutions, - convert_types=True) + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True), + allow_substs=True) stdout_linebuf_envvar = SetEnvironmentVariable( 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') diff --git a/nav2_bringup/launch/localization_launch.py b/nav2_bringup/launch/localization_launch.py index 2d631c43900..f5b3e3f1f26 100644 --- a/nav2_bringup/launch/localization_launch.py +++ b/nav2_bringup/launch/localization_launch.py @@ -22,7 +22,7 @@ from launch.substitutions import LaunchConfiguration, PythonExpression from launch_ros.actions import LoadComposableNodes from launch_ros.actions import Node -from launch_ros.descriptions import ComposableNode +from launch_ros.descriptions import ComposableNode, ParameterFile from nav2_common.launch import RewrittenYaml @@ -57,11 +57,13 @@ def generate_launch_description(): 'use_sim_time': use_sim_time, 'yaml_filename': map_yaml_file} - configured_params = RewrittenYaml( - source_file=params_file, - root_key=namespace, - param_rewrites=param_substitutions, - convert_types=True) + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True), + allow_substs=True) stdout_linebuf_envvar = SetEnvironmentVariable( 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') diff --git a/nav2_bringup/launch/navigation_launch.py b/nav2_bringup/launch/navigation_launch.py index 1f2c62fe087..adb5a929166 100644 --- a/nav2_bringup/launch/navigation_launch.py +++ b/nav2_bringup/launch/navigation_launch.py @@ -22,7 +22,7 @@ from launch.substitutions import LaunchConfiguration, PythonExpression from launch_ros.actions import LoadComposableNodes from launch_ros.actions import Node -from launch_ros.descriptions import ComposableNode +from launch_ros.descriptions import ComposableNode, ParameterFile from nav2_common.launch import RewrittenYaml @@ -62,11 +62,13 @@ def generate_launch_description(): 'use_sim_time': use_sim_time, 'autostart': autostart} - configured_params = RewrittenYaml( + configured_params = ParameterFile( + RewrittenYaml( source_file=params_file, root_key=namespace, param_rewrites=param_substitutions, - convert_types=True) + convert_types=True), + allow_substs=True) stdout_linebuf_envvar = SetEnvironmentVariable( 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') diff --git a/nav2_bringup/launch/slam_launch.py b/nav2_bringup/launch/slam_launch.py index 944b6d1fe1d..52c810ab3aa 100644 --- a/nav2_bringup/launch/slam_launch.py +++ b/nav2_bringup/launch/slam_launch.py @@ -22,6 +22,7 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node +from launch_ros.descriptions import ParameterFile from nav2_common.launch import HasNodeParams, RewrittenYaml @@ -46,11 +47,13 @@ def generate_launch_description(): param_substitutions = { 'use_sim_time': use_sim_time} - configured_params = RewrittenYaml( - source_file=params_file, - root_key=namespace, - param_rewrites=param_substitutions, - convert_types=True) + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True), + allow_substs=True) # Declare the launch arguments declare_namespace_cmd = DeclareLaunchArgument( diff --git a/nav2_collision_monitor/launch/collision_monitor_node.launch.py b/nav2_collision_monitor/launch/collision_monitor_node.launch.py index d03d723498e..e1a570b23b4 100644 --- a/nav2_collision_monitor/launch/collision_monitor_node.launch.py +++ b/nav2_collision_monitor/launch/collision_monitor_node.launch.py @@ -22,6 +22,7 @@ from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node +from launch_ros.descriptions import ParameterFile from nav2_common.launch import RewrittenYaml @@ -59,11 +60,13 @@ def generate_launch_description(): param_substitutions = { 'use_sim_time': use_sim_time} - configured_params = RewrittenYaml( - source_file=params_file, - root_key=namespace, - param_rewrites=param_substitutions, - convert_types=True) + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True), + allow_substs=True) # Nodes launching commands start_lifecycle_manager_cmd = Node( From aaee5ae36615d03076e95b3632b717e4822b2ab0 Mon Sep 17 00:00:00 2001 From: Vineet <52542471+VineetTambe@users.noreply.github.com> Date: Wed, 13 Sep 2023 14:28:46 -0400 Subject: [PATCH 12/39] Added missing destructor to MPPI critic manager (#3812) * Added missing virtual destructor * Updated CriticManger Destructor to be same as other branches --- .../include/nav2_mppi_controller/critic_manager.hpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp index d8eeb87a3a7..b2e2c178d96 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp @@ -46,7 +46,12 @@ class CriticManager * @brief Constructor for mppi::CriticManager */ CriticManager() = default; - + + /** + * @brief Virtual Destructor for mppi::CriticManager + */ + virtual ~CriticManager() = default; + /** * @brief Configure critic manager on bringup and load plugins * @param parent WeakPtr to node From 9e63d4349c2d53302bb41435beba7e323400fbf1 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Thu, 14 Sep 2023 07:34:18 -0700 Subject: [PATCH 13/39] mppi: return NO_INFORMATION when the checked point is outside the costmap (#3816) (#3818) otherwise the controller crashes at ObstaclesCritic::costAtPose because x_i and y_i isn't initialized. (cherry picked from commit 6b250a7c57536ee43a402c9820ac2a2acdb8bc13) Co-authored-by: Chuanhong Guo --- nav2_mppi_controller/src/critics/obstacles_critic.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index fa98d569739..4568e1eb14f 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -192,7 +192,10 @@ CollisionCost ObstaclesCritic::costAtPose(float x, float y, float theta) float & cost = collision_cost.cost; collision_cost.using_footprint = false; unsigned int x_i, y_i; - collision_checker_.worldToMap(x, y, x_i, y_i); + if (!collision_checker_.worldToMap(x, y, x_i, y_i)) { + cost = nav2_costmap_2d::NO_INFORMATION; + return collision_cost; + } cost = collision_checker_.pointCost(x_i, y_i); if (consider_footprint_ && (cost >= possibly_inscribed_cost_ || possibly_inscribed_cost_ < 1)) { From b3474caa7a316dda00b30e265aa71e3c0f3bb355 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 25 Sep 2023 13:23:40 -0700 Subject: [PATCH 14/39] [Humble] Sync 8 - Sept 25 (#3836) * Same orientation of coordinate frames in rviz ang gazebo (#3751) * rviz view straight in default xy orientation Signed-off-by: Christian Henkel * gazebo orientation to match rviz Signed-off-by: Christian Henkel * rotating in direction of view --------- Signed-off-by: Christian Henkel * Fix flaky costmap filters tests: (#3754) 1. Set forward_prune_distance to 1.0 to robot not getting lost 2. Correct map name for costmap filter tests * Fix missing mutex in PlannerServer::isPathValid (#3756) Signed-off-by: ymd-stella * Rewrite the scan topic costmap plugins for multi-robot(namespace) before launch navigation. (#3572) * Make it possible to launch namspaced robot which rewrites `` to namespace. - It allows to apply namespace automatically on specific target topic path in costmap plugins. Add new nav2 params file for multi-robot(rewriting ``) as an example. - nav2_multirobot_params_all.yaml Modify nav2_common.ReplaceString - add condition argument * Update nav2_bringup/launch/bringup_launch.py Co-authored-by: Steve Macenski * Add new luanch script for multi-robot bringup Rename luanch script for multi-robot simulation bringup Add new nav2_common script - Parse argument - Parse multirobot pose Update README.md * Update README.md Apply suggestions from code review Fix pep257 erors Co-authored-by: Steve Macenski --------- Co-authored-by: Steve Macenski * use ros clock for wait (#3782) * use ROS clock for wait * fix backport issue --------- Co-authored-by: Guillaume Doisy * fixing external users of the BT action node template (#3792) * fixing external users of the BT action node template * Update nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp Co-authored-by: Guillaume Doisy --------- Co-authored-by: Guillaume Doisy * Using Simple Commander API for multi robot systems (#3803) * support multirobot namespaces * add docs * adding copy all params primitive for BT navigator (to ingest into rclcpp) (#3804) * adding copy all params primitive * fix linting * lint * I swear to god, this better be the last linting issue * allowing params to be declared from yaml * Update bt_navigator.cpp * some minor optimizations (#3821) * fix broken behaviortree doc link (#3822) Signed-off-by: Anton Kesy * [MPPI] complete minor optimaization with floating point calculations (#3827) * floating point calculations * Update optimizer_unit_tests.cpp * Update critics_tests.cpp * Update critics_tests.cpp * 25% speed up of goal critic; 1% speed up from vy striding when not in use * bumping 1.1.9 to 1.1.10 for Humble release --------- Signed-off-by: Christian Henkel Signed-off-by: ymd-stella Signed-off-by: Anton Kesy Co-authored-by: Christian Henkel <6976069+ct2034@users.noreply.github.com> Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Co-authored-by: ymd-stella <7959916+ymd-stella@users.noreply.github.com> Co-authored-by: Hyunseok Co-authored-by: Guillaume Doisy Co-authored-by: Guillaume Doisy Co-authored-by: Anton Kesy --- nav2_amcl/package.xml | 2 +- nav2_behavior_tree/README.md | 2 +- .../bt_action_server_impl.hpp | 12 + nav2_behavior_tree/package.xml | 2 +- .../include/nav2_behaviors/plugins/wait.hpp | 2 +- nav2_behaviors/package.xml | 2 +- nav2_behaviors/plugins/wait.cpp | 12 +- nav2_bringup/README.md | 33 +- nav2_bringup/launch/bringup_launch.py | 11 +- .../cloned_multi_tb3_simulation_launch.py | 174 +++++++++ ... => unique_multi_tb3_simulation_launch.py} | 0 nav2_bringup/package.xml | 2 +- .../params/nav2_multirobot_params_all.yaml | 345 ++++++++++++++++++ nav2_bringup/rviz/nav2_default_view.rviz | 8 +- nav2_bringup/rviz/nav2_namespaced_view.rviz | 8 +- nav2_bringup/worlds/world_only.model | 2 +- .../nav2_bt_navigator/bt_navigator.hpp | 2 +- nav2_bt_navigator/package.xml | 2 +- nav2_bt_navigator/src/bt_navigator.cpp | 20 +- nav2_collision_monitor/package.xml | 2 +- nav2_common/nav2_common/launch/__init__.py | 1 + .../launch/parse_multirobot_pose.py | 82 +++++ .../nav2_common/launch/replace_string.py | 36 +- nav2_common/package.xml | 2 +- nav2_constrained_smoother/package.xml | 2 +- nav2_controller/package.xml | 2 +- nav2_core/package.xml | 2 +- nav2_costmap_2d/package.xml | 2 +- nav2_dwb_controller/costmap_queue/package.xml | 2 +- nav2_dwb_controller/dwb_core/package.xml | 2 +- nav2_dwb_controller/dwb_critics/package.xml | 2 +- nav2_dwb_controller/dwb_msgs/package.xml | 2 +- nav2_dwb_controller/dwb_plugins/package.xml | 2 +- .../nav2_dwb_controller/package.xml | 2 +- nav2_dwb_controller/nav_2d_msgs/package.xml | 2 +- nav2_dwb_controller/nav_2d_utils/package.xml | 2 +- nav2_lifecycle_manager/package.xml | 2 +- nav2_map_server/package.xml | 2 +- nav2_mppi_controller/CMakeLists.txt | 4 +- .../critics/obstacles_critic.hpp | 10 +- .../critics/path_angle_critic.hpp | 2 +- .../models/constraints.hpp | 14 +- .../tools/path_handler.hpp | 4 +- .../nav2_mppi_controller/tools/utils.hpp | 30 +- nav2_mppi_controller/package.xml | 2 +- .../src/critics/constraint_critic.cpp | 4 +- .../src/critics/goal_critic.cpp | 2 +- .../src/critics/obstacles_critic.cpp | 22 +- .../src/critics/path_align_critic.cpp | 8 +- nav2_mppi_controller/src/noise_generator.cpp | 6 +- nav2_mppi_controller/src/optimizer.cpp | 18 +- nav2_mppi_controller/src/path_handler.cpp | 8 +- nav2_mppi_controller/test/critics_tests.cpp | 8 +- .../test/optimizer_unit_tests.cpp | 12 +- nav2_msgs/package.xml | 2 +- nav2_navfn_planner/package.xml | 2 +- nav2_planner/package.xml | 2 +- nav2_planner/src/planner_server.cpp | 1 + .../package.xml | 2 +- nav2_rotation_shim_controller/package.xml | 2 +- nav2_rviz_plugins/package.xml | 2 +- nav2_simple_commander/README.md | 2 + .../nav2_simple_commander/robot_navigator.py | 14 +- nav2_simple_commander/package.xml | 2 +- nav2_smac_planner/package.xml | 2 +- nav2_smoother/package.xml | 2 +- nav2_system_tests/package.xml | 2 +- .../src/costmap_filters/keepout_params.yaml | 1 + .../costmap_filters/test_keepout_launch.py | 2 +- .../src/costmap_filters/test_speed_launch.py | 2 +- nav2_theta_star_planner/package.xml | 2 +- nav2_util/CMakeLists.txt | 1 + nav2_util/include/nav2_util/node_utils.hpp | 21 ++ nav2_util/package.xml | 3 +- nav2_util/test/test_node_utils.cpp | 32 ++ nav2_velocity_smoother/package.xml | 2 +- nav2_voxel_grid/package.xml | 2 +- nav2_waypoint_follower/package.xml | 2 +- navigation2/package.xml | 2 +- 79 files changed, 893 insertions(+), 162 deletions(-) create mode 100644 nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py rename nav2_bringup/launch/{multi_tb3_simulation_launch.py => unique_multi_tb3_simulation_launch.py} (100%) create mode 100644 nav2_bringup/params/nav2_multirobot_params_all.yaml create mode 100644 nav2_common/nav2_common/launch/parse_multirobot_pose.py diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index 074f39ef540..afea3eb3b7a 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 1.1.9 + 1.1.10

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_behavior_tree/README.md b/nav2_behavior_tree/README.md index db761cb31dd..b2c30c432f0 100644 --- a/nav2_behavior_tree/README.md +++ b/nav2_behavior_tree/README.md @@ -63,4 +63,4 @@ The BehaviorTree engine has a run method that accepts an XML description of a BT See the code in the [BT Navigator](../nav2_bt_navigator/src/bt_navigator.cpp) for an example usage of the BehaviorTreeEngine. -For more information about the behavior tree nodes that are available in the default BehaviorTreeCPP library, see documentation here: https://www.behaviortree.dev/bt_basics/ +For more information about the behavior tree nodes that are available in the default BehaviorTreeCPP library, see documentation here: https://www.behaviortree.dev/docs/learn-the-basics/bt_basics/ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 592d1257a27..022f32de590 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -25,6 +25,7 @@ #include "nav2_msgs/action/navigate_to_pose.hpp" #include "nav2_behavior_tree/bt_action_server.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" +#include "nav2_util/node_utils.hpp" namespace nav2_behavior_tree { @@ -87,6 +88,17 @@ bool BtActionServer::on_configure() // Support for handling the topic-based goal pose from rviz client_node_ = std::make_shared("_", options); + // Declare parameters for common client node applications to share with BT nodes + // Declare if not declared in case being used an external application, then copying + // all of the main node's parameters to the client for BT nodes to obtain + nav2_util::declare_parameter_if_not_declared( + node, "global_frame", rclcpp::ParameterValue(std::string("map"))); + nav2_util::declare_parameter_if_not_declared( + node, "robot_base_frame", rclcpp::ParameterValue(std::string("base_link"))); + nav2_util::declare_parameter_if_not_declared( + node, "transform_tolerance", rclcpp::ParameterValue(0.1)); + nav2_util::copy_all_parameters(node, client_node_); + action_server_ = std::make_shared( node->get_node_base_interface(), node->get_node_clock_interface(), diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index cb9333efbbd..d5117b45d95 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.1.9 + 1.1.10 TODO Michael Jeronimo Carlos Orduno diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/wait.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/wait.hpp index 38e85fd14aa..16d6169e253 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/wait.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/wait.hpp @@ -53,7 +53,7 @@ class Wait : public TimedBehavior Status onCycleUpdate() override; protected: - std::chrono::time_point wait_end_; + rclcpp::Time wait_end_; WaitAction::Feedback::SharedPtr feedback_; }; diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml index 6f6494b7557..107c044bb71 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -2,7 +2,7 @@ nav2_behaviors - 1.1.9 + 1.1.10 TODO Carlos Orduno Steve Macenski diff --git a/nav2_behaviors/plugins/wait.cpp b/nav2_behaviors/plugins/wait.cpp index 5100ab01b0a..11a643eee00 100644 --- a/nav2_behaviors/plugins/wait.cpp +++ b/nav2_behaviors/plugins/wait.cpp @@ -30,21 +30,19 @@ Wait::~Wait() = default; Status Wait::onRun(const std::shared_ptr command) { - wait_end_ = std::chrono::steady_clock::now() + - rclcpp::Duration(command->time).to_chrono(); + wait_end_ = node_.lock()->now() + rclcpp::Duration(command->time); return Status::SUCCEEDED; } Status Wait::onCycleUpdate() { - auto current_point = std::chrono::steady_clock::now(); - auto time_left = - std::chrono::duration_cast(wait_end_ - current_point).count(); + auto current_point = node_.lock()->now(); + auto time_left = wait_end_ - current_point; - feedback_->time_left = rclcpp::Duration(rclcpp::Duration::from_nanoseconds(time_left)); + feedback_->time_left = time_left; action_server_->publish_feedback(feedback_); - if (time_left > 0) { + if (time_left.nanoseconds() > 0) { return Status::RUNNING; } else { return Status::SUCCEEDED; diff --git a/nav2_bringup/README.md b/nav2_bringup/README.md index 574a817ecb3..16088452cb8 100644 --- a/nav2_bringup/README.md +++ b/nav2_bringup/README.md @@ -1,8 +1,8 @@ # nav2_bringup -The `nav2_bringup` package is an example bringup system for Nav2 applications. +The `nav2_bringup` package is an example bringup system for Nav2 applications. -This is a very flexible example for nav2 bringup that can be modified for different maps/robots/hardware/worlds/etc. It is our expectation for an application specific robot system that you're mirroring `nav2_bringup` package and modifying it for your specific maps/robots/bringup needs. This is an applied and working demonstration for the default system bringup with many options that can be easily modified. +This is a very flexible example for nav2 bringup that can be modified for different maps/robots/hardware/worlds/etc. It is our expectation for an application specific robot system that you're mirroring `nav2_bringup` package and modifying it for your specific maps/robots/bringup needs. This is an applied and working demonstration for the default system bringup with many options that can be easily modified. Usual robot stacks will have a `_nav` package with config/bringup files and this is that for the general case to base a specific robot system off of. @@ -10,8 +10,35 @@ Dynamically composed bringup (based on [ROS2 Composition](https://docs.ros.org/ * Some discussions about performance improvement of composed bringup could be found here: https://discourse.ros.org/t/nav2-composition/22175. -To use, please see the Nav2 [Getting Started Page](https://navigation.ros.org/getting_started/index.html) on our documentation website. Additional [tutorials will help you](https://navigation.ros.org/tutorials/index.html) go from an initial setup in simulation to testing on a hardware robot, using SLAM, and more. +To use, please see the Nav2 [Getting Started Page](https://navigation.ros.org/getting_started/index.html) on our documentation website. Additional [tutorials will help you](https://navigation.ros.org/tutorials/index.html) go from an initial setup in simulation to testing on a hardware robot, using SLAM, and more. Note: * gazebo should be started with both libgazebo_ros_init.so and libgazebo_ros_factory.so to work correctly. * spawn_entity node could not remap /tf and /tf_static to tf and tf_static in the launch file yet, used only for multi-robot situations. Instead it should be done as remapping argument /tf:=tf /tf_static:=tf_static under ros2 tag in each plugin which publishs transforms in the SDF file. It is essential to differentiate the tf's of the different robot. + +## Launch + +### Multi-robot Simulation + +This is how to launch multi-robot simulation with simple command line. Please see the Nav2 documentation for further augments. + +#### Cloned + +This allows to bring up multiple robots, cloning a single robot N times at different positions in the map. The parameter are loaded from `nav2_multirobot_params_all.yaml` file by default. +The multiple robots that consists of name and initial pose in YAML format will be set on the command-line. The format for each robot is `robot_name={x: 0.0, y: 0.0, yaw: 0.0, roll: 0.0, pitch: 0.0, yaw: 0.0}`. + +Please refer to below examples. + +```shell +ros2 launch nav2_bringup cloned_multi_tb3_simulation_launch.py robots:="robot1={x: 1.0, y: 1.0, yaw: 1.5707}; robot2={x: 1.0, y: 1.0, yaw: 1.5707}" +``` + +#### Unique + +There are two robots including name and intitial pose are hard-coded in the launch script. Two separated unique robots are required params file (`nav2_multirobot_params_1.yaml`, `nav2_multirobot_params_2.yaml`) for each robot to bring up. + +If you want to bringup more than two robots, you should modify the `unique_multi_tb3_simulation_launch.py` script. + +```shell +ros2 launch nav2_bringup unique_multi_tb3_simulation_launch.py +``` diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index cec4f520855..40cbd680c61 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -25,7 +25,7 @@ from launch_ros.actions import Node from launch_ros.actions import PushRosNamespace from launch_ros.descriptions import ParameterFile -from nav2_common.launch import RewrittenYaml +from nav2_common.launch import RewrittenYaml, ReplaceString def generate_launch_description(): @@ -59,6 +59,15 @@ def generate_launch_description(): 'use_sim_time': use_sim_time, 'yaml_filename': map_yaml_file} + # Only it applys when `use_namespace` is True. + # '' keyword shall be replaced by 'namespace' launch argument + # in config file 'nav2_multirobot_params.yaml' as a default & example. + # User defined config file should contain '' keyword for the replacements. + params_file = ReplaceString( + source_file=params_file, + replacements={'': ('/', namespace)}, + condition=IfCondition(use_namespace)) + configured_params = ParameterFile( RewrittenYaml( source_file=params_file, diff --git a/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py b/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py new file mode 100644 index 00000000000..fc1499a0f27 --- /dev/null +++ b/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py @@ -0,0 +1,174 @@ +# Copyright (c) 2023 LG Electronics. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import (DeclareLaunchArgument, ExecuteProcess, GroupAction, + IncludeLaunchDescription, LogInfo) +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, TextSubstitution +from nav2_common.launch import ParseMultiRobotPose + + +def generate_launch_description(): + """ + Bring up the multi-robots with given launch arguments. + + Launch arguments consist of robot name(which is namespace) and pose for initialization. + Keep general yaml format for pose information. + ex) robots:="robot1={x: 1.0, y: 1.0, yaw: 1.5707}; robot2={x: 1.0, y: 1.0, yaw: 1.5707}" + ex) robots:="robot3={x: 1.0, y: 1.0, z: 1.0, roll: 0.0, pitch: 1.5707, yaw: 1.5707}; + robot4={x: 1.0, y: 1.0, z: 1.0, roll: 0.0, pitch: 1.5707, yaw: 1.5707}" + """ + # Get the launch directory + bringup_dir = get_package_share_directory('nav2_bringup') + launch_dir = os.path.join(bringup_dir, 'launch') + + # Simulation settings + world = LaunchConfiguration('world') + simulator = LaunchConfiguration('simulator') + + # On this example all robots are launched with the same settings + map_yaml_file = LaunchConfiguration('map') + params_file = LaunchConfiguration('params_file') + autostart = LaunchConfiguration('autostart') + rviz_config_file = LaunchConfiguration('rviz_config') + use_robot_state_pub = LaunchConfiguration('use_robot_state_pub') + use_rviz = LaunchConfiguration('use_rviz') + log_settings = LaunchConfiguration('log_settings', default='true') + + # Declare the launch arguments + declare_world_cmd = DeclareLaunchArgument( + 'world', + default_value=os.path.join(bringup_dir, 'worlds', 'world_only.model'), + description='Full path to world file to load') + + declare_simulator_cmd = DeclareLaunchArgument( + 'simulator', + default_value='gazebo', + description='The simulator to use (gazebo or gzserver)') + + declare_map_yaml_cmd = DeclareLaunchArgument( + 'map', + default_value=os.path.join(bringup_dir, 'maps', 'turtlebot3_world.yaml'), + description='Full path to map file to load') + + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'params', 'nav2_multirobot_params_all.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes') + + declare_autostart_cmd = DeclareLaunchArgument( + 'autostart', default_value='false', + description='Automatically startup the stacks') + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + 'rviz_config', + default_value=os.path.join(bringup_dir, 'rviz', 'nav2_namespaced_view.rviz'), + description='Full path to the RVIZ config file to use.') + + declare_use_robot_state_pub_cmd = DeclareLaunchArgument( + 'use_robot_state_pub', + default_value='True', + description='Whether to start the robot state publisher') + + declare_use_rviz_cmd = DeclareLaunchArgument( + 'use_rviz', + default_value='True', + description='Whether to start RVIZ') + + # Start Gazebo with plugin providing the robot spawning service + start_gazebo_cmd = ExecuteProcess( + cmd=[simulator, '--verbose', '-s', 'libgazebo_ros_init.so', + '-s', 'libgazebo_ros_factory.so', world], + output='screen') + + robots_list = ParseMultiRobotPose('robots').value() + + # Define commands for launching the navigation instances + bringup_cmd_group = [] + for robot_name in robots_list: + init_pose = robots_list[robot_name] + group = GroupAction([ + LogInfo(msg=['Launching namespace=', robot_name, ' init_pose=', str(init_pose)]), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir, 'rviz_launch.py')), + condition=IfCondition(use_rviz), + launch_arguments={'namespace': TextSubstitution(text=robot_name), + 'use_namespace': 'True', + 'rviz_config': rviz_config_file}.items()), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(bringup_dir, + 'launch', + 'tb3_simulation_launch.py')), + launch_arguments={'namespace': robot_name, + 'use_namespace': 'True', + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': params_file, + 'autostart': autostart, + 'use_rviz': 'False', + 'use_simulator': 'False', + 'headless': 'False', + 'use_robot_state_pub': use_robot_state_pub, + 'x_pose': TextSubstitution(text=str(init_pose['x'])), + 'y_pose': TextSubstitution(text=str(init_pose['y'])), + 'z_pose': TextSubstitution(text=str(init_pose['z'])), + 'roll': TextSubstitution(text=str(init_pose['roll'])), + 'pitch': TextSubstitution(text=str(init_pose['pitch'])), + 'yaw': TextSubstitution(text=str(init_pose['yaw'])), + 'robot_name':TextSubstitution(text=robot_name), }.items()) + ]) + + bringup_cmd_group.append(group) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_world_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_rviz_config_file_cmd) + ld.add_action(declare_use_robot_state_pub_cmd) + + # Add the actions to start gazebo, robots and simulations + ld.add_action(start_gazebo_cmd) + + ld.add_action(LogInfo(msg=['number_of_robots=', str(len(robots_list))])) + + ld.add_action(LogInfo(condition=IfCondition(log_settings), + msg=['map yaml: ', map_yaml_file])) + ld.add_action(LogInfo(condition=IfCondition(log_settings), + msg=['params yaml: ', params_file])) + ld.add_action(LogInfo(condition=IfCondition(log_settings), + msg=['rviz config file: ', rviz_config_file])) + ld.add_action(LogInfo(condition=IfCondition(log_settings), + msg=['using robot state pub: ', use_robot_state_pub])) + ld.add_action(LogInfo(condition=IfCondition(log_settings), + msg=['autostart: ', autostart])) + + for cmd in bringup_cmd_group: + ld.add_action(cmd) + + return ld diff --git a/nav2_bringup/launch/multi_tb3_simulation_launch.py b/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py similarity index 100% rename from nav2_bringup/launch/multi_tb3_simulation_launch.py rename to nav2_bringup/launch/unique_multi_tb3_simulation_launch.py diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index 6b018950c4d..488f5486d32 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.1.9 + 1.1.10 Bringup scripts and configurations for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bringup/params/nav2_multirobot_params_all.yaml b/nav2_bringup/params/nav2_multirobot_params_all.yaml new file mode 100644 index 00000000000..372dcdd3f6e --- /dev/null +++ b/nav2_bringup/params/nav2_multirobot_params_all.yaml @@ -0,0 +1,345 @@ +amcl: + ros__parameters: + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::DifferentialMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + +bt_navigator: + ros__parameters: + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + navigators: ["navigate_to_pose", "navigate_through_poses"] + navigate_to_pose: + plugin: "nav2_bt_navigator/NavigateToPoseNavigator" + navigate_through_poses: + plugin: "nav2_bt_navigator/NavigateThroughPosesNavigator" + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_are_error_codes_active_condition_bt_node + - nav2_would_a_controller_recovery_help_condition_bt_node + - nav2_would_a_planner_recovery_help_condition_bt_node + - nav2_would_a_smoother_recovery_help_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + error_code_names: + - compute_path_error_code + - follow_path_error_code + +controller_server: + ros__parameters: + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugins: ["progress_checker"] + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + # '' keyword shall be replaced with 'namespace' where user defined. + # It doesn't need to start with '/' + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + # '' keyword shall be replaced with 'namespace' where user defined. + # It doesn't need to start with '/' + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +# The yaml_filename does not need to be specified since it going to be set by defaults in launch. +# If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py +# file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used. +# map_server: +# ros__parameters: +# yaml_filename: "" + +map_saver: + ros__parameters: + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + local_costmap_topic: local_costmap/costmap_raw + global_costmap_topic: global_costmap/costmap_raw + local_footprint_topic: local_costmap/published_footprint + global_footprint_topic: global_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + local_frame: odom + global_frame: map + robot_base_frame: base_link + transform_tolerance: 0.1 + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/nav2_bringup/rviz/nav2_default_view.rviz b/nav2_bringup/rviz/nav2_default_view.rviz index d2a4b5e136d..d8bade77d81 100644 --- a/nav2_bringup/rviz/nav2_default_view.rviz +++ b/nav2_bringup/rviz/nav2_default_view.rviz @@ -560,7 +560,7 @@ Visualization Manager: Value: true Views: Current: - Angle: -1.6150002479553223 + Angle: -1.5708 Class: rviz_default_plugins/TopDownOrtho Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 @@ -570,11 +570,11 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: 127.88431549072266 + Scale: 160 Target Frame: Value: TopDownOrtho (rviz_default_plugins) - X: -0.044467076659202576 - Y: -0.38726311922073364 + X: 0 + Y: 0 Saved: ~ Window Geometry: Displays: diff --git a/nav2_bringup/rviz/nav2_namespaced_view.rviz b/nav2_bringup/rviz/nav2_namespaced_view.rviz index 57b2d7bf741..e95196a7fba 100644 --- a/nav2_bringup/rviz/nav2_namespaced_view.rviz +++ b/nav2_bringup/rviz/nav2_namespaced_view.rviz @@ -342,7 +342,7 @@ Visualization Manager: Value: true Views: Current: - Angle: -1.5700000524520874 + Angle: -1.5708 Class: rviz_default_plugins/TopDownOrtho Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 @@ -352,11 +352,11 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: 134.638427734375 + Scale: 160 Target Frame: Value: TopDownOrtho (rviz_default_plugins) - X: -0.032615214586257935 - Y: -0.0801941454410553 + X: 0 + Y: 0 Saved: ~ Window Geometry: Displays: diff --git a/nav2_bringup/worlds/world_only.model b/nav2_bringup/worlds/world_only.model index 4c45d4e2f94..aed412a8d4f 100644 --- a/nav2_bringup/worlds/world_only.model +++ b/nav2_bringup/worlds/world_only.model @@ -16,7 +16,7 @@ - 0.319654 -0.235002 9.29441 0 1.5138 0.009599 + 0 0 10 0 1.570796 0 orbit perspective diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp index 0406c8735ef..09a82e85ddc 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp @@ -43,7 +43,7 @@ class BtNavigator : public nav2_util::LifecycleNode * @brief A constructor for nav2_bt_navigator::BtNavigator class * @param options Additional options to control creation of the node. */ - explicit BtNavigator(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + explicit BtNavigator(rclcpp::NodeOptions options = rclcpp::NodeOptions()); /** * @brief A destructor for nav2_bt_navigator::BtNavigator class */ diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index cdba8c07f13..15975252c7e 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.1.9 + 1.1.10 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 4f0f690d01e..c6a87f00138 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -28,8 +28,9 @@ namespace nav2_bt_navigator { -BtNavigator::BtNavigator(const rclcpp::NodeOptions & options) -: nav2_util::LifecycleNode("bt_navigator", "", options) +BtNavigator::BtNavigator(rclcpp::NodeOptions options) +: nav2_util::LifecycleNode("bt_navigator", "", + options.automatically_declare_parameters_from_overrides(true)) { RCLCPP_INFO(get_logger(), "Creating"); @@ -83,11 +84,16 @@ BtNavigator::BtNavigator(const rclcpp::NodeOptions & options) "nav2_is_battery_charging_condition_bt_node" }; - declare_parameter("plugin_lib_names", plugin_libs); - declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.1)); - declare_parameter("global_frame", std::string("map")); - declare_parameter("robot_base_frame", std::string("base_link")); - declare_parameter("odom_topic", std::string("odom")); + declare_parameter_if_not_declared( + this, "plugin_lib_names", rclcpp::ParameterValue(plugin_libs)); + declare_parameter_if_not_declared( + this, "transform_tolerance", rclcpp::ParameterValue(0.1)); + declare_parameter_if_not_declared( + this, "global_frame", rclcpp::ParameterValue(std::string("map"))); + declare_parameter_if_not_declared( + this, "robot_base_frame", rclcpp::ParameterValue(std::string("base_link"))); + declare_parameter_if_not_declared( + this, "odom_topic", rclcpp::ParameterValue(std::string("odom"))); } BtNavigator::~BtNavigator() diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index eb750b4e607..6ca825d1fa8 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -2,7 +2,7 @@ nav2_collision_monitor - 1.1.9 + 1.1.10 Collision Monitor Alexey Merzlyakov Steve Macenski diff --git a/nav2_common/nav2_common/launch/__init__.py b/nav2_common/nav2_common/launch/__init__.py index c25fadb220c..7eba78e1928 100644 --- a/nav2_common/nav2_common/launch/__init__.py +++ b/nav2_common/nav2_common/launch/__init__.py @@ -15,3 +15,4 @@ from .has_node_params import HasNodeParams from .rewritten_yaml import RewrittenYaml from .replace_string import ReplaceString +from .parse_multirobot_pose import ParseMultiRobotPose diff --git a/nav2_common/nav2_common/launch/parse_multirobot_pose.py b/nav2_common/nav2_common/launch/parse_multirobot_pose.py new file mode 100644 index 00000000000..9025f967fc5 --- /dev/null +++ b/nav2_common/nav2_common/launch/parse_multirobot_pose.py @@ -0,0 +1,82 @@ +# Copyright (c) 2023 LG Electronics. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import yaml +import sys +from typing import Text, Dict + + +class ParseMultiRobotPose(): + """ + Parsing argument using sys module + """ + + def __init__(self, target_argument: Text): + """ + Parse arguments for multi-robot's pose + + for example, + `ros2 launch nav2_bringup bringup_multirobot_launch.py + robots:="robot1={x: 1.0, y: 1.0, yaw: 0.0}; + robot2={x: 1.0, y: 1.0, z: 1.0, roll: 0.0, pitch: 1.5707, yaw: 1.5707}"` + + `target_argument` shall be 'robots'. + Then, this will parse a string value for `robots` argument. + + Each robot name which is corresponding to namespace and pose of it will be separted by `;`. + The pose consists of x, y and yaw with YAML format. + + :param: target argument name to parse + """ + self.__args: Text = self.__parse_argument(target_argument) + + def __parse_argument(self, target_argument: Text) -> Text: + """ + get value of target argument + """ + if len(sys.argv) > 4: + argv = sys.argv[4:] + for arg in argv: + if arg.startswith(target_argument + ":="): + return arg.replace(target_argument + ":=", "") + return "" + + def value(self) -> Dict: + """ + get value of target argument + """ + args = self.__args + parsed_args = list() if len(args) == 0 else args.split(';') + multirobots = dict() + for arg in parsed_args: + key_val = arg.strip().split('=') + if len(key_val) != 2: + continue + key = key_val[0].strip() + val = key_val[1].strip() + robot_pose = yaml.safe_load(val) + if 'x' not in robot_pose: + robot_pose['x'] = 0.0 + if 'y' not in robot_pose: + robot_pose['y'] = 0.0 + if 'z' not in robot_pose: + robot_pose['z'] = 0.0 + if 'roll' not in robot_pose: + robot_pose['roll'] = 0.0 + if 'pitch' not in robot_pose: + robot_pose['pitch'] = 0.0 + if 'yaw' not in robot_pose: + robot_pose['yaw'] = 0.0 + multirobots[key] = robot_pose + return multirobots diff --git a/nav2_common/nav2_common/launch/replace_string.py b/nav2_common/nav2_common/launch/replace_string.py index 4b5c82e757a..9d22f1732df 100644 --- a/nav2_common/nav2_common/launch/replace_string.py +++ b/nav2_common/nav2_common/launch/replace_string.py @@ -15,6 +15,7 @@ from typing import Dict from typing import List from typing import Text +from typing import Optional import tempfile import launch @@ -27,7 +28,8 @@ class ReplaceString(launch.Substitution): def __init__(self, source_file: launch.SomeSubstitutionsType, - replacements: Dict) -> None: + replacements: Dict, + condition: Optional[launch.Condition] = None) -> None: super().__init__() from launch.utilities import normalize_to_list_of_substitutions # import here to avoid loop @@ -35,28 +37,38 @@ def __init__(self, self.__replacements = {} for key in replacements: self.__replacements[key] = normalize_to_list_of_substitutions(replacements[key]) + self.__condition = condition @property def name(self) -> List[launch.Substitution]: """Getter for name.""" return self.__source_file + @property + def condition(self) -> Optional[launch.Condition]: + """Getter for condition.""" + return self.__condition + def describe(self) -> Text: """Return a description of this substitution as a string.""" return '' def perform(self, context: launch.LaunchContext) -> Text: - output_file = tempfile.NamedTemporaryFile(mode='w', delete=False) - replacements = self.resolve_replacements(context) - try: - input_file = open(launch.utilities.perform_substitutions(context, self.name), 'r') - self.replace(input_file, output_file, replacements) - except Exception as err: # noqa: B902 - print('ReplaceString substitution error: ', err) - finally: - input_file.close() - output_file.close() - return output_file.name + yaml_filename = launch.utilities.perform_substitutions(context, self.name) + if self.__condition is None or self.__condition.evaluate(context): + output_file = tempfile.NamedTemporaryFile(mode='w', delete=False) + replacements = self.resolve_replacements(context) + try: + input_file = open(yaml_filename, 'r') + self.replace(input_file, output_file, replacements) + except Exception as err: # noqa: B902 + print('ReplaceString substitution error: ', err) + finally: + input_file.close() + output_file.close() + return output_file.name + else: + return yaml_filename def resolve_replacements(self, context): resolved_replacements = {} diff --git a/nav2_common/package.xml b/nav2_common/package.xml index d215166a495..6c0d093102f 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.1.9 + 1.1.10 Common support functionality used throughout the navigation 2 stack Carl Delsey Apache-2.0 diff --git a/nav2_constrained_smoother/package.xml b/nav2_constrained_smoother/package.xml index df524559579..7c0f7fb4154 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -2,7 +2,7 @@ nav2_constrained_smoother - 1.1.9 + 1.1.10 Ceres constrained smoother Matej Vargovcik Steve Macenski diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index d3ae1711be1..9066014856c 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.1.9 + 1.1.10 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_core/package.xml b/nav2_core/package.xml index c5a5fcae00e..219e74f0a89 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.1.9 + 1.1.10 A set of headers for plugins core to the Nav2 stack Steve Macenski Carl Delsey diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index daf0c64fab0..72e03fa90f8 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 1.1.9 + 1.1.10 This package provides an implementation of a 2D costmap that takes in sensor data from the world, builds a 2D or 3D occupancy grid of the data (depending diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index 881883872c3..d32c36b01e2 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 1.1.9 + 1.1.10 The costmap_queue package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml index 93228c13f51..f135f172289 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 1.1.9 + 1.1.10 TODO Carl Delsey BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index 025990ae337..1fd6afba514 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 1.1.9 + 1.1.10 The dwb_critics package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml index 4ebe4e1404e..0934647b561 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 1.1.9 + 1.1.10 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index f26bfef2510..1a6611135d4 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 1.1.9 + 1.1.10 Standard implementations of the GoalChecker and TrajectoryGenerators for dwb_core diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index b3e77825da6..880ae326541 100644 --- a/nav2_dwb_controller/nav2_dwb_controller/package.xml +++ b/nav2_dwb_controller/nav2_dwb_controller/package.xml @@ -2,7 +2,7 @@ nav2_dwb_controller - 1.1.9 + 1.1.10 ROS2 controller (DWB) metapackage diff --git a/nav2_dwb_controller/nav_2d_msgs/package.xml b/nav2_dwb_controller/nav_2d_msgs/package.xml index 5561c30bdfa..d41c64dd415 100644 --- a/nav2_dwb_controller/nav_2d_msgs/package.xml +++ b/nav2_dwb_controller/nav_2d_msgs/package.xml @@ -2,7 +2,7 @@ nav_2d_msgs - 1.1.9 + 1.1.10 Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D. David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml index 417e2e7d62e..4cd7a6051e2 100644 --- a/nav2_dwb_controller/nav_2d_utils/package.xml +++ b/nav2_dwb_controller/nav_2d_utils/package.xml @@ -2,7 +2,7 @@ nav_2d_utils - 1.1.9 + 1.1.10 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index c1ec76b748c..e56ba055e8c 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 1.1.9 + 1.1.10 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index 94ea2640c89..08e007a505e 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 1.1.9 + 1.1.10 Refactored map server for ROS2 Navigation diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index 4abc9c7454d..5cc8a8c0e90 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -10,6 +10,7 @@ set(XTENSOR_USE_OPENMP 0) find_package(ament_cmake REQUIRED) find_package(xtensor REQUIRED) +find_package(xsimd REQUIRED) set(dependencies_pkgs rclcpp @@ -46,7 +47,8 @@ if(COMPILER_SUPPORTS_FMA) add_compile_options(-mfma) endif() -add_compile_options(-O3 -finline-limit=1000000 -ffp-contract=fast -ffast-math) +# If building one the same hardware to be deployed on, try `-march=native`! +add_compile_options(-O3 -finline-limit=10000000 -ffp-contract=fast -ffast-math -mtune=generic) add_library(mppi_controller SHARED src/controller.cpp diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp index a026b0fb2a9..fe17906bae4 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp @@ -54,7 +54,7 @@ class ObstaclesCritic : public CriticFunction * @param cost Costmap cost * @return bool if in collision */ - bool inCollision(float cost) const; + inline bool inCollision(float cost) const; /** * @brief cost at a robot pose @@ -63,14 +63,14 @@ class ObstaclesCritic : public CriticFunction * @param theta theta of pose * @return Collision information at pose */ - CollisionCost costAtPose(float x, float y, float theta); + inline CollisionCost costAtPose(float x, float y, float theta); /** * @brief Distance to obstacle from cost * @param cost Costmap cost * @return float Distance to the obstacle represented by cost */ - float distanceToObstacle(const CollisionCost & cost); + inline float distanceToObstacle(const CollisionCost & cost); /** * @brief Find the min cost of the inflation decay function for which the robot MAY be @@ -79,14 +79,14 @@ class ObstaclesCritic : public CriticFunction * @return double circumscribed cost, any higher than this and need to do full footprint collision checking * since some element of the robot could be in collision */ - double findCircumscribedCost(std::shared_ptr costmap); + float findCircumscribedCost(std::shared_ptr costmap); protected: nav2_costmap_2d::FootprintCollisionChecker collision_checker_{nullptr}; bool consider_footprint_{true}; - double collision_cost_{0}; + float collision_cost_{0}; float inflation_scale_factor_{0}, inflation_radius_{0}; float possibly_inscribed_cost_; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp index 6bf412eb258..e3ae2f67a28 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp @@ -44,7 +44,7 @@ class PathAngleCritic : public CriticFunction void score(CriticData & data) override; protected: - double max_angle_to_furthest_{0}; + float max_angle_to_furthest_{0}; float threshold_to_consider_{0}; size_t offset_from_furthest_{0}; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp index b7f9b6f3ccb..5e1885c271f 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp @@ -24,10 +24,10 @@ namespace mppi::models */ struct ControlConstraints { - double vx_max; - double vx_min; - double vy; - double wz; + float vx_max; + float vx_min; + float vy; + float wz; }; /** @@ -36,9 +36,9 @@ struct ControlConstraints */ struct SamplingStd { - double vx; - double vy; - double wz; + float vx; + float vy; + float wz; }; } // namespace mppi::models diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp index 9f0803a23ae..2c9f3aef8bb 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp @@ -150,8 +150,8 @@ class PathHandler double max_robot_pose_search_dist_{0}; double prune_distance_{0}; double transform_tolerance_{0}; - double inversion_xy_tolerance_{0.2}; - double inversion_yaw_tolerance{0.4}; + float inversion_xy_tolerance_{0.2}; + float inversion_yaw_tolerance{0.4}; bool enforce_path_inversion_{false}; unsigned int inversion_locale_{0u}; }; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index 93c1aae1061..b21b1cf8b3e 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -309,7 +309,7 @@ inline size_t findPathFurthestReachedPoint(const CriticData & data) const auto dists = dx * dx + dy * dy; size_t max_id_by_trajectories = 0; - double min_distance_by_path = std::numeric_limits::max(); + float min_distance_by_path = std::numeric_limits::max(); for (size_t i = 0; i < dists.shape(0); i++) { size_t min_id_by_path = 0; @@ -337,7 +337,7 @@ inline size_t findPathTrajectoryInitialPoint(const CriticData & data) const auto dy = data.path.y - data.trajectories.y(0, 0); const auto dists = dx * dx + dy * dy; - double min_distance_by_path = std::numeric_limits::max(); + float min_distance_by_path = std::numeric_limits::max(); size_t min_id = 0; for (size_t j = 0; j < dists.shape(0); j++) { if (dists(j) < min_distance_by_path) { @@ -420,23 +420,23 @@ inline void setPathCostsIfNotSet( * @param forward_preference If reversing direction is valid * @return Angle between two points */ -inline double posePointAngle( +inline float posePointAngle( const geometry_msgs::msg::Pose & pose, double point_x, double point_y, bool forward_preference) { - double pose_x = pose.position.x; - double pose_y = pose.position.y; - double pose_yaw = tf2::getYaw(pose.orientation); + float pose_x = pose.position.x; + float pose_y = pose.position.y; + float pose_yaw = tf2::getYaw(pose.orientation); - double yaw = atan2(point_y - pose_y, point_x - pose_x); + float yaw = atan2f(point_y - pose_y, point_x - pose_x); // If no preference for forward, return smallest angle either in heading or 180 of heading if (!forward_preference) { return std::min( - abs(angles::shortest_angular_distance(yaw, pose_yaw)), - abs(angles::shortest_angular_distance(yaw, angles::normalize_angle(pose_yaw + M_PI)))); + fabs(angles::shortest_angular_distance(yaw, pose_yaw)), + fabs(angles::shortest_angular_distance(yaw, angles::normalize_angle(pose_yaw + M_PI)))); } - return abs(angles::shortest_angular_distance(yaw, pose_yaw)); + return fabs(angles::shortest_angular_distance(yaw, pose_yaw)); } /** @@ -625,17 +625,17 @@ inline unsigned int findFirstPathInversion(nav_msgs::msg::Path & path) // Iterating through the path to determine the position of the path inversion for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) { // We have two vectors for the dot product OA and AB. Determining the vectors. - double oa_x = path.poses[idx].pose.position.x - + float oa_x = path.poses[idx].pose.position.x - path.poses[idx - 1].pose.position.x; - double oa_y = path.poses[idx].pose.position.y - + float oa_y = path.poses[idx].pose.position.y - path.poses[idx - 1].pose.position.y; - double ab_x = path.poses[idx + 1].pose.position.x - + float ab_x = path.poses[idx + 1].pose.position.x - path.poses[idx].pose.position.x; - double ab_y = path.poses[idx + 1].pose.position.y - + float ab_y = path.poses[idx + 1].pose.position.y - path.poses[idx].pose.position.y; // Checking for the existance of cusp, in the path, using the dot product. - double dot_product = (oa_x * ab_x) + (oa_y * ab_y); + float dot_product = (oa_x * ab_x) + (oa_y * ab_y); if (dot_product < 0.0) { return idx + 1; } diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index 017957da525..1cc0ae9e738 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -2,7 +2,7 @@ nav2_mppi_controller - 1.1.9 + 1.1.10 nav2_mppi_controller Aleksei Budyakov Steve Macenski diff --git a/nav2_mppi_controller/src/critics/constraint_critic.cpp b/nav2_mppi_controller/src/critics/constraint_critic.cpp index 4dc2bcf1deb..e9774c7775f 100644 --- a/nav2_mppi_controller/src/critics/constraint_critic.cpp +++ b/nav2_mppi_controller/src/critics/constraint_critic.cpp @@ -34,8 +34,8 @@ void ConstraintCritic::initialize() getParentParam(vx_min, "vx_min", -0.35); const float min_sgn = vx_min > 0.0 ? 1.0 : -1.0; - max_vel_ = std::sqrt(vx_max * vx_max + vy_max * vy_max); - min_vel_ = min_sgn * std::sqrt(vx_min * vx_min + vy_max * vy_max); + max_vel_ = sqrtf(vx_max * vx_max + vy_max * vy_max); + min_vel_ = min_sgn * sqrtf(vx_min * vx_min + vy_max * vy_max); } void ConstraintCritic::score(CriticData & data) diff --git a/nav2_mppi_controller/src/critics/goal_critic.cpp b/nav2_mppi_controller/src/critics/goal_critic.cpp index e38a98ed6b0..852db11529d 100644 --- a/nav2_mppi_controller/src/critics/goal_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_critic.cpp @@ -57,7 +57,7 @@ void GoalCritic::score(CriticData & data) xt::pow(traj_x - goal_x, 2) + xt::pow(traj_y - goal_y, 2)); - data.costs += xt::pow(xt::mean(dists, {1}) * weight_, power_); + data.costs += xt::pow(xt::mean(dists, {1}, immediate) * weight_, power_); } } // namespace mppi::critics diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index 4568e1eb14f..9c80134f49d 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -32,7 +32,7 @@ void ObstaclesCritic::initialize() collision_checker_.setCostmap(costmap_); possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_); - if (possibly_inscribed_cost_ < 1) { + if (possibly_inscribed_cost_ < 1.0f) { RCLCPP_ERROR( logger_, "Inflation layer either not found or inflation is not set sufficiently for " @@ -50,7 +50,7 @@ void ObstaclesCritic::initialize() "footprint" : "circular"); } -double ObstaclesCritic::findCircumscribedCost( +float ObstaclesCritic::findCircumscribedCost( std::shared_ptr costmap) { double result = -1.0; @@ -76,7 +76,7 @@ double ObstaclesCritic::findCircumscribedCost( if (!inflation_layer_found) { RCLCPP_WARN( - rclcpp::get_logger("computeCircumscribedCost"), + logger_, "No inflation layer found in costmap configuration. " "If this is an SE2-collision checking plugin, it cannot use costmap potential " "field to speed up collision checking by only checking the full footprint " @@ -84,7 +84,7 @@ double ObstaclesCritic::findCircumscribedCost( "significantly slow down planning times and not avoid anything but absolute collisions!"); } - return result; + return static_cast(result); } float ObstaclesCritic::distanceToObstacle(const CollisionCost & cost) @@ -116,21 +116,21 @@ void ObstaclesCritic::score(CriticData & data) } auto && raw_cost = xt::xtensor::from_shape({data.costs.shape(0)}); - raw_cost.fill(0.0); + raw_cost.fill(0.0f); auto && repulsive_cost = xt::xtensor::from_shape({data.costs.shape(0)}); - repulsive_cost.fill(0.0); + repulsive_cost.fill(0.0f); const size_t traj_len = data.trajectories.x.shape(1); bool all_trajectories_collide = true; for (size_t i = 0; i < data.trajectories.x.shape(0); ++i) { bool trajectory_collide = false; - float traj_cost = 0.0; + float traj_cost = 0.0f; const auto & traj = data.trajectories; CollisionCost pose_cost; for (size_t j = 0; j < traj_len; j++) { pose_cost = costAtPose(traj.x(i, j), traj.y(i, j), traj.yaws(i, j)); - if (pose_cost.cost < 1) {continue;} // In free space + if (pose_cost.cost < 1.0f) {continue;} // In free space if (inCollision(pose_cost.cost)) { trajectory_collide = true; @@ -138,7 +138,7 @@ void ObstaclesCritic::score(CriticData & data) } // Cannot process repulsion if inflation layer does not exist - if (inflation_radius_ == 0 || inflation_scale_factor_ == 0) { + if (inflation_radius_ == 0.0f || inflation_scale_factor_ == 0.0f) { continue; } @@ -198,7 +198,9 @@ CollisionCost ObstaclesCritic::costAtPose(float x, float y, float theta) } cost = collision_checker_.pointCost(x_i, y_i); - if (consider_footprint_ && (cost >= possibly_inscribed_cost_ || possibly_inscribed_cost_ < 1)) { + if (consider_footprint_ && + (cost >= possibly_inscribed_cost_ || possibly_inscribed_cost_ < 1.0f)) + { cost = static_cast(collision_checker_.footprintCostAtPose( x, y, theta, costmap_ros_->getRobotFootprint())); collision_cost.using_footprint = true; diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index 2585193ade5..4ff12d4e553 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -88,12 +88,12 @@ void PathAlignCritic::score(CriticData & data) return; } - float dist_sq = 0, dx = 0, dy = 0, dyaw = 0, summed_dist = 0; - double min_dist_sq = std::numeric_limits::max(); + float dist_sq = 0.0f, dx = 0.0f, dy = 0.0f, dyaw = 0.0f, summed_dist = 0.0f; + float min_dist_sq = std::numeric_limits::max(); size_t min_s = 0; for (size_t t = 0; t < batch_size; ++t) { - summed_dist = 0; + summed_dist = 0.0f; for (size_t p = trajectory_point_step_; p < time_steps; p += trajectory_point_step_) { min_dist_sq = std::numeric_limits::max(); min_s = 0; @@ -118,7 +118,7 @@ void PathAlignCritic::score(CriticData & data) // The nearest path point to align to needs to be not in collision, else // let the obstacle critic take over in this region due to dynamic obstacles if (min_s != 0 && (*data.path_pts_valid)[min_s]) { - summed_dist += std::sqrt(min_dist_sq); + summed_dist += sqrtf(min_dist_sq); } } diff --git a/nav2_mppi_controller/src/noise_generator.cpp b/nav2_mppi_controller/src/noise_generator.cpp index 60173789f0d..09ee4ab92d7 100644 --- a/nav2_mppi_controller/src/noise_generator.cpp +++ b/nav2_mppi_controller/src/noise_generator.cpp @@ -94,14 +94,14 @@ void NoiseGenerator::generateNoisedControls() auto & s = settings_; xt::noalias(noises_vx_) = xt::random::randn( - {s.batch_size, s.time_steps}, 0.0, + {s.batch_size, s.time_steps}, 0.0f, s.sampling_std.vx); xt::noalias(noises_wz_) = xt::random::randn( - {s.batch_size, s.time_steps}, 0.0, + {s.batch_size, s.time_steps}, 0.0f, s.sampling_std.wz); if (is_holonomic_) { xt::noalias(noises_vy_) = xt::random::randn( - {s.batch_size, s.time_steps}, 0.0, + {s.batch_size, s.time_steps}, 0.0f, s.sampling_std.vy); } } diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index e3485f5449a..a7bbd35002a 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -285,8 +285,8 @@ void Optimizer::integrateStateVelocities( const auto yaw_offseted = xt::view(traj_yaws, xt::range(1, _)); - xt::noalias(xt::view(yaw_cos, 0)) = std::cos(initial_yaw); - xt::noalias(xt::view(yaw_sin, 0)) = std::sin(initial_yaw); + xt::noalias(xt::view(yaw_cos, 0)) = cosf(initial_yaw); + xt::noalias(xt::view(yaw_sin, 0)) = sinf(initial_yaw); xt::noalias(xt::view(yaw_cos, xt::range(1, _))) = xt::cos(yaw_offseted); xt::noalias(xt::view(yaw_sin, xt::range(1, _))) = xt::sin(yaw_offseted); @@ -315,8 +315,8 @@ void Optimizer::integrateStateVelocities( auto && yaw_cos = xt::xtensor::from_shape(trajectories.yaws.shape()); auto && yaw_sin = xt::xtensor::from_shape(trajectories.yaws.shape()); - xt::noalias(xt::view(yaw_cos, xt::all(), 0)) = std::cos(initial_yaw); - xt::noalias(xt::view(yaw_sin, xt::all(), 0)) = std::sin(initial_yaw); + xt::noalias(xt::view(yaw_cos, xt::all(), 0)) = cosf(initial_yaw); + xt::noalias(xt::view(yaw_sin, xt::all(), 0)) = sinf(initial_yaw); xt::noalias(xt::view(yaw_cos, xt::all(), xt::range(1, _))) = xt::cos(yaws_cutted); xt::noalias(xt::view(yaw_sin, xt::all(), xt::range(1, _))) = xt::sin(yaws_cutted); @@ -357,16 +357,16 @@ void Optimizer::updateControlSequence() auto bounded_noises_vx = state_.cvx - control_sequence_.vx; auto bounded_noises_wz = state_.cwz - control_sequence_.wz; xt::noalias(costs_) += - s.gamma / std::pow(s.sampling_std.vx, 2) * xt::sum( + s.gamma / powf(s.sampling_std.vx, 2) * xt::sum( xt::view(control_sequence_.vx, xt::newaxis(), xt::all()) * bounded_noises_vx, 1, immediate); xt::noalias(costs_) += - s.gamma / std::pow(s.sampling_std.wz, 2) * xt::sum( + s.gamma / powf(s.sampling_std.wz, 2) * xt::sum( xt::view(control_sequence_.wz, xt::newaxis(), xt::all()) * bounded_noises_wz, 1, immediate); if (isHolonomic()) { auto bounded_noises_vy = state_.cvy - control_sequence_.vy; xt::noalias(costs_) += - s.gamma / std::pow(s.sampling_std.vy, 2) * xt::sum( + s.gamma / powf(s.sampling_std.vy, 2) * xt::sum( xt::view(control_sequence_.vy, xt::newaxis(), xt::all()) * bounded_noises_vy, 1, immediate); } @@ -377,8 +377,10 @@ void Optimizer::updateControlSequence() auto && softmaxes_extened = xt::eval(xt::view(softmaxes, xt::all(), xt::newaxis())); xt::noalias(control_sequence_.vx) = xt::sum(state_.cvx * softmaxes_extened, 0, immediate); - xt::noalias(control_sequence_.vy) = xt::sum(state_.cvy * softmaxes_extened, 0, immediate); xt::noalias(control_sequence_.wz) = xt::sum(state_.cwz * softmaxes_extened, 0, immediate); + if (isHolonomic()) { + xt::noalias(control_sequence_.vy) = xt::sum(state_.cvy * softmaxes_extened, 0, immediate); + } applyControlSequenceConstraints(); } diff --git a/nav2_mppi_controller/src/path_handler.cpp b/nav2_mppi_controller/src/path_handler.cpp index cfefb3b6d38..893f9286c8d 100644 --- a/nav2_mppi_controller/src/path_handler.cpp +++ b/nav2_mppi_controller/src/path_handler.cpp @@ -166,8 +166,8 @@ bool PathHandler::transformPose( double PathHandler::getMaxCostmapDist() { const auto & costmap = costmap_->getCostmap(); - return std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY()) * - costmap->getResolution() / 2.0; + return static_cast(std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY())) * + costmap->getResolution() * 0.50; } void PathHandler::setPath(const nav_msgs::msg::Path & plan) @@ -190,11 +190,11 @@ bool PathHandler::isWithinInversionTolerances(const geometry_msgs::msg::PoseStam { // Keep full path if we are within tolerance of the inversion pose const auto last_pose = global_plan_up_to_inversion_.poses.back(); - double distance = std::hypot( + float distance = hypotf( robot_pose.pose.position.x - last_pose.pose.position.x, robot_pose.pose.position.y - last_pose.pose.position.y); - double angle_distance = angles::shortest_angular_distance( + float angle_distance = angles::shortest_angular_distance( tf2::getYaw(robot_pose.pose.orientation), tf2::getYaw(last_pose.pose.orientation)); diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index a1a42aa7764..d0f7a1bfe8e 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -307,19 +307,19 @@ TEST(CriticTests, PreferForwardCritic) path.reset(10); path.x(9) = 10.0; critic.score(data); - EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0f, 1e-6f); // provide state pose and path close to trigger behavior but with all forward motion path.x(9) = 0.15; state.vx = xt::ones({1000, 30}); critic.score(data); - EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0f, 1e-6f); // provide state pose and path close to trigger behavior but with all reverse motion state.vx = -1.0 * xt::ones({1000, 30}); critic.score(data); - EXPECT_GT(xt::sum(costs, immediate)(), 0.0); - EXPECT_NEAR(costs(0), 15.0, 1e-6); // 1.0 * 0.1 model_dt * 5.0 weight * 30 length + EXPECT_GT(xt::sum(costs, immediate)(), 0.0f); + EXPECT_NEAR(costs(0), 15.0f, 1e-3f); // 1.0 * 0.1 model_dt * 5.0 weight * 30 length } TEST(CriticTests, TwirlingCritic) diff --git a/nav2_mppi_controller/test/optimizer_unit_tests.cpp b/nav2_mppi_controller/test/optimizer_unit_tests.cpp index b562da1668f..45cf7203701 100644 --- a/nav2_mppi_controller/test/optimizer_unit_tests.cpp +++ b/nav2_mppi_controller/test/optimizer_unit_tests.cpp @@ -432,20 +432,20 @@ TEST(OptimizerTests, SpeedLimitTests) // Test Speed limits API auto [v_min, v_max] = optimizer_tester.getVelLimits(); - EXPECT_EQ(v_max, 0.5); - EXPECT_EQ(v_min, -0.35); + EXPECT_EQ(v_max, 0.5f); + EXPECT_EQ(v_min, -0.35f); optimizer_tester.setSpeedLimit(0, false); auto [v_min2, v_max2] = optimizer_tester.getVelLimits(); - EXPECT_EQ(v_max2, 0.5); - EXPECT_EQ(v_min2, -0.35); + EXPECT_EQ(v_max2, 0.5f); + EXPECT_EQ(v_min2, -0.35f); optimizer_tester.setSpeedLimit(50.0, true); auto [v_min3, v_max3] = optimizer_tester.getVelLimits(); EXPECT_NEAR(v_max3, 0.5 / 2.0, 1e-3); EXPECT_NEAR(v_min3, -0.35 / 2.0, 1e-3); optimizer_tester.setSpeedLimit(0, true); auto [v_min4, v_max4] = optimizer_tester.getVelLimits(); - EXPECT_EQ(v_max4, 0.5); - EXPECT_EQ(v_min4, -0.35); + EXPECT_EQ(v_max4, 0.5f); + EXPECT_EQ(v_min4, -0.35f); optimizer_tester.setSpeedLimit(0.75, false); auto [v_min5, v_max5] = optimizer_tester.getVelLimits(); EXPECT_NEAR(v_max5, 0.75, 1e-3); diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index 49b585af2a1..284eac8295e 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 1.1.9 + 1.1.10 Messages and service files for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index 2b5ca370092..c56e2d44304 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 1.1.9 + 1.1.10 TODO Steve Macenski Carlos Orduno diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index 18873edabdd..306fbe20984 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 1.1.9 + 1.1.10 TODO Steve Macenski Apache-2.0 diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index ba817d6617a..2126503bae4 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -592,6 +592,7 @@ void PlannerServer::isPathValid( * The lethal check starts at the closest point to avoid points that have already been passed * and may have become occupied */ + std::unique_lock lock(*(costmap_->getMutex())); unsigned int mx = 0; unsigned int my = 0; for (unsigned int i = closest_point_index; i < request->path.poses.size(); ++i) { diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index 23e7cc44b53..7484baad8cf 100644 --- a/nav2_regulated_pure_pursuit_controller/package.xml +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -2,7 +2,7 @@ nav2_regulated_pure_pursuit_controller - 1.1.9 + 1.1.10 Regulated Pure Pursuit Controller Steve Macenski Shrijit Singh diff --git a/nav2_rotation_shim_controller/package.xml b/nav2_rotation_shim_controller/package.xml index f97c3662249..649ebd63fa1 100644 --- a/nav2_rotation_shim_controller/package.xml +++ b/nav2_rotation_shim_controller/package.xml @@ -2,7 +2,7 @@ nav2_rotation_shim_controller - 1.1.9 + 1.1.10 Rotation Shim Controller Steve Macenski Apache-2.0 diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 20200ebeefd..b38dea33ce7 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 1.1.9 + 1.1.10 Navigation 2 plugins for rviz Michael Jeronimo Apache-2.0 diff --git a/nav2_simple_commander/README.md b/nav2_simple_commander/README.md index a43fab366e1..339d986ed41 100644 --- a/nav2_simple_commander/README.md +++ b/nav2_simple_commander/README.md @@ -14,6 +14,8 @@ See its [API Guide Page](https://navigation.ros.org/commander_api/index.html) fo The methods provided by the basic navigator are shown below, with inputs and expected returns. If a server fails, it may throw an exception or return a `None` object, so please be sure to properly wrap your navigation calls in try/catch and check results for `None` type. +New as of September 2023: the simple navigator constructor will accept a `namespace` field to support multi-robot applications or namespaced Nav2 launches. + | Robot Navigator Method | Description | | --------------------------------- | -------------------------------------------------------------------------- | | setInitialPose(initial_pose) | Sets the initial pose (`PoseStamped`) of the robot to localization. | diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index 3421c3f000d..64f2117a08a 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -46,8 +46,8 @@ class TaskResult(Enum): class BasicNavigator(Node): - def __init__(self, node_name='basic_navigator'): - super().__init__(node_name=node_name) + def __init__(self, node_name='basic_navigator', namespace=''): + super().__init__(node_name=node_name, namespace=namespace) self.initial_pose = PoseStamped() self.initial_pose.header.frame_id = 'map' self.goal_handle = None @@ -83,13 +83,13 @@ def __init__(self, node_name='basic_navigator'): self.initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped, 'initialpose', 10) - self.change_maps_srv = self.create_client(LoadMap, '/map_server/load_map') + self.change_maps_srv = self.create_client(LoadMap, 'map_server/load_map') self.clear_costmap_global_srv = self.create_client( - ClearEntireCostmap, '/global_costmap/clear_entirely_global_costmap') + ClearEntireCostmap, 'global_costmap/clear_entirely_global_costmap') self.clear_costmap_local_srv = self.create_client( - ClearEntireCostmap, '/local_costmap/clear_entirely_local_costmap') - self.get_costmap_global_srv = self.create_client(GetCostmap, '/global_costmap/get_costmap') - self.get_costmap_local_srv = self.create_client(GetCostmap, '/local_costmap/get_costmap') + ClearEntireCostmap, 'local_costmap/clear_entirely_local_costmap') + self.get_costmap_global_srv = self.create_client(GetCostmap, 'global_costmap/get_costmap') + self.get_costmap_local_srv = self.create_client(GetCostmap, 'local_costmap/get_costmap') def destroyNode(self): self.destroy_node() diff --git a/nav2_simple_commander/package.xml b/nav2_simple_commander/package.xml index 830d297706f..33141e36236 100644 --- a/nav2_simple_commander/package.xml +++ b/nav2_simple_commander/package.xml @@ -2,7 +2,7 @@ nav2_simple_commander - 1.1.9 + 1.1.10 An importable library for writing mobile robot applications in python3 steve Apache-2.0 diff --git a/nav2_smac_planner/package.xml b/nav2_smac_planner/package.xml index 0ab37485fcf..91548a5bcec 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -2,7 +2,7 @@ nav2_smac_planner - 1.1.9 + 1.1.10 Smac global planning plugin: A*, Hybrid-A*, State Lattice Steve Macenski Apache-2.0 diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml index f361b621dbf..d26ccdea5ee 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -2,7 +2,7 @@ nav2_smoother - 1.1.9 + 1.1.10 Smoother action interface Matej Vargovcik Steve Macenski diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index bfdee60c710..7151035b7fb 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 1.1.9 + 1.1.10 TODO Carlos Orduno Apache-2.0 diff --git a/nav2_system_tests/src/costmap_filters/keepout_params.yaml b/nav2_system_tests/src/costmap_filters/keepout_params.yaml index 8a3c9511c54..bc99c50cd72 100644 --- a/nav2_system_tests/src/costmap_filters/keepout_params.yaml +++ b/nav2_system_tests/src/costmap_filters/keepout_params.yaml @@ -127,6 +127,7 @@ controller_server: plugin: "dwb_core::DWBLocalPlanner" debug_trajectory_details: True prune_distance: 1.0 + forward_prune_distance: 1.0 min_vel_x: 0.0 min_vel_y: 0.0 max_vel_x: 0.26 diff --git a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py index 796667c65d7..996287dc4d6 100755 --- a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py +++ b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py @@ -48,7 +48,7 @@ def generate_launch_description(): param_substitutions = { 'planner_server.ros__parameters.GridBased.use_astar': os.getenv('ASTAR'), 'filter_mask_server.ros__parameters.yaml_filename': filter_mask_file, - 'yaml_filename': filter_mask_file} + 'map_server.ros__parameters.yaml_filename': map_yaml_file} configured_params = RewrittenYaml( source_file=params_file, root_key='', diff --git a/nav2_system_tests/src/costmap_filters/test_speed_launch.py b/nav2_system_tests/src/costmap_filters/test_speed_launch.py index 5b0c61ed9d7..d4467e1de4d 100755 --- a/nav2_system_tests/src/costmap_filters/test_speed_launch.py +++ b/nav2_system_tests/src/costmap_filters/test_speed_launch.py @@ -47,7 +47,7 @@ def generate_launch_description(): param_substitutions = { 'planner_server.ros__parameters.GridBased.use_astar': os.getenv('ASTAR'), 'filter_mask_server.ros__parameters.yaml_filename': filter_mask_file, - 'yaml_filename': filter_mask_file} + 'map_server.ros__parameters.yaml_filename': map_yaml_file} configured_params = RewrittenYaml( source_file=params_file, root_key='', diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index 7f0ca4cafdb..7dcb6c71871 100644 --- a/nav2_theta_star_planner/package.xml +++ b/nav2_theta_star_planner/package.xml @@ -2,7 +2,7 @@ nav2_theta_star_planner - 1.1.9 + 1.1.10 Theta* Global Planning Plugin Steve Macenski Anshumaan Singh diff --git a/nav2_util/CMakeLists.txt b/nav2_util/CMakeLists.txt index 86fa5fdaee1..4635bab0b0d 100644 --- a/nav2_util/CMakeLists.txt +++ b/nav2_util/CMakeLists.txt @@ -32,6 +32,7 @@ set(dependencies bondcpp bond action_msgs + rcl_interfaces ) nav2_package() diff --git a/nav2_util/include/nav2_util/node_utils.hpp b/nav2_util/include/nav2_util/node_utils.hpp index d1a39d4a5bd..62201624a6d 100644 --- a/nav2_util/include/nav2_util/node_utils.hpp +++ b/nav2_util/include/nav2_util/node_utils.hpp @@ -15,8 +15,10 @@ #ifndef NAV2_UTIL__NODE_UTILS_HPP_ #define NAV2_UTIL__NODE_UTILS_HPP_ +#include #include #include "rclcpp/rclcpp.hpp" +#include "rcl_interfaces/srv/list_parameters.hpp" namespace nav2_util { @@ -150,6 +152,25 @@ std::string get_plugin_type_param( return plugin_type; } +/** + * @brief A method to copy all parameters from one node (parent) to another (child). + * May throw parameter exceptions in error conditions + * @param parent Node to copy parameters from + * @param child Node to copy parameters to + */ +template +void copy_all_parameters(const NodeT1 & parent, const NodeT2 & child) +{ + using Parameters = std::vector; + std::vector param_names = parent->list_parameters({}, 0).names; + Parameters params = parent->get_parameters(param_names); + for (Parameters::const_iterator iter = params.begin(); iter != params.end(); ++iter) { + if (!child->has_parameter(iter->get_name())) { + child->declare_parameter(iter->get_name(), iter->get_parameter_value()); + } + } +} + } // namespace nav2_util #endif // NAV2_UTIL__NODE_UTILS_HPP_ diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 861f323f241..534459fa716 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 1.1.9 + 1.1.10 TODO Michael Jeronimo Mohammad Haghighipanah @@ -28,6 +28,7 @@ launch launch_testing_ament_cmake action_msgs + rcl_interfaces libboost-program-options diff --git a/nav2_util/test/test_node_utils.cpp b/nav2_util/test/test_node_utils.cpp index 7c439271615..9d6d6ba133b 100644 --- a/nav2_util/test/test_node_utils.cpp +++ b/nav2_util/test/test_node_utils.cpp @@ -94,3 +94,35 @@ TEST(GetPluginTypeParam, GetPluginTypeParam) ASSERT_EQ(get_plugin_type_param(node, "Foo"), "bar"); ASSERT_EXIT(get_plugin_type_param(node, "Waldo"), ::testing::ExitedWithCode(255), ".*"); } + +TEST(TestParamCopying, TestParamCopying) +{ + auto node1 = std::make_shared("test_node1"); + auto node2 = std::make_shared("test_node2"); + + // Tests for (1) multiple types, (2) recursion, (3) overriding values + node1->declare_parameter("Foo1", rclcpp::ParameterValue(std::string(("bar1")))); + node1->declare_parameter("Foo2", rclcpp::ParameterValue(0.123)); + node1->declare_parameter("Foo", rclcpp::ParameterValue(std::string(("bar")))); + node1->declare_parameter("Foo.bar", rclcpp::ParameterValue(std::string(("steve")))); + node2->declare_parameter("Foo", rclcpp::ParameterValue(std::string(("barz2")))); + + // Show Node2 is empty of Node1's parameters, but contains its own + EXPECT_FALSE(node2->has_parameter("Foo1")); + EXPECT_FALSE(node2->has_parameter("Foo2")); + EXPECT_FALSE(node2->has_parameter("Foo.bar")); + EXPECT_TRUE(node2->has_parameter("Foo")); + EXPECT_EQ(node2->get_parameter("Foo").as_string(), std::string("barz2")); + + nav2_util::copy_all_parameters(node1, node2); + + // Test new parameters exist, of expected value, and original param is not overridden + EXPECT_TRUE(node2->has_parameter("Foo1")); + EXPECT_EQ(node2->get_parameter("Foo1").as_string(), std::string("bar1")); + EXPECT_TRUE(node2->has_parameter("Foo2")); + EXPECT_EQ(node2->get_parameter("Foo2").as_double(), 0.123); + EXPECT_TRUE(node2->has_parameter("Foo.bar")); + EXPECT_EQ(node2->get_parameter("Foo.bar").as_string(), std::string("steve")); + EXPECT_TRUE(node2->has_parameter("Foo")); + EXPECT_EQ(node2->get_parameter("Foo").as_string(), std::string("barz2")); +} diff --git a/nav2_velocity_smoother/package.xml b/nav2_velocity_smoother/package.xml index d0009942e7b..878bde60764 100644 --- a/nav2_velocity_smoother/package.xml +++ b/nav2_velocity_smoother/package.xml @@ -2,7 +2,7 @@ nav2_velocity_smoother - 1.1.9 + 1.1.10 Nav2's Output velocity smoother Steve Macenski Apache-2.0 diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index d19fe4815b3..de604b2c8f5 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 1.1.9 + 1.1.10 voxel_grid provides an implementation of an efficient 3D voxel grid. The occupancy grid can support 3 different representations for the state of a cell: marked, free, or unknown. Due to the underlying implementation relying on bitwise and and or integer operations, the voxel grid only supports 16 different levels per voxel column. However, this limitation yields raytracing and cell marking performance in the grid comparable to standard 2D structures making it quite fast compared to most 3D structures. diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index c1a88efec7c..c6c41236382 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 1.1.9 + 1.1.10 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/navigation2/package.xml b/navigation2/package.xml index 43315f5da6d..9c22eccb766 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 1.1.9 + 1.1.10 ROS2 Navigation Stack From c50d7b2b458f9751cbf6acee93cd0174355a233b Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 27 Sep 2023 15:54:06 -0700 Subject: [PATCH 15/39] Update CMakeLists.txt (#3843) (#3845) (cherry picked from commit 2d6e9a96354c0ea763e70eedd81225635f7b9db5) Co-authored-by: Steve Macenski --- nav2_mppi_controller/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index 5cc8a8c0e90..9d8e0d690d0 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -48,7 +48,7 @@ if(COMPILER_SUPPORTS_FMA) endif() # If building one the same hardware to be deployed on, try `-march=native`! -add_compile_options(-O3 -finline-limit=10000000 -ffp-contract=fast -ffast-math -mtune=generic) +add_compile_options(-O3 -finline-limit=10000000 -ffp-contract=fast -ffast-math -mtune=generic -mno-avx512f) add_library(mppi_controller SHARED src/controller.cpp From 6e35cc7417463c3f441c0d7fc596cff251cdfe4b Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Wed, 27 Sep 2023 16:13:11 -0700 Subject: [PATCH 16/39] bump to 1.1.11 for release for AVX512 fixes --- nav2_amcl/package.xml | 2 +- nav2_behavior_tree/package.xml | 2 +- nav2_behaviors/package.xml | 2 +- nav2_bringup/package.xml | 2 +- nav2_bt_navigator/package.xml | 2 +- nav2_collision_monitor/package.xml | 2 +- nav2_common/package.xml | 2 +- nav2_constrained_smoother/package.xml | 2 +- nav2_controller/package.xml | 2 +- nav2_core/package.xml | 2 +- nav2_costmap_2d/package.xml | 2 +- nav2_dwb_controller/costmap_queue/package.xml | 2 +- nav2_dwb_controller/dwb_core/package.xml | 2 +- nav2_dwb_controller/dwb_critics/package.xml | 2 +- nav2_dwb_controller/dwb_msgs/package.xml | 2 +- nav2_dwb_controller/dwb_plugins/package.xml | 2 +- nav2_dwb_controller/nav2_dwb_controller/package.xml | 2 +- nav2_dwb_controller/nav_2d_msgs/package.xml | 2 +- nav2_dwb_controller/nav_2d_utils/package.xml | 2 +- nav2_lifecycle_manager/package.xml | 2 +- nav2_map_server/package.xml | 2 +- nav2_mppi_controller/package.xml | 2 +- nav2_msgs/package.xml | 2 +- nav2_navfn_planner/package.xml | 2 +- nav2_planner/package.xml | 2 +- nav2_regulated_pure_pursuit_controller/package.xml | 2 +- nav2_rotation_shim_controller/package.xml | 2 +- nav2_rviz_plugins/package.xml | 2 +- nav2_simple_commander/package.xml | 2 +- nav2_smac_planner/package.xml | 2 +- nav2_smoother/package.xml | 2 +- nav2_system_tests/package.xml | 2 +- nav2_theta_star_planner/package.xml | 2 +- nav2_util/package.xml | 2 +- nav2_velocity_smoother/package.xml | 2 +- nav2_voxel_grid/package.xml | 2 +- nav2_waypoint_follower/package.xml | 2 +- navigation2/package.xml | 2 +- 38 files changed, 38 insertions(+), 38 deletions(-) diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index afea3eb3b7a..374808e1fa8 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 1.1.10 + 1.1.11

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index d5117b45d95..4fa59657f60 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.1.10 + 1.1.11 TODO Michael Jeronimo Carlos Orduno diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml index 107c044bb71..5759ebd778f 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -2,7 +2,7 @@ nav2_behaviors - 1.1.10 + 1.1.11 TODO Carlos Orduno Steve Macenski diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index 488f5486d32..6a1d02cba98 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.1.10 + 1.1.11 Bringup scripts and configurations for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index 15975252c7e..74a560213be 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.1.10 + 1.1.11 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index 6ca825d1fa8..ee85e2874e9 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -2,7 +2,7 @@ nav2_collision_monitor - 1.1.10 + 1.1.11 Collision Monitor Alexey Merzlyakov Steve Macenski diff --git a/nav2_common/package.xml b/nav2_common/package.xml index 6c0d093102f..183a0053526 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.1.10 + 1.1.11 Common support functionality used throughout the navigation 2 stack Carl Delsey Apache-2.0 diff --git a/nav2_constrained_smoother/package.xml b/nav2_constrained_smoother/package.xml index 7c0f7fb4154..9ebc521f0cc 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -2,7 +2,7 @@ nav2_constrained_smoother - 1.1.10 + 1.1.11 Ceres constrained smoother Matej Vargovcik Steve Macenski diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 9066014856c..bde756e13fd 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.1.10 + 1.1.11 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_core/package.xml b/nav2_core/package.xml index 219e74f0a89..e1f9b56c0c7 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.1.10 + 1.1.11 A set of headers for plugins core to the Nav2 stack Steve Macenski Carl Delsey diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index 72e03fa90f8..76c253e0a94 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 1.1.10 + 1.1.11 This package provides an implementation of a 2D costmap that takes in sensor data from the world, builds a 2D or 3D occupancy grid of the data (depending diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index d32c36b01e2..1a52177c500 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 1.1.10 + 1.1.11 The costmap_queue package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml index f135f172289..16cb8ca025e 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 1.1.10 + 1.1.11 TODO Carl Delsey BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index 1fd6afba514..0e7b5322330 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 1.1.10 + 1.1.11 The dwb_critics package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml index 0934647b561..7450fd46576 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 1.1.10 + 1.1.11 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index 1a6611135d4..2d5af1aedb9 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 1.1.10 + 1.1.11 Standard implementations of the GoalChecker and TrajectoryGenerators for dwb_core diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index 880ae326541..12c79f3d986 100644 --- a/nav2_dwb_controller/nav2_dwb_controller/package.xml +++ b/nav2_dwb_controller/nav2_dwb_controller/package.xml @@ -2,7 +2,7 @@ nav2_dwb_controller - 1.1.10 + 1.1.11 ROS2 controller (DWB) metapackage diff --git a/nav2_dwb_controller/nav_2d_msgs/package.xml b/nav2_dwb_controller/nav_2d_msgs/package.xml index d41c64dd415..454dfbdeaed 100644 --- a/nav2_dwb_controller/nav_2d_msgs/package.xml +++ b/nav2_dwb_controller/nav_2d_msgs/package.xml @@ -2,7 +2,7 @@ nav_2d_msgs - 1.1.10 + 1.1.11 Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D. David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml index 4cd7a6051e2..a8074a1fa2f 100644 --- a/nav2_dwb_controller/nav_2d_utils/package.xml +++ b/nav2_dwb_controller/nav_2d_utils/package.xml @@ -2,7 +2,7 @@ nav_2d_utils - 1.1.10 + 1.1.11 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index e56ba055e8c..b45c6b41150 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 1.1.10 + 1.1.11 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index 08e007a505e..9d203b1fe8c 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 1.1.10 + 1.1.11 Refactored map server for ROS2 Navigation diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index 1cc0ae9e738..c8e626f22b9 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -2,7 +2,7 @@ nav2_mppi_controller - 1.1.10 + 1.1.11 nav2_mppi_controller Aleksei Budyakov Steve Macenski diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index 284eac8295e..914ab4d7efb 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 1.1.10 + 1.1.11 Messages and service files for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index c56e2d44304..a2768f47e2f 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 1.1.10 + 1.1.11 TODO Steve Macenski Carlos Orduno diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index 306fbe20984..c7f12173741 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 1.1.10 + 1.1.11 TODO Steve Macenski Apache-2.0 diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index 7484baad8cf..c92ad15d9b3 100644 --- a/nav2_regulated_pure_pursuit_controller/package.xml +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -2,7 +2,7 @@ nav2_regulated_pure_pursuit_controller - 1.1.10 + 1.1.11 Regulated Pure Pursuit Controller Steve Macenski Shrijit Singh diff --git a/nav2_rotation_shim_controller/package.xml b/nav2_rotation_shim_controller/package.xml index 649ebd63fa1..2f278e19606 100644 --- a/nav2_rotation_shim_controller/package.xml +++ b/nav2_rotation_shim_controller/package.xml @@ -2,7 +2,7 @@ nav2_rotation_shim_controller - 1.1.10 + 1.1.11 Rotation Shim Controller Steve Macenski Apache-2.0 diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index b38dea33ce7..ccc8327cf7e 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 1.1.10 + 1.1.11 Navigation 2 plugins for rviz Michael Jeronimo Apache-2.0 diff --git a/nav2_simple_commander/package.xml b/nav2_simple_commander/package.xml index 33141e36236..a83c8a83a55 100644 --- a/nav2_simple_commander/package.xml +++ b/nav2_simple_commander/package.xml @@ -2,7 +2,7 @@ nav2_simple_commander - 1.1.10 + 1.1.11 An importable library for writing mobile robot applications in python3 steve Apache-2.0 diff --git a/nav2_smac_planner/package.xml b/nav2_smac_planner/package.xml index 91548a5bcec..07c0e29b4c8 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -2,7 +2,7 @@ nav2_smac_planner - 1.1.10 + 1.1.11 Smac global planning plugin: A*, Hybrid-A*, State Lattice Steve Macenski Apache-2.0 diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml index d26ccdea5ee..2e2b7393073 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -2,7 +2,7 @@ nav2_smoother - 1.1.10 + 1.1.11 Smoother action interface Matej Vargovcik Steve Macenski diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 7151035b7fb..8cacb4bdbe9 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 1.1.10 + 1.1.11 TODO Carlos Orduno Apache-2.0 diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index 7dcb6c71871..57a0ff03b32 100644 --- a/nav2_theta_star_planner/package.xml +++ b/nav2_theta_star_planner/package.xml @@ -2,7 +2,7 @@ nav2_theta_star_planner - 1.1.10 + 1.1.11 Theta* Global Planning Plugin Steve Macenski Anshumaan Singh diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 534459fa716..e38f1c51941 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 1.1.10 + 1.1.11 TODO Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_velocity_smoother/package.xml b/nav2_velocity_smoother/package.xml index 878bde60764..c0dcd97098e 100644 --- a/nav2_velocity_smoother/package.xml +++ b/nav2_velocity_smoother/package.xml @@ -2,7 +2,7 @@ nav2_velocity_smoother - 1.1.10 + 1.1.11 Nav2's Output velocity smoother Steve Macenski Apache-2.0 diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index de604b2c8f5..f9722635386 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 1.1.10 + 1.1.11 voxel_grid provides an implementation of an efficient 3D voxel grid. The occupancy grid can support 3 different representations for the state of a cell: marked, free, or unknown. Due to the underlying implementation relying on bitwise and and or integer operations, the voxel grid only supports 16 different levels per voxel column. However, this limitation yields raytracing and cell marking performance in the grid comparable to standard 2D structures making it quite fast compared to most 3D structures. diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index c6c41236382..5189731d2a9 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 1.1.10 + 1.1.11 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/navigation2/package.xml b/navigation2/package.xml index 9c22eccb766..cdb046ed025 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 1.1.10 + 1.1.11 ROS2 Navigation Stack From 9876fe1b4faa79bcfe8a8fa50b3c4da2eeaf9d1a Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 2 Oct 2023 13:24:08 -0700 Subject: [PATCH 17/39] add option for sse4 and avs512 (#3853) (#3855) (cherry picked from commit 7274811c5cb512a05b87523183e29e75ace77f4a) Co-authored-by: Steve Macenski --- nav2_mppi_controller/CMakeLists.txt | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index 9d8e0d690d0..5f0c5359dbe 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -36,9 +36,19 @@ nav2_package() include(CheckCXXCompilerFlag) +check_cxx_compiler_flag("-mno-avx512f" COMPILER_SUPPORTS_AVX512) +check_cxx_compiler_flag("-msse4.2" COMPILER_SUPPORTS_SSE4) check_cxx_compiler_flag("-mavx2" COMPILER_SUPPORTS_AVX2) check_cxx_compiler_flag("-mfma" COMPILER_SUPPORTS_FMA) +if(COMPILER_SUPPORTS_AVX512) + add_compile_options(-mno-avx512f) +endif() + +if(COMPILER_SUPPORTS_SSE4) + add_compile_options(-msse4.2) +endif() + if(COMPILER_SUPPORTS_AVX2) add_compile_options(-mavx2) endif() @@ -48,7 +58,7 @@ if(COMPILER_SUPPORTS_FMA) endif() # If building one the same hardware to be deployed on, try `-march=native`! -add_compile_options(-O3 -finline-limit=10000000 -ffp-contract=fast -ffast-math -mtune=generic -mno-avx512f) +add_compile_options(-O3 -finline-limit=10000000 -ffp-contract=fast -ffast-math -mtune=generic) add_library(mppi_controller SHARED src/controller.cpp From 7327cbc021093e7e2ae114546d5008eb9c7c34f2 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Wed, 4 Oct 2023 15:58:30 -0700 Subject: [PATCH 18/39] Bumping to 1.1.12 for binary release of AVX512 patches --- nav2_amcl/package.xml | 2 +- nav2_behavior_tree/package.xml | 2 +- nav2_behaviors/package.xml | 2 +- nav2_bringup/package.xml | 2 +- nav2_bt_navigator/package.xml | 2 +- nav2_collision_monitor/package.xml | 2 +- nav2_common/package.xml | 2 +- nav2_constrained_smoother/package.xml | 2 +- nav2_controller/package.xml | 2 +- nav2_core/package.xml | 2 +- nav2_costmap_2d/package.xml | 2 +- nav2_dwb_controller/costmap_queue/package.xml | 2 +- nav2_dwb_controller/dwb_core/package.xml | 2 +- nav2_dwb_controller/dwb_critics/package.xml | 2 +- nav2_dwb_controller/dwb_msgs/package.xml | 2 +- nav2_dwb_controller/dwb_plugins/package.xml | 2 +- nav2_dwb_controller/nav2_dwb_controller/package.xml | 2 +- nav2_dwb_controller/nav_2d_msgs/package.xml | 2 +- nav2_dwb_controller/nav_2d_utils/package.xml | 2 +- nav2_lifecycle_manager/package.xml | 2 +- nav2_map_server/package.xml | 2 +- nav2_mppi_controller/package.xml | 2 +- nav2_msgs/package.xml | 2 +- nav2_navfn_planner/package.xml | 2 +- nav2_planner/package.xml | 2 +- nav2_regulated_pure_pursuit_controller/package.xml | 2 +- nav2_rotation_shim_controller/package.xml | 2 +- nav2_rviz_plugins/package.xml | 2 +- nav2_simple_commander/package.xml | 2 +- nav2_smac_planner/package.xml | 2 +- nav2_smoother/package.xml | 2 +- nav2_system_tests/package.xml | 2 +- nav2_theta_star_planner/package.xml | 2 +- nav2_util/package.xml | 2 +- nav2_velocity_smoother/package.xml | 2 +- nav2_voxel_grid/package.xml | 2 +- nav2_waypoint_follower/package.xml | 2 +- navigation2/package.xml | 2 +- 38 files changed, 38 insertions(+), 38 deletions(-) diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index 374808e1fa8..5725a6e6345 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 1.1.11 + 1.1.12

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index 4fa59657f60..c841446540e 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.1.11 + 1.1.12 TODO Michael Jeronimo Carlos Orduno diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml index 5759ebd778f..898573b9a3b 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -2,7 +2,7 @@ nav2_behaviors - 1.1.11 + 1.1.12 TODO Carlos Orduno Steve Macenski diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index 6a1d02cba98..559608e61f6 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.1.11 + 1.1.12 Bringup scripts and configurations for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index 74a560213be..4f89b94fe56 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.1.11 + 1.1.12 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index ee85e2874e9..0672683c27e 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -2,7 +2,7 @@ nav2_collision_monitor - 1.1.11 + 1.1.12 Collision Monitor Alexey Merzlyakov Steve Macenski diff --git a/nav2_common/package.xml b/nav2_common/package.xml index 183a0053526..42b183c6e98 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.1.11 + 1.1.12 Common support functionality used throughout the navigation 2 stack Carl Delsey Apache-2.0 diff --git a/nav2_constrained_smoother/package.xml b/nav2_constrained_smoother/package.xml index 9ebc521f0cc..b1879c57434 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -2,7 +2,7 @@ nav2_constrained_smoother - 1.1.11 + 1.1.12 Ceres constrained smoother Matej Vargovcik Steve Macenski diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index bde756e13fd..8c1208f9ccc 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.1.11 + 1.1.12 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_core/package.xml b/nav2_core/package.xml index e1f9b56c0c7..e88f728efeb 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.1.11 + 1.1.12 A set of headers for plugins core to the Nav2 stack Steve Macenski Carl Delsey diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index 76c253e0a94..d957ed8550d 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 1.1.11 + 1.1.12 This package provides an implementation of a 2D costmap that takes in sensor data from the world, builds a 2D or 3D occupancy grid of the data (depending diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index 1a52177c500..0e99b4609ce 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 1.1.11 + 1.1.12 The costmap_queue package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml index 16cb8ca025e..3bb4035cdb6 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 1.1.11 + 1.1.12 TODO Carl Delsey BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index 0e7b5322330..a8ffe5a974c 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 1.1.11 + 1.1.12 The dwb_critics package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml index 7450fd46576..1be7a8306ff 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 1.1.11 + 1.1.12 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index 2d5af1aedb9..406a8443df0 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 1.1.11 + 1.1.12 Standard implementations of the GoalChecker and TrajectoryGenerators for dwb_core diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index 12c79f3d986..627be612b62 100644 --- a/nav2_dwb_controller/nav2_dwb_controller/package.xml +++ b/nav2_dwb_controller/nav2_dwb_controller/package.xml @@ -2,7 +2,7 @@ nav2_dwb_controller - 1.1.11 + 1.1.12 ROS2 controller (DWB) metapackage diff --git a/nav2_dwb_controller/nav_2d_msgs/package.xml b/nav2_dwb_controller/nav_2d_msgs/package.xml index 454dfbdeaed..0f253c4ce0c 100644 --- a/nav2_dwb_controller/nav_2d_msgs/package.xml +++ b/nav2_dwb_controller/nav_2d_msgs/package.xml @@ -2,7 +2,7 @@ nav_2d_msgs - 1.1.11 + 1.1.12 Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D. David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml index a8074a1fa2f..e785710e8dc 100644 --- a/nav2_dwb_controller/nav_2d_utils/package.xml +++ b/nav2_dwb_controller/nav_2d_utils/package.xml @@ -2,7 +2,7 @@ nav_2d_utils - 1.1.11 + 1.1.12 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index b45c6b41150..0848de20348 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 1.1.11 + 1.1.12 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index 9d203b1fe8c..8ba7ad0271a 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 1.1.11 + 1.1.12 Refactored map server for ROS2 Navigation diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index c8e626f22b9..db61bacd283 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -2,7 +2,7 @@ nav2_mppi_controller - 1.1.11 + 1.1.12 nav2_mppi_controller Aleksei Budyakov Steve Macenski diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index 914ab4d7efb..49ec7f14bd8 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 1.1.11 + 1.1.12 Messages and service files for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index a2768f47e2f..82398167361 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 1.1.11 + 1.1.12 TODO Steve Macenski Carlos Orduno diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index c7f12173741..58f154da09f 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 1.1.11 + 1.1.12 TODO Steve Macenski Apache-2.0 diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index c92ad15d9b3..f095b39a259 100644 --- a/nav2_regulated_pure_pursuit_controller/package.xml +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -2,7 +2,7 @@ nav2_regulated_pure_pursuit_controller - 1.1.11 + 1.1.12 Regulated Pure Pursuit Controller Steve Macenski Shrijit Singh diff --git a/nav2_rotation_shim_controller/package.xml b/nav2_rotation_shim_controller/package.xml index 2f278e19606..ff3f2bbf91a 100644 --- a/nav2_rotation_shim_controller/package.xml +++ b/nav2_rotation_shim_controller/package.xml @@ -2,7 +2,7 @@ nav2_rotation_shim_controller - 1.1.11 + 1.1.12 Rotation Shim Controller Steve Macenski Apache-2.0 diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index ccc8327cf7e..ac4f352e347 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 1.1.11 + 1.1.12 Navigation 2 plugins for rviz Michael Jeronimo Apache-2.0 diff --git a/nav2_simple_commander/package.xml b/nav2_simple_commander/package.xml index a83c8a83a55..67ca36792d8 100644 --- a/nav2_simple_commander/package.xml +++ b/nav2_simple_commander/package.xml @@ -2,7 +2,7 @@ nav2_simple_commander - 1.1.11 + 1.1.12 An importable library for writing mobile robot applications in python3 steve Apache-2.0 diff --git a/nav2_smac_planner/package.xml b/nav2_smac_planner/package.xml index 07c0e29b4c8..56d4f3fa6da 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -2,7 +2,7 @@ nav2_smac_planner - 1.1.11 + 1.1.12 Smac global planning plugin: A*, Hybrid-A*, State Lattice Steve Macenski Apache-2.0 diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml index 2e2b7393073..59afdb92999 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -2,7 +2,7 @@ nav2_smoother - 1.1.11 + 1.1.12 Smoother action interface Matej Vargovcik Steve Macenski diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 8cacb4bdbe9..e083dbd9597 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 1.1.11 + 1.1.12 TODO Carlos Orduno Apache-2.0 diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index 57a0ff03b32..f750f4fe3b0 100644 --- a/nav2_theta_star_planner/package.xml +++ b/nav2_theta_star_planner/package.xml @@ -2,7 +2,7 @@ nav2_theta_star_planner - 1.1.11 + 1.1.12 Theta* Global Planning Plugin Steve Macenski Anshumaan Singh diff --git a/nav2_util/package.xml b/nav2_util/package.xml index e38f1c51941..9d42dee49a9 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 1.1.11 + 1.1.12 TODO Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_velocity_smoother/package.xml b/nav2_velocity_smoother/package.xml index c0dcd97098e..9fe27742d59 100644 --- a/nav2_velocity_smoother/package.xml +++ b/nav2_velocity_smoother/package.xml @@ -2,7 +2,7 @@ nav2_velocity_smoother - 1.1.11 + 1.1.12 Nav2's Output velocity smoother Steve Macenski Apache-2.0 diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index f9722635386..eecbeaa500b 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 1.1.11 + 1.1.12 voxel_grid provides an implementation of an efficient 3D voxel grid. The occupancy grid can support 3 different representations for the state of a cell: marked, free, or unknown. Due to the underlying implementation relying on bitwise and and or integer operations, the voxel grid only supports 16 different levels per voxel column. However, this limitation yields raytracing and cell marking performance in the grid comparable to standard 2D structures making it quite fast compared to most 3D structures. diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index 5189731d2a9..90c74bf7b63 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 1.1.11 + 1.1.12 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/navigation2/package.xml b/navigation2/package.xml index cdb046ed025..8494a40f057 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 1.1.11 + 1.1.12 ROS2 Navigation Stack From 1b97e3efb6c2b81f3e997474e5b839930730720b Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 10 Oct 2023 19:40:48 -0700 Subject: [PATCH 19/39] [MPPI Optimization] adding regenerate noise param + adding docs (#3868) (#3870) * adding regenerate noise param + adding docs * fix tests * remove unnecessary normalization * Update optimizer.cpp (cherry picked from commit 924f167382080f3ccdd000ffc34b921cb64bcf95) # Conflicts: # nav2_mppi_controller/README.md Co-authored-by: Steve Macenski --- nav2_mppi_controller/CMakeLists.txt | 5 +++- nav2_mppi_controller/README.md | 7 ++++++ .../tools/noise_generator.hpp | 13 +++++++---- .../src/critics/goal_angle_critic.cpp | 6 +---- .../src/critics/goal_critic.cpp | 6 +---- .../src/critics/obstacles_critic.cpp | 2 +- .../src/critics/prefer_forward_critic.cpp | 9 +++----- .../src/critics/twirling_critic.cpp | 8 +++---- nav2_mppi_controller/src/noise_generator.cpp | 23 +++++++++++++++---- nav2_mppi_controller/src/optimizer.cpp | 12 +++++----- .../test/noise_generator_test.cpp | 12 ++++++++-- 11 files changed, 64 insertions(+), 39 deletions(-) diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index 5f0c5359dbe..af64e1b1ed7 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -6,7 +6,10 @@ add_definitions(-DXTENSOR_USE_XSIMD) set(XTENSOR_USE_TBB 0) set(XTENSOR_USE_OPENMP 0) +set(XTENSOR_USE_XSIMD 1) +# set(XTENSOR_DEFAULT_LAYOUT column_major) # row_major, column_major +# set(XTENSOR_DEFAULT_TRAVERSAL row_major) # row_major, column_major find_package(ament_cmake REQUIRED) find_package(xtensor REQUIRED) @@ -86,7 +89,7 @@ set(libraries mppi_controller mppi_critics) foreach(lib IN LISTS libraries) target_compile_options(${lib} PUBLIC -fconcepts) - target_include_directories(${lib} PUBLIC include ${xsimd_INCLUDE_DIRS} ${OpenMP_INCLUDE_DIRS}) + target_include_directories(${lib} PUBLIC include ${xsimd_INCLUDE_DIRS}) # ${OpenMP_INCLUDE_DIRS} target_link_libraries(${lib} xtensor xtensor::optimize xtensor::use_xsimd) ament_target_dependencies(${lib} ${dependencies_pkgs}) endforeach() diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 951ffb15624..0bb6bb6caa7 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -29,6 +29,7 @@ This process is then repeated a number of times and returns a converged solution - Includes fallback mechanisms to handle soft-failures before escalating to recovery behaviors - High-quality code implementation with Doxygen, high unit test coverage, documentation, and parameter guide - Easily extensible to support modern research variants of MPPI +- Comes pre-tuned for good out-of-the-box behavior ## Configuration @@ -52,7 +53,11 @@ This process is then repeated a number of times and returns a converged solution | gamma | double | Default: 0.015. A trade-off between smoothness (high) and low energy (low). This is a complex parameter that likely won't need to be changed from the default of `0.1` which works well for a broad range of cases. See Section 3D-2 in "Information Theoretic Model Predictive Control: Theory and Applications to Autonomous Driving" for detailed information. | | visualize | bool | Default: false. Publish visualization of trajectories, which can slow down the controller significantly. Use only for debugging. | | retry_attempt_limit | int | Default 1. Number of attempts to find feasible trajectory on failure for soft-resets before reporting failure. | +<<<<<<< HEAD | reset_period | double | Default 1.0. required time of inactivity to reset optimizer (only in Humble due to backport ABI policies) | +======= + | regenerate_noises | bool | Default false. Whether to regenerate noises each iteration or use single noise distribution computed on initialization and reset. Practically, this is found to work fine since the trajectories are being sampled stochastically from a normal distribution and reduces compute jittering at run-time due to thread wake-ups to resample normal distribution. | +>>>>>>> 924f1673 ([MPPI Optimization] adding regenerate noise param + adding docs (#3868)) #### Trajectory Visualizer | Parameter | Type | Definition | @@ -258,6 +263,8 @@ The most common parameters you might want to start off changing are the velocity If you don't require path following behavior (e.g. just want to follow a goal pose and let the model predictive elements decide the best way to accomplish that), you may easily remove the PathAlign, PathFollow and PathAngle critics. +By default, the controller is tuned and has the capabilities established in the PathAlign/Obstacle critics to generally follow the path closely when no obstacles prevent it, but able to deviate from the path when blocked. See `PathAlignCritic::score()` for details, but it is disabled when the local path is blocked so the obstacle critic takes over in that state. + ### Prediction Horizon, Costmap Sizing, and Offsets As this is a predictive planner, there is some relationship between maximum speed, prediction times, and costmap size that users should keep in mind while tuning for their application. If a controller server costmap is set to 3.0m in size, that means that with the robot in the center, there is 1.5m of information on either side of the robot. When your prediction horizon (time_steps * model_dt) at maximum speed (vx_max) is larger than this, then your robot will be artificially limited in its maximum speeds and behavior by the costmap limitation. For example, if you predict forward 3 seconds (60 steps @ 0.05s per step) at 0.5m/s maximum speed, the **minimum** required costmap radius is 1.5m - or 3m total width. diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp index 6eb1b36384c..a811d998e41 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp @@ -25,8 +25,9 @@ #include #include "nav2_mppi_controller/models/optimizer_settings.hpp" -#include -#include +#include "nav2_mppi_controller/tools/parameters_handler.hpp" +#include "nav2_mppi_controller/models/control_sequence.hpp" +#include "nav2_mppi_controller/models/state.hpp" namespace mppi { @@ -47,8 +48,12 @@ class NoiseGenerator * @brief Initialize noise generator with settings and model types * @param settings Settings of controller * @param is_holonomic If base is holonomic + * @param name Namespace for configs + * @param param_handler Get parameters util */ - void initialize(mppi::models::OptimizerSettings & settings, bool is_holonomic); + void initialize( + mppi::models::OptimizerSettings & settings, + bool is_holonomic, const std::string & name, ParametersHandler * param_handler); /** * @brief Shutdown noise generator thread @@ -99,7 +104,7 @@ class NoiseGenerator std::thread noise_thread_; std::condition_variable noise_cond_; std::mutex noise_lock_; - bool active_{false}, ready_{false}; + bool active_{false}, ready_{false}, regenerate_noises_{false}; }; } // namespace mppi diff --git a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp index 62bfdf0676d..604ee24eb1d 100644 --- a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp @@ -35,11 +35,7 @@ void GoalAngleCritic::initialize() void GoalAngleCritic::score(CriticData & data) { - if (!enabled_) { - return; - } - - if (!utils::withinPositionGoalTolerance( + if (!enabled_ || !utils::withinPositionGoalTolerance( threshold_to_consider_, data.state.pose.pose, data.path)) { return; diff --git a/nav2_mppi_controller/src/critics/goal_critic.cpp b/nav2_mppi_controller/src/critics/goal_critic.cpp index 852db11529d..f61d6fd2b21 100644 --- a/nav2_mppi_controller/src/critics/goal_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_critic.cpp @@ -35,11 +35,7 @@ void GoalCritic::initialize() void GoalCritic::score(CriticData & data) { - if (!enabled_) { - return; - } - - if (!utils::withinPositionGoalTolerance( + if (!enabled_ || !utils::withinPositionGoalTolerance( threshold_to_consider_, data.state.pose.pose, data.path)) { return; diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index 9c80134f49d..bbca7f7fc71 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -153,7 +153,7 @@ void ObstaclesCritic::score(CriticData & data) } if (!trajectory_collide) {all_trajectories_collide = false;} - raw_cost[i] = static_cast(trajectory_collide ? collision_cost_ : traj_cost); + raw_cost[i] = trajectory_collide ? collision_cost_ : traj_cost; } data.costs += xt::pow( diff --git a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp index 18b6900177a..08e755dd6af 100644 --- a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp +++ b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp @@ -33,12 +33,9 @@ void PreferForwardCritic::initialize() void PreferForwardCritic::score(CriticData & data) { using xt::evaluation_strategy::immediate; - - if (!enabled_) { - return; - } - - if (utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path)) { + if (!enabled_ || + utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path)) + { return; } diff --git a/nav2_mppi_controller/src/critics/twirling_critic.cpp b/nav2_mppi_controller/src/critics/twirling_critic.cpp index 28eb71b48b6..6b492d13ba0 100644 --- a/nav2_mppi_controller/src/critics/twirling_critic.cpp +++ b/nav2_mppi_controller/src/critics/twirling_critic.cpp @@ -31,11 +31,9 @@ void TwirlingCritic::initialize() void TwirlingCritic::score(CriticData & data) { using xt::evaluation_strategy::immediate; - if (!enabled_) { - return; - } - - if (utils::withinPositionGoalTolerance(data.goal_checker, data.state.pose.pose, data.path)) { + if (!enabled_ || + utils::withinPositionGoalTolerance(data.goal_checker, data.state.pose.pose, data.path)) + { return; } diff --git a/nav2_mppi_controller/src/noise_generator.cpp b/nav2_mppi_controller/src/noise_generator.cpp index 09ee4ab92d7..b7c452aa6a4 100644 --- a/nav2_mppi_controller/src/noise_generator.cpp +++ b/nav2_mppi_controller/src/noise_generator.cpp @@ -23,12 +23,22 @@ namespace mppi { -void NoiseGenerator::initialize(mppi::models::OptimizerSettings & settings, bool is_holonomic) +void NoiseGenerator::initialize( + mppi::models::OptimizerSettings & settings, bool is_holonomic, + const std::string & name, ParametersHandler * param_handler) { settings_ = settings; is_holonomic_ = is_holonomic; active_ = true; - noise_thread_ = std::thread(std::bind(&NoiseGenerator::noiseThread, this)); + + auto getParam = param_handler->getParamGetter(name); + getParam(regenerate_noises_, "regenerate_noises", false); + + if (regenerate_noises_) { + noise_thread_ = std::thread(std::bind(&NoiseGenerator::noiseThread, this)); + } else { + generateNoisedControls(); + } } void NoiseGenerator::shutdown() @@ -44,7 +54,7 @@ void NoiseGenerator::shutdown() void NoiseGenerator::generateNextNoises() { // Trigger the thread to run in parallel to this iteration - // to generate the next iteration's noises. + // to generate the next iteration's noises (if applicable). { std::unique_lock guard(noise_lock_); ready_ = true; @@ -76,7 +86,12 @@ void NoiseGenerator::reset(mppi::models::OptimizerSettings & settings, bool is_h xt::noalias(noises_wz_) = xt::zeros({settings_.batch_size, settings_.time_steps}); ready_ = true; } - noise_cond_.notify_all(); + + if (regenerate_noises_) { + noise_cond_.notify_all(); + } else { + generateNoisedControls(); + } } void NoiseGenerator::noiseThread() diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index a7bbd35002a..fddcc39600a 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -48,7 +49,7 @@ void Optimizer::initialize( getParams(); critic_manager_.on_configure(parent_, name_, costmap_ros_, parameters_handler_); - noise_generator_.initialize(settings_, isHolonomic()); + noise_generator_.initialize(settings_, isHolonomic(), name_, parameters_handler_); reset(); } @@ -267,7 +268,7 @@ void Optimizer::integrateStateVelocities( xt::xtensor & trajectory, const xt::xtensor & sequence) const { - double initial_yaw = tf2::getYaw(state_.pose.pose.orientation); + float initial_yaw = tf2::getYaw(state_.pose.pose.orientation); const auto vx = xt::view(sequence, xt::all(), 0); const auto vy = xt::view(sequence, xt::all(), 2); @@ -277,8 +278,7 @@ void Optimizer::integrateStateVelocities( auto traj_y = xt::view(trajectory, xt::all(), 1); auto traj_yaws = xt::view(trajectory, xt::all(), 2); - xt::noalias(traj_yaws) = - utils::normalize_angles(xt::cumsum(wz * settings_.model_dt, 0) + initial_yaw); + xt::noalias(traj_yaws) = xt::cumsum(wz * settings_.model_dt, 0 + initial_yaw); auto && yaw_cos = xt::xtensor::from_shape(traj_yaws.shape()); auto && yaw_sin = xt::xtensor::from_shape(traj_yaws.shape()); @@ -306,10 +306,10 @@ void Optimizer::integrateStateVelocities( models::Trajectories & trajectories, const models::State & state) const { - const double initial_yaw = tf2::getYaw(state.pose.pose.orientation); + const float initial_yaw = tf2::getYaw(state.pose.pose.orientation); xt::noalias(trajectories.yaws) = - utils::normalize_angles(xt::cumsum(state.wz * settings_.model_dt, 1) + initial_yaw); + xt::cumsum(state.wz * settings_.model_dt, 1) + initial_yaw; const auto yaws_cutted = xt::view(trajectories.yaws, xt::all(), xt::range(0, -1)); diff --git a/nav2_mppi_controller/test/noise_generator_test.cpp b/nav2_mppi_controller/test/noise_generator_test.cpp index 3788f2b8a30..db8667e9404 100644 --- a/nav2_mppi_controller/test/noise_generator_test.cpp +++ b/nav2_mppi_controller/test/noise_generator_test.cpp @@ -42,13 +42,21 @@ TEST(NoiseGeneratorTest, NoiseGeneratorLifecycle) settings.batch_size = 100; settings.time_steps = 25; - generator.initialize(settings, false); + auto node = std::make_shared("node"); + node->declare_parameter("test_name.regenerate_noises", rclcpp::ParameterValue(false)); + ParametersHandler handler(node); + + generator.initialize(settings, false, "test_name", &handler); + generator.reset(settings, false); generator.shutdown(); } TEST(NoiseGeneratorTest, NoiseGeneratorMain) { // Tests shuts down internal thread cleanly + auto node = std::make_shared("node"); + node->declare_parameter("test_name.regenerate_noises", rclcpp::ParameterValue(true)); + ParametersHandler handler(node); NoiseGenerator generator; mppi::models::OptimizerSettings settings; settings.batch_size = 100; @@ -70,7 +78,7 @@ TEST(NoiseGeneratorTest, NoiseGeneratorMain) state.reset(settings.batch_size, settings.time_steps); // Request an update with no noise yet generated, should result in identical outputs - generator.initialize(settings, false); + generator.initialize(settings, false, "test_name", &handler); generator.reset(settings, false); // sets initial sizing and zeros out noises generator.setNoisedControls(state, control_sequence); EXPECT_EQ(state.cvx(0), 0); From 5b87fabadbdb5ad6c1cc56066e77e4de1a8b26df Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 13 Oct 2023 16:39:23 -0700 Subject: [PATCH 20/39] Updating default map path --- tools/planner_benchmarking/planning_benchmark_bringup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/planner_benchmarking/planning_benchmark_bringup.py b/tools/planner_benchmarking/planning_benchmark_bringup.py index e2a251405d3..d6589e89968 100644 --- a/tools/planner_benchmarking/planning_benchmark_bringup.py +++ b/tools/planner_benchmarking/planning_benchmark_bringup.py @@ -23,7 +23,7 @@ def generate_launch_description(): nav2_bringup_dir = get_package_share_directory('nav2_bringup') config = os.path.join(get_package_share_directory('nav2_bringup'), 'params', 'nav2_params.yaml') - map_file = os.path.join(nav2_bringup_dir, 'maps', 'map.yaml') + map_file = os.path.join(nav2_bringup_dir, 'maps', 'turtlebot3_world.yaml') lifecycle_nodes = ['map_server', 'planner_server'] return LaunchDescription([ From 3b791e8aefaa9c4deb0c623c7d4b8ea69ed884fb Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Fri, 13 Oct 2023 18:52:20 -0700 Subject: [PATCH 21/39] [MPPI] Reworked Path Align Critic; 70% faster + Tracks Paths Better! Edit: strike that, now 80% (#3872) (#3882) * adding regenerate noise param + adding docs * fix tests * remove unnecessary normalization * Update optimizer.cpp * adding refactored path alignment critic * fix visualization bug * speed up another 30% * remove a little jitter * a few more small optimizaitons * fixing unit tests * retain legacy critic * adding tests for legacy (cherry picked from commit 7009ffba5f85c50ac97fd0057924b0f1447c5e85) Co-authored-by: Steve Macenski --- nav2_mppi_controller/CMakeLists.txt | 1 + nav2_mppi_controller/LICENSE.md | 1 + nav2_mppi_controller/README.md | 2 + nav2_mppi_controller/critics.xml | 4 + .../critics/path_align_critic.hpp | 2 +- .../critics/path_align_legacy_critic.hpp | 60 ++++++++ .../nav2_mppi_controller/tools/utils.hpp | 27 +++- .../src/critics/path_align_critic.cpp | 84 ++++++----- .../src/critics/path_align_legacy_critic.cpp | 137 ++++++++++++++++++ nav2_mppi_controller/src/optimizer.cpp | 2 +- nav2_mppi_controller/test/critics_tests.cpp | 107 +++++++++++++- 11 files changed, 380 insertions(+), 47 deletions(-) create mode 100644 nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_legacy_critic.hpp create mode 100644 nav2_mppi_controller/src/critics/path_align_legacy_critic.cpp diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index af64e1b1ed7..29dfde96729 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -78,6 +78,7 @@ add_library(mppi_critics SHARED src/critics/goal_critic.cpp src/critics/goal_angle_critic.cpp src/critics/path_align_critic.cpp + src/critics/path_align_legacy_critic.cpp src/critics/path_follow_critic.cpp src/critics/path_angle_critic.cpp src/critics/prefer_forward_critic.cpp diff --git a/nav2_mppi_controller/LICENSE.md b/nav2_mppi_controller/LICENSE.md index da24c0064fc..57a5719427a 100644 --- a/nav2_mppi_controller/LICENSE.md +++ b/nav2_mppi_controller/LICENSE.md @@ -2,6 +2,7 @@ MIT License Copyright (c) 2021-2022 Fast Sense Studio Copyright (c) 2022-2023 Samsung Research America +Copyright (c) 2023 Open Navigation LLC Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 0bb6bb6caa7..d28cbb5672a 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -125,6 +125,8 @@ This process is then repeated a number of times and returns a converged solution | max_path_occupancy_ratio | double | Default 0.07 (7%). Maximum proportion of the path that can be occupied before this critic is not considered to allow the obstacle and path follow critics to avoid obstacles while following the path's intent in presence of dynamic objects in the scene. | | use_path_orientations | bool | Default false. Whether to consider path's orientations in path alignment, which can be useful when paired with feasible smac planners to incentivize directional changes only where/when the smac planner requests them. If you want the robot to deviate and invert directions where the controller sees fit, keep as false. If your plans do not contain orientation information (e.g. navfn), keep as false. | +Note: There is a "Legacy" version of this critic also available with the same parameters of an old formulation pre-October 2023. + #### Path Angle Critic | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | diff --git a/nav2_mppi_controller/critics.xml b/nav2_mppi_controller/critics.xml index 669944f2fe7..790568e7967 100644 --- a/nav2_mppi_controller/critics.xml +++ b/nav2_mppi_controller/critics.xml @@ -17,6 +17,10 @@ mppi critic for aligning to path + + mppi critic for aligning to path (legacy) + + mppi critic for tracking the path in the correct heading diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp index a5499955575..f7ad2fda6a9 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_legacy_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_legacy_critic.hpp new file mode 100644 index 00000000000..65d9d0d61e7 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_legacy_critic.hpp @@ -0,0 +1,60 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_MPPI_CONTROLLER__CRITICS__PATH_ALIGN_LEGACY_CRITIC_HPP_ +#define NAV2_MPPI_CONTROLLER__CRITICS__PATH_ALIGN_LEGACY_CRITIC_HPP_ + +#include "nav2_mppi_controller/critic_function.hpp" +#include "nav2_mppi_controller/models/state.hpp" +#include "nav2_mppi_controller/tools/utils.hpp" + +namespace mppi::critics +{ + +/** + * @class mppi::critics::PathAlignLegacyCritic + * @brief Critic objective function for aligning to the path. Note: + * High settings of this will follow the path more precisely, but also makes it + * difficult (or impossible) to deviate in the presence of dynamic obstacles. + * This is an important critic to tune and consider in tandem with Obstacle. + * This is the initial 'Legacy' implementation before replacement Oct 2023. + */ +class PathAlignLegacyCritic : public CriticFunction +{ +public: + /** + * @brief Initialize critic + */ + void initialize() override; + + /** + * @brief Evaluate cost related to trajectories path alignment + * + * @param costs [out] add reference cost values to this tensor + */ + void score(CriticData & data) override; + +protected: + size_t offset_from_furthest_{0}; + int trajectory_point_step_{0}; + float threshold_to_consider_{0}; + float max_path_occupancy_ratio_{0}; + bool use_path_orientations_{false}; + unsigned int power_{0}; + float weight_{0}; +}; + +} // namespace mppi::critics + +#endif // NAV2_MPPI_CONTROLLER__CRITICS__PATH_ALIGN_LEGACY_CRITIC_HPP_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index b21b1cf8b3e..ab860428113 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -308,14 +308,16 @@ inline size_t findPathFurthestReachedPoint(const CriticData & data) const auto dists = dx * dx + dy * dy; - size_t max_id_by_trajectories = 0; + size_t max_id_by_trajectories = 0, min_id_by_path = 0; float min_distance_by_path = std::numeric_limits::max(); + float cur_dist = 0.0f; for (size_t i = 0; i < dists.shape(0); i++) { - size_t min_id_by_path = 0; + min_id_by_path = 0; for (size_t j = 0; j < dists.shape(1); j++) { - if (dists(i, j) < min_distance_by_path) { - min_distance_by_path = dists(i, j); + cur_dist = dists(i, j); + if (cur_dist < min_distance_by_path) { + min_distance_by_path = cur_dist; min_id_by_path = j; } } @@ -663,6 +665,23 @@ inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path) return first_after_inversion; } +/** + * @brief Compare to trajectory points to find closest path point along integrated distances + * @param vec Vect to check + * @return dist Distance to look for + */ +inline size_t findClosestPathPt(const std::vector & vec, float dist, size_t init = 0) +{ + auto iter = std::lower_bound(vec.begin() + init, vec.end(), dist); + if (iter == vec.begin()) { + return 0; + } + if (dist - *(iter - 1) < *iter - dist) { + return iter - 1 - vec.begin(); + } + return iter - vec.begin(); +} + } // namespace mppi::utils #endif // NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_ diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index 4ff12d4e553..4b4d93abf3f 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -54,7 +54,8 @@ void PathAlignCritic::score(CriticData & data) // Don't apply when first getting bearing w.r.t. the path utils::setPathFurthestPointIfNotSet(data); - if (*data.furthest_reached_path_point < offset_from_furthest_) { + const size_t path_segments_count = *data.furthest_reached_path_point; // up to furthest only + if (path_segments_count < offset_from_furthest_) { return; } @@ -70,59 +71,62 @@ void PathAlignCritic::score(CriticData & data) } } - const auto & T_x = data.trajectories.x; - const auto & T_y = data.trajectories.y; - const auto & T_yaw = data.trajectories.yaws; - const auto P_x = xt::view(data.path.x, xt::range(_, -1)); // path points const auto P_y = xt::view(data.path.y, xt::range(_, -1)); // path points const auto P_yaw = xt::view(data.path.yaws, xt::range(_, -1)); // path points - const size_t batch_size = T_x.shape(0); - const size_t time_steps = T_x.shape(1); - const size_t traj_pts_eval = floor(time_steps / trajectory_point_step_); - const size_t path_segments_count = data.path.x.shape(0) - 1; + const size_t batch_size = data.trajectories.x.shape(0); + const size_t time_steps = data.trajectories.x.shape(1); auto && cost = xt::xtensor::from_shape({data.costs.shape(0)}); - if (path_segments_count < 1) { - return; + // Find integrated distance in the path + std::vector path_integrated_distances(path_segments_count, 0.0f); + float dx = 0.0f, dy = 0.0f; + for (unsigned int i = 1; i != path_segments_count; i++) { + dx = P_x(i) - P_x(i - 1); + dy = P_y(i) - P_y(i - 1); + float curr_dist = sqrtf(dx * dx + dy * dy); + path_integrated_distances[i] = path_integrated_distances[i - 1] + curr_dist; } - float dist_sq = 0.0f, dx = 0.0f, dy = 0.0f, dyaw = 0.0f, summed_dist = 0.0f; - float min_dist_sq = std::numeric_limits::max(); - size_t min_s = 0; - + float traj_integrated_distance = 0.0f; + float summed_path_dist = 0.0f, dyaw = 0.0f; + float num_samples = 0.0f; + float Tx = 0.0f, Ty = 0.0f; + size_t path_pt = 0; for (size_t t = 0; t < batch_size; ++t) { - summed_dist = 0.0f; + traj_integrated_distance = 0.0f; + summed_path_dist = 0.0f; + num_samples = 0.0f; + const auto T_x = xt::view(data.trajectories.x, t, xt::all()); + const auto T_y = xt::view(data.trajectories.y, t, xt::all()); for (size_t p = trajectory_point_step_; p < time_steps; p += trajectory_point_step_) { - min_dist_sq = std::numeric_limits::max(); - min_s = 0; - - // Find closest path segment to the trajectory point - for (size_t s = 0; s < path_segments_count - 1; s++) { - xt::xtensor_fixed> P; - dx = P_x(s) - T_x(t, p); - dy = P_y(s) - T_y(t, p); - if (use_path_orientations_) { - dyaw = angles::shortest_angular_distance(P_yaw(s), T_yaw(t, p)); - dist_sq = dx * dx + dy * dy + dyaw * dyaw; - } else { - dist_sq = dx * dx + dy * dy; - } - if (dist_sq < min_dist_sq) { - min_dist_sq = dist_sq; - min_s = s; - } - } + Tx = T_x(p); + Ty = T_y(p); + dx = Tx - T_x(p - trajectory_point_step_); + dy = Ty - T_y(p - trajectory_point_step_); + traj_integrated_distance += sqrtf(dx * dx + dy * dy); + path_pt = utils::findClosestPathPt( + path_integrated_distances, traj_integrated_distance, path_pt); // The nearest path point to align to needs to be not in collision, else // let the obstacle critic take over in this region due to dynamic obstacles - if (min_s != 0 && (*data.path_pts_valid)[min_s]) { - summed_dist += sqrtf(min_dist_sq); + if ((*data.path_pts_valid)[path_pt]) { + dx = P_x(path_pt) - Tx; + dy = P_y(path_pt) - Ty; + num_samples += 1.0f; + if (use_path_orientations_) { + const auto T_yaw = xt::view(data.trajectories.yaws, t, xt::all()); + dyaw = angles::shortest_angular_distance(P_yaw(path_pt), T_yaw(p)); + summed_path_dist += sqrtf(dx * dx + dy * dy + dyaw * dyaw); + } else { + summed_path_dist += sqrtf(dx * dx + dy * dy); + } } } - - cost[t] = summed_dist / traj_pts_eval; + if (num_samples > 0) { + cost[t] = summed_path_dist / num_samples; + } } data.costs += xt::pow(std::move(cost) * weight_, power_); diff --git a/nav2_mppi_controller/src/critics/path_align_legacy_critic.cpp b/nav2_mppi_controller/src/critics/path_align_legacy_critic.cpp new file mode 100644 index 00000000000..a04f4a9fa95 --- /dev/null +++ b/nav2_mppi_controller/src/critics/path_align_legacy_critic.cpp @@ -0,0 +1,137 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "nav2_mppi_controller/critics/path_align_legacy_critic.hpp" + +#include +#include + +namespace mppi::critics +{ + +using namespace xt::placeholders; // NOLINT +using xt::evaluation_strategy::immediate; + +void PathAlignLegacyCritic::initialize() +{ + auto getParam = parameters_handler_->getParamGetter(name_); + getParam(power_, "cost_power", 1); + getParam(weight_, "cost_weight", 10.0); + + getParam(max_path_occupancy_ratio_, "max_path_occupancy_ratio", 0.07); + getParam(offset_from_furthest_, "offset_from_furthest", 20); + getParam(trajectory_point_step_, "trajectory_point_step", 4); + getParam( + threshold_to_consider_, + "threshold_to_consider", 0.5); + getParam(use_path_orientations_, "use_path_orientations", false); + + RCLCPP_INFO( + logger_, + "ReferenceTrajectoryCritic instantiated with %d power and %f weight", + power_, weight_); +} + +void PathAlignLegacyCritic::score(CriticData & data) +{ + // Don't apply close to goal, let the goal critics take over + if (!enabled_ || + utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path)) + { + return; + } + + // Don't apply when first getting bearing w.r.t. the path + utils::setPathFurthestPointIfNotSet(data); + if (*data.furthest_reached_path_point < offset_from_furthest_) { + return; + } + + // Don't apply when dynamic obstacles are blocking significant proportions of the local path + utils::setPathCostsIfNotSet(data, costmap_ros_); + const size_t closest_initial_path_point = utils::findPathTrajectoryInitialPoint(data); + unsigned int invalid_ctr = 0; + const float range = *data.furthest_reached_path_point - closest_initial_path_point; + for (size_t i = closest_initial_path_point; i < *data.furthest_reached_path_point; i++) { + if (!(*data.path_pts_valid)[i]) {invalid_ctr++;} + if (static_cast(invalid_ctr) / range > max_path_occupancy_ratio_ && invalid_ctr > 2) { + return; + } + } + + const auto & T_x = data.trajectories.x; + const auto & T_y = data.trajectories.y; + const auto & T_yaw = data.trajectories.yaws; + + const auto P_x = xt::view(data.path.x, xt::range(_, -1)); // path points + const auto P_y = xt::view(data.path.y, xt::range(_, -1)); // path points + const auto P_yaw = xt::view(data.path.yaws, xt::range(_, -1)); // path points + + const size_t batch_size = T_x.shape(0); + const size_t time_steps = T_x.shape(1); + const size_t traj_pts_eval = floor(time_steps / trajectory_point_step_); + const size_t path_segments_count = data.path.x.shape(0) - 1; + auto && cost = xt::xtensor::from_shape({data.costs.shape(0)}); + + if (path_segments_count < 1) { + return; + } + + float dist_sq = 0.0f, dx = 0.0f, dy = 0.0f, dyaw = 0.0f, summed_dist = 0.0f; + float min_dist_sq = std::numeric_limits::max(); + size_t min_s = 0; + + for (size_t t = 0; t < batch_size; ++t) { + summed_dist = 0.0f; + for (size_t p = trajectory_point_step_; p < time_steps; p += trajectory_point_step_) { + min_dist_sq = std::numeric_limits::max(); + min_s = 0; + + // Find closest path segment to the trajectory point + for (size_t s = 0; s < path_segments_count - 1; s++) { + xt::xtensor_fixed> P; + dx = P_x(s) - T_x(t, p); + dy = P_y(s) - T_y(t, p); + if (use_path_orientations_) { + dyaw = angles::shortest_angular_distance(P_yaw(s), T_yaw(t, p)); + dist_sq = dx * dx + dy * dy + dyaw * dyaw; + } else { + dist_sq = dx * dx + dy * dy; + } + if (dist_sq < min_dist_sq) { + min_dist_sq = dist_sq; + min_s = s; + } + } + + // The nearest path point to align to needs to be not in collision, else + // let the obstacle critic take over in this region due to dynamic obstacles + if (min_s != 0 && (*data.path_pts_valid)[min_s]) { + summed_dist += sqrtf(min_dist_sq); + } + } + + cost[t] = summed_dist / traj_pts_eval; + } + + data.costs += xt::pow(std::move(cost) * weight_, power_); +} + +} // namespace mppi::critics + +#include + +PLUGINLIB_EXPORT_CLASS( + mppi::critics::PathAlignLegacyCritic, + mppi::critics::CriticFunction) diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index fddcc39600a..fc7fc7e0bfd 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -278,7 +278,7 @@ void Optimizer::integrateStateVelocities( auto traj_y = xt::view(trajectory, xt::all(), 1); auto traj_yaws = xt::view(trajectory, xt::all(), 2); - xt::noalias(traj_yaws) = xt::cumsum(wz * settings_.model_dt, 0 + initial_yaw); + xt::noalias(traj_yaws) = xt::cumsum(wz * settings_.model_dt, 0) + initial_yaw; auto && yaw_cos = xt::xtensor::from_shape(traj_yaws.shape()); auto && yaw_sin = xt::xtensor::from_shape(traj_yaws.shape()); diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index d0f7a1bfe8e..42208e56787 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -24,6 +24,7 @@ #include "nav2_mppi_controller/critics/goal_critic.hpp" #include "nav2_mppi_controller/critics/obstacles_critic.hpp" #include "nav2_mppi_controller/critics/path_align_critic.hpp" +#include "nav2_mppi_controller/critics/path_align_legacy_critic.hpp" #include "nav2_mppi_controller/critics/path_angle_critic.hpp" #include "nav2_mppi_controller/critics/path_follow_critic.hpp" #include "nav2_mppi_controller/critics/prefer_forward_critic.hpp" @@ -513,7 +514,111 @@ TEST(CriticTests, PathAlignCritic) path.x(21) = 0.9; generated_trajectories.x = 0.66 * xt::ones({1000, 30}); critic.score(data); - // 0.04 * 1000 * 10 weight * 4 num pts eval / 4 normalization term + // 0.66 * 1000 * 10 weight * 6 num pts eval / 6 normalization term + EXPECT_NEAR(xt::sum(costs, immediate)(), 6600.0, 1e-2); + + // provide state pose and path far enough to enable, with data to pass condition + // but path is blocked in collision + auto * costmap = costmap_ros->getCostmap(); + // island in the middle of lethal cost to cross. Costmap defaults to size 5x5 @ 10cm resolution + for (unsigned int i = 11; i <= 30; ++i) { // 1.1m-3m + for (unsigned int j = 11; j <= 30; ++j) { // 1.1m-3m + costmap->setCost(i, j, 254); + } + } + + data.path_pts_valid.reset(); // Recompute on new path + costs = xt::zeros({1000}); + path.x = 1.5 * xt::ones({22}); + path.y = 1.5 * xt::ones({22}); + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); +} + +TEST(CriticTests, PathAlignLegacyCritic) +{ + // Standard preamble + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + ParametersHandler param_handler(node); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + + models::State state; + state.reset(1000, 30); + models::ControlSequence control_sequence; + models::Trajectories generated_trajectories; + generated_trajectories.reset(1000, 30); + models::Path path; + xt::xtensor costs = xt::zeros({1000}); + float model_dt = 0.1; + CriticData data = + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, + std::nullopt}; + data.motion_model = std::make_shared(); + TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally + data.goal_checker = &goal_checker; + + // Initialization testing + + // Make sure initializes correctly + PathAlignLegacyCritic critic; + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); + EXPECT_EQ(critic.getName(), "critic"); + + // Scoring testing + + // provide state poses and path close within positional tolerances + state.pose.pose.position.x = 1.0; + path.reset(10); + path.x(9) = 0.85; + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + + // provide state pose and path far enough to enable + // but data furthest point reached is 0 and offset default is 20, so returns + path.x(9) = 0.15; + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + + // provide state pose and path far enough to enable, with data to pass condition + // but with empty trajectories and paths, should still be zero + *data.furthest_reached_path_point = 21; + path.x(9) = 0.15; + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + + // provide state pose and path far enough to enable, with data to pass condition + // and with a valid path to pass invalid path condition + state.pose.pose.position.x = 0.0; + data.path_pts_valid.reset(); // Recompute on new path + path.reset(22); + path.x(0) = 0; + path.x(1) = 0.1; + path.x(2) = 0.2; + path.x(3) = 0.3; + path.x(4) = 0.4; + path.x(5) = 0.5; + path.x(6) = 0.6; + path.x(7) = 0.7; + path.x(8) = 0.8; + path.x(9) = 0.9; + path.x(10) = 0.9; + path.x(11) = 0.9; + path.x(12) = 0.9; + path.x(13) = 0.9; + path.x(14) = 0.9; + path.x(15) = 0.9; + path.x(16) = 0.9; + path.x(17) = 0.9; + path.x(18) = 0.9; + path.x(19) = 0.9; + path.x(20) = 0.9; + path.x(21) = 0.9; + generated_trajectories.x = 0.66 * xt::ones({1000, 30}); + critic.score(data); + // 0.04 * 1000 * 10 weight * 6 num pts eval / 6 normalization term EXPECT_NEAR(xt::sum(costs, immediate)(), 400.0, 1e-2); // provide state pose and path far enough to enable, with data to pass condition From 57f55c4dca45d93b61adbf7bcd24ad83106192bd Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 16 Oct 2023 15:03:33 -0700 Subject: [PATCH 22/39] Fix incorrect auto merge conflict issue --- nav2_mppi_controller/test/critics_tests.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index 42208e56787..fd05ab4bee6 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -540,7 +540,7 @@ TEST(CriticTests, PathAlignLegacyCritic) // Standard preamble auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( - "dummy_costmap", "", "dummy_costmap", true); + "dummy_costmap", "", "dummy_costmap"); ParametersHandler param_handler(node); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); From 67a366a816ddb926ada96e1ad3bc3694a6b4fe0c Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 23 Oct 2023 11:50:16 -0700 Subject: [PATCH 23/39] Use mutex to protect costmap reads. (backport #3877) (#3897) * Use mutex to protect costmap reads. (#3877) * Use mutex to protect costmap reads. Otherwise costmap can be read during a map update and return 0. * Revert "Use mutex to protect costmap reads." This reverts commit e16a44c65ee7064e2271118894b92bb6e24ce28d. * Lock costmap before running MPPI controller. * Fix typo. * Protect against costmap updates in MPP and RotationShim controllers. --------- Co-authored-by: Leif Terry (cherry picked from commit a1c9fd5ad29bb00e40ce6e696d899a2bcd50cde5) # Conflicts: # nav2_mppi_controller/src/controller.cpp * fix merge conflict --------- Co-authored-by: LeifHookedWireless Co-authored-by: Steve Macenski --- nav2_mppi_controller/src/controller.cpp | 5 ++++- .../src/regulated_pure_pursuit_controller.cpp | 3 +++ .../src/nav2_rotation_shim_controller.cpp | 3 +++ 3 files changed, 10 insertions(+), 1 deletion(-) diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index b87373ace87..8a631f97526 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -91,9 +91,12 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( } last_time_called_ = clock_->now(); - std::lock_guard lock(*parameters_handler_->getLock()); + std::lock_guard param_lock(*parameters_handler_->getLock()); nav_msgs::msg::Path transformed_plan = path_handler_.transformPath(robot_pose); + nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); + std::unique_lock costmap_lock(*(costmap->getMutex())); + geometry_msgs::msg::TwistStamped cmd = optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, goal_checker); diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 4b52dd36166..48703cc66dd 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -282,6 +282,9 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity { std::lock_guard lock_reinit(mutex_); + nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); + std::unique_lock lock(*(costmap->getMutex())); + // Update for the current goal checker's state geometry_msgs::msg::Pose pose_tolerance; geometry_msgs::msg::Twist vel_tolerance; diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index bc86900f640..acbf2d1bf71 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -141,6 +141,9 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands nav2_core::GoalChecker * goal_checker) { if (path_updated_) { + nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); + std::unique_lock lock(*(costmap->getMutex())); + std::lock_guard lock_reinit(mutex_); try { geometry_msgs::msg::Pose sampled_pt_base = transformPoseToBaseFrame(getSampledPathPt()); From afe56f7bbf0b7b1a6e80a4e61130c2e65d0d70b1 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 24 Oct 2023 09:31:57 -0700 Subject: [PATCH 24/39] Adjust the Variable types in Nav2_costmap_2d pkg in [nav2_humble] #3891 (#3900) (#3902) * image.hpp #3891 * Update image.hpp (cherry picked from commit 7a7c6da59a923f06f51ca98ad0b0cc412801ad12) Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> --- nav2_costmap_2d/include/nav2_costmap_2d/denoise/image.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image.hpp index db7ae8fce84..888455168d3 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image.hpp @@ -50,7 +50,7 @@ class Image * Share image data between new and old object. * Changing data in a new object will affect the given one and vice versa */ - Image(Image & other); + Image(const Image & other); /** * @brief Create image from the other (move constructor) @@ -132,7 +132,7 @@ Image::Image(size_t rows, size_t columns, T * data, size_t step) } template -Image::Image(Image & other) +Image::Image(const Image & other) : data_start_{other.data_start_}, rows_{other.rows_}, columns_{other.columns_}, step_{other.step_} {} From 6377c72a9469f5170e87e3a0cf2e574da220e755 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Sat, 28 Oct 2023 15:29:05 -0700 Subject: [PATCH 25/39] Log if BT rate is exceeded (#3909) (#3913) (cherry picked from commit a11cdd80665238628be9eef4b439067d4b675b7b) Co-authored-by: Steve Macenski --- nav2_behavior_tree/src/behavior_tree_engine.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index 7bfc003cf1c..a5ee96af5e1 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -55,7 +55,12 @@ BehaviorTreeEngine::run( onLoop(); - loopRate.sleep(); + if (!loopRate.sleep()) { + RCLCPP_WARN( + rclcpp::get_logger("BehaviorTreeEngine"), + "Behavior Tree tick rate %0.2f was exceeded!", + 1.0 / (loopRate.period().count() * 1.0e-9)); + } } } catch (const std::exception & ex) { RCLCPP_ERROR( From 75454d70f42bec3a5533e32b0f0b68ae2e5ea421 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 30 Oct 2023 13:41:26 -0700 Subject: [PATCH 26/39] Update theta_star_planner.cpp (#3918) (#3922) (cherry picked from commit 0629ff36e36ecc135b9b13fc78f213cfe0a361ef) Co-authored-by: Steve Macenski --- nav2_theta_star_planner/src/theta_star_planner.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index 2913b5ef7e5..23d4398db80 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -91,6 +91,8 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan( nav_msgs::msg::Path global_path; auto start_time = std::chrono::steady_clock::now(); + std::unique_lock lock(*(planner_->costmap_->getMutex())); + // Corner case of start and goal beeing on the same cell unsigned int mx_start, my_start, mx_goal, my_goal; planner_->costmap_->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, my_start); From 95b23227d49782ecf8ec445beffcf4995b148fe5 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 30 Oct 2023 13:56:21 -0700 Subject: [PATCH 27/39] Fixing subtree issues with blackboard shared resources (3640) (backport #3911) (#3916) * Fixing subtree issues with blackboard shared resources (3640) (#3911) * fixing subtree issues * Update bt_action_server_impl.hpp (cherry picked from commit 4b4465dfc9427b95e98aef70620bedd933cfbe56) # Conflicts: # nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp * Update bt_action_server_impl.hpp --------- Co-authored-by: Steve Macenski --- .../include/nav2_behavior_tree/bt_action_server_impl.hpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 022f32de590..0fcb46fcf54 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -186,6 +186,11 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filena // Create the Behavior Tree from the XML input try { tree_ = bt_->createTreeFromText(xml_string, blackboard_); + for (auto & blackboard : tree_.blackboard_stack) { + blackboard->set("node", client_node_); + blackboard->set("server_timeout", default_server_timeout_); + blackboard->set("bt_loop_duration", bt_loop_duration_); + } } catch (const std::exception & e) { RCLCPP_ERROR(logger_, "Exception when loading BT: %s", e.what()); return false; From 362540757f79d5fa73bb11829480cf6fa92f24ca Mon Sep 17 00:00:00 2001 From: kevin Date: Wed, 1 Nov 2023 23:40:11 +0900 Subject: [PATCH 28/39] fix build Signed-off-by: kevin --- nav2_mppi_controller/CMakeLists.txt | 4 ++++ nav2_mppi_controller/test/noise_generator_test.cpp | 1 + 2 files changed, 5 insertions(+) diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index 29dfde96729..5df3c248c71 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -31,6 +31,10 @@ set(dependencies_pkgs tf2_ros ) +include_directories( + include +) + foreach(pkg IN LISTS dependencies_pkgs) find_package(${pkg} REQUIRED) endforeach() diff --git a/nav2_mppi_controller/test/noise_generator_test.cpp b/nav2_mppi_controller/test/noise_generator_test.cpp index db8667e9404..b552ed33610 100644 --- a/nav2_mppi_controller/test/noise_generator_test.cpp +++ b/nav2_mppi_controller/test/noise_generator_test.cpp @@ -17,6 +17,7 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "nav2_mppi_controller/tools/noise_generator.hpp" #include "nav2_mppi_controller/models/optimizer_settings.hpp" #include "nav2_mppi_controller/models/state.hpp" From 79252f7020dcf7d3ead90f063ac833c027849823 Mon Sep 17 00:00:00 2001 From: kevin Date: Mon, 20 Nov 2023 13:45:16 +0900 Subject: [PATCH 29/39] test Signed-off-by: kevin --- .../include/nav2_behaviors/plugins/drive_on_heading.hpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp index 79c8d81c719..466daab2750 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp @@ -164,13 +164,17 @@ class DriveOnHeading : public TimedBehavior const int max_cycle_count = static_cast(this->cycle_frequency_ * simulate_ahead_time_); geometry_msgs::msg::Pose2D init_pose = pose2d; bool fetch_data = true; - + double sim_distance; while (cycle_count < max_cycle_count) { sim_position_change = cmd_vel->linear.x * (cycle_count / this->cycle_frequency_); + sim_distance = distance + sim_position_change; pose2d.x = init_pose.x + sim_position_change * cos(init_pose.theta); pose2d.y = init_pose.y + sim_position_change * sin(init_pose.theta); cycle_count++; - + + if(sim_distance < 0.05){ + continue; + } if (diff_dist - abs(sim_position_change) <= 0.) { break; } From fd9e529c640eaa7afbed07de85d6772fa9bc8682 Mon Sep 17 00:00:00 2001 From: kevin Date: Mon, 20 Nov 2023 13:54:52 +0900 Subject: [PATCH 30/39] test Signed-off-by: kevin --- .../include/nav2_behaviors/plugins/drive_on_heading.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp index 466daab2750..63accde1aa1 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp @@ -171,8 +171,9 @@ class DriveOnHeading : public TimedBehavior pose2d.x = init_pose.x + sim_position_change * cos(init_pose.theta); pose2d.y = init_pose.y + sim_position_change * sin(init_pose.theta); cycle_count++; - + if(sim_distance < 0.05){ + RCLCPP_INFO(this->logger_, "sim dist : %f", sim_distance); continue; } if (diff_dist - abs(sim_position_change) <= 0.) { From 59225512f134e653b07098247d5ae746c700d46c Mon Sep 17 00:00:00 2001 From: kevin Date: Mon, 20 Nov 2023 13:55:03 +0900 Subject: [PATCH 31/39] disdt Signed-off-by: kevin --- .../include/nav2_behaviors/plugins/drive_on_heading.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp index 63accde1aa1..6838fdb08e4 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp @@ -171,9 +171,8 @@ class DriveOnHeading : public TimedBehavior pose2d.x = init_pose.x + sim_position_change * cos(init_pose.theta); pose2d.y = init_pose.y + sim_position_change * sin(init_pose.theta); cycle_count++; - + RCLCPP_INFO(this->logger_, "sim dist : %f", sim_distance); if(sim_distance < 0.05){ - RCLCPP_INFO(this->logger_, "sim dist : %f", sim_distance); continue; } if (diff_dist - abs(sim_position_change) <= 0.) { From 4b73424ec859af05d4d43be7833b0c6eab100687 Mon Sep 17 00:00:00 2001 From: kevin Date: Mon, 20 Nov 2023 14:10:09 +0900 Subject: [PATCH 32/39] test Signed-off-by: kevin --- .../nav2_behaviors/plugins/drive_on_heading.hpp | 14 +++----------- 1 file changed, 3 insertions(+), 11 deletions(-) diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp index 6838fdb08e4..e2cac40194f 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp @@ -163,7 +163,7 @@ class DriveOnHeading : public TimedBehavior const double diff_dist = abs(command_x_) - distance; const int max_cycle_count = static_cast(this->cycle_frequency_ * simulate_ahead_time_); geometry_msgs::msg::Pose2D init_pose = pose2d; - bool fetch_data = true; + bool can_free = false; double sim_distance; while (cycle_count < max_cycle_count) { sim_position_change = cmd_vel->linear.x * (cycle_count / this->cycle_frequency_); @@ -171,20 +171,12 @@ class DriveOnHeading : public TimedBehavior pose2d.x = init_pose.x + sim_position_change * cos(init_pose.theta); pose2d.y = init_pose.y + sim_position_change * sin(init_pose.theta); cycle_count++; - RCLCPP_INFO(this->logger_, "sim dist : %f", sim_distance); - if(sim_distance < 0.05){ - continue; - } if (diff_dist - abs(sim_position_change) <= 0.) { break; } - - if (!this->collision_checker_->isCollisionFree(pose2d, fetch_data)) { - return false; - } - fetch_data = false; + can_free = !this->collision_checker_->isCollisionFree(pose2d, fetch_data); } - return true; + return can_free; } /** From 012de5504818549fb1bc298400508f69743e6a79 Mon Sep 17 00:00:00 2001 From: kevin Date: Mon, 20 Nov 2023 14:13:32 +0900 Subject: [PATCH 33/39] test Signed-off-by: kevin --- .../include/nav2_behaviors/plugins/drive_on_heading.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp index e2cac40194f..1df7f23cdb6 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp @@ -164,6 +164,7 @@ class DriveOnHeading : public TimedBehavior const int max_cycle_count = static_cast(this->cycle_frequency_ * simulate_ahead_time_); geometry_msgs::msg::Pose2D init_pose = pose2d; bool can_free = false; + bool fetch_data = true; double sim_distance; while (cycle_count < max_cycle_count) { sim_position_change = cmd_vel->linear.x * (cycle_count / this->cycle_frequency_); @@ -175,6 +176,7 @@ class DriveOnHeading : public TimedBehavior break; } can_free = !this->collision_checker_->isCollisionFree(pose2d, fetch_data); + fetch_data = false; } return can_free; } From 0c3af4383d71292d237a3d30e00b1e013a1d6a56 Mon Sep 17 00:00:00 2001 From: kevin Date: Mon, 20 Nov 2023 14:15:32 +0900 Subject: [PATCH 34/39] remove unused Signed-off-by: kevin --- .../include/nav2_behaviors/plugins/drive_on_heading.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp index 1df7f23cdb6..8f77616be1e 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp @@ -165,7 +165,6 @@ class DriveOnHeading : public TimedBehavior geometry_msgs::msg::Pose2D init_pose = pose2d; bool can_free = false; bool fetch_data = true; - double sim_distance; while (cycle_count < max_cycle_count) { sim_position_change = cmd_vel->linear.x * (cycle_count / this->cycle_frequency_); sim_distance = distance + sim_position_change; From f2552a53c7e6c54cdc31aead4454841a56a16141 Mon Sep 17 00:00:00 2001 From: kevin Date: Mon, 20 Nov 2023 14:17:37 +0900 Subject: [PATCH 35/39] test Signed-off-by: kevin --- .../include/nav2_behaviors/plugins/drive_on_heading.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp index 8f77616be1e..f42eb82b967 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp @@ -167,7 +167,6 @@ class DriveOnHeading : public TimedBehavior bool fetch_data = true; while (cycle_count < max_cycle_count) { sim_position_change = cmd_vel->linear.x * (cycle_count / this->cycle_frequency_); - sim_distance = distance + sim_position_change; pose2d.x = init_pose.x + sim_position_change * cos(init_pose.theta); pose2d.y = init_pose.y + sim_position_change * sin(init_pose.theta); cycle_count++; From 8e984463d661c068ca37f683121892c2741406b5 Mon Sep 17 00:00:00 2001 From: kevin Date: Mon, 20 Nov 2023 14:21:17 +0900 Subject: [PATCH 36/39] test Signed-off-by: kevin --- .../include/nav2_behaviors/plugins/drive_on_heading.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp index f42eb82b967..861fd26a910 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp @@ -173,7 +173,7 @@ class DriveOnHeading : public TimedBehavior if (diff_dist - abs(sim_position_change) <= 0.) { break; } - can_free = !this->collision_checker_->isCollisionFree(pose2d, fetch_data); + can_free = this->collision_checker_->isCollisionFree(pose2d, fetch_data); fetch_data = false; } return can_free; From 3f1c4aa547e0d6df431496a461b6bf39d044b278 Mon Sep 17 00:00:00 2001 From: ladianchad Date: Wed, 22 Nov 2023 19:51:27 +0900 Subject: [PATCH 37/39] fix spin Signed-off-by: ladianchad --- nav2_behaviors/plugins/spin.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/nav2_behaviors/plugins/spin.cpp b/nav2_behaviors/plugins/spin.cpp index b7df8b36541..cc783ad529c 100644 --- a/nav2_behaviors/plugins/spin.cpp +++ b/nav2_behaviors/plugins/spin.cpp @@ -168,7 +168,7 @@ bool Spin::isCollisionFree( const int max_cycle_count = static_cast(cycle_frequency_ * simulate_ahead_time_); geometry_msgs::msg::Pose2D init_pose = pose2d; bool fetch_data = true; - + bool can_free = true; while (cycle_count < max_cycle_count) { sim_position_change = cmd_vel->angular.z * (cycle_count / cycle_frequency_); pose2d.theta = init_pose.theta + sim_position_change; @@ -177,13 +177,10 @@ bool Spin::isCollisionFree( if (abs(relative_yaw) - abs(sim_position_change) <= 0.) { break; } - - if (!collision_checker_->isCollisionFree(pose2d, fetch_data)) { - return false; - } + can_free = collision_checker_->isCollisionFree(pose2d, fetch_data); fetch_data = false; } - return true; + return can_free; } } // namespace nav2_behaviors From 3989a03633d5a1247f2c466a872d30c571c2d5c5 Mon Sep 17 00:00:00 2001 From: ladianchad Date: Thu, 7 Dec 2023 12:43:56 +0900 Subject: [PATCH 38/39] remove looprate log Signed-off-by: ladianchad --- nav2_behavior_tree/src/behavior_tree_engine.cpp | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index a5ee96af5e1..c81d72cf6ff 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -50,17 +50,14 @@ BehaviorTreeEngine::run( tree->rootNode()->halt(); return BtStatus::CANCELED; } - result = tree->tickRoot(); - onLoop(); - - if (!loopRate.sleep()) { - RCLCPP_WARN( - rclcpp::get_logger("BehaviorTreeEngine"), - "Behavior Tree tick rate %0.2f was exceeded!", - 1.0 / (loopRate.period().count() * 1.0e-9)); - } + // if (!loopRate.sleep()) { + // RCLCPP_WARN( + // rclcpp::get_logger("BehaviorTreeEngine"), + // "Behavior Tree tick rate %0.2f was exceeded!", + // 1.0 / (loopRate.period().count() * 1.0e-9)); + // } } } catch (const std::exception & ex) { RCLCPP_ERROR( From 0df2efc91f932a761f650ed331dc57843a3f0728 Mon Sep 17 00:00:00 2001 From: ladianchad Date: Thu, 7 Dec 2023 14:54:43 +0900 Subject: [PATCH 39/39] removed loop rate waring Signed-off-by: ladianchad --- nav2_behavior_tree/src/behavior_tree_engine.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index c81d72cf6ff..361054a22f5 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -52,6 +52,7 @@ BehaviorTreeEngine::run( } result = tree->tickRoot(); onLoop(); + loopRate.sleep(); // if (!loopRate.sleep()) { // RCLCPP_WARN( // rclcpp::get_logger("BehaviorTreeEngine"),