add humble-navigation2

navigation2/nav2_planner/CMakeLists.txt

@@ -0,0 +1,93 @@
+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(rclcpp_components 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 planner_server)
+set(library_name ${executable_name}_core)
+
+set(dependencies
+  rclcpp
+  rclcpp_action
+  rclcpp_lifecycle
+  rclcpp_components
+  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}
+)
+
+rclcpp_components_register_nodes(${library_name} "nav2_planner::PlannerServer")
+
+install(TARGETS ${library_name}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(TARGETS ${executable_name}
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+  add_subdirectory(test)
+endif()
+
+ament_export_include_directories(include)
+ament_export_libraries(${library_name})
+ament_export_dependencies(${dependencies})
+ament_package()

navigation2/nav2_planner/README.md

@@ -0,0 +1,9 @@
+# Nav2 Planner
+
+The Nav2 planner is a Task Server in Nav2 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 map of potential planner plugins to do the path generation in different user-defined situations.
+
+See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available planner plugins. 
+
+See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-planner-server.html) for additional parameter descriptions and a [tutorial about writing planner plugins](https://navigation.ros.org/plugin_tutorials/docs/writing_new_nav2planner_plugin.html).

navigation2/nav2_planner/include/nav2_planner/planner_server.hpp

@@ -0,0 +1,257 @@
+// 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 <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/action/compute_path_to_pose.hpp"
+#include "nav2_msgs/action/compute_path_through_poses.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"
+#include "nav2_msgs/srv/is_path_valid.hpp"
+
+namespace nav2_planner
+{
+/**
+ * @class nav2_planner::PlannerServer
+ * @brief An action server implements the behavior tree's ComputePathToPose
+ * interface and hosts various plugins of different algorithms to compute plans.
+ */
+class PlannerServer : public nav2_util::LifecycleNode
+{
+public:
+  /**
+   * @brief A constructor for nav2_planner::PlannerServer
+   * @param options Additional options to control creation of the node.
+   */
+  explicit PlannerServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+  /**
+   * @brief A destructor for nav2_planner::PlannerServer
+   */
+  ~PlannerServer();
+
+  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);
+
+protected:
+  /**
+   * @brief Configure member variables and initializes planner
+   * @param state Reference to LifeCycle node state
+   * @return SUCCESS or FAILURE
+   */
+  nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Activate member variables
+   * @param state Reference to LifeCycle node state
+   * @return SUCCESS or FAILURE
+   */
+  nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Deactivate member variables
+   * @param state Reference to LifeCycle node state
+   * @return SUCCESS or FAILURE
+   */
+  nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Reset member variables
+   * @param state Reference to LifeCycle node state
+   * @return SUCCESS or FAILURE
+   */
+  nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Called when in shutdown state
+   * @param state Reference to LifeCycle node state
+   * @return SUCCESS or FAILURE
+   */
+  nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
+
+  using ActionToPose = nav2_msgs::action::ComputePathToPose;
+  using ActionThroughPoses = nav2_msgs::action::ComputePathThroughPoses;
+  using ActionServerToPose = nav2_util::SimpleActionServer<ActionToPose>;
+  using ActionServerThroughPoses = nav2_util::SimpleActionServer<ActionThroughPoses>;
+
+  /**
+   * @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(
+    std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server,
+    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 action_server Action server to terminate if required
+   * @param start The starting pose to transform
+   * @param goal Goal pose to transform
+   * @return bool If successful in transforming poses
+   */
+  template<typename T>
+  bool transformPosesToGlobalFrame(
+    std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server,
+    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(
+    std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server,
+    const geometry_msgs::msg::PoseStamped & curr_goal,
+    const nav_msgs::msg::Path & path,
+    const std::string & planner_id);
+
+  // Our action server implements the ComputePathToPose action
+  std::unique_ptr<ActionServerToPose> action_server_pose_;
+  std::unique_ptr<ActionServerThroughPoses> action_server_poses_;
+
+  /**
+   * @brief The action server callback which calls planner to get the path
+   * ComputePathToPose
+   */
+  void computePlan();
+
+  /**
+   * @brief The action server callback which calls planner to get the path
+   * ComputePathThroughPoses
+   */
+  void computePlanThroughPoses();
+
+  /**
+   * @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);
+
+  /**
+   * @brief Callback executed when a parameter change is detected
+   * @param event ParameterEvent message
+   */
+  rcl_interfaces::msg::SetParametersResult
+  dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
+
+  // 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_;
+
+  // TF buffer
+  std::shared_ptr<tf2_ros::Buffer> tf_;
+
+  // Global Costmap
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
+  std::unique_ptr<nav2_util::NodeThread> costmap_thread_;
+  nav2_costmap_2d::Costmap2D * costmap_;
+
+  // Publishers for the path
+  rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr plan_publisher_;
+
+  // Service to deterime if the path is valid
+  rclcpp::Service<nav2_msgs::srv::IsPathValid>::SharedPtr is_path_valid_service_;
+};
+
+}  // namespace nav2_planner
+
+#endif  // NAV2_PLANNER__PLANNER_SERVER_HPP_

navigation2/nav2_planner/package.xml

@@ -0,0 +1,33 @@
+<?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>1.1.18</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>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

navigation2/nav2_planner/src/main.cpp

@@ -0,0 +1,29 @@
+// Copyright (c) 2018 Intel Corporation
+// 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;
+}

navigation2/nav2_planner/src/planner_server.cpp

@@ -0,0 +1,639 @@
+// Copyright (c) 2018 Intel Corporation
+// 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 <iterator>
+#include <memory>
+#include <string>
+#include <vector>
+#include <utility>
+
+#include "builtin_interfaces/msg/duration.hpp"
+#include "lifecycle_msgs/msg/state.hpp"
+#include "nav2_util/costmap.hpp"
+#include "nav2_util/node_utils.hpp"
+#include "nav2_util/geometry_utils.hpp"
+#include "nav2_costmap_2d/cost_values.hpp"
+
+#include "nav2_planner/planner_server.hpp"
+
+using namespace std::chrono_literals;
+using rcl_interfaces::msg::ParameterType;
+using std::placeholders::_1;
+
+namespace nav2_planner
+{
+
+PlannerServer::PlannerServer(const rclcpp::NodeOptions & options)
+: nav2_util::LifecycleNode("planner_server", "", options),
+  gp_loader_("nav2_core", "nav2_core::GlobalPlanner"),
+  default_ids_{"GridBased"},
+  default_types_{"nav2_navfn_planner/NavfnPlanner"},
+  costmap_(nullptr)
+{
+  RCLCPP_INFO(get_logger(), "Creating");
+
+  // Declare this node's parameters
+  declare_parameter("planner_plugins", default_ids_);
+  declare_parameter("expected_planner_frequency", 1.0);
+
+  get_parameter("planner_plugins", planner_ids_);
+  if (planner_ids_ == default_ids_) {
+    for (size_t i = 0; i < default_ids_.size(); ++i) {
+      declare_parameter(default_ids_[i] + ".plugin", default_types_[i]);
+    }
+  }
+
+  // Setup the global costmap
+  costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
+    "global_costmap", std::string{get_namespace()}, "global_costmap");
+}
+
+PlannerServer::~PlannerServer()
+{
+  /*
+   * Backstop ensuring this state is destroyed, even if deactivate/cleanup are
+   * never called.
+   */
+  planners_.clear();
+  costmap_thread_.reset();
+}
+
+nav2_util::CallbackReturn
+PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Configuring");
+
+  costmap_ros_->configure();
+  costmap_ = costmap_ros_->getCostmap();
+
+  // Launch a thread to run the costmap node
+  costmap_thread_ = std::make_unique<nav2_util::NodeThread>(costmap_ros_);
+
+  RCLCPP_DEBUG(
+    get_logger(), "Costmap size: %d,%d",
+    costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY());
+
+  tf_ = costmap_ros_->getTfBuffer();
+
+  planner_types_.resize(planner_ids_.size());
+
+  auto node = shared_from_this();
+
+  for (size_t i = 0; i != planner_ids_.size(); i++) {
+    try {
+      planner_types_[i] = nav2_util::get_plugin_type_param(
+        node, planner_ids_[i]);
+      nav2_core::GlobalPlanner::Ptr planner =
+        gp_loader_.createUniqueInstance(planner_types_[i]);
+      RCLCPP_INFO(
+        get_logger(), "Created global planner plugin %s of type %s",
+        planner_ids_[i].c_str(), planner_types_[i].c_str());
+      planner->configure(node, planner_ids_[i], tf_, costmap_ros_);
+      planners_.insert({planner_ids_[i], planner});
+    } catch (const pluginlib::PluginlibException & ex) {
+      RCLCPP_FATAL(
+        get_logger(), "Failed to create global planner. Exception: %s",
+        ex.what());
+      return nav2_util::CallbackReturn::FAILURE;
+    }
+  }
+
+  for (size_t i = 0; i != planner_ids_.size(); i++) {
+    planner_ids_concat_ += planner_ids_[i] + std::string(" ");
+  }
+
+  RCLCPP_INFO(
+    get_logger(),
+    "Planner Server has %s planners available.", planner_ids_concat_.c_str());
+
+  double expected_planner_frequency;
+  get_parameter("expected_planner_frequency", expected_planner_frequency);
+  if (expected_planner_frequency > 0) {
+    max_planner_duration_ = 1 / expected_planner_frequency;
+  } else {
+    RCLCPP_WARN(
+      get_logger(),
+      "The expected planner frequency parameter is %.4f Hz. The value should to be greater"
+      " than 0.0 to turn on duration overrrun warning messages", expected_planner_frequency);
+    max_planner_duration_ = 0.0;
+  }
+
+  // Initialize pubs & subs
+  plan_publisher_ = create_publisher<nav_msgs::msg::Path>("plan", 1);
+
+  // Create the action servers for path planning to a pose and through poses
+  action_server_pose_ = std::make_unique<ActionServerToPose>(
+    shared_from_this(),
+    "compute_path_to_pose",
+    std::bind(&PlannerServer::computePlan, this),
+    nullptr,
+    std::chrono::milliseconds(500),
+    true);
+
+  action_server_poses_ = std::make_unique<ActionServerThroughPoses>(
+    shared_from_this(),
+    "compute_path_through_poses",
+    std::bind(&PlannerServer::computePlanThroughPoses, this),
+    nullptr,
+    std::chrono::milliseconds(500),
+    true);
+
+  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_pose_->activate();
+  action_server_poses_->activate();
+  costmap_ros_->activate();
+
+  PlannerMap::iterator it;
+  for (it = planners_.begin(); it != planners_.end(); ++it) {
+    it->second->activate();
+  }
+
+  auto node = shared_from_this();
+
+  is_path_valid_service_ = node->create_service<nav2_msgs::srv::IsPathValid>(
+    "is_path_valid",
+    std::bind(
+      &PlannerServer::isPathValid, this,
+      std::placeholders::_1, std::placeholders::_2));
+
+  // Add callback for dynamic parameters
+  dyn_params_handler_ = node->add_on_set_parameters_callback(
+    std::bind(&PlannerServer::dynamicParametersCallback, this, _1));
+
+  // create bond connection
+  createBond();
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+PlannerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Deactivating");
+
+  action_server_pose_->deactivate();
+  action_server_poses_->deactivate();
+  plan_publisher_->on_deactivate();
+
+  /*
+   * The costmap is also a lifecycle node, so it may have already fired on_deactivate
+   * via rcl preshutdown cb. Despite the rclcpp docs saying on_shutdown callbacks fire
+   * in the order added, the preshutdown callbacks clearly don't per se, due to using an
+   * unordered_set iteration. Once this issue is resolved, we can maybe make a stronger
+   * ordering assumption: https://github.com/ros2/rclcpp/issues/2096
+   */
+  costmap_ros_->deactivate();
+
+  PlannerMap::iterator it;
+  for (it = planners_.begin(); it != planners_.end(); ++it) {
+    it->second->deactivate();
+  }
+
+  dyn_params_handler_.reset();
+
+  // destroy bond connection
+  destroyBond();
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+PlannerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Cleaning up");
+
+  action_server_pose_.reset();
+  action_server_poses_.reset();
+  plan_publisher_.reset();
+  tf_.reset();
+
+  costmap_ros_->cleanup();
+
+  PlannerMap::iterator it;
+  for (it = planners_.begin(); it != planners_.end(); ++it) {
+    it->second->cleanup();
+  }
+
+  planners_.clear();
+  costmap_thread_.reset();
+  costmap_ = nullptr;
+  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;
+}
+
+template<typename T>
+bool PlannerServer::isServerInactive(
+  std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server)
+{
+  if (action_server == nullptr || !action_server->is_server_active()) {
+    RCLCPP_DEBUG(get_logger(), "Action server unavailable or inactive. Stopping.");
+    return true;
+  }
+
+  return false;
+}
+
+void PlannerServer::waitForCostmap()
+{
+  // Don't compute a plan until costmap is valid (after clear costmap)
+  rclcpp::Rate r(100);
+  while (!costmap_ros_->isCurrent()) {
+    r.sleep();
+  }
+}
+
+template<typename T>
+bool PlannerServer::isCancelRequested(
+  std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server)
+{
+  if (action_server->is_cancel_requested()) {
+    RCLCPP_INFO(get_logger(), "Goal was canceled. Canceling planning action.");
+    action_server->terminate_all();
+    return true;
+  }
+
+  return false;
+}
+
+template<typename T>
+void PlannerServer::getPreemptedGoalIfRequested(
+  std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server,
+  typename std::shared_ptr<const typename T::Goal> goal)
+{
+  if (action_server->is_preempt_requested()) {
+    goal = action_server->accept_pending_goal();
+  }
+}
+
+template<typename T>
+bool PlannerServer::getStartPose(
+  std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server,
+  typename std::shared_ptr<const typename T::Goal> goal,
+  geometry_msgs::msg::PoseStamped & start)
+{
+  if (goal->use_start) {
+    start = goal->start;
+  } else if (!costmap_ros_->getRobotPose(start)) {
+    action_server->terminate_current();
+    return false;
+  }
+
+  return true;
+}
+
+template<typename T>
+bool PlannerServer::transformPosesToGlobalFrame(
+  std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server,
+  geometry_msgs::msg::PoseStamped & curr_start,
+  geometry_msgs::msg::PoseStamped & curr_goal)
+{
+  if (!costmap_ros_->transformPoseToGlobalFrame(curr_start, curr_start) ||
+    !costmap_ros_->transformPoseToGlobalFrame(curr_goal, curr_goal))
+  {
+    RCLCPP_WARN(
+      get_logger(), "Could not transform the start or goal pose in the costmap frame");
+    action_server->terminate_current();
+    return false;
+  }
+
+  return true;
+}
+
+template<typename T>
+bool PlannerServer::validatePath(
+  std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server,
+  const geometry_msgs::msg::PoseStamped & goal,
+  const nav_msgs::msg::Path & path,
+  const std::string & planner_id)
+{
+  if (path.poses.size() == 0) {
+    RCLCPP_WARN(
+      get_logger(), "Planning algorithm %s failed to generate a valid"
+      " path to (%.2f, %.2f)", planner_id.c_str(),
+      goal.pose.position.x, goal.pose.position.y);
+    action_server->terminate_current();
+    return false;
+  }
+
+  RCLCPP_DEBUG(
+    get_logger(),
+    "Found valid path of size %zu to (%.2f, %.2f)",
+    path.poses.size(), goal.pose.position.x,
+    goal.pose.position.y);
+
+  return true;
+}
+
+void
+PlannerServer::computePlanThroughPoses()
+{
+  std::lock_guard<std::mutex> lock(dynamic_params_lock_);
+
+  auto start_time = this->now();
+
+  // Initialize the ComputePathToPose goal and result
+  auto goal = action_server_poses_->get_current_goal();
+  auto result = std::make_shared<ActionThroughPoses::Result>();
+  nav_msgs::msg::Path concat_path;
+
+  try {
+    if (isServerInactive(action_server_poses_) || isCancelRequested(action_server_poses_)) {
+      return;
+    }
+
+    waitForCostmap();
+
+    getPreemptedGoalIfRequested(action_server_poses_, goal);
+
+    if (goal->goals.size() == 0) {
+      RCLCPP_WARN(
+        get_logger(),
+        "Compute path through poses requested a plan with no viapoint poses, returning.");
+      action_server_poses_->terminate_current();
+    }
+
+    // Use start pose if provided otherwise use current robot pose
+    geometry_msgs::msg::PoseStamped start;
+    if (!getStartPose(action_server_poses_, goal, start)) {
+      return;
+    }
+
+    // Get consecutive paths through these points
+    geometry_msgs::msg::PoseStamped curr_start, curr_goal;
+    for (unsigned int i = 0; i != goal->goals.size(); i++) {
+      // Get starting point
+      if (i == 0) {
+        curr_start = start;
+      } else {
+        // pick the end of the last planning task as the start for the next one
+        // to allow for path tolerance deviations
+        curr_start = concat_path.poses.back();
+        curr_start.header = concat_path.header;
+      }
+      curr_goal = goal->goals[i];
+
+      // Transform them into the global frame
+      if (!transformPosesToGlobalFrame(action_server_poses_, curr_start, curr_goal)) {
+        return;
+      }
+
+      // Get plan from start -> goal
+      nav_msgs::msg::Path curr_path = getPlan(curr_start, curr_goal, goal->planner_id);
+
+      // check path for validity
+      if (!validatePath(action_server_poses_, curr_goal, curr_path, goal->planner_id)) {
+        return;
+      }
+
+      // Concatenate paths together
+      concat_path.poses.insert(
+        concat_path.poses.end(), curr_path.poses.begin(), curr_path.poses.end());
+      concat_path.header = curr_path.header;
+    }
+
+    // Publish the plan for visualization purposes
+    result->path = concat_path;
+    publishPlan(result->path);
+
+    auto cycle_duration = this->now() - start_time;
+    result->planning_time = cycle_duration;
+
+    if (max_planner_duration_ && cycle_duration.seconds() > max_planner_duration_) {
+      RCLCPP_WARN(
+        get_logger(),
+        "Planner loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz",
+        1 / max_planner_duration_, 1 / cycle_duration.seconds());
+    }
+
+    action_server_poses_->succeeded_current(result);
+  } catch (std::exception & ex) {
+    RCLCPP_WARN(
+      get_logger(),
+      "%s plugin failed to plan through %zu points with final goal (%.2f, %.2f): \"%s\"",
+      goal->planner_id.c_str(), goal->goals.size(), goal->goals.back().pose.position.x,
+      goal->goals.back().pose.position.y, ex.what());
+    action_server_poses_->terminate_current();
+  }
+}
+
+void
+PlannerServer::computePlan()
+{
+  std::lock_guard<std::mutex> lock(dynamic_params_lock_);
+
+  auto start_time = this->now();
+
+  // Initialize the ComputePathToPose goal and result
+  auto goal = action_server_pose_->get_current_goal();
+  auto result = std::make_shared<ActionToPose::Result>();
+
+  try {
+    if (isServerInactive(action_server_pose_) || isCancelRequested(action_server_pose_)) {
+      return;
+    }
+
+    waitForCostmap();
+
+    getPreemptedGoalIfRequested(action_server_pose_, goal);
+
+    // Use start pose if provided otherwise use current robot pose
+    geometry_msgs::msg::PoseStamped start;
+    if (!getStartPose(action_server_pose_, goal, start)) {
+      return;
+    }
+
+    // Transform them into the global frame
+    geometry_msgs::msg::PoseStamped goal_pose = goal->goal;
+    if (!transformPosesToGlobalFrame(action_server_pose_, start, goal_pose)) {
+      return;
+    }
+
+    result->path = getPlan(start, goal_pose, goal->planner_id);
+
+    if (!validatePath(action_server_pose_, goal_pose, result->path, goal->planner_id)) {
+      return;
+    }
+
+    // Publish the plan for visualization purposes
+    publishPlan(result->path);
+
+    auto cycle_duration = this->now() - start_time;
+    result->planning_time = cycle_duration;
+
+    if (max_planner_duration_ && cycle_duration.seconds() > max_planner_duration_) {
+      RCLCPP_WARN(
+        get_logger(),
+        "Planner loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz",
+        1 / max_planner_duration_, 1 / cycle_duration.seconds());
+    }
+
+    action_server_pose_->succeeded_current(result);
+  } catch (std::exception & ex) {
+    RCLCPP_WARN(
+      get_logger(), "%s plugin failed to plan calculation to (%.2f, %.2f): \"%s\"",
+      goal->planner_id.c_str(), goal->goal.pose.position.x,
+      goal->goal.pose.position.y, ex.what());
+    action_server_pose_->terminate_current();
+  }
+}
+
+nav_msgs::msg::Path
+PlannerServer::getPlan(
+  const geometry_msgs::msg::PoseStamped & start,
+  const geometry_msgs::msg::PoseStamped & goal,
+  const std::string & planner_id)
+{
+  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.position.x, goal.pose.position.y);
+
+  if (planners_.find(planner_id) != planners_.end()) {
+    return planners_[planner_id]->createPlan(start, goal);
+  } else {
+    if (planners_.size() == 1 && planner_id.empty()) {
+      RCLCPP_WARN_ONCE(
+        get_logger(), "No planners specified in action call. "
+        "Server will use only plugin %s in server."
+        " This warning will appear once.", planner_ids_concat_.c_str());
+      return planners_[planners_.begin()->first]->createPlan(start, goal);
+    } else {
+      RCLCPP_ERROR(
+        get_logger(), "planner %s is not a valid planner. "
+        "Planner names are: %s", planner_id.c_str(),
+        planner_ids_concat_.c_str());
+    }
+  }
+
+  return nav_msgs::msg::Path();
+}
+
+void
+PlannerServer::publishPlan(const nav_msgs::msg::Path & path)
+{
+  auto msg = std::make_unique<nav_msgs::msg::Path>(path);
+  if (plan_publisher_->is_activated() && plan_publisher_->get_subscription_count() > 0) {
+    plan_publisher_->publish(std::move(msg));
+  }
+}
+
+void PlannerServer::isPathValid(
+  const std::shared_ptr<nav2_msgs::srv::IsPathValid::Request> request,
+  std::shared_ptr<nav2_msgs::srv::IsPathValid::Response> response)
+{
+  response->is_valid = true;
+
+  if (request->path.poses.empty()) {
+    response->is_valid = false;
+    return;
+  }
+
+  geometry_msgs::msg::PoseStamped current_pose;
+  unsigned int closest_point_index = 0;
+  if (costmap_ros_->getRobotPose(current_pose)) {
+    float current_distance = std::numeric_limits<float>::max();
+    float closest_distance = current_distance;
+    geometry_msgs::msg::Point current_point = current_pose.pose.position;
+    for (unsigned int i = 0; i < request->path.poses.size(); ++i) {
+      geometry_msgs::msg::Point path_point = request->path.poses[i].pose.position;
+
+      current_distance = nav2_util::geometry_utils::euclidean_distance(
+        current_point,
+        path_point);
+
+      if (current_distance < closest_distance) {
+        closest_point_index = i;
+        closest_distance = current_distance;
+      }
+    }
+
+    /**
+     * The lethal check starts at the closest point to avoid points that have already been passed
+     * and may have become occupied
+     */
+    std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
+    unsigned int mx = 0;
+    unsigned int my = 0;
+    for (unsigned int i = closest_point_index; i < request->path.poses.size(); ++i) {
+      costmap_->worldToMap(
+        request->path.poses[i].pose.position.x,
+        request->path.poses[i].pose.position.y, mx, my);
+      unsigned int cost = costmap_->getCost(mx, my);
+
+      if (cost == nav2_costmap_2d::LETHAL_OBSTACLE ||
+        cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
+      {
+        response->is_valid = false;
+      }
+    }
+  }
+}
+
+rcl_interfaces::msg::SetParametersResult
+PlannerServer::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
+{
+  std::lock_guard<std::mutex> lock(dynamic_params_lock_);
+  rcl_interfaces::msg::SetParametersResult result;
+
+  for (auto parameter : parameters) {
+    const auto & type = parameter.get_type();
+    const auto & name = parameter.get_name();
+
+    if (type == ParameterType::PARAMETER_DOUBLE) {
+      if (name == "expected_planner_frequency") {
+        if (parameter.as_double() > 0) {
+          max_planner_duration_ = 1 / parameter.as_double();
+        } else {
+          RCLCPP_WARN(
+            get_logger(),
+            "The expected planner frequency parameter is %.4f Hz. The value should to be greater"
+            " than 0.0 to turn on duration overrrun warning messages", parameter.as_double());
+          max_planner_duration_ = 0.0;
+        }
+      }
+    }
+  }
+
+  result.successful = true;
+  return result;
+}
+
+}  // namespace nav2_planner
+
+#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_planner::PlannerServer)

navigation2/nav2_planner/test/CMakeLists.txt

@@ -0,0 +1,10 @@
+# Test dynamic parameters
+ament_add_gtest(test_dynamic_parameters
+  test_dynamic_parameters.cpp
+)
+ament_target_dependencies(test_dynamic_parameters
+  ${dependencies}
+)
+target_link_libraries(test_dynamic_parameters
+  ${library_name}
+)

navigation2/nav2_planner/test/test_dynamic_parameters.cpp

@@ -0,0 +1,90 @@
+// Copyright (c) 2021, 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. Reserved.
+
+#include <math.h>
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "gtest/gtest.h"
+#include "nav2_util/lifecycle_node.hpp"
+#include "nav2_planner/planner_server.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+class PlannerShim : public nav2_planner::PlannerServer
+{
+public:
+  PlannerShim()
+  : nav2_planner::PlannerServer(rclcpp::NodeOptions())
+  {
+  }
+
+  // Since we cannot call configure/activate due to costmaps
+  // requiring TF
+  void setDynamicCallback()
+  {
+    auto node = shared_from_this();
+    // Add callback for dynamic parameters
+    dyn_params_handler_ = node->add_on_set_parameters_callback(
+      std::bind(&PlannerShim::dynamicParamsShim, this, std::placeholders::_1));
+  }
+
+  rcl_interfaces::msg::SetParametersResult
+  dynamicParamsShim(std::vector<rclcpp::Parameter> parameters)
+  {
+    rcl_interfaces::msg::SetParametersResult result;
+    result.successful = true;
+    dynamicParametersCallback(parameters);
+    return result;
+  }
+};
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+TEST(WPTest, test_dynamic_parameters)
+{
+  auto planner = std::make_shared<PlannerShim>();
+  planner->setDynamicCallback();
+
+  auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
+    planner->get_node_base_interface(), planner->get_node_topics_interface(),
+    planner->get_node_graph_interface(),
+    planner->get_node_services_interface());
+
+  auto results = rec_param->set_parameters_atomically(
+    {rclcpp::Parameter("expected_planner_frequency", 100.0)});
+
+  rclcpp::spin_until_future_complete(
+    planner->get_node_base_interface(),
+    results);
+
+  EXPECT_EQ(planner->get_parameter("expected_planner_frequency").as_double(), 100.0);
+
+  // test edge case for = 0
+  results = rec_param->set_parameters_atomically(
+    {rclcpp::Parameter("expected_planner_frequency", -1.0)});
+
+  rclcpp::spin_until_future_complete(
+    planner->get_node_base_interface(),
+    results);
+
+  EXPECT_EQ(planner->get_parameter("expected_planner_frequency").as_double(), -1.0);
+}