base nav2 planning server

nav2_planner/CMakeLists.txt

+cmake_minimum_required(VERSION 3.5)
+project(nav2_planner)
+
+find_package(ament_cmake REQUIRED)
+find_package(nav2_common REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(visualization_msgs REQUIRED)
+find_package(nav2_util REQUIRED)
+find_package(nav2_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(nav2_costmap_2d REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(nav2_core REQUIRED)
+
+nav2_package()
+
+include_directories(
+  include
+)
+
+set(executable_name nav2_planner)
+set(library_name ${executable_name}_core)
+
+set(dependencies
+  rclcpp
+  rclcpp_action
+  rclcpp_lifecycle
+  std_msgs
+  visualization_msgs
+  nav2_util
+  nav2_msgs
+  nav_msgs
+  geometry_msgs
+  builtin_interfaces
+  tf2_ros
+  nav2_costmap_2d
+  pluginlib
+  nav2_core
+)
+
+add_library(${library_name} SHARED
+  src/planner_server.cpp
+)
+
+ament_target_dependencies(${library_name}
+  ${dependencies}
+)
+
+add_executable(${executable_name}
+  src/main.cpp
+)
+
+target_link_libraries(${executable_name} ${library_name})
+
+ament_target_dependencies(${executable_name}
+  ${dependencies}
+)
+
+install(TARGETS ${executable_name} ${library_name}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_export_include_directories(include)
+ament_export_libraries(${library_name})
+ament_export_dependencies(${dependencies})
+ament_package()

nav2_planner/README.md

+# Nav2 Planner
+
+The Nav2 planner is a [planning module](../doc/requirements/requirements.md) that implements the `nav2_behavior_tree::ComputePathToPose` interface.
+
+A planning module implementing the `nav2_behavior_tree::ComputePathToPose` interface is responsible for generating a feasible path given start and end robot poses. It loads a planner plugin like NavFn to do the path generation.

nav2_planner/include/nav2_planner/planner_server.hpp

+// Copyright (c) 2019 Samsung Research America
+//
+// 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_PLANNER__PLANNER_SERVER_HPP_
+#define NAV2_PLANNER__PLANNER_SERVER_HPP_
+
+#include <chrono>
+#include <string>
+#include <memory>
+#include <vector>
+
+#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/action/compute_path_to_pose.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 "tf2_ros/transform_listener.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"
+
+namespace nav2_planner
+{
+
+class PlannerServer : public nav2_util::LifecycleNode
+{
+public:
+  PlannerServer();
+  ~PlannerServer();
+
+protected:
+  // Implement the lifecycle interface
+  nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
+  nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
+  nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
+  nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
+  nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
+  nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override;
+
+  using ActionServer = nav2_util::SimpleActionServer<nav2_msgs::action::ComputePathToPose>;
+
+  // Our action server implements the ComputePathToPose action
+  std::unique_ptr<ActionServer> action_server_;
+
+  // The action server callback
+  void computePathToPose();
+
+  // Publish a path for visualization purposes
+  void publishPlan(const nav_msgs::msg::Path & path);
+
+  // Planner
+  nav2_core::GlobalPlanner::Ptr planner_;
+  pluginlib::ClassLoader<nav2_core::GlobalPlanner> gp_loader_;
+  std::string planner_plugin_name_;
+
+  // TF buffer
+  std::shared_ptr<tf2_ros::Buffer> tf_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+
+  // Global Costmap
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
+  nav2_costmap_2d::Costmap2D * costmap_;
+  std::unique_ptr<std::thread> costmap_thread_;
+  std::unique_ptr<rclcpp::executors::SingleThreadedExecutor> costmap_executor_;
+
+  // Publishers for the path
+  rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr plan_publisher_;
+};
+
+}  // namespace nav2_planner
+
+#endif  // NAV2_PLANNER__PLANNER_SERVER_HPP_

nav2_planner/package.xml

+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nav2_planner</name>
+  <version>0.2.4</version>
+  <description>TODO</description>
+  <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>visualization_msgs</depend>
+  <depend>nav2_util</depend>
+  <depend>nav2_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>builtin_interfaces</depend>
+  <depend>nav2_common</depend>
+  <depend>tf2_ros</depend>
+  <depend>nav2_costmap_2d</depend>
+  <depend>pluginlib</depend>
+  <depend>nav2_core</depend>
+
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_cmake_pytest</test_depend>
+  <test_depend>launch</test_depend>
+  <test_depend>launch_testing</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

nav2_planner/src/main.cpp

+// Copyright (c) 2019 Samsung Research America
+//
+// 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.
+
+#include <memory>
+
+#include "nav2_planner/planner_server.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<nav2_planner::PlannerServer>();
+  rclcpp::spin(node->get_node_base_interface());
+  rclcpp::shutdown();
+
+  return 0;
+}

nav2_planner/src/planner_server.cpp

+// Copyright (c) 2019 Samsung Research America
+//
+// 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.
+
+#include <chrono>
+#include <cmath>
+#include <iomanip>
+#include <iostream>
+#include <limits>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "builtin_interfaces/msg/duration.hpp"
+#include "nav2_util/costmap.hpp"
+#include "nav2_util/node_utils.hpp"
+#include "nav2_costmap_2d/cost_values.hpp"
+
+#include "nav2_planner/planner_server.hpp"
+
+using namespace std::chrono_literals;
+
+namespace nav2_planner
+{
+
+PlannerServer::PlannerServer()
+: nav2_util::LifecycleNode("nav2_planner", "", true),
+  gp_loader_("nav2_core", "nav2_core::GlobalPlanner"), costmap_(nullptr)
+{
+  RCLCPP_INFO(get_logger(), "Creating");
+
+  // Declare this node's parameters
+  declare_parameter("planner_plugin", "nav2_navfn_planner::NavfnPlanner");
+
+  // Setup the global costmap
+  costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
+    "global_costmap", nav2_util::add_namespaces(std::string{get_namespace()},
+    "global_costmap"));
+  costmap_executor_ = std::make_unique<rclcpp::executors::SingleThreadedExecutor>();
+
+  // Launch a thread to run the costmap node
+  costmap_thread_ = std::make_unique<std::thread>(
+    [&](rclcpp_lifecycle::LifecycleNode::SharedPtr node)
+    {
+      // TODO(mjeronimo): Once Brian pushes his change upstream to rlcpp executors, we'll
+      // be able to provide our own executor to spin(), reducing this to a single line
+      costmap_executor_->add_node(node->get_node_base_interface());
+      costmap_executor_->spin();
+      costmap_executor_->remove_node(node->get_node_base_interface());
+    }, costmap_ros_);
+}
+
+PlannerServer::~PlannerServer()
+{
+  RCLCPP_INFO(get_logger(), "Destroying");
+  costmap_executor_->cancel();
+  costmap_thread_->join();
+  planner_.reset();
+}
+
+nav2_util::CallbackReturn
+PlannerServer::on_configure(const rclcpp_lifecycle::State & state)
+{
+  RCLCPP_INFO(get_logger(), "Configuring");
+
+  costmap_ros_->on_configure(state);
+  costmap_ = costmap_ros_->getCostmap();
+
+  RCLCPP_DEBUG(get_logger(), "Costmap size: %d,%d",
+    costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY());
+
+  tf_ = costmap_ros_->getTfBuffer();
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_);
+
+  get_parameter("planner_plugin", planner_plugin_name_);
+
+  try {
+    planner_ = gp_loader_.createSharedInstance(planner_plugin_name_);
+    RCLCPP_INFO(get_logger(), "Created global planner %s",
+      planner_plugin_name_.c_str());
+    std::string name = gp_loader_.getName(planner_plugin_name_);
+    planner_->configure(shared_from_this().get(), name,
+      tf_.get(), costmap_ros_.get());
+  } catch (const pluginlib::PluginlibException & ex) {
+    RCLCPP_INFO(get_logger(), "Failed to create global planner. Exception: %s",
+      ex.what());
+    exit(-1);
+  }
+
+  // Initialize pubs & subs
+  plan_publisher_ = create_publisher<nav_msgs::msg::Path>("plan", 1);
+
+  // Create the action server that we implement with our navigateToPose method
+  action_server_ = std::make_unique<ActionServer>(rclcpp_node_, "ComputePathToPose",
+      std::bind(&PlannerServer::computePathToPose, this));
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+PlannerServer::on_activate(const rclcpp_lifecycle::State & state)
+{
+  RCLCPP_INFO(get_logger(), "Activating");
+
+  plan_publisher_->on_activate();
+  action_server_->activate();
+  costmap_ros_->on_activate(state);
+  planner_->activate();
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+PlannerServer::on_deactivate(const rclcpp_lifecycle::State & state)
+{
+  RCLCPP_INFO(get_logger(), "Deactivating");
+
+  action_server_->deactivate();
+  plan_publisher_->on_deactivate();
+  costmap_ros_->on_deactivate(state);
+  planner_->deactivate();
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+PlannerServer::on_cleanup(const rclcpp_lifecycle::State & state)
+{
+  RCLCPP_INFO(get_logger(), "Cleaning up");
+
+  action_server_.reset();
+  plan_publisher_.reset();
+  planner_.reset();
+  tf_listener_.reset();
+  tf_.reset();
+  costmap_ros_->on_cleanup(state);
+  costmap_ros_.reset();
+  planner_->cleanup();
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+PlannerServer::on_error(const rclcpp_lifecycle::State &)
+{
+  RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state");
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+PlannerServer::on_shutdown(const rclcpp_lifecycle::State &)
+{
+  RCLCPP_INFO(get_logger(), "Shutting down");
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+void
+PlannerServer::computePathToPose()
+{
+  // Initialize the ComputePathToPose goal and result
+  auto goal = action_server_->get_current_goal();
+  auto result = std::make_shared<nav2_msgs::action::ComputePathToPose::Result>();
+
+  try {
+    if (action_server_ == nullptr) {
+      RCLCPP_DEBUG(get_logger(), "Action server unavailable. Stopping.");
+      return;
+    }
+
+    if (!action_server_->is_server_active()) {
+      RCLCPP_DEBUG(get_logger(), "Action server is inactive. Stopping.");
+      return;
+    }
+
+    if (action_server_->is_cancel_requested()) {
+      RCLCPP_INFO(get_logger(), "Goal was canceled. Canceling planning action.");
+      action_server_->terminate_goals();
+      return;
+    }
+
+    geometry_msgs::msg::PoseStamped start;
+    if (!nav2_util::getCurrentPose(start, *tf_)) {
+      return;
+    }
+
+    if (action_server_->is_preempt_requested()) {
+      RCLCPP_INFO(get_logger(), "Preempting the goal pose.");
+      goal = action_server_->accept_pending_goal();
+    }
+
+    RCLCPP_DEBUG(get_logger(), "Attempting to a find path from (%.2f, %.2f) to "
+      "(%.2f, %.2f).", start.pose.position.x, start.pose.position.y,
+      goal->pose.pose.position.x, goal->pose.pose.position.y);
+
+    result->path = planner_->createPlan(start, goal->pose);
+
+    if (result->path.poses.size() == 0) {
+      RCLCPP_WARN(get_logger(), "Planning algorithm %s failed to generate a valid"
+        " path to (%.2f, %.2f)", planner_plugin_name_.c_str(),
+        goal->pose.pose.position.x, goal->pose.pose.position.y);
+      // TODO(orduno): define behavior if a preemption is available
+      action_server_->terminate_goals();
+      return;
+    }
+
+    RCLCPP_DEBUG(get_logger(),
+      "Found valid path of size %u to (%.2f, %.2f)",
+      result->path.poses.size(), goal->pose.pose.position.x,
+      goal->pose.pose.position.y);
+
+    // Publish the plan for visualization purposes
+    RCLCPP_DEBUG(get_logger(), "Publishing the valid path");
+    publishPlan(result->path);
+
+    action_server_->succeeded_current(result);
+    return;
+  } catch (std::exception & ex) {
+    RCLCPP_WARN(get_logger(), "%s plugin failed to plan calculation to (%.2f, %.2f): \"%s\"",
+      planner_plugin_name_.c_str(), goal->pose.pose.position.x,
+      goal->pose.pose.position.y, ex.what());
+
+    // TODO(orduno): provide information about fail error to parent task,
+    //               for example: couldn't get costmap update
+    action_server_->terminate_goals();
+    return;
+  } catch (...) {
+    RCLCPP_WARN(get_logger(), "Plan calculation failed");
+
+    // TODO(orduno): provide information about the failure to the parent task,
+    //               for example: couldn't get costmap update
+    action_server_->terminate_goals();
+    return;
+  }
+}
+
+void
+PlannerServer::publishPlan(const nav_msgs::msg::Path & path)
+{
+  plan_publisher_->publish(path);
+}
+
+}  // namespace nav2_planner