Skip to content
Open
Show file tree
Hide file tree
Changes from 2 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<rclcpp::Node::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
2 changes: 1 addition & 1 deletion nav2_route/include/nav2_route/route_server.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -130,7 +130,7 @@ class RouteServer : public nav2::LifecycleNode
template<typename GoalT>
Route findRoute(
const std::shared_ptr<const GoalT> goal,
ReroutingState & rerouting_info = ReroutingState());
ReroutingState rerouting_info = ReroutingState());

/**
* @brief Main processing called by both action server callbacks to centralize
Expand Down
2 changes: 1 addition & 1 deletion nav2_route/src/route_server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -214,7 +214,7 @@ void RouteServer::populateActionResult(
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