From acc952010e757ba240cadbb93629fa9fd1ff88d4 Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Thu, 7 Aug 2025 18:10:59 +0200 Subject: [PATCH 1/2] Construct TF listeners passing nodes, spinning on separate thread (#5406) * Construct TF listeners passing nodes, spinning on separate thread Signed-off-by: Patrick Roncagliolo * (tentative) pin down of the impacting change Signed-off-by: Patrick Roncagliolo --------- Signed-off-by: Patrick Roncagliolo (cherry picked from commit 1468484f1f96fe0f06241819315c8371311d0f52) # Conflicts: # nav2_route/src/route_server.cpp --- nav2_amcl/src/amcl_node.cpp | 2 +- nav2_behaviors/src/behavior_server.cpp | 2 +- nav2_bt_navigator/src/bt_navigator.cpp | 2 +- .../src/collision_detector_node.cpp | 2 +- .../src/collision_monitor_node.cpp | 2 +- .../opennav_docking/src/docking_server.cpp | 2 +- nav2_route/src/route_server.cpp | 406 ++++++++++++++++++ nav2_smoother/src/nav2_smoother.cpp | 2 +- 8 files changed, 413 insertions(+), 7 deletions(-) create mode 100644 nav2_route/src/route_server.cpp diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 88e28e2903b..9775a769d81 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1512,7 +1512,7 @@ AmclNode::initTransforms() get_node_timers_interface(), callback_group_); tf_buffer_->setCreateTimerInterface(timer_interface); - tf_listener_ = std::make_shared(*tf_buffer_); + tf_listener_ = std::make_shared(*tf_buffer_, this, true); tf_broadcaster_ = std::make_shared(shared_from_this()); sent_first_transform_ = false; diff --git a/nav2_behaviors/src/behavior_server.cpp b/nav2_behaviors/src/behavior_server.cpp index a04aea40315..69e99344c0f 100644 --- a/nav2_behaviors/src/behavior_server.cpp +++ b/nav2_behaviors/src/behavior_server.cpp @@ -87,7 +87,7 @@ BehaviorServer::on_configure(const rclcpp_lifecycle::State & state) get_node_base_interface(), get_node_timers_interface()); tf_->setCreateTimerInterface(timer_interface); - transform_listener_ = std::make_shared(*tf_); + transform_listener_ = std::make_shared(*tf_, this, true); behavior_types_.resize(behavior_ids_.size()); if (!loadBehaviorPlugins()) { diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index c49fae0e2b9..3a97da516d0 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -65,7 +65,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & state) get_node_base_interface(), get_node_timers_interface()); tf_->setCreateTimerInterface(timer_interface); tf_->setUsingDedicatedThread(true); - tf_listener_ = std::make_shared(*tf_, this, false); + tf_listener_ = std::make_shared(*tf_, this, true); global_frame_ = get_parameter("global_frame").as_string(); robot_frame_ = get_parameter("robot_base_frame").as_string(); diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index 7f87bc1bb92..8150671e165 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -50,7 +50,7 @@ CollisionDetector::on_configure(const rclcpp_lifecycle::State & state) this->get_node_base_interface(), this->get_node_timers_interface()); tf_buffer_->setCreateTimerInterface(timer_interface); - tf_listener_ = std::make_shared(*tf_buffer_); + tf_listener_ = std::make_shared(*tf_buffer_, this, true); state_pub_ = this->create_publisher( "collision_detector_state", rclcpp::SystemDefaultsQoS()); diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 2b0991c9941..7e94bc9790d 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -52,7 +52,7 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & state) this->get_node_base_interface(), this->get_node_timers_interface()); tf_buffer_->setCreateTimerInterface(timer_interface); - tf_listener_ = std::make_shared(*tf_buffer_); + tf_listener_ = std::make_shared(*tf_buffer_, this, true); std::string cmd_vel_in_topic; std::string cmd_vel_out_topic; diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index 2aab39cb9b1..232a38dfa38 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -104,7 +104,7 @@ DockingServer::on_activate(const rclcpp_lifecycle::State & /*state*/) auto node = shared_from_this(); - tf2_listener_ = std::make_unique(*tf2_buffer_); + tf2_listener_ = std::make_unique(*tf2_buffer_, this, true); dock_db_->activate(); navigator_->activate(); vel_publisher_->on_activate(); diff --git a/nav2_route/src/route_server.cpp b/nav2_route/src/route_server.cpp new file mode 100644 index 00000000000..fee6bf7b671 --- /dev/null +++ b/nav2_route/src/route_server.cpp @@ -0,0 +1,406 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include "nav2_route/route_server.hpp" + +using nav2::declare_parameter_if_not_declared; +using std::placeholders::_1; +using std::placeholders::_2; + +namespace nav2_route +{ + +RouteServer::RouteServer(const rclcpp::NodeOptions & options) +: nav2::LifecycleNode("route_server", "", options) +{} + +nav2::CallbackReturn +RouteServer::on_configure(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Configuring"); + + tf_ = std::make_shared(get_clock()); + auto timer_interface = std::make_shared( + get_node_base_interface(), + get_node_timers_interface()); + tf_->setCreateTimerInterface(timer_interface); + transform_listener_ = std::make_shared(*tf_, this, true); + + auto node = shared_from_this(); + graph_vis_publisher_ = + node->create_publisher( + "route_graph", nav2::qos::LatchedPublisherQoS()); + + compute_route_server_ = create_action_server( + "compute_route", + std::bind(&RouteServer::computeRoute, this), + nullptr, std::chrono::milliseconds(500), true); + + compute_and_track_route_server_ = create_action_server( + "compute_and_track_route", + std::bind(&RouteServer::computeAndTrackRoute, this), + nullptr, std::chrono::milliseconds(500), true); + + set_graph_service_ = node->create_service( + std::string(node->get_name()) + "/set_route_graph", + std::bind( + &RouteServer::setRouteGraph, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + declare_parameter_if_not_declared( + node, "route_frame", rclcpp::ParameterValue(std::string("map"))); + declare_parameter_if_not_declared( + node, "base_frame", rclcpp::ParameterValue(std::string("base_link"))); + declare_parameter_if_not_declared( + node, "max_planning_time", rclcpp::ParameterValue(2.0)); + + route_frame_ = node->get_parameter("route_frame").as_string(); + base_frame_ = node->get_parameter("base_frame").as_string(); + max_planning_time_ = node->get_parameter("max_planning_time").as_double(); + + // Create costmap subscriber + nav2::declare_parameter_if_not_declared( + node, "costmap_topic", + rclcpp::ParameterValue(std::string("global_costmap/costmap_raw"))); + std::string costmap_topic = node->get_parameter("costmap_topic").as_string(); + costmap_subscriber_ = std::make_shared(node, costmap_topic); + + try { + graph_loader_ = std::make_shared(node, tf_, route_frame_); + if (!graph_loader_->loadGraphFromParameter(graph_, id_to_graph_map_)) { + return nav2::CallbackReturn::FAILURE; + } + + goal_intent_extractor_ = std::make_shared(); + goal_intent_extractor_->configure( + node, graph_, &id_to_graph_map_, tf_, costmap_subscriber_, route_frame_, base_frame_); + + route_planner_ = std::make_shared(); + route_planner_->configure(node, tf_, costmap_subscriber_); + + route_tracker_ = std::make_shared(); + route_tracker_->configure( + node, tf_, costmap_subscriber_, compute_and_track_route_server_, route_frame_, base_frame_); + + path_converter_ = std::make_shared(); + path_converter_->configure(node); + } catch (std::exception & e) { + RCLCPP_FATAL(get_logger(), "Failed to configure route server: %s", e.what()); + return nav2::CallbackReturn::FAILURE; + } + + return nav2::CallbackReturn::SUCCESS; +} + +nav2::CallbackReturn +RouteServer::on_activate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Activating"); + compute_route_server_->activate(); + compute_and_track_route_server_->activate(); + graph_vis_publisher_->on_activate(); + graph_vis_publisher_->publish(utils::toMsg(graph_, route_frame_, this->now())); + createBond(); + return nav2::CallbackReturn::SUCCESS; +} + +nav2::CallbackReturn +RouteServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Deactivating"); + compute_route_server_->deactivate(); + compute_and_track_route_server_->deactivate(); + graph_vis_publisher_->on_deactivate(); + destroyBond(); + return nav2::CallbackReturn::SUCCESS; +} + +nav2::CallbackReturn +RouteServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Cleaning up"); + compute_route_server_.reset(); + compute_and_track_route_server_.reset(); + set_graph_service_.reset(); + graph_loader_.reset(); + route_planner_.reset(); + route_tracker_.reset(); + path_converter_.reset(); + goal_intent_extractor_.reset(); + graph_vis_publisher_.reset(); + transform_listener_.reset(); + tf_.reset(); + graph_.clear(); + return nav2::CallbackReturn::SUCCESS; +} + +nav2::CallbackReturn +RouteServer::on_shutdown(const rclcpp_lifecycle::State &) +{ + RCLCPP_INFO(get_logger(), "Shutting down"); + return nav2::CallbackReturn::SUCCESS; +} + +rclcpp::Duration +RouteServer::findPlanningDuration(const rclcpp::Time & start_time) +{ + auto cycle_duration = this->now() - start_time; + if (max_planning_time_ > 0.0 && cycle_duration.seconds() > max_planning_time_) { + RCLCPP_WARN( + get_logger(), + "Route planner missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz", + 1 / max_planning_time_, 1 / cycle_duration.seconds()); + } + + return cycle_duration; +} + +template +bool +RouteServer::isRequestValid( + typename nav2::SimpleActionServer::SharedPtr & action_server) +{ + if (!action_server || !action_server->is_server_active()) { + RCLCPP_DEBUG(get_logger(), "Action server unavailable or inactive. Stopping."); + return false; + } + + if (action_server->is_cancel_requested()) { + RCLCPP_INFO(get_logger(), "Goal was canceled. Canceling route planning action."); + action_server->terminate_all(); + return false; + } + + if (graph_.empty()) { + RCLCPP_INFO(get_logger(), "No graph set! Aborting request."); + action_server->terminate_current(); + return false; + } + + return true; +} + +void RouteServer::populateActionResult( + std::shared_ptr result, + const Route & route, + const nav_msgs::msg::Path & path, + const rclcpp::Duration & planning_duration) +{ + result->route = utils::toMsg(route, route_frame_, this->now()); + result->path = path; + result->planning_time = planning_duration; +} + +void RouteServer::populateActionResult( + std::shared_ptr result, + const Route &, + const nav_msgs::msg::Path &, + const rclcpp::Duration & execution_duration) +{ + result->execution_duration = execution_duration; +} + +template +Route RouteServer::findRoute( + const std::shared_ptr goal, + ReroutingState & rerouting_info) +{ + // Find the search boundaries + auto [start_route, end_route] = goal_intent_extractor_->findStartandGoal(goal); + + // If we're rerouting, use the rerouting start node and pose as the new start + if (rerouting_info.rerouting_start_id != std::numeric_limits::max()) { + start_route = id_to_graph_map_.at(rerouting_info.rerouting_start_id); + goal_intent_extractor_->overrideStart(rerouting_info.rerouting_start_pose); + } + + Route route; + if (start_route == end_route) { + // Succeed with a single-point route + route.route_cost = 0.0; + route.start_node = &graph_.at(start_route); + } else { + // Populate request data (start & goal id, start & goal pose, if set) for routing + RouteRequest route_request; + route_request.start_nodeid = start_route; + route_request.goal_nodeid = end_route; + route_request.start_pose = goal_intent_extractor_->getStart(); + route_request.goal_pose = goal->goal; + route_request.use_poses = goal->use_poses; + + // Compute the route via graph-search, returns a node-edge sequence + route = route_planner_->findRoute( + graph_, start_route, end_route, rerouting_info.blocked_ids, route_request); + } + + return goal_intent_extractor_->pruneStartandGoal(route, goal, rerouting_info); +} + +template +void +RouteServer::processRouteRequest( + typename nav2::SimpleActionServer::SharedPtr & action_server) +{ + auto goal = action_server->get_current_goal(); + auto result = std::make_shared(); + ReroutingState rerouting_info; + auto start_time = this->now(); + + try { + while (rclcpp::ok()) { + if (!isRequestValid(action_server)) { + return; + } + + if (action_server->is_preempt_requested()) { + RCLCPP_INFO(get_logger(), "Computing new preempted route to goal."); + goal = action_server->accept_pending_goal(); + rerouting_info.reset(); + } + + // Find the route + Route route = findRoute(goal, rerouting_info); + RCLCPP_INFO(get_logger(), "Route found with %zu nodes and %zu edges", + route.edges.size() + 1u, route.edges.size()); + auto path = path_converter_->densify(route, rerouting_info, route_frame_, this->now()); + + if (std::is_same::value) { + // blocks until re-route requested or task completion, publishes feedback + switch (route_tracker_->trackRoute(route, path, rerouting_info)) { + case TrackerResult::COMPLETED: + populateActionResult(result, route, path, this->now() - start_time); + action_server->succeeded_current(result); + return; + case TrackerResult::INTERRUPTED: + // Reroute, cancel, or preempt requested + break; + case TrackerResult::EXITED: + // rclcpp::ok() is false, so just return + return; + } + } else { + // Return route if not tracking + populateActionResult(result, route, path, findPlanningDuration(start_time)); + action_server->succeeded_current(result); + return; + } + } + } catch (nav2_core::NoValidRouteCouldBeFound & ex) { + exceptionWarning(goal, ex); + result->error_code = ActionT::Result::NO_VALID_ROUTE; + result->error_msg = ex.what(); + action_server->terminate_current(result); + } catch (nav2_core::TimedOut & ex) { + exceptionWarning(goal, ex); + result->error_code = ActionT::Result::TIMEOUT; + result->error_msg = ex.what(); + action_server->terminate_current(result); + } catch (nav2_core::RouteTFError & ex) { + exceptionWarning(goal, ex); + result->error_code = ActionT::Result::TF_ERROR; + result->error_msg = ex.what(); + action_server->terminate_current(result); + } catch (nav2_core::NoValidGraph & ex) { + exceptionWarning(goal, ex); + result->error_code = ActionT::Result::NO_VALID_GRAPH; + result->error_msg = ex.what(); + action_server->terminate_current(result); + } catch (nav2_core::IndeterminantNodesOnGraph & ex) { + exceptionWarning(goal, ex); + result->error_code = ActionT::Result::INDETERMINANT_NODES_ON_GRAPH; + result->error_msg = ex.what(); + action_server->terminate_current(result); + } catch (nav2_core::InvalidEdgeScorerUse & ex) { + exceptionWarning(goal, ex); + result->error_code = ActionT::Result::INVALID_EDGE_SCORER_USE; + result->error_msg = ex.what(); + action_server->terminate_current(result); + } catch (nav2_core::OperationFailed & ex) { + // A special case since Operation Failed is only in Compute & Track + // actions, specifying it to allow otherwise fully shared code + exceptionWarning(goal, ex); + result->error_code = ComputeAndTrackRoute::Result::OPERATION_FAILED; + result->error_msg = ex.what(); + action_server->terminate_current(result); + } catch (nav2_core::RouteException & ex) { + exceptionWarning(goal, ex); + result->error_code = ActionT::Result::UNKNOWN; + result->error_msg = ex.what(); + action_server->terminate_current(result); + } catch (std::exception & ex) { + exceptionWarning(goal, ex); + result->error_code = ActionT::Result::UNKNOWN; + result->error_msg = ex.what(); + action_server->terminate_current(result); + } +} + +void +RouteServer::computeRoute() +{ + RCLCPP_INFO(get_logger(), "Computing route to goal."); + processRouteRequest(compute_route_server_); +} + +void +RouteServer::computeAndTrackRoute() +{ + RCLCPP_INFO(get_logger(), "Computing and tracking route to goal."); + processRouteRequest(compute_and_track_route_server_); +} + +void RouteServer::setRouteGraph( + const std::shared_ptr/*request_header*/, + const std::shared_ptr request, + std::shared_ptr response) +{ + RCLCPP_INFO(get_logger(), "Setting new route graph: %s.", request->graph_filepath.c_str()); + graph_.clear(); + id_to_graph_map_.clear(); + try { + if (graph_loader_->loadGraphFromFile(graph_, id_to_graph_map_, request->graph_filepath)) { + goal_intent_extractor_->setGraph(graph_, &id_to_graph_map_); + graph_vis_publisher_->publish(utils::toMsg(graph_, route_frame_, this->now())); + response->success = true; + return; + } + } catch (std::exception & ex) { + } + + RCLCPP_WARN( + get_logger(), + "Failed to set new route graph: %s!", request->graph_filepath.c_str()); + response->success = false; +} + +template +void RouteServer::exceptionWarning( + const std::shared_ptr goal, + const std::exception & ex) +{ + RCLCPP_WARN( + get_logger(), + "Route server failed on request: Start: [(%0.2f, %0.2f) / %i] Goal: [(%0.2f, %0.2f) / %i]:" + " \"%s\"", goal->start.pose.position.x, goal->start.pose.position.y, goal->start_id, + goal->goal.pose.position.x, goal->goal.pose.position.y, goal->goal_id, ex.what()); +} + +} // namespace nav2_route + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(nav2_route::RouteServer) diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp index fe462cb1c6b..b2a2653f99d 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -82,7 +82,7 @@ SmootherServer::on_configure(const rclcpp_lifecycle::State & state) auto timer_interface = std::make_shared( get_node_base_interface(), get_node_timers_interface()); tf_->setCreateTimerInterface(timer_interface); - transform_listener_ = std::make_shared(*tf_); + transform_listener_ = std::make_shared(*tf_, this, true); std::string costmap_topic, footprint_topic, robot_base_frame; double transform_tolerance; From 681406291a5e5af9f389f10452166b63f7f5b86d Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 8 Aug 2025 09:34:19 -0700 Subject: [PATCH 2/2] Delete nav2_route/src/route_server.cpp Signed-off-by: Steve Macenski --- nav2_route/src/route_server.cpp | 406 -------------------------------- 1 file changed, 406 deletions(-) delete mode 100644 nav2_route/src/route_server.cpp diff --git a/nav2_route/src/route_server.cpp b/nav2_route/src/route_server.cpp deleted file mode 100644 index fee6bf7b671..00000000000 --- a/nav2_route/src/route_server.cpp +++ /dev/null @@ -1,406 +0,0 @@ -// Copyright (c) 2025, Open Navigation LLC -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. Reserved. - -#include "nav2_route/route_server.hpp" - -using nav2::declare_parameter_if_not_declared; -using std::placeholders::_1; -using std::placeholders::_2; - -namespace nav2_route -{ - -RouteServer::RouteServer(const rclcpp::NodeOptions & options) -: nav2::LifecycleNode("route_server", "", options) -{} - -nav2::CallbackReturn -RouteServer::on_configure(const rclcpp_lifecycle::State & /*state*/) -{ - RCLCPP_INFO(get_logger(), "Configuring"); - - tf_ = std::make_shared(get_clock()); - auto timer_interface = std::make_shared( - get_node_base_interface(), - get_node_timers_interface()); - tf_->setCreateTimerInterface(timer_interface); - transform_listener_ = std::make_shared(*tf_, this, true); - - auto node = shared_from_this(); - graph_vis_publisher_ = - node->create_publisher( - "route_graph", nav2::qos::LatchedPublisherQoS()); - - compute_route_server_ = create_action_server( - "compute_route", - std::bind(&RouteServer::computeRoute, this), - nullptr, std::chrono::milliseconds(500), true); - - compute_and_track_route_server_ = create_action_server( - "compute_and_track_route", - std::bind(&RouteServer::computeAndTrackRoute, this), - nullptr, std::chrono::milliseconds(500), true); - - set_graph_service_ = node->create_service( - std::string(node->get_name()) + "/set_route_graph", - std::bind( - &RouteServer::setRouteGraph, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - - declare_parameter_if_not_declared( - node, "route_frame", rclcpp::ParameterValue(std::string("map"))); - declare_parameter_if_not_declared( - node, "base_frame", rclcpp::ParameterValue(std::string("base_link"))); - declare_parameter_if_not_declared( - node, "max_planning_time", rclcpp::ParameterValue(2.0)); - - route_frame_ = node->get_parameter("route_frame").as_string(); - base_frame_ = node->get_parameter("base_frame").as_string(); - max_planning_time_ = node->get_parameter("max_planning_time").as_double(); - - // Create costmap subscriber - nav2::declare_parameter_if_not_declared( - node, "costmap_topic", - rclcpp::ParameterValue(std::string("global_costmap/costmap_raw"))); - std::string costmap_topic = node->get_parameter("costmap_topic").as_string(); - costmap_subscriber_ = std::make_shared(node, costmap_topic); - - try { - graph_loader_ = std::make_shared(node, tf_, route_frame_); - if (!graph_loader_->loadGraphFromParameter(graph_, id_to_graph_map_)) { - return nav2::CallbackReturn::FAILURE; - } - - goal_intent_extractor_ = std::make_shared(); - goal_intent_extractor_->configure( - node, graph_, &id_to_graph_map_, tf_, costmap_subscriber_, route_frame_, base_frame_); - - route_planner_ = std::make_shared(); - route_planner_->configure(node, tf_, costmap_subscriber_); - - route_tracker_ = std::make_shared(); - route_tracker_->configure( - node, tf_, costmap_subscriber_, compute_and_track_route_server_, route_frame_, base_frame_); - - path_converter_ = std::make_shared(); - path_converter_->configure(node); - } catch (std::exception & e) { - RCLCPP_FATAL(get_logger(), "Failed to configure route server: %s", e.what()); - return nav2::CallbackReturn::FAILURE; - } - - return nav2::CallbackReturn::SUCCESS; -} - -nav2::CallbackReturn -RouteServer::on_activate(const rclcpp_lifecycle::State & /*state*/) -{ - RCLCPP_INFO(get_logger(), "Activating"); - compute_route_server_->activate(); - compute_and_track_route_server_->activate(); - graph_vis_publisher_->on_activate(); - graph_vis_publisher_->publish(utils::toMsg(graph_, route_frame_, this->now())); - createBond(); - return nav2::CallbackReturn::SUCCESS; -} - -nav2::CallbackReturn -RouteServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) -{ - RCLCPP_INFO(get_logger(), "Deactivating"); - compute_route_server_->deactivate(); - compute_and_track_route_server_->deactivate(); - graph_vis_publisher_->on_deactivate(); - destroyBond(); - return nav2::CallbackReturn::SUCCESS; -} - -nav2::CallbackReturn -RouteServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) -{ - RCLCPP_INFO(get_logger(), "Cleaning up"); - compute_route_server_.reset(); - compute_and_track_route_server_.reset(); - set_graph_service_.reset(); - graph_loader_.reset(); - route_planner_.reset(); - route_tracker_.reset(); - path_converter_.reset(); - goal_intent_extractor_.reset(); - graph_vis_publisher_.reset(); - transform_listener_.reset(); - tf_.reset(); - graph_.clear(); - return nav2::CallbackReturn::SUCCESS; -} - -nav2::CallbackReturn -RouteServer::on_shutdown(const rclcpp_lifecycle::State &) -{ - RCLCPP_INFO(get_logger(), "Shutting down"); - return nav2::CallbackReturn::SUCCESS; -} - -rclcpp::Duration -RouteServer::findPlanningDuration(const rclcpp::Time & start_time) -{ - auto cycle_duration = this->now() - start_time; - if (max_planning_time_ > 0.0 && cycle_duration.seconds() > max_planning_time_) { - RCLCPP_WARN( - get_logger(), - "Route planner missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz", - 1 / max_planning_time_, 1 / cycle_duration.seconds()); - } - - return cycle_duration; -} - -template -bool -RouteServer::isRequestValid( - typename nav2::SimpleActionServer::SharedPtr & action_server) -{ - if (!action_server || !action_server->is_server_active()) { - RCLCPP_DEBUG(get_logger(), "Action server unavailable or inactive. Stopping."); - return false; - } - - if (action_server->is_cancel_requested()) { - RCLCPP_INFO(get_logger(), "Goal was canceled. Canceling route planning action."); - action_server->terminate_all(); - return false; - } - - if (graph_.empty()) { - RCLCPP_INFO(get_logger(), "No graph set! Aborting request."); - action_server->terminate_current(); - return false; - } - - return true; -} - -void RouteServer::populateActionResult( - std::shared_ptr result, - const Route & route, - const nav_msgs::msg::Path & path, - const rclcpp::Duration & planning_duration) -{ - result->route = utils::toMsg(route, route_frame_, this->now()); - result->path = path; - result->planning_time = planning_duration; -} - -void RouteServer::populateActionResult( - std::shared_ptr result, - const Route &, - const nav_msgs::msg::Path &, - const rclcpp::Duration & execution_duration) -{ - result->execution_duration = execution_duration; -} - -template -Route RouteServer::findRoute( - const std::shared_ptr goal, - ReroutingState & rerouting_info) -{ - // Find the search boundaries - auto [start_route, end_route] = goal_intent_extractor_->findStartandGoal(goal); - - // If we're rerouting, use the rerouting start node and pose as the new start - if (rerouting_info.rerouting_start_id != std::numeric_limits::max()) { - start_route = id_to_graph_map_.at(rerouting_info.rerouting_start_id); - goal_intent_extractor_->overrideStart(rerouting_info.rerouting_start_pose); - } - - Route route; - if (start_route == end_route) { - // Succeed with a single-point route - route.route_cost = 0.0; - route.start_node = &graph_.at(start_route); - } else { - // Populate request data (start & goal id, start & goal pose, if set) for routing - RouteRequest route_request; - route_request.start_nodeid = start_route; - route_request.goal_nodeid = end_route; - route_request.start_pose = goal_intent_extractor_->getStart(); - route_request.goal_pose = goal->goal; - route_request.use_poses = goal->use_poses; - - // Compute the route via graph-search, returns a node-edge sequence - route = route_planner_->findRoute( - graph_, start_route, end_route, rerouting_info.blocked_ids, route_request); - } - - return goal_intent_extractor_->pruneStartandGoal(route, goal, rerouting_info); -} - -template -void -RouteServer::processRouteRequest( - typename nav2::SimpleActionServer::SharedPtr & action_server) -{ - auto goal = action_server->get_current_goal(); - auto result = std::make_shared(); - ReroutingState rerouting_info; - auto start_time = this->now(); - - try { - while (rclcpp::ok()) { - if (!isRequestValid(action_server)) { - return; - } - - if (action_server->is_preempt_requested()) { - RCLCPP_INFO(get_logger(), "Computing new preempted route to goal."); - goal = action_server->accept_pending_goal(); - rerouting_info.reset(); - } - - // Find the route - Route route = findRoute(goal, rerouting_info); - RCLCPP_INFO(get_logger(), "Route found with %zu nodes and %zu edges", - route.edges.size() + 1u, route.edges.size()); - auto path = path_converter_->densify(route, rerouting_info, route_frame_, this->now()); - - if (std::is_same::value) { - // blocks until re-route requested or task completion, publishes feedback - switch (route_tracker_->trackRoute(route, path, rerouting_info)) { - case TrackerResult::COMPLETED: - populateActionResult(result, route, path, this->now() - start_time); - action_server->succeeded_current(result); - return; - case TrackerResult::INTERRUPTED: - // Reroute, cancel, or preempt requested - break; - case TrackerResult::EXITED: - // rclcpp::ok() is false, so just return - return; - } - } else { - // Return route if not tracking - populateActionResult(result, route, path, findPlanningDuration(start_time)); - action_server->succeeded_current(result); - return; - } - } - } catch (nav2_core::NoValidRouteCouldBeFound & ex) { - exceptionWarning(goal, ex); - result->error_code = ActionT::Result::NO_VALID_ROUTE; - result->error_msg = ex.what(); - action_server->terminate_current(result); - } catch (nav2_core::TimedOut & ex) { - exceptionWarning(goal, ex); - result->error_code = ActionT::Result::TIMEOUT; - result->error_msg = ex.what(); - action_server->terminate_current(result); - } catch (nav2_core::RouteTFError & ex) { - exceptionWarning(goal, ex); - result->error_code = ActionT::Result::TF_ERROR; - result->error_msg = ex.what(); - action_server->terminate_current(result); - } catch (nav2_core::NoValidGraph & ex) { - exceptionWarning(goal, ex); - result->error_code = ActionT::Result::NO_VALID_GRAPH; - result->error_msg = ex.what(); - action_server->terminate_current(result); - } catch (nav2_core::IndeterminantNodesOnGraph & ex) { - exceptionWarning(goal, ex); - result->error_code = ActionT::Result::INDETERMINANT_NODES_ON_GRAPH; - result->error_msg = ex.what(); - action_server->terminate_current(result); - } catch (nav2_core::InvalidEdgeScorerUse & ex) { - exceptionWarning(goal, ex); - result->error_code = ActionT::Result::INVALID_EDGE_SCORER_USE; - result->error_msg = ex.what(); - action_server->terminate_current(result); - } catch (nav2_core::OperationFailed & ex) { - // A special case since Operation Failed is only in Compute & Track - // actions, specifying it to allow otherwise fully shared code - exceptionWarning(goal, ex); - result->error_code = ComputeAndTrackRoute::Result::OPERATION_FAILED; - result->error_msg = ex.what(); - action_server->terminate_current(result); - } catch (nav2_core::RouteException & ex) { - exceptionWarning(goal, ex); - result->error_code = ActionT::Result::UNKNOWN; - result->error_msg = ex.what(); - action_server->terminate_current(result); - } catch (std::exception & ex) { - exceptionWarning(goal, ex); - result->error_code = ActionT::Result::UNKNOWN; - result->error_msg = ex.what(); - action_server->terminate_current(result); - } -} - -void -RouteServer::computeRoute() -{ - RCLCPP_INFO(get_logger(), "Computing route to goal."); - processRouteRequest(compute_route_server_); -} - -void -RouteServer::computeAndTrackRoute() -{ - RCLCPP_INFO(get_logger(), "Computing and tracking route to goal."); - processRouteRequest(compute_and_track_route_server_); -} - -void RouteServer::setRouteGraph( - const std::shared_ptr/*request_header*/, - const std::shared_ptr request, - std::shared_ptr response) -{ - RCLCPP_INFO(get_logger(), "Setting new route graph: %s.", request->graph_filepath.c_str()); - graph_.clear(); - id_to_graph_map_.clear(); - try { - if (graph_loader_->loadGraphFromFile(graph_, id_to_graph_map_, request->graph_filepath)) { - goal_intent_extractor_->setGraph(graph_, &id_to_graph_map_); - graph_vis_publisher_->publish(utils::toMsg(graph_, route_frame_, this->now())); - response->success = true; - return; - } - } catch (std::exception & ex) { - } - - RCLCPP_WARN( - get_logger(), - "Failed to set new route graph: %s!", request->graph_filepath.c_str()); - response->success = false; -} - -template -void RouteServer::exceptionWarning( - const std::shared_ptr goal, - const std::exception & ex) -{ - RCLCPP_WARN( - get_logger(), - "Route server failed on request: Start: [(%0.2f, %0.2f) / %i] Goal: [(%0.2f, %0.2f) / %i]:" - " \"%s\"", goal->start.pose.position.x, goal->start.pose.position.y, goal->start_id, - goal->goal.pose.position.x, goal->goal.pose.position.y, goal->goal_id, ex.what()); -} - -} // namespace nav2_route - -#include "rclcpp_components/register_node_macro.hpp" - -// Register the component with class_loader. -// This acts as a sort of entry point, allowing the component to be discoverable when its library -// is being loaded into a running process. -RCLCPP_COMPONENTS_REGISTER_NODE(nav2_route::RouteServer)