* goal checker selector

* updating main nav yaml configuration file to support multiple goal checkers

* imised defaiñt gpañ cjecler àra,eter

* changes requested in the PR

* improving selector subscribers for laching behavior

* fixing cpplint uncristify style

* fixing issues

* setting original yaml configuration for the general_goal_checker

* the usage of the goal_checker vector now totally mimic the controller vector

* [Selector Actions] re-enabling back transient local for subscribers and tests

* commit failure on non default avaialable

* Some comments in selector behaviors code and some aesthetic changes

* fixing some linting issues

* minor linting error

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

* first commit

* Added empty line at end

Signed-off-by: Deepak Talwar <deepaktalwardt@farmwise.io>

* linting

* linting

Signed-off-by: Deepak Talwar <deepaktalwardt@farmwise.io>

* Variable name fix

Signed-off-by: Deepak Talwar <deepaktalwardt@farmwise.io>

* changed shared_ptr to weak_ptr

Signed-off-by: Deepak Talwar <deepaktalwardt@farmwise.io>

* get node handle from blackboard

Signed-off-by: Deepak Talwar <deepaktalwardt@farmwise.io>

* changed initialization of odom smoother object

* changed initialization of odom smoother object

* changed initialization of odom smoother object

* changed initialization of odom smoother object

* addressed comment re std::hypot

* added overloaded constructor

* added overloaded constructor

* added overloaded constructor

* added try catch and template to blackboard get path

Signed-off-by: Deepak Talwar <deepaktalwardt@farmwise.io>

* lint

Signed-off-by: Deepak Talwar <deepaktalwardt@farmwise.io>

* lint - pushing to rerun CI due to cc1plus failure

Signed-off-by: Deepak Talwar <deepaktalwardt@farmwise.io>

* setting of current_pose in feedback msg outside of try/catch

Signed-off-by: Deepak Talwar <deepaktalwardt@farmwise.io>

* added path integrator to nav2_controller for better distance feedback

Signed-off-by: Deepak Talwar <deepaktalwardt@farmwise.io>

* added path integrator to nav2_controller for better distance feedback

Signed-off-by: Deepak Talwar <deepaktalwardt@farmwise.io>

* fixed issue with lambda capturing class member

Signed-off-by: Deepak Talwar <deepaktalwardt@farmwise.io>

* retrigger CI

Signed-off-by: Deepak Talwar <deepaktalwardt@farmwise.io>

* add limits header

Signed-off-by: Deepak Talwar <deepaktalwardt@farmwise.io>

* retrigger CI

Signed-off-by: Deepak Talwar <deepaktalwardt@farmwise.io>

Co-authored-by: Deepak Talwar <deepaktalwardt@farmwise.io>

* first implementation

* restructure + param

* lint

* remove uneeded publishVelocity and deal with controller_patience_ 1.0

* change param name

* add reasonable param

* 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 support of absolute limits to Speed Filter

* Fix review items

* Fix comment

* Support for nav2_regulated_pure_pursuit_controller

* Add speed limit filter

* Fix review items

* Remain only percent speed limit support

* Small review fixes

* Add error message for unknown cells on mask

* Fix missed items

* Ensure SpeedFilter is using accurate in time transform
instead of latest in TF Buffer

* Add costmap_filter_info_server into param description

* Set transform_tolerance to 0.1

* Update comment to be more informative

* WIP attempt at fixing recovery-straight-path issues

* linting!

* handle not current due to old measurement readings separately from resets

* 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

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

* 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

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

* adding CLI test

* adding tests for more lines to map_server

* fix last test

* adding out of bounds and higher bounds checks

* adding planner tests for plugins working

* add cleanup

* working

* ping

* adding more testing and debug info messages

* adding CLI test

* adding tests for more lines to map_server

* fix last test

* adding out of bounds and higher bounds checks

* adding planner tests for plugins working

* add cleanup

* working

* ping

* adding waypoint follower failure test

* adding voxel, more logging

* reset -> clear

* remove erraneous handling done by prior

* adding a bunch of voxel unit tests

* retrigger

* failures tests

* adding copyrights

* cancel test in recoveries

* remove on_errors in favor of a general lifecycle warning

* adding removed thing

* prototype of lifecycle bond system

* adding more structure to get around weak ptr issue

* working prototype for manager

* adding some ns -> s conversions

* changing to service node

* adding bond connections to all servers

* update logs

* fixing review comments

* fix types

* remove extraneous functions

* make linters happy

* simplifications

* adding spinner to get working but now unstable

* moving bond connections to activate state

* adding defaults

* working complete prototype for review

* update dependencies

* adding connection logging

* remove accidental file

* fix server side timeout for heartbeats

* adding complete unit coverage of bond support

* fixing lifecycle test

* trying to activate since autostart was removed

* Move dwb goal/progress checker plugins to nav2_controller

Signed-off-by: Siddarth Gore <siddarth.gore@gmail.com>

* Move goal/progress checker plugins to nav2_controller

Address review comments

Signed-off-by: Siddarth Gore <siddarth.gore@gmail.com>

* Move goal/progress checker plugins to nav2_controller

Use new plugin declaration format and doc update

Signed-off-by: Siddarth Gore <siddarth.gore@gmail.com>

* Update bringup.yaml for new plugins in nav2_controller

Also remove redundent file from dwb_plugins
Signed-off-by: Siddarth Gore <siddarth.gore@gmail.com>

* Fix doc errors and update remaining yaml files

Signed-off-by: Siddarth Gore <siddarth.gore@gmail.com>

* Remove mention of goal_checker from dwb docs

Signed-off-by: Siddarth Gore <siddarth.gore@gmail.com>

* Add .plugin params to doc

Signed-off-by: Siddarth Gore <siddarth.gore@gmail.com>

* Tests for progress_checker plugin

declare .pl...

* Add utility function to get plugin type param and refactor controller params

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

* Exit when plugin param is not provided

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

* Minor fix

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

* Remove defaults

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

* Fix uncrustify error

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

* Default to DWB

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

* Add tests

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

* Refactor planner server plugin param

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

* Refactor recovery server plugin param

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

* Refactor costmap plugin param

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

* Update bringup parmas

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

* Update parameter list in docs

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

* Rename plugins parameter

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

* Update parameter list and fix defaults in costmap node

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

* Declare default .plugin params only when plugin name parameter is default

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

* Update parameter list

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

* Update nav2_recoveries/src/recovery_server.cpp

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

* Update params.md

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

* Fix uncrustify errors

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

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

* adding navigate to pose feedback and remove random crawl from master

* adding controller feedback

* recovery feedback actions

* Update nav2_controller/src/nav2_controller.cpp

Co-Authored-By: Carl Delsey <1828778+cdelsey@users.noreply.github.com>

* Add feedback in wait action and make general improvements

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

* Fix rebase errors

* Make feedback reset across different goals

Co-authored-by: stevemacenski <stevenmacenski@gmail.com>
Co-authored-by: Carl Delsey <1828778+cdelsey@users.noreply.github.com>

* Windows bringup.

* nullity check.

* nullity check.

* Change publishers to publish unique ptrs

* Revert test case modification

* Update function signature to receive unique_ptrs

* Update publishers in nav2_costmap_2d to publish unique ptrs

* Update publishers in nav2_planner and nav2_map_server

* Change nav2_map_server publisher to publish occupancy grid unique ptr

* Change publisher in nav2_planner to publish path unique ptr

* Remove smart pointer return from functions in nav2_costmap_2d

* Run cpp_lint manually in nav2_costmap_2d

* Minor fixes

* Adhere to conventions of smart pointers passing to function

* Change publisher in dwb_core to publish unique pointer

Signed-off-by: Pablo Vega <epvega@gmail.com>

move odom sub thresholds to the controller server for no change in behavior but prior change in authority (#1528)

* Declare progress checker parameters

* Use declare_if_not_declared function

This should allow the node to be brought down and back up using
the lifecycle manager.

* TEMP remove all system tests from running to add back in one by one

* hottt patch, working simple action server again

* removing a few debugs

* adding debug

* fixed test actions file

* removing debug statements and removing opensplice and connext jobs

* reintroduce planner tests

* reintro localization tests

* reintroduce updown tests

* updating defaults

* allow controller to react to different controlller requests

* add params

* planners tests online

* add tests back in

* remove flaky multirobot test for CI but leaving here for utility

* adding back connext dds

* crusty

* refactor lots and lots of code...

* adding gtest mains

* disable planner tests

* Moving tuning to configuration.

* WIP writing up controller docs.

* DWB Parameter Structure Complete.

* Fixing review comments.

* Fix wording on progress checker.

Fixing build error introduced by #1356

* Handle action server deactivation while the action is still running.
* Handle race condition between completing an action and receiving a preemption.
* Terminate all goals if cancel was requested on any goal handle.
* Terminate only the current goal if the module fails to execute the action.

Added parallel recovery behaviors

* changing behavior tree to use snake_case actions

* correct action servers not related to recovery

* resolve recovery server plugin names with action server names

* changing references from local planner to controller outside of DWB

* change minimum in DWB planner to be controllers

* Update nav2_core/include/nav2_core/controller.hpp

Co-Authored-By: bpwilcox <bpwilcox@eng.ucsd.edu>