Implement intermediate planner into nav2_controller (#17)

* Implement intermediate planner into nav2_controller

* Implement tolerance search

* Publish debug spiral markers

* Update validatePath

* Fix transform

* Decrease logging

* Fix smac crash

* Remove worldToMap error print

* Fix transform time

* Lint

* Fix close to goal planning

* Print exception string

* Make intermediate planner always part of controller_server

* Fix fail close to goal

* Make points_per_rotation a param

* Fix path end

nav2_controller/CMakeLists.txt

@@ -14,6 +14,7 @@ find_package(nav2_msgs REQUIRED)
 find_package(nav_2d_utils REQUIRED)
 find_package(nav_2d_msgs REQUIRED)
 find_package(pluginlib REQUIRED)
+find_package(service_robot_msgs REQUIRED)
 
 nav2_package()
 
@@ -31,6 +32,7 @@ set(library_name ${executable_name}_core)
 
 add_library(${library_name} SHARED
   src/controller_server.cpp
+  src/intermediate_planner_server.cpp
 )
 
 set(dependencies
@@ -45,6 +47,7 @@ set(dependencies
   nav2_util
   nav2_core
   pluginlib
+  service_robot_msgs
 )
 
 add_library(simple_progress_checker SHARED plugins/simple_progress_checker.cpp)

nav2_controller/include/nav2_controller/controller_server.hpp

@@ -36,6 +36,8 @@
 #include "pluginlib/class_loader.hpp"
 #include "pluginlib/class_list_macros.hpp"
 
+#include "nav2_controller/intermediate_planner_server.hpp"
+
 namespace nav2_controller
 {
 
@@ -213,6 +215,9 @@ class ControllerServer : public nav2_util::LifecycleNode
   rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
   std::mutex dynamic_params_lock_;
 
+  // Intermediate planner
+  std::shared_ptr<nav2_controller::IntermediatePlannerServer> intermediate_planner_;
+
   // The controller needs a costmap node
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
   std::unique_ptr<nav2_util::NodeThread> costmap_thread_;

nav2_controller/include/nav2_controller/intermediate_planner_server.hpp

@@ -0,0 +1,278 @@
+// Copyright (c) 2019 Intel Corporation
+//
+// 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.
+
+/* Copyright 2023 Enjoy Robotics Zrt - All Rights Reserved
+ *
+ * Unauthorized copying of this file, via any medium is strictly prohibited
+ * Modifications to this file is to be shared with the code owner.
+ * Proprietary and confidential
+ * Owner: Enjoy Robotics Zrt [email protected], 2024
+ */
+
+#ifndef NAV2_CONTROLLER__INTERMEDIATE_PLANNER_SERVER_HPP_
+#define NAV2_CONTROLLER__INTERMEDIATE_PLANNER_SERVER_HPP_
+
+#include <chrono>
+#include <string>
+#include <memory>
+#include <vector>
+#include <unordered_map>
+#include <mutex>
+
+#include "geometry_msgs/msg/point.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "nav2_msgs/msg/costmap.hpp"
+#include "nav2_util/robot_utils.hpp"
+#include "nav2_util/simple_action_server.hpp"
+#include "visualization_msgs/msg/marker.hpp"
+#include "visualization_msgs/msg/marker_array.hpp"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_broadcaster.h"
+#include "tf2_ros/create_timer_ros.h"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "pluginlib/class_loader.hpp"
+#include "pluginlib/class_list_macros.hpp"
+#include "nav2_core/global_planner.hpp"
+#include "nav2_msgs/srv/is_path_valid.hpp"
+#include "nav2_core/planner_exceptions.hpp"
+
+#include "service_robot_msgs/action/compute_local_path.hpp"
+
+namespace nav2_controller
+{
+/**
+ * @class nav2_controller::IntermediateIntermediatePlannerServer
+ * @brief An action server implements the behavior tree's ComputeLocalPath
+ * interface and hosts various plugins of different algorithms to compute plans.
+ */
+class IntermediatePlannerServer
+{
+public:
+  /**
+   * @brief A constructor for nav2_planner::IntermediatePlannerServer
+   * @param options Additional options to control creation of the node.
+   */
+  explicit IntermediatePlannerServer(
+    nav2_util::LifecycleNode::SharedPtr parent,
+    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros);
+
+  /**
+   * @brief A destructor for nav2_planner::IntermediatePlannerServer
+   */
+  ~IntermediatePlannerServer();
+
+  using PlannerMap = std::unordered_map<std::string, nav2_core::GlobalPlanner::Ptr>;
+
+  /**
+   * @brief Method to get plan from the desired plugin
+   * @param start starting pose
+   * @param goal goal request
+   * @return Path
+   */
+  nav_msgs::msg::Path getPlan(
+    const geometry_msgs::msg::PoseStamped & start,
+    const geometry_msgs::msg::PoseStamped & goal,
+    const std::string & planner_id);
+
+  /**
+   * @brief Configure member variables and initializes planner
+   * @param state Reference to LifeCycle node state
+   * @return SUCCESS or FAILURE
+   */
+  nav2_util::CallbackReturn configure();
+  /**
+   * @brief Activate member variables
+   * @param state Reference to LifeCycle node state
+   * @return SUCCESS or FAILURE
+   */
+  nav2_util::CallbackReturn activate();
+  /**
+   * @brief Deactivate member variables
+   * @param state Reference to LifeCycle node state
+   * @return SUCCESS or FAILURE
+   */
+  nav2_util::CallbackReturn deactivate();
+  /**
+   * @brief Reset member variables
+   * @param state Reference to LifeCycle node state
+   * @return SUCCESS or FAILURE
+   */
+  nav2_util::CallbackReturn cleanup();
+
+protected:
+  using ActionToPose = service_robot_msgs::action::ComputeLocalPath;
+  using ActionToPoseGoal = ActionToPose::Goal;
+  using ActionServerToPose = nav2_util::SimpleActionServer<ActionToPose>;
+
+  /**
+   * @brief Check if an action server is valid / active
+   * @param action_server Action server to test
+   * @return SUCCESS or FAILURE
+   */
+  template<typename T>
+  bool isServerInactive(std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server);
+
+  /**
+   * @brief Check if an action server has a cancellation request pending
+   * @param action_server Action server to test
+   * @return SUCCESS or FAILURE
+   */
+  template<typename T>
+  bool isCancelRequested(std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server);
+
+  /**
+   * @brief Wait for costmap to be valid with updated sensor data or repopulate after a
+   * clearing recovery. Blocks until true without timeout.
+   */
+  void waitForCostmap();
+
+  /**
+   * @brief Check if an action server has a preemption request and replaces the goal
+   * with the new preemption goal.
+   * @param action_server Action server to get updated goal if required
+   * @param goal Goal to overwrite
+   */
+  template<typename T>
+  void getPreemptedGoalIfRequested(
+    std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server,
+    typename std::shared_ptr<const typename T::Goal> goal);
+
+  /**
+   * @brief Get the starting pose from costmap or message, if valid
+   * @param action_server Action server to terminate if required
+   * @param goal Goal to find start from
+   * @param start The starting pose to use
+   * @return bool If successful in finding a valid starting pose
+   */
+  template<typename T>
+  bool getStartPose(
+    typename std::shared_ptr<const typename T::Goal> goal,
+    geometry_msgs::msg::PoseStamped & start);
+
+  /**
+   * @brief Transform start and goal poses into the costmap
+   * global frame for path planning plugins to utilize
+   * @param start The starting pose to transform
+   * @param goal Goal pose to transform
+   * @return bool If successful in transforming poses
+   */
+  bool transformPosesToGlobalFrame(
+    geometry_msgs::msg::PoseStamped & curr_start,
+    geometry_msgs::msg::PoseStamped & curr_goal);
+
+  /**
+   * @brief Validate that the path contains a meaningful path
+   * @param action_server Action server to terminate if required
+   * @param goal Goal Current goal
+   * @param path Current path
+   * @param planner_id The planner ID used to generate the path
+   * @return bool If path is valid
+   */
+  template<typename T>
+  bool validatePath(
+    const geometry_msgs::msg::PoseStamped & curr_goal,
+    const nav_msgs::msg::Path & path,
+    const std::string & planner_id);
+
+  /**
+   * @brief The action server callback which calls planner to get the path
+   * ComputePathToPose
+   */
+  void computePlan();
+
+  /**
+   * @brief The service callback to determine if the path is still valid
+   * @param request to the service
+   * @param response from the service
+   */
+  void isPathValid(
+    const std::shared_ptr<nav2_msgs::srv::IsPathValid::Request> request,
+    std::shared_ptr<nav2_msgs::srv::IsPathValid::Response> response);
+
+  /**
+   * @brief Publish a path for visualization purposes
+   * @param path Reference to Global Path
+   */
+  void publishPlan(const nav_msgs::msg::Path & path);
+
+  void exceptionWarning(
+    const geometry_msgs::msg::PoseStamped & start,
+    const geometry_msgs::msg::PoseStamped & goal,
+    const std::string & planner_id,
+    const std::exception & ex);
+
+  /**
+   * @brief Callback executed when a parameter change is detected
+   * @param event ParameterEvent message
+   */
+  rcl_interfaces::msg::SetParametersResult
+  dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
+
+  // Parent node (didn't make this its own node because otherwise
+  // it behaved differently composed vs standalone: if composed,
+  // it would be a part of the controller_server node, but if standalone,
+  // it would be its own node)
+  nav2_util::LifecycleNode::SharedPtr node_;
+  rclcpp::Logger logger_{rclcpp::get_logger("intermediate_planner_server")};
+
+  // Our action server implements the ComputePathToPose action
+  std::unique_ptr<ActionServerToPose> action_server_pose_;
+
+  // Dynamic parameters handler
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
+  std::mutex dynamic_params_lock_;
+
+  // Planner
+  PlannerMap planners_;
+  pluginlib::ClassLoader<nav2_core::GlobalPlanner> gp_loader_;
+  std::vector<std::string> default_ids_;
+  std::vector<std::string> default_types_;
+  std::vector<std::string> planner_ids_;
+  std::vector<std::string> planner_types_;
+  double max_planner_duration_;
+  std::string planner_ids_concat_;
+
+  // Clock
+  rclcpp::Clock steady_clock_{RCL_STEADY_TIME};
+
+  // TF
+  std::shared_ptr<tf2_ros::Buffer> tf_;
+  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+
+  // Global Costmap
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
+  nav2_costmap_2d::Costmap2D * costmap_;
+
+  // Publishers for the path
+  rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr plan_publisher_;
+  rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseStamped>::SharedPtr
+    intermediate_goal_publisher_;
+  rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr
+    spiral_markers_pub_;
+  bool publish_spiral_markers_;
+
+  // Tolerance parameters
+  double tolerance_;
+  int n_points_near_goal_;
+  int points_per_rotation_;
+
+  // Service to determine if the path is valid
+  rclcpp::Service<nav2_msgs::srv::IsPathValid>::SharedPtr is_path_valid_service_;
+};
+
+}  // namespace nav2_controller
+
+#endif  // NAV2_CONTROLLER__INTERMEDIATE_PLANNER_SERVER_HPP_

nav2_controller/package.xml

@@ -19,6 +19,7 @@
   <depend>nav_2d_msgs</depend>
   <depend>nav2_core</depend>
   <depend>pluginlib</depend>
+  <depend>service_robot_msgs</depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>

nav2_controller/src/controller_server.cpp

@@ -27,6 +27,8 @@
 #include "nav2_util/geometry_utils.hpp"
 #include "nav2_controller/controller_server.hpp"
 
+#include "nav2_controller/intermediate_planner_server.hpp"
+
 using namespace std::chrono_literals;
 using rcl_interfaces::msg::ParameterType;
 using std::placeholders::_1;
@@ -77,9 +79,13 @@ ControllerServer::~ControllerServer()
 nav2_util::CallbackReturn
 ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
 {
+  RCLCPP_INFO(get_logger(), "Configuring controller interface");
+
   auto node = shared_from_this();
 
-  RCLCPP_INFO(get_logger(), "Configuring controller interface");
+  // Setup intermediate planner
+  intermediate_planner_ = std::make_shared<IntermediatePlannerServer>(
+    node, costmap_ros_);
 
   get_parameter("progress_checker_plugin", progress_checker_id_);
   if (progress_checker_id_ == default_progress_checker_id_) {
@@ -124,6 +130,8 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
   // Launch a thread to run the costmap node
   costmap_thread_ = std::make_unique<nav2_util::NodeThread>(costmap_ros_);
 
+  intermediate_planner_->configure();
+
   try {
     progress_checker_type_ = nav2_util::get_plugin_type_param(node, progress_checker_id_);
     progress_checker_ = progress_checker_loader_.createUniqueInstance(progress_checker_type_);
@@ -225,6 +233,8 @@ ControllerServer::on_activate(const rclcpp_lifecycle::State & /*state*/)
   vel_publisher_->on_activate();
   action_server_->activate();
 
+  intermediate_planner_->activate();
+
   auto node = shared_from_this();
   // Add callback for dynamic parameters
   dyn_params_handler_ = node->add_on_set_parameters_callback(
@@ -260,6 +270,8 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
     costmap_ros_->deactivate();
   }
 
+  intermediate_planner_->deactivate();
+
   publishZeroVelocity();
   vel_publisher_->on_deactivate();
   dyn_params_handler_.reset();
@@ -290,6 +302,8 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
     costmap_ros_->cleanup();
   }
 
+  intermediate_planner_->cleanup();
+
   // Release any allocated resources
   action_server_.reset();
   odom_sub_.reset();