diff --git a/nav2_behavior_tree/README.md b/nav2_behavior_tree/README.md index aae588d8d61..a8061bf8de9 100644 --- a/nav2_behavior_tree/README.md +++ b/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. | diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/compute_path_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/compute_path_to_pose_action.hpp index c4c29ed04cd..32483fd4f2a 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/compute_path_to_pose_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/compute_path_to_pose_action.hpp @@ -19,6 +19,7 @@ #include #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 BtActionNodeget("path")) = result_.result->path; + *(blackboard()->get("path")) = result_.result->path; if (first_time_) { first_time_ = false; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/follow_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/follow_path_action.hpp index 923eddc8b12..c0e35a557f5 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/follow_path_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/follow_path_action.hpp @@ -39,7 +39,7 @@ class FollowPathAction : public BtActionNode void on_tick() override { - goal_.path = *(blackboard()->get("path")); + goal_.path = *(blackboard()->get("path")); } void on_loop_timeout() override @@ -51,7 +51,7 @@ class FollowPathAction : public BtActionNode // 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("path")); + goal_.path = *(blackboard()->get("path")); goal_updated_ = true; } } diff --git a/nav2_bringup/launch/nav2_bringup_launch.py b/nav2_bringup/launch/nav2_bringup_launch.py index 0ef4338565a..285bd94837e 100644 --- a/nav2_bringup/launch/nav2_bringup_launch.py +++ b/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]) diff --git a/nav2_bringup/launch/nav2_navigation_launch.py b/nav2_bringup/launch/nav2_navigation_launch.py index 777be0c5cbe..3be794f0f11 100644 --- a/nav2_bringup/launch/nav2_navigation_launch.py +++ b/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']}]), ]) diff --git a/nav2_bringup/launch/nav2_params.yaml b/nav2_bringup/launch/nav2_params.yaml index fcba4158177..bd19d8632ff 100644 --- a/nav2_bringup/launch/nav2_params.yaml +++ b/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: diff --git a/nav2_bringup/launch/nav2_simulation_launch.py b/nav2_bringup/launch/nav2_simulation_launch.py index 010d5d42236..9f3045136e4 100644 --- a/nav2_bringup/launch/nav2_simulation_launch.py +++ b/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') diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp index ee1abeff741..a468f937017 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp +++ b/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 goal_; // The path (on the blackboard) to be returned from ComputePath and sent to the FollowPath task - std::shared_ptr path_; + std::shared_ptr path_; // The XML string that defines the Behavior Tree to create std::string xml_string_; diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index eb4e54212f3..ff03494f3fe 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/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(); - path_ = std::make_shared(); + path_ = std::make_shared(); // Create the blackboard that will be shared by all of the nodes in the tree blackboard_ = BT::Blackboard::create(); // Put items on the blackboard blackboard_->set("goal", goal_); // NOLINT - blackboard_->set("path", path_); // NOLINT + blackboard_->set("path", path_); // NOLINT blackboard_->set("node", client_node_); // NOLINT blackboard_->set("node_loop_timeout", std::chrono::milliseconds(10)); // NOLINT blackboard_->set("path_updated", false); // NOLINT diff --git a/nav2_core/CMakeLists.txt b/nav2_core/CMakeLists.txt index c8d15ac9d32..c0287620464 100644 --- a/nav2_core/CMakeLists.txt +++ b/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() diff --git a/nav2_core/include/nav2_core/global_planner.hpp b/nav2_core/include/nav2_core/global_planner.hpp index 810433ef6a8..50aa406295f 100644 --- a/nav2_core/include/nav2_core/global_planner.hpp +++ b/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 -#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; + /** * @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_ diff --git a/nav2_core/include/nav2_core/recovery.hpp b/nav2_core/include/nav2_core/recovery.hpp index e097a0268a9..9c5a558c5fc 100644 --- a/nav2_core/include/nav2_core/recovery.hpp +++ b/nav2_core/include/nav2_core/recovery.hpp @@ -16,8 +16,9 @@ #define NAV2_CORE_RECOVERY_H_ #include -#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 diff --git a/nav2_core/package.xml b/nav2_core/package.xml index a1f54f05177..4ebe23e0817 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -13,6 +13,8 @@ geometry_msgs std_msgs tf2_ros + nav2_util + nav_msgs ament_cmake diff --git a/nav2_dwb_controller/dwb_controller/include/dwb_controller/dwb_controller.hpp b/nav2_dwb_controller/dwb_controller/include/dwb_controller/dwb_controller.hpp index 25fa490191a..c67d86a08ef 100644 --- a/nav2_dwb_controller/dwb_controller/include/dwb_controller/dwb_controller.hpp +++ b/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); diff --git a/nav2_dwb_controller/dwb_controller/src/dwb_controller.cpp b/nav2_dwb_controller/dwb_controller/src/dwb_controller.cpp index bde721c6b49..ad057ca0d60 100644 --- a/nav2_dwb_controller/dwb_controller/src/dwb_controller.cpp +++ b/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() diff --git a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/conversions.hpp b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/conversions.hpp index 5dcac8c353d..618ef0e17df 100644 --- a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/conversions.hpp +++ b/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 & 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 & poses, const std::string & frame, const rclcpp::Time & stamp); diff --git a/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp b/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp index 8a6da00676b..b21f05bf8e0 100644 --- a/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp +++ b/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp @@ -130,12 +130,12 @@ nav_msgs::msg::Path posesToPath(const std::vector 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 diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 2061a6f6d43..6e07807da97 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/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) diff --git a/nav2_msgs/action/ComputePathToPose.action b/nav2_msgs/action/ComputePathToPose.action index 4af3b3da134..0a6b62ee4a6 100644 --- a/nav2_msgs/action/ComputePathToPose.action +++ b/nav2_msgs/action/ComputePathToPose.action @@ -2,6 +2,6 @@ geometry_msgs/PoseStamped pose --- #result definition -nav2_msgs/Path path +nav_msgs/Path path --- #feedback diff --git a/nav2_msgs/action/FollowPath.action b/nav2_msgs/action/FollowPath.action index 33487826176..b73cbf3ae24 100644 --- a/nav2_msgs/action/FollowPath.action +++ b/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 diff --git a/nav2_msgs/msg/Path.msg b/nav2_msgs/msg/Path.msg deleted file mode 100644 index 5a096b91562..00000000000 --- a/nav2_msgs/msg/Path.msg +++ /dev/null @@ -1,4 +0,0 @@ -std_msgs/Header header - -# An array of poses that represents a Path for a robot to follow -geometry_msgs/Pose[] poses \ No newline at end of file diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index 1b3c725d47d..76217cce580 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -18,6 +18,7 @@ rosidl_default_generators geometry_msgs action_msgs + nav_msgs ament_lint_common ament_lint_auto diff --git a/nav2_navfn_planner/CMakeLists.txt b/nav2_navfn_planner/CMakeLists.txt index 4949da40807..57fb09bdc9f 100644 --- a/nav2_navfn_planner/CMakeLists.txt +++ b/nav2_navfn_planner/CMakeLists.txt @@ -9,6 +9,7 @@ find_package(rclcpp_lifecycle REQUIRED) find_package(std_msgs REQUIRED) find_package(visualization_msgs REQUIRED) find_package(nav2_util REQUIRED) +find_package(nav2_core REQUIRED) find_package(nav2_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(geometry_msgs REQUIRED) @@ -22,8 +23,7 @@ include_directories( include ) -set(executable_name navfn_planner) -set(library_name ${executable_name}_core) +set(library_name nav2_navfn_planner) set(dependencies rclcpp @@ -38,6 +38,7 @@ set(dependencies builtin_interfaces tf2_ros nav2_costmap_2d + nav2_core ) add_library(${library_name} SHARED @@ -49,17 +50,9 @@ ament_target_dependencies(${library_name} ${dependencies} ) -add_executable(${executable_name} - src/main.cpp -) - -target_link_libraries(${executable_name} ${library_name}) +pluginlib_export_plugin_description_file(nav2_core global_planner_plugin.xml) -ament_target_dependencies(${executable_name} - ${dependencies} -) - -install(TARGETS ${executable_name} ${library_name} +install(TARGETS ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME} @@ -69,6 +62,10 @@ install(DIRECTORY include/ DESTINATION include/ ) +install(FILES global_planner_plugin.xml + DESTINATION share +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() diff --git a/nav2_navfn_planner/README.md b/nav2_navfn_planner/README.md index 150a13b0ef5..90bb5467b36 100644 --- a/nav2_navfn_planner/README.md +++ b/nav2_navfn_planner/README.md @@ -1,8 +1,8 @@ # Navfn Planner -The NavfnPlanner is a [planning module](../doc/requirements/requirements.md) that implements the `nav2_behavior_tree::ComputePathToPose` interface. +The NavfnPlanner is a plugin for the Nav2 Planner server. -A planning module implementing the `nav2_behavior_tree::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). +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, 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). @@ -14,15 +14,6 @@ In A* mode (`use_astar = true`) A*'s search algorithm is not guaranteed to find The Navfn planner assumes a circular robot and operates on a costmap. -## Task Interface - -The [Navigation System]((../doc/requirements/requirements.md)) is composed of three tasks: NavigateToPose, ComputePathToPose and FollowPathToPose. - -The DijkstraPlanner implements the Task Server interface for ComputePathToPose, specifically derives from `nav2_behavior_tree::ComputePathToPoseTaskServer`. The client to DijkstraPlanner is 'NavigateToPoseTask', which periodically sends requests to the path planner. - -![alt text](../doc/design/NavigationSystemTasks.png "Navigation Tasks") - ## Next Steps -- Refactor Navfn. Currently difficult to modify/extend. [Issue #244](http://github.com/ros-planning/navigation2/issues/224) - Implement additional planners based on optimal control, potential field or other graph search algorithms that require transformation of the world model to other representations (topological, tree map, etc.) to confirm sufficient generalization. [Issue #225](http://github.com/ros-planning/navigation2/issues/225) - Implement planners for non-holonomic robots. [Issue #225](http://github.com/ros-planning/navigation2/issues/225) diff --git a/nav2_navfn_planner/global_planner_plugin.xml b/nav2_navfn_planner/global_planner_plugin.xml new file mode 100644 index 00000000000..25bb27e09f4 --- /dev/null +++ b/nav2_navfn_planner/global_planner_plugin.xml @@ -0,0 +1,5 @@ + + + + + diff --git a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp index fe318cd34be..1e5b0d10b1d 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp @@ -24,50 +24,49 @@ #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "nav2_util/lifecycle_node.hpp" -#include "nav2_msgs/action/compute_path_to_pose.hpp" -#include "nav2_msgs/msg/costmap.hpp" -#include "nav2_msgs/msg/path.hpp" +#include "nav2_core/global_planner.hpp" +#include "nav_msgs/msg/path.hpp" #include "nav2_navfn_planner/navfn.hpp" #include "nav2_util/robot_utils.hpp" -#include "nav2_util/simple_action_server.hpp" -#include "nav_msgs/msg/path.hpp" -#include "visualization_msgs/msg/marker.hpp" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/create_timer_ros.h" +#include "nav2_util/lifecycle_node.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" namespace nav2_navfn_planner { -class NavfnPlanner : public nav2_util::LifecycleNode +class NavfnPlanner : public nav2_core::GlobalPlanner { public: NavfnPlanner(); ~NavfnPlanner(); -protected: - // Implement the lifecycle interface - nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; - nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; - nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; - nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; - nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; - nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; + // plugin configure + void configure( + nav2_util::LifecycleNode::SharedPtr parent, + std::string name, tf2_ros::Buffer * tf, + nav2_costmap_2d::Costmap2DROS * costmap_ros) override; + + // plugin cleanup + void cleanup() override; - using ActionServer = nav2_util::SimpleActionServer; + // plugin activate + void activate() override; - // Our action server implements the ComputePathToPose action - std::unique_ptr action_server_; + // plugin deactivate + void deactivate() override; - // The action server callback - void computePathToPose(); + // plugin create path + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) override; + +protected: // Compute a plan given start and goal poses, provided in global world frame. bool makePlan( const geometry_msgs::msg::Pose & start, const geometry_msgs::msg::Pose & goal, double tolerance, - nav2_msgs::msg::Path & plan); + nav_msgs::msg::Path & plan); // Compute the navigation function given a seed point in the world to start from bool computePotential(const geometry_msgs::msg::Point & world_point); @@ -75,12 +74,12 @@ class NavfnPlanner : public nav2_util::LifecycleNode // Compute a plan to a goal from a potential - must call computePotential first bool getPlanFromPotential( const geometry_msgs::msg::Pose & goal, - nav2_msgs::msg::Path & plan); + nav_msgs::msg::Path & plan); // Remove artifacts at the end of the path - originated from planning on a discretized world void smoothApproachToGoal( const geometry_msgs::msg::Pose & goal, - nav2_msgs::msg::Path & plan); + nav_msgs::msg::Path & plan); // Compute the potential, or navigation cost, at a given point in the world // - must call computePotential first @@ -111,12 +110,6 @@ class NavfnPlanner : public nav2_util::LifecycleNode // Set the corresponding cell cost to be free space void clearRobotCell(unsigned int mx, unsigned int my); - // Print costmap to terminal - void printCostmap(const nav2_msgs::msg::Costmap & costmap); - - // Publish a path for visualization purposes - void publishPlan(const nav2_msgs::msg::Path & path); - // Determine if a new planner object should be made bool isPlannerOutOfDate(); @@ -124,23 +117,19 @@ class NavfnPlanner : public nav2_util::LifecycleNode std::unique_ptr planner_; // TF buffer - std::shared_ptr tf_; - std::shared_ptr tf_listener_; + tf2_ros::Buffer * tf_; + + // node ptr + nav2_util::LifecycleNode::SharedPtr node_; // Global Costmap - std::shared_ptr costmap_ros_; nav2_costmap_2d::Costmap2D * costmap_; - std::unique_ptr costmap_thread_; - std::unique_ptr costmap_executor_; - - // Publishers for the path - rclcpp_lifecycle::LifecyclePublisher::SharedPtr plan_publisher_; // The global frame of the costmap - const std::string global_frame_{"map"}; + std::string global_frame_, name_; // Whether or not the planner should be allowed to plan through unknown space - const bool allow_unknown_{true}; + bool allow_unknown_; // If the goal is obstructed, the tolerance specifies how many meters the planner // can relax the constraint in x and y before failing diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index 917695aa2d4..d37ae7d3fd9 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -23,6 +23,7 @@ nav2_common tf2_ros nav2_costmap_2d + nav2_core ament_lint_common ament_lint_auto @@ -33,5 +34,6 @@ ament_cmake + diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index fb683fbe693..5f327a5ce4c 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -31,15 +31,9 @@ #include #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 "nav2_navfn_planner/navfn.hpp" #include "nav2_util/costmap.hpp" #include "nav2_util/node_utils.hpp" -#include "nav_msgs/msg/path.hpp" -#include "visualization_msgs/msg/marker.hpp" #include "nav2_costmap_2d/cost_values.hpp" using namespace std::chrono_literals; @@ -48,215 +42,79 @@ namespace nav2_navfn_planner { NavfnPlanner::NavfnPlanner() -: nav2_util::LifecycleNode("navfn_planner", "", true), costmap_(nullptr) +: tf_(nullptr), costmap_(nullptr) { - RCLCPP_INFO(get_logger(), "Creating"); - - // Declare this node's parameters - declare_parameter("tolerance", rclcpp::ParameterValue(0.0)); - declare_parameter("use_astar", rclcpp::ParameterValue(false)); - - // Setup the global costmap - costmap_ros_ = std::make_shared( - "global_costmap", nav2_util::add_namespaces(std::string{get_namespace()}, - "global_costmap")); - costmap_executor_ = std::make_unique(); - - // Launch a thread to run the costmap node - costmap_thread_ = std::make_unique( - [&](rclcpp_lifecycle::LifecycleNode::SharedPtr node) - { - // TODO(mjeronimo): Once Brian pushes his change upstream to rlcpp executors, we'll - // be able to provide our own executor to spin(), reducing this to a single line - costmap_executor_->add_node(node->get_node_base_interface()); - costmap_executor_->spin(); - costmap_executor_->remove_node(node->get_node_base_interface()); - }, costmap_ros_); } NavfnPlanner::~NavfnPlanner() { - RCLCPP_INFO(get_logger(), "Destroying"); - costmap_executor_->cancel(); - costmap_thread_->join(); + RCLCPP_INFO(node_->get_logger(), "Destroying plugin %s", name_.c_str()); } -nav2_util::CallbackReturn -NavfnPlanner::on_configure(const rclcpp_lifecycle::State & state) +void +NavfnPlanner::configure( + nav2_util::LifecycleNode::SharedPtr parent, + std::string name, tf2_ros::Buffer * tf, + nav2_costmap_2d::Costmap2DROS * costmap_ros) { - RCLCPP_INFO(get_logger(), "Configuring"); - - // Initialize parameters - get_parameter("tolerance", tolerance_); - get_parameter("use_astar", use_astar_); - - costmap_ros_->on_configure(state); - costmap_ = costmap_ros_->getCostmap(); + node_ = parent; + tf_ = tf; + name_ = name; + costmap_ = costmap_ros->getCostmap(); + global_frame_ = costmap_ros->getGlobalFrameID(); - RCLCPP_DEBUG(get_logger(), "Costmap size: %d,%d", - costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY()); + RCLCPP_INFO(node_->get_logger(), "Configuring plugin %s", name_.c_str()); - tf_ = costmap_ros_->getTfBuffer(); - tf_listener_ = std::make_shared(*tf_); + // Initialize parameters + // Declare this plugin's parameters + node_->declare_parameter("tolerance", rclcpp::ParameterValue(0.0)); + node_->get_parameter("tolerance", tolerance_); + node_->declare_parameter("use_astar", rclcpp::ParameterValue(false)); + node_->get_parameter("use_astar", use_astar_); + node_->declare_parameter("allow_unknown", rclcpp::ParameterValue(true)); + node_->get_parameter("allow_unknown", allow_unknown_); // Create a planner based on the new costmap size - if (isPlannerOutOfDate()) { - planner_ = std::make_unique(costmap_->getSizeInCellsX(), - costmap_->getSizeInCellsY()); - } - - // Initialize pubs & subs - plan_publisher_ = create_publisher("plan", 1); - - // Create the action server that we implement with our navigateToPose method - action_server_ = std::make_unique(rclcpp_node_, "ComputePathToPose", - std::bind(&NavfnPlanner::computePathToPose, this)); - - return nav2_util::CallbackReturn::SUCCESS; + planner_ = std::make_unique(costmap_->getSizeInCellsX(), + costmap_->getSizeInCellsY()); } -nav2_util::CallbackReturn -NavfnPlanner::on_activate(const rclcpp_lifecycle::State & state) +void +NavfnPlanner::activate() { - RCLCPP_INFO(get_logger(), "Activating"); - - plan_publisher_->on_activate(); - action_server_->activate(); - costmap_ros_->on_activate(state); - - return nav2_util::CallbackReturn::SUCCESS; + RCLCPP_INFO(node_->get_logger(), "Activating plugin %s", name_.c_str()); } -nav2_util::CallbackReturn -NavfnPlanner::on_deactivate(const rclcpp_lifecycle::State & state) +void +NavfnPlanner::deactivate() { - RCLCPP_INFO(get_logger(), "Deactivating"); - - action_server_->deactivate(); - plan_publisher_->on_deactivate(); - costmap_ros_->on_deactivate(state); - - return nav2_util::CallbackReturn::SUCCESS; + RCLCPP_INFO(node_->get_logger(), "Deactivating plugin %s", name_.c_str()); } -nav2_util::CallbackReturn -NavfnPlanner::on_cleanup(const rclcpp_lifecycle::State & state) +void +NavfnPlanner::cleanup() { - RCLCPP_INFO(get_logger(), "Cleaning up"); - - action_server_.reset(); - plan_publisher_.reset(); + RCLCPP_INFO(node_->get_logger(), "Cleaning up plugin %s", name_.c_str()); planner_.reset(); - tf_listener_.reset(); - tf_.reset(); - costmap_ros_->on_cleanup(state); - costmap_ros_.reset(); - - return nav2_util::CallbackReturn::SUCCESS; -} - -nav2_util::CallbackReturn -NavfnPlanner::on_error(const rclcpp_lifecycle::State &) -{ - RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state"); - return nav2_util::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn -NavfnPlanner::on_shutdown(const rclcpp_lifecycle::State &) +nav_msgs::msg::Path NavfnPlanner::createPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) { - RCLCPP_INFO(get_logger(), "Shutting down"); - return nav2_util::CallbackReturn::SUCCESS; -} - -void -NavfnPlanner::computePathToPose() -{ - // Initialize the ComputePathToPose goal and result - auto goal = action_server_->get_current_goal(); - auto result = std::make_shared(); - - try { - if (action_server_ == nullptr) { - RCLCPP_DEBUG(get_logger(), "Action server unavailable. Stopping."); - return; - } - - if (!action_server_->is_server_active()) { - RCLCPP_DEBUG(get_logger(), "Action server is inactive. Stopping."); - return; - } - - if (action_server_->is_cancel_requested()) { - RCLCPP_INFO(get_logger(), "Goal was canceled. Canceling planning action."); - action_server_->terminate_goals(); - return; - } - - // Get the current costmap - RCLCPP_DEBUG(get_logger(), "Costmap size: %d,%d", - costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY()); - - geometry_msgs::msg::PoseStamped start; - if (!nav2_util::getCurrentPose(start, *tf_)) { - return; - } - - // Update planner based on the new costmap size - if (isPlannerOutOfDate()) { - planner_->setNavArr(costmap_->getSizeInCellsX(), - costmap_->getSizeInCellsY()); - } - - if (action_server_->is_preempt_requested()) { - RCLCPP_INFO(get_logger(), "Preempting the goal pose."); - goal = action_server_->accept_pending_goal(); - } - - 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.pose.position.x, goal->pose.pose.position.y); - - // Make the plan for the provided goal pose - bool foundPath = makePlan(start.pose, goal->pose.pose, tolerance_, result->path); + // Update planner based on the new costmap size + if (isPlannerOutOfDate()) { + planner_->setNavArr(costmap_->getSizeInCellsX(), + costmap_->getSizeInCellsY()); + } - if (!foundPath) { - RCLCPP_WARN(get_logger(), "Planning algorithm failed to generate a valid" - " path to (%.2f, %.2f)", goal->pose.pose.position.x, goal->pose.pose.position.y); - // TODO(orduno): define behavior if a preemption is available - action_server_->terminate_goals(); - return; - } + nav_msgs::msg::Path path; - RCLCPP_DEBUG(get_logger(), "Found valid path of size %u", result->path.poses.size()); - - // Publish the plan for visualization purposes - RCLCPP_DEBUG(get_logger(), "Publishing the valid path"); - publishPlan(result->path); - - // TODO(orduno): Enable potential visualization - - RCLCPP_DEBUG(get_logger(), - "Successfully computed a path to (%.2f, %.2f) with tolerance %.2f", - goal->pose.pose.position.x, goal->pose.pose.position.y, tolerance_); - action_server_->succeeded_current(result); - return; - } catch (std::exception & ex) { - RCLCPP_WARN(get_logger(), "Plan calculation to (%.2f, %.2f) failed: \"%s\"", - 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 - action_server_->terminate_goals(); - 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 - action_server_->terminate_goals(); - return; + if (!makePlan(start.pose, goal.pose, tolerance_, path)) { + RCLCPP_WARN(node_->get_logger(), "%s: failed to create plan with " + "tolerance %.2f.", name_.c_str(), tolerance_); } + return path; } bool @@ -275,7 +133,7 @@ bool NavfnPlanner::makePlan( const geometry_msgs::msg::Pose & start, const geometry_msgs::msg::Pose & goal, double tolerance, - nav2_msgs::msg::Path & plan) + nav_msgs::msg::Path & plan) { // clear the plan, just in case plan.poses.clear(); @@ -285,13 +143,13 @@ NavfnPlanner::makePlan( double wx = start.position.x; double wy = start.position.y; - RCLCPP_DEBUG(get_logger(), "Making plan from (%.2f,%.2f) to (%.2f,%.2f)", + RCLCPP_DEBUG(node_->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(), + node_->get_logger(), "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?"); @@ -315,7 +173,7 @@ NavfnPlanner::makePlan( wy = goal.position.y; if (!worldToMap(wx, wy, mx, my)) { - RCLCPP_WARN(get_logger(), + RCLCPP_WARN(node_->get_logger(), "The goal sent to the planner is off the global costmap." " Planning will always fail to this goal."); return false; @@ -366,7 +224,7 @@ NavfnPlanner::makePlan( smoothApproachToGoal(best_pose, plan); } else { RCLCPP_ERROR( - get_logger(), + node_->get_logger(), "Failed to create a plan from potential when a legal" " potential was found. This shouldn't happen."); } @@ -378,7 +236,7 @@ NavfnPlanner::makePlan( void NavfnPlanner::smoothApproachToGoal( const geometry_msgs::msg::Pose & goal, - nav2_msgs::msg::Path & plan) + nav_msgs::msg::Path & plan) { // Replace the last pose of the computed path if it's actually further away // to the second to last pose than the goal pose. @@ -386,12 +244,13 @@ NavfnPlanner::smoothApproachToGoal( auto second_to_last_pose = plan.poses.end()[-2]; auto last_pose = plan.poses.back(); if ( - squared_distance(last_pose, second_to_last_pose) > - squared_distance(goal, second_to_last_pose)) + squared_distance(last_pose.pose, second_to_last_pose.pose) > + squared_distance(goal, second_to_last_pose.pose)) { - plan.poses.back() = goal; + plan.poses.back().pose = goal; } else { - geometry_msgs::msg::Pose goal_copy = goal; + geometry_msgs::msg::PoseStamped goal_copy; + goal_copy.pose = goal; plan.poses.push_back(goal_copy); } } @@ -431,7 +290,7 @@ NavfnPlanner::computePotential(const geometry_msgs::msg::Point & world_point) bool NavfnPlanner::getPlanFromPotential( const geometry_msgs::msg::Pose & goal, - nav2_msgs::msg::Path & plan) + nav_msgs::msg::Path & plan) { // clear the plan, just in case plan.poses.clear(); @@ -444,7 +303,7 @@ NavfnPlanner::getPlanFromPotential( unsigned int mx, my; if (!worldToMap(wx, wy, mx, my)) { RCLCPP_WARN( - get_logger(), + node_->get_logger(), "The goal sent to the navfn planner is off the global costmap." " Planning will always fail to this goal."); return false; @@ -463,7 +322,7 @@ NavfnPlanner::getPlanFromPotential( float * y = planner_->getPathY(); int len = planner_->getPathLen(); - plan.header.stamp = this->now(); + plan.header.stamp = node_->now(); plan.header.frame_id = global_frame_; for (int i = len - 1; i >= 0; --i) { @@ -471,14 +330,14 @@ NavfnPlanner::getPlanFromPotential( double world_x, world_y; mapToWorld(x[i], y[i], world_x, world_y); - geometry_msgs::msg::Pose pose; - pose.position.x = world_x; - pose.position.y = world_y; - pose.position.z = 0.0; - pose.orientation.x = 0.0; - pose.orientation.y = 0.0; - pose.orientation.z = 0.0; - pose.orientation.w = 1.0; + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = world_x; + pose.pose.position.y = world_y; + pose.pose.position.z = 0.0; + pose.pose.orientation.x = 0.0; + pose.pose.orientation.y = 0.0; + pose.pose.orientation.z = 0.0; + pose.pose.orientation.w = 1.0; plan.poses.push_back(pose); } @@ -531,7 +390,8 @@ bool NavfnPlanner::worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my) { if (wx < costmap_->getOriginX() || wy < costmap_->getOriginY()) { - RCLCPP_ERROR(get_logger(), "wordToMap failed: wx,wy: %f,%f, size_x,size_y: %d,%d", wx, wy, + RCLCPP_ERROR(node_->get_logger(), "worldToMap failed: wx,wy: %f,%f, " + "size_x,size_y: %d,%d", wx, wy, costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY()); return false; } @@ -545,7 +405,7 @@ NavfnPlanner::worldToMap(double wx, double wy, unsigned int & mx, unsigned int & return true; } - RCLCPP_ERROR(get_logger(), "wordToMap failed: mx,my: %d,%d, size_x,size_y: %d,%d", mx, my, + RCLCPP_ERROR(node_->get_logger(), "worldToMap failed: mx,my: %d,%d, size_x,size_y: %d,%d", mx, my, costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY()); return false; @@ -566,49 +426,7 @@ NavfnPlanner::clearRobotCell(unsigned int mx, unsigned int my) costmap_->setCost(mx, my, nav2_costmap_2d::FREE_SPACE); } -void -NavfnPlanner::printCostmap(const nav2_msgs::msg::Costmap & costmap) -{ - std::cout << "Costmap" << std::endl; - std::cout << " size: " << - costmap.metadata.size_x << "," << costmap.metadata.size_x << std::endl; - std::cout << " origin: " << - costmap.metadata.origin.position.x << "," << costmap.metadata.origin.position.y << std::endl; - std::cout << " resolution: " << costmap.metadata.resolution << std::endl; - std::cout << " data: " << - "(" << costmap.data.size() << " cells)" << std::endl << " "; - - const char separator = ' '; - const int valueWidth = 4; - - unsigned int index = 0; - for (unsigned int h = 0; h < costmap.metadata.size_y; ++h) { - for (unsigned int w = 0; w < costmap.metadata.size_x; ++w) { - std::cout << std::left << std::setw(valueWidth) << std::setfill(separator) << - static_cast(costmap.data[index]); - index++; - } - std::cout << std::endl << " "; - } - std::cout << std::endl; -} - -void -NavfnPlanner::publishPlan(const nav2_msgs::msg::Path & path) -{ - // Publish as a nav1 path msg - nav_msgs::msg::Path rviz_path; - - rviz_path.header = path.header; - rviz_path.poses.resize(path.poses.size()); - - // Assuming path is already provided in world coordinates - for (unsigned int i = 0; i < path.poses.size(); i++) { - rviz_path.poses[i].header = path.header; - rviz_path.poses[i].pose = path.poses[i]; - } - - plan_publisher_->publish(rviz_path); -} - } // namespace nav2_navfn_planner + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_navfn_planner::NavfnPlanner, nav2_core::GlobalPlanner) diff --git a/nav2_planner/CHANGELOG.rst b/nav2_planner/CHANGELOG.rst new file mode 100644 index 00000000000..e69de29bb2d diff --git a/nav2_planner/CMakeLists.txt b/nav2_planner/CMakeLists.txt new file mode 100644 index 00000000000..8648152d5db --- /dev/null +++ b/nav2_planner/CMakeLists.txt @@ -0,0 +1,85 @@ +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(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 + 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} +) + +target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +add_executable(${executable_name} + src/main.cpp +) + +target_link_libraries(${executable_name} ${library_name}) + +ament_target_dependencies(${executable_name} + ${dependencies} +) + +install(TARGETS ${executable_name} ${library_name} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include/ +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_include_directories(include) +ament_export_libraries(${library_name}) +ament_export_dependencies(${dependencies}) +ament_package() diff --git a/nav2_planner/README.md b/nav2_planner/README.md new file mode 100644 index 00000000000..ec4bb7b460f --- /dev/null +++ b/nav2_planner/README.md @@ -0,0 +1,5 @@ +# Nav2 Planner + +The Nav2 planner is a [planning module](../doc/requirements/requirements.md) 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 planner plugin like NavFn to do the path generation. diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp new file mode 100644 index 00000000000..c295a1c9ebc --- /dev/null +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -0,0 +1,88 @@ +// 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 +#include +#include +#include + +#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/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" + +namespace nav2_planner +{ + +class PlannerServer : public nav2_util::LifecycleNode +{ +public: + PlannerServer(); + ~PlannerServer(); + +protected: + // Implement the lifecycle interface + nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; + nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; + nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; + nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; + nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; + + using ActionServer = nav2_util::SimpleActionServer; + + // Our action server implements the ComputePathToPose action + std::unique_ptr action_server_; + + // The action server callback + void computePathToPose(); + + // Publish a path for visualization purposes + void publishPlan(const nav_msgs::msg::Path & path); + + // Planner + nav2_core::GlobalPlanner::Ptr planner_; + pluginlib::ClassLoader gp_loader_; + std::string planner_plugin_name_; + + // TF buffer + std::shared_ptr tf_; + + // Global Costmap + std::shared_ptr costmap_ros_; + nav2_costmap_2d::Costmap2D * costmap_; + std::unique_ptr costmap_thread_; + rclcpp::executors::SingleThreadedExecutor costmap_executor_; + + // Publishers for the path + rclcpp_lifecycle::LifecyclePublisher::SharedPtr plan_publisher_; +}; + +} // namespace nav2_planner + +#endif // NAV2_PLANNER__PLANNER_SERVER_HPP_ diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml new file mode 100644 index 00000000000..0a7a776856e --- /dev/null +++ b/nav2_planner/package.xml @@ -0,0 +1,37 @@ + + + + nav2_planner + 0.2.4 + TODO + Steve Macenski + Apache-2.0 + + ament_cmake + + rclcpp + rclcpp_action + rclcpp_lifecycle + visualization_msgs + nav2_util + nav2_msgs + nav_msgs + geometry_msgs + builtin_interfaces + nav2_common + tf2_ros + nav2_costmap_2d + pluginlib + nav2_core + + ament_lint_common + ament_lint_auto + ament_cmake_gtest + ament_cmake_pytest + launch + launch_testing + + + ament_cmake + + diff --git a/nav2_navfn_planner/src/main.cpp b/nav2_planner/src/main.cpp similarity index 83% rename from nav2_navfn_planner/src/main.cpp rename to nav2_planner/src/main.cpp index dfab078595a..987db78ccf5 100644 --- a/nav2_navfn_planner/src/main.cpp +++ b/nav2_planner/src/main.cpp @@ -1,4 +1,5 @@ // 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. @@ -14,13 +15,13 @@ #include -#include "nav2_navfn_planner/navfn_planner.hpp" +#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(); + auto node = std::make_shared(); rclcpp::spin(node->get_node_base_interface()); rclcpp::shutdown(); diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp new file mode 100644 index 00000000000..e74bb4f05f2 --- /dev/null +++ b/nav2_planner/src/planner_server.cpp @@ -0,0 +1,251 @@ +// 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 +#include +#include +#include +#include +#include +#include +#include + +#include "builtin_interfaces/msg/duration.hpp" +#include "nav2_util/costmap.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_costmap_2d/cost_values.hpp" + +#include "nav2_planner/planner_server.hpp" + +using namespace std::chrono_literals; + +namespace nav2_planner +{ + +PlannerServer::PlannerServer() +: nav2_util::LifecycleNode("nav2_planner", "", true), + gp_loader_("nav2_core", "nav2_core::GlobalPlanner"), costmap_(nullptr) +{ + RCLCPP_INFO(get_logger(), "Creating"); + + // Declare this node's parameters + declare_parameter("planner_plugin", "nav2_navfn_planner/NavfnPlanner"); + + // Setup the global costmap + costmap_ros_ = std::make_shared( + "global_costmap", nav2_util::add_namespaces(std::string{get_namespace()}, + "global_costmap")); + + // Launch a thread to run the costmap node + costmap_thread_ = std::make_unique( + [&](rclcpp_lifecycle::LifecycleNode::SharedPtr node) + { + // TODO(mjeronimo): Once Brian pushes his change upstream to rlcpp executors, we'll + costmap_executor_.add_node(node->get_node_base_interface()); + costmap_executor_.spin(); + costmap_executor_.remove_node(node->get_node_base_interface()); + }, costmap_ros_); +} + +PlannerServer::~PlannerServer() +{ + RCLCPP_INFO(get_logger(), "Destroying"); + costmap_executor_.cancel(); + costmap_thread_->join(); + planner_.reset(); +} + +nav2_util::CallbackReturn +PlannerServer::on_configure(const rclcpp_lifecycle::State & state) +{ + RCLCPP_INFO(get_logger(), "Configuring"); + + costmap_ros_->on_configure(state); + costmap_ = costmap_ros_->getCostmap(); + + RCLCPP_DEBUG(get_logger(), "Costmap size: %d,%d", + costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY()); + + tf_ = costmap_ros_->getTfBuffer(); + + get_parameter("planner_plugin", planner_plugin_name_); + auto node = shared_from_this(); + + try { + planner_ = gp_loader_.createUniqueInstance(planner_plugin_name_); + RCLCPP_INFO(get_logger(), "Created global planner plugin %s", + planner_plugin_name_.c_str()); + planner_->configure(node, + gp_loader_.getName(planner_plugin_name_), tf_.get(), costmap_ros_.get()); + } catch (const pluginlib::PluginlibException & ex) { + RCLCPP_FATAL(get_logger(), "Failed to create global planner. Exception: %s", + ex.what()); + exit(-1); + } + + // Initialize pubs & subs + plan_publisher_ = create_publisher("plan", 1); + + // Create the action server that we implement with our navigateToPose method + action_server_ = std::make_unique(rclcpp_node_, "ComputePathToPose", + std::bind(&PlannerServer::computePathToPose, this)); + + 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_->activate(); + costmap_ros_->on_activate(state); + planner_->activate(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +PlannerServer::on_deactivate(const rclcpp_lifecycle::State & state) +{ + RCLCPP_INFO(get_logger(), "Deactivating"); + + action_server_->deactivate(); + plan_publisher_->on_deactivate(); + costmap_ros_->on_deactivate(state); + planner_->deactivate(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +PlannerServer::on_cleanup(const rclcpp_lifecycle::State & state) +{ + RCLCPP_INFO(get_logger(), "Cleaning up"); + + action_server_.reset(); + plan_publisher_.reset(); + tf_.reset(); + costmap_ros_->on_cleanup(state); + costmap_ros_.reset(); + planner_->cleanup(); + planner_.reset(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +PlannerServer::on_error(const rclcpp_lifecycle::State &) +{ + RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state"); + 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; +} + +void +PlannerServer::computePathToPose() +{ + // Initialize the ComputePathToPose goal and result + auto goal = action_server_->get_current_goal(); + auto result = std::make_shared(); + + try { + if (action_server_ == nullptr) { + RCLCPP_DEBUG(get_logger(), "Action server unavailable. Stopping."); + return; + } + + if (!action_server_->is_server_active()) { + RCLCPP_DEBUG(get_logger(), "Action server is inactive. Stopping."); + return; + } + + if (action_server_->is_cancel_requested()) { + RCLCPP_INFO(get_logger(), "Goal was canceled. Canceling planning action."); + action_server_->terminate_goals(); + return; + } + + geometry_msgs::msg::PoseStamped start; + if (!nav2_util::getCurrentPose(start, *tf_)) { + return; + } + + if (action_server_->is_preempt_requested()) { + RCLCPP_INFO(get_logger(), "Preempting the goal pose."); + goal = action_server_->accept_pending_goal(); + } + + 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.pose.position.x, goal->pose.pose.position.y); + + result->path = planner_->createPlan(start, goal->pose); + + if (result->path.poses.size() == 0) { + RCLCPP_WARN(get_logger(), "Planning algorithm %s failed to generate a valid" + " path to (%.2f, %.2f)", planner_plugin_name_.c_str(), + goal->pose.pose.position.x, goal->pose.pose.position.y); + // TODO(orduno): define behavior if a preemption is available + action_server_->terminate_goals(); + return; + } + + RCLCPP_DEBUG(get_logger(), + "Found valid path of size %u to (%.2f, %.2f)", + result->path.poses.size(), goal->pose.pose.position.x, + goal->pose.pose.position.y); + + // Publish the plan for visualization purposes + RCLCPP_DEBUG(get_logger(), "Publishing the valid path"); + publishPlan(result->path); + + action_server_->succeeded_current(result); + return; + } catch (std::exception & ex) { + RCLCPP_WARN(get_logger(), "%s plugin failed to plan calculation to (%.2f, %.2f): \"%s\"", + planner_plugin_name_.c_str(), 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 + action_server_->terminate_goals(); + return; + } catch (...) { + RCLCPP_WARN(get_logger(), "Plan calculation failed, " + "An unexpected error has occurred. The planner server" + " may not be able to continue operating correctly."); + + // TODO(orduno): provide information about the failure to the parent task, + // for example: couldn't get costmap update + action_server_->terminate_goals(); + return; + } +} + +void +PlannerServer::publishPlan(const nav_msgs::msg::Path & path) +{ + plan_publisher_->publish(path); +} + +} // namespace nav2_planner diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index fe831265596..2d7ce8f682b 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(nav2_amcl REQUIRED) find_package(nav2_lifecycle_manager REQUIRED) find_package(rclpy REQUIRED) find_package(nav2_navfn_planner REQUIRED) +find_package(nav2_planner REQUIRED) find_package(navigation2) nav2_package() @@ -34,6 +35,7 @@ set(dependencies std_msgs tf2_geometry_msgs rclpy + nav2_planner nav2_navfn_planner ) diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 99da8fe278b..b2f3821df0d 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -27,6 +27,7 @@ gazebo_ros_pkgs launch_ros launch_testing + nav2_planner launch_ros launch_testing @@ -49,6 +50,7 @@ navigation2 lcov robot_state_publisher + nav2_planner launch_ros launch_testing diff --git a/nav2_system_tests/src/planning/planner_tester.cpp b/nav2_system_tests/src/planning/planner_tester.cpp index ca32a2dd8ee..932b0c1007c 100644 --- a/nav2_system_tests/src/planning/planner_tester.cpp +++ b/nav2_system_tests/src/planning/planner_tester.cpp @@ -63,13 +63,14 @@ void PlannerTester::activate() }); // We start with a 10x10 grid with no obstacles + costmap_ = std::make_unique(this); loadSimpleCostmap(TestCostmap::open_space); startRobotTransform(); // The navfn wrapper auto state = rclcpp_lifecycle::State(); - planner_tester_ = std::make_unique(); + planner_tester_ = std::make_shared(); planner_tester_->onConfigure(state); publishRobotTransform(); planner_tester_->onActivate(state); @@ -208,8 +209,6 @@ void PlannerTester::loadSimpleCostmap(const TestCostmap & testCostmapType) RCLCPP_DEBUG(this->get_logger(), "Setting a new costmap with fake values"); } - costmap_ = std::make_unique(this); - costmap_->set_test_costmap(testCostmapType); costmap_set_ = true; @@ -405,12 +404,12 @@ bool PlannerTester::isCollisionFree(const ComputePathToPoseResult & path) for (auto pose : path.poses) { collisionFree = costmap_->is_free( - static_cast(std::round(pose.position.x)), - static_cast(std::round(pose.position.y))); + static_cast(std::round(pose.pose.position.x)), + static_cast(std::round(pose.pose.position.y))); if (!collisionFree) { RCLCPP_WARN(this->get_logger(), "Path has collision at (%.2f, %.2f)", - pose.position.x, pose.position.y); + pose.pose.position.x, pose.pose.position.y); printPath(path); return false; } @@ -443,10 +442,10 @@ bool PlannerTester::isWithinTolerance( auto path_end = path.poses.end()[-1]; if ( - path_start.position.x == robot_position.x && - path_start.position.y == robot_position.y && - path_end.position.x == goal.pose.position.x && - path_end.position.y == goal.pose.position.y) + path_start.pose.position.x == robot_position.x && + path_start.pose.position.y == robot_position.y && + path_end.pose.position.x == goal.pose.position.x && + path_end.pose.position.y == goal.pose.position.y) { RCLCPP_DEBUG(this->get_logger(), "Path has correct start and end points"); @@ -458,7 +457,8 @@ bool PlannerTester::isWithinTolerance( robot_position.x, robot_position.y, goal.pose.position.x, goal.pose.position.y); RCLCPP_DEBUG(this->get_logger(), "Computed path starts at (%.2f, %.2f) and ends at (%.2f, %.2f)", - path_start.position.x, path_start.position.y, path_end.position.x, path_end.position.y); + path_start.pose.position.x, path_start.pose.position.y, + path_end.pose.position.x, path_end.pose.position.y); return false; } @@ -470,8 +470,8 @@ void PlannerTester::printPath(const ComputePathToPoseResult & path) const for (auto pose : path.poses) { ss << " point #" << index << " with" << - " x: " << std::setprecision(3) << pose.position.x << - " y: " << std::setprecision(3) << pose.position.y << '\n'; + " x: " << std::setprecision(3) << pose.pose.position.x << + " y: " << std::setprecision(3) << pose.pose.position.y << '\n'; ++index; } diff --git a/nav2_system_tests/src/planning/planner_tester.hpp b/nav2_system_tests/src/planning/planner_tester.hpp index 5dfacacdf7d..de523f4c639 100644 --- a/nav2_system_tests/src/planning/planner_tester.hpp +++ b/nav2_system_tests/src/planning/planner_tester.hpp @@ -32,17 +32,17 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "tf2_msgs/msg/tf_message.hpp" -#include "nav2_navfn_planner/navfn_planner.hpp" +#include "nav2_planner/planner_server.hpp" #include "tf2_ros/transform_broadcaster.h" namespace nav2_system_tests { -class NavFnPlannerTester : public nav2_navfn_planner::NavfnPlanner +class NavFnPlannerTester : public nav2_planner::PlannerServer { public: NavFnPlannerTester() - : NavfnPlanner() + : PlannerServer() { } @@ -73,19 +73,14 @@ class NavFnPlannerTester : public nav2_navfn_planner::NavfnPlanner bool createPath( const geometry_msgs::msg::PoseStamped & goal, - nav2_msgs::msg::Path & path) + nav_msgs::msg::Path & path) { geometry_msgs::msg::PoseStamped start; if (!nav2_util::getCurrentPose(start, *tf_, "map", "base_link", 0.1)) { return false; } - - if (isPlannerOutOfDate()) { - planner_->setNavArr(costmap_->getSizeInCellsX(), - costmap_->getSizeInCellsY()); - } - - return makePlan(start.pose, goal.pose, tolerance_, path); + path = planner_->createPlan(start, goal); + return true; } void onCleanup(const rclcpp_lifecycle::State & state) @@ -115,7 +110,7 @@ class PlannerTester : public rclcpp::Node, public ::testing::Test { public: using ComputePathToPoseCommand = geometry_msgs::msg::PoseStamped; - using ComputePathToPoseResult = nav2_msgs::msg::Path; + using ComputePathToPoseResult = nav_msgs::msg::Path; PlannerTester(); ~PlannerTester(); @@ -169,7 +164,7 @@ class PlannerTester : public rclcpp::Node, public ::testing::Test std::unique_ptr costmap_; // The global planner - std::unique_ptr planner_tester_; + std::shared_ptr planner_tester_; // A thread for spinning the ROS node and the executor used std::unique_ptr spin_thread_; diff --git a/nav2_system_tests/src/planning/test_planner_costmaps_node.cpp b/nav2_system_tests/src/planning/test_planner_costmaps_node.cpp index 6d43a1c58c5..3b9bee0ed0f 100644 --- a/nav2_system_tests/src/planning/test_planner_costmaps_node.cpp +++ b/nav2_system_tests/src/planning/test_planner_costmaps_node.cpp @@ -26,7 +26,7 @@ using nav2_system_tests::PlannerTester; using nav2_util::TestCostmap; using ComputePathToPoseCommand = geometry_msgs::msg::PoseStamped; -using ComputePathToPoseResult = nav2_msgs::msg::Path; +using ComputePathToPoseResult = nav_msgs::msg::Path; // rclcpp::init can only be called once per process, so this needs to be a global variable class RclCppFixture @@ -40,8 +40,6 @@ RclCppFixture g_rclcppfixture; TEST_F(PlannerTester, testSimpleCostmaps) { - activate(); - std::vector costmaps = { TestCostmap::open_space, TestCostmap::bounded, @@ -53,6 +51,8 @@ TEST_F(PlannerTester, testSimpleCostmaps) ComputePathToPoseResult result; + activate(); + for (auto costmap : costmaps) { loadSimpleCostmap(costmap); EXPECT_EQ(true, defaultPlannerTest(result)); diff --git a/nav2_system_tests/src/planning/test_planner_random_node.cpp b/nav2_system_tests/src/planning/test_planner_random_node.cpp index 51c60ac5c7f..f37d6d810fd 100644 --- a/nav2_system_tests/src/planning/test_planner_random_node.cpp +++ b/nav2_system_tests/src/planning/test_planner_random_node.cpp @@ -26,7 +26,7 @@ using nav2_system_tests::PlannerTester; using nav2_util::TestCostmap; using ComputePathToPoseCommand = geometry_msgs::msg::PoseStamped; -using ComputePathToPoseResult = nav2_msgs::msg::Path; +using ComputePathToPoseResult = nav_msgs::msg::Path; // rclcpp::init can only be called once per process, so this needs to be a global variable class RclCppFixture diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py index 551ca84b0f9..b173e5f18a3 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/nav2_system_tests/src/system/test_system_launch.py @@ -82,9 +82,9 @@ def generate_launch_description(): parameters=[params_file]), 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=[{'use_sim_time': use_sim_time}, {'use_astar': astar}]), @@ -109,7 +109,7 @@ def generate_launch_description(): output='screen', parameters=[{'use_sim_time': use_sim_time}, {'node_names': ['map_server', 'amcl', - 'dwb_controller', 'navfn_planner', 'bt_navigator']}, + 'dwb_controller', 'planner_server', 'bt_navigator']}, {'autostart': True}]), ]) diff --git a/nav2_system_tests/src/updown/test_updown_launch.py b/nav2_system_tests/src/updown/test_updown_launch.py index b537b9e3ecb..15e322b39cc 100755 --- a/nav2_system_tests/src/updown/test_updown_launch.py +++ b/nav2_system_tests/src/updown/test_updown_launch.py @@ -90,8 +90,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', params_file], cwd=[launch_dir], output='screen') diff --git a/navigation2/package.xml b/navigation2/package.xml index 73deb1b94ce..3fb6fae27ec 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -21,6 +21,7 @@ nav2_lifecycle_manager nav2_map_server nav2_recoveries + nav2_planner nav2_msgs nav2_navfn_planner nav2_rviz_plugins