* Fix build on macOS with LLVM/Clang12

* nav2_costmap_2d's observation buffer is now using tf_tolerance.
Removed pragmas and os specific logic

* adding new logo!

* limit dimensions

* limit dimensions

* limit dimensions

- Add Map Saver resource sharing
- Add forgotten reset of save_map_service_
- Remove unnecessary map republisher
- Increase save_map_timeout

* Fix shared_ptr cyclic dependencies

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

* Refactor costmap downsampler and fix test

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

* Revert costmap downsampler test changes

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

* Abort analytic expansion if crossing through already visited node

* Check that we are not creating an infinite loop at the goal node

* Mark nodes in analytic expansion as visited for the sake of completeness

* Move checking of already visited nodes to final stage of analytic expansion

* Support in keepout filter for mask and costmap published in different frames

This fixes incorrect keepouts position issue when filter mask and current
costmap layer are published in different frames. This might appear
(but not restricted only to) when keepout filter is enabled for local costmap
with rolling window.

* Add transform initialization

* Enhance transform failure message

* nav2_way_point_follower; introduce photo at waypoint arrivals plugin

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

* resolve cmake lint errors

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

* resolve requested chages of first review

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

* minor corrections on photo_at_waypoint header

* resolve requested changes of second review

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

* update default save_dir

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

* move directory checking code to initialize()

* add try catch block to catch possible execeptions while creating a directory for photo at waypoint plugin

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

* adding logging and disabling

* Update nav2_waypoint_follower/plugins/photo_at_waypoint.cpp

Co-authored-by: Fetullah Atas <fetulahatas1@gmail.com>

* Update photo_at_waypoint.cpp

Co-authored-by: jediofgever <fetulahatas1@gmail.com>

* update to use on my fork to test changes to bondcpp

* using chrono literals in lifecycle manager

* nav2_rviz_plugins using chrono literals for API change

* using chrono literals in costmap_2d package from API change

* using chrono literals in observation buffer

* chrono literals for tests costmap

* chrono literal API changes

* changing API

* changing API

* API changes

* API change

* API change

* API change

* API change test

* API change test

* remove

* api updates

* update test values

* nav2_way_point_follower; introduce photo at waypoint arrivals plugin

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

* resolve cmake lint errors

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

* resolve requested chages of first review

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

* minor corrections on photo_at_waypoint header

* resolve requested changes of second review

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

* update default save_dir

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

* move directory checking code to initialize()

* adding input at waypoint executor

* exporting depedency

* fixing review suggestions

* make input topic a parameter

* working input executor

* adding docs to params md

* Fix flaky tests

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

* Fix shutdown in waypoint follower test

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

* Add run-time system test for keepout filter

* Add keepout_filter specific logic

* Fix review items

* Move goal to be on the straight line by plan without keepout

* Added a test with controller_frequency of 0. The controller should get a default frequency of 20.

* Add kinematic parameters test (#2031)

* Add kinematic parameters tests.

* Solve uncrustify error.

* [WIP fixing CI] changing gazebo base to my fork for testing (#2034)

* changing gazebo base to my fork for testing

* adding buffer header per TF2 API changes

* moving header to header

* Adding additional SmacPlanner tests (#2036)

* adding some more tests to smac_planner

* addtl smac tests

* remove unused functions

* adding additional constants and smoothers tests (#2038)

* Added tests for different controller frequencies.

* Added a test with controller_frequency of 0. The controller should get a default frequency of 20.

* Added tests for different controller frequencies.

* Solve uncrustify issue.

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* adding some more tests to smac_planner

* addtl smac tests

* remove unused functions

* changing gazebo base to my fork for testing

* adding buffer header per TF2 API changes

* moving header to header

* Add kinematic parameters tests.

* Solve uncrustify error.

* commenting out or removing AMCL dead code

* remove file from cmakelists

* Remove the parameters.hpp file.

* Put parameters.hpp back. Only removed the two functions for depricated parameters.

* Removed DWBLocalPlanner::loadBackwardsCompatibleParameters(). It will now throw a runtime error if there are no critics declared in the parameters.

* Remove moveParameter. It was not used anywhere.

* 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`