Skip to content

Commit

Permalink
Nav2 Sync 3 2024 April 9 (#15)
Browse files Browse the repository at this point in the history
* Fix ros-navigation#4001 (ros-navigation#4028)

* Humble Sync 9: Jan 23, 2024 (ros-navigation#4064)

* Update README.md: fix typo (ros-navigation#3842)

* Update package.xml

* fix typo (ros-navigation#3850)

* adjust link to point to v3.8 of behavior tree docs (ros-navigation#3851)

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

* Fix bug in nav2_behavior_tree/bt_action_node (ros-navigation#3849)

* 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 <cihataltiparmak1@gmail.com>

* 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 <cihataltiparmak1@gmail.com>

---------

Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com>

* simplication of lattice_generator.py, fixes ros-navigation#3858 (ros-navigation#3859)

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

* Fix class doxygen

* fix minor typos (ros-navigation#3892)

Signed-off-by: Anton Kesy <antonkesy@gmail.com>

* fix use after free (ros-navigation#3910)

* fix build mppi (ros-navigation#3927)

Signed-off-by: kevin <kevin@floatic.io>
Co-authored-by: kevin <kevin@floatic.io>

* Removing old TODOs

* Fixed links for install and build in README (ros-navigation#3963)

Currently the readme is linking to an invalida page in the docs (404 error).

* fix a few outdated comments in smac planners (ros-navigation#3978)

* Update README.md

* bump to 1.1.13 for release

---------

Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com>
Signed-off-by: Anton Kesy <antonkesy@gmail.com>
Signed-off-by: kevin <kevin@floatic.io>
Co-authored-by: thandal <than@timbrel.org>
Co-authored-by: Anton Kesy <antonkesy@gmail.com>
Co-authored-by: CihatAltiparmak <cihataltiparmak1@gmail.com>
Co-authored-by: Anil Kumar Chavali <44644339+akchobby@users.noreply.github.com>
Co-authored-by: 정찬희 <60467877+ladianchad@users.noreply.github.com>
Co-authored-by: kevin <kevin@floatic.io>
Co-authored-by: Abiel Fernandez <empoleom@gmail.com>
Co-authored-by: Michael Ferguson <mfergs7@gmail.com>

* Fixes for compiling with clang on macOS (ros-navigation#4051)

amcl: declare void parameter for functions with no args

amcl: remove unused variables

behavior_tree: address clang compilation issues

behaviors: add missing override specifier

bt_navigator: add missing override specifier

collision_monitor: address clang compilation issues

constrained_smoother: address clang compilation issues

controller: address clang compilation issues

costmap_2d: add missing override specifier

costmap_2d: address clang compilation issues

costmap_2d: fix link issue for order_layer

dwb_controller: fix clang compile issue, replace ulong with uint32_t

map_server: replace std::experimental::filesystem

map_server: remove dependency on stdc++fs

waypoint_follower: address clang compilation issues

waypoint_follower: remove dependency on stdc++fs

waypoint_follower: replace std::experimental::filesystem

smoother: address clang compilation issues

smoother: remove unused variables

system_tests: remove dependency on stdc++fs

rotation_shim_controller: update percentage arg in setSpeedLimit to boolean

planner: remove unused variables

costmap_2d: address clang compilation issues

mppi_controller: replace use of auto as function param with templates

mppi_controller: address clang compilation issues

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

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

* critical fix for path tracking MPPI (ros-navigation#4099) (ros-navigation#4101)

(cherry picked from commit 8f7b6ce)

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

* backport graceful controller to humble (ros-navigation#4119)

* Added cast to float to getClosestAngularBin return for behavior consistency (ros-navigation#4123) (ros-navigation#4125)

* Added cast to float to getClosestAngularBin return for behavior consistency.

Signed-off-by: Hunter Song <hsong@macleanengineering.com>

* Revised test name

Signed-off-by: Hunter Song <hsong@macleanengineering.com>

---------

Signed-off-by: Hunter Song <hsong@macleanengineering.com>
(cherry picked from commit c59e0f3)

Co-authored-by: hsong-MLE <158601314+hsong-MLE@users.noreply.github.com>

* Fix goal handle was not freed correctly (ros-navigation#4137) (ros-navigation#4143)

* Fix goal handle was not freed correctly

* Update nav2_util/include/nav2_util/simple_action_server.hpp

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

---------

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Co-authored-by: Ziohang <hang.zhou@agvsz.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
(cherry picked from commit 2e491b1)

Co-authored-by: iceuw <m201770623@hust.edu.cn>

* bug fix (ros-navigation#4160) (ros-navigation#4162)

Signed-off-by: Tianchu <tianchu.zhang0706@gmail.com>
(cherry picked from commit 623434b)

Co-authored-by: Chortine <107395103+Chortine@users.noreply.github.com>

* Add uint suffix (ros-navigation#4213) (ros-navigation#4216)

(cherry picked from commit 46ff0ec)

Co-authored-by: Michael Wrock <michael.wrock@picknik.ai>

* Fix merge conflict in nav2_mppi_controller README (humble) (ros-navigation#4239)

Signed-off-by: Stephanie Eng <stephanie-eng@users.noreply.github.com>

* Humble sync 10: April 4, 2024 (ros-navigation#4241)

* change pointer free order in amcl to avoid use-after-free bug mentioned in ros-navigation#4068 (ros-navigation#4070)

Signed-off-by: GoesM <GoesM@buaa.edu.cn>
Co-authored-by: GoesM <GoesM@buaa.edu.cn>

* Allow path end pose deviation revive (ros-navigation#4065)

* Support stitching paths in compute path to poses

* Update nav2_planner/src/planner_server.cpp

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

* Rename parameter to allow_path_through_poses_goal_deviation

* Fix description

* restore nav2_params

* missing whitespace

* lint fix

* removed parameter

Signed-off-by: gg <josho.wallace@gmail.com>

* Update planner_server.hpp

* Update planner_server.cpp

---------

Signed-off-by: gg <josho.wallace@gmail.com>
Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co>
Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Change costmap_queue to shared library (ros-navigation#4072)

Signed-off-by: cybaol <cybao292261@163.com>

* free `map_sub_` before `map_free(map_)` to avoid UAF&&NullPtr bug mentioned in ros-navigation#4078 (ros-navigation#4079)

* free `map_sub_` before `map_free(map_)`

Signed-off-by: GoesM <GoesM@buaa.edu.cn>

* reformat

Signed-off-by: GoesM <GoesM@buaa.edu.cn>

---------

Signed-off-by: GoesM <GoesM@buaa.edu.cn>
Co-authored-by: GoesM <GoesM@buaa.edu.cn>

* adding progress checker selector BT node (ros-navigation#4109)

* New MPPI Cost Critic (Contrib: Brice Renaudeau) (ros-navigation#4090)

* Share code

Signed-off-by: Brice <brice.renaudeau@gmail.com>

* Update inflation_cost_critic.hpp

- copyright
- ifndef

Signed-off-by: Brice <brice.renaudeau@gmail.com>

* fix lint cpp

- extra space

Signed-off-by: Brice <brice.renaudeau@gmail.com>

* Fix Smac Planner confined collision checker  (ros-navigation#4055)

* Update collision_checker.cpp

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

* Fix tests

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

* Update test_a_star.cpp

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

---------

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: Brice <brice.renaudeau@gmail.com>

* Prevent analytic expansions from shortcutting Smac Planner feasible paths (ros-navigation#3962)

* a potential solution to smac shortcutting

* costmap reoslution

* some fixes

* completed prototype

* some fixes for collision detection and performance

* completing shortcutting fix

* updating tests

* adding readme

---------

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: Brice <brice.renaudeau@gmail.com>

* change pointer free order in amcl to avoid use-after-free bug mentioned in ros-navigation#4068 (ros-navigation#4070)

Signed-off-by: GoesM <GoesM@buaa.edu.cn>
Co-authored-by: GoesM <GoesM@buaa.edu.cn>
Signed-off-by: Brice <brice.renaudeau@gmail.com>

* [Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings (ros-navigation#4067)

* prototype to test SE2 footprint H improvements

* some fixes

* fixed

* invert logic

* Working final prototype to be tested

* complete unit test conversions

* Update inflation_layer.hpp

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

---------

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: Brice <brice.renaudeau@gmail.com>

* Adding new Smac paper to readme

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: Brice <brice.renaudeau@gmail.com>

* Update README.md

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: Brice <brice.renaudeau@gmail.com>

* [behavior_tree] don't repeat yourself in "blackboard->set" (ros-navigation#4074)

* don't repeat yourself: templates in tests

Signed-off-by: Davide Faconti <davide.faconti@gmail.com>

* misse change

Signed-off-by: Davide Faconti <davide.faconti@gmail.com>

---------

Signed-off-by: Davide Faconti <davide.faconti@gmail.com>
Signed-off-by: Brice <brice.renaudeau@gmail.com>

* Allow path end pose deviation revive (ros-navigation#4065)

* Support stitching paths in compute path to poses

* Update nav2_planner/src/planner_server.cpp

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

* Rename parameter to allow_path_through_poses_goal_deviation

* Fix description

* restore nav2_params

* missing whitespace

* lint fix

* removed parameter

Signed-off-by: gg <josho.wallace@gmail.com>

* Update planner_server.hpp

* Update planner_server.cpp

---------

Signed-off-by: gg <josho.wallace@gmail.com>
Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co>
Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: Brice <brice.renaudeau@gmail.com>

* Updated code to use getInflationLayer() method (ros-navigation#4076)

* updated code to use getInflationLayer method

Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com>

* Fix linting

Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com>

---------

Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com>
Signed-off-by: Brice <brice.renaudeau@gmail.com>

* 1594 twist stamped publisher (ros-navigation#4077)

* Add TwistStamped to controller_server via TwistPublisher util

* Add a new util class for publishing either Twist or TwistStamped
* Add a new parameter for selecting to stamp the twist data
* Consume TwistPublisher in nav2_controller

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* Fix small issues

* Unused variable
* Incorrect doxygen

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* Remove stored node and assert

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* Add tests for node

* Facing timeout even though it does the same thing as velocity smoother test

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* Add missing spin call to solve timeout

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* Fix copyright (me instead of intel)

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* Add full test coverage with subscriber

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* Remove unused rclcpp fixture

* Can't use it due to needing to join the pub thread after rclcpp shuts down

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* Use TwistStamped in nav2_behaviors

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* Use TwistStamped in collision monitor node

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* Add TwistStamped readme updates to velocity smoother

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* Add TwistSubscriber implementation

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* Fix syntax errors

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* Use TwistSubscriber in test_velocity_smoother

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* Use TwistSubscriber in assisted_teleop

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* Use TwistSubscriber in collision monitor node

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* Use TwistSubscriber in velocity smoother

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* Remove unused code

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* add timestamp and frame_id to TwistStamped message

* Add missing utility include

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* Document TwistPublisher and TwistSubscriber usage

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* Use pass-by-reference

* Instead of std::move(std::unique_ptr<TwistStamped>)

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* Finish twist subscriber tests

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* Add other constructor and docs

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* Fix linter issues

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* Manually fix paren alignment

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* Remove GSoC reference

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* Document twist bool param in README

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* Handle twistPublisher in collision monitor

* Implement behavior in the stamped callback
* Unstamped callback calls the stamped callback
* Switch to unique pointer for publisher

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* Convert to using TwistStamped interally

* Use incoming twistStamped timestamp if available
* Convert all internal representations to use TwistStamped

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* Remove nav2_util usage instructions

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* Remove unused Twist only subscriber

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* More linter fixes

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* Prefer working with unique_ptr for cmd_vel

* This makes it easier to switch to std::move instead of dereference on
  publish

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* Completing twist stamped migration

* shared to unique ptr

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

* twist add stamps and properly propogated

* nav2_util: fix for compiling with clang

- Resolve error: moving a temporary object prevents copy elision [-Werror,-Wpessimizing-move]

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

---------

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>
Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com>
Signed-off-by: Brice <brice.renaudeau@gmail.com>

* Change costmap_queue to shared library (ros-navigation#4072)

Signed-off-by: cybaol <cybao292261@163.com>
Signed-off-by: Brice <brice.renaudeau@gmail.com>

* fix include of hpp

Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com>

* inflation cost optmiizations and cleanu

* rename, add defaults, and docs

* smoke test addition

* lintg

* normalize weight

* update readme

* increment cache

* Update cost_critic.hpp

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

* Update cost_critic.cpp

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

---------

Signed-off-by: Brice <brice.renaudeau@gmail.com>
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: GoesM <GoesM@buaa.edu.cn>
Signed-off-by: Davide Faconti <davide.faconti@gmail.com>
Signed-off-by: gg <josho.wallace@gmail.com>
Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com>
Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>
Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>
Signed-off-by: cybaol <cybao292261@163.com>
Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com>
Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com>
Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com>
Co-authored-by: GoesM <GoesM@buaa.edu.cn>
Co-authored-by: Davide Faconti <davide.faconti@gmail.com>
Co-authored-by: Joshua Wallace <josho.wallace@gmail.com>
Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co>
Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com>
Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com>
Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com>
Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com>
Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com>
Co-authored-by: Kino <cybao292261@163.com>

* Use ament_export_targets for all targets (ros-navigation#4112)

* Matches new internal ALIAS targets
* Use ALIAS targets for all internal linkage
* Remove unnecessary calls to ament_target_dependencies in test code
* Export includes in proper folders for overlays in colcon

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* Update default recommendation from Obstacles to Cost critic in MPPI (ros-navigation#4170)

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

* completely shutdown inital_pose_sub_ (ros-navigation#4176)

Signed-off-by: GoesM <GoesM@buaa.edu.cn>
Co-authored-by: GoesM <GoesM@buaa.edu.cn>

* fix typos in description messages (ros-navigation#4188)

Signed-off-by: Antonio Park <kaypaklabs@gmail.com>

* AMCL: Set an initial guess by service call (ros-navigation#4182)

* Added initial guess service. Signed-off-by: Alexander Mock

Signed-off-by: Alexander Mock <amock@uos.de>

* - Removed added empty line
- Renamed initialGuessCallback to initialPoseReceivedSrv
- Added new line to SetInitialPose service definition
- Removed mutex from initialPoseReceived
- Cleanup service server

Signed-off-by: Alexander Mock <amock@uos.de>

* added whitespace

Signed-off-by: Alexander Mock <amock@uos.de>

* renamed initial pose service in callback bind

Signed-off-by: Alexander Mock <amock@uos.de>

---------

Signed-off-by: Alexander Mock <amock@uos.de>

* Move lines for pre-computation to outside a loop (ros-navigation#4191)

Signed-off-by: Kyungsik Park <dkalxkwl@gmail.com>

* Fix typo (ros-navigation#4196)

* Fix BT.CPP import

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Update README.md

---------

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Update footprint iif changed (ros-navigation#4193)

Signed-off-by: Brice <brice.renaudeau@gmail.com>

* bump humble to 1.1.14 for sync

* fix merge conflict

* custom version of cost critic's cost method

---------

Signed-off-by: GoesM <GoesM@buaa.edu.cn>
Signed-off-by: gg <josho.wallace@gmail.com>
Signed-off-by: cybaol <cybao292261@163.com>
Signed-off-by: Brice <brice.renaudeau@gmail.com>
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: Davide Faconti <davide.faconti@gmail.com>
Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com>
Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>
Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>
Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com>
Signed-off-by: Antonio Park <kaypaklabs@gmail.com>
Signed-off-by: Alexander Mock <amock@uos.de>
Signed-off-by: Kyungsik Park <dkalxkwl@gmail.com>
Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>
Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com>
Co-authored-by: GoesM <GoesM@buaa.edu.cn>
Co-authored-by: Joshua Wallace <josho.wallace@gmail.com>
Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co>
Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com>
Co-authored-by: Kino <cybao292261@163.com>
Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com>
Co-authored-by: Davide Faconti <davide.faconti@gmail.com>
Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com>
Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com>
Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com>
Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com>
Co-authored-by: Antonio Park <kaypaklabs@gmail.com>
Co-authored-by: Alexander Mock <amock@uos.de>
Co-authored-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Fix graceful humble release version

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

* Fix graceful_controller exceptions

---------

Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com>
Signed-off-by: Anton Kesy <antonkesy@gmail.com>
Signed-off-by: kevin <kevin@floatic.io>
Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>
Signed-off-by: Stephanie Eng <stephanie-eng@users.noreply.github.com>
Signed-off-by: GoesM <GoesM@buaa.edu.cn>
Signed-off-by: gg <josho.wallace@gmail.com>
Signed-off-by: cybaol <cybao292261@163.com>
Signed-off-by: Brice <brice.renaudeau@gmail.com>
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: Davide Faconti <davide.faconti@gmail.com>
Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com>
Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>
Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com>
Signed-off-by: Antonio Park <kaypaklabs@gmail.com>
Signed-off-by: Alexander Mock <amock@uos.de>
Signed-off-by: Kyungsik Park <dkalxkwl@gmail.com>
Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
Co-authored-by: thandal <than@timbrel.org>
Co-authored-by: Anton Kesy <antonkesy@gmail.com>
Co-authored-by: CihatAltiparmak <cihataltiparmak1@gmail.com>
Co-authored-by: Anil Kumar Chavali <44644339+akchobby@users.noreply.github.com>
Co-authored-by: 정찬희 <60467877+ladianchad@users.noreply.github.com>
Co-authored-by: kevin <kevin@floatic.io>
Co-authored-by: Abiel Fernandez <empoleom@gmail.com>
Co-authored-by: Michael Ferguson <mfergs7@gmail.com>
Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com>
Co-authored-by: mergify[bot] <37929162+mergify[bot]@users.noreply.github.com>
Co-authored-by: hsong-MLE <158601314+hsong-MLE@users.noreply.github.com>
Co-authored-by: iceuw <m201770623@hust.edu.cn>
Co-authored-by: Chortine <107395103+Chortine@users.noreply.github.com>
Co-authored-by: Michael Wrock <michael.wrock@picknik.ai>
Co-authored-by: Stephanie Eng <stephanie-eng@users.noreply.github.com>
Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com>
Co-authored-by: GoesM <GoesM@buaa.edu.cn>
Co-authored-by: Joshua Wallace <josho.wallace@gmail.com>
Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co>
Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com>
Co-authored-by: Kino <cybao292261@163.com>
Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com>
Co-authored-by: Davide Faconti <davide.faconti@gmail.com>
Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com>
Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com>
Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com>
Co-authored-by: Antonio Park <kaypaklabs@gmail.com>
Co-authored-by: Alexander Mock <amock@uos.de>
Co-authored-by: Tony Najjar <tony.najjar.1997@gmail.com>
  • Loading branch information
1 parent b7953db commit 7046850
Show file tree
Hide file tree
Showing 147 changed files with 4,284 additions and 337 deletions.
6 changes: 4 additions & 2 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -9,8 +9,8 @@
For detailed instructions on how to:
- [Getting Started](https://navigation.ros.org/getting_started/index.html)
- [Concepts](https://navigation.ros.org/concepts/index.html)
- [Build](https://navigation.ros.org/build_instructions/index.html#build)
- [Install](https://navigation.ros.org/build_instructions/index.html#install)
- [Build](https://navigation.ros.org/development_guides/build_docs/index.html#build)
- [Install](https://navigation.ros.org/development_guides/build_docs/index.html#install)
- [General Tutorials](https://navigation.ros.org/tutorials/index.html) and [Algorithm Developer Tutorials](https://navigation.ros.org/plugin_tutorials/index.html)
- [Configure](https://navigation.ros.org/configuration/index.html)
- [Navigation Plugins](https://navigation.ros.org/plugins/index.html)
Expand All @@ -20,6 +20,8 @@ For detailed instructions on how to:

Please visit our [documentation site](https://navigation.ros.org/). [Please visit our community Slack here](https://join.slack.com/t/navigation2/shared_invite/zt-hu52lnnq-cKYjuhTY~sEMbZXL8p9tOw) (if this link does not work, please contact maintainers to reactivate).

If you need professional services related to Nav2, please contact Open Navigation at info@opennav.org.

## Our Sponsors

Please thank our amazing sponsors for their generous support of Nav2 on behalf of the community to allow the project to continue to be professionally maintained, developed, and supported for the long-haul! [Open Navigation LLC](https://www.opennav.org/) provides project leadership, maintenance, development, and support services to the Nav2 & ROS community.
Expand Down
11 changes: 11 additions & 0 deletions nav2_amcl/include/nav2_amcl/amcl_node.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@
#include "nav2_amcl/sensors/laser/laser.hpp"
#include "nav2_msgs/msg/particle.hpp"
#include "nav2_msgs/msg/particle_cloud.hpp"
#include "nav2_msgs/srv/set_initial_pose.hpp"
#include "nav_msgs/srv/set_map.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "std_srvs/srv/empty.hpp"
Expand Down Expand Up @@ -210,6 +211,16 @@ class AmclNode : public nav2_util::LifecycleNode
const std::shared_ptr<std_srvs::srv::Empty::Request> request,
std::shared_ptr<std_srvs::srv::Empty::Response> response);

// service server for providing an initial pose guess
rclcpp::Service<nav2_msgs::srv::SetInitialPose>::SharedPtr initial_guess_srv_;
/*
* @brief Service callback for an initial pose guess request
*/
void initialPoseReceivedSrv(
const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<nav2_msgs::srv::SetInitialPose::Request> request,
std::shared_ptr<nav2_msgs::srv::SetInitialPose::Response> response);

// Let amcl update samples without requiring motion
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr nomotion_update_srv_;
/*
Expand Down
4 changes: 2 additions & 2 deletions nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@ typedef struct


// Return a zero vector
pf_vector_t pf_vector_zero();
pf_vector_t pf_vector_zero(void);

// Check for NAN or INF in any component
// int pf_vector_finite(pf_vector_t a);
Expand All @@ -71,7 +71,7 @@ pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b);


// Return a zero matrix
pf_matrix_t pf_matrix_zero();
pf_matrix_t pf_matrix_zero(void);

// Check for NAN or INF in any component
// int pf_matrix_finite(pf_matrix_t a);
Expand Down
2 changes: 1 addition & 1 deletion nav2_amcl/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>nav2_amcl</name>
<version>1.1.12</version>
<version>1.1.14</version>
<description>
<p>
amcl is a probabilistic localization system for a robot moving in
Expand Down
26 changes: 21 additions & 5 deletions nav2_amcl/src/amcl_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -325,13 +325,17 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
// Get rid of the inputs first (services and message filter input), so we
// don't continue to process incoming messages
global_loc_srv_.reset();
initial_guess_srv_.reset();
nomotion_update_srv_.reset();
executor_thread_.reset(); // to make sure initial_pose_sub_ completely exit
initial_pose_sub_.reset();
laser_scan_connection_.disconnect();
tf_listener_.reset(); // listener may access lase_scan_filter_, so it should be reset earlier
laser_scan_filter_.reset();
laser_scan_sub_.reset();

// Map
map_sub_.reset(); // map_sub_ may access map_, so it should be reset earlier
if (map_ != NULL) {
map_free(map_);
map_ = nullptr;
Expand All @@ -341,7 +345,6 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/)

// Transforms
tf_broadcaster_.reset();
tf_listener_.reset();
tf_buffer_.reset();

// PubSub
Expand Down Expand Up @@ -493,6 +496,15 @@ AmclNode::globalLocalizationCallback(
pf_init_ = false;
}

void
AmclNode::initialPoseReceivedSrv(
const std::shared_ptr<rmw_request_id_t>/*request_header*/,
const std::shared_ptr<nav2_msgs::srv::SetInitialPose::Request> req,
std::shared_ptr<nav2_msgs::srv::SetInitialPose::Response>/*res*/)
{
initialPoseReceived(std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>(req->pose));
}

// force nomotion updates (amcl updating without requiring motion)
void
AmclNode::nomotionUpdateCallback(
Expand Down Expand Up @@ -1093,20 +1105,20 @@ AmclNode::initParameters()
// Semantic checks
if (laser_likelihood_max_dist_ < 0) {
RCLCPP_WARN(
get_logger(), "You've set laser_likelihood_max_dist to be negtive,"
get_logger(), "You've set laser_likelihood_max_dist to be negative,"
" this isn't allowed so it will be set to default value 2.0.");
laser_likelihood_max_dist_ = 2.0;
}
if (max_particles_ < 0) {
RCLCPP_WARN(
get_logger(), "You've set max_particles to be negtive,"
get_logger(), "You've set max_particles to be negative,"
" this isn't allowed so it will be set to default value 2000.");
max_particles_ = 2000;
}

if (min_particles_ < 0) {
RCLCPP_WARN(
get_logger(), "You've set min_particles to be negtive,"
get_logger(), "You've set min_particles to be negative,"
" this isn't allowed so it will be set to default value 500.");
min_particles_ = 500;
}
Expand All @@ -1120,7 +1132,7 @@ AmclNode::initParameters()

if (resample_interval_ <= 0) {
RCLCPP_WARN(
get_logger(), "You've set resample_interval to be zero or negtive,"
get_logger(), "You've set resample_interval to be zero or negative,"
" this isn't allowed so it will be set to default value to 1.");
resample_interval_ = 1;
}
Expand Down Expand Up @@ -1533,6 +1545,10 @@ AmclNode::initServices()
"reinitialize_global_localization",
std::bind(&AmclNode::globalLocalizationCallback, this, _1, _2, _3));

initial_guess_srv_ = create_service<nav2_msgs::srv::SetInitialPose>(
"set_initial_pose",
std::bind(&AmclNode::initialPoseReceivedSrv, this, _1, _2, _3));

nomotion_update_srv_ = create_service<std_srvs::srv::Empty>(
"request_nomotion_update",
std::bind(&AmclNode::nomotionUpdateCallback, this, _1, _2, _3));
Expand Down
5 changes: 0 additions & 5 deletions nav2_amcl/src/pf/pf.c
Original file line number Diff line number Diff line change
Expand Up @@ -463,7 +463,6 @@ void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set)

// Workspace
double m[4], c[2][2];
size_t count;
double weight;

// Cluster the samples
Expand All @@ -474,7 +473,6 @@ void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set)

for (i = 0; i < set->cluster_max_count; i++) {
cluster = set->clusters + i;
cluster->count = 0;
cluster->weight = 0;
cluster->mean = pf_vector_zero();
cluster->cov = pf_matrix_zero();
Expand All @@ -490,7 +488,6 @@ void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set)
}

// Initialize overall filter stats
count = 0;
weight = 0.0;
set->mean = pf_vector_zero();
set->cov = pf_matrix_zero();
Expand Down Expand Up @@ -521,10 +518,8 @@ void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set)

cluster = set->clusters + cidx;

cluster->count += 1;
cluster->weight += sample->weight;

count += 1;
weight += sample->weight;

// Compute mean
Expand Down
4 changes: 2 additions & 2 deletions nav2_amcl/src/pf/pf_vector.c
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@


// Return a zero vector
pf_vector_t pf_vector_zero()
pf_vector_t pf_vector_zero(void)
{
pf_vector_t c;

Expand Down Expand Up @@ -130,7 +130,7 @@ pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b)


// Return a zero matrix
pf_matrix_t pf_matrix_zero()
pf_matrix_t pf_matrix_zero(void)
{
int i, j;
pf_matrix_t c;
Expand Down
22 changes: 11 additions & 11 deletions nav2_amcl/src/sensors/laser/likelihood_field_model.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,17 @@ LikelihoodFieldModel::sensorFunction(LaserData * data, pf_sample_set_t * set)

self = reinterpret_cast<LikelihoodFieldModel *>(data->laser);

// Pre-compute a couple of things
double z_hit_denom = 2 * self->sigma_hit_ * self->sigma_hit_;
double z_rand_mult = 1.0 / data->range_max;

step = (data->range_count - 1) / (self->max_beams_ - 1);

// Step size must be at least 1
if (step < 1) {
step = 1;
}

total_weight = 0.0;

// Compute the sample weights
Expand All @@ -65,17 +76,6 @@ LikelihoodFieldModel::sensorFunction(LaserData * data, pf_sample_set_t * set)

p = 1.0;

// Pre-compute a couple of things
double z_hit_denom = 2 * self->sigma_hit_ * self->sigma_hit_;
double z_rand_mult = 1.0 / data->range_max;

step = (data->range_count - 1) / (self->max_beams_ - 1);

// Step size must be at least 1
if (step < 1) {
step = 1;
}

for (i = 0; i < data->range_count; i += step) {
obs_range = data->ranges[i][0];
obs_bearing = data->ranges[i][1];
Expand Down
3 changes: 3 additions & 0 deletions nav2_behavior_tree/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -204,6 +204,9 @@ list(APPEND plugin_libs nav2_smoother_selector_bt_node)
add_library(nav2_goal_checker_selector_bt_node SHARED plugins/action/goal_checker_selector_node.cpp)
list(APPEND plugin_libs nav2_goal_checker_selector_bt_node)

add_library(nav2_progress_checker_selector_bt_node SHARED plugins/action/progress_checker_selector_node.cpp)
list(APPEND plugin_libs nav2_progress_checker_selector_bt_node)

add_library(nav2_goal_updated_controller_bt_node SHARED plugins/decorator/goal_updated_controller.cpp)
list(APPEND plugin_libs nav2_goal_updated_controller_bt_node)

Expand Down
2 changes: 1 addition & 1 deletion nav2_behavior_tree/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -63,4 +63,4 @@ The BehaviorTree engine has a run method that accepts an XML description of a BT
See the code in the [BT Navigator](../nav2_bt_navigator/src/bt_navigator.cpp) for an example usage of the BehaviorTreeEngine.
For more information about the behavior tree nodes that are available in the default BehaviorTreeCPP library, see documentation here: https://www.behaviortree.dev/docs/learn-the-basics/bt_basics/
For more information about the behavior tree nodes that are available in the default BehaviorTreeCPP library, see documentation here: https://www.behaviortree.dev/docs/3.8/learn-the-basics/BT_basics
19 changes: 15 additions & 4 deletions nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -163,7 +163,7 @@ class BtActionNode : public BT::ActionNodeBase
}

/**
* @brief Function to perform some user-defined operation whe the action is aborted.
* @brief Function to perform some user-defined operation when the action is aborted.
* @return BT::NodeStatus Returns FAILURE by default, user may override return another value
*/
virtual BT::NodeStatus on_aborted()
Expand Down Expand Up @@ -207,7 +207,7 @@ class BtActionNode : public BT::ActionNodeBase
// if new goal was sent and action server has not yet responded
// check the future goal handle
if (future_goal_handle_) {
auto elapsed = (node_->now() - time_goal_sent_).to_chrono<std::chrono::milliseconds>();
auto elapsed = (node_->now() - time_goal_sent_).template to_chrono<std::chrono::milliseconds>();
if (!is_future_goal_handle_complete(elapsed)) {
// return RUNNING if there is still some time before timeout happens
if (elapsed < server_timeout_) {
Expand Down Expand Up @@ -237,7 +237,7 @@ class BtActionNode : public BT::ActionNodeBase
{
goal_updated_ = false;
send_new_goal();
auto elapsed = (node_->now() - time_goal_sent_).to_chrono<std::chrono::milliseconds>();
auto elapsed = (node_->now() - time_goal_sent_).template to_chrono<std::chrono::milliseconds>();
if (!is_future_goal_handle_complete(elapsed)) {
if (elapsed < server_timeout_) {
return BT::NodeStatus::RUNNING;
Expand Down Expand Up @@ -266,7 +266,7 @@ class BtActionNode : public BT::ActionNodeBase
// Action related failure that should not fail the tree, but the node
return BT::NodeStatus::FAILURE;
} else {
// Internal exception to propogate to the tree
// Internal exception to propagate to the tree
throw e;
}
}
Expand Down Expand Up @@ -300,6 +300,7 @@ class BtActionNode : public BT::ActionNodeBase
void halt() override
{
if (should_cancel_goal()) {
auto future_result = action_client_->async_get_result(goal_handle_);
auto future_cancel = action_client_->async_cancel_goal(goal_handle_);
if (callback_group_executor_.spin_until_future_complete(future_cancel, server_timeout_) !=
rclcpp::FutureReturnCode::SUCCESS)
Expand All @@ -308,6 +309,16 @@ class BtActionNode : public BT::ActionNodeBase
node_->get_logger(),
"Failed to cancel action server for %s", action_name_.c_str());
}

if (callback_group_executor_.spin_until_future_complete(future_result, server_timeout_) !=
rclcpp::FutureReturnCode::SUCCESS)
{
RCLCPP_ERROR(
node_->get_logger(),
"Failed to get result for %s in node halt!", action_name_.c_str());
}

on_cancelled();
}

setStatus(BT::NodeStatus::IDLE);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -116,7 +116,7 @@ class BtCancelActionNode : public BT::ActionNodeBase
return basic;
}

void halt()
void halt() override
{
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -183,7 +183,7 @@ class BtServiceNode : public BT::ActionNodeBase
*/
virtual BT::NodeStatus check_future()
{
auto elapsed = (node_->now() - sent_time_).to_chrono<std::chrono::milliseconds>();
auto elapsed = (node_->now() - sent_time_).template to_chrono<std::chrono::milliseconds>();
auto remaining = server_timeout_ - elapsed;

if (remaining > std::chrono::milliseconds(0)) {
Expand All @@ -199,7 +199,7 @@ class BtServiceNode : public BT::ActionNodeBase

if (rc == rclcpp::FutureReturnCode::TIMEOUT) {
on_wait_for_result();
elapsed = (node_->now() - sent_time_).to_chrono<std::chrono::milliseconds>();
elapsed = (node_->now() - sent_time_).template to_chrono<std::chrono::milliseconds>();
if (elapsed < server_timeout_) {
return BT::NodeStatus::RUNNING;
}
Expand Down
Loading

0 comments on commit 7046850

Please sign in to comment.