* adding goal checker to the API for controllers and the controller server

* updating DWB and RPP to include the new controller API change to include the goal checker pointer

* adding getTolerances functions for goal checkers with unset default method

* use getTolerances for goal checkers in RPP

* adding missing headers to goal stoppers

* fix linting of RPP

* add parameter_event_sub

* recompute xy_goal_tolerance_sq_ after param change

* Fix SingleTrigger BT node

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

* Add explicit failure case

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

* Fix uncrustify error

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

* Forse ROS2 parameters to be static to meet RCLCPP API changes

* Fix comments

* Move all common CostmapFilter parameter declarations into try-catch block

* Fix #2186

* goal_pose instead of goal->goal

* avoid redundant code

* change name and switch to input + output param pattern

* code style

* change timeout default value

* create and use callback group

* remove generate_internal_node option

* unit test to do a service call in a topic callback

* fix unit test

* set default value to -1

* working on canceling state machine for waypoint mode

* fixing cancelation logic

* fix linting isue

Ability to get a path from the planner from any 2 poses (start_pose and goal): add use_start_pose and start_pose to ComputePathToPose action (#2179)

* add use_start_pose to ComputePathToPose.action

* add use_start_pose to ComputePathToPose bt

* start instead of start_pose + else if

* [ComputePathToPose Action API break] change 'pose' for 'goal' +use_start

* test wip

* add compute path to pose BT test for use_start

* last start_pose renaming

* Transform start and goal in costmap frame

* Revert "Transform start and goal in costmap frame"

This reverts commit 4ed8eb0aca6840d57768823a20406b7287dad837.

* lint

* costmap bt

* bt double type

* change bt actions name to be consistent with service names

* add tests

* code style

* readme update

* improve readme

* If provided param file has no slam_toolbox params, don't forward it

* Update copyright

* Linter fixes

* Reorder slam_launch to hide uglyness at the end

* fixing depricated API

* converting types on substitutions

* removing warnings from std buffer parameterization

Signed-off-by: Marco Lampacrescia <Marco.Lampacrescia@de.bosch.com>

* Remove dependency on collision checking to carrot location

* Fix i removal

* changing API to be consistent with collision updates

* Add support of absolute limits to Speed Filter

* Fix review items

* Fix comment

* Support for nav2_regulated_pure_pursuit_controller

* make the 3 params changeable at runtime

* use parameter events callbacks

* doxygen

* lint

* Fix recovery action collision check.

* Fix linting issue.

* Adding RPP to metapackage.xml

* Remove excessive exec depend

* Write doxygen for navfn

* Remove forward slashes

(WIP) Recoveries always return success, regardless if they're able to complete their required tasks or not (#1855)

* modified such that if the recovery is aborted due to potential collision, the return status will be FAILED

* Changed from SequenceStar to RoundRobin to mask any failure that might occur in the execution of the recovery action

* Change Backup recovery test such that the aborted recovery is expected and checked for

* fixing linting error

* Change depricated argument for backup recovery test

* added backup to the recovery actions, using defaults from the BackUp header

* Update nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml

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

* Reverting changes to BT. Seq. Star control node is in line to what we want

* checkpoitn commit

* Changed collision failure critera as backup collision abortion occurs @ -0.2 m as well. This was previously not caught as  the server always returned SUCCESS

* bool to control whether fake costmap should be generated

* added fake tester nodes

* Fake costmap passed through environmental variable now

* Fake Footprint being sent out

* breadcrumbs of fake spin tests

* fake odom

* launch file was messed up

* still troubleshooting spin

* trying to fake spin, goal keeps on getting rejected

* goal is now being accepted

* Fake spin not working well when asking for PI radians out

* playing around with timing wehn publishing fake transforms

* BT changes, testing TBD

* [WIP] Fix CI build issues (#2076)

* 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

* Patch for PhotoAtWaypoint plugin (#2067)

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

* Fix for double free issue in map server testcases (#2078)

* Support in keepout filter for mask and costmap published in different… (#2054)

* 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

* Loop fix (#2068)

* 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

* test if action server failures correctly propagate into BT context

* change recovery subtree so that RoundRobin replaces SequenceStar for good

* uncrustify and line nav2_behavior_tree changes

* uncrustify and linting

* adding clearing actions to own subtree

* more linting

* reduced testing value to see if that will stop the robot from aborting due to potential collision

* tighten up tolerance on backup since the requested backup is so small

* delint

* cleaned up spin recovery such that we simulate the robot slowly spinning into place, now I just have to simulate collision scenarios

* linting

* change order of Spin and Wait to match original

* fake spin failing due to potential collision even when empty costmap

* lint

* more lint

* can get all the spins to pass now

* spin test strangely passing all cases even though costmap is populated

* spin recovery

* commmit before I try to visualize this

* now I know that there are two things publishing to the same footprint

* 1) figured out conflicting pubs to footprint, 2) why can't I get costmap occupancy to cause collision

* Fake Spin Test shows failure correctly for angles greater than pi / 2. change launch file such that two publishers are not generating costmaps

* fake launch lint

* check in with linting to show the structure

* Add overall behavior tree system tests

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

* Add RecoverySubtreeGoalUpdated BT test

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

* Update readme and docs

* Add tests to check if BT XML files are well-formed

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

* Uncomment tests that were commented out

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

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
Co-authored-by: jediofgever <fetulahatas1@gmail.com>
Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com>
Co-authored-by: James Ward <james.ward@appliedev.com>
Co-authored-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* regulated pure pursuit migration commit

* adding speed limit API

* adding review comments + adding rotate to goal heading

* adding test dir

* add some initial tests

* more tests

* remove old comment

* improve readme

* fix CI

* first attempt at changing algos in tests

* allowing full path parameter substitutions

* adding integration tests

* enable SMAC testing too with new changes

* swap algos

* revert

* Update angular velocity after constraining linear velocity (#2165)

This ensures the robot moves towards the lookahead point more closely.
If the angular velocity is not updated, then the robot tries to take cuts while turning,
which could lead to collisions when near obstacles

Signed-off-by: Shrijit Singh <shrijitsingh99@gmail.com>

* Update cost scaling heuristic to vary speed linearly with distance (#2164)

* Update cost scaling to vary linearly with distance instead of relying on costmap cost

Signed-off-by: Shrijit Singh <shrijitsingh99@gmail.com>

* Resolve suggested changes

Signed-off-by: Shrijit Singh <shrijitsingh99@gmail.com>

* Add documentation for cost scaling parameters

Signed-off-by: Shrijit Singh <shrijitsingh99@gmail.com>

* Improve parameter descriptions

Signed-off-by: Shrijit Singh <shrijitsingh99@gmail.com>

* Comment cost scaling tests since layered costmap is not initialized

A valid layered costmap reference is needed to get the inscribed radius

Signed-off-by: Shrijit Singh <shrijitsingh99@gmail.com>

Co-authored-by: Shrijit Singh <shrijitsingh99@gmail.com>