…avior (ros-navigation#3650)

* fixing path angle critic's non-directional bias

* adding reformat

* handle linting

* add utility unit tests

* adding unit tests for path angle

…#3659)

* alternative to path align critic for inversion control

* fix default behavior (enforce_path_inversion: false) (ros-navigation#3643)

Co-authored-by: Guillaume Doisy <[email protected]>

* adding dyaw option for path alignment to incentivize following the path's intent where necessary

* add docs for use path orientations

* fix typo

---------

Co-authored-by: Guillaume Doisy <[email protected]>
Co-authored-by: Guillaume Doisy <[email protected]>

* Resolves 3646: Update CMAKE_COMPILER_IS_GNUCXX

* Update CMakeLists.txt

* Fix smoother server tests

* Update test_smoother_server.cpp

…igation#3605)

* CI test fixing for BT.CPP (paired with BehaviorTree.CPP/579)

* lint

* fixing smoother unit test failures

* fix some new build warnings

* fixing last issue

* Update navigate_through_poses_action.hpp

* adding unsigned int to tests

* all to unsigned shorts

* test new warning resolution

* Update

* convert unsigned shorts to uint16_t for linter

* added printouts

* ignore system tests

* fix

* cleanup

* Update test_costmap_2d_publisher.cpp

remove space

…tion#3692)

* Fix max_points -> min_points in parameters
* Move robot_utils.hpp include to source where it actually using
* Remove double-description of getTransform()

…avigation#3693)

* moved error code enumerations to the result of action messages

* test fixes

* code review

Signed-off-by: ymd-stella <[email protected]>

…os-navigation#3690)

* Move last_cmd update before deadband

* fix lint

* onCycle has access to the result

* revert

* pr review

…itialization (ros-navigation#3720)

It is very useful to know the current location considered by the
bt_navigator for debug purposes.

It allows external packages to include those headers and create child
classes through inheritance.

* Modified nav2_behavior_tree and nav2_bt_navigator to use bt_navigator parameters as backup for behavior tree parameters

* Modified nav2_behavior_tree and nav2_bt_navigator to use bt_navigator parameters as backup for behavior tree parameters

* implemented feedback from pull request

* fixed RCLCPP_DEBUG error

Signed-off-by: vboxuser <[email protected]>

* fixed hpp layout, removed previous code line in distance_controller

* fixed all CI/CD errors, modified tests

* fixed bt_navigator coding style. Modified behavior trees for updated nodes

* implemented comments, fixed distance in distance_controller

* updated circleci v14 to v15

---------

Signed-off-by: vboxuser <[email protected]>
Co-authored-by: vboxuser <[email protected]>

… and Smac which impact performance substantially (ros-navigation#3728)

* adding error warnings around incorrect inflation layer setups in MPPI and Smac which impact performance substantially

* fix test failures

…d acceleration limits (ros-navigation#3731)

* Fix GoalUpdater QoS

* Fixes

…-navigation#3734)

…vigation#3733)

* smach_planner_hybrid: add support visualization for hybrid Astar

* smac_planner_hyrid: revert some

* smach_planner_hybrid: improving code quality

* utils: add some useful functions

* utils: fix mistake

* nav2_smac_planner: fix format problem

* utils: fix format and revise functions

* smach_planner_hybrid: delete _viz_expansion parameter

* smac_planner_hybrid: fix format

* README: update parameter

* utils: corrct mistake return

* utils: make timestamp a const reference

* nav2_smac_planner: correct format problem

* add unit test functions

* further detection of element equality

* test_utils: add non-trival translation and rotation

* smac_planner_hybrid: pass value instead of references

* completing hybrid A* visualization

---------

Co-authored-by: xianglunkai <[email protected]>

* Update README.md

* Update README.md

* adding uid to the logged message

Signed-off-by: Christian Henkel <[email protected]>

* less changes

Signed-off-by: Christian Henkel <[email protected]>

---------

Signed-off-by: Christian Henkel <[email protected]>

…ion#3751)

* rviz view straight in default xy orientation

Signed-off-by: Christian Henkel <[email protected]>

* gazebo orientation to match rviz

Signed-off-by: Christian Henkel <[email protected]>

* rotating in direction of view

---------

Signed-off-by: Christian Henkel <[email protected]>

1. Set forward_prune_distance to 1.0 to robot not getting lost
2. Correct map name for costmap filter tests

Signed-off-by: ymd-stella <[email protected]>

…ore launch navigation. (ros-navigation#3572)

* Make it possible to launch namspaced robot which rewrites `<robot_namespace>` to namespace.
- It allows to apply namespace automatically on specific target topic path in costmap plugins.

Add new nav2 params file for multi-robot(rewriting `<robot_namespace>`) as an example.
- nav2_multirobot_params_all.yaml

Modify nav2_common.ReplaceString
- add condition argument

* Update nav2_bringup/launch/bringup_launch.py

Co-authored-by: Steve Macenski <[email protected]>

* Add new luanch script for multi-robot bringup

Rename luanch script for multi-robot simulation bringup

Add new nav2_common script
- Parse argument
- Parse multirobot pose

Update README.md

* Update README.md

Apply suggestions from code review

Fix pep257 erors

Co-authored-by: Steve Macenski <[email protected]>

---------

Co-authored-by: Steve Macenski <[email protected]>

* use ROS clock for wait

* fix backport issue

---------

Co-authored-by: Guillaume Doisy <[email protected]>

…3792)

* fixing external users of the BT action node template

* Update nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

Co-authored-by: Guillaume Doisy <[email protected]>

---------

Co-authored-by: Guillaume Doisy <[email protected]>

Bumps [actions/checkout](https://github.com/actions/checkout) from 3 to 4.
- [Release notes](https://github.com/actions/checkout/releases)
- [Changelog](https://github.com/actions/checkout/blob/main/CHANGELOG.md)
- [Commits](actions/checkout@v3...v4)

---
updated-dependencies:
- dependency-name: actions/checkout
  dependency-type: direct:production
  update-type: version-update:semver-major
...

Signed-off-by: dependabot[bot] <[email protected]>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>

…ons (ros-navigation#3752)

* adding intial prototype for smac hybrid-A* multiple primitives quantization interpolation

* fixing unsigned int issue

* cleaning up expansions log

* fix primitives to be of equal dist

* adding test for expansions getting

* fixing tests from changes

* adding a theoretical guardrail

* updating Hybrid-A* to include directional information in the primitives for enabling multi-primitive planning & accurate traversal cost functions

* moving Hybrid-A* specifics to template specialization

* expand support for debug visualizations to State Lattice and 2D A*'s

* fix some test oddities

* dynamic parameters

* remove unused variable

* minor changes

* adding options for downsampling obstacle heurisic and for using quadratic cost penalty functions

* fixing unit tests

…#3799)

Signed-off-by: Mannucci, Anna (Bosch (CR)) <[email protected]>
Co-authored-by: Mannucci, Anna (Bosch (CR)) <[email protected]>

* Fixes for flaky WPF test:
* New RewrittenYaml ability to add non-existing parameters
* Prune distance fix for WPF test
* Treat UNKNOWN status as error in WPF
* Clear error codes after BT run
* Remove unnecessary setInitialPose() from WPF test

* Update nav2_waypoint_follower/src/waypoint_follower.cpp

Co-authored-by: Steve Macenski <[email protected]>

* Clean error code in any situation

* Fix UNKNOWN WPF status handling

---------

Co-authored-by: Steve Macenski <[email protected]>

* Fix min_points checking

…rvers (ros-navigation#3787)

* Expose action server default timeout in bt navigator servers

* typo

* duplicated comment

* Expose result timeout in other actions

* Proper timeout in bt node

* Change default timeouts and remove comments

* Remove comment in params file

* uncrustify controller server

* Update collision_monitor_params.yaml

* Update collision_monitor_node.cpp

* Update default topics in collision_monitor_node.cpp

* Update default topics in collision_monitor_params.yaml

* Update collision_monitor_params.yaml

* Update collision_monitor_node.cpp

* Update default topics in collision_monitor_node.cpp

* Update default topics in collision_monitor_params.yaml

)

* support multirobot namespaces

* add docs

Bumps [docker/build-push-action](https://github.com/docker/build-push-action) from 4 to 5.
- [Release notes](https://github.com/docker/build-push-action/releases)
- [Commits](docker/build-push-action@v4...v5)

---
updated-dependencies:
- dependency-name: docker/build-push-action
  dependency-type: direct:production
  update-type: version-update:semver-major
...

Signed-off-by: dependabot[bot] <[email protected]>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>

Bumps [docker/login-action](https://github.com/docker/login-action) from 2 to 3.
- [Release notes](https://github.com/docker/login-action/releases)
- [Commits](docker/login-action@v2...v3)

---
updated-dependencies:
- dependency-name: docker/login-action
  dependency-type: direct:production
  update-type: version-update:semver-major
...

Signed-off-by: dependabot[bot] <[email protected]>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>

Bumps [docker/setup-buildx-action](https://github.com/docker/setup-buildx-action) from 2 to 3.
- [Release notes](https://github.com/docker/setup-buildx-action/releases)
- [Commits](docker/setup-buildx-action@v2...v3)

---
updated-dependencies:
- dependency-name: docker/setup-buildx-action
  dependency-type: direct:production
  update-type: version-update:semver-major
...

Signed-off-by: dependabot[bot] <[email protected]>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>

…cpp) (ros-navigation#3804)

* adding copy all params primitive

* fix linting

* lint

* I swear to god, this better be the last linting issue

* allowing params to be declared from yaml

* Update bt_navigator.cpp

…tmap (ros-navigation#3816)

otherwise the controller crashes at ObstaclesCritic::costAtPose
because x_i and y_i isn't initialized.

* Fix CD configuration page reference

* Add CM work on 6th ROS Developers Day reference

Signed-off-by: Anton Kesy <[email protected]>

…os-navigation#3827)

* floating point calculations

* Update optimizer_unit_tests.cpp

* Update critics_tests.cpp

* Update critics_tests.cpp

… use

* Add nav2_gps_waypoint_follower

* use correct client node while calling it to spin

* changed after 1'st review

* apply requested changes

* nav2_util::ServiceClient instead of CallbackGroup

* another  iteration to adress issues

* update poses with function in the follower logic

* add deps of robot_localization: diagnostics

* fix typo in underlay.repo

* add deps of robot_localization: geographic_info

* minor clean-ups

* bond_core version has been updated

* rotation should also be considered in GPS WPFing

* use better namings related to gps wpf orientation

* handle cpplint errors

* tf_listener needs to be initialized

* apply requested changes

* apply requested changes 3.0/3.0

* fix misplaced ";"

* use run time param for gps transform timeout

* change timeout var name

* make use of stop_on_failure for GPS too

* passing emptywaypont vectors are seen as failure

* update warning for empty requests

* consider utm -> map yaw offset

* fix missed RCLCPP info

* reorrect action;s name

* waypoint stamps need to be updated

* Fix segmentation fault on waypoint follower

* Parametric frames and matrix multiplications

* Replace oriented navsatfix for geographic_msgs/geopose

* Remove deprecated oriented  navsatfix message

* Update branch name on robot_localization dependency

* Fix parametric frames logic

* Rename functions and adress comments

* fix style in underlay.repos

* remove duplicate word in underlay.repos

* update dependency version of ompl

* Template ServiceClient class to accept lifecycle node

* Remove link to stackoverflow answer

* Remove yaw offset compensation

* Fix API change

* Fix styling

* Minor docs fixes

* Fix style divergences

* Style fixes

* Style fixes v2

* Style fixes v3

* Remove unused variables and timestam overrides

* restore goal timestamp override

* WIP: Add follow gps waypoints test

* Style fixes and gazebo world inertia fix

* Reduce velocity smoother timeout

* empty commit to rerun tests

* Increment circle ci cache idx

* Remove extra space in cmakelists.txt

* Fix wrong usage of the global action server

* update follow gps waypoints action definition

* Fix action definition and looping

* update params for the unit testing

* WIP: update tests

* fix tests

* fixes to nav2 simple commander

* add robot_localization localizer

* Bring back from LL client

* Update nav2_simple_commander/nav2_simple_commander/robot_navigator.py

Co-authored-by: Steve Macenski <[email protected]>

* missing argument in test function

* small test error

* style fixes nav2 simple commander

* rename cartesian action server

---------

Co-authored-by: jediofgever <[email protected]>
Co-authored-by: Steve Macenski <[email protected]>

…igation#3825)

* Rename PushRosNamespace to PushROSNamespace

* Fix min_points checking

* initial

* fix

* fix

* remove unrelated change

* reset

* fix format

* PR fixes

* Add test

* fix comments

* add to params

* publish only if enabled

* Add source dynamic enable/disable

* add enabled param to sources

* fix

* add same to collision detector

* amcl: declare void parameter for functions with no args

Signed-off-by: Rhys Mainwaring <[email protected]>

* amcl: remove unused variables

Signed-off-by: Rhys Mainwaring <[email protected]>

* behavior_tree: address clang compilation issues

Signed-off-by: Rhys Mainwaring <[email protected]>

* behaviors: add missing override specifier

Signed-off-by: Rhys Mainwaring <[email protected]>

* bt_navigator: add missing override specifier

Signed-off-by: Rhys Mainwaring <[email protected]>

* collision_monitor: address clang compilation issues

Signed-off-by: Rhys Mainwaring <[email protected]>

* collision_monitor: remove unused variables

Signed-off-by: Rhys Mainwaring <[email protected]>

* constrained_smoother: address clang compilation issues

Signed-off-by: Rhys Mainwaring <[email protected]>

* controller: address clang compilation issues

Signed-off-by: Rhys Mainwaring <[email protected]>

* costmap_2d: add missing override specifier

Signed-off-by: Rhys Mainwaring <[email protected]>

* costmap_2d: address clang compilation issues

Signed-off-by: Rhys Mainwaring <[email protected]>

* costmap_2d: fix link issue for order_layer

Signed-off-by: Rhys Mainwaring <[email protected]>

* dwb_controller: fix clang compile issue, replace ulong with uint32_t

Signed-off-by: Rhys Mainwaring <[email protected]>

* map_server: replace std::experimental::filesystem

Signed-off-by: Rhys Mainwaring <[email protected]>

* map_server: remove dependency on stdc++fs

Signed-off-by: Rhys Mainwaring <[email protected]>

* waypoint_follower: address clang compilation issues

Signed-off-by: Rhys Mainwaring <[email protected]>

* waypoint_follower: remove dependency on stdc++fs

Signed-off-by: Rhys Mainwaring <[email protected]>

* waypoint_follower: replace std::experimental::filesystem

Signed-off-by: Rhys Mainwaring <[email protected]>

* smoother: address clang compilation issues

Signed-off-by: Rhys Mainwaring <[email protected]>

* smoother: remove unused variables

Signed-off-by: Rhys Mainwaring <[email protected]>

* system_tests: remove dependency on stdc++fs

Signed-off-by: Rhys Mainwaring <[email protected]>

* rotation_shim_controller: update percentage arg in setSpeedLimit to boolean

Signed-off-by: Rhys Mainwaring <[email protected]>

* planner: remove unused variables

Signed-off-by: Rhys Mainwaring <[email protected]>

* costmap_2d: address clang compilation issues

Signed-off-by: Rhys Mainwaring <[email protected]>

* mppi_controller: replace use of auto as function param with templates

Signed-off-by: Rhys Mainwaring <[email protected]>

* mppi_controller: address clang compilation issues

Signed-off-by: Rhys Mainwaring <[email protected]>

* costmap_2d: resolve clang issue with std::pair non-const copy

Signed-off-by: Rhys Mainwaring <[email protected]>

* smac_planner: suppress unused parameter warnings

Signed-off-by: Rhys Mainwaring <[email protected]>

* behavior_tree: fix code style

Signed-off-by: Rhys Mainwaring <[email protected]>

* costmap_2d: fix code style

Signed-off-by: Rhys Mainwaring <[email protected]>

* mppi_controller: revert reindexing of for-loop

Signed-off-by: Rhys Mainwaring <[email protected]>

* smoother: remove commented code

Signed-off-by: Rhys Mainwaring <[email protected]>

* system_tests: remove commented target link library

Signed-off-by: Rhys Mainwaring <[email protected]>

* collision_monitor: revert parameter type to vector and use explicit constructor

Signed-off-by: Rhys Mainwaring <[email protected]>

---------

Signed-off-by: Rhys Mainwaring <[email protected]>

BT.CPP_v3 is used, thereby the correct docs should be linked

* Fix bug in nav2_behavior_tree/bt_action_node

* Fixed the bug in halt function inside
  nav2_behavior_tree/plugin/action/bt_action_node.hpp

* Added new case to nav2_behavior_tree/plugin/action/bt_action_node.hpp
  for testing the scenario to cancel

* Refactored existing cases in
  nav2_behavior_tree/plugin/action/bt_action_node.hpp

Signed-off-by: CihatAltiparmak <[email protected]>

* Fix bug in nav2_behavior_tree/bt_action_node

* Fixed the bug in halt function inside
  nav2_behavior_tree/plugin/action/bt_action_node.hpp

* Added new case to nav2_behavior_tree/plugin/action/bt_action_node.hpp
  for testing the scenario to cancel

* Refactored existing cases in
  nav2_behavior_tree/plugin/action/bt_action_node.hpp

Signed-off-by: CihatAltiparmak <[email protected]>

---------

Signed-off-by: CihatAltiparmak <[email protected]>

…s-navigation#3852)

* Update transform_available_condition.cpp

* wrapping all examples of get_plugin_type_param in exceptions and making that throw instead of crash

* some linting

* fix typo

* Update controller.cpp

* Update nav2_params.yaml

* Update nav2_params.yaml

…navigation#3859)

* simplification of equation to compute the max_value/outer edge of the lattice based on number of headings.

* Added nodestate enum and a variable to keep track of current state of managed nodes.

* Updating state_of_managed_nodes_ when switching states and using it to determine an accurate diagnostics message.

* Fixing bugs.

* Updated/added docstrings.

* Publishing OK status when nodes are unconfigured. Changed if-else chain to switch case.

* Renamed NodeState PAUSED to INACTIVE, state_of_managed_nodes_ to managed_nodes_state_ and replaced system_active_ with an inline method.

* Bugfix.

---------

Co-authored-by: Pekka Myller <[email protected]>

…navigation#3868)

* adding regenerate noise param + adding docs

* fix tests

* remove unnecessary normalization

* Update optimizer.cpp

…gation#3871)

* change duration tyype from int to double

* change "1" to "1.0" as default wait_duration value

* modify wait_action tests

* fix

* change int values to double in trees

* add usage of Duration::from_seconds()

* fix build failing issue

* delete whitespace

…on. (ros-navigation#3866)

* Add a timeout to the wait for transforms step of the costmap activation.

Signed-off-by: Fabian König <[email protected]>

* Rename wait_for_transforms_timeout to initial_transform_timeout

* Activate costmap publishers only after transforms are checked

* Check if controller server activation was succesful in planner_server

* Add unittest for costmap activation

Signed-off-by: Fabian König <[email protected]>

---------

Signed-off-by: Fabian König <[email protected]>

…Edit: strike that, now 80% (ros-navigation#3872)

* adding regenerate noise param + adding docs

* fix tests

* remove unnecessary normalization

* Update optimizer.cpp

* adding refactored path alignment critic

* fix visualization bug

* speed up another 30%

* remove a little jitter

* a few more small optimizaitons

* fixing unit tests

* retain legacy critic

* adding tests for legacy

* Use mutex to protect costmap reads.
Otherwise costmap can be read during a map update and return 0.

* Revert "Use mutex to protect costmap reads."

This reverts commit e16a44c.

* Lock costmap before running MPPI controller.

* Fix typo.

* Protect against costmap updates in MPP and RotationShim controllers.

---------

Co-authored-by: Leif Terry <[email protected]>

Signed-off-by: Anton Kesy <[email protected]>

…-navigation#3891 (ros-navigation#3900)

* image.hpp ros-navigation#3891

* Update image.hpp

* Rename PushRosNamespace to PushROSNamespace

* Fix min_points checking

* .

* fixes

* add to collision detector

* fix

* fix

* .

* fixes

* add namespace to topic

* fixes

…vigation#3911)

* fixing subtree issues

* Update bt_action_server_impl.hpp

Signed-off-by: Ignacio Vizzo <[email protected]>

* Add block_if_invalid and allow sensor specific  source_timeout

* remove block_if_invalid param and make it default behavior

* allow unblocking if data not yet published

* log source name when invalid

* getData return true if invalid AND source_timeout == 0.0

* fixed logic without source feedback

* fix test

* rebase artefact

* format artefact

* better log

* move per sensor param source_timeout logic to source.cpp

* fix ignore invalid source behavior

* add source_timeout tests

* no needed anymore

* fix testSourceTimeoutOverride test

---------

Co-authored-by: Guillaume Doisy <[email protected]>

Signed-off-by: kevin <[email protected]>
Co-authored-by: kevin <[email protected]>