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

* 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