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

* Add GoalReachedCondition test and move around headers

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

* Rename to BehaviorTreeTestFixture

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

* Remove GoalReachedCondition test

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

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

* Changing eloquent to foxy

* Replace eloquent with foxy in build_all script

* Add SpeedController decorator node and update default BTs

* Fix lint errors

* Add OdomSmoother class and fix SpeedController to use smoothed velocity

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

* Add duration input port to tree nodes xml for groot

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

* Revert BT images

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

* Address reviewer's comments

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

* Throw BT exception when rate <= 0

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

* Add simple behavior tree using speed controller

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

* Set default value to maximum rate on IDLE

* Add test for speed controller

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

* Use smart pointers in tests

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

* Fix test

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

* Move speed controller header file

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

* Update speed controller to reset only on new goal

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

* Fix test

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

* Add params to parameter list

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

* Fix memory leak

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

* Select BT in NavigateToPose action

Signed-off-by: Francisco Martin Rico <fmrico@gmail.com>

* Change to unique_ptr. Default bt_xml to a valid file

Signed-off-by: Francisco Martin Rico <fmrico@gmail.com>

* Return tree to stack

Signed-off-by: Francisco Martin Rico <fmrico@gmail.com>

* Removed default_bt_xml_filename default value

Signed-off-by: Francisco Martin Rico <fmrico@gmail.com>

* Failing fast, early and loud when BT is not valid

Signed-off-by: Francisco Martin Rico <fmrico@gmail.com>

* Return inmediately after terminate

Signed-off-by: Francisco Martin Rico <fmrico@gmail.com>

Parameter ``bt_xml_filename`` has changed to ``default_bt_xml_filename``

* Add test

* Add more coverage

* Merge test header file with executable

* Address PR reviewer's comment

* Add some more coverage

* Revert accidental changes to localization_launch.py file.

* adding a bunch of parameter descriptions

* ading costmap param descriptions

* add AMCL parameters

* adding DWB params

* resolving review questions

* include planner and controller ID parameters

* fixing jack's requests

* include bt_conversions.hpp to convert input properly

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

* delete redundant include statement

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

* Add SLAM launch file

Fix possible collision of laser scan with camera on waffle

* Simplifications and fixes after review

* Increase save_map_timeout to 5 sec to comply with SLAM Toolbox map rate

Signed-off-by: ymd-stella <world.applepie@gmail.com>

* Initial commit

* Added AMCL params and some formatting fixes

* Added some missing params

* Added bt nodes ports

* fixed typo

* Added <> to base names + some reformating

* added default_critic_namespaces param to dwb_local_planner

* adding backup tests

* add negative case

* correct termination logic

* increase speed

* fixish failure test case

* Update deprecated launch params

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

* Revert internal python changes

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

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

* Add condition nodes and behavior tree to enable replan on new goal

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

* Fix time expired and distance traveled conditions

* Remove new_goal_received from blackboard

* Fix IDLE check condition in new condition nodes

* Fix lint errors

* Fix lint errors

* Address reviewer's comments

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

* Add tests

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

The following is an example of the error.
[ERROR] [1590171638.335693813] [bt_navigator]: Action server failed while executing action callback: "failed to send request: ros_request argument is null, at /opt/ros/src/ros2/rcl/rcl/src/rcl/client.c:278"a bugfix of clear costmap service action

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

* Added use of declare_parameter_if_not_declared.

* Fixed column width.

* Whitespace removal

* Style cleanup.

Initial pose is needed for the test to run
so it makes sense to wait for it till it times out.

Looks like codecov analytics from merged PRs alone do not update codecov status for the master branch.
Re-enabling nightly debug builds to keep codecov status on master up-to-date

* Consider the goal itself when looking to potential within tolerance region

* Fixed comments

* Use parameterized frames (ros-planning#1726)

Signed-off-by: ymd-stella <world.applepie@gmail.com>

* Expore frame to ports in GoalReached and add params to bringu yaml files

* Fix recovery interface

* Update launch file and add recovery parameters to bringup yaml

* Remove lifecycle node params

* Update bringup param files

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

* Fix cpplint error

* Fix nav2_recoveries cpplint errors

Co-authored-by: ymd-stella <world.applepie@gmail.com>

Context: https://github.com/osrf/docker_images/pull/399

So we don't have to wait for the nightly parent image to update rosdep

* make basic failing test for tf2 wrapper

* add file :-)

The forward_point_distance used in the GoalAlign and PathAlign critics projects the current pose forward a default 0.325 meters before scoring the trajectory. This can cause velocities that create sharp turns into obstacles to be chosen (reproducible with the Turtlebot3 simulation).

For TB3, which is small in size, 0.325 meters is too far. Instead, enable GoalAlign and PathAlign critics in the DWB controller and shorten the critics' forward_point_distance (how far the current pose is project given the current orientation before scoring the trajectory) from the default 0.325 meters to 0.1 meters which improves stability when running with TB3.

Signed-off-by: Jack Pien <jack@jackpien.com>

* Revert libgazebo11 workaround

* Revert gazebo_ros_pkgs to main ros2 branch

* parametrized collision_checker getCurrentPose timeout

* Parameterized tf_tol for spin and backup recoveries

* Parameterized tf_tol for goal_reached_condition

* moved transform_tolerance_ to recovery.hpp

* moved transform_tolerance parameter declaration to bt_navigator

* Fixed typo

* Fixed transform_tolerance_ location and transform_tolerance param declaration location

* Parameterized tf_tol for distance_controller.cpp

as tests for connext rmw have been disabled.54

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

* Add collision checking for footprint without using subscibers.

* Address reviewer's comments

- Changed the design if the footprint collision checkers
- And propogate the changes to dependencies such as nav2_recoveries and nav2_core

* Remove some extra headers

* Remove debuging code

* Add requested test

* Change weird test names.

* Remove unorientFootprint function dependency

* Imporve tests

* Fix commented Varible

* Add BehaviorTree visualization support using Groot

* Add nav2 TreeNodesModel containing all BT Nodes used

* Add instructions on using Groot to visualize behavior trees

Signed-off-by: Sarathkrishnan Ramesh <sarathkrishnan99@gmail.com>

* Rearrange files and minor updates

* Move nav2_tree_nodes.xml and groot instruction to nav2_beahvior_tree

* Run cmake install rules for nav2_tree_nodes.xml

Signed-off-by: Sarathkrishnan Ramesh <sarathkrishnan99@gmail.com>

* Add distance controller node in nav2 TreeNodesModel

Signed-off-by: Sarathkrishnan Ramesh <sarathkrishnan99@gmail.com>

* Add option to inflate around unknown space

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

* Fix bug regarding lower bound of double in worldToMapEnforceBounds

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

* Convert 2D caches to 1D vectors for automatic memory management and better locality

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

* Add general optimizations and modern syntax

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

* Switch from map<double> to vector<> in using precached integer distances

Credits to original author from https://github.com/ros-planning/navigation/pull/839



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

* Add tests for inflate_unknown and inflate_around_unknown

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

* Remove commented out assert

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

* Add DistanceController decorator node

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

* Update documentation and rename behavior tree XMLs

* Add tests for distance controller

* Fix test

* Update readme and add BT image

* Rename test files

* Remove protected setStatus calls

* Fix uninitialized warning for variable temp_tf_tol.

* Change default GridBased.tolerance to 0.5 meters