Skip to content
Open
Show file tree
Hide file tree
Changes from 1 commit
Commits
File filter

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
Fix: adding default template findRoute function
> reverting to nav2::LifecycleNode rather using rclcpp::Node

Signed-off-by: Dhruv Patel <dhruvpatel2991998@gmail.com>
  • Loading branch information
idesign0 committed Nov 27, 2025
commit 6ab59fe7afca129ae4266023571cbd6b0e001572
Original file line number Diff line number Diff line change
Expand Up @@ -176,7 +176,7 @@ bool BtActionServer<ActionT, NodeT>::on_configure()
blackboard_ = BT::Blackboard::create();

// Put items on the blackboard
blackboard_->template set<rclcpp::Node::SharedPtr>("node", client_node_); // NOLINT
blackboard_->template set<nav2::LifecycleNode::SharedPtr>("node", client_node_); // NOLINT
blackboard_->template set<std::chrono::milliseconds>("server_timeout", default_server_timeout_); // NOLINT
blackboard_->template set<std::chrono::milliseconds>("bt_loop_duration", bt_loop_duration_); // NOLINT
blackboard_->template set<std::chrono::milliseconds>(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -164,7 +164,7 @@ class RouteOperationClient : public RouteOperation
"Route operation service (" + getName() + ") failed to lock node.");
}
auto client =
node->create_client<SrvT>(srv_name, true);
node->template create_client<SrvT>(srv_name, true);
response = client->invoke(req, std::chrono::nanoseconds(500ms));
}
} catch (const std::exception & e) {
Expand Down
39 changes: 31 additions & 8 deletions nav2_route/include/nav2_route/route_server.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<typename GoalT>
Route findRoute(
const std::shared_ptr<const GoalT> 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<typename GoalT>
Route findRoute(
const std::shared_ptr<const GoalT> goal,
ReroutingState rerouting_info = ReroutingState());
ReroutingState & rerouting_info);

/**
* @brief Main processing called by both action server callbacks to centralize
Expand Down
10 changes: 9 additions & 1 deletion nav2_route/src/route_server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -211,10 +211,18 @@ void RouteServer::populateActionResult(
result->execution_duration = execution_duration;
}

template<typename GoalT>
Route RouteServer::findRoute(
const std::shared_ptr<const GoalT> goal)
{
ReroutingState rerouting_info;
return findRoute<GoalT>(goal, rerouting_info);
}

template<typename GoalT>
Route RouteServer::findRoute(
const std::shared_ptr<const GoalT> goal,
ReroutingState rerouting_info)
ReroutingState & rerouting_info)
{
// Find the search boundaries
auto [start_route, end_route] = goal_intent_extractor_->findStartandGoal(goal);
Expand Down
Loading