* Add test for tf_help.

* Fix lint error.

* adding smac_planner to navigation2 metapackage

* adding params to metapackage

* update config files

* adding navfn benchmark testing

* updates to costmap_2d for flexility

* update planner API for new changes

* adding ompl to underlay because ros2 master doesn't contain the rosdep key

* patching templated footprint collision checker

* fix typo

* updating readme config file

* Analytic expansion (#43)

* Use OMPL to generate heuristics

The calculation is run at every planning cycle. It does not seem to slow down
the planner - the calculation time seems to be quick enough that the
improvement in graph expansion accounts for it.

* Use OMPL to calculate analytic solution when near goal

* Make angles multiples of the bin size to stop looping behaviour

* Uncrustify

* Use faster std::sqrt function

* Fix analytic path so that the collision checker has coordinates to check!

* Pre-allocate variables in analytic path expansion

* Rename typedef to NodeGetter to more accurately describe function

* Use distance rather than heuristic to determine when to perform analytic expansion

Also force the analytic expansion to run on first iteration in case path is trivial.

* Move the check for motion model into the main A* loop

* Add copyright notices

* Remove comment about relaxing node match tolerances

The analytic expansion removes the need for this.

* Correctly reset node coordinates when aborting from analytic expansion

* Move analytic expansion logic to separate function

* Uncrustify

* Remove unneeded call to get goal coordinates

* Fix the calculation of intervals in the analytic path

Reserve the number of candidate nodes we are expecting.
Base calculations on intervals rather than points - makes distances between nodes work properly.

* Rescale heuristic so that analytic expansions are based on distance

* Repeatedly split analytic path in half when checking for collision

* Add parameter to control rate of analytic expansion attempts

* Uncrustify

* Fix incorrect type in templated function

* Cpplint

* Revert "Repeatedly split analytic path in half when checking for collision"

This reverts commit 94d9ee0257c47fa6bd5a396ce74d180430c1c814.

There was a marginal speed gain (perhaps!) and the splitting approach made
the code harder to understand and maintain.

* Uncrustify

* Add doxygen comments

* Add parameter description for analytic expansion ratio

* Set lower limit of 2 on number of iterations between analytic expansions

* Reduce expected number of iterations because of analytic completion

* Refactor analytic expansion ratio calcs to make logic easier to understand

* add readme color

* fix linting

* ceil from floor (and speed up)

* a few updates

* fix smac tests

* fixing smoother test

* remove cost check - to be readded at another time

* working last test from debug issues

* Update README.md

* Update README.md

* adding getUseRadius API doxygen

Co-authored-by: James Ward <james@robomo.co>

Successor PR for #1993, Introduces A plugin interface to define behaviours at waypoint arrivals when using nav2_waypoint_follower (#2015)

* Add WaypointTaskExecutor plugin to main

Signed-off-by: jediofgever <fetulahatas1@gmail.com>

* couple result of task execution with stop_on_failure

Signed-off-by: jediofgever <fetulahatas1@gmail.com>

* add  failed ids to it's vector

* experimental change in nav2_waypoint_tester

Signed-off-by: jediofgever <fetulahatas1@gmail.com>

* remove unused code

* Add conversion tests.

* Add path_obs test. Resolution test is not completed. What is the expected behavior?

* Added more details and tests to messages.

* Added rotation checks.

* Update the test for the length of the segments.

* Changed back variable name to min_. Removed couts. Use EXPECT_TRUE.

* Also set the frame_id and check for the results.

* Update the copyright statement.

* Move codecov config back to project root
to work arround inconsistencies in  codecove

As OMPL isn't yet registering properly with the ament index for ros2
and searching all of << parameters.underlay >>/install will stall rosdep
when searching entire $ROS_WS/install folder

* Fix max path cycles for case where map has larger Y dimension than X dimension

* Improve readability

* fix ament_cpplint and ament_uncrustify issues

* Controllers should not be influenced by time jumps

Therefore use rclcpp::GenericRate<std::chrono::steady_clock> instead of
rclcpp::Rate

Signed-off-by: Martijn Buijs <martijn.buijs@gmail.com>

* Change to using `rclcpp::WallRate` for better readability

Signed-off-by: Martijn Buijs <martijn.buijs@gmail.com>

* Swap codecov orb for equivalent bash command

* Change working directory to find codecov.yml

* Clean root dir by moving codecov.yml to CI folder
still findable by codecov bash script

* Update .dockerignore file

* Fix RecoveryNode logic

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Fix reset logic in all control nodes

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Revert RoundRobin changes

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Revert RoundRobin changes

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Fix RecoveryNode halt

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

Signed-off-by: Martijn Buijs <martijn.buijs@gmail.com>

* Add timeouts to all spin_until_future_complete calls

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Update default timeout value

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Add costmap filters with KeepoutFilter realization

* Cover preferred lanes in industries by KeepoutFilter (#1522)

* * Resolve review questions
* Add new OccupancyGrid class into nav2_util - API wrapper for original OccupancyGrid
* Add declareParameter() with one argument to Layer API

* Resolve 2nd review questions
* Remove OccupancyGrid class wrapper
* Add Costmap2D constructor with OccupancyGrid instead
* Add launch-file to help running CostmapFilterInfo publisher

* Code clean-up

* Resolve 3rd review questions (partially)

* Fix hidden problems during the shared handling of copstmap_ resource

* Cast into double during the linear conversion of OccupancyGrid data

* Add keepout_filter system test

* Add error message when base or multiplier is not set in ConstmapFilterInfo message

* Add comment to dummy info publisher launch-file

* Add costmap_conversion unit test

* Add declareParameter() unit test

* Add keepout zone entering check in FiltersTester

* Fix upper window boundary calculation in KeepoutFilter

* Correct mutex header in costmap_filters

* Added KeepoutFilter::isActive() method for filter readiness check

* Add KeepoutFilter unit test

* Correct the code to consider node_ as a weak pointer

* Enhance KeepoutFilter unit test coverage by adding different surfaces

* Made CostmapFilters and minor documentation updates

* Fixed typo

* Move CostmapFilterInfo publisher to Map Server

* Add costmap_filter_info.launch.py into tb3_simulation_launch.py

* Remove hard-coded bar areas from test_keepout_filter

* Add CostmapFilters HLD

* Fix doc after review

* Fix mask parameter initialization in bringup_launch.py

* Improve tests and remove costmap-filters from nav2_bringup

* Update map mask to filter mask in HLD

* Small fix-ups

* Fix error message

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Fix nav2_waypoint_follower bugs

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Update function description

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Revert to simple simlinks
now that image builds are triggered by cron job on CI
rather than repository links on Docker Hub

* Parallelize install/build steps using multi stages
by cascading copies of manifests and workspaces

* Rename stages for installing dependencies

* Remove redundant ARGs and fix WORKDIRs
ARGs do not need to be redeclared for child stages

* Fix typo

* Simplify ARGs and WORKDIRs

* Write package list outside of workspace
to generalize skip up to pattern

* Simplify tests
Assume current WORKDIR
Don't use build mixins for test

* Add parallel stages for testing
use target arg to build added stages, e.g:
`docker build --target=underlay_tester ...`

* Add more comments

* Add simple stage to collect test results
and to easly build all other tester stages in parallel, e.g:
`docker build  --build-arg=RUN_TESTS=True --target=workspaces_tester`

* Initialize inflate_cone_ variable.

* initialize inflate_cone_ based on parameter.

* Increase the sleep time in the tests makes the costmap test always succeed on my machine.

* add test with only 2 points

* Added toPolygon test.

* Added test for footPrintFromString function.

* Add IsBatteryLow condition node

* Update default battery topic and switch to battery %

* Fix test

* Switch to sensor_msgs/BatteryState

* Add option to use voltage by default or switch to percentage

* Add sensor_msgs dependency in package.xml

* Make percentage default over voltage

* Update parameter list

* return if the number of waypoints is zero.

* terminate the action.

* Succeed action instead of terminating.

* fix invalid memory access

this fix was applied as part of the noetic release of costmap_2d

* uncrustify

* Fix memory leak in nav2_recoveries

* Fix recovery server memory leak (better interface)

* Fix costmap2d memory leak

* Fix nav2_navfn_planner memory leak

* Fix planner server and navfn planner memory leak

* Make all rclcpp::Node::SharedPtr argument passing const

* Fix controller server and DWB plugins memory leak

* Minor fixes

* Fix formatting errors

* Change all plugin interfaces to use weak_ptr intead of shared_ptr to parent rclcpp::Node

* Convert all SharedPtr to WeakPtr

* Check shared_ptr after lock and before dereferencing

* Fix tests declaring parameters real nodes don't

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Fix loadParameterWithDeprecation not getting initial parameter values

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Create sim_time variable before using it

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Line length < 100

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add missing {

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Linter fixes

Signed-off-by: Shane Loretz <sloretz@openrobotics.org>

* sim_granularity -> time_granularity

Signed-off-by: Shane Loretz <sloretz@openrobotics.org>

* Linter fix

Signed-off-by: Shane Loretz <sloretz@openrobotics.org>

* remove path_updated blackboard variable to check if the path is updated

Signed-off-by: Daisuke Sato <daisukes@cmu.edu>

* remove path_udpated from tests

Signed-off-by: Daisuke Sato <daisukes@cmu.edu>

* make bt_action_node logic clear and fix test failures

Signed-off-by: Daisuke Sato <daisukes@cmu.edu>

* Revert "make bt_action_node logic clear and fix test failures"

This reverts commit bb29d801d504cf5de8c33524d01f76974f76e23a.

Signed-off-by: Daisuke Sato <daisukes@cmu.edu>

Signed-off-by: Daisuke Sato <daisukes@cmu.edu>

* Remove unused achor

* Trigger dockerhub build via cron job

* Rename workflow

* Change timezone
trigger dockerhub build 12 am pst
test nightly job 6am pst

* Clean up dockerhub job refrences

* Add missing executors for python_exec

* Remove unused devel folder

[WIP] removing rviz plugins from test coverage since its not a production run-time component (#1953)

* removing rviz plugins from test coverage since its not a production run-time component

* Update code_coverage_report.bash

The plan recieved is usually in global frame, but
our local costmap is often in odom frame. This
fixes a regression from #1857

To fix cpplint

* Use colcon to check for build failure
instead of canary file

* Simplify if case for shell

* Move extra dockerfiles to docker related folder

* Update Dockerfile location

* Stage refactor

* Use build kit

* Update underlay and overlay build

* Dockerfile tweek

* Rename underlay .repos file

* Remove uneeded key

* Clone matching distro branch by default

* Add optional directive to RUN colcon test

* Add comments

* Skip the use of symlinks

* Roll back to nightly image with all rmws
now that connext is fixed

* Add slam_toolbox to skipped rosdep keys

* Set DEBIAN_FRONTEND env to noninteractive

* Update to tag foxy

* Updated to latest codecov orb