Skip to content

Commit 47f84de

Browse files
SteveMacenskidoisygGuillaume DoisyMarcoMatteoBassatraversaro
authored
Sync kilted Aug 19, 2025 1.4.1 (#5468)
* enable_groot_monitoring_ false (#5246) Signed-off-by: Guillaume Doisy <[email protected]> Co-authored-by: Guillaume Doisy <[email protected]> * Add min_distance_to_obstacle parameter to RPP (#4543) * min_distance_to_obstacle Signed-off-by: Guillaume Doisy <[email protected]> * suggestion to time base and combine Signed-off-by: Guillaume Doisy <[email protected]> * typo Signed-off-by: Guillaume Doisy <[email protected]> * use min_approach_linear_velocity Signed-off-by: Guillaume Doisy <[email protected]> --------- Signed-off-by: Guillaume Doisy <[email protected]> Co-authored-by: Guillaume Doisy <[email protected]> * Parametrizing obstacle layer tf filter tolerance (#5261) Signed-off-by: Marco Bassa <[email protected]> * Fix backport compiler warning (#5277) Signed-off-by: Steve Macenski <[email protected]> * Add missing include of algorithm in differential_motion_model.cpp (#5293) Signed-off-by: Silvio Traversaro <[email protected]> * Remove unused unistd.h header from route_tool.cpp (#5292) Signed-off-by: Silvio Traversaro <[email protected]> * Adding epsilon for voxel_layer precision loss (#5314) * Adding epsilon for voxel_layer precision loss Signed-off-by: bhx <[email protected]> * Update nav2_costmap_2d/plugins/voxel_layer.cpp Signed-off-by: Steve Macenski <[email protected]> * Update nav2_costmap_2d/plugins/voxel_layer.cpp Signed-off-by: Steve Macenski <[email protected]> * Update nav2_costmap_2d/plugins/voxel_layer.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: bhx <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Co-authored-by: Steve Macenski <[email protected]> * fix: correct ThroughActionResult type alias in would_a_planner_recovery_help_condition (#5326) The ThroughActionResult type alias was incorrectly referencing Action::Result instead of ThroughAction::Result, causing the condition to not work properly for ComputePathThroughPoses actions. Fixes #5324 Co-authored-by: claude[bot] <209825114+claude[bot]@users.noreply.github.com> * outputting tracker feedback on ComputeAndTrack BT node (#5327) * outputting tracker feedback on BT node Signed-off-by: Alexander Yuen <[email protected]> * initializing outputs Signed-off-by: Alexander Yuen <[email protected]> * outputting last state on success Signed-off-by: Alexander Yuen <[email protected]> * linting Signed-off-by: Alexander Yuen <[email protected]> * fixed nav2_tree_nodes.xml Signed-off-by: Alexander Yuen <[email protected]> * Update nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: alexanderjyuen <[email protected]> * consolidated function to set outputs null, only setOutput with active feedback Signed-off-by: Alexander Yuen <[email protected]> * add class to method Signed-off-by: Alexander Yuen <[email protected]> * linting Signed-off-by: Alexander Yuen <[email protected]> --------- Signed-off-by: Alexander Yuen <[email protected]> Signed-off-by: alexanderjyuen <[email protected]> Co-authored-by: Steve Macenski <[email protected]> * Adding slow down at target heading to RPP Controller (#5361) * Adding slow down at target heading to RPP Signed-off-by: SteveMacenski <[email protected]> * Update test_regulated_pp.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: SteveMacenski <[email protected]> Signed-off-by: Steve Macenski <[email protected]> * Include <stdexcept> in docking_exceptions.hpp for exception handling (#5363) Signed-off-by: Alberto Tudela <[email protected]> * Eexception rethrow in dockRobot method (#5364) Signed-off-by: Alberto Tudela <[email protected]> * Add global min obstacle height in voxel layer (#5389) * Add min obstacle height in voxel layer Signed-off-by: mini-1235 <[email protected]> * Fix linting Signed-off-by: Maurice <[email protected]> --------- Signed-off-by: mini-1235 <[email protected]> Signed-off-by: Maurice <[email protected]> * [DEX] Enforce 3 digits precision (#5398) Signed-off-by: Guillaume Doisy <[email protected]> Co-authored-by: Guillaume Doisy <[email protected]> * [static_layer] limit comparison precision (#5405) * [DEX] limit comparison precision Signed-off-by: Guillaume Doisy <[email protected]> * EPSILON 1e-5 Signed-off-by: Guillaume Doisy <[email protected]> --------- Signed-off-by: Guillaume Doisy <[email protected]> Co-authored-by: Guillaume Doisy <[email protected]> * corner case bin check (#5413) Signed-off-by: Guillaume Doisy <[email protected]> Co-authored-by: Guillaume Doisy <[email protected]> * Add IndexType definition for Nanoflann KDTree in `node_spatial_tree`. (#5420) * fix: Add KDTree type definition to include unsigned int for IndexType Signed-off-by: Ericsii <[email protected]> * code format Signed-off-by: Ericsii <[email protected]> --------- Signed-off-by: Ericsii <[email protected]> * Smooth path even if goal pose is so much near to the robot (#5423) * Smooth path even if goal pose is so much near to the robot Signed-off-by: CihatAltiparmak <[email protected]> * Apply suggestions Signed-off-by: CihatAltiparmak <[email protected]> * Remove unnecessary diff Signed-off-by: CihatAltiparmak <[email protected]> --------- Signed-off-by: CihatAltiparmak <[email protected]> * Fix KeepoutFilter on the ARM architecture (#5436) Signed-off-by: Sushant Chavan <[email protected]> * Fix missing dependency (#5460) * Support loading multiple behavior tree files as subtrees (#5426) * Support loading multiple behavior tree files as subtrees Signed-off-by: Jad El Hajj <[email protected]> * Fix code style Signed-off-by: Jad El Hajj <[email protected]> * Added default param value Signed-off-by: Jad El Hajj <[email protected]> * Added recursive check to loadBehaviorTree and adapted unit test accordingly Signed-off-by: Jad El Hajj <[email protected]> * Removed nested loadBehaviorTree check in navigators Signed-off-by: Jad El Hajj <[email protected]> * Removed whitespace cpplint Signed-off-by: Jad El Hajj <[email protected]> * Fixed goalReceived Signed-off-by: Jad El Hajj <[email protected]> * Let loadbehaviorTree use its own search_directories var Signed-off-by: Jad El Hajj <[email protected]> * PR fixes-format-lint and test Signed-off-by: Jad El Hajj <[email protected]> * fix pointer Signed-off-by: Jad El Hajj <[email protected]> * Added unit test for BT xml validity Signed-off-by: Jad El Hajj <[email protected]> * CPPLint Signed-off-by: Jad El Hajj <[email protected]> * Check non existent search directory for bt Signed-off-by: Jad El Hajj <[email protected]> * CPPLint Signed-off-by: Jad El Hajj <[email protected]> * Fixed BT tests Signed-off-by: Jad El Hajj <[email protected]> * Fixed BT tests Signed-off-by: Jad El Hajj <[email protected]> --------- Signed-off-by: Jad El Hajj <[email protected]> * bump 1.4.0 to 1.4.1 for aug 19 sync Signed-off-by: SteveMacenski <[email protected]> * Fixing backport error Signed-off-by: SteveMacenski <[email protected]> * load balance CI Signed-off-by: SteveMacenski <[email protected]> * Fixing BT Navigator backport merge conflict issue Signed-off-by: SteveMacenski <[email protected]> --------- Signed-off-by: Guillaume Doisy <[email protected]> Signed-off-by: Marco Bassa <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Silvio Traversaro <[email protected]> Signed-off-by: bhx <[email protected]> Signed-off-by: Alexander Yuen <[email protected]> Signed-off-by: alexanderjyuen <[email protected]> Signed-off-by: SteveMacenski <[email protected]> Signed-off-by: Alberto Tudela <[email protected]> Signed-off-by: mini-1235 <[email protected]> Signed-off-by: Maurice <[email protected]> Signed-off-by: Ericsii <[email protected]> Signed-off-by: CihatAltiparmak <[email protected]> Signed-off-by: Sushant Chavan <[email protected]> Signed-off-by: Jad El Hajj <[email protected]> Co-authored-by: Guillaume Doisy <[email protected]> Co-authored-by: Guillaume Doisy <[email protected]> Co-authored-by: Marco Bassa <[email protected]> Co-authored-by: Silvio Traversaro <[email protected]> Co-authored-by: hutao <[email protected]> Co-authored-by: claude[bot] <209825114+claude[bot]@users.noreply.github.com> Co-authored-by: alexanderjyuen <[email protected]> Co-authored-by: Alberto Tudela <[email protected]> Co-authored-by: mini-1235 <[email protected]> Co-authored-by: YLFeng <[email protected]> Co-authored-by: Cihat Kurtuluş Altıparmak <[email protected]> Co-authored-by: Sushant Chavan <[email protected]> Co-authored-by: Tim Clephas <[email protected]> Co-authored-by: Jad EL HAJJ <[email protected]>
1 parent 09d72e7 commit 47f84de

File tree

80 files changed

+512
-174
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

80 files changed

+512
-174
lines changed

‎.circleci/config.yml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -532,7 +532,7 @@ jobs:
532532

533533
_parameters:
534534
release_parameters: &release_parameters
535-
packages_skip_regex: "nav2_system_tests"
535+
packages_skip_regex: "'(nav2_system_tests|nav2_smac_planner|nav2_mppi_controller|nav2_route|nav2_rviz_plugins|nav2_rotation_shim_controller|nav2_waypoint_follower|nav2_smoother|opennav_docking|nav2_behaviors|nav2_bringup|navigation2)'"
536536

537537
workflows:
538538
version: 2

‎nav2_amcl/package.xml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
33
<package format="3">
44
<name>nav2_amcl</name>
5-
<version>1.4.0</version>
5+
<version>1.4.1</version>
66
<description>
77
<p>
88
amcl is a probabilistic localization system for a robot moving in

‎nav2_amcl/src/motion_model/differential_motion_model.cpp

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919
*
2020
*/
2121

22+
#include <algorithm>
2223
#include <cmath>
2324

2425
#include "nav2_amcl/angleutils.hpp"

‎nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -98,6 +98,12 @@ class BehaviorTreeEngine
9898
*/
9999
void resetGrootMonitor();
100100

101+
/**
102+
* @brief Function to register a BT from an XML file
103+
* @param file_path Path to BT XML file
104+
*/
105+
void registerTreeFromFile(const std::string & file_path);
106+
101107
/**
102108
* @brief Function to explicitly reset all BT nodes to initial state
103109
* @param tree Tree to halt

‎nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -55,6 +55,7 @@ class BtActionServer
5555
const std::string & action_name,
5656
const std::vector<std::string> & plugin_lib_names,
5757
const std::string & default_bt_xml_filename,
58+
const std::vector<std::string> & search_directories,
5859
OnGoalReceivedCallback on_goal_received_callback,
5960
OnLoopCallback on_loop_callback,
6061
OnPreemptCallback on_preempt_callback,
@@ -104,7 +105,8 @@ class BtActionServer
104105
* @return bool true if the resulting BT correspond to the one in bt_xml_filename. false
105106
* if something went wrong, and previous BT is maintained
106107
*/
107-
bool loadBehaviorTree(const std::string & bt_xml_filename = "");
108+
bool loadBehaviorTree(
109+
const std::string & bt_xml_filename = "");
108110

109111
/**
110112
* @brief Getter function for BT Blackboard
@@ -247,6 +249,7 @@ class BtActionServer
247249
// The XML file that contains the Behavior Tree to create
248250
std::string current_bt_xml_filename_;
249251
std::string default_bt_xml_filename_;
252+
std::vector<std::string> search_directories_;
250253

251254
// The wrapper class for the BT functionality
252255
std::unique_ptr<nav2_behavior_tree::BehaviorTreeEngine> bt_;
@@ -285,7 +288,7 @@ class BtActionServer
285288
bool always_reload_bt_xml_ = false;
286289

287290
// Parameters for Groot2 monitoring
288-
bool enable_groot_monitoring_ = true;
291+
bool enable_groot_monitoring_ = false;
289292
int groot_server_port_ = 1667;
290293

291294
// User-provided callbacks

‎nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

Lines changed: 30 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -39,12 +39,14 @@ BtActionServer<ActionT>::BtActionServer(
3939
const std::string & action_name,
4040
const std::vector<std::string> & plugin_lib_names,
4141
const std::string & default_bt_xml_filename,
42+
const std::vector<std::string> & search_directories,
4243
OnGoalReceivedCallback on_goal_received_callback,
4344
OnLoopCallback on_loop_callback,
4445
OnPreemptCallback on_preempt_callback,
4546
OnCompletionCallback on_completion_callback)
4647
: action_name_(action_name),
4748
default_bt_xml_filename_(default_bt_xml_filename),
49+
search_directories_(search_directories),
4850
plugin_lib_names_(plugin_lib_names),
4951
node_(parent),
5052
on_goal_received_callback_(on_goal_received_callback),
@@ -245,6 +247,8 @@ void BtActionServer<ActionT>::setGrootMonitoring(const bool enable, const unsign
245247
template<class ActionT>
246248
bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filename)
247249
{
250+
namespace fs = std::filesystem;
251+
248252
// Empty filename is default for backward compatibility
249253
auto filename = bt_xml_filename.empty() ? default_bt_xml_filename_ : bt_xml_filename;
250254

@@ -254,19 +258,38 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
254258
return true;
255259
}
256260

257-
// if a new tree is created, than the Groot2 Publisher must be destroyed
261+
// Reset any existing Groot2 monitoring
258262
bt_->resetGrootMonitor();
259263

260-
// Read the input BT XML from the specified file into a string
261264
std::ifstream xml_file(filename);
262-
263265
if (!xml_file.good()) {
264266
setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
265-
"Couldn't open input XML file: " + filename);
267+
"Couldn't open BT XML file: " + filename);
266268
return false;
267269
}
268270

269-
// Create the Behavior Tree from the XML input
271+
const auto canonical_main_bt = fs::canonical(filename);
272+
273+
// Register all XML behavior Subtrees found in the given directories
274+
for (const auto & directory : search_directories_) {
275+
try {
276+
for (const auto & entry : fs::directory_iterator(directory)) {
277+
if (entry.path().extension() == ".xml") {
278+
// Skip registering the main tree file
279+
if (fs::equivalent(fs::canonical(entry.path()), canonical_main_bt)) {
280+
continue;
281+
}
282+
bt_->registerTreeFromFile(entry.path().string());
283+
}
284+
}
285+
} catch (const std::exception & e) {
286+
setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
287+
"Exception reading behavior tree directory: " + std::string(e.what()));
288+
return false;
289+
}
290+
}
291+
292+
// Try to load the main BT tree
270293
try {
271294
tree_ = bt_->createTreeFromFile(filename, blackboard_);
272295
for (auto & subtree : tree_.subtrees) {
@@ -280,15 +303,15 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
280303
}
281304
} catch (const std::exception & e) {
282305
setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
283-
std::string("Exception when loading BT: ") + e.what());
306+
std::string("Exception when creating BT tree from file: ") + e.what());
284307
return false;
285308
}
286309

310+
// Optional logging and monitoring
287311
topic_logger_ = std::make_unique<RosTopicLogger>(client_node_, tree_);
288312

289313
current_bt_xml_filename_ = filename;
290314

291-
// Enable monitoring with Groot2
292315
if (enable_groot_monitoring_) {
293316
bt_->addGrootMonitoring(&tree_, groot_server_port_);
294317
RCLCPP_DEBUG(

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp

Lines changed: 28 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -78,6 +78,11 @@ class ComputeAndTrackRouteAction : public BtActionNode<nav2_msgs::action::Comput
7878
void on_wait_for_result(
7979
std::shared_ptr<const Action::Feedback> feedback) override;
8080

81+
/**
82+
* @brief Function to set all feedbacks and output ports to be null values
83+
*/
84+
void resetFeedbackAndOutputPorts();
85+
8186
/**
8287
* @brief Creates list of BT ports
8388
* @return BT::PortsList Containing basic ports along with node-specific ports
@@ -98,14 +103,36 @@ class ComputeAndTrackRouteAction : public BtActionNode<nav2_msgs::action::Comput
98103
"Whether to use the start pose or the robot's current pose"),
99104
BT::InputPort<bool>(
100105
"use_poses", false, "Whether to use poses or IDs for start and goal"),
101-
BT::OutputPort<builtin_interfaces::msg::Duration>("execution_duration",
106+
BT::OutputPort<builtin_interfaces::msg::Duration>(
107+
"execution_duration",
102108
"Time taken to compute and track route"),
103109
BT::OutputPort<ActionResult::_error_code_type>(
104110
"error_code_id", "The compute route error code"),
105111
BT::OutputPort<std::string>(
106112
"error_msg", "The compute route error msg"),
113+
BT::OutputPort<uint16_t>(
114+
"last_node_id",
115+
"ID of the previous node"),
116+
BT::OutputPort<uint16_t>(
117+
"next_node_id",
118+
"ID of the next node"),
119+
BT::OutputPort<uint16_t>(
120+
"current_edge_id",
121+
"ID of current edge"),
122+
BT::OutputPort<nav2_msgs::msg::Route>(
123+
"route",
124+
"List of RouteNodes to go from start to end"),
125+
BT::OutputPort<nav_msgs::msg::Path>(
126+
"path",
127+
"Path created by ComputeAndTrackRoute node"),
128+
BT::OutputPort<bool>(
129+
"rerouted",
130+
"Whether the plan has rerouted"),
107131
});
108132
}
133+
134+
protected:
135+
Action::Feedback feedback_;
109136
};
110137

111138
} // namespace nav2_behavior_tree

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.hpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -29,7 +29,7 @@ class WouldAPlannerRecoveryHelp : public AreErrorCodesPresent
2929
using Action = nav2_msgs::action::ComputePathToPose;
3030
using ActionResult = Action::Result;
3131
using ThroughAction = nav2_msgs::action::ComputePathThroughPoses;
32-
using ThroughActionResult = Action::Result;
32+
using ThroughActionResult = ThroughAction::Result;
3333

3434
public:
3535
WouldAPlannerRecoveryHelp(

‎nav2_behavior_tree/nav2_tree_nodes.xml

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -128,6 +128,12 @@
128128
<output_port name="execution_time">Duration to compute route</output_port>
129129
<output_port name="error_code_id">"Compute route to pose error code"</output_port>
130130
<output_port name="error_msg">"Compute route to pose error message"</output_port>
131+
<output_port name="last_node_id">"ID of the previous node"</output_port>
132+
<output_port name="next_node_id">"ID of the next node"</output_port>
133+
<output_port name="current_edge_id">"ID of the current edge"</output_port>
134+
<output_port name="route">"List of RouteNodes to go from start to end"</output_port>
135+
<output_port name="path">"Path created by ComputeAndTrackRoute node"</output_port>
136+
<output_port name="rerouted">"Whether the plan has rerouted"</output_port>
131137
</Action>
132138

133139
<Action ID="RemovePassedGoals">

‎nav2_behavior_tree/package.xml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
33
<package format="3">
44
<name>nav2_behavior_tree</name>
5-
<version>1.4.0</version>
5+
<version>1.4.1</version>
66
<description>Nav2 behavior tree wrappers, nodes, and utilities</description>
77
<maintainer email="[email protected]">Michael Jeronimo</maintainer>
88
<maintainer email="[email protected]">Carlos Orduno</maintainer>

0 commit comments

Comments
 (0)