diff --git a/.travis.yml b/.travis.yml index 39a847d4b35da72e9012ddc9917c65da35124f74..d2186b46fb2d1bad6692bcf44338e55e7ae6dba3 100644 --- a/.travis.yml +++ b/.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" diff --git a/doc/design/ROS_COMPARISON.md b/doc/design/ROS_COMPARISON.md index a5e964ff201d99fbd4d0740f56de0de05bb745b8..a81cf83f93d779d3088eaa742911b3b5120e6f5e 100644 --- a/doc/design/ROS_COMPARISON.md +++ b/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)  @@ -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*. diff --git a/nav2_bringup/launch/core.launch.py b/nav2_bringup/launch/core.launch.py index 696d762e3645dc195a4111865385fb43e22696f9..1cdc2ae91fa6ebe841cfdb0da9b089869086f628 100755 --- a/nav2_bringup/launch/core.launch.py +++ b/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 = [ diff --git a/nav2_bringup/launch/nav2_bringup_launch.py b/nav2_bringup/launch/nav2_bringup_launch.py index 72d9fcc3d3982a6b1a356f6379ed03d74b448a29..4e4de098b092e5393ad8653fd6df717408e6eddc 100644 --- a/nav2_bringup/launch/nav2_bringup_launch.py +++ b/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}]), diff --git a/nav2_smart_planner/CHANGELOG.rst b/nav2_navfn_planner/CHANGELOG.rst similarity index 100% rename from nav2_smart_planner/CHANGELOG.rst rename to nav2_navfn_planner/CHANGELOG.rst diff --git a/nav2_smart_planner/CMakeLists.txt b/nav2_navfn_planner/CMakeLists.txt similarity index 91% rename from nav2_smart_planner/CMakeLists.txt rename to nav2_navfn_planner/CMakeLists.txt index d59fd0e2f686d45290245afa514a351b1382052e..9d850f9beec26a63e0fb4593c6dce35c4c69aba2 100644 --- a/nav2_smart_planner/CMakeLists.txt +++ b/nav2_navfn_planner/CMakeLists.txt @@ -1,5 +1,5 @@ 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 ) diff --git a/nav2_smart_planner/README.md b/nav2_navfn_planner/README.md similarity index 93% rename from nav2_smart_planner/README.md rename to nav2_navfn_planner/README.md index 794c9b656770709053fefc6114c95c0d6412ea92..9a7a3968460198de82c11674c836abf53f99f41b 100644 --- a/nav2_smart_planner/README.md +++ b/nav2_navfn_planner/README.md @@ -1,11 +1,11 @@ -# 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 diff --git a/nav2_smart_planner/include/nav2_smart_planner/navfn.hpp b/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp similarity index 98% rename from nav2_smart_planner/include/nav2_smart_planner/navfn.hpp rename to nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp index b29323db871ca571ac1e3c860f98dfc0e5c48e82..e527786f110efe4088a409956a3720f88f72d7a8 100644 --- a/nav2_smart_planner/include/nav2_smart_planner/navfn.hpp +++ b/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_ diff --git a/nav2_smart_planner/include/nav2_smart_planner/smart_planner.hpp b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp similarity index 93% rename from nav2_smart_planner/include/nav2_smart_planner/smart_planner.hpp rename to nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp index 9145d9ed8f8a882e45ccbb90020bc83452d42a24..11a7d20559df1163f11905d7e426c215468cc69b 100644 --- a/nav2_smart_planner/include/nav2_smart_planner/smart_planner.hpp +++ b/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_ diff --git a/nav2_smart_planner/package.xml b/nav2_navfn_planner/package.xml similarity index 97% rename from nav2_smart_planner/package.xml rename to nav2_navfn_planner/package.xml index d7836e8cb06a25f1da8b3ab816177a31eb001bda..eeddcd293664d11f935cfb75321e0f141f8b3c66 100644 --- a/nav2_smart_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -1,7 +1,7 @@ <?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> diff --git a/nav2_smart_planner/src/main.cpp b/nav2_navfn_planner/src/main.cpp similarity index 87% rename from nav2_smart_planner/src/main.cpp rename to nav2_navfn_planner/src/main.cpp index 5051eb343bcd72b94bfbe2a18b7813ff67e53703..dc63bb75bb651a9e6a0c1a877f56457113ec1590 100644 --- a/nav2_smart_planner/src/main.cpp +++ b/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; diff --git a/nav2_smart_planner/src/navfn.cpp b/nav2_navfn_planner/src/navfn.cpp similarity index 99% rename from nav2_smart_planner/src/navfn.cpp rename to nav2_navfn_planner/src/navfn.cpp index 5c9cd9ae761a7d7ca97073360533b60f4867217f..734754047c98f3bce609fb1eff1499f9159d9411 100644 --- a/nav2_smart_planner/src/navfn.cpp +++ b/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 diff --git a/nav2_smart_planner/src/smart_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp similarity index 87% rename from nav2_smart_planner/src/smart_planner.cpp rename to nav2_navfn_planner/src/navfn_planner.cpp index a356929d3b1a5021d0221a1c388682833bcc5fc7..5d35a1446b0cdc08ce921ae29596b503eb280c42 100644 --- a/nav2_smart_planner/src/smart_planner.cpp +++ b/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 diff --git a/nav2_smart_planner/test/.gitignore b/nav2_navfn_planner/test/.gitignore similarity index 100% rename from nav2_smart_planner/test/.gitignore rename to nav2_navfn_planner/test/.gitignore diff --git a/nav2_system_tests/src/planning/test_planner_launch.py b/nav2_system_tests/src/planning/test_planner_launch.py index 96b44de4c3892cb4aaaa4344e7af22147afdeb57..89389075b430139894ca5d81f80af936647bce00 100755 --- a/nav2_system_tests/src/planning/test_planner_launch.py +++ b/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'), ]) diff --git a/navigation2/package.xml b/navigation2/package.xml index 29dd4a62960bbbd2b88f1e7ff415fc7e8ccb1a49..6e941435a734e36b1e9af9b0e0bd51423e7985d5 100644 --- a/navigation2/package.xml +++ b/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>