Merge pull request #332 from SteveMacenski/smart_to_navfn

Smart planner to navfn planner renaming

.travis.yml

@@ -26,7 +26,7 @@ script:
   - docker run --name nav2_bash --rm -d -i -t nav2:latest bash -l
   - docker exec nav2_bash bash -c "colcon test --packages-skip
       dwb_controller dwb_core nav2_amcl nav2_costmap_2d nav2_dynamic_params 
-      nav2_map_server nav2_smart_planner nav2_system_tests nav2_tasks 
+      nav2_map_server nav2_navfn_planner nav2_system_tests nav2_tasks 
       nav2_util nav2_voxel_grid costmap_queue dwb_critics dwb_plugins 
       nav2_bt_navigator nav2_mission_executor nav2_robot 
       nav2_simple_navigator nav2_world_model nav_2d_utils"

doc/design/ROS_COMPARISON.md

@@ -11,7 +11,7 @@ AMCL and map_server were ported to ROS2 with minimal functional changes, but som
 In addition, move_base itself has been split into multiple components:
 
   * [nav2_simple_navigator](/nav2_simple_navigator/README.md) (replaces move_base)
-  * [nav2_smart_planner](/nav2_smart_planner/README.md) (replaces global_planner)
+  * [nav2_navfn_planner](/nav2_navfn_planner/README.md) (replaces global_planner)
   * [nav2_dwb_controller](/nav2_dwb_controller/README.md) (replaces local_planner)
 
 ![Move Base 2](./move_base_compare_2.png)
@@ -24,7 +24,7 @@ The reason for the change was to make it so that global and local planners would
 
 The *nav2_simple_navigator* itself is also a *Task Server* and can also be replaced with other implementations. The first such implementation is currently in progress called *nav2_bt_navigator*. It uses *Behavior Trees* to make it possible to have more complex state machines and to add in recovery behaviors as additional *Task Servers*. See *nav2_bt_navigator* for that implementation. (currently WIP in [Pull request 91](https://github.com/ros-planning/navigation2/pull/91))
 
-The *nav2_smart_planner* is ported from the *navfn* package in ROS, but adds the *Task Server* interface to enable it to be strongly decoupled from the nav2_simple_navigator.
+The *nav2_navfn_planner* is ported from the *navfn* package in ROS, but adds the *Task Server* interface to enable it to be strongly decoupled from the nav2_simple_navigator.
 
 Similarly, the *nav2_dwb_controller* is ported from the [dwb controller](https://github.com/locusrobotics/robot_navigation/tree/master/dwb_local_planner) package, and also adds a *Task Server* interface to also enable it to be decoupled from the *nav2_simple_navigator*.
 

nav2_bringup/launch/core.launch.py

@@ -17,7 +17,7 @@ def generate_launch_description():
                 ], period = 5.0),
         launch.actions.TimerAction(
             actions = [
-                launch_ros.actions.Node( package='nav2_smart_planner', node_executable='smart_planner', output='screen')
+                launch_ros.actions.Node( package='nav2_navfn_planner', node_executable='navfn_planner', output='screen')
                 ], period = 10.0),
         launch.actions.TimerAction(
             actions = [

nav2_bringup/launch/nav2_bringup_launch.py

@@ -59,9 +59,9 @@ def generate_launch_description():
             parameters=[{ 'prune_plan': False }, {'debug_trajectory_details': True }, { 'use_sim_time': use_sim_time }]),
 
         launch_ros.actions.Node(
-            package='nav2_smart_planner',
-            node_executable='smart_planner_node',
-            node_name='smart_planner',
+            package='nav2_navfn_planner',
+            node_executable='navfn_planner',
+            node_name='navfn_planner',
             output='screen',
             parameters=[{ 'use_sim_time': use_sim_time}]),
 

nav2_smart_planner/CMakeLists.txt → nav2_navfn_planner/CMakeLists.txt

 cmake_minimum_required(VERSION 3.5)
-project(nav2_smart_planner)
+project(nav2_navfn_planner)
 
 # Default to C++14
 if(NOT CMAKE_CXX_STANDARD)
@@ -23,8 +23,8 @@ include_directories(
   include
 )
 
-set(library_name smart_planner)
-set(executable_name ${library_name}_node)
+set(executable_name navfn_planner)
+set(library_name ${executable_name}_core)
 
 set(dependencies
   rclcpp
@@ -37,7 +37,7 @@ set(dependencies
 )
 
 add_library(${library_name} SHARED
-  src/smart_planner.cpp
+  src/navfn_planner.cpp
   src/navfn.cpp
 )
 

nav2_smart_planner/README.md → nav2_navfn_planner/README.md

-# Smart Planner
+# Navfn Planner
 
-The SmartPlanner is a [planning module](../doc/requirements/requirements.md) that implements the `nav2_tasks::ComputePathToPose` interface.
+The NavfnPlanner is a [planning module](../doc/requirements/requirements.md) that implements the `nav2_tasks::ComputePathToPose` interface.
 
 A planning module implementing the `nav2_tasks::ComputePathToPose` interface is responsible for generating a feasible path given start and end robot poses. It provides the equivalent functionality to a [GlobalPlanner](http://wiki.ros.org/nav_core#BaseGlobalPlanner) in ROS1 [MoveBase](http://wiki.ros.org/move_base).
 
 ## Status
-Currently, SmartPlanner's core algorithm is a direct port from the ROS1 MoveBase [Navfn](http://wiki.ros.org/navfn) planner. The Navfn planning algorithm is based on the [Global Dynamic Window Approach](https://cs.stanford.edu/group/manips/publications/pdfs/Brock_1999_ICRA.pdf).
+Currently, NavfnPlanner's core algorithm is a direct port from the ROS1 MoveBase [Navfn](http://wiki.ros.org/navfn) planner. The Navfn planning algorithm is based on the [Global Dynamic Window Approach](https://cs.stanford.edu/group/manips/publications/pdfs/Brock_1999_ICRA.pdf).
 
 ## Characteristics
 

nav2_smart_planner/include/nav2_smart_planner/navfn.hpp → nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp

@@ -38,8 +38,8 @@
 // Modified for Euclidean-distance computation
 //
 
-#ifndef NAV2_SMART_PLANNER__NAVFN_HPP_
-#define NAV2_SMART_PLANNER__NAVFN_HPP_
+#ifndef NAV2_NAVFN_PLANNER__NAVFN_HPP_
+#define NAV2_NAVFN_PLANNER__NAVFN_HPP_
 
 #include <math.h>
 #include <stdint.h>
@@ -266,4 +266,4 @@ public:
   void savemap(const char * fname);
 };
 
-#endif  // NAV2_SMART_PLANNER__NAVFN_HPP_
+#endif  // NAV2_NAVFN_PLANNER__NAVFN_HPP_

nav2_smart_planner/include/nav2_smart_planner/smart_planner.hpp → nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp

@@ -13,8 +13,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef NAV2_SMART_PLANNER__SMART_PLANNER_HPP_
-#define NAV2_SMART_PLANNER__SMART_PLANNER_HPP_
+#ifndef NAV2_NAVFN_PLANNER__NAVFN_PLANNER_HPP_
+#define NAV2_NAVFN_PLANNER__NAVFN_PLANNER_HPP_
 
 #include <string>
 #include <vector>
@@ -24,20 +24,20 @@
 #include "nav2_tasks/compute_path_to_pose_task.hpp"
 #include "nav2_msgs/msg/costmap.hpp"
 #include "nav2_tasks/costmap_service_client.hpp"
-#include "nav2_smart_planner/navfn.hpp"
+#include "nav2_navfn_planner/navfn.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/point.hpp"
 #include "nav_msgs/msg/path.hpp"
 #include "visualization_msgs/msg/marker.hpp"
 
-namespace nav2_smart_planner
+namespace nav2_navfn_planner
 {
 
-class SmartPlanner : public rclcpp::Node
+class NavfnPlanner : public rclcpp::Node
 {
 public:
-  SmartPlanner();
-  ~SmartPlanner();
+  NavfnPlanner();
+  ~NavfnPlanner();
 
   nav2_tasks::TaskStatus computePathToPose(
     const nav2_tasks::ComputePathToPoseCommand::SharedPtr command);
@@ -135,6 +135,6 @@ private:
   bool use_astar_;
 };
 
-}  // namespace nav2_smart_planner
+}  // namespace nav2_navfn_planner
 
-#endif  // NAV2_SMART_PLANNER__SMART_PLANNER_HPP_
+#endif  // NAV2_NAVFN_PLANNER__NAVFN_PLANNER_HPP_

nav2_smart_planner/package.xml → nav2_navfn_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_smart_planner</name>
+  <name>nav2_navfn_planner</name>
   <version>0.1.0</version>
   <description>TODO</description>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

nav2_smart_planner/src/main.cpp → nav2_navfn_planner/src/main.cpp

@@ -14,12 +14,12 @@
 
 #include <memory>
 #include "rclcpp/rclcpp.hpp"
-#include "nav2_smart_planner/smart_planner.hpp"
+#include "nav2_navfn_planner/navfn_planner.hpp"
 
 int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);
-  rclcpp::spin(std::make_shared<nav2_smart_planner::SmartPlanner>());
+  rclcpp::spin(std::make_shared<nav2_navfn_planner::NavfnPlanner>());
   rclcpp::shutdown();
 
   return 0;

nav2_smart_planner/src/navfn.cpp → nav2_navfn_planner/src/navfn.cpp

@@ -44,7 +44,7 @@
 
 #include <algorithm>
 #include "rclcpp/rclcpp.hpp"
-#include "nav2_smart_planner/navfn.hpp"
+#include "nav2_navfn_planner/navfn.hpp"
 
 //
 // function to perform nav fn calculation

nav2_smart_planner/src/smart_planner.cpp → nav2_navfn_planner/src/navfn_planner.cpp

@@ -29,8 +29,8 @@
 #include <algorithm>
 #include <exception>
 #include <cmath>
-#include "nav2_smart_planner/smart_planner.hpp"
-#include "nav2_smart_planner/navfn.hpp"
+#include "nav2_navfn_planner/navfn_planner.hpp"
+#include "nav2_navfn_planner/navfn.hpp"
 #include "nav2_util/costmap.hpp"
 #include "nav2_msgs/msg/costmap.hpp"
 #include "nav2_msgs/srv/get_costmap.hpp"
@@ -43,17 +43,17 @@
 using namespace std::chrono_literals;
 using nav2_tasks::TaskStatus;
 
-namespace nav2_smart_planner
+namespace nav2_navfn_planner
 {
 
-SmartPlanner::SmartPlanner()
-: Node("SmartPlanner"),
+NavfnPlanner::NavfnPlanner()
+: Node("NavfnPlanner"),
   global_frame_("map"),
   allow_unknown_(true),
   default_tolerance_(1.0),
   use_astar_(false)
 {
-  RCLCPP_INFO(get_logger(), "Initializing SmartPlanner...");
+  RCLCPP_INFO(get_logger(), "Initializing.");
 
   // Grab params off the param server
   auto temp_node = std::shared_ptr<rclcpp::Node>(this, [](auto) {});
@@ -70,21 +70,21 @@ SmartPlanner::SmartPlanner()
 
   task_server_ = std::make_unique<nav2_tasks::ComputePathToPoseTaskServer>(temp_node, false),
   task_server_->setExecuteCallback(
-    std::bind(&SmartPlanner::computePathToPose, this, std::placeholders::_1));
+    std::bind(&NavfnPlanner::computePathToPose, this, std::placeholders::_1));
 
   // Start listening for incoming ComputePathToPose task requests
   task_server_->startWorkerThread();
 }
 
-SmartPlanner::~SmartPlanner()
+NavfnPlanner::~NavfnPlanner()
 {
-  RCLCPP_INFO(get_logger(), "Shutting down SmartPlanner");
+  RCLCPP_INFO(get_logger(), "Shutting down.");
 }
 
 TaskStatus
-SmartPlanner::computePathToPose(const nav2_tasks::ComputePathToPoseCommand::SharedPtr command)
+NavfnPlanner::computePathToPose(const nav2_tasks::ComputePathToPoseCommand::SharedPtr command)
 {
-  RCLCPP_INFO(get_logger(), "SmartPlanner: Attempting to a find path from (%.2f, %.2f) to "
+  RCLCPP_INFO(get_logger(), "Attempting to a find path from (%.2f, %.2f) to "
     "(%.2f, %.2f).", command->start.position.x, command->start.position.y,
     command->goal.position.x, command->goal.position.y);
 
@@ -92,7 +92,7 @@ SmartPlanner::computePathToPose(const nav2_tasks::ComputePathToPoseCommand::Shar
   try {
     // Get an updated costmap
     getCostmap(costmap_);
-    RCLCPP_DEBUG(get_logger(), "SmartPlanner: Costmap size: %d,%d",
+    RCLCPP_DEBUG(get_logger(), "Costmap size: %d,%d",
       costmap_.metadata.size_x, costmap_.metadata.size_y);
 
     // Create a planner based on the new costmap size
@@ -107,41 +107,41 @@ SmartPlanner::computePathToPose(const nav2_tasks::ComputePathToPoseCommand::Shar
 
     // TODO(orduno): should check for cancel within the makePlan() method?
     if (task_server_->cancelRequested()) {
-      RCLCPP_INFO(get_logger(), "SmartPlanner: Cancelled global planning task.");
+      RCLCPP_INFO(get_logger(), "Cancelled global planning task.");
       task_server_->setCanceled();
       return TaskStatus::CANCELED;
     }
 
     if (!foundPath) {
-      RCLCPP_WARN(get_logger(), "SmartPlanner: Planning algorithm failed to generate a valid"
+      RCLCPP_WARN(get_logger(), "Planning algorithm failed to generate a valid"
         " path to (%.2f, %.2f)", command->goal.position.x, command->goal.position.y);
       return TaskStatus::FAILED;
     }
 
     RCLCPP_INFO(get_logger(),
-      "SmartPlanner: Found valid path of size %u", result.poses.size());
+      "Found valid path of size %u", result.poses.size());
 
     // Publish the plan for visualization purposes
-    RCLCPP_INFO(get_logger(), "SmartPlanner: Publishing the valid path.");
+    RCLCPP_INFO(get_logger(), "Publishing the valid path.");
     publishPlan(result);
     publishEndpoints(command);
 
     // TODO(orduno): Enable potential visualization
 
     RCLCPP_INFO(get_logger(),
-      "SmartPlanner: Successfully navigated to (%.2f, %.2f) with tolerance %.2f",
+      "Successfully navigated to (%.2f, %.2f) with tolerance %.2f",
       command->goal.position.x, command->goal.position.y, command->tolerance);
     task_server_->setResult(result);
     return TaskStatus::SUCCEEDED;
   } catch (std::exception & ex) {
-    RCLCPP_WARN(get_logger(), "SmartPlanner: Plan calculation to (%.2f, %.2f) failed: \"%s\"",
+    RCLCPP_WARN(get_logger(), "Plan calculation to (%.2f, %.2f) failed: \"%s\"",
       command->goal.position.x, command->goal.position.y, ex.what());
 
     // TODO(orduno): provide information about fail error to parent task,
     //               for example: couldn't get costmap update
     return TaskStatus::FAILED;
   } catch (...) {
-    RCLCPP_WARN(get_logger(), "SmartPlanner: Plan calculation failed");
+    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
@@ -150,7 +150,7 @@ SmartPlanner::computePathToPose(const nav2_tasks::ComputePathToPoseCommand::Shar
 }
 
 bool
-SmartPlanner::isPlannerOutOfDate()
+NavfnPlanner::isPlannerOutOfDate()
 {
   if (!planner_.get() || current_costmap_size_[0] != costmap_.metadata.size_x ||
     current_costmap_size_[1] != costmap_.metadata.size_y)
@@ -161,7 +161,7 @@ SmartPlanner::isPlannerOutOfDate()
 }
 
 bool
-SmartPlanner::makePlan(
+NavfnPlanner::makePlan(
   const geometry_msgs::msg::Pose & start,
   const geometry_msgs::msg::Pose & goal, double tolerance,
   nav2_msgs::msg::Path & plan)
@@ -174,14 +174,14 @@ SmartPlanner::makePlan(
   double wx = start.position.x;
   double wy = start.position.y;
 
-  RCLCPP_INFO(get_logger(), "SmartPlanner: Making plan from (%.2f,%.2f) to (%.2f,%.2f)",
+  RCLCPP_INFO(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;
   if (!worldToMap(wx, wy, mx, my)) {
     RCLCPP_WARN(
       get_logger(),
-      "SmartPlanner: Cannot create a plan: the robot's start position is off the global"
+      "Cannot create a plan: the robot's start position is off the global"
       " costmap. Planning will always fail, are you sure"
       " the robot has been properly localized?");
     return false;
@@ -204,7 +204,7 @@ SmartPlanner::makePlan(
 
   if (!worldToMap(wx, wy, mx, my)) {
     RCLCPP_WARN(get_logger(),
-      "SmartPlanner: The goal sent to the planner is off the global costmap."
+      "The goal sent to the planner is off the global costmap."
       " Planning will always fail to this goal.");
     return false;
   }
@@ -255,7 +255,7 @@ SmartPlanner::makePlan(
     } else {
       RCLCPP_ERROR(
         get_logger(),
-        "SmartPlanner: Failed to create a plan from potential when a legal"
+        "Failed to create a plan from potential when a legal"
         " potential was found. This shouldn't happen.");
     }
   }
@@ -264,7 +264,7 @@ SmartPlanner::makePlan(
 }
 
 void
-SmartPlanner::smoothApproachToGoal(
+NavfnPlanner::smoothApproachToGoal(
   const geometry_msgs::msg::Pose & goal,
   nav2_msgs::msg::Path & plan)
 {
@@ -285,7 +285,7 @@ SmartPlanner::smoothApproachToGoal(
 }
 
 bool
-SmartPlanner::computePotential(const geometry_msgs::msg::Point & world_point)
+NavfnPlanner::computePotential(const geometry_msgs::msg::Point & world_point)
 {
   // make sure to resize the underlying array that Navfn uses
   planner_->setNavArr(costmap_.metadata.size_x, costmap_.metadata.size_y);
@@ -319,7 +319,7 @@ SmartPlanner::computePotential(const geometry_msgs::msg::Point & world_point)
 }
 
 bool
-SmartPlanner::getPlanFromPotential(
+NavfnPlanner::getPlanFromPotential(
   const geometry_msgs::msg::Pose & goal,
   nav2_msgs::msg::Path & plan)
 {
@@ -376,7 +376,7 @@ SmartPlanner::getPlanFromPotential(
 }
 
 double
-SmartPlanner::getPointPotential(const geometry_msgs::msg::Point & world_point)
+NavfnPlanner::getPointPotential(const geometry_msgs::msg::Point & world_point)
 {
   unsigned int mx, my;
   if (!worldToMap(world_point.x, world_point.y, mx, my)) {
@@ -388,13 +388,13 @@ SmartPlanner::getPointPotential(const geometry_msgs::msg::Point & world_point)
 }
 
 bool
-SmartPlanner::validPointPotential(const geometry_msgs::msg::Point & world_point)
+NavfnPlanner::validPointPotential(const geometry_msgs::msg::Point & world_point)
 {
   return validPointPotential(world_point, default_tolerance_);
 }
 
 bool
-SmartPlanner::validPointPotential(
+NavfnPlanner::validPointPotential(
   const geometry_msgs::msg::Point & world_point, double tolerance)
 {
   double resolution = costmap_.metadata.resolution;
@@ -418,7 +418,7 @@ SmartPlanner::validPointPotential(
 }
 
 bool
-SmartPlanner::worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my)
+NavfnPlanner::worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my)
 {
   if (wx < costmap_.metadata.origin.position.x || wy < costmap_.metadata.origin.position.y) {
     RCLCPP_ERROR(get_logger(), "wordToMap failed: wx,wy: %f,%f, size_x,size_y: %d,%d", wx, wy,
@@ -442,14 +442,14 @@ SmartPlanner::worldToMap(double wx, double wy, unsigned int & mx, unsigned int &
 }
 
 void
-SmartPlanner::mapToWorld(double mx, double my, double & wx, double & wy)
+NavfnPlanner::mapToWorld(double mx, double my, double & wx, double & wy)
 {
   wx = costmap_.metadata.origin.position.x + mx * costmap_.metadata.resolution;
   wy = costmap_.metadata.origin.position.y + my * costmap_.metadata.resolution;
 }
 
 void
-SmartPlanner::clearRobotCell(unsigned int mx, unsigned int my)
+NavfnPlanner::clearRobotCell(unsigned int mx, unsigned int my)
 {
   // TODO(orduno): check usage of this function, might instead be a request to
   //               world_model / map server
@@ -458,7 +458,7 @@ SmartPlanner::clearRobotCell(unsigned int mx, unsigned int my)
 }
 
 void
-SmartPlanner::getCostmap(
+NavfnPlanner::getCostmap(
   nav2_msgs::msg::Costmap & costmap, const std::string /*layer*/,
   const std::chrono::milliseconds /*waitTime*/)
 {
@@ -473,7 +473,7 @@ SmartPlanner::getCostmap(
 }
 
 void
-SmartPlanner::printCostmap(const nav2_msgs::msg::Costmap & costmap)
+NavfnPlanner::printCostmap(const nav2_msgs::msg::Costmap & costmap)
 {
   std::cout << "Costmap" << std::endl;
   std::cout << "  size:       " <<
@@ -500,7 +500,7 @@ SmartPlanner::printCostmap(const nav2_msgs::msg::Costmap & costmap)
 }
 
 void
-SmartPlanner::publishEndpoints(const nav2_tasks::ComputePathToPoseCommand::SharedPtr & endpoints)
+NavfnPlanner::publishEndpoints(const nav2_tasks::ComputePathToPoseCommand::SharedPtr & endpoints)
 {
   visualization_msgs::msg::Marker marker;
 
@@ -567,7 +567,7 @@ SmartPlanner::publishEndpoints(const nav2_tasks::ComputePathToPoseCommand::Share
 }
 
 void
-SmartPlanner::publishPlan(const nav2_msgs::msg::Path & path)
+NavfnPlanner::publishPlan(const nav2_msgs::msg::Path & path)
 {
   // Publish as a nav1 path msg
   nav_msgs::msg::Path rviz_path;
@@ -584,4 +584,4 @@ SmartPlanner::publishPlan(const nav2_msgs::msg::Path & path)
   plan_publisher_->publish(rviz_path);
 }
 
-}  // namespace nav2_smart_planner
+}  // namespace nav2_navfn_planner

nav2_system_tests/src/planning/test_planner_launch.py

@@ -30,8 +30,8 @@ def main(argv=sys.argv[1:]):
     testExecutable = os.getenv('TEST_EXECUTABLE')
 
     ld = LaunchDescription([launch_ros.actions.Node(
-        package='nav2_smart_planner',
-        node_executable='smart_planner',
+        package='nav2_navfn_planner',
+        node_executable='navfn_planner',
         output='screen'),
     ])
 

navigation2/package.xml

@@ -17,7 +17,7 @@
   <exec_depend>nav2_bt_navigator</exec_depend>
   <exec_depend>nav2_costmap_2d</exec_depend>
   <exec_depend>nav2_world_model</exec_depend>
-  <exec_depend>nav2_smart_planner</exec_depend>
+  <exec_depend>nav2_navfn_planner</exec_depend>
   <exec_depend>nav2_dwb_controller</exec_depend>
   <exec_depend>nav2_dynamic_params</exec_depend>
   <exec_depend>nav2_map_server</exec_depend>