route_server.hpp

// 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.
 
#ifndef NAV2_ROUTE__ROUTE_SERVER_HPP_
#define NAV2_ROUTE__ROUTE_SERVER_HPP_
 
#include <chrono>
#include <string>
#include <memory>
#include <vector>
#include <unordered_map>
#include <mutex>
 
#include "nav2_ros_common/lifecycle_node.hpp"
#include "nav2_ros_common/tf2_factories.hpp"
#include "nav2_ros_common/simple_action_server.hpp"
#include "nav2_ros_common/node_utils.hpp"
#include "nav2_util/robot_utils.hpp"
#include "nav2_ros_common/service_server.hpp"
#include "nav2_msgs/action/compute_route.hpp"
#include "nav2_msgs/action/compute_and_track_route.hpp"
#include "nav2_msgs/msg/route.hpp"
#include "nav2_msgs/msg/route_node.hpp"
#include "nav2_msgs/srv/set_route_graph.hpp"
#include "nav2_core/route_exceptions.hpp"
#include "visualization_msgs/msg/marker_array.hpp"
 
#include "nav2_route/types.hpp"
#include "nav2_route/utils.hpp"
#include "nav2_route/graph_loader.hpp"
#include "nav2_route/route_planner.hpp"
#include "nav2_route/path_converter.hpp"
#include "nav2_route/route_tracker.hpp"
#include "nav2_route/goal_intent_extractor.hpp"
 
namespace nav2_route
{
class RouteServer : public nav2::LifecycleNode
{
public:
  using ComputeRoute = nav2_msgs::action::ComputeRoute;
  using ComputeRouteGoal = ComputeRoute::Goal;
  using ComputeRouteResult = ComputeRoute::Result;
  using ComputeRouteServer = nav2::SimpleActionServer<ComputeRoute>;
 
  using ComputeAndTrackRoute = nav2_msgs::action::ComputeAndTrackRoute;
  using ComputeAndTrackRouteGoal = ComputeAndTrackRoute::Goal;
  using ComputeAndTrackRouteFeedback = ComputeAndTrackRoute::Feedback;
  using ComputeAndTrackRouteResult = ComputeAndTrackRoute::Result;
  using ComputeAndTrackRouteServer = nav2::SimpleActionServer<ComputeAndTrackRoute>;
 
  explicit RouteServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
  ~RouteServer() = default;
 
protected:
  nav2::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
 
  nav2::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
 
  nav2::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
 
  nav2::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
 
  nav2::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
 
  void computeRoute();
  void computeAndTrackRoute();
 
  template<typename GoalT>
  Route findRoute(
    const std::shared_ptr<const GoalT> goal,
    ReroutingState & rerouting_info);
 
  template<typename ActionT>
  void processRouteRequest(
    typename nav2::SimpleActionServer<ActionT>::SharedPtr & action_server);
 
  rclcpp::Duration findPlanningDuration(const rclcpp::Time & start_time);
 
  template<typename ActionT>
  bool isRequestValid(typename nav2::SimpleActionServer<ActionT>::SharedPtr & action_server);
 
  void populateActionResult(
    std::shared_ptr<ComputeRoute::Result> result,
    const Route & route,
    const nav_msgs::msg::Path & path,
    const rclcpp::Duration & planning_duration);
 
  void populateActionResult(
    std::shared_ptr<ComputeAndTrackRoute::Result>/*result*/,
    const Route & /*route*/,
    const nav_msgs::msg::Path & /*path*/,
    const rclcpp::Duration & /*planning_duration*/);
 
  void setRouteGraph(
    const std::shared_ptr<rmw_request_id_t>/*request_header*/,
    const std::shared_ptr<nav2_msgs::srv::SetRouteGraph::Request> request,
    std::shared_ptr<nav2_msgs::srv::SetRouteGraph::Response> response);
 
  void publishRoute(const Route & route);
 
  template<typename GoalT>
  void exceptionWarning(const std::shared_ptr<const GoalT> goal, const std::exception & ex);
 
  typename ComputeRouteServer::SharedPtr compute_route_server_;
  typename ComputeAndTrackRouteServer::SharedPtr compute_and_track_route_server_;
 
  // TF
  nav2::TransformBuffer::SharedPtr tf_;
  nav2::TransformListener::SharedPtr transform_listener_;
 
  // Publish the route graph for visualization
  nav2::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr
    graph_vis_publisher_;
 
  // Route publisher
  nav2::Publisher<nav2_msgs::msg::Route>::SharedPtr route_publisher_;
 
  // Set or modify graph
  nav2::ServiceServer<nav2_msgs::srv::SetRouteGraph>::SharedPtr set_graph_service_;
 
  // Internal tools
  std::shared_ptr<GraphLoader> graph_loader_;
  std::shared_ptr<RoutePlanner> route_planner_;
  std::shared_ptr<RouteTracker> route_tracker_;
  std::shared_ptr<PathConverter> path_converter_;
  std::shared_ptr<GoalIntentExtractor> goal_intent_extractor_;
 
  std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_subscriber_;
 
  // State Data
  Graph graph_;
  GraphToIDMap id_to_graph_map_;
  std::string route_frame_, base_frame_;
  double max_planning_time_;
};
 
}  // namespace nav2_route
 
#endif  // NAV2_ROUTE__ROUTE_SERVER_HPP_