Nav2 Planner + Navfn plugin

nav2_behavior_tree/README.md

@@ -66,7 +66,7 @@ The nav2_behavior_tree package provides several navigation-specific nodes that a
 | BT Node   |      Type      |  Description |
 |----------|:-------------|------|
 | Backup |  Action | Invokes the BackUp ROS2 action server, which causes the robot to back up to a specific pose. This is used in nav2 Behavior Trees as a recovery behavior. The nav2_recoveries module implements the BackUp action server. | 
-| ComputePathToPose |    Action   | Invokes the ComputePathToPose ROS2 action server, which is implemented by the nav2_navfn_planner module. |
+| ComputePathToPose |    Action   | Invokes the ComputePathToPose ROS2 action server, which is implemented by the nav2_planner module. |
 | FollowPath | Action |Invokes the FollowPath ROS2 action server, which is implemented by the nav2_dwb_controller module. |
 | GoalReached | Condition | Checks the distance to the goal, if the distance to goal is less than the pre-defined threshold, the tree returns SUCCESS, otherwise it returns FAILURE. |
 | IsStuck | Condition | Determines if the robot is not progressing towards the goal. If the robot is stuck and not progressing, the condition returns SUCCESS, otherwise it returns FAILURE. |

nav2_behavior_tree/include/nav2_behavior_tree/compute_path_to_pose_action.hpp

@@ -19,6 +19,7 @@
 #include <string>
 
 #include "nav2_msgs/action/compute_path_to_pose.hpp"
+#include "nav_msgs/msg/path.h"
 #include "nav2_behavior_tree/bt_action_node.hpp"
 
 namespace nav2_behavior_tree
@@ -39,7 +40,7 @@ class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePa
 
   void on_success() override
   {
-    *(blackboard()->get<nav2_msgs::msg::Path::SharedPtr>("path")) = result_.result->path;
+    *(blackboard()->get<nav_msgs::msg::Path::SharedPtr>("path")) = result_.result->path;
 
     if (first_time_) {
       first_time_ = false;

nav2_behavior_tree/include/nav2_behavior_tree/follow_path_action.hpp

@@ -39,7 +39,7 @@ class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
 
   void on_tick() override
   {
-    goal_.path = *(blackboard()->get<nav2_msgs::msg::Path::SharedPtr>("path"));
+    goal_.path = *(blackboard()->get<nav_msgs::msg::Path::SharedPtr>("path"));
   }
 
   void on_loop_timeout() override
@@ -51,7 +51,7 @@ class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
 
       // Grab the new goal and set the flag so that we send the new goal to
       // the action server on the next loop iteration
-      goal_.path = *(blackboard()->get<nav2_msgs::msg::Path::SharedPtr>("path"));
+      goal_.path = *(blackboard()->get<nav_msgs::msg::Path::SharedPtr>("path"));
       goal_updated_ = true;
     }
   }

nav2_bringup/launch/nav2_bringup_launch.py

@@ -97,9 +97,9 @@ def generate_launch_description():
         parameters=[configured_params])
 
     start_planner_cmd = launch_ros.actions.Node(
-        package='nav2_navfn_planner',
-        node_executable='navfn_planner',
-        node_name='navfn_planner',
+        package='nav2_planner',
+        node_executable='planner_server',
+        node_name='planner_server',
         output='screen',
         parameters=[configured_params])
 

nav2_bringup/launch/nav2_navigation_launch.py

@@ -75,9 +75,9 @@ def generate_launch_description():
             parameters=[configured_params]),
 
         launch_ros.actions.Node(
-            package='nav2_navfn_planner',
-            node_executable='navfn_planner',
-            node_name='navfn_planner',
+            package='nav2_planner',
+            node_executable='planner_server',
+            node_name='planner_server',
             output='screen',
             parameters=[configured_params]),
 
@@ -103,7 +103,7 @@ def generate_launch_description():
             parameters=[{'use_sim_time': use_sim_time},
                         {'autostart': autostart},
                         {'node_names': ['dwb_controller',
-                                        'navfn_planner',
+                                        'planner_server',
                                         'bt_navigator']}]),
 
     ])

nav2_bringup/launch/nav2_params.yaml

@@ -153,7 +153,7 @@ lifecycle_manager:
     use_sim_time: True
     autostart: True
     node_names: ['map_server', 'amcl', 'dwb_controller',
-                 'navfn_planner', 'bt_navigator']
+                 'planner_server', 'bt_navigator']
 
 lifecycle_manager_service_client:
   ros__parameters:

nav2_bringup/launch/nav2_simulation_launch.py

@@ -148,8 +148,8 @@ def generate_launch_description():
     start_planner_cmd = launch.actions.ExecuteProcess(
         cmd=[
             os.path.join(
-                get_package_prefix('nav2_navfn_planner'),
-                'lib/nav2_navfn_planner/navfn_planner'),
+                get_package_prefix('nav2_planner'),
+                'lib/nav2_planner/planner_server'),
             '--ros-args', '--params-file', configured_params],
         cwd=[launch_dir], output='screen')
 

nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp

@@ -23,7 +23,7 @@
 #include "nav2_behavior_tree/behavior_tree_engine.hpp"
 #include "nav2_util/lifecycle_node.hpp"
 #include "nav2_msgs/action/navigate_to_pose.hpp"
-#include "nav2_msgs/msg/path.hpp"
+#include "nav_msgs/msg/path.hpp"
 #include "nav2_util/simple_action_server.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
 
@@ -67,7 +67,7 @@ class BtNavigator : public nav2_util::LifecycleNode
   std::shared_ptr<geometry_msgs::msg::PoseStamped> goal_;
 
   // The path (on the blackboard) to be returned from ComputePath and sent to the FollowPath task
-  std::shared_ptr<nav2_msgs::msg::Path> path_;
+  std::shared_ptr<nav_msgs::msg::Path> path_;
 
   // The XML string that defines the Behavior Tree to create
   std::string xml_string_;

nav2_bt_navigator/src/bt_navigator.cpp

@@ -69,14 +69,14 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
 
   // Create the path that will be returned from ComputePath and sent to FollowPath
   goal_ = std::make_shared<geometry_msgs::msg::PoseStamped>();
-  path_ = std::make_shared<nav2_msgs::msg::Path>();
+  path_ = std::make_shared<nav_msgs::msg::Path>();
 
   // Create the blackboard that will be shared by all of the nodes in the tree
   blackboard_ = BT::Blackboard::create<BT::BlackboardLocal>();
 
   // Put items on the blackboard
   blackboard_->set<geometry_msgs::msg::PoseStamped::SharedPtr>("goal", goal_);  // NOLINT
-  blackboard_->set<nav2_msgs::msg::Path::SharedPtr>("path", path_);  // NOLINT
+  blackboard_->set<nav_msgs::msg::Path::SharedPtr>("path", path_);  // NOLINT
   blackboard_->set<rclcpp::Node::SharedPtr>("node", client_node_);  // NOLINT
   blackboard_->set<std::chrono::milliseconds>("node_loop_timeout", std::chrono::milliseconds(10));  // NOLINT
   blackboard_->set<bool>("path_updated", false);  // NOLINT

nav2_core/CMakeLists.txt

@@ -6,9 +6,12 @@ find_package(nav2_costmap_2d REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
+find_package(nav2_util REQUIRED)
+find_package(nav_msgs REQUIRED)
 
 install(DIRECTORY include/
   DESTINATION include/
 )
 
+ament_export_include_directories(include)
 ament_package()

nav2_core/include/nav2_core/global_planner.hpp

@@ -12,15 +12,16 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef NAV2_CORE_GLOBAL_PLANNER_H_
-#define NAV2_CORE_GLOBAL_PLANNER_H_
+#ifndef NAV2_CORE__GLOBAL_PLANNER_H_
+#define NAV2_CORE__GLOBAL_PLANNER_H_
 
 #include <string>
-#include "rclcpp/rclcpp.h"
-#include "nav2_costmap_2d/nav2_costmap_2d.hpp"
-#include "tf2_ros/Buffer.h"
-#include "nav2_msgs/msg/path.h"
-#include "geometry_msgs/msg/pose_stamped.h"
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "tf2_ros/buffer.h"
+#include "nav_msgs/msg/path.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_util/lifecycle_node.hpp"
 
 namespace nav2_core
 {
@@ -32,6 +33,8 @@ namespace nav2_core
 class GlobalPlanner
 {
 public:
+  using Ptr = std::shared_ptr<GlobalPlanner>;
+
   /**
    * @brief Virtual destructor
    */
@@ -43,8 +46,8 @@ class GlobalPlanner
    * @param  tf A pointer to a TF buffer
    * @param  costmap_ros A pointer to the costmap
    */
-  virtual void configure(const rclcpp::LifecycleNode * parent,
-    const std::string & name, tf2_ros::Buffer * tf,
+  virtual void configure(nav2_util::LifecycleNode::SharedPtr parent,
+    std::string name, tf2_ros::Buffer * tf,
     nav2_costmap_2d::Costmap2DROS * costmap_ros) = 0;
 
   /**
@@ -62,22 +65,17 @@ class GlobalPlanner
    */
   virtual void deactivate() = 0;
 
-  /**
-   * @brief Method to shutdown planner and any threads involved in execution.
-   */
-  virtual void shutdown() = 0;
-
   /**
    * @brief Method create the plan from a starting and ending goal.
    * @param start The starting pose of the robot
    * @param goal  The goal pose of the robot
    * @return      The sequence of poses to get from start to goal, if any
    */
-  virtual nav2_msgs::Path createPlan(
+  virtual nav_msgs::msg::Path createPlan(
     const geometry_msgs::msg::PoseStamped & start,
     const geometry_msgs::msg::PoseStamped & goal) = 0;
 };
 
 }  // namespace nav2_core
 
-#endif  // NAV2_CORE_GLOBAL_PLANNER_H_
+#endif  // NAV2_CORE__GLOBAL_PLANNER_H_

nav2_core/include/nav2_core/recovery.hpp

@@ -16,8 +16,9 @@
 #define NAV2_CORE_RECOVERY_H_
 
 #include <string>
-#include "rclcpp/rclcpp.h"
-#include "tf2_ros/Buffer.h"
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "tf2_ros/buffer.h"
 
 namespace nav2_core
 {
@@ -40,7 +41,7 @@ class Recovery
    * @param  tf A pointer to a TF buffer
    * @param  costmap_ros A pointer to the costmap
    */
-  virtual void configure(const rclcpp::LifecycleNode * parent,
+  virtual void configure(const nav2_util::LifecycleNode * parent,
     const std::string & name, tf2_ros::Buffer * tf) = 0;
 
   /**
@@ -58,11 +59,6 @@ class Recovery
    */
   virtual void deactivate() = 0;
 
-  /**
-   * @brief Method to shutdown recovery and any threads involved in execution.
-   */
-  virtual void shutdown() = 0;
-
   /**
    * @brief Method Execute recovery behavior
    * @param  name The name of this planner

nav2_core/package.xml

@@ -13,6 +13,8 @@
   <depend>geometry_msgs</depend>
   <depend>std_msgs</depend>
   <depend>tf2_ros</depend>
+  <depend>nav2_util</depend>
+  <depend>nav_msgs</depend>
 
   <export>
     <build_type>ament_cmake</build_type>

nav2_dwb_controller/dwb_controller/include/dwb_controller/dwb_controller.hpp

@@ -56,7 +56,7 @@ class DwbController : public nav2_util::LifecycleNode
   // The action server callback
   void followPath();
 
-  void setPlannerPath(const nav2_msgs::msg::Path & path);
+  void setPlannerPath(const nav_msgs::msg::Path & path);
   void computeAndPublishVelocity();
   void updateGlobalPath();
   void publishVelocity(const nav_2d_msgs::msg::Twist2DStamped & velocity);

nav2_dwb_controller/dwb_controller/src/dwb_controller.cpp

@@ -206,7 +206,7 @@ void DwbController::followPath()
   action_server_->succeeded_current();
 }
 
-void DwbController::setPlannerPath(const nav2_msgs::msg::Path & path)
+void DwbController::setPlannerPath(const nav_msgs::msg::Path & path)
 {
   auto path2d = nav_2d_utils::pathToPath2D(path);
 
@@ -216,7 +216,7 @@ void DwbController::setPlannerPath(const nav2_msgs::msg::Path & path)
   auto end_pose = *(path.poses.end() - 1);
 
   RCLCPP_DEBUG(get_logger(), "Path end point is (%.2f, %.2f)",
-    end_pose.position.x, end_pose.position.y);
+    end_pose.pose.position.x, end_pose.pose.position.y);
 }
 
 void DwbController::computeAndPublishVelocity()

nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/conversions.hpp

@@ -45,7 +45,6 @@
 #include "nav_msgs/msg/path.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "tf2/convert.h"
-#include "nav2_msgs/msg/path.hpp"
 
 namespace nav_2d_utils
 {
@@ -60,7 +59,7 @@ geometry_msgs::msg::PoseStamped pose2DToPoseStamped(
   const geometry_msgs::msg::Pose2D & pose2d,
   const std::string & frame, const rclcpp::Time & stamp);
 nav_msgs::msg::Path posesToPath(const std::vector<geometry_msgs::msg::PoseStamped> & poses);
-nav_2d_msgs::msg::Path2D pathToPath2D(const nav2_msgs::msg::Path & path);
+nav_2d_msgs::msg::Path2D pathToPath2D(const nav_msgs::msg::Path & path);
 nav_msgs::msg::Path poses2DToPath(
   const std::vector<geometry_msgs::msg::Pose2D> & poses,
   const std::string & frame, const rclcpp::Time & stamp);

nav2_dwb_controller/nav_2d_utils/src/conversions.cpp

@@ -130,12 +130,12 @@ nav_msgs::msg::Path posesToPath(const std::vector<geometry_msgs::msg::PoseStampe
   return path;
 }
 
-nav_2d_msgs::msg::Path2D pathToPath2D(const nav2_msgs::msg::Path & path)
+nav_2d_msgs::msg::Path2D pathToPath2D(const nav_msgs::msg::Path & path)
 {
   nav_2d_msgs::msg::Path2D path2d;
   path2d.header = path.header;
   for (auto & pose : path.poses) {
-    path2d.poses.push_back(poseToPose2D(pose));
+    path2d.poses.push_back(poseToPose2D(pose.pose));
   }
   return path2d;
 }

nav2_lifecycle_manager/src/lifecycle_manager.cpp

@@ -38,7 +38,7 @@ LifecycleManager::LifecycleManager()
 
   // The default set of node names for the nav2 stack
   std::vector<std::string> default_node_names{"map_server", "amcl",
-    "navfn_planner", "dwb_controller", "bt_navigator"};
+    "planner_server", "dwb_controller", "bt_navigator"};
 
   // The list of names is parameterized, allowing this module to be used with a different set
   // of nodes

nav2_msgs/CMakeLists.txt

@@ -4,6 +4,7 @@ project(nav2_msgs)
 find_package(ament_cmake REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(builtin_interfaces REQUIRED)
+find_package(nav_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 find_package(std_msgs REQUIRED)
@@ -14,7 +15,6 @@ nav2_package()
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/Costmap.msg"
   "msg/CostmapMetaData.msg"
-  "msg/Path.msg"
   "msg/VoxelGrid.msg"
   "srv/GetCostmap.srv"
   "srv/ClearCostmapExceptRegion.srv"
@@ -28,7 +28,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "action/Spin.action"
   "action/DummyRecovery.action"
   "action/RandomCrawl.action"
-  DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs
+  DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs nav_msgs
 )
 
 ament_export_dependencies(rosidl_default_runtime)

nav2_msgs/action/ComputePathToPose.action

@@ -2,6 +2,6 @@
 geometry_msgs/PoseStamped pose
 ---
 #result definition
-nav2_msgs/Path path
+nav_msgs/Path path
 ---
 #feedback

nav2_msgs/action/FollowPath.action

@@ -1,5 +1,5 @@
 #goal definition
-nav2_msgs/Path path
+nav_msgs/Path path
 ---
 #result definition
 std_msgs/Empty result

nav2_msgs/package.xml

@@ -18,6 +18,7 @@
   <depend>rosidl_default_generators</depend>
   <depend>geometry_msgs</depend>
   <depend>action_msgs</depend>
+  <depend>nav_msgs</depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>