Port the navfn global planner to lifecycle nodes (#650)

* Port the navfn global planner to lifecycle nodes
* Address review feedback; transition to ROS2 actions; update for latest ROS2 code

nav2_navfn_planner/CMakeLists.txt

@@ -4,12 +4,13 @@ project(nav2_navfn_planner)
 find_package(ament_cmake REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(visualization_msgs REQUIRED)
-find_package(nav2_tasks REQUIRED)
 find_package(nav2_util REQUIRED)
 find_package(nav2_msgs REQUIRED)
 find_package(nav2_robot REQUIRED)
+find_package(nav2_lifecycle REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(builtin_interfaces REQUIRED)
@@ -25,12 +26,13 @@ set(library_name ${executable_name}_core)
 
 set(dependencies
   rclcpp
+  rclcpp_action
   std_msgs
   visualization_msgs
   nav2_util
-  nav2_tasks
   nav2_msgs
   nav2_robot
+  nav2_lifecycle
   nav_msgs
   geometry_msgs
   builtin_interfaces

nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp

@@ -44,6 +44,9 @@
 #include <string.h>
 #include <stdio.h>
 
+namespace nav2_navfn_planner
+{
+
 // cost defs
 #define COST_UNKNOWN_ROS 255  // 255 is unknown cost
 #define COST_OBS 254  // 254 for forbidden regions
@@ -264,4 +267,6 @@ public:
   void savemap(const char * fname);
 };
 
+}  // namespace nav2_navfn_planner
+
 #endif  // NAV2_NAVFN_PLANNER__NAVFN_HPP_

nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp

@@ -16,35 +16,50 @@
 #ifndef NAV2_NAVFN_PLANNER__NAVFN_PLANNER_HPP_
 #define NAV2_NAVFN_PLANNER__NAVFN_PLANNER_HPP_
 
+#include <chrono>
 #include <string>
-#include <vector>
 #include <memory>
-#include <chrono>
+#include <vector>
 
-#include "nav2_tasks/compute_path_to_pose_task.hpp"
+#include "geometry_msgs/msg/point.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_lifecycle/lifecycle_node.hpp"
+#include "nav2_msgs/action/compute_path_to_pose.hpp"
 #include "nav2_msgs/msg/costmap.hpp"
-#include "nav2_tasks/costmap_service_client.hpp"
+#include "nav2_msgs/msg/path.hpp"
 #include "nav2_navfn_planner/navfn.hpp"
-#include "geometry_msgs/msg/pose_stamped.hpp"
-#include "geometry_msgs/msg/point.hpp"
+#include "nav2_robot/robot.hpp"
+#include "nav2_util/costmap_service_client.hpp"
+#include "nav2_util/simple_action_server.hpp"
 #include "nav_msgs/msg/path.hpp"
 #include "visualization_msgs/msg/marker.hpp"
-#include "nav2_robot/robot.hpp"
 
 namespace nav2_navfn_planner
 {
 
-class NavfnPlanner : public rclcpp::Node
+class NavfnPlanner : public nav2_lifecycle::LifecycleNode
 {
 public:
   NavfnPlanner();
   ~NavfnPlanner();
 
-  nav2_tasks::TaskStatus computePathToPose(
-    const nav2_tasks::ComputePathToPoseCommand::SharedPtr command);
+protected:
+  // Implement the lifecycle interface
+  nav2_lifecycle::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
+  nav2_lifecycle::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
+  nav2_lifecycle::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
+  nav2_lifecycle::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
+  nav2_lifecycle::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
+  nav2_lifecycle::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override;
+
+  using GoalHandle = rclcpp_action::ServerGoalHandle<nav2_msgs::action::ComputePathToPose>;
+  using ActionServer = nav2_util::SimpleActionServer<nav2_msgs::action::ComputePathToPose>;
+
+  // Our action server implements the ComputePathToPose action
+  std::unique_ptr<ActionServer> action_server_;
 
-private:
-  std::unique_ptr<nav2_tasks::ComputePathToPoseTaskServer> task_server_;
+  // The action server callback
+  void computePathToPose(const std::shared_ptr<GoalHandle> goal_handle);
 
   // Compute a plan given start and goal poses, provided in global world frame.
   bool makePlan(
@@ -111,33 +126,35 @@ private:
   // Determine if a new planner object should be made
   bool isPlannerOutOfDate();
 
+  std::unique_ptr<nav2_robot::Robot> robot_;
+
   // Planner based on ROS1 NavFn algorithm
   std::unique_ptr<NavFn> planner_;
 
   // Service client for getting the costmap
-  nav2_tasks::CostmapServiceClient costmap_client_;
+  nav2_util::CostmapServiceClient costmap_client_{"navfn_planner"};
 
   // Publishers for the path and endpoints
-  rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr plan_publisher_;
-  rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr plan_marker_publisher_;
+  rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr plan_publisher_;
+  rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::Marker>::SharedPtr
+    plan_marker_publisher_;
 
-  // The costmap to use
+  // The costmap to use and its size
   nav2_msgs::msg::Costmap costmap_;
   uint current_costmap_size_[2];
 
   // The global frame of the costmap
-  std::string global_frame_;
+  const std::string global_frame_{"map"};
 
   // Whether or not the planner should be allowed to plan through unknown space
-  bool allow_unknown_;
+  const bool allow_unknown_{true};
 
-  // Amount the planner can relax the space constraint
+  // If the goal is obstructed, the tolerance specifies how many meters the planner
+  // can relax the constraint in x and y before failing
   double tolerance_;
 
   // Whether to use the astar planner or default dijkstras
   bool use_astar_;
-
-  std::unique_ptr<nav2_robot::Robot> robot_;
 };
 
 }  // namespace nav2_navfn_planner

nav2_navfn_planner/package.xml

@@ -9,24 +9,25 @@
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
-  <build_depend>nav2_common</build_depend>
 
   <build_depend>rclcpp</build_depend>
+  <build_depend>rclcpp_action</build_depend>
   <build_depend>visualization_msgs</build_depend>
-  <build_depend>nav2_tasks</build_depend>
   <build_depend>nav2_util</build_depend>
   <build_depend>nav2_msgs</build_depend>
   <build_depend>nav2_robot</build_depend>
+  <build_depend>nav_lifecycle</build_depend>
   <build_depend>nav_msgs</build_depend>
   <build_depend>geometry_msgs</build_depend>
   <build_depend>builtin_interfaces</build_depend>
 
   <exec_depend>rclcpp</exec_depend>
+  <exec_depend>rclcpp_action</exec_depend>
   <exec_depend>visualization_msgs</exec_depend>
-  <exec_depend>nav2_tasks</exec_depend>
   <exec_depend>nav2_msgs</exec_depend>
   <exec_depend>nav2_robot</exec_depend>
   <exec_depend>nav2_util</exec_depend>
+  <exec_depend>nav2_lifecycle</exec_depend>
   <exec_depend>nav_msgs</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>builtin_interfaces</exec_depend>

nav2_navfn_planner/src/main.cpp

@@ -10,16 +10,18 @@
 // 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.
+// limitations under the License.
 
 #include <memory>
-#include "rclcpp/rclcpp.hpp"
+
 #include "nav2_navfn_planner/navfn_planner.hpp"
+#include "rclcpp/rclcpp.hpp"
 
 int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);
-  rclcpp::spin(std::make_shared<nav2_navfn_planner::NavfnPlanner>());
+  auto node = std::make_shared<nav2_navfn_planner::NavfnPlanner>();
+  rclcpp::spin(node->get_node_base_interface());
   rclcpp::shutdown();
 
   return 0;

nav2_navfn_planner/src/navfn.cpp

@@ -41,9 +41,13 @@
 // Path calc has sanity check that it succeeded
 //
 
+#include "nav2_navfn_planner/navfn.hpp"
+
 #include <algorithm>
 #include "rclcpp/rclcpp.hpp"
-#include "nav2_navfn_planner/navfn.hpp"
+
+namespace nav2_navfn_planner
+{
 
 //
 // function to perform nav fn calculation
@@ -1035,3 +1039,5 @@ NavFn::savemap(const char * fname)
   fwrite(costarr, 1, nx * ny, fp);
   fclose(fp);
 }
+
+}  // namespace nav2_navfn_planner

nav2_navfn_planner/src/navfn_planner.cpp

@@ -13,153 +13,214 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-
 // Navigation Strategy based on:
 // Brock, O. and Oussama K. (1999). High-Speed Navigation Using
 // the Global Dynamic Window Approach. IEEE.
 // https://cs.stanford.edu/group/manips/publications/pdfs/Brock_1999_ICRA.pdf
 
-#include <string>
-#include <vector>
-#include <memory>
+#include "nav2_navfn_planner/navfn_planner.hpp"
+
 #include <chrono>
-#include <limits>
-#include <iostream>
-#include <iomanip>
-#include <algorithm>
-#include <exception>
 #include <cmath>
-#include "nav2_navfn_planner/navfn_planner.hpp"
-#include "nav2_navfn_planner/navfn.hpp"
-#include "nav2_util/costmap.hpp"
-#include "nav2_util/node_utils.hpp"
+#include <iomanip>
+#include <iostream>
+#include <limits>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "builtin_interfaces/msg/duration.hpp"
+#include "geometry_msgs/msg/point.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
 #include "nav2_msgs/msg/costmap.hpp"
 #include "nav2_msgs/srv/get_costmap.hpp"
-#include "geometry_msgs/msg/pose_stamped.hpp"
-#include "geometry_msgs/msg/point.hpp"
-#include "visualization_msgs/msg/marker.hpp"
-#include "builtin_interfaces/msg/duration.hpp"
+#include "nav2_navfn_planner/navfn.hpp"
+#include "nav2_util/costmap.hpp"
 #include "nav_msgs/msg/path.hpp"
+#include "visualization_msgs/msg/marker.hpp"
 
 using namespace std::chrono_literals;
-using nav2_tasks::TaskStatus;
 
 namespace nav2_navfn_planner
 {
 
 NavfnPlanner::NavfnPlanner()
-: Node("NavfnPlanner", nav2_util::get_node_options_default()),
-  global_frame_("map"),
-  allow_unknown_(true)
+: nav2_lifecycle::LifecycleNode("navfn_planner", "", true)
 {
-  RCLCPP_INFO(get_logger(), "Initializing.");
+  RCLCPP_INFO(get_logger(), "Creating");
 
-  // Grab params off the param server
-  auto temp_node = std::shared_ptr<rclcpp::Node>(this, [](auto) {});
+  // Declare this node's parameters
+  declare_parameter("tolerance", rclcpp::ParameterValue(0.0));
+  declare_parameter("use_astar", rclcpp::ParameterValue(false));
+}
 
-  auto parameters_client = std::make_shared<rclcpp::SyncParametersClient>(temp_node);
+NavfnPlanner::~NavfnPlanner()
+{
+  RCLCPP_INFO(get_logger(), "Destroying");
+}
 
-  // If the goal is obstructed, the tolerance specifies how many meters the planner
-  // can relax the constraint in x and y before failing
-  tolerance_ = parameters_client->get_parameter("tolerance", 0.0);
+nav2_lifecycle::CallbackReturn
+NavfnPlanner::on_configure(const rclcpp_lifecycle::State & state)
+{
+  RCLCPP_INFO(get_logger(), "Configuring");
+
+  // Initialize parameters
+  get_parameter("tolerance", tolerance_);
+  get_parameter("use_astar", use_astar_);
 
-  use_astar_ = parameters_client->get_parameter("use_astar", false);
+  getCostmap(costmap_);
+  RCLCPP_DEBUG(get_logger(), "Costmap size: %d,%d",
+    costmap_.metadata.size_x, costmap_.metadata.size_y);
 
-  // TODO(orduno): Enable parameter server and get costmap service name from there
+  // Create a planner based on the new costmap size
+  if (isPlannerOutOfDate()) {
+    current_costmap_size_[0] = costmap_.metadata.size_x;
+    current_costmap_size_[1] = costmap_.metadata.size_y;
+    planner_ = std::make_unique<NavFn>(costmap_.metadata.size_x, costmap_.metadata.size_y);
+  }
 
-  // Create publishers for visualization of the path and endpoints
-  plan_publisher_ = this->create_publisher<nav_msgs::msg::Path>("plan", 1);
-  plan_marker_publisher_ = this->create_publisher<visualization_msgs::msg::Marker>(
+  // Initialize pubs & subs
+  plan_publisher_ = create_publisher<nav_msgs::msg::Path>("plan", 1);
+  plan_marker_publisher_ = create_publisher<visualization_msgs::msg::Marker>(
     "endpoints", 1);
 
-  robot_ = std::make_unique<nav2_robot::Robot>(temp_node);
+  auto node = shared_from_this();
 
-  task_server_ = std::make_unique<nav2_tasks::ComputePathToPoseTaskServer>(temp_node, false),
-  task_server_->setExecuteCallback(
-    std::bind(&NavfnPlanner::computePathToPose, this, std::placeholders::_1));
+  // Initialize supporting objects
+  robot_ = std::make_unique<nav2_robot::Robot>(node);
+  robot_->on_configure(state);
 
-  // Start listening for incoming ComputePathToPose task requests
-  task_server_->start();
+  // Create the action server that we implement with our navigateToPose method
+  action_server_ = std::make_unique<ActionServer>(rclcpp_node_, "ComputePathToPose",
+      std::bind(&NavfnPlanner::computePathToPose, this, std::placeholders::_1));
+
+  return nav2_lifecycle::CallbackReturn::SUCCESS;
 }
 
-NavfnPlanner::~NavfnPlanner()
+nav2_lifecycle::CallbackReturn
+NavfnPlanner::on_activate(const rclcpp_lifecycle::State & state)
+{
+  RCLCPP_INFO(get_logger(), "Activating");
+
+  plan_publisher_->on_activate();
+  plan_marker_publisher_->on_activate();
+  robot_->on_activate(state);
+
+  return nav2_lifecycle::CallbackReturn::SUCCESS;
+}
+
+nav2_lifecycle::CallbackReturn
+NavfnPlanner::on_deactivate(const rclcpp_lifecycle::State & state)
 {
-  RCLCPP_INFO(get_logger(), "Shutting down.");
+  RCLCPP_INFO(get_logger(), "Deactivating");
+
+  plan_publisher_->on_deactivate();
+  plan_marker_publisher_->on_deactivate();
+  robot_->on_deactivate(state);
+
+  return nav2_lifecycle::CallbackReturn::SUCCESS;
 }
 
-TaskStatus
-NavfnPlanner::computePathToPose(const nav2_tasks::ComputePathToPoseCommand::SharedPtr command)
+nav2_lifecycle::CallbackReturn
+NavfnPlanner::on_cleanup(const rclcpp_lifecycle::State & state)
 {
-  nav2_tasks::ComputePathToPoseResult result;
-  try {
-    // Get an updated costmap
-    getCostmap(costmap_);
-    RCLCPP_DEBUG(get_logger(), "Costmap size: %d,%d",
-      costmap_.metadata.size_x, costmap_.metadata.size_y);
+  RCLCPP_INFO(get_logger(), "Cleaning up");
 
-    // Create a planner based on the new costmap size
-    if (isPlannerOutOfDate()) {
-      current_costmap_size_[0] = costmap_.metadata.size_x;
-      current_costmap_size_[1] = costmap_.metadata.size_y;
-      planner_ = std::make_unique<NavFn>(costmap_.metadata.size_x, costmap_.metadata.size_y);
-    }
+  robot_->on_cleanup(state);
 
+  plan_publisher_.reset();
+  plan_marker_publisher_.reset();
+  robot_.reset();
+
+  action_server_.reset();
+  planner_.reset();
+
+  return nav2_lifecycle::CallbackReturn::SUCCESS;
+}
+
+nav2_lifecycle::CallbackReturn
+NavfnPlanner::on_error(const rclcpp_lifecycle::State &)
+{
+  RCLCPP_ERROR(get_logger(), "Handling error state");
+  return nav2_lifecycle::CallbackReturn::SUCCESS;
+}
+
+nav2_lifecycle::CallbackReturn
+NavfnPlanner::on_shutdown(const rclcpp_lifecycle::State &)
+{
+  RCLCPP_INFO(get_logger(), "Shutting down");
+  return nav2_lifecycle::CallbackReturn::SUCCESS;
+}
+
+void
+NavfnPlanner::computePathToPose(const std::shared_ptr<GoalHandle> goal_handle)
+{
+  // Initialize the ComputePathToPose goal and result
+  auto goal = goal_handle->get_goal();
+  auto result = std::make_shared<nav2_msgs::action::ComputePathToPose::Result>();
+
+  // TODO(mjeronimo): handle or reject an attempted pre-emption
+
+  try {
     // Get the current pose from the robot
     auto start = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>();
 
     if (!robot_->getCurrentPose(start)) {
       RCLCPP_ERROR(get_logger(), "Current robot pose is not available.");
-      return TaskStatus::FAILED;
+      goal_handle->abort(result);
+      return;
     }
 
     RCLCPP_INFO(get_logger(), "Attempting to a find path from (%.2f, %.2f) to "
       "(%.2f, %.2f).", start->pose.pose.position.x, start->pose.pose.position.y,
-      command->pose.position.x, command->pose.position.y);
+      goal->pose.pose.position.x, goal->pose.pose.position.y);
 
     // Make the plan for the provided goal pose
-    bool foundPath = makePlan(start->pose.pose, command->pose, tolerance_, result);
+    bool foundPath = makePlan(start->pose.pose, goal->pose.pose, tolerance_, result->path);
 
     // TODO(orduno): should check for cancel within the makePlan() method?
-    if (task_server_->cancelRequested()) {
-      RCLCPP_INFO(get_logger(), "Cancelled global planning task.");
-      task_server_->setCanceled();
-      return TaskStatus::CANCELED;
+    if (goal_handle->is_canceling()) {
+      RCLCPP_INFO(get_logger(), "Canceling global planning task");
+      goal_handle->canceled(result);
+      return;
     }
 
     if (!foundPath) {
       RCLCPP_WARN(get_logger(), "Planning algorithm failed to generate a valid"
-        " path to (%.2f, %.2f)", command->pose.position.x, command->pose.position.y);
-      return TaskStatus::FAILED;
+        " path to (%.2f, %.2f)", goal->pose.pose.position.x, goal->pose.pose.position.y);
+      goal_handle->abort(result);
+      return;
     }
 
-    RCLCPP_INFO(get_logger(),
-      "Found valid path of size %u", result.poses.size());
+    RCLCPP_DEBUG(get_logger(), "Found valid path of size %u", result->path.poses.size());
 
     // Publish the plan for visualization purposes
-    RCLCPP_INFO(get_logger(), "Publishing the valid path.");
-    publishPlan(result);
-    publishEndpoints(start->pose.pose, command->pose);
+    RCLCPP_DEBUG(get_logger(), "Publishing the valid path");
+    publishPlan(result->path);
+    publishEndpoints(start->pose.pose, goal->pose.pose);
 
     // TODO(orduno): Enable potential visualization
 
     RCLCPP_INFO(get_logger(),
-      "Successfully navigated to (%.2f, %.2f) with tolerance %.2f",
-      command->pose.position.x, command->pose.position.y, tolerance_);
-    task_server_->setResult(result);
-    return TaskStatus::SUCCEEDED;
+      "Successfully computed a path to (%.2f, %.2f) with tolerance %.2f",
+      goal->pose.pose.position.x, goal->pose.pose.position.y, tolerance_);
+    goal_handle->succeed(result);
+    return;
   } catch (std::exception & ex) {
     RCLCPP_WARN(get_logger(), "Plan calculation to (%.2f, %.2f) failed: \"%s\"",
-      command->pose.position.x, command->pose.position.y, ex.what());
+      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
-    return TaskStatus::FAILED;
+    goal_handle->abort(result);
+    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
-    return TaskStatus::FAILED;
+    goal_handle->abort(result);
+    return;
   }
 }
 
@@ -188,7 +249,7 @@ NavfnPlanner::makePlan(
   double wx = start.position.x;
   double wy = start.position.y;
 
-  RCLCPP_INFO(get_logger(), "Making plan from (%.2f,%.2f) to (%.2f,%.2f)",
+  RCLCPP_DEBUG(get_logger(), "Making plan from (%.2f,%.2f) to (%.2f,%.2f)",
     start.position.x, start.position.y, goal.position.x, goal.position.y);
 
   unsigned int mx, my;
@@ -479,7 +540,7 @@ NavfnPlanner::getCostmap(
   // TODO(orduno): explicitly provide specifications for costmap using the costmap on the request,
   //               including master (aggregate) layer
 
-  auto request = std::make_shared<nav2_tasks::CostmapServiceClient::CostmapServiceRequest>();
+  auto request = std::make_shared<nav2_util::CostmapServiceClient::CostmapServiceRequest>();
   request->specs.resolution = 1.0;
 
   auto result = costmap_client_.invoke(request);