Fix: Ensure `server_timeout` Respects Default Values Using `getInputPortOrBlackboard()` by alanxuefei · Pull Request #4649 · ros-navigation/navigation2

alanxuefei · 2024-08-25T13:38:23Z

Basic Info

Info	Details
Ticket(s) this addresses	#4618
Primary OS tested on	Ubuntu 22.04
Robotic platform tested on	Gazebo simulation, Turtlebot3 hardware
Does this PR contain AI generated software?	No

Description of contribution in a few bullet points

Updated bt_action_node, bt_cancel_action_node, and bt_service_node to use getInputPortOrBlackboard() for initializing server_timeout.
Ensured that the server_timeout parameter now respects the default value from the blackboard if not explicitly set in the BT action node.
This change addresses the issue where the default_server_timeout was always overridden, leading to more consistent behavior and reducing configuration overhead.

Description of documentation updates required from your changes

No new parameters were added, so no updates to default configs are necessary.

Future work that may be required in bullet points

Further testing on different robotic platforms to ensure compatibility.
Potential optimizations in the BT nodes to enhance performance.
Documentation updates in case of additional related changes in the future.

For Maintainers:

Check that any new parameters added are updated in docs.nav2.org
Check that any significant change is added to the migration guide
Check that any new features OR changes to existing behaviors are reflected in the tuning guide
Check that any new functions have Doxygen added
Check that any new features have test coverage
Check that any new plugins are added to the plugins page
If BT Node, Additionally: add to BT's XML index of nodes for Groot, BT package's readme table, and BT library lists

mergify · 2024-08-25T13:41:47Z

@alanxuefei, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2024-08-25T13:49:19Z

@alanxuefei, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

…tInputPortOrBlackboard()` Signed-off-by: Alan Xue <alan.xuefei@googlemail.com>

SteveMacenski · 2024-08-26T18:41:46Z

@alanxuefei isn't this still true?

We'd have the change all the BT nodes not to have defaults in that case then so that getInput would return false when unset to use the global timeout.

alanxuefei · 2024-08-30T14:09:20Z

@alanxuefei isn't this still true?

We'd have the change all the BT nodes not to have defaults in that case then so that getInput would return false when unset to use the global timeout.

Yes, the statement is correct.

In the default behavior trees (e.g., navigate_to_pose_w_replanning_goal_patience_and_recovery.xml), there is no server_timeout parameter.

After second analysis, the original code actually works:

// Check ROS2 parameter.
server_timeout_ = config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
// Check BT port (if the BT port is unset, server_timeout_ remains unchanged).    
getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);

However, the new code is more concise and efficient (inline):

template<typename T> inline
bool getInputPortOrBlackboard(
  const BT::TreeNode & bt_node,
  const BT::Blackboard & blackboard,
  const std::string & param_name,
  T & value)
{
  if (bt_node.getInput<T>(param_name, value)) {       //check BT port first
    return true;
  }
  if (blackboard.get<T>(param_name, value)) {           // check ROS2 param
    return true;
  }
  return false;
}

SteveMacenski · 2024-08-30T18:16:54Z

So this doesn't change the behavior? I agree this is cleaner so happy to merge either way, but what sparked this PR / work if you weren't running into a problem (and do you still have a problem)?

alanxuefei · 2024-08-31T15:43:41Z

So this doesn't change the behavior? I agree this is cleaner so happy to merge either way, but what sparked this PR / work if you weren't running into a problem (and do you still have a problem)?

We mistakenly thought that once a BT port is declared in a BT node, it must also be included in the XML file. In fact, if the port is not defined in the XML file, getInput will return false without causing any system errors and remain server_timeout unchanged. For server_timeout, we can simply remove it from xml and it will use default value.

SteveMacenski · 2024-08-31T19:41:28Z

So we're good to merge this? I assume so, just wanting to verify there's nothing else you want to include here

alanxuefei · 2024-09-01T00:09:15Z

Since it is a base function for each BT node, it's good to double-check its functionality. The tests have passed, and the logic has been verified. I believe it's ready to be merged.

…tInputPortOrBlackboard()` (ros-navigation#4649) Signed-off-by: Alan Xue <alan.xuefei@googlemail.com>

…tInputPortOrBlackboard()` (#4649) Signed-off-by: Alan Xue <alan.xuefei@googlemail.com>

* Adding non-charging dock support to docking server (for conveyers, pallots, etc) (#4627) * adding non-charging dock support to docking server Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * docs and linting * adding unit tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Publish optimal trajectory as a Path message (#4640) * Publish optimal trajectory as a Path message Signed-off-by: Alyssa Agnissan <alyssa.agnissan@quantillion.io> * move publish_optimal_path to TrajectoryVisualizer + minor refactoring Signed-off-by: Alyssa Agnissan <alyssa.agnissan@quantillion.io> * tests added for optimal path publication Signed-off-by: Alyssa Agnissan <alyssa.agnissan@quantillion.io> * populate optimal path message in add() Signed-off-by: Alyssa Agnissan <alyssa.agnissan@quantillion.io> * move path population in add_marker Signed-off-by: Alyssa Agnissan <alyssa.agnissan@quantillion.io> --------- Signed-off-by: Alyssa Agnissan <alyssa.agnissan@quantillion.io> * [collision monitor] Select the observation sources used with each polygon (#4227) * Collision monitor: select specific observation sources for polygon Signed-off-by: asarazin <anaelle.sarazin@robocc.com> * optimization Signed-off-by: asarazin <anaelle.sarazin@robocc.com> * add tests Signed-off-by: asarazin <anaelle.sarazin@robocc.com> --------- Signed-off-by: asarazin <anaelle.sarazin@robocc.com> Co-authored-by: asarazin <anaelle.sarazin@robocc.com> * Restore exported BT test utils header files after cmake revamp (#4652) (#4654) Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * fix(bt_nodes): Correct default `server_timeout` behavior by using `getInputPortOrBlackboard()` (#4649) Signed-off-by: Alan Xue <alan.xuefei@googlemail.com> * PoseStamped vector specialization (#4607) * PoseStamped vector specialization Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * merge master Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * add path Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix size check Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix test Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Revert "fix test" This reverts commit 51f54eb. * fix test Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> --------- Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * [DWB] Option to limit velocity commands in trajectory generator (#4663) * Option to limit vel cmd through traj generator Signed-off-by: huiyulhy <lhyleonghuiyu@gmail.com> * Cleanup Signed-off-by: huiyulhy <lhyleonghuiyu@gmail.com> * fix linting Signed-off-by: huiyulhy <lhyleonghuiyu@gmail.com> * Update linting Signed-off-by: huiyulhy <lhyleonghuiyu@gmail.com> * uncrustify Signed-off-by: huiyulhy <lhyleonghuiyu@gmail.com> * uncrustify Signed-off-by: huiyulhy <lhyleonghuiyu@gmail.com> --------- Signed-off-by: huiyulhy <lhyleonghuiyu@gmail.com> * Adding planner server timeout for costmap waiting (#4673) * Adding planner server timeout for costmap waiting Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Adding controller server's costmap timeout as well Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fixing path longer on approach (#4622) * fixing path longer on approach Signed-off-by: Pradheep <padhupradheep@gmail.com> * removing the short circuit Signed-off-by: Pradheep <padhupradheep@gmail.com> * adding additional layer of check Signed-off-by: Pradheep <padhupradheep@gmail.com> --------- Signed-off-by: Pradheep <padhupradheep@gmail.com> * fix to bt action server logging before bt execution result being ready (#4677) Signed-off-by: DreamWest <sirjamestsao@gmail.com> * Correct paper name for graceful controller Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Added missing action clients in robot_navigator(BasicNavigator) to destroy_node (#4698) * fix: added assisted_teleop_client to robot_navigator(BasicNavigator) destroy_node Signed-off-by: Tiwa Ojo <tiwa@reindeere.ca> * fix: added other missing action clients to robot_navigator(BasicNavigator) destroy_node Signed-off-by: Tiwa Ojo <tiwa@reindeere.ca> --------- Signed-off-by: Tiwa Ojo <tiwa@reindeere.ca> * Fixing SGF in MPPI and Smoother (#4669) Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fix: handle transition failures in all servers (#4708) * fix: handle transition failures in planner/controller/smoother servers Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> * adding support for rest of servers + review comments Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Replacing throws with error and failed lifecycle transitions Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fix vel smoother unit tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fixing docking server unit testing Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fixing last bits Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Kemal Bektas <kemal.bektas@node-robotics.com> * [RotationShimController] fix: rotate on short paths (#4716) Add header data to goal for short paths. Commit d8ae3c1 added the possibility to the rotation shim controller to rotate towards the goal when the goal was closer that the `forward_sampling_distance`. This feature was not fully working as the goal was missing proper header data, causing the rotation shim to give back control to the main controller. Co-authored-by: agennart <antoine.gennart@quimesis.be> * Added parameter `rotate_to_heading_once` (#4721) Signed-off-by: Daniil Khaninaev <khaninaev@yahoo.com> * [RotationShimController] fix: rotate to goal heading (#4724) Add frame_id to goal when rotating towards goal heading, otherwise the transform would fail. This bug was introduced in 30e2cde by not setting the frame_id. Signed-off-by: agennart <antoine.gennart@quimesis.be> Co-authored-by: agennart <antoine.gennart@quimesis.be> * [loopback_sim] Publish clock, [nav2_costmap_2d] Fix Qos (#4726) * Publish /clock from loopback sim Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * [nav2_costmap_2d] Fix obstacle_layer trying to use RELIABLE QoS Use QoS profile from rclcpp::SensorDataQoS() instead of rmw_qos_profile_t. This solves an issue where the subscriber uses RELIABLE setting even when initialized from rmw_qos_profile_sensor_data. In addition the Subscriber(..., rmw_qos_profile_t) constructor is deprecated in favor of Subscriber(..., rclcpp::QoS) Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * [nav2_smac_planner] fix typos Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * Use single quotes Signed-off-by: Adi Vardi <adi.vardi@enway.ai> --------- Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * Fix incorrect doxygen comment (#4741) Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> * Updating error logging in Smac collision detector object (#4743) * Updating error logging in Smac configs Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * linting Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * [map_io] Replace std logs by rclcpp logs (#4720) * replace std logs by rclcpp logs Signed-off-by: Guillaume Doisy <guillaume@dexory.com> * RCLCPP_DEBUG to RCLCPP_INFO for visibility Signed-off-by: Guillaume Doisy <guillaume@dexory.com> --------- Signed-off-by: Guillaume Doisy <guillaume@dexory.com> Co-authored-by: Guillaume Doisy <guillaume@dexory.com> * manual backport to Jazzy of 6b2e244 Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * bump to 1.3.3 for jazzy sync Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fixing backport issue Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fixing backport of docking linking changes Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Alyssa Agnissan <alyssa.agnissan@quantillion.io> Signed-off-by: asarazin <anaelle.sarazin@robocc.com> Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> Signed-off-by: Alan Xue <alan.xuefei@googlemail.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> Signed-off-by: huiyulhy <lhyleonghuiyu@gmail.com> Signed-off-by: Pradheep <padhupradheep@gmail.com> Signed-off-by: DreamWest <sirjamestsao@gmail.com> Signed-off-by: Tiwa Ojo <tiwa@reindeere.ca> Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> Signed-off-by: Daniil Khaninaev <khaninaev@yahoo.com> Signed-off-by: agennart <antoine.gennart@quimesis.be> Signed-off-by: Adi Vardi <adi.vardi@enway.ai> Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> Signed-off-by: Guillaume Doisy <guillaume@dexory.com> Co-authored-by: alyquantillion <158563995+alyquantillion@users.noreply.github.com> Co-authored-by: anaelle-sw <63144493+anaelle-sw@users.noreply.github.com> Co-authored-by: asarazin <anaelle.sarazin@robocc.com> Co-authored-by: aosmw <116058035+aosmw@users.noreply.github.com> Co-authored-by: Alan <alan.xuefei@googlemail.com> Co-authored-by: Tony Najjar <tony.najjar.1997@gmail.com> Co-authored-by: Huiyu Leong <26198479+huiyulhy@users.noreply.github.com> Co-authored-by: Pradheep Krishna <padhupradheep@gmail.com> Co-authored-by: DreamWest <sirjamestsao@gmail.com> Co-authored-by: Tiwa Ojo <55967921+tiwaojo@users.noreply.github.com> Co-authored-by: Kemal Bektas <kemal.bektas@node-robotics.com> Co-authored-by: Saitama <gennartan@users.noreply.github.com> Co-authored-by: agennart <antoine.gennart@quimesis.be> Co-authored-by: Daniil Khaninaev <khaninaev@yahoo.com> Co-authored-by: Adi Vardi <57910756+adivardi@users.noreply.github.com> Co-authored-by: Ryan <25047695+Ryanf55@users.noreply.github.com> Co-authored-by: Guillaume Doisy <doisyg@users.noreply.github.com> Co-authored-by: Guillaume Doisy <guillaume@dexory.com>

…tInputPortOrBlackboard()` (ros-navigation#4649) Signed-off-by: Alan Xue <alan.xuefei@googlemail.com> Signed-off-by: Joseph Duchesne <josephgeek@gmail.com>

…tInputPortOrBlackboard()` (ros-navigation#4649) Signed-off-by: Alan Xue <alan.xuefei@googlemail.com> Signed-off-by: stevedanomodolor <stevedan.o.omodolor@gmail.com>

alanxuefei force-pushed the fix_bt_port_init_value branch from 26eb486 to b064eb5 Compare August 25, 2024 13:45

fix(bt_nodes): Correct default server_timeout behavior by using `ge…

75c9cfc

…tInputPortOrBlackboard()` Signed-off-by: Alan Xue <alan.xuefei@googlemail.com>

alanxuefei force-pushed the fix_bt_port_init_value branch from b064eb5 to 75c9cfc Compare August 25, 2024 14:01

SteveMacenski approved these changes Aug 26, 2024

View reviewed changes

SteveMacenski linked an issue Aug 26, 2024 that may be closed by this pull request

The default_server_timeout parameter in bt_navigator is always overridden by the server_timeout port in each BT action node. #4618

Closed

SteveMacenski merged commit 34d41ed into ros-navigation:main Sep 3, 2024

masf7g pushed a commit to quasi-robotics/navigation2 that referenced this pull request Oct 23, 2024

fix(bt_nodes): Correct default server_timeout behavior by using `ge…

925ed20

…tInputPortOrBlackboard()` (ros-navigation#4649) Signed-off-by: Alan Xue <alan.xuefei@googlemail.com>

SteveMacenski pushed a commit that referenced this pull request Nov 8, 2024

fix(bt_nodes): Correct default server_timeout behavior by using `ge…

4597214

…tInputPortOrBlackboard()` (#4649) Signed-off-by: Alan Xue <alan.xuefei@googlemail.com>

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Fix: Ensure `server_timeout` Respects Default Values Using `getInputPortOrBlackboard()`#4649

Fix: Ensure `server_timeout` Respects Default Values Using `getInputPortOrBlackboard()`#4649
SteveMacenski merged 1 commit intoros-navigation:mainfrom
alanxuefei:fix_bt_port_init_value

alanxuefei commented Aug 25, 2024

Uh oh!

mergify bot commented Aug 25, 2024

Uh oh!

mergify bot commented Aug 25, 2024

Uh oh!

SteveMacenski commented Aug 26, 2024

Uh oh!

alanxuefei commented Aug 30, 2024

Uh oh!

SteveMacenski commented Aug 30, 2024

Uh oh!

alanxuefei commented Aug 31, 2024 •

edited

Loading

Uh oh!

SteveMacenski commented Aug 31, 2024 •

edited

Loading

Uh oh!

alanxuefei commented Sep 1, 2024

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

Conversation

alanxuefei commented Aug 25, 2024

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Future work that may be required in bullet points

For Maintainers:

Uh oh!

mergify bot commented Aug 25, 2024

Uh oh!

mergify bot commented Aug 25, 2024

Uh oh!

SteveMacenski commented Aug 26, 2024

Uh oh!

alanxuefei commented Aug 30, 2024

Uh oh!

SteveMacenski commented Aug 30, 2024

Uh oh!

alanxuefei commented Aug 31, 2024 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

SteveMacenski commented Aug 31, 2024 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

alanxuefei commented Sep 1, 2024

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

alanxuefei commented Aug 31, 2024 •

edited

Loading

SteveMacenski commented Aug 31, 2024 •

edited

Loading