Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
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
Original file line number Diff line number Diff line change
Expand Up @@ -176,10 +176,10 @@ bool BtActionServer<ActionT, NodeT>::on_configure()
blackboard_ = BT::Blackboard::create();

// Put items on the blackboard
blackboard_->set<nav2::LifecycleNode::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>(
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>(
"wait_for_service_timeout",
wait_for_service_timeout_);

Expand Down Expand Up @@ -328,11 +328,13 @@ bool BtActionServer<ActionT, NodeT>::loadBehaviorTree(const std::string & bt_xml

for (auto & subtree : tree_.subtrees) {
auto & blackboard = subtree->blackboard;
blackboard->set("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_);
blackboard->template set("node", client_node_);
blackboard->template set<std::chrono::milliseconds>("server_timeout",
default_server_timeout_);
blackboard->template set<std::chrono::milliseconds>("bt_loop_duration", bt_loop_duration_);
blackboard->template set<std::chrono::milliseconds>(
"wait_for_service_timeout",
wait_for_service_timeout_);
}
} catch (const std::exception & e) {
setInternalError(
Expand Down Expand Up @@ -498,9 +500,9 @@ void BtActionServer<ActionT, NodeT>::cleanErrorCodes()
std::string name;
for (const auto & error_code_name_prefix : error_code_name_prefixes_) {
name = error_code_name_prefix + "_error_code";
blackboard_->set<unsigned short>(name, 0); //NOLINT
blackboard_->template set<unsigned short>(name, 0); //NOLINT
name = error_code_name_prefix + "_error_msg";
blackboard_->set<std::string>(name, "");
blackboard_->template set<std::string>(name, "");
}
resetInternalError();
}
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
8 changes: 8 additions & 0 deletions nav2_route/src/route_server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -211,6 +211,14 @@ 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,
Expand Down
Loading