diff --git a/doc/design/ROS_COMPARISON.md b/doc/design/ROS_COMPARISON.md index a81cf83f93d779d3088eaa742911b3b5120e6f5e..c254481cfed8bea0ac5604ac8d0054bb98dcb18b 100644 --- a/doc/design/ROS_COMPARISON.md +++ b/doc/design/ROS_COMPARISON.md @@ -10,28 +10,24 @@ 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_bt_navigator](/nav2_bt_navigator/README.md) (replaces move_base) * [nav2_navfn_planner](/nav2_navfn_planner/README.md) (replaces global_planner) * [nav2_dwb_controller](/nav2_dwb_controller/README.md) (replaces local_planner)  -The *nav2_simple_navigator* replaces move_base at the top level, with a *Task* interface to call the global and local planners. +The *nav2_bt_navigator* replaces move_base at the top level, with a *Task* interface to call the global and local planners with a configurable tree-based action model. * Note: the *Task* interface is a temporary proxy for ROS2 *Actions* which are not yet implemented. When *Actions* become available, the planners will be called through ROS2 *Actions*. The reason for the change was to make it so that global and local planners would be *Action Servers* and could be replaced at launch or run time with other implementations providing the same *Action*. -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_bt_navigator* itself is also a *Task Server* and can also be replaced with other implementations. 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_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. +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_bt_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*. +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_bt_navigator*. All these changes make it possible to replace any of these nodes at launch/run time with any other algorithm that implements that same interface. **See each package README.md for more details** - - - - diff --git a/nav2_simple_navigator/CHANGELOG.rst b/nav2_simple_navigator/CHANGELOG.rst deleted file mode 100644 index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000 diff --git a/nav2_simple_navigator/CMakeLists.txt b/nav2_simple_navigator/CMakeLists.txt deleted file mode 100644 index 1b45d24164f34740429b3034917fe5c97ce9cafb..0000000000000000000000000000000000000000 --- a/nav2_simple_navigator/CMakeLists.txt +++ /dev/null @@ -1,63 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(nav2_simple_navigator) - -find_package(ament_cmake REQUIRED) -find_package(nav2_common REQUIRED) -find_package(rclcpp REQUIRED) -find_package(std_msgs REQUIRED) -find_package(nav2_tasks REQUIRED) -find_package(nav2_msgs REQUIRED) - -nav2_package() - -include_directories( - include -) - -set(executable_name simple_navigator) - -add_executable(${executable_name} - src/main.cpp -) - -set(library_name ${executable_name}_core) - -add_library(${library_name} SHARED - src/simple_navigator.cpp -) - -set(dependencies - rclcpp - std_msgs - nav2_tasks - nav2_msgs -) - -ament_target_dependencies(${executable_name} - ${dependencies} -) - -target_link_libraries(${executable_name} ${library_name}) - -ament_target_dependencies(${library_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_package() diff --git a/nav2_simple_navigator/README.md b/nav2_simple_navigator/README.md deleted file mode 100644 index 7f0eb5a61ea97b59ee513a873698225fff629c0a..0000000000000000000000000000000000000000 --- a/nav2_simple_navigator/README.md +++ /dev/null @@ -1,17 +0,0 @@ -# Simple Navigator - -The Simple Navigator module implements a `nav2_tasks::NavigateToPose` task server. As such, its responsibility is to navigate the robot to the specified pose. - -## Overview - -The Simple Navigator implements NavigateToPose using two sub-tasks, `ComputePathToPose` and `FollowPath`, which are, in turn, task servers that implement the `nav2_tasks::ComputePathToPose` and `nav2_tasks::FollowPath` interfaces. It is conceptually similar to the MoveBase in the ROS 1 Navigation stack in that it coordinates the global planner (ComputePathToPose) and the local planner (FollowPath). - -The Simple Navigator sits in the task hierarchy, as follows: - -<img src="https://github.com/ros-planning/navigation2/blob/master/nav2_tasks/doc/hierarchy.svg" width="400" title="Navigation Task Hiearchy"> - -The Simple Navigator module is meant to show how one can utilize (sub-)task clients in a direct way to implement a task. The BtNavigator module, an alternative implementation of NavigateToPose, is a drop-in replacement for the Simple Navigator and uses behavior trees to implement more complex behavior, including recovery. - -## Implementation - -In its [*execute* method](https://github.com/ros-planning/navigation2/blob/master/nav2_simple_navigator/src/simple_navigator.cpp), which directs the task server to begin its operation, the Simple Navigator invokes ComputePathToPose and FollowPath in sequence, managing the task clients for each directly. diff --git a/nav2_simple_navigator/include/nav2_simple_navigator/simple_navigator.hpp b/nav2_simple_navigator/include/nav2_simple_navigator/simple_navigator.hpp deleted file mode 100644 index 99e1761f6fa9f239f66ccde93514dbee7c60dad9..0000000000000000000000000000000000000000 --- a/nav2_simple_navigator/include/nav2_simple_navigator/simple_navigator.hpp +++ /dev/null @@ -1,44 +0,0 @@ -// Copyright (c) 2018 Intel Corporation -// -// 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_SIMPLE_NAVIGATOR__SIMPLE_NAVIGATOR_HPP_ -#define NAV2_SIMPLE_NAVIGATOR__SIMPLE_NAVIGATOR_HPP_ - -#include <string> -#include <memory> -#include "nav2_tasks/task_status.hpp" -#include "nav2_tasks/navigate_to_pose_task.hpp" -#include "nav2_tasks/compute_path_to_pose_task.hpp" -#include "nav2_tasks/follow_path_task.hpp" - -namespace nav2_simple_navigator -{ - -class SimpleNavigator : public rclcpp::Node -{ -public: - SimpleNavigator(); - ~SimpleNavigator(); - - nav2_tasks::TaskStatus navigateToPose(const nav2_tasks::NavigateToPoseCommand::SharedPtr command); - -private: - std::unique_ptr<nav2_tasks::ComputePathToPoseTaskClient> planner_client_; - std::unique_ptr<nav2_tasks::FollowPathTaskClient> controller_client_; - std::unique_ptr<nav2_tasks::NavigateToPoseTaskServer> task_server_; -}; - -} // namespace nav2_simple_navigator - -#endif // NAV2_SIMPLE_NAVIGATOR__SIMPLE_NAVIGATOR_HPP_ diff --git a/nav2_simple_navigator/package.xml b/nav2_simple_navigator/package.xml deleted file mode 100644 index ab30a08fbae021094b96661734c9a3ae13a218e8..0000000000000000000000000000000000000000 --- a/nav2_simple_navigator/package.xml +++ /dev/null @@ -1,31 +0,0 @@ -<?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_simple_navigator</name> - <version>0.1.5</version> - <description>TODO</description> - <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer> - <license>Apache License 2.0</license> - - <buildtool_depend>ament_cmake</buildtool_depend> - <build_depend>nav2_common</build_depend> - - <build_depend>rclcpp</build_depend> - <build_depend>nav2_tasks</build_depend> - <build_depend>nav2_msgs</build_depend> - - <exec_depend>rclcpp</exec_depend> - <exec_depend>nav2_tasks</exec_depend> - <exec_depend>nav2_msgs</exec_depend> - - <test_depend>ament_lint_common</test_depend> - <test_depend>ament_lint_auto</test_depend> - <test_depend>ament_cmake_gtest</test_depend> - <test_depend>ament_cmake_pytest</test_depend> - <test_depend>launch</test_depend> - <test_depend>launch_testing</test_depend> - - <export> - <build_type>ament_cmake</build_type> - </export> -</package> diff --git a/nav2_simple_navigator/src/main.cpp b/nav2_simple_navigator/src/main.cpp deleted file mode 100644 index a9801a484001cc0935512c619b2107996c2f2d93..0000000000000000000000000000000000000000 --- a/nav2_simple_navigator/src/main.cpp +++ /dev/null @@ -1,25 +0,0 @@ -// Copyright (c) 2018 Intel Corporation -// -// 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 <memory> -#include "rclcpp/rclcpp.hpp" -#include "nav2_simple_navigator/simple_navigator.hpp" - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared<nav2_simple_navigator::SimpleNavigator>()); - rclcpp::shutdown(); - return 0; -} diff --git a/nav2_simple_navigator/src/simple_navigator.cpp b/nav2_simple_navigator/src/simple_navigator.cpp deleted file mode 100644 index 5d91d3c75e989fd0b268c94e30749d0550d75351..0000000000000000000000000000000000000000 --- a/nav2_simple_navigator/src/simple_navigator.cpp +++ /dev/null @@ -1,170 +0,0 @@ -// Copyright (c) 2018 Intel Corporation -// -// 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 "nav2_simple_navigator/simple_navigator.hpp" - -#include <string> -#include <memory> -#include <exception> -#include <chrono> - -using namespace std::chrono_literals; -using nav2_tasks::TaskStatus; - -namespace nav2_simple_navigator -{ - -SimpleNavigator::SimpleNavigator() -: Node("SimpleNavigator") -{ - RCLCPP_INFO(get_logger(), "Initializing"); - - auto temp_node = std::shared_ptr<rclcpp::Node>(this, [](auto) {}); - - planner_client_ = - std::make_unique<nav2_tasks::ComputePathToPoseTaskClient>(temp_node); - - controller_client_ = std::make_unique<nav2_tasks::FollowPathTaskClient>(temp_node); - - task_server_ = std::make_unique<nav2_tasks::NavigateToPoseTaskServer>(temp_node); - - task_server_->setExecuteCallback( - std::bind(&SimpleNavigator::navigateToPose, this, std::placeholders::_1)); -} - -SimpleNavigator::~SimpleNavigator() -{ - RCLCPP_INFO(get_logger(), "Shutting down"); -} - -TaskStatus -SimpleNavigator::navigateToPose(const nav2_tasks::NavigateToPoseCommand::SharedPtr command) -{ - RCLCPP_INFO(get_logger(), "Begin navigating from current location to (%.2f, %.2f)", - command->pose.position.x, command->pose.position.y); - - // Create the path to be returned from ComputePath and sent to the FollowPath task - auto path = std::make_shared<nav2_tasks::ComputePathToPoseResult>(); - - RCLCPP_DEBUG(get_logger(), "Getting the path from the planner"); - RCLCPP_DEBUG(get_logger(), "goal->pose.position.x: %f", command->pose.position.x); - RCLCPP_DEBUG(get_logger(), "goal->pose.position.y: %f", command->pose.position.y); - RCLCPP_DEBUG(get_logger(), "goal->pose.position.z: %f", command->pose.position.z); - RCLCPP_DEBUG(get_logger(), "goal->pose.orientation.x: %f", command->pose.orientation.x); - RCLCPP_DEBUG(get_logger(), "goal->pose.orientation.y: %f", command->pose.orientation.y); - RCLCPP_DEBUG(get_logger(), "goal->pose.orientation.z: %f", command->pose.orientation.z); - RCLCPP_DEBUG(get_logger(), "goal->pose.orientation.w: %f", command->pose.orientation.w); - - planner_client_->sendCommand(command); - - // Loop until the subtasks are completed - for (;; ) { - // Check to see if this task (navigation) has been canceled. If so, cancel any child - // tasks and then cancel this task - if (task_server_->cancelRequested()) { - RCLCPP_INFO(get_logger(), "Navigation task has been canceled."); - planner_client_->cancel(); - task_server_->setCanceled(); - return TaskStatus::CANCELED; - } - - // Check if the planning task has completed - TaskStatus status = planner_client_->waitForResult(path, 100ms); - - switch (status) { - case TaskStatus::SUCCEEDED: - RCLCPP_INFO(get_logger(), "Achieved navigation goal of (%.2f, %.2f)", - command->pose.position.x, command->pose.position.y); - goto planning_succeeded; - - case TaskStatus::FAILED: - RCLCPP_ERROR(get_logger(), "Planning task failed."); - return TaskStatus::FAILED; - - case TaskStatus::CANCELED: - RCLCPP_INFO(get_logger(), "Planning task canceled"); - break; - - case TaskStatus::RUNNING: - RCLCPP_DEBUG(get_logger(), "Planning task still running."); - break; - - default: - RCLCPP_ERROR(get_logger(), "Invalid status value."); - throw std::logic_error("Invalid status value"); - } - } - -planning_succeeded: - - RCLCPP_INFO(get_logger(), "Received path of size %u from planner", path->poses.size()); - - int index = 0; - for (auto pose : path->poses) { - RCLCPP_DEBUG(get_logger(), "point %u x: %0.2f, y: %0.2f", - index, pose.position.x, pose.position.y); - index++; - } - - RCLCPP_INFO(get_logger(), "Sending path to the controller to execute."); - - controller_client_->sendCommand(path); - - // Loop until the control task completes - for (;; ) { - // Check to see if this task (navigation) has been canceled. If so, cancel any child - // tasks and then cancel this task - if (task_server_->cancelRequested()) { - RCLCPP_INFO(get_logger(), "Navigation task has been canceled."); - controller_client_->cancel(); - task_server_->setCanceled(); - return TaskStatus::CANCELED; - } - - // Check if the control task has completed - auto controlResult = std::make_shared<nav2_tasks::FollowPathResult>(); - TaskStatus status = controller_client_->waitForResult(controlResult, 100ms); - - switch (status) { - case TaskStatus::SUCCEEDED: - { - RCLCPP_INFO(get_logger(), "Control task completed."); - - // This is an empty message, so there are no fields to set - nav2_tasks::NavigateToPoseResult navigationResult; - - task_server_->setResult(navigationResult); - return TaskStatus::SUCCEEDED; - } - - case TaskStatus::FAILED: - RCLCPP_ERROR(get_logger(), "Control task failed."); - return TaskStatus::FAILED; - - case TaskStatus::CANCELED: - RCLCPP_INFO(get_logger(), "Control task canceled"); - break; - - case TaskStatus::RUNNING: - RCLCPP_DEBUG(get_logger(), "Control task still running"); - break; - - default: - RCLCPP_ERROR(get_logger(), "Invalid status value."); - throw std::logic_error("Invalid status value"); - } - } -} - -} // namespace nav2_simple_navigator diff --git a/nav2_system_tests/src/system/CMakeLists.txt b/nav2_system_tests/src/system/CMakeLists.txt index e932a45f046228fd70f0a1370d664b3ccea3b232..5dd6f3f9f7abb429cef1c4542e1ca37457254cbd 100644 --- a/nav2_system_tests/src/system/CMakeLists.txt +++ b/nav2_system_tests/src/system/CMakeLists.txt @@ -1,20 +1,3 @@ -# this test represents the current default configuration used in the docs and -# nav2_bringup package. -ament_add_test(test_simple_navigator - GENERATE_RESULT_FOR_RETURN_CODE_ZERO - COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py" - WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" - TIMEOUT 120 - ENV - TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} - TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml - TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world - TEST_PARAMS=${PROJECT_SOURCE_DIR}/params/nav2_params.yaml - GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models - NAVIGATOR=simple - ASTAR=False -) - # this tests the other navigator and planner option not used by default # in the nav2_bringup package. ament_add_test(test_bt_navigator @@ -28,7 +11,6 @@ ament_add_test(test_bt_navigator TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world TEST_PARAMS=${PROJECT_SOURCE_DIR}/params/nav2_params.yaml GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models - NAVIGATOR=bt BT_NAVIGATOR_XML=navigate_w_recovery_retry.xml ASTAR=True ) diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py index 9e9fb936cac382b236fbfcb37dd554fc8327b83b..1eeb85f997b17f68d051791097ee72d85b1ae637 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/nav2_system_tests/src/system/test_system_launch.py @@ -32,27 +32,16 @@ def generate_launch_description(): world = os.getenv('TEST_WORLD') params_file = os.getenv('TEST_PARAMS') astar = (os.getenv('ASTAR').lower() == 'true') - navigator = os.getenv('NAVIGATOR') - if (navigator == 'simple'): - navigator_action = launch_ros.actions.Node( - package='nav2_simple_navigator', - node_executable='simple_navigator', - node_name='simple_navigator', - output='screen', - parameters=[{'use_sim_time': use_sim_time}]) - elif (navigator == 'bt'): - bt_navigator_install_path = get_package_prefix('nav2_bt_navigator') - bt_navigator_xml = os.path.join(bt_navigator_install_path, - 'behavior_trees', - os.getenv('BT_NAVIGATOR_XML')) - navigator_action = launch_ros.actions.Node( - package='nav2_bt_navigator', - node_executable='bt_navigator', - node_name='bt_navigator', - output='screen', - parameters=[{'use_sim_time': use_sim_time}, {'bt_xml_filename': bt_navigator_xml}]) - else: - raise EnvironmentError('NAVIGATOR environment variable not set correctly.') + bt_navigator_install_path = get_package_prefix('nav2_bt_navigator') + bt_navigator_xml = os.path.join(bt_navigator_install_path, + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML')) + navigator_action = launch_ros.actions.Node( + package='nav2_bt_navigator', + node_executable='bt_navigator', + node_name='bt_navigator', + output='screen', + parameters=[{'use_sim_time': use_sim_time}, {'bt_xml_filename': bt_navigator_xml}]) return LaunchDescription([ # Launch gazebo server for simulation diff --git a/navigation2/package.xml b/navigation2/package.xml index d576eed99a503c54b12856b43d81880e32c4ea18..ca9fc9934dfce5ef9eef62233d17a59fb09c228d 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -25,7 +25,6 @@ <exec_depend>nav2_motion_primitives</exec_depend> <exec_depend>nav2_msgs</exec_depend> <exec_depend>nav2_robot</exec_depend> - <exec_depend>nav2_simple_navigator</exec_depend> <exec_depend>nav2_tasks</exec_depend> <exec_depend>nav2_util</exec_depend> <exec_depend>nav2_voxel_grid</exec_depend> diff --git a/tools/run_test_suite.bash b/tools/run_test_suite.bash index 0902be7c5a8040b699716372139fca2286173ac8..2d25289a8a80ee1efc3501aa20ecd1885ff22be8 100755 --- a/tools/run_test_suite.bash +++ b/tools/run_test_suite.bash @@ -23,5 +23,4 @@ colcon test --packages-select nav2_system_tests --ctest-args --exclude-regex "te colcon test-result --verbose $SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_localization -$SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_simple_navigator $SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_bt_navigator