From 636668e20b957dd56900ca17f6ee2117153af5f6 Mon Sep 17 00:00:00 2001 From: Dhruv Patel Date: Wed, 26 Nov 2025 21:19:43 +0100 Subject: [PATCH 1/4] Fix macOS compilation issues in Navigation2 packages Signed-off-by: Dhruv Patel --- .../bt_action_server_impl.hpp | 23 ++++++++++--------- .../include/nav2_route/route_server.hpp | 2 +- nav2_route/src/route_server.cpp | 2 +- 3 files changed, 14 insertions(+), 13 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 40c220ec33a..88fc0084477 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -176,10 +176,10 @@ bool BtActionServer::on_configure() blackboard_ = BT::Blackboard::create(); // Put items on the blackboard - blackboard_->set("node", client_node_); // NOLINT - blackboard_->set("server_timeout", default_server_timeout_); // NOLINT - blackboard_->set("bt_loop_duration", bt_loop_duration_); // NOLINT - blackboard_->set( + blackboard_->template set("node", client_node_); // NOLINT + blackboard_->template set("server_timeout", default_server_timeout_); // NOLINT + blackboard_->template set("bt_loop_duration", bt_loop_duration_); // NOLINT + blackboard_->template set( "wait_for_service_timeout", wait_for_service_timeout_); @@ -328,11 +328,12 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml for (auto & subtree : tree_.subtrees) { auto & blackboard = subtree->blackboard; - blackboard->set("node", client_node_); - blackboard->set("server_timeout", default_server_timeout_); - blackboard->set("bt_loop_duration", bt_loop_duration_); - blackboard->set( - "wait_for_service_timeout", wait_for_service_timeout_); + blackboard->template set("node", client_node_); + blackboard->template set("server_timeout", default_server_timeout_); + blackboard->template set("bt_loop_duration", bt_loop_duration_); + blackboard->template set( + "wait_for_service_timeout", + wait_for_service_timeout_); } } catch (const std::exception & e) { setInternalError( @@ -498,9 +499,9 @@ void BtActionServer::cleanErrorCodes() std::string name; for (const auto & error_code_name_prefix : error_code_name_prefixes_) { name = error_code_name_prefix + "_error_code"; - blackboard_->set(name, 0); //NOLINT + blackboard_->template set(name, 0); //NOLINT name = error_code_name_prefix + "_error_msg"; - blackboard_->set(name, ""); + blackboard_->template set(name, ""); } resetInternalError(); } diff --git a/nav2_route/include/nav2_route/route_server.hpp b/nav2_route/include/nav2_route/route_server.hpp index ce52c07e750..b445c9ea793 100644 --- a/nav2_route/include/nav2_route/route_server.hpp +++ b/nav2_route/include/nav2_route/route_server.hpp @@ -130,7 +130,7 @@ class RouteServer : public nav2::LifecycleNode template Route findRoute( const std::shared_ptr goal, - ReroutingState & rerouting_info = ReroutingState()); + ReroutingState rerouting_info = ReroutingState()); /** * @brief Main processing called by both action server callbacks to centralize diff --git a/nav2_route/src/route_server.cpp b/nav2_route/src/route_server.cpp index c68ebbca5d5..0e0c71d5311 100644 --- a/nav2_route/src/route_server.cpp +++ b/nav2_route/src/route_server.cpp @@ -214,7 +214,7 @@ void RouteServer::populateActionResult( template Route RouteServer::findRoute( const std::shared_ptr goal, - ReroutingState & rerouting_info) + ReroutingState rerouting_info) { // Find the search boundaries auto [start_route, end_route] = goal_intent_extractor_->findStartandGoal(goal); From d25683f67270be9fb55263ab5a7f8693bd33973b Mon Sep 17 00:00:00 2001 From: Dhruv Patel Date: Wed, 26 Nov 2025 21:35:12 +0100 Subject: [PATCH 2/4] Pre-commit Fix Signed-off-by: Dhruv Patel --- .../include/nav2_behavior_tree/bt_action_server_impl.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 88fc0084477..4f7a16299ea 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -329,7 +329,8 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml for (auto & subtree : tree_.subtrees) { auto & blackboard = subtree->blackboard; blackboard->template set("node", client_node_); - blackboard->template set("server_timeout", default_server_timeout_); + blackboard->template set("server_timeout", + default_server_timeout_); blackboard->template set("bt_loop_duration", bt_loop_duration_); blackboard->template set( "wait_for_service_timeout", From 6ab59fe7afca129ae4266023571cbd6b0e001572 Mon Sep 17 00:00:00 2001 From: Dhruv Patel Date: Thu, 27 Nov 2025 21:23:59 +0100 Subject: [PATCH 3/4] Fix: adding default template findRoute function > reverting to nav2::LifecycleNode rather using rclcpp::Node Signed-off-by: Dhruv Patel --- .../bt_action_server_impl.hpp | 2 +- .../plugins/route_operation_client.hpp | 2 +- .../include/nav2_route/route_server.hpp | 39 +++++++++++++++---- nav2_route/src/route_server.cpp | 10 ++++- 4 files changed, 42 insertions(+), 11 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 4f7a16299ea..6736b6f0c3b 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -176,7 +176,7 @@ bool BtActionServer::on_configure() blackboard_ = BT::Blackboard::create(); // Put items on the blackboard - blackboard_->template set("node", client_node_); // NOLINT + blackboard_->template set("node", client_node_); // NOLINT blackboard_->template set("server_timeout", default_server_timeout_); // NOLINT blackboard_->template set("bt_loop_duration", bt_loop_duration_); // NOLINT blackboard_->template set( diff --git a/nav2_route/include/nav2_route/plugins/route_operation_client.hpp b/nav2_route/include/nav2_route/plugins/route_operation_client.hpp index 16df9c33a4e..41db78e4984 100644 --- a/nav2_route/include/nav2_route/plugins/route_operation_client.hpp +++ b/nav2_route/include/nav2_route/plugins/route_operation_client.hpp @@ -164,7 +164,7 @@ class RouteOperationClient : public RouteOperation "Route operation service (" + getName() + ") failed to lock node."); } auto client = - node->create_client(srv_name, true); + node->template create_client(srv_name, true); response = client->invoke(req, std::chrono::nanoseconds(500ms)); } } catch (const std::exception & e) { diff --git a/nav2_route/include/nav2_route/route_server.hpp b/nav2_route/include/nav2_route/route_server.hpp index b445c9ea793..a88ccb3e28f 100644 --- a/nav2_route/include/nav2_route/route_server.hpp +++ b/nav2_route/include/nav2_route/route_server.hpp @@ -119,18 +119,41 @@ class RouteServer : public nav2::LifecycleNode void computeAndTrackRoute(); /** - * @brief Abstract method combining finding the starting/ending nodes and the route planner - * to find the Node locations of interest and route to the goal - * @param goal The request goal information - * @param blocked_ids The IDs of blocked graphs / edges - * @param updated_start_id The nodeID of an updated starting point when tracking - * a route that corresponds to the next point to route to from to continue progress - * @return A route of the request + * @brief Compute a route to the goal. + * + * This method determines the start and goal nodes and uses the route planner + * to generate a path from the current start to the requested goal. This + * overload uses a default (empty) ReroutingState and performs a standard, + * non-rerouted route computation. + * + * @param goal The request goal information. + * @return A route from the selected start node to the goal. + */ + template + Route findRoute( + const std::shared_ptr goal); + + /** + * @brief Compute a route to the goal, incorporating rerouting information. + * + * This method combines finding the starting and ending nodes with the route + * planner to determine the relevant node locations and compute the route. + * The provided ReroutingState can specify: + * - an updated starting node ID when tracking a previous route, + * - an overridden starting pose for rerouting, + * - optional constraints such as blocked elements to avoid (if provided). + * + * This allows progress to continue smoothly when re-planning or recovering + * from partial execution of a previous route. + * + * @param goal The request goal information. + * @param rerouting_info State describing updated start and rerouting context. + * @return A route from the selected start node to the goal. */ template Route findRoute( const std::shared_ptr goal, - ReroutingState rerouting_info = ReroutingState()); + ReroutingState & rerouting_info); /** * @brief Main processing called by both action server callbacks to centralize diff --git a/nav2_route/src/route_server.cpp b/nav2_route/src/route_server.cpp index 0e0c71d5311..56b629aead7 100644 --- a/nav2_route/src/route_server.cpp +++ b/nav2_route/src/route_server.cpp @@ -211,10 +211,18 @@ void RouteServer::populateActionResult( result->execution_duration = execution_duration; } +template +Route RouteServer::findRoute( + const std::shared_ptr goal) +{ + ReroutingState rerouting_info; + return findRoute(goal, rerouting_info); +} + template Route RouteServer::findRoute( const std::shared_ptr goal, - ReroutingState rerouting_info) + ReroutingState & rerouting_info) { // Find the search boundaries auto [start_route, end_route] = goal_intent_extractor_->findStartandGoal(goal); From c7322168b809041a6a9a3a6744f68fd6a154ba65 Mon Sep 17 00:00:00 2001 From: Dhruv Patel Date: Tue, 2 Dec 2025 19:41:27 +0100 Subject: [PATCH 4/4] Fix: removing temporary default in template function argument Signed-off-by: Dhruv Patel --- nav2_route/include/nav2_route/route_server.hpp | 15 --------------- nav2_route/src/route_server.cpp | 8 -------- 2 files changed, 23 deletions(-) diff --git a/nav2_route/include/nav2_route/route_server.hpp b/nav2_route/include/nav2_route/route_server.hpp index a88ccb3e28f..81d644afaa7 100644 --- a/nav2_route/include/nav2_route/route_server.hpp +++ b/nav2_route/include/nav2_route/route_server.hpp @@ -118,21 +118,6 @@ class RouteServer : public nav2::LifecycleNode void computeRoute(); void computeAndTrackRoute(); - /** - * @brief Compute a route to the goal. - * - * This method determines the start and goal nodes and uses the route planner - * to generate a path from the current start to the requested goal. This - * overload uses a default (empty) ReroutingState and performs a standard, - * non-rerouted route computation. - * - * @param goal The request goal information. - * @return A route from the selected start node to the goal. - */ - template - Route findRoute( - const std::shared_ptr goal); - /** * @brief Compute a route to the goal, incorporating rerouting information. * diff --git a/nav2_route/src/route_server.cpp b/nav2_route/src/route_server.cpp index 56b629aead7..c68ebbca5d5 100644 --- a/nav2_route/src/route_server.cpp +++ b/nav2_route/src/route_server.cpp @@ -211,14 +211,6 @@ void RouteServer::populateActionResult( result->execution_duration = execution_duration; } -template -Route RouteServer::findRoute( - const std::shared_ptr goal) -{ - ReroutingState rerouting_info; - return findRoute(goal, rerouting_info); -} - template Route RouteServer::findRoute( const std::shared_ptr goal,