Skip to content
Snippets Groups Projects
Commit af6b8cdd authored by Steven Macenski's avatar Steven Macenski Committed by Michael Jeronimo
Browse files

Remove simple navigator (#721) 

* remove simple navigator in factor of behavior tree

* removing things that depend on simple_navigator

* Fix line issue
parent 90f12bc1
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 15 additions and 400 deletions
doc/design/ROS_COMPARISON.md
+ 5
− 9
View file @ af6b8cdd
......@@ -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)
![Move Base 2](./move_base_compare_2.png)
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**
nav2_simple_navigator/CHANGELOG.rst deleted 100644 → 0
+ 0
− 0
View file @ 90f12bc1
nav2_simple_navigator/CMakeLists.txt deleted 100644 → 0
+ 0
− 63
View file @ 90f12bc1
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()
nav2_simple_navigator/README.md deleted 100644 → 0
+ 0
− 17
View file @ 90f12bc1
# 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.
nav2_simple_navigator/include/nav2_simple_navigator/simple_navigator.hpp deleted 100644 → 0
+ 0
− 44
View file @ 90f12bc1
// 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_
nav2_simple_navigator/package.xml deleted 100644 → 0
+ 0
− 31
View file @ 90f12bc1
<?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>
nav2_simple_navigator/src/main.cpp deleted 100644 → 0
+ 0
− 25
View file @ 90f12bc1
// 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;
}
nav2_simple_navigator/src/simple_navigator.cpp deleted 100644 → 0
+ 0
− 170
View file @ 90f12bc1
// 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
nav2_system_tests/src/system/CMakeLists.txt
+ 0
− 18
View file @ af6b8cdd
# 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
)
nav2_system_tests/src/system/test_system_launch.py
+ 10
− 21
View file @ af6b8cdd
......@@ -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
......
navigation2/package.xml
+ 0
− 1
View file @ af6b8cdd
......@@ -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>
......
tools/run_test_suite.bash
+ 0
− 1
View file @ af6b8cdd
......@@ -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
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment