Skip to content

Commit

Permalink
Nav2 Sync 4 2024 Sept 26 (#24)
Browse files Browse the repository at this point in the history
* replace throw-error with error-log to avoid UAF mentioned in ros-navigation#4175 (ros-navigation#4180) (ros-navigation#4305)

* replace throw-error with error-log to avoid UAF



* fix typo



---------

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

* Cherry-pick from 15c9be0 (ros-navigation#4317)

Convert all wall timers and wall rates to ROS clock respecting rates and timers (ros-navigation#4000)

* Convert all wall timers and wall rates to ROS clock respecting rates and timers

* linty mclint face

* WPF wait plugin respect time

* move duration metrics to use local clocks

* bumping version for cache to break it

* complete timing refactor

* remove old variable

* Add dynamic parameter (ros-navigation#4319)

To fix ros-navigation#4315

Signed-off-by: Huy Nguyen Van <34271857+Huyhust13@users.noreply.github.com>

* Humble release 11: May 23, 2024 (ros-navigation#4365)

* Scale cost critic's weight when dynamically updated (ros-navigation#4246)

* Scale cost critic's weight when dynamically updated

Signed-off-by: pepisg <pedro.gonzalez@eia.edu.co>

* sign off

Signed-off-by: pepisg <pedro.gonzalez@eia.edu.co>

---------

Signed-off-by: pepisg <pedro.gonzalez@eia.edu.co>

* Add expanding the ~/ to the full home dir of user in the path to the map yaml.  (ros-navigation#4258)

* Add user home expander of home sequence

Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com>

* Add passing home dir as string instead of const char*

Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com>

* Add docs

Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com>

* Fix function declaration

Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com>

* Fix linter issues

Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com>

* Uncrustify linter

Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com>

* Uncrustify linter

Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com>

* Uncrustify linter: remove remove whitespace

Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com>

---------

Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com>

* Implement Critic for Velocity Deadband Hardware Constraints (ros-navigation#4256)

* Adding new velocity deadband critic.

- add some tests
- cast double to float
- add new features from "main" branch

- fix formating

- add cost test
- fix linting issue
- add README

Signed-off-by: Denis Sokolov <denis.sokolov48@gmail.com>

* Remove velocity deadband critic from defaults

Signed-off-by: Denis Sokolov <denis.sokolov48@gmail.com>

* remove old weight

Signed-off-by: Denis Sokolov <denis.sokolov48@gmail.com>

* fix velocity deadband critic tests

Signed-off-by: Denis Sokolov <denis.sokolov48@gmail.com>

---------

Signed-off-by: Denis Sokolov <denis.sokolov48@gmail.com>

* removing clearable layer param (unused) (ros-navigation#4280)

* provide message validation check API (ros-navigation#4276)

* provide validation_message.hpp

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

* fix typo

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

* add test_validation_messages.cpp

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

* change include-order

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

* reformat

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

* update test

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

---------

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

* Add footprint clearing for static layer (ros-navigation#4282)

* Add footprint clearing for static layer

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

* fix flckering

---------

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

* Fix ros-navigation#4268 (ros-navigation#4296)

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

* Update README.md of nav2_bt_navigator (ros-navigation#4309)

Update link to docs

Signed-off-by: João Britto <cbn.joao@gmail.com>

* Fix undefined symbols in `libpf_lib.so` (ros-navigation#4312)

When I build `nav2_amcl` with `-Wl,--no-undefined` I noticed
`libpf_lib.so` has undefined symbols. This PR correctly links
`libpf_lib.so` to `libm` so all symbols can be found.

You can verify this by executing the following command:
```
ldd -r ./build/nav2_amcl/src/pf/libpf_lib.so
	linux-vdso.so.1 (0x00007ffd1f8c0000)
	libc.so.6 => /lib/x86_64-linux-gnu/libc.so.6 (0x000074e909a00000)
	/lib64/ld-linux-x86-64.so.2 (0x000074e909e60000)
undefined symbol: ceil	(./build/nav2_amcl/src/pf/libpf_lib.so)
undefined symbol: atan2	(./build/nav2_amcl/src/pf/libpf_lib.so)
undefined symbol: sin	(./build/nav2_amcl/src/pf/libpf_lib.so)
undefined symbol: hypot	(./build/nav2_amcl/src/pf/libpf_lib.so)
undefined symbol: cos	(./build/nav2_amcl/src/pf/libpf_lib.so)
undefined symbol: log	(./build/nav2_amcl/src/pf/libpf_lib.so)
undefined symbol: sqrt	(./build/nav2_amcl/src/pf/libpf_lib.so)
undefined symbol: floor	(./build/nav2_amcl/src/pf/libpf_lib.so)
```

Signed-off-by: Ramon Wijnands <ramon.wijnands@nobleo.nl>

* msg validation check for `/initialpose`  in `nav2_amcl` (ros-navigation#4301)

* add validation check for PoseWithCovarianceStamped

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

* remove rebundant check before

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

* reformat

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

* typo fixed

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

* change the type-name

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

* update test

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

* reformat

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

* .

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

* add comment

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

* update comment

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

* change header

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

* update test

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

* typo fixed

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

---------

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

* 4320: Changed precision of calculations of the HybridNode MotionTable::getClosestAngularBin. (ros-navigation#4324)

Signed-off-by: Krzysztof Pawełczyk <k.t.pawelczyk@gmail.com>
Co-authored-by: Krzysztof Pawełczyk <kpawelczyk@autonomous-systems.pl>

* [LifecycleNode] add bond_heartbeat_period (ros-navigation#4342)

* add bond_heartbeat_period

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>

* lint

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>

---------

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>
Co-authored-by: Guillaume Doisy <guillaume@dexory.com>

* Update path_longer_on_approach.cpp (ros-navigation#4344)

Fix the bug that isPathUpdated function will return false for the reason that it compare the timestaped between new path and old path's last pose

Signed-off-by: StetroF <120172218+StetroF@users.noreply.github.com>

* [LifecycleManagerClient] clean set_initial_pose and navigate_to_pose (ros-navigation#4346)

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>
Co-authored-by: Guillaume Doisy <guillaume@dexory.com>

* Move projectState after getPointsInside (ros-navigation#4356)

* Modify test to check fix

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

* Add static polygon check before simulation

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

---------

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

* adding final pose in analytic expansion to check (ros-navigation#4353)

* fix sync merge conflicts

* bump humble to 1.1.15 for release

* Revert "[LifecycleNode] add bond_heartbeat_period (ros-navigation#4342)"

This reverts commit 6e44178.

---------

Signed-off-by: pepisg <pedro.gonzalez@eia.edu.co>
Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com>
Signed-off-by: Denis Sokolov <denis.sokolov48@gmail.com>
Signed-off-by: goes <GoesM@buaa.edu.cn>
Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: João Britto <cbn.joao@gmail.com>
Signed-off-by: Ramon Wijnands <ramon.wijnands@nobleo.nl>
Signed-off-by: Krzysztof Pawełczyk <k.t.pawelczyk@gmail.com>
Signed-off-by: Guillaume Doisy <guillaume@dexory.com>
Signed-off-by: StetroF <120172218+StetroF@users.noreply.github.com>
Signed-off-by: Brice <brice.renaudeau@gmail.com>
Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com>
Co-authored-by: Wiktor Bajor <69388767+Wiktor-99@users.noreply.github.com>
Co-authored-by: Sokolov Denis <52282102+perchess@users.noreply.github.com>
Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com>
Co-authored-by: goes <GoesM@buaa.edu.cn>
Co-authored-by: Tony Najjar <tony.najjar.1997@gmail.com>
Co-authored-by: João Britto <cbn.joao@gmail.com>
Co-authored-by: Ramon Wijnands <ramon.wijnands@nobleo.nl>
Co-authored-by: AzaelCicero <k.t.pawelczyk@gmail.com>
Co-authored-by: Krzysztof Pawełczyk <kpawelczyk@autonomous-systems.pl>
Co-authored-by: Guillaume Doisy <doisyg@users.noreply.github.com>
Co-authored-by: Guillaume Doisy <guillaume@dexory.com>
Co-authored-by: StetroF <120172218+StetroF@users.noreply.github.com>
Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com>

* adding mutex lock around map resizing due to dynamic parameter changes and associated processes (ros-navigation#4373) (ros-navigation#4378)

(cherry picked from commit b0abc78)

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

* Make BT nodes have configurable wait times (Backport ros-navigation#3960 ros-navigation#4178 ros-navigation#4203) (ros-navigation#4409)

* WIP: Make BT nodes have configurable wait times. (ros-navigation#3960)

* Make BT nodes have configurable wait times.

Previous solution provided hardcoded 1s value.
Right now the value can be configured for BT
Action, Cancel and Service nodes.

[ros-navigation#3920]

Signed-off-by: Adam Galecki <embeddedadam@gmail.com>

* Make BT nodes have configurable wait times.

Previous solution provided hardcoded 1s value.
Right now the value can be configured for BT
Action, Cancel and Service nodes.

[ros-navigation#3920]

Signed-off-by: Adam Galecki <embeddedadam@gmail.com>

* Fix typos, linting errors and value type from float to int

* Fix extra underscores

* Fix extra underscore

* Update unit tests with blackboard parameter

Signed-off-by: Adam Galecki <embeddedadam@gmail.com>

* Fix formatting errors

Signed-off-by: Adam Galecki <embeddedadam@gmail.com>

* Update system tests to match new parameter

Signed-off-by: Adam Galecki <embeddedadam@gmail.com>

---------

Signed-off-by: Adam Galecki <embeddedadam@gmail.com>
Signed-off-by: Christoph Froehlich <christoph.froehlich@ait.ac.at>

* chore(nav2_behavior_tree): log actual wait period in bt_action_node (ros-navigation#4178)

Signed-off-by: Felix <felix@fzeltner.de>
Co-authored-by: Felix <felix@fzeltner.de>
Signed-off-by: Christoph Froehlich <christoph.froehlich@ait.ac.at>

* fix missing param declare (ros-navigation#4203)

Signed-off-by: nelson <kaichie.lee@gmail.com>
Signed-off-by: Christoph Froehlich <christoph.froehlich@ait.ac.at>

* Fix error messages (ros-navigation#4411)

Signed-off-by: Christoph Froehlich <christoph.froehlich@ait.ac.at>

---------

Signed-off-by: Adam Galecki <embeddedadam@gmail.com>
Signed-off-by: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Signed-off-by: Felix <felix@fzeltner.de>
Signed-off-by: nelson <kaichie.lee@gmail.com>
Co-authored-by: Adam Gałecki <73710544+embeddedadam@users.noreply.github.com>
Co-authored-by: bi0ha2ard <bi0ha2ard@users.noreply.github.com>
Co-authored-by: Felix <felix@fzeltner.de>
Co-authored-by: nelson <kaichie.lee@gmail.com>

* Enable reloading BT xml file with same name (ros-navigation#4209) (ros-navigation#4422)

* Let BtActionServer overwrite xml

* Make a ROS parameter for it

* Rename flag to always reload BT xml file

---------

Signed-off-by: Johannes Huemer <johannes.huemer@ait.ac.at>
Signed-off-by: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Co-authored-by: Johannes Huemer <johannes.huemer@ait.ac.at>

* fix bug mentioned in ros-navigation#3958 (ros-navigation#3972) (ros-navigation#4463)

* bug fixed

* add space

* Update planner_server.cpp

* add space for code style

* add childLifecycleNode mode to costmap_2d_ros

* add childLifecycleNode mode to costmap_2d_ros

* add childLifecycleNode mode to costmap_2d_ros

* add childLifecycleNode mode in costmap_2d_ros

* add childLifecycleNode mode in costmap_2d_ros

* add childLifecycleNode mode in costmap_2d_ros

* add ChildLifecycleNode mode in costmap_2d_ros

* NodeOption: is_lifecycle_follower_

* NodeOption: is_lifecycle_follower_

* fit to NodeOption: is_lifecycle_follower_

* NodeOption: is_lifecycle_follower_

* fit to NodeOption: is_lifecycle_follower

* fit to NodeOption: is_lifecycle_follower

* fit reorder Werror

* fix wrong use of is_lifecycle_follower

* remove blank line

* NodeOption: is_lifecycle_follower_

* NodeOption: is_lifecycle_follower_

* Add files via upload

* NodeOption: is_lifecycle_follower_

* NodeOption:is_lifecycle_follower_

* NodeOption:is_lifecycle_follower

* NodeOption:is_lifecycle_follower

* NodeOption:is_lifecycle_follower

* change default

* add NodeOption for costmap_2d_ros

* add node options for costmap2dros as an independent node

* code style reformat

* fit to NodeOption of Costmap2DROS

* fit to NodeOption of Costmap2DROS

* fit to NodeOption of Costmap2DROS

* Update nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp



* Update nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp



* Update nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp



* changes

* comment changes

* change get_parameter into =false

* comment modification

* missing line

* Update nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp

* Update nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp

* delete last line

* change lifecycle_test fit to NodeOption

---------

Co-authored-by: GoesM <GoesM@buaa.edu.cn>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* bt_service_node and bt_action_node: Don't block BT loop (backport ros-navigation#4214) (ros-navigation#4408) (ros-navigation#4475)

* bt_service_node and bt_action_node: Don't block BT loop (ros-navigation#4214)

* Set smaller timeout for service node



* Fix timeout calculation for service node



* Add a feasible timeout also for action node



---------



* Increasing test count from timeout handling changes (ros-navigation#4234)



---------

Signed-off-by: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Behavior tree node for extracting pose from path (ros-navigation#4518) (ros-navigation#4525)

* add get pose from path action

Signed-off-by: MarcM0 <marc.morcos9@gmail.com>

* cleanup from PR suggestions

Signed-off-by: MarcM0 <marc.morcos9@gmail.com>

* Updates for main compatibility

Signed-off-by: MarcM0 <marc.morcos9@gmail.com>

* Lint and build fix

Signed-off-by: MarcM0 <marc.morcos9@gmail.com>

* More Lint and warnings

Signed-off-by: MarcM0 <marc.morcos9@gmail.com>

* More Lint and build

Signed-off-by: MarcM0 <marc.morcos9@gmail.com>

* remove code left over from older file

Signed-off-by: MarcM0 <marc.morcos9@gmail.com>

* fix test blackboard var name

Signed-off-by: MarcM0 <marc.morcos9@gmail.com>

* only populate pose frame if empty

Signed-off-by: MarcM0 <marc.morcos9@gmail.com>

* lint

Signed-off-by: MarcM0 <marc.morcos9@gmail.com>

---------

Signed-off-by: MarcM0 <marc.morcos9@gmail.com>
(cherry picked from commit 12a9c1d)

Co-authored-by: Marc Morcos <30278842+MarcM0@users.noreply.github.com>

* Make ros-navigation#4525 compile on humble (ros-navigation#4526)

* Make it compile on humble

Signed-off-by: Christoph Froehlich <christoph.froehlich@ait.ac.at>

* Remove formatting

Signed-off-by: Christoph Froehlich <christoph.froehlich@ait.ac.at>

---------

Signed-off-by: Christoph Froehlich <christoph.froehlich@ait.ac.at>

* Fix backward motion for graceful controller (ros-navigation#4527) (ros-navigation#4566)

* Fix backward motion for graceful controller

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Update smooth_control_law.cpp

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

---------

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
(cherry picked from commit d1ad640)

Co-authored-by: Alberto Tudela <ajtudela@gmail.com>

* nav2_collision_monitor dynamic parameters polygon and source enabled for Humble (ros-navigation#4615)

* Copy modification from c2d84df into humble collision_monitor for dynamic parameter enabled in polygon

* Add the enabled dynamic parameter for source. Signed-off-by: Enzo Ghisoni <enzo.ghisoni@hotmail.fr>

* Start backport action_state_ declaration in collision_monitor_node_test.cpp

Signed-off-by: EnzoGhisoni <enzo.ghisoni@botronics.be>

---------

Signed-off-by: EnzoGhisoni <enzo.ghisoni@botronics.be>
Co-authored-by: EnzoGhisoni <enzo.ghisoni@botronics.be>

* Humble release 12: August 23 (ros-navigation#4644)

* Add configure and cleanup transitions to lifecycle manager and client (ros-navigation#4371)

Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi>

* [RotationShimController] Rotate to goal heading (ros-navigation#4332)

When arriving in the goal xy tolerance, the rotation shim controller
takes back the control to command the robot to rotate in the goal
heading orientation.

The initial goal of the rotationShimController was to rotate the robot
at the beginning of a navigation towards the paths orientation because
some controllers are not good at performing in place rotations. For the
same reason, the rotationShimController should be able to rotate the
robot towards the goal heading.

Signed-off-by: Antoine Gennart <gennart.antoine@gmail.com>

* [RotationShimController] Fix test for rotate to goal heading (ros-navigation#4289) (ros-navigation#4391)

* Fix rotate to goal heading tests

Signed-off-by: Antoine Gennart <gennartan@disroot.org>

* reset laser_scan_filter before reinit (ros-navigation#4397)

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

* Warn if inflation_radius_ < inscribed_radius_ (ros-navigation#4423)

* Warn if inflation_radius_ < inscribed_radius_

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

* convert to error

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

---------

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

* chore: cleanup ros1 leftovers (ros-navigation#4446)

Signed-off-by: Rein Appeldoorn <rein.appeldoorn@nobleo.nl>

* precomputeDistanceHeuristic is now computed once (ros-navigation#4451)

Signed-off-by: Vincent Belpois <vincent.belpois@gmail.com>
Co-authored-by: SiddharthaUpase <s1dupase34@gmail.com>

* shutdown services in destructor of `ClearCostmapService` (ros-navigation#4495)

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

* fix(nav2_costmap_2d): make obstacle layer not current on enabled toggle (ros-navigation#4507)

Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com>
Co-authored-by: Kemal Bektas <kemal.bektas@node-robotics.com>

* min_turning_r_ getting param fix (ros-navigation#4510)

* min_turning_r_ getting param fix

Signed-off-by: Ivan Radionov <i.a.radionov@gmail.com.com>

* Update nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: Ivan Radionov <i.a.radionov@gmail.com.com>

---------

Signed-off-by: Ivan Radionov <i.a.radionov@gmail.com.com>
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Co-authored-by: Ivan Radionov <i.a.radionov@gmail.com.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Return out of map update if frames mismatch. Signed-off-by Joey Yang (ros-navigation#4517)

Signed-off-by: Joey Yang <joeyyang.ai@gmail.com>

* check nullptr in smoothPlan() (ros-navigation#4544)

* check nullptr in smoothPlan()

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

* code-style

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

* code-style

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

* simple change

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

---------

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

* bump to 1.1.15

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

* Revert "Add configure and cleanup transitions to lifecycle manager and client (ros-navigation#4371)"

This reverts commit 06ec958.

* fix merge conflict with humble sync

* fix merge conflict with humble sync

---------

Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi>
Signed-off-by: Antoine Gennart <gennart.antoine@gmail.com>
Signed-off-by: Antoine Gennart <gennartan@disroot.org>
Signed-off-by: goes <GoesM@buaa.edu.cn>
Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>
Signed-off-by: Rein Appeldoorn <rein.appeldoorn@nobleo.nl>
Signed-off-by: Vincent Belpois <vincent.belpois@gmail.com>
Signed-off-by: GoesM_server <GoesM@buaa.edu.cn>
Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com>
Signed-off-by: Ivan Radionov <i.a.radionov@gmail.com.com>
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: Joey Yang <joeyyang.ai@gmail.com>
Signed-off-by: GoesM <goesm@buaa.edu.cn>
Co-authored-by: Joni Pöllänen <jonipol@users.noreply.github.com>
Co-authored-by: Saitama <gennartan@users.noreply.github.com>
Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com>
Co-authored-by: goes <GoesM@buaa.edu.cn>
Co-authored-by: Tony Najjar <tony.najjar.1997@gmail.com>
Co-authored-by: Rein Appeldoorn <reinzor@gmail.com>
Co-authored-by: Vincent <46542431+VincidaB@users.noreply.github.com>
Co-authored-by: SiddharthaUpase <s1dupase34@gmail.com>
Co-authored-by: Kemal Bektas <34746077+bektaskemal@users.noreply.github.com>
Co-authored-by: Kemal Bektas <kemal.bektas@node-robotics.com>
Co-authored-by: Ivan Radionov <45877502+JJRedmond@users.noreply.github.com>
Co-authored-by: Ivan Radionov <i.a.radionov@gmail.com.com>
Co-authored-by: Joey Yang <joeyyang.ai@gmail.com>

* Fixing ros-navigation#4661: MPPI ackermann reversing taking incorrect sign sometimes (ros-navigation#4664) (ros-navigation#4668)

* Fixing ros-navigation#4661: MPPI ackermann reversing taking incorrect sign sometimes

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

* fixing unit test for type implicit cast

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

---------

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
(cherry picked from commit 7eb47d8)

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

---------

Signed-off-by: GoesM <GoesM@buaa.edu.cn>
Signed-off-by: Huy Nguyen Van <34271857+Huyhust13@users.noreply.github.com>
Signed-off-by: pepisg <pedro.gonzalez@eia.edu.co>
Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com>
Signed-off-by: Denis Sokolov <denis.sokolov48@gmail.com>
Signed-off-by: goes <GoesM@buaa.edu.cn>
Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: João Britto <cbn.joao@gmail.com>
Signed-off-by: Ramon Wijnands <ramon.wijnands@nobleo.nl>
Signed-off-by: Krzysztof Pawełczyk <k.t.pawelczyk@gmail.com>
Signed-off-by: Guillaume Doisy <guillaume@dexory.com>
Signed-off-by: StetroF <120172218+StetroF@users.noreply.github.com>
Signed-off-by: Brice <brice.renaudeau@gmail.com>
Signed-off-by: Adam Galecki <embeddedadam@gmail.com>
Signed-off-by: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Signed-off-by: Felix <felix@fzeltner.de>
Signed-off-by: nelson <kaichie.lee@gmail.com>
Signed-off-by: Johannes Huemer <johannes.huemer@ait.ac.at>
Signed-off-by: EnzoGhisoni <enzo.ghisoni@botronics.be>
Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi>
Signed-off-by: Antoine Gennart <gennart.antoine@gmail.com>
Signed-off-by: Antoine Gennart <gennartan@disroot.org>
Signed-off-by: Rein Appeldoorn <rein.appeldoorn@nobleo.nl>
Signed-off-by: Vincent Belpois <vincent.belpois@gmail.com>
Signed-off-by: GoesM_server <GoesM@buaa.edu.cn>
Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com>
Signed-off-by: Ivan Radionov <i.a.radionov@gmail.com.com>
Signed-off-by: Joey Yang <joeyyang.ai@gmail.com>
Signed-off-by: GoesM <goesm@buaa.edu.cn>
Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com>
Co-authored-by: GoesM <GoesM@buaa.edu.cn>
Co-authored-by: Benjamin-Tan <tan.benjaminkx@gmail.com>
Co-authored-by: Huy Nguyen Van <34271857+Huyhust13@users.noreply.github.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com>
Co-authored-by: Wiktor Bajor <69388767+Wiktor-99@users.noreply.github.com>
Co-authored-by: Sokolov Denis <52282102+perchess@users.noreply.github.com>
Co-authored-by: Tony Najjar <tony.najjar.1997@gmail.com>
Co-authored-by: João Britto <cbn.joao@gmail.com>
Co-authored-by: Ramon Wijnands <ramon.wijnands@nobleo.nl>
Co-authored-by: AzaelCicero <k.t.pawelczyk@gmail.com>
Co-authored-by: Krzysztof Pawełczyk <kpawelczyk@autonomous-systems.pl>
Co-authored-by: Guillaume Doisy <doisyg@users.noreply.github.com>
Co-authored-by: Guillaume Doisy <guillaume@dexory.com>
Co-authored-by: StetroF <120172218+StetroF@users.noreply.github.com>
Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com>
Co-authored-by: mergify[bot] <37929162+mergify[bot]@users.noreply.github.com>
Co-authored-by: Christoph Fröhlich <christophfroehlich@users.noreply.github.com>
Co-authored-by: Adam Gałecki <73710544+embeddedadam@users.noreply.github.com>
Co-authored-by: bi0ha2ard <bi0ha2ard@users.noreply.github.com>
Co-authored-by: Felix <felix@fzeltner.de>
Co-authored-by: nelson <kaichie.lee@gmail.com>
Co-authored-by: Johannes Huemer <johannes.huemer@ait.ac.at>
Co-authored-by: Marc Morcos <30278842+MarcM0@users.noreply.github.com>
Co-authored-by: Alberto Tudela <ajtudela@gmail.com>
Co-authored-by: Enzo Ghisoni <33607172+EnzoGhisoni@users.noreply.github.com>
Co-authored-by: EnzoGhisoni <enzo.ghisoni@botronics.be>
Co-authored-by: Joni Pöllänen <jonipol@users.noreply.github.com>
Co-authored-by: Saitama <gennartan@users.noreply.github.com>
Co-authored-by: Rein Appeldoorn <reinzor@gmail.com>
Co-authored-by: Vincent <46542431+VincidaB@users.noreply.github.com>
Co-authored-by: SiddharthaUpase <s1dupase34@gmail.com>
Co-authored-by: Kemal Bektas <34746077+bektaskemal@users.noreply.github.com>
Co-authored-by: Kemal Bektas <kemal.bektas@node-robotics.com>
Co-authored-by: Ivan Radionov <45877502+JJRedmond@users.noreply.github.com>
Co-authored-by: Ivan Radionov <i.a.radionov@gmail.com.com>
Co-authored-by: Joey Yang <joeyyang.ai@gmail.com>
  • Loading branch information
1 parent 24c2381 commit 0254af6
Show file tree
Hide file tree
Showing 161 changed files with 2,078 additions and 391 deletions.
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.14</version>
<version>1.1.16</version>
<description>
<p>
amcl is a probabilistic localization system for a robot moving in
Expand Down
13 changes: 8 additions & 5 deletions nav2_amcl/src/amcl_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,7 @@
#pragma GCC diagnostic pop

#include "nav2_amcl/portable_utils.hpp"
#include "nav2_util/validate_messages.hpp"

using namespace std::placeholders;
using rcl_interfaces::msg::ParameterType;
Expand Down Expand Up @@ -523,11 +524,8 @@ AmclNode::initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::Sha

RCLCPP_INFO(get_logger(), "initialPoseReceived");

if (msg->header.frame_id == "") {
// This should be removed at some point
RCLCPP_WARN(
get_logger(),
"Received initial pose with empty frame_id. You should always supply a frame_id.");
if (!nav2_util::validateMsg(*msg)) {
RCLCPP_ERROR(get_logger(), "Received initialpose message is malformed. Rejecting.");
return;
}
if (nav2_util::strip_leading_slash(msg->header.frame_id) != global_frame_id_) {
Expand Down Expand Up @@ -1360,6 +1358,7 @@ AmclNode::dynamicParametersCallback(
lasers_update_.clear();
frame_to_laser_.clear();
laser_scan_connection_.disconnect();
laser_scan_filter_.reset();
laser_scan_sub_.reset();

initMessageFilters();
Expand All @@ -1381,6 +1380,10 @@ void
AmclNode::mapReceived(const nav_msgs::msg::OccupancyGrid::SharedPtr msg)
{
RCLCPP_DEBUG(get_logger(), "AmclNode: A new map was received.");
if (!nav2_util::validateMsg(*msg)) {
RCLCPP_ERROR(get_logger(), "Received map message is malformed. Rejecting.");
return;
}
if (first_map_only_ && first_map_received_) {
return;
}
Expand Down
1 change: 1 addition & 0 deletions nav2_amcl/src/pf/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@ target_include_directories(pf_lib PRIVATE ../include)
if(HAVE_DRAND48)
target_compile_definitions(pf_lib PRIVATE "HAVE_DRAND48")
endif()
target_link_libraries(pf_lib m)

install(TARGETS
pf_lib
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 @@ -183,6 +183,9 @@ list(APPEND plugin_libs nav2_navigate_through_poses_action_bt_node)
add_library(nav2_remove_passed_goals_action_bt_node SHARED plugins/action/remove_passed_goals_action.cpp)
list(APPEND plugin_libs nav2_remove_passed_goals_action_bt_node)

add_library(nav2_get_pose_from_path_action_bt_node SHARED plugins/action/get_pose_from_path_action.cpp)
list(APPEND plugin_libs nav2_get_pose_from_path_action_bt_node)

add_library(nav2_pipeline_sequence_bt_node SHARED plugins/control/pipeline_sequence.cpp)
list(APPEND plugin_libs nav2_pipeline_sequence_bt_node)

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@
#include "behaviortree_cpp_v3/xml_parsing.h"
#include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h"

#include "rclcpp/rclcpp.hpp"

namespace nav2_behavior_tree
{
Expand All @@ -46,7 +47,8 @@ class BehaviorTreeEngine
* @brief A constructor for nav2_behavior_tree::BehaviorTreeEngine
* @param plugin_libraries vector of BT plugin library names to load
*/
explicit BehaviorTreeEngine(const std::vector<std::string> & plugin_libraries);
explicit BehaviorTreeEngine(
const std::vector<std::string> & plugin_libraries);
virtual ~BehaviorTreeEngine() {}

/**
Expand Down
21 changes: 15 additions & 6 deletions nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -56,11 +56,16 @@ class BtActionNode : public BT::ActionNodeBase
callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());

// Get the required items from the blackboard
bt_loop_duration_ =
auto bt_loop_duration =
config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
server_timeout_ =
config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
wait_for_service_timeout_ =
config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");

// timeout should be less than bt_loop_duration to be able to finish the current tick
max_timeout_ = std::chrono::duration_cast<std::chrono::milliseconds>(bt_loop_duration * 0.5);

// Initialize the input and output messages
goal_ = typename ActionT::Goal();
Expand Down Expand Up @@ -93,10 +98,11 @@ class BtActionNode : public BT::ActionNodeBase

// Make sure the server is actually there before continuing
RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str());
if (!action_client_->wait_for_action_server(10s)) {
if (!action_client_->wait_for_action_server(wait_for_service_timeout_)) {
RCLCPP_ERROR(
node_->get_logger(), "\"%s\" action server not available after waiting for 1 s",
action_name.c_str());
node_->get_logger(), "\"%s\" action server not available after waiting for %.2fs",
action_name.c_str(),
wait_for_service_timeout_.count() / 1000.0);
throw std::runtime_error(
std::string("Action server ") + action_name +
std::string(" not available"));
Expand Down Expand Up @@ -405,7 +411,7 @@ class BtActionNode : public BT::ActionNodeBase
return false;
}

auto timeout = remaining > bt_loop_duration_ ? bt_loop_duration_ : remaining;
auto timeout = remaining > max_timeout_ ? max_timeout_ : remaining;
auto result =
callback_group_executor_.spin_until_future_complete(*future_goal_handle_, timeout);
elapsed += timeout;
Expand Down Expand Up @@ -461,7 +467,10 @@ class BtActionNode : public BT::ActionNodeBase
std::chrono::milliseconds server_timeout_;

// The timeout value for BT loop execution
std::chrono::milliseconds bt_loop_duration_;
std::chrono::milliseconds max_timeout_;

// The timeout value for waiting for a service to response
std::chrono::milliseconds wait_for_service_timeout_;

// To track the action server acknowledgement when a new goal is sent
std::shared_ptr<std::shared_future<typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr>>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -242,6 +242,12 @@ class BtActionServer
// Default timeout value while waiting for response from a server
std::chrono::milliseconds default_server_timeout_;

// The timeout value for waiting for a service to response
std::chrono::milliseconds wait_for_service_timeout_;

// should the BT be reloaded even if the same xml filename is requested?
bool always_reload_bt_xml_ = false;

// User-provided callbacks
OnGoalReceivedCallback on_goal_received_callback_;
OnLoopCallback on_loop_callback_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -61,6 +61,12 @@ BtActionServer<ActionT>::BtActionServer(
if (!node->has_parameter("default_server_timeout")) {
node->declare_parameter("default_server_timeout", 20);
}
if (!node->has_parameter("always_reload_bt_xml")) {
node->declare_parameter("always_reload_bt_xml", false);
}
if (!node->has_parameter("wait_for_service_timeout")) {
node->declare_parameter("wait_for_service_timeout", 1000);
}

std::vector<std::string> error_code_names = {
"follow_path_error_code",
Expand Down Expand Up @@ -90,7 +96,7 @@ BtActionServer<ActionT>::BtActionServer(
error_codes_str += " " + error_code;
}
RCLCPP_INFO_STREAM(logger_, "Error_code parameters were set to:" << error_codes_str);
}
}
}
}

Expand Down Expand Up @@ -144,6 +150,10 @@ bool BtActionServer<ActionT>::on_configure()
bt_loop_duration_ = std::chrono::milliseconds(timeout);
node->get_parameter("default_server_timeout", timeout);
default_server_timeout_ = std::chrono::milliseconds(timeout);
int wait_for_service_timeout;
node->get_parameter("wait_for_service_timeout", wait_for_service_timeout);
wait_for_service_timeout_ = std::chrono::milliseconds(wait_for_service_timeout);
node->get_parameter("always_reload_bt_xml", always_reload_bt_xml_);

// Get error code id names to grab off of the blackboard
error_code_names_ = node->get_parameter("error_code_names").as_string_array();
Expand All @@ -158,6 +168,9 @@ bool BtActionServer<ActionT>::on_configure()
blackboard_->set<rclcpp::Node::SharedPtr>("node", client_node_); // NOLINT
blackboard_->set<std::chrono::milliseconds>("server_timeout", default_server_timeout_); // NOLINT
blackboard_->set<std::chrono::milliseconds>("bt_loop_duration", bt_loop_duration_); // NOLINT
blackboard_->set<std::chrono::milliseconds>(
"wait_for_service_timeout",
wait_for_service_timeout_);

return true;
}
Expand Down Expand Up @@ -200,8 +213,8 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
// Empty filename is default for backward compatibility
auto filename = bt_xml_filename.empty() ? default_bt_xml_filename_ : bt_xml_filename;

// Use previous BT if it is the existing one
if (current_bt_xml_filename_ == filename) {
// Use previous BT if it is the existing one and always reload flag is not set to true
if (!always_reload_bt_xml_ && current_bt_xml_filename_ == filename) {
RCLCPP_DEBUG(logger_, "BT will not be reloaded as the given xml is already loaded");
return true;
}
Expand All @@ -225,6 +238,9 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
blackboard->set<rclcpp::Node::SharedPtr>("node", client_node_);
blackboard->set<std::chrono::milliseconds>("server_timeout", default_server_timeout_);
blackboard->set<std::chrono::milliseconds>("bt_loop_duration", bt_loop_duration_);
blackboard->set<std::chrono::milliseconds>(
"wait_for_service_timeout",
wait_for_service_timeout_);
}
} catch (const std::exception & e) {
RCLCPP_ERROR(logger_, "Exception when loading BT: %s", e.what());
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,8 @@ class BtCancelActionNode : public BT::ActionNodeBase
server_timeout_ =
config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
wait_for_service_timeout_ =
config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");

std::string remapped_action_name;
if (getInput("server_name", remapped_action_name)) {
Expand Down Expand Up @@ -89,10 +91,10 @@ class BtCancelActionNode : public BT::ActionNodeBase

// Make sure the server is actually there before continuing
RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str());
if (!action_client_->wait_for_action_server(1s)) {
if (!action_client_->wait_for_action_server(wait_for_service_timeout_)) {
RCLCPP_ERROR(
node_->get_logger(), "\"%s\" action server not available after waiting for 1 s",
action_name.c_str());
node_->get_logger(), "\"%s\" action server not available after waiting for %.2fs",
action_name.c_str(), wait_for_service_timeout_.count() / 1000.0);
throw std::runtime_error(
std::string("Action server ") + action_name +
std::string(" not available"));
Expand Down Expand Up @@ -168,6 +170,8 @@ class BtCancelActionNode : public BT::ActionNodeBase
// The timeout value while waiting for response from a server when a
// new action goal is canceled
std::chrono::milliseconds server_timeout_;
// The timeout value for waiting for a service to response
std::chrono::milliseconds wait_for_service_timeout_;
};

} // namespace nav2_behavior_tree
Expand Down
20 changes: 14 additions & 6 deletions nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -57,11 +57,16 @@ class BtServiceNode : public BT::ActionNodeBase
callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());

// Get the required items from the blackboard
bt_loop_duration_ =
auto bt_loop_duration =
config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
server_timeout_ =
config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
wait_for_service_timeout_ =
config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");

// timeout should be less than bt_loop_duration to be able to finish the current tick
max_timeout_ = std::chrono::duration_cast<std::chrono::milliseconds>(bt_loop_duration * 0.5);

// Now that we have node_ to use, create the service client for this BT service
getInput("service_name", service_name_);
Expand All @@ -77,10 +82,10 @@ class BtServiceNode : public BT::ActionNodeBase
RCLCPP_DEBUG(
node_->get_logger(), "Waiting for \"%s\" service",
service_name_.c_str());
if (!service_client_->wait_for_service(1s)) {
if (!service_client_->wait_for_service(wait_for_service_timeout_)) {
RCLCPP_ERROR(
node_->get_logger(), "\"%s\" service server not available after waiting for 1 s",
service_node_name.c_str());
node_->get_logger(), "\"%s\" service server not available after waiting for %.2fs",
service_name_.c_str(), wait_for_service_timeout_.count() / 1000.0);
throw std::runtime_error(
std::string(
"Service server %s not available",
Expand Down Expand Up @@ -187,7 +192,7 @@ class BtServiceNode : public BT::ActionNodeBase
auto remaining = server_timeout_ - elapsed;

if (remaining > std::chrono::milliseconds(0)) {
auto timeout = remaining > bt_loop_duration_ ? bt_loop_duration_ : remaining;
auto timeout = remaining > max_timeout_ ? max_timeout_ : remaining;

rclcpp::FutureReturnCode rc;
rc = callback_group_executor_.spin_until_future_complete(future_result_, timeout);
Expand Down Expand Up @@ -247,7 +252,10 @@ class BtServiceNode : public BT::ActionNodeBase
std::chrono::milliseconds server_timeout_;

// The timeout value for BT loop execution
std::chrono::milliseconds bt_loop_duration_;
std::chrono::milliseconds max_timeout_;

// The timeout value for waiting for a service to response
std::chrono::milliseconds wait_for_service_timeout_;

// To track the server response when a new request is sent
std::shared_future<typename ServiceT::Response::SharedPtr> future_result_;
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,55 @@
// Copyright (c) 2024 Marc Morcos
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GET_POSE_FROM_PATH_ACTION_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GET_POSE_FROM_PATH_ACTION_HPP_

#include <vector>
#include <memory>
#include <string>

#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "nav2_util/robot_utils.hpp"
#include "behaviortree_cpp_v3/action_node.h"
#include "nav_msgs/msg/path.h"

namespace nav2_behavior_tree
{

class GetPoseFromPath : public BT::ActionNodeBase
{
public:
GetPoseFromPath(
const std::string & xml_tag_name,
const BT::NodeConfiguration & conf);


static BT::PortsList providedPorts()
{
return {
BT::InputPort<nav_msgs::msg::Path>("path", "Path to extract pose from"),
BT::OutputPort<geometry_msgs::msg::PoseStamped>("pose", "Stamped Extracted Pose"),
BT::InputPort<int>("index", 0, "Index of pose to extract from. -1 is end of list"),
};
}

private:
void halt() override {}
BT::NodeStatus tick() override;
};

} // namespace nav2_behavior_tree

#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GET_POSE_FROM_PATH_ACTION_HPP_
2 changes: 1 addition & 1 deletion nav2_behavior_tree/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_behavior_tree</name>
<version>1.1.14</version>
<version>1.1.16</version>
<description>TODO</description>
<maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
<maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>
Expand Down
Loading

0 comments on commit 0254af6

Please sign in to comment.